CN111891252A - 一种四足仿生机器人的身体姿态斜坡自适应控制方法 - Google Patents

Abstract

本公开提供了一种四足仿生机器人的身体姿态斜坡自适应控制方法，本公开在模型预测控制下实现最优足底力分配与足端落地预测；由身体姿态斜坡自适应算法实现机器人爬坡时的姿态调整，从而可以实现机器人对斜坡地形环境的感知，实现机器人在斜坡地形的姿态自适应调整和稳定运动行走，不需要利用机器人的视觉感知信息。

Description

一种四足仿生机器人的身体姿态斜坡自适应控制方法

技术领域

本公开属于机器人姿态控制技术领域，涉及一种四足仿生机器人的身体姿态斜坡自适应控制方法。

背景技术

本部分的陈述仅仅是提供了与本公开相关的背景技术信息，不必然构成在先技术。

当前，仿生机器人逐渐从实验室走向作业场地，其高动态性、复杂环境适应性和大负载能力显著提高。为了完成给定作业任务，机器人一般都是采用某种单一的地形适应模式，这显著降低了机器人对各种不同复杂环境的应变能力。在自然界中四足哺乳动物可以根据不同环境状况进行姿态的自适应调整，以提高机器人运动过程的稳定和对复杂环境的自适应性。因此，模仿自然界四足哺乳动物对复杂环境的强适应性，提高机器人的复杂环境的人机交互能力，需要研究机器人在有坡度地形下的姿态自适应调整策略和方法。

最近几年，模型预测控制(Model Predictive Control，MPC)逐渐运用到了四足机器人的稳定运动控制中，取得了较好的运动控制效果，使得越来越多的学者对模型预测控制进行深入研究。这些研究成果都对模型预测控制的应用与扩展，实现了机器人足端位置、足底力控制的优化与求解。但据发明人了解，其对复杂路面的适应性相对不高，没有考虑如何将地形信息引入到机器人的姿态运动控制中，以提高机器人运动过程中的稳定裕度，更好地提高机器人对斜坡等崎岖地形环境的适应性。

同时，现有文献大都在静步态或者基于视觉信息的条件下实现的坡面自适应调整，导致机器人运动速度降低，斜坡行走不够稳定，或增加了处理视觉信息的工作量。

发明内容

本公开为了解决上述问题，提出了一种四足仿生机器人的身体姿态斜坡自适应控制方法，本公开在模型预测控制下实现最优足底力分配与足端落地预测；由身体姿态斜坡自适应算法实现机器人爬坡时的姿态调整，从而可以实现机器人对斜坡地形环境的感知，实现机器人在斜坡地形的姿态自适应调整和稳定运动行走，不需要利用机器人的视觉感知信息。

根据一些实施例，本公开采用如下技术方案：

一种四足仿生机器人的身体姿态斜坡自适应控制方法，包括以下步骤：

搭建仿生机器人机体与世界坐标系，将机器人建模为单个刚体，且在接触点处受力，建立机器人动力学模型；

计算并跟踪仿生机器人腿部在世界坐标系中的轨迹，并计算腿部关节力矩，考虑地面力控制过程中的关节力矩，利用模型预测控制确定最优控制轨迹；

根据得到的最优控制轨迹控制仿生机器人的运动；

获取仿生机器人行走过程中此时刻的姿态参数，确定足端位置的坐标映射，调整机器人在斜坡上的重心位置；

设置一虚拟斜坡，根据机器人此时刻姿态参数中躯干俯仰角度，计算当前状态下虚拟斜坡的角度，将此角度作为下一时刻机器人俯仰角的调整值，不断调整机器人的姿态，直到虚拟斜坡角度、躯干俯仰角和实际斜坡角度三者相等。

作为可选择的实施方式，搭建仿生机器人机体与世界坐标系，将机器人建模为单个刚体，且在接触点处受力，建立机器人动力学模型的具体过程包括：

搭建四足机器人机体与世界坐标系，使该模型把机器人建模为单个刚体，且在接触点处受力，根据每条腿受到的地面反力，机器人各个腿在地面的接触点位于机体坐标系的位置，确定机器人受到的重力与地面反作用力；

根据角动量与转矩之间的关系，计算机器人在世界坐标系下的惯性张量。

作为可选择的实施方式，计算并跟踪仿生机器人腿部在世界坐标系中的轨迹，并计算腿部关节力矩的具体过程包括：

