四足机器人行走与对角步态运动控制方法

1.本技术涉及机器人技术领域，具体地涉及一种四足机器人行走与对角步态运动控制方法。


背景技术：

2.四足步行机器人是机器人的一个重要分支，四足机器人比两足步行机器人承载能力强、稳定性好，又比六足、八足步行机器人结构简单，但四足机器人属于典型的高维度，非线性系统，具有强非线性，离散动力学等问题，所以设计机器人步态算法以保证机器人在不平坦地形中能够稳定的移动，对外界扰动具有较强的鲁棒性是步态生成及优化方法中的关键。
3.目前，对于四足机器人在行走步态规划和稳定控制算法的研究大多集中在基于动力学模型、基于跟踪控制的步态控制方法、基于智能算法的步态控制方法等方面，基于动力学模型的步态控制方法往往需要复杂的轨迹规划及运算求解，基于跟踪控制的步态控制方法速度慢且适用的步态形式少，基于智能算法的步态控制方法需要经过大量的样本数据训练，计算量大。因此，需要一种能够在线规划、不简化及忽略双足机器人全身动力学因素的相互耦合、结构简单、计算量小、鲁棒性高并适用于双足和四足机器人的步态生成及优化方法。


技术实现要素：

4.为了克服现有技术的不足，本发明提出的运动控制方法利用低维点质量模型抽象机器人整机动力学，通过离散化方法预测整段时间内广义离散状态方程；将四足机器人步态模式与双足机器人步态模式进行映射，引入线性约束域对四足机器人运动步态统一化处理；能够同时解决四足机器人行走和小跑步态的运动控制问题。
5.为实现上述目的，本发明所采用的解决方案为：
6.一种四足机器人行走与对角步态运动控制方法，其包括以下步骤：
7.步骤1：利用低维点质量模型模拟四足机器人运动，用离散化方法建立预观时域运动过程的离散状态方程；
8.采用低维点质量模型模拟四足机器人运动，并用状态空间方程表示点质量模型，并通过线性化将点质量模型等效为线性倒立摆模型，进一步转化为离散时间状态空间表达式，最终对离散时间状态空间模型进行递归，推导出质心位置、质心速度、质心加速度、质心加加速度和质心压力中心位置的离散状态方程；
9.步骤2：通过线性约束域处理四足机器人行走和小跑步态，确定优化区间；
10.将四足机器人步态模式与双足机器人步态模式进行映射，通过线性约束域对四足机器人行走和小跑步态进行统一化处理；根据线性约束域确定的优化区间如下所示：
[0011][0012]
式中：p
f0
表示当前落足点位置；δx和δy分别表示横坐标方向和纵坐标方向的压力中心设定范围；x
cop
和y
cop
分别表示横坐标方向和纵坐标方向机器人的压力中心；和分别表示横坐标方向和纵坐标方向机器人的状态向量；p
zu
和p
zs
分别为机器人压力中心的控制和状态系数矩阵；和分别表示横坐标方向和纵坐标方向的质心加加速度；
[0013]
步骤3：建立预观时域内机器人步态运动控制模型的性能指标评价函数，求解机器人的期望落足点和质心位置；
[0014]
基于横坐标方向和纵坐标方向系统状态变量的不相关性，定义质心速度跟踪、压力中心跟踪与控制输入最小的性能指标评价函数；预观时域内横坐标方向和纵坐标方向的性能指标评价函数如下所示：
[0015][0016]
式中：x和y分别表示横坐标方向和纵坐标方向；j
x/y
表示性能评价指标函数；ω1表示质心速度跟踪权重系数；ω2表示压力中心跟踪权重系数；ω3表示质心加加速度跟踪权重系数；表示k 1时刻的质心速度向量；表示k 1时刻的期望质心速度状态向量；x/y
cop
(k 1)表示k 1时刻的压力中心状态向量；表示k 1时刻的期望压力中心状态向量；u
x/y
(k)表示k时刻的状态方程的控制输入向量；
[0017]
步骤4：根据机器人的期望落足点和质心位置，实现四足机器人运动控制；
[0018]
获取步骤3中期望质心位置和期望落足点位置，确定线性预测控制模型，根据步骤2确定的优化区间，进一步通过正则凸二次规划方法，构建性能指标评价函数的最优化模型，如下所示：
[0019][0020]
式中：u
p
表示状态方程的待优化向量；u
pt
表示状态方程的待优化向量的转置；h表示海森矩阵；f
t
表示性能指标的一次项函数；c表示不等式系数矩阵；c表示不等式设定系数；u
l
和uu表示状态方程控制输入向量下极限和上极限；
[0021]
其中，状态方程的待优化变量具体如下所示：
[0022]up
＝[u
x/y
(k)
t p
x/y
(k)
t
]
t
；
[0023]
式中：u
x/y
(k)表示状态方程的控制输入向量；p
x/y
(k)表示预观时域内落足点位置；
[0024]
根据状态方程的控制输入向量和预观时域内落足点位置实现四足机器人的运动
控制。
[0025]
可优选的是，所述步骤1中的推导出整个预观时域中的广义离散状态方程具体过程如下：
[0026]
首先，采用低维点质量模型等效机器人动力学表达式为：
[0027][0028]
式中：x(t)表示t时状态方程的系统状态向量；表示t时系统状态向量的一阶导数；y(t)表示t时系统状态输出向量；u(t)表示t时状态方程控制输入向量；i表示单位向量；z
com
表示z方向质心位置；表示z方向质心加速度；g表示重力加速度；t表示时间尺度；
[0029]
然后，设定滚动预观时域为nt，系统在第k个采样时间间隔，将模型改写为离散时间状态空间表达式，如下所示：
[0030][0031]
式中：a表示系统状态矩阵；b表示系统输入变量矩阵；c表示系统输出变量矩阵；x(k)表示离散时间为k时状态方程的系统状态向量；x(k 1)表示离散时间为k 1时状态方程的系统状态向量；u(k)表示离散时间为k时状态方程控制输入向量；y(k)表示离散时间为k时系统状态输出向量。
[0032]
系统状态矩阵、系统输入变量矩阵和系统输出变量矩阵如下所示：
[0033][0034]
可优选的是，所述步骤1中的推导出质心加加速度，质心位置、质心速度和质心加速度，质心压力中心位置的离散状态方程，具体为：
[0035]
所述的质心加加速度的离散状态方程如下所示：
[0036][0037]
式中：u(k)表示离散时间k时刻的状态方程控制输入向量；u(ki|ki)表示ki时刻的状态方程控制输入值，为质心加加速度；ki表示第i个时刻的离散时间点；n表示预观时域的长度；
[0038]
[0039]
所述质心位置、质心速度和质心加速度的离散时间状态向量如下所示：
[0040][0041]
式中：x[k 1]表示离散时间k 1时刻的系统状态向量；x(ki 1|ki)表示ki 1时刻的系统状态向量；
[0042][0043]
式中：x
com
和y
com
分别表示横坐标方向和纵坐标方向的质心位置；和分别表示横坐标方向和纵坐标方向的质心速度；和分别表示横坐标方向和纵坐标方向的质心加速度；
[0044]
所述质心压力中心位置的离散状态方程如下所示：
[0045][0046]
式中：y[k 1]表示离散时间k 1时刻的系统状态输出向量；y(ki 1|ki)表示ki 1时刻的系统状态输出值；
[0047]
y(ki 1|ki)＝[x
cop
,y
cop
]
t
；
[0048]
式中：x
cop
和y
cop
分别表示横坐标方向和纵坐标方向的压力中心位置。
[0049]
可优选的是，所述步骤2中引入的压力中心约束域为矩形约束域，能够解决模型解耦过程中存在一个方向压力中心运动会造成对另一个方向运动影响问题。
[0050]
可优选的是，所述步骤2中机器人压力中心的控制和状态系数矩阵，具体为；
[0051]
机器人压力中心的控制系数矩阵如下所示：
[0052][0053]
式中：t
l
表示采样时间间隔；
[0054]
所述的机器人压力中心的状态系数矩阵如下所示：
[0055][0056]
可优选的是，所述步骤3中期望落足点位置与期望质心位置具体求解过程如下：
[0057]
确定当前落足点与预观时域内的未来落足点，以确定预观时域内的参考压力中心，期望落足点如下所示：
[0058][0059]
式中：v(k 1)表示当前时域内所有采样时刻的落足点选择向量；表示当前时刻落足点；表示预观时域内所有采样时刻的落足点选择矩阵；表示预观时域落足
点位置；
[0060]
预观时域落足点位置如下所示：
[0061][0062]
式中：表示第j个落足点位置，j取值为1、2或3；
[0063]
任意一个时刻质心期望位置表达式如下所示：
[0064][0065]
式中：表示第i时刻横坐标方向期望落足点位置；ωa表示第一权重系数；d表示相对于位置的函数矩阵；表示结束状态横坐标方向的质心位置；表示开始状态横坐标方向的质心位置；tc表示质心期望位置的采样间隔时间，表示质心期望位置的采样间隔时间，表示开始状态横坐标方向的质心速度；ωb表示第二权重系数；
[0066]
所述相对于位置的函数矩阵获取方法如下所示：
[0067][0068]
与现有技术相比，本发明的有益效果在于：
[0069]
(1)本发明提出的运动控制方法利用低维点质量模型抽象机器人整机动力学，用离散化方法预测整段时间内广义离散状态方程；将四足机器人步态模式与双足机器人步态模式进行映射，引入线性约束域对四足机器人行走和小跑步态统一化处理；能同时解决四足机器人行走和小跑步态的运动控制问题；
[0070]
(2)本发明方法与传统运动步态控制方法相比，结构简单，模型预测控制和基于改进参考压力中心的立足点适应使四足机器人运动对外干扰具有高鲁棒性，更符合实际运用工况，应用前景广阔。
附图说明
[0071]
图1为本发明实施例四足机器人行走与对角步态运动控制方法的控制框图；
[0072]
图2为本发明实施例四足机器人与双足机器人l-s步态模式映射图；
[0073]
图3为本发明实施例四足机器人与双足机器人对角步态模式映射图；
[0074]
图4为本发明实施例四足机器人行走步态与对角运动步态运动说明图；
[0075]
图5为本发明实施例预测视界上矢状面离散质心位置与落足点模型展示图；
[0076]
图6为本发明实施例walk步态质心轨迹与落足点规划结果图；
[0077]
图7为本发明实施例trot步态质心轨迹与落足点规划结果图；
[0078]
图8为本发明实施例该发明技术方法抗干扰能力展示图。
具体实施方式
[0079]
以下，参照附图对本发明的实施方式进行说明。
[0080]
本发明实施例提出的运动控制方法利用低维点质量模型抽象机器人整机动力学，
用离散化方法预测整段时间内广义离散状态方程；将四足机器人步态模式与双足机器人步态模式进行映射，引入线性约束域对四足机器人行走和小跑步态统一化处理；能同时解决四足机器人行走和小跑步态的运动控制问题。如图1所示为本发明实施例四足机器人行走与对角步态运动控制方法的控制框图。本发明实施例提出的方法与传统运动步态控制方法相比，结构简单，模型预测控制和基于改进参考压力中心的立足点适应使四足机器人运动对外干扰具有高鲁棒性，更符合实际运用工况，应用前景更加广阔。
[0081]
本发明实施例提供了一种四足机器人行走与对角步态运动控制方法，为了证明本发明的适用性，将其应用于实例，具体包含如下步骤：
[0082]
s1：利用低维点质量模型模拟四足机器人运动，用离散化方法建立预观时域运动过程的离散状态方程；
[0083]
采用低维点质量模型模拟四足机器人运动，并用状态空间方程表示点质量模型，并通过线性化将点质量模型等效为线性倒立摆模型，进一步转化为离散时间状态空间表达式，最终对离散时间状态空间模型进行递归，推导出质心位置、质心速度、质心加速度、质心加加速度和质心压力中心位置的离散状态方程。
[0084]
推导出整个预观时域中的广义离散状态方程具体过程如下：
[0085]
首先，采用低维点质量模型等效机器人动力学表达式为：
[0086][0087]
式中：x(t)表示t时状态方程的系统状态向量；表示t时系统状态向量的一阶导数；y(t)表示t时系统状态输出向量；u(t)表示t时状态方程控制输入向量；i表示单位向量；z
com
表示z方向质心位置；表示z方向质心加速度；g表示重力加速度；t表示时间尺度。
[0088]
然后，设定滚动预观时域为nt，系统在第k个采样时间间隔，将模型改写为离散时间状态空间表达式，如下所示：
[0089][0090]
式中：a表示系统状态矩阵；b表示系统输入变量矩阵；c表示系统输出变量矩阵；x(k)表示离散时间为k时状态方程的系统状态向量；x(k 1)表示离散时间为k 1时状态方程的系统状态向量；u(k)表示离散时间为k时状态方程控制输入向量；y(k)表示离散时间为k时系统状态输出向量。
[0091]
所述的质心加加速度的离散状态方程如下所示：
[0092][0093]
式中：u(k)表示离散时间k时刻的状态方程控制输入向量；u(ki|ki)表示ki时刻的状态方程控制输入值，为质心加加速度；ki表示第i个时刻的离散时间点；n表示预观时域的长度。
[0094][0095]
所述质心位置、质心速度和质心加速度的离散时间状态向量如下所示：
[0096][0097]
式中：x[k 1]表示离散时间k 1时刻的系统状态向量；x(ki 1|ki)表示ki 1时刻的系统状态向量。
[0098][0099]
式中：x
com
和y
com
分别表示横坐标方向和纵坐标方向的质心位置；和分别表示横坐标方向和纵坐标方向的质心速度；和分别表示横坐标方向和纵坐标方向的质心加速度。
[0100]
所述质心压力中心位置的离散状态方程如下所示：
[0101][0102]
式中：y[k 1]表示离散时间k 1时刻的系统状态输出向量；y(ki 1|ki)表示ki 1时刻的系统状态输出值；
[0103]
y(ki 1|ki)＝[x
cop
,y
cop
]
t
；
[0104]
式中：x
cop
和y
cop
分别表示横坐标方向和纵坐标方向的压力中心位置。
[0105]
s2：通过线性约束域处理四足机器人行走和小跑步态，确定优化区间；
[0106]
将四足机器人步态模式与双足机器人步态模式进行映射，通过线性约束域对四足机器人行走和小跑步态进行统一化处理；根据线性约束域确定的优化区间如下所示：
[0107][0108]
式中：p
f0
表示当前落足点位置；δx和δy分别表示横坐标方向和纵坐标方向的压力中心设定范围；x
cop
和y
cop
分别表示横坐标方向和纵坐标方向机器人的压力中心；和分别表示横坐标方向和纵坐标方向机器人的状态向量；p
zu
和p
zs
分别为机器人压力中心的控制和状态系数矩阵；和分别表示横坐标方向和纵坐标方向的质心加加速度。
[0109]
引入的压力中心约束域为矩形约束域，能够解决模型解耦过程中存在一个方向压力中心运动会造成对另一个方向运动影响问题。
[0110]
机器人压力中心的控制系数矩阵如下所示：
[0111]
[0112]
式中：t
l
表示采样时间间隔；
[0113]
所述的机器人压力中心的状态系数矩阵如下所示：
[0114][0115]
如图2为所示本发明实施例四足机器人与双足机器人l-s步态模式映射图；如图3所示为本发明实施例四足机器人与双足机器人对角步态模式映射图；对于四足行走步态，其右后腿或左后腿与右前腿或左后腿的腾空与着地相类似于双足机器人的虚拟右腿或左腿的腾空与着地相，灰色阴影区域为四足支撑相。四足机器人对角运动步态，其四足机器人运动步态模式可较为直观的转化为双足机器人步态模式，进而与线性倒立摆模型进行匹配。
[0116]
常规情况下，如图4为所示为本发明实施例四足机器人行走步态与对角运动步态运动说明图；四足机器人在行走步态的压力中心约束可行运动域较大，对应到虚拟双足为非线性约束，对应到虚拟双足着地相的足形状从三角形1变化到三角形2。对角运动步态中虚拟足形状虽然不变，但其压力中心的可行域为两着地足连线的直线段，不仅其对于压力中心可行域为非线性约束，且对压力中心约束域的限制更大。
[0117]
为了实现两种步态的统一化处理，引入线性约束域，如图4中的矩形区域所示，将该矩形线性约束域作为对角运动步态压力中心的约束域，通过限制矩形约束域解决模型解耦过程中，一个方向压力中心运动造成对另一方向运动影响；模型误差造成的预观轨迹偏差通过以下捕获点理论解决。
[0118]
s3：建立预观时域内机器人步态运动控制模型的性能指标评价函数，求解机器人的期望落足点和质心位置；
[0119]
基于横坐标方向和纵坐标方向系统状态变量的不相关性，定义质心速度跟踪、压力中心跟踪与控制输入最小的性能指标评价函数；预观时域内横坐标方向和纵坐标方向的性能指标评价函数如下所示：
[0120][0121]
式中：x和y分别表示横坐标方向和纵坐标方向；j
x/y
表示性能评价指标函数；ω1表示质心速度跟踪权重系数；ω2表示压力中心跟踪权重系数；ω3表示质心加加速度跟踪权重系数；表示k 1时刻的质心速度向量；表示k 1时刻的期望质心速度状态向量；x/y
cop
(k 1)表示k 1时刻的压力中心状态向量；表示k 1时刻的期望压力中心状态向量；u
x/y
(k)表示k时刻的状态方程的控制输入向量。
[0122]
期望落足点位置与期望质心位置具体求解过程如下：
[0123]
确定当前落足点与预观时域内的未来落足点，以确定预观时域内的参考压力中心，期望落足点如下所示：
[0124]
[0125]
式中：v(k 1)表示当前时域内所有采样时刻的落足点选择向量；表示当前时刻落足点；表示预观时域内所有采样时刻的落足点选择矩阵；表示预观时域落足点位置。
[0126]
预观时域落足点位置如下所示：
[0127][0128]
式中：表示第j个落足点位置，j取值为1、2或3。
[0129]
任意一个时刻质心期望位置表达式如下所示：
[0130][0131]
式中：表示第i时刻横坐标方向期望落足点位置；ωa表示第一权重系数；d表示相对于位置的函数矩阵；表示结束状态横坐标方向的质心位置；表示开始状态横坐标方向的质心位置；tc表示质心期望位置的采样间隔时间，表示质心期望位置的采样间隔时间，表示开始状态横坐标方向的质心速度；ωb表示第二权重系数。
[0132]
所述相对于位置的函数矩阵获取方法如下所示：
[0133][0134]
如图5所示为本发明实施例预测视界上矢状面离散质心位置与落足点模型展示图。
[0135]
s4：根据机器人的期望落足点和质心位置，实现四足机器人运动控制；
[0136]
获取s3中期望质心位置和期望落足点位置，确定线性预测控制模型，根据s2确定的优化区间，进一步通过正则凸二次规划方法，构建性能指标评价函数的最优化模型，如下所示：
[0137][0138]
式中：u
p
表示状态方程的待优化向量；u
pt
表示状态方程的待优化向量的转置；h表示海森矩阵；f
t
表示性能指标的一次项函数；c表示不等式系数矩阵；c表示不等式设定系数；u
l
和uu表示状态方程控制输入向量下极限和上极限。
[0139]
其中，状态方程的待优化变量具体如下所示：
[0140]up
＝[u
x/y
(k)
t p
x/y
(k)
t
]
t
；
[0141]
式中：u
x/y
(k)表示状态方程的控制输入向量；p
x/y
(k)表示预观时域内落足点位置。
[0142]
根据状态方程的控制输入向量和预观时域内落足点位置实现四足机器人的运动控制。
[0143]
最终，将该在线运动规划算法用于四足机器人行走步态与对角运动步态中进行测试，可到规划的质心轨迹与落足点位置，如图6所示为本发明实施例walk步态质心轨迹与落
足点规划结果图；如图7所示为本发明实施例trot步态质心轨迹与落足点规划结果图；通过mpc在线运动规划算法在四足机器人行走步态与对角运动步态中的实施，四足机器人不仅能按照预期规划出质心轨迹与落足点，而且在外界扰动下能实时调节质心轨迹与落足点，对扰动具有一定的鲁棒性。如图8所示为本发明实施例该发明技术方法抗干扰能力展示图，证明了基于改进参考压力中心的立足点适应使四足机器人运动对外界干扰具有高鲁棒性。
[0144]
综上，本案例四足机器人行走与对角步态运动控制方法的计算结果证明了具有很好的效果。
[0145]
(1)本发明实施例提出的运动控制方法利用低维点质量模型抽象机器人整机动力学，用离散化方法预测整段时间内广义离散状态方程；将四足机器人步态模式与双足机器人步态模式进行映射，引入线性约束域对四足机器人行走和小跑步态统一化处理；能同时解决四足机器人行走和小跑步态的运动控制问题，通过附图的验证结果证明本方法能够较好的完成机器人的运动控制过程；
[0146]
(2)本发明实施例介绍的方法与传统运动步态控制方法相比，结构更加简单，模型预测控制效果更好，附图证明了基于改进参考压力中心的立足点适应使四足机器人运动对外界干扰具有高鲁棒性，更符合实际运用工况，应用前景广阔。
[0147]
以上所述的实施例仅是对本发明的优选实施方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。