Mobile robot position control in environment with static obstacles

: This study addresses the problem of efficiently navigating a differential drive mobile robot to a target pose in a region with obstacles, without explicitly generating a trajectory. The robot is assumed to be equipped with an omnidirectional range sensor, while the region may or may not be a priori known. Given the known obstacles in each iteration of the controller, the shortest path connecting the robot and the target point provides a raw desired movement direction. Considering the unobstructed area in that direction, the size of the robot and the obstacle contours in its visibility range, the reference direction is determined. Finally, respecting the velocity and acceleration constraints of the robot, the angular velocity is properly selected to rotate the robot towards the reference direction, while the linear velocity is chosen to efficiently minimise the distance to the final target, as well as to avoid collisions. After the robot has reached the target, the controller switches to orientation mode in order to fix the orientation. Experimental studies demonstrate the effectiveness of the algorithm.


Introduction
Mobile robots have proven to be a reliable choice for a variety of applications. Beside exploration [1][2][3], area coverage [4] and surveillance [5], industries are now using mobile robots to automate the management of their warehouse inventory [6,7]. Whatever the task, the main problem always falls to efficiently move a robot from one location to another, avoiding all the obstacles in its path. This justifies mobile robot navigation to be a field of continuously increasing interest [8][9][10][11][12].
Over the years, various methods have been developed to deal with the problem of autonomous robot navigation. Khatib [13] introduced the artificial potential field method, where the robot movement is based on artificial forces from the target and the obstacles. This method has been very popular due to the simplicity of its core idea [14][15][16], but has the drawback that it suffers from local minima. Another well-known method is the vector field histogram [17], where the area of interest is represented as a 2D grid of obstacle probability. This method has proven to be efficient in real-time applications as it utilises directly the range sensor data, but is prone to local minima and it does not take the robot dynamics into account. Besides that, it may also fall short in cases of narrow areas, such as doors or corridors. On the other hand, a method that uses the robot dynamics in real-time applications is the dynamic window approach [18]. Here the velocity space of a sychro-drive robot is searched directly for admissible control inputs, ensuring that the robot will be able to stop before hitting an obstacle. This search is limited to the dynamic window of achievable values, considering the acceleration capabilities of the robot and the time interval of the controller. The main drawback of the dynamic window approach is that convergence is not guaranteed as it is susceptible to local minima. Improvements on this method in order to address this drawback have been suggested by other authors, such as the ones in [19,20].
In this paper, we will focus on the motion planning for a smooth obstacle avoidance, without imposing unnecessary velocity constraints. The presented method assumes a differential drive robot with constrained velocity and acceleration. A partial or whole region is required for the first, path planning step, where the shortest path [21] connecting the robot to the target hints the movement direction. In our experiments we will assume a known region, but the algorithm is intended to work also in exploration cases, where the shortest path is extracted given the explored region. If the path suggests a full turn after encountering a dead end, the motion planning will successfully handle it. The next steps incorporate some core ideas of the above methods and require only the obstacle boundaries in the visible field. First of all, an approach similar to the potential field forces has been taken, in order for the visible obstacles to directly affect the movement direction. Besides that, in a similar manner as in the dynamic window approach, the linear velocity of the robot is constrained to prevent potential collisions.
The movement direction acquired from the shortest path, which is the theoretically optimal, is the base to determine the reference direction for the robot. Initially, the direction of the shortest path is modified to point to the furthest unobstructed location considering the size of the robot, which will be referred to as the effective direction. Subsequently, the obstacles in the visible field are scanned in order to produce the actual reference direction. For each point of the visible obstacles, an admissible direction is determined, pointing away from obstacle and closer to the effective direction. These are then weighted by an importance factor, based on the distance of the respective point from the robot and the correlation of its visibility angle with the effective direction.
Having the reference direction and assuming a differential drive robot, the final step is to determine its velocity inputs. Firstly, we define an estimated collision distance function for the obstacles in the visible space. Then we determine the sign of the linear velocity depending on which direction the distance from target is minimised. Given the minimum collision distance based on that direction, the orientation deviation and the acceleration capabilities of the robot, the linear velocity is selected to ensure no collision and smooth movement, while preserving high values. Finally, the angular velocity endeavours to minimise the orientation deviation, but not surpass it. This is achieved by properly utilising the acceleration capabilities of the robot and the time interval of the controller.
The paper is structured in the following manner. In Section 2, the model of the robot is presented and some key elements are defined. In Section 3, the movement direction towards the target is determined, while in Sections 4 and 5 the actual control inputs for the robot are formulated. Additionally, Section 6 states when and how the orientation mode takes effect. Finally, some experimental results are presented in Section 7, followed by some concluding remarks in Section 8.

