CN114021376B - 一种四足机器人地形坡度估计方法 - Google Patents

Abstract

本发明提出一种四足机器人地形坡度估计方法，主要步骤包括单腿运动学解算，姿态解算坐标系转换，TD状态缓存。最小二乘平面拟合和ARMA模型回归预测。本发明基于ARMA模型，引入地形类型先验知识实现对坡面角度拟合数据的滤波、预测与估计，面向四足机器人常用Trot步态仅有两腿支撑的特点，以历史足地接触信号为触发采集足端运动学数据与机载惯性传感器测量姿态，从而获取四个离散点数据并进一步采用最小二乘法进行拟合坡面角度，能够提高拟合结果的平滑性能。

Description

一种四足机器人地形坡度估计方法

技术领域

本发明属于自动控制技术领域，具体涉及一种结合最小二乘法与ARMA时间序列的四足机器人地形坡度估计方法。

背景技术

地形坡度估计是四足机器人在通过起伏地形时的重要状态估计数据，四足机器人在坡度地形移动中姿态需要跟随地形调节，从而为腿部提供充足工作空间，避免摆动腿与地形干涉。因此，地形坡度估计技术十分重要。为实现在线地形坡度估计并降低由于摆动控制误差、误触地以及姿态失稳引起的估计误差，需要提出一种能拟合融合地形先验知识对坡度平滑、滤波与预测的估计方法。

发明内容

(一)要解决的技术问题

本发明提出一种四足机器人地形坡度估计方法，以解决如何提高地形坡度估计精度的技术问题。

(二)技术方案

为了解决上述技术问题，本发明提出一种四足机器人地形坡度估计方法，该方法包括如下步骤：

S1.基于机器人结构参数与单腿构型，使用运动学正解计算各支撑腿足端在机体坐标系{B}下的三维位置

S1-1.使用关节角度反馈值，通过运动学正解计算各支撑腿足端在胯关节坐标系{H}下的三维位置：

P_i ^H＝f(c₁,c₂,c₃),i＝1…4

其中，P_i ^H为第i条支撑腿足端在胯关节坐标系{H}下的三维位置；f(c₁,c₂,c₃)为对应单腿构型的运动学正解计算函数：

其中，c₁为机器人大腿角度，c₂为小腿角度，c₃为胯关节角度；L₁为大腿长度，L₂为小腿长度，L₃为侧摆电机相对跨关节的偏差；

S1-2.将各支撑腿足端在胯关节坐标系{H}下的三维位置转换为机体坐标系{B}下的三维位置：

其中，P_i ^B为第i条支撑腿足端在机体坐标系{B}下的三维位置，W为机器人左右两侧支撑腿胯关节间距，H为机器人前后两侧支撑腿胯关节间距；

S2.基于机器人机载IMU测量得到的姿态四元数，获取当前全局坐标系{N}与机体坐标系{B}间的转换矩阵，并将各支撑腿足端在机体坐标系{B}下的三维位置转换为全局坐标系{N}下的三维位置：

其中，为全局坐标系{N}与机体坐标系{B}间的转换矩阵，q₀、q₁、q₂、q₃为机器人当前姿态四元数；/>为/>的转置；P_i ^N为第i条支撑腿足端在全局坐标系{N}下的三维位置；

S3.定义第i条支撑腿着地触发标志位TD_i，当该标志位由摆动状态切换为支撑状态时刻，将当前全局坐标系{N}系下支撑腿足端位置P_i ^N存入数组当数组/>中数据数量大于4时，对当前支撑平面的方程系数进行拟合：

S3-1.设支撑平面函数为z＝ax+by+c，其待拟合系数为X＝[a b c]；设最小二乘法代价函数为：

对代价函数求偏导：

其对应矩阵形式如下：

通过对上述矩阵求逆运算，计算出拟合系数X＝[a b c]；

S3-2.采用如下公式计算t时刻支撑平面的拟合坡度角：

其中，θ_t为t时刻拟合坡度角，α为坡度俯仰角，β为坡度横滚角；

S4.采用ARMA模型对拟合坡度角进行滤波与预测

S4-1.定义时间序列t并将对应坡度角拟合结果存入数据集中，采用自回归模型，按如下公式消除预测中的随机波动：

其中，为自回归模型修正结果，θ_off为固定角度偏差即IMU安装角度偏差，为相对t的j时刻对应自回归模型的历史处理结果，p为自回归模型对应的***阶次，γ_j为自回归模型对应的系数；

采用滑动平均模型，按如下公式剔除时间序列t内数据中存在的异常值：

