ISSN 0005-1179, Automation and Remote Control, 2011, Vol. 72, No. 7, pp. 1548–1556. © Pleiades Publishing, Ltd., 2011. Original Russian Text © R.F. Gilimyanov, 2010, published in Problemy Upravleniya, 2010, No. 1, pp. 71–76.

CONTROL SCIENCES

Recursive Method of Smoothing Curvature of Path in Path Planning Problems for Wheeled Robots R. F. Gilimyanov Trapeznikov Institute of Control Sciences, Russian Academy of Sciences, Moscow, Russia Received October 20, 2009

Abstract—Path planning problem is considered as follows. Wheeled robot is manually guided over the required path; coordinates of the path are measured by GNSS receiver. Following the initial path in automatic mode makes it necessary to construct a path which satisfies certain smoothness requirements and curvature constraints. Small measurement errors for the coordinates of the corresponding points may substantially modify the curvature of the constructed path (thus, making it inapplicable for control). A recursive method of smoothing curvature of the curve (composed of uniform cubic B-splines) is proposed; the method can be used in real-time under constraints imposed on available random-access memory. DOI: 10.1134/S0005117911070204

1. INTRODUCTION Path planning problem is considered for wheeled robot with two control modes, viz., manual and automatic ones. Wheeled robot is manually guided over the required path; coordinates of the path are measured by GNSS receiver and saved as a reference path follow in automatic mode. The problem to construct the path of wheeled robot based on the given array of 2-D points is immediate. The relevance of this problem seems obvious in many applications; for instance, agricultural vehicles have to follow the same path many times (and with high accuracy) for seeding, distributing water and fertilizers, etc. The constructed path must satisfy those constraints imposed on nonholonomic system (i.e., wheeled robot). The matter is that wheeled robot can not move in lateral direction without sliding; in addition, turning angle of the front wheels and steering angular velocity are both limited. Consequently, the path must be, first, smooth in the sense of its curvature and, second, twice continuously differentiable function (thus, belonging to the class C 2 ). Moreover, the curvature must not exceed the maximum level kmax = 1/Rmin which corresponds to the minimum turning radius Rmin of the robot. Another important issue lies in efficient evaluation of the distance between the robot and path. In the present paper we will understand smoothness (or fairness) of the path and curve (in the sense of smooth curvature plot) as follows. The curve is smooth (or fair) if the plot of the curvature is continuous, has the appropriate sign, and is as close as possible to a piecewise monotone function with as few as possible monotone pieces [1, 2]. There exist numerous approaches to path representation (e.g., using a sequence of straight lines and circular arcs, polynomial splines, clothoids or generalized spirals of Cornu). One of such approaches is based on the path approximation involving uniform cubic B-splines [3] (the cited paper either introduces the function of quasi-distance which serves to estimate the deviation of the robot from the reference path). The described procedure of approximation results in smooth C 2 -class curve. However, small errors in GNSS measurements of the points’ coordinates lead to 1548

RECURSIVE METHOD OF SMOOTHING CURVATURE

1549

substantial modification of the trajectory’s curvature; the latter oscillates and may even exceed the maximum level kmax . In practice, this impairs control performance within the system. In addition to path planning problems for wheeled robots, smooth curves are important in several fields; for instance, one should mention path planning for robot-manipulator or tools migration in CNC machines, as well as CAD systems and computer graphics systems. Various methods are used to construct smooth spline curves, e.g., based on smoothing splines [4–7] and fairing [1, 2, 8–12]. The following two techniques have become widespread. In the first one, construction of the curve is combined with its smoothing; the second technique starts with construction of the approximating (alternatively, interpolating) curve with subsequent smoothing. In fact, the method described in [12] is of the second type. The foundation is provided by quadratic programming problem; both the complexity level and time used to solve the problem possess nonlinear growth with respect to increasing number of the points. In order to improve efficiency, in this paper we suggest passing from constrained minimization problem to the unconstrained one. Solving the latter involves the construction of recursive procedure. The procedure is used to design special methods with sliding windows, allowing for smoothing curvature of long paths (under constraints imposed on available random-access memory and in real-time mode with small delay). The feature mentioned last could be either beneficial in formation control problems. The present paper illustrates the results of applying these methods to actual data acquired during GNSS measurements, as well as the results of comparison with the previous method discussed in [12]. 2. CONSTRAINED MINIMIZATION PROBLEM Let us introduce the notation and recall the fundamentals of paper [12]. Suppose there is a set of uniformly precise GNSS measurements r1 , r2 , . . ., rn , ri ∈ R2 (i.e., the measurement noises have identical variance). Assume the distance between any couple of adjacent reference points is almost the same: ri+1 − ri  ≈ rj+1 − rj ; here · stands for Euclidean vector norm. The required path is approximated by uniform cubic B-splines, each being constructed with respect to the quadruple of the reference points as follows: r (i) (t) = Ri M T (t), ⎡

