CN111896007A - 一种补偿足地冲击的四足机器人姿态解算方法 - Google Patents

Abstract

本发明公开了一种补偿足地冲击的四足机器人姿态解算方法，精确的姿态角度是四足机器人稳定控制的必要前提，其在坐标系变换、足地力分解以及状态估计中都作为主要测量数据，姿态解算是基于机器人本体惯性传感器(加速度计、陀螺仪和磁力计)测量数据融合得到最终精确的姿态角度，因此本发明基于EKF作为核心姿态解算方法，通过引入步态相序与足底力估计结果，动态调节滤波器测量方差，补偿落足冲击力带来的运动角速度，从而提供姿态解算的精度，提供四足机器人稳定控制的能力。

Description

一种补偿足地冲击的四足机器人姿态解算方法

技术领域

本发明涉及自动控制领域，具体涉及一种补偿足地冲击的四足机器人姿态解算方法。

背景技术

目前基于EKF的姿态解算方法在无人机、无人车中被广泛应用，近年来随着四足机器人技术的发展，其相关姿态解算技术也逐步发展，基于传统的EKF姿态计算方法能获取实时的机器人姿态角度，但对于足式机器人来说由于其往往采用动态稳定技术需要不断摆动踏步来保持机器人稳定，足地冲击会造成本体传感器数据的噪声和运动加速度的测量，受限于目前柔顺阻抗控制技术的发展难以完全消除落足的冲击，采用传统姿态解算方法难以解决周期冲击带来的测量误差使得最终姿态测量结果发散，因此需要设计一种考虑四足机器人步态相序补偿落足冲击的新姿态解算方法。

发明内容

本发明公布了一种补偿足地冲击的四足机器人姿态解算方法，以EKF滤波器为姿态解算模块核心，通过引入步态相序信息与足底力估计结果来补偿冲击力带来的运动加速度干扰并动态调节传感器测量方差，在大冲击时降低加速度传感器的修正权重，从而提高四足机器人在步态运动中的姿态解算稳定性与可靠性。

本发明的技术方案如下：

步骤1：初始化欧拉角，采用机载加速度数据求取俯仰、横滚角度：

步骤2：使用求取的初始俯仰、横滚角分解磁力计数据求取初始航向角度：

步骤3：采用计算得到的初始角度俯仰角α、横滚角β和航向角γ，计算初始化姿态四元数：

步骤4：初始化滤波器对应的传感器测量噪声参数，加速度测量噪声a_n，陀螺仪测量噪声w_m，陀螺仪测量偏差噪声w_n，磁力计测量偏差噪声m_n，得到对应的***噪声矩阵与测量噪声矩阵：

步骤5：采用足端里程计修正航向陀螺数据，首先计算各支撑腿移动速度引起的转向速度，计算速度矢量，以第i条支撑腿为例：

步骤6：计算末端速度矢量v_i末端位置p_i质心连线间的夹角

步骤7：使用该夹角对速度矢量进行分解求取转向方向的足端线速度分量

v_r,i＝|v|_i cos(γ₃)

步骤8：基于末端位置将速度矢量转换为对应的里程计角速度值

步骤9：累加各支撑腿的里程计角速度并求取均值

步骤10：采用足端里程计结果修正陀螺仪角速度测量值

步骤11：采用一阶龙格库卡法，采用陀螺仪测量数据基于时间T更新四元数状态：

步骤12：计算当前机体与世界坐标系下的旋转矩阵

步骤13：更新陀螺仪测量偏差状态：

w_n，k+1＝w_n，k

步骤14：设定卡尔曼滤波器的***状态为四元数与陀螺仪测量偏差x_k＝[q_k,w_k]

步骤15：求取***状态方程对应的雅克比矩阵

步骤16：计算先验协方差矩阵

P_k＝GP_k-1G^T+Q

步骤17：使用当前四元数状态估计结果求取估计的归一化加速度值：

步骤18：使用步态相序与关节力测试数据补偿加速度测量值，首先使用运动学和关节力矩测量值求取全局坐标系下的足底力：

其中J为当前单腿雅克比矩阵，其基于当前单腿运动学正解计算方程与关节角度值得到。

步骤12：基于各支撑腿触地相序，建立长度为T_w的采用窗口，计算窗口内由于触地冲击产生的外部瞬时加速度，设有N条腿着地，机器人质量为M，重力加速度为g，则触地扰动力为：

