CN102736628A - 一种具有环境适应性的足式机器人稳定性控制方法及*** - Google Patents

Abstract

一种具有环境适应性的足式机器人稳定性控制方法及***，该控制方法通过利用上一触地过程相关参数信息与期望达到的控制目标进行比较，对飞行相水平运动速度和***总能量实行反馈控制，预测控制触地角度并进行***能量补偿控制，最终实现足式机器人SLIP等效模型在不同地面环境下的期望稳定周期运动。***包括***状态检测模块和稳定控制模块。本发明不需要建立具体的机器人动力学模型，不需要计算精确的不动点触地角度，通过反馈控制实现控制收敛，控制方法简单，计算迅速，很好的解决了现有方法控制实时性不足、适应性不够等问题。且具有较好的未知环境适应性，为足式机器人稳定性控制提供了一种较好的解决方案。

Description

一种具有环境适应性的足式机器人稳定性控制方法及***

技术领域

本发明属于机器人技术领域，具体涉及一种具有环境适应性的足式机器人稳定性控制方法及***。

背景技术

地球陆地表面有超过50％以上是山丘或沼泽等复杂地形，相对于轮式机器人和履带机器人，足式机器人在这些复杂地形上具有更好的适应性和运动性能。人们希望足式机器人能够像自然界中各种足式动物一样可以在陆地复杂地形上稳定、快速的运动，从而辅助人类完成各种任务。因此，越来越多的研究人员将仿生学方法引入到机器人研究之中，通过研究、学习、模仿的仿生学方法来复制和再造生物的结构、功能及控制机制。

自然界中的生物往往是一个复杂的高度非线性***，而且多腿、多关节和肌肉冗余严重，因此对生物体不加简化的模仿和复现相当困难且不现实。为了解决这个问题，研究人员科学合理的将各种具有不同骨骼结构、腿数和姿态的动物用相对统一的模型表示，同时获取相应的等效参数，力求通过合理有效的简化模型来等效足式动物的运动。

为考察***整体的行为特性所建立的等效模型应该是最为简化的，一种有效的尝试是弹簧倒立摆模型（SLIP,Spring Loaded InvertedPendulum），即将生物体简化为单质量——弹簧***，用不同质量、刚度、阻尼以及相关形态参数来表征具体的生物体运动。SLIP模型是20世纪80年代以来发展起来的研究足式机器人的有效模型，研究人员通过对哺乳动物的腿部和肌肉的细致研究表明弹性机制确实存在于动物的运动中，而且对动物的性能有很大的影响。该模型可以有效的解释腿部对地面冲击的缓冲机制，通过调整***着地角度、腿部等效刚度等参数可以达到控制***水平运动速度和弹跳高度的目的。且通过相关学者提出的等效虚拟腿的概念，足式机器人（如双足机器人、四足机器人、六足机器人等）均可等效为最简化的单质量——弹簧***即单SLIP等效模型进行研究和分析。

现有的足式机器人大都停留在实验室样机阶段，少有能够真正实现复杂环境下的运动控制，其难点就在于足式机器人复杂环境下的动态稳定控制方法尚不成熟。足式机器人是一种多自由度、强非线性、多冗余的***，其完整动力学特性分析十分复杂，直接对其进行复杂环境下的动态稳定性控制往往不能满足控制实时性的要求。因此越来越多的研究人员开始从SLIP等效模型入手，研究和分析SLIP等效模型在复杂环境下的动态稳定性控制方法，从而以该等效模型为基础，再次延伸和扩展至整个足式机器人，由简入繁，最终达到对整个足式机器人***稳定控制的目的。

现有技术一公开了一种针对无阻尼SLIP模型的周期稳定运动控制方法，其通过推导无阻尼SLIP模型在飞行相和着地相的动力学方程，近似得到整个着地过程中SLIP模型着地角度和离地角度与时间的关系，以及整个着地过程中弹簧长度和时间的关系等。进而通过对不同可变参数的取值迭代，仿真得到不同条件下SLIP模型一次着地前后的状态，从而找到满足SLIP不动点周期运动的合适初始参数，实现其稳定运动控制。

现有技术二公开了一种针对有阻尼SLIP模型的周期稳定运动控制方法，其在SLIP动力学模型推导中增加了阻尼项，考虑了整个运动过程中的能量损失，近似得到SLIP模型着地过程中的能量损失方程，通过改变弹簧刚度等方法对损失的能量进行补偿，继续通过对不同可变参数的取值迭代，仿真得到能量补偿后的SLIP模型着地前后的状态，从而找到满足SLIP不动点周期运动的合适初始参数，实现其稳定运动控制。