Problem formulation
Let us consider a non-holonomic mobile robot with differential drive, as presented in Fig. 1. The kinodynamic equations are given as follows: where t ∈ ℝ + is the time instance, x(t), y(t), θ(t) ∈ ℝ are the coordinates and the orientation of the robot, while v(t), w(t) ∈ ℝ are its linear and angular velocities, respectively. We define the kinematic constraints of the robot in the following manner: where v min ∈ ℝ − * , v max ∈ ℝ + * and ω max ∈ ℝ + * are the linear and angular velocity bounds, a(t), α(t) ∈ ℝ are the linear and angular accelerations, while a d , a a ∈ ℝ + * and α d , α a ∈ ℝ + * are the linear and angular deceleration and acceleration limits. The time interval of the controller is T s ∈ ℝ + and at each iteration k ∈ ℕ we have access to the full measured state vector of the robot x(kT s ) = x m y m θ m v m ω m T . For simplicity, the subscript k will be omitted from the variable names, because all of them are local to the current time instance. Additionally, concerning the dimensions of the robot, we assume that it has a circular shape of radius R ∈ ℝ + * around its rotational centre. Except from the aforementioned constraints, the differential drive robot has an additional constraint between its linear and angular velocities. If v R , v L ∈ ℝ are its right and left wheel velocities and L ∈ ℝ + * the wheel-to-wheel distance, then v = (v R + v L )/2, while ω = (v R − v L )/L. It is obvious that when the linear velocity has its maximum absolute value, then the angular velocity is zero and vice versa. If we define the wheel velocity limit imposed by each motor as v lim ∈ ℝ + * , we deduce that the mechanical constraints for the linear and angular velocities are, respectively, v lim and ω lim = 2v lim /L. Now given a value v r ∈ [ − v lim , v lim ] for the linear velocity, the effective mechanical constraint for the angular velocity can be obtained by It is very common for the kinematic constraints to differ from the mechanical, but of course v min , v max ≤ v lim and w max ≤ w lim need to be satisfied.
Let Ω ⊂ ℝ 2 be the region of interest. In most reality cases, a map of the region is incrementally build by a SLAM algorithm [22], using an omnidirectional range sensor of radius r d > 2R equipped on the robot. We define the mapped region as where the entire unexplored area is presumed to be free. In our case, we assume that the problem of localisation and the acquisition of the region are already solved, while in the experiments we assume a known region. The measured position of the robot will be referred as p m = x m y m T ∈ Ω k m , while the localisation error is assumed bounded by a disc of radius r u ∈ ℝ + . Let p t ∈ Ω with θ t ∈ ( − π, π] be the target pose for the robot. We define the shortest path from p m to p t in region Ω k m , as the ordered set of points P( p m , p t ) = [ p 0 , p 1 , …, p n ], where n ≥ 1, p 0 = P 0 ( p m , p t ) = p m and p n = P n ( p m , p t ) = p t . It is assumed that this path is wide enough to be traversed by the robot. Additionally, we define the visible obstacles with respect to the visibility angle θ ∈ ( − π, π] as V(θ) ∈ ∂Ω and their distance from the robot centre as d(θ) ∈ ℝ + ∪ {∞}, where d( ⋅ ) = ∞ for obstacles further than r d . We define as R s = R + r u , the minimum allowed distance from the obstacles and as l s ∈ (r u , R), the extra safe distance, which the robot will try to preserve when possible.

