Desired Orientation RRT (DO-RRT) for Autonomous Vehicle in NarrowCluttered Spaces

Seho Shin1 ,Joonwoo Ahn1 and Jaeheung Park1,2

Abstract— Autonomous vehicles are actively being developedfrom ADAS(Advanced Driver Assistance Systems) toward fullyautonomous vehicles. Motion planning is one of the mostimportant key technologies for fully autonomous vehicles,especially when they are operated in constrained narrowspace such as parking lot. In this environment, the motionplanning is challenging because it requires many changes inforward and reverse directions and adjustments of positionand orientation. In this paper, an efficient motion planningalgorithm is proposed based on Rapidly-exploring RandomTrees (RRT) by specifying desired orientation during the treeexpansion. A tangential vector space for desired orientationis used to model nonholonomic constraints of a vehicle andgeometric constraints of obstacles. The proposed algorithm hasbeen tested on various situations and its results demonstratedmuch faster performance compared to a nonholonomic RRTalgorithm.

I. INTRODUCTION

The goal of autonomous vehicles is fully autonomous driv-ing in our daily life. A great deal of study in various researchfields have been conducted for the fully automated drivingwithout driver intervention. To achieve fully autonomousdriving not only on a highway but also in various complicatedand unpredictable situations in a city, the safe and efficientpath planning is one of the major issues to be considered.

A motion planning approach can be classified into threetypes: potential field-based, grid-based, and sampling-based.The potential field-based approach identifies a collision-freepath by moving from high- to low-potential energy [1].This approach can thus quickly identify a collision-free path.However, falling into local minima is easy. The grid-basedapproach generates an optimal path by dividing a spacewith grids. A∗ and D∗ are representative algorithms in thiscategory [2], [3]. This approach has been used to identifythe optimal path within the grid space. A disadvantage ofthe grid-based approach is that additional computation isrequired to search the path in a high-dimensional configu-ration. Moreover, adopting a system kinematics or dynamicsmodel is difficult. The sampling-based approach formulatesa roadmap through the node connection in the configurationspace [4]. Its disadvantage is that the search results mayvary. Nevertheless, it is widely employed because it is usefulfor high-dimensional configuration space and easy to reflectconstraint conditions of systems.

1Graduate School of Convergence Science and Tech-nology, Seoul National University, Republic of Koreashinsh,joonwooahn,park73@snu.ac.kr

2Advanced Institutes of Convergence Technology, Seoul National Uni-versity, Republic of Korea park73@snu.ac.kr

The rapidly-exploring random tree (RRT) algorithm isa sampling-based approach [5]. The basic version of theRRT algorithm has been extended to various fields of study.To improve performance, this tree-expand method has beenmodified as follows. RRT-CONNECT connects trees betweenstarting and target points through a simple greedy heuristic[6]. RRT-BLOSSOM adopts a flood-fill-like mechanism intomultiple expansions [7]. T-RRT improves path quality byfollowing valleys or saddle points, and applies a cost mapto expand the tree [8]. To improve the path quality, RRT∗

has been proposed to find the optimal path through treerefinements. With respect to car-like models, kinodynamicRRT, kinodynamic RRT∗ and TP-RRT methods reflect non-holonomic constraints [9], [10], [11]. Otherwise, the CL-RRT and fast RRT methods consider road characteristicsfor autonomous driving of vehicles [12], [13]. However,this method is less useful in the event that the space isvery narrow, or when the accuracy of the positions andorientations is required near obstacles. That is because theprobability is very low that random nodes are located in anarrow region with the desired position of the vehicle.

In this study, we propose a method to conduct effectivepath planning in a narrow space. In particular, the parkingpath in a narrow target space is a challenging issue, requir-ing several forward and backward vehicle movements. Toovercome this limitation, the desired orientation RRT (DO-RRT) is proposed. This algorithm models nonholonomic con-straints of the vehicle and geometric constraints of obstacleson the tangential vector space and extends a tree using thistangential vector as a desired orientation of random samples.

The study is organized as follows. Section 2 provides apreliminary definition of the path planning algorithm anddescribes the problem space. Section 3 explains the proposedDO-RRT algorithm. Section 4 describes the method fordetermining a desired orientation based on a magnetic-field-based model and Section 5 introduces the proposed extendfunction for the DO-RRT. In Section 6, the effectiveness ofthe proposed algorithm is demonstrated by the simulationresults and also by experimental results in the real parkingenvironment. Section 7 concludes the study.

