Velocity obstacles for car-like mobile robots: Determination of colliding velocity and curvature pairs

A R T I C L E I N F O A B S T R A C T Article history: Received: 29 November, 2017 Accepted: 15 January, 2018 Online: 30 January, 2018 This paper addresses the motion planning problem of Reeds-Shepp-type car-like mobile robots moving among static and dynamic obstacles. If the positions and the velocity vectors of the obstacles are known or well estimated, the Velocity Obstacles (VO) method and its non-linear version (NLVO) can be used to plan a collision-free trajectory for a robot in the dynamic environment. VO and NLVO algorithms determine a velocity vector for the robot which corresponds not necessarily to the orientation of the robot, hence a nonholonomic car-like mobile robot cannot apply it exactly. Previously, the NLVO method was adopted for Dubins-like mobile robots, which are able to move forward only. In this paper, a method similar to NLVO is presented, but it results motions feasible for Reeds-Shepp-type robots, which are able to drive both forward and backward. Longitudinal velocities and curvatures of turning circles are calculated, which ensure collision-free motion if the arbitrary movement of the obstacles are known for some time-horizon.


Introduction
This paper is an extension of work originally presented in the 25th Mediterranean Conference on Control and Automation [1].
One of the main tasks of autonomous mobile robots is to execute a safe motion in their workspace from the actual position to a desired goal configuration. In some applications, the environment is fix, i.e. the positions and velocities of the obstacles are known. Otherwise the robot has to use some sensors to be able to estimate this information. Robots should be able to plan their motions such that they will not collide with static or moving obstacles. The literature presents several path planning methods for the avoidance of static obstacles (see e.g. [2,3]).
There are two possibilities for motion planning among moving obstacles: 1, The planning can be done in two steps: geometric path planning and then velocity planning. 2, The geometry of the path and the velocity profile along it are determined simultaneously.
In the first case, the geometry of the path and its time distribution are calculated separately (e.g. [4,5]). At the path planning phase, the moving obstacles are disregarded. The planner calculates a path which ensures collision-free motion to the goal among the static obstacles. The motions of the obstacles influence only the velocity profile of the path.
The second possibility is to calculate both the path geometry and the velocity profile in one step (e.g. [6]). In this second case the shape of the path depends also on the positions and movements of the dynamic obstacles.
If the information about the obstacles (positions, velocities) are known, a global path planner can be applied. If the environment is unknown, a local reactive obstacle avoidance algorithm has to be used based on the sensory information (e.g. [7]).
Several motion planning methods in static environment can be extended to solve the dynamic problem as well. Some methods use the configuration space of the robot to find a feasible path among static obstacles (e.g. [3]). If the workspace of the robot contains moving obstacles as well, the configuration space should be ASTESJ ISSN: 2415-6698 extended by a temporal dimension, and the motion should be searched in this extended space. Using this solution, one has to modify the distance metric to deal with the temporal dimension [5].
The Artificial Potential Field (APF) algorithm is a quite simple method which can be used for path planning in static environment [8]. Applying special potential force functions, one can solve the motion planning problem in case of moving target and obstacles using APF [9,10].
The Dynamic Window (DW) method is a local obstacle avoidance algorithm used in static environment [11]. The planning is done in the velocity space of the robot. Reachable and admissible velocity values are selected. Reachable values satisfy the kinematic and dynamic constraints of the robot and admissible values guarantee that the robot can stop before hitting an obstacle. An adaptation of DW can be used with moving obstacles as well [7].
The inevitable collision states (ICS) for robots are presented in [12]. If the robot is in an IC state, it surely collides with an obstacle independently from the future trajectory of the robot. If a state is non-ICS, there exists at least one motion possibility for the robot to avoid collision. Using the ICS concept, the motion planning problem in dynamic environment can also be solved [13].
The concept of velocity obstacles (VO) was introduced in [6] for such environments where the velocity vectors of the obstacles are supposed to be unchanged for some time-interval. Using VOs, an avoidance maneuver can be determined in the velocity space of the robot, based on the current positions and velocities of the robot and obstacles. Velocity obstacles represent the set of robot velocities that would result in a collision with a static or moving obstacle. The basic VO method was extended for obstacles moving along arbitrary trajectories (non-linear velocity obstacles -NLVO [14]).
The inverse version of NLVO (INLVO) can be used to plan the velocity profile for a robot along a path with fix geometry [15]. A modified VO method can be used to plan autonomous navigation for unmanned surface vehicles as well [16]. The hybrid reciprocal VO (HRVO) is a method to plan the motion of multiple mobile robots without central coordination [17].
Our conference paper [1] presented a modified version of VO to plan the motion for Dubins-like mobile robots (VOD -Velocity Obstacles for Dubins-like robots). These robots go only forward. VOD defined pairs of colliding velocity and turning radius.
In this paper, the goal is to define velocity obstacles for Reeds-Shepp-type car-like mobile robots, which are able to drive both forward and backward. The determination of colliding velocity and curvature pairs is discussed. In this paper, the curvature is used instead of turning radius, hence the graphical representation of the colliding pairs is easier. The method and the equations of [1] were modified to deal with negative velocities and to use curvature instead of turning radius.
The paper is organized as follows. Section 2 gives a short review of velocity obstacle methods. Section 3 presents the properties of Reeds-Shepp-type mobile robots. Section 4 describes how the 'velocity' of a car-like robot is represented in this work. In Section 5, the velocity obstacles for car-like mobile robots (VOCL) are presented. Some simulation results are given in Section 6. Finally, a short section concludes the paper.

