Motion Planning of Non-holonomic Parallel Orienting Platform: A Jacobian Approach

Abstract

This chapter addressed the motion planning problem for a non-holonomic parallel orienting mechanism whose kinematics are represented by a driftlesscontrol system on $SO(3)$. A coordinate-free, Jacobian motion planning algorithm is proposed. A normal form of singularity locus is derived. Two example motion planning problems are solved numerically.

Appendix

1.1 Normal Form

Originally, the singularity locus of the non-holonomic orienting platform is given by the equation $\varDelta (R)=r_{12}r_{31}+r_{21}r_{32}=0.$ Let’s first use the Roll-Pitch-Yaw coordinates, $RPY(\varphi ,\vartheta ,\psi )=R(Z,\varphi )R(Y,\vartheta )R(X,\psi )$. Obviously, $RPY(0,0,0)=\mathbb {I}_3$. It is easily seen that in these coordinates $\varDelta (\varphi ,\vartheta ,\psi )=\sin \varphi \sin \vartheta \cos \psi + \sin \psi (\sin \varphi \cos ^2\vartheta -\cos \varphi \sin ^2\vartheta ).$ Next, we choose $\xi =\sin \vartheta \cos \psi$, $\eta =\sin \varphi \cos ^2\vartheta -\cos \varphi \sin ^2\vartheta$, and $\zeta =\sin \psi$. This is a well defined coordinate system around $0$, so $\varDelta (\xi ,\eta ,\zeta )=\xi \sin \varphi (\xi ,\eta ,\zeta )+\eta \zeta .$ The function $\varphi$ satisfies $\eta -\sin \varphi (\xi ,\eta ,\zeta )\cos ^2\vartheta (\xi ,\eta ,\zeta )+\cos \varphi (\xi ,\eta ,\zeta )\sin ^2\vartheta (\xi ,\eta ,\zeta )=0,$ that after suitable substitutions, converts into $-\sin \varphi (\xi ,\eta ,\zeta )+\eta +\frac{\xi ^2(\cos \varphi (\xi ,\eta ,\zeta )+\sin \varphi (\xi ,\eta ,\zeta ))}{1-\zeta ^2}=0.$ It follows that $\varphi (0,0,0)=0$, and in new coordinates, $\varDelta (\xi ,\eta ,\zeta )=\frac{\xi ^3(\sin \varphi (\xi ,\eta ,\zeta )+\cos \varphi (\xi ,\eta ,\zeta ))}{1-\zeta ^2}+\xi \eta +\eta \zeta .$ Finally, a change of coordinates $x=\xi +\zeta$, $y=\eta$, $z=\xi \left( \frac{\sin \varphi (\xi ,\eta ,\zeta )+\cos \varphi (\xi ,\eta ,\zeta )}{1-\zeta ^2}\right) ^{1/3}$ provides the desired normal form $f(x,y,z)=z^3+xy.$

1.2 Motion Planning Algorithm

We begin with any admissible control function $u_0(\cdot )$ of (1). If $R_T=\varphi _{R_0,T}(u_0(\cdot ))=R_d,$ we are done. Otherwise, we choose a smooth curve $u_{\theta }(\cdot )$ of control functions, starting at $\theta =0$ from $u_0(\cdot )$, and compute a logarithmic error $e(\theta )=\log (R_T(\theta )R_d^T)=[l_T(\theta )]$, where $R_t(\theta )=\phi _{R_0,t}(u_{\theta }(\cdot ))$. We request that along the control curve the error decreases exponentially, i.e.

$$\begin{aligned} \frac{d e(\theta )}{d\theta }=-\gamma e(\theta ), \end{aligned}$$

(4)

for a positive decay rate $\gamma$. It follows that the identity (4) is tantamount to

$$\begin{aligned} \frac{d l_T(\theta )}{d\theta }=-\gamma l_T(\theta ). \end{aligned}$$

(5)

Letting $S_t(\theta )=\frac{\partial R_t(\theta )}{\partial \theta }R_t^T(\theta )=[s_t(\theta )],$ we get

$$ S_T(\theta )=\frac{d R_T(\theta )}{d\theta }R^T_T(\theta )=\frac{d \left( R_T(\theta )R_d^T\right) }{d\theta }\left( R_T(\theta )R_d^T\right) ^T=\frac{d\exp (\left[ l_T(\theta )\right] )}{d\theta }\exp (-\left[ l_T(\theta )\right] ). $$

Now, the Hausdorff formula [7] yields

$$\begin{aligned} \frac{d\exp (\left[ l_T(\theta )\right] )}{d\theta }\exp (-\left[ l_T(\theta )\right] )=\left[ P(\theta )\frac{d l_T(\theta )}{d\theta }\right] , \end{aligned}$$

(6)

what means that

$$\begin{aligned} s_T(\theta )=P(\theta )\frac{d l_T(\theta )}{d\theta }, \end{aligned}$$

(7)

where

$$ P(\theta )=\exp ([l_T(\theta )])=\mathbb {I}_3+\frac{1-\cos \alpha }{\alpha ^2}[l_T(\theta )]+\frac{\alpha -\sin \alpha }{\alpha ^3}[l_T(\theta )]^2, $$

and $\alpha (\theta )$ denotes the rotation angle of $R_T(\theta )R_d^T$, given by $\cos \alpha (\theta )=\frac{1}{2}(tr(R_T(\theta )R_d^T)-1)$. Referring to the system (1), we compute

