Rapid Navigation Function Control for Two-Wheeled Mobile Robots

This paper presents a kinematic controller for a differentially driven mobile robot. The controller is based on the navigation function (NF) concept that guarantees goal achievement from almost all initial states. Slow convergence in some cases is a significant disadvantage of this approach, especially when narrow passages exist in the environment and/or specific values of design parameters are set. The main reason of this phenomenon is that the velocity control strongly depends on the slope of the NF. The algorithm proposed in this paper is based on a method introduced in Urakubo (Nonlin. Dyn. 81(3): 1475–1487 2015), that extends NF to nonholonomic mobile platforms and allows stabilizing not only the position of robots but also their orientation. This algorithm is used as a reference in experimental performance comparison. In the new algorithm, the gradient of the NF is used to generate motion direction but the velocity is computed as a function of position and orientation errors. This approach results in much better state converge. Analysis of the convergence shows how the location of the eigenvalues of linearized system affects time of goal achievement. The paper describes saddle point detection and avoidance methodology and presents their experimental verification. It also shows what happens in practice if initial position is located exactly in the saddle point and its detection/avoidance procedures are turned off.


Introduction
The breakthrough idea to use artificial potential fields to control manipulators and mobile robots was introduced by Khatib [4] in 1986. In this approach both attraction to the goal and repulsion from the obstacles are negated gradients of the artificial potential functions. The original solution had one major drawback: local minima may occur depending on the configuration of obstacles in the task space and inadequate potential functions choice. The problem is caused by the fact that attracting and repulsive components of the control are combined by simple addition. In some states which are far from the target location the addition of attracting and repulsive vectors may result in zero vector. gradient cannot be used directly to control the robot like in [13,14]. It is used as a reference for a nonlinear controller that generates linear and angular velocities applied to the mobile platform. In contrast to the Rimon and Koditschek algorithm the orientation also converges to the desired value. Convergence proof was enclosed in the paper.
The author of this paper together with its co-authors conducted extensive tests of this algorithm including simulations for sphere worlds [10], star worlds [6], and experiments for sphere worlds [7][8][9].
The beginning of the 21st century saw publication of many propositions of extension of the NF approach for the groups of mobile robots [2,15,16]. The publications addressed the problem of collision avoidance in multi-agent robotic systems. The first paper introduces multi-robot navigation function (MRNF). The second and third use prioritization to solve conflicts in the case of concurrent goals. In the [11] a new NF is proposed that can be computed locally, and only the knowledge of the target and obstacles in the robots' neighborhood is required. In [17,18] NF is used to control aircrafts. This work is motivated by the poor time performance of known NF control methods. This is caused by the direct dependency of the velocity control on the module of the NF gradient. The gradient depends on many conditions, including a number of obstacles, their relative positions, obstacle functions that avoid collisions, goal location, and some design parameters that must be tuned to avoid local minima. As a result, the module of the gradient vector is not usually the best choice for the velocity control.
The main contribution of this paper is to propose modification of the method described in [20] that makes the convergence to desired coordinates much faster. The convergence of closed-loop system is analyzed by linearization in the neighborhood of the critical point. It is shown that by properly selecting the new design parameters, the real parts of eigenvalues of the linearized system can be easily moved to greater negative values. This results in faster convergence of the system to the desired values. The effectiveness of the proposed method is verified by experiments. In addition, experimental verification of the NF saddle point detection and avoidance is included. This kind of results has never been published previously to the best of the author's knowledge.
Section 2 introduces the model of the differentially driven mobile platform and the environment. Section 3 presents control algorithm. In Section 4 convergence proof is given. Section 5 describes method of saddle point detection and avoidance. Section 6 presents experimental test-bed and results of experiments that illustrate how convergence of the new algorithm has improved in comparison to the reference algorithm. Detection of the saddle point and algorithm that drives the robot out of this point is presented. The final section includes conclusion.

