CN115963850B - 四足机器人及其坡度地形环境自适应运动方法与控制系统 - Google Patents

Abstract

本发明公开了一种四足机器人坡度地形环境自适应运动方法，四足机器人在运动期间实时感知地形坡度，根据所述坡度自适应调节机器人的运行状态；其中，在运动期间实时感知地形坡度的过程包括：将当前步态周期内支撑腿触地点位置以及摆动腿在前一步态周期内的触地点位置分别代入地形的平面方程，构成超定方程组；利用二次规划方法求解所述超定方程组的最小二乘解，得到当前地形的坡度。在四足机器人运动期间通过上述方法实时感知地形坡度，并根据最近计算出的坡度值调整自适应调节机器人的运行状态，无需提前设定固定的坡度，可使机器人在坡度变化的复杂地形下自适应行走。

Description

四足机器人及其坡度地形环境自适应运动方法与控制系统

技术领域

本发明属于机器人控制技术领域，更具体地，涉及一种四足机器人及其坡度地形环境自适应运动方法与控制系统。

背景技术

足式移动形式依靠离散的落足点推进机体前进，相比于传统的轮式和履带移动形式，具备更强的复杂野外地形环境适应性和机动性，因而成为野外环境下机器人的最佳移动形式。足式机器人已经成为机器人领域研究和应用开发的热点，四足机器人兼具运动的快速性、灵活性和动态稳定性，更加受到足式机器人研究者和开发者的青睐。

近年来，在足式机器人，特别是四足机器人的运动控制方面，取得了令人鼓舞的研究进展。论文《四足机器人坡面行走稳定性分析》(东北大学学报，2018年，第39卷，第5期，673-678页)中通过调节四足机器人前后腿腿长的方法来调节质心位置和姿态，实现四足机器人对角小跑步态在10°的坡面上行走。韩宝玲等人在期刊论文《四足机器人坡面运动时的姿态调整技术》(北京理工大学学报，2016年，第36卷，第3期，242-246页)中以机器人质心在斜面上的落点到支撑线的距离作为稳定性判据，调整机身姿态，实现四足机器人在18°的坡面以对角小跑步态上下坡运动。上述几种方法需要提前设定斜坡角度，没有对斜坡地形坡面角度在线估计，无法实现斜坡地形的自适应行走。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种四足机器人及其坡度地形环境自适应运动方法与控制系统，其目的在于在机器人运动时在线估计坡面角度，实现在斜坡地形上的自适应行走。

为实现上述目的，按照本发明的一个方面，提供了一种四足机器人坡度地形环境自适应运动方法，四足机器人在运动期间实时感知地形坡度，根据所述坡度自适应调节机器人的运行状态；

其中，在运动期间实时感知地形坡度的过程包括：

将当前步态周期内支撑腿触地点位置以及摆动腿在前一步态周期内的触地点位置分别代入地形的平面方程，构成超定方程组；

利用二次规划方法求解所述超定方程组的最小二乘解，得到当前地形的坡度。

在其中一个实施例中，根据当前步态周期的步态规划识别第i条腿为支撑腿还是摆动腿，识别方法包括：

当第i条腿在当前时刻的规划步态为摆动相，则直接认为第i条腿为摆动腿，并以第i条腿在前一步态周期内的触地点位置代入地形的平面方程；

当第i条腿的在当前时刻的规划步态为接触相且α<0.2或α>0.8，则认为第i条腿为摆动腿，并以第i条腿在前一步态周期内的触地点位置代入地形的平面方程；

当第i条腿的在当前时刻的规划步态为接触相且α<0.2或α>0.8，则认为第i条腿为支撑腿，并以第i条腿在当前步态周期内的触地点位置代入地形的平面方程；

其中，α为当前时刻在接触相内的时间相位。

在其中一个实施例中，令地形的平面方程为：

Ax+By+Cz+D＝0

其中，A,B,C,D是所述平面方程的系数，(x,y,z)表示地形平面上的点在世界坐标系下的坐标；

利用二次规划方法求解所述超定方程组的最小二乘解，包括：

根据所述超定方程组构建二次规划求解模型为：

其中，‖·‖²运算符表示矩阵的2-范数，为第i个腿代入所述平面方程的触地点位置；

设定约束条件后求解所述二次规划求解模型，其中，所述约束条件包括：

