# Nonholonomic Motion Planning Strategy for Underactuated Manipulator

Authors: Liang Li; Yuegang Tan; Zhang Li
Journal: Journal of Robotics
 (2014)

---

## Abstract

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 the manipulator 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.


---

## Body

## 1. 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 n-joint underactuated manipulator [2]. And inspired by [3–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–8]. Most of the techniques developed for path planning are classified as geometric path planners [9–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–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. Section2 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.


## 2. An Introduction to Underactuated Manipulator

Figures1 and 2 show the structural diagram and photo of the prototype: three-joint underactuated manipulator with two actuators. Obviously, the configuration of the n-joint underactuated manipulator has n
+
1 dimensions, which are determined by angular displacement θ
i
(
i
=
1,2
,
…
,
n
) of each joint and the angle φ
1 of a horizontally mounted actuator. And another vertically mounted actuator connects with joint 1.Figure 1
Structural diagram of three-joint underactuated manipulator.Figure 2
Prototype of three joints and two actuators.Figure3 shows the detailed structure of joint i 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 A and disk B. Disk B
i with radius r
1 rotates around the fixed axis with a given angular velocity φ
˙
i which drives disk A
i to rotate. The angular velocity α
˙
i of disk A
i is divided into two parts; one is transmitted into disk B
i
+
1 as angular velocity input φ
˙
i
+
1 through a set of bevel gears and spur gears; the other drives the next joint i
+
1 to rotate through a set of friction disk mechanisms and timing belts. ϕ
˙
i and β
˙
i represent the angular velocity of disk B
i
′ with radius r
2 and disk A
i
′, respectively. R and r
3 express the distance from rotation axle of disk A to rolling contact points P1, P2, and P3.Figure 3
Structure diagram of jointi mechanical system.The underactuated manipulator’s nonholonomic constraints are due to the rolling contact between diskA and disk B. By setting configuration variables [
φ
n
-
1
,
θ
1
,
θ
2
,
…
,
θ
n
]
T, the kinematic model can be described as follows (for detailed mathematical derivation, see [2]):
(1)
φ
˙
n
-
1
=
k
g
∏
j
=
1
n
-
2
cos
⁡
θ
j
·
u
1
≜
v
1
,
θ
˙
1
=
u
2
≜
v
2
,
W
⋮
θ
˙
n
=
k
n
cos
⁡
2
θ
n
-
1
p
n
θ
n
_
v
1
≜
f
n
+
1
θ
n
_
v
1
,
where θ
i
_
≜
[
θ
i
,
θ
i
+
1
,
…
,
θ
n
]
T, φ
˙
n
≜
0, i
∈
{
2,3
,
…
,
n
}, k
g
=
r
1
/
λ
R
2, k
i
=
λ
R
/
η
r
3
r
1
/
λ
R
i
-
1, η
=
β
˙
i
/
θ
˙
i
+
1, λ
=
α
˙
i
/
φ
˙
i
+
1, and p
i
(
θ
i
_
)
≜
k
g
∏
j
=
i
n
-
2
cos
⁡
θ
j.This formulation of the kinematic model has the triangular structure, which can be converted into chained form. Equations (1) can be expressed as follows:
(2)
q
˙
=
g
1
q
·
u
1
+
g
2
q
·
u
2
,
where q
≜
[
φ
n
-
1
,
θ
1
,
…
,
θ
n
]
T,
(3)
g
1
q
=
1,0
,
…
,
k
n
c
n
-
1
3
P
1
n
-
2
,
k
g
P
1
n
-
2
T
,
g
2
q
=
0,1
,
0
,
…
,
0
T
.g
1
(
q
) and g
2
(
q
) 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.


## 3. Transformation Singularity Avoidance

### 3.1. Conversion into Chained Form

Inspired by [3], Sordalen shows conversions of the kinematic model of a car with n 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:(4)
z
4
=
θ
3
,
z
3
=
∂
z
4
∂
q
4
_
f
4
_
q
3
_
=
k
3
k
g
c
2
3
,
z
2
=
∂
z
3
∂
q
3
_
f
3
_
q
2
_
=
-
3
k
3
k
2
k
g
2
c
1
2
c
2
2
s
2
1
,
z
1
=
φ
2
,
(5)
v
1
=
u
1
=
φ
2
,
v
2
=
3
k
2
2
k
3
k
g
3
c
1
4
2
c
2
s
2
2
-
c
2
3
u
1
+
6
k
2
k
3
k
g
2
c
1
s
1
c
2
2
s
2
u
2
,
where s
i
j
=
sin
j
θ
i and c
i
j
=
cos
⁡
j
θ
i.Reference [2] has proven nonholonomy and controllability of the n-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 Jacobianf
i
+
1
(
q
i
_
) of kinematic model in (1) is nonsingular:
(6)
∂
f
i
+
1
q
i
_
∂
q
i
θ
i
-
1
≠
0
=
-
k
i
sin
2
θ
i
-
1
p
i
θ
i
_
θ
i
-
1
≠
0
≠
0
,
where q
i
_
=
[
q
i
,
q
i
+
1
,
…
,
q
n
]
T
=
[
φ
n
-
1
,
θ
1
,
…
,
θ
n
]
T.We do not show the detailed proof of Theorem1 here. Therefore,
(7)
∂
f
i
+
1
q
i
_
∂
q
i
θ
i
-
1
=
0
=
-
k
i
sin
2
θ
i
-
1
p
i
θ
i
_
θ
i
-
1
=
0
=
0
.Equation (7) shows that the coordinated conversions for n-joint manipulator are diffeomorphic except at θ
i
=
0. Therefore, the kinematic model can be diffeomorphically convertible into chained form in the subspace θ
i
∈
(
-
π
/
2,0
)
∪
(
0
,
π
/
2
).


