Robot Path Optimization Based on a Reference Model and Sigmoid Functions

The purpose of this study is to minimize the arc length for the path described by the model of a robot platform when the path is constrained to have smooth transitions given by sigmoid functions. The optimization required a proof of stability for the resulting control law, the selection of the best sigmoid function among ten functions, and the definition of two gains necessary to parameterize the control law. The optimization was carried out by simulating the system under several kinematic and dynamical conditions, and the best sigmoid function was a hyperbolic tangent. Thus, the motion control first implied the simulation of a reference model to define an optimal path, and later the control of an actual robot platform, which followed the optimal path. The use of the optimized path reduced the complexity of the controller while allowing natural and intuitive paths for the robot platform.


Introduction
Path optimization has been a growing area in motion planning for robot platforms.Optimization looks for the best route from point A to point B under certain constraints such as energy consumption, speed limits, or obstacle avoidance [1][2][3].Path optimization techniques encompass pseudospectral optimization [4], elastic bands [5], or bioinspired algorithms such as ant colony optimization [6] among many others.Applications include but are not limited to aerial vehicle navigation [7], underwater vehicle command [8], and motion of robot platforms [9].Almost every application requiring motion can benefit from optimization, which offers potential savings in energy and more natural and intuitive paths for devices.
The work in [10] proposes the use of a smooth control law to define the path of a robot instead of limiting the motion planning to define the goal, as in traditional mobile robotics.This control strategy divides the kinematics of the model into a fast component that defines the heading angle of the robot and a slow component that sets the forward speed.The resulting trajectory is intuitive and comfortable, making it suitable for applications such as motion planning for a wheelchair.The authors test a single sigmoid function, for which the application of the control law requires the definition of two constants (k 1 and k 2 ).The values of these constants were determined by experimentation; thus, their selection did not result from an optimization process as in the present paper, in which the optimization uses ten sigmoid functions with the goal of minimizing the arc length.
Most of the works in motion control for robot platforms divide the analysis of the plant in two.For instance, the work in [11] proposes a continuous and smooth control law.In this case, an outer control problem defines the angular velocity in terms of radial and angular errors, and also a constant k 1 .The inner control defines the velocity of the robot in terms of the outer control part, errors in angular velocity and a constant, k 2 .The paper presents a proof of stability, but the work lacks application.Nevertheless, simulations demonstrate the performance of the control law, which requires a long time to stabilize.
The applications of path optimization are numerous.One of the most futuristic applications is the UAV, particularly the PAV (Personal Aerial Vehicle), interest in which has increased as a result of the development of aerial space technology.For instance, the work in [12] presents a methodology to define the best route for a PAV in order to minimize probabilistic risks.Thus, the best path reaches point B after avoiding one or more obstacles, for example, a no-fly zone or a storm.This paper uses ten sigmoid functions, instead of a single function as in [10], and experimentally looks for every possible kinematic condition for pairs of departure and arrival angles of a platform model, in order to choose the three sigmoid functions that produce the shortest paths.These paths require smoother control laws, which in addition prove more intuitive than the paths that result from the application of a traditional PID controller to set a path.This paper also proposes a method to choose the best sigmoid function, which consists of setting a path that forces the platform to express a broad dynamic range.The validation in [10] consists of applying the definition of the single sigmoid function to the control of a wheelchair, but there are no comparisons with other techniques in order to evaluate the control performance.In contrast, this paper compares every result with traditional PID control of a kinetic and dynamic model, and also presents the control of an actual robot platform, a National Instruments robot platform (DaNI 2.0).This paper is organized as follows.Section 2 presents the design of a path for a robot based on sigmoid functions, including a proof of stability using Lyapunov stability theory.In Section 3 an optimization process comes up with the sigmoid function that generates the shortest arc length produced by the motion of a robot under a wide range of dynamic conditions.The definition of a path for the model of a robot platform in Simulink serves to test the optimal sigmoid function.This model includes kinematics and dynamics, as shown in Section 4. Finally, in Section 5, the optimal sigmoid function defines a reference path for the DaNI 2.0 robot.

