[发明专利]一种足式机器人多模态感知的状态估计方法及系统

权利要求书

1.一种足式机器人多模态感知的状态估计方法，其特征在于，所述方法包括：

基于IMU传感器测量足式机器人的加速度和角速度，并预测所述足式机器人的状态均值和协方差，其中所述状态均值包括本体位置、本体速度、本体姿态的四元数、各脚部位置、IMU加速度计偏置和IMU陀螺仪偏置；

基于关节编码器获取所述足式机器人中所有关节的角度位置，并计算所述足式机器人中每一只脚的脚点位置所对应的计量值与预测值；

基于所述计量值与所述预测值，获取所述足式机器人的测量残差；

基于所述测量残差和测量雅可比，对所述状态均值和所述协方差进行校正，获取所述足式机器人的最终状态；

其中，预测所述足式机器人的本体位置为：

预测所述足式机器人的本体速度为：

预测所述足式机器人的本体姿态的四元数为：

预测所述足式机器人的各脚部位置为：

预测所述足式机器人的IMU加速度计偏置为：

预测所述足式机器人的IMU陀螺仪偏置为：

式中：Δt为时间步长，v_t为t时刻下的本体速度，R_wb为世界坐标系与本体坐标系间的转换矩阵，a_t为t时刻下IMU测量的加速度，w为IMU测量的角速度，g为重力加速度；

其中，所述足式机器人的测量残差为：

式中：e为测量残差，s为所述足式机器人中每一只脚的脚点位置所对应的计量值，为所述足式机器人中每一只脚的脚点位置所对应的预测值，α为所述足式机器人的关节角度，为预测的旋转矩阵，为预测的第i只脚的脚点位置，为预测的所述足式机器人本体的质心位置。

2.根据权利要求1所述的足式机器人多模态感知的状态估计方法，其特征在于，所述基于IMU传感器测量足式机器人的加速度和角速度，并预测所述足式机器人的状态均值和协方差包括：

基于所述IMU传感器中的加速度计测量所述足式机器人的线性加速度；

基于所述IMU传感器中的陀螺仪测量所述足式机器人的角速度；

基于所述线性加速度和所述角速度，通过积分变换计算所述足式机器人的状态均值和协方差，并在本体坐标系表示出来。

3.一种足式机器人多模态感知的状态估计系统，其特征在于，所述系统包括：

预测模块，用于基于IMU传感器测量足式机器人的加速度和角速度，并预测所述足式机器人的状态均值和协方差，其中所述状态均值包括本体位置、本体速度、本体姿态的四元数、各脚部位置、IMU加速度计偏置和IMU陀螺仪偏置；

计算模块，用于基于关节编码器获取所述足式机器人中所有关节的角度位置，并计算所述足式机器人中每一只脚的脚点位置所对应的计量值与预测值；

获取模块，用于基于所述计量值与所述预测值，获取所述足式机器人的测量残差；

校正模块，用于基于所述测量残差和测量雅可比，对所述状态均值和所述协方差进行校正，获取所述足式机器人的最终状态；

其中，预测所述足式机器人的本体位置为：

预测所述足式机器人的本体速度为：

预测所述足式机器人的本体姿态的四元数为：

预测所述足式机器人的各脚部位置为：

预测所述足式机器人的IMU加速度计偏置为：

预测所述足式机器人的IMU陀螺仪偏置为：

式中：Δt为时间步长，v_t为t时刻下的本体速度，R_wb为世界坐标系与本体坐标系间的转换矩阵，a_t为t时刻下IMU测量的加速度，w为IMU测量的角速度，g为重力加速度；

其中，所述足式机器人的测量残差为：

式中：e为测量残差，s为所述足式机器人中每一只脚的脚点位置所对应的计量值，为所述足式机器人中每一只脚的脚点位置所对应的预测值，α为所述足式机器人的关节角度，为预测的旋转矩阵，为预测的第i只脚的脚点位置，为预测的所述足式机器人本体的质心位置。

4.根据权利要求3所述的足式机器人多模态感知的状态估计系统，其特征在于，所述预测模块包括：

第一测量单元，用于基于所述IMU传感器中的加速度计测量所述足式机器人的线性加速度；

第二测量单元，用于基于所述IMU传感器中的陀螺仪测量所述足式机器人的角速度；

计算单元，用于基于所述线性加速度和所述角速度，通过积分变换计算所述足式机器人的状态均值和协方差，并在本体坐标系表示出来。