### 3.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:(8)
v
1
=
b
0
,
v
2
=
c
0
+
c
1
t
+
c
2
t
2
.Although the control law polynomial inputs can steer chained form statez 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θ
i
=
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 z
2
>
0 because expression of z
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 z
2
>
0 as in the following section.


### 3.3. 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 withn states and m constraint points for chained form is established as follows:
(9)
v
1
=
b
0
,
v
2
=
∑
i
=
0
n
-
2
c
i
t
i
+
∑
j
=
1
m
d
j
t
n
+
j
-
2
,
where i
∈
{
0,1
,
…
,
n
-
2
}, j
∈
{
1,2
,
…
,
m
}, and d
j is coefficient. Constraint points (
t
j
,
z
i
(
t
j
)
) are applied to chained form state z
i
(
t
) which can be expressed as follows:
(10)
z
1
t
=
b
0
t
+
z
1
0
,
z
2
t
=
∑
i
=
0
n
-
2
1
i
+
1
c
i
t
i
+
1
+
∑
j
=
1
m
1
n
+
j
-
1
d
j
t
n
+
j
-
1
+
z
2
0
,
z
3
(
t
)
=
b
0
∑
i
=
0
n
-
2
1
i
+
2
i
+
1
c
i
t
i
+
2
∑
j
=
1
m
W
W
W
W
W
+
∑
j
=
1
m
1
n
+
j
n
+
j
-
1
d
j
t
n
+
j
WWWW
+
b
0
z
2
0
·
t
+
z
3
0
,
WWI
⋮
z
n
(
t
)
=
b
0
n
-
2
∑
i
=
0
n
-
2
i
!
i
+
n
-
1
!
c
i
t
i
+
n
-
1
∑
j
=
1
m
W
W
W
W
W
W
+
∑
j
=
1
m
n
+
j
-
2
!
2
n
+
j
-
3
!
d
j
t
2
n
+
j
-
3
WWWW
+
∑
k
=
2
n
b
0
n
-
k
·
t
n
-
k
·
z
k
0
n
-
k
!
.Obviously, the statez
(
t
) is function of initial conditions z
(
0
) as well as n
+
m undetermined coefficients (
b
0
,
c
0
,
…
,
c
n
-
2
,
d
1
,
…
,
d
m
). Equations (10) inputs will steer the chained form system from z
i
(
0
) to z
i
(
T
) in finite time T and pass through constraint state z
i
(
t
j
) at time t
i. It can be seen clearly that the existence and uniqueness of solutions to (10) can guarantee the chained form state z
(
t
) satisfying nonholonomy at constraint points:
(11)
z
2
(
T
)
z
3
(
T
)
⋮
z
n
(
T
)
z
i
1
(
t
1
)
⋮
z
i
m
(
t
m
)
=
M
T
c
0
c
1
⋮
c
n
d
1
⋮
d
m
+
D
z
2
(
0
)
z
3
(
0
)
⋮
z
n
(
0
)
z
i
1
(
0
)
⋮
z
i
m
(
0
)
,
where z
i
m
(
t
m
) is point on the z
i
(
t
) curve at time t
m
∈
(
0
,
T
) and the matrix M
(
T
) has the form(12)
M
T
=
T
T
2
2
⋯
T
n
-
1
n
-
1
⋯
T
n
+
m
-
1
n
+
m
-
1
b
0
T
2
2
!
b
0
T
3
3
!
⋯
n
-
2
!
b
0
T
n
n
!
⋯
n
+
m
-
2
!
b
0
T
n
+
m
n
+
m
!
⋮
⋮
⋮
⋮
b
0
n
-
2
T
n
-
1
n
-
1
!
b
0
n
-
2
T
n
n
!
⋯
n
-
2
!
b
0
n
-
2
T
2
n
-
3
2
n
-
3
!
⋯
n
+
m
-
2
!
b
0
n
-
2
T
2
n
+
m
-
3
2
n
+
m
-
3
!
b
0
i
1
-
2
t
1
i
1
-
1
i
1
-
1
!
b
0
i
1
-
2
t
1
i
1
i
1
!
⋯
n
-
2
!
b
0
i
1
-
2
t
1
n
+
i
1
-
3
n
+
i
1
-
3
!
⋯
n
+
m
-
2
!
b
0
n
-
2
t
1
n
+
m
+
i
m
-
3
n
+
m
+
i
1
-
3
!
⋮
⋮
⋮
⋮
b
0
i
m
-
2
t
m
i
m
-
1
i
m
-
1
!
b
0
i
m
-
2
t
m
i
m
i
m
!
⋯
n
-
2
!
b
0
i
m
-
2
t
m
n
+
i
m
-
3
n
+
i
m
-
3
!
⋯
n
+
m
-
2
!
b
0
i
m
-
2
t
m
n
+
m
+
i
m
-
3
n
+
m
+
i
m
-
3
!
.Cramer law guarantees existence and uniqueness of solutions to (11) if and only if matrix M
(
T
) is nonsingular for b
0
≠
0.Remark 2.
Whenz
1
(
0
)
=
z
1
(
T
), M
(
T
) becomes singular. This case can be dealt with by the intermediate point method mentioned in [19].Remark 3.
This major advantage of the constraint point method is an effective and simple way. Of course, other methods, such as penalty function method [20], can be applied to avoid transformation singularity.Initial and desired configuration are given asq
(
0
)
=
[
1
°
,
1
°
,
1
°
]
T and q
(
T
)
=
[
30
°
,
30
°
,
30
°
]
T in the configuration space, which correspond to Z
(
0
)
=
[
-
0.032,0.711,0.018
]
T and Z
(
T
)
=
[
-
0.532,0.462,0.524
]
T in chained form space, respectively. Figure 4 indicates the trajectory of Z without any constraint points. Although the state Z can move to desired configuration z
(
T
), z
2
(
t
) path has two zero-crossing points against the conditions of keeping negative. The inverse chained form transformation singularity happens in interval between two zero-crossing points, as shown in Figure 4 cyan marker of z
2
(
t
) trajectory. Figure 5 indicates that a constraint point (5
,
-
0.2) is chosen in the coordinate system. The initial and desired configuration of control parameter Z are redefined as Z
(
0
)
=
[
-
0.032,0.711,0.018
,
-
0.032
]
T and Z
(
T
)
=
[
-
0.532,0.462,0.524
,
-
0.2
]
T. It can be seen that z
2
(
t
) can still move to target values z
(
T
) smoothly and avoid transformation singularity.Figure 4
Chained form statez.Figure 5
Chained form statez with one constraint point.In Figure6, 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.Figure 6
Joints angular displacement with constraint point (simulation 1).