Model of the System
A kinematic model of a differentially driven mobile robot is given by the following equation: where vector q [x y θ] denotes the pose, x, y are position coordinates and θ is orientation of the platform expressed in a global, fixed coordinate frame. Vector u v ω is the control vector with v denoting linear velocity and ω denoting angular velocity of the robot.
The obstacles are circle-shaped and modeled with obstacle function: where ρ i is the radius of the i-th obstacle (i = 1, ..., N), p i represents its center, r = [x y] is the location of the robot and N is the number of obstacles. The obstacle function has a zero value at the boundary of the obstacle and increases if the distance to the obstacle grows. It must be at least twice differentiable.
The task space has an external boundary described by the additional obstacle function that can be considered as representing one more obstacle denoted in the further equations with index zero: where ρ 0 is the radius of the task space and p 0 is the center of the task space (usually the origin of the global coordinate frame).

Control Algorithm
The goal is to stabilize robot in the origin with orientation equal to zero. The total NF is as follows: where κ is a positive, constant design parameter and Design parameter k w in Eq. 5 is a positive constant that allows tuning the function representing the importance of the orientation with respect to the Euclidean distance to the target. A common obstacle function is obtained by the aggregation of individual obstacle functions with the following equation: The internal obstacles are represented by i = 1, ..., N while index zero denotes the task space boundary.
The new control algorithm is as follows: where a is a positive, constant design parameter, and Design parametersb, e , f in Eqs. 8 and 9 are positive constants, L [sin θ − cos θ 0] and g ||B ∇V || ≥ 0. Function h(g) is as follows: where g is a small positive constant. Note that h(g) is non-decreasing function and h(0) = 0. Finally, ∇V denotes the gradient of the navigation function with respect to variables x, y and θ . Regardless of the number of obstacles, the gradient can be obtained in the analytical form as: where and As noted in [14] all undesired local minima of the NF (4) disappear as the parameter κ is increased. An algorithm for automatically tuning κ for sphere worlds is presented in [3]. The tuning parameter must satisfy a lower bound to ensure convergence to the desired value. For the sufficiently high value of the parameter κ NF (4) has a critical point associated with each isolated obstacle, the saddle point. V has no other critical points other than these points and the goal.
Saddle points are unstable equilibrium points. Urakubo et al. [21] proposes a special control procedure for saddle point avoidance. It uses a time varying trigonometric function to push the robot away from the unstable equilibrium point. In Section 5, this method is recalled together with a detailed description of saddle point detection. Section 6 shows the results of experimental verification.
The NF V (4) has the following properties: -V is at least twice continuously differentiable and all elements of ∇V and Hessian matrix ∂(∇V )/∂q are bounded -V ≥ 0, and V = 0 ⇔ q = 0 -the critical points of V are a minimum at q = 0 and saddle points associated with the obstacles; V does not have any other critical points -V is a Morse function -the Hessian matrix is nonsingular at the critical points