在其中一个实施例中，根据所述坡度自适应调节机器人的运行状态，包括：根据所述坡度调整机身俯仰角和摆动腿足端名义支撑点位置，具体包括：

以所述坡度的角度θ作为机身的俯仰角；

以机器人髋关节沿垂直于地形平面的投影位置沿坡度下移调整值Δ的位置作为与髋关节相连的摆动腿的足端名义支撑点位置，Δ＝h^cmdtanθ，其中，h^cmd表示机器人机身的期望高度。

在其中一个实施例中，根据所述坡度自适应调节机器人的运行状态，包括：根据所述坡度调整摆动轨迹，具体包括：

在世界坐标系(X_W,Y_W,Z_W)下，采用三次贝塞尔曲线规划假定所述坡度的角度为0时第i条腿在摆动相的足端运动轨迹，得到在第i条腿摆动时所规划的起始点位置p_i,lift和终止点位置p_i,touch，其中，X_WY_W为水平面；

将世界坐标旋转角度θ，得到摆动坐标系(X_S,Y_S,Z_S)，计算从世界坐标系到摆动坐标系的旋转变化矩阵其中，角度θ为所述坡度的真实角度，X_SY_S构成的平面与地形的平面平行；

根据起始点位置p_i,lift和终止点位置p_i,touch计算得到在所述坡度的角度为θ时的起始点位置和终止点位置/>

在其中一个实施例中，机器人运动控制任务按照优先级由高至低包括足端接触力控制、机身位置控制、机身姿态控制和足端摆动轨迹跟踪四个子任务；

根据所述坡度自适应调节机器人的运行状态，包括：将各子任务的阻抗力映射至关节驱动力矩，具体包括：

分别计算机身位置控制子任务的阻抗力机身姿态控制子任务的阻抗力第i摆动腿足端摆动轨迹跟踪子任务的阻抗力/>

计算关节驱动力矩，计算公式为：

其中，τ表示关节驱动力矩矢量，表示足端接触力对应的雅可比矩阵，n_c表示四足机器人支撑腿数量，n表示四足机器人广义坐标系数量，表示规划的足端接触力且下标表示支撑腿的序号，J_i|p(i)表示子任务i的雅可比矩阵，/>表示摆动腿阻抗力矢量。

按照本发明的另一方面，提供了一种四足机器人坡度地形环境自适应运动控制系统，包括地形估计器，用于在四足机器人运动期间实时感知地形坡度；

所述地形估计器包括：

超定方程组构建单元，用于获取当前步态周期内支撑腿触地点位置以及摆动腿在前一步态周期内的触地点位置并带入地形的平面方程，构成超定方程组；

求解单元，用于利用二次规划方法求解所述超定方程组的最小二乘解，得到当前地形的坡度。

在其中一个实施例中，还包括轨迹规划器，所述轨迹规划器包括：

机身俯仰角调节单元，用于以最新计算出的所述坡度的角度作为机身的俯仰角；

足端名义支撑点位置调节单元，用于以机器人髋关节沿垂直于地形平面的投影位置沿坡度下移调整值Δ的位置作为与髋关节相连的摆动腿的足端名义支撑点位置，Δ＝h^cmd tanθ，其中，h^cmd表示机器人机身的期望高度；

摆动轨迹调节单元，用于规划摆动腿的摆动轨迹。

在其中一个实施例中，还包括用于输出关节控制指令的轨迹控制器，所述轨迹控制器包括：

阻抗力计算单元，用于分别计算机身位置控制子任务的阻抗力机身姿态控制子任务的阻抗力/>第i摆动腿足端摆动轨迹跟踪子任务的阻抗力/>

关节驱动力矩计算单元，用于关节驱动力矩计算，计算公式为：

其中，τ表示关节驱动力矩矢量，表示足端接触力对应的雅可比矩阵，n_c表示四足机器人支撑腿数量，n表示四足机器人广义坐标系数量，表示规划的足端接触力且下标表示支撑腿的序号，J_i|p(i)表示子任务i的雅可比矩阵，/>表示摆动腿阻抗力矢量。

按照本发明的再一方面，一种四足机器人，包含上述的四足机器人坡度地形环境自适应运动控制系统。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