步骤19：在采用窗口内补偿加速度计测量值，减少落足冲击带来的外部加速度对姿态解算的影响

步骤20：求取加速度测量值对应的雅克比矩阵：

步骤21：使用估计的四元数与旋转矩阵将测量的磁场数据转换到机体坐标系下：

步骤22：同理可以使用绝对的磁场数据结合状态估计结果来得到估计的机体磁场测量结果：

步骤23：求取磁力计测量值对应的雅克比矩阵

步骤24：计算最终的测量方程雅克比矩阵：

步骤25：计算卡尔曼增益

K＝PH^T(HPH^T+R)^-1

步骤26：采用补偿足地冲击后的测量值与估计的测量值误差来更新状态估计结果

步骤27：更新后验协方差矩阵

P＝(I-KH)P

步骤28：采用四元数状态估计计算姿态角并输出补偿陀螺仪测量偏差的角速度值

有益效果：本发明公布的一种补偿足地冲击的四足机器人姿态解算方法，以EKF滤波器为姿态解算模块核心，通过引入步态相序信息与足底力估计结果来补偿冲击力带来的运动加速度干扰并动态调节传感器测量方差，在大冲击时降低加速度传感器的修正权重，从而提高四足机器人在步态运动中的姿态解算稳定性与可靠性。

说明书附图

图1为采用本方法改进后的机器人的俯仰角和横滚角随时间变化的波动图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

一种补偿足地冲击的四足机器人姿态解算方法，包括如下步骤：

步骤1：初始化欧拉角，采用机载加速度数据求取俯仰、横滚角度：

步骤2：使用求取的初始俯仰、横滚角分解磁力计数据求取初始航向角度：

步骤3：采用计算得到的初始角度俯仰角α、横滚角β和航向角γ，计算初始化姿态四元数：

步骤4：初始化滤波器对应的传感器测量噪声参数，加速度测量噪声a_n，陀螺仪测量噪声w_m，陀螺仪测量偏差噪声w_n，磁力计测量偏差噪声m_n，得到对应的***噪声矩阵与测量噪声矩阵：

步骤5：采用足端里程计修正航向陀螺数据，首先计算各支撑腿移动速度引起的转向速度，计算速度矢量，以第i条支撑腿为例：

步骤6：计算末端速度矢量v_i、末端位置p_i质心连线间的夹角：

γ₁＝arctan(p_i,x/p_i,y)

γ₂＝arctan(v_i,x/v_i,y)

步骤7：使用所述夹角对速度矢量进行分解求取转向方向的足端线速度分量

v_r,i＝|v|_i cos(γ₃)

步骤8：基于末端位置将速度矢量转换为对应的里程计角速度值

步骤9：累加各支撑腿的里程计角速度并求取均值；

步骤10：采用足端里程计结果修正陀螺仪角速度测量值

步骤11：采用一阶龙格库卡法，采用陀螺仪测量数据基于时间T更新四元数状态：

步骤12：计算当前机体与世界坐标系下的旋转矩阵

步骤13：更新陀螺仪测量偏差状态：

w_n，k+1＝w_n，k

步骤14：设定卡尔曼滤波器的***状态为四元数与陀螺仪测量偏差x_k＝[q_k,w_k]

步骤15：求取***状态方程对应的雅克比矩阵

步骤16：计算先验协方差矩阵

P_k＝GP_k-1G^T+Q

步骤17：使用当前四元数状态估计结果求取估计的归一化加速度值：

步骤18：使用步态相序与关节力测试数据补偿加速度测量值，首先使用运动学和关节力矩测量值求取全局坐标系下的足底力：

其中J为单腿当前雅克比矩阵，其基于当前单腿运动学正解计算方程与关节角度值得到；

步骤19：基于各支撑腿触地相序，建立长度为T_w的采用窗口，计算窗口内由于触地冲击产生的外部瞬时加速度，设有N条腿着地，机器人质量为M，则触地扰动力为：

步骤20：在采用窗口内补偿加速度计测量值，减少落足冲击带来的外部加速度对姿态解算的影响:

步骤21：求取加速度测量值对应的雅克比矩阵：

步骤22：使用估计的四元数与旋转矩阵将测量的磁场数据转换到机体坐标系下：

步骤23：同理可以使用绝对的磁场数据结合状态估计结果来得到估计的机体磁场测量结果：