### 3.4. 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 [
φ
n
-
1
,
θ
1
,
θ
2
,
θ
3
]
T, and our goal is to steer 3-dimensional state variable θ
i
(
t
) to θ
i
(
T
), i
∈
{
1,2
,
3
}. Since b
0 can be expressed as b
0
=
(
φ
2
T
-
φ
2
0
)
/
T, we can deal with the polynomials input law by choosing an appropriate b
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(13)
z
2
t
<
0
t
∈
0
,
T
.Equation (13) can be satisfied if maximum value of z
2
(
t
) keeps negative. The time corresponding to stagnation point of z
2
(
t
) can be expressed as follows:
(14)
t
1
=
-
c
1
+
c
1
2
-
4
c
0
c
2
2
c
2
,
t
2
=
-
c
1
-
c
1
2
-
4
c
0
c
2
2
c
2
,
d
2
z
2
t
2
d
t
2
=
-
c
1
2
-
4
c
0
c
2
<
0
.From (11), z
2
(
t
2
) can be represented as function of control parameter b
0:
(15)
c
i
=
c
i
b
0
,
z
2
t
2
=
c
2
3
t
2
3
+
c
1
2
t
2
2
+
c
0
t
2
+
z
2
0
,
W
W
W
W
W
W
W
W
W
I
I
i
=
0,1
,
2
.Consider (15) as a map:
(16)
Φ
b
0
:
I
R
⟹
I
R
which maps input control parameter b
0 into the maximum z
2
(
t
2
). Obviously, the mapping Φ
b
0 is surjection (many-to-one) since the choice of b
0 for solving z
2
(
t
2
) is not unique. The range of value b
0 can be determined by solving roots of (15).In Figure7, 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
°
]
T and θ
(
T
)
=
[
30
°
,
30
°
,
30
°
]
T, 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.Figure 7
Joints angular displacement atφ
2
=
34
° (simulation 2).A major advantage of this approach is that analytical solution of control parameterb
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 b
0. Comparing overparameterization methods the choice of input control parameter b
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 forb
0 when underactuated manipulator has 5 or more joints.


