GAIT TRANSITION CONTROL OF FLEXIBLE BIPED ROBOT WITH VARIABLE LEG LENGTH AND SWING LEG DYNAMICS
-
摘要:
弹簧负载倒立摆模型是一种典型的双足行走模型，已经成为研究机器人类人行走的基础。本文在此模型的基础上进行了扩展，通过添加刚性躯干、脚质量及采用变长度伸缩腿，充分考虑了躯干及摆动腿动力学对机器人行走步态的影响。首先，利用欧拉–拉格朗日法推导了动力学方程。其次，设计了反馈线性化控制器来跟踪目标轨迹，以及调节摆动腿和躯干的姿态。第三，提出了步态切换策略，通过控制腿部长度和髋关节力矩来实现步态切换，从而改变平均行走速度。最后，通过计算机仿真验证了该方法的有效性。仿真结果表明：该控制策略能够有效地跟踪系统的期望轨迹及实现两种自然步态之间的切换，并形成稳定的极限环，实现机器人的稳定行走。
Abstract:The spring-loaded inverted pendulum (SLIP) model is a typical biped walking model, which has been the basis for the study of human-like walking robots. In this paper, we extend the SLIP model to include a rigid torso, foot mass and to adopt telescopic legs of variable length. The influence of the dynamics of the torso and swing leg on the gait of the robot is fully considered. Firstly, the dynamic equations are derived using the Euler– Lagrange method. Secondly, the feedback linearization controller is designed to track the desired trajectory and regulate the swing leg orientation and the attitude of the torso. Thirdly, the gait switching strategy is presented to realize walking gait transition by controlling the legs length and the hip torques, thus the average walking speed can be changed. Finally, computer simulations are carried out to verify the effectiveness of the presented method. Simulation results show that the controller is effective for tracking the desired trajectory of the system and realize the transition between two natural gaits. Furthermore, a stable limit cycle is formed and the robot can walk stably.
-
Keywords:
- biped robot /
- torso /
- swing leg dynamics /
- gait transition /
- variable leg length /
- hip torque
-
1990年，McGeer[1]首次提出了被动动态行走(passive dynamic walking, PDW), 它是一类依靠重力作用和腿的自然摆动便能沿着斜坡向下行走的双足机器人。与主动驱动系统(如本田ASIMO)相比，基于PDW原理设计的机器人效率要高得多[2]。然而，由于它们依赖于重力势能，因此无法实现在平地上行走。此外，这种机器人的腿是刚性的，与人类的柔性腿有很大的不同。
1989年，Blickhan[3]首次提出了描述人类奔跑与跳跃的弹簧–质点模型。虽然人的腿具有复杂的神经控制系统，但在跑步阶段，腿会表现出简单的弹簧状行为，可以用弹簧–质点模型来描述。2006年，Geyer等[4]在弹簧–质点模型的基础上，建立了双足弹簧负载倒立摆(spring-loaded inverted pendulum, SLIP)模型。Geyer的研究表明：如果要使一个双足机器人在行走过程中具有类人步态，必须采用柔性腿而不是刚性腿。这是因为柔性腿能准确地模拟人类行走过程中髋关节运动的轨迹和站立时的动力学特性。更重要的是，借助于弹簧的弹性势能，基于柔性腿的双足机器人可以实现平地行走。
尽管SLIP模型可以以类人的步态在平地上行走，但其抗干扰能力较差。2010年，Rummel等[5]研究了机器人行走时腿部刚度与鲁棒性之间的关系。研究表明：可变腿刚度能有效地抑制外部干扰对机器人的影响，并且，系统的能量效率也会随着腿刚度的增加而提高。2012年，Visser等[6]将SLIP模型扩展为变腿刚度的V-SLIP模型，显著地提高了双足机器人对外界扰动的鲁棒性。2020年，张瑞等[7]以V-SLIP模型为研究对象，提出了一种能量高效、步态自然的模仿人类行走的控制策略，实现了零输入的纯被动周期行走。
但V-SLIP模型存在一个显著的缺点，模型假设中没有考虑腿的质量，即忽略了摆动腿的动力学对机器人行走的影响。为此，2013年，Visser等[8]将V-SLIP模型扩展为考虑摆动腿动力学的V-SLIP-SL模型，即在每条腿的末端增加了一个脚质量。仿真结果表明，在控制器的作用下，机器人具有足够的鲁棒性，能够抑制由摆动腿动态所引起的扰动。2017年，Vu等[9]在Visser 模型的基础上，增加了躯干装置，提出了带躯干V-TSLIP模型。采用线性二次型调节器(discrete linear quadratic regulator, DLQR)实现了系统的稳定运行。
虽然双足机器人的变刚度控制能使机器人拥有良好的鲁棒性能，但变刚度实验装置的设计和制造都比较复杂[10-11]，而变长度的实验装置要简单得多[12]。2005年，Asano等[13]提出了一种基于参数激励的动态步态生成方法，在这种情况下，能量的恢复是由变长度的伸缩腿上下运动来实现的。2008年，Asano等[14]针对平面伸缩腿双足机器人模型，提出了一种简单的控制律，在不考虑零力矩点(zero moment point, ZMP)条件的情况下，通过伸缩摆动腿即可轻松实现水平步态生成。2012年，Asano[15]针对具有伸缩腿的平面极限环步行器，提出了一种基于前倾碰撞姿态的步态生成方法。该方法的主要目的是使机器人的质心向前倾斜或移动，从而容易克服中间位置的势垒，恢复机器人的机械能。首先，引入了一个由8个伸缩腿组成的平面无缘轮模型，并研究了所提方法的有效性。其次，将该方法推广到带有制动弹簧的平面伸缩腿双足机器人，并通过数值仿真验证了该方法的有效性。仿真结果表明，只需将碰撞姿态微微前倾，即可获得稳定的动态步态。2017年，Hanazawa等[16]提出了一种利用非对称摆动腿运动来提高双足机器人极限环步行速度的新方法。通过数值模拟证明了摆动腿运动对步行速度的影响。然后用数学方法证明了摆动腿运动的收缩和伸长会产生推进效果的力矩。基于此原理，采用非对称摆腿运动的双足机器人可以实现较高的步行速度。2018年，Harata等[17]研究了包含伸缩腿的欠驱动双足机器人的稳定性。将输出跟随控制律应用于机器人的线性化运动方程中，将被控机器人的运动方程指定为一个线性时变系统。建立了误差转移方程来评估稳定性。通过数值计算说明了腿部伸缩运动对稳定性的影响: 伸缩摆动腿的运动比支撑腿的运动更稳定。
上述文献的变长度伸缩腿控制基本采用的都是刚性腿，本课题组在此基础上，对双足机器人变长度的柔性伸缩腿控制展开研究。2019年，张奇志等[18]研究半被动伸缩腿双足机器人行走控制和周期解的全局稳定性问题，设计了腿伸缩长度的支撑腿角度反馈控制率，证明了伸缩腿双足机器人行走过程不动点的全局稳定性。同年，宋嘉琦等[19]采用变长度柔性伸缩腿结构，研究了半被动双足机器人的平面稳定行走的控制问题。结果表明变长度控制算法可以实现机器人的稳定行走，并具有较强的抗干扰性。
虽然双足机器人的模型在过去的几十年里得到了很好的发展，但是不同步态之间的切换仍然是一个具有挑战性的研究方向。2014年，Roozing等[20]采用分段SLIP 模型[21]即S-SLIP模型，及V-SLIP模型，通过控制腿的刚度和摆动腿触地角实现了步态切换。在他们的研究中，未曾考虑躯干和摆动腿动力学对系统性能的影响。
在上述研究的基础上，本文提出了考虑摆动腿动力学的变腿长度VL-TSLIP-SL模型。提出了一种通过控制腿部长度和髋关节力矩来实现步态切换的控制策略，从而改变平均步行速度。针对机器人模型高度非线性的特点，设计了反馈线性化控制器来跟踪髋关节质点的期望轨迹，调节摆动腿和躯干的姿态。仿真结果表明，该控制器能够有效地控制躯干和摆动腿的姿态，控制策略能够实现两种不同速度的自然步态之间的切换，形成稳定的极限环。
1. 双足机器人动力学模型
1.1 机器人模型描述
VL-TSLIP-SL模型如图1所示，由质量为M的髋关节质点连接着一个刚性躯干和两条弹簧腿组成，每条腿的末端各有一个质量为
$ m_{\rm f} $ 的脚。髋关节的姿态定义为变量($ x, y)=:q_{\rm h}\in Q_{\rm h} $ 。躯干是长度为l，中心质量为m的刚性杆，转动惯量为I。双腿为可变长度的伸缩腿,长度为$ L_0+ u_{i} $ ，$ i=1,2 $ ，其中$ L_0 $ 为原长度，$ u_{i}(i= 1, 2 $ )为可控部分的腿长度。弹簧刚度为$ k_0 $ ，并假设为单向的，只能被压缩。髋关节力矩$ \tau _{1} $ 和$ \tau_{2} $ 用来控制躯干姿态角α和摆动腿与法线夹角β。由于机器人的运动是周期性的，因此我们只需研究周期步态中的一个周期。考虑到机器人双腿的对称性，定义一个步态周期为：一个支撑腿垂直站立状态(vertical leg orientation, VLO)到另一个支撑腿垂直站立状态之间的时间间隔。如图2所示，机器人的行走在单支撑阶段(single support, SS)与双支撑阶段(double support, DS)之间交替。一个周期步态从处于VLO瞬间开始，此时，左腿(黑色)处于SS阶段，右腿(蓝色)处于摆动阶段。当右腿以原长度
$ L_0 $ ，与地面夹角θ接触地面时，机器人进入DS阶段。由于系统有足够的能量，它将继续向前移动，直到左腿完全伸展，达到原长度，此时左腿离地，机器人进入右腿SS阶段。当到达下一个VLO时，一个周期步态结束。由此可得，从SS切换到DS的条件是
$$ S_{\rm SS\to DS} =\{q_{\rm h} \in Q_{\rm h} \vert y-L_{0} \sin \theta =0\} $$ DS切换为SS的条件为
$$ S_{\rm DS\to SS} =\{q_{\rm h} \in Q_{\rm h} \vert L_{i} -L_{0} =0\}, \;\;i\in [1,2] $$ 其中，
$ L_{i} $ 是后支撑腿的长度。1.2 系统动力学
利用拉格朗日方程建立VL-TSLIP-SL模型的动力学方程。拉格朗日函数为
$$ L=K-P $$ (1) 其中，K为系统的动能，P为系统的势能。
动力学方程为
$$ \dfrac{{\rm d}}{{\rm d}t}\left( {\dfrac{\partial L}{\partial \dot{{q}}_{i} }} \right)-\dfrac{\partial L}{\partial q_{i} }=F_{i}\ \ (i=1,2,\cdots,n) $$ (2) 其中n为系统的自由度，
$ q_{i} $ 为系统的变量，$ F_{i} $ 为作用于系统的广义力。1.2.1 单支撑阶段的动力学方程
SS阶段的简化模型如图3所示。右腿(蓝色)是摆动腿，长度为
$ L_0 $ ，与法线夹角为β，因为在此阶段不需要对右腿进行变长度控制，所以假设为刚性腿。在这个阶段，可控腿长度$ u_{1} $ ，力矩$ \tau_{1} $ 和$ \tau_{2} $ 是控制髋关节质点运动，躯干姿态角和摆动腿夹角的控制输入，系统状态变量为($x,y, \alpha , \beta )^{\rm T}=: {{\boldsymbol{q}}}_{\rm SS}\in {\boldsymbol{Q}}_{\rm SS}$ ，其中α和β的方向定义为顺时针。$$ {\boldsymbol{Q}}_{\rm SS} =\{{{\boldsymbol{q}}}_{\rm SS} \in {\boldsymbol{Q}}_{\rm SS} \vert L_{0} \sin \theta <y<L_{0} \} $$ 首先计算双足机器人系统的动能K和势能P，则拉格朗日函数L可表示为
$$ \begin{split} &L=\dfrac{M+m+m_{\rm f} }{2}\left( {\dot{{x}}^{2}+\dot{{y}}^{2}} \right)+\dfrac{1}{2}I\dot{{\alpha }}^{2}+{}\\ &\qquad \dfrac{m}{2}\left( {l^{2}\dot{{\alpha }}^{2}+2l\dot{{x}}\dot{{\alpha }}\cos \alpha -2l\dot{{y}}\dot{{\alpha }}\sin \alpha } \right) +{}\\ &\qquad \dfrac{m_{\rm f} }{2}\left( {L_{0}^{2}\dot{{\beta }}^{2}+2L_{0} \dot{{x}}\dot{{\beta }}\cos \beta +2L_{0} \dot{{y}}\dot{{\beta }}\sin \beta } \right)-{}\\ &\qquad Mgy-mg\left( {y+l\cos \alpha } \right)-m_{\rm f} g\left( {y-L_{0} \cos \beta } \right)-{}\\ &\qquad \dfrac{1}{2}k_{0} \left[ {L_{0} -\left( {L_{1} +u_{1} } \right)} \right]^{2} \end{split} $$ (3) 其中g是重力加速度。
其次，将式(3)代入式(2)，则SS阶段的动力学方程为
$$ \begin{split} &(M+m+m_{\rm f} )\ddot{{x}}+ml\ddot{{\alpha }}\cos \alpha -ml\dot{{\alpha }}^{2}\sin \alpha +{}\\& \qquad m_{\rm f} L_{0} \ddot{{\beta }}\cos \beta -m_{\rm f} L_{0} \dot{{\beta }}^{2}\sin \beta-{}\\ &\qquad k_{0} \left(\dfrac{L_{0} -u_{1} }{L_{1} }-1\right)x=F_{1} \end{split} $$ (4) $$ \begin{split} & (M+m+m_{\rm f} )\ddot{{y}}-ml\ddot{{\alpha }}\sin \alpha -ml\dot{{\alpha }}^{2}\cos \alpha +{}\\& \qquad m_{\rm f} L_{0} \ddot{{\beta }}\sin \beta +m_{\rm f} L_{0} \dot{{\beta }}^{2}\cos \beta +{}\\ &\qquad(M+m+m_{\rm f} )g -{}\\& \qquad k_{0} \left(\dfrac{L_{0} -u_{1} }{L_{1} }-1\right)y=F_{2} \end{split} $$ (5) $$ \begin{split} &(I+ml^{2})\ddot{{\alpha }}+ml(\ddot{{x}}\cos \alpha -\ddot{{y}}\sin \alpha )-{}\\ &\qquad mgl\sin \alpha =F_{3} \end{split} $$ (6) $$ \begin{split} & m_{\rm f} L_{0}^{2}\ddot{{\beta }}+m_{\rm f} L_{0} (\ddot{{x}}\cos \beta +\ddot{{y}}\sin \beta )+{}\\ &\qquad m_{\rm f} gL_{0} \sin \beta =F_{4} \end{split} $$ (7) 其中
$ L_{1} =\sqrt {x^{2}+y^{2}} $ ，为左腿的长度。最后，根据虚功原理，计算广义力得
$$ \left.\begin{array}{l} F_{1} =\dfrac{y}{x^{2}+y^{2}}\tau_{1} \\ F_{2} =-\dfrac{x}{x^{2}+y^{2}}\tau_{1} \\ F_{3} =-\tau_{1} -\tau_{2} \\ F_{4} =-\tau_{2} \\ \end{array}\right\} $$ (8) 则动力学方程用矩阵形式可表示为
$$ \begin{split} & {\boldsymbol{M}} _{\rm SS} \ddot{\boldsymbol{q}}_{\rm SS}+ {\boldsymbol{C}} _{\rm SS} \dot{\boldsymbol{q}}_{\rm SS}- {\boldsymbol{F}} _{\rm SS}+ {\boldsymbol{G}} _{\rm SS}={}\\&\qquad{\boldsymbol{B}} _{\rm SS1} \tau_{1}+ {\boldsymbol{B}} _{\rm SS2} \tau_{2}+ {\boldsymbol{B}} _{\rm SS3} u_{1} \end{split} $$ (9) 其中
$$ \begin{split} &{{\boldsymbol{M}}_{{\rm{SS}}}} = \left[ {\begin{array}{*{20}{c}} {M + m + {m_{\rm{f}}}}&0&{ml\cos \alpha }&{{m_{\rm{f}}}{L_0}\cos \beta }\\ 0&{M + m + {m_{\rm{f}}}}&{ - ml\sin \alpha }&{{m_{\rm{f}}}{L_0}\sin \beta }\\ {ml\cos \alpha }&{ - ml\sin \alpha }&{m{l^2} + I}&0\\ {{m_{\rm{f}}}{L_0}\cos \beta }&{{m_{\rm{f}}}{L_0}\sin \beta }&0&{{m_{\rm{f}}}{L_0}^2} \end{array}} \right],\;\;{{\boldsymbol{C}}_{{\rm{SS}}}} = \left[ {0\;\;\begin{array}{*{20}{c}} { - ml\dot \alpha \sin \alpha }&{ - {m_{\rm{f}}}{L_0}\dot \beta \sin \beta }\\ { - ml\dot \alpha \cos \alpha }&{{m_{\rm{f}}}{L_0}\dot \beta \cos \beta }\\ 0&0\\ 0&0 \end{array}} \right]\\ &{{\boldsymbol{G}}_{{\rm{SS}}}} = \left[ {\begin{array}{*{20}{c}} 0\\ {\left( {M + m + {m_{\rm{f}}}} \right)g}\\ { - mgl\sin \alpha }\\ {{m_{\rm{f}}}g{L_0}\sin \beta } \end{array}} \right],\;\;{{\boldsymbol{F}}_{{\rm{SS}}}} = {k_0}\left( {\frac{{{L_0}}}{{{L_1}}} - 1} \right)\left[ {\begin{array}{*{20}{l}} x\\ y\\ 0\\ 0 \end{array}} \right],\;\;{{\boldsymbol{B}}_{{\rm{SS1}}}} = \left[ {\begin{array}{*{20}{c}} {y/({x^2} + {y^2})}\\ { - x/({x^2} + {y^2})}\\ { - 1}\\ 0 \end{array}} \right]\\ &{{\boldsymbol{B}}_{{\rm{SS2}}}} = \left[ {\begin{array}{*{20}{c}} 0\\ 0\\ { - 1}\\ { - 1} \end{array}} \right],\;\;{{\boldsymbol{B}}_{{\rm{SS3}}}} = \left[ {\begin{array}{*{20}{c}} { - \dfrac{{{k_0}}}{{{L_1}}}}\\ 0 \end{array}} \right]\left[ {\begin{array}{*{20}{l}} x\\ y\\ 0\\ 0 \end{array}} \right] \end{split}$$ 1.2.2 双支撑阶段的动力学方程
DS阶段的简化模型如图4所示。此时双腿接触地面，由于右腿与法线夹角β可以通过髋关节位置和脚触地点计算得到，因此系统的状态变量为(
$x, y, \alpha)^{\rm T}=: {{\boldsymbol{q}}}_{\rm DS}\in {\boldsymbol{Q}}_{\rm DS}$ 。在这个阶段中，可控腿长度$ u_{1} $ ，$ u_{2} $ 和力矩$ \tau _{1} $ 是控制髋关节运动和躯干姿态角的输入。其中：${\boldsymbol{Q}}_{\rm DS} =\{{{\boldsymbol{q}}}_{\rm DS} \in {\boldsymbol{Q}}_{\rm DS} \vert 0 < y\leqslant L_{0} \sin \theta \}$ 采用与SS阶段相同的推导方法，得到DS阶段的动力学方程
$$ \begin{split}& {\boldsymbol{M}} _{\rm DS} \ddot{\boldsymbol{q}}_{\rm DS}+ {\boldsymbol{C}} _{\rm DS} \dot{\boldsymbol{q}}_{\rm DS}- {\boldsymbol{F}} _{\rm DS 1}- {\boldsymbol{F}} _{\rm DS 2}+ {\boldsymbol{G}} _{\rm DS} ={}\\ &\qquad {\boldsymbol{B}} _{\rm DS 1} \tau_{1}+ {\boldsymbol{B}} _{\rm DS 2} u_{1}+ {\boldsymbol{B}} _{\rm DS 3} u_{2} \end{split} $$ (10) 其中
$$ {{{\boldsymbol{M}}_{{\rm{DS}}}} = \left[ {\begin{array}{*{20}{c}} {M + m}&0&{ml\cos \alpha }\\ 0&{M + m}&{ - ml\sin \alpha }\\ {ml\cos \alpha }&{ - ml\sin \alpha }&{m{l^2} + I} \end{array}} \right]}$$ $$ \begin{array}{l}{{{\boldsymbol{C}}_{{\rm{DS}}}} = \left[ {\begin{array}{*{20}{c}} { - ml\dot \alpha \sin \alpha }\\ { - ml\dot \alpha \cos \alpha }\\ 0 \end{array}} \right]} \end{array} $$ $$ \begin{split} &{{{\boldsymbol{F}}_{{\rm{DS1}}}} = {k_0}\left( {\dfrac{{{L_0}}}{{{L_1}}} - 1} \right)\left[ {\begin{array}{*{20}{l}} x\\ y\\ 0 \end{array}} \right]}\\ &{{{\boldsymbol{F}}_{{\rm{DS2}}}} = {k_0}\left( {\dfrac{{{L_0}}}{{{L_2}}} - 1} \right)\left[ {\begin{array}{*{20}{c}} {x - a}\\ y\\ 0 \end{array}} \right]}\\ &{{{\boldsymbol{G}}_{{\rm{DS}}}} = \left[ {\begin{array}{*{20}{c}} 0\\ {(M + m)g}\\ { - mgl\sin \alpha } \end{array}} \right]}\\ &{{{\boldsymbol{B}}_{{\rm{DS1}}}} = \left[ {\begin{array}{*{20}{c}} {\dfrac{y}{{{x^2} + {y^2}}}}\\ { - \dfrac{x}{{{x^2} + {y^2}}}}\\ { - 1} \end{array}} \right],{{\boldsymbol{B}}_{{\rm{DS2}}}} = - \dfrac{{{k_0}}}{{{L_1}}}\left[ {\begin{array}{*{20}{l}} x\\ y\\ 0 \end{array}} \right]}\\ &{{{\boldsymbol{B}}_{{\rm{DS3}}}} = - \dfrac{{{k_0}}}{{{{L}_2}}}\left[ {\begin{array}{*{20}{c}} {x - a}\\ y\\ 0 \end{array}} \right],\;\;{{{L}}_2} = \sqrt {{{(x - a)}^2} + {y^2}} } \end{split} $$ 其中
$ L_{2} $ 为右腿的长度，a为步长，$ a=x+ L_0\cos\theta $ 。由式(9)和式(10)可知，系统在SS阶段为欠驱动模型，在DS阶段为全驱动模型，但总的来说，系统是欠驱动系统。
1.3 状态空间表达式
为了便于后续的反馈线性化控制器的设计，将动力学方程转换为标准的状态空间表达式。在SS阶段，通过引入
$$ {{\boldsymbol{z}}}_{\rm SS} :=\left( {x,y,\alpha ,\beta ,\dot{{x}},\dot{{y}},\dot{{\alpha }},\dot{{\beta }}} \right)^{\rm T}=\left[ {{\begin{array}{*{20}c} {{\boldsymbol{q}}_{\rm SS} } \\ {{\dot{{\boldsymbol{q}}}}_{\rm SS} } \\ \end{array} }} \right] $$ 式(9)的等效状态方程为
$$ \begin{split} & {\dot{{{\boldsymbol{z}}}}}_{\rm SS} ={{\boldsymbol{f}}}\left( {{{\boldsymbol{z}}}_{\rm SS} } \right)+{{\boldsymbol{g}}}_{\tau_{1} } \left( {{{\boldsymbol{z}}}_{\rm SS} } \right)\tau_{1} +{{\boldsymbol{g}}}_{\tau_{2} } \left( {{{\boldsymbol{z}}}_{\rm SS} } \right)\tau_{2} +{}\\&\qquad{{\boldsymbol{g}}}_{1} \left( {{{\boldsymbol{z}}}_{\rm SS} } \right)u_{1} \end{split} $$ (11) 其中
$$\boldsymbol{f}\left(\boldsymbol{z}_{\mathrm{SS}}\right)=\left[\begin{array}{c} \dot{\boldsymbol{q}}_\mathrm{SS} \\ \boldsymbol{M}_{\mathrm{SS}}^{-1}\left(-\boldsymbol{C}_{\mathrm{SS}} \dot{\boldsymbol{q}}+\boldsymbol{F}_{\mathrm{SS}}-\boldsymbol{G}_{\mathrm{SS}}\right) \end{array}\right] $$ $$\begin{split} &\boldsymbol{g}_{\tau_{1}}\left(\boldsymbol{z}_{\mathrm{SS}}\right)=\left[\begin{array}{c} {\boldsymbol{0}} \\ \boldsymbol{M}_{\mathrm{SS}}^{-1} \boldsymbol{B}_{\mathrm{SS} 1} \end{array}\right] \\ &\boldsymbol{g}_{\tau_{2}}\left(\boldsymbol{z}_{\mathrm{SS}}\right)=\left[\begin{array}{c} {\boldsymbol{0}} \\ \boldsymbol{M}_{\mathrm{SS}}^{-1} \boldsymbol{B}_{\mathrm{SS} 2} \end{array}\right] \\ &\boldsymbol{g}_{1}\left(\boldsymbol{z}_{\mathrm{SS}}\right)=\left[\begin{array}{c} {\boldsymbol{0}} \\ \boldsymbol{M}_{\mathrm{SS}}^{-1} \boldsymbol{B}_{\mathrm{SS} 3} \end{array}\right] \end{split}$$ 为了简洁起见，省略参数
$ {\boldsymbol{z}}_{\rm SS} $ ，则SS阶段的状态方程可以简写为$$ {\dot{{{\boldsymbol{z}}}}}_{\rm SS} ={{\boldsymbol{f}}}+{{\boldsymbol{g}}}_{\tau_{1} } \tau_{1} +{{\boldsymbol{g}}}_{\tau_{2} } \tau_{2} +{{\boldsymbol{g}}}_{1} u_{1} $$ (12) 类似地，在DS阶段，通过引入
$$ {{\boldsymbol{z}}}_{\rm DS} :=\left( {x,y,\alpha ,\dot{{x}},\dot{{y}},\dot{{\alpha }}} \right)^{\rm T}=\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{q}}_{{\rm{DS}}}}}\\ {{{\mathop {\boldsymbol{q}}\limits^. }_{{\rm{DS}}}}} \end{array}} \right] $$ 可得
$$ \begin{split} &{\dot{{{{\boldsymbol{z}}}}}}_{{\rm DS}} ={{{{\boldsymbol{f}}}}}\left( {{{{\boldsymbol{z}}}}_{{\rm DS}} } \right)+{{{{\boldsymbol{g}}}}}_{\tau_{1} } \left( {{{{\boldsymbol{z}}}}_{{\rm DS}} } \right)\tau_{1} +{{{{\boldsymbol{g}}}}}_{1} \left( {{{{\boldsymbol{z}}}}_{{\rm DS}} } \right)u_{1} +{}\\& \qquad {{{{\boldsymbol{g}}}}}_{2} \left( {{{{\boldsymbol{z}}}}_{{\rm DS}} } \right)u_{2} \end{split} $$ (13) 其中，
$$\begin{split} &\boldsymbol{f}\left(\boldsymbol{z}_{\mathrm{DS}}\right)=\left[\begin{array}{c} \dot{\boldsymbol{q}}_{\mathrm{DS}} \\ \boldsymbol{M}_{\mathrm{DS}}^{-1}\left(\boldsymbol{F}_{\mathrm{DS} 1}+\boldsymbol{F}_{\mathrm{DS} 2}-\boldsymbol{G}_{\mathrm{DS}}-C_{\mathrm{DS}} \dot{\boldsymbol{q}}\right) \end{array}\right] \\ &\boldsymbol{g}_{\tau_{1}}\left(\boldsymbol{z}_{\mathrm{DS}}\right)=\left[\begin{array}{c} {\boldsymbol{0}} \\ \boldsymbol{M}_{\mathrm{DS}}^{-1} \boldsymbol{B}_{\mathrm{DS} 1} \end{array}\right] \\ &\boldsymbol{g}_{1}\left(\boldsymbol{z}_{\mathrm{DS}}\right)=\left[\begin{array}{c} {\boldsymbol{0}} \\ \boldsymbol{M}_{\mathrm{DS}}^{-1} \boldsymbol{B}_{\mathrm{DS} 2} \end{array}\right] \\ &\boldsymbol{g}_{2}\left(\boldsymbol{z}_{\mathrm{DS}}\right)=\left[\begin{array}{c} {\boldsymbol{0}} \\ \boldsymbol{M}_{\mathrm{DS}}^{-1} \boldsymbol{B}_{\mathrm{DS} 3} \end{array}\right] \end{split}$$ 同理，DS阶段的状态方程可以简写为
$$ {\dot{{{\boldsymbol{z}}}}}_{{\rm DS}} ={{\boldsymbol{f}}}+{{\boldsymbol{g}}}_{\tau_{1} } \tau _{1} +{{\boldsymbol{g}}}_{1} u_{1} +{{\boldsymbol{g}}}_{2} u_{2} $$ (14) 2. 控制器设计
2.1 问题陈述
在本节中，我们提出了三个需要完成的任务：(a) 跟踪髋关节质点的期望高度
$ y^{\ast }(x) $ 和期望水平速度$ \dot{x}^{\ast }(x) $ ；(b) 跟踪SS阶段摆动腿运动的参考轨迹$ \beta^{\ast }(x) $ ；(c) 保持躯干直立。由于水平位置x随时间单调递增，因此这里采用x替代时间t作为自变量，即采用空间轨迹方式。定义
$$ \begin{split} z =& {({{\boldsymbol{z}}_1},{z_2},{z_3},{z_4},{z_5},{z_6},{z_7},{z_8})^{\rm{T}}} =\\& {({q_1},{q_2},{q_3},{q_4},{\dot q_1},{\dot q_2},{\dot q_3},{\dot q_4})^{\rm{T}}} =\\& {({\boldsymbol{q}},\dot {\boldsymbol{q}})^{\rm{T}}} = {(x,y,\alpha ,\beta ,\dot x,\dot y,\dot \alpha ,\dot \beta )^{\rm{T}}}\ \ {{\boldsymbol{z}}}\in {{\boldsymbol{z}}}_{\rm SS} \cup {{\boldsymbol{z}}}_{\rm DS} \end{split} $$ 期望轨迹可以表示为
$$ \begin{array}{l} z^{\ast }_{2} \left( {z_{1} } \right):=y^{\ast }(x) \\ z^{\ast }_{3} \left( {z_{1} } \right):=\alpha^{\ast }(x) \\ z^{\ast }_{4} \left( {z_{1} } \right):=\beta^{\ast }(x) \\ z^{\ast }_{5} \left( {z_{1} } \right):=\dot{{x}}^{\ast }(x) \\ \end{array} $$ 为了避免符号混乱，省略函数参数
$ z_{1} $ 。定义控制目标的误差函数为$$ \begin{array}{l} e_{p} =z_{2} -z_{2}^{\ast } \\ e_{\alpha } =z_{3} -z_{3}^{\ast } \\ e_{\beta } =z_{4} -z_{4}^{\ast } \\ e_{v} =z_{5} -z_{5}^{\ast } \\ \end{array} $$ 由于系统在SS阶段处于欠驱动状态，所以水平方向速度误差
$ e_{v} $ 在SS阶段是不受控制的。在DS阶段，两脚都与地面接触，所以误差$ e_{\beta} $ 不需要控制。控制的目标是使误差尽可能地收敛到0的邻域，即
$$ \left. \begin{array}{l} \mathop {\lim }\limits_{x \to \infty } \left| {{e_p}} \right| < {\varepsilon _p}\\ \mathop {\lim }\limits_{x \to \infty } \left| {{e_\alpha }} \right| < {\varepsilon _\alpha }\\ \mathop {\lim }\limits_{x \to \infty } \left| {{e_\beta }} \right| < {\varepsilon _\beta }\\ \mathop {\lim }\limits_{x \to \infty } \left| {{e_v}} \right| < {\varepsilon _v} \end{array} \right\} $$ (15) 其中
$ \varepsilon_{p} $ ，$ \varepsilon_{\alpha } $ ，$ \varepsilon_{\beta } $ 和$ \varepsilon_{v} $ 为大于0的足够小的数。2.2 反馈线性化控制
在本节中，通过反馈线性化方法来控制腿部长度和髋关节力矩，使机器人的行走能够稳定到期望的步态。
2.2.1 单支撑阶段
在SS阶段，控制变量为髋关节高度轨迹y，躯干姿态角α，摆动腿夹角β。则系统的状态空间表达式为
$${\dot{{{\boldsymbol{z}}}}}_{\rm SS} ={{\boldsymbol{f}}}+{{\boldsymbol{g}}}_{\tau_{1} } \tau _{1} +{{\boldsymbol{g}}}_{\tau_{2} } \tau_{2} +{{\boldsymbol{g}}}_{1} u_{1}\;\;{\text{（状态方程）}} $$ $$ \left. {\begin{array}{l} e_{p} =z_{2} -z_{2}^{\ast } \\ e_{\alpha } =z_{3} -z_{3}^{\ast } \\ e_{\beta } =z_{4} -z_{4}^{\ast } \\ \end{array}} \right\}\ \ {\text{（输出方程）}} $$ 为了将该非线性系统转换为标准线性形式，首先需要确定误差的关系度
$ \boldsymbol \gamma_{\rm SS}=\{\gamma_{p},\gamma_{\alpha },\gamma_{\beta }\} $ 。对于位置误差$$ \begin{split} & \dot{{e}}_{p} =\dfrac{\partial e_{p} }{\partial {{\boldsymbol{z}}}}{\dot{{{\boldsymbol{z}}}}}_{\rm SS} =\left( \begin{array}{*{20}c} \dfrac{\partial e_{p} }{\partial {\boldsymbol{q}}} \dfrac{\partial e_{p} }{\partial {\dot{{\boldsymbol{q}}}}} \end{array} \right){}\\\\&\left( {{\boldsymbol{f}}}+{{\boldsymbol{g}}}_{\tau_{1} } \tau_{1} +{{\boldsymbol{g}}}_{\tau_{2} } \tau_{2} +{{\boldsymbol{g}}}_{1} u_{1} \right) =\\&\underbrace{\left( \begin{array}{*{20}c} \dfrac{\partial e_{p}}{\partial {\boldsymbol{q}}} & 0 \end{array} \right)\left[ \begin{array}{*{20}c} {\dot{\boldsymbol{q}}} \\ {{\boldsymbol{M}}}_{\rm SS}^{-1}\left({{\boldsymbol{F}}}_{\rm SS} -{{\boldsymbol{G}}}_{\rm SS} -C_{\rm SS} {\dot{\boldsymbol{q}}} \right) \\ \end{array}\right]}_{L_{\rm f} e_{p}} + \end{split} $$ $$ \begin{split} &\underbrace{\left(\begin{array}{*{20}c} \dfrac{\partial e_{p}}{\partial {\boldsymbol{q}}} & 0 \end{array} \right)\left[\begin{array}{*{20}c} {{0\mathit{\boldsymbol{}}}} \\ {{\boldsymbol{M}}}_{\rm SS}^{-1}{{\boldsymbol{B}}}_{\rm SS1} \\ \end{array} \right]}_{L_{g\tau_{1}} e_{p} =0}\tau_{1} + \underbrace{\left(\begin{array}{*{20}c} \dfrac{\partial e_{p} }{\partial {\boldsymbol{q}}} & 0 \\ \end{array} \right)\left[ \begin{array}{*{20}c} {{0\mathit{\boldsymbol{}}}} \\ {{\boldsymbol{M}}}_{\rm SS}^{-1}{{\boldsymbol{B}}}_{\rm SS2} \\ \end{array} \right]}_{L_{g\tau_{2} } e_{p} =0}\tau_{2} + \\&\underbrace{\left(\begin{array}{*{20}c} \dfrac{\partial e_{p}}{\partial {\boldsymbol{q}}} & 0 \\ \end{array} \right)\left[ \begin{array}{*{20}c} {{0\mathit{\boldsymbol{}}}} \\ {{\boldsymbol{M}}}_{\rm SS}^{-1}{{\boldsymbol{B}}}_{\rm SS3} \\ \end{array} \right]}_{L_{g_{1}} e_{p} =0}u_{1} =\\& L_{\rm f} e_{p} ({\boldsymbol{q}},{\dot{{\boldsymbol{q}}}}) \end{split} $$ (16) 其中
$ L_{g\tau_{1} } e_{p} =L_{g\tau_{2} } e_{p} =L_{g_{1} } e_{p} =0 $ ，继续计算二阶导数$$ \begin{split}& \ddot{{e}}_{p} =\left(\begin{array}{*{20}c} \dfrac{\partial}{\partial {\boldsymbol{q}}}\left(\dfrac{\partial e_{p} }{\partial {\boldsymbol{q}}}{\dot{{\boldsymbol{q}}}} \right) & \dfrac{\partial e_{p} }{\partial {\boldsymbol{q}}} \end{array} \right)\left( {{\boldsymbol{f}}}+{{\boldsymbol{g}}}_{\tau_{1} } \tau_{1} +{{\boldsymbol{g}}}_{\tau_{2} } \tau_{2} +{{\boldsymbol{g}}}_{1} u_{1} \right) ={}\\ &\qquad \underbrace{\left( \begin{array}{*{20}c} \dfrac{\partial}{\partial {\boldsymbol{q}}}\left( \dfrac{\partial e_{p} }{\partial {\boldsymbol{q}}}{\dot{{\boldsymbol{q}}}} \right) & \dfrac{\partial e_{p} }{\partial {\boldsymbol{q}}} \\ \end{array} \right)\left[ \begin{array}{*{20}c} \dot{\boldsymbol{q}}\\ {{\boldsymbol{M}}}_{\rm SS}^{-1}\left({{\boldsymbol{F}}}_{\rm SS} -{{\boldsymbol{G}}}_{\rm SS} -{\boldsymbol{C}}_{\rm SS} \dot{\boldsymbol{q}} \right) \end{array} \right]}_{L_{\rm f}^{2}e_{p}} +{}\\ &\qquad\underbrace{\left( \begin{array}{*{20}c} \dfrac{\partial }{\partial {\boldsymbol{q}}}\left( \dfrac{\partial e_{p} }{\partial {\boldsymbol{q}}}{\dot{{\boldsymbol{q}}}} \right) & \dfrac{\partial e_{p} }{\partial {\boldsymbol{q}}} \\ \end{array} \right)\left[ \begin{array}{*{20}c} {{0\mathit{\boldsymbol{}}}} \\ {{\boldsymbol{M}}}_{\rm SS}^{-1}{{\boldsymbol{B}}}_{\rm SS1} \\ \end{array} \right]}_{L_{g\tau_{1} } L_{\rm f} e_{p}} \tau_{1} +\underbrace{\left( \begin{array}{*{20}c} \dfrac{\partial }{\partial {\boldsymbol{q}}}\left( \dfrac{\partial e_{p} }{\partial {\boldsymbol{q}}}{\dot{{\boldsymbol{q}}}} \right) & \dfrac{\partial e_{p} }{\partial {\boldsymbol{q}}} \end{array} \right)\left[ \begin{array}{*{20}c} {{0\mathit{\boldsymbol{}}}} \\ {{\boldsymbol{M}}}_{\rm SS}^{-1}{{\boldsymbol{B}}}_{2} \\ \end{array} \right]}_{L_{g\tau_{2} } L_{\rm f} e_{p}} \tau_{2} +{}\\ &\qquad\underbrace{\left( \begin{array}{*{20}c} \dfrac{\partial }{\partial {\boldsymbol{q}}}\left( \dfrac{\partial e_{p} }{\partial {\boldsymbol{q}}}{\dot{{\boldsymbol{q}}}} \right) & \dfrac{\partial e_{p} }{\partial {\boldsymbol{q}}} \\ \end{array} \right)\left[ \begin{array}{*{20}c} {{0\mathit{\boldsymbol{}}}} \\ {{\boldsymbol{M}}}_{\rm SS}^{-1}{{\boldsymbol{B}}}_{\rm SS3} \\ \end{array} \right]}_{L_{g1} L_{\rm f} e_{p}} u_{1} ={}\\ &\qquad L^{2}_{\rm f} e_{p} ({\boldsymbol{q}},{\dot{{\boldsymbol{q}}}})+L_{g\tau_{1} } L{ }_{\rm f}e_{p} ({\boldsymbol{q}})\tau_{1} +L_{g\tau_{2} } L{ }_{\rm f}e_{p} ({\boldsymbol{q}})\tau_{2} +L_{g1} L_{\rm f}e_{p} ({\boldsymbol{q}})u_{1} \end{split} $$ (17) 其中
$ L_{g\tau_{1}} L_{\rm f} e_{p} \ne 0 $ ，$ L_{g\tau_{2}} L_{\rm f} e_{p} \ne 0 $ ，$ L_{g_{1}} L_{\rm f} e_{p} \ne 0 $ 。所以关系度
$ \gamma_{p}=2 $ ，同理可得$ \gamma_{\alpha } $ 和$ \gamma_{\beta } $ 的关系度也为2。因此，在SS阶段中，误差的关系度$ {\boldsymbol{ \gamma}}_{\rm SS}= \{ \gamma_{p}, \gamma_{\alpha }, \boldsymbol \gamma_{\beta }\}=\{ 2,2,2\} $ 。根据反馈线性化原理，可得
$$\left[ {\begin{array}{*{20}{c}} {{{\mathop e\limits^{..} }_p}}\\ {{{\mathop e\limits^{..} }_\alpha }}\\ {{{\mathop e\limits^{..} }_\beta }} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {L_{\rm{f}}^2{e_p}}\\ {L_{\rm{f}}^2{e_\alpha }}\\ {L_{\rm{f}}^2{e_\beta }} \end{array}} \right] + {{\boldsymbol{A}}_{{\rm{SS}}}}\left[ {\begin{array}{*{20}{c}} {{\tau _1}}\\ {{\tau _2}}\\ {{u_1}} \end{array}} \right] $$ (18) 其中
$$ {{\boldsymbol{A}}}_{\rm SS} =\left[ {{\begin{array}{*{20}c} {L_{g\tau_{1} } L_{\rm f} e_{p} } & {L_{g\tau_{2} } L_{\rm f} e_{p} } & {L_{g1} L_{\rm f} e_{p} } \\ {L_{g\tau_{1} } L_{\rm f} e_{\alpha } } & {L_{g\tau_{2} } L_{\rm f} e_{\alpha } } & {L_{g1} L_{\rm f} e_{\alpha } } \\ {L_{g\tau_{1} } L_{\rm f} e_{\beta } } & {L_{g\tau_{2} } L_{\rm f} e_{\beta } } & {L_{g1} L_{\rm f} e_{\beta } } \\ \end{array} }} \right] $$ 由于ASS是可逆的，所以反馈控制律定义为
$$\mathop {\left[ {\begin{array}{*{20}{c}} {{\tau _1}}\\ {{\tau _2}}\\ {{u_1}} \end{array}} \right]}\limits_{{u_2} \equiv 0} = {\boldsymbol{A}}_{{\rm{SS}}}^{ - 1}\left[ {\begin{array}{*{20}{c}} { - L_{\rm{f}}^2{e_p} + {v_1}}\\ { - L_{\rm{f}}^2{e_\alpha } + {v_2}}\\ { - L_{\rm{f}}^2{e_\beta } + {v_3}} \end{array}} \right] $$ (19) 其中
$ v_{1} $ ,$ v_{2} $ 和$ v_{3} $ 是新的控制输入。本文采用比例微分控制$$ \left.\begin{array}{l} v_{1} =-k_{d\tau_{1}s} \dot{{e}}_{p} -k_{p\tau_{1}s} e_{p} \\ v_{2} =-k_{d\tau_{2}s} \dot{{e}}_{\alpha } -k_{p\tau_{2}s} e_{\beta } \\ v_{3} =-k_{\rm DS} \dot{{e}}_{\beta } -k_{ps} e_{\beta } \\ \end{array} \right\} $$ (20) 因此
$$ \begin{split} \\ \mathop {\left[ {\begin{array}{*{20}{c}} {{\tau _1}}\\ {{\tau _2}}\\ {{u_1}} \end{array}} \right]}\limits_{{u_2} \equiv 0} = {\boldsymbol{A}}_{{\rm{SS}}}^{ - 1}\left[ {\begin{array}{*{20}{c}} { - L_{\rm{f}}^2{e_p} - {k_{d{\tau _1}s}}{{\mathop e\limits^. }_p} - {k_{p{\tau _1}s}}{e_p}}\\ { - L_{\rm{f}}^2{e_\alpha } - {k_{d{\tau _2}s}}{{\mathop e\limits^. }_\alpha } - {k_{p{\tau _2}s}}{e_\alpha }}\\ { - L_{\rm{f}}^2{e_\beta } - {k_{{\rm{DS}}}}{{\mathop e\limits^. }_\beta } - {k_{ps}}{e_\beta }} \end{array}} \right] \end{split} $$ (21) 根据式（18）和式（21）可得，误差动力学方程为
$$ \left.\begin{array}{l} \ddot{{e}}_{p} +k_{d\tau_{1} s} \dot{{e}}_{p} +k_{p\tau_{1} s} e_{p} =0 \\ \ddot{{e}}_{\alpha } +k_{d\tau_{2} s} \dot{{e}}_{\alpha } +k_{p\tau_{2} s} e_{\alpha } =0 \\ \ddot{{e}}_{\beta } +k_{\rm DS} \dot{{e}}_{\beta } +k_{ps} e_{\beta } =0 \\ \end{array}\right\} $$ (22) 其中
$ k_{d\tau_{1} s} $ ，$ k_{p\tau_{1} s} $ ，$ k_{d\tau_{2} s} $ ，$ k_{p\tau_{2} s} $ ，$ k_{\rm DS} $ 和$ k_{ps} $ 为可调控制器系数。2.2.2 双支撑阶段
在DS阶段，控制变量为髋关节高度y，躯干姿态角α和前进速度
$ \dot{x} $ 。则系统的状态空间表达式为$$ \begin{array}{l} {\dot{{{\boldsymbol{z}}}}}_{{\rm DS}} ={{\boldsymbol{f}}}+{{\boldsymbol{g}}}_{\tau_{1} } \tau _{1} +{{\boldsymbol{g}}}_{1} u_{1} +{{\boldsymbol{g}}}_{2} u_{2} \\ e_{p} =z_{2} -z_{2}^{\ast } \\ e_{\alpha } =z_{3} -z_{3}^{\ast } \\ e_{v} =z_{5} -z_{5}^{\ast } \\ \end{array} $$ 采用与SS阶段相同的方法，可以得到关系度
$ \boldsymbol \gamma_{\rm DS}=\{\gamma_{p}, \gamma_{\alpha }, \gamma_{v}\}=\{2,2,1\} $ ，则$$ \left[ \begin{array}{l} {\tau _1}\\ {u_1}\\ {u_2} \end{array} \right] = {\boldsymbol{A}}_{{\rm{DS}}}^{ - 1}\left[ {\begin{array}{*{20}{c}} { - L_{\rm{f}}^2{e_p} - {k_{d{\tau _1}d}}{{\mathop e\limits^. }_p} - {k_{p{\tau _1}d}}{e_p}}\\ {\begin{array}{*{20}{l}} { - L_{\rm{f}}^2{e_\alpha } - {k_{dd}}{{\mathop e\limits^. }_\alpha } - {k_{pd}}{e_\alpha }}\\ { - {L_{\rm{f}}}{e_4} - {k_{vd}}{e_v}} \end{array}} \end{array}} \right] $$ (23) 其中
$$ {{\boldsymbol{A}}}_{{\rm DS}} =\left[ {{\begin{array}{*{20}c} {L_{g\tau_{1} } L_{\rm f} e_{p} } & {L_{g1} L_{\rm f} e_{p} } & {L_{g2} L_{\rm f} e_{p} } \\ {L_{g\tau_{1} } L_{\rm f} e_{\alpha } } & {L_{g1} L_{\rm f} e_{\alpha } } & {L_{g2} L_{\rm f} e_{\alpha } } \\ {L_{g\tau_{1} } e_{v} } & {L_{g1} e_{v} } & {L_{g2} e_{v} } \\ \end{array} }} \right] $$ (24) 误差动力学方程为
$$ \left.\begin{array}{l} \ddot{{e}}_{p} +k_{d\tau_{1} d} \dot{{e}}_{p} +k_{p\tau_{1} d} e_{p} =0 \\ \ddot{{e}}_{\alpha } +k_{dd} \dot{{e}}_{\alpha } +k_{pd} e_{\alpha } =0 \\ \dot{{e}}_{v} +k_{vd} e_{v} =0 \\ \end{array}\right\} $$ (25) 其中
$ k_{d\tau_{1} d} $ ，$ k_{p\tau_{1} d} $ ，$ k_{dd} $ ，$ k_{pd} $ 和$ k_{vd} $ 是控制参数，比例微分控制用于控制髋关节高度y，躯干姿态角α，比例控制用于控制髋关节前进速度$ \dot{x} $ 。3. 步态切换
为了实现双足机器人能在不同的自然步态之间切换的目标，本节给出了步态切换的优化准则和切换策略。
3.1 生成所需的行走步态
文献[5]表明，在合适的初始条件和系统参数下，SLIP模型的行走表现为一种稳定的被动极限环步态，称为自然步态。在SLIP模型中，假设腿是无质量的弹簧腿，行走时没有能量损失，因此，机器人不需要额外的能量来维持稳定的步态。本文将该理想SLIP模型作为VL-TSLIP-SL模型的参考模型，将其髋关节运动轨迹作为VL-TSLIP-SL模型的期望轨迹。这样做的理由是：如果系统能收敛到该期望轨迹，则所提的控制策略是节能的。一般情况下，对于SLIP模型来说，很难找到解析解[22]，因此我们采用傅里叶级数展开式来拟合期望的自然步态，但自然步态与参考步态之间的微小不匹配是不可避免的。
3.2 步态切换的优化准则
在SLIP模型中，当(
$ \theta ,k_0) $ 值相等时，给定两个不同的初始前进速度，可以生成两个不同的自然步态$ \Sigma i $ 和$ \Sigma j $ ，本文要实现的目标是双足机器人能从$ \Sigma i $ 切换到$ \Sigma j $ 。可以用变量x来确定从一种步态到另一种步态的准确过渡点。因此，在步态$ \Sigma i $ 和$ \Sigma j $ 中需要考虑两点，即$ \Sigma i $ 中执行切换的点$ x_{i} $ 和切换到$ \Sigma j $ 中的点$ x_{j} $ 。目标是找到一个最优组合($ x_{i,\rm opt} $ ,$x _{j,\rm opt}) $ 使准则J最小化。优化准则J (
$ x_{i} $ ,$ x_{j}) $ 定义为$$ \begin{split}& J\left( {x_{i} ,x_{j} } \right)=\mu_{1} \left\| {y_{j} (x_{j} )-y_{i} (x_{i} )} \right\|+{}\\& \qquad\mu_{2}\left\| \dot{{x}}_{j} (x_{j} )-\dot{{x}}_{i} (x_{i} ) \right\|+{}\\& \qquad \mu _{3} \left\| {\dot{{y}}_{j} (x_{j} )-\dot{{y}}_{i} (x_{i} )} \right\| \end{split} $$ (26) 其中，
$ \mu_{1} $ ，$ \mu_{2} $ ，$ \mu_{3} $ 为权重。优化准则J除了包含变量$ \dot{x} $ 和$ \dot{y} $ 外，髋关节高度y也包括在内，这样有利于在两个步态轨迹接近的点进行切换，这样得到的高度位置误差将更小。通过使准则J相对于
$ x_{i} $ 和$ x_{j} $ 最小化，可得最优切换点，即$$ \min\limits_{x_{i} ,x_{j} } J\left( {x_{i},x_{j} } \right)\to \left({x_{i,\rm opt} ,x_{j,\rm opt} } \right) $$ (27) 一旦达到当前步态的最优切换点，系统就可以切换到新的步态。值得注意的是，
$ x_{i,\rm opt} $ 和$ x_{j,\rm opt} $ 应该对齐，以保证双足机器人可以从步态$ \Sigma i $ 的最优点切换到步态$ \Sigma j $ 的最优点。3.3 切换策略
如图5所示，如果双足机器人处于当前步态的SS阶段，则可以切换到新的步态的SS阶段，反之亦然。但从步态
$ \Sigma i $ 的DS阶段到步态$ \Sigma j $ 的SS阶段(或反之)的切换是无效的，因为在这种情况下，当控制器控制切换步态时，机器人的脚需要腾空离地，这显然是不可行的。考虑到机器人整体为欠驱动系统，前进速度
$ \dot{x} $ 仅在DS阶段进行控制，所以步态切换在DS阶段通过改变腿的长度和髋关节力矩来实现。其基本原理是：步态的切换可以被认为是对系统的一个扰动，系统可以通过闭环调整控制输入来抑制此扰动。4. 仿真分析
为了验证本文所提的控制器对双足机器人控制的有效性，在MATLAB环境下进行了仿真分析，系统参数和初始条件如表1所示。首先，从理想SLIP模型中得到期望的自然步态，分别用二阶和六阶傅里叶级数拟合得到参考轨迹
$ z_{2}^{\ast} $ 和$ z_{5}^{\ast} $ 。躯干的期望角度$ z_{3}^{\ast }=0 $ ，摆动腿期望轨迹设计为表 1 系统参数和初始条件Table 1. System parameters and initial conditionsParameters Value hip mass M/kg 15 torso mass $m/{\rm{kg}} $ 3 foot mass $m_{{\rm{f}}}/{\rm{kg}}$ 2.5 spring rest length L0/m 1 torso length $l/{\rm{m}} $ 0.5 gravitational acceleration $g /\left(\mathrm{m} \cdot \mathrm{s}^{-2}\right) $ 9.8 angle of attack θ/(◦) 62.5 moment of inertia I/(kg·m2) 0.25 nominal leg stiffness $k_{0} /\left(\mathrm{N} \cdot \mathrm{m}^{-1}\right) $ 1500 initial hip horizontal position $x(0) / \mathrm{m} $ 0 initial hip vertical position $y(0) / \mathrm{m} $ 1 initial torso angle α (0)/rad 0 initial swing leg angle $\beta(0) / \mathrm{rad} $ 0 initial forward velocity $\dot{x}(0) /\left(\mathrm{m}^{\cdot} \mathrm{s}^{-1}\right) $ 1.074 used for gait 1 $\dot{x}_{1}(0) /\left(\mathrm{m} \cdot \mathrm{s}^{-1}\right) $ 1.032 used for gait 2 $\dot{x}_{2}(0) /\left(\mathrm{m} \cdot \mathrm{s}^{-1}\right) $ 1.239 initial vertical velocity $\dot{y}(0) /\left(\mathrm{m} \cdot \mathrm{s}^{-1}\right) $ 0 initial angle velocity of the torso/(rad·s–1) 0 initial angle velocity of the swing leg/(rad·s–1) 0.1 step size $a / \mathrm{m} $ 0.644 3 $$ \begin{split}& z_{4}^{\ast }=b_{1} +b_{2} \sin [9.747z_{1} -154.334\,0z_{1} ^{3}+{\nonumber}\\ &\qquad 733.117\,5z_{1}^{5}+o(z_{1}^{5})] \end{split} $$ 其中
$ b_{1}=b_{2}=0.24 $ 。取单支撑阶段的控制参数为$ k_{d\tau_{1} s}=100 $ ，$ k_{d\tau_{2} s} =50 $ ，$ k_{p\tau_{1} s} =100 $ ，$ k_{p\tau_{2} s} = 100 $ ，$ k_{ps} =100 $ ，$ k_{\rm DS} =50 $ ，双支撑阶段的控制参数为$ k_{p\tau_{1} d} =50 $ ，$ k_{d\tau_{1} d} =100 $ ，$ k_{pd} =100 $ ，$ k_{dd} =50 $ ，$ k_{vd}=100 $ 。系统仿真20步，运行情况如下。4.1 步态分析
图6给出的是髋关节高度y，躯干姿态角α和摆动腿夹角β的轨迹图。可以看出，所提出的控制器能够维持稳定的周期步态，并保持躯干直立。从图中可以看出：由于SLIP模型动力学是受弹性力作用的二阶系统，稳定自然步态的解是周期的，因此髋关节的运动轨迹也呈现周期性变化趋势。从图7中可以看出，得到的误差曲线收敛于零的邻域，在DS阶段，因为摆动腿角度β在此阶段没有定义，所以
$ e_{\beta }\equiv0 $ 。此外，如前所述，系统在SS阶段处于欠驱动状态，仅在DS阶段控制水平速度，因此，误差$ e_{v} $ 在DS阶段趋于零，而在单支撑阶段，存在一定的误差。![]()
图 6 髋关节高度、躯干姿态角和摆动腿夹角轨迹Figure 6. Hip trajectories, the torso angle, and the swing leg orientation图8给出了稳定状态下的控制输入变化曲线，灰色部分为DS阶段。黑色虚线对应腿原长度(
$ L=1 $ m)。理想的SLIP参考模型可以在没有任何控制输入的情况下(即$ u_{1}=u_{2}=0 $ )维持稳定的步态。本文提出的VL-TSLIP-SL模型包括躯干姿态和摆动腿控制，这些非理想动态都被认为是系统的外部扰动，因此需要较大的控制输入来抑制这些扰动，在图8中可以清楚地看到控制输入随着机器人的行走做出的变化，由于摆动腿夹角β在DS阶段没有定义，所以在DS阶段$ \tau_{2}\equiv0 $ 。另外，由于在行走的过程中，左右腿交替作为摆动腿，所以在SS阶段，控制输入$ u_{1} $ 和$ u_{2} $ 交替为零。系统的能量曲线如图9所示，其中虚线为理想SLIP模型的能量水平。可以看出，总能量在SLIP模型的能量水平上波动。一方面原因是控制器作用在躯干和摆动腿上，需要消耗能量；另一方面原因是期望轨迹由傅里叶级数拟合而来，存在着一定的偏差。因此，实际上能量水平不是恒值，存在一定的波动。
4.2 步态切换
在本节中，给定步态1平均速度为1.1 m/s，步态2平均速度为1.31 m/s。图10给出的是这两个自然步态在一步内的髋关节质点运动轨迹。设定权重
$ \{\mu_{1}, \mu_{2},\mu_{3}\}=\{15,2,5\} $ ，通过计算准则J，可以得到最优切换点($ x_{1,\rm opt}, x_{2,\rm opt})=(0.324\,4, 0.325\,8) $ ，如图10中的实点所示。![]()
图 10 两种自然步态在一步内的髋关节轨迹Figure 10. Hip trajectories of two natural gaits during a single step假定机器人以步态1开始行走，在1 m时切换到步态2，然后在4.84 m时切换回步态1。经计算，最优切换距离
$ S^{1,2}_{\rm opt}=1.472 $ m，$ S^{2,1}_{\rm opt}=5.337 $ m。图11给出了步态切换过程中的髋关节轨迹和水平速度轨迹，蓝色虚线表示发出切换命令点，绿色虚线表示最优切换点。可以看出，系统在1.472 m (第一条绿色虚线)从步态1切换到步态2，在5.337 m (第二条绿色虚线)从步态2切换回步态1。图12给出了系统的髋关节轨迹与自然参考步态的比较。可以看出，系统轨迹在三步内便能收敛到新的步态，我们发现，由于腿变长度的控制，机器人在步态切换时，伸缩腿得到调整，以维持髋关节的初始高度保持不变。图13和图14给出的是误差函数和控制输入。如第3节所述，步态切换被看作是一种施加在系统上的外部扰动。可以看出，通过调整腿部长度和髋关节的力矩，可以迅速抑制切换到新步态时引起的扰动。经过一段时间后，系统稳定到一个新的步态，误差收敛到零的邻域。
由图15的系统能量变化图可以看出，系统从步态1切换到步态2后总能量增加，包括动能和弹性势能的增加，由于变长度控制器的作用，并未引起髋关节高度的较大变化，因此，系统的重力势能维持稳定的变化规律。当系统从步态2切换回步态1时，系统的能量变化回到步态1的能量水平。
图16给出了步态切换过程中的极限环。系统以步态1启动。在第21步切换到步态2，然后在第41步切换回步态1。可以清楚地看到，机器人的相位轨迹在两个极限环之间切换，特别是在步态切换后，系统依然可以形成稳定的极限环，总的来说，该系统具有良好的鲁棒性。
5. 结论
本文提出了VL-TSLIP-SL模型，该模型由一个髋关节质点、一个刚性躯干和两条柔性腿组成，每条腿的末端都有一个脚质量。本模型包含了躯干姿势控制和摆动腿动力学。针对机器人模型高度非线性的特点，设计了反馈线性化控制器来跟踪髋关节质点的期望轨迹，以及调节躯干和摆动腿的姿态。将理想的SLIP模型作为参考，并将其髋关节质点运动轨迹作为目标轨迹，从而实现了能量的高效控制。基于优化准则的最小化原则，提出了通过控制腿部长度和髋关节力矩来实现步态切换的控制策略。该方法实现了行走过程中的步态切换和前进速度的改变。实际上，控制器之所以能够调节躯干和摆动腿姿态并进行步态切换，是因为这些非理想动力学都被认为是外部扰动，闭环系统可以通过调节控制输入来抑制这些扰动。仿真结果表明，该系统能够在3步内跟踪期望步态，并实现步态切换。在本文理论研究和仿真实验基础上进行硬件实验平台的验证是需要进一步研究的课题。
-
![]()
图 6 髋关节高度、躯干姿态角和摆动腿夹角轨迹
Figure 6. Hip trajectories, the torso angle, and the swing leg orientation
![]()
图 10 两种自然步态在一步内的髋关节轨迹
Figure 10. Hip trajectories of two natural gaits during a single step
表 1 系统参数和初始条件
Table 1 System parameters and initial conditions
Parameters Value hip mass M/kg 15 torso mass $m/{\rm{kg}} $ 3 foot mass $m_{{\rm{f}}}/{\rm{kg}}$ 2.5 spring rest length L0/m 1 torso length $l/{\rm{m}} $ 0.5 gravitational acceleration $g /\left(\mathrm{m} \cdot \mathrm{s}^{-2}\right) $ 9.8 angle of attack θ/(◦) 62.5 moment of inertia I/(kg·m2) 0.25 nominal leg stiffness $k_{0} /\left(\mathrm{N} \cdot \mathrm{m}^{-1}\right) $ 1500 initial hip horizontal position $x(0) / \mathrm{m} $ 0 initial hip vertical position $y(0) / \mathrm{m} $ 1 initial torso angle α (0)/rad 0 initial swing leg angle $\beta(0) / \mathrm{rad} $ 0 initial forward velocity $\dot{x}(0) /\left(\mathrm{m}^{\cdot} \mathrm{s}^{-1}\right) $ 1.074 used for gait 1 $\dot{x}_{1}(0) /\left(\mathrm{m} \cdot \mathrm{s}^{-1}\right) $ 1.032 used for gait 2 $\dot{x}_{2}(0) /\left(\mathrm{m} \cdot \mathrm{s}^{-1}\right) $ 1.239 initial vertical velocity $\dot{y}(0) /\left(\mathrm{m} \cdot \mathrm{s}^{-1}\right) $ 0 initial angle velocity of the torso/(rad·s–1) 0 initial angle velocity of the swing leg/(rad·s–1) 0.1 step size $a / \mathrm{m} $ 0.644 3 
下载: 导出CSV
-
[1] McGeer T. Passive dynamic walking. International Journal of Robotics Research, 1990, 9(2): 62-82 doi: 10.1177/027836499000900206
[2] Collins S, Ruina A, Tedrake R, et al. Efficient bipedal robots based on passive-dynamic walkers. Science, 2005, 307(5712): 1082-1085 doi: 10.1126/science.1107799
[3] Blickhan R. The spring-mass model for running and hopping. Journal of Biomechanics, 1989, 22(11): 1217-1227
[4] Geyer H, Seyfarth A, Blickhan R. Compliant leg behaviour explains basic dynamics of walking and running. Biological Sciences, 2006, 273(1603): 2861-2867
[5] Rummel J, Blum Y, Seyfart A. Robust and efficient walking with spring-like legs. Bioinspiration&Biomimetics, 2010, 5(4): 046004
[6] Visser LC, Stramigioli S, Carloni R. Robust bipedal walking with variable leg stiffness. The 4th IEEE RAS & EMBS International Conference on Biomedical Robotics and Biomechatronics, Roma, Italy, 2012
[7] 张瑞, 张奇志, 周亚丽. 变刚度半被动双足机器人行走的仿人控制. 力学与实践, 2020, 42(5): 551-557 doi: 10.6052/1000-0879-20-139 Zhang Rui, Zhang Qizhi, Zhou Yali. Human-like control for semi-passive bipedal robot walking with variable leg stiffness. Mechanics in Engineering, 2020, 42(5): 551-557 (in Chinese) doi: 10.6052/1000-0879-20-139
[8] Visser LC, Stramigioli S, Carloni R. Control strategy for energy-efficient bipedal walking with variable leg stiffness. The 2013 IEEE International Conference on Robotics and Automation, Karlsruhe, Germany, 2013
[9] Vu MN, Lee J, Oh Y. Control strategy for stabilization of the biped trunk-SLIP walking model. The 14th International Conference on Ubiquitous Robots & Ambient Intelligence, Jeju, Korea, 2017
[10] Luo J, Wang S, Zhao Y, et al. Variable stiffness control of series elastic actuated biped locomotion. Intelligent Service Robotics, 2018, 11: 225-235 doi: 10.1007/s11370-018-0248-y
[11] Stramigioli S, Carloni R, Visser LC, et al. Controller design for a bipedal robot with variable stiffness actuators. The 2013 IEEE International Conference on Robotics and Automation, Karlsruhe, Germany, 2013
[12] 张奇志, 周亚丽, 徐鑫鑫等. 单电动机驱动的半被动双足机器人设计与实现. 北京信息科技大学学报(自然科学版), 2016, 31(2): 1-6 Zhang Qizhi, Zhou Yali, Xu Xinxin, et al. Design and realization of semi-passive biped robot driven by single motor. Journal of Beijing Information Science&Technology University, 2016, 31(2): 1-6 (in Chinese)
[13] Asano F, Hayashi T, Luo ZW, et al. Parametric excitation approaches to efficient dynamic bipedal walking. The 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems, San Diego, CA, USA, 2007
[14] Asano F, Luo ZW. Energy-efficient and high-speed dynamic biped locomotion based on principle of parametric excitation. IEEE Transactions on Robotics, 2008, 24(6): 1289-1301 doi: 10.1109/TRO.2008.2006234
[15] Asano F. High-speed dynamic gait generation for limit cycle walkers based on forward-tilting impact posture. Multibody System Dynamics, 2013, 30(3): 287-310 doi: 10.1007/s11044-012-9326-7
[16] Hanazawa Y, Asano F. Asymmetric swing-leg motions for speed-up of biped walking. Journal of Robotics and mechatronics, 2017, 29(3): 490-499 doi: 10.20965/jrm.2017.p0490
[17] Harata Y, Kato Y, Asano F. Efficiency analysis of telescopic-legged bipedal robots. Artificial Life and Robotics, 2018, 23(4): 585-592 doi: 10.1007/s10015-018-0492-4
[18] 张奇志, 周亚丽. 伸缩腿双足机器人半被动行走控制研究. 动力学与控制学报, 2019, 17(70): 3-8 Zhang Qizhi, Zhou Yali. Study on walking control of a semi-passive biped robot with telecopic legs. Journal of Dynamics and Control, 2019, 17(70): 3-8 (in Chinese)
[19] 宋嘉琦, 张奇志, 周亚丽. 变长度弹性伸缩腿双足机器人动力学与控制. 力学季刊, 2019, 40(2): 235-242 Song Jiaqi, Zhang Qizhi, Zhou Yali. Dynamics and control of bipedal robot with variable length telescopic legs. Chinese Quarterly of Mechanics, 2019, 40(2): 235-242 (in Chinese)
[20] Roozing W, Carloni R. Variable bipedal walking gait with variable leg stiffness. The 5th IEEE RAS & EMBS International Conference on Biomedical Robotics and Biomechatronics, São Paulo, Brazil, 2014
[21] Roozing W, Carloni R. Bipedal walking gait with variable stiffness knees. The 5th IEEE RAS & EMBS International Conference on Biomedical Robotics and Biomechatronics, São Paulo, Brazil, 2014
[22] Kuznetsov SV. The motion of the elastic pendulum. Regular and Chaotic Dynamics, 1999, 4(3): 3-12 doi: 10.1070/rd1999v004n03ABEH000110
-
期刊类型引用(5)
1. 郭爱军，吴松松. 基于分步式递阶反馈的井下四足机器人行走姿态控制. 机械制造与自动化. 2025(01): 245-250 . 
百度学术
2. 王睿垠，袁威，冯放，金长江. 刚体动量矩和动能的一般表示. 力学与实践. 2024(03): 631-635 . 本站查看
3. 于茜，齐飞，田广昊. 基于改进遗传算法的舞蹈机器人步态稳定控制技术研究. 自动化与仪器仪表. 2024(09): 272-275 . 百度学术
4. 王子威，张奇志，戈新生. 基于变腿长的弹性无缘轮动力学与行走控制. 力学季刊. 2024(04): 970-982 . 百度学术
5. 杨超. 工业机器人自适应柔性焊接控制方法. 自动化应用. 2023(15): 19-20+24 . 百度学术
其他类型引用(4)
计量
- 文章访问数: 131
- HTML全文浏览量: 36
- PDF下载量: 13
- 被引次数: 9