利用控制律计算腿i的关节力矩：

这里的J_i∈R^3×3为足端雅可比矩阵，K_p,K_d∈R^3×3是对角正定比例和导数增益矩阵，_Rp_i,Rv_i∈R³为第i腿的位置和速度，_Rp_i,ref,Rv_i,ref∈R³为摆动腿位置和速度的参考运动轨迹；τ_i,ff∈R³为前馈转矩，相应得到下式：

其中Λ_i∈R^3×3是操作空间的惯性矩阵，a_i,ref∈R³为机体坐标系下的参考加速度，q_i∈R³是关节位置的矢量，

是腿的重力和科里奥利力产生的力矩。

作为可选择的实施方式，考虑地面力控制过程中的关节力矩，利用模型预测控制确定最优控制轨迹的具体过程包括：

将确定最优控制轨迹转化为二次规划问题，在约束条件下，求解目标函数的最优化值：

其中，x_i+1,ref是第i+1时刻的***参考轨迹，x_i+1是第i+1时刻***状态，约束包括：

x_i+1＝A_ix_i+B_iu_i,i＝0...k-1

D_iu_i＝0,i＝0...k-1

其中x_i为第i步时的***状态，u_i为第i步时的控制输入，Q_i和R_i是权值的对角正半定矩阵，A_i和B_i表示离散时间***动力学，C_i,

c _i表示控制输入的不等式约束，D_i是选择时间步长为i时，处于摆动相的脚所对应的力的矩阵，通过最优化寻找一个控制输入序列，该序列将保证机器人***沿着x_ref参考轨迹运行，在控制效果和跟踪进度之间进行折中。

作为进一步的限定，在求解目标函数的最优化值的过程中，采用精确离散法将目标函数进行离散化，离散化的过程使用零阶保持器进行采样，以固定的时间周期进行采样、离散化，将离散化后的目标函数转化为二次规划问题进行求解。

作为可选择的实施方式，根据机器人的姿态参数，确定足端位置的坐标映射，调整机器人在斜坡上的重心位置的具体过程包括：

基于零点力矩的稳定条件，通过调节机器人足端位置，以实现机器人质心的调整，在动力学基础上，根据机体坐标系、世界坐标系、轨迹规划坐标系和斜面坐标系的关系，将依据躯干姿态信息进行足端位置的坐标映射，实现斜坡足端位置的调整适应。

作为可选择的实施方式，设置一虚拟斜坡的具体过程为：将仿生机器人由平坦路面运动到斜坡时前后两支撑足端所形成的二维平面作为虚拟斜坡。

一种仿生机器人的身体姿态斜坡自适应控制***，包括：

动力学模型构建模块，被配置为搭建仿生机器人机体与世界坐标系，将机器人建模为单个刚体，且在接触点处受力，建立机器人动力学模型；

模型预测控制模块，被配置为计算并跟踪仿生机器人腿部在世界坐标系中的轨迹，并计算腿部关节力矩，考虑地面力控制过程中的关节力矩，利用模型预测控制确定最优控制轨迹，并根据所述最优控制轨迹控制仿生机器人的运动；

足端位置调整模块，被配置为根据机器人此时刻的姿态参数，确定足端位置的坐标映射，调整机器人在斜坡上的重心位置；

斜坡姿态调整模块，被配置为设置一虚拟斜坡，根据机器人此时刻姿态参数中躯干俯仰角度，计算当前状态下虚拟斜坡的角度，将此角度作为下一时刻机器人俯仰角的调整值，不断调整机器人的姿态，直到虚拟斜坡角度、躯干俯仰角和实际斜坡角度三者相等。

一种计算机可读存储介质，其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行所述的一种四足仿生机器人的身体姿态斜坡自适应控制方法。

一种终端设备，包括处理器和计算机可读存储介质，处理器用于实现各指令；计算机可读存储介质用于存储多条指令，所述指令适于由处理器加载并执行所述的一种四足仿生机器人的身体姿态斜坡自适应控制方法。

与现有技术相比，本公开的有益效果为：

本公开先构建机器人斜坡身体姿态调整的控制框架，利用模型预测控制实现仿生机器人的最优路径行走。