其中，为滑动平均模型修正结果，n为滑动平均模型对应的***阶次，μ_j为滑动平均模型对应的系数，/>为相对t的前一时刻滑动平均模型的历史处理结果，θ_t-j为相对t的j时刻的坡度角历史拟合值，/>为相对t的j-1时刻滑动平均模型的历史处理结果；

S4-2.采用加权平均法，按如下公式计算自回归模型与滑动平均模型处理结果的加权平均结果即为最终的坡度角估计结果：

其中，λ为模型分配权重。

进一步地，步骤S4-2中，使用外部基于图像数据识别的结果对当前所处地形进行分类，采用离线数据实验和专家数据库获取分配权重。

(三)有益效果

本发明提出一种四足机器人地形坡度估计方法，主要步骤包括单腿运动学解算，姿态解算坐标系转换，TD状态缓存。最小二乘平面拟合和ARMA模型回归预测。本发明基于ARMA模型，引入地形类型先验知识实现对坡面角度拟合数据的滤波、预测与估计，面向四足机器人常用Trot步态仅有两腿支撑的特点，以历史足地接触信号为触发采集足端运动学数据与机载惯性传感器测量姿态，从而获取四个离散点数据并进一步采用最小二乘法进行拟合坡面角度，能够提高拟合结果的平滑性能。

附图说明中

图1为本发明实施例的机器人坐标系示意图：(a)侧视图，(b)俯视图({H}为胯关节坐标系，{B}为机体坐标系)；

图2为本发明实施例的地形坡度估计方法流程；

图3为本发明实施例中坡度角拟合结果。

具体实施方式

为使本发明的目的、内容和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

本实施例提出一种四足机器人地形坡度估计方法，所涉及的机器人坐标系如图1所示，其中{H}为胯关节坐标系，{B}为机体坐标系，该方法的主要流程如图2所示，具体包括如下步骤：

S1.基于机器人结构参数与单腿构型，使用运动学正解计算各支撑腿足端在机体坐标系{B}下的三维位置

S1-1.使用关节角度反馈值，通过运动学正解计算各支撑腿足端在胯关节坐标系{H}下的三维位置：

P_i ^H＝f(c₁,c₂,c₃),i＝1…4

其中，P_i ^H为第i条支撑腿足端在胯关节坐标系{H}下的三维位置；f(c₁,c₂,c₃)为对应单腿构型的运动学正解计算函数：

其中，c₁为机器人大腿角度，c₂为小腿角度，c₃为胯关节角度；L₁为大腿长度，L₂为小腿长度，L₃为侧摆电机相对跨关节的偏差。

S1-2.将各支撑腿足端在胯关节坐标系{H}下的三维位置转换为机体坐标系{B}下的三维位置：

其中，P_i ^B为第i条支撑腿足端在机体坐标系{B}下的三维位置，W为机器人左右两侧支撑腿胯关节间距，H为机器人前后两侧支撑腿胯关节间距。

S2.基于机器人机载IMU测量得到的姿态四元数，获取当前全局坐标系{N}与机体坐标系{B}间的转换矩阵，并将各支撑腿足端在机体坐标系{B}下的三维位置转换为全局坐标系{N}下的三维位置：

其中，为全局坐标系{N}与机体坐标系{B}间的转换矩阵，q₀、q₁、q₂、q₃为机器人当前姿态四元数；/>为/>的转置；P_i ^N为第i条支撑腿足端在全局坐标系{N}下的三维位置。

S3.定义第i条支撑腿着地触发标志位TD_i，当该标志位由摆动状态切换为支撑状态时刻，将当前全局坐标系{N}系下支撑腿足端位置P_i ^N存入数组当数组/>中数据数量大于4时，对当前支撑平面的方程系数进行拟合，计算支撑平面的拟合坡度角

S3-1.设支撑平面函数为z＝ax+by+c，其待拟合系数为X＝[a b c]；设最小二乘法代价函数为：

对代价函数求偏导：

其对应矩阵形式如下：

通过对上述矩阵求逆运算，计算出拟合系数X＝[a b c]。

S3-2.采用如下公式计算t时刻支撑平面的拟合坡度角

其中，θ_t为t时刻支撑平面的拟合坡度角，α为坡度俯仰角，β为坡度横滚角。坡度角的拟合结果如图3所示。

S4.采用ARMA模型对拟合坡度角进行滤波与预测

S4-1.定义时间序列t并将对应坡度角拟合结果存入数据集中，采用自回归(AR)模型，按如下公式消除预测中的随机波动：

其中，为AR模型修正结果，θ_off为固定角度偏差即IMU安装角度偏差，/>为相对t的j时刻对应AR模型的历史处理结果，p为自回归模型对应的***阶次，γ_j为自回归模型对应的系数。

