A symplectic kinodynamic planning method for cable-driven tensegrity manipulators in a dynamic environment

Abstract

Kinodynamic planning of tensegrity robots is a thorny problem, and there are few works that have been reported on this subject, especially for tensegrity manipulators. In this study, a symplectic instantaneous optimal control (IOC) method for the obstacle-avoiding kinodynamic planning of a spinal tensegrity manipulator driven by sliding cables is first developed. This tensegrity mechanism can imitate the basic operations of the humanoid spine, such as bending, scoliosis, contraction and rotation. The actuation of sliding cables is treated as the kinematic constraints of the system inspired by the concept of multibody dynamics, so that a general dynamic model of the sliding cable-driven tensegrity robots is constructed by differential algebraic equations (DAEs). Subsequently, based on the discrete variational principle and Lagrange–d’Alembert principle, an IOC planner coupled with a symplectic penalty iteration is proposed to solve the kinodynamic planning problem of DAE systems. The proposed algorithm provides a novel unified control framework for the kinodynamic planning of tensegrity manipulators with fewer sliding cable actuators. A suboptimal collision-free path with input saturation can be planned in a complex dynamic environment where the target or the obstacles are moving. Finally, certain numerical experiments on the kinodynamic planning of a spinal tensegrity manipulator are carried out to demonstrate the effectiveness and advantages of the proposed symplectic IOC approach.

Appendix A

Taking into account Eqs. (28) and (34), it can be found that ${{\varvec{\Phi}}}_{{\varvec{x}}} = {\hat{\mathbf{\Phi }}}_{{\varvec{x}}}$. Then, each part of the Jacobian matrix shown in Eq. (36) is derived as follows:

