# Position and posture control for a class of second-order nonholonomic underactuated mechanical system

Position and posture control for a class of second-order nonholonomic underactuated mechanical... Abstract This paper presents a position and posture control strategy for a $$n$$-link planar underactuated manipulator with passive second joint in horizontal plane. The $$n$$-link planar underactuated mechanical manipulator is a second-order nonholonomic system, the control objective is to move the end-effector to a given position with a desired posture. The whole control process is divided into $$n-$$2 stages. In each stage, the first link is maintained at its initial states unchanged, there exists an angle constraint between the passive link and one of the active links. Based on the angle constraints, the target angles of the control objective are calculated by using genetic algorithm. The controllers of each stages are designed, respectively, to achieve the control objective of one of the active links. Finally, taking a 5-link planar underactuated mechanical manipulator, for example, the simulation results demonstrate the validity of the proposed control method. 1. Introduction Nonholonomic mechanical systems (see Hussein & Bloch, 2008; Chen et al., 2013) usually have nonintegrable velocity or acceleration constraints. Underactuated mechanical systems (see Xin et al., 2009; Shiriaev et al., 2014) have fewer controls than degrees of freedoms. In general, underactuated mechanical systems are the classical nonholonomic systems. Underactuated manipulators are important subclass of underactuated mechanical systems. There are two categories underactuated manipulators: vertical plane ones and horizontal plane ones (planar underactuated manipulators). For the former, the linear approximation model of balance point is controllable. To reduce the complexity of control, the motion space is usually divided into two areas: a swing-up area and a balance area, the swing-up and balancing control approaches for the systems are proposed in Eom & Chwa (2015) and Lai et al. (2015a). For the latter, there is no gravity torque, any position of the system is the equilibrium point and its linear approximation model of balance point is not controllable. Thus, the control approaches of the former are not suitable for the latter. Since the different positions of the passive joint located, the planar underactuated manipulators have various nonlinear characteristics. Thus, different control approaches are needed for the planar underactuated manipulators when the passive joint is located in different position. A planar acrobot (two link manipulator with passive first joint) is a special underactuated manipulator, which is holonomic system, i.e. there exist angle constraint (see Oriolo & Nakamura, 1991). Based on holonomic characteristics, Lai et al. (2015b) propose the motion control method to realize position control of the planar acrobot. The planar $$n$$-link underactuated manipulator with passive first joint is a first-order nonholonomic system, i.e. there only exists angular velocity constraint. By using the angle constraint of the planar acrobot, the multi-stage stable control strategy is proposed to realize the control objective of the system, for example, the planar three-link underactuated manipulator system (see Lai et al., 2016). The planar underactuated manipulator with first joint being not passive is second-order nonholonomic system. For a planar pendubot (two link manipulator with passive second joint), nilpotent approximation is used to transform the dynamic equations, and the control method is devised based the iterative steering paradigm (see De Luca et al., 2000). Through mathematical conversion, the dynamic equations of the planar pendubot are in Byrnes–Isidori normal form, and an approach of sliding mode control is proposed (see Knoll & Röbenack, 2011). Unfortunately, both approaches only control the system to swing around the target location, but cannot stabilize the manipulator at the target position. In general, for the planar $$n$$-link ($$n>2$$) underactuated manipulator with first joint being active, there is still not effective stability control approach. Now, the control of the planar underactuated manipulator is mainly focus on the position control. However, the posture control is another important aspect for the mechanical systems in practical applications. Feedback compensation (see Sharbafi et al., 2013), feedback linearization and model reference adaptive control (see Hirose et al., 2015) are proposed to realize some special posture control problems. But these approaches are mainly for the vertical plane underactuated manipulator or the fully actuated systems which cannot be applied to the planar underactuated manipulator. Hence, it is a challenge to move the end-effector of the planar underactuated manipulator to a given position with a desired posture. This paper presents a position and posture strategy for the $$n$$-link planar underactuated manipulator with a passive second joint. First, a dynamic model of the system is built. Then, the whole control process is divided into $$n-$$2 stages. In each stage, the first link is controlled to maintain at its initial states unchanged, one of the active links is controlled to the target value, and the other active links are maintained to be the initial states or the target values of the previous stage. Therefore, the $$n$$-link planar manipulator is like a planar acrobot in each stage, and there exists an angle constraint between the passive link and the active link. Based on those angle constraints, the target angles of the control objective are calculated by using genetic algorithm. The controllers of each stage are designed, respectively, to realize the control target of each stage. Finally, the simulation results demonstrate the validity of the proposed control method. 2. Dynamic model Figure 1 shows the $$n$$-link planar underactuated manipulator with passive second joint. The mass, inertia, length of the link $$i$$ are denoted by $${m_i},{J_i},{L_i}$$, respectively, and $${l_i}$$ is the distance between the joint $$i$$ and the centre of mass of the link $$i$$. $$(X,Y)$$ is the coordinate of the end-effector, $$Z$$ is the posture angle. Fig. 1. View largeDownload slide The $$n$$-link planar underactuated manipulator. Fig. 1. View largeDownload slide The $$n$$-link planar underactuated manipulator. The Lagrange function of the system is L(q,q˙)=K(q,q˙)=12qTM(q)q, (2.1) where $$q = {\left[ {{q_1},\ldots,{q_n}} \right]^T}$$, $$\dot{q} = {\left[ {{\dot{q}_1},\ldots,{\dot{q}_n}} \right]^T}$$ are the vectors of generalized angles and angular velocities, $$K(q,\dot q)$$ is the kinetic energy of the system. $$M(q)=(M_{ij}(q_{s},\ldots,q_{n}))_{n\times n}$$ is the positive definite symmetric inertia matrix, $$s={\rm min}\{i,j\}+1$$ for $$i,j<n$$ and $$M_{nn}$$ is constant. The Euler–Lagrange equation of the system is ddt[∂L(q,q˙)∂q˙]−∂L(q,q˙)∂q=τ, (2.2) according to (2.1), (2.2) can be rewritten as M(q)q¨+H(q,q˙)=τ, (2.3) i.e. ∑j=1nMijq¨j+Hi=τi,i≠2, (2.3a) ∑j=1nM2jq¨j+H2=0, (2.3b) where $$\ddot{q}=[\ddot{q}_{1},\ldots,\ddot{q}_{n}]^T$$ is the vectors of generalized angular accelerations, $$H(q,\dot q)= [H_1,\ldots,H_n]^T$$ is the centripetal and coriolis term, $$\tau={\left[\tau _1,0,\tau _3,\ldots,\tau _n \right]^T}$$ is the vectors of applied torques. Lemma 2.1 The constraint equation of the passive joint (2.3b) is partially integrable if and only if (see Oriolo & Nakamura, 1991) (1) the gravitational torque in the constraint equation (2.3b) is constant, (2) the passive joint variable does not appear in the inertia matrix $$M(q)$$. Obviously, the gravitational torque in the constraint equation (2.3b) is zero, but the passive joint variable $$q_{2}$$ appears in the inertia matrix $$M(q)$$, the constraint equation (2.3b) does not meet the condition (2) of Lemma 2.1, thus it is not integrable. Therefore, the $$n$$-link planar underactuated manipulator is a second-order nonholonomic system. 3. Target angles In this section, the angle constraints between the passive link and the active $$k$$th link $$(k=3,\ldots,n)$$ are obtained, and then the target angles of the control objective are calculated by using genetic algorithm (see Köker, 2013). 3.1. Angle constraint Based on the holonomic characteristics of a planar acrobot, the whole control process of the system is divided into $$n-$$2 stages, the planar $$n$$-link manipulator is like a planar acrobot in each stage. In $$p$$th stage ($$1\leq p\leq n-2$$), the active $$k$$th link $$(k=p+2)$$ is controlled to the target angle, and the other active links are maintained to be the initial states or the target values of the previous stage. Let $$\dot{q}_{i}= 0,\ddot{q}_{i}\equiv 0, i\neq 2,k$$ and $$q^{( {p})}_{i0},i=1,\ldots,n$$ denote the initial angle of the $$i$$th link in the $$p$$th stage. From (2.3b), we have M¯22q¨2+M¯2kq¨k+H¯2=0, (3.1) where $$\bar{M}_{22}=M_{22}|_{q_{i}=q^{({p})}_{i0}},\bar{M}_{2k}=M_{2k}|_{q_{i}=q^{({p})}_{i0}},\bar{H}_{2}=H_{2}|_{{q_{i}=q^{({p})}_{i0}},\dot{q}_{i}=0},i=1,\ldots,n,i\neq 2,k$$. According to the complete integrability characteristics of the planar acrobot (see Lai et al., 2015b), there is M¯22q˙2+M¯2kq˙k=0. (3.2) Since $$\bar{M}_{22}>0$$ and there is no variable $$q_{2}$$ appears in $$\bar{M}_{22}>0$$, the equation (3.2) formed with separated variables $$q_{2}$$ and $$q_{k}$$. By using the separation of variables, the following equation is obtained through integration. q2=q20(p)−∫qk0(p)qkM¯2kM¯22dqk, (3.3) the equation (3.3) represents the angle constraint between the active $$k$$th link and the passive link. The equation (3.2) implies that if the $$k$$th link is stabilized at $${{q}}_{kd}$$, the passive link 2 is controlled to a certain angle $$q^{({p})}_{2d}$$. Considering the above angle constraints, the target angles of the control objective are calculated by using genetic algorithm in the following subsection. 3.2. Target angles calculation Based on the inverse kinematics of the system (see Wei et al., 2014), the coordinates of end-effector $$(X,Y)$$ and the posture angle $$Z$$ are the functions of the link angles $$q_1,\ldots,q_n$$ (see Fig. 1). The functions are as follows {X=−∑i=1nLisin⁡(∑j=1iqj)Y=∑i=1nLicos⁡(∑j=1iqj)Z=rem((∑j=1nqj),2π),  (3.4) where the function $${\rm rem} (x, y)$$ is the remainder of $$x$$ divided by $$y$$ ($$y$$ is not equal to zero). Due to the complexity of the angle constraints (3.3), it is difficult to solve this inverse kinematics problem. The genetic algorithm is employed to obtain the target angles of the control objective. The searching range of $$q_{3}, \ldots,q_{n}$$ is $$[-10,10]$$ rad, each chromosome consists $$(n-2)\times18$$ bytes of the binary vector. Considering the relative errors of the horizontal and vertical co-ordinates of the position as well as the posture, the fitness function is defined as follows: f=1/(|X−Xd|/|X|+|Y−Yd|/|Y|+|Z−Zd|/|Z|), (3.5) where $${X_d},{Y_d}$$ indicate the target position of the end-effector, and $${Z_d}$$ denotes the target posture. Given a random initial population, the genetic algorithm operates in cycles, as follows: Step 1. Each member of the population is evaluated by the fitness function. The population undergoes reproduction in a number of iterations. One or more parents are chosen stochastically, but strings with higher fitness values have higher probability of contributing to the offsprings. Step 2. The offsprings are produced by applying the genetic operators (crossover and mutation, and so on). Step 3. The offsprings are inserted into the population and the process is repeated. Under the restrictions of (3.3), The feasible solutions $${q_{3d}},\ldots,{q_{nd}}$$ for (3.5) are obtained by using the genetic algorithm, and $${q^{({p})}_{2d}}$$ can be worked out by using (3.3). Since the diversity and the periodicity of the links angle, there are multiple solutions for the control objective. But, when one set of solutions is searched by employing the genetic algorithm, the genetic algorithm operate is automatically stopped. 4. Controller design Let $$x = {\left[ {{x_1},\ldots,{x_n},{x_{n+1}},\ldots{\rm{,}}{x_{2n}}} \right]^T} = {\left[ {q,\dot q} \right]^T}$$, the state space equation of the planar underactuated manipulator can be written to be {x˙i=xi+nx˙i+n=Fi(x)+∑j=1ngijτj i=1,…,n, (4.1) i.e. x˙=f(x)+g(x)τ, (4.2) where $$f(x) =\left[ {{x_{n+1}},\ldots,{x_{2n}},{F}} \right]^T, g(x)=\left[ {g_1}(x),{g_2}(x) \right]^T$$, with $$F^{T}=\left[{{F_1},\ldots,{F_{n}}}\right]^{T} =-{M^{ - 1}}(q)H(q,\dot q)$$, $${M^{ - 1}}(q)={g_2}(x) = {({g_{ij}}{\rm{)}}_{n \times n}}$$, $${g_1}(x) \in {O^{n \times n}}$$, $${g_{ij}}$$ is nonlinear function of $$x_{i},i=1,\ldots,2n$$. In the $$p$$th stage, we control the active $$k$$th link to the desired angle while the other active links are maintained to be the target values of the previous stage. The Lyapunov function in the stage is constructed to be Vk(x)=12(xk+n2+(xk−qkd)2)+12∑i≠2,k(xi+n2+(xi−qi0(p))2). (4.3) Taking the time derivative of $$V_{k}(x)$$, we have V˙k(x)=xk+n(xk−qkd+Fk+∑j=1ngkjτj)+∑i≠2,kxi+n(xi−qi0(p)+Fi+∑j=1ngijτj). (4.4) According to (4.4), the control laws are designed to be {τi=(−xi+qi0(p)−Fi−γixi+n−∑j=1,j≠ingijτj)gii−1,i≠2,kτk=(−xk+qkd−Fk−γkxk+n−∑j=1,j≠kngkjτj)gkk,−1  (4.5) where $${\gamma _1},{\gamma _3},\ldots,{\gamma _n}>0$$ are constant parameters, $$g_{ii}>0$$ is the element of main diagonal of the positive matrix $$M(q)^{-1}$$. Hence, there is no singular point. Substituting (4.5) into (4.4), yields V˙k(x)=−γ1xn+12−γ3xn+32−⋯−γnx2n2≤0. (4.6) For $${\dot V_{k}}(x)$$ is negative semidefinite, LaSalle invariable principle (see LaSalle, 2012) is used to prove that the control objective can be realized. Theorem 4.1 Considering the $$n$$-link underactuated manipulator (4.1). In $$p$$th stage, under the control with the controller (4.5), the desired angle of the $$k$$th link ($$k=p+2$$) is achieved, the angular velocity asymptotically reaches zero, i.e. limt→∞{x1,…,xn,xn+1,…,x2n}={q10(p),q2d(p),q30(p),…,q(k−1)0(p),qkd,q(k+1)0(p),…,qn(p),0,…,0} (4.7) When $${p}$$ traverses through the set $$\{3,4,\ldots,n\}$$, the position and posture control objective of the planar $$n$$-link underactuated manipulator is realized. Proof. First, we prove that the system controlled by the controller (4.5) is globally asymptotically stable, or, rather,(4.7) holds. Let $${{\it\Omega}} = \left\{ {x \in {R^n}\left| {V_{k}(x) \le c,c > 0} \right.} \right\}$$, From (4.5), we know that $$V_{k}(x) \to \infty$$, as $$\left\| {{x_i}} \right\|\to \infty, i\neq 2,n+2$$. (3.2) and (3.3) imply $$\left\| {{x_k}} \right\| \to \infty$$ , as $$\left\| {{x_2}} \right\| \to \infty$$ and $$\left\| {{x_{k+n}}} \right\| \to \infty$$,as $$\left\| {{x_{k+2}}} \right\| \to \infty$$. So, $$V_{k}(x)\rightarrow\infty$$, as $$\left\| {{x_i}} \right\| \to \infty,i=1,\cdots,n$$, that is $$V_{k}(x)$$ is radial unbounded. Therefore, $${{\it\Omega}}$$ is a positive invariant compact set. Defined the invariant set $$E_{k}$$ and maximum invariant set $$M_{k}$$ of the system. From (4.6), we have $$\dot V(x)_{k} \le 0,x \in {\it\Omega}$$. If $$\dot V_{k}(x) = 0$$, we obtain $${x_{n+i}}= 0,i=1,\ldots,n,i\neq 2$$, substituting it into (3.2), yield $${x_{n+2}} = 0$$. Let the set $$E_{k} = \{ {x \in {\it\Omega} \left| \dot V_{k}(x) = 0, \right.} \} = \{ {x \in {\it\Omega} \left|{x_{n+i}}= 0,i=1,\ldots,n, \right.} \}$$. Since every points is the equilibrium of the system, $$E_{k}$$ is the invariant set of the system. If $${x_{i+n}} = 0,i = 1,\ldots,n$$, $${x_{i}}$$ are constants and $${H_i} = 0$$ holds. From the state space model (4.2), we have $$F^T = - {M^{ - 1}}(q)H(q,\dot q)$$ and $${[{\dot x_{n+1}},\ldots,{\dot x_{2n}}]^T} = F^T + {g_2}(x)\tau$$, it easy to see $$F^T =0$$ and $${g_2}(x)\tau = 0$$. Owing to $${g_2}(x)\tau = - {M^{ - 1}}(q)$$ and $$M(q)$$ is positive, $${\left[ {{\tau _1}{\kern 1pt} {\kern 1pt} {\kern 1pt} ,0,{\tau _3},\ldots,{\tau _n}} \right]^T} = 0$$ holds. Substituting it into (4.5), we have $$x_1=q^{({p})}_{10},x_3=q^{({p})}_{30},\ldots,x_{k-1}={q^{({p})}_{(k-1)0}},x_k={q_{kd}},x_{k+1}={q^{({p})}_{(k+1)0}},\ldots,x_n={q^{({p})}_{n}}$$. Using the angle constraint (3.3), we can get $${x_2} = q^{({p})}_{2d}$$ and $$E_{k} = \{q^{({p})}_{10},q^{({p})}_{2d},q^{({p})}_{30},\ldots,{q^{({p})}_{(k-1)0}},{q_{kd}},{q^{({p})}_{(k+1)0}},\ldots,{q^{({p})}_{n}}\}$$. Obviously, there is only one point in the invariant set, the maximum invariant set $$M_{k}=E_{k}$$. According to LaSalle’s theorem, every trajectories starting at $${\it\Omega}$$ approaches $$E_{k}$$, as $$t\rightarrow\infty$$, i.e. (4.7) holds. Then, we prove that all the target angles of links can be realized in the whole control process. The Switch conditions are $$\left| {{x_k} - {q_{kd}}} \right|\le {e_{k1}},\left| {{x_{n+k}}} \right|\le {e_{k2}}$$, $${e_{k1}},{e_{k2}}$$ (the small positive constants) are the switching threshold. After moving the $$k$$th link to the desired angle, the angles of links $$q^{({p})}_{10}, q^{(k)}_{2d}, q^{({p})}_{30}, \ldots$$, $${q^{({p})}_{(k-1)0}}, {q_{kd}},{q^{({p})}_{(k+1)0}},\ldots,{q^{({p})}_{n}}$$ are the initial value of controlling the other link. As $$p$$$$(1\leq p\leq n-2)$$ taking any other integer value, according to the same methods above, the system can be stabilized at the target angle of that stage. If $$p$$ traverses the set $$\{1,\ldots,n-2\}$$ successfully, all the target angles of links are realized. □ From what has mentioned above, after all the $$n-2$$ stages of control, the system can be stabilized at the target position with the desired posture finally. 5. Simulation results This section presents simulation results of a 5-link underactuated manipulator with passive second joint. The SIMULINK simulation tool of MATLAB software is applied. The parameters of the system are shown in Appendix A and the following Table 1. The initial position and posture are $$(X,Y)=(0,3.3)$$ m and $$Z=0$$ rad, the control objective(position and posture) are $$(X,Y)=(0.64,1.70)$$ m and $$Z=\pi/3$$ rad. Table 1. Parameters of a 5-link planar underactuated manipulator Linki $${m_i}$$(kg) $${L_i}$$(m) $${l_i}$$(m) $${J_i}$$(kg $${{\rm{m}}^2}$$) i = 1 0.5 0.5 0.25 0.0104 i = 2 0.6 0.6 0.3 0.0180 i = 3 0.6 0.6 0.3 0.0180 i = 4 0.8 0.8 0.4 0.0427 i = 5 0.8 0.8 0.4 0.0427 Linki $${m_i}$$(kg) $${L_i}$$(m) $${l_i}$$(m) $${J_i}$$(kg $${{\rm{m}}^2}$$) i = 1 0.5 0.5 0.25 0.0104 i = 2 0.6 0.6 0.3 0.0180 i = 3 0.6 0.6 0.3 0.0180 i = 4 0.8 0.8 0.4 0.0427 i = 5 0.8 0.8 0.4 0.0427 Table 1. Parameters of a 5-link planar underactuated manipulator Linki $${m_i}$$(kg) $${L_i}$$(m) $${l_i}$$(m) $${J_i}$$(kg $${{\rm{m}}^2}$$) i = 1 0.5 0.5 0.25 0.0104 i = 2 0.6 0.6 0.3 0.0180 i = 3 0.6 0.6 0.3 0.0180 i = 4 0.8 0.8 0.4 0.0427 i = 5 0.8 0.8 0.4 0.0427 Linki $${m_i}$$(kg) $${L_i}$$(m) $${l_i}$$(m) $${J_i}$$(kg $${{\rm{m}}^2}$$) i = 1 0.5 0.5 0.25 0.0104 i = 2 0.6 0.6 0.3 0.0180 i = 3 0.6 0.6 0.3 0.0180 i = 4 0.8 0.8 0.4 0.0427 i = 5 0.8 0.8 0.4 0.0427 In the genetic algorithm, the simulation step length is 0.001 s, the crossover possibility is 0.8 and the mutation possibility is 0.002. According to the control objective, the target angles calculated by using the genetic algorithm. The target angles of the active links are generated $${q_{3d}} = -6.993$$ rad, $${q_{4d}} = 7.138$$ rad, $${q_{5d}} = 4.899$$ rad, as Fig. 2(a–c) show. Based on the angle constraints, $${q_{2d}} =-8.156$$ rad is obtained. Figure 2(d) implies that the fitness function reaches the maximum value $$(f=32.238)$$. At this time, the reciprocal of the fitness function $$1/f=0.031$$ is the minimum. Therefore, the relative errors of the horizontal and vertical coordinates of the position as well as the posture are small, which shows the accuracy for those target angles is high. Fig. 2. View largeDownload slide Target angles calculation. Fig. 2. View largeDownload slide Target angles calculation. The simulation results Fig. 3(a) shows that the angles converge to the target values smoothly and the control strategy switch at 7.703 and 15.421 s, the system realize stabilization in 22 s. The angles of the links finally reach steady state at $${q_{1d}} = 0$$ rad, $${q_{2d}} =-8.156$$ rad, $${q_{3d}} = -6.993$$ rad, $${q_{4d}} = 7.138$$ rad, $${q_{5d}} = 4.899$$ rad. Figure 3(b) implies that the control torques vary within 4 Nm, which are relatively small. The co-ordinates of the end effector stabilized at $$(0.630, 1.711)$$ m with the posture angle $$1.057$$ rad, as the Fig. 3(c and d) show. For different position or posture, the proposed control strategy can realize the control objective. Fig. 3. View largeDownload slide Simulation curves of the motion states. Fig. 3. View largeDownload slide Simulation curves of the motion states. 6. Conclusion We have presented a multi-stage control strategy to realize the position and posture control of $$n$$-link planar underactuated manipulator with passive second joint. The control strategy consists of $$n-$$2 stages. In each stage, the $$n$$-link planar underactuated manipulator is like a planar acrobot. Based on the angle constraint relation of each stage of the system, genetic algorithm is employed to obtain the target angles of links according to the control objective. Through the control of the $$n-$$2 stages, the active links and the passive link reach their target angles, which ensures that the control objective of the system is realized. Simulation results demonstrate the validity of the proposed control method. The control strategy in the paper can be extended to the $$n$$-link planar underactuated manipulators with one passive joint, but the reachable area is different because of the different position of the passive joint. In the future, we will find a control strategy to control all active links before the passive joint to their arbitrary target angles, so we can stabilize the second-order nonholonomic underactuated mechanical system at arbitrary point of the geometric reachable region. Funding National Natural Science Foundation of China (grant 61374106); and the Hubei Provincial Natural Science Foundation of China (grant 2015CFA010). Appendix A The inertia matrix of a 5-link planar underactuated manipulator, $$M(q) = (M_{ij}(q_{s},\ldots,q_{5}))_{5\times5}, s={\rm min}\{i,j\}+1$$, is the positive definite symmetric inertia matrix, the parameters are as following {α1=J1+m1l12+(m2+m3+m4+m5)L12,α2=J2+m2l22+(m3+m4+m5)L22,                                      α3=(m2l2+(m3+m4+m5)L2)L1,α4=J3+m3l32+(m4+m5)L32,α5=m3L1l3+(m4+m5)L1L3,a6=m3L2l3+m4L2L3+m5L2L3,α7=m4L1l4+m5L1L4,a8=m4L2l4+m5L2L4,α9=m4L3l4+m5L3L4,α10=J4+m4l42+m5L42,α11=m5L1l5,α12=m5L3l5,α13=m5L4l5,α14=m5L2l5,α15=J5+m5l52  {M55=α15,M45=α13cos⁡q5+α15,M44=α10+α13cos⁡q5+M45,M35=α12cos⁡q45+α13cos⁡q5+α15,M34=α10+α9cos⁡q4+α13cos⁡q5+M35,M33=α4+α9cos⁡q4+α12cos⁡q45+M34,M25=α14cos⁡q35+α12cos⁡q45+α13cos⁡q5+α15,      M24=α10+α13cos⁡q5+α8cos⁡q34+α9cos⁡q4+M25,M23=α4+α6cos⁡q3+α9cos⁡q4+α12cos⁡q45+M24,M22=α2+α6cos⁡q3+α8cos⁡q34+α14cos⁡q35+M23,M15=α11cos⁡q25+α12cos⁡q45+α13cos⁡q5+α14cos⁡q35+α15,M14=α10+α7cos⁡q24+α8cos⁡q34+α9cos⁡q4+α13cos⁡q5+M15,M13=α4+α5cos⁡q23+α6cos⁡q3+α9cos⁡q4+α12cos⁡q45+M14,M12=α2+α3cos⁡q2+α6cos⁡q3+α8cos⁡q34+α9cos⁡q4+α12cos⁡q45+M13,M11=α1+α3cos⁡q2+α5cos⁡q23+α7cos⁡q24+α11cos⁡q25+M12.  {H1=−α3(2q˙1+q˙2)q˙2sin⁡q˙2−α5(2q˙1+q˙23)q˙23sin⁡q˙23−α6(2q˙12+q˙3)q˙3sin⁡q˙3−α7(2q˙1+q˙24)q˙24sin⁡q˙24−α8(2q˙12+q˙34)q˙34sin⁡q˙34−α9(2q˙13+q˙4)q˙4sin⁡q˙4−α11(2q˙1+q˙25)q˙25sin⁡q˙25−α12(2q˙13+q˙45)q˙45sin⁡q˙45−α13(2q˙14+q˙5)q˙5sin⁡q˙5−α14(2q˙12+q˙35)q˙35sin⁡q˙35H2=α3q˙22sin⁡q˙2+α5q˙12sin⁡q˙23−α6(2q˙12+q˙3)q˙3sin⁡q˙3+α7q˙12sin⁡q˙23−α8(2q˙12+q˙34)q˙34sin⁡q˙34  −α11q˙12sin⁡q˙25−α9(2q˙13+q˙4)q˙4sin⁡q˙4−α12(2q˙23+q˙45)q˙45sin⁡q˙45−α13(2q˙14+q˙5)q˙5sin⁡q˙5−α14(2q˙12+q˙35)q˙35sin⁡q˙35,H3=α5q˙12sin⁡q˙23−α6q˙122sin⁡q˙3+α7q˙12sin⁡q˙24+α8q˙122sin⁡q˙34−α9q˙14q˙4sin⁡q˙4+α11q˙12sin⁡q˙25+α14q˙122sin⁡q˙35−α12(2q˙13+q˙45)q˙45sin⁡q˙45−α13(2q˙14+q˙5)q˙5sin⁡q˙5,H4=α8q˙122sin⁡q˙34+α9q˙132sin⁡q˙4+α11q˙12sin⁡q˙24+α12q˙132sin⁡q˙45−α13(2q˙14+q˙5)q˙5sin⁡q˙5+α14q˙122sin⁡q˙35,H5=α11q˙12sin⁡q˙25+α12q˙132sin⁡q˙45+α13(2q˙14+q˙5)q˙14sin⁡q˙5+α14q˙122sin⁡q˙35,  where $${q_{ij}} = {q_i} + {q_{i + 1}} + \cdot \cdot \cdot + {q_j},i<>< j$$. References Aguilar-Ibáñez C. & Sira-Ramírez H. (2007) A linear differential flatness approach to controlling the Furuta pendulum . IMA J. Math. Control Inform., 24 , 31 – 45 . Google Scholar CrossRef Search ADS Chen H. Zhang J. & Chen B. (2013) Global practical stabilization for non-holonomic mobile robots with uncalibrated visual parameters by using a switching controller . IMA J. Math. Control Inform., 30 , 543 – 557 . Google Scholar CrossRef Search ADS De Luca A. Mattone R. & Oriolo G. (2000) Stabilization of an underactuated planar 2R manipulator . Int. J. Robust Nonlin. Control, 10 , 181 – 198 . Google Scholar CrossRef Search ADS Eom M. & Chwa D. (2015) Robust swing-up and balancing control using a nonlinear disturbance observer for the pendubot system with dynamic friction . IEEE Trans. Robot., 31 , 331 – 343 . Google Scholar CrossRef Search ADS Hirose N. Tajima R. Tajima R. Sukigara K. Kazutoshi S & Yuji T. (2015) Posture control for personal mobility robot using feedback compensation with unstable pole . Electr. Eng. Jpn., 193 , 33 – 42 . Google Scholar CrossRef Search ADS Hussein I. & Bloch A.M. (2008) Optimal control of underactuated nonholonomic mechanical systems, . IEEE Trans. Automat. Contr., 53 , 668 – 682 . Google Scholar CrossRef Search ADS Knoll C. & Röbenack K. (2011) Control of an underactuated manipulator using similarities to the double integrator . IFAC Proceedings Volumes , 44 , 11501 – 11507 . Google Scholar CrossRef Search ADS Köker R. (2013) A genetic algorithm approach to a neural-network-based inverse kinematics solution of robotic manipulators based on error minimization . Inf. Sci., 222 , 528 – 543 . Google Scholar CrossRef Search ADS Lai X. Z. She J. H. Cao W. H & Yang S. X. (2015b) Stabilization of underactuated planar acrobot based on motion-state constraints . Int. J. Nonlin. Mech., 77 , 342 – 347 . Google Scholar CrossRef Search ADS Lai X. Z Sheng Y. Wu M. (2016) Stable control strategy for planar three-link underactuated mechanical system . IEEE/ASME Trans. Mechatron. , 21 , 1345 – 1356 . Google Scholar CrossRef Search ADS Lai X. Z. Zhang A. C. Wu M. & She J. H. (2015a) Singularity-avoiding swing-up control for underactuated three-link gymnast robot using virtual coupling between control torques, . Int. J. Robust Nonlin. Control, 25 , 207 – 221 . Google Scholar CrossRef Search ADS La Salle J. P. (2012) The Stability and Control of Discrete Processes , vol. 62. New York: Springer . Oriolo G. & Nakamura Y. (1991) Control of mechanical systems with second-order nonholonomic constraints: underactuated manipulators . Proc. 30th IEEE Decisi. Control, 14 , 1072 – 1088 . Sharbafi M. A. Maufroy C. Ahmadabadi M. N. Yazdanpanah M. J. & Seyfarth A. (2013) A robust hopping based on virtual pendulum posture control . Bioinspir. & Biomim., 8 , 036002 . Shiriaev A. S. Freidovich L. B. & Spong M. W. (2014) Controlled invariants and trajectory planning for underactuated mechanical systems . IEEE Trans. Automat. Control, 59 , 2555 – 2561 . Google Scholar CrossRef Search ADS Wei Y. Jian S. He S. & Wang Z. (2014) General approach for inverse kinematics of nR robots . Mech. Mach. Theory, 75 , 97 – 106 . Google Scholar CrossRef Search ADS Xin X. She J. H. She, Yamasaki T. & Liu Y. (2009) Swing-up control based on virtual composite links for n-link underactuated robot with passive first joint . Automatica , 45 , 1986 – 1994 . 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

# Position and posture control for a class of second-order nonholonomic underactuated mechanical system

, Volume Advance Article (2) – Dec 17, 2016
11 pages

/lp/ou_press/position-and-posture-control-for-a-class-of-second-order-nonholonomic-OHHS0GCin0
Publisher
Oxford University Press
ISSN
0265-0754
eISSN
1471-6887
D.O.I.
10.1093/imamci/dnw056
Publisher site
See Article on Publisher Site

### Abstract

Abstract This paper presents a position and posture control strategy for a $$n$$-link planar underactuated manipulator with passive second joint in horizontal plane. The $$n$$-link planar underactuated mechanical manipulator is a second-order nonholonomic system, the control objective is to move the end-effector to a given position with a desired posture. The whole control process is divided into $$n-$$2 stages. In each stage, the first link is maintained at its initial states unchanged, there exists an angle constraint between the passive link and one of the active links. Based on the angle constraints, the target angles of the control objective are calculated by using genetic algorithm. The controllers of each stages are designed, respectively, to achieve the control objective of one of the active links. Finally, taking a 5-link planar underactuated mechanical manipulator, for example, the simulation results demonstrate the validity of the proposed control method. 1. Introduction Nonholonomic mechanical systems (see Hussein & Bloch, 2008; Chen et al., 2013) usually have nonintegrable velocity or acceleration constraints. Underactuated mechanical systems (see Xin et al., 2009; Shiriaev et al., 2014) have fewer controls than degrees of freedoms. In general, underactuated mechanical systems are the classical nonholonomic systems. Underactuated manipulators are important subclass of underactuated mechanical systems. There are two categories underactuated manipulators: vertical plane ones and horizontal plane ones (planar underactuated manipulators). For the former, the linear approximation model of balance point is controllable. To reduce the complexity of control, the motion space is usually divided into two areas: a swing-up area and a balance area, the swing-up and balancing control approaches for the systems are proposed in Eom & Chwa (2015) and Lai et al. (2015a). For the latter, there is no gravity torque, any position of the system is the equilibrium point and its linear approximation model of balance point is not controllable. Thus, the control approaches of the former are not suitable for the latter. Since the different positions of the passive joint located, the planar underactuated manipulators have various nonlinear characteristics. Thus, different control approaches are needed for the planar underactuated manipulators when the passive joint is located in different position. A planar acrobot (two link manipulator with passive first joint) is a special underactuated manipulator, which is holonomic system, i.e. there exist angle constraint (see Oriolo & Nakamura, 1991). Based on holonomic characteristics, Lai et al. (2015b) propose the motion control method to realize position control of the planar acrobot. The planar $$n$$-link underactuated manipulator with passive first joint is a first-order nonholonomic system, i.e. there only exists angular velocity constraint. By using the angle constraint of the planar acrobot, the multi-stage stable control strategy is proposed to realize the control objective of the system, for example, the planar three-link underactuated manipulator system (see Lai et al., 2016). The planar underactuated manipulator with first joint being not passive is second-order nonholonomic system. For a planar pendubot (two link manipulator with passive second joint), nilpotent approximation is used to transform the dynamic equations, and the control method is devised based the iterative steering paradigm (see De Luca et al., 2000). Through mathematical conversion, the dynamic equations of the planar pendubot are in Byrnes–Isidori normal form, and an approach of sliding mode control is proposed (see Knoll & Röbenack, 2011). Unfortunately, both approaches only control the system to swing around the target location, but cannot stabilize the manipulator at the target position. In general, for the planar $$n$$-link ($$n>2$$) underactuated manipulator with first joint being active, there is still not effective stability control approach. Now, the control of the planar underactuated manipulator is mainly focus on the position control. However, the posture control is another important aspect for the mechanical systems in practical applications. Feedback compensation (see Sharbafi et al., 2013), feedback linearization and model reference adaptive control (see Hirose et al., 2015) are proposed to realize some special posture control problems. But these approaches are mainly for the vertical plane underactuated manipulator or the fully actuated systems which cannot be applied to the planar underactuated manipulator. Hence, it is a challenge to move the end-effector of the planar underactuated manipulator to a given position with a desired posture. This paper presents a position and posture strategy for the $$n$$-link planar underactuated manipulator with a passive second joint. First, a dynamic model of the system is built. Then, the whole control process is divided into $$n-$$2 stages. In each stage, the first link is controlled to maintain at its initial states unchanged, one of the active links is controlled to the target value, and the other active links are maintained to be the initial states or the target values of the previous stage. Therefore, the $$n$$-link planar manipulator is like a planar acrobot in each stage, and there exists an angle constraint between the passive link and the active link. Based on those angle constraints, the target angles of the control objective are calculated by using genetic algorithm. The controllers of each stage are designed, respectively, to realize the control target of each stage. Finally, the simulation results demonstrate the validity of the proposed control method. 2. Dynamic model Figure 1 shows the $$n$$-link planar underactuated manipulator with passive second joint. The mass, inertia, length of the link $$i$$ are denoted by $${m_i},{J_i},{L_i}$$, respectively, and $${l_i}$$ is the distance between the joint $$i$$ and the centre of mass of the link $$i$$. $$(X,Y)$$ is the coordinate of the end-effector, $$Z$$ is the posture angle. Fig. 1. View largeDownload slide The $$n$$-link planar underactuated manipulator. Fig. 1. View largeDownload slide The $$n$$-link planar underactuated manipulator. The Lagrange function of the system is L(q,q˙)=K(q,q˙)=12qTM(q)q, (2.1) where $$q = {\left[ {{q_1},\ldots,{q_n}} \right]^T}$$, $$\dot{q} = {\left[ {{\dot{q}_1},\ldots,{\dot{q}_n}} \right]^T}$$ are the vectors of generalized angles and angular velocities, $$K(q,\dot q)$$ is the kinetic energy of the system. $$M(q)=(M_{ij}(q_{s},\ldots,q_{n}))_{n\times n}$$ is the positive definite symmetric inertia matrix, $$s={\rm min}\{i,j\}+1$$ for $$i,j<n$$ and $$M_{nn}$$ is constant. The Euler–Lagrange equation of the system is ddt[∂L(q,q˙)∂q˙]−∂L(q,q˙)∂q=τ, (2.2) according to (2.1), (2.2) can be rewritten as M(q)q¨+H(q,q˙)=τ, (2.3) i.e. ∑j=1nMijq¨j+Hi=τi,i≠2, (2.3a) ∑j=1nM2jq¨j+H2=0, (2.3b) where $$\ddot{q}=[\ddot{q}_{1},\ldots,\ddot{q}_{n}]^T$$ is the vectors of generalized angular accelerations, $$H(q,\dot q)= [H_1,\ldots,H_n]^T$$ is the centripetal and coriolis term, $$\tau={\left[\tau _1,0,\tau _3,\ldots,\tau _n \right]^T}$$ is the vectors of applied torques. Lemma 2.1 The constraint equation of the passive joint (2.3b) is partially integrable if and only if (see Oriolo & Nakamura, 1991) (1) the gravitational torque in the constraint equation (2.3b) is constant, (2) the passive joint variable does not appear in the inertia matrix $$M(q)$$. Obviously, the gravitational torque in the constraint equation (2.3b) is zero, but the passive joint variable $$q_{2}$$ appears in the inertia matrix $$M(q)$$, the constraint equation (2.3b) does not meet the condition (2) of Lemma 2.1, thus it is not integrable. Therefore, the $$n$$-link planar underactuated manipulator is a second-order nonholonomic system. 3. Target angles In this section, the angle constraints between the passive link and the active $$k$$th link $$(k=3,\ldots,n)$$ are obtained, and then the target angles of the control objective are calculated by using genetic algorithm (see Köker, 2013). 3.1. Angle constraint Based on the holonomic characteristics of a planar acrobot, the whole control process of the system is divided into $$n-$$2 stages, the planar $$n$$-link manipulator is like a planar acrobot in each stage. In $$p$$th stage ($$1\leq p\leq n-2$$), the active $$k$$th link $$(k=p+2)$$ is controlled to the target angle, and the other active links are maintained to be the initial states or the target values of the previous stage. Let $$\dot{q}_{i}= 0,\ddot{q}_{i}\equiv 0, i\neq 2,k$$ and $$q^{( {p})}_{i0},i=1,\ldots,n$$ denote the initial angle of the $$i$$th link in the $$p$$th stage. From (2.3b), we have M¯22q¨2+M¯2kq¨k+H¯2=0, (3.1) where $$\bar{M}_{22}=M_{22}|_{q_{i}=q^{({p})}_{i0}},\bar{M}_{2k}=M_{2k}|_{q_{i}=q^{({p})}_{i0}},\bar{H}_{2}=H_{2}|_{{q_{i}=q^{({p})}_{i0}},\dot{q}_{i}=0},i=1,\ldots,n,i\neq 2,k$$. According to the complete integrability characteristics of the planar acrobot (see Lai et al., 2015b), there is M¯22q˙2+M¯2kq˙k=0. (3.2) Since $$\bar{M}_{22}>0$$ and there is no variable $$q_{2}$$ appears in $$\bar{M}_{22}>0$$, the equation (3.2) formed with separated variables $$q_{2}$$ and $$q_{k}$$. By using the separation of variables, the following equation is obtained through integration. q2=q20(p)−∫qk0(p)qkM¯2kM¯22dqk, (3.3) the equation (3.3) represents the angle constraint between the active $$k$$th link and the passive link. The equation (3.2) implies that if the $$k$$th link is stabilized at $${{q}}_{kd}$$, the passive link 2 is controlled to a certain angle $$q^{({p})}_{2d}$$. Considering the above angle constraints, the target angles of the control objective are calculated by using genetic algorithm in the following subsection. 3.2. Target angles calculation Based on the inverse kinematics of the system (see Wei et al., 2014), the coordinates of end-effector $$(X,Y)$$ and the posture angle $$Z$$ are the functions of the link angles $$q_1,\ldots,q_n$$ (see Fig. 1). The functions are as follows {X=−∑i=1nLisin⁡(∑j=1iqj)Y=∑i=1nLicos⁡(∑j=1iqj)Z=rem((∑j=1nqj),2π),  (3.4) where the function $${\rm rem} (x, y)$$ is the remainder of $$x$$ divided by $$y$$ ($$y$$ is not equal to zero). Due to the complexity of the angle constraints (3.3), it is difficult to solve this inverse kinematics problem. The genetic algorithm is employed to obtain the target angles of the control objective. The searching range of $$q_{3}, \ldots,q_{n}$$ is $$[-10,10]$$ rad, each chromosome consists $$(n-2)\times18$$ bytes of the binary vector. Considering the relative errors of the horizontal and vertical co-ordinates of the position as well as the posture, the fitness function is defined as follows: f=1/(|X−Xd|/|X|+|Y−Yd|/|Y|+|Z−Zd|/|Z|), (3.5) where $${X_d},{Y_d}$$ indicate the target position of the end-effector, and $${Z_d}$$ denotes the target posture. Given a random initial population, the genetic algorithm operates in cycles, as follows: Step 1. Each member of the population is evaluated by the fitness function. The population undergoes reproduction in a number of iterations. One or more parents are chosen stochastically, but strings with higher fitness values have higher probability of contributing to the offsprings. Step 2. The offsprings are produced by applying the genetic operators (crossover and mutation, and so on). Step 3. The offsprings are inserted into the population and the process is repeated. Under the restrictions of (3.3), The feasible solutions $${q_{3d}},\ldots,{q_{nd}}$$ for (3.5) are obtained by using the genetic algorithm, and $${q^{({p})}_{2d}}$$ can be worked out by using (3.3). Since the diversity and the periodicity of the links angle, there are multiple solutions for the control objective. But, when one set of solutions is searched by employing the genetic algorithm, the genetic algorithm operate is automatically stopped. 4. Controller design Let $$x = {\left[ {{x_1},\ldots,{x_n},{x_{n+1}},\ldots{\rm{,}}{x_{2n}}} \right]^T} = {\left[ {q,\dot q} \right]^T}$$, the state space equation of the planar underactuated manipulator can be written to be {x˙i=xi+nx˙i+n=Fi(x)+∑j=1ngijτj i=1,…,n, (4.1) i.e. x˙=f(x)+g(x)τ, (4.2) where $$f(x) =\left[ {{x_{n+1}},\ldots,{x_{2n}},{F}} \right]^T, g(x)=\left[ {g_1}(x),{g_2}(x) \right]^T$$, with $$F^{T}=\left[{{F_1},\ldots,{F_{n}}}\right]^{T} =-{M^{ - 1}}(q)H(q,\dot q)$$, $${M^{ - 1}}(q)={g_2}(x) = {({g_{ij}}{\rm{)}}_{n \times n}}$$, $${g_1}(x) \in {O^{n \times n}}$$, $${g_{ij}}$$ is nonlinear function of $$x_{i},i=1,\ldots,2n$$. In the $$p$$th stage, we control the active $$k$$th link to the desired angle while the other active links are maintained to be the target values of the previous stage. The Lyapunov function in the stage is constructed to be Vk(x)=12(xk+n2+(xk−qkd)2)+12∑i≠2,k(xi+n2+(xi−qi0(p))2). (4.3) Taking the time derivative of $$V_{k}(x)$$, we have V˙k(x)=xk+n(xk−qkd+Fk+∑j=1ngkjτj)+∑i≠2,kxi+n(xi−qi0(p)+Fi+∑j=1ngijτj). (4.4) According to (4.4), the control laws are designed to be {τi=(−xi+qi0(p)−Fi−γixi+n−∑j=1,j≠ingijτj)gii−1,i≠2,kτk=(−xk+qkd−Fk−γkxk+n−∑j=1,j≠kngkjτj)gkk,−1  (4.5) where $${\gamma _1},{\gamma _3},\ldots,{\gamma _n}>0$$ are constant parameters, $$g_{ii}>0$$ is the element of main diagonal of the positive matrix $$M(q)^{-1}$$. Hence, there is no singular point. Substituting (4.5) into (4.4), yields V˙k(x)=−γ1xn+12−γ3xn+32−⋯−γnx2n2≤0. (4.6) For $${\dot V_{k}}(x)$$ is negative semidefinite, LaSalle invariable principle (see LaSalle, 2012) is used to prove that the control objective can be realized. Theorem 4.1 Considering the $$n$$-link underactuated manipulator (4.1). In $$p$$th stage, under the control with the controller (4.5), the desired angle of the $$k$$th link ($$k=p+2$$) is achieved, the angular velocity asymptotically reaches zero, i.e. limt→∞{x1,…,xn,xn+1,…,x2n}={q10(p),q2d(p),q30(p),…,q(k−1)0(p),qkd,q(k+1)0(p),…,qn(p),0,…,0} (4.7) When $${p}$$ traverses through the set $$\{3,4,\ldots,n\}$$, the position and posture control objective of the planar $$n$$-link underactuated manipulator is realized. Proof. First, we prove that the system controlled by the controller (4.5) is globally asymptotically stable, or, rather,(4.7) holds. Let $${{\it\Omega}} = \left\{ {x \in {R^n}\left| {V_{k}(x) \le c,c > 0} \right.} \right\}$$, From (4.5), we know that $$V_{k}(x) \to \infty$$, as $$\left\| {{x_i}} \right\|\to \infty, i\neq 2,n+2$$. (3.2) and (3.3) imply $$\left\| {{x_k}} \right\| \to \infty$$ , as $$\left\| {{x_2}} \right\| \to \infty$$ and $$\left\| {{x_{k+n}}} \right\| \to \infty$$,as $$\left\| {{x_{k+2}}} \right\| \to \infty$$. So, $$V_{k}(x)\rightarrow\infty$$, as $$\left\| {{x_i}} \right\| \to \infty,i=1,\cdots,n$$, that is $$V_{k}(x)$$ is radial unbounded. Therefore, $${{\it\Omega}}$$ is a positive invariant compact set. Defined the invariant set $$E_{k}$$ and maximum invariant set $$M_{k}$$ of the system. From (4.6), we have $$\dot V(x)_{k} \le 0,x \in {\it\Omega}$$. If $$\dot V_{k}(x) = 0$$, we obtain $${x_{n+i}}= 0,i=1,\ldots,n,i\neq 2$$, substituting it into (3.2), yield $${x_{n+2}} = 0$$. Let the set $$E_{k} = \{ {x \in {\it\Omega} \left| \dot V_{k}(x) = 0, \right.} \} = \{ {x \in {\it\Omega} \left|{x_{n+i}}= 0,i=1,\ldots,n, \right.} \}$$. Since every points is the equilibrium of the system, $$E_{k}$$ is the invariant set of the system. If $${x_{i+n}} = 0,i = 1,\ldots,n$$, $${x_{i}}$$ are constants and $${H_i} = 0$$ holds. From the state space model (4.2), we have $$F^T = - {M^{ - 1}}(q)H(q,\dot q)$$ and $${[{\dot x_{n+1}},\ldots,{\dot x_{2n}}]^T} = F^T + {g_2}(x)\tau$$, it easy to see $$F^T =0$$ and $${g_2}(x)\tau = 0$$. Owing to $${g_2}(x)\tau = - {M^{ - 1}}(q)$$ and $$M(q)$$ is positive, $${\left[ {{\tau _1}{\kern 1pt} {\kern 1pt} {\kern 1pt} ,0,{\tau _3},\ldots,{\tau _n}} \right]^T} = 0$$ holds. Substituting it into (4.5), we have $$x_1=q^{({p})}_{10},x_3=q^{({p})}_{30},\ldots,x_{k-1}={q^{({p})}_{(k-1)0}},x_k={q_{kd}},x_{k+1}={q^{({p})}_{(k+1)0}},\ldots,x_n={q^{({p})}_{n}}$$. Using the angle constraint (3.3), we can get $${x_2} = q^{({p})}_{2d}$$ and $$E_{k} = \{q^{({p})}_{10},q^{({p})}_{2d},q^{({p})}_{30},\ldots,{q^{({p})}_{(k-1)0}},{q_{kd}},{q^{({p})}_{(k+1)0}},\ldots,{q^{({p})}_{n}}\}$$. Obviously, there is only one point in the invariant set, the maximum invariant set $$M_{k}=E_{k}$$. According to LaSalle’s theorem, every trajectories starting at $${\it\Omega}$$ approaches $$E_{k}$$, as $$t\rightarrow\infty$$, i.e. (4.7) holds. Then, we prove that all the target angles of links can be realized in the whole control process. The Switch conditions are $$\left| {{x_k} - {q_{kd}}} \right|\le {e_{k1}},\left| {{x_{n+k}}} \right|\le {e_{k2}}$$, $${e_{k1}},{e_{k2}}$$ (the small positive constants) are the switching threshold. After moving the $$k$$th link to the desired angle, the angles of links $$q^{({p})}_{10}, q^{(k)}_{2d}, q^{({p})}_{30}, \ldots$$, $${q^{({p})}_{(k-1)0}}, {q_{kd}},{q^{({p})}_{(k+1)0}},\ldots,{q^{({p})}_{n}}$$ are the initial value of controlling the other link. As $$p$$$$(1\leq p\leq n-2)$$ taking any other integer value, according to the same methods above, the system can be stabilized at the target angle of that stage. If $$p$$ traverses the set $$\{1,\ldots,n-2\}$$ successfully, all the target angles of links are realized. □ From what has mentioned above, after all the $$n-2$$ stages of control, the system can be stabilized at the target position with the desired posture finally. 5. Simulation results This section presents simulation results of a 5-link underactuated manipulator with passive second joint. The SIMULINK simulation tool of MATLAB software is applied. The parameters of the system are shown in Appendix A and the following Table 1. The initial position and posture are $$(X,Y)=(0,3.3)$$ m and $$Z=0$$ rad, the control objective(position and posture) are $$(X,Y)=(0.64,1.70)$$ m and $$Z=\pi/3$$ rad. Table 1. Parameters of a 5-link planar underactuated manipulator Linki $${m_i}$$(kg) $${L_i}$$(m) $${l_i}$$(m) $${J_i}$$(kg $${{\rm{m}}^2}$$) i = 1 0.5 0.5 0.25 0.0104 i = 2 0.6 0.6 0.3 0.0180 i = 3 0.6 0.6 0.3 0.0180 i = 4 0.8 0.8 0.4 0.0427 i = 5 0.8 0.8 0.4 0.0427 Linki $${m_i}$$(kg) $${L_i}$$(m) $${l_i}$$(m) $${J_i}$$(kg $${{\rm{m}}^2}$$) i = 1 0.5 0.5 0.25 0.0104 i = 2 0.6 0.6 0.3 0.0180 i = 3 0.6 0.6 0.3 0.0180 i = 4 0.8 0.8 0.4 0.0427 i = 5 0.8 0.8 0.4 0.0427 Table 1. Parameters of a 5-link planar underactuated manipulator Linki $${m_i}$$(kg) $${L_i}$$(m) $${l_i}$$(m) $${J_i}$$(kg $${{\rm{m}}^2}$$) i = 1 0.5 0.5 0.25 0.0104 i = 2 0.6 0.6 0.3 0.0180 i = 3 0.6 0.6 0.3 0.0180 i = 4 0.8 0.8 0.4 0.0427 i = 5 0.8 0.8 0.4 0.0427 Linki $${m_i}$$(kg) $${L_i}$$(m) $${l_i}$$(m) $${J_i}$$(kg $${{\rm{m}}^2}$$) i = 1 0.5 0.5 0.25 0.0104 i = 2 0.6 0.6 0.3 0.0180 i = 3 0.6 0.6 0.3 0.0180 i = 4 0.8 0.8 0.4 0.0427 i = 5 0.8 0.8 0.4 0.0427 In the genetic algorithm, the simulation step length is 0.001 s, the crossover possibility is 0.8 and the mutation possibility is 0.002. According to the control objective, the target angles calculated by using the genetic algorithm. The target angles of the active links are generated $${q_{3d}} = -6.993$$ rad, $${q_{4d}} = 7.138$$ rad, $${q_{5d}} = 4.899$$ rad, as Fig. 2(a–c) show. Based on the angle constraints, $${q_{2d}} =-8.156$$ rad is obtained. Figure 2(d) implies that the fitness function reaches the maximum value $$(f=32.238)$$. At this time, the reciprocal of the fitness function $$1/f=0.031$$ is the minimum. Therefore, the relative errors of the horizontal and vertical coordinates of the position as well as the posture are small, which shows the accuracy for those target angles is high. Fig. 2. View largeDownload slide Target angles calculation. Fig. 2. View largeDownload slide Target angles calculation. The simulation results Fig. 3(a) shows that the angles converge to the target values smoothly and the control strategy switch at 7.703 and 15.421 s, the system realize stabilization in 22 s. The angles of the links finally reach steady state at $${q_{1d}} = 0$$ rad, $${q_{2d}} =-8.156$$ rad, $${q_{3d}} = -6.993$$ rad, $${q_{4d}} = 7.138$$ rad, $${q_{5d}} = 4.899$$ rad. Figure 3(b) implies that the control torques vary within 4 Nm, which are relatively small. The co-ordinates of the end effector stabilized at $$(0.630, 1.711)$$ m with the posture angle $$1.057$$ rad, as the Fig. 3(c and d) show. For different position or posture, the proposed control strategy can realize the control objective. Fig. 3. View largeDownload slide Simulation curves of the motion states. Fig. 3. View largeDownload slide Simulation curves of the motion states. 6. Conclusion We have presented a multi-stage control strategy to realize the position and posture control of $$n$$-link planar underactuated manipulator with passive second joint. The control strategy consists of $$n-$$2 stages. In each stage, the $$n$$-link planar underactuated manipulator is like a planar acrobot. Based on the angle constraint relation of each stage of the system, genetic algorithm is employed to obtain the target angles of links according to the control objective. Through the control of the $$n-$$2 stages, the active links and the passive link reach their target angles, which ensures that the control objective of the system is realized. Simulation results demonstrate the validity of the proposed control method. The control strategy in the paper can be extended to the $$n$$-link planar underactuated manipulators with one passive joint, but the reachable area is different because of the different position of the passive joint. In the future, we will find a control strategy to control all active links before the passive joint to their arbitrary target angles, so we can stabilize the second-order nonholonomic underactuated mechanical system at arbitrary point of the geometric reachable region. Funding National Natural Science Foundation of China (grant 61374106); and the Hubei Provincial Natural Science Foundation of China (grant 2015CFA010). Appendix A The inertia matrix of a 5-link planar underactuated manipulator, $$M(q) = (M_{ij}(q_{s},\ldots,q_{5}))_{5\times5}, s={\rm min}\{i,j\}+1$$, is the positive definite symmetric inertia matrix, the parameters are as following {α1=J1+m1l12+(m2+m3+m4+m5)L12,α2=J2+m2l22+(m3+m4+m5)L22,                                      α3=(m2l2+(m3+m4+m5)L2)L1,α4=J3+m3l32+(m4+m5)L32,α5=m3L1l3+(m4+m5)L1L3,a6=m3L2l3+m4L2L3+m5L2L3,α7=m4L1l4+m5L1L4,a8=m4L2l4+m5L2L4,α9=m4L3l4+m5L3L4,α10=J4+m4l42+m5L42,α11=m5L1l5,α12=m5L3l5,α13=m5L4l5,α14=m5L2l5,α15=J5+m5l52  {M55=α15,M45=α13cos⁡q5+α15,M44=α10+α13cos⁡q5+M45,M35=α12cos⁡q45+α13cos⁡q5+α15,M34=α10+α9cos⁡q4+α13cos⁡q5+M35,M33=α4+α9cos⁡q4+α12cos⁡q45+M34,M25=α14cos⁡q35+α12cos⁡q45+α13cos⁡q5+α15,      M24=α10+α13cos⁡q5+α8cos⁡q34+α9cos⁡q4+M25,M23=α4+α6cos⁡q3+α9cos⁡q4+α12cos⁡q45+M24,M22=α2+α6cos⁡q3+α8cos⁡q34+α14cos⁡q35+M23,M15=α11cos⁡q25+α12cos⁡q45+α13cos⁡q5+α14cos⁡q35+α15,M14=α10+α7cos⁡q24+α8cos⁡q34+α9cos⁡q4+α13cos⁡q5+M15,M13=α4+α5cos⁡q23+α6cos⁡q3+α9cos⁡q4+α12cos⁡q45+M14,M12=α2+α3cos⁡q2+α6cos⁡q3+α8cos⁡q34+α9cos⁡q4+α12cos⁡q45+M13,M11=α1+α3cos⁡q2+α5cos⁡q23+α7cos⁡q24+α11cos⁡q25+M12.  {H1=−α3(2q˙1+q˙2)q˙2sin⁡q˙2−α5(2q˙1+q˙23)q˙23sin⁡q˙23−α6(2q˙12+q˙3)q˙3sin⁡q˙3−α7(2q˙1+q˙24)q˙24sin⁡q˙24−α8(2q˙12+q˙34)q˙34sin⁡q˙34−α9(2q˙13+q˙4)q˙4sin⁡q˙4−α11(2q˙1+q˙25)q˙25sin⁡q˙25−α12(2q˙13+q˙45)q˙45sin⁡q˙45−α13(2q˙14+q˙5)q˙5sin⁡q˙5−α14(2q˙12+q˙35)q˙35sin⁡q˙35H2=α3q˙22sin⁡q˙2+α5q˙12sin⁡q˙23−α6(2q˙12+q˙3)q˙3sin⁡q˙3+α7q˙12sin⁡q˙23−α8(2q˙12+q˙34)q˙34sin⁡q˙34  −α11q˙12sin⁡q˙25−α9(2q˙13+q˙4)q˙4sin⁡q˙4−α12(2q˙23+q˙45)q˙45sin⁡q˙45−α13(2q˙14+q˙5)q˙5sin⁡q˙5−α14(2q˙12+q˙35)q˙35sin⁡q˙35,H3=α5q˙12sin⁡q˙23−α6q˙122sin⁡q˙3+α7q˙12sin⁡q˙24+α8q˙122sin⁡q˙34−α9q˙14q˙4sin⁡q˙4+α11q˙12sin⁡q˙25+α14q˙122sin⁡q˙35−α12(2q˙13+q˙45)q˙45sin⁡q˙45−α13(2q˙14+q˙5)q˙5sin⁡q˙5,H4=α8q˙122sin⁡q˙34+α9q˙132sin⁡q˙4+α11q˙12sin⁡q˙24+α12q˙132sin⁡q˙45−α13(2q˙14+q˙5)q˙5sin⁡q˙5+α14q˙122sin⁡q˙35,H5=α11q˙12sin⁡q˙25+α12q˙132sin⁡q˙45+α13(2q˙14+q˙5)q˙14sin⁡q˙5+α14q˙122sin⁡q˙35,  where $${q_{ij}} = {q_i} + {q_{i + 1}} + \cdot \cdot \cdot + {q_j},i<>< j$$. References Aguilar-Ibáñez C. & Sira-Ramírez H. (2007) A linear differential flatness approach to controlling the Furuta pendulum . IMA J. Math. Control Inform., 24 , 31 – 45 . Google Scholar CrossRef Search ADS Chen H. Zhang J. & Chen B. (2013) Global practical stabilization for non-holonomic mobile robots with uncalibrated visual parameters by using a switching controller . IMA J. Math. Control Inform., 30 , 543 – 557 . Google Scholar CrossRef Search ADS De Luca A. Mattone R. & Oriolo G. (2000) Stabilization of an underactuated planar 2R manipulator . Int. J. Robust Nonlin. Control, 10 , 181 – 198 . Google Scholar CrossRef Search ADS Eom M. & Chwa D. (2015) Robust swing-up and balancing control using a nonlinear disturbance observer for the pendubot system with dynamic friction . IEEE Trans. Robot., 31 , 331 – 343 . Google Scholar CrossRef Search ADS Hirose N. Tajima R. Tajima R. Sukigara K. Kazutoshi S & Yuji T. (2015) Posture control for personal mobility robot using feedback compensation with unstable pole . Electr. Eng. Jpn., 193 , 33 – 42 . Google Scholar CrossRef Search ADS Hussein I. & Bloch A.M. (2008) Optimal control of underactuated nonholonomic mechanical systems, . IEEE Trans. Automat. Contr., 53 , 668 – 682 . Google Scholar CrossRef Search ADS Knoll C. & Röbenack K. (2011) Control of an underactuated manipulator using similarities to the double integrator . IFAC Proceedings Volumes , 44 , 11501 – 11507 . Google Scholar CrossRef Search ADS Köker R. (2013) A genetic algorithm approach to a neural-network-based inverse kinematics solution of robotic manipulators based on error minimization . Inf. Sci., 222 , 528 – 543 . Google Scholar CrossRef Search ADS Lai X. Z. She J. H. Cao W. H & Yang S. X. (2015b) Stabilization of underactuated planar acrobot based on motion-state constraints . Int. J. Nonlin. Mech., 77 , 342 – 347 . Google Scholar CrossRef Search ADS Lai X. Z Sheng Y. Wu M. (2016) Stable control strategy for planar three-link underactuated mechanical system . IEEE/ASME Trans. Mechatron. , 21 , 1345 – 1356 . Google Scholar CrossRef Search ADS Lai X. Z. Zhang A. C. Wu M. & She J. H. (2015a) Singularity-avoiding swing-up control for underactuated three-link gymnast robot using virtual coupling between control torques, . Int. J. Robust Nonlin. Control, 25 , 207 – 221 . Google Scholar CrossRef Search ADS La Salle J. P. (2012) The Stability and Control of Discrete Processes , vol. 62. New York: Springer . Oriolo G. & Nakamura Y. (1991) Control of mechanical systems with second-order nonholonomic constraints: underactuated manipulators . Proc. 30th IEEE Decisi. Control, 14 , 1072 – 1088 . Sharbafi M. A. Maufroy C. Ahmadabadi M. N. Yazdanpanah M. J. & Seyfarth A. (2013) A robust hopping based on virtual pendulum posture control . Bioinspir. & Biomim., 8 , 036002 . Shiriaev A. S. Freidovich L. B. & Spong M. W. (2014) Controlled invariants and trajectory planning for underactuated mechanical systems . IEEE Trans. Automat. Control, 59 , 2555 – 2561 . Google Scholar CrossRef Search ADS Wei Y. Jian S. He S. & Wang Z. (2014) General approach for inverse kinematics of nR robots . Mech. Mach. Theory, 75 , 97 – 106 . Google Scholar CrossRef Search ADS Xin X. She J. H. She, Yamasaki T. & Liu Y. (2009) Swing-up control based on virtual composite links for n-link underactuated robot with passive first joint . Automatica , 45 , 1986 – 1994 . 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: Dec 17, 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

Abstract access only

18 million full-text articles

Print

20 pages / month

PDF Discount

20% off