Movement direction
In order for the robot to reach the final target, the shortest path P( p m , p t ) is taken as a reference. Specifically, the furthest visible point of the path, p 1 , is chosen as an initial guess for the movement direction. This direction is then modified to the closest unobstructed direction with respect to the robot size, leading to the effective target. Based on that, the visible obstacles are scanned to produce the reference direction.

Effective target
To determine the effective target we need to search for the effective obstacle direction. Firstly, we have to define the permissible search directions from the right, clockwise, and from the left, counterclockwise, of p 1 , which are, respectively Θ cw = (θ 1 − π/2, θ 1 ), Fig. 2. The set of all search directions will be referred as Given a test direction, the deviation to its closest unobstructed one is defined as which adds an offset R s + l s from respective obstacle point. Starting the search from θ 1 , the direction of the first effective obstacle point, θ b , and the effective target, p t, e = p m + d t, e e^t , e , with e^t , e = 1∠θ t, e , can be found in Algorithm 1 (Fig. 3) So, the effective target is in most cases an admissible location, reachable from the current position of the robot. Setting d t, e = min (r d , ∥ p 1 − p m ∥ ) as an initial value and requiring d(θ) ≤ d t, e for the effective obstacles, ensures that, if p t is visible, thus p 1 = p t , it will be selected as the effective target even if it is not really reachable due to the obstacles behind it. This is intended and the robot will get close enough, but will not collide due to the imposed limits on the velocity.

Reference direction
Having the effective target, the visible obstacle contours are used to compute the actual reference direction of the controller. Specifically, the idea here is that each visible obstacle point tries to enforce a movement direction away from it and closer to the target. These directions will be referred as the effective directions imposed by the obstacles and their properly weighted combination will result in the reference direction.
There are some prerequisites before getting to the effective direction. Firstly, we define the tangent vector to the visible obstacles, even on the discontinuity locations, as follows: Consequently, the normal vector to the obstacle contours, pointing away from the obstacle, is as follows: Finally we can define the unit vector of the direction tangent to the obstacle and towards the target as follows: The permissible values for the effective direction are the ones that minimise the distance from the target but not the distance from the obstacle. Thus, the values are limited in the quadrant bounded by e^b(θ) and e^n(θ). As a result, the effective direction imposed by the obstacles is defined as follows: The selection areas are depicted in Fig. 4 for both cases of e^b. Also, Fig. 5 shows all these directions in an example, while Fig. 6 demonstrates the selection of the effective direction for various cases.
The importance of each obstacle point is measured by two factors. The first is its distance from the robot. Closer obstacles take priority as their avoidance is more urgent. The second is the visibility direction of the obstacle. Obstacles at directions closer to θ t, e take priority, because they are the potential threats, while obstacles in the opposite direction are of no concern. As a result, the combined direction is computed like so Except that, the robot should try to maintain a safe distance l s when possible. To achieve that we add an extra weight for directions away from the obstacles which are closer than l s . The visibility angles of these obstacles are as follows: Θ s = {θ ∈ ( − π, π]: d(θ) < l s } and the direction we choose for these is e^s(θ) = 3e^n(θ) + e^b(θ) 10 .
Therefore, the weighed direction for all these points is Finally, as reference direction is chosen θ r = ∠(E e + E s ). Consequently, the deviation from the current orientation of the robot, the desired rotation angle, is determined as follows: This value also signifies the sign of the linear velocity, which will follow the sign of cos Δθ r .