A Short Review of Velocity Obstacles
VO method can be used to find a feasible velocity vector for the robot such that the robot is able to avoid static and moving obstacles. It is assumed that the position and velocities of the obstacles are known, and the obstacles follow a straight-line path for some time-horizon. The VO method uses circular representation of the robot and the obstacles with known radii.
Given are a robot and some moving obstacles ( = 1 … < ∞), where denotes the number of obstacles. (According to this concept, a static obstacle is a moving obstacle with zero velocity.) The velocity obstacle contains all the robot velocity vectors which would result a collision with obstacle : where ( , ) denotes the robot at time if velocity was applied. The shape of is a cone (see Figure 1). The union of the individual reads Selecting a velocity vector outside guarantees that no collision will occur between the robot and the obstacles.
An example for a point robot and two moving ( 1 , 2 ) and a static obstacle ( 3 ) is presented in Figure 1.

Non-Linear Velocity Obstacles
The non-linear velocity obstacle (NLVO) defines the set of all linear robot velocities that would result a collision with obstacle ( ) moving along arbitrary known trajectory. At time instant , one can define robot velocity vectors which move the robot to a position during time − 0 such that a collision occurs with : where ( ) denotes the trajectory of the obstacle . Considering all > 0 , one gets The shape of the non-linear velocity obstacle is a warped cone with apex at (see Figure 2). Similar to (2), the union of the individual defines the set of all robot velocity vectors which result a collision for some > 0 .

Generalized Velocity Obstacles
The concept of velocity obstacles was generalized to apply it for car-like robots [18]. The obstacle is defined in the control space of the robot: where is the control input of the robot, ( , ) is the position of if control was applied up to . and denote the radii of the circular robot and obstacle.
The controls are sampled, and for each control , the minimum distance between and is determined numerically, if control was applied to the robot. If the minimum distance is smaller than the sum of the radii, ∈ and it means that a collision will occur between and for the control .

Idea for Extension
In this paper, the velocity obstacles are applied to similar robots as presented in Subsection 2.2. This solution will not be restricted to sampled control inputs, as suggested by [18]. The presented method represents the velocity obstacles for car-like robots as a subset of a two-dimensional plane similar to the methods of VO and NLVO. In this work, this plane is determined by the velocity of the robot and the curvature of its path.

Reeds-Shepp-Type Car-Like Mobile Robots
A car-like mobile robot (see Figure 3) can move in a twodimensional workspace. The state of the robot is defined by its position and orientation. The position is given by the ( , ) coordinates of the midpoint of its rear axle. The orientation of the robot is denoted by , and it is defined by the angle of the positive axis of the coordinate system and the longitudinal axis of the robot. Ackermann-steering is supposed (see [19]), and the movement of the robot is described by the motion of a bicycle putting on the longitudinal axis of the robot. The inputs of the robot are the longitudinal velocity and the angle of the front turning wheel ( ). Notice, that is a scalar. The kinematics of the robot reads: where is distance between the front and the rear wheels. The turning angle of the front wheel determines the radius of the turning circle of the robot: = tan . The reciprocal of the turning radius is called curvature: The turning radius cannot be arbitrary small, since the turning angle of the front wheels is also limited. Depending on the limit of and the geometrical parameter of the robot, a minimal turning radius can be determined: ≤ | |. Similarly, the curvature is also limited: | | ≤ .
The car-like mobile robot is a nonholonomic system since the direction of its velocity vector is constrained by the following equation: There are different types of car-like mobile robots: Dubinsrobots can only move forward (i.e. > 0). It was proved that for given start and goal states the path with the minimal length consists of three segments: two or three circular segments with maximal curvature (i.e. with minimal turning radius) and, if needed, a straight-line between two circles if no obstacles are presented in the workspace [20]. The Reeds-Shepp-type mobile robots are able to travel both forward and backward. It was also shown that the path with minimal length between given start and goal states contains (like Dubins-robots) circular segments with maximal curvature, and it may contain straight-lines and cusp points where the driving direction changes [21].  (11)) has several solutions for given and . If the robot does not go more than ones around in a circle, (10) (or (11)) will have a single solution for (or for ) with − < atan ≤ .

