Kinematics and Motion Planning of the Multi-Bar System

This paper studies a generalization to 3D space of the planar system composed of a tractor pulling a number of trailers, called the multi-bar system. Assuming a natural coordinate description of the system, its kinematics equations are derived in the form of a driftless control system with three controls. The motion planning problem is stated, and solved by means of a Jacobian algorithm resulting from the endogenous configuration space approach. Solutions of example motion planning problems are obtained by numeric computations.


Introduction
The system comprising a tractor pulling trailers that move without the lateral slip of the wheels has been a classical object of mobile robotics for decades. A mathematical model of this system, commonly referred to as the n-trailer system, can be found e.g. in Laumond [7]. The discovery that the n-trailer system is flat, and that the Cartesian positions of the last trailer are the flat outputs has been a milestone of the theory of differentially flat systems [2,12]. Recently, it has been shown that the flat outputs for the n-trailer system, despite the position of the last trailer, include a whole family of flat outputs, parametrized by a function of three variables [8]. The n-trailer structure has been generalized to the R m+1 space by Jakubczyk [3]. Recently, differential geometric properties of such a system have been studied intensely by Li and Respondek [9,10], where it was called the n-bar system, and Slayman and Pelletier [13,14], under the name of the articulated arm. The system consists of n rigid, unit length bars connected by articulated joints in such a way that the ith bar connects the joints number i − 1 and i, i = 1, . . . , n. The 0-joint coincides with the origin of the bar number 1 and can move freely in R m+1 . The end point of each bar moves on the sphere S m . A characteristic feature of the n-bar system is that during its motion the instantaneous velocity of the ith joint is aligned with the (i + 1)th bar. This assumption imposes on the motion of the n-bar system a collection of nonholonomic constraints. Using an implicit mathematical model of the n-bar system kinematics, it has been shown that outside singular configurations this system is feedback equivalent to the m-chained form, therefore flat [10]. Somewhat unexpectedly, differently to the n-trailer system, the Cartesian position of the last bar appears to be the only flat output of the n-bar system.
In the aforementioned works on the n-bar system, its properties were proved for a general form of the system, without derivation of explicit kinematics formulas. In this paper we study the R 3 case of the n-bar system with arbitrary lengths of bars, that will be called the multi-bar system. Using a coordinate description we define the Pfaffian constraints, and derive a kinematics representation of the system in the form of a driftless control system with three controls. We show that after a suitable restriction of some coordinates the multibar system with n bars becomes equivalent to the (n − 1)-trailer planar system. Using the control system representation of the kinematics, we address the motion planning problem for the multibar system. To solve the problem we adopt a Jacobian algorithm deduced from the endogenous configuration space approach [15,16]. The motion planning algorithm is illustrated with computer simulations. To our best knowledge, the motion planning problem of the multi-bar system has not been addressed in the literature. Alternatively to the endogenous configuration approach presented here, such an algorithm could be based on the flatness property, although an advantage of the presented approach is that it is able to account for configuration or control constraints [5,11] resulting, e.g. from the request of avoiding obstacles or configuration singularities. Preliminary results for 2-bar system and imbalanced Jacobian has been presented in [4]. To sum up, the main contribution of this paper is derivation of a control theoretic model of multi-bar system kinematics and demonstration that this model may serve as verification tool for motion planning algorithms, such as the one derived from the endogenous configuration space approach.
Concerning a physical interpretation of this system, we are not aware of any kind of physical realization of the system. Li and Respondek [10] have compared the multi-bar system to a train composed of rigid bars moving in a very viscous liquid. Alternatively, multi-bar system can also serve as a model of a formation of UAV's (Unmanned aerial vehicles) called the snake [1], organized in accordance with the neighbourreferenced principle. Observe that in the former case the nonholonomic are of material type, while in the latter they represent the program constraints [6]. In what follows, the multi-bar system will be regarded primarily as a 3-input, nonholonomic testbed for motion planning algorithms.
The structure of this paper is the following. In Section 2 we define the Pfaffian constraints imposed on the motion of the multi-bar system, derive its general model of kinematic, and illustrate with examples of 1-, 2-and 3-bar systems. Section 3 describes a motion planning algorithm based on the endogenous configuration space approach. Examples of numerical solutions of the problem for the system composed of three bars are provided in Section 4. Section 5 concludes the paper. Proofs of some technical results are included in Appendix.

System Kinematics
The multi-bar system consists of n rigid bars moving in R 3 , connected to each other with 2 DOF articulated joints. The origin of the bar number 1 referred to as the 0 joint, can move freely. The bar number i has length l i , and connects the joints number i − 1 and i, for i = 1, . . . , n. The end point of the ith bar can move on the 2D sphere of radius l i . The motion of the system is constrained in such a way that the instantaneous velocity of the joint i is aligned with the (i + 1)st bar. A schematic view of the system is presented in Fig. 1. The configuration of the multi-bar system is defined by the Cartesian position (x, y, z) of the origin of the first bar, and pairs of angles (ϕ i , θ i ) determining the spherical coordinates of the end point of the bar i. The absolute angles will be chosen, measured with respect to the inertial coordinate frame. The configuration variable where N = 2n + 3. Note that the spherical coordinates are well defined for θ i ∈ (0, π), so θ i = 0, π are representation singularities.

