Constrained Model Predictive Control for a Hexapod Robot Walking on Irregular Terrain

Abstract

It is usually a common task that walking on irregular terrain for a hexapod robot. This paper treats the problem of control of a hexapod robot that has been developed in a collaborative project of the Departments of Mechanical Engineering and Automation at Shanghai Jiao Tong University. We derive the hexapod robot’s kinematics and dynamics models, which are non-periodic, nonlinear, hybrid and open-loop unstable. In order to solve this challenge, we compute a transverse linearization about the desired motion which is a linear impulsive system. The real environment, where the robot performs tasks, are usually unstructured and dynamic, it is always difficult to know the size of obstacles the robot strikes on. In order to reduce its reliance on terrain information for locomotion control to the minimum, taking account of physical constraints, we propose a constrained model predictive controller for stabilization of non-periodic trajectories of the hexapod robot walking over irregular terrain. We prove the stability of the proposed controller theoretically. In the end we perform several simulations and experiments to verify the effectiveness of the control algorithm we proposed.

Appendices

Appendix A: Kinematic parameters derivation

Basic coordinate system $O_{0} x_{0} y_{0} z_{0}$ is fixed on the base as shown in Fig. 3a, while $O_{0}$ is the midpoint of $R_{D1}R_{D2}$, $x_{0}$ axis is coincident with $R_{D1}R_{D2}$ and from $R_{D2}$ to $R_{D1}$; $z_{0}$ axis is vertically upward; $y_{0}$ axis satisfies the right-hand rule. The coordinate system $U_{i} x_{i} y_{i} z_{i}$ and U_ie_if_ig_i(i = 1, 2, 3) are built at the joint $U_{i}$, $x_{i}$ axis is coincident with axis of rotation of $U_{i}$ and vertical to $R_{Di} U_{i}$, $y_{i}$ is parallel to the $R_{Di} U_{i}$, $z_{i}$ axis satisfies the right-hand rule, $f_{i}$ axis is coincident with axis of rotation of $U_{i}$ and vertical to $U_{i} S_{i}$, $g_{i}$ axis is coincident with $U_{i} S_{i}$, $e_{i}$ axis is satisfies the right-hand rule. The coordinate system $H x_{4} y_{4} z_{4}$ is built at the joint H, $y_{4}$ is coincident with $y_{0}$, $x_{4}$ is vertical to parallelogram $R_{1} R_{2} R_{5} R_{3}$, $z_{4}$ satisfies the right-hand rule.

Based on the analysis of leg structure and the building of the coordinate system, we know that the coordinate system $U_{i} x_{i} y_{i} z_{i}$ is gotten by rotating O₀x₀y₀z₀ by $\theta _{pi}$ about the $x_{0}$ axis. the coordinate system $U_{i} e_{i} f_{i} g_{i}$ can be gotten by rotating the coordinate system $U_{i} x_{i} y_{i} z_{i}$ twice, the first rotation is by the angle $\alpha _{ti}$ about the $x_{i}$ axis, and the second rotation is by the angle $\beta _{i}$ about the $f_{i}$ axis. So we can get the attitude matrix of U_iS_i with the following equation.

$$\begin{array}{@{}rcl@{}} {R}_{i}&=&Rot(x_{0}, \theta_{pi}) Rot(x_{i}, \alpha_{ti}) Rot(f_{i},\beta_{i})\\ &=&[ {e}_{i}\quad {f}_{i}\quad {g}_{i}], \end{array} $$

(33)

$$ {g}_{i}=(g_{ix},g_{iy},g_{iz})^{T}, i = 1,2,3, $$

(34)

where ${e}_{i}$, ${f}_{i}$ and ${g}_{i}$ is unit vector of $e_{i}$, $f_{i}$ and $g_{i}$.

Then we can get the posture angle of $U_{i} S_{i}$ as follows.

$$\begin{array}{@{}rcl@{}} \alpha_{i} &=&\theta_{pi}+\alpha_{ti}=\arctan(g_{iy}/g_{iz}),\\ \beta_{i} &=&\arcsin g_{ix}, \end{array} $$

(35)