本公开根据机器人运动时的姿态参数，通过推导的足端轨迹算法，得到足端位置的坐标映射，调整机器人在斜坡上的重心位置；之后通过构建虚拟斜坡，实现了机器人在上坡的过程中躯干姿态的自适应调整，以达到增强机器人在坡面上的稳定裕度，优化足端运动空间，实现了四足机器人在斜坡地形的稳定行走。

附图说明

构成本公开的一部分的说明书附图用来提供对本公开的进一步理解，本公开的示意性实施例及其说明用于解释本公开，并不构成对本公开的不当限定。

图1为动力学建模下的机器人机体与世界坐标系示意图；

图2为模型预测控制框架示意图；

图3为足端位置调整前机器人质心投影示意图；

图4为足端位置调整后机器人质心投影示意图；

图5为足端位置调整中坐标系之间的位置关系示意图；

图6(a)、图6(b)为机器人躯干姿态调整前后图；

图7为机器人身体姿态斜坡自适应控制流程图；

图8为机器人身体姿态斜坡自适应调整流程图。

具体实施方式：

下面结合附图与实施例对本公开作进一步说明。

应该指出，以下详细说明都是例示性的，旨在对本公开提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本公开所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本公开的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

本实施例给出一种应用于四足四足仿生机器人的身体姿态斜坡自适应控制方法，包括以下步骤：

首先需要构建机器人斜坡身体姿态调整的控制框架；具体包括：

(1)建立简化动力学模型：

搭建四足机器人机体与世界坐标系如图1，使该模型把机器人建模为单个刚体，且在接触点处受力。设每条腿受到的地面反力f_i∈R³，机器人第i个腿在地面的接触点位于机体坐标系的位置r_i∈R³。由机器人受到的重力与地面反作用力可得牛顿公式：

其中p∈R³是机器人重心在世界坐标系下的位置，

是机器人在世界坐标系下的加速度，n表示机器人运动过程中与地面接触的腿的个数。m是机器人的质量，g∈R³是重力加速度。

根据角动量与转矩之间的关系得欧拉公式：

这里I∈R³是机器人在世界坐标系下的惯性张量，ω∈R³是机器人在世界坐标系下的角速度。

1.角速度动力学估计：

机器人的方向向量由z-y-x欧拉角表示，即

其中ψ为偏转，θ为俯仰，

为横滚。这些角度对应于一系列的旋转，因此从机体到世界坐标的转换可以表示为

其中，R表示机体到世界坐标的旋转矩阵，R_n(α)表示α绕n轴的正旋转。根据这些角的变化率能求出世界坐标下的角速度：

如果(cos(θ)≠0)，则方程可以反向求出：

对小的回转和俯仰的值

式(10)可以近似为

相当于

其中式(6)能近似为:

机器人在世界坐标系下的惯性张量I能够用旋转矩阵R和机体坐标系下的惯性张量_RI表示：

I＝R_RIR^T (9)

在小的回转角和俯仰角下，机器人的惯性张量为：

2.动力学的简化：

结合牛顿和欧拉方程可以构建四足机器人***状态空间方程:

其中，S_g＝[0 0 1]^T，g＝[0 0 g]^T，g表示重力加速度，一般取值为g＝9.81m/s。

对式(11)进行简化表示，可以把动力学变成简便的状态空间形式：

其中A_c∈R^13×13和B_c∈R^13×3n。这种形式只取决于偏转和足端位置。如果这些可以提前计算，动力学就变成了线性时变。该方法适用于凸模型预测控制。

(2)MPC控制器构建：

1.摆动腿控制：

摆动控制器计算并跟踪脚在世界坐标系中的轨迹。轨迹跟踪控制器采用反馈项和前馈项之和来计算关节力矩。利用控制律计算腿i的关节力矩：

这里的J_i∈R^3×3为足端雅可比矩阵，K_p,K_d∈R^3×3是对角正定比例和导数增益矩阵，_Rp_i,Rv_i∈R³为第i腿的位置和速度，_Rp_i,ref,Rv_i,ref∈R³为摆动腿位置和速度的参考运动轨迹。τ_i,ff∈R³为前馈转矩，相应得到下式：

其中Λ_i∈R^3×3是操作空间的惯性矩阵，a_i,ref∈R³为机体坐标系下的参考加速度，q_i∈R³是关节位置的矢量，

是腿的重力和科里奥利力产生的力矩。