本发明通过解算足端位置信息并带入平面方程，其中，由于运动过程中始终存在两脚同时着地的情况，但无法根据这两个点确定平面方程，为此选取另外两条摆动腿在上一步态周期时的触地点位置，这样在每个控制周期内就能组成四个落脚点位置，可得到四元齐次线性方程组，该方程组为四个方程三个未知数，是典型的超定方程组，存在唯一的最小二乘解。因此，通过二次规划方法可对该方程求解，解得平面方程中的各系数，从而得到地形的坡度。在四足机器人运动期间通过上述方法实时感知地形坡度，并根据最近计算出的坡度值自适应调节机器人的运行状态，无需提前设定固定的坡度，可使机器人在坡度变化的复杂地形下自适应行走。

附图说明

图1为一实施例的四足机器人在运动期间实时感知地形坡度的步骤流程图；

图2为一实施例的四足机器人爬坡状态示意图；

图3为一实施例的四足机器人机身俯仰角和足端名义支撑点位置调整策略对比图，其中，(a)为第一种策略的机身俯仰角和足端名义支撑点位置调节示意图，(b)为第二种策略的机身俯仰角和足端名义支撑点位置调节示意图，(c)和(d)分别为本实施例中的上坡和下坡对应的机身俯仰角和足端名义支撑点位置调节示意图；

图4为一实施例的四足机器人斜坡地形下腿部摆动轨迹调整示意图；

图5为一实施例的四足机器人坡度地形环境自适应运动控制系统的结构框图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示为四足机器人在运动期间实时感知地形坡度的步骤流程图，其主要包括以下步骤：

步骤S110：将当前步态周期内支撑腿触地点位置以及摆动腿在前一步态周期内的触地点位置分别代入地形的平面方程，构成超定方程组。

如图2所示为四足机器人爬坡状态示意图，对四足机器人而言，其独立的四个支撑点，是机器人与地形交互作用的主要途径，根据四足机器人机身惯性测量单元和关节位置反馈，可以实时估计出落脚点在世界坐标系下的位置。图2中，θ表示斜坡地形的倾斜角度，和/>分别表示世界坐标系和机身坐标系，p_f表示机身质心在世界坐标系下的位置，p_i表示第i条腿足端在世界坐标系下的位置矢量，r_i表示在世界坐标系下描述的机身质心到第i条腿足端的位置矢量。

首先，我们假设机器人四条支撑腿所在的虚拟平面始终与地形平面的倾斜角度相一致，这样地形的平面方程定义为，

Ax+By+Cz+D＝0 (1)

式中，[x,y,z]^T表示地形平面上的点在世界坐标系下的坐标，A,B,C,D是平面方程的系数，则地形平面的法向量表示为[A,B,C]^T。根据平面方程的定义，确定平面方程至少需要三个不共线的点。针对四足机器人对角小跑步态，运动过程中始终存在两脚同时着地的情况，但无法根据这两个点确定平面方程，为此可选取另外两条摆动腿在上一步态周期时的触地点位置，这样在每个控制周期内就能组成四个落脚点位置，可得到四元齐次线性方程组：

式中，为第i个腿代入方程的世界坐标系下的触地点位置。

由于需获取当前时刻两支撑腿的触地点位置和两摆动腿的触地点位置，因此，需要先识别四条腿中的摆动腿和支撑腿。在一实施例中，可以直接通过足端传感器检测是否触地来判断当前腿为支撑腿还是摆动腿。

然而，考虑到足端传感器容易损坏，本实施例不依赖足端传感器，而是根据当前步态周期的步态规划识别第i条腿为支撑腿还是摆动腿。因为机器人在运动时会规划每条腿的步态，即提前规划何时处于接触相，何时处于摆动相，控制机器人按照随规划的步运动。由于四足机器人采用的步态规划方法以时间为参考，难免出现实际接触状态与规划接触时刻错位的情况，例如，按照规划，当前该腿应该是已经触地，但实际情况可能还没来得及触地，或者按照规划，当前该腿应该是还处于触地状态，但实际情况可能已经提前摆动。为了减少接触状态不确定带来的支撑点位置误差，对于支撑腿，本实施例选取一个置信区间，在该区间内，认为支撑腿足端与地面稳定且不打滑的接触，这样的选择策略为：