Velocity Representation for Car-Like Mobile Robots
If the robot goes straight, its motion is described by: = Δ , = 0, = 0, = ∞, = Δ . In the sequel, the goal is to determine the ( , ) (i.e. ( , ) and ( , )) pairs which result a collision-free motion for the robot.

Velocity Obstacles for Car-Like Robots
Let the robot be at the [ 0 , 0 , 0 ] = [0,0,0] initial state. First, such ( , ) pairs are determined which result a collision with static obstacles. Then the collision with moving obstacles is also considered.

Collision with Static Obstacles
Suppose that a static obstacle is presented in the workspace of the robot. Let a circle represent the obstacle. Its position is given by the coordinates of its center: ( , ) and its radius is . For the sake of simplicity, the robot is also represented by a circle with radius . (A non-circular robot or obstacle can be approximated by one or more circles.) To examine the collision for a point-like robot, the radius of the obstacle should be enlarged by the radius of the robot: ′ = + similar to the method proposed in [6].
First, such circular motions (i.e. curvatures ) are considered, where the robot grazes (touches) the obstacle. Geometrically, the problem is the following: given a point (position of the robot), a straight-line going through this point (line of the robot's orientation) and a circle (obstacle with enlarged radius ′ ), such a circle has to be found, which is tangential to the circle of the obstacle and grazes the line of orientation at the given point (robot's position). There are two possibilities (see Figure 4). In both cases the center of the turning circle lies on the -axis (i.e. the coordinate of the center of the turning circle is 0). The coordinate of the center of the turning circle defines the radius of the circle. The two possible values for the curvatures (or for reciprocals of y coordinates of the center) are: All curvatures between 1 and 2 will result in a collision with the static obstacle. More precisely, moving on a circle with curvature ≠ 0 will cause a collision if  min( 1 , 2 ) < < max( 1 , 2 ) .  = 0 can only cause a collision if The coordinates of the grazing points (( 1 , 1 ) resp. ( 2 , 2 )) can be calculated as well: Consider now the time instant . The ( , ) input pairs can be determined, which cause collision with the static obstacle at . The velocity obstacle for a car-like robot (VOCL) is the union of these points (see Figure 5): where ( , , ) represents the robot at time-moment moving from 0 = 0 and from the origin on a circular path with radius 1 with velocity . and can be calculated from ( , ) position using (9)-(11).  Taking time-moments in a time-interval ∈ [ 0 , ℎ ] one can get for a static obstacle :

 
If the robot moves according to ( , ) ∉ , the robot will not collide with the static obstacle in the given time interval Notice, that the selection of ℎ influences the effectiveness and the computational demand of the method. If a small value was selected for ℎ , a mobile robot may collide with a moving obstacle. On the other hand, a large time-horizon can result that VO includes almost the complete velocity space [13]. can be determined as well, according to (21) and (22).

Calculating VOCL for Moving Obstacles
The main difference is the following: the grazing points in ( ) (( 1 , 1 ) and ( 2 , 2 ) ) do not represent grazing cases any more if the obstacle is moving since the position of the grazing points depends on the motion of the obstacle. To analyze VOCL and the grazing points, the following notations are used (see Figure 6): • A time instant is considered.
• The position of the moving obstacle at is denoted by ( ( ), ( )). The angle between the positive x-axis and the velocity vector ( ) of the obstacle is denoted by ( ), the absolute value of the velocity is ( ). ( ) denotes the turning rate of the obstacle. The vector ( ) is parallel to the positive z-axis, if the direction of the rotation is positive, and to the negative z-axis in case of clockwise rotation.
• If a robot is moving with a velocity on a circle with curvature for time , its position ( ( ), ( )) and orientation ( ) can be determined according to (8). The absolute value of the robot's velocity remains unchanged during the circular motion, but the vector of the velocity ( ) changes, since the orientation of the robot is also modified. The turning rate of the robot is = . The direction of vector is parallel to the positive z-axis, if > 0, and to the negative z-axis if < 0. The vector is constant during the circular motion, since its direction and length (i.e. ) are unchanged. .  • is a vector connecting the center of the obstacle to the center of the robot.
• The angle between the positive x-axis and is:

 
The following two propositions follow directly from the definition of ( ).
A point is selected on the boundary of the robot. is a vector which goes from the center of the robot to . The velocity of the point is: Similarly, a point can be selected on the boundary of the obstacle.
is a vector which goes from the center of the obstacle to . The velocity of reads: (and accordingly to and as well). The relative velocity of according to is: Both ( × ) ⋅ and ( ( ) × ) ⋅ equal 0, hence one gets which is equivalent to that | ( ) is perpendicular to . ■ Notice, that the obstacles can move on arbitrary paths with arbitrary velocity profiles. The algorithm only supposes that for every time moment ∈ [ 0 , ℎ ] the positions and the velocity vectors of each obstacles are known.