## 3.1. Conversion into Chained Form

Inspired by [3], Sordalen shows conversions of the kinematic model of a car with n 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:(4)
z
4
=
θ
3
,
z
3
=
∂
z
4
∂
q
4
_
f
4
_
q
3
_
=
k
3
k
g
c
2
3
,
z
2
=
∂
z
3
∂
q
3
_
f
3
_
q
2
_
=
-
3
k
3
k
2
k
g
2
c
1
2
c
2
2
s
2
1
,
z
1
=
φ
2
,
(5)
v
1
=
u
1
=
φ
2
,
v
2
=
3
k
2
2
k
3
k
g
3
c
1
4
2
c
2
s
2
2
-
c
2
3
u
1
+
6
k
2
k
3
k
g
2
c
1
s
1
c
2
2
s
2
u
2
,
where s
i
j
=
sin
j
θ
i and c
i
j
=
cos
⁡
j
θ
i.Reference [2] has proven nonholonomy and controllability of the n-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 Jacobianf
i
+
1
(
q
i
_
) of kinematic model in (1) is nonsingular:
(6)
∂
f
i
+
1
q
i
_
∂
q
i
θ
i
-
1
≠
0
=
-
k
i
sin
2
θ
i
-
1
p
i
θ
i
_
θ
i
-
1
≠
0
≠
0
,
where q
i
_
=
[
q
i
,
q
i
+
1
,
…
,
q
n
]
T
=
[
φ
n
-
1
,
θ
1
,
…
,
θ
n
]
T.We do not show the detailed proof of Theorem1 here. Therefore,
(7)
∂
f
i
+
1
q
i
_
∂
q
i
θ
i
-
1
=
0
=
-
k
i
sin
2
θ
i
-
1
p
i
θ
i
_
θ
i
-
1
=
0
=
0
.Equation (7) shows that the coordinated conversions for n-joint manipulator are diffeomorphic except at θ
i
=
0. Therefore, the kinematic model can be diffeomorphically convertible into chained form in the subspace θ
i
∈
(
-
π
/
2,0
)
∪
(
0
,
π
/
2
).


## 3.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:(8)
v
1
=
b
0
,
v
2
=
c
0
+
c
1
t
+
c
2
t
2
.Although the control law polynomial inputs can steer chained form statez 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θ
i
=
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 z
2
>
0 because expression of z
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 z
2
>
0 as in the following section.