M=

1⎢ ⎢ ⎢ 6⎣

1 −3 3 −1 4 0 −6 3 1 3 3 −3 0 0 0 1

Ri = [ri−1 , ri , ri+1 , ri+2 ],

⎤

⎥ ⎥ ⎥, ⎦

⎡ ⎢ ⎢ ⎣

T (t) = ⎢

1 t t2 t3

⎤

⎥ ⎥ ⎥, ⎦

t ∈ [0, 1].

In the formula above, t specifies spline parameter. Small measurement noise exerts a significant influence on the curvature defined by k(t) = 3 . The results of numerical experiment below demonstrate that the curvar(t) ˙ × r¨(t) / r(t) ˙ ture of the curve (constructed according to the described formula based on the initial data) has inadequate shape. The whole point is that wheeled robot having limited turning angle and steering angular velocity could hardly follow the path with such curvature in automatic mode. In the case of uniformly selected reference points, the vector norm of the second derivative with respect to the spline parameter, r(t), ˙ is almost constant. Thus, the curvature of the curve is proportional to the norm of the second derivative of the spline: k ∼ ¨ r(t). This function is (i) (i−1) (1) = ri−1 − 2ri + ri+1 in continuous for any elementary spline and takes the values r¨ (0) = r¨ ... the junction point with the adjacent spline. The derivative of the curvature k˙ ∼  r (t) is fixed for ... a single elementary spline r (i) = −ri−1 + 3ri − 3ri+1 + ri+2 and possesses discontinuity at junction ... ... ... point with the adjacent spline Δ r i ≡ r (i) − r (i−1) = ri−2 − 4ri−1 + 6ri − 4ri+1 + ri+2 . These jumps of the third derivative at junction points of two splines have an impact on the curvature plot. AUTOMATION AND REMOTE CONTROL

Vol. 72

No. 7

2011

1550

GILIMYANOV

... In [12] it has been suggested to minimize the jump projections of the third derivative (Δ r i , Ni ) via small variations εi of the reference points in direction being perpendicular to the curve: r˜i = ri + Ni εi , since small shift along the curve slightly modifies its shape; here Ni indicates the normal to B-spline curve at the junction point of the ith and (i + 1)th elementary splines. The variations should be upper bounded by a certain quantity equal to the measurement error δ. We rewrite jump projections in the form of variations: Fi (ε) = Fi (0) + (Ni−2 , Ni )εi−2 − 4(Ni−1 , Ni )εi−1

(1)

+ 6εi − 4(Ni+1 , Ni )εi+1 + (Ni+2 , Ni )εi+2 ,

... with Fi (0) ≡ (Δ r i , Ni ) = (ri−2 , Ni ) − 4(ri−1 , Ni ) + 6(ri , Ni ) − 4(ri+1 , Ni ) + (ri+2 , Ni ). Evaluation of F1 , F2 and Fn−1 , Fn requires additional points, viz., two points r−1 , r0 at the beginning of path and two points rn+1 , rn+2 at the end of it. For these points we set ε−1 = ε0 = εn+1 = εn+2 = 0. The issues regarding proper choice of additional boundary points and consequences of the choice are treated in [12]. Introduce the vector notation ε = [ε1 , . . . , εn ]T , F (ε) = [Fi (ε), . . . , Fn (ε)]T and rewrite (1) in matrix form F (ε) = F (0) + Cε,

