Path Planning of Mobile Robot in Relative Velocity Coordinates

Path planning of mobile robot in dynamic environment is one of the most challenging issues. To be more specific, path planning in multi-obstacle avoidance environment is defined as: given a vehicle A and a target G that are moving, planning a trajectory that will allow the vehicle to catch the target satisfy some specified constrains while avoiding obstacle O, and each of the obstacles can be either mobile or immobile in the environment. The corresponding problem is named target-pursuit and obstacles-avoidance (TPOA) and will be researched extensively in this chapter. The traditional method, such as probability road map, can achieve a successful path in 2D static environments. The planning process using this method generally consists of two phases: a construction and a query phase. In construction stage, the workspace of the robot is sampled randomly for generating candidate waypoints. In the query stage, the waypoints between the start and goal position are connected to be a graph, and the path is obtained by some searching algorithm, such as Dijkstra, A* algorithm and so on. Hraba researched the 3D application of probability road map where A* algorithm is used to find the near-optimal path (Hrabar, 2006). Although probability road map method is provably probabilistically complete (Ladd & Kavraki, 2004), it does not deal with the environment where the information is time-varying. The underlying reason is that this method only focuses on the certain environment. Once some uncertainty appears in the robot workspace, probability road map can not update with the changing environment and plan a valid trajectory for the mobile robot, never an optimal path. Artificial potential field is another traditional method which is generally used in both 2D and 3D environment. The mechanism that the robot is driven by attractive and repulsive force in a cooperative way is simple and often works efficiently even in dynamic environment. Kitamura et al. construct the path planning model based on the artificial potential field in threedimensional space which is described by octree (Kitamura et al, 1995). Traditionally, artificial potential field applies in two dimensions extensively. Also some other field concepts are invented. For example, there are harmonic potential functions (Kim & Khosla, 1992; Fahimi et al, 2009; Cocaud et al, 2008; Zhang & Valavanis, 1997), hydrodynamics (Liu et al, 2007),


Introduction
Path planning of mobile robot in dynamic environment is one of the most challenging issues.To be more specific, path planning in multi-obstacle avoidance environment is defined as: given a vehicle A and a target G that are moving, planning a trajectory that will allow the vehicle to catch the target satisfy some specified constrains while avoiding obstacle O, and each of the obstacles can be either mobile or immobile in the environment.The corresponding problem is named target-pursuit and obstacles-avoidance (TPOA) and will be researched extensively in this chapter.The traditional method, such as probability road map, can achieve a successful path in 2D static environments.The planning process using this method generally consists of two phases: a construction and a query phase.In construction stage, the workspace of the robot is sampled randomly for generating candidate waypoints.In the query stage, the waypoints between the start and goal position are connected to be a graph, and the path is obtained by some searching algorithm, such as Dijkstra, A* algorithm and so on.Hraba researched the 3D application of probability road map where A* algorithm is used to find the near-optimal path (Hrabar, 2006).Although probability road map method is provably probabilistically complete (Ladd & Kavraki, 2004), it does not deal with the environment where the information is time-varying.The underlying reason is that this method only focuses on the certain environment.Once some uncertainty appears in the robot workspace, probability road map can not update with the changing environment and plan a valid trajectory for the mobile robot, never an optimal path.Artificial potential field is another traditional method which is generally used in both 2D and 3D environment.The mechanism that the robot is driven by attractive and repulsive force in a cooperative way is simple and often works efficiently even in dynamic environment.Kitamura et al. construct the path planning model based on the artificial potential field in threedimensional space which is described by octree (Kitamura et al, 1995).Traditionally, artificial potential field applies in two dimensions extensively.Also some other field concepts are invented.For example, there are harmonic potential functions (Kim & Khosla, 1992;Fahimi et al, 2009;Cocaud et al, 2008;Zhang & Valavanis, 1997), hydrodynamics (Liu et al, 2007)

Temporal variable
Table 1.Nomenclatures used in this chapter (see Fig. 1 and Fig. 2).
shown in Section 4.1 and 4.2, respectively.In Section 5, an application about the multiple task assignment (MTA) is used to verify the method proposed in this chapter.Finally, a brief conclusion is drawn in Section 6.