## 3.3. 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 withn states and m constraint points for chained form is established as follows:
(9)
v
1
=
b
0
,
v
2
=
∑
i
=
0
n
-
2
c
i
t
i
+
∑
j
=
1
m
d
j
t
n
+
j
-
2
,
where i
∈
{
0,1
,
…
,
n
-
2
}, j
∈
{
1,2
,
…
,
m
}, and d
j is coefficient. Constraint points (
t
j
,
z
i
(
t
j
)
) are applied to chained form state z
i
(
t
) which can be expressed as follows:
(10)
z
1
t
=
b
0
t
+
z
1
0
,
z
2
t
=
∑
i
=
0
n
-
2
1
i
+
1
c
i
t
i
+
1
+
∑
j
=
1
m
1
n
+
j
-
1
d
j
t
n
+
j
-
1
+
z
2
0
,
z
3
(
t
)
=
b
0
∑
i
=
0
n
-
2
1
i
+
2
i
+
1
c
i
t
i
+
2
∑
j
=
1
m
W
W
W
W
W
+
∑
j
=
1
m
1
n
+
j
n
+
j
-
1
d
j
t
n
+
j
WWWW
+
b
0
z
2
0
·
t
+
z
3
0
,
WWI
⋮
z
n
(
t
)
=
b
0
n
-
2
∑
i
=
0
n
-
2
i
!
i
+
n
-
1
!
c
i
t
i
+
n
-
1
∑
j
=
1
m
W
W
W
W
W
W
+
∑
j
=
1
m
n
+
j
-
2
!
2
n
+
j
-
3
!
d
j
t
2
n
+
j
-
3
WWWW
+
∑
k
=
2
n
b
0
n
-
k
·
t
n
-
k
·
z
k
0
n
-
k
!
.Obviously, the statez
(
t
) is function of initial conditions z
(
0
) as well as n
+
m undetermined coefficients (
b
0
,
c
0
,
…
,
c
n
-
2
,
d
1
,
…
,
d
m
). Equations (10) inputs will steer the chained form system from z
i
(
0
) to z
i
(
T
) in finite time T and pass through constraint state z
i
(
t
j
) at time t
i. It can be seen clearly that the existence and uniqueness of solutions to (10) can guarantee the chained form state z
(
t
) satisfying nonholonomy at constraint points:
(11)
z
2
(
T
)
z
3
(
T
)
⋮
z
n
(
T
)
z
i
1
(
t
1
)
⋮
z
i
m
(
t
m
)
=
M
T
c
0
c
1
⋮
c
n
d
1
⋮
d
m
+
D
z
2
(
0
)
z
3
(
0
)
⋮
z
n
(
0
)
z
i
1
(
0
)
⋮
z
i
m
(
0
)
,
where z
i
m
(
t
m
) is point on the z
i
(
t
) curve at time t
m
∈
(
0
,
T
) and the matrix M
(
T
) has the form(12)
M
T
=
T
T
2
2
⋯
T
n
-
1
n
-
1
⋯
T
n
+
m
-
1
n
+
m
-
1
b
0
T
2
2
!
b
0
T
3
3
!
⋯
n
-
2
!
b
0
T
n
n
!
⋯
n
+
m
-
2
!
b
0
T
n
+
m
n
+
m
!
⋮
⋮
⋮
⋮
b
0
n
-
2
T
n
-
1
n
-
1
!
b
0
n
-
2
T
n
n
!
⋯
n
-
2
!
b
0
n
-
2
T
2
n
-
3
2
n
-
3
!
⋯
n
+
m
-
2
!
b
0
n
-
2
T
2
n
+
m
-
3
2
n
+
m
-
3
!
b
0
i
1
-
2
t
1
i
1
-
1
i
1
-
1
!
b
0
i
1
-
2
t
1
i
1
i
1
!
⋯
n
-
2
!
b
0
i
1
-
2
t
1
n
+
i
1
-
3
n
+
i
1
-
3
!
⋯
n
+
m
-
2
!
b
0
n
-
2
t
1
n
+
m
+
i
m
-
3
n
+
m
+
i
1
-
3
!
⋮
⋮
⋮
⋮
b
0
i
m
-
2
t
m
i
m
-
1
i
m
-
1
!
b
0
i
m
-
2
t
m
i
m
i
m
!
⋯
n
-
2
!
b
0
i
m
-
2
t
m
n
+
i
m
-
3
n
+
i
m
-
3
!
⋯
n
+
m
-
2
!
b
0
i
m
-
2
t
m
n
+
m
+
i
m
-
3
n
+
m
+
i
m
-
3
!
.Cramer law guarantees existence and uniqueness of solutions to (11) if and only if matrix M
(
T
) is nonsingular for b
0
≠
0.Remark 2.
Whenz
1
(
0
)
=
z
1
(
T
), M
(
T
) becomes singular. This case can be dealt with by the intermediate point method mentioned in [19].Remark 3.
This major advantage of the constraint point method is an effective and simple way. Of course, other methods, such as penalty function method [20], can be applied to avoid transformation singularity.Initial and desired configuration are given asq
(
0
)
=
[
1
°
,
1
°
,
1
°
]
T and q
(
T
)
=
[
30
°
,
30
°
,
30
°
]
T in the configuration space, which correspond to Z
(
0
)
=
[
-
0.032,0.711,0.018
]
T and Z
(
T
)
=
[
-
0.532,0.462,0.524
]
T in chained form space, respectively. Figure 4 indicates the trajectory of Z without any constraint points. Although the state Z can move to desired configuration z
(
T
), z
2
(
t
) path has two zero-crossing points against the conditions of keeping negative. The inverse chained form transformation singularity happens in interval between two zero-crossing points, as shown in Figure 4 cyan marker of z
2
(
t
) trajectory. Figure 5 indicates that a constraint point (5
,
-
0.2) is chosen in the coordinate system. The initial and desired configuration of control parameter Z are redefined as Z
(
0
)
=
[
-
0.032,0.711,0.018
,
-
0.032
]
T and Z
(
T
)
=
[
-
0.532,0.462,0.524
,
-
0.2
]
T. It can be seen that z
2
(
t
) can still move to target values z
(
T
) smoothly and avoid transformation singularity.Figure 4
Chained form statez.Figure 5
Chained form statez with one constraint point.In Figure6, 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.Figure 6
Joints angular displacement with constraint point (simulation 1).