采用滑动平均(MA)模型，按如下公式剔除时间序列t内数据中存在的异常值：

其中，为MA模型修正结果，n为滑动平均模型对应的***阶次，μ_j为滑动平均模型对应的系数，/>为相对t的前一时刻滑动平均模型的历史处理结果，θ_t-j为相对t的j时刻的坡度角历史拟合值，/>为相对t的j-1时刻滑动平均模型的历史处理结果。

S4-2.采用加权平均法，按如下公式计算AR模型与MA模型处理结果的加权平均结果即为最终的坡度角估计结果：

其中，λ为模型分配权重，可使用外部基于图像数据识别的结果对当前所处地形进行分类，如平地、起伏坡地、砾石地、台阶等，采用离线数据实验和专家数据库获取分配权重。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (2)

1.一种四足机器人地形坡度估计方法，其特征在于，所述方法包括如下步骤：

S1.基于机器人结构参数与单腿构型，使用运动学正解计算各支撑腿足端在机体坐标系{B}下的三维位置

S1-1.使用关节角度反馈值，通过运动学正解计算各支撑腿足端在胯关节坐标系{H}下的三维位置：

P_i ^H＝f(c₁,c₂,c₃),i＝1…4

其中，P_i ^H为第i条支撑腿足端在胯关节坐标系{H}下的三维位置；f(c₁,c₂,c₃)为对应单腿构型的运动学正解计算函数：

其中，c₁为机器人大腿角度，c₂为小腿角度，c₃为胯关节角度；L₁为大腿长度，L₂为小腿长度，L₃为侧摆电机相对跨关节的偏差；

S1-2.将各支撑腿足端在胯关节坐标系{H}下的三维位置转换为机体坐标系{B}下的三维位置：

其中，P_i ^B为第i条支撑腿足端在机体坐标系{B}下的三维位置，W为机器人左右两侧支撑腿胯关节间距，H为机器人前后两侧支撑腿胯关节间距；

S2.基于机器人机载IMU测量得到的姿态四元数，获取当前全局坐标系{N}与机体坐标系{B}间的转换矩阵，并将各支撑腿足端在机体坐标系{B}下的三维位置转换为全局坐标系{N}下的三维位置：

其中，为全局坐标系{N}与机体坐标系{B}间的转换矩阵，q₀、q₁、q₂、q₃为机器人当前姿态四元数；/>为/>的转置；P_i ^N为第i条支撑腿足端在全局坐标系{N}下的三维位置；

S3.定义第i条支撑腿着地触发标志位TD_i，当该标志位由摆动状态切换为支撑状态时刻，将当前全局坐标系{N}系下支撑腿足端位置P_i ^N存入数组当数组/>中数据数量大于4时，对当前支撑平面的方程系数进行拟合：

S3-1.设支撑平面函数为z＝ax+by+c，其待拟合系数为X＝[a b c]；设最小二乘法代价函数为：

对代价函数求偏导：

其对应矩阵形式如下：

通过对上述矩阵求逆运算，计算出拟合系数X＝[a b c]；

S3-2.采用如下公式计算t时刻支撑平面的拟合坡度角：

其中，θ_t为t时刻拟合坡度角，α为坡度俯仰角，β为坡度横滚角；

S4.采用ARMA模型对拟合坡度角进行滤波与预测

S4-1.定义时间序列t并将对应坡度角拟合结果存入数据集中，采用自回归模型，按如下公式消除预测中的随机波动：

其中，为自回归模型修正结果，θ_off为固定角度偏差即IMU安装角度偏差，/>为相对t的j时刻对应自回归模型的历史处理结果，p为自回归模型对应的***阶次，γ_j为自回归模型对应的系数；

采用滑动平均模型，按如下公式剔除时间序列t内数据中存在的异常值：

其中，为滑动平均模型修正结果，n为滑动平均模型对应的***阶次，μ_j为滑动平均模型对应的系数，/>为相对t的前一时刻滑动平均模型的历史处理结果，θ_t-j为相对t的j时刻的坡度角历史拟合值，/>为相对t的j-1时刻滑动平均模型的历史处理结果；

S4-2.采用加权平均法，按如下公式计算自回归模型与滑动平均模型处理结果的加权平均结果即为最终的坡度角估计结果：

其中，λ为模型分配权重。

2.如权利要求1所述的地形坡度估计方法，其特征在于，步骤S4-2中，使用外部基于图像数据识别的结果对当前所处地形进行分类，采用离线数据实验和专家数据库获取分配权重。