We let revolute joint $R_{Di}(i = 1,2,3)$ as driven input point, $m_{i}(i = 1,2,3)$ as the length of $R_{Di}U_{i}$, and ${n}_{i}$ as the unit vector of $R_{Di}U_{i} (i = 1,2,3)$, we have

$$ \frac{ {l}_{RDiUi}}{m_{i}}= {n}_{i}, $$

(36)

Where ${n}_{i}=(0,\cos \theta _{pi},\sin \theta _{pi})^{T}$, $\theta _{pi}$ is the angular displacement of the $R_{Di}$, ${l}_{RDiUi}$ is the vector of $R_{Di}U_{i}$ in the basic coordinate system.

For Driving mechanism in Fig. 3a, we choose G as our reference point, and set $G=(x_{0},y_{0},z_{0})^{T}$ in the basic coordinate system , based on the geometric conditions of the mechanism, we have

$$ {G}= {u}_{3}+ {L}_{3}=m_{3} {n_{3}}+ {R_{D3}}+l_{3} {g}_{3}, $$

(37)

$$ {G}= {u}_{i}+ {L}_{i}- {a}_{i}=m_{i} {n_{i}}+ {R_{Di}}+l_{3} {g}_{i}- {R}_{3} {a}_{0i}, i = 1,2, $$

(38)

where ${u}_{i}(i = 1,2,3)$ is the vector of $U_{i}$, ${L}_{i}(i = 1,2)$ and ${L}_{3}$ is the vector of $U_{i} S_{i}(i = 1,2)$ and $U_{3} G$ respectively, $l_{i}(i = 1,2)$ and $l_{3}$ is the length of $U_{i} S_{i}(i = 1,2)$ and $U_{3} G$ respectively, ${a}_{0i}$ and ${a}_{i}$ is the vector of $S_{i}$ in the coordinate system $U_{i} x_{i} y_{i} z_{i}$ and $O_{0} x_{0} y_{0} z_{0}$, respectively. ${R}_{D3}$ and ${R}_{Di}$ is the vector of ${R}_{D3}$ and ${R}_{Di}$ in the coordinate system $O_{0} x_{0} y_{0} z_{0}$, respectively.

Take the norm of both sides of the Eq. 37, we obtain

$$ l_{3}=|| {G}- {u}_{3}||, $$

(39)

$$ {g}_{3}=({G}- {u}_{3})/l_{3}, $$

(40)

Take the norm of both sides of the Eq. 38, we obtain

$$ l_{i}=|| {G}- {u}_{i}+ {a}_{i}||, \qquad i = 1,2, $$

(41)

$$ {g}_{i}=({G}- {u}_{i}+ {a}_{i})/l_{i}, $$

(42)

From Eq. 33 to 47, we can get the relationship between vector ${G}$ and ${R}_{Di}$ as follows.

$$\begin{array}{@{}rcl@{}} {R}_{Di}\!&=&\!(G+ {a}_{i} - {u}_{i})^{T} {n}_{i}\\&& - \sqrt{((G\,+\, {a}_{i} \,-\, {u}_{i})^{T} {n}_{i})^{2}\,-\, (G\,+\, {a}_{i} \,-\, {u}_{i})^{T}(G\,+\, {a}_{i}\,-\, {u}_{i})\,+\,{l_{i}^{2}} } ~,\\&& i = 1,2, \end{array} $$

(43)

$$\begin{array}{@{}rcl@{}} {R}_{D3}\!&=&\!(G- {u}_{3})^{T} {n}_{3}\\&& - \sqrt{((G\,-\, {u}_{3})^{T} {n}_{3})^{2}\,-\, (G\,-\, {u}_{3})^{T}(G\,-\, {u}_{3})\!+{l_{3}^{2}} } ~, \end{array} $$

(44)

Then we can get angular displacement $(\theta _{p1},\theta _{p2},\theta _{p3})$. of ${R}_{Di}$ as follows.

$$ \theta_{pi}=arccos({R}_{Di_{y}}) , $$

(45)