Nonholonomic Constraints
The multi-bar system is subject to the nonholonomic constraints resulting from the assumption that during the motion the velocity of the origin of each bar is aligned with that bar. Denote by y i the Cartesian coordinates of the end-point of the ith bar (which is also the origin of the bar i + 1), y 0 = (x, y, z) T , and letẏ i be the corresponding velocity. Then, the constraints for the ith bar can be expressed in the following waẏ To make this identity explicit, we first compute the coordinates of the end point of the ith bar (for the sake of conciseness, further on we shall use s φ and c φ to denote, respectively, sine and cosine of the angle φ) By differentiation, we find the velocitẏ After the substitution of Eqs. 2 and 3, and division by l i , condition 1 becomes for i = 1, . . . , n. The choice of absolute angles requires that the term s θ i is non-zero, therefore we can divide the third equation by it. Observe that the rows on the left hand side of Eq. 4denoted by (w 1 , w 2 , w 3 )-are dependent, as their combination The complete set of constraints resulting from Eq. 4 introduced by all bars (i = 1, . . . , n) can be then written in the Pfaffian form Matrix A(q) is a block matrix of the form where the dimension of A i1 equals 3 × 3, and The first block column of matrix A(q) consists of sub-matrices while the remaining non-zero blocks are given by for i = 2, . . . , n and j = 2, . . . , i. The dimension of matrix A(q) is 3n × N, however due to dependence of rows shown in Eq. 5, its rank is 2n.

Control System
The kinematics of the multi-bar system can be represented in the form of a driftless control system with three inputṡ The nonholonomic constraints 6-8 determine the N × 3 control matrix G(q) as consisting of vector fields which belong to the kernel of A(q) Let us assume that control inputs of system 9 have the meaning of the linear and angular velocities of the last bar (the leader). Then, the explicit form of matrix G(q) is defined by the following theorem.

Theorem 1 The control matrix
has the property (Eq. 10) for the constraints 6-8, provided that the components of g 1 (q) (dim g 1 (q) = N − 2) are def ined as where Proof It is easily seen that the constraints are fulfilled for the first row of the block matrix A(q) (Eq. 6) For each of the remaining rows i = 2, 3, . . . , n we shall apply the following lemma (proof of the lemma is given in Appendix).
Lemma 1 Products of sub-matrices A ij and subvectors g 1, j are given by where In order to simplify the notation, we set l 0 = 1 obtaining j = j−1 l j−2 for j = 2, 3, . . . , n, and using Lemma 1 conclude that For the reader's convenience, below we present explicit kinematics equations for 1-and 2-bar systems.
Example 1 If the system consists of a single bar only, the dimension of its configuration vector N = 5, the change of its position results from forward velocity u 1 , and its the orientation is directly controlled by u 2 and u 3 , Example 2 After attaching an additional bar, the dimension of the configuration space increases to N = 7. In this case the system equations include the terms β 2 in the origin position part and α 2 in the equations for angles (φ 1 ,θ 1 ). The term u 1 l 1 determines the forward velocity, and controls u 2 and u 3 directly affect the angular velocities of the leader, Example 3 In the case of the 3-bar system, the dimension N = 9. The term u 1 l 1 l 2 equals the system's forward velocity, the meaning of controls u 2 and u 3 is analogous as for the 2-bar. The control representation of the kinematics is Remark 1 Notice, that when all θ i = π/2, the multi-bar system is forced to move in a plane like a car-with-trailers and its kinematics equations are equivalent to those for a tractor pulling (n − 1) trailers, as presented by Laumond [7].
In the planar case the rows of the control matrix (Eq. 11) corresponding to z and θ i coordinates can be skipped. Let us set all l i = 1, and apply the following change of coordinateŝ Then, using Eqs. 11-13, the kinematics in new coordinates are given by It is easily checked that the resulting kinematic equations 18 coincide with those from [7].