Trajectories Avoiding Obstacles
If a goal position is given in the workspace of the robot, a motion should be planned which arrives to this point. During the motion, no collision should occur.
Fiorini and Shiller presented some heuristic search methods using Velocity Obstacles [6]. Applying these, one can easily select at each time moment a feasible velocity vector which moves the robot to the direction of the goal such that it avoids collisions with the obstacles. Using these heuristics alone does not guarantee to find an optimal solution. They designed an off-line global search method as well. A tree was generated for avoidance maneuvers using VO.
Shiller et al. presented avoidance maneuvers using NLVO [14]. These avoidance maneuvers can be used in a local or global motion planner as well.
The VOCL method can also be used in all above methods to plan the motion for car-like mobile robots to a given goal avoiding static and moving obstacles.

The Safest Solution
Another possibility for the selection of ( , ) pair is to determine the safest motion. This method is called as the Safety Velocity Obstacles method (SVO).
The most important goal of the SVO method is to ensure the safest path for the robot during the motion.
This method checks, how far a possible ( , ) ∉ pair is from the nearest (name this distance ). If the distance is bigger than a predefined threshold , then the value of is set to this maximum distance.
Hence, a normalized distance can be defined: After that the value of the cost can be defined as If the selected ( , ) pair is far from the nearest , then the value of will be a big number (near to 1). So, such ( , ) pair has to be chosen for the robot, where the value of is minimal.
So, with the SVO method one can plan the safest motion in dynamic environment.

Simulation Results
An example for VOCL is presented here. A circular robot with radius = 0.9m is given. Its minimal turning radius is = 3m (i.e. = 1 3 m −1 ) and its maximal velocity is = 7 m . Seven circular obstacles with different radii are presented in the workspace (see Figure 7). Four obstacles move ( 1 , 2 , 3 , 4 ) and there are three static obstacles ( 5 , 6 , 7 ). The velocity vectors and the paths of the dynamic obstacles are also depicted in Figure 7. The VOCL and FVOCL for ℎ = 10s is given in Figure 8. White areas represent ( , ) pairs which correspond to collision-free motion for ℎ . A collision-free example is also presented here. = 0.15m −1 ( = 6. 6̇m ) and = −5 m was selected. The corresponding ( , ) point is depicted by a red star in Figure 8. The motion of the robot and the movement of the obstacles are depicted in Figure 9.

Conclusion
Velocity obstacles (VO) and non-linear velocity obstacles (NLVO) methods can be used to plan a collision-free motion for a planar robot moving among static and moving obstacles. VO supposes that the obstacles move on straight-lines with constant velocities. If the path of the obstacle is not a straight-line, NLVO can be applied. These methods determine a velocity vector for the robot, which results in a collision-free motion in a time-interval. Applying this velocity, the robot will move on a straight-line according to the direction of the selected velocity vector. Both methods assume that the position and the motion of the obstacles are known. The method, presented in this paper is similar to NLVO but in this case the robot is a car-like mobile robot. This robot cannot move to arbitrary direction. The direction of its velocity vector is determined by its orientation. The robot can move on a straightline according to its orientation or on a circular path. Hence, the presented VOCL method determines only the magnitude of the velocity vector and, additionally, the curvature of the circular path to follow to avoid collisions.
The next stage of this work is to implement the VOCL method for a car-like mobile robot, such that the collision-free motion is determined in real-time.
Our future goal is to take the non-circular shape of the robot (e.g. car-like rectangle) also into consideration during the construction of VOCL.
More feasible motion could be calculated if the path does not contain circular and straight-line segments only, but clothoid arcs as well as in [22]. In this case a continuous curvature path could be determined. The drawback is that the computational complexity would be larger.