当第i条腿在当前时刻的规划步态为摆动相，则直接认为第i条腿为摆动腿，并以第i条腿在前一步态周期内的触地点位置代入地形的平面方程，即

当第i条腿的在当前时刻的规划步态为接触相且α<0.2或α>0.8，则认为第i条腿为摆动腿，并以第i条腿在前一步态周期内的触地点位置代入地形的平面方程，即

当第i条腿的在当前时刻的规划步态为接触相且α<0.2或α>0.8，则认为第i条腿为支撑腿，并以第i条腿在当前步态周期内的触地点位置代入地形的平面方程，即

综合为：

式中，表示摆动腿i在上一个接触相周期时的落脚点位置，α为接触相内的时间相位，我们取接触相位在0.2～0.8之间为足端与地面形成铰接状态的置信区间。其中，设定接触相时间区间为[t1，t2]，当前时刻t3在接触相内的时间相位为/>

由于实际机器人所要攀爬的斜坡地形不会平行于世界坐标系的z坐标轴，则平面方程的系数C不可能为0，通过对上述等式两端同时除以C，并变换方程顺序，我们可以变形得到如下等式，

式中，代表需要估计的新的平面方程系数。可以看出，式(4)具有四个方程三个未知数，是典型的超定方程组，一般是无解的，但是存在唯一的最小二乘解，因此，继续执行：

步骤S120：利用二次规划方法求解超定方程组的最小二乘解，得到当前地形的坡度。

求解最小二乘解即为求

式中，‖·‖²运算符表示矩阵的2-范数。

在一实施例中，为了保证自适应运动控制的可靠性，在进行最小二乘解求解时，设置约束条件：

|δ₁|≤1.2， |δ₂|≤1.2 (5)

其中，

通过上述约束条件，可以使地形的倾斜角在-50°～50°范围内变化，在该角度内的坡度地形，认为机器人能运用正常的步态行走，而对于倾斜角大于50°的斜坡地形来说，无法保证机器人还能运用正常的步态行走，采用的地形估计方法的可靠性将会降低。

根据优化得到的最优解，估计的地形倾斜角度为

式中，函数atan(x)为反正切函数，即求x的反正切值。

四足机器人在运动期间实时感知地形坡度后，便能根据所计算的坡度自适应调节机器人的运行状态，相比于传统技术中需要提前设定坡度，本发明可使机器人在坡度变化的复杂地形下自适应行走。

与平坦地形条件下的步态运动相比，四足机器人在斜坡地形的动态行走面对更多挑战和困难。四足机器人的腿部运动范围受限于机身尺寸、关节运动限位和腿部构型等因素，而关节出力大小受限于驱动单元能量密度、机器人重心分布和散热条件等因素。不恰当的运动策略会使机器人达到运动-力极限，无法保证机器人顺利在坡上行走，这也就解释了为什么现有的四足机器人运动控制算法只能抵抗平地下或小坡度地形环境下的扰动。在前文对地形坡度实时在线估计的基础上，设计四足机器人在斜坡条件下的行走策略，使原有的平坦地形运动控制算法展现出良好的斜坡地形适应能力。

四足机器人在斜坡上行走时，为了获得足够大的摩擦力，同时避免腿部运动达到极限范围，最直接的方法就是调整机身的姿态和足端名义支撑点相对于机器人髋关节正下方的偏移位置。图3比较了四足机器人上、下坡过程中的几种运动策略，图中假设机器人的重心位于几何中心。

第一种运动策略如图3的(a)所示，该策略保持机器人的俯仰角水平，足端名义支撑点位置位于机器人髋关节正下方，在这种姿态下行走时，可以保证机器人的重心投影基本落在前后腿中间位置，稳定裕度较大，但遇到大坡度的地形时，由于后腿极限伸长而前腿极限压缩，关节的运动范围靠近运动极限，造成关节出力无法满足需求。

第二种策略如图3的(b)所示，与第一种运动策略相比，第二种运动策略调整了机身的俯仰角度，使其与地形倾斜角度相适应，而足端名义支撑点位置仍然位于机器人髋关节正下方，可以看出，虽然这种策略改变了腿部运动范围受限的情况，但其重心投影几乎落在了两个支撑点外面，对机器人的稳定控制带来难度。

在本实施例中，具体按照以下方法调整机身俯仰角和摆动腿足端名义支撑点位置。具体包括：

