Local Incremental Planning for a Car-Like Robot Navigating among Obstacles∗

Alberto Bemporad
Dipartimento di Sistemi e Informatica Universit` di Firenze a Via di S. Marta 3, 50139 Firenze, Italy bemporad@dsi.ing.unifi.it

Alessandro De Luca

Giuseppe Oriolo

Dipartimento di Informatica e Sistemistica Universit` di Roma “La Sapienza” a Via Eudossiana 18, 00184 Roma, Italy {adeluca,oriolo}@giannutri.caspur.it

We present a local approach for planning the motion of a car-like robot navigating among obstacles, suitable for sensor-based implementation. The nonholonomic nature of the robot kinematics is explicitly taken into account. The strategy is to modify the output of a generic local holonomic planner, so as to provide commands that realize the desired motion in a leastsquares sense. A feedback action tends to align the vehicle with the local force field. In order to avoid motion stops away from the desired goal, various force fields are considered and compared by simulation.



Motion planning for mobile robots in the presence of obstacles usually deals with holonomic vehicles, that can move in any direction of the free configuration space [1]. Following a hierarchical approach, the specific kinematics of the robot is not taken into account in the higher level planning phase. With this simplifying assumption, two major approaches can be identified, namely algorithmic and incremental planning. Methods of the first class search for a solution path in the free configuration space, directly facing the combinatorial complexity of the problem. The associated algorithms, although complete (i.e., a solution is found whenever one exists), are often difficult to implement and require a priori knowledge of the environment. Incremental methods are heuristic in nature but can operate in a feedback mode, thus being more suitable for sensor-based navigation through partially unknown environments. With artificial potential field methods, the robot moves under the local effects of repulsive fields associated to obstacles and an attractive
∗ Work supported by EEC BR Project 6546 (PROMotion) and by MURST 40% funds

field pulling toward the goal [2]. The main limitation is the arising of local minima in the total potential field, where no descent direction exists for the motion. Several modifications have been introduced to overcome this problem, such as repulsive fields with elliptic isocontours [3], biharmonic functions [4], navigation functions [5] and numerical potential fields [6]. Another method that can avoid motion stops using only local information is the vortex field method [7]: repulsive actions are replaced by vortical velocity flows so that the robot is forced to turn around the obstacles. On the other hand, wheeled mobile robots are subject to nonholonomic (non-integrable) constraints involving time derivatives of the configuration variables [8]. These constraints depend on the kinematics of the vehicle and limit the local mobility without restricting in the large the accessibility of the whole free configuration space. Due to the nonholonomy, the design of feasible trajectories and of stabilizing control laws to a desired goal configuration is fairly complicated. Open-loop commands that exactly drive the mobile robot to the goal are easily obtained when the kinematic equations can be transformed in the socalled chained form [9], e.g., for a car with N trailers. Moreover, since continuously differentiable feedback schemes cannot stabilize a nonholonomic system to a given configuration [10], more complex discontinuous [11] or smooth time-varying [12] feedback control laws have been proposed. All the above techniques do not consider the presence of obstacles. Methods that solve the planning problem for mobile robots by taking explicitly into account both the nonholonomic constraints and the presence of obstacles have been proposed in [13–16]. In [13, 14], a two-stage approach is proposed for a car-like robot: a complete path avoiding obstacles is generated first with a conventional (holonomic) planner, and then decomposed and approximated with feasible segments complying