II. PRELIMINARY DEFINITION

A. Problem Statements

The configuration and control spaces for the path-planproblems are defined as Q ⊂ Rm and U ⊂ Rn respectively.A nonholonomic vehicle system can be expressed with the

2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)Daejeon Convention CenterOctober 9-14, 2016, Daejeon, Korea

978-1-5090-3761-2/16/$31.00 ©2016 IEEE 4736

following differential equation.

q(t) = f(q(t), u(t)), q(0) = qinit (1)

where q(t) ∈ Q and u(t) ∈ U are states and inputs of thevehicle respectively; qinit ∈ Q is the initial condition, wherem and n are the dimension of the initial conditions statesand inputs, respectively; and f is the function of vehicle’skinematics or dynamics. In the configuration space, Qobs ⊂Q is the space with obstacles and Qfree ⊂ Q\Qobs is definedas a free space without obstacles. A planning problem canbe formulated as follows.

q(t) ∈ Qfree,∀t ∈ [0, tf ]

q(tf ) = qgoal, (2)u(t) ∈ U,∀t ∈ [0, tf ]

where qgoal is a target position and orientation, and tf is thetime to reach the goal (qgoal).

B. Autonomous Vehicle Model

In this study, rear-wheel driving model is considered as avehicle’s kinematics[14]. The model is expressed as follows.

xy

θ

δ

=

cos(θ)sin(θ)

tan(δ)/L0

−→u 1 +

0001

−→u 2, (3)

where x and y indicate vehicle’s position, θ is a vehicle’sheading angle, L represents the distance between the frontand rear axles of the vehicle, and δ denotes the steeringangle. Control inputs u1 and u2 are the longitudinal andangular velocities of the steering wheel, respectively.

III. ALGORITHM DESCRIPTION

In this section, we describe the DO-RRT algorithm afterreviewing the basic version of the RRT algorithm and theproblems that arise in a narrow area such as a parking space.

A. RRT Algorithm and limitation

The RRT algorithm expand the tree until it arrives at thetarget position. Here, a vehicles initial and target positions aswell as the position of obstacles are all given. A random node(qrand) is selected at each iteration of the algorithm on theconfiguration space. Then, qnear becomes the nearest nodeto qrand among the nodes on a tree topology. The selectedqnear is then expanded toward the qrand. This expandingprocedure can be modeled in a variety of forms depending onvehicle kinematics and environment constraints. Therefore, acontrol input and qnew to be added to the tree are determined.In the extension procedure for a nonholonomic system, aset of candidate paths is generated by considering vehicledynamics or kinematics. An appropriate candidate (qnew) isthen selected by using the distance metric to expand fromqnear to qrand. Finally, qnew is added to the tree. The majoradvantage of this algorithm is its usefulness in consideringsystem characteristics. For this reason, vehicle kinematicsand environment model have adopted the RRT algorithmto identify a collision-free path. However, this method is

(a)

(b)

Fig. 1. Illustration of conventional RRT algorithm problems in constrainednarrow spaces. (a) the new node cannot be expanded anymore because ofobstacles. (b) the new node cannot be converged to the goal position.

(a)

(b)

Fig. 2. Illustration of DO-RRT algorithm in constrained narrow spaces. Therandom node is expanded using a desired orientation vector. (a) The direc-tion of random node is generated to avoid collision with obstacles. (b) Thedesired orientation vector has been determined considering nonholonomicconstraints of the vehicle.

less useful in the event that the space is very narrow fora car to pass, or when the accuracy of the positions andorientations is required. This is because the probability is toolow that random nodes are located in a narrow passagewayand connected with the desired vehicle posture.

Fig. 1 illustrates these problems in the case of a non-holonomic vehicle system. Fig. 1(a) is a schematizationof nodes that cannot be extended because of surroundingobstacles. The left portion of the picture shows the positionof qnew reached during the extending process after qrandand qnear are determined. After a tree is extended fromqnear to qnew, a new random sample node q′rand is chosenin the next iteration. However, this node cannot extend

4737