步骤S200：以坡度的角度θ作为机身的俯仰角；以机器人髋关节沿垂直于地形平面的投影位置沿坡度下移调整值Δ的位置作为与髋关节相连的摆动腿的足端名义支撑点位置，Δ＝h^cmd tanθ，其中，h^cmd表示机器人机身的期望高度。

针对第二种策略，适当调整足端名义支撑点与机器人髋关节正下方的相对位置，使机器人的重心投影重新落在支撑点范围内，这也就是本实施例所采取的四足机器人上坡行走策略。如图3的(c)，首先根据对地形坡度的估计，令机器人机身俯仰角指令与估计的地形倾角一致，即

θ^cmd＝θ, (7)而足端名义支撑点与机器人髋关节正下方的相对位置调整策略为

Δ＝h^cmd tanθ, (8)

式中，h^cmd表示机器人机身的期望高度，足端名义支撑点与机器人髋关节正下方的偏移位置会随着地形倾斜角的增大而增大，而在平坦地形运动时，该偏移量会减为0，即足端名义支撑点位于机器人髋关节正下方处。为了防止偏移量过大而达到运动极限，Δ的值可以根据实际机器人的大小做限幅处理。同理而言，可以得到四足机器人下坡时的机身俯仰角和足端名义支撑点位置调整策略，如图3的(d)所示。与上坡不同，下坡时落脚点的名义位置相对于机身前移Δ距离，保证机器人的质心投影落在支撑区域内。

在一实施例中，根据坡度自适应调节机器人的运行状态，还包括：根据坡度调整摆动轨迹，具体包括：

步骤S310：在世界坐标系(X_W,Y_W,Z_W)下，采用三次贝塞尔曲线规划假定坡度的角度为0时第i条腿在摆动相的足端运动轨迹，得到在第i条腿摆动时所规划的起始点位置p_i,lift和终止点位置p_i,touch，其中，X_WY_W为水平面。

四足机器人在平地行走时，为了保证摆动轨迹的平滑同时保证足端有充分的离地间隙，本实施例采用三次贝塞尔曲线规划每条腿在摆动相的足端运动轨迹，定义足端一个维度方向上的摆动轨迹B(t)为

式中，e表示摆动相相位，T_sw表示摆动相周期，系数b_i可以根据足端摆动起始点和终止点的位置、速度解析求得，其表达式为

式中，分别表示第i条腿在一个维度上的摆动轨迹起始点位置、终止点位置、起始点位置速度、终止点速度。

对足端x和y方向上的摆动轨迹，直接应用上述贝塞尔曲线，而对于z向的摆动轨迹由于其需要将足端摆到一定高度再下落，需要采用两段贝塞尔曲线分别描述z方向摆动轨迹上升和下降的过程，其表达式为

式中，p_i,z(0)表示第i条腿足端摆动起始位置z方向的分量，表示第i条腿足端摆动期望终止点位置z方向的分量，H_sw为摆动高度。

假定坡度的角度为0，通过上述方法规划出每条摆动腿的足端运动轨迹，得到在第i条腿摆动时所规划的起始点位置p_i,lift和终止点位置p_i,touch。

当机器人在斜坡地形行走时，若直接以上述轨迹摆动，造成足端与地面的冲击(上斜坡)或足端的踏空(下斜坡)，这些扰动都对机器人的运动稳定性带来挑战。在实时在线估计地形倾斜角后，本实施例对摆动轨迹做调整，即执行：

步骤S320：将世界坐标旋转角度θ，得到摆动坐标系(X_S,Y_S,Z_S)，计算从世界坐标系到摆动坐标系的旋转变化矩阵其中，角度θ为坡度的真实角度，X_SY_S构成的平面与地形的平面平行。

步骤S330：根据起始点位置p_i,lift和终止点位置p_i,touch计算得到在坡度的角度为θ时的起始点位置和终止点位置/>

如图4所示。定义摆动坐标系其通过将世界坐标系绕y轴旋转角度θ而得到。通过这样一个调整，可以保证摆动轨迹的规划离地高度与在平地条件下运动时一致，同时原有的摆动轨迹规划方法可直接应用到斜坡地形条件下。

在一实施例中，还进一步对关节驱动力矩的计算方法进行优化，具体包括：