为了保证上述腿运动过程中的高增益稳定性，矩阵K_p需要进行调整，以此确保在腿的表观质量改变下，闭环***的固有频率保持相对恒定。K_p的第i个轴方向上的第i个对角线元素需要保持一个恒定的固有频率ω_i，对此可近似为：

这里的Λ_i,i是质量矩阵的第i,i项，对应于沿着第i轴的腿的表观质量。

2.地面作用力控制：

在地面力控制过程中，关节力矩如下：

其中R是将机体变换为世界坐标的旋转矩阵，J∈R^3×3为足端雅可比矩阵，f是预测控制器在世界坐标系中计算出的力向量。

考虑一个具有步长k的标准形式的MPC问题，x_i+1,ref是第i+1时刻的***参考轨迹，x_i+1是第i+1时刻***状态，转化为二次规划问题求解下面方程最优化的值：

约束x_i+1＝A_ix_i+B_iu_i,i＝0...k-1 (18)

D_iu_i＝0,i＝0...k-1 (20)

其中x_i为第i步时的***状态，u_i为第i步时的控制输入，Q_i和R_i是权值的对角正半定矩阵，A_i和B_i表示离散时间***动力学，C_i,

c_i表示控制输入的不等式约束，D_i是选择时间步长为i时，处于摆动相的脚所对应的力的矩阵。上述控制器通过最优化寻找一个控制输入序列，该序列将保证***沿着x_ref参考轨迹运行，在控制效果和跟踪进度之间进行折中。

3.MPC的求解

力约束：

对应的上面的公式(17)，结合摩擦锥进行受力分析，将所有的离开地面的足端所受到的足底力设为0，每条腿的6个不等式约束如下：

参考轨迹的生成：

利用所期望机器人得运动行为来构建参考轨迹。在我们设计的应用中，参考轨迹很简单，只包含非零的xy-速度、xy-位置、z位置、偏转和偏转率。采用适当的速度进行积分来确定偏转和xy-位置外，其他状态(横滚、俯仰、偏转率、俯仰率和z速度)总是设置为0。参考轨迹也被用来确定动力学约束和足端位置。

***离散化：

根据构建完的MPC控制器，求解MPC的方程，MPC的求解过程需要先将MPC的状态方程进行离散化，离散化以后再求解最优值。根据连续***离散化的方法，本实施例采用精确离散法将式(6)进行离散化，离散化的过程需要使用零阶保持器进行采样，假设以固定的很小的时间周期T进行采样、离散化，A_c和B_c离散化后可表示为：

利用上面的简化，再结合线性***的离散化公式可以化为下面形式：

x[n+1]＝A_dx[n]+B_d[n]u[n] (23)

将离散后的方程扩展到k维，将k维度的MPC转换成二次规划问题，则

离散化后的方程转化为二次规划问题进行求解，二次规

划问题的目标函数中变量u_i，关系如下：

X＝A_qpx₀+B_qpU (25)

其中，X表示下一刻的状态，A_qp和B_qp是式(22)离散化后得到的矩阵，x₀是***的初始状态，即上一时刻的***状态。

那么目标函数可以写成

J(U)＝(A_qpx₀+B_qpU-x_ref)^TL(A_qpx₀+B_qpU-x_ref)+U^TKU (26)

其中，矩阵L和矩阵K是对角阵。

利用qpOASES开源库对式(21)进行求解，可以得到最优的控制输入量U，即足底力F_i。构建完毕的模型预测控制结构如图2。

(3)斜坡稳定性分析：

当四足机器人以对角小跑步态在斜坡行走时，其质心沿重力方向上的投影在机器人四足支撑面内的位置会偏移到斜面负梯度方向，影响机器人在坡面的稳定性。此时，机器人步态规划中各腿关节的角度值不仅与模型预测控制中已有的足端轨迹有关，还应当结合所处地形的坡度。基于零点力矩稳定条件，通过调节机器人足端位置，以实现机器人质心的调整。

机器人在坡面上以对角小跑步态运动时，初始状态如图3所示。以质心作为零点力矩的稳定性参考，以机体坐标系原点作为机器人质心，足端调整前机器人质心投影落在支撑足对角线偏后方，若坡度倾角较大，会导致投影点落在支撑多边形外部，致使机器人失稳倾倒。

以两条支撑对角线交点为原点作半径为R的稳定圆，当机器人质心投影在稳定圆内时，投影点与两条对角支撑线距离的最大值小于圆的半径，即S<R，此时机器人满足零点力矩稳定判据。调整机器人质心，使其投影点落在稳定圆内，如图4所示，即可保证质心投影点同时位于两条支撑对角线的支撑多边形内。