Convergence of the Closed-Loop System
This section presents analysis of the system convergence. It only includes steps that differ from the analysis shown in [20]. Substituting (7) with (1) one obtains: As the above equation is not differentiable it is modified as follows to get a differentiable one: Note that the differentiable model of the closed-loop system (15) is needed for the further analysis only. It is not used in the implementation. For → 0 (15) approaches (14). By linearizing (15) in the neighborhood of equilibrium point q = q 0 ∈ H where set H = {q | B ∇V = 0} one obtains The A(q 0 ) matrix is computed taking into account that ||B ∇V || → 0 as t → ∞. This condition can be rewritten as ( ∂V ∂x cos θ + ∂V ∂y sin θ) 2 + ( ∂V ∂θ ) 2 → 0. This leads to the conclusion that ∂V ∂θ → 0 and ∂V ∂x → 0 as the system approaches the equilibrium point. Moreover, assuming that the nominator of the fraction on the right hand side of Eq. 8 L ∇V → 0 as t → ∞ (that may be rewritten as follows: ∂V ∂x sin θ − ∂V ∂y cos θ → 0) and knowing that ∂V ∂x → 0, θ → 0 one can state that ∂V ∂y → 0. Summarizing, due to the fact that ∇V → 0, the closed-loop system is asymptotically stable to the equilibrium point (lim t→∞ x, y, θ = 0).
Notice that as the denominator of the fraction on the right hand side of Eq. 8 tends to zero (h(g) → 0) as t → ∞, the nominator must convergence to zero (L ∇V → 0) faster then the denominator to avoid b going to infinity and violating controllability of the system.
For the further analysis the control (7) can be rewritten as follows: wherē The vectorsĪ andJ can be expressed as follows: (19) and Note that vectorsĪ and −J are perpendicular because their scalar product is zero:Ī · (−J ) = 0. It follows that the control is a composition of two perpendicular vectorsĪ and J scaled byγ a andγ b, respectively. Scalar b given by Eq. 8 is computed using Lie bracket The third vector filed ad b 1 b 2 fulfills controllability condition: Nonlinear control system is controllable.
Assuming that only first part of the controlĪ is active (J = 0) Lyapunov function decreases and stops in the set {x, y, θ} ∈ : ( ∂V ∂x cos θ + ∂V ∂y sin θ) 2 + ( ∂V ∂θ ) 2 = 0 that is equivalent to ∂V ∂x = 0 and ∂V ∂θ = 0. Second component of the control (that contains b) makes the effect of the Lie bracket L that is responsible for convergence in the y coordinate. If both components of the control are active then V is decreasing and ∂V ∂q = 0 because the system is controllable. The condition ∇V = 0 leads to the conclusion thatq = 0 because the system is in set g = 0. It minds that x = 0, y = 0 and θ = 0. These intuitive arguments are extended to formal stability analysis that is presented in [20].
The matrix A(q 0 ) is given by the equation: In above equation b is as follows: The three eigenvalues of A(q 0 ) are as follows: Eigenvalues λ 2,3 differ from the ones obtained in [20] only by the coefficient ( e / 2 f ). All the conclusions for linearized system presented in [20] remain the same for the modified algorithm, in particular the necessary conditions for an equilibrium point to be stable are: If the system has a zero eigenvalue, there exists in the neighborhood of 0 a function V (q) such that ∂V ∂qq is negative for q = 0 and which has the further property it remains negative definite for some reasonable class of perturbations [1]. The experiments carried out seem to confirm that in presented analysis the higher order terms (rejected as a result of approximation) belong to this class because the behavior of the closed-loop system, even for the states far from the equilibrium point, is consistent with the results of the analysis based on the linear approximation. This local analysis seems to be applicable to the initial state points being quite far from the equilibrium point. The eigenvector corresponding to the zero eigenvalue lies in the tangent space of H set at the equilibrium point. Velocityq in the direction of the eigenvector of the zero eigenvalue cannot be generated, even in nonlinear system (14). The motion in that direction is restricted due to a nonholonomic constraint. The two non-zero eigenvalues determine stability of the equilibrium point.
It can be clearly seen from the analysis of λ 2,3 eigenvalues that by tuning e and f parameters the term ( e / 2 f ) can be increased. This raises both real and imaginary components of these two eigenvalues. The real parts are relocated to the left (negative) side of the complex plane, an operation that allows better convergence of the system.
The eigenvalues can be ensured to have only real parts by fulfilling the condition: 2a This condition is fulfilled if the following, simpler inequalities are met: In general, designing NF to fulfill these conditions may be quite difficult; however, they are not necessary to achieve asymptotic stability. They assure non-oscillatory behavior of the linearized system, which is a desired property in most applications.
If the robot is far from the equilibrium point the linear approximation may not be accurate, and thus, the improvement of the convergence is deduced by analyzing nonlinear control (7). In the NF V (4) function of distance to the goal C (5) and obstacles β (6) is mapped from (0, ∞) to (0, 1) -the counterdomain of the V . As a result, the influence of the distance to the goal on the control intensity (mainly for the locations far from the goal) is reduced. In the proposed modification the control is extended by including the term (||q || + e ) that introduces dependency of control intensity on the distance to the goal. The term in the denominator (g 2 + f ) intensifies the control for small ∇V values (and thus B ∇V ). The described observations were confirmed by simulations and experiments but of course this is not formal analysis. Properties of the NF given in Section 3 are conducive to extend the region in which desired attributes of the linear approximation (resulting from the analysis of the eigenvalues) remain in force.
For improperly chosen e and f , the proposed algorithm may also characterize slow convergence, especially for distances far from the goal. Numerical tests show that even for these cases (far from the goal) the influence of ( e / 2 f ) was consistent with the conclusions resulting from the analysis of the linearized system: by increasing ( e / 2 f ) the convergence could be significantly improved. Of course, it is not guaranteed that this property always holds in the nonlinear system.