## 3.4. 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 [
φ
n
-
1
,
θ
1
,
θ
2
,
θ
3
]
T, and our goal is to steer 3-dimensional state variable θ
i
(
t
) to θ
i
(
T
), i
∈
{
1,2
,
3
}. Since b
0 can be expressed as b
0
=
(
φ
2
T
-
φ
2
0
)
/
T, we can deal with the polynomials input law by choosing an appropriate b
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(13)
z
2
t
<
0
t
∈
0
,
T
.Equation (13) can be satisfied if maximum value of z
2
(
t
) keeps negative. The time corresponding to stagnation point of z
2
(
t
) can be expressed as follows:
(14)
t
1
=
-
c
1
+
c
1
2
-
4
c
0
c
2
2
c
2
,
t
2
=
-
c
1
-
c
1
2
-
4
c
0
c
2
2
c
2
,
d
2
z
2
t
2
d
t
2
=
-
c
1
2
-
4
c
0
c
2
<
0
.From (11), z
2
(
t
2
) can be represented as function of control parameter b
0:
(15)
c
i
=
c
i
b
0
,
z
2
t
2
=
c
2
3
t
2
3
+
c
1
2
t
2
2
+
c
0
t
2
+
z
2
0
,
W
W
W
W
W
W
W
W
W
I
I
i
=
0,1
,
2
.Consider (15) as a map:
(16)
Φ
b
0
:
I
R
⟹
I
R
which maps input control parameter b
0 into the maximum z
2
(
t
2
). Obviously, the mapping Φ
b
0 is surjection (many-to-one) since the choice of b
0 for solving z
2
(
t
2
) is not unique. The range of value b
0 can be determined by solving roots of (15).In Figure7, 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
°
]
T and θ
(
T
)
=
[
30
°
,
30
°
,
30
°
]
T, 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.Figure 7
Joints angular displacement atφ
2
=
34
° (simulation 2).A major advantage of this approach is that analytical solution of control parameterb
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 b
0. Comparing overparameterization methods the choice of input control parameter b
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 forb
0 when underactuated manipulator has 5 or more joints.


## 4. 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.


### 4.1. 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 distanced
a and extreme point number t
p. The indicator d
a, 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. t
p, 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.Let us consider a configuration spaceCS with l
:
CS
×
CS
→
C
S
[
0,1
] being a local planner for two configurations a
,
b
∈
CS, and l
(
a
,
b
) is a nonholonomic path l
a
,
b
(
t
), t
∈
[
0,1
], such that l
a
,
b
(
0
)
=
a, l
a
,
b
(
1
)
=
b. The reference path P
(
a
,
b
) is straight line, t
∈
[
0,1
].Definition 4.
d
a is the maximum value of vertical distance between the reference path and the point on the nonholonomic path, and let ∀
n
,
h
∈
CS, as shown in Figure 8, be l
a
,
b
(
t
)
=
n, p
a
,
b
(
t
′
)
=
h
n
,
m
∈
CS, and l
a
·
l
1
=
0. p
a
,
b
(
t
)
=
(
t
-
1
/
0
-
1
)
·
a
+
(
t
-
0
/
1
-
0
)
·
b:
(17)
∃
l
a
2
=
l
a
l
a
T
=
l
2
l
2
T
l
1
l
1
T
-
l
2
l
1
T
2
l
1
l
1
T
,
d
a
=
max
⁡
l
a
,
where n
h
→
=
l
a, a
b
→
=
l
1, n
a
→
=
l
2. l
2
·
l
1
T
=
l
2
·
l
1
·
cos
⁡
α, and l
a
l
a
T
=
l
2
l
2
T
·
(
sin
α
)
2
=
(
l
2
l
2
T
)
(
sin
α
)
2.Figure 8
Illustration of the indicators of evaluation.Definition 5.
t
p is the number of extreme points of the actual trajectory. The actuator rotational direction changes at time t
i if d
(
t
i
-
η
)
·
d
(
t
i
+
η
)
<
0, where η
>
0, and d
(
t
i
±
η
) is the vertical distance at t
i
±
η between the actual trajectory and reference path.We make further explanation about two indicators for searching optimal path. The values ofd
a and t
p are calculated within control parameter b
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 t
p. With slight increment of t
p, the errors of transmission parts of underactuated manipulator will be increased significantly. Therefore, t
p is more significant than d
a for performance of nonholonomic motion planning. In conclusion, a search in parameter space b
0 is carried out to calculate the above two indicators with priority to t
p. In other words, the minimum value d
a is searched in the min
⁡
(
t
p
) 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
°
]
T and θ
(
T
)
=
[
30
°
,
30
°
,
30
°
]
T, respectively. The parameter b
0, which is determined by d
a
=
15.978
° and t
p
=
1, is 36.6. Simulation 3 result is shown in Figure 9.Figure 9
Joints angular optimal displacement (simulation 3).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 Figures1 and 2. Equations (4) reveal that inverse mapping from z
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.Figure10 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.Figure 10
Joints angular optimal displacement (experiment 1).