(4)足端位置调整的扩展：

在动力学基础上扩展坐标系如图5：

机体坐标系∑_A：原点G_A位于机器人机体重心，X_A轴指向机器人运动前方，Y_A轴指向机器人的左方，Z_A轴垂直于机体向上。

世界坐标系∑_B：Z_B轴垂直于水平面向上，X_B轴为X_A轴在水平面的垂直投影，Y_B的方向遵循右手螺旋准则。

轨迹规划坐标系∑_M：其原点与∑_A原点重合，∑_M由∑_A旋转而成。

斜面坐标系∑_C：原点为机器人重心在斜面的投影，∑_C由∑_M旋转和平移得到。

由各坐标系间的关系可以得到：

^PHIPp_TOE＝R_y(θ_ad)R_x(ψ_ad)(^CHIPp_TOE+^Cp_HIP)+^MP_C-^MP_HIP (27)

其中^CHIPp_TOE为坐标系∑_C中足端相对于髋关节的位置，^PHIPp_TOE为坐标系∑_M中足端相对于髋关节的位置，^Cp_HIP和^MP_HIP为坐标系∑_C和∑_M中髋关节相对于原点的位置，^MP_C为坐标系∑_C在坐标系∑_M的位置。θ_ad与ψ_ad为坐标系∑_M到∑_C旋转中分别绕y轴、x轴的调整角。同时足端位置调整需要结合机器人躯干姿态，将直接引用下一小节中得出的俯仰角度值，由坐标系间的关系可得：

其中，^AHIPp_TOE为坐标系∑_A中足端相对于髋关节的位置，^AP_HIP为坐标系∑_A中髋关节的位置。

θ_ref与ψ_ref分别表示坐标系∑_M到∑_A旋转中绕z轴、y轴和x轴的扭转角、俯仰角与横滚角。

当四足机器人爬坡时，需要将支撑位置调整和躯干姿态调整相结合，本实施例依据躯干的姿态反馈设置支撑位置和躯干姿态参数，即令

这里θ_IMU为固定于躯干的惯性测量单元(IMU)测得的俯仰角，ψ_IMU为测得的横滚角。

于是式(1)和式(2)可结合并简化为下式

^AHIPp_TOE＝^SHIPp_TOE+R_x(-ψ_IMU)R_y(-θ_IMU)^MP_C (30)

其中，^Mp_C＝[0 0 -H]^T，是保持∑_C原点在∑_M中的坐标系为常向量，H为站立时的质心高度。此方程依据躯干姿态信息实现了足端位置的坐标映射，四足机器人可使用此映射实现斜坡足端位置的调整适应。

(5)斜坡姿态调整：

本发明身体姿态斜坡自适应算法在躯干调整中提出了“虚拟斜坡”的概念，“虚拟斜坡”如图7中所示，是四足机器人由平坦路面运动到斜坡时前后两支撑足端所形成的的二维平面(此时前腿支撑足落在斜坡上，后腿支撑足在地平面上)。在此过程中，“虚拟斜坡”的角度随着机器人的前进而实时改变，同时机器人的躯干受虚拟斜坡的影响而发生俯仰；此时IMU将当前状态下测得的躯干俯仰角度作为输入参数传递给自适应调整算法，求得当前状态下“虚拟斜坡”的角度，之后将此角度作为下一时刻机器人俯仰角的调整值，以此往复，当“虚拟斜坡”角度、躯干俯仰角和实际斜坡角度三者相等时，算法循环完毕，实现了坡面姿态自适应调整，如图6(a)、图6(b)所示。

以下两式为四足机器人斜坡姿态调整算法：

p＝(L+Δ_F+Δ_H)cosα (31)

β’＝α＝β (33)

式(31)中，p为前后两足端在水平地面上的投影，L为机器人躯干长度，Δ_F为机器人前腿足端相对于前腿髋关节的位移，α为IMU实时测得的机体俯仰角；式(32)中，β’为前后足端所成虚拟斜坡的角度，h为机器人前后足端的高度差，可由状态估计器得到。

考虑到四足机器人在以对角小跑步态爬坡时会产生姿态振荡，因此加入惯性环节以减小姿态扰动对姿态调整的影响，其中参数k与机器人的关节力控、状态估计值、行走速度和地面材料特性有关。因此式(32)可以重写为