(2)

where C means five-diagonal matrix ⎡

6

−4n12

n13

0

···

.. .

.. .

.. .

.. .

..

⎤

⎢ ⎥ ⎢ −4n21 6 −4n23 n24 · · · ⎥ ⎢ ⎥ ⎢ ⎥ ⎥, −4n 6 −4n · · · n C=⎢ 31 32 34 ⎢ ⎥ ⎢ ⎥ ⎢ 6 ··· ⎥ 0 n42 −4n43 ⎣ ⎦

.

and nij = (Ni , Nj ) stands for scalar product of two normals. Hence, minimization of the third derivative jumps has been reduced to minimization of quadratic functional F (ε)2 . Raising expression (2) to the second power and casting out the free term, we obtain the quadratic functional 1 Φc (ε) = εT Hc ε + f T ε, 2

Hc = C T C,

f = C T F (0).

Therefore, we arrive at the following quadratic programming problem with simple constraints min Φc (ε), ε

ε ∈ Rn ,

−δ ≤ εi ≤ δ.

(3)

Growing number of the points leads to nonlinear increase both in complexity level and time consumed to solve the problem. In general case, quadratic programming problem is NP -hard. In the present case of positive definite matrix Hc , problem (3) is solved in polynomial time and requires execution of approximately O(np L) arithmetic operations (e.g., see [13] and the references therein). Note that L stands for the length of the problem input (this quantity describes the number of binary symbols being necessary to formulate input data of the problem), while p > 1 is a certain parameter. For instance, p = 3.5 when the problem is solved through interior point method. 3. UNCONSTRAINED MINIMIZATION PROBLEM With the aim of improving the efficiency, it is suggested to pass from constrained optimization problem to unconstrained one. For this, a penalty is introduced for large variations of the reference AUTOMATION AND REMOTE CONTROL

Vol. 72

No. 7

2011

RECURSIVE METHOD OF SMOOTHING CURVATURE

1551

points as follows: 1 1 Φ(ε) = Φc (ε) + γεT ε = εT Hε + f T ε, 2 2 H = Hc + γI. Here I stands for identity matrix of the size n × n, while γ designates a certain parameter (being selected experimentally). Thus, the original problem has been reduced to unconstrained minimization of the quadratic functional min Φ(ε), ε

ε ∈ Rn .

(4)

Solving it requires, in fact, solving the system of linear equations Hε = −f T

(5)

involving symmetrical positive definite matrix H. This property of the matrix allows for its compact storage in memory; in addition, it allows for efficient solving the system via the factorization H = LLT (with upper and lower triangular band matrices). As the result of factorization, it suffices to solve the following triangular band systems Ly = −f T ,

(6)

T

L ε = y.

(7)

The lower band width p of the matrix L being equal to 4, solving the system of Eqs. (5) requires execution of approximately 46n operations of summation and multiplication, as well as n operations of square root evaluation [14]. This is less in comparison with those operations being necessary to solve quadratic programming problem (3). 4. RECURSIVE PROCEDURE Let us construct recursive procedure to solve problem (4). Suppose there exist measured coordinates of the points r1 , r2 , . . ., rk+1 . Add two boundary points r−1 , r0 at the beginning of the path. Use the points rk , rk+1 as two boundary points at the end of the path (recall coordinates of the boundary points are fixed). Based on the measured data, construct the matrices C k−1 , H k−1 and Lk−1 having the size (k − 1) × (k − 1), as well as the vectors F k−1 (0), f k−1 , y k−1 , εk−1 of the size (k − 1). Now, consider what changes take place in the above-mentioned parameters, if new measurement rk+2 appears and rk+1 , rk+2 serve as new boundary points at the end of the path. • •

The vector F (0) is extended by the new element Fkk (0). The matrix C is extended by the new row and column: 

Ck = •

C k−1 ck cTk ckk



,

ck = [0, . . . , 0, ck−2,k , ck−1,k ]T ,