Relative velocity coordinates
RVCs are constructed on the mass point of the vehicle as shown in Fig. 1 and Fig. 2. Fig. 1 shows the relative coordinates when the obstacle-avoiding is considered while Fig. 2 shows the scenario where the target-pursuing is considered.In these figures, the target G and obstacle O, which are 2D or 3D movers, are assumed as circles or spheres having www.intechopen.com Recent Advances in Mobile Robotics 130 certain radiuses which are denoted by R O s.As mentioned above, the relative parameters are measurable or estimable for the current planning time, by using certain on-board sensors.For the convenience of description, the relative velocity and relative position between the vehicle A and the target G are denoted by V AG and L AG .Similarly, parameters about the obstacle O are denoted by V AO and L AO .Some other nomenclatures are listed in Table 1.We define the relative obstacle angle, γ AO , as the angle between V AO and L AO .
Collision cone is defined as an area in which the vehicle will collide with an obstacle by current velocity V AO , and it is denoted by CC AO .Generally, CC AO is in the cone AEF, as shown in Fig. 1.The half cone angle of CC AO , γ AOC , is defined as collision region angle, i.e.
Similar to the definition in the obstacle-avoiding scenario in Fig. 1, the angle between the relative velocity V AG and relative distance L AG is defined as relative target angle, denoted by γ AG 樺[0,π], as shown in Fig. 2. The pursuit cone, as well as the pursuit region angle, is defined in the same as obstacle avoidance scenario.Some assumptions should be declared as the prerequisites of the planning principles.First, in order to guarantee the robot to catch the target successfully, the maximum velocity of the robot is assumed to be larger than that of the target and the obstacles.Due to this precondition, the vehicle A has the ability to catch the target, as well as avoid the collision with obstacles.Second, the target and the obstacles are supposed to keep their speed as constants during the planning period, τ.In fact, this hypothesis is usually accepted because τ is short enough for a real vehicle.Owe to the mechanism of numerical approximation, the path planning problem can be solved and optimized with a receding horizon fashion in  which a new waypoint of the total path is computed gradually over time.Here, we obtain the following equivalence.

Obstacle-avoidance principle
For each obstacle O, under the assumption that its velocity is constant in τ, it will be avoided if the γ AO is large enough to make the V AO out of the CC AO over the interval τ when the vehicle moves from time step k to k+1.This fact suggests that the obstacle-avoiding principle hold the following inequality.That is where γ AOC(k) is the collision region angle in time step k, which is shown in Fig. 1.If there are multi obstacles, Eq. (3) changes to where the subscript i denotes the label of the obstacles.i=1, 2… N. N stands for the number of obstacles.

Target-pursuit principle
The V AG can be resolved into a pair of orthogonal components, as shown in Fig. 3.The vehicle is expected to tune its velocity to the optimum.Only when the velocity direction of the vehicle is identical to L AG , it will not lose its target.In the meanwhile, the vehicle should better minimize the magnitude of V T .Consequently, the policy for the optimal velocity is obvious in that V T should be minimized while V C maximized.We formulate these principles as two cost functions.They are )

Linear programming model
In this section, we first introduce some knowledge about the mathematical description of the standard linear programming model.Then, the mentioned principles for path planning are decomposed and linearized according to the linear prototype of the standard model.

Standard linear programming model
The standard Linear Programming is composed of a cost function and some constrains, all of which need to be affine and linear (Boyd & Vandenberghe, 2004).The following is the standard and inequality form.where D樺R m×n .The vector x樺R n×1 denotes the variables.In the standard linear programming formulation, the vector c樺R n×1 , representing the coefficients, the vector h樺R m×1 , and the matrix D are all known when the model is going to be solved by some method.
During the decades, several methods have been invented to solve the optimization as shown in ( 7).From those methods, simplex algorithm and interior-point method are two most famous algorithms.More information about these algorithms can be found in (Boyd & Vandenberghe, 2004).In this chapter, an open library named Qsopt is used where the primal and dual simplex algorithms are imbedded.For the corresponding procedures, the user can specify the particular variant of the algorithm which is applied (David et al., 2011).

Linear programming model for path planning problem
In this subsection, the problem of obstacle-avoidance is formulated and we obtain some inequality constraints after we introduce two slack variables.Additionally, the formulation of where ω j , ω v1 , ω v2 ≥0.The letter j denotes the subscript of the components of V AG .If the path planning is operated in two-dimensional environment, j belongs to a two-element-set, i.e., j樺{x, y}.And if in three-dimensional environment, j belongs to a three-element-set, i.e., j樺{x, y, z}.The variables d j , q 1 , q 2 will be explained later.