在对现有技术进行研究后，本发明人发现：现有技术一中考虑的是一种十分理想的等效模型，其忽略了足式机器人运动过程中存在的阻尼因素，在反映机器人实际运动过程中存在模型误差；而现有技术二虽然考虑了等效模型的阻尼因素，且对其运动过程中损失的能量进行了补偿，但是该技术实现不动点稳定控制需要通过大量的赋值迭代才能找到合适的初值条件，不满足机器人***实时性控制的要求，不易运用在实际的机器人稳定控制中。且现有技术均只考虑了SLIP模型在平面地形上的控制，对相对较为复杂的环境地形不具有适应性。

发明内容

本发明的目的在于提供一种能够克服上述缺陷的足式机器人稳定性控制方法及***，以SLIP等效模型为研究对象，不仅考虑足式机器人运动过程中的阻尼损失，而且使得控制后的SLIP模型不再仅仅适应于平面地形，对台阶、坑洼等较为复杂的地形也具有较好的稳定适应性，另外对模型的初始条件要求较低，不需要多次迭代，解决了控制实时性不能满足的问题。

本发明提供的一种具有环境适应性的足式机器人稳定性控制方法，该方法包括下述步骤：

第1步***参数初始化，包括***基本信息、转角PID控制的比例增益系数k_P、积分时间常数k_I和微分时间常数k_D；令n=1；令第一次触地相补偿的能量

为零；初始化控制目标，并计算得到稳定周期运动期望***总能量E_d；所述***基本信息包括质量m、等效弹簧刚度k、等效阻尼c和初始等效腿长r₀；所述控制目标包括稳定周期运动期望水平速度和期望最高点竖直高度

第2步检测第一次触地前的初始水平速度

第3步在第n次触地之前计算得到第n次触地角θ_TD；

第4步实时检测***状态，得到当前时刻SLIP模型的状态信息，包括质心A的水平速度

竖直方向速度

腿的摆角θ、腿长度r、竖直方向脚力F_c和当前***时间t；

第5步判断SLIP模型是否触地，触地时刻判定条件为足端脚力F_C发生由等于零到大于零的突变，且***竖直速度如果触地执行第7步，如果没有触地执行第6步；

第6步通过PID控制，在第n次触地之前将SLIP模型控制到预定的触地角，即使腿的摆角等于触地角θ_TD；

第7步读取触地时刻***相关状态参数，包括触地时刻的水平速度

竖直速度

等效腿长r_TD，以及实际触地角度θ_TD，并计算触地时刻***总能量E_n，即为第n次触地之前***总能量；

第8步判断SLIP模型是否压缩至底部，如果压缩至底部执行第9步，否则执行第4步；

第9步进行***能量补偿，使***达到期望***总能量E_d：

第10步判断SLIP模型是否离地，如果离地执行第11步，否则执行第9步；

第11步读取离地时刻***相关状态参数，包括离地时刻的水平速度

竖直速度

等效腿长r_LO，以及实际触地角度θ_LO，并计算离地时刻***总能量E_n+1，即为第n次触地之后***总能量；

第12步计算第n次触地过程中***损失的能量其计算公式为：

${\mathrm{\ΔE}}_n^{-}=E_n+{\mathrm{\ΔE}}_n^{+}-E_{n+1}$

其中，为第n次触地过程中应该补偿的能量，

的初始值为0；

第13步计算第n+1次触地过程中应该补偿的能量

${\mathrm{\ΔE}}_{n+1}^{+}=E_d+{\mathrm{\ΔE}}_n^{-}-E_{n+1};$

第14步判断SLIP***是否运动到达目的地，如果没有到达目的地则进行第3步，否则结束。

本发明与现有技术相比有以下优点：1）不需要对等效SLIP模型进行复杂的动力学方程推导，不需得到整个着地过程中能量损失和着地角度的近似数学表达式，简化了控制算法的同时降低了模型等效误差；2）具有实时性，不需要进行多次初值迭代，可以满足实际足式机器人的实时性控制要求；3）具有较好的地形适应能力，有一定的自主适应性，可以运用于地形未知环境下的足式机器人稳定控制。

附图说明

图1为本发明实例控制对象等效SLIP模型结构示意图；

图2为等效SLIP模型运动过程示意图；

图3为本发明实例提供的稳定性控制方法控制器结构图；

图4为本发明实例提供的稳定性控制方法流程图；

图5为图4中402计算触地角度方法示意图；

图6为图4中408进行***能量补偿方法示意图；

图7为等效SLIP模型无稳定性控制下的竖直高度仿真图；

图8为等效SLIP模型应用本发明提供的稳定性控制方法下的竖直高度仿真图；

图9为等效SLIP模型在凸凹地形下的运动示意图；

图10为等效SLIP模型应用本发明提供的稳定性控制方法在凸凹地形下运动的竖直高度仿真图；

图11为等效SLIP模型应用本发明提供的稳定性控制方法在凸凹地形下运动的水平速度仿真图；