Model and control laws
Traditional motion control defines the coordinates of a target, ‹x t ,y t ,θ t ›, as shown in Figure 1, leaving intermediate points unspecified.In contrast, the control scheme in this paper defines the whole path from an initial coordinate to the target.In addition, this control law requires a reference model, which tracks the target describing the route for the actual robot platform.This reference may be the model of the robot platform itself.This case guarantees that the actual platform can follow the reference path.One of the main features of the path defined in this paper is its smoothness.As a result, the reference model, located at ‹x m ,y m ,θ m ›, follows the dotted line in Figure 1.The coordinates of the actual robot platform in Figure 1 are ‹x,y,θ›.Thus, the reference model tracks the target, while the robot follows the model.
The control law for the reference model aims to reduce the distance r i and at the same time transform the angular position from θ m into θ t .The work of the robot platform controller consists of minimizing the distance r c between platform and reference model while changing the angle θ in such a way as to reduce its difference with θ m .The model of the platform corresponds to a differential wheeled mobile robot in a 2D environment, as shown in equation (1).This model may be divided into two parts for analysis.The first part corresponds to the first two variables, r i and θ i .The dynamic of this part is slow compared with the dynamic of angle δ i .Thus, the control strategy consists of defining ω m in such a way that δ i has an appropriate value.Thus, once δ i is controlled, the remaining part of the control process consists of reducing the radius r i , for instance, by defining a constant speed v m , as in this paper.

( ) ( ) ( )
In this section, we define ω m such that transitions in δ i remain smooth across the whole reference path.The work in [10] proposes the use of an arctan(x) function as a candidate to define the smooth transitions.Extending this work, this paper compares ten sigmoid functions in order to find the best function to minimize the arc length travelled by a reference model between the initial coordinates ‹x,y,θ› and the final coordinates ‹x t ,y t ,θ t ›.
The constant k ii serves to generate ten functions from these five.
( ) ( ) The first five functions, in the upper part of Figure 2, share a unitary slope at x = 0, which can be achieved by properly defining k ii , for instance the first function atan = arctan(x).
The other five functions in the bottom of Figure 2, to complete ten trial functions are also based on the definitions in equation ( 2), but all these reach 0.99 at x = π.A normalization process computes adequate gains of k ii in each case, for instance atan1 = (1/1.5692)arctan(18.4338x).
The other functions follow the same rules.The use of a Lyapunov candidate function as in equation (3) serves to demonstrate that the sigmoid functions produce stable control laws for the slow part of the model (r i and θ i ).The control angle is defined as shown in equation ( 4).
( ) The derivative of the function V is written in equation ( 5).
( ) An observation of the sigmoid functions sf1, sf2, sf3, sf4 and equation ( 5) allows the analysis in equation ( 6). ( ( ) The rate of change of V (i.e., dot V) is negative definite, which guarantees asymptotic stability.The sigmoid function sf5 is negative semidefinite (NSD), thus establishing stability according to Lyapunov's second method.
Now we define the control law for the steering as the difference between δ i and the sigmoid function, as shown in equation (7).Thus, given the model in equation ( 1), the derivative of z corresponds to the expression in equation (8).
( ) sin If the rate of change of z has the opposite sign of z, as defined in equation ( 9), then we can guarantee stability of the control law.The final control law results from equating ż in equation ( 8) and equation ( 9), as shown in equation (10).
The robot model is controlled by defining a constant linear speed v m , while the angular speed ω m leads the model to reach the target coordinates.Thus, optimizing the motion path corresponds to finding k 1 , k 2 , and sf in equation (10) such that the arc length between an initial coordinate ‹x m ,y m ,θ m › and the target ‹x t ,y t ,θ t › is minimized.

Path optimization
The motion planning for the model of a robot platform requires defining the coordinate ‹x m ,y m ,θ m › at every instant in time.Therefore the design of an optimal path looks to find those coordinates that minimize the trip of the platform under certain constraints.The minimization process will exclusively consider paths that terminate within 1° of θ t .In addition, all simulations define the linear speed of the platform v m in 0.2 m/s.Furthermore, in order to avoid instabilities, the speed falls to v = r i /5 if r i is smaller than 5 cm.The arc length computation uses only the length for the constant speed path.The model of the platform was simulated in discrete mode using a sampling time of 20 ms and a final time equal to 30 s.
The optimization comprises two stages: 1) finding the three sigmoid functions that produce the shortest paths among the ten trial functions; 2) finding the sigmoid function that produces the shortest arc length, and also the corresponding pair ‹k 1 , k 2 › that minimizes the path.In the first stage, combinations of departure-arrival angles are tried, choosing the sigmoid function that minimizes the arc length for each trial.A whole set of values k 1 and k 2 are tried for each function.Once each pair of angles has been tried, the three functions that repeat the most are chosen as winners.In the second stage, and using the path defined below and the three best functions of the first stage, the sigmoid function that minimizes the arc length is chosen, and also the pair k 1 and k 2 that produces the minimum length as the best pair of gains.The first stage starts with setting a departure angle (θ 0 ) and an arrival angle (θ 1 ) in the range (-π, π], for instance θ 0 = 3π/4 and θ 1 = -3π/4, as shown in Figure 3. Every initial coordinate has the form ‹0,0,θ 0 ›, whereas the final coordinate equals ‹1,1,θ 1 ›.Then, for each pair ‹k 1 , k 2 ›, the optimization process finds the sigmoid function that generates the shortest path.This process forms an elevation map, as shown in Figure 4, where each pixel represents the sigmoid function with the shortest path associated to each coordinate ‹k 1 , k 2 ›.
The winning function of all shown in Figure 4 is erf.This sigmoid corresponds to the function that minimizes the arc length across the whole plane.The ranges for ‹k 1 , k 2 › were chosen experimentally, given that, in general, k 1 defines the arrival angle, whereas k 2 defines the curvature of the whole path.Thus, the range for k 1 was chosen to allow errors within 1° for θ 1 , and the wide range for k 2 assures that the best curvature is within permissible values.Finding the shortest arc length given a single pair 0 , θ 1 ›, as shown in Figure 4, presents a very restricted case.A more elaborate minimization uses any possible departure and arrival angles, as shown in Figure 5.Each pixel in this figure summarizes a whole behaviour, such the one in Figure 4, in one single point, and corresponds to the sigmoid function that minimizes the path length across the plane, regardless of k 1 and k 2 values.The best sigmoid in Figure 5, also the result of the first stage, covers the maximum area of the elevation map.The three best sigmoid functions are erf, alf, and tanh1.These three functions cover more than 70% of the plot in Figure 5.
The second stage of the optimization process uses the three best sigmoid functions from the first stage to find the best pair ‹k 1 , k 2 › as well as the best function.Instead of reading those constants from Figure 5, in this stage a simulated platform follows an imaginary path, forcing the platform to express a broad range of dynamics.Thus, the path is formed by a set of segments of unit length, as shown in Figure 6.
The initial coordinate in Figure 6 is ‹0,0› in the xy-plane, and the final one is ‹3.26,-2.73›.The transition angles between consecutive segments change from -150° to 150° in increments of 30°.This stage defines the angle to cross every intersection as perpendicular to the line that bisects two consecutive segments.The value for k 1 varies from 0.2 to 2, while the value for k 2 varies from 1 to 150.The best result in this second stage was achieved using tanh1 with k 1 = 1, and k 2 = 5.The arc length for this arrangement was 13.78 m, and 13.89 m and 13.83 m for erf and alf, respectively.