Linear velocity
Now that the reference direction is determined, all that remains is to select the velocity inputs for the robot. We start with the linear velocity, as the limit of the angular will depend on that. The sign of the linear velocity aims to minimise the robot to target distance, while its maximum absolute value is limited based on three things; the collision distance, the target distance and the expected rotation angle after reaching the effective target. Firstly, we will define the collision distance for the visible obstacles. The collision distance is computed with respect to the current direction of the robot, θ m , and expresses the proximity of the obstacle points, even if they do not directly obstruct the movement at that direction. To express that, the distance of the obstacle points from the line through p m and direction θ m is needed. This line can be defined as p = p m + λe^m, with e^m = 1∠θ m and λ ∈ ℝ. Therefore, the distance from an obstacle point V(θ) is When l b (θ) ≤ R s , the collision distance is selected as the distance that can be traversed by the robot in direction θ m before touching the obstacles. In this case, the distance is as shown in Fig. 7a, with l e (θ) = R s 2 − l b 2 (θ). In order to extend it for all the visible obstacles, we choose The selection for the case of l b (θ) < R s , which ensures continuity as the robot moves and θ m changes, expresses the proximity of the indirect obstacles in terms of the capabilities of the robot to decelerate, considering the time interval of the controller. Finally, the minimum values from the front and from the rear of the robot are, respectively with Θ f = [θ m − π/2, θ m + π/2] and Θ r = [θ m + π/2, θ m + π].
Besides the collision avoidance, the robot may need to decelerate before taking a turn around an obstacle or while reaching the target. To express that need for deceleration, we introduce the expected distance, d e , which incorporates the distance d t, e along with the expected rotation angle while considering the capabilities of the robot. The expected rotation angle is the supplement of the angle formed by p m , p t, e and the next point of the path. By choosing p′ 1 = P 1 ( p t, e , p t ) as the next point of the path, we define the expected rotation angle as follows: Δθ e ′ = π − ( p m p t, e p′ 1 ) .
There are two requirements that need to be met while selecting the expected distance. Firstly, when Δθ e ′ ≥ π/2, it should force a full deceleration in d t, e , hence d e = d t, e . Secondly, it should express no need for deceleration when Δθ e ′ ≃ 0. The latter can be achieved by adding enough room for deceleration, thus selecting d e = d t, e + 3/2 v max 2 /a d . Lastly, considering the relation between the durations of a full deceleration, v max /a d , and for a 90 ∘ rotation, π/2 2/α a , we define the expected distance as follows: Finally, we have to compute the velocity limit so that the robot can be able to decelerate to zero at the aforementioned distances. The minimum signed distance at the movement direction is determined as follows: Also, the minimum signed distance that the robot will traverse due to its current linear velocity is Depending on the values of these distances, the following cases are formulated: i. Case of d l d d ≥ 0 and d l < d d + v m T s : In this case, the robot needs to decelerate. For a simple deceleration we need to solve the following for v l with Δt = ( v m − v l )/a d being the time interval to reach v l .
• If ∃v l and Δt ≥ T s : Then we select the positive root.
• If ∃v l and Δt < T s : Then the control input will not be able to change on time for a full deceleration. To fix that we introduce a delay by including a part with constant velocity, thus the polynomial to solve becomes of which we chose the largest root. • If ∄v l : Then the robot needs to stop the fastest it can, so we select v l = 0. ii. Case of d l d d ≥ 0 and d l ≥ d d + v m T s : In this case, the robot has more room to move and can, possibly, even accelerate. To obtain the limiting value, we solve the following polynomial: with Δt = (v l − v m )/a a being the time interval to reach v l .
• If Δt ≥ T s : Then we choose the largest root.
• If Δt < T s : Then we add again the constant velocity delay and solve the following for its largest root: iii. Case of d l d d < 0: Here, the linear velocity has to change sign. Based on that, we need to take into account a full deceleration from the current velocity, an acceleration to the to-be-computed velocity limit and then a deceleration again. This can be expressed as follows: where Δt l = v l /a a and the time interval until v l is • If Δt ≥ T s : Then we choose the largest root.
• If Δt < T s : Then we choose the largest root of The above velocity limit ensures deceleration at some distance but does not necessarily respect the constraints. To choose an appropriate value for the velocity, we have v l, r = min (v l , v lim ), v r = max v min , min (v max , v l, r cos Δθ r ) .
Multiplying with cos Δθ r not only sets the sign of the velocity, but it also imposes an extra limit. This ensures that, depending on the desired rotation angle, the choice of the linear velocity does not compromise the requirements for angular velocity.

Angular velocity
The angular velocity is chosen to minimise the deviation from the reference direction, Δθ r . Same way as with the linear velocity, the minimum signed rotation angle due to the current angular velocity is Moreover, the sign is the same with Δθ r . Thus, the angular velocity is selected as follows:

Orientation controller
The controller needs to switch to orientation mode when the robot has reached the target. The target is assumed to be reached when d t, e < 2r u . In that case, we simply set d t e = 0 and θ r = θ t and follow the same procedure for the selection of the linear and angular velocity.

Experimental results
The mobile robot in our experiments is the AmigoBot from ActiveMedia, shown in Fig. 9. The robot can be bounded in a circle of radius R = 0.17m around its rotation centre and its wheel-towheel distance is L = 0. The localisation is based on a combination of camera and encoder measurements. The augmented reality library ArUco [23] is used in combination with two cameras on the ceiling as the main way of retrieving the position and orientation of the robot in the known region of interest. For completeness and reproducibility, the polygon vertices of our region are given in Table 1. Moreover, the robot is equipped with encoders, used by the robot to calculate its wheel velocities and its relative pose. This relative pose is used to update our localisation whenever the camera system is slow. The localisation error is bounded in a radius r u ≃ 0.015 m and the normal sampling time interval in our experiments is T s ≃ 160 ms. The safe distance that the robot should maintain from the obstacles when possible, is selected with respect to the robot radius as l s = R/2. Furthermore, as our region is known and we are not concerned with mapping it, we assume a large sensing radius r d = 2 m.
In our experiments, we present two comparison case studies. The first one compares the algorithm for the normal velocity and acceleration bounds, and for the maximum/mechanical bounds. The second compares the algorithm for the normal sampling time and for a relatively big one. For variety, the initial and final pose are different in each case study.

Case study on the velocity and acceleration bounds
This case study compares between normal and maximum velocity and acceleration bounds. The initial pose of the robot was p m, 0 ≃ [ − 0.45, 1.12] m, with θ m, 0 ≃ 0 ∘ and for the final target we chose p t = [1.2, − 0.4] m, with θ t = 0°. The trajectory of the robot for the normal bounds case is shown in Fig. 10a and for the maximum bounds case in Fig. 11a, while Figs. 10b and 11b depict the actual motion of the robot. Additionally, Figs. 10c, d and 11c, d depict the linear and angular velocities, respectively, for both cases.
In both cases, the robot successfully reaches the target under a smooth trajectory. The traversed distance is ∼9.6 m, with 0.16 m/s mean linear velocity in a duration of ∼49 s for the first case and 0.2 m/s mean linear velocity in a duration of 40 s for the second. The maximum linear velocity was 0.383 and 0.53 m/s for the first and second case, respectively. We notice that, even though the maximum linear velocity in the second case is above the limit of the first's, it is far from its maximum allowed value. This was anticipated and proves that the algorithm effectively adapts the capabilities of the robot with respect to the complexity of the region, eliminating any possibility of collision.

Case study on the sampling time interval
Here we compare between the normal sampling time interval and a larger one of 1 s. The robot was initially positioned at p m, 0 ≃ [0, 0] m, with θ m, 0 ≃ − 90° and the target was chosen as p t = [1.2, 1.65] m, with θ t = 90°. The trajectories of the robot for the two cases are shown in Figs. 12a and 13a, respectively, whereas Figs. 12b and 13b depict the actual motion of the robot.   Moreover, the linear and angular velocities are depicted in Figs. 12c, d and 13c, d, respectively, for both cases. In this case, too, the robot smoothly reaches the target. The duration until the target for the normal sampling time was ∼24 s, while for the larger sampling time it was 45 s. The larger sampling time enforces more constraints on the movement of the robot, thus leading to smaller velocity values. The other interesting thing to notice is the reverse movement of the robot at the start of the experiment, following the direction that minimises the shortest path.

Conclusions
This paper exploits the robot dynamics to efficiently navigate a differential drive robot from one pose to another, in a region with obstacles. The shortest path to the target is used as a reference, in order to ensure that the robot reaches the target in a relatively optimal way. Obstacle avoidance is performed, ensuring the best movement direction while considering the size of the robot. Additionally, the robot dynamics and the sampling time of the controller were utilised, so that the velocities of the robot are maximised while ensuring deceleration before hitting any obstacle. Finally, experimental studies demonstrate the effectiveness of the algorithm.