Motion Planning
The motion planning problem of the multi-bar system 9 consists in defining a control u d (t) that steers the system from an initial configuration q 0 = q(0) to a prescribed desired configuration q(T) = q d within a given time horizon T > 0. Let q(t) = ϕ q 0 ,t (u(·)) denote the system trajectory starting from q 0 and driven by u(·). The end point map of the multi-bar system, defined as where u(·) ∈ L 2 3 [0, T] belongs to the Hilbert space of Lebesgue square integrable functions from [0, T] into R 3 , will be called the instantaneous kinematics of the multi-bar system. The motion planning problem amounts to determining a control function u d (·) such that K q 0 ,T (u d (·)) = q d .
A motion planning algorithm will be derived from the endogenous configuration space approach [15,16]. To this aim, let u ϑ (·) denote a curve of control functions parametrized by ϑ ∈ R, passing through an initial control function u 0 (·). The error of the motion planning problem corresponding to u ϑ (·) is computed as e(ϑ) = K q 0 ,T (u ϑ (·)) − q d . Now, the curve should be chosen in such a way that the error converges to 0 exponentially with a prescribed ratio γ > 0, Differentiation of the error e(ϑ) yields where J q 0 ,T (u(·)) denotes the system Jacobian representing the end point map of the linear approximation to system 9 along the control-trajectory pair (u(t), q(t)), initialized at ξ(0) = 0. The matrices A(t) and B(t) represent partial derivatives of G(q)u with respect to the state variables and controls, respectively G(q(t)).
The transition matrix (t, s) of the approximating system 22 is a solution of the differential equation As a result, the Jacobian can be computed by integrating system 22, Denote by J # q 0 ,T (u(·)) : R N → L 2 3 [0, T] a right inverse of the Jacobian (so the composition J q 0 ,T (u(·))J # q 0 ,T (u(·)) = I N ). Having substituted (Eq. 21) into (Eq. 20) and applied the inverse, we obtain a dynamic system In what follows the right inverse will be the Jacobian pseudo inverse, so for η ∈ R N where is the dexterity matrix [16] of the multi-bar system at u(·). Given a control function u(·), the dexterity matrix can be computed as D q 0 ,T (u(·)) = (T) that results from solving the Lyapunov differential equation with initial condition (0) = 0. The solution to the motion planning problem is obtained by passing to the limit with the trajectory of Eq. 25 for either Jacobian or singularity robust Jacobian pseudo inverse 26,

Simulations
To illustrate results of the algorithm, we have chosen a 3-bar system which kinematics are described by Eq. 17. The computations have been done in MATLAB. Control functions, to which the model is subjected to, were calculated as continuous functions and obtained by solving Eq. 25 with initial condition u 0 (·) until the norm of the final error ||e(T)|| decreased below 10 −4 .
We present two examples of motion, which differ in desired configuration. Initial configuration for both cases was set to q 0 = (0, 0, 0, 0, π 4 , 0, π 4 , 0, π 4 ). In the first example, the destination , which is fairly easy to be reached from the initial point. In the second example the destination was set to be sideways to the starting point q d = (0, 3, 0, 0, π 4 , 0, π 4 , 0, π 4 ), so the line connecting initial and final configurations is orthogonal to allowed velocities in those configurations. Therefore the motion in the second example requires more complex trajectory and may be considered more difficult. For the consistence, the remaining parameters of the simulations were set the same in the two examples: all bars had unit lengths (l 1 = l 2 = l 3 = 1) and the algorithm was configured for motion time T = 10, the convergence rate γ = 5. The initial control function of Eq. 25 was u 0 (t) = (0.1, 0.1, 0.1).
Paths resulting from motion planning algorithms together with initial, final and selected intermediate configurations have been displayed in  Fig. 4 for the example 2. In Fig. 5 one can observe the error covergence with respect to ϑ. As assumed in Eq. 20, the convergence is exponential.

Conclusions
The objective of this paper was to develop the kinematics equations for the multi-bar system. Theorem 1 has provided the control representation of the multi-bar system kinematics, consisting of arbitrary number of bars. Using this representation, we designed the Jacobian pseudo inverse motion planning algorithm within the endogenous configuration space approach. Calculations used in computer simulations to solve the system utilized non-parametric control method and confirmed that the Jacobian methods provide a tool for motion planning of the multi-bar system.
Presented algorithm may be further expanded to avoid representation singularities or task space obstacles by invoking either the imbalanced Jacobian motion planning algorithm [5] or the task-priority approach [11]. Preliminary results for 2-bar system and imbalanced Jacobian [4] confirm that the first approach may be effective.
Open Access This article is distributed under the terms of the Creative Commons Attribution License which permits any use, distribution, and reproduction in any medium, provided the original author(s) and the source are credited.

Appendix A: Proof of Lemma 1
For the sake of conciseness we shall use s φψ to denote sin(φ − ψ) and c φψ for cos(φ − ψ). We need to show that elements in a product of the ith row of the matrix A(q) and the matrix G(q) have the form prescribed in Eq. 14.
Proof Let us consider separately the elements of the above sum.
-Products of elements from the first column of A(q) and g 1,1 are equal to -The products including the last non-zero element in the ith row of A(q) satisfy with elements w iik , k = 1, 2, 3, After a substitution of w iik , we obtain -The terms containing A ij for j = 2, . . . , i − 1 A ij g 1, j = A ij α j β n j+1 = l j−1 j β n