to candidate nodes because of obstacles. This can causeconsiderable computation time and memory usage. Fig. 1(b)presents the problem that can occur in the case of forwardparking, when the desired target position and heading angleare both given. Here, the left portion of Fig. 1(b) shows theprocess of selecting qnew when considering the relationshipbetween qnear and qrand. However, the orientation of qnewis not suitable for converging on the target position becauseof the nonholonomic constraints. This can produce a paththat requires several repetitions of forward and backwardmovements in order to reach the target position. To solvethis problem, a novel method assigns a desired orientationfor random samples that considers nonholonomic constraintsand obstacle positions, as shown in Fig. 2. Our proposedalgorithm is described in detail in the following subsections.

B. Overview of DO-RRT Algorithm

The DO-RRT algorithm models nonholonomic constraintsof the vehicle and geometric constraints of obstacles onthe tangential vector space and extends a tree to considerthe desired orientations of random samples. Pseudo code isshown in Algorithm 1. This algorithm has four main phases:generating the random node (RandomSampling), finding anearest node (FindNearestNeighbor), assigning the desiredorientation of a random node (GetDesiredOrientation), andextending the tree (DO Extend). The third and fourth phasesrepresent contributions of the proposed algorithm. The thirdphase determines the desired orientation of a random nodeusing a magnetic-field-based model. The fourth phase gen-erates trajectories by considering both the orientation of thenear node and the desired orientation of the random node.Further details are given in the next section.

function DO RRT (qinit,qgoal,Qobs)InitializeTree(τ ,qinit)for i = 1 to N do

qrand←RandomSampling()qnear←FindNearestNeighbor(τ ,qinit)q′rand,isValid←

GetDesiredOrientation(qrand,xnear,qgoal,Qobs)if isValid = false then

continueendQnew← DO Extend(qnear,q′rand)if CollisionFree(Qnew) then

τ ← InsertNodes(τ ,Qnew)if qgoal∈Qnew then

return ExtractTrajectory(τ )end

endendreturn failure

Algorithm 1: Pseudo-code of the DO-RRT algorithm

(a)

(b)

Fig. 3. Illustration of the relationship between magnetic field lines of twomagnets and vehicle paths. (a) Direction of fields has similar charateristicsto the shape of Dubins path. (b) Both forward and backward paths can bedetermined using magnetic field lines

IV. MAGNETIC-FIELD-BASED DESIREDORIENTATION MODEL

The magnetic field is the space in which a magnets forcecan be detected. This field can be described using vectorsthat indicate the magnitude and direction of magnetic forces.The shape of a magnetic dipole field is similar to that ofnonholonomic constraints of the vehicle model. A dipole-like potential energy model is proposed for nonholonomicrobots [15]. Another characteristic of magnetic fields exists:the direction of magnetic fields is from the N pole of onemagnet to the S pole of the other magnet when dipolemoments of two magnetics occur within an influence space.The left portion of Fig. 3 shows the shape of two magneticdipole fields (using arrows). The direction of fields is shownfrom the N pole of the lower magnet to the S pole of theupper magnet. This is similar to the shape of Dubins path,which is formed by connecting tangential circular arcs and astraight line [16]. By utilizing this characteristic, a vehiclesorientation can be determined to consider its nonholonomicconstraint of both forward and backward motions. This canbe applied equally in a situation in which an initial positionfaces the target position. In this section, the magnet-field-based model similar to the nonholonomic constraints ofthe vehicle is proposed to determine desired orientations ofrandom nodes for the RRT algorithm.

A. Magnet-like Field Model

Magnetic fields can be modeled by utilizing the Biot-Savart law [17]. This model provides the magnitude of amagnetic field in terms of the electric current. When thecurrents run in opposite directions on two straight wires, themagnetic dipole vector field can be modeled by applying thesuperposition principle of magnetic fields as follows.

−→B (q) = K

∫c

−→dl

|−→r |2×−→r|−→r |2

(4)

where−→dl refers to the current-carrying segment and −→r

indicates the distance vector between the current-carrying

4738

segment and the position q. K is the magnetic constant.The similarity between the magnetic vector field and non-holonomic constraints of bicycle models can be verified asfollows. Let us define the current-carrying segment vector−→dl parallel to the Z axis, and d as the distance betweentwo wires. Here, locations of two wires crossing on thexy surface are derived as (−d

2 sin(θ)i +d2 cos(θ)j) and

(d2 sin(θ)i −d2 cos(θ)j), respectively. Then

−→B (q) can be

expressed as K ‖−→dl‖d

2|−→r |2 (cos(θ)i + sin(θ)j)|c on q = (0, 0, 0).The bicycle constraint matrix can also be expressed as:

AT =

(sin(θ + δ) − cos(θ + δ) −L cos(δ) 0sin(θ + δ) − cos(θ) 0 0

). (5)

This matrix satisfy AT (q) · q = 0 as Pfaffian constraint.When q term is substituted into K ‖

−→dl‖d

2|−→r |2 (cos(θ)i+sin(θ)j)|c,Pfaffian constraint is satisfied. Thus, the magnetic vector fieldand nonholonomic constraints of bicycle models have similarcharacteristics. Using this relationship between the magneticvector field and nonholonomic constraints, a magnetic-field-based model for the initial and target positions of the vehicleis derived as follows.

−→Vm(q) = km

[∫cs

( −−→dls1|−→rs |2

×−→r|−→r |2

+

−−→dls2|−→rs |2

×−→r|−→r |2

)+

∫cg

( −−→dlg1|−→rg |2

×−→r|−→r |2

+

−−→dlg2|−→rg |2

×−→r|−→r |2

)] (6)

where −→rs is the directional vector between the current-carrying segment and the vehicles start position and −→rg is thetarget position. Here, the distance between two wires is equalto the width of a vehicle. For the convergence at the initial ortarget position of the vehicle, desired orientation vectors arecalculated to satisfy the nonholonomic constraint. Moreover,the desired orientation vector between two positions can becomputed to reach the other position based on the forwardor backward movements of the vehicle.

B. DO(Desired Orientation) Model

In the DO-RRT Algorithm, the desired orientation of avehicle at the qrand, as explained later, can be determinedby utilizing the magnetic-field-based model on qnear andqgoal. However, adopting the magnetic-field-based model isdifficult when the distance between qnear and qgoal is toofar to influence magnetic forces. In addition, the directionof magnetic field lines may not be valid depending onthe presence of obstacles. For these reasons, the desiredorientation can be modeled as the sum of three type ofvectors as follows.−→v = km

−→Vm(q) + kr

−→Vr(q) + ko

−→Vo(q), ‖

−→Vm(q)‖ < ε (7)

where−→Vr is the random direction vector weighted to the

direction of the target position and−→Vo is the direction of the

repulsive force from the obstacle. The value of kr gain isdetermined by considering the distance between qnode andqnear, or qgoal. The km and the ko are set to the magnitudeof the magnetic and obstacles’ potentials, respectively. In

case of a long distance of qrand from qnear and qgoal, krgain is set to be greatly affected by the random direction.The desired orientation vector is valid when the magnitudeof−→Vm(q) is less than the threshold value (ε). To avoid

selecting an invalid random node, the value of ε is assignedby considering the correlation between the magnitude of−→Vm(q) and a minimum turning radius.

V. EXTEND FUNCTION OF DO-RRT

The proposed extend function of DO-RRT generates tra-jectories using the desired orientation and position of qnearand qrand. First, candidate trajectories are calculated for theset of velocities for forward and backward driving. For theset of determined velocities, the steering angle is computedto converge to the target position using the SteeringControllerfunction. Using this steering angle, a ForwardSimulationfunction calculates the estimated vehicle positions after Ts. This process is repeated N times until a candidate trajec-tory (Qcand) is generated which then converges to a targetposition. These candidate trajectories with respect to the setof velocities are compared to select a best path (Qnew) usingthe cost function. This cost function can be derived using theposition and orientation error between qnew and qrand andnumber of changes in the direction.

function DO Extend(qnear, qrand)for i = 1 to Size(V) do

vd = V(i)q0 ← qnearfor j = 1 to N do

δj ← SteeringController(q0,qrand,vd)Qcand ← ForwardSimulation(Qcand,q0,δj ,vd,T)

endQnew ← CostFuction(Qcand)

endreturn Qnew

Algorithm 2: Pseudo-code of the expand function forthe DO-RRT algorithm

VI. EXPERIMENTAL RESULTS

In this section, the DO-RRT algorithm is analyzed ina narrow parking space using simulation and autonomousvehicle test.

A. Experimental Condition

The algorithm proposed in this study is developed forpath planning of an autonomous driving system. Therefore,parameters for the path planning were set to match thespecifications of an autonomous driving system in our labo-ratory. The vehicle length and width are 2845 and 1614 mm,respectively. The distance from the center of the vehicle tothe real wheel center is 1100 mm and to the front wheelcenter is 1745 mm. The maximum wheel angle is 35.0◦. Thefinal condition for the position and orientation error were setto within 0.300 m and 0.174 rad, respectively.

