# Dynamic non-holonomic motion planning by means of dynamically consistent Jacobian inverse

Dynamic non-holonomic motion planning by means of dynamically consistent Jacobian inverse Abstract We develop a concept of dynamically consistent Jacobian inverse for non-holonomic robotic systems with dynamics. The guidelines for the development are provided by the endogenous configuration space approach. Similarly as for holonomic and non-holonomic kinematics, the dynamic consistency consists in preventing a transmission of forces from the null space of the dual Jacobian inverse to the operational space. The dynamically consistent Jacobian inverse for the non-holonomic systems with dynamics is introduced using the force method, and then applied to example motion planning problems for the rolling ball and the trident snake robot. 1. Introduction Jacobian motion planning algorithms serve as basic tools for solving the motion planning problem for holonomic and non-holonomic robotic systems. At the kinematics level these algorithms are defined by a dynamic system involving a right inverse of the Jacobian. A desirable feature of the Jacobian inverse is its dynamic consistency that means that forces contained in the null space of the dual Jacobian inverse do not affect the acceleration in the operational space. The dynamic consistency enables to decompose any configuration force into a component coming from the operational space and an internal force that does not affect the motion in the operational space. The dynamically consistent Jacobian inverse for robotic manipulators was invented by Khatib (1990, 1995), and justified in the context of operational space control of manipulators, humanoid robots and biomedical systems (Sentis et al., 2010; Demircan & Khatib, 2012). An analogous concept has been recently announced by these authors in a conference paper (Ratajczak & Tchoń, 2015), and extended to the mobile manipulators and general non-holonomic robotic systems, respectively, in Ratajczak & Tchoń (2016) and Tchoń & Ratajczak (2016). This paper focuses on the non-holonomic systems with dynamics. It is assumed that these systems are fully actuated, and that their equations of motion are represented by a control-affine system with output. In such a control system framework we define a Jacobian, extend the derivation of the dynamically consistent Jacobian inverse from the non-holonomic kinematics to the non-holonomic system’s dynamics, introduce a corresponding motion planning algorithm, and evaluate its performance on example motion planning problems for the rolling ball and the trident snake robot. Our derivation is based on the endogenous configuration space approach (Tchoń & Jakubiak, 2003) that easily generalizes to non-holonomic dynamics (Tchoń et al., 2009), and is guided by the force method presented in Tchoń & Ratajczak (2016). With reference to mathematical tools employed in this paper we have been mostly inspired by Hauser (2002). The organization of this paper is the following. Section 2 introduces basics of the control system representation of non-holonomic systems as well of the endogenous configuration space approach. In Section 3 we derive the dynamically consistent Jacobian inverse for non-holonomic systems with dynamics, represented by control-affine systems. Computations illustrating the performance of this inverse are collected in Section 4. Section 5 concludes the paper. 2. Basics The kinematics of a non-holonomic robotic system subject to Pfaffian motion constraints $$\mathrm{A}(q)\dot{q}=0,$$ will be represented by a driftless control system with output {q˙=G(q)η=∑i=1mGi(q)ηiy=h(q)=(h1(q),…,hr(q)). (2.1) Above, the number of Pfaffian constraints is $$l\le n$$, while $$q\in R^n$$, $$\eta\in R^m$$ and $$y\in Y=R^r$$ denote, respectively, the configuration variable, the kinematic control and the operational variable. By definition, the control distribution of (2.1) is annihilated by $$\mathrm{A}(q)$$, so $$\mathrm{A}(q)G_i(q)=0$$ for every $$i=1,\ldots,m=n-l$$. The dynamics equations of the non-holonomic system can be derived from the Lagrange–d’Alembert Principle (Bloch, 2003). To this objective, let $$L(q,\dot{q})=\frac{1}{2}\dot{q}^TM(q)\dot{q}-V(q)$$ denote the Lagrangian of the unconstrained system, the inertia matrix $$M(q)=M^T(q)>0$$ and the potential function $$V(q)$$ defining, respectively, the kinetic and the potential energy. Then, the Euler–Lagrange equations take the form M(q)q¨+N(q,q˙)+R(q)=T(q)+P(q)u, (2.2) where $$N(q,\dot{q})$$ represents centripetal and Coriolis terms, $$R(q)$$ denotes the gravity forces. $$T(q)$$ stands for the traction forces, $$P(q)$$ is a control matrix and $$u$$ denotes the control forces (torques). The traction forces can be expressed as $$T(q)=\mathrm{A}^T(q)\mu$$, with an $$l$$-dimensional vector of Lagrange multipliers $$\mu$$. Now, using the fact that $$G^T(q)T(q)=G^T(q)\mathrm{A}^T(q)\mu=0$$, we eliminate the traction forces from (2.2), and then make a substitution $$\ddot{q}=\dot{G}(q)\eta+G(q)\dot{\eta}$$, to finally obtain the following equations GT(q)M(q)G(q)η˙+GT(q)M(q)G˙(q)η+GT(q)N(q,G(q)η)+GT(q)R(q)=GT(q)P(q)u. (2.3) It is easily observed that the $$m\times m$$ matrix F(q)=GT(q)M(q)G(q) (2.4) plays the role of the inertia matrix in the presence of non-holonomic constraints. The matrix $$F(q)$$ is symmetric and, for $$M(q)$$ being positive definite and $$G(q)$$ of full rank, $$F(q)$$ is also positive definite. In what follows we shall assume that the matrix $$G^T(q)P(q)$$ is square $$m\times m$$ and invertible, so the dynamics (2.3) are fully actuated. After necessary formal developments, the motion equations of a non-holonomic system, including the kinematics (2.1) and the dynamics (2.3), can be given in the form of a control-affine system with output {x˙=f(x)+g(x)u=f(x)+∑i=1mgi(x)uiy=k(x)=(k1(x),…,kr(x)), (2.5) where $$x=(q,\eta)\in R^{n+m}$$, $$u\in R^m$$, $$y\in Y= R^r$$ stand for the state, control and output variable, $$f(x)=\left(G(q)\eta,-F^{-1}(q)G^T(q)(M(q)\dot{G}(q)\eta+N(q,G(q)\eta)+R(q))\right)$$ and $$g(x)=\left[0_{n\times m},\,F^{-1}(q)G^T(q)P(q)\right]$$. The output function characterizes the system’s behaviour in the operational space. The system (2.5) will be controlled over a fixed time horizon $$T>0$$. Its admissible control functions are assumed to belong to the Hilbert space $$L^2_m[0,T]$$ equipped with the inner product $$\langle u_1(\cdot),u_2(\cdot)\rangle=\int_0^Tu_1^T(t)u_2(t)dt$$. The space of control functions $$\mathcal{X}=L_m^2[0,T]$$ will be referred to as the endogenous configuration space. Suppose that for a given control $$u(\cdot)$$, $$x(t)=\varphi_{x_0,t}(u(\cdot))$$ is a state trajectory of this control system, initialized at $$x_0$$. A standing assumption will be made of the existence of this trajectory for every $$t\in [0,T]$$. If the corresponding output trajectory $$y(t)=k(x(t))$$ then the end-point map of the system (2.5) Kx0,T(u(⋅))=k(x(T))=k(φx0,T(u(⋅))) (2.6) assigns to any control function a value at $$T$$ of the operational variable. The motion planning problem for the non-holonomic system (2.5) consists in computing a control function $$u_d(\cdot)\in\mathcal{X}$$, such that at $$T$$ the operational variable assumes the desired value $$y_d$$, i.e. $$K_{x_0,T}(u_d(\cdot))=y_d$$. This motion planning problem can be solved by means of a Jacobian inverse of the map (2.6). To this objective we define the Jacobian of the system (2.5) by differentiation of $$K_{x_0,T}(u_d(\cdot))$$, so that $$J_{x_0,T}(u(\cdot))=\mathrm{D}\,K_{x_0,T}(u(\cdot)),$$$$\mathrm{D}$$ denoting the derivative with respect to $$u(\cdot)$$. It follows that the Jacobian is determined by the linear approximation to (2.5) along a control-trajectory pair $$(u(t),x(t))$$ (Sontag, 1985). Specifically, we first compute the linear approximation A(t)=∂(f(x(t))+g(x(t))u(t))∂x,B(t)=g(x(t)),C(t)=∂k(x(t))∂x, and then, for a given control function $$v(\cdot)\in\mathcal{X}$$, define ξ(t)=Dφx0,t(u(⋅))v(⋅)∈Rn+m,ξ(0)=0. It can be shown that $$\xi(t)$$ is a trajectory of a variational system {ξ˙=A(t)ξ+B(t)v,w(t)=C(t)ξ,  with output $$w\in R^r$$. Finally, the Jacobian Jx0,T(u(⋅))v(⋅)=C(T)ξ(T)=C(T)∫0TΦ(T,t)B(t)v(t)dt, (2.7) where the transition matrix $${\it\Phi}(t,s)$$ satisfies the evolution equation $$\frac{\partial{\it\Phi}(t,s)}{\partial t}=A(t){\it\Phi}(t,s)$$ for the initial condition $${\it\Phi}(s,s)=I_{n+m}.$$ By definition, the Jacobian is a linear map of tangent spaces Jx0,T(u(⋅)):Tu(⋅)X⟶TKx0,T(u(⋅))Y,Tu(⋅)X≅X,TyY≅Y. This being so, a right Jacobian inverse Jx0,T#(u(⋅)):TKx0,T(u(⋅))Y⟶Tu(⋅)X,Jx0,T(u(⋅))Jx0,T#(u(⋅))=idTKx0,T(u(⋅))Y, transforms tangent vectors from the operational to the endogenous configuration space. The following reasoning, rooted in the continuation method (Sussmann, 1992), provides a background for defining right Jacobian inverses. Let $$u_{\theta}(\cdot)$$, $$\theta\in R$$ denote a differentiable curve in the endogenous configuration space. We compute the operational space error $$e(\theta)=K_{x_0,T}(u_\theta(\cdot))-y_d$$ along this curve. The curve should make the error decrease with $$\theta$$, therefore e′(θ)=ddθ(Kx0,T(uθ(⋅))−yd)=Jx0,T(uθ(⋅))uθ′(⋅)=−γe(θ), where $$(\cdot)'$$ refers to the derivation with respect to $$\theta$$, and $$\gamma$$ is a positive constant. Now, the right-hand side identity can be converted into an explicit differential equation by means of a right inverse of the Jacobian. Every such inverse $$J_{x_0,T}^{\#}(u(\cdot))$$ results in the dynamic system uθ′(⋅)=−γJx0,T#(uθ(⋅))(Kx0,T(uθ(⋅))−yd),uθ=0(t)=u0(t), (2.8) whose trajectory provides a solution of the motion planning problem as the limit $$\lim_{\theta\rightarrow+\infty}u_\theta(t)=u_d(t)$$. Depending on the choice of the Jacobian inverse the identity (2.8) defines a specific Jacobian motion planning algorithm for the non-holonomic system (2.5) with dynamics. A kind of canonical choice is the Jacobian pseudoinverse (the Moore–Penrose generalized inverse) defined as (Jx0,T#P(u(⋅))w)(t)=BT(t)ΦT(T,t)(C(T)∫0TΦ(T,t)B(t)BT(t)ΦT(T,t)dtCT(T))−1w. (2.9) It is easily seen that the reasoning presented above applies directly to a purely kinematic motion planning problem addressed in the system (2.1). 3. Dynamically consistent inverse Given the Jacobian (2.7) and any right inverse, we introduce two dual maps acting between respective dual spaces: the dual Jacobian Jx0,T∗(u(⋅)):Ty∗Y⟶Tu(⋅)∗X,(Jx0,T∗(u(⋅))r)v(⋅)=rJx0,T(u(⋅))v(⋅)=⟨BT(⋅)ΦT(T,⋅)rT,v(⋅)⟩, and the dual Jacobian inverse Jx0,T#∗(u(⋅)):Tu(⋅)∗X⟶Ty∗Y,(Jx0,T#∗(u(⋅))p(⋅))w=p(⋅)Jx0,T#(u(⋅))w, where $$y=K_{x_0,T}(u(\cdot))$$, and (for row vectors $$p(\cdot),\,r$$) the identities $$p(\cdot)v(\cdot)=\int_0^T\!p(t)v(t)dt=\langle p^T(\cdot),v(\cdot)\rangle$$ and $$rw$$ denote the pairings between suitable co-vectors and vectors. Geometrically, the tangent vectors to the endogenous configuration space can be treated as endogenous velocities, while the co-vectors are referred to as the endogenous forces. In accordance with this terminology the maps $$J_{x_0,T}(u(\cdot))$$, $$J^{\#*}_{x_0,T}(u(\cdot))$$ transform velocities and forces from the endogenous configuration space to the operational space. The postulate of dynamic consistency of the Jacobian inverse refers to these maps and means that the endogenous forces annihilated by the latter of these two maps cannot produce any acceleration in the operational space. Pursuing the force method proposed in Tchoń & Ratajczak (2016), we begin with the introduction of a Riemannian metric into the endogenous configuration space. To this aim we use the inertia matrix $$F(q)$$, see (2.4), take the trajectory $$q(t)=\varphi_{x_0,t}(u(\cdot))$$ of the system (2.5) steered by the control function $$u(\cdot)\in\mathcal{X}$$, and define a matrix function Mx0(u(⋅))(t)=F(φx0,t(u(⋅))). (3.1) In consequence, the Riemannian metric on $$\mathcal{X}$$ can be obtained as gX(u(⋅))(v1(⋅),v2(⋅))=∫0Tv1T(t)Mx0(u(⋅))(t)v2(t)dt, (3.2) where $$v_1(\cdot),v_2(\cdot)\in T_{u(\cdot)}\mathcal{X}$$ are endogenous velocities. This being so, let us suppose that the endogenous configuration moves in $$\mathcal{X}$$ along a smooth curve $$u_{\theta}(\cdot)$$, with endogenous velocity $$u'_\theta(\cdot)$$ (the derivative being with respect to $$\theta$$). Then, the corresponding endogenous momentum pθ(⋅)=uθ′T(⋅)Mx0(uθ(⋅))(⋅), so that $$p_\theta(t)=u'^T_\theta(t)\mathcal{M}_{x_0}(u_\theta(\cdot))(t)$$, whereas the endogenous force fθ(⋅)=pθ′(⋅)=uθ″T(⋅)Mx0(uθ(⋅))(⋅)+uθ′T(⋅)Mx0′(uθ(⋅))(⋅), (3.3) and $$f_\theta(t)=u''^T_\theta(t)\mathcal{M}_{x_0}(u_\theta(\cdot))(t)+u'^T_\theta(t) \mathcal{M}'_{x_0}(u_\theta(\cdot))(t)$$, with $$(\cdot)''$$ denoting the second order derivative with respect to $$\theta$$. Now, using the decomposition of the cotangent space at $$u(\cdot)$$ Tu(⋅)∗X=ImJx0,T∗(u(⋅))⊕KerJx0,T#∗(u(⋅)), see Tchoń & Ratajczak (2016), we partition the endogenous force $$f_\theta(\cdot)$$ into a sum of two components fθT(⋅)=Jx0,T∗(uθ(⋅))Γ+(idTuθ(⋅)∗X−Jx0,T∗(uθ(⋅))Jx0,T#∗(uθ(⋅)))f0(⋅), (3.4) with $${\it\Gamma}^T$$ acting in the operational space and $$f_0^T(\cdot)$$ coming from the endogenous configuration space. Since the curve $$u_\theta(\cdot)$$ in the endogenous configuration space produces a curve $$y_\theta=K_{x_0,T}(u_\theta(\cdot))$$ in the operational space, the respective velocities are transformed by the Jacobian, yθ′=Jx0,T(uθ(⋅))uθ′(⋅), and the acceleration is equal to yθ″=Jx0,T(uθ(⋅))uθ″(⋅)+Jx0,T′(uθ(⋅))uθ′(⋅). After a substitution into the latter identity from (3.3) and (3.4) for, respectively, $$u''_\theta(\cdot)$$ and $$f_\theta(\cdot)$$, we get yθ″ =Jx0,T(uθ(⋅))Mx0−1(uθ(⋅))(⋅)(Jx0,T∗(uθ(⋅))Γ+(idTuθ(⋅)∗X−Jx0,T∗(uθ(⋅))Jx0,T#∗(uθ(⋅)))f0(⋅)  −Mx0′(uθ(⋅))(⋅)uθ′(⋅))+Jx0,T′(uθ(⋅))uθ′(⋅), (3.5) where the term in brackets multiplied by $$f_0(\cdot)$$ refers to the endogenous forces belonging to the null space of $$J_{x_0,T}^{\# *}(u_\theta(\cdot))$$, so to be annihilated. The defining condition of the dynamic consistency of the Jacobian inverse states that this term should not affect the acceleration in the operational space, i.e. at any $$u(\cdot)$$ Jx0,T(u(⋅))Mx0−1(u(⋅))(⋅)=Jx0,T(u(⋅))Mx0−1(u(⋅))(⋅)Jx0,T∗(u(⋅))Jx0,T#∗(u(⋅)). Taking this into account we finally obtain the following formula for the dynamically consistent Jacobian inverse (Jx0,T#DC(u(⋅))w)(t)=Mx0−1(u(⋅))(t)BT(t)ΦT(T,t)CT(T)(Dx0,TDC)−1(u(⋅))w, (3.6) where $$w\in T_yY$$. The matrix Dx0,TDC(u(⋅))=C(T)∫0TΦ(T,t)B(t)Mx0−1(u(⋅))(t)BT(t)ΦT(T,t)dtCT(T) (3.7) may be called a dynamically consistent mobility matrix. The inverse is well defined provided that this mobility matrix is full rank, i.e. when the Jacobian map is surjective. The endogenous configurations at which $$\text{rank} \mathcal{D}_{x_0,T}^{{\rm DC}}(u(\cdot))=r$$ are called regular, otherwise they are (dynamically) singular. It follows that the motion planning algorithm involving the inverse (3.6) is governed by the dynamic system uθ′(⋅)=−γJx0,T#DC(uθ(⋅))(Kx0,T(uθ(⋅))−yd),uθ=0(t)=u0(t). (3.8) 4. Computations For illustration of performance of the motion planning algorithm based on the dynamically consistent Jacobian inverse (3.6), we shall solve two motion planning problems: for the ball with dynamics rolling on a horizontal plane and for the trident snake mobile robot (Ishikawa, 2004; Paszuk et al., 2012). For comparison, the same problem will also be solved by means of the Jacobian pseudoinverse (2.9). 4.1. Rolling ball The ball displayed in Fig. 1 has coordinate vector $$q=(x,y,\varphi,\theta,\psi)\in R^5$$, with components marked in the figure. The data (2.3) characterizing this system are the following: the velocity control matrix G(q)=[Rsin⁡q4sin⁡q5Rcos⁡q5−Rsin⁡q4cos⁡q5Rsin⁡q51001−cos⁡q40], the inertia matrix $$F(q)=(I+mR^2)\mathrm{diag}\{\sin^2q_4,1\}$$, the gravity term $$R(q)=0$$, the centripetal and Coriolis term $$G^T(q)\big(M(q)\dot{G}(q)\eta+N(q,G(q)\eta)\big)=(I+mR^2)\sin q_4\cos q_4(2\eta_1\eta_2,-\eta_1^2)^T$$, and the torque control matrix $$G^T(q)P(q)=I_2$$. The output function is chosen as the ball’s position and orientation $$k(q)=(q_1,q_2,q_5)$$. In computations it has been assumed that the ball has the unit mass $$m=1$$, distributed uniformly, the radius $$R=0.1$$, and the inertia $$I=\frac{2}{5}mR^2$$. The ball’s equations of motion are described by a control-affine system (2.5) with $$q\in R^5$$ and $$\eta, u\in R^2$$. Fig. 1. View largeDownload slide Rolling ball. Fig. 1. View largeDownload slide Rolling ball. A motion planning problem is addressed, consisting in reaching by the ball the desired position and orientation $$y_d=(1,0,-\frac{\pi}{2})$$ in time $$T=5$$. The initial state of the ball is equal to $$x_0=(0,0,0,\frac{\pi}{4},\frac{\pi}{2},0,0)\in R^7$$. The motion planning problem has been solved using two Jacobian motion planning algorithms: the dynamically consistent Jacobian inverse, and the Jacobian pseudoinverse. Figure 2 shows the results. The computations have been run until the error in the operational space drops below $$10^{-4}$$. It may be observed that these algorithm generate qualitatively different $$XY$$ paths; in the case of the Jacobian pseudoinverse the ball stops at approximately at the time instant $$t=3.8$$, whereas the dynamically consistent Jacobian produces permanent and smooth rolling. Fig. 2. View largeDownload slide Motion planning of the rolling ball with dynamics: convergence, position paths in $$XY$$ plane and orientation. Fig. 2. View largeDownload slide Motion planning of the rolling ball with dynamics: convergence, position paths in $$XY$$ plane and orientation. 4.2. Trident snake The trident snake mobile robot consist of a triangular-shape body, depicted in Fig. 3. The robot is equipped with three rotatable links supported by passive wheels. The lateral slip of the wheels is not permitted giving rise the non-holonomic phase constraints. The trident snake was invented by Ishikawa (2004) and Ishikawa et al. (2010), as a physical example of a non-holonomic system whose control distribution is three-dimensional and bracket-generating. In accordance with Fig. 3, the vector of generalized coordinates $$q=(x,y,\theta,\phi_1,\phi_2,\phi_3)\in R^6$$ describes the position and orientation of the robot’s body, and the angular positions of its links. The trident snake’s kinematics are represented by the driftless control system (2.1), of the form borrowed from Paszuk et al. (2012) q˙=[−1a[f1f2f3g1g2g3h1h2h3]I3]η, (4.1) where $$\eta\in R^3$$ denotes the kinematic control, and a =l−3((l+rcos⁡ϕ1)sin⁡(ϕ3−ϕ2+2π3)+(l+rcos⁡ϕ2)sin⁡(ϕ1−ϕ3+2π3)  + (l+rcos⁡ϕ3)sin⁡(ϕ2−ϕ1+2π3)),fi =l−2((l+rcos⁡ϕi+2)cos⁡(ϕi+1+αi+1+θ)−(l+rcos⁡ϕi+1)cos⁡(ϕi+2+αi+2+θ)),gi =l−2((l+rcos⁡ϕi+2)sin⁡(ϕi+1+αi+1+θ)−(l+rcos⁡ϕi+1)sin⁡(ϕi+2+αi+2+θ)),hi =−l−2sin⁡(ϕi+1−ϕi+2+αi+1−αi+2), with indexes $$i$$ to be counted modulo $$3$$ (i.e. $$\alpha_4=\alpha_1,$$$$\alpha_5=\alpha_2$$, etc). The following values of geometric parameters have been assumed: the length of each link $$l=0.12$$ m, the distance between the centre of the robot’s body and the joints $$r=0.12$$ m, and the angles $$\alpha_1=-\frac{2\pi}{3}$$, $$\alpha_2=0$$, $$\alpha_3=\frac{2\pi}{3}$$. Fig. 3. View largeDownload slide Trident snake robot: physical (built in our laboratory) and schematic. Fig. 3. View largeDownload slide Trident snake robot: physical (built in our laboratory) and schematic. The dynamics equations of the trident snake have been derived from the Lagrange–d’Alembert Principle by Pietrowska (2012). The inertia matrix $$M(q)=M^T(q)=[m_{ij}]$$, $$i,j=1,\ldots,6$$, has the following entries m11=m22=mc+3ml+3mm,m44=m55=m66=I0w+mwl2+13mll2,m14=−l(mw+12ml)sin⁡(α1+ϕ1+θ),m24=l(mw+12ml)cos⁡(α1+ϕ1+θ),m15=−l(mw+12ml)sin⁡(α2+ϕ2+θ),m25=l(mw+12ml)cos⁡(α2+ϕ2+θ),m16=−l(mw+12ml)sin⁡(α3+ϕ3+θ),m26=l(mw+12ml)cos⁡(α3+ϕ3+θ),m34=I0w+mwl(l+rcos⁡(ϕ1))+16mll(2l+3rcos⁡(ϕ1)),m35=I0w+mwl(l+rcos⁡(ϕ2))+16mll(2l+3rcos⁡(ϕ2)),m36=I0w+mwl(l+rcos⁡(ϕ3))+16mll(2l+3rcos⁡(ϕ3)),m13=−mwl(sin⁡(α1+ϕ1+θ)+sin⁡(α2+ϕ2+θ)+sin⁡(α3+ϕ3+θ))−mwr(sin⁡(α1+θ) +sin⁡(α2+θ)+sin⁡(α3+θ))−12ml(2r(sin⁡(α1+θ)+sin⁡(α2+θ)+sin⁡(α3+θ)) +l(sin⁡(α1+ϕ1+θ)+sin⁡(α2+ϕ2+θ)+sin⁡(α3+ϕ3+θ))),m23=mwl(cos⁡(α1+ϕ1+θ)+cos⁡(α2+ϕ2+θ)+cos⁡(α3+ϕ3+θ))+mwr(cos⁡(α1+θ) +cos⁡(α2+θ)+cos⁡(α3+θ))+12ml(2r(cos⁡(α1+θ)+cos⁡(α2+θ)+cos⁡(α3+θ)) +l(cos⁡(α1+ϕ1+θ)+cos⁡(α2+ϕ2+θ)+cos⁡(α3+ϕ3+θ))) +mmr(cos⁡(θ+α1)+cos⁡(θ+α2)+cos⁡(θ+α3)),m33=I0+3I0w+3mw(r2+l2)+2mwrl(cos⁡(ϕ1)+cos⁡(ϕ2)+cos⁡(ϕ3)) +ml(l2+3r2+lr(cos⁡(ϕ1)+cos⁡(ϕ2)+cos⁡(ϕ3)))+6mmr2. Parameters of the robot’s dynamics are: the body mass $$m_0=0.52$$ kg, the wheel mass $$m_w=0.03$$ kg, the wheel radius $$r_w=0.02$$ m, the wheel width $$d=0.01$$ m, the link mass $$m_l=0.07$$ kg, the motor mass $$m_m=0.055$$ kg, the total mass $$m_c=m_0+3(m_w+m_m+m_l)$$, and the moments of inertia $$I_0=\frac{m_0r^2}{4}$$, $$I_{0w}=\frac{m_w(3r_w^2+d^2)}{12}$$. Further on, for the sake of computational simplicity, we have assumed that the robot moves very slowly (actually it crawls), so the centripetal and Coriolis term in (2.3) can be skipped. Also, the gravitational term is zero, because the robots moves on a horizontal plane. The trident snake’s kinematics and dynamics define a control affine system (2.5), with system variables $$q\in R^6$$, and $$\eta,\,u,\,y\in R^3$$. The output function is chosen as the robot’s body position and orientation $$k(x)=(q_1,q_2,q_3)$$, the initial state $$x_0=(0,0,0,0,0,0,0,0,0)\in R^9$$ and $$\gamma=0.5$$. Two motion planning problems are addressed. In first problem we set $$y_d=(0.3,0,\frac{\pi}{2})$$ and the time horizon $$T=15$$, while in second one $$y_d=(0.1,0.1,\frac{\pi}{2})$$ and $$T=10$$. Figure 4 shows the solutions of these problems obtained by means of the dynamically consistent Jacobian inverse and the Jacobian pseudoinverse. One can observe that the solutions provided by both these inverses are quite similar; however the speed of convergence of the former is remarkably bigger than of the latter. The computation time depends on the complexity of the system. Here, in trident snake case, one iteration of the dynamically consistent Jacobian algorithm takes about $$1.9$$ s and $$1.6$$ s when we apply the Jacobian pseudoinverse algorithm (PC with $$3.40$$ GHz processor). Fig. 4. View largeDownload slide Motion planning of the trident snake with dynamics: convergence, position paths in $$XY$$ plane and orientation (top—problem $$1$$, bottom—problem $$2$$). Fig. 4. View largeDownload slide Motion planning of the trident snake with dynamics: convergence, position paths in $$XY$$ plane and orientation (top—problem $$1$$, bottom—problem $$2$$). 5. Conclusion Using a control system representation of the dynamic equations of motion for a non-holonomic robotic system, we have defined the Jacobian and derived a dynamically consistent Jacobian inverse. Our derivation extends the concept of dynamically consistent Jacobian inverse from the non-holonomic kinematics to the non-holonomic dynamics, under assumption that the dynamics are fully actuated. The new inverse is put into a Jacobian motion planning algorithm whose performance has been illustrated by example applications to the rolling ball and to the trident snake robot. Advantages of the dynamically consistent Jacobian motion planning in comparison to the Jacobian pseudoinverse have been discovered. Acknowledgements The authors are very much indebted to anonymous reviewers whose remarks allowed them to improve the presentation of this paper. Funding National Science Centre, Poland, under the grant decision no. (DEC-2013/09/B/ST7/02368). References Bloch A. M. (2003) Nonholonomic Mechanics and Control . New York: Springer . Google Scholar CrossRef Search ADS Demircan E. & Khatib O. (2012) Constraint-consistent analysis of muscle force contribution to human gait . Latest Advances in Robot Kinematics ( Lenarcic J. & Husty M. eds). Netherlands: Springer , pp. 301 – 308 . Google Scholar CrossRef Search ADS Hauser J. (2002) A projection operator approach to the optimization of trajectory functionals . Preprints of 15th IFAC World Congress , Barcelona, Pergamon, Elsevier, July 21st – 26th . Ishikawa M. (2004) Trident snake robot: locomotion analysis and control . IFAC Symp. on Nonlinear Control Systems . Houston, TX: Gulf Professional Publishing , pp. 1169 – 1174 . Google Scholar CrossRef Search ADS Ishikawa M. , Minati Y. & Sugie T. (2010) Development and control experiment of the trident snake robot . IEEE/ASME Trans. Mechatron. , 15 , 9 – 16 . Google Scholar CrossRef Search ADS Khatib O. (1990) Motion Force Redundancy of Manipulators . 1990 Japan-USA Symposium on Flexible Automation , Japan: ISCIE, Kyoto, pp. 337 – 342 . Khatib O. (1995) Inertial Properties in Robotics Manipulation: An Object-Level Framework . Int. J. Robotics Res., 14 , 19 – 36 . Google Scholar CrossRef Search ADS Paszuk D. Tchoń K. & Pietrowska Z. (2012) Motion planning of the trident snake robot equipped with passive or active wheels . Bull. Pol. Acad. Sci., Tech. Sci. , 60 , 547 – 554 . Pietrowska Z. (2012) Kinematics, dynamics, and control of a trident snake nonholonomic system . Master’s Thesis, Wrocław University of Technology , Poland (in Polish) . Ratajczak J. & Tchoń K. (2015) Non-holonomic motion planning using dynamically consistent Jacobian inverse . Proc. IMA Conf. on Mathematics of Robotics , St. Anne’s College, University of Oxford , Oxford , UK . Ratajczak J. & Tchoń K. (2016) Dynamically consistent Jacobian inverse for mobile manipulators . Int. J. Control , 89 , 1159 – 1168 . Google Scholar CrossRef Search ADS Sentis L. Park J. & Khatib O. (2010) Compliant Control of Multicontact and Center-of-Mass Behaviors in Humanoid Robots . IEEE Trans. Robot. , 26 , 483 – 501 . Google Scholar CrossRef Search ADS Sontag E. D. (1985) Mathematical Control Theory . New York: Springer . Sussmann H. J. (1992) New differential geometric methods in nonholonomic path finding . Systems, Models, and Feedback ( Isidori A. & Tarn T. J. eds). Boston, MA: Birkhäuser , pp. 365 – 384 . Google Scholar CrossRef Search ADS Tchoń K. & Jakubiak J. (2003) Endogenous configuration space approach to mobile manipulators: a derivation and performance assessment of Jacobian inverse kinematics algorithms . Int. J. Control 76 , 1387 – 1419 . Google Scholar CrossRef Search ADS Tchoń K. Jakubiak J. & Małek ł. (2009) Motion Planning of Nonholonomic Systems with Dynamics . Computational Kinematics ( Kecskeméthy A. & Müller A. eds). Berlin: Springer , pp. 125 – 132 . Google Scholar CrossRef Search ADS Tchoń K. & Ratajczak J. (2016) Dynamically consistent Jacobian inverse for non-holonomic robotic systems . Nonlinear Dyn. , 85 , 107 – 122 . Google Scholar CrossRef Search ADS © The authors 2016. Published by Oxford University Press on behalf of the Institute of Mathematics and its Applications. All rights reserved. This article is published and distributed under the terms of the Oxford University Press, Standard Journals Publication Model (https://academic.oup.com/journals/pages/open_access/funder_policies/chorus/standard_publication_model) http://www.deepdyve.com/assets/images/DeepDyve-Logo-lg.png IMA Journal of Mathematical Control and Information Oxford University Press

# Dynamic non-holonomic motion planning by means of dynamically consistent Jacobian inverse

, Volume Advance Article (2) – Nov 15, 2016
11 pages

/lp/ou_press/dynamic-non-holonomic-motion-planning-by-means-of-dynamically-LnJWZn9Va0
Publisher
Oxford University Press
ISSN
0265-0754
eISSN
1471-6887
D.O.I.
10.1093/imamci/dnw058
Publisher site
See Article on Publisher Site

### Abstract

Abstract We develop a concept of dynamically consistent Jacobian inverse for non-holonomic robotic systems with dynamics. The guidelines for the development are provided by the endogenous configuration space approach. Similarly as for holonomic and non-holonomic kinematics, the dynamic consistency consists in preventing a transmission of forces from the null space of the dual Jacobian inverse to the operational space. The dynamically consistent Jacobian inverse for the non-holonomic systems with dynamics is introduced using the force method, and then applied to example motion planning problems for the rolling ball and the trident snake robot. 1. Introduction Jacobian motion planning algorithms serve as basic tools for solving the motion planning problem for holonomic and non-holonomic robotic systems. At the kinematics level these algorithms are defined by a dynamic system involving a right inverse of the Jacobian. A desirable feature of the Jacobian inverse is its dynamic consistency that means that forces contained in the null space of the dual Jacobian inverse do not affect the acceleration in the operational space. The dynamic consistency enables to decompose any configuration force into a component coming from the operational space and an internal force that does not affect the motion in the operational space. The dynamically consistent Jacobian inverse for robotic manipulators was invented by Khatib (1990, 1995), and justified in the context of operational space control of manipulators, humanoid robots and biomedical systems (Sentis et al., 2010; Demircan & Khatib, 2012). An analogous concept has been recently announced by these authors in a conference paper (Ratajczak & Tchoń, 2015), and extended to the mobile manipulators and general non-holonomic robotic systems, respectively, in Ratajczak & Tchoń (2016) and Tchoń & Ratajczak (2016). This paper focuses on the non-holonomic systems with dynamics. It is assumed that these systems are fully actuated, and that their equations of motion are represented by a control-affine system with output. In such a control system framework we define a Jacobian, extend the derivation of the dynamically consistent Jacobian inverse from the non-holonomic kinematics to the non-holonomic system’s dynamics, introduce a corresponding motion planning algorithm, and evaluate its performance on example motion planning problems for the rolling ball and the trident snake robot. Our derivation is based on the endogenous configuration space approach (Tchoń & Jakubiak, 2003) that easily generalizes to non-holonomic dynamics (Tchoń et al., 2009), and is guided by the force method presented in Tchoń & Ratajczak (2016). With reference to mathematical tools employed in this paper we have been mostly inspired by Hauser (2002). The organization of this paper is the following. Section 2 introduces basics of the control system representation of non-holonomic systems as well of the endogenous configuration space approach. In Section 3 we derive the dynamically consistent Jacobian inverse for non-holonomic systems with dynamics, represented by control-affine systems. Computations illustrating the performance of this inverse are collected in Section 4. Section 5 concludes the paper. 2. Basics The kinematics of a non-holonomic robotic system subject to Pfaffian motion constraints $$\mathrm{A}(q)\dot{q}=0,$$ will be represented by a driftless control system with output {q˙=G(q)η=∑i=1mGi(q)ηiy=h(q)=(h1(q),…,hr(q)). (2.1) Above, the number of Pfaffian constraints is $$l\le n$$, while $$q\in R^n$$, $$\eta\in R^m$$ and $$y\in Y=R^r$$ denote, respectively, the configuration variable, the kinematic control and the operational variable. By definition, the control distribution of (2.1) is annihilated by $$\mathrm{A}(q)$$, so $$\mathrm{A}(q)G_i(q)=0$$ for every $$i=1,\ldots,m=n-l$$. The dynamics equations of the non-holonomic system can be derived from the Lagrange–d’Alembert Principle (Bloch, 2003). To this objective, let $$L(q,\dot{q})=\frac{1}{2}\dot{q}^TM(q)\dot{q}-V(q)$$ denote the Lagrangian of the unconstrained system, the inertia matrix $$M(q)=M^T(q)>0$$ and the potential function $$V(q)$$ defining, respectively, the kinetic and the potential energy. Then, the Euler–Lagrange equations take the form M(q)q¨+N(q,q˙)+R(q)=T(q)+P(q)u, (2.2) where $$N(q,\dot{q})$$ represents centripetal and Coriolis terms, $$R(q)$$ denotes the gravity forces. $$T(q)$$ stands for the traction forces, $$P(q)$$ is a control matrix and $$u$$ denotes the control forces (torques). The traction forces can be expressed as $$T(q)=\mathrm{A}^T(q)\mu$$, with an $$l$$-dimensional vector of Lagrange multipliers $$\mu$$. Now, using the fact that $$G^T(q)T(q)=G^T(q)\mathrm{A}^T(q)\mu=0$$, we eliminate the traction forces from (2.2), and then make a substitution $$\ddot{q}=\dot{G}(q)\eta+G(q)\dot{\eta}$$, to finally obtain the following equations GT(q)M(q)G(q)η˙+GT(q)M(q)G˙(q)η+GT(q)N(q,G(q)η)+GT(q)R(q)=GT(q)P(q)u. (2.3) It is easily observed that the $$m\times m$$ matrix F(q)=GT(q)M(q)G(q) (2.4) plays the role of the inertia matrix in the presence of non-holonomic constraints. The matrix $$F(q)$$ is symmetric and, for $$M(q)$$ being positive definite and $$G(q)$$ of full rank, $$F(q)$$ is also positive definite. In what follows we shall assume that the matrix $$G^T(q)P(q)$$ is square $$m\times m$$ and invertible, so the dynamics (2.3) are fully actuated. After necessary formal developments, the motion equations of a non-holonomic system, including the kinematics (2.1) and the dynamics (2.3), can be given in the form of a control-affine system with output {x˙=f(x)+g(x)u=f(x)+∑i=1mgi(x)uiy=k(x)=(k1(x),…,kr(x)), (2.5) where $$x=(q,\eta)\in R^{n+m}$$, $$u\in R^m$$, $$y\in Y= R^r$$ stand for the state, control and output variable, $$f(x)=\left(G(q)\eta,-F^{-1}(q)G^T(q)(M(q)\dot{G}(q)\eta+N(q,G(q)\eta)+R(q))\right)$$ and $$g(x)=\left[0_{n\times m},\,F^{-1}(q)G^T(q)P(q)\right]$$. The output function characterizes the system’s behaviour in the operational space. The system (2.5) will be controlled over a fixed time horizon $$T>0$$. Its admissible control functions are assumed to belong to the Hilbert space $$L^2_m[0,T]$$ equipped with the inner product $$\langle u_1(\cdot),u_2(\cdot)\rangle=\int_0^Tu_1^T(t)u_2(t)dt$$. The space of control functions $$\mathcal{X}=L_m^2[0,T]$$ will be referred to as the endogenous configuration space. Suppose that for a given control $$u(\cdot)$$, $$x(t)=\varphi_{x_0,t}(u(\cdot))$$ is a state trajectory of this control system, initialized at $$x_0$$. A standing assumption will be made of the existence of this trajectory for every $$t\in [0,T]$$. If the corresponding output trajectory $$y(t)=k(x(t))$$ then the end-point map of the system (2.5) Kx0,T(u(⋅))=k(x(T))=k(φx0,T(u(⋅))) (2.6) assigns to any control function a value at $$T$$ of the operational variable. The motion planning problem for the non-holonomic system (2.5) consists in computing a control function $$u_d(\cdot)\in\mathcal{X}$$, such that at $$T$$ the operational variable assumes the desired value $$y_d$$, i.e. $$K_{x_0,T}(u_d(\cdot))=y_d$$. This motion planning problem can be solved by means of a Jacobian inverse of the map (2.6). To this objective we define the Jacobian of the system (2.5) by differentiation of $$K_{x_0,T}(u_d(\cdot))$$, so that $$J_{x_0,T}(u(\cdot))=\mathrm{D}\,K_{x_0,T}(u(\cdot)),$$$$\mathrm{D}$$ denoting the derivative with respect to $$u(\cdot)$$. It follows that the Jacobian is determined by the linear approximation to (2.5) along a control-trajectory pair $$(u(t),x(t))$$ (Sontag, 1985). Specifically, we first compute the linear approximation A(t)=∂(f(x(t))+g(x(t))u(t))∂x,B(t)=g(x(t)),C(t)=∂k(x(t))∂x, and then, for a given control function $$v(\cdot)\in\mathcal{X}$$, define ξ(t)=Dφx0,t(u(⋅))v(⋅)∈Rn+m,ξ(0)=0. It can be shown that $$\xi(t)$$ is a trajectory of a variational system {ξ˙=A(t)ξ+B(t)v,w(t)=C(t)ξ,  with output $$w\in R^r$$. Finally, the Jacobian Jx0,T(u(⋅))v(⋅)=C(T)ξ(T)=C(T)∫0TΦ(T,t)B(t)v(t)dt, (2.7) where the transition matrix $${\it\Phi}(t,s)$$ satisfies the evolution equation $$\frac{\partial{\it\Phi}(t,s)}{\partial t}=A(t){\it\Phi}(t,s)$$ for the initial condition $${\it\Phi}(s,s)=I_{n+m}.$$ By definition, the Jacobian is a linear map of tangent spaces Jx0,T(u(⋅)):Tu(⋅)X⟶TKx0,T(u(⋅))Y,Tu(⋅)X≅X,TyY≅Y. This being so, a right Jacobian inverse Jx0,T#(u(⋅)):TKx0,T(u(⋅))Y⟶Tu(⋅)X,Jx0,T(u(⋅))Jx0,T#(u(⋅))=idTKx0,T(u(⋅))Y, transforms tangent vectors from the operational to the endogenous configuration space. The following reasoning, rooted in the continuation method (Sussmann, 1992), provides a background for defining right Jacobian inverses. Let $$u_{\theta}(\cdot)$$, $$\theta\in R$$ denote a differentiable curve in the endogenous configuration space. We compute the operational space error $$e(\theta)=K_{x_0,T}(u_\theta(\cdot))-y_d$$ along this curve. The curve should make the error decrease with $$\theta$$, therefore e′(θ)=ddθ(Kx0,T(uθ(⋅))−yd)=Jx0,T(uθ(⋅))uθ′(⋅)=−γe(θ), where $$(\cdot)'$$ refers to the derivation with respect to $$\theta$$, and $$\gamma$$ is a positive constant. Now, the right-hand side identity can be converted into an explicit differential equation by means of a right inverse of the Jacobian. Every such inverse $$J_{x_0,T}^{\#}(u(\cdot))$$ results in the dynamic system uθ′(⋅)=−γJx0,T#(uθ(⋅))(Kx0,T(uθ(⋅))−yd),uθ=0(t)=u0(t), (2.8) whose trajectory provides a solution of the motion planning problem as the limit $$\lim_{\theta\rightarrow+\infty}u_\theta(t)=u_d(t)$$. Depending on the choice of the Jacobian inverse the identity (2.8) defines a specific Jacobian motion planning algorithm for the non-holonomic system (2.5) with dynamics. A kind of canonical choice is the Jacobian pseudoinverse (the Moore–Penrose generalized inverse) defined as (Jx0,T#P(u(⋅))w)(t)=BT(t)ΦT(T,t)(C(T)∫0TΦ(T,t)B(t)BT(t)ΦT(T,t)dtCT(T))−1w. (2.9) It is easily seen that the reasoning presented above applies directly to a purely kinematic motion planning problem addressed in the system (2.1). 3. Dynamically consistent inverse Given the Jacobian (2.7) and any right inverse, we introduce two dual maps acting between respective dual spaces: the dual Jacobian Jx0,T∗(u(⋅)):Ty∗Y⟶Tu(⋅)∗X,(Jx0,T∗(u(⋅))r)v(⋅)=rJx0,T(u(⋅))v(⋅)=⟨BT(⋅)ΦT(T,⋅)rT,v(⋅)⟩, and the dual Jacobian inverse Jx0,T#∗(u(⋅)):Tu(⋅)∗X⟶Ty∗Y,(Jx0,T#∗(u(⋅))p(⋅))w=p(⋅)Jx0,T#(u(⋅))w, where $$y=K_{x_0,T}(u(\cdot))$$, and (for row vectors $$p(\cdot),\,r$$) the identities $$p(\cdot)v(\cdot)=\int_0^T\!p(t)v(t)dt=\langle p^T(\cdot),v(\cdot)\rangle$$ and $$rw$$ denote the pairings between suitable co-vectors and vectors. Geometrically, the tangent vectors to the endogenous configuration space can be treated as endogenous velocities, while the co-vectors are referred to as the endogenous forces. In accordance with this terminology the maps $$J_{x_0,T}(u(\cdot))$$, $$J^{\#*}_{x_0,T}(u(\cdot))$$ transform velocities and forces from the endogenous configuration space to the operational space. The postulate of dynamic consistency of the Jacobian inverse refers to these maps and means that the endogenous forces annihilated by the latter of these two maps cannot produce any acceleration in the operational space. Pursuing the force method proposed in Tchoń & Ratajczak (2016), we begin with the introduction of a Riemannian metric into the endogenous configuration space. To this aim we use the inertia matrix $$F(q)$$, see (2.4), take the trajectory $$q(t)=\varphi_{x_0,t}(u(\cdot))$$ of the system (2.5) steered by the control function $$u(\cdot)\in\mathcal{X}$$, and define a matrix function Mx0(u(⋅))(t)=F(φx0,t(u(⋅))). (3.1) In consequence, the Riemannian metric on $$\mathcal{X}$$ can be obtained as gX(u(⋅))(v1(⋅),v2(⋅))=∫0Tv1T(t)Mx0(u(⋅))(t)v2(t)dt, (3.2) where $$v_1(\cdot),v_2(\cdot)\in T_{u(\cdot)}\mathcal{X}$$ are endogenous velocities. This being so, let us suppose that the endogenous configuration moves in $$\mathcal{X}$$ along a smooth curve $$u_{\theta}(\cdot)$$, with endogenous velocity $$u'_\theta(\cdot)$$ (the derivative being with respect to $$\theta$$). Then, the corresponding endogenous momentum pθ(⋅)=uθ′T(⋅)Mx0(uθ(⋅))(⋅), so that $$p_\theta(t)=u'^T_\theta(t)\mathcal{M}_{x_0}(u_\theta(\cdot))(t)$$, whereas the endogenous force fθ(⋅)=pθ′(⋅)=uθ″T(⋅)Mx0(uθ(⋅))(⋅)+uθ′T(⋅)Mx0′(uθ(⋅))(⋅), (3.3) and $$f_\theta(t)=u''^T_\theta(t)\mathcal{M}_{x_0}(u_\theta(\cdot))(t)+u'^T_\theta(t) \mathcal{M}'_{x_0}(u_\theta(\cdot))(t)$$, with $$(\cdot)''$$ denoting the second order derivative with respect to $$\theta$$. Now, using the decomposition of the cotangent space at $$u(\cdot)$$ Tu(⋅)∗X=ImJx0,T∗(u(⋅))⊕KerJx0,T#∗(u(⋅)), see Tchoń & Ratajczak (2016), we partition the endogenous force $$f_\theta(\cdot)$$ into a sum of two components fθT(⋅)=Jx0,T∗(uθ(⋅))Γ+(idTuθ(⋅)∗X−Jx0,T∗(uθ(⋅))Jx0,T#∗(uθ(⋅)))f0(⋅), (3.4) with $${\it\Gamma}^T$$ acting in the operational space and $$f_0^T(\cdot)$$ coming from the endogenous configuration space. Since the curve $$u_\theta(\cdot)$$ in the endogenous configuration space produces a curve $$y_\theta=K_{x_0,T}(u_\theta(\cdot))$$ in the operational space, the respective velocities are transformed by the Jacobian, yθ′=Jx0,T(uθ(⋅))uθ′(⋅), and the acceleration is equal to yθ″=Jx0,T(uθ(⋅))uθ″(⋅)+Jx0,T′(uθ(⋅))uθ′(⋅). After a substitution into the latter identity from (3.3) and (3.4) for, respectively, $$u''_\theta(\cdot)$$ and $$f_\theta(\cdot)$$, we get yθ″ =Jx0,T(uθ(⋅))Mx0−1(uθ(⋅))(⋅)(Jx0,T∗(uθ(⋅))Γ+(idTuθ(⋅)∗X−Jx0,T∗(uθ(⋅))Jx0,T#∗(uθ(⋅)))f0(⋅)  −Mx0′(uθ(⋅))(⋅)uθ′(⋅))+Jx0,T′(uθ(⋅))uθ′(⋅), (3.5) where the term in brackets multiplied by $$f_0(\cdot)$$ refers to the endogenous forces belonging to the null space of $$J_{x_0,T}^{\# *}(u_\theta(\cdot))$$, so to be annihilated. The defining condition of the dynamic consistency of the Jacobian inverse states that this term should not affect the acceleration in the operational space, i.e. at any $$u(\cdot)$$ Jx0,T(u(⋅))Mx0−1(u(⋅))(⋅)=Jx0,T(u(⋅))Mx0−1(u(⋅))(⋅)Jx0,T∗(u(⋅))Jx0,T#∗(u(⋅)). Taking this into account we finally obtain the following formula for the dynamically consistent Jacobian inverse (Jx0,T#DC(u(⋅))w)(t)=Mx0−1(u(⋅))(t)BT(t)ΦT(T,t)CT(T)(Dx0,TDC)−1(u(⋅))w, (3.6) where $$w\in T_yY$$. The matrix Dx0,TDC(u(⋅))=C(T)∫0TΦ(T,t)B(t)Mx0−1(u(⋅))(t)BT(t)ΦT(T,t)dtCT(T) (3.7) may be called a dynamically consistent mobility matrix. The inverse is well defined provided that this mobility matrix is full rank, i.e. when the Jacobian map is surjective. The endogenous configurations at which $$\text{rank} \mathcal{D}_{x_0,T}^{{\rm DC}}(u(\cdot))=r$$ are called regular, otherwise they are (dynamically) singular. It follows that the motion planning algorithm involving the inverse (3.6) is governed by the dynamic system uθ′(⋅)=−γJx0,T#DC(uθ(⋅))(Kx0,T(uθ(⋅))−yd),uθ=0(t)=u0(t). (3.8) 4. Computations For illustration of performance of the motion planning algorithm based on the dynamically consistent Jacobian inverse (3.6), we shall solve two motion planning problems: for the ball with dynamics rolling on a horizontal plane and for the trident snake mobile robot (Ishikawa, 2004; Paszuk et al., 2012). For comparison, the same problem will also be solved by means of the Jacobian pseudoinverse (2.9). 4.1. Rolling ball The ball displayed in Fig. 1 has coordinate vector $$q=(x,y,\varphi,\theta,\psi)\in R^5$$, with components marked in the figure. The data (2.3) characterizing this system are the following: the velocity control matrix G(q)=[Rsin⁡q4sin⁡q5Rcos⁡q5−Rsin⁡q4cos⁡q5Rsin⁡q51001−cos⁡q40], the inertia matrix $$F(q)=(I+mR^2)\mathrm{diag}\{\sin^2q_4,1\}$$, the gravity term $$R(q)=0$$, the centripetal and Coriolis term $$G^T(q)\big(M(q)\dot{G}(q)\eta+N(q,G(q)\eta)\big)=(I+mR^2)\sin q_4\cos q_4(2\eta_1\eta_2,-\eta_1^2)^T$$, and the torque control matrix $$G^T(q)P(q)=I_2$$. The output function is chosen as the ball’s position and orientation $$k(q)=(q_1,q_2,q_5)$$. In computations it has been assumed that the ball has the unit mass $$m=1$$, distributed uniformly, the radius $$R=0.1$$, and the inertia $$I=\frac{2}{5}mR^2$$. The ball’s equations of motion are described by a control-affine system (2.5) with $$q\in R^5$$ and $$\eta, u\in R^2$$. Fig. 1. View largeDownload slide Rolling ball. Fig. 1. View largeDownload slide Rolling ball. A motion planning problem is addressed, consisting in reaching by the ball the desired position and orientation $$y_d=(1,0,-\frac{\pi}{2})$$ in time $$T=5$$. The initial state of the ball is equal to $$x_0=(0,0,0,\frac{\pi}{4},\frac{\pi}{2},0,0)\in R^7$$. The motion planning problem has been solved using two Jacobian motion planning algorithms: the dynamically consistent Jacobian inverse, and the Jacobian pseudoinverse. Figure 2 shows the results. The computations have been run until the error in the operational space drops below $$10^{-4}$$. It may be observed that these algorithm generate qualitatively different $$XY$$ paths; in the case of the Jacobian pseudoinverse the ball stops at approximately at the time instant $$t=3.8$$, whereas the dynamically consistent Jacobian produces permanent and smooth rolling. Fig. 2. View largeDownload slide Motion planning of the rolling ball with dynamics: convergence, position paths in $$XY$$ plane and orientation. Fig. 2. View largeDownload slide Motion planning of the rolling ball with dynamics: convergence, position paths in $$XY$$ plane and orientation. 4.2. Trident snake The trident snake mobile robot consist of a triangular-shape body, depicted in Fig. 3. The robot is equipped with three rotatable links supported by passive wheels. The lateral slip of the wheels is not permitted giving rise the non-holonomic phase constraints. The trident snake was invented by Ishikawa (2004) and Ishikawa et al. (2010), as a physical example of a non-holonomic system whose control distribution is three-dimensional and bracket-generating. In accordance with Fig. 3, the vector of generalized coordinates $$q=(x,y,\theta,\phi_1,\phi_2,\phi_3)\in R^6$$ describes the position and orientation of the robot’s body, and the angular positions of its links. The trident snake’s kinematics are represented by the driftless control system (2.1), of the form borrowed from Paszuk et al. (2012) q˙=[−1a[f1f2f3g1g2g3h1h2h3]I3]η, (4.1) where $$\eta\in R^3$$ denotes the kinematic control, and a =l−3((l+rcos⁡ϕ1)sin⁡(ϕ3−ϕ2+2π3)+(l+rcos⁡ϕ2)sin⁡(ϕ1−ϕ3+2π3)  + (l+rcos⁡ϕ3)sin⁡(ϕ2−ϕ1+2π3)),fi =l−2((l+rcos⁡ϕi+2)cos⁡(ϕi+1+αi+1+θ)−(l+rcos⁡ϕi+1)cos⁡(ϕi+2+αi+2+θ)),gi =l−2((l+rcos⁡ϕi+2)sin⁡(ϕi+1+αi+1+θ)−(l+rcos⁡ϕi+1)sin⁡(ϕi+2+αi+2+θ)),hi =−l−2sin⁡(ϕi+1−ϕi+2+αi+1−αi+2), with indexes $$i$$ to be counted modulo $$3$$ (i.e. $$\alpha_4=\alpha_1,$$$$\alpha_5=\alpha_2$$, etc). The following values of geometric parameters have been assumed: the length of each link $$l=0.12$$ m, the distance between the centre of the robot’s body and the joints $$r=0.12$$ m, and the angles $$\alpha_1=-\frac{2\pi}{3}$$, $$\alpha_2=0$$, $$\alpha_3=\frac{2\pi}{3}$$. Fig. 3. View largeDownload slide Trident snake robot: physical (built in our laboratory) and schematic. Fig. 3. View largeDownload slide Trident snake robot: physical (built in our laboratory) and schematic. The dynamics equations of the trident snake have been derived from the Lagrange–d’Alembert Principle by Pietrowska (2012). The inertia matrix $$M(q)=M^T(q)=[m_{ij}]$$, $$i,j=1,\ldots,6$$, has the following entries m11=m22=mc+3ml+3mm,m44=m55=m66=I0w+mwl2+13mll2,m14=−l(mw+12ml)sin⁡(α1+ϕ1+θ),m24=l(mw+12ml)cos⁡(α1+ϕ1+θ),m15=−l(mw+12ml)sin⁡(α2+ϕ2+θ),m25=l(mw+12ml)cos⁡(α2+ϕ2+θ),m16=−l(mw+12ml)sin⁡(α3+ϕ3+θ),m26=l(mw+12ml)cos⁡(α3+ϕ3+θ),m34=I0w+mwl(l+rcos⁡(ϕ1))+16mll(2l+3rcos⁡(ϕ1)),m35=I0w+mwl(l+rcos⁡(ϕ2))+16mll(2l+3rcos⁡(ϕ2)),m36=I0w+mwl(l+rcos⁡(ϕ3))+16mll(2l+3rcos⁡(ϕ3)),m13=−mwl(sin⁡(α1+ϕ1+θ)+sin⁡(α2+ϕ2+θ)+sin⁡(α3+ϕ3+θ))−mwr(sin⁡(α1+θ) +sin⁡(α2+θ)+sin⁡(α3+θ))−12ml(2r(sin⁡(α1+θ)+sin⁡(α2+θ)+sin⁡(α3+θ)) +l(sin⁡(α1+ϕ1+θ)+sin⁡(α2+ϕ2+θ)+sin⁡(α3+ϕ3+θ))),m23=mwl(cos⁡(α1+ϕ1+θ)+cos⁡(α2+ϕ2+θ)+cos⁡(α3+ϕ3+θ))+mwr(cos⁡(α1+θ) +cos⁡(α2+θ)+cos⁡(α3+θ))+12ml(2r(cos⁡(α1+θ)+cos⁡(α2+θ)+cos⁡(α3+θ)) +l(cos⁡(α1+ϕ1+θ)+cos⁡(α2+ϕ2+θ)+cos⁡(α3+ϕ3+θ))) +mmr(cos⁡(θ+α1)+cos⁡(θ+α2)+cos⁡(θ+α3)),m33=I0+3I0w+3mw(r2+l2)+2mwrl(cos⁡(ϕ1)+cos⁡(ϕ2)+cos⁡(ϕ3)) +ml(l2+3r2+lr(cos⁡(ϕ1)+cos⁡(ϕ2)+cos⁡(ϕ3)))+6mmr2. Parameters of the robot’s dynamics are: the body mass $$m_0=0.52$$ kg, the wheel mass $$m_w=0.03$$ kg, the wheel radius $$r_w=0.02$$ m, the wheel width $$d=0.01$$ m, the link mass $$m_l=0.07$$ kg, the motor mass $$m_m=0.055$$ kg, the total mass $$m_c=m_0+3(m_w+m_m+m_l)$$, and the moments of inertia $$I_0=\frac{m_0r^2}{4}$$, $$I_{0w}=\frac{m_w(3r_w^2+d^2)}{12}$$. Further on, for the sake of computational simplicity, we have assumed that the robot moves very slowly (actually it crawls), so the centripetal and Coriolis term in (2.3) can be skipped. Also, the gravitational term is zero, because the robots moves on a horizontal plane. The trident snake’s kinematics and dynamics define a control affine system (2.5), with system variables $$q\in R^6$$, and $$\eta,\,u,\,y\in R^3$$. The output function is chosen as the robot’s body position and orientation $$k(x)=(q_1,q_2,q_3)$$, the initial state $$x_0=(0,0,0,0,0,0,0,0,0)\in R^9$$ and $$\gamma=0.5$$. Two motion planning problems are addressed. In first problem we set $$y_d=(0.3,0,\frac{\pi}{2})$$ and the time horizon $$T=15$$, while in second one $$y_d=(0.1,0.1,\frac{\pi}{2})$$ and $$T=10$$. Figure 4 shows the solutions of these problems obtained by means of the dynamically consistent Jacobian inverse and the Jacobian pseudoinverse. One can observe that the solutions provided by both these inverses are quite similar; however the speed of convergence of the former is remarkably bigger than of the latter. The computation time depends on the complexity of the system. Here, in trident snake case, one iteration of the dynamically consistent Jacobian algorithm takes about $$1.9$$ s and $$1.6$$ s when we apply the Jacobian pseudoinverse algorithm (PC with $$3.40$$ GHz processor). Fig. 4. View largeDownload slide Motion planning of the trident snake with dynamics: convergence, position paths in $$XY$$ plane and orientation (top—problem $$1$$, bottom—problem $$2$$). Fig. 4. View largeDownload slide Motion planning of the trident snake with dynamics: convergence, position paths in $$XY$$ plane and orientation (top—problem $$1$$, bottom—problem $$2$$). 5. Conclusion Using a control system representation of the dynamic equations of motion for a non-holonomic robotic system, we have defined the Jacobian and derived a dynamically consistent Jacobian inverse. Our derivation extends the concept of dynamically consistent Jacobian inverse from the non-holonomic kinematics to the non-holonomic dynamics, under assumption that the dynamics are fully actuated. The new inverse is put into a Jacobian motion planning algorithm whose performance has been illustrated by example applications to the rolling ball and to the trident snake robot. Advantages of the dynamically consistent Jacobian motion planning in comparison to the Jacobian pseudoinverse have been discovered. Acknowledgements The authors are very much indebted to anonymous reviewers whose remarks allowed them to improve the presentation of this paper. Funding National Science Centre, Poland, under the grant decision no. (DEC-2013/09/B/ST7/02368). References Bloch A. M. (2003) Nonholonomic Mechanics and Control . New York: Springer . Google Scholar CrossRef Search ADS Demircan E. & Khatib O. (2012) Constraint-consistent analysis of muscle force contribution to human gait . Latest Advances in Robot Kinematics ( Lenarcic J. & Husty M. eds). Netherlands: Springer , pp. 301 – 308 . Google Scholar CrossRef Search ADS Hauser J. (2002) A projection operator approach to the optimization of trajectory functionals . Preprints of 15th IFAC World Congress , Barcelona, Pergamon, Elsevier, July 21st – 26th . Ishikawa M. (2004) Trident snake robot: locomotion analysis and control . IFAC Symp. on Nonlinear Control Systems . Houston, TX: Gulf Professional Publishing , pp. 1169 – 1174 . Google Scholar CrossRef Search ADS Ishikawa M. , Minati Y. & Sugie T. (2010) Development and control experiment of the trident snake robot . IEEE/ASME Trans. Mechatron. , 15 , 9 – 16 . Google Scholar CrossRef Search ADS Khatib O. (1990) Motion Force Redundancy of Manipulators . 1990 Japan-USA Symposium on Flexible Automation , Japan: ISCIE, Kyoto, pp. 337 – 342 . Khatib O. (1995) Inertial Properties in Robotics Manipulation: An Object-Level Framework . Int. J. Robotics Res., 14 , 19 – 36 . Google Scholar CrossRef Search ADS Paszuk D. Tchoń K. & Pietrowska Z. (2012) Motion planning of the trident snake robot equipped with passive or active wheels . Bull. Pol. Acad. Sci., Tech. Sci. , 60 , 547 – 554 . Pietrowska Z. (2012) Kinematics, dynamics, and control of a trident snake nonholonomic system . Master’s Thesis, Wrocław University of Technology , Poland (in Polish) . Ratajczak J. & Tchoń K. (2015) Non-holonomic motion planning using dynamically consistent Jacobian inverse . Proc. IMA Conf. on Mathematics of Robotics , St. Anne’s College, University of Oxford , Oxford , UK . Ratajczak J. & Tchoń K. (2016) Dynamically consistent Jacobian inverse for mobile manipulators . Int. J. Control , 89 , 1159 – 1168 . Google Scholar CrossRef Search ADS Sentis L. Park J. & Khatib O. (2010) Compliant Control of Multicontact and Center-of-Mass Behaviors in Humanoid Robots . IEEE Trans. Robot. , 26 , 483 – 501 . Google Scholar CrossRef Search ADS Sontag E. D. (1985) Mathematical Control Theory . New York: Springer . Sussmann H. J. (1992) New differential geometric methods in nonholonomic path finding . Systems, Models, and Feedback ( Isidori A. & Tarn T. J. eds). Boston, MA: Birkhäuser , pp. 365 – 384 . Google Scholar CrossRef Search ADS Tchoń K. & Jakubiak J. (2003) Endogenous configuration space approach to mobile manipulators: a derivation and performance assessment of Jacobian inverse kinematics algorithms . Int. J. Control 76 , 1387 – 1419 . Google Scholar CrossRef Search ADS Tchoń K. Jakubiak J. & Małek ł. (2009) Motion Planning of Nonholonomic Systems with Dynamics . Computational Kinematics ( Kecskeméthy A. & Müller A. eds). Berlin: Springer , pp. 125 – 132 . Google Scholar CrossRef Search ADS Tchoń K. & Ratajczak J. (2016) Dynamically consistent Jacobian inverse for non-holonomic robotic systems . Nonlinear Dyn. , 85 , 107 – 122 . Google Scholar CrossRef Search ADS © The authors 2016. Published by Oxford University Press on behalf of the Institute of Mathematics and its Applications. All rights reserved. This article is published and distributed under the terms of the Oxford University Press, Standard Journals Publication Model (https://academic.oup.com/journals/pages/open_access/funder_policies/chorus/standard_publication_model)

### Journal

IMA Journal of Mathematical Control and InformationOxford University Press

Published: Nov 15, 2016

## You’re reading a free preview. Subscribe to read the entire article.

### DeepDyve is your personal research library

It’s your single place to instantly
that matters to you.

over 18 million articles from more than
15,000 peer-reviewed journals.

All for just $49/month ### Explore the DeepDyve Library ### Search Query the DeepDyve database, plus search all of PubMed and Google Scholar seamlessly ### Organize Save any article or search result from DeepDyve, PubMed, and Google Scholar... all in one place. ### Access Get unlimited, online access to over 18 million full-text articles from more than 15,000 scientific journals. ### Your journals are on DeepDyve Read from thousands of the leading scholarly journals from SpringerNature, Elsevier, Wiley-Blackwell, Oxford University Press and more. All the latest content is available, no embargo periods. DeepDyve ### Freelancer DeepDyve ### Pro Price FREE$49/month
\$360/year

Save searches from
PubMed

Create lists to

Export lists, citations