图12为等效SLIP模型在阶梯地形下的运动示意图；

图13为等效SLIP模型应用本发明提供的稳定性控制方法在阶梯地形下运动的竖直高度仿真图；

图14为等效SLIP模型应用本发明提供的稳定性控制方法在阶梯地形下运动的水平速度仿真图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面结合附图对本发明实施方式作进一步的详细描述。显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有创造性劳动前提下所获得的所有其他实施例，都属于本发明保护范围。

如图1(a)所示为本发明实例控制对象等效SLIP模型结构示意图。足式机器人等效的质量—弹簧倒立摆SLIP模型由等效质量块101，等效腿杆102，等效弹簧阻尼***103和等效足杆104组成。等效质量块101为整个足式机器人***等效质量所在，其质心为A点，等效质量用m表示。等效腿杆102与等效质量块101为转动副连接，连接点为点A；等效弹簧阻尼***103的一端与等效腿杆102固接，另一端与等效足杆104固接，等效腿杆102、等效足杆104和等效弹簧阻尼***103三者同轴线共同组成足式机器人等效腿***，等效腿长用r表示。等效弹簧阻尼***103由刚度为k的弹簧和阻尼为c的阻尼器组成，分别表征足式机器人的等效腿刚度和等效阻尼。整个等效SLIP模型触地时与地面接触点用P_TD表示，触地角为等效腿轴线A-P_TD和y方向的夹角，用θ表示。由于质量全部等效集中至等效质量块101，故等效腿杆102，等效弹簧阻尼***103和等效足杆104质量忽略不计。

如图1(b)所示，等效SLIP模型具有两个运动自由度，分别是等效腿杆102相对于等效质量块101的转动自由度和等效弹簧阻尼***103沿等效腿轴线A-P_TD的移动自由度。转动自由度由转角驱动器105驱动，驱动力矩用T表示，移动自由度由等效力驱动器106驱动，驱动力用F表示。足式机器人上安装有检测***状态的多种传感器，在等效SLIP模型中分别为角度传感器107，速度传感器108，弹簧长度传感器109和足端脚力传感器110组成，传感***时刻检测SLIP模型运动状态，为本发明实例提供的稳定性控制方法提供传感信息。

如图2所示，SLIP运动过程可以看作两个不同运动阶段的交替，即飞行相阶段和触地相阶段。飞行相阶段为SLIP模型在空中运动阶段，此阶段***只受到重力的作用，如第n次（n从1开始）触地前的飞行相201和第n+1次触地前的飞行相203。触地相阶段为SLIP模型足端触地至再次离开地面腾空的阶段，此阶段***会因弹簧压缩释放而影响其动力学特性，如第n次触地后的触地相202和第n+1次触地前的触地相204。飞行相具有一个特殊状态，即竖直y方向运动速度为零时刻的飞行相最高点状态，如第n次触地前飞行相最高点205和第n+1次触地前飞行相最高点206。因为飞行相中只有重力作用，整个***为保守***无能量损失，可以用第n次触地前飞行相最高点205的代表信息表征第n次触地前的***状态，即第n次触地前飞行相最高点的水平速度

等效质心到地面的竖直高度

和此时的***总能量E_n。同理可用第n+1次触地前飞行相最高点206的代表信息表征第n+1次触地前的***状态，即第n+1次触地前飞行相最高点的水平速度

等效质心到地面的竖直高度

和此时的***总能量E_n+1。第n次触地前飞行相最高点205到第n+1次触地前飞行相最高点206之间的阶段为一个弹跳周期。SLIP稳定性控制的目标就是通过控制使得经过第n次触地后，***均能够达到运动开始前的设定期望状态，即期望水平速度

和期望最高点竖直高度

从而实现稳定周期运动。触地相具有三个特殊状态，即刚刚触地时刻、弹簧压缩到最低竖直y方向运动速度为零时刻和再次离地时刻，如第n次触地时刻207和第n+1次触地时刻210，第n次压缩至底部时刻208和第n+1次压缩至底部时刻211，第n次离地时刻209和第n+1次离地时刻212。能量损失和补偿均发生在触地相阶段，其中第n次触地损失的能量用

表示，第n次触地补偿的能量用

表示，第n+1次触地损失的能量用

表示，第n+1次触地补偿的能量用

表示。

如图4所示，本发明实例提供的稳定性控制方法通过触地角预测、***能量补偿和反馈控制实现稳定周期运动收敛，具体步骤如下：

步骤401：***参数初始化，包括初始化***基本信息如质量m，等效弹簧刚度k，等效阻尼c，初始等效腿长r₀。此处的***指由等效质量块、等效腿、转动副以及相应的控制方法构成的有机整体，***参数为控制对象的结构参数，在控制对象确定后可直接得到；转角PID控制的比例增益系数k_P，积分时间常数k_I，微分时间常数k_D，以上三个参数的确定需根据具体的***，通过试验或仿真确定，以摆动腿可以快速稳定的控制到预定的角度，且不发生超调为准；令n=1；令第一次触地相补偿的能量为零；初始化控制目标如稳定周期运动期望水平速度