Application
This section uses a model of a robot platform as the actual plant, whereas the next section uses the commercial platform DaNI2.0.In order to approach normal operation conditions, the model includes not only the kinematics, given by equation ( 1), but the dynamical description for some parts of the platform.The model of each wheel includes a control loop governed by a PI with gains kp = 1 and ki = 1, and a transfer function H(s) = 1/(0.2s+ 1) representing the dynamic of each motor, as shown in [13].The variable speeds v R and v L in equation ( 11) represent linear speed of the right and left wheel, as defined in [14], and b, the distance between wheels, equals 0.37 m.
( According to the scheme in Figure 1, the reference model defines a smooth path to reach the target ‹x t ,y t ,θ t ›.The goals for the reference model consist of shrinking r i to zero and changing θ m to approach θ t .At the same time, the robot platform follows the path described by the reference model as closely as possible.Therefore, the control law for the platform actually may require two laws.The distance r c should go to zero, while θ approaches θ c .Definitions for variable r c and θ c correspond to the ones given in equation (12).
Once the initial and final coordinates for each segment of the path have been defined, the application of the reference model concept defines ‹x m , y m › for each instant in time.The first control algorithm (PID 1) controls the θ angle, while the second (PID 2) regulates the distance between platform and reference model, r c .These are traditional PID controllers, where the actuating signal, u, equals kp(e) + ki(ʃe) + kd(de/dt), with input e.The error e = θ c -θ for PID 1, and e = r c for PID 2. Experimental gains for both controllers are 0.1, 0, 0 for angle control, and 0.25, 0, 0 for radial control.The gains when the platform follows a reference path are 0.2, 0, 0 for the angle and 0.5, 0, 0 for the radius.The differences between these gains find their basis in the error for each strategy.In classical control, the goal is far from the robot, while the platform is close to the reference model.Each controller, including the one that represents the dynamic of the wheels includes natural limits, for instance, maximum speeds and maximum angles.The constants x 0 , y 0 , θ 0 , b in Figure 7 are the initial conditions for the robot.Figure 8 shows a simulation of half the path defined to find the optimal values of k 1 and k 2 .The robot platform follows the reference model closely, producing a short path that assures crossing for the points marked with numbers in Figure 8, which may be seen as landmarks according to [15].In contrast, the same platform, using the same controllers, but targeting only the end of each segment, results in strong transitions and a longer path, which also results in a longer trip duration.Additionally, the robot cannot pass through point 3, because it receives the coordinates for point 4 before reaching point 3.

Real-time control application
The experimental path optimization was tested using the robot DaNI 2.0 from National Instruments.This threewheeled, pre-assembled, mobile robot platform has sensors, motors, and an NI 9632 board mounted on top of a robot base.The control algorithm looks for a match between θ and θ c , every 50 ms, while maintaining the linear speed constant at 0.2 m/s.
The motion control starts by defining the reference path, given a model of the platform as defined in equation ( 1) with the control rule of equation (10).This process is performed before the actual motion of the robot.Every pair of consecutive sub-goals (numbered from 1 to 7 in Figure 9) serves as initial and final sub-goal for defining r i .The resulting ‹x m , y m › coordinates are saved in a text file.The communication with the robot as well as the estimation for position and angle (using signals coming from the robot's encoders) are wired into Labview.However, the control algorithm is programmed in Matlab.Thus, every 50 ms, Labview calls a function which is run in Matlab.This function reads the current position ‹x,y,θ› as well as an index for the current time from Labview, and the corresponding pair of coordinates ‹x m ,y m › from the text file.The outputs for the function are the left v L and right v R speeds, which are later transformed into linear speed v and angular velocity ω in Labview.These last two are the parameters used to control the robot in Labview.Matlab internally computes θ c based on ‹x m ,y m › and ‹x,y› according to equation (11).Then the actuating signal is defined as a proportion of the difference between θ c and θ, plus the constant 0.2 representing the nominal linear speed.The same script records x, y, θ, θ c , v L , v R , in order to run comparisons and also to tune the P controller.
The path that was used to test the motion of the robot platform is the same as in Figures 6 and 8.The only difference is that the distance between one landmark and the next is two metres, instead of one.The results in Figure 9 show that the reference model presents a smooth and easy path for the robot.This may be guaranteed by two conditions: 1) the maximum error in angle is lower than π/6 radians, which appears at the beginning of the motion; 2) the average error is less than π/15 radians.On the other hand, a P controller was enough to set the dynamic, which shows that a very low complexity for the controller is required to control the system.
The upper panel in Figure 9 shows the path defined by the reference model and the actual path of the platform.Excluding the behaviour immediately after landmark 3, there is a good match between these paths.The reference model defines an ideal position x m and y m at every instant in time, as shown in the middle panel of Figure 9.The lower panel shows the error in the control variable θ.