本实施例为了补偿这种不确定因素的影响，使四足机器人在与斜坡相同材质的平坦地形中反复行走测试，结果显示在不确定因素影响下机器人俯仰角误差在0.7度至1.2度之间。并根据此误差结果，在爬坡实验中结合实际情况调整惯性参数k。四足机器人斜坡上的自适应调整流程如图8所示。

还提供以下产品实施例：

一种仿生机器人的身体姿态斜坡自适应控制***，包括：

动力学模型构建模块，被配置为搭建仿生机器人机体与世界坐标系，将机器人建模为单个刚体，且在接触点处受力，建立机器人动力学模型；

模型预测控制模块，被配置为计算并跟踪仿生机器人腿部在世界坐标系中的轨迹，并计算腿部关节力矩，考虑地面力控制过程中的关节力矩，利用模型预测控制确定最优控制轨迹，并根据所述最优控制轨迹控制仿生机器人的运动；

足端位置调整模块，被配置为根据机器人此时刻的姿态参数，确定足端位置的坐标映射，调整机器人在斜坡上的重心位置；

斜坡姿态调整模块，被配置为设置一虚拟斜坡，根据机器人此时刻姿态参数中躯干俯仰角度，计算当前状态下虚拟斜坡的角度，将此角度作为下一时刻机器人俯仰角的调整值，不断调整机器人的姿态，直到虚拟斜坡角度、躯干俯仰角和实际斜坡角度三者相等。

一种计算机可读存储介质，其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行所述的一种四足仿生机器人的身体姿态斜坡自适应控制方法。

一种终端设备，包括处理器和计算机可读存储介质，处理器用于实现各指令；计算机可读存储介质用于存储多条指令，所述指令适于由处理器加载并执行所述的一种四足仿生机器人的身体姿态斜坡自适应控制方法。

本领域内的技术人员应明白，本公开的实施例可提供为方法、***、或计算机程序产品。因此，本公开可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本公开可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本公开是参照根据本公开实施例的方法、设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述仅为本公开的优选实施例而已，并不用于限制本公开，对于本领域的技术人员来说，本公开可以有各种更改和变化。凡在本公开的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本公开的保护范围之内。

上述虽然结合附图对本公开的具体实施方式进行了描述，但并非对本公开保护范围的限制，所属领域技术人员应该明白，在本公开的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本公开的保护范围以内。

Claims (10)

1.一种四足仿生机器人的身体姿态斜坡自适应控制方法，其特征是：包括以下步骤：

搭建仿生机器人机体与世界坐标系，将机器人建模为单个刚体，且在接触点处受力，建立机器人动力学模型；

计算并跟踪仿生机器人腿部在世界坐标系中的轨迹，并计算腿部关节力矩，考虑地面力控制过程中的关节力矩，利用模型预测控制确定最优控制轨迹；

根据得到的最优控制轨迹控制仿生机器人的运动；

获取仿生机器人行走过程中此时刻的姿态参数，确定足端位置的坐标映射，调整机器人在斜坡上的重心位置；

设置一虚拟斜坡，根据机器人此时刻姿态参数中躯干俯仰角度，计算当前状态下虚拟斜坡的角度，将此角度作为下一时刻机器人俯仰角的调整值，不断调整机器人的姿态，直到虚拟斜坡角度、躯干俯仰角和实际斜坡角度三者相等。

2.如权利要求1所述的一种四足仿生机器人的身体姿态斜坡自适应控制方法，其特征是：搭建仿生机器人机体与世界坐标系，将机器人建模为单个刚体，且在接触点处受力，建立机器人动力学模型的具体过程包括：

搭建四足机器人机体与世界坐标系，使该模型把机器人建模为单个刚体，且在接触点处受力，根据每条腿受到的地面反力，机器人各个腿在地面的接触点位于机体坐标系的位置，确定机器人受到的重力与地面反作用力；

根据角动量与转矩之间的关系，计算机器人在世界坐标系下的惯性张量。

3.如权利要求1所述的一种四足仿生机器人的身体姿态斜坡自适应控制方法，其特征是：计算并跟踪仿生机器人腿部在世界坐标系中的轨迹，并计算腿部关节力矩的具体过程包括：

利用控制律计算腿i的关节力矩：