ckk = 6.

The vector f has two last elements modified; in addition, it is extended by the new element fkk :

T f k = (f˜k−1 )T , f k . Note f˜k−1 indicates the vector f k−1 , such that the last two components k

are increased by the corresponding components of the vector Fkk (0)[ck−2,k , ck−1,k ]T , fkk =

 k (0), F k (0), F k (0) [c T Fk−2 k−2,k , ck−1,k , ck−1,k ] . k−1 k AUTOMATION AND REMOTE CONTROL

Vol. 72

No. 7

2011

1552 •

GILIMYANOV

The matrices H and L have the right lower block of the size 2 × 2 modified; in addition, they ˜ k−1 hk H ˜ k−1 stands are extended by the row and column: H k = . Note the matrix H hTk hkk + γ for the matrix H k−1 , such that the right lower block is elementwise increased by the matrix 

D=

d1 d3 d2 d4

 •

Lk =

˜ k−1 0 L lkT lkk

˜ k−1 lows: L k−2,k−2 =





=

hTk hkk



c2k−2,k ck−2,k ck−1,k ck−2,k ck−1,k c2k−1,k



,

d2 = d3 ,

= [ cTk ckk ]C k (1 : k, k − 4 : k).



˜ k−1 means the matrix Lk−1 with three elements eliminated, as fol. Here L

k−1 k−1 ˜ k−1 Lk−1 k−2,k−2 + d1 , Lk−1,k−2 = Lk−2,k−2 Lk−1,k−2 + d2



˜ k−1 ˜ k−1 L k−2,k−2 , Lk−1,k−1 =

k−1 2 2 ˜ k−1 ˜ k−1 (Cholesky factoriza(Lk−1 )2 + d4 . With the matrix L k−1,k−2 ) − (Lk−1,k−2 ) + (Lk−1,k−1

 ˜ k−1 ) and row hT , hkk + γ being known, one may further evaluate the tion of the matrix H k



•

•