Linearization of the avoidance constrain
From Fig. 1, we get the relative obstacle angle function γ AO .
arccos arccos The definitions of V, L, and P refer to Table 1.Here, Eq. ( 10) is linearized by Taylor's theorem and we obtain the following equation.
( ) Let γ AO(k+1) represent the relative obstacle angle in time step k+1 after vehicle's movement.The variables, ΔV AO , in (11), are that we are trying to plan in step k for step k+1.If there are multiple obstacles, Eq. ( 12) changes to Eq. ( 13).

Minimize the relative distance
In this subsection, the relative distance between the robot and the target-objective is minimized.By tuning the velocity of the robot, the relative distance between the robot and target will became smaller through each step time.This distance is resolved into two or three elements by the axis number.That is to minimize the elements of the vector L AG , i.e., ( ) where l Gj is the element of L AG and v Gj is the element of V AG .See Fig. 2. Δv Gj denotes the acceleration of v Gj .j樺{x, y} is for two-dimensional environment and j樺{x, y, z} is for threedimensional environment.The inherited objective function is derived from Eq. ( 15).
( ) subject to : where Δv Gj is the variable that we hope to compute in the linear programming model, as stated in Eq. ( 2).In a manner of preserving convexity of the problem, d j ≥0 is the newly introduced slack variable associated with each element (Boyd & Vandenberghe, 2004).

Optimize the relative velocity
The relative velocity between the robot and the target-objective needs to optimize on the consideration of target-pursuit principle.We respectively discuss the optimization of the pair component, V T and V C , as shown in Fig. 3.
(1) Minimize the magnitude of the component V T .We compute So the optimization of ( 5) is equal to ( ) min : Using Taylor's theorem and introducing slack variable, q 1 , we get the new formulation for optimization.
( ) where ΔV AG is the variable vector that we hope to include in the linear programming model.q 1 is the slack variable.椛g represents the grades of the function g(.).It is computed with V AGmax is the maximum of the relative velocity between the robot and the target.We estimate it by V AGmax = 2V Amax .V Amax denotes the upper boundary of V A .
(2) Maximize the magnitude of the component V C .
Since we can maximize V C by minimizing -V C , we refer to a minimize problem with affine cost function and constraint functions as a linear programming.Consequently, the problem described by ( 6) can be rewritten as Linearized using Taylor's theorem, Eq. ( 21) changes to a standard linear programming problem.That is 2 T 2 max 2 max min : subject to: where ΔV AG is the variable that we are trying to include in our model.q 2 is also a slack variable.Here, the grades is computed with 椛V C = τ L AG /L AG .

Other constraints
One of the advantages of the LP method is that various constraints can easily be added in the opening constraint set.Here we provide some demonstrations of constraints that are transformed from the dynamics, sensor data, and searching boundaries.

The constraints from the kinematics and dynamics
The kinematics and dynamics are extremely simplified in the path planning model where only the bound of the velocity and acceleration are added in the feasible set.These bound can be described mathematically as max max max max where v Aj denotes the components of V A , as shown in Fig. 1.Δ max denotes the max magnitude of the changing of v Aj in each period τ.

The constraints from the limit of the sensor
All sensors can not detect the area that is beyond their capability.In real application, the obstacle that has been avoided by the vehicle will possibly disappear from the sensor's operating region.Similarly, any new obstacle from long distance is possible entering the detecting area.On this point, we evaluate the threat of each obstacle in the environment and propose a threat factor for each of them to estimate the performance.More specially, if the relative distance between the vehicle and the obstacle O i satisfies L AOi ≥L mini , this www.intechopen.comobstacle is assigned λ i =0.L mini is the threshold.On the contrary, O i is assigned In addition, the relative obstacle angle has impact on the vehicle.If the velocity vector of the vehicle satisfies γ AOi ≥ γ AOCi + Δγ AOimax , the vehicle can be looked as moving from the obstacle O i , λ i will be assigned zero.

The constraints from the searching region
The domain of the variables is generally computed in a square in 2D or a cube in 3D for the interceptive magnitude limit of the acceleration on each axis.In fact, the acceleration on each axis is coupled.So if the domain is treated as a circle in 2D (Richards & How, 2002) or a sphere in 3D, the algorithm will lower its conservation.See Fig. 4. We denote the domain as dom.That is where m, n=0,1,2,…,M-1.Here, M represents the quantity of the plane used for approximation.At this point, the linear programming model is formulized.The objective function is (8).All constrains are recapitulated as the following.