这里的J_i∈R^3×3为足端雅可比矩阵，K_p,K_d∈R^3×3是对角正定比例和导数增益矩阵，_Rp_{i, R}v_i∈R³为第i腿的位置和速度，_Rp_i,ref,Rv_i,ref∈R³为摆动腿位置和速度的参考运动轨迹；τ_i,ff∈R³为前馈转矩，相应得到下式：

其中Λ_i∈R^3×3是操作空间的惯性矩阵，a_i,ref∈R³为机体坐标系下的参考加速度，q_i∈R³是关节位置的矢量，

是腿的重力和科里奥利力产生的力矩。

4.如权利要求1所述的一种四足仿生机器人的身体姿态斜坡自适应控制方法，其特征是：考虑地面力控制过程中的关节力矩，利用模型预测控制确定最优控制轨迹的具体过程包括：

将确定最优控制轨迹转化为二次规划问题，在约束条件下，求解目标函数的最优化值：

其中，x_i+1,ref是第i+1时刻的***参考轨迹，x_i+1是第i+1时刻***状态，约束包括：

x_i+1＝A_ix_i+B_iu_i,i＝0...k-1

D_iu_i＝0,i＝0...k-1

其中x_i为第i步时的***状态，u_i为第i步时的控制输入，Q_i和R_i是权值的对角正半定矩阵，A_i和B_i表示离散时间***动力学，C_i,

c _i表示控制输入的不等式约束，D_i是选择时间步长为i时，处于摆动相的脚所对应的力的矩阵，通过最优化寻找一个控制输入序列，该序列将保证机器人***沿着x_ref参考轨迹运行，在控制效果和跟踪进度之间进行折中。

5.如权利要求4所述的一种四足仿生机器人的身体姿态斜坡自适应控制方法，其特征是：在求解目标函数的最优化值的过程中，采用精确离散法将目标函数进行离散化，离散化的过程使用零阶保持器进行采样，以固定的时间周期进行采样、离散化，将离散化后的目标函数转化为二次规划问题进行求解。

6.如权利要求1所述的一种四足仿生机器人的身体姿态斜坡自适应控制方法，其特征是：根据机器人的姿态参数，确定足端位置的坐标映射，调整机器人在斜坡上的重心位置的具体过程包括：

基于零点力矩的稳定条件，通过调节机器人足端位置，以实现机器人质心的调整，在动力学基础上，根据机体坐标系、世界坐标系、轨迹规划坐标系和斜面坐标系的关系，将依据躯干姿态信息进行足端位置的坐标映射，实现斜坡足端位置的调整适应。

7.如权利要求1所述的一种四足仿生机器人的身体姿态斜坡自适应控制方法，其特征是：设置一虚拟斜坡的具体过程为：将仿生机器人由平坦路面运动到斜坡时前后两支撑足端所形成的二维平面作为虚拟斜坡。

8.一种仿生机器人的身体姿态斜坡自适应控制***，其特征是：包括：

动力学模型构建模块，被配置为搭建仿生机器人机体与世界坐标系，将机器人建模为单个刚体，且在接触点处受力，建立机器人动力学模型；

模型预测控制模块，被配置为计算并跟踪仿生机器人腿部在世界坐标系中的轨迹，并计算腿部关节力矩，考虑地面力控制过程中的关节力矩，利用模型预测控制确定最优控制轨迹，并根据所述最优控制轨迹控制仿生机器人的运动；

足端位置调整模块，被配置为根据机器人此时刻的姿态参数，确定足端位置的坐标映射，调整机器人在斜坡上的重心位置；

斜坡姿态调整模块，被配置为设置一虚拟斜坡，根据机器人此时刻姿态参数中躯干俯仰角度，计算当前状态下虚拟斜坡的角度，将此角度作为下一时刻机器人俯仰角的调整值，不断调整机器人的姿态，直到虚拟斜坡角度、躯干俯仰角和实际斜坡角度三者相等。

9.一种计算机可读存储介质，其特征是：其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行权利要求1-7中任一项所述的一种四足仿生机器人的身体姿态斜坡自适应控制方法。

10.一种终端设备，其特征是：包括处理器和计算机可读存储介质，处理器用于实现各指令；计算机可读存储介质用于存储多条指令，所述指令适于由处理器加载并执行权利要求1-7中任一项所述的一种四足仿生机器人的身体姿态斜坡自适应控制方法。

Images