期望最高点竖直高度

这两个参数为期望***达到的运动状态；并计算稳定周期运动期望***总能量E_d，其计算公式为：

$E_d=m{\stackrel{\·}{x}}_d^2/2+{\mathrm{mgy}}_{{\mathrm{apex}}_d}---\left(1\right)$

其中，g为重力加速度。

步骤402：检测第一次触地前的初始水平速度为步骤403的触地角度计算提供数据。

步骤403：在第n次触地之前计算第n次触地时刻的触地角θ_TD。

如图5所示，具体计算过程为：

通过控制不同的触地角完成第n次触地过程后会有三种不同的结果， $\left(a\right){\stackrel{\&CenterDot;}{x}}_n={\stackrel{\&CenterDot;}{x}}_{n+1},$ $\left(b\right){\stackrel{\&CenterDot;}{x}}_n<{\stackrel{\&CenterDot;}{x}}_{n+1},$ $\left(c\right){\stackrel{\&CenterDot;}{x}}_n>{\stackrel{\&CenterDot;}{x}}_{n+1},$ 其中为第n次触地之前的水平速度，

为第n次触地之后的水平速度。当

时，我们称此时的触地点P_TD恰好为中性点P_N，即能够保证触地前后状态相同的触地点。在SLIP模型每次触地之前对其水平速度进行判定比较也会有三种不同情况， $\left(a\right){\stackrel{\&CenterDot;}{x}}_n={\stackrel{\&CenterDot;}{x}}_d,$ $\left(b\right){\stackrel{\&CenterDot;}{x}}_n<{\stackrel{\&CenterDot;}{x}}_d,$ $\left(c\right){\stackrel{\&CenterDot;}{x}}_n>{\stackrel{\&CenterDot;}{x}}_d,$ 若为情况(a)，则我们希望控制SLIP触地角度继续落在中性点上，这样触地之后的状态仍然为我们期望的水平速度；若为情况(b)，则控制触地角度使得触地点P_TD提前于中性点P_N，这样触地之后的水平速度

会增大以趋近期望的水平速度

ΔS即为实际触地点P_TD与中性点P_N之间的调整距离；同理若为情况(c)，则控制触地角度使得触地点P_TD落后于中性点P_N，这样触地之后的水平速度

会减小以趋近期望的水平速度

ΔS即为实际触地点P_TD与中性点P_N之间的调整距离。将S₀定义为触地点是中性点时触地过程运动水平距离的一半。其中ΔS、S₀计算公式为：

$S_0={\stackrel{\&CenterDot;}{x}}_{{\mathrm{avg}}_n}\&CenterDot;t_s/2---\left(2\right)$

$\mathrm{\&Delta;S}=\mathrm{\&mu;}\&CenterDot;({\stackrel{\&CenterDot;}{x}}_n-{\stackrel{\&CenterDot;}{x}}_d)/r_0---\left(3\right)$

则整个预测控制触地角θ_TD计算公式为：

${\mathrm{\&theta;}}_{\mathrm{TD}}=\arcsin ((S_0+\mathrm{\&Delta;S})/r_0)=\arcsin (({\stackrel{\&CenterDot;}{x}}_{{\mathrm{avg}}_n}\&CenterDot;t_s/2+\mathrm{\&mu;}({\stackrel{\&CenterDot;}{x}}_n-{\stackrel{\&CenterDot;}{x}}_d))/r_0)---\left(4\right)$

其中t_s为第n次触地过程总时间，可通过检测触地时刻和离地时刻计算得到；

为第n次触地过程平均水平速度，这里采用平均速度计算更为准确；μ为触地点与中性点调整距离比例系数，该参数的确定需根据具体的机器人***，通过试验或仿真的方法获得，以能使机器人稳定行走为准。

步骤404：实时检测***状态，包括质心A的水平速度

竖直方向速度

腿的摆角θ，腿长度r，竖直方向脚力F_c，当前***时间t，得到当前时刻SLIP模型的状态信息，便于进行相关判断。

步骤405：通过足端脚力传感器和机身速度传感器判断SLIP模型是否触地，触地时刻判定条件为足端脚力F_C发生由等于零到大于零的突变，且***竖直速度

如果触地执行步骤407，如果没有触地执行步骤406。

步骤406：通过PID控制，在第n次触地之前将SLIP模型控制到预定的触地角度。其控制输入为当前摆角，输出为关节力矩。

步骤407：读取触地时刻***相关状态参数，包括触地时刻的水平速度