Saddle Points
Saddle points are unstable equilibrium points that occur when the attracting vector is balanced by the collision avoidance vector. There is one such point associated with each obstacle. From the theoretical point of view the saddle point is a zero measure set. One might suspect that the designer does not have to deal with this problem; however, it is not obvious how the behavior of the robot is affected in a real application by the saddle point. Some researchers states that saddle points present no problem in real world robotics since being trapped by saddle point can only occur in infinite precision dynamics [19,22]. On the other hand, measurement quantization may cause the saddle point to expand to the some area surrounding its exact, theoretical location.
In this section some observations made during experiments are presented. It should be noted that behavior of the system may differ in other test-beds. Especially resolution of localization system, its precision, noises, repeatability and resolution of the actuator controllers may influence the system behavior.

Saddle Point Detection
The detection of the saddle point is based on analysis of the gradient ∇V and Hessian of the navigation function H (V ). The saddle point is detected if two conditions are fulfilled: 1. ∇V = 0 (in practice ||∇V || < s , where s -small positive constant), 2. one eigenvalue of H (V ) is negative. In Section 6 detailed analysis of the influence of parameter s on the process of saddle point detection and avoidance is presented.
The Hessian of the navigation function is as follows: The Hessian of the attraction function is given by the following equation: Hessian of the obstacle function is given by equation:

Control in the Saddle Point
The control given by Eq. 7 is temporarily replaced with the following one [21]: where a 1 , a 2 , w and b 1 are chosen such that [b 1 a 1 a 2 2w 0] T is parallel to the H (V ) eigenvector of the negative eigenvalue and | a 1 w | 1, | a 2 w | 1 and |b 1 | 1. In [21], a detailed analysis of the system with this control is given. It is shown that by moving along the eigenvector of the negative eigenvalue and taking into account that the value of the NF in the area to which the robot is driven is less than in the saddle point the proper behavior is obtained: the robot does not approach the saddle point and it converges to the goal.

Experiments
In this section experimental test-bed is described and results of conducted tests are presented.

Experimetal Setup
The mobile platform that was used in the presented experiments is the differentially driven MTracker robot (Fig. 1). It wast designed at Poznan University of Technology. It is controlled by a two-level hardware controller: the low-level motion controller uses the signal processor TMS320F28335 150MHz and the high-level one is a single-core Intel Atom 1.2GHz board, equipped with WiFi radio used for remote management, task setting and communication with the external localization system. Depending on the requirements the high-level controller works on the Linux Ubuntu or Windows XP operating systems. The MTracker robot is a small platform: its diameter is 0.14m, its height is 0.13m, its weight is 1.4kg and its wheels have a diameter of 0.05m. The on-board power supply is LiIo 3.7Ah battery that allows two hour active operation.
During the test the robot is localized by the OptiTrack motion capture system. On the top of the robot four infra-red reflecting markers were mounted.
Wheel velocity control signals were scaled down when their value(s) exceeded the limit. This limit was set to 12rd/s, while the physical limitation of actuators is 24rd/s. The lower value of the limit prevented robot wheels from longitudinal slip. The obstacles were known a priori to prevent the influence of measurement inaccuracies on the experimental results.
A special scaling procedure is applied to the wheel controls. The desired wheel velocities are scaled down when at least one of the wheels exceeds the assumed limitation. The scaled control signal u s is calculated as follows:   [20] where and where ω r , ω l denote right and left wheel angular velocity, ω max is the predefined maximal allowed angular velocity for each wheel. This scaling procedure preserves the direction of motion of the mobile platform.