$$ \left\{ \begin{gathered} \frac{{\partial {\varvec{f}}_{1} }}{{\partial {\varvec{x}}_{k + 1} }} = - \frac{1}{\eta }{\varvec{M}} - \left. {\left( {\frac{\eta }{4}\frac{{\partial (\hat{\mathbf{\Phi }}_{{\varvec{x}}}^{{\text{T}}} \left( {\varvec{x}} \right) \cdot {\varvec{\lambda}}_{k + 1} )}}{{\partial {\varvec{x}}}}} \right)} \right|_{{{\varvec{x}} = \tilde{\mathbf{x}}}} + \left. {\left( {\frac{\eta }{4}\frac{{\partial {\varvec{F}}{(}{\varvec{x}},\dot{\mathbf{x}})}}{{\partial {\varvec{x}}}} + \frac{1}{2}\frac{{\partial {\varvec{F}}{(}{\varvec{x}},\dot{\mathbf{x}})}}{{\partial \dot{\mathbf{x}}}}} \right)} \right|_{\begin{subarray}{l} {\varvec{x}} = \tilde{\mathbf{x}} \\ \dot{\mathbf{x}} = \tilde{\mathbf{v}} \end{subarray} } \hfill \\ \frac{{\partial {\varvec{f}}_{1} }}{{\partial {\varvec{\lambda}}_{k + 1} }} = - \frac{\eta }{2} \cdot \hat{\mathbf{\Phi }}_{{\varvec{x}}}^{{\text{T}}} \left( {\tilde{\mathbf{x}}} \right) \hfill \\ \end{gathered} \right. $$

(A1)

The main terms in Eq. (A1) are $\hat{\mathbf{\Phi }}_{{\varvec{x}}}$ and $\frac{{\partial (\hat{\mathbf{\Phi }}_{{\varvec{x}}}^{{\text{T}}} \left( {\varvec{x}} \right) \cdot {\varvec{\lambda}}_{k + 1} )}}{{\partial {\varvec{x}}}}$, which will be presented in detail.

(1) The derivation for ${\hat{\Phi }}_{{\text{x}}}$.

According to Eq. (34), the constrained Jacobian matrix ${\hat{\Phi }}_{{\text{x}}}$ can be derived as follows:

$$ \hat{\mathbf{\Phi }}_{{\varvec{x}}} = \frac{{\partial {\hat{\mathbf{\Phi }}}({\varvec{x}}_{k + 1} )}}{{\partial {\varvec{x}}_{k + 1} }} = \left. {\left[ {\begin{array}{*{20}c} {\left( {\frac{{\partial l_{1} }}{{\partial {\varvec{x}}}}} \right)^{T} } & {\left( {\frac{{\partial l_{2} }}{{\partial {\varvec{x}}}}} \right)^{T} } & \cdots & {\left( {\frac{{\partial l_{m} }}{{\partial {\varvec{x}}}}} \right)^{T} } \\ \end{array} } \right]^{T} } \right|_{{{\varvec{x}} = {\varvec{x}}_{k + 1} }} $$

(A2)

$$ \frac{{\partial l_{i} }}{{\partial {\varvec{x}}}} = {\varvec{B}}_{i}^{T} = \left\{ {\begin{array}{*{20}c} {\hat{\mathbf{l}}_{i}^{{{(1)}}} } \\ { - \hat{\mathbf{l}}_{i}^{{{(1)}}} + \hat{\mathbf{l}}_{i}^{{{(2)}}} } \\ \vdots \\ {\begin{array}{*{20}c} { - \hat{\mathbf{l}}_{i}^{{{(}n{ - 1)}}} + \hat{\mathbf{l}}_{i}^{{{(}n{)}}} } \\ { - \hat{\mathbf{l}}_{i}^{{{(}n{)}}} } \\ \end{array} } \\ \end{array} } \right\}^{T} = \left\{ {\begin{array}{*{20}c} {\frac{{{\varvec{R}}_{1}^{i} - {\varvec{R}}_{2}^{i} }}{{l_{i}^{{{(1)}}} }}} \\ { - \frac{{{\varvec{R}}_{1}^{i} - {\varvec{R}}_{2}^{i} }}{{l_{i}^{{{(1)}}} }} + \frac{{{\varvec{R}}_{2}^{i} - {\varvec{R}}_{3}^{i} }}{{l_{i}^{{{(2)}}} }}} \\ \vdots \\ {\begin{array}{*{20}c} { - \frac{{{\varvec{R}}_{{n{ - 1}}}^{i} - {\varvec{R}}_{n}^{i} }}{{l_{i}^{{{(}n{ - 1)}}} }} + \frac{{{\varvec{R}}_{n}^{i} - {\varvec{R}}_{{n{ + 1}}}^{i} }}{{l_{i}^{{{(}n{)}}} }}} \\ { - \frac{{{\varvec{R}}_{n}^{i} - {\varvec{R}}_{{n{ + 1}}}^{i} }}{{l_{i}^{{{(}n{)}}} }}} \\ \end{array} } \\ \end{array} } \right\}^{T} $$

(A3)

where $\hat{\mathbf{l}}_{i}^{{{(}j{)}}} = \frac{{{\varvec{l}}_{i}^{{{(}j{)}}} }}{{l_{i}^{{{(}j{)}}} }}$ and ${\varvec{R}}_{k}^{i}$ denote the direction vector of the jth segment and the kth nodal position vector for the ith sliding cable actuator, respectively, as presented in Fig. 1.

(2) The derivation for $\frac{{\partial (\hat{\mathbf{\Phi }}_{{\varvec{x}}}^{{\text{T}}} \left( {\varvec{x}} \right) \cdot {\varvec{\lambda}}_{k + 1} )}}{{\partial {\varvec{x}}}}$.

Substituting Eqs. (A2) and (A3) into this item yields

$$ \frac{{\partial (\hat{\mathbf{\Phi }}_{{\varvec{x}}}^{{\text{T}}} \left( {\varvec{x}} \right) \cdot {\varvec{\lambda}}_{k + 1} )}}{{\partial {\varvec{x}}}} = \mathop \Lambda \limits_{i = 1}^{m} \frac{{\partial ({\varvec{B}}_{i} {\varvec{\lambda}}_{k + 1}^{i} )}}{{\partial {\varvec{x}}}} $$

(A4)

where the symbol $\mathop \Lambda \limits_{i = 1}^{m}$ denotes the assembly operation over the m sliding cable elements by the finite element method, and

$$ \frac{{\partial ({\varvec{B}}_{i} {\varvec{\lambda}}_{k + 1}^{i} )}}{{\partial {\varvec{x}}}} = \left[ {\begin{array}{*{20}c} {\frac{1}{{l_{i}^{{{(1)}}} }}} & { - \frac{1}{{l_{i}^{{{(1)}}} }}} & 0 & 0 & \cdots & 0 & 0 & 0 \\ { - \frac{1}{{l_{i}^{{{(1)}}} }}} & {\frac{1}{{l_{i}^{{{(1)}}} }} + \;\frac{1}{{l_{i}^{{{(2)}}} }}} & { - \frac{1}{{l_{i}^{{{(2)}}} }}} & 0 & \cdots & 0 & 0 & 0 \\ 0 & { - \frac{1}{{l_{i}^{{{(2)}}} }}} & { - \frac{1}{{l_{i}^{{{(2)}}} }} + \frac{1}{{l_{i}^{{{(3)}}} }}} & { - \frac{1}{{l_{i}^{{{(3)}}} }}} & \cdots & 0 & 0 & 0 \\ \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots \\ 0 & 0 & 0 & 0 & \cdots & { - \frac{1}{{l_{i}^{{{(}n{ - 1)}}} }}} & {\frac{1}{{l_{i}^{{{(}n{ - 1)}}} }} + \frac{1}{{l_{i}^{{{(}n{)}}} }}} & { - \frac{1}{{l_{i}^{{{(}n{)}}} }}} \\ 0 & 0 & 0 & 0 & \cdots & 0 & { - \frac{1}{{l_{i}^{{{(}n{)}}} }}} & { - \frac{1}{{l_{i}^{{{(}n{)}}} }}} \\ \end{array} } \right] \otimes ({\varvec{\lambda}}_{k + 1}^{i} {\varvec{I}}_{3} ) $$

(A5)

where “$\otimes$” denotes the Kronecker product.