步骤S410：分别计算机身位置控制子任务的阻抗力机身姿态控制子任务的阻抗力/>第i摆动腿足端摆动轨迹跟踪子任务的阻抗力/>

轨迹控制器根据参考轨迹和机器人状态反馈，生成驱动关节的控制指令，控制机器人完成上下斜坡的运动。机器人运动控制任务按照优先级由高至低可分为四个子任务，分别为足端接触力控制、机身位置控制、机身姿态控制和足端摆动轨迹跟踪。为保证任务空间操作的顺应性，分别建立子任务空间的阻抗模型。

机身位置控制子任务的阻抗力的计算公式为：

机身姿态控制子任务的阻抗力的计算公式为：

式中，p_f,d、p_f、和/>分别表示机身在世界坐标系下的期望位置、位置反馈、期望速度和速度反馈，Ψ_d和Ψ分别表示世界坐标系下的机身期望姿态和实际姿态，/>和表示机身坐标系下的机身期望角速度和实际角速度，K_pos和D_pos表示机身位置控制阻抗模型的增益，K_ori和D_ori表示机身姿态控制阻抗模型的增益。

机器人腿部运动控制在接触相和摆动相采用不同的控制方法，接触相阶段根据规划的足端接触力进行力控制，摆动相阶段根据足端摆动参考轨迹，建立足端摆动轨迹跟踪阻抗模型：

式中，p_i,d、p_i、和/>分别表示第i个摆动腿足端在世界坐标系下的期望位置、实际位置、期望速度和实际速度。/>和/>表示足端轨迹跟踪阻抗模型的增益。

为了实现任务空间到关节空间的映射，我们采用零空间投影技术将子任务的阻抗力映射到关节驱动力矩命令，零空间投影技术在高优先级任务的零空间中找到满足低优先级任务执行的驱动力，从而保证低优先级任务的执行不会对高优先级任务的执行产生反作用。根据建立的子任务阻抗模型，关节驱动力矩指令计算公式为，

式中，表示足端接触力规划，通常采用模型预测控制计算得到；n_c表示四足机器人支撑腿数量，/> 表示摆动腿阻抗力矢量，/>表示接触雅可比矩阵，n表示四足机器人广义坐标系数量，J_i|p(i)表示子任务i的雅可比矩阵，其定义在子任务p(i)(比子任务i高一个优先级)的零空间N_p(i)中，每个子任务的J_i|p(i)和N_p(i)通过递归计算依次得到，

式中，I表示单位矩阵，表示雅可比矩阵J的动态一致广义逆矩阵，其计算公式为，

M为广义惯性矩阵。

通过公式(14)～(20)，便能生成驱动关节的控制指令。

相应的，本申请还涉及一种四足机器人坡度地形环境自适应运动控制系统，如图5所示，其包括状态估计器、地形估计器、轨迹规划器和轨迹控制器，状态估计器接收来自机器人的运动传感器信息，估算机器人机身位姿与运动速度以及关节转角和转速，解算出足式机器人足端在世界坐标系下的位置。地形估计器根据状态估计器解算的足端位置信息，建立足端支撑点构成的支撑平面方程，利用二次规划方法，求解足端支撑平面与地平面的夹角，实时在线估计地形坡度。轨迹规划器根据估计得到的地形坡度，调整机身俯仰角；根据规划的轨迹，调整足端摆动轨迹，防止摆动腿提前触地或踏空，同时自适应地调整足端名义支撑点偏移距离，上坡时向后偏移，下坡时向前偏移，保证机器人质心投影落在足端支撑区域内。轨迹控制器根据调整后的参考轨迹，控制机器人关节作动器，实现四足机器人在大坡度地形下动态稳定运动。

其中，地形估计器具体包括超定方程组构建单元和求解单元。超定方程组构建单元用于获取当前步态周期内支撑腿触地点位置以及摆动腿在前一步态周期内的触地点位置并带入地形的平面方程，构成超定方程组。求解单元用于利用二次规划方法求解超定方程组的最小二乘解，得到当前地形的坡度。

