[3] S. Scherer, S. Singh, L. Chamberlain and S. Saripalli, ... [5] J. Saunders, B. Call, A. Curtis, R. Beard and T. ... [14] Richard H.Bartels, John C. Beatty, Brian A.
Planning Continuous Curvature Paths for UAVs Amongst Obstacles Kwangjin Yang
and
Salah Sukkarieh
ARC Centre of Excellence in Autonomous Systems Australian Centre for Field Robotics University of Sydney, Australia Email : {k.yang, salah}@cas.edu.au Abstract A smooth path planing algorithm for UAVs in cluttered natural environments is presented. A Rapidly-exploring Random Tree (RRT) algorithm yields a piecewise collision-free linear path amongst obstacles and cubic B´ezier spiral curves are used to generate a continuous curvature path which satisfies the minimum radius curvature constraint. After this path smoothing algorithm, there still exists extra length margins which can make the path smoother. To fully utilize these length margins, the Interactive Algorithm is suggested. Furthermore, if there is a violation of the maximum curvature constraint, a Bisection Algorithm can mitigate this constraint. Simulation results show that the Interactive Algorithm combined with the Bisection Algorithm can utilize the full length margins and satisfy the maximum curvature constraint.
1
Introduction
Path planning for Unmanned Aerial Vehicles (UAVs) in complex environments is an active research area [1]-[3]. The path planning problem for UAVs is difficult since these vehicles have fast and complicated dynamics and are compounded by the issues of real time navigation in 3D space. The computational time of deterministic and complete algorithms grows exponentially with the dimension of the configuration space, so these algorithms do not provide an adequate solution for real-time UAV path planning in unknown natural environments [4]. Recently, sampling-based motion planning has gained much interest as an alternative to complete motion planning methods. Amongst these methods, is the Rapidlyexploring Random Tree (RRT), which has been demonstrated successfully in UAV applications [4]-[7]. The standard RRT algorithm produces a timeparameterized set of control inputs to move from the ini-
tial state (xstart ) to the goal state (xgoal ). The validity of the result is dependant on the accuracy of the statespace model being used. In real UAV applications, we encounter sensor inaccuracies, wind, and other unmodeled factors. These are some of the key disadvantages of using an open-loop path planner such as standard RRT [5]. In this paper, a decoupled path planning and control method is used instead of generating a timeparameterized set of control inputs. This method generates a purely geometric path consisting of straight line segments. Since it is not possible for the UAV to follow this path exactly without stopping at sharp angles, it is necessary to smooth the path. In the UAV community, most researchers apply the Dubins algorithm to generate a smooth path [8]-[10]. Cubic splines are used to generate a smooth path for an autonomous unmanned helicopter in [6] but it is still of C 1 continuity as with Dubins curves. When the UAV begins to fly the circular path, there is a step change of angular velocity as the previous angular velocity is zero whilst the vehicle was traversing along the straight line segment. We use cubic B´ezier curves to generate a continuous curvature path. However a significant disadvantage of this parametric cubic curve is that its curvature is a complicated function of its parameter. If a geometric property of the curve is used, it is possible to reduce the complexity of the curve. In [11], the authors suggested a computationally efficient path smoothing algorithm which satisfies G2 continuity and nonholonomic constraints. However there still exists extra length margins. We suggest an algorithm to fully utilize the maximum length margins. Moreover, if there is a violation of the maximum curvature constraint, a Bisection algorithm can mitigate this constraint. The rest of the paper is organized as follows: Section 2 describes path planning methods, and the C 1 Continuous Cubic B´ezier Curve (C1CBC) and the G2 Continuous Cubic B´ezier Spiral (G2CBS) path smoothing algo-
4
rithms are shown at Section 3. The Interactive smoothing algorithm and the Bisection algorithm are presented in Sections 4 and 5. 3D path smoothing method is shown at Section 6 and the G2CBS path smoothing process are described at Section 7. Simulation results are shown at Section 8 and conclusions are presented thereafter.
RRT Path
RRT Path Pruning
25 20
Altitude(m)
15
300
10
5
200
250
Altitude(m)
20 10 0 300
0
50
150
100
200
100
50
50
North(m)
0
East(m) 0
0
0
(a) Path planning using RRT (b) algorithm. Figure 1: (Left) Fig. 2. (a)Path Path pruning planning using Biased-Greedy (Right) algorithm: The RRT pathalgorithm. consists(b)ofPath pruning algorithm: The path consists of 45 nodes but after removing redundant 47 nodes but after removing redundant waypoints, the waypoints, the number of nodes is reduced to only 1.
number of nodes is reduced to only 3.
III. but PATH MOOTHING target point initially theSnumber of nodes is reduced 1 1 after thePath redundant waypoints are pruned. A.toConly Continuous Smoothing path inSmoothing Figure 2 is piecewise linear and not followable 3ThePath for a UAV with kinematic and dynamic constraints. Therefore, path in Figure (Right) piecewise linear not itThe is necessary to make1 the path is smooth in order to and be suitable suitable for a UAV with kinematic and dynamic conto UAVs. straints. Therefore, it is necessary to make the path smooth. To smoothly connect this piecewise path, the notion of continuity is needed. Generally continuity is associated with the smoothness of the joints between adjoint curves. The first order parameter continuous (C 1 ) is achieved if the first derivatives of the segments are equal at the junctions. To achieve G2 continuity the tangent and the curvature values must be same at the joints. (a) (b) In this section, the C 1 Continuous Cubic B´ezier Curve(C1CBC) and G2 Continuous Cubic B´ezier Spiral(G2CBS) path smoothing methods are briefly reviewed. The detailed algorithms are suggested in [11]. 3000
Altitude(m)
(b) (a) RRT was first suggested in [13] as a alternative to comFig. 1. (a) Our helicopter flying in the flight test site located in Marulan. plete path planning high degree of freedom situations. (b) The simulation environmentincluttered with “trees”. This control-theoretic method is open-loop planning and works well if the state-space model of the platform is exact. However, if there is a model mismatch, then the of generating time-parameterized we generatedapath will differ from set the of realcontrol path. inputs, Moreover, decouple planning and control Wepath generate since the thepath sensor information is notproblems. perfect, the may a purely geometric using a variantenvironments, of RRT to define also be unsafe. path Finally, in natural there waypoints. Ourwinds RRT which algorithm as follows.An First, are always affectoperates UAVs maneuvers. opena position is chosen method at random from consider within thethese workspace, loopxpath cannot disturrand planning and this point compared existing tree nodes to find the bances at is the planningwith stage and cannot compensate for closest point in the tree, xnear . A line is drawn connecting their effects. Instead generating a time-parameterized of conxnear to xrand ,ofand a new point xnew is generated set along this trol inputs, we decouple the path planning and control ray at a fixed distance d from xnear . If there is no collision problems. We generate purely geometric path using on the interval between xneara and xnew , the latter is added a variant of RRT to define waypoints. Our RRT to the tree. The planning time of RRT is reduced greatly alby gorithm operates of as xfollows. First, a position x time is biasing the generation it rand , such that 10% of the rand chosen random withinlocation. the workspace, and this equals xgoalatrather thanfrom a random Fig. 2 (a) shows pointplanning is compared existing tree nodes findlines the the path resultwith of RRT Algorithm. The to green closest point in the tree, x . A line is drawn connectnear are all trees generated by the algorithm and the red line is the ing path xnearwhich to xrand , and athenew pointpoint xnewand is generated shortest connects starting the target along this ray at a fixed distance d from xnear . If there point. is no collision on the interval between xnear and xnew , the latter is added to the tree. Figure 1 (Left) shows the path planning result of RRT Algorithm. B. Path PruningThe thin lines are all trees generated by the algorithm and the thick line is the shortest path which connects starting andand the computationally target point. effiEven thoughtheRRT is an point effective Even though RRT is an effective and computationally cient tool for complex online motion planning, the solution is efficient tool for complex online motion planning, sofar from optimal due to its random exploration of the the space. lution is far from optimal due to its random exploration It is thus required to remove unnecessary waypoints. In [7], of the space. We use a simple but quite efficient method Dijkstra’s algorithm is used to prune the waypoints to generate which can quickly find a path that eliminates most exa near optimal path, but this method requires a bit large amount traneous nodes. of computational time. Therefore it loses the strong advantage Let the original path of nodes from start to goal point of RRT, namely fast planning. Instead, we use a simple but be denoted {x1 , . . . , xN }, such that xN is the goal locaquitetion. efficient method which quicklyanfind a path that Let the pruned path can be initially empty set, and eliminates most extraneous nodes. let j = N . The pruning operation is as follows. First Let thexjoriginal path of path. nodesThen from for starteach to goal point be add to the pruned i ∈ [1..j − 1], denoted {xthe . , xNbetween }, such (x that is the goal location. Let check ) for a collision, stopping 1 , . .line i , xxjN the pruned bei without initially collision. an empty Let set, jand j =xjNto. The on the path first x = i,letadd the pruning operation as follows. add xj tountil the pruned path. pruned path,isand repeat First the process a complete is generated. This can getbetween rid of unnecesThenpath for each i ∈ [1..j − 1], method check the line (xi , xj ) waypoints very on quickly. Figure 1 (Right) shows the for asary collision, stopping the first xi without collision. Let result algorithm applied the initial j = i, add xofj path to thepruning pruned path, and repeat theto process until RRT path. path has 45 nodes between a complete pathThe is generated. Even though thisstarting path is and not optimal, this method can get rid of unnecessary waypoints very quickly. Fig. 2 (b) shows the result of path pruning algorithm applied to the initial RRT path in Fig. 2 (a). The path has 45 nodes between starting and target point initially but the number of nodes is reduced to only 1 after the redundant waypoints are pruned.
100
North(m)
150 100 50 0
3000
2500
2000
3000
150 100 50 0
2500
2000
3000
1500
2500
1500
2500
1000
2000
1000
2000
1500
1500
500
1000
500
1000
East(m)
500
North(m)
East(m)
500
0
0
North(m)
0
0
3000
3000 Altitude(m)
Path Planning
150 100
East(m) 50
150
East(m)
2
200 150
50 250
300
Altitude(m)
0
250
250 200
50
200
300 250
100
150 100 50 0
2500
3.1
Altitude(m)
100
North(m)
20 10 0
300
150
200
150
Altitude(m)
250
0
300
2500
150 100 50 0
C1CBC Path Smoothing 2000
3000
1500
2000
3000
1500
2500
2000
2500 1000
2000
1000
1500
Let the degree n B´ezier curve with n + 1 control points (P0 , P1 , · · · , Pn ) be defined as [14] 1500
1000
500
1000
0
0
East(m)
500
East(m)
North(m)
500
North(m)
500
0
0
(d) n X P (s) = P B (s) (1) i n,i Fig. 3. Comparison of Path Smoothing Methods : (a) is the piecewise linear (c)
path we want to make smooth. (b) is the path smoothing output of cubic spline i=0 interpolation,(c) is cubic B-spline and (d) is cubic B´ezier curve smoothing method. where the coefficients Bi,n (s) are named Berstein poly-
nomials and are defined as follows: were cubic spline interpolation Two methods we considered n Bn,i (s) = we used si (1 cubic − s)n−i (2) and cubic B-spline. First, spline interpolation i to generate a smooth path. However this path collides with obstacles as it cansegments be seeninina composite Figure 3 (b). As curves our goal The consecutive B´ezier iscan to be reach pointsimply without collision, that visiting madethe C 1 target continuous by arranging the all waypoints is not necessary. Second, we applied approximation first control vertex prior to endpoint of the first curve, methods instead of interpolation. B-spine is used the shared endpoint, and the second vertex of thewidely next to [14]property curvesmooth to be collinear spacedhull . make curve. Itand hasequally the convex which Therefore the first two by control pointsn P be means the B-spine defined the given +0 ,1Pcontrol 1 must points located between W1 convex and Whull the given last two control lies completely in the of the control points. 2 and points P2 , P be located 3 must 2 and 3 as The smooth path of Figure 3 (c) isbetween generatedWby cubicWB-spline method but this path also collides with obstacles. We hope to make the path smooth without any collisions like Figure 3 (d). Rather than smoothing over all waypoints at once, smoothing can be applied piecewise on consecutive triplets of waypoints. Dubins path generates a shortest path between two postures in the plane for a vehicle having limited
by arranging that the first control vertex prior to endpoint of the first curve, the shared endpoint, and the second vertex of the next curve to be collinear and equally spaced [38]. Therefore the first two control points P0 , P1 must be located
25
ke
= 1.31 · he cos(γ − β)
and u1 is a unit vector between P2 and P1 , u2 is that of P3 and P2 and ud is a unit vector between B2 and E2 . 15
k
10
W2
u2
0
g
160
u1
0
0
50
100
150
P1
P2
200
250
P2
P1
F (β) = d c
E1
80
70
θ
h
P0
140
Using a trig ranged as fo
E
5
100
90
(13)
P3
20
120
D Fig. 6. Geometric Interpretation of Cubic B´ezier curves : The complexity of the cure can be reduced if we use geometric property. E 100
60
50
80
P0
P3
2
6c
[−
40 60
E
(0, 0) and P0 , P1 , P2 are aligned3 inB x-axis uafter translation 2 3 = 7.2 and rotation and a geometric property polygon γ β of the control 6d is used, it is possible to reduce the complexity of the curve. W1 W3 u1 B B B1 W2 2 Fig. 6 shows0 a planar cubic B´ ezier curve with 8 degrees 6d freedom, which may be (a) described in just 4 degrees of Figure 2: C 1 Continuous Cubic B´ezier Curve Path ofFigure 2 3: G Continuous Cubic B´ezier Spiral Path freedom according to the three Fig.Smoothing. 5. C 1 Continuous Cubic B´ezier Curve(C1CBC) Smoothing P3 lengths (g, h, k) and a angle Smoothing. (θ). If we substitute these four variables into equation (8), three If β is chose between W1 and W2 and the last two control points P2 , P3 conditions for generating a spiral curve may be found. equation (10 canbebe seen between in Figure The must located W22.and W34ascontrol can be points seen in can Fig. be 5. If we use E [ ] equation (7) which in 0 11 , the pathπ generk is6suggested cos θ g bypoints the following equation: Thedecided 4 control can be decided by the following equation: ≥ 0.58, ≤ (9) and 0 < θ < 2 determined. g ates which satisfies the maximum curvature h a G path h 2 h +4 as follows: constraint. d1d1· u· 1u1 P0P0 == WW d1d·1 u· 1u,1 , PP ,, 2+ 1 1==WW 2 2++ 1.1228 sin β 2+ If these conditions are applied to connect two straight lines, 22 (7) d= hb = the following expression can be obtained κmax · cos2 β whichγ makes a G2 d2d·2 u· 2u2 [1.23 β P2P2 == WW + , P = W + d · u (6) 2 2+ 33 =W 2 2 + 2d2 · 2 , P u2 (3) continuous composite curve. 22 where κmaxP1is a maximum curvature value.P2 B0 hb co he = 2 where u is a unit vector between W and W and u is that 1 1 2 2 F (β) = cos (γ − β) sin β[(7.2364 + 6 cos2 β)Dy where u1 is a unit vector between W1 and W2 and u2 is sin 3.3 Interpolating G2CBS Path Smoothing (b) of that W3 and , andWd1, and is a length W2 and W P0 and of WW3 2and d1 is a between length between 2 2 and −6Dx cos β sin β] + cos2 β sin(γ − β) Therefore, e d2 Pis that of W2 and P . and P . In some it is needed Fig. 7. (a)situations Curvature continuous conditiontotointerpolate connecting twothe lines.way(b) of3 W 0 and d2 is that 2 3 1 [7.2364(D cos γ − D sin γ) + y x G2CBS smoothing resultafter between twoUAV line segments. However, this C continuous curve has a discontinuity of points.pathFor example, the arrives at one target hb curvature at the joints. WeSmoothing want to generate a continuous 3.2 G2CBS Path 6 cos(γ − β)(D β − Dtox sin β)] point the mission planner can request go another tary cos gb curvature curve, a G2 continuous curve. The curvature get point.= In0 this case, the UAV must pass the first However, thenamely C 1 continuous curve has a discontinuity of (10) kb of curvature a planar parametric curveThe is defined as follows [38]: curve target point and then move to the second target point. at the joints. continuous curvature B. Analytical Solution of G2 Continuity Even though it is not required to interpolate between where D = (D , D ) = E − B and β and γ are angles as x y 0 0 which satisfies the maximum curvature constraint can be |P˙ (s) × P¨ (s)| From equ In to Fig. determine the 8point, control (10) starting and target thepoints, targetequation point must be order seenpoint in 7 (a). = obtained usingκ(s) the following two curves as can(7) be can 3 ˙ (s)|spiral generated on | P must be solved. If there are many cusps to smooth, the combe satisfyvariable this requirement, theDinterpolating As passed. the onlyToknown is γ, Dx and seen in Figure 3. y need to be putational time is increased. Our goal is to find the solution G2CBS path smoothing needed. in order to solvealgorithm equation is(10). If we select d1 The first curve consist following control Therefore, the path must satisfyofthe conditionthe of four equation (7) determined equation (10) using numerical methods. out without curvature interpolating path C. Nonholon toofbeTo thefind length between P2continuous and B0 , and d2 as The the length length points at the junction points instead of equation (5). of d is assigned to be the same as d (d = d = d which 1 2 1 2 ), can satisfying constraint, we need to calbetween P2 the and nonholonomic E0 , the solution of equation (10) be does notα change the 4. maximum curvature of dthe which curve. is With In the G2 B0 =2 W2 + d · u1 , B1 = B0 − gb · u1 , culate in Figure Once α is decided, the obtained by the bisection method. Having obtained the solution k1 IV. G C ONTINUOUS PATH S MOOTHING this condition, an analytical solution of equation (10) can be use Theorem distance and W obtain equation equationbetween (10), theQeight control points canusing be determined 2 can be B 2 = B 1 − hb · u 1 , B3 = B2 + kb · ud (4) toobtained. satisfies the A. G2 Continuity Condition (7). construct Using α and interpolating G2CBS pathcurve can which two dcubic B´ezier spiral curves. The first k1 , the The seconda curve consists of curve the following control be obtained. of following the four control points Generating G2 continuous is not asfour simple as consist 1 pointsa C continuous curve. Planar G2 transition curves First, dk1 can be obtained using equation (7), making B0 = P2 + d1 · u1 , B1 = B0 − gb · u1 , are generated, composing of cubic B´ezier spiral segments α E = W + d · u , E = E − g · u , 0 2 2 1 0 e 2 sin B2 = B1 − hb · u1.1228 B (11) [18]. Spiral segments are defined to have no interior curvature 1, 3 =2 B2 + kb · ud d = (8) k1 E 2 = E1 − he · u 2 , E3 = E2 − ke · ud (5) κmax · cos2 α2 30
40
20
20
10
0
0
0
50
100
150
200
250
0
20
40
60
80
100
120
140
160
180
200
300
250
200
150
100
50
0
where hb gb kb
= = =
0.346d, 0.58 · hb , 1.31 · hb cos β,
he = hb , ge = 0.58 · hb , ke = 1.31 · hb cos β
0
50
100
150
200
250
300
From Figure 4, another equation about dk1 can be obtained using the law of sines.
(6)
Here u1 is a unit vector between W2 and W1 , u2 is that of W3 and W2 and ud is a unit vector between B2 and E2 , and d is a length between B0 and W2 , and β = γ2 . The only design variable to generate a G2 path is d as can be seen in equation (4)-(6). If d is selected as
dk1 =
D · sin(α − γ) sin(π − α)
(9)
where D is the distance between W2 and W3 . From equation (8) and (9), the following equation can be obtained. α α −D·κmax ·cos ·sin(α−γ) = 0 (10) 2.2456 sin2 2 2
to interpolate between starting point and target point, the target point must be passed. To satisfy this requirement, the interpolating G2CBS path smoothing algorithm is needed.
W3
250
200
150
D 100
50
γ
α
0 0
50
100
150
200
250
300
350
dk1
400
on the path, n − 1 number of length L and n − 2 number of dκ exist. The larger the dκ , the smoother the path. It is, however, not straightforward to decide the dκ values, since they are affected by each other. Two strategies are applied to solve this problem. The first one is a fixed method and the second is a flexible method. IFone n == 3 First method is to decide the dκ as a half of the smaller 1 dκ = min(L(1), L(2)) length between two adjoint lines. Full length of the lines ELSEIF n > 3 cannot be used because each L(i) must be used twice for FOR k=1:n-2 path smoothing except for the first and last lines. However the IF k==1 first and the last lines are used justone time, full length can L(k+1) be used2if it is smaller than=the half L(k), of the adjoint lines. dκ (k) min 2 Fig. 8 shows the Half Length Algorithm (HLA).
ELSEIF k==n-2
L(k)
450
3 3 dκ (k) = min IF n == 2 , L(k + 1) 1 dκ = min(L(1), L(2)) ELSE Figure 4: Inscribing G2CBS (Left) and Interpolating ELSEIF n > 3 L(k+1) 4FOR k=1:n-2 dκ (k) = min L(k) , 2 2 G2CBS Path Smoothing. Fig. 7. (Right) Inscribing G2CBS (Left) and Interpolating G2CBS (Right) Path Smoothing. IF k==1 END W1
W2
Q
find out continuous interpolating HereToonly α iscurvature the unknown variable. Thispath cansatisfybe obing the nonholonomic constraint, it is needed to calculate α at tained easily by the bisection method. Once α is obFig. 7. Once α is decided, dk1 which is the distance between tained, d can canbebe decided using equation (8) or (9) and Q and Pk1 obtain using Theorem 2. Using these α and s finally G2CBS can be generated using equation (6). dk1 , interpolating G2CBS path can be obtained. Figure 4 shows the inscribing and First, dk1 can obtained using Theorem 2, interpolating IFbe n == 3 G2CBS path. As can be seen, the αinterpolating G2CBS 1 dκ = min(L(1), 1.1228 sinL(2)) 2 dk1 from = (23) path deviates ELSEIF more the collision-free linear path n>3 κmax · cos2 α2 k=1:n-2 than the inscribing FOR G2CBS path. Moreover, the length Fig. 7, another equation about dk1 can be obtained the usinginIF path k==1 ofFrom interpolating G2CBS is 25% larger than L(k+1) the law of sines.2 dκ (k)causes = min aL(k), scribing G2CBS path which waste 2of time and D · sin(α − γ) ELSEIF k==n-2 energy. dk1 = (24) sin(π=−min α) L(k) , L(k + 1) dκ (k) G2CBS Therefore, the3 interpolating path smoothing is 2 where D is thethe distance between P2 andtarget P3 . point and the applied when pathELSE interpolates can be From equation (23) and (24), the following L(k) equation inscribing G2CBS implemented 4 path smoothing dκ (k) = min will ,beL(k+1) 2 2 obtained. αcases. α for the rest of END 2.2456 sin2
2
−D · κmax · cos END
2
· sin(α − γ) = 0 (25)
END Smoothing Algorithm 4HereInteractive only α is unknown variable. This can be obtained
END dκ (k) = min L(k),
2
3
ENDELSEIF k==n-2
4
L(k+1) 2
dκ (k) = min L(k) , L(k + 1) 2 Figure 5: Half Length Algorithm ELSE
dκ (k) = min P2 ENDL(1) L(2) END d (1)
L(k) L(k+1) 2 , 2
P6
L(5)
k
dk(5)
END Fig. 8.
Half Length Algorithm P1
L(3) dk(2)
8
L(6)
P7
P4 L(4) dk(3) dk(4)
However the HLA cannot fully use the margin of the length as can be seen in Fig. 9 (a).P3The thick lineP5represents the dκ and thin line represent the margin of each line. The dκ (1) and dκ (2) could haveP2larger value (a) since there is a remaining P6 margin L(1), L(2), but the HLA fails to use the L(5) margin. L(6) L(1) L(3) L(2) dk(5)
dk(1)
F. Interactive Algorithm
P7
P4
L(4) length systematiSince the HLA can not utilizeL(3) the extra Fig. 9. Half Length Algorithm d (3) d (4) P1 easilyG2CBS by bisection Oncethere α is obtained, dk1 to canmake be cally, a new After path method. smoothing, exists room d (2)to use these extra margins of method is needed decided equationTo(23) or (24) finally G2CBS the path using smoother. utilize theand maximum lengthcanfor the lengths to make the path smoother. To solve this problem, P5 be generated using equation (18). and black line represent the margin of each line.that The adκfeasibility (1) condition of pathP3 is defined in equation (26). path smoothing, dκ is defined as a maximum length 7 shows inscribing and larger interpolating G2CBS path. andthe dκ (2) could have since is a remaining can Fig. be used for path smoothing andvalue dκmax is there the length Lr (i) ≤ L(i)(b) (26) As can be seen, the L interpolating G2CBS path deviates margin the HLA failsmore to use the margin. 1 , L2 , L3 but satisfying the maximum curvature constraint. If there from the collision-free linear path than inscribing G2CBS path. where Fig. 10. (a) Half Length Algorithm. (b) Interactive Algorithm. are n waypoints on of theinterpolating path, n −G2CBS 1 number length Figure 6: (a) Half Length Algorithm. (b) Interactive Moreover, theF.length path of 25% larger L Interactive Algorithm and − 2 number of path dκ which exist. causes The alarger the dκ ,and the Algorithm. Lr (1) = dκmax (1) thanninscribing G2CBS waste of time Since the HLA can not utilize the extra length systematiFirstly, the dκ (k − 1) will be decided. To do this, the smoother path. It is, however, not straightforward energy. thecally, Lr (i) dκmaxby (i − 1) +among dκmax (i) a new method is needed to use these extra margins of minimum value,= defined MIN, the following 4 to decide thethe dκlengths values, since are affected by each Therefore, the interpolating G2CBS smoothing is this ap- problem, to make thethey path path smoother. To solve Ld (b−2) Lr (nare − 1) = dκL(n (n − 2) values calculated: − 1) , d , max κmax (n − 2) + (27) 2 plied when interpolates target point and this theininscribing other. Twothe strategies are applied to solve problem. a path feasibility condition of path is defined equation (26). L (n−3) L (n−2) each line. The d (1) and d (2) could have larger values d d κ dκmax (n and dκmax − 3) + length . If we define Ld−as3)a+κdifference between the(nrequired G2CBS be implemented at the rest of The first path one smoothing is a fixed will method 2 2 Lrand (i) ≤ the L(i) second one is a (26) since there is a remaining margin L(1), L(2), L(3) but L (n−2) cases. L and the real length L, the length of each piecewise linear d r flexible method. If L(n − 1) or d (n − 2) + are the MIN, the k
k
k
κmax the HLA fails to use the margin. 2
where
dκ (n − 2) will be decided as this value. If the minimum value exists among the latter two values, then the dκ (n − 2) is decided among the small value between min((L(n − The Half Length Algorithm to(idecide Lr (i)(HLA) = dκismax − 1) + dthe κ as κmaxd(i) 4.2value Interactive Algorithm 1)), (L(n − 2) − MIN)). half of the smaller length between Lr (n − 1) = two dκmaxadjoint (n − 2) lines. Full (27) Next, the dκ (n − 3) will be decided. The difference of length of the lines cannot be used because each L(i) must If we define Ld as a difference between the required length thisthe step and can the not first utilize step is the that extra L(k) length is replaced by Since HLA systematbe used twiceLfor path smoothing except for the first and L(k) − d (k). Since the d (k − 1) is already decided at r and the real length L, the length of each piecewise linear κ κ max ically, a new method is needed to use these extra margins last lines. However first as and the last lines are used path L isthe expressed following. the previous step, this value must be reflected. This algorithm of the lengths to make the path smoother. To solve this is repeated until dκ (2) is decided. just one time, full length can be used if it is smaller than L(1) = dκmax (1) + Ld (1) problem, a feasibility condition of Since the path defined The final step is to decide the dκ (1). all dκ is values are in the half of the adjoint lines. Figure 5 presents the HLA. L(i) = dκmax (i − 1) + dκmax (i) + Ld (i) equation already (11) decided except for the dκ (1), only L(1) and L(2) − However the HLA cannot fully use the margin of the L(n) = dκmax (n − 1) + Ld (n) (28) dκmax (2) values are compared. Among them, the smaller value length as can be seen in Figure 6 (a). The thick line is decided as a dκ (1). satisfythin the line feasibility conditionthe of margin equation of (26), Ld represents the dToκ and represents The result of thisLalgorithm is described at Fig. 10 (b).(11) r (i) ≤ L(i) must be larger than 0. Here Ld is the margin which can be In Half length algorithm, only L(4) can be utilized without used to make the path smoother. Therefore dκmax (1) can be remaining any margins. However, in Interactive algorithm, all extended with the amount of Ld (1) and dκmax (n − 1) can length margin can be utilized except L(1) and L(5) and L(1) be extended with the amount of Ld (n). However, this is not value is also much utilized than Half length algorithm. always possible since all dκmax are affected by each other. For If there are only 3 waypoints including starting and ending example, even though dκmax (1) can be extended to Ld (1) it point, there is only one cusp. In this case, the dκ is the smaller can violate the constraint of second line feasibility condition. value between L(1) and L(2). If the number of waypoints are If dκmax (1) is larger than L2 −dκmax (2)−Ld (2), it violates the larger than 3, Interactive algorithm is applied to solve this feasibility condition of the second line. Therefore, dκmax (1)
4.1
Half Length Algorithm Lr (1) = dκ
max
(1)
where Lr (1) = dκmax (1) Lr (i) = dκmax (i − 1) + dκmax (i) Lr (n − 1) = dκmax (n − 2)
(12)
If we define Ld as a difference between the required length Lr and the real length L, the length of each piecewise linear path L is expressed as following. L(1) = dκmax (1) + Ld (1) L(i) = dκmax (i − 1) + dκmax (i) + Ld (i) L(n) = dκmax (n − 1) + Ld (n)
(13)
To satisfy the feasibility condition of equation (11), Ld must be larger than 0. Here Ld is the margin which can be used to make the path smoother. Therefore dκmax (1) can be extended with the amount of Ld (1) and dκmax (n− 1) can be extended with the amount of Ld (n). However, this is not always possible since all dκmax are affected by each other. For example, even though dκmax (1) can be extended to Ld (1) it can violate the constraint of second line feasibility condition. If dκmax (1) is larger than L2 − dκmax (2) − Ld (2), it violates the feasibility condition of the second line. Therefore, dκmax (1) has to consider the value of dκmax (2). What complicates the problem is that the dκmax (2) is also related to dκmax (3). This problem can be solved if three consecutive dκmax are considered simultaneously when deciding dκ except for the first and last dκ . This algorithm decides dκ from end point to starting point. Firstly, the dκ (k − 1) will be decided. To do this, the minimum value, defined by MIN, among are calculated: L(n − the following 4 values Ld (b−2) , dκmax (n − 3) + Ld (n−2) 1) , dκmax (n − 2) + 2 2 Ld (n−3) . If L(n − 1) or and dκmax (n − 3) + 2 Ld (n−2) dκmax (n − 2) + are the MIN, the dκ (n−2) will 2 be decided as this value. If the minimum value exists among the latter two values, then the dκ (n − 2) value is decided among the small value between min((L(n − 1)), (L(n − 2) − MIN)). Next, the dκ (n − 3) will be decided. The difference of this step and the first step is that L(k) is replaced by L(k) − dκmax (k). Since the dκ (k − 1) is already decided at the previous step, this value must be reflected. This algorithm is repeated until dκ (2) is decided. The final step is to determine the dκ (1). Since all dκ values are already decided except for dκ (1), only L(1) and L(2) − dκmax (2) values are compared. Among them, the smaller value is decided as dκ (1). The result of this algorithm is described at Figure 6 (b). In the HLA, only L(4) can be fully utilized. However, in the Interactive Algorithm, all length margins can be utilized except L(1) and L(5).
If there are only 3 waypoints including starting and ending point, there is only one cusp. In this case, the dκ is the smaller value between L(1) and L(2). If the number of waypoints are larger than 3, the Interactive Algorithm is applied to solve this problem. This algorithm is shown in Figure 7. IF n == 3 1 dκ = min(L(1), L(2)) ELSEIF n > 3 2 Interactive Algorithm END Figure 7: Interactive Smoothing Algorithm
4.3
Enhancement of Interactive Algorithm
In the Interactive Algorithm, the margin of the length (Ld = L − Lr ) is used evenly. However, it is better to use more length if one angle is sharper than the other to make the path smoother. Therefore, instead of using the half value of the margin, we want to use Ld proportional to the required rate. To do this, the relationship between adjoint dκ must be calculated. The ratio between dk (n) and dk (n−1) can be described as follows: η(k) =
dκmax (k−1) cos2 β(k) sin β(k−1) = dκmax (k) cos2 β(k−1) sin β(k)
(14)
In this algorithm the dκ (k) and dκ (k − 1) will take the margin in proportional to the ratio η(k) such as equation (15).
dκd (k) =
Ld (k) , 1 + η(k)
dκd (k − 1) =
η(k) · Ld (k) (15) 1 + η(k)
where dκd (k) is the margin length which dκ (k) can use. If Ld (k−1) is replaced by dκd (k − 1) and Ld (k−2) by 2 2 dκd (k − 2) in Figure 19, the curvature of the path will be more evenly distributed.
5
Bisection Algorithm
If Ld is smaller than zero in equation (13), it means that the maximum curvature constraint is no longer satisfied. Therefore, dκmax (i) is larger than L(i) − dκmax (i − 1) − Ld (i) which violates the feasibility condition. To fix this problem, two curves are used for path smoothing which are defined by the Bisection Algorithm. Since the angle between two points is larger if we divide it in two, the required length to satisfy the maximum curvature will be reduced. Figure 8 (Left) shows the original G2CBS algorithm. We bisect the length between B1 and B2 since we use the same length variable dκ . This makes the angle α
two curves are used for path smoothing which defined by Bisection algorithm. Since the angle between two points is larger if we divide it to two, the required length to satisfy maximum curvature will be reduced. E
k
b
J
D
E
b
The final s 1.1228 sin α a plane. Therefore, this of waypoints, these 3 waypoints form space using (30) db = 2 αspace plane (in global plane can be mapped from κmaxthe · cos3D frame) on to a 2D plane using a homogenous coordinate Once db is decided, Lb can be decided using the following transformation. I. G2CBS
equation.
J
b
Z
b
db Lb = cos β
P3
uz
uy
(31)
k b
(b) (a) Figure 8: (Left) Original Algorithm. (Right) Bisection Fig. 13. (a) Original Algorithm. (b) Bisection Algorithm Algorithm Fig. 13 (a) shows the original G2CBS algorithm. We bisect a half of β as can be seen at Figure 8 (Right). Therefore, the angle α, β and γ has the following relation.
γ β = (16) 2 4 The first step is to find the db which satisfies the maximum curvature constraint using equation (7). α=
1.1228 sin α (17) κmax · cos2 α Once db is decided, Lb can be decided using the following equation. db =
Lb =
db cos β
(18)
Therefore, if we apply the Bisection Algorithm, the required length for satisfying maximum curvature constraint can be obtained as follows: dbisect = db + Lb
(19)
The Bisection Algorithm reduces the required length satisfying maximum curvature constraint as a rate of (cos β/ cos β2 ). The required length of the original algorithm can be obtained as follows: 1.1228 sin β dκ = (20) κmax · cos2 β The required length of the Bisection Algorithm can be obtained as follows: ! 1.1228 sin β2 1 dbisect = 1 + (21) cos β κmax · cos2 β2 Therefore, the reducing rate can be obtained as follows: 1.1228 sin β 1 2 1 + cos β dbisect κmax ·cos2 β 2 = η = 1.1228 sin β dκ 2 κmax ·cos β ! cos2 β sin β2 1 + cos β cos β = = (22) β 2 cos β cos 2 sin β cos β2 Since 0 < β < π2 , equation (22) is always smaller than 1. It means that by using the Bisection algorithm, the required length can be reduced by the rate of η. For example, if β = π3 , then the η = 0.58. Therefore, the length can be reduced by 42%.
Therefore, if we apply Bisection algorithm, the required length u x P2 for satisfying maximum curvature constraint can be obtained P1 as follows: Local Coordinate X Y dbisect = db + Lb (32) Global Coordinate
Theorem 3. The Bisection algorithm reduces the required Figure Coordinate Transformation : Using coordinate Fig. 14.9: Coordinate Transformation : Using coordinate transformation, the length satisfying maximum curvature constraint as a rate of 3D space can be mapped to 2D plane transformation, the 3D space can be mapped to 2D plane β (cos β/ cos 2 ). The 3D G2CBS path smoothing algorithm can be described
6 in the 3Dfollowing G2CBS steps.Path Smoothing The first step is to find 3 unit vectors (ux , uy , uz ) in the There are two approaches to solve 3D G2CBS path P1 P2 P3 plane. The ux can be obtained by calculating the unit smoothing. The first method is to directly solve the vector between P1 and P2 . problem in 3D space. This is a quite complex problem P2 − P1 and it is not easy to ufind the analytical solution. The (36) x = ||Pwaypoints 2 − P1 || second method is to map 3D to 2D, apply the 2D G2CBS path smoothing method and then map these values back again into 3D. Since the path smoothing algorithm is applied on consecutive triplets of waypoints, these 3 waypoints form a plane. Therefore, this plane can be mapped from the 3D space plane (in global frame) on to a 2D plane using a homogenous coordinate transformation. The 3D G2CBS path smoothing algorithm can be described in the following steps. The first step is to find 3 unit vectors (ux , uy , uz ) in the P1 P2 P3 plane. The ux can be obtained by calculating the unit vector between P1 and P2 .
ux =
P2 − P1 ||P2 − P1 ||
(23)
Since the uz is perpendicular to the P1 P2 P3 plane, it can be obtained by the cross product of two vectors in this plane. 0 uz = ux × uy (24) 0
where uy is the unit vector between P2 and P3 . 0
uy =
P3 − P2 ||P3 − P2 ||
(25)
The uy can be obtained from the cross product between ux and uz . uy = uz × ux (26) Finally the transformation matrix can be obtained, which maps the local (2D) frame to global (3D) frame by translating the P1 to (0,0,0) point of global frame. ux (x) uy (x) uz (x) P1 (x) ux (y) uy (y) uz (y) P1 (y) TM = (27) ux (z) uy (z) uz (z) P1 (z) 0 0 0 1
Fig 15 algorithm w 1) Stage a set of w G2CBS pa 3D space w done by ap 2) Stage (18), G2CB The role o at each cus Algorithm After thi checked. If is applied section Alg the maxim is used to g equation (3 applying eq
11
7
Start Waypoints Map 3D Waypoints to 2D
Stage I 2D Mapping
Interactive Algorithm
Is dK < d
Yes K max
7.2
?
Bisection Algorithm
No G2CBS Path Generation
Stage II dk Decision
Map 2D to 3D
Is there a Collision ?
Yes
No while {collision=1 or d=d k }, d = d + INC
while {collision=0}, d = d - INC
G2CBS Path Generation Map 2D to 3D
END
Fig. 15. 10: Flowchart of G2CBS Smoothing Algorithm Figure Flowchart ofPath G2CBS Path Smoothing Algorithm
3) Stage III - Collision Check: In Stage III, collision check is conducted. there is no acollision, the design d will This matrix If transforms coordinate in thevariable local frame be the increased dκmax to maximum length dκ until collision to globalfrom frame. is detected. This process makes the path smoother and the maximum curvature of the=path be reduced. However, if P3D T Mcan · P2D (28) there is a collision, dκ must be reduced to avoid the collision. Here INC is the predefined value. If there is no collision, the Since the z value is zero in the local plane, it is expressed G2CBS path smoothing algorithm generate the final 3D space by P2D . Therefore the 3D waypoints can be mapped to collision-free smooth path. the 2D plane using the following equation. −1 V. TP RAJECTORY G ENERATION · P3D (29) 2D = T M The G2CBS path smoothing algorithm generates a smooth The step maximum is to applycurvature a 2D G2CBS pathHowever smoothing path second considering constraint. the algorithm to the 3 constraint waypointscan obtained by the equation maximum curvature be violated since the environment is filled with obstacles which explained Stage 3 (29). The final step is to map these values to theatoriginal in Fig 3D 15. space If thereusing is a equation violation of the maximum curvature global (28). constraint, the helicopter can not follow this path exactly and the tracking error will be increased. Therefore to make the path
Stage II - dκ Decision
where V isbethe velocity helicopter(6), in global frame. As can seen fromofequation G2CBS path can be Since the maximum roll angle is specified as ofφmax the obtained if dκ values are decided. The role this, stage allowable velocity can be obtained as following: is to decide the maximum length dκ at each cusp. These p the Interactive Algorithm which values are decided using V = g · R · tan(φmax ) (44) is described in Figures 7 and 19. After this, the minimum radius of curvature constraint In addition to this constraint, the helicopter has the desired is checked. If dκ is smaller than dκmax , the Bisection velocity defined by umax . Algorithm is applied to satisfy the nonholonomic conTherefore there are two velocity constraints which the straint. When the Bisection Algorithm is applied, dbisect helicopter satisfies. is set to dκ to fully utilize the maximum length. If dκ is larger than dκmax , then to generate G2CBS u1 d=κmax V is · nused x path which can be obtained using equation (1). Finally, u2 = umax (45) this G2CBS path is mapped to 3D space applying equation (29) for collision checks. where
7.3
Stage III Collision Check
G2CBS Path Smoothing Process
dynamically feasible, the desired velocity must be generated Figure 10 the shows theofflowchart considering radius curvature.of G2CBS path smoothing algorithm which consist of three stages.for safety reason, Since the maximum roll angle is restricted the maximum velocity of helicopter is also confined. For 7.1 Stage - 2D Mapping coordinated turn Icentripetal force must be compensated by The algorithm begins by receiving set of roll motion. A ‘coordinated turn’ is one in awhich the waypoints resultant offrom gravity centrifugal lies in the aircraft plane path of theand path pruningforce module. Since G2CBS symmetry [39]. The kinematics of the in coordinated as smoothing algorithm is operated 2D plane,turn 3Disspace following: waypoints must be mapped to 2D plane. This is done by applying equation (29). V 2 tan φ = (43) g·R
Tx Stage III - Collision Check nx =
|T | In Stage III, a collision check is conducted. If there is which is the projection of the unit vector of the vector no collision, the design variable d will be tangent increased from ondκhelicopter x directionlength body axis. to maximum dκ until collision is detected. max Finally the reference velocity be selected between the This process makes the path will smoother and the maximum minimum value among thecan these velocities.However, if there curvature of the path be 2reduced. is a collision, dκ must be reduced to avoid the collision. uref = min(u ) there is no collision, (46) 1 , u2If Here INC is the predefined value. the G2CBS path smoothing algorithm generates the final 3D space collision-free smooth path. VI. H ELICOPTER C ONTROL
Simulation A.8Helicopter Model
Results
8.1 Interactive In [40], Mettler et al Algorithm suggested the small-size unmanned helicopter dynamics with first-order differential equations. In this Simulation, the performance of the HLA and the This linear model capturesare thecompared. nonlinear helicopter Interactive algorithm The HLAdynamics is applied very exactly at operating point this modelAlgorithm applied to isthe to the C1CBC path and theand Interactive apreal system successfully [41], It [42]. For possible weed detection and plied to the G2CBS path. is not to apply the crop dusting the helicopter to needs to fly fast. the hover Interactive Algorithm the not C1CBC pathSosince the dκ model is appropriate for in ourC1CBC application which valid up can not be obtained path. Theisresult cantobe 6m/s [43]. seen in Figure 11-14. Figure 11 shows the path pruning A stateand space of helicopter the differential result Figure 12 shows isthegiven pathbysmoothing result. equation. There is seemingly little differences between two paths x˙ = headings Ax + Bu have a large difference. (47) but the curvatures and The curvatures of the C1CBS path are discontinuous at where the junction points while the curvatures of the G2CBS path are continuous throughout the path. x = [u, v, p, q, φ, θ, a, b, w, r, rf b ]T u = [ulon , ulat , ucol , utr ]
The zero curvature implies the margin of the length in Figure 13. The HLA only utilizes the full length of L(2) but Interactive algorithm utilizes the full length of L(2) and L(3). Figure 14 shows the heading change of the C1CBC and G2CBS path smoothing. The heading change of the C1CBC path is very sharp at every junction but its change of the G2CBS path is very smooth. The UAV can not change the heading instantaneously as the heading change of C1CBC path is very sharp. This increases the tracking error because it can not follow this path exactly.
8.2
Bisection Algorithm
Figure 15-18 show the simulation result of the HLA and the Bisection algorithm. The maximum curvature is set as 0.02. The C1CBC path uses the HLA, and the G2CBS path uses both Interactive Algorithm and the Bisection Algorithm. The C1CBC path violates the maximum curvature constraints but the G2CBS path satisfies this constraint. In addition, the combined Interactive and Bisection algorithm utilizes more margin of the lengths which makes the G2CBS path smoother than the C1CBC path. The heading change of the C1CBC path is also very sharp at every junction comparing to its change of the G2CBS path.
9
Conclusions
A collision-free path among obstacles is generated using a RRT algorithm. Since this path consists of piecewise linear segments, a smoothing process is needed in order to be suitable for UAVs which have kinematic and dynamic constraints. A continuous curvature path smoothing algorithm has been developed considering nonholonomic constraints. Since there exists extra room of the length after the path smoothing, this margin can be used to make the path smoother. The suggested Interactive Algorithm can utilize this margin systematically. Finally, the Bisection Algorithm is proposed to alleviate the maximum curvature constraint if there is a violation of this constraint. Simulation Results show the superiority of the combined Interactive Algorithm and Bisection Algorithm..
References [1] J. Langelaan and S. Rock, “Towards Autonomous UAV Flight in Forests”, AIAA Guidance, Navigation, and Control Conference and Exhibit, San Francisco, California, August 2005. [2] N. Vandapel, J. Kuffner, O. Amidi, “Planning 3D Path Networks in Unstructured Environments”, IEEE International Conference on Robotics and Automation, Barcelona, Spain, April 2005.
[3] S. Scherer, S. Singh, L. Chamberlain and S. Saripalli, “Flying Fast and Low Among Obstacles”, IEEE International Conference on Robotics and Automation, Roma, Italy, April 2007. [4] E. Frazzoli, M. Dahleh and E.Feron, “Real-Time Motion Plannig for Agile Autonomous Vehicles”, American Control Conference, Arlington, Virginia, June 2001. [5] J. Saunders, B. Call, A. Curtis, R. Beard and T. McLain, “Static and Dynamic Obstacle Avoidance in Miniature Air Vehicles”, AIAA Infotech@Aerospace, Arlington, Virginia, Sep, 2005. [6] M.Wzorek, P.Doherty, “Reconfigurable Path Planning for an Autonomous Unmanned Aerial Vehicle”, IEEE International Conference on Hybrid Information Technology, 2006. [7] Jayesh N. Aminy, Jovan D. Boskovic and Raman K. Mehra, “A Fast and Efficient Approach to Path Planning for Unmanned Vehicle”, AIAA Guidance, Navigation, and Control Conference and Exhibit, Keystone, Colorado, August 2006. [8] Anderson, E.P. Beard, R.W. McLain, “Real-time dynamic trajectory smoothing for unmanned air vehicles”, IEEE Transactions on Control System Technology, VOL. 13, NO. 3, MAY, 2005. [9] P. R. Chandler, S. Rasumussen, M. Pachter, “UAV cooperative path planning”, AIAA Guidance, Navigation, Control Conference, Denver, CO, Aug. 2000. [10] G. Yang and V. Kapila, “Optimal path planning for unmanned air vehicles with kinematic and tactical constraints”, IEEE Conf. Decision Control, Las Vegas, 2002. [11] Kwangjin Yang and Salah Sukkarieh, “3D Smooth Path Planning for a UAV in Cluttered Natural Environments”, IEEE/RSJ International Conference on Intelligent Robots and Systems, Nice, France, September, 22-26, 2008. [12] D. J. Walton and D. S. Meek, “Curvature extrema of planar parametric polynomial cubic curves ”, Journal of Computational and Applied Mathematics, VOL 134, Issues 1-2, September, 2001 [13] Steven M. LaValle, “Rapidly-exploring random trees: A new tool for path planning”, TR 98-11, Computer Science Dept, Iowa State University, 1998. [14] Richard H.Bartels, John C. Beatty, Brian A. Barsky, An Introduction to Splines for use in Computer Graphics and Geometric Modeling, Morgan Ksufmann Publishers, Los Altos, California; 1986.
2 m(23) Fig. 6. Fig. 6 (a) also shows that there are fourof L and L . This makes the gorithm utilizes more length 1 causes4 a tracking error change of C1CBC is very sharp. This of L2 and Ling . In addition, Interactive al3 . Fig. 6 (b)G2CBS shows the path smoothing result. than can not this path exactly. ore length because of L1 anditpath L Thisfollow makes theC1CBC. 4 . smoother little differences between two paths but the than C1CBC. Fig. 6 (d) shows the heading change of C1CBC and G2CBS ings TheThe zeroheading curvature 1. It are very pathdifferent. smoothing. e heading change of C1CBC and G2CBS change of C1CBC is very sharp of the length Fig. 6 (c). isHalf length algongth at at every junction but its change of G2CBS is very smooth. The heading change of C1CBC very sharp full length of L2 but Interactive algorithm nhe not change the heading instanteously as the heads the change of helicopter G2CBS iscan very smooth. The (23) th of al- This causes a tracking error 2 and 3 . In addition, ingLchange of C1CBC very sharp. ge theLheading instanteously as Interactive theisheadss more length of Lcauses . follow This makes the exactly. 1itand 4tracking because canaLnot this path very sharp. This error her thanpath C1CBC. w this exactly. It heading change of C1CBC and G2CBS s1.the ngth he heading changeFigure of 11: C1CBC is very sharp Path Pruning Figure 15: Path Pruning (a) Path Reducing (b) Path Smoothing (a) Path Reducing nconthe t its change of G2CBS is very smooth. The hing the heading instanteously as the headhange here BC is very sharp. This causes a tracking error cur- this path exactly. ollow 200
200
20 10 0 300
200
150
50
50
North(m)
100
North(m)
100
East(m)
050 0
North(m)
50
0
200
150
50
East(m)
0
0
250
200
100
100
300
250
150
200
200
100
100
20 10 0 300
300
250
250
150
150
C1 G2
Altitude(m)
20 10 0 300
300
250
250
Altitude(m)
20 10 0 300
Altitude(m)
Altitude(m)
C1CBC Path Smoothing G2CBS Path Smoothing
East(m)
0
C1CBC Path Smoothing G2CBS Path Smoothing
[4](bM H a n C Heading of the G2CBS Curvature of the G2CBS He Curvature of the G2CBS g. 5 [5] S t the f over (a) Path Reducing (b) Path Smoothing Figure 12: Path Smoothing (a) Path Reducing (t conFigure 16: Path Smoothing Curvature of the C1CBC Heading of the C1CBC Curvature of the C1CBC ghing (b) Path Smoothing 1 (a) Path Reducing (b) Path Smoothing Heading of the C1CBC Heading of the C1CBC Curvature of the C1CBC (d) Heading (c) Curvature [6](d) T (c) Curvature here curFigure 6. Simulation results of the Half length algorithm and the InteracFigure 7. Simulation results of the Half lengtht Heading of the Curvature G2CBS of the G2CBS Curvature of the G2CBS R g. 5 tive algorithm. tion algorithm. Heading of the G2CBS Heading of the G2CBS Curvature of the G2CBS [7] D t the c over 6 CONCLUSIONS o Path Reducing (b) 17: Path Smoothing cing (b) (a) Path Smoothing Figure Curvature Figure 13: Curvature UAV path has been devel(d of the planner C1CBC Curvature 2 C of the C1CBC of the C1CBC (c)Heading Curvature (d) A Heading (c)Heading Curvature Heading 5.3 (d) Bisection Algorithm RRT(d) algorithm, combining biased sampli [8] T (c) Curvature Figure 7.Heading Simulation results of the Half lengt Figure 6. Simulation results of the Half length algorithm and the InteracFig.algorithm 7 shows resulttension of the Half length Since algo- this path consisa of nodes. ults of the Half length andthe thesimulation InteracFigure 7. Simulation results of the Half length algorithm and the Bisection algorithm. tive algorithm. segments, a smoothing rithm and the Bisection algorithm. The maximum curvature is setprocess is neede Heading of the G2CBS t Curvature of the G2CBS Heading of the G2CBS S tion algorithm. used widely but the curvature of the path2 as 0.02. C1CBC uses the Half length algorithm and G2CBS uses path joints. Since the helicopter both Interactive algorithm and the Bisection algorithm. Even can not immed [9] J 6 CONCLUSIONS re of rightcurvature angular velocity, this has pathbeen can deve not sb though path viloates the maximum 6 C1CBC CONCLUSIONS A UAV constraints, path planner pairs provide transition the G2CBS satisfies this constraint. The heading ofsmooth Figure 18:change Heading A UAV path planner has beenClothoids developed using a modified RRT algorithm, combining biased sampu Figure 14: Heading 5.3 Bisection Algorithm (d) Heading vatures but are undesirable due to comput (c) Curvature orithm C1CBC isalgorithm, very sharp at every junction but its change of G2CBS Heading [10] RRT combining biased sampling and the greedy re nodes. Since this path cons Fig. (d) 7 shows the simulation resulttension of the of Half length algo-exA continuous curvature path smoothi is smooth. simulation result of the Half length algoS of7.Bisection nodes. this of path consists ofcurvature piecewise linear Figure Simulation thesegments, Half length and the Bisec-is need aalgorithm smoothing process esults of therithm Halftension length algorithm andSince theresults Interacand the algorithm. The maximum is set satisfying nonholonomic algorithm. Thesegments, maximum curvature is setprocess isdeveloped T a uses smoothing needed. Dubins is of thecons used widely but the curves curvature path tion algorithm. as 0.02. C1CBC the Half length algorithm and G2CBS uses cis tween two wapoints is the Copyright 2008 byhelicopter ASME he Half7length algorithm and but G2CBS uses [11]in used widely the curvature of the path discontinuous at only the joints. Since the canrequired not imme path both Interactive algorithm and the Bisection algorithm. Even of continuous curvature path. Tht thm and the Bisection algorithm. Even can not eration joints. Since the helicopter immediately accelerate to the right angular velocity, this path can not ure of though C1CBC path viloates the maximum curvature constraints, algorithm ispairs very simple and fasttransitio enoughT oates the maximum constraints, 6 curvature CONCLUSIONS right angular velocity, this path can not be smoothly executed. Clothoids provide smooth 20 250 10 0 200 300
200
150
150 100 250
50
200
200
150
200
250
50
50
150
0
100
0
50
400 150
100
50
00
00
80
East(m)
400
300
350
−0.01
40
−0.02 0 50
50
80
250
300
350
100 100
North(m) North(m)
100 −0.02150 0
150
200
East(m)
50
5050
0 0 0 0
250
300
100100
350
350
400
20 0
Altitude(m)
0
80
50 50
−0.04 100 150 0
300
200 250 250 150 300 200 350 250400 300 50 100
0 0
0.02 50 100
200 250 300 350 400 50 100 150 200 250 300
350
100
20 0
200
250
300
100
0
350
50
North(m)
50 50
−0.02 100 150 0 50
100
200 50
150
50
250
300
350
East(m)
350
400
350
400
400
50
80 60
250 350
300 400
350
400
0 0
0.02
100
0
50
50
100
150
200
250
300
50
100
150
200
250
300
40
350
400
20 0
50
−0.02 0 50 100 150 200 250 300 100 150 200 250 300 350 400
350
400
0 0
50
100
50
100
100
100
100
50 100 150 200 150 200 250 300
0 0
50
0
250 150 300 200 350 250 400 300 100 200
150
400
0
0 0
400
250
400
150
150
40
400
350
200
60
East(m) East(m)
350
−0.04 0 100
100
50
20 10 0 300
−0.02
50
Altitude(m)
0 0
400
200
400
0
40
50
50
0
0.02
60
100
100
250 300 350 400 100 C1CBC 150 Path 200Smoothing 250 300
200
150 150
350
300
250 0
0.04
80
50
100
East(m) 150
100
−0.02
20
250
400
200
200 250 150300 350 400 50 200 100 150 200 250 300
100
0 0
250
400
200
0.02
100
200
0 50
0
50
0
200 50
0
50 East(m)
300
0.04
40
200
0
50
North(m)
100 −0.02 150 0
400100 250 300 100 350
50
250
200
150
350 200
50 0
North(m) 150 North(m)
0
300
100 250150
300
100 150
50
250
200
150
G2CBS Path Smoothing
Altitude(m)
100 0 50 300 −0.02
250
100
20 0
400
300
250
0.02
40
0
20 10 0 300
0.01 100 50 −0.04150 0 0 0.02
60
20 0
0.02 20
Altitude(m)
−0.04 0
200
20 0
250
50
60
0.04
−0.02
300
40
200
East(m)
0
80
250
150
20 0 10 0
20 10 0 300
300
60
C1CBC Path Smoothing G2CBS Path Smoothing
0.02
60
100
100
East(m)
50
10 0
0.02 300
200 200 150 100 −0.04 150250 200 250 0 50 100 200 100
80
150
100
0
−0.01
50
East(m)
50
50 0
0
250
200
100
0.04 20
−0.02 300
300 20
150
0
100
North(m) North(m)
40
300
250
300 100350 200400
100
North(m)
0
−0.02 0
Altitude(m)
250
0.01
m)
20 10 0 300
60
Altitude(m)
200
0
300
Altitude(m)
300
G2CBS Smoothing Heading of the Path C1CBC Curvature of the C1CBC
80
20 0 10 0 300 −0.02 20 10 0 −0.04 3000
250
C1CBC Path Smoothing
of the C1CBC
Altitude(m)
C1CBC Path Smoothing Curvature G2CBS Path Smoothing
Altitude(m) Altitude(m)
0.02
350
400
0 0
FOR IF 1 2 3
k=n-1:2 k==2 temp=[L(k − 1), (L(k) − dκmax (k))]; MIN=min(temp); dκ (k − 1) = MIN;
ELSEIF k==n-1 h 4 temp= L(k), dκmax (k − 1) + 5 MIN=min(temp); 6 7
8 9 10
IF
MIN == L(k) or dκ (k − 1) = MIN;
Ld (k−1) 2
, dκmax (k − 2) +
dκmax (k − 1) +
Ld (k−1) 2
Ld (k−1) 2
, dκmax (k − 2) +
Ld (k−2) 2
i
;
or dκmax (k − 2) + ELSEIF MIN == dκmax (k − 2) + Ld (k−1) 2 dκ (k − 1) = min((L(k)), (L(k − 1) − MIN)); END
Ld (k−2) 2
ELSE h 11 temp= (L(k) − dκmax (k)), dκmax (k − 1) + 12 MIN=min(temp);
13 14 15 16 17
IF
MIN == (L(k) − dκmax (k)) or dκ (k − 1) = MIN;
Ld (k−1) 2
, dκmax (k − 2) +
dκmax (k − 1) +
Ld (k−1) 2
END Figure 19: Interactive Algorithm
, dκmax (k − 2) +
or dκmax (k − 2) + ELSEIF MIN == dκmax (k − 2) + Ld (k−1) 2 dκ (k − 1) = min((L(k) − dκmax (k)), (L(k − 1) − MIN)); END
END
Ld (k−1) 2
Ld (k−2) 2
Ld (k−2) 2
i
;