Conclusions
A smooth motion allows a robot platform to express natural dynamics, avoiding strong transitions that force the robot and controller beyond their design limits.Thus, the use of a reference model to lead the motion of a robot resulted in strong simplifications in the motion controller's complexity.The reference model that was used in this paper comes from a kinematics model of the plant.Thus, future studies may use more elaborate models, including aspects beyond kinematics, such as dynamics, among many other topics.
The optimization in this paper seeks the minimum arc length generated by a kinematics model of a robot.The sigmoid function optimal for performing smooth motion was a hyperbolic tangent, normalized to reach amplitude 0.99 when the angle is π radians.Optimal gains for the controller were 1 for k 1 and 5 for k 2 .In addition, the experiments with an actual robot platform use a sampling time of 50 ms.Subsequent studies may consider additional aspects in the optimization, such as energy consumption, elapsed time, and especially variable linear speeds, which may help to deal with centrifugal force as explained in [16].

Figure 2 .
Figure 2. Sigmoid functions.Slopes are normalized in the upper plot, amplitudes and steady-state values in the lower.

Figure 3 .
Figure 3. Motion of the robot platform between two points using five sigmoid functions

Figure 4 .
Figure 4. Best sigmoid functions for pairs ‹k 1 , k 2 ›, for fixed depart and arrive angles

Figure 6 .
Figure 6.Result for the second stage of the optimization.Resulting path for tanh1, k 1 = 1, and k 2 = 5.

Figure 5 .
Figure 5. Best sigmoid function depending on departure and arrival angles

Figure 7 .
Figure 7.Control scheme for the robot platform

Figure 8 .
Figure 8. Position control with and without a reference model

Figure 9 .
Figure 9. Motion control of a DaNI 2.0 robot platform