竖直速度等效腿长r_TD，实际触地角度θ_TD，并计算触地时刻***总能量E_n，即为第n次触地之前***总能量，其计算公式为：

$E_n=m{\stackrel{\&CenterDot;}{x}}_{\mathrm{TD}}^2/2+m{\stackrel{\&CenterDot;}{y}}_{\mathrm{TD}}^2/2+{\mathrm{mgr}}_{\mathrm{TD}}{\cos \mathrm{\&theta;}}_{\mathrm{TD}}+k{(r_0-r_{\mathrm{TD}})}^2/2---\left(6\right)$

步骤408：通过足端脚力传感器和机身速度传感器判断SLIP模型是否压缩至底部，压缩至底部时刻判定条件为足端脚力F_C>0，且***竖直速度如果压缩至底部执行步骤409，如果没有压缩至底部执行步骤404。

步骤409：进行***能量补偿：

如图6所示，本发明实例列举三种不同补偿能量的方法，具体过程如下：

方法(a)，在压缩至底部时刻增加沿弹簧方向的冲量F_n·dt，通过动量定理可以计算欲补偿能量为

时的驱动力F_n，其计算公式为：

$F_n=(-{\mathrm{mv}}_B+\sqrt{{\left({\mathrm{mv}}_B\right)}^2+2m\&CenterDot;{\mathrm{\&Delta;E}}_n^{+}})/\mathrm{dt}---\left(7\right)$

其中，ΔE_n ⁺为第n次触地补偿的能量,这里，当n=1时，ΔE_n ⁺=0，当n>1时，ΔE_n ⁺已在上一个飞行相过程中计算得到，即为飞行相n-1时计算得到的ΔE_n+1 ⁺，其计算方法见步骤413，v_B为冲量补偿时刻***沿弹簧方向的合速度，dt为冲量增加的时间间隔，合速度大小的计算方法如下：

$v_B=\stackrel{\&CenterDot;}{x}\&CenterDot;\cos \mathrm{\&theta;}+\stackrel{\&CenterDot;}{y}\&CenterDot;\sin \mathrm{\&theta;}---\left(8\right)$

θ、

和

为步骤404得到的检测值。

方法(b)，在压缩至底部时刻开始直到***离地时刻持续增加沿弹簧方向的补偿恒力F_n，可以计算欲补偿能量为

时的驱动力F_n，其计算公式为：

$F_n={\mathrm{\&Delta;E}}_n^{+}/(r_0-r_B)---\left(9\right)$

其中r₀为无压缩时的等效腿长，r_B为压缩至底部时刻的等效腿长。

方法(c)，在压缩至底部时刻开始直到***离地时刻改变***等效刚度，在原等效刚度k的基础上增加补偿刚度Δk，则可以计算欲补偿能量为

时的补偿刚度Δk，其计算公式为：

$\mathrm{\&Delta;k}={2\mathrm{\&Delta;E}}_n^{+}/{(r_0-r_B)}^2---\left(10\right)$

其中r₀为无压缩时的等效腿长，r_B为压缩至底部时刻的等效腿长。

补偿刚度Δk通过控制驱动力F_n实现，F_n在整个过程中时刻变换，通过胡克定理可以计算得出，其计算公式为：

F_n=Δk·(r₀-r)                                        (11)

其中r₀为无压缩时的等效腿长，r为任意当前时刻的等效腿长，F_n在整个过程中随着弹簧释放长度变化而变化。

步骤4lO：通过足端脚力传感器和机身速度传感器判断SLIP模型是否离地，离地时刻判定条件为足端脚力F_C发生由大于零到等于零的突变，且***竖直速度

如果离地执行步骤411，如果没有离地执行步骤409。

步骤411：读取离地时刻***相关状态参数，如离地时刻的水平速度

竖直速度

等效腿长r_LO，实际触地角度θ_LO，并计算离地时刻***总能量E_n+l，即为第n次触地之后***总能量，其计算公式为：

$E_{n+1}=m{\stackrel{\&CenterDot;}{x}}_{\mathrm{LO}}^2/2+m{\stackrel{\&CenterDot;}{y}}_{\mathrm{LO}}^2/2+{\mathrm{mgr}}_{\mathrm{LO}}{\cos \mathrm{\&theta;}}_{\mathrm{LO}}+k{(r_0-r_{\mathrm{LO}})}^2/2---\left(12\right)$

步骤412：计算第n次触地过程中***损失的能量

其计算公式为：

${\mathrm{\&Delta;E}}_n^{-}=E_n+{\mathrm{\&Delta;E}}_n^{+}-E_{n+1}---\left(13\right)$

其中，

为第n次触地过程中应该补偿的能量，的初始值为0。

步骤413：计算下一次触地即第n+1次触地过程中应该补偿的能量

不仅考虑当前***能量与稳定周期运动期望***总能量的差值，而且额外增加上一次触地过程损失的能量作为能量预补偿，其计算公式为：