New Algorithm Comparison
This section discusses results of experiments. In all the presented cases the desired state was  Figures 2b and 3b show time graphs of position and orientation coordinates. It can be seen that the new algorithm drives system error to zero in 15s while the algorithm from [20] requires 30s to reach this state. The controls generated by the new algorithm are larger by the order of magnitude (Figs. 2c and 3c). As a result, also the controls of robots wheels are much higher (Figs. 2d and 3d). After transformation by the scaling procedure the control signal goes into saturation in the beginning and stays in this state until 12.5s (Figs. 2e and 3e). In the reference algorithm control was saturated a few times but only during short time intervals. To summarize, both algorithms produce a similar path shape but the new algorithm results in twice faster convergence. As shown in Kowalczyk [5], for the initial location further from the origin the improvement may even be much better. In some cases, the new algorithm works properly even in the cases in which the reference algorithm may be considered useless. Note that velocity limits on actuators also reduce the observed improvement. Applying the new algorithm is more beneficial for larger κ that must be increased as narrower passages between obstacles exist in the task space.  Figure 4b presents time graphs of position and orientation coordinates. They converge to the desired values in 17s. Figure 4c and d show time graphs of controls for the platform and wheels after scaling procedure. Figure 4e presents activity of the saddle point procedure.
As can be observed it is active by the first 0.5s of the experiment. The next experiment was conducted for s = 0.05. In this case the detection areas surrounding the saddle points are much larger, marked with dashed lines in Fig. 5a. Figure 5b shows time graph of robots coordinates. Greater s resulted in slower convergence (2s more). Figure 5c shows linear and angular controls for the platform. Figure 5d presents robot wheel controls after scaling. Figure 5e shows the saddle avoidance activation variable. As one can see, the robot was got into the avoidance area four times in the first 4s of the experiment.
The last experiment was conducted with the saddle point avoidance procedure turned off. Figure 6a shows the (x, y)-path. Figure 6b presents the initial part of the path. It can be noted that the platform oscillates around the saddle point and finally leaves its surroundings and goes to the desired location. Figure 6c shows the time graph of the position and orientation coordinates. Note that largest oscillation amplitude can be observed in orientation signal (±0.25rd). After 16s the robot leaves the trap. Oscillations are also observed in angular velocity control signal (Fig. 6d -dashed lines). Figure 6e shows wheel controls after scaling. By the first 16s wheel controls switch between positive and negative velocity limits.
Note that whether the robot was driven left or right side of the obstacle depended on the time variable in Eq. 27. The best convergence time (that was mostly dependent on the time of leaving the saddle point) was obtained for s = 0.01. For much larger or smaller values of s the convergence was slower.

Conclusions
Navigation function algorithms are used for set-point control of the vehicles moving in 2D and 3D environments, with and without nonholonomic constraints, with sphere-shaped obstacles, star-shaped obstacles and more complex environments in which obstacles can be modeled as trees-of-stars. These methods can be tuned to ensure one global minimum of the NF; however, in general they do not guarantee fast convergence to the target. This paper proposes a rapidly converging NF control algorithm. The convergence of the closed-loop system is analyzed. The advantages of the new algorithm are significant for greater values of the κ parameter that must be increased if narrow passages between obstacles exist in the task space. The method was tested by experiments on an actual robot. It provides not only better convergence time, but also rapid convergence in cases in which other methods produce questionable results. The experiments presented illustrate clear improvement. The paper also includes experimental verification of the robots behavior in the saddle point. It was tested for the saddle point procedure turned off and for two different sizes of the saddle point detection regions.
Open Access This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http:// creativecommons.org/licenses/by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made.
Publisher's Note Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.