For walking mechanism in Fig. ??a, let the vector of C and S be ${C}_{0}=(x_{C_{0}},y_{C_{0}},z_{C_{0}})^{T}$ and ${S}_{0}=(x_{r_{0}},y_{r_{0}},z_{r_{0}})^{T}$ in the coordinate system $H x_{4} y_{4} z_{4}$, respectively. Based on the geometric conditions of the mechanism the following equations are obtained.

$$ \left\{\begin{array}{lllll} y_{r_{0}} = l_{4} \cos\theta_{1}+c \cos\theta_{2} \\ z_{r_{0}} = l_{4} \sin\theta_{1}+c \sin\theta_{2} \\ z_{C_{0}} = (l_{4}+l_{5}+l_{7})\sin\theta_{1}+l_{6}\sin\theta_{2} \\ y_{C_{0}} = (l_{4}+l_{5}+l_{7})\cos\theta_{1}+l_{6}\cos\theta_{2} \\ x_{C_{0}} = x_{r_{0}}= 0, \end{array}\right. $$

(46)

where $\theta _{1}$ and $\theta _{2}$ is the angle by rotating $l_{4}$ and $l_{6}$ about $x_{4}$ axis, respectively.

Hx₄y₄z₄ is gotten by rotating $O_{0} x_{0} y_{0} z_{0}$ about the $y_{4}$ axis, and the posture matrix is given as follows.

$$ {R}_{4}=Rot(y_{3},\theta_{3}), $$

(47)

From Eq. 15, the relationship between ${C}_{0}$ and ${S}_{0}$ in the basic coordinate system is given as follows.

$$ {C}= {R}_{4} {C}_{0}+ {H}, $$

(48)

$$ {S}= {R}_{4} {S}_{0}+ {H}, $$

(49)

where ${S}$, ${C}$ and ${H}$ is the vector of S, C and H in the basic coordinate system, respectively.

Based on Eq. 46, we can obtain the following equation.

$$ {S}_{0}=f({C}_{0}), $$

(50)

where f represents the mapping relationships between ${C}_{0}$ and ${S}_{0}$.

From Eqs. 47 and 50, the relationship between S and C is given as follows.

$$ {S}= {R}_{4}f({R}^{-1}({C}- {H}))+ {H}, $$

(51)

The relationship between S and G is given in the following equation.

$$ {S}= {L}_{8}+ {G}= {R}_{3} {S}_{G}+ {G}, $$

(52)

where ${S}_{G}$ and ${L}_{8}$ is the vector of S and GS in the moving and basic coordinate system, respectively.

From Eqs. 51 and 52, we can get the relationship between vector $C$ and $G$ as follows.

$$ {G}= {R}_{4}f({R}^{-1}({C}- {H}))+ {H}- {R}_{3} {S}_{G}, $$

(53)

Substituting Eqs. 53 into 43 and 44, we can get relationship between vector $C$ and ${R}_{Di}$, then from Eq. 45, we can get the relationship between the vector $C$ and angular displacement $(\theta _{p1},\theta _{p2},\theta _{p3})$.

Appendix B: Design of non-periodic gait trajectory

As the gait trajectory of the robot walking over irregular terrain is non-periodic, we give the design of non-periodic gait trajectory below.

If the foot of the hexapod robot doesn’t touch the obstacles, this situation of which is the same as robot walking on even terrain, the gait trajectory of the robot is periodic. Once the foot of the robot touches the obstacles, the gait trajectory will change into non-periodic. So we design the non-periodic gait trajectory with two parts.

Periodic part

During the period of the robot doesn’t touch the obstacles, we design the periodic trajectory with B$\acute {e}$zier polynomials in Eq. 24. Each period includes active and passive movement phase. In active movement phase, since we have six constraints in (24) to design the joint trajectory, we use a Bezier curve of order 5 to define the joint trajectory. The joint trajectory from Eq. 24 is specified as follows.

$$\begin{array}{@{}rcl@{}} F_{a}&=&q(t)=P_{0} (1-t)^{5} + 5P_{1} (1-t)^{4} t + 10P_{2} (1-t)^{3} t^{2} \\&&+ 10P_{3}(1-t)^{2} t^{3} + 5P_{4}(1-t) t^{4} + P_{5} t^{5} , \end{array} $$

(54)

We let the T be the time of one walking period and set the duty factor here is 0.5 which means a joint will take only $0.5T$ to complete its own active motion. Equation 54 is subject to the following constraints.

$$\begin{array}{@{}rcl@{}} &&\text{Position:} \left\{\begin{array}{ll} q(0) = q_{0} \\ q(0.5T) = q_{T/2} \end{array}\right.\\ &&\text{Velocity:} \left\{\begin{array}{ll} \dot{q}(0) = \dot{q}_{0} \\ \dot{q}(0.5T) = \dot{q}_{T/2}, \end{array}\right.\\ &&\text{Acceleration:} \left\{\begin{array}{ll} \ddot{q}(0) = \ddot{q}_{0} \\ \ddot{q}(0.5T) = \ddot{q}_{T/2}, \end{array}\right. \end{array} $$

(56)

where $q_{0}$ and $q_{T/2}$ are initial position value and end position value of joint q, respectively. $\dot {q}_{0}$ and $\dot {q}_{T/2}$ are initial velocity value and end velocity value, respectively. $\ddot {q}_{0}$ and $\ddot {q}_{T/2}$ are the corresponding initial acceleration value and end acceleration value, respectively. We can get the coefficient $P_{0}$ to $P_{5}$ from (55). Here, we let $\dot {q}_{0}=\dot {q}_{T/2}= 0$, $\ddot {q}_{0}=\ddot {q}_{T/2}= 0$. Then we get the polynomials $F_{a}$ as the trajectory of active movement phase. Similarly, we can get polynomials $F_{p}$ as the trajectory of passive movement phase. The desired trajectory curve of angular displacements is shown in Fig. 11.

Non-periodic part

Once the foot of the robot touches the obstacles, the gait trajectory will change into non-periodic. The touches always happen in active movement phase. During one walking period, before a touch happens, we use $F_{a}$ in above paragraph as the trajectory of active movement phase. When a touch happens, we record the time ${\Delta } t$ (Δt < 0.5T), the position $q_{{\Delta } t}$ of the joint, which can be detected by the sensors installed on the robot. After the touch happens, the passive movement phase of the joint happens. The trajectory $F_{p}^{\prime }$ of passive movement phase is subject to the following constraints.

$$\begin{array}{@{}rcl@{}} &&\text{Position:} \left\{\begin{array}{ll} q({\Delta} t) = q_{{\Delta} t }\\ q(T) = q_{0}-2(q_{T/2}-q_{{\Delta} t}) \end{array}\right.\\ &&\text{Velocity:} \left\{\begin{array}{ll} \dot{q}({\Delta} t) = 0 \\ \dot{q}(T) = 0, \end{array}\right.\\ &&\text{Acceleration:} \left\{\begin{array}{ll} \ddot{q}({\Delta} t) = 0\\ \ddot{q}(T) = 0, \end{array}\right. \end{array} $$

(24)

Then we get the polynomials $F_{p}^{\prime }$ as the trajectory of passive movement phase.

In the following walking period. The trajectory $F_{a}^{\prime }$ of active movement phase is subject to the following constraints.

$$\begin{array}{@{}rcl@{}} &&\text{Position:} \left\{\begin{array}{ll} \theta(0) = q_{0}-2(q_{T/2}-q_{{\Delta} t})\\ \theta(0.5T) = q_{T/2} \end{array}\right.\\ &&\text{Velocity:} \left\{\begin{array}{ll} \dot{q}({\Delta} t) = 0 \\ \dot{q}(T) = 0, \end{array}\right.\\ &&\text{Acceleration:} \left\{\begin{array}{ll} \ddot{q}({\Delta} t) = 0\\ \ddot{q}(T) = 0, \end{array}\right. \end{array} $$

(57)

Then we get the polynomials $F_{a}^{\prime }$ as the trajectory of active movement phase. If there is no touch in the active movement phase, we use $F_{p}$ we designed in periodic part as trajectory of passive movement phase. If another touch happens, we update $F_{p}^{\prime \prime }$ using the method we used in the previous period. The desired trajectory curve of angular displacements is shown in Fig. 13a.