${\mathrm{\&Delta;E}}_{n+1}^{+}=E_d+{\mathrm{\&Delta;E}}_n^{-}-E_{n+1}---\left(14\right)$

其中E_d为稳定周期运动期望***总能量。

步骤414：判断SLIP***是否运动到达目的地，如果没有到达目的地则进行步骤403，如果到达目的地则控制流程结束。

本发明实例中，能量补偿方法是考虑在***压缩至底部时刻开始直到***离地时刻结束阶段，在其他阶段也可以类似进行能量补偿，如***触地时刻开始至***离地时刻结束阶段，***触地时刻开始至***压缩至底部时刻阶段等等，在此不一一说明，均在本发明保护范围之中。

如图3所示，本发明实例提供的稳定性控制***采用反馈控制，它包括***状态检测模块301和稳定控制模块302。

稳定控制模块302由触地角控制模块305，PID控制器306，能量补偿模块307和补偿力控制器308组成。

触地角控制模块305实现飞行时触地角的计算，计算方法见式(4)。

PID控制器306通过PID控制实现模块305传来的控制信号θ_TD。能量补偿模块307计算***从弹簧压缩至最低点到起飞期间需要补偿的能量，计算方法如式(14)所示。

补偿力控制器308通过改变弹簧刚度，改变作用在等效质量上的作用力实现***能量的补偿，计算方法见式(10)、(11)。

***状态检测模块301，该模块对控制中用到的***状态量进行实时检测，若某些状态量被控对象本身可以检测，则控制***直接读取即可，否则，需安装相应的传感器进行检测。

***工作时，输入U_d为SLIP模型稳定运动后的期望状态，包括期望水平速度

和期望最高点竖直高度

可计算出期望最高点总能量E_d。通过***状态检测模块301将当前***状态与期望***状态进行反馈比较，得到相差的水平速度和相差的***总能量ΔE_n。相差的水平速度

通过触地角控制模块305计算得到触地角θ_TD，继续通过PID控制器306得到驱动力矩T_n+1，通过伺服驱动器303作用在被控对象304上对其进行控制；相差的***总能量ΔE_n通过能量补偿控制模块307计算得到第n+1次触地相需要补偿的能量继续通过补偿力控制器308得到补偿驱动力F_n+1，通过伺服驱动器303作用在被控对象304上对其进行控制。具体实例：

下面以具体实例仿真来进行控制效果说明，基本参数设置如下：

SLIP模型等效质量m=0.54kg，等效弹簧刚度k=6N/mm，等效阻尼c=0.00035N·s/mm，初始等效腿长r₀=120mm，初始竖直高度H₀=170mm。

PID控制的输入为当前摆角，输出为关节力矩。本实例所采用的规律如下：