$$\begin{aligned} \frac{\partial }{\partial t}\frac{\partial R_t(\theta )}{\partial \theta }&=\frac{\partial }{\partial \theta }\frac{\partial R_t(\theta )}{\partial t}\\&=\left[ \left( \frac{\partial L_t(\theta )}{\partial \theta }\right) u_{\theta }(t)+L_t(\theta )\frac{\partial u_{\theta }(t)}{\partial \theta }\right] R_t(\theta )+\left[ L_t(\theta )u_{\theta }(t)\right] \frac{\partial R_t(\theta )}{\partial \theta }, \end{aligned}$$

and, after some mathematical developments, conclude that

$$\begin{aligned} \frac{\partial s_t(\theta )}{\partial t}=\frac{\partial L_t(\theta )}{\partial \theta }u_{\theta }(t)+L_t(\theta )\frac{\partial u_{\theta }(t)}{\partial \theta }+\left[ L_t(\theta )u_{\theta }(t)\right] s_t(\theta ). \end{aligned}$$

(8)

A little more computation shows that $\frac{\partial L_t(\theta )}{\partial \theta }u_{\theta }(t)=Q_t(\theta )s_t(\theta )$, for a certain matrix $Q_t(\theta )$, that will be displayed below. Thus, (8) can be represented as a linear, time dependent control system

$$\begin{aligned} \frac{\partial s_t(\theta )}{\partial t}=A_{\theta }(t)s_t(\theta )+B_{\theta }(t)\frac{\partial u_{\theta }(t)}{\partial \theta }, \end{aligned}$$

(9)

where $A_{\theta }(t)=Q_t(\theta )+\left[ L_t(\theta )u_{\theta }(t)\right]$, $B_{\theta }(t)=L_t(\theta )$, and $\frac{\partial u_{\theta }(t)}{\partial \theta }$ playing the role of control. After the integration of (9) with initial condition $s_0(\theta )=0$, we deduce from (7) a basic motion planning integral equation

$$\begin{aligned} J_{R_0,T}(u_{\theta }(\cdot ))\frac{\partial u_{\theta }(t)}{\partial \theta }=\int \limits _{0}^{T}\varPhi _{\theta }(T,t)B_{\theta }(t)\frac{\partial u_{\theta }(t)}{\partial \theta }dt=-\gamma l_T(\theta ), \end{aligned}$$

(10)

where the transition $\varPhi _{\theta }(t,s)$ of (9) satisfies the evolution equation $\frac{\partial \varPhi _{\theta }(t,s)}{\partial t}=A_{\theta }(t)\varPhi _{\theta }(t,s)$ along with $\varPhi _{\theta }(t,t)=\mathbb {I}_3$. Specifically, $\frac{\partial \varPhi _{\theta }(T,t)}{\partial t}=-\varPhi _{\theta }(T,t)A_{\theta }(t)$. The operator $J_{R_0,T}(u_{\theta }(\cdot ))$ appearing in (10) can be regarded as a Jacobian operator associated with the motion planning problem in the system (1). Using the Moore-Penrose inverse of this Jacobian, $J_{R_0,T}^{\#}(u_{\theta }(\cdot )),$ we derive the motion planning algorithm

$$\begin{aligned} \frac{\partial u_{\theta }(t)}{\partial \theta }= -\gamma \left( J_{R_0,T}^{\#}\left( u_{\theta }(\cdot )\right) \right) (t)l_T(\theta )= -\gamma B_{\theta }^T(t)\varPhi _{\theta }^T(T,t)M_{\theta }^{-1}(T)l_T(\theta ). \end{aligned}$$

(11)

The matrix $M(T)_{\theta }=\int _0^T\varPhi _{\theta }(T,t)B_{\theta }(t)B_{\theta }^T(t)\varPhi _{\theta }^T(T,t)dt$ that can be called a mobility matrix of the system (1) is found as a solution of the Lyapunov matrix differential equation $\frac{\partial M_{\theta }(t)}{\partial t}=B_{\theta }(t)B_{\theta }^T(t)+A_{\theta }(t)M_{\theta }(t)+M_{\theta }(t)A_{\theta }^T(t),\;\;M_{\theta }(0)=0.$ Finally, taking into account the identity (2), the matrix $Q_t(\theta )$ has been computed as

$$\begin{aligned} Q_t(\theta )=\frac{2}{\varDelta ^2}\begin{bmatrix} -r_{12}(m_1e_2^T -m_2e_3^T)[r_1]+r_{21}(m_1e_1^T+m_2e_3^T)[r_2]\\ r_{12}(m_1(e_2^T -m_2e_3^T)[r_1]-r_{21}(m_1e_1^T+m_2e_3^T)[r_2]\\ -r_{32}(m_1(e_2^T -m_2e_3^T)[r_1]-r_{31}(m_1e_1^T+m_2e_3^T)[r_2] \end{bmatrix}. \end{aligned}$$

(12)

Here above $R_t(\theta )=[r_{ij}]$, $m_1=r_{31}u_{\theta 1}-r_{32}u_{\theta 2}$, $m_2=r_{21}u_{\theta 1}+r_{12}u_{\theta 2}$, $r_1,r_2,r_3$ denote columns of $R_t(\theta )$, $e_1,e_2,e_3$ are unit vectors in $\mathbb {R}^3$, and $\varDelta =r_{12}r_{31}+r_{21}r_{32}$.