Nonholonomic Motion Planning Strategy for Underactuated Manipulator

This paper develops nonholonomic motion planning strategy for three-joint underactuated manipulator, which uses only two actuators and can be converted into chained form. Since the manipulator was designed focusing on the control simplicity, there are several issues for motion planning, mainly including transformation singularity, path estimation, and trajectory robustness in the presence of initial errors, which need to be considered. Although many existing motion planning control laws for chained form system can be directly applied to themanipulator and steer it to desired configuration, coordinate transformation singularities often happen. We propose two mathematical techniques to avoid the transformation singularities. Then, two evaluation indicators are defined and used to estimate control precision and linear approximation capability. In the end, the initial error sensitivity matrix is introduced to describe the interference sensitivity, which is called robustness. The simulation and experimental results show that an efficient and robust resultant path of three-joint underactuated manipulator can be successfully obtained by use of the motion planning strategy we presented.


Introduction
In the past few years, nonholonomic motion planning has become an attractive research field.Much theoretical development and application have been exploited.This paper can be viewed as the further study on the basis of [1], which proposed -joint underactuated manipulator [2].And inspired by [3][4][5], its physical design is carried out mainly focusing on the kinematic model with triangular structure, which can be converted into chained form and achieve the control simplicity.
Although motion planning control law can steer the chained form system to desired state, there exists transformation singularity while chained form path is transformed back into actual coordinates.Even in the absence of singularity, planning nonholonomic motion is not an easy task.Transformation singularities add a second level of difficulty: we must take into account both transformation singularities and nonholonomic constraint.In order to solve transformation singularity problem, the path in chained form space should remain in the singularity-free regions.In other words, when a path does not belong to those regions in chained form coordinate, transformation singularity will appear in actual coordinate.Therefore, transformation singularity avoidance is equivalent to obstacle avoidance in chained form space. Singularity regions can be checked by diffeomorphism of chained form conversion, but their shape and location depend on the mechanical structure and cannot be generically described for an arbitrary mechanism [6][7][8].Most of the techniques developed for path planning are classified as geometric path planners [9][10][11].An alternative methodology of obstacle avoidance is artificial potential field (APF), which considers the robot as a particle or rigid body without constraints [12], and significant effort has been devoted to elimination of local minima [13,14].References [15][16][17] indicate that a control law with topological property has better obstacle avoidance capability by setting a serial of intermediate points and applies the sinusoidal inputs law to steer the tractor-trailer system.However, some motion planning laws, which can achieve a better resultant path than sinusoidal does for the underactuated manipulator, have no topological property, such as time polynomial inputs.
To decrease the trajectory oscillations and increase precision for motion planning in practical experiments, two indicators are defined to evaluate the planning path performance of approximating linear path and control precision.Then, we propose the notion of initial state errors robustness, which is used to evaluate sensitivity in the presence of initial errors.
This paper is organized as follows.Section 2 gives a brief introduction for underactuated manipulator.Section 3 illustrates the methods for transformation singularity avoidance of the three-joint prototype.Sections 4 and 5 introduce the path estimation and robustness, respectively.Conclusions and future works are given in Section 6.

An Introduction to Underactuated Manipulator
Figures 1 and 2 show the structural diagram and photo of the prototype: three-joint underactuated manipulator with two actuators.Obviously, the configuration of the -joint underactuated manipulator has  + 1 dimensions, which are determined by angular displacement   ( = 1, 2, . . ., ) of each joint and the angle  1 of a horizontally mounted actuator.And another vertically mounted actuator connects with joint 1.
Figure 3 shows the detailed structure of joint  of the underactuated manipulator.Each joint except the last one has two sets of same size friction disk transmission mechanisms [18].P1, P2, and P3 represent the rolling without slipping contact points between disk  and disk .Disk   with radius  1 rotates around the fixed axis with a given angular velocity φ  which drives disk   to rotate.The angular velocity α  of disk   is divided into two parts; one is transmitted into disk  +1 as angular velocity input φ +1 through a set of bevel gears and spur gears; the other drives the next joint  + 1 to rotate through a set of friction disk mechanisms and timing belts.φ