4739

Fig. 4. The environments of four parking cases and example solutions found with the DO-RRT and Nonholonomic RRT (NH-RRT).

B. Simulation Test Results

To compare the performances of the DO-RRT, experimentsinvolving forward, backward, and parallel parking were con-ducted. These parking simulations were performed in narrowspaces that required driving in both forward and reverse. Inaddition, the experiment was conducted to generate a pathfor the environment without obstacles. For comparison ofthe proposed algorithm, we also applied a standard RRT-based nonholonomic planner within simulations. Here, thisnonholonomic planner can be modified according to thetype of robot (kinematic model, kinematics and dynamicsconstraints, motion direction, etc.) and workspace (dimensionof configuration space, shape of obstacles, etc.). To generatea parking path in the narrow space, a nonholonomic versionof the RRT algorithm was implemented to allow forward andbackward movements based on car-like robots. Both the DO-RRT and nonholonomic RRT adopted a goal-biased samplingmethod and avoided unnecessary nodes through regressionsteps. Moreover, both algorithms used a uniform distancemetric function to identify nodes for expansion. This distancemetric function includes position and direction errors, pathlength, and number of changes in the direction of motion(forward or reverse). Fig. 4 shows details of the experiments.The proposed DO-RRT algorithm was compared to thenonholonomic RRT method using the iteration count andpath length to reach the target position. These performanceswere measured using the average and standard deviation of100 trials. Based on the paths generated from Cases 1−4,the number of iterations of the DO-RRT proved to be muchfewer than those of the nonholonomic-RRT. The path lengthusing the DO-RRT was also shorter than the length using thenonholonomic-RRT. The path from the nonholonomic RRThas many direction changes to reduce the heading error in

TABLE ICOMPARISON RESULTS BETWEEN DO-RRT AND CONVENTIONAL

NONHOLONOMIC RRT

# of iteration Length(m)Average Std.Dev Average Std.Dev

CASE1 DO-RRT 31.328 14.190 49.925 5.919NH-RRT 131.500 63.986 54.827 3.250

CASE2 DO-RRT 23.289 9.249 49.768 4.739NH-RRT 298.637 191.035 61.795 13.574

CASE3 DO-RRT 44.908 53.033 48.066 10.606NH-RRT 367.400 275.129 73.018 7.611

CASE4 DO-RRT 7.666 2.546 41.166 3.562NH-RRT 95.139 64.972 42.693 7.532

∗DO-RRT: Desired Orientation RRT, ∗NH-RRT: Nonholonomic RRT

the narrow region. For this reason, the length of the path wasincreased. Overall, the performance was improved by 9.402times fewer iteration on average in four cases, and the pathlength is decreased about 0.834 times.

C. Vehicle Test Results

Experiments were conducted to generate a parking pathusing the autonomous vehicle system shown in Fig. 5. Theenvironmental information was collected from the 3D lidar(Velodyne lidar). The ego vehicle position and heading anglewere estimated by the simultaneous localization and mapping(SLAM) based on Velodyne and inertial measurement unit(IMU) data. The test procedure is described as follows.When an available parking space was identified by a user, anautonomous navigation system drove the car to that parkingspace by following waypoints. When the vehicle reaches theparking space, a parking path is computed from the currentposition of the vehicle to the parking spot using the DO-RRT algorithm. Experimental results are shown in Fig. 6.The upper figure shows the point cloud map obtained by the

4740

(a)

(b)

Fig. 5. The experimental autonomous vehicle system : (a) SPIRIT-1, (b)Specification of sensors

Velodyne sensor in the parking lot where the experiment wasconducted. The target position of the vehicle required back-ward parking. Experiments were conducted from two initialpositions. In the first test, each of forward and backwardmovement were performed without collision with obstacles.The second situation was difficult to conduct directly becauseof other parked vehicles. In this case, the parking path forthe backward parking was generated using two backward andone forward movements.

VII. CONCLUSIONThis paper presents the DO-RRT method for effective