We assume pointwise and pure rolling contact between the ground and the wheels. The kinematic equations are x ˙ y ˙ ˙ ℓθ ˙ φ Letting uf = u1 (7) and uφ as in eq. the x-axis. (1) Depending on which wheel is active.r. Let P = (x. Their output is filtered through the vehicle kinematics. eqs. so as to realize the desired cartesian motion in a least-squares sense. 1. of a unicycle with one trailer . by dropping the third equation. and (5) Front-wheel driving.and rear-wheel driving are considered in a unified model. Note that. y) be the position of the front wheel. Rear-wheel driving. proposing a nonholonomic planner for a car-like vehicle among obstacles. denote by uf and ur the velocity of the front and rear wheel. with local information acquired by sensors. equivalently. Moreover. we proposed an algorithm working under these assumptions for a robot with unicycle kinematics. (iii) a priori unknown environment. The coordinates of the two points P and Pr are related by the rigid body constraint x = xr + ℓ cos θ y = yr + ℓ sin θ.yr) θ β P (x. in order to align the vehicle to the local field.with the nonholonomic constraints. In [17]. (6) φ ℓ Pr (xr. A common requirement of this class of methods is the a priori knowledge of the environment. We refer to the car-like robot model depicted in Fig. The kinematic equations can be written in the form xr ˙ yr ˙ ˙ ℓθ ˙ φ = = = = ur cos θ ur sin θ ur tan φ uφ . yr ) the position of the rear wheel. model (6) takes the form (5). independently of which is the driving wheel. and by uφ the steering rate of the front wheel. Suitably defined artificial force fields are used as holonomic planner. (2) Defining two new system inputs u1 and u2 through ur uφ = u1 cos φ u1 sin φ = u2 − ℓ (3) (4) 2 Car-like robot kinematics and using eqs. (1). φ the steering angle. (5) formally represent the kinematic model of a unicycle.t. respectively. We will develop motion control algorithms directly for model (5). (8) . (ii) presence of obstacles. so that the solutions are inherently off-line. front and rear axles are collapsed respectively in a front and rear median wheel. the x-axis. the motion of a wheeled mobile robot is described by the kinematic model Figure 1 – Car-like robot model ˙ X = G(X)u.r. (4). (iv) on-line feedback control solution. reducing the model to that of a bicycle or. As usual. = = = = uf cos(θ + φ) uf sin(θ + φ) uf sin φ uφ . ℓ the distance between the two wheels. Shortest paths of bounded curvature were obtained in [15]. Pr = (xr . We are interested in obtaining a model format that is valid for both types of driving. In this paper we extend the results of [17]. model (2) can be transformed in x ˙ y ˙ ˙ ℓθ ˙ β = = = = u1 cos β u1 sin β u1 sin(β − θ) u2 . There is indeed a lack of work considering all the following aspects together: (i) nonholonomy of the wheeled vehicle kinematics. θ the orientation of the robot w. we distinguish between rear-wheel and front-wheel driving.y) 3 Control law In general. Heuristic rules have been used in [18] for a car-like robot so as to guarantee collision avoidance.t. A feedback scheme is used to define reference values for the remaining free variables. while a discretization of the configuration space is used in [16]. Both front. β = θ + φ the heading angle of the front wheel w.

By eqs. αℓθ.y ) and Fr = (Fr. Analytical forms for the fields will be discussed in the next section. a weighted pseudoinverse can be used.y ) be the force acting on the front and rear wheel. Given any desired smooth trajectory Xd (t) (feasible or not). 2. yd .y =J (X)  Fr. (11) and (14). acting only on the front wheel (i. If the desired velocity Xd is feasible at the current X. J(X) =   0 0 1 0  0 0 0 1 and the feedback law (9) for tracking a desired trajectory Xd = (xd . acting on both wheels. Fr. β Figure 2 – Forces and torque acting on the robot keeping the robot away from the obstacles. βd ) becomes u1 u2 ˙ xd cos β + yd sin β + α2 ℓθd sin φ ˙ ˙ 1 + α2 sin2 φ ˙ = βd . . ±π}. (5) the pseudoinverse of G(X) takes the form G# (X) = cos β 0 1 · 1 + sin2 (β − θ) sin β α sin(β − θ) 0 0 0 1 + α2 sin2 (β − θ) α2 ˙ ˙ This solution locally minimizes the error (Xd − X) ˙ in a least-squares sense. As a result. (13) xd ˙ yd ˙ = kf F ˙ θd = kf M. Defining Xr = (xr .x sin θ − Fr.. We suppose that this field is given by the superposition of an attractive component. θd . In order to balance error components and to handle nonhomogeneous units. Referring to Fig. (9) will result in zero velocity error. the reference point (x. from eqs.x Fr. we shall indicate with F M = Fx Fy r = Ff + F f = F r + F f ˙ When φ = {0.x Fr. = (11) (12) By the virtual work principle. (9) M Fr Ff F where α > 0 is a weighting real number. and θ coordinates. equivalently. from eq.e. ˙ The desired values xd . ˙ ˙ ˙ = Mθ 4 Local trajectory generation Let an artificial force field be defined in the cartesian workspace so as to pull the robot toward the target position while avoiding obstacles. R Equation (8) represents an underactuated mechanical system. the state X can be pre-weighted or. the control input u1 is completely defined by eq. we have ˙ ˙ Xr = J(X)X. respectively. (14) with kf > 0. Consider now the car-like model (5) and define   x  y   X= (10)  αℓθ  . where. (1) and (10).x .. y. yd .y  f. i. let Ff = (Ff.x  Fr  T     Fr. and βd . yr . the Jacobian has the form   1 0 ℓ sin θ 0  0 1 −ℓ cos θ 0  . y)). and θd are selected as the ˙ ˙ natural motion in quasi-static conditions arising from the above force field. the desired θd has no effect on u1 . Ff. eq.y cos θ)  . In order to apply the control law (11–12). a force Fr on the rear r wheel is equivalent to a force Ff on the front wheel and a torque Mθ acting on the vehicle orientation. and of a component aimed at the total force and torque performing work on the x. we need ˙ to specify the desired values for xd .where X ∈ I n is the vector of generalized coordinates.x . a straightforward approach is to design the input command u using the pseudoinverse control law ˙ ˙ u = G# (X)Xd = [GT (X)G(X)]−1 GT (X)Xd . as given by  r      Ff. β). yd .e. with less inputs than generalized coordinates. Note that the pseudoinversion (9) gives the command input u as a feedback law depending on the current state X.y =   Mθ   0   ℓ(Fr. αℓθd . R and u ∈ I m (m < n) is the control input vector. 0 0 Mβ (13) Hereafter.