𝑖 and β
represent the angular velocity of disk    with radius  2 and disk    , respectively. and  3 express the distance from rotation axle of disk  to rolling contact points P1, P2, and P3.
The underactuated manipulator's nonholonomic constraints are due to the rolling contact between disk  and disk .By setting configuration variables  the kinematic model can be described as follows (for detailed mathematical derivation, see [2]): where = cos   .This formulation of the kinematic model has the triangular structure, which can be converted into chained form.Equations (1) can be expressed as follows: where  ≜ [ −1 ,  1 , . . .,   ]  , 1 () and  2 () are vector fields.This system is said to be driftless; that is to say, the state of the system does not drift when the controls are set to zero.

Conversion into Chained Form.
Inspired by [3], Sordalen shows conversions of the kinematic model of a car with  trailers.The underactuated manipulator is designed to convert into chained form and achieve control simplicity, although a little complication should be added to mechanical structures.
Nonlinear coordinate transformation and input feedback transformation of kinematics model of three-joint underactuated manipulator can be expressed as follows: where    = sin    and    = cos    .Reference [2] has proven nonholonomy and controllability of the -joint underactuated manipulator.Then, we will discuss that conversions are diffeomorphic.Inspired by [3], there is Theorem 1 as follows.
Theorem 1.The chained form conversions of underactuated manipulator are diffeomorphic if and only if the Jacobian  +1 (  ) of kinematic model in (1) is nonsingular: where We do not show the detailed proof of Theorem 1 here.Therefore, Equation (7) shows that the coordinated conversions for -joint manipulator are diffeomorphic except at   = 0. Therefore, the kinematic model can be diffeomorphically convertible into chained form in the subspace   ∈ (−/2, 0)∪ (0, /2).

Three-Joint Underactuated Manipulator Motion Planning.
There are two major control schemes for chained form.One is open loop control, and the other is feedback control.A major advantage of open loop control is that solutions for practical applications with low computational cost can be provided.Furthermore, chained form of feedback control has two drawbacks: one is that stabilizing chained system to the nonzero configuration is extremely difficult in practice; the other is that obstacle or singularity avoidance problem is hardly solved by some form of feedback control laws because of no specified extent of overshoot.
There are many existing open loop controllers for the chained form, such as sinusoidal inputs, time polynomials inputs, and piecewise constant inputs.Any one of the control laws can be applied to the underactuated manipulator.Since time polynomials inputs law is easily obtained by solving simple algebraic equations and generates a better resultant path for three-joint underactuated manipulator, it is given as follows: Although the control law polynomial inputs can steer chained form state  to their desired configuration, there is no guarantee that this path, when mapped back into the actual coordinate, will avoid the transformation singularity.That means we must check every path and ensure the existence of every state variable in configuration space.If a singularity does really happen, some measures should be taken to find a valid path.
The transformation singularity happens at   = 0 and trajectories in the chained form space should satisfy the conditions, which can be directly checked from ( 4) and (7).Actually, the primary cause of transformation singularity of three-joint manipulator is due to  2 > 0 because expression of  2 not only is more complex but also is affected by  2 .The two mathematical techniques avoiding transformation singularities are proposed in the case of  2 > 0 as in the following section.

Constraint Point Method.
In order to avoid transformation singularity, the chained form state, existing singularity region, is modified by setting some points which are called constraint points.The constraint point method is just used to avoid the transformation singularity in the path.The polynomial inputs law with  states and  constraint points for chained form is established as follows: where  ∈ {0, 1, . . .,  − 2},  ∈ {1, 2, . . ., }, and   is coefficient.Constraint points (  ,   (  )) are applied to chained form state   () which can be expressed as follows: Obviously, the state () is function of initial conditions (0) as well as  +  undetermined coefficients ( 0 ,  0 , . . .,  −2 ,  1 , . . .,   ).Equations (10) inputs will steer the chained form system from   (0) to   () in finite time  and pass through constraint state   (  ) at time   .It can be seen clearly that the existence and uniqueness of solutions to (10) can guarantee the chained form state () satisfying nonholonomy at constraint points: where   (  ) is point on the   () curve at time   ∈ (0, ) and the matrix () has the form Cramer law guarantees existence and uniqueness of solutions to (11) if and only if matrix () is nonsingular for  0 ̸ = 0.
In Figure 6, simulation 1 shows three-joint angular displacement versus time.It is clear that the joint angles can reach the desired configuration smoothly without any transformation singularities.