path planning of an autonomous vehicle driving in a narrowspace, which requires many changes in forward and reversedirections. The magnetic-field-based model is proposed todetermine the preferred direction of a vehicle, based onnonholonomic constraints of the vehicle and geometric con-straints of obstacles. Algorithmic performance was verifiedthrough simulation and experiments on an autonomous ve-hicle in narrow parking spaces. Our method can benefitboth the number of iterations and the planned path lengthcompared to the conventional nonholonomic RRT algorithm.In particular, while the number of iterations decreases about9 times, it significantly improved the performance of the pathplanning. In a future study, we will consider the extensionof this desired orientation model to RRT∗. The rewiringprocedure in RRT∗ would be more efficient by using theconcept of preferred direction of vehicles.

REFERENCES

[1] O. Khatib, “Real-time obstacle avoidance for manipulators and mobilerobots,” The International Journal of Robotics Resaech, vol. 5, no. 1,pp. 90–98, 1986.

[2] P. E. Hart, N. J. Nilsson, and B. Raphael, “A formal basis for theheuristic determination of minimum cost paths,” IEEE Transactionson Systems Science and Cybernetics, vol. 4, no. 2, pp. 100–107, 1968.

Fig. 6. Results of trajectories generated by DO-RRT algorithm in a standardparking lot.

[3] A. Stentz, “Optimal and efficient path planning for unknown anddynamic environments,” DTIC Document, Tech. Rep., 1993.

[4] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Prob-abilistic roadmaps for path planning in high-dimensional configurationspaces,” IEEE Transactions on Robotics and Automation, vol. 12,no. 4, pp. 566–580, 1996.

[5] S. M. Lavalle, “Rapidly-exploring random trees: A new tool for pathplanning,” Computer Science Dept., Iowa State University, Tech. Rep.,1998.

[6] J. J. Kuffner and S. M. LaValle, “Rrt-connect: An efficient approach tosingle-query path planning,” in 2000 IEEE International Conferenceon Robotics and Automation (ICRA), vol. 2. IEEE, 2000, pp. 995–1001.

[7] M. Kalisiak and M. van de Panne, “Rrt-blossom: Rrt with a local flood-fill behavior.” in 2006 IEEE International Conference on Robotics andAutomation (ICRA). IEEE, 2006, pp. 1237–1242.

[8] L. Jaillet, J. Cortes, and T. Simeon, “Transition-based rrt for pathplanning in continuous cost spaces,” in 2008 IEEE/RSJ InternationalConference on Intelligent Robots and Systems (IROS). IEEE, 2008,pp. 2145–2150.

[9] S. M. LaValle and J. J. Kuffner, “Randomized kinodynamic planning,”The International Journal of Robotics Research, vol. 20, no. 5, pp.378–400, 2001.

[10] D. J. Webb and J. van den Berg, “Kinodynamic rrt*: Asymptoticallyoptimal motion planning for robots with linear dynamics,” in 2013IEEE International Conference on Robotics and Automation (ICRA).IEEE, 2013, pp. 5054–5061.

[11] J. L. Blanco, M. Bellone, and A. Gimenez-Fernandez, “Tp-spacerrt-kinematic path planning of non-holonomic any-shape vehicles,”International Journal of Advanced Robotic Systems, vol. 12, 2015.

[12] Y. Kuwata, S. Karaman, J. Teo, E. Frazzoli, J. P. How, and G. Fiore,“Real-time motion planning with applications to autonomous urbandriving,” IEEE Transactions on Control Systems Technology, vol. 17,no. 5, pp. 1105–1118, 2009.

[13] L. Ma, J. Xue, K. Kawabata, J. Zhu, C. Ma, and N. Zheng, “Efficientsampling-based motion planning for on-road autonomous driving,”IEEE Transactions on Intelligent Transportation Systems, vol. 16,no. 4, pp. 1961–1976, 2015.

[14] A. De Luca, G. Oriolo, and C. Samson, “Feedback control of anonholonomic car-like robot,” in Robot Motion Planning and Control.Springer, 1998, pp. 171–253.

[15] H. G. Tanner and K. J. Kyriakopoulos, “Nonholonomic motion plan-ning for mobile manipulators,” in 2000 IEEE International Conferenceon Robotics and Automation (ICRA), vol. 2. IEEE, 2000, pp. 1233–1238.

[16] L. E. Dubins, “On curves of minimal length with a constraint onaverage curvature, and with prescribed initial and terminal positionsand tangents,” American Journal of mathematics, vol. 79, no. 3, pp.497–516, 1957.

[17] D. J. Griffiths and R. College, Introduction to electrodynamics. pren-tice Hall Upper Saddle River, NJ, 1999, vol. 3.

4741