$F=k_P(\mathrm{\&theta;}-{\mathrm{\&theta;}}_{\mathrm{TD}})+k_d(\stackrel{\&CenterDot;}{\mathrm{\&theta;}}-{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{\mathrm{TD}})+\&Integral;(\mathrm{\&theta;}-{\mathrm{\&theta;}}_{\mathrm{TD}})\mathrm{dt}$

其中F为关节驱动力矩，k_P、k_I、k_D为PID控制的增益参数，θ为第4步检测到的腿的当前摆角，θ_TD为第3步中计算的触地角即摆角，

分别为θ、θ_TD的导数，即当前摆角和期望的摆角的变化率。

图7为等效SLIP模型无稳定性控制下的竖直高度仿真图。该仿真是为了验证无运动稳定控制时SLIP模型竖直跳跃过程的情况。此时SLIP模型初始水平速度

控制目标为期望水平速度

期望最高点竖直高度

由仿真曲线可见，SLIP模型随着触地次数增加，在没有能量补偿下的最高点竖直高度逐渐下降，离期望的最高点竖直高度

越来越远，***达不到期望稳定周期运动的控制目标。

图8为等效SLIP模型应用本发明实例提供的稳定性控制方法下的竖直高度仿真图。初始条件和控制目标均与图7相同，从仿真曲线可以看到，除了第一次触地因为地形未知而导致超越期望最高点竖直高度外，之后的触地跳跃很快即控制收敛，基本达到了期望的最高点竖直高度

从而实现了***期望稳定周期运动的控制目标。

图9为等效SLIP模型在凸凹地形下的运动示意图。此时SLIP模型初始水平速度

控制目标为期望水平速度

期望最高点竖直高度

此仿真为了验证本发明实例提供的稳定性控制方法和***在非平地地形等较为复杂地形下的适应能力。图9所示为凸凹地形，凹陷深度为h₁=30mm。最高点竖直高度仿真数据均是SLIP模型质心相对与图9中平面CD测量得到。

图10为等效SLIP模型应用本发明实例提供的稳定性控制方法在凸凹地形下运动的竖直高度仿真图。由仿真曲线可以看到实际竖直高度与期望最高点竖直高度可以较好实现重合，实现了凸凹地形下SLIP模型最高点竖直高度快速稳定的控制要求。

图11为等效SLIP模型应用本发明实例提供的稳定性控制方法在凸凹地形下运动的水平速度仿真图。由仿真曲线可以看到飞行相水平速度除了第一次触地因为地形未知而相差较大以外，以后的水平速度均在期望水平速度

上下小幅度波动，符合实际机器人运动情况，能够实现凸凹地形下SLIP模型飞行相水平速度快速稳定的控制要求。

图12为等效SLIP模型在阶梯地形下的运动示意图。此时SLIP模型初始水平速度

控制目标为期望水平速度

期望最高点竖直高度

此仿真为了验证本发明实例提供的稳定性控制方法和***在非平地地形等较为复杂地形下的适应能力。图12所示为阶梯地形，阶梯高度为h₂=30mm。最高点竖直高度仿真数据均是SLIP模型质心相对与图12中平面CD测量得到。

图13为等效SLIP模型应用本发明实例提供的稳定性控制方法在阶梯地形下运动的竖直高度仿真图。由仿真曲线可以看到实际竖直高度与期望最高点竖直高度可以较好实现重合，实现了阶梯地形下SLIP模型最高点竖直高度快速稳定的控制要求。

图14为等效SLIP模型应用本发明实例提供的稳定性控制方法在阶梯地形下运动的水平速度仿真图。由仿真曲线可以看到飞行相水平速度除了第一次触地因为地形未知而相差较大以外，以后的水平速度均在期望水平速度

上下小幅度波动，符合实际机器人运动情况，能够实现凸凹地形下SLIP模型飞行相水平速度快速稳定的控制要求。

本发明实例提出的稳定性控制方法及***无需建立具体的机器人动力学模型，不需要推导精确的不动点触地角度，而是通过不断与期望状态比较反馈最终实现稳定性收敛，控制方法简单，计算迅速，很好的解决了现有方法实时性不足、适应性不够等问题。且具有较好的未知环境适应性，为足式机器人稳定性控制提供了一种较好的解决方案。

Claims (8)

1.一种具有环境适应性的足式机器人稳定性控制方法，该方法包括下述步骤：

第1步***参数初始化，包括***基本信息、转角PID控制的比例增益系数k_P、积分时间常数k_I和微分时间常数k_D；令n=1；令第一次触地相补偿的能量

为零；初始化控制目标，并计算得到稳定周期运动期望***总能量E_d；所述***基本信息包括质量m、等效弹簧刚度k、等效阻尼c和初始等效腿长r₀；所述控制目标包括稳定周期运动期望水平速度和期望最高点竖直高度

第2步检测第一次触地前的初始水平速度

第3步在第n次触地之前计算得到第n次触地角θ_TD；

第4步实时检测***状态，得到当前时刻SLIP模型的状态信息，包括质心A的水平速度

竖直方向速度

腿的摆角θ、腿长度r、竖直方向脚力F_c和当前***时间t；

第5步判断SLIP模型是否触地，触地时刻判定条件为足端脚力F_C发生由等于零到大于零的突变，且***竖直速度

如果触地执行第7步，如果没有触地执行第6步；

第6步通过PID控制，在第n次触地之前将SLIP模型控制到预定的触地角，即使腿的摆角等于θ_TD；

第7步读取触地时刻***相关状态参数，包括触地时刻的水平速度

竖直速度

等效腿长r_TD，以及实际触地角度θ_TD，并计算触地时刻***总能量E_n，即为第n次触地之前***总能量；

第8步判断SLIP模型是否压缩至底部；如果压缩至底部执行第9步，否则执行第4步；

第9步进行***能量补偿，使***达到期望***总能量E_d：

第10步判断SLIP模型是否离地，如果离地执行第11步，否则执行第9步；

第11步读取离地时刻***相关状态参数，包括离地时刻的水平速度

竖直速度

等效腿长r_LO，以及实际触地角度θ_LO，并计算离地时刻***总能量E_n+1，即为第n次触地之后***总能量；

第12步计算第n次触地过程中***损失的能量

其计算公式为：

${\mathrm{\&Delta;E}}_n^{-}=E_n+{\mathrm{\&Delta;E}}_n^{+}-E_{n+1}$

其中，

为第n次触地过程中应该补偿的能量，

的初始值为0；

第13步计算第n+1次触地过程中应该补偿的能量

${\mathrm{\&Delta;E}}_{n+1}^{+}=E_d+{\mathrm{\&Delta;E}}_n^{-}-E_{n+1};$

第14步判断SLIP***是否运动到达目的地，如果没有到达目的地则进行第3步，否则结束。

2.根据权利要求1所述的具有环境适应性的足式机器人稳定性控制方法，其特征在于，第3步中，

第n次触地角θ_TD的计算过程为：

设ΔS即为实际触地点P_TD与中性点P_N之间的调整距离；S₀为触地点是中性点时触地过程运动水平距离的一半，

$\mathrm{\&Delta;S}=\mathrm{\&mu;}\&CenterDot;({\stackrel{\&CenterDot;}{x}}_n-{\stackrel{\&CenterDot;}{x}}_d)/r_0,$ 则

${\mathrm{\&theta;}}_{\mathrm{TD}}=\arcsin ((S_0+\mathrm{\&Delta;S})/r_0)=\arcsin (({\stackrel{\&CenterDot;}{x}}_{{\mathrm{avg}}_n}\&CenterDot;t_s/2+\mathrm{\&mu;}({\stackrel{\&CenterDot;}{x}}_n-{\stackrel{\&CenterDot;}{x}}_d))/r_0)$

其中t_s为第n次触地过程总时间，

为第n次触地过程平均水平速度，μ为触地点与中性点调整距离比例系数。

3.根据权利要求1或2所述的具有环境适应性的足式机器人稳定性控制方法，其特征在于，触地时刻***总能量E_n，即为第n次触地之前***总能量的计算公式为：

$E_n=m{\stackrel{\&CenterDot;}{x}}_{\mathrm{TD}}^2/2+m{\stackrel{\&CenterDot;}{y}}_{\mathrm{TD}}^2/2+{\mathrm{mgr}}_{\mathrm{TD}}{\cos \mathrm{\&theta;}}_{\mathrm{TD}}+k{(r_0-r_{\mathrm{TD}})}^2/2.$

4.根据权利要求1或2所述的具有环境适应性的足式机器人稳定性控制方法，其特征在于，第9步中，补偿能量的方法如下：

在压缩至底部时刻增加沿弹簧方向的冲量F_n·dt，计算欲补偿能量为时的驱动力F_n，其计算公式为：

$F_n=(-{\mathrm{mv}}_B+\sqrt{{\left({\mathrm{mv}}_B\right)}^2+2m\&CenterDot;{\mathrm{\&Delta;E}}_n^{+}})/\mathrm{dt}$

其中，v_B为冲量补偿时刻***沿弹簧方向的合速度，dt为冲量增加的时间间隔， $v_B=\stackrel{\&CenterDot;}{x}\&CenterDot;\cos \mathrm{\&theta;}+\stackrel{\&CenterDot;}{y}\&CenterDot;\sin \mathrm{\&theta;}.$

5.根据权利要求1或2所述的具有环境适应性的足式机器人稳定性控制方法，其特征在于，第9步中，补偿能量的方法如下：

在压缩至底部时刻开始直到***离地时刻持续增加沿弹簧方向的补偿恒力F_n，计算欲补偿能量为

时的驱动力F_n，其计算公式为：

$F_n={\mathrm{\&Delta;E}}_n^{+}/(r_0-r_B)$

其中r₀为无压缩时的等效腿长，r_B为压缩至底部时刻的等效腿长。

6.根据权利要求1或2所述的具有环境适应性的足式机器人稳定性控制方法，其特征在于，第9步中，补偿能量的方法如下：

在压缩至底部时刻开始直到***离地时刻改变***等效刚度，在原等效刚度k的基础上增加补偿刚度Δk，其计算公式为：

$\mathrm{\&Delta;k}={2\mathrm{\&Delta;E}}_n^{+}/{(r_0-r_B)}^2$

其中r₀为无压缩时的等效腿长，r_B为压缩至底部时刻的等效腿长；

补偿刚度Δk通过控制驱动力F_n实现，F_n在整个过程中时刻变换，其计算公式为：

F_n=Δk·(r₀-r)

其中r₀为无压缩时的等效腿长，r为任意当前时刻的等效腿长。

7.根据权利要求1或2所述的具有环境适应性的足式机器人稳定性控制方法，其特征在于，第6步中，PID控制的输入为当前摆角，输出为关节力矩。

8.一种具有环境适应性的足式机器人稳定性控制***，其特征在于，该***包括***状态检测模块和稳定控制模块；

稳定控制模块由触地角控制模块，PID控制器，能量补偿模块和补偿力控制器组成；触地角控制模块用于进行计算飞行时的触地角，并提供给PID控制器；PID控制器根据触地角控制模块提供的触地角控制腿到对应的摆角；能量补偿模块计算***从弹簧压缩至最低点到起飞期间需要补偿的能量；补偿力控制器通过改变弹簧刚度，改变作用在等效质量上的作用力实现***能量的补偿；

***状态检测模块用于实时检测***控制所需的***状态量，并提供给稳定控制模块。

Images