## 4.1. 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 distanced
a and extreme point number t
p. The indicator d
a, 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. t
p, 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.Let us consider a configuration spaceCS with l
:
CS
×
CS
→
C
S
[
0,1
] being a local planner for two configurations a
,
b
∈
CS, and l
(
a
,
b
) is a nonholonomic path l
a
,
b
(
t
), t
∈
[
0,1
], such that l
a
,
b
(
0
)
=
a, l
a
,
b
(
1
)
=
b. The reference path P
(
a
,
b
) is straight line, t
∈
[
0,1
].Definition 4.
d
a is the maximum value of vertical distance between the reference path and the point on the nonholonomic path, and let ∀
n
,
h
∈
CS, as shown in Figure 8, be l
a
,
b
(
t
)
=
n, p
a
,
b
(
t
′
)
=
h
n
,
m
∈
CS, and l
a
·
l
1
=
0. p
a
,
b
(
t
)
=
(
t
-
1
/
0
-
1
)
·
a
+
(
t
-
0
/
1
-
0
)
·
b:
(17)
∃
l
a
2
=
l
a
l
a
T
=
l
2
l
2
T
l
1
l
1
T
-
l
2
l
1
T
2
l
1
l
1
T
,
d
a
=
max
⁡
l
a
,
where n
h
→
=
l
a, a
b
→
=
l
1, n
a
→
=
l
2. l
2
·
l
1
T
=
l
2
·
l
1
·
cos
⁡
α, and l
a
l
a
T
=
l
2
l
2
T
·
(
sin
α
)
2
=
(
l
2
l
2
T
)
(
sin
α
)
2.Figure 8
Illustration of the indicators of evaluation.Definition 5.
t
p is the number of extreme points of the actual trajectory. The actuator rotational direction changes at time t
i if d
(
t
i
-
η
)
·
d
(
t
i
+
η
)
<
0, where η
>
0, and d
(
t
i
±
η
) is the vertical distance at t
i
±
η between the actual trajectory and reference path.We make further explanation about two indicators for searching optimal path. The values ofd
a and t
p are calculated within control parameter b
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 t
p. With slight increment of t
p, the errors of transmission parts of underactuated manipulator will be increased significantly. Therefore, t
p is more significant than d
a for performance of nonholonomic motion planning. In conclusion, a search in parameter space b
0 is carried out to calculate the above two indicators with priority to t
p. In other words, the minimum value d
a is searched in the min
⁡
(
t
p
) 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
°
]
T and θ
(
T
)
=
[
30
°
,
30
°
,
30
°
]
T, respectively. The parameter b
0, which is determined by d
a
=
15.978
° and t
p
=
1, is 36.6. Simulation 3 result is shown in Figure 9.Figure 9
Joints angular optimal displacement (simulation 3).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 Figures1 and 2. Equations (4) reveal that inverse mapping from z
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.Figure10 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.Figure 10
Joints angular optimal displacement (experiment 1).


## 5. 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.


### 5.1. A Simulation with Initial Condition Error for Optimal Path

Figure11 illustrates that a simulation in the presence of initial errors θ
(
0
)
=
[
9.5
°
,
10.5
°
,
9.5
°
]
T, 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 d
a
=
57.57
° and t
p
=
2 of θ
1. The maximum distance d
a 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.Figure 11
Joints angular displacement with initial errors (simulation 4).


### 5.2. Initial Condition Error Sensitivity Analysis

In order to specify effect of initial errors, the expression ofz
(
t
) can be obtained while the time polynomial inputs law is applied to chained form system existing initial error:
(18)
z
=
M
C
+
D
z
e
0
,
where M
=
{
m
i
,
j
}
∈
R
n
×
R
n and D
=
{
d
i
,
j
}
∈
R
n
×
R
n:
(19)
m
i
,
j
=
j
-
1
!
·
b
0
i
-
1
i
+
j
-
1
!
t
i
+
j
-
1
,
d
i
,
j
=
0
i
<
j
b
0
i
-
j
(
i
-
j
)
!
t
i
-
j
i
≥
j
,
z
e
0
=
z
0
+
e
,
C
=
c
i
,
W
W
W
W
w
I
W
i
,
j
∈
1,2
,
…
,
n
,
where e
i is initial condition error. The effect is calculated along e directional derivatives by the following Jacobian matrix:
(20)
J
R
=
∂
z
∂
e
,
where z
i
=
[
z
1
,
z
2
,
…
,
z
n
+
1
]
T and J
R
∈
R
n
+
1. We see from (18) mathematical structure that
(21)
J
R
i
,
j
=
∂
z
i
e
∂
e
j
=
0
if
i
<
j
,
J
R
i
,
i
=
1
i
∈
1,2
,
…
,
n
+
1
.The matrixJ
R is lower triangular and diagonal elements equal 1, and the following relation can be expressed:
(22)
J
R
=
1
0
0
D
.The parameterb
0 just depends on initial condition φ
n
-
1 and is independent of the given boundary condition. From (22), we can conclude that
(23)
J
R
=
J
R
t
,
b
0
.
The J
R is independent of the given initial condition z
(
0
) and has correlation with t, b
0. Since Jacobian matrix J
R describes the sensitivity along e direction, F-norm J
R
F of J
R is defined initial condition error sensitivity matrix.


### 5.3. Robustness of Motion Planning