y) > 0. the force field F (x. and hence the dynamic range of the steering command u2 . we choose the differentiable expression βd = β − arcsin(sin(β − βa )). In the case of rear-wheel driving. To this end. up to a rotation of π rad. drives the car-like robot (5) to G parking the steering angle φ to a desired φg . φ would tend to ± π and the robot 2 would stop. when F = 0.) ∇U (x. β β βd = βa βa βd Then. We overcome this problem by saturating the term arcsin(sin(β − βa )) to ±π/4 in eq. 1 + α2 sin2 φ as a steering control law. ˙ V Recalling eq. y). If both F and M are zero. We have βa = ATAN2(Fy . = . assume a potential function U : I 2 → I is defined with R R the following properties: (1. y) + (β − βd )2 . y) = (xg . Define the Lyapunov-like function 1 V (x. and defining sign(0) = 1.x ). ∇F = ∂Fx ∂x ∂Fy ∂x ∂Fx ∂y ∂Fy ∂y along with the control law (11–12) and the associated definitions. (17). if Ff = Fr = 0. 2 Then.y . ∀(x. We have ˙ ˙ d ˙ F x F y − Fy F x ) (β − βd ) = sign(cos(β − βa ))(β − 2 + F2 dt Fx y and ˙ Fx ˙ Fy = ∇F x ˙ y ˙ . Otherwise. (17) (18) Figure 3 – Choice of βd : βa − β < π/2 (left) and βa − β > π/2 (right) We must still define the steering command u2 in eq. aligning β to βd . the robot tends to move in the direction of the field. y) = (xg . if F = 0 choice (17) may lead to problems when Ff is almost orthogonal to θ. The following theorem gives a stability result for the proposed control scheme in the absence of obstacles and under mild hypotheses on the force field. yg ). y) = 0. we have two possible situations. dt (15) u2 = −kβ (β − βd ). where G = (xg . the robot will rest and we can park the steering wheel at any desired angle φg . we define βd = and β − arcsin(sin(β − βa )). yg ). kβ > 0. we have M = 0 and θd = 0. if (x. Then. ∀(x. 3). we obtain u2=− u1 kβ (β − βd ) −Fy Fx ∇F + sign(cos(β −βa )) ||F ||2 cos β sin β (16) ˙ Since Fr = 0. 3. y. Proof. . In this case. yg ) is the goal position for the representative point (x. the robot is at rest. (5) and (15) ∂U ∂U ˙ ˙ x+ ˙ y + (β − βd )(β − βd ) ˙ ∂x ∂y = −(Fx cos β + Fy sin β)u1 − kβ (β − βd )2 . Theorem 1 In the absence of obstacles (Fr = 0). yg ) = 0 (2. we choose the desired direction βa as the angle formed by vector Ff with the x-axis βa = ATAN2(Ff.) U (x. Therefore. by eqs. Ff. With reference to Fig. y) = (xg .F F Case F = 0. 0. (5). Fx ). y). if (x. y) = −∇U (x. The parameter kβ determines the readiness of the steering subsystem. and hence from eq. (11) and (14) (Fx cos β + Fy sin β)2 ˙ V = −kf − kβ (β − βd )2 ≤ 0. For βd . θ + φg . yg ) (3.) U (xg . In order to let β track βd . one should select βd so as to allow the rotation of the robot. β) = U (x. if Ff = −Fr = 0. so that βd − β is an acute angle (see Fig. If this situation arises with both Ff and Fr being zero. βd will be defined as the angle βa of the direction of the force F . we will determine a desired angle βd such that. Case F = 0. When F = 0 but M = 0. (12). we impose d (β − βd ) + kβ (β − βd ) = 0. yg ). y) = (xg .

y) with η = ρ − r0 and iϑ = (cos ϑ. repulsive. A possible choice for the field is Fvor (ρ. else. (19) or (20) or. (18). In ˙ order to overcome this shortcoming. (20) Figure 4 – Obstacle polar coordinate frame When the obstacles are modeled as circles (e.g. avoiding chattering phenomena. Referring again to Fig. let ϑ0 be the angle formed by vector (xg − x0 . In the presence of obstacles. we describe the fields generated by a single. setting u2 as in eq. y0 ). The first repels the robot in a direction which is orthogonal to the border of the obstacle. (17). else. where i⊥ = (− sin ϑ. let (x0 . sin β). and γ > 1 determines how fast the potential decays away from the obstacle. ϑ where η = η(x. It is convenient to consider a polar coordinate frame (ρ. Both fields described above have some drawbacks. by approximating real shapes acquired by sensors). the desired direction βd becomes undetermined and the tracking defined by eq. The repulsive force becomes then Frep (ρ. 2 (19) y ρ y0 S x0 r0 η0 ϑ G(xg. vortex and circumventive fields.yg) ϑ0 x or with conical profile U (x. 4). simply connected obstacle in the two-dimensional workspace. For the sake of illustration. if η ≤ η0 . or. In fact. If F = 0. while the second may lead the robot to graze the obstacle. this modification did not appreciably change the robot behavior. if η(x. else. even better. Vortex fields. On the other hand. a combination of the two. the result follows. by eq. it is advisable to have a field that is repulsive close to the obstacle and vortical at larger distances. such a field smoothly slows down the robot near the goal. then ˙ β − βd = ±π/2. we remove βd from eq. if η ≤ η0 . y) ≤ η0 . For obstacle avoidance. dominating over the other forces acting on the robot. Consider the unit vectors respectively associated to Frep and Fvor Eϑ ⊥ Eϑ = iϑ = − sign(sin(ϑ − ϑ0 )) i⊥ . y) is the minimum distance of the point from the obstacle. y) = 1 (x − xg . and hence V < 0. ϑ) centered in (x0 . − 1 γ η0 ) . respectively.. the coordinates of the center and the radius of the obstacle (see Fig. ϑ) = 1 (η − 0. Repulsive fields. Theorem 1 applies for example when F is the negated gradient of a potential with paraboloidic profile U (x. sin ϑ). we have to define the way in which the forces acting on the wheels of the car-like robot are generated from the task environment (goal+obstacles). cos ϑ). By LaSalle’s theorem. three kinds of force fields are considered. ϑ)= 1 1 − sign(sin(ϑ−ϑ0 ))( η − η0 )γ−1 i⊥ . and the smooth weighting function η η σ(η) = (1 + )e− ησ . ϑ 0. with ϑ = 0 the direction of the x-axis. y0 ) and r0 be. In the performed simulations. As F → 0. y − yg ) 2 . ϑ Circumventive fields. viz.If F = 0 and F is perpendicular to (cos β. the force Fr acting on the rear wheel may introduce some difficulties. we can either use eq. an attractive force depending linearly on the error norm (paraboloidic potential) becomes very large at large distances from the goal. 5 Artificial force fields In order to complete the design of the motion planner. 4. η0 is the distance of influence of the repulsive field. Although vortex fields can be defined for obstacles of generic shape [7]. then ˙ V = 0 iff β = βd . we describe here a modification for the case of circular obstacles (or for circles enclosing real obstacles). y − yg ) . 0. y) = 1 1 γ ( η(x. For the goal attractive potential. (16)). (15). ησ . yg − y0 ) with the x-axis. The idea is to build a field similar to the velocity field described by the steady flow of a liquid in the presence of a cylindrical obstacle. often keeping it too far from the obstacle itself. y) = (x − xg . y) as [2] Urep (x. 1 γ−1 1 η0 ) η 2 iϑ . iff φ = φg . Hence. An hyperboloidic repulsive potential is defined at each point (x. (15) is impossible with bounded inputs (see also eq.

5 the dynamic range of the inputs. else. The attractive potential is conical outside a circle of unit radius centered at the goal. kf = 1. thanks to its vortical component. vortex (dashed).3505 max |uφ | 5. which are reported in Table 1. the trade-off between traveling time and input effort can be regulated by premultiplying the inputs ur (or uf ) and uφ by the same factor ku . (15) by setting u2 as in eq. the total field is obtained by adding the fields independently defined for each obstacle.0145 28.which is monotonically decreasing from 1 to 0. However. since the robot meets a local minimum of the overall force field. Since the geometric paths are invariant to time-scaling. and paraboloidic inside. and circumventive (solid) fields Figure 7 – Third simulation: results with vortex (left) and circumventive (right) field method 7 6 5 4 3 2 1 0 -1 -1 0 1 2 3 4 5 6 7 . The rate of decay of σ(η) depends on the parameter ησ . (18). In Fig.3648 Table 1 – Comparison of input dynamic ranges in the simulation of Fig.8776 5. In the second simulation (Fig. The region of influence around each obstacle is represented with a circle. 5 we report the results obtained with the three fields proposed in Sect. 6). kβ = 10.75 4. ησ = η0 /10. α = 1. 6 Simulation results We simulated the proposed planner for a car-like robot moving among circular obstacles in a two-dimensional workspace. in comparing these values it is fair to consider Figure 6 – Second simulation: results with repulsive (left) and circumventive (right) field method Figure 5 – First simulation: results with repulsive (dotted). even if the path generated by the repulsive field (dotted line) is more erratic. 7. 5 was slightly shifted to the left. with a repulsive field this time resulted in about 20 s. the rightmost obstacle of Fig. In the simulation shown in Fig.9419 15. The vortex field method fails as the robot touches the obstacle In the presence of multiple obstacles. Instead. The repulsive field method fails in this case. Field Type Repulsive Vortex Circumventive max |ur | 1. ϑ)= ⊥ 1 1 (σ(η)Eϑ +(1−σ(η))Eϑ )( η −η0 )γ−1 if η ≤ η0 . While in the circumventive and the vortex field case the robot reached the goal in about 10 s. the circumventive field successfully drives the robot to the goal. the rightmost obstacle was shifted further to the left. ˙ while βd was removed from eq. 5 for a first simulation. 0. All of them are successful. We call circumventive field the convex combination of a repulsive and a vortical component Fcir (ρ. We have set γ = 4. The integration of the kinematic and planning equations was performed using Simulink and the fifth-order Runge-Kutta method.

1983. and C.” in Differential Geometric Control Theory. 659–665. 12. Latombe. 55–64. Conf.” IEEE Trans. pp. pp. “Asymptotic stability and feedback stabilization.” IEEE Trans. 38. [8] J. and J. Latombe. and Cybernetics. M. CA. [12] C. NC. J. Stifter and J. vol. 1791–1797. Conf. Man. on Robotics and Automation. pp. Krogh. Millmann. 700–716. vol. on Robotics and Automation.-C. the car with N trailers. pp. 224–241. 20. Latombe. [16] J. 1987. 1991. [4] A. 124–129. [7] C.). Conf. Barraquand and J. [18] D. Khatib. Oriolo. Jacobs.” 1991 IEEE Int. [6] J. on Robotics and Automation. [2] O. on Robotics and Automation.-C. An additional command is used to align the steering wheel with the force field. Masoud. on Systems. [5] D. San Diego. on Automatic Control . Dec. a pp. Conf. pp. Future work includes the implementation of this method on a laboratory prototype and its extension to other vehicles. Khosla.-P. Nagoya.-C. on Artificial Intelligence. 1990. G. M. the planner is more suited for navigation through a semi-cluttered environment rather than for parking purposes with limited clearance. J. Bicchi. Boston. 5. on Automatic Control . Anaheim. Therefore. “Nonholonomic motion planning: Steering using sinusoids. The feedback component of the scheme is able to stabilize the robot to a given cartesian position. J. 1992. Man. “Nonholonomic multibody mobile robots: Controllability and motion planning in the presence of obstacles. 1986. A. Wien. [14] J. Koditschek. 1993. Canudas de Wit and O. [17] A. “Satisficing feedback strategies for local navigation of autonomous mobile robots. 1995. CA. R. on Systems. 1349–1354. Brockett.( Fvor → ∞). J. R. M.-C. P. J. Volpe and P.). Langlois. B. “Artificial potential with elliptical isopotential contours for obstacle avoidance. Casalino. 1987. Oriolo. Los Angeles. “Local incremental planning for nonholonomic mobile robots. 1993. Barraquand. pp. 1994. 227–235. of Robotics Research. Springer-Verlag. 1383–1395.” 1994 IEEE Int. 1994. Sussmann (Eds. S. pp.” Rep. [10] R. thanks to its repulsive component. This is avoided when the circumventive field is used. and Cybernetics. Simulation results show that the proposed planner performs satisfactorily in situations where high maneuverability is not essential. Feng and B.” 1995 IEEE Int. “Robot obstacle avoidance using vortex fields. vol. of Robotics Research. Stanford University. [13] J. Santilli. “A motion planner for nonholonomic mobile robots. 10. and H. 577–593. “Nonholonomic motion planning versus controllability via the multibody car system example.-P. “Exponential stabilization of mobile robots with nonholonomic constraints. Murray. pp. Brockett. [15] A. Robot Motion Planning. pp.” IEEE Trans. 1994. “Planning shortest bounded-curvature paths for a class of nonholonomic vehicles among obstacles. Robot Kinematics. STAN-CS-90-1345. vol. Boston. no. S.” IEEE Trans. [9] R. S.” 1987 IEEE Int. in the absence of obstacles. W. and M. R. CA. Lenarˇiˇ cc (Eds. Sastry. on Decision and Control . vol. pp.” in Advances in . The obstacle avoidance action is obtained through a combined use of repulsive and vortex fields. 1991. Murray and S. Kluwer Academic.” Int.g. E. pp. Laumond. 1990.” 28th IEEE Conf. M. Laumond. “A fast path planner for a car-like indoor mobile robot. pp. “Real-time obstacle avoidance for manipulators and mobile robots. Bayoumi. References [1] J. Masoud. Raleigh. “Robot navigation using a pressure generated mechanical stress field—The biharmonic potential approach. “Numerical potential fields techniques for robot path planning. e. pp. CA. 1991. “Time-varying feedback stabilization of car-like wheeled mobile robots. Ta¨ and ıx. on Robotics and Automation. 181–191. E. vol. CA. 180–185. 90–99. vol. S. Conf.” IEEE Trans. A. Conf. Samson. 2328–2335.” 1994 IEEE Int. De Medio and G. [3] R.” 9th Nat. Sørdalen. 1. on Robotics and Automation. [11] C. De Luca and G. 1991. Sacramento. Latombe. 37. CA. W. 1992. Birkh¨user. 1–6.” Int. pp. pp. 22. H. 104–110. “Exact robot navigation by means of potential functions: Some topological considerations. San Diego. 7 Conclusions We have proposed an integrated approach for locally planning the motion of a car-like nonholonomic robot among obstacles.

Sign up to vote on this title
UsefulNot useful