Path planning simulation for TPOA simulation
The approach proposed in this chapter is simulated with three obstacles and the results are given scenarios of in 2D and 3D, respectively.See Fig. 5 and Fig. 6.All the simulations run on the platform of WinXP/Pentium IV 2.53 GHz/2G RAM.A linear programming solver, named QSopt, is called from the library (David et al., 2011).We run the examples with three obstacles and give the results in two-dimensional environment and three-dimensional environment, respectively.

Path planning in 2D
According to the LP model, as shown with Eq. ( 29), all initial parameters are listed in Table 2. Assuming that the maximal velocity of the vehicle in 2D is 50cm/s, and the maximal acceleration of the vehicle is 350cm/s 2 .The panning period is τ=20ms.The three parameters will be kept the same in the following simulation.Fig. 5(a)~(d) show the key scenarios when avoiding the three obstacles.Fig. 5(a) shows the situation when the robot avoiding SO.It is obvious that the robot can rapidly adjust its velocity to the most favorable one and go on pursuing the target.The planner calls LP algorithm about 13 times in this stage.Fig. 5(b) shows the case while the robot has to avoid the moving obstacle MO1.At this time, the robot turns left to avoid MO1 because the over speeding is accessible for the robot.Fig. 5(c) shows the different decision that the robot selected to avoid MO2 from its back.Two conclusions can be drawn from this simulation.First, the robot is indeed qualified the avoiding and pursuing ability in uncertain environment.Second, the robot can adjust its velocity autonomously, including the magnitude and the direction, and adapt the optimal decision to avoid the obstacle and to catch the target.The whole pursuing process of this example lasts about 640ms and each period of calculation of our algorithm takes only about 0.469ms which is a low time complexity for real-time application.

Path planning in 3D
For the specific 3D environment, assuming that there is one static obstacle SO1, and two moving obstacles, MO1 and MO2.All the initial parameters are listed in Table 3.

Path planning for multiple tasks planning (MTP): an application
MTP problem is a variant of multiple TPOA.It can be stated that given N targets and N vehicles, let each vehicle can only pursue one target and each target is pursued exactly by one vehicle at any interval.The task is finished until all the targets have been caught.Then, the goal of MTP problem is to assign the tasks to vehicles within each interval online as well as complete the whole mission as fast as possible.In this section, MTP problem is decomposed into two consecutive models, i.e., the taskassignment model and the path planning model.The path planning model has already been introduced above.With respect to the task-assignment model, a minimax assignment criterion is proposed to direct the task assignment.Under this assignment criterion, the optimal assignment will cost the least time to catch all the targets from the current measurable information.Some simulations in Fig. 7 are given to show the efficiency of the method.Vehicles pursuing target are assigned according to the minimax assignment.According to the criterion, the current task assignment prefers to finish the whole mission fastest (Yang et al., 2009).

Assignment model under minimax assignment criterion
In the assignment problem, the payment in each vehicle for a target is assumed to be known.The problem is how to assign the target for each particular vehicle that the total pursuit mission can be completed as fast as possible.Let x ij be the n 2 0-1 decision variables, where x ij =1 represents vehicle i for target j; otherwise, x ij =0.Because the "payment" is understood as time, it is important to minimize the maximal time expended by vehicle for its pursuing process.This task assignment problem may be described as the minimax assignment model mathematically.where c ij represents the payment in vehicle i for target j and will be given in Section 5.3.The elements c ij give an n 2 binary cost matrix.This is a classic integer programming where the objective function is nonlinear and difficult to solve.A solution method for the above minimax assignment problem named the operations on matrix is proposed.This solution method finds the solution directly from the cost matrix () ni j Cc = and the objective function (Yang et al., 2008).

Global cost function
We give the cost function of this MTP problem as the following.Let N v , N T , and N o be the number of vehicles, targets and obstacles, respectively.The payment of the vehicle-i to pursue the target-j in the assignment problem is written as: where d ij , Δv ij and ΔΦ ij are, respectively, the distance, velocity difference and heading difference between vehicle-i (V-i) and target-j (T-j).ω i is the maximum turning rate of V-i; d kij is the "additional distance" due to the obstacles and the vehicles other than V-i itself (with respect to vehicle-i, not only the obstacles, but also the other vehicles are its obstacles).ξ 1 and ξ 2 are positive constants.the c ij in Eq. ( 33) indicates the time possibly needed by V-i to pursue T-j, and the "extended distance" consists of the linear distance, the angle distance, as well as the possible obstacle avoidance between V-i and T-j (Yang et al., 2009).