factorization of Lk and find the row lkT lkk . The vector y which solves the system of Eqs. (6) has two last components modified; the vector in question is either extended by the new element ykk . All the components are easily computed k , y k , y k ]T . through three steps of direct run y k = [(y k−1 (1 : k − 3))T , yk−2 k−1 k The vector ε which solves system (7) is changed dramatically. The vector εk results from complete reverse run.

In order to perform direct runs and obtain solution y of system (6) (under new measurement), it suffices to use nothing more than five last elements of the vectors F (0) and f , as well as the right lower 5 × 5 block of the matrices C, H and L. Evaluation of ε requires the knowledge of the whole matrix L and the vector y. The constructed recursive procedure used to evaluate the matrix L and the vector y possesses the following advantages. It enables solving the problems having high dimensionality under constraints imposed on available random-access memory. In particular, having obtained new measurements of the point coordinates, perform direct run and save the matrix L and the vector y in the storage (until the end of the trajectory is reached). Next, proceed with complete reverse run through reading the parameters L and y in reverse direction, and find the required solution ε. 5. METHODS BASED ON SLIDING WINDOW For sufficiently great k ≤ n, it has been observed that the initial elements of the vectors εk−1 and εk almost coincide. The underlying reason is that the problems involving the matrices H k−1 and H k have identical boundary conditions at the beginning and different at the end of the path. Hence, instead of solution to system (5) (with the matrix H k ) one may employ the first (k − l) components of the solution εk−1 to system (5) (with the matrix H k−1 ). The rest elements of εk can be computed at l steps of the reverse run of system (7) with the matrix (Lk )T , notably,

εk = (εk−1 (1 : k − l))T , εkk−l+1 , . . . , εkk

T

.

It is also possible to construct recursive method of smoothing the curvature of path which involves the so-called sliding window of the fixed size l × l, l ≥ p + 1, p = 4. The matrix L and vector y are stored in the arrays of the length l. As soon as cells of the array are filled in, a shift takes place; as the result, the first element is deleted, while the last one is emptied. This AUTOMATION AND REMOTE CONTROL

Vol. 72

No. 7

2011

RECURSIVE METHOD OF SMOOTHING CURVATURE

1553

Fig. 1. Methods based on sliding window.

process is illustrated by the left-hand part of Fig. 1. Following the occurrence of new measurement, the matrix L and the vector y are evaluated according to the recursive procedure, as well as the corresponding arrays are filled in. In the case when the arrays have not been completely filled in (and the path is still not finished), the reverse run is not applied. Otherwise, l steps of the reverse run are performed and the first element of the derived solution ε gives the result (if the path is not finished, all l elements of ε are used). The following modification of the procedure is proposed (it differs in the technique of the reverse run and the window size w, w > l). When new measurement appears, direct run takes place as usual; however, the reverse run is not applied until the arrays L and y are filled in. With this being the case, the complete reverse run (w steps) is performed and the first (w − l) components of the solution ε are supplied. The shift follows resulting in deletion of the first (w − l) components of the arrays L, y and ε. The right-hand part of Fig. 1 visualizes the described process. Denote by n the number of measurements. Application of the initial procedure requires (n−l+1)l steps of the reverse run, while the modified one employs approximately n(1 + l/(w − l)) steps. The greater is the window size w (relative to l), the less reverse runs one needs; however, we will obtain the solution with greater delay. Both methods based on sliding window can be used to smooth long paths under certain constraints imposed on available random-access memory (not only for postprocessing, but also in real-time with small delay). 6. NUMERICAL EXPERIMENTS The suggested method of smoothing curvature has been tested at numerous paths constructed according to the actual data of navigation measurements. The car equipped with satellite antenna, GNSS receiver, turn drive for the steering wheels and angle sensor of the wheels serves as wheeled robot during the experiments. The minimum turning radius of the car is Rmin ≈ 5 m (i.e., kmax ≈ 0.2 m−1 ). Here we present results of the following experiment. Being manually guided, the car follows the required trajectory; coordinates of the points are measured by the receiver in phasedifferential mode (note the horizontal root-mean-square error constitutes approximately 1.5 cm). Dotted line in Fig. 2 represents the path constructed for the obtained set of points r1 , . . . , rn , n = 454. Fine line in Fig. 3 is used to demonstrate the curvature of this path depending on the traversed path s. Evidently, the curvature of the path constructed with respect to the initial points is oscillatory-type and exceeds the threshold kmax . In addition, we have derived two sets of points through shifting the initial points along the normals at the distances εcon and εunc (these values have been computed when solving the problems of constrained minimization with δ = 0.025 m and unconstrained minimization with γ = 0.001, respectively). Figure 2 displays the corresponding paths, see the solid and dashed lines. The curvature of the second path is (only) demonstrated by Fig. 3 using the heavy line; the reason is AUTOMATION AND REMOTE CONTROL

Vol. 72

No. 7

2011

1554

GILIMYANOV

Fig. 2. Paths for different sets of points (the right-hand part shows a scaled-up fragment).

Fig. 3. Relationship between the curvature and traversed path.

that the curvature plot of both paths are almost the same. None of the absolute-valued components of the derived solution εcon exceeds 0.025 m. On the other hand, merely 6 components (out of 454) of the derived solution εunc are greater than the threshold value in question (the maximum component makes 0.036 m). The application of the method based on sliding window (l = 50) and the modified method (w = 150, l = 50) has yielded the solutions εsw1 and εsw2 that almost coincide with εunc (see AUTOMATION AND REMOTE CONTROL

Vol. 72

No. 7

2011

RECURSIVE METHOD OF SMOOTHING CURVATURE

1555

Fig. 4. The difference between solutions (X-axis indicates the number of the vector component).

Fig. 4). The corresponding curves of the curvature are not provided here, since they slightly differ from the one of the heavy line in Fig. 3. All the paths in Fig. 2 are close to each other; nevertheless, only those ones plotted by solid and dashed lines have proper curvature for the wheeled robot (follow in automatic mode); this fact has been verified by several experiments on car stabilization along the path in automatic mode. Note the control law discussed in [15] has been utilized. Steering angular velocity has served as control variable. During the motion along the path (constructed with respect to the initial points) the steering wheel has permanently turned to the left and to the right; in other words, the car has tried to follow the path with rapidly oscillating curvature. Such control appears inefficient (the car follows the path with large lateral deviations, as well as consumes much energy to turn the wheels). This may lead to damaged steering. The motion along the path with smoothed curvature has shown better performance. 7. CONCLUSIONS In this paper we have developed recursive methods of smoothing the curvature of paths in high-dimensional problems under constraints imposed on available random-access memory. Those methods based on sliding window may be either used in real-time mode with small delay. Efficiency of the suggested algorithms has been illustrated by numerical examples of their application to paths constructed according to actual GNSS data. ACKNOWLEDGMENTS This work was supported by the Department for Energy, Mechanical Engineering and Control Processes (Russian Academy of Sciences), program no. 15, and by National Program for Leading Scientific Centers of the Russian Federation, project no. NSh-1676.2008.1. REFERENCES 1. Farin, G. and Sapidis, N., Curvature and the Fairness of Curves and Surfaces, IEEE Computer Graph. Appl., 1989, vol. 9, no. 2, pp. 52–57. AUTOMATION AND REMOTE CONTROL

Vol. 72

No. 7

2011

1556

GILIMYANOV

2. Sapidis, N. and Farin, G., Automatic Fairing Algorithm for B-Spline Curves, Computer-Aided Design, 1990, vol. 22, pp. 121–129. 3. Pesterev, A.V. and Gilimyanov, R.F., Path Planning for a Wheeled Robot, in Trudy Inst. Sist. Anal., Ross. Akad. Nauk (Proc. of Inst. for System Analysis), Moscow: KomKniga, 2006, vol. 25, pp. 204–211. 4. Schoenberg, I., Spline Functions and the Problem of Graduation, Proc. Nat. Acad. Sci., 1964, vol. 52, pp. 947–950. 5. Reinsch, C., Smoothing by Spline Functions, Numer. Math., 1967, vol. 10, pp. 177–183. 6. De Boor, C., A Practical Guide to Splines, New York: Springer-Verlag, 1978. 7. Zavyalov, Yu.S., Kvasov, B.I., and Miroshnichenko, V.L., Metody splain-funktsii (Methods of Spline Functions), Nauka: Moscow, 1980. 8. Kjellander, J., Smoothing of Cubic Parametric Splines, Computer-Aided Design, 1983, vol. 15(3), pp. 175–179. 9. Farin, G. et al. Fairing Cubic B-Spline Curves, Computer Aided Geom. Design, 1987, vol. 4, pp. 91–103. 10. Eck, M. and Hadenfeld, J., Local Energy Fairing of B-spline Curves, Computing Supplement, 1995, vol. 10, pp. 129–147. 11. Xu, S., Li, W., and Zhao, G., Target Curvature Based Automatic Fairing of Planar B-Spline Curves, Computer Aided Geom. Design, 2004, vol. 21(5), pp. 427–530. 12. Gilimyanov, R.F., Pesterev, A.V., and Rapoport, L.B., Smoothing Curvature of Trajectories Constructed by Noisy Measurements in Path Planning Problems for Wheeled Robots, Izv. Ross. Akad. Nauk , Teor. Sist. Upravlen., 2008, vol. 47, no. 5, pp. 152–159. 13. Floudas, C.A. and Pardalos, P.M., Encyclopedia of Optimization, New York: Springer-Verlag, 2009. 14. Golub, G.H. and Van Loan, C.F., Matrix Computations, London: John Hopkins Univ. Press, 1996. 15. Gilimyanov, R.F., Pesterev, A.V., and Rapoport, L.B., Motion Control for a Wheeled Robot Following a Curvilinear Path, Izv. Ross. Akad. Nauk , Teor. Sist. Upravlen., 2008, vol. 47, no. 6, pp. 209–216.

This paper was recommended for publication by V.Yu. Rutkovskii, a member of the Editorial Board

AUTOMATION AND REMOTE CONTROL

Vol. 72

No. 7

2011