Input Control Parameter Adjustment Method.
Although constraint point method has low numerical computational cost, the choice of constraint points sometimes needs a lot of trials for transformation singularity avoidance.Inspired by overparameterization of sinusoidal inputs in [19], the input control parameter adjustment method is presented.The actual coordinate of underactuated manipulator has 4 dimensions [ −1 ,  1 ,  2 ,  3 ]  , and our goal is to steer 3dimensional state variable   () to   (),  ∈ {1, 2, 3}.Since  0 can be expressed as  0 = ( 2 () −  2 (0))/, we can deal with the polynomials input law by choosing an appropriate  0 in singularity-free regions to achieve transformation singularity avoidance.This method is applied to three-joint underactuated manipulator.
As mentioned, the main condition of avoidance transformation singularity is Equation ( 13) can be satisfied if maximum value of  2 () keeps negative.The time corresponding to stagnation point of  2 () can be expressed as follows: From ( 11),  2 ( 2 ) can be represented as function of control parameter  0 : Consider ( 15) as a map: which maps input control parameter  0 into the maximum  2 ( 2 ).Obviously, the mapping Φ  0 is surjection (many-toone) since the choice of  0 for solving  2 ( 2 ) is not unique.The range of value  0 can be determined by solving roots of (15).In Figure 7, simulation 2 shows joint angles versus time for three-joint underactuated manipulator.The joint angles of initial configuration and terminal configuration are (0) = [5 ∘ , 5 ∘ , 5 ∘ ]  and () = [30 ∘ , 30 ∘ , 30 ∘ ]  , respectively.Input control parameter  2 = 34 ∘ since there is no transformation singularity if and only if  2 is always chosen in the range of 32.35 ∘ to 35.44 ∘ .It can be seen that the joints angles reach the desired configuration after the setting time.
A major advantage of this approach is that analytical solution of control parameter  0 can be calculated in transformation singularity-free region.And the order of polynomials does not increase in comparison to constraint point method since a valid path without singularity depends on choosing suitable value of  0 .Comparing overparameterization methods the choice of input control parameter  0 is not arbitrary but definite.
This method leaves one parameter free and searches for a value in this parameter space, which guarantees that the path belongs to transformation singularity-free region.However, according to Galois theory, closed-form solution does not exist for polynomial order higher than 4. In other words, there is no analytical solution but numerical one with high computational cost for  0 when underactuated manipulator has 5 or more joints.

Nonholonomic Path Estimation
Although a valid path without transformation singularity can be generated in the previous sections, actual shape of path shows a detour to desired configuration.Meandering trajectory with high amplitude causes the control losing in practice.

Nonholonomic Path Evaluation Scheme.
A resultant path of underactuated manipulator must satisfy nonholonomic constraint, which is essentially nonlinear.A reference path is established to be a straight line connecting initial configuration with desired configuration in the configuration space.We define two indicators: approximation distance   and extreme point number   .The indicator   , which is the maximum value of vertical distance between the reference path and the point on the nonholonomic path, can evaluate approaching the reference path capability.  , which is the number of extreme points on the path, estimates the number of actuators rotation direction changes.
The nonholonomic path evaluation method in [5] estimates the distance between linear path and planned path and does not precisely describe the extent of meandering of the path.An evaluation scheme is proposed to decrease the numbers of changes of actuator rotational direction and errors from transmission of manipulator, such as backlash.
We make further explanation about two indicators for searching optimal path.The values of   and   are calculated within control parameter  0 range which guarantees that the path belongs to transformation singularity-free region.In practice, the performance for motion planning of three-joint manipulator is mainly decided by   .With slight increment of   , the errors of transmission parts of underactuated manipulator will be increased significantly.Therefore,   is more significant than   for performance of nonholonomic motion planning.In conclusion, a search in parameter space  0 is carried out to calculate the above two indicators with priority to   .In other words, the minimum value   is searched in the min(  ) region.
To verify the usefulness of the evaluation scheme, simulation 3 has been carried out for three-joint underactuated manipulator.Let  2 be free, and initial configuration and desired configuration are (0) = [10 ∘ , 10 ∘ , 10 ∘ ]  and () = [30 ∘ , 30 ∘ , 30 ∘ ]  , respectively.The parameter  0 , which is determined by   = 15.978∘ and   = 1, is 36.6.Simulation 3 result is shown in Figure 9.In simulation 3, we just estimate the first joint angle trajectory instead of all of them because the underactuated manipulator is an open-chain mechanism and the actuator is connected with joint 1, as shown in Figures 1 and 2. Equations (4) reveal that inverse mapping from  2 to  1 not only is more complicated than others but also depends on  2 .The simulations (1, 2, and 3) show that the  1 trajectory has more oscillation in comparison with  2 and  3 .
Figure 10 illustrates the experimental result.In addition, there is existence of 5 ∘ maximum error on the  3 curve.
The backlash at the bevel gear, the low stiffness of the long shaft, transmission parts with low resolution, and the lack of drive torque under guaranteeing rolling without slipping condition would have caused these errors.In addition, the error of  3 is obviously larger than the others because of cumulative error of transmission system.