在一实施例中，轨迹规划器包括机身俯仰角调节单元、足端名义支撑点位置调节单元和摆动轨迹调节单元。机身俯仰角调节单元用于以最新计算出的坡度的角度作为机身的俯仰角。足端名义支撑点位置调节单元用于以机器人髋关节沿垂直于地形平面的投影位置沿坡度下移调整值Δ的位置作为与髋关节相连的摆动腿的足端名义支撑点位置，Δ＝h^cmd tanθ，其中，h^cmd表示机器人机身的期望高度。摆动轨迹调节单元用于规划摆动腿的摆动轨迹。

在一实施例中，轨迹控制器包括阻抗力计算单元、关节驱动力矩计算单元。阻抗力计算单元用于分别计算机身位置控制子任务的阻抗力机身姿态控制子任务的阻抗力/>第i摆动腿足端摆动轨迹跟踪子任务的阻抗力/>关节驱动力矩计算单元用于关节驱动力矩，计算公式为：

其中，τ表示关节驱动力矩矢量，表示足端接触力对应的雅可比矩阵，n_c表示四足机器人支撑腿数量，n表示四足机器人广义坐标系数量，表示规划的足端接触力且下标表示支撑腿的序号，J_i|p(i)表示子任务i的雅可比矩阵，/>表示摆动腿阻抗力矢量。

相应的，本发明还涉及一种四足机器人，其装载有上文介绍的四足机器人坡度地形环境自适应运动控制系统，能够在大坡度地形下动态稳定运动。

本领域的技术人员容易理解，以上仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种四足机器人坡度地形环境自适应运动方法，其特征在于，四足机器人在运动期间实时感知地形坡度，根据所述坡度自适应调节机器人的运行状态；

其中，在运动期间实时感知地形坡度的过程包括：

将当前步态周期内支撑腿触地点位置以及摆动腿在前一步态周期内的触地点位置分别代入地形的平面方程Ax+By+Cz+D＝0，构成超定方程组，A,B,C,D是所述平面方程的系数，C≠0，(x,y,z)表示地形平面上的点在世界坐标系下的坐标；所述超定方程组为：

对所述超定方程组等式两边同时除以C，得到变形后的等式：

式中，代表需要估计的新的平面方程系数；

利用二次规划方法求解所述超定方程组的最小二乘解，得到当前地形的坡度；

其中，利用二次规划方法求解所述超定方程组的最小二乘解，包括：

根据所述超定方程组构建二次规划求解模型为：

式中，‖·‖²运算符表示矩阵的2-范数，为第i个腿代入所述平面方程的触地点位置；

设定约束条件：

求解所述二次规划求解模型，得到矩阵δ的最优解，基于矩阵δ确定当前地形倾斜角度

2.如权利要求1所述的四足机器人坡度地形环境自适应运动方法，其特征在于，根据当前步态周期的步态规划识别第i条腿为支撑腿还是摆动腿，识别方法包括：

当第i条腿在当前时刻的规划步态为摆动相，则直接认为第i条腿为摆动腿，并以第i条腿在前一步态周期内的触地点位置代入地形的平面方程；

当第i条腿的在当前时刻的规划步态为接触相且α<0.2或α>0.8，则认为第i条腿为摆动腿，并以第i条腿在前一步态周期内的触地点位置代入地形的平面方程；

当第i条腿的在当前时刻的规划步态为接触相且α<0.2或α>0.8，则认为第i条腿为支撑腿，并以第i条腿在当前步态周期内的触地点位置代入地形的平面方程；

其中，α为当前时刻在接触相内的时间相位。

3.如权利要求1所述的四足机器人坡度地形环境自适应运动方法，其特征在于，根据所述坡度自适应调节机器人的运行状态，包括：根据所述坡度调整机身俯仰角和摆动腿足端名义支撑点位置，具体包括：

以所述坡度的角度θ作为机身的俯仰角；

以机器人髋关节沿垂直于地形平面的投影位置沿坡度下移调整值Δ的位置作为与髋关节相连的摆动腿的足端名义支撑点位置，Δ＝h^cmdtanθ，其中，h^cmd表示机器人机身的期望高度。

4.如权利要求3所述的四足机器人坡度地形环境自适应运动方法，其特征在于，根据所述坡度自适应调节机器人的运行状态，包括：根据所述坡度调整摆动轨迹，具体包括：

在世界坐标系(X_W,Y_W,Z_W)下，采用三次贝塞尔曲线规划假定所述坡度的角度为0时第i条腿在摆动相的足端运动轨迹，得到在第i条腿摆动时所规划的起始点位置p_i,lift和终止点位置p_i,tou，其中，X_WY_W为水平面；

