[发明专利]一种四足机器人姿态复位控制方法、装置及存储介质

权利要求书

1.一种四足机器人姿态复位控制方法，其特征在于，所述姿态复位控制方法包括：

数据解算步骤：获取所述四足机器人的姿态角、加速度和角速度；根据所述四足机器人的尺寸数据得出所述四足机器人的数学模型，并根据所述四足机器人的数学模型、姿态角和加速度计算得出四足机器人的角速度；所述四足机器人的数学模型是根据所述四足机器人的尺寸数据转换形成的数学模型；所述四足机器人的尺寸数据包括机器人的身体的长度、宽度、高度、大腿长度和小腿长度；

矩阵解算步骤：采用运动学正解对所述四足机器人的角速度、姿态角和加速度进行计算得出所述四足机器人的每个腿的足端位置，并结合所述四足机器人的尺寸数据进行里程计计算得出所述四足机器人的实时位置和实时速度；

摆动相计算步骤：获取并根据所述四足机器人的期望运动速度、实时速度与实时位置计算得出所述四足机器人的摆动相的每条腿的足端运动轨迹；同时，采用运动学逆解对所述四足机器人的摆动相的每条腿的足端运动轨迹进行计算得出所述四足机器人的摆动相的每条腿的伺服电机摇臂角度；

支撑相计算步骤：使用VMC三通道控制算法对所述四足机器人的实时速度和实时位置进行计算以得出所述四足机器人的支撑相的各个腿的关节力矩，以及采用伪力矩模型将所述四足机器人的支撑相的每条腿的关节力矩转换为伺服电机期望转速；同时更新四足机器人的支撑相的每条腿的伺服电机摇臂角度；控制步骤：根据所述四足机器人的每条腿的伺服电机摇臂角度和伺服电机的类型生成对应的伺服控制指令并控制四足机器人的每条腿的运动；

检测步骤：当四足机器人的摆动相的每条腿均触地时，将作为摆动相的每条腿立即停止运动并切换为支撑相，将作为支撑相的每条腿切换为摆动相，然后执行数据解算步骤；直到四足机器人停止运动。

2.根据权利要求1所述的四足机器人姿态复位控制方法，其特征在于，所述数据解算步骤还包括：通过姿态传感器获取IMU数据，并对IMU数据进行预处理，然后对预处理后的IMU数据进行姿态解算得出所述四足机器人的姿态角和加速度，最后结合所述四足机器人的尺寸数据、姿态角和加速度计算得出所述四足机器人的角速度。

3.根据权利要求2所述的四足机器人姿态复位控制方法，其特征在于，所述姿态传感器包括三轴陀螺仪和三轴加速度计；所述预处理包括中值滤波处理和/或低通滤波处理。

4.根据权利要求1所述的四足机器人姿态复位控制方法，其特征在于，所述矩阵解算步骤具体包括：

首先根据三角函数和余弦定理得出运动学正解表达式如公式(1)：

其中，l为虚拟腿长，L₁为四足机器人的小腿长度，L₂为四足机器人的大腿长度，θ＝[θ₁,θ₂]为电机驱动摇臂角度；

然后对机器人的虚拟腿长l进行分解得出四足机器人每条腿的足端位置p＝[x,z]为：

再根据公式(1)和(2)推算得出电机驱动摇臂角度θ＝[θ₁,θ₂]为：

对公式(3)进行求偏导得出雅可比矩阵：

其中，

最后根据雅可比矩阵将四足机器人的每个腿的关节速度转换为足端移动速度，进而得出四足机器人的每个腿的足端位置。

5.根据权利要求1所述的四足机器人姿态复位控制方法，其特征在于，所述摆动相计算步骤之前还包括：

修正步骤：获取外部定位传感器的数据，并将通过里程计计算得出的四足机器人的实时速度和实时位置和外部定位传感器的数据进行融合得出所述四足机器人的实时速度和实时位置；所述外部定位传感器的数据包括通过检测设备检测得出的四足机器人的实时位置和实时速度。

6.根据权利要求5所述的四足机器人姿态复位控制方法，其特征在于，所述修正步骤还包括：

通过卡尔曼滤波算法将里程计计算得出的所述四足机器人的实时速度与实时位置与外部定位传感器的检测得到的所述四足机器人的实时速度与实时位置进行融合得出所述四足机器人的实时速度和实时位置。