Simulation of MTP in 2D
The simulations demonstrated here include three robots, three moving target and three moving obstacles in two-dimensional environment (see Fig. 7).All the initial parameters are listed in Table 4.As Fig. 8 shows, the pair assignment alters between {R1-G3, R2-G2, R3-G1} and {R1-G1, R2-G2, R3-G3} at initial 15 steps according to the minimax assignment computation.At time step 16, assignment is kept for {R1-G3, R2-G2, R3-G1} due to that R3 is the slowest robot after its maximal velocity reducing 50%, and it can only catch G1.R2 is the fastest robot, so it pursues the fastest target G2.When k=35 and k=42, target G2 and G3 are caught by R2 and R1, respectively.Finally, G1 is caught at k=45 by R3, so the mission is completed successfully.The dot-lines are the planned trajectories of the robots and the colorful dots distinguish every step.From the figures we can see that the robots avoid the obstacles and catch all the moving targets successfully.

Conclusion
Path planning is a fundamental problem in robot application.In order to solve the path planning in dynamic environment, this chapter proposes a method based on LP/MILP to plan the acceleration of the robot in relative velocity coordinates.This method has the uniform formulation for 2D and 3D environment and it can give the information of the optimal velocity and acceleration in the view of the special cost function.Multiple constrains, such as the bounds of velocity, acceleration, and sensors, are included in the LP model and the MILP model.We also can add other constrains easily.A particular application of this method is discussed for the problem of the multi-task planning where several robots are set to pursuit several targets.In the classical cooperation problem, the targets are assumed to be dynamic, similar to the TPOA problem.In the solution of MTP problem, a minimax assignment criterion and a global cost function are proposed to direct the task assignment.Many simulations about the path planning in 2D/3D and in the multi-task planning requirements are taken to verify this novel method.The results show the low computing load of this method so it is potential to apply in real time manner.

Fig. 1 .
Fig. 1.Geometrical representation of the relative parameters in the relative coordinates when the obstacle-avoiding problem is considered.(a) and (b) show the definition of relative obstacle angle in 2D scenario and 3D scenario, respectively.

Fig. 2 .
Fig. 2. Geometrical representation of the relative parameters in the relative coordinates when the target-pursuing problem is considered.(a) and (b) show the definition of relative target angle in 2D scenario and 3D scenario, respectively.
(a) Target pursuit in two dimensions (b) Target pursuit in three dimensions

Fig. 3 .
Fig.3.The resolution of V AG .The velocity of the vehicle relative to the target is resolved into two orthogonal components, V C and V T .One component, V C , is in the direction of L AG and pointed to the target.The other, V T , is orthogonal to L AG .
Fig. 4. Searching region for acceleration.(a) The domain of the acceleration will be approximated by multiple lines in 2D.(b) The domain of the acceleration will be approximated by multiple planes in 3D.We try to approximate the dom with multiple lines in 2D, or multiple planes in 3D.That is

Fig. 5 .
Fig. 5. Simulation in dynamic environment of 2D.The dot-line denotes the trajectories of the robot, the obstacles, and the target.And the circles with dot-line edge are the shape profile of any mover.SO represents static obstacle.MO1 and MO2 represent moving obstacles.G represents the moving target.
Fig.6shows the results.At the beginning, SO is on the line between the robot and the target, and the robot is blocked.In the following time, the robot avoids MO1 and MO2 at step=19 and step=24, respectively.See Fig.6(b) and (c).It is evident that the velocity-decision of the robot is optimized online.The time complexities of this simulation are 0.556ms in every period averagely.

Fig. 6 .
Fig. 6.Simulation in dynamic environment of 3D.The dot-line denotes the trajectories, and the sphere denotes the target and the obstacles.

Fig. 7 .Fig. 8 .
Fig. 7. Robots, targets and obstacles.(a) Initial positions at k=0; (b) The target and obstacle trajectories till k=30.The targets and the obstacles are moving in a pso-sine curve.
.com Path Planning of Mobile Robot in Relative Velocity Coordinates 133 the target-pursuit problem is transferred to some cost functions.The final linear programming model consists of a weighted sum of the cost functions and a set of linear constraints.It is

Table 2 .
The initial parameters for simulation in 2D.