将世界坐标旋转角度θ，得到摆动坐标系(X_S,Y_S,Z_S)，计算从世界坐标系到摆动坐标系的旋转变化矩阵其中，角度θ为所述坡度的真实角度，X_SY_S构成的平面与地形的平面平行；

根据起始点位置p_i,lift和终止点位置p_i,touch计算得到在所述坡度的角度为θ时的起始点位置和终止点位置/>

5.如权利要求1所述的四足机器人坡度地形环境自适应运动方法，其特征在于，机器人运动控制任务按照优先级由高至低包括足端接触力控制、机身位置控制、机身姿态控制和足端摆动轨迹跟踪四个子任务；

根据所述坡度自适应调节机器人的运行状态，包括：将各子任务的阻抗力映射至关节驱动力矩，具体包括：

分别计算机身位置控制子任务的阻抗力机身姿态控制子任务的阻抗力/>第i摆动腿足端摆动轨迹跟踪子任务的阻抗力/>

计算关节驱动力矩，计算公式为：

其中，τ表示关节驱动力矩矢量，表示足端接触力对应的雅可比矩阵，n_c表示四足机器人支撑腿数量，n表示四足机器人广义坐标系数量，/>表示规划的足端接触力且下标表示支撑腿的序号，J_i|p(i)表示子任务i的雅可比矩阵，表示摆动腿阻抗力矢量。

6.一种四足机器人坡度地形环境自适应运动控制系统，其特征在于，包括地形估计器，用于在四足机器人运动期间实时感知地形坡度；

所述地形估计器包括：

超定方程组构建单元，用于获取当前步态周期内支撑腿触地点位置以及摆动腿在前一步态周期内的触地点位置并带入地形的平面方程Ax+By+Cz+D＝0，构成超定方程组，A,B,C,D是所述平面方程的系数，C≠0，(x,y,z)表示地形平面上的点在世界坐标系下的坐标；所述超定方程组为：

求解单元，用于利用二次规划方法求解所述超定方程组的最小二乘解，得到当前地形的坡度，其中，利用二次规划方法求解所述超定方程组的最小二乘解，包括：

对所述超定方程组等式两边同时除以C，得到变形后的等式：

式中，代表需要估计的新的平面方程系数；根据所述超定方程组构建二次规划求解模型为：

式中，‖·‖²运算符表示矩阵的2-范数，为第i个腿代入所述平面方程的触地点位置；

设定约束条件：

求解所述二次规划求解模型，得到矩阵δ的最优解，基于矩阵δ确定当前地形倾斜角度

7.如权利要求6所述的四足机器人坡度地形环境自适应运动控制系统，其特征在于，还包括轨迹规划器，所述轨迹规划器包括：

机身俯仰角调节单元，用于以最新计算出的所述坡度的角度作为机身的俯仰角；

足端名义支撑点位置调节单元，用于以机器人髋关节沿垂直于地形平面的投影位置沿坡度下移调整值Δ的位置作为与髋关节相连的摆动腿的足端名义支撑点位置，Δ＝h^cmdtanθ，其中，h^cmd表示机器人机身的期望高度；

摆动轨迹调节单元，用于规划摆动腿的摆动轨迹。

8.如权利要求6所述的四足机器人坡度地形环境自适应运动控制系统，其特征在于，还包括用于输出关节控制指令的轨迹控制器，所述轨迹控制器包括：

阻抗力计算单元，用于分别计算机身位置控制子任务的阻抗力机身姿态控制子任务的阻抗力/>第i摆动腿足端摆动轨迹跟踪子任务的阻抗力/>

关节驱动力矩计算单元，用于关节驱动力矩计算，计算公式为：

其中，τ表示关节驱动力矩矢量，表示足端接触力对应的雅可比矩阵，n_c表示四足机器人支撑腿数量，n表示四足机器人广义坐标系数量，/>表示规划的足端接触力且下标表示支撑腿的序号，J_i|p(i)表示子任务i的雅可比矩阵，表示摆动腿阻抗力矢量。

9.一种四足机器人，其特征在于，包含权利要求6至8任一项所述的四足机器人坡度地形环境自适应运动控制系统。