The computed result ofJ
R
F is shown as in Figure 12 for three-joint underactuated manipulator. The maximum of J
R
F at t
=
T implies the highest sensitivity under the disturbance of initial error conditions. It can be concluded that the lower b
0 is in favor of reducing the effect of the initial condition error.Figure 12
J
R
F versus t
×
b
0.According to path estimation scheme in Section5.1, the minimum b
0, which is determined under the condition of t
p
=
1, is 35.8. Other parameters except b
0 are the same as simulation 3. The computed result, as shown in Figure 13, indicates that d
a
=
23.52
° in simulation 5.Figure 13
Joints angular displacement (simulation 5).The purpose of tuningb
0 is to improve the robustness of nonholonomic system and decrease the sensitivity for initial condition error at the expense of d
a increase. Simulation 6 with the same initial condition error is carried out for three-joint underactuated manipulator. The simulation result (d
a
=
26.11
° and t
p
=
1), as shown in Figure 14, demonstrates the advantage of the robustness and less sensitivity for initial condition error.Figure 14
Joint angles displacement with initial errors (simulation 6).The usefulness of motion planning robustness is experimentally verified as shown in Figures15 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.Figure 15
Joints angular displacement (experiment 2).Figure 16
Joints angular displacement with initial errors (experiment 3).


## 5.1. A Simulation with Initial Condition Error for Optimal Path

Figure11 illustrates that a simulation in the presence of initial errors θ
(
0
)
=
[
9.5
°
,
10.5
°
,
9.5
°
]
T, 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 d
a
=
57.57
° and t
p
=
2 of θ
1. The maximum distance d
a 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.Figure 11
Joints angular displacement with initial errors (simulation 4).


## 5.2. Initial Condition Error Sensitivity Analysis

In order to specify effect of initial errors, the expression ofz
(
t
) can be obtained while the time polynomial inputs law is applied to chained form system existing initial error:
(18)
z
=
M
C
+
D
z
e
0
,
where M
=
{
m
i
,
j
}
∈
R
n
×
R
n and D
=
{
d
i
,
j
}
∈
R
n
×
R
n:
(19)
m
i
,
j
=
j
-
1
!
·
b
0
i
-
1
i
+
j
-
1
!
t
i
+
j
-
1
,
d
i
,
j
=
0
i
<
j
b
0
i
-
j
(
i
-
j
)
!
t
i
-
j
i
≥
j
,
z
e
0
=
z
0
+
e
,
C
=
c
i
,
W
W
W
W
w
I
W
i
,
j
∈
1,2
,
…
,
n
,
where e
i is initial condition error. The effect is calculated along e directional derivatives by the following Jacobian matrix:
(20)
J
R
=
∂
z
∂
e
,
where z
i
=
[
z
1
,
z
2
,
…
,
z
n
+
1
]
T and J
R
∈
R
n
+
1. We see from (18) mathematical structure that
(21)
J
R
i
,
j
=
∂
z
i
e
∂
e
j
=
0
if
i
<
j
,
J
R
i
,
i
=
1
i
∈
1,2
,
…
,
n
+
1
.The matrixJ
R is lower triangular and diagonal elements equal 1, and the following relation can be expressed:
(22)
J
R
=
1
0
0
D
.The parameterb
0 just depends on initial condition φ
n
-
1 and is independent of the given boundary condition. From (22), we can conclude that
(23)
J
R
=
J
R
t
,
b
0
.
The J
R is independent of the given initial condition z
(
0
) and has correlation with t, b
0. Since Jacobian matrix J
R describes the sensitivity along e direction, F-norm J
R
F of J
R is defined initial condition error sensitivity matrix.


## 5.3. Robustness of Motion Planning

The computed result ofJ
R
F is shown as in Figure 12 for three-joint underactuated manipulator. The maximum of J
R
F at t
=
T implies the highest sensitivity under the disturbance of initial error conditions. It can be concluded that the lower b
0 is in favor of reducing the effect of the initial condition error.Figure 12
J
R
F versus t
×
b
0.According to path estimation scheme in Section5.1, the minimum b
0, which is determined under the condition of t
p
=
1, is 35.8. Other parameters except b
0 are the same as simulation 3. The computed result, as shown in Figure 13, indicates that d
a
=
23.52
° in simulation 5.Figure 13
Joints angular displacement (simulation 5).The purpose of tuningb
0 is to improve the robustness of nonholonomic system and decrease the sensitivity for initial condition error at the expense of d
a increase. Simulation 6 with the same initial condition error is carried out for three-joint underactuated manipulator. The simulation result (d
a
=
26.11
° and t
p
=
1), as shown in Figure 14, demonstrates the advantage of the robustness and less sensitivity for initial condition error.Figure 14
Joint angles displacement with initial errors (simulation 6).The usefulness of motion planning robustness is experimentally verified as shown in Figures15 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.Figure 15
Joints angular displacement (experiment 2).Figure 16
Joints angular displacement with initial errors (experiment 3).


## 6. Conclusions and Further Works

For solving transformation singularity problem of three-joint 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.



---

*Source: 743857-2014-12-18.xml*