步骤24：求取磁力计测量值对应的雅克比矩阵

步骤25：计算最终的测量方程雅克比矩阵：

步骤26：计算卡尔曼增益:

K＝PH^T(HPH^T+R)^-1

步骤27：采用补偿足地冲击后的测量值与估计的测量值误差来更新状态估计结果

步骤28：更新后验协方差矩阵

P＝(I-KH)P

步骤29：采用四元数状态估计计算姿态角并输出补偿陀螺仪测量偏差的角速度值

本发明公布的一种补偿足地冲击的四足机器人姿态解算方法，以EKF滤波器为姿态解算模块核心，通过引入步态相序信息与足底力估计结果来补偿冲击力带来的运动加速度干扰并动态调节传感器测量方差，在大冲击时降低加速度传感器的修正权重，从而提高四足机器人在步态运动中的姿态解算稳定性与可靠性，具体见图1。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (1)

1.一种补偿足地冲击的四足机器人姿态解算方法，其特征在于，包括如下步骤：

步骤1：初始化欧拉角，采用机载加速度数据求取俯仰、横滚角度：

步骤2：使用求取的初始俯仰、横滚角分解磁力计数据求取初始航向角度：

步骤3：采用计算得到的初始角度俯仰角α、横滚角β和航向角γ，计算初始化姿态四元数：

步骤4：初始化滤波器对应的传感器测量噪声参数，加速度测量噪声a_n，陀螺仪测量噪声w_m，陀螺仪测量偏差噪声w_n，磁力计测量偏差噪声m_n，得到对应的***噪声矩阵与测量噪声矩阵：

步骤5：采用足端里程计修正航向陀螺数据，首先计算各支撑腿移动速度引起的转向速度，计算速度矢量，以第i条支撑腿为例：

步骤6：计算末端速度矢量v_i末端位置p_i质心连线间的夹角

γ₁＝arctan(p_i,x/p_i,y)

γ₂＝arctan(v_i,x/v_i,y)

步骤7：使用该夹角对速度矢量进行分解求取转向方向的足端线速度分量

v_r,i＝|v|_icos(γ₃)

步骤8：基于末端位置将速度矢量转换为对应的里程计角速度值

步骤9：累加各支撑腿的里程计角速度并求取均值

步骤10：采用足端里程计结果修正陀螺仪角速度测量值

步骤11：采用一阶龙格库卡法，采用陀螺仪测量数据基于时间T更新四元数状态：

步骤12：计算当前机体与世界坐标系下的旋转矩阵

步骤13：更新陀螺仪测量偏差状态：

w_n，k+1＝w_n，k

步骤14：设定卡尔曼滤波器的***状态为四元数与陀螺仪测量偏差x_k＝[q_k,w_k]

步骤15：求取***状态方程对应的雅克比矩阵

步骤16：计算先验协方差矩阵

P_k＝GP_k-1G^T+Q

步骤17：使用当前四元数状态估计结果求取估计的归一化加速度值：

步骤18：使用步态相序与关节力测试数据补偿加速度测量值，首先使用运动学和关节力矩测量值求取全局坐标系下的足底力：

其中J为单腿当前雅克比矩阵，其基于当前单腿运动学正解计算方程与关节角度值得到。

步骤12：基于各支撑腿触地相序，建立长度为T_w的采用窗口，计算窗口内由于触地冲击产生的外部瞬时加速度，设有N条腿着地，机器人质量为M，则触地扰动力为：

步骤19：在采用窗口内补偿加速度计测量值，减少落足冲击带来的外部加速度对姿态解算的影响

步骤20：求取加速度测量值对应的雅克比矩阵：

步骤21：使用估计的四元数与旋转矩阵将测量的磁场数据转换到机体坐标系下：

步骤22：同理可以使用绝对的磁场数据结合状态估计结果来得到估计的机体磁场测量结果：

步骤23：求取磁力计测量值对应的雅克比矩阵

步骤24：计算最终的测量方程雅克比矩阵：

步骤25：计算卡尔曼增益

K＝PH^T(HPH^T+R)^-1

步骤26：采用补偿足地冲击后的测量值与估计的测量值误差来更新状态估计结果

步骤27：更新后验协方差矩阵

P＝(I-KH)P

步骤28：采用四元数状态估计计算姿态角并输出补偿陀螺仪测量偏差的角速度值

Images