Motion Planning of a Second-Order Nonholonomic Chained Form System Based on Holonomy Extraction

: This paper proposes a motion planning algorithm for dynamic nonholonomic systems represented in a second-order chained form. The proposed approach focuses on the so-called holonomy resulting from a kind of motion that traverses a closed path in a reduced conﬁguration space of the system. According to the author’s literature survey, control approaches that make explicit use of holonomy exist for kinematic nonholonomic systems but does not exist for dynamic nonholonomic systems. However, the second-order chained form system is controllable. Also, the structure of the second-order chained form system analogizes with the one of the ﬁrst-order chained form for kinematic nonholonomic systems. These survey and perspectives brought a hypothesis that there exists a speciﬁc control strategy for extracting holonomy of the second-order chained form system to the author. To verify this hypothesis, this paper shows that the holonomy of the second-order chained form system can be extracted by combining two appropriate pairs of sinusoidal inputs. Then, based on such holonomy extraction, a motion planning algorithm is constructed. Furthermore, the effectiveness is demonstrated through some simulations including an application to an underactuated manipulator.


Introduction
Nonholonomic systems-dynamical systems with non-integrable differential constraints-have attracted attention as challenging robotic systems in the fields of motion planning and control [1][2][3][4]. The most symbolical control problem is characterized by Brockett's theorem [5]. It provides a well-known fact that nonholonomic systems cannot be stabilized by using pure smooth state feedback control. According to the kind of constraints, the nonholonomic systems are generally classified into two types: kinematic ones and dynamic ones. The former, which are subject to velocity constraints, include a wheeled mobile robot without or with trailers [1], a snakeboard [1] and a trident snake robot [6]; the latter, which are subject to acceleration constraints, include an underactuated manipulator [7][8][9][10], a surface vessel [11,12], and a blimp [13].
Nonholonomic systems have an intrinsic property that a part of states is impossible to change by an individual control input. The key to control such states is to combine the effects of multiple control inputs in order to extract a kind of motion expressed by Lie brackets. For a class of kinematic nonholonomic systems, holonomy (or geometric phase) is defined as "the extent to which a closed path in the base space fails to be closed in the configuration space" [14], where the base space is a reduced configuration space. The effect which corresponds to Lie brackets is generated by periodic control inputs. It can be said as the essential motion of kinematic nonholonomic systems. In fact, several control approaches utilize the holonomy [1,[14][15][16][17][18][19][20]. Table 1 summarizes main control approaches proposed so far for nonholonomic systems. The control approaches are classified into three groups by the type of constraints and also, in each group, sorted by the published year. From Table 1, the following points can be seen: • As a canonical form for nonholonomic systems, the chained form is often used over all kinds of constraints.

•
Many studies have been attracted to a feedback control problem related Brockett's theorem independently of the type of constraints.
On the other hand, by comparing the first-and second-order chained form systems, it can be found that the structures are similar unless the number of integrators is different. This perspective and the above-mentioned survey led the author to a hypothesis that there exists an appropriate pair of sinusoids to extract holonomy of the second-order chained form system.
Motivated by the hypothesis, this paper addresses a motion planning problem for the second-order chained form system. A class of dynamic nonholonomic systems can be represented as the second-order chained form by transforming the generalized coordinates and control inputs appropriately [8,9,12]. The second-order chained form system is described as a type of affine systems which is nonlinear controllable. Inspired by the structural difference and analogy between the first-and second-order chained form systems, it is found that a combination of two appropriate pairs of sinusoidal inputs can be used to extract the holonomy. This verifies the hypothesis. The idea of holonomy extraction is available directly to motion planning. The proposed motion planning algorithm can be applied to an underactuated manipulator. Its effectiveness is demonstrated by some simulation results.
The main contributions of the proposed approach are emphasized as follows: • A specific way to extract holonomy of the second-order chained form system was proposed. Based on holonomy extraction, a motion planning algorithm was constructed.

•
The holonomy-based motion planning algorithm was applied to an underactuated manipulator. The usefulness of the proposed algorithm was validated through some simulation results.

•
To the best of the author's knowledge, no control approach that makes explicit use of the holonomy for the second-order chained form system has been previously reported as shown in Table 1.
This paper has improved the preliminary results of the author's previous studies [33,34]. In Reference [33], the author has first proposed a specific strategy of holonomy extraction for the second-order chained form system. In addition to that, this paper presents another strategy (see Remark 1). The holonomy obtained in each strategy was also visualized in some phase spaces to understand the differences (see Figures 2 and 4). In Reference [34], the author has applied the holonomy-based motion planning algorithm into an underactuated manipulator and discussed singularities of the system transformation therein. Instead of such discussion, this paper examine an effect from the parameters of control inputs (see the second last paragraph in Section 4, that is, the paragraph before Remark 5).
The rest of the paper is organized as follows. In Section 2, a system representation of the second-order chained form system is given and then its controllability is confirmed. In Section 3, to extract the holonomy of the system, appropriate pairs of sinusoids is concretely provided; based on the maneuver of holonomy extraction, a motion planning algorithm is also proposed. In Section 4, the algorithm is applied to an underactuated manipulator. In the last section, the paper is concluded with directions for future work.

Second-Order Nonholonomic Chained Form System and Its Controllability
Consider the following second-order chained form system: which is one canonical representation for a class of dynamic nonholonomic systems. This system can be obtained from the original dynamical model via an appropriate transformation of the generalized coordinates and control inputs.
By defining a state vector by z = [ z 1 , z 2 , . . . , z 6 ] := [ ξ ,ξ ] (ξ := [ ξ 1 , ξ 2 , ξ 3 ] ), in state space form, (1) is represented as an affine nonlinear system d dt where Note that the system (2) has equilibrium points at z e := [(ξ ) , 0 3 ] , ξ ∈ R 3 with u 1 = u 2 = 0. According to Sussmann's theorem [35], the small-time local controllability (STLC) of the affine system (2) is easily confirmed. For a Lie bracket h, let δ 0 (h), δ 1 (h) and δ 2 (h) be defined as the number of times that f , g 1 and g 2 occur in h, respectively. If δ 0 (h) is odd and δ i (h) is even for each i = 1, 2, the bracket h is called "bad"; otherwise, the bracket h is called "good". Also, let ∑ 2 i=0 δ i (h) be defined as the degree of h. Then, in the Philip Hall bases [1] of (2), the non-zero vector fields at z e are as follows: [g 2 , [ f , Six appropriate vector fields out of (6)-(13) can span R 6 , which means that the system is locally accessible. In other words, the so-called Lie Algebra Rank Condition (LARC) is satisfied. On the other hand, all bad brackets , , are zero vector fields at z e . This obviously can be expressed by linear combinations of good brackets of lower degree Thus the second-order chained form system (1) is small-time local controllable at z e . From the above-mentioned controllability analysis, there exists an admissible control input such that the system can be steered from any equilibrium point to its neighborhood for a small time. This, however, does not imply that a specific control input is obtained. As a specific solution, the subsequent section presents a motion planning algorithm that makes use of holonomy extraction.

Motion Planning Based on Holonomy Extraction
This section considers a motion planning problem of the second-order chained form system and presents an algorithm to solve it. The key idea is to extract holonomy of the system by using sinusoidal inputs. Based on that, the proposed algorithm can be simply constructed.

Problem Formulation
The following motion planning problem is addressed in this paper: Problem 1. Suppose that the second-order chained form system (1) (or (2)) is given. Then, find a control input u(t) = [ u 1 (t), u 2 (t) ] which steers the system between initial and final equilibrium points: The affine system (2) can be divided into two parts: the double-integrator part with respect to (z 1 , z 2 , z 4 , z 5 ) (= (ξ 1 , ξ 2 ,ξ 1 ,ξ 2 )); and the residual part with respect to (z 3 , z 6 ) (= (ξ 3 ,ξ 3 )). The former is linear controllable, so it is easy to control the four states. The latter nonlinear part is what we should focus on here. According to the controllability analysis in the last section, displacement of only z 6 (=ξ 3 ) requires the third-order Lie brackets such as (10) and (11); displacement of only z 3 (= ξ 3 ) requires the fourth-order Lie brackets such as (12) and (13). In general, however, handling higher-order Lie brackets is quite difficult [6].

Holonomy Extraction by Using Sinusoidal Inputs
For the difficulty to obtain displacement of ξ 3 andξ 3 , this subsection presents how to break it through by using sinusoidal inputs.
What we should do for verifying the hypothesis presented in Section 1 is to find an appropriate pair of sinusoids such that displacement of ξ 3 andξ 3 is obtained as holonomy-a kind of motion that traverses a closed path in a reduced configuration space of (ξ 1 , ξ 2 ,ξ 1 ,ξ 2 ). Note that the motion planning problem we address is based on the equilibrium points, that is,ξ = 0 3 . So, the desired holonomy is the one to excite ξ 3 to be a certain value andξ 3 to be zero. Now consider two control inputs of zero-mean sinusoidal functions with angular frequency ω, that is, period T = 2π/ω and amplitude a ω 2 , b ω 2 , where a and b are positive constants. The time span for applying control inputs is also assumed to be composed of two periods: [0, T) and [T, 2T]. By direct calculation, we analyze how the system is steered by a pair of given sinusoidal inputs over each period. First, let the initial equilibrium point be ξ(0) = ξ 0 := [ ξ 10 , ξ 20 , ξ 30 ] ,ξ(0) = 0 3 and let the pair of control inputs over the first period [0, T) be Then, the trajectories of ξ 1 , ξ 2 and their time derivatives becomė Consequently, the values at t = T arė From (18), the trajectories of ξ 3 and its time derivative are given bẏ Hence, we obtain the values of ξ 3 andξ 3 at t = T as follows: At this moment, each non-zero displacement from the initial value is expressed as ∆ξ 1 (T) and ∆ξ 3 (T) in (20) and (26), respectively. As one of a pair of control inputs such that ξ 1 (2T) = ξ 10 , ξ 2 (2T) = ξ 20 and ξ 3 (2T) = ξ 30 at t = 2T, we choose the following pair of sinusoidal functions over the second period [T, 2T]: Note that (27) is the sign inversion of (14). By using (19)- (22), the trajectories of ξ 1 , ξ 2 and their time derivatives can be written asξ Their values at t = 2T are also computed asξ Since the trajectory of ξ 2 behaves according to (31), ξ 3 andξ 3 over [T, 2T] can be expressed bẏ Finally, from (36) and (37), the reaching values at t = 2T are provided aṡ Therefore, the results (32)- (35), (38) and (39) mean that combining two pairs of sinusoidal inputs (14) and (27), that is, provides the desired holonomy. This verified the hypothesis. Although this approach is heuristic and constructive, it brings the obvious relationship between parameters of the inputs and the magnitude of the holonomy. In fact, the displacement of ξ 3 at t = 2T, that is, ∆ξ 3 (2T) with the parameters: a and b; the displacement of ξ 3 at t = T, that is, the ∆ξ 3 (T) state, depends on not only a and b but also ξ 20 . Figure 1 shows the control inputs with a = b = 0.1, ω = 2π (i.e., T = 1), the resultant velocities and positions. Also, Figure 2 depicts the trajectories in various phase spaces. Trajectories of velocities and positions were (not analytically but) numerically obtained. The simulator was developed in C with the GNU Scientific Library [36]. The embedded eight-th order Runge-Kutta Prince-Dormand method with nine-th order error estimate (rk8pd) was selected as an ODE solver, where an absolute and relative errors were set to 10 −12 and 0, respectively.  From Figure 1b,c, it can be observed that a positive displacement with respect to ξ 3 is obtained by two-period motion. Since a = b = 0.1 in (39), the magnitude of the holonomy is computed as ∆ξ 3 (2) ≈ 9.4 × 10 −2 . Figure 2 shows the holonomy in six different types of phase spaces, which visualizes that the displacement of ξ 3 results from a kind of motion that traverses a closed path in a reduced configuration space of (ξ 1 , ξ 2 ,ξ 1 ,ξ 2 ). In particular, Figure 2b,f present that the motion to extract the holonomy is characterized by two closed circular paths in the reduced configuration space.

Holonomy-Based Motion Planning Algorithm
Based on the holonomy extraction of the previous subsection, a motion planning algorithm can be easily constructed.
Here, let us assume the following points: • The entire motion planning consists of three phases: P 1 , P 2 and P 3 . Also, the periods of P 1 and P 2 are T, whereas that of P 3 is 2T.

•
At the beginning and end of each phase, the system (1) stops; that is to say, each velocity is zero. • Letũ i (t), i = 1, 2, be an appropriate sinusoidal function whose period is T.
Note that Steps 1-3 are replaceable. Now let us consider an example of motion planning from an initial state (0, 0, 0, 0, 0, 0) to a desired state (1, 1, 1, 0, 0, 0) for four seconds (i.e., T = 1). As control inputs such that the control objective is achieved, we adopt The system starts to move from the initial state at t = 0, through at t = 1 and at t = 2 and lastly reaches the following final states:
Stop Figure 5. The algorithm of motion planning. Therefore, the results (60)-(62) indicate the following things: • to realize ξ i (4) = 1 (i = 1, 2), a P 1 and b P 2 must be • to realize ξ 3 (4) = 1, a P 3 and b P 3 should be assigned as, for example, Figure 6 depicts the control inputs with (64) and (65), the resultant velocities and positions. From Figure 6b,c, it can be confirmed that the state arrives at (1, 1, 1, 0, 0, 0), that is, the desired motion is successfully planned.

Remark 2.
The control inputs with (64) and (65) have switching points between P 1 and P 2 , between P 2 and P 3 and in the middle of P 3 . The corresponding effect cannot be found in the resultant velocities and positions. In a practical situation, however, a combination with feedback control should be required to compensate the occurred error. This is included in future work.

Remark 3.
Note that there exists freedom to choose a P 3 and b P 3 . The freedom can be utilized for designing the motion of the system. Under the above-mentioned control objective, you can choose a P 3 and b P 3 such that a P 3 b P 3 = 1/(3π). Its example of use will be shown in the next section.

Application to Rest-to-Rest Motion of a Three-Joint Manipulator with Passive Third Joint
This section applies the proposed algorithm into a rest-to-rest motion of an underactuated planar manipulator that moves in a horizontal plane. The manipulator has three joints whose third joint is passive (i.e., unactuated) as depicted in Figure 7.

Y X O
3rd revolute joint (unactuated) 2nd revolute joint (actuated) 1st revolute joint  The main variables and parameters are defined as in Table 2.  For simplicity, assume that there is no external disturbance such as load, friction, linear and nonlinear damping acting on each joint. Then, based on Lagrange's equation of motion, the dynamics of the manipulator is given by where q = [ q 1 , q 2 , q 3 ] and τ 12 = [ τ 1 , τ 2 ] are vectors of joint angles and torques, respectively. Note that (66) does not include the gravitational term because the dynamics is not affected from gravitational forces. See Appendix A for the details of the inertia matrix M(q) and the centrifugal and Coriolis term c(q,q).

Step 3:
Apply the sinusoidal inputs u to the system (67) through the input transformation (69).

Control input u
Step 2 holonomy-based motion planning
• If a P 2 is less than b P 3 , then, on the basis of the case when a P 2 = b P 3 , 2 1 x(t)dt becomes smaller and 3 1 θ(t)dt becomes bigger; that is to say, the third link moves narrowly in direction of x-axis while its orientation varies slightly larger.    The last two facts indicate that the freedom in choosing a P 3 and b P 3 can be used for obstacle avoidance.
If there is an obstacle in the gray-hatched area of Figure 11, choosing a P 3 and b P 3 such that a P 3 is less than b P 3 can make the third link avoid colliding to the obstacle. Therefore, the simulation results illustrated the effectiveness of the proposed algorithm.

Remark 5.
To avoid a collision with an obstacle in the real world, it is necessary to detect the obstacle and measure the distance between the obstacle and the manipulator. For achieving such things, e.g., an optical 3D laser measurement system [37] and a stereo vision system [38] would be useful.

Conclusions
In this paper, a holonomy-based motion planning algorithm for the second-order chained form system was proposed. First, it was shown that a combination of two appropriate pairs of sinusoidal inputs extracts the holonomy. This verified the hypothesis that motivated this study. Next, the idea of holonomy extraction was directly used to construct a motion planning algorithm. Finally, the effectiveness of the proposed algorithm was confirmed from the simulation results including an application to an underactuated manipulator.
This algorithm is heuristic but simple and useful. By adopting specific sinusoids as control inputs, the relationship between parameters of control inputs and the magnitude of the holonomy was revealed. Also, it was found that the freedom to choose the parameters can be utilized for designing the motion of the system. Accordingly, the proposed algorithm will provide a new perspective on how to control dynamic nonholonomic systems.
Future work will include experimental validation and generalization of the controlled systems and control inputs. In particular, a combination with feedback control should be needed in practice to make the proposed approach robust against the modeling error and external disturbance. Also, to enlarge applicable systems, an extension to an underactuated system under the gravity will be considered.
Funding: This work was supported by JSPS KAKENHI Grant Number 26820169.