Nonholonomic Motion Planning Robustness
Since motion planning is an open loop control, a resultant path will be affected by various errors.Although the motion planning using time polynomial inputs shows the satisfactory result in the previous section, actual path needs to be checked in the presence of errors.

A Simulation with Initial Condition Error for Optimal
Path. Figure 11 illustrates that a simulation in the presence of initial errors (0) = [9.5 ∘ , 10.5 ∘ , 9.5 ∘ ]  , which implies each joint angle, has 0.5 ∘ error.Other parameters are the same as simulation 3.As shown in Figure 11, simulation 4 result shows   = 57.57∘ and   = 2 of  1 .The maximum distance   in simulation 4 is 3.6 times higher than that in simulation 3, and the rotation direction of joint 1 will change twice.It is easily concluded that the actual performance of motion planning is greatly affected by 0.5 degrees in the presence of initial errors.Simulation 4 results illustrate the lack of robustness and high sensitivity with initial condition errors.

Initial Condition Error Sensitivity Analysis.
In order to specify effect of initial errors, the expression of () can be obtained while the time polynomial inputs law is applied to chained form system existing initial error: where  = { , } ∈   ×   and  = { , } ∈   ×   : where   is initial condition error.The effect is calculated along  directional derivatives by the following Jacobian matrix: where  The matrix   is lower triangular and diagonal elements equal 1, and the following relation can be expressed: The parameter  0 just depends on initial condition  −1 and is independent of the given boundary condition.From (22), we can conclude that The   is independent of the given initial condition (0) and has correlation with ,  0 .Since Jacobian matrix   describes the sensitivity along  direction, -norm ‖  ‖  of   is defined initial condition error sensitivity matrix.

Robustness of Motion
Planning.The computed result of ‖  ‖  is shown as in Figure 12 for three-joint underactuated manipulator.The maximum of ‖  ‖  at  =  implies the highest sensitivity under the disturbance of initial error conditions.It can be concluded that the lower  0 is in favor of reducing the effect of the initial condition error.According to path estimation scheme in Section 5.1, the minimum  0 , which is determined under the condition of   = 1, is 35.8.Other parameters except  0 are the same as simulation 3. The computed result, as shown in Figure 13, indicates that   = 23.52 ∘ in simulation 5.
The purpose of tuning  0 is to improve the robustness of nonholonomic system and decrease the sensitivity for initial condition error at the expense of   increase.Simulation 6 with the same initial condition error is carried out for threejoint underactuated manipulator.The simulation result (  = 26.11∘ and   = 1), as shown in Figure 14, demonstrates the advantage of the robustness and less sensitivity for initial condition error.The usefulness of motion planning robustness is experimentally verified as shown in Figures 15 and 16.Although there are little differences between simulations (5,6) and experiments (2, 3), Figures 15 and 16 are similar to Figures 13 and 14, respectively.Actually, the experimental results are quite acceptable if the errors of mechanical structure are neglected.

Conclusions and Further Works
For solving transformation singularity problem of threejoint underactuated manipulator, we present two simple and effective mathematical techniques to find a valid path in actual coordinate.Then, a motion planning strategy is developed to estimate efficiency and open loop robustness of resultant path.This strategy is dealt with by two steps: the first step is to estimate the efficiency and capability of linearization approximation for a resultant path.Although nonholonomic path estimation scheme can generate a satisfactory resultant path, actual trajectory is greatly affected in the presence of initial error.The second step is to generate a robustness trajectory based on initial condition error sensitivity analysis.The simulation and experimental results show that the motion planning strategy can improve motion planning performance for three-joint underactuated manipulator.
With increase of number of joints, the kinematic model will be complex and the conversion into the chained form will become ill-conditioned.The next work is how to improve the mechanism structure and find valid path for high dimension system.

Figure 2 :
Figure 2: Prototype of three joints and two actuators.

Figure 3 :
Figure 3: Structure diagram of joint  mechanical system.

Figure 5 :Figure 6 :
Figure 5: Chained form state  with one constraint point.

Figure 8 :
Figure 8: Illustration of the indicators of evaluation.