[发明专利]四足机器人的机身姿态控制方法及装置

权利要求书

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

实时获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标；

基于所述各足端的位置坐标计算所述待控制四足机器人的足端支撑平面相对于机身平面的角度；

基于所述角度，所述机身姿态角和所述机身姿态角速度计算所述待控制四足机器人的机身姿态角控制量；

基于所述机身姿态角控制量计算所述待控制四足机器人的各关节目标位置，以根据所述各关节目标位置控制所述待控制四足机器人的各关节进行运动；

其中，基于所述角度，所述机身姿态角和所述机身姿态角速度计算所述待控制四足机器人的机身姿态角控制量包括：

根据机身姿态角控制量计算算式计算所述待控制四足机器人的机身姿态角控制量，

其中，φ_cmd，θ_cmd，ψ_cmd表示所述机身姿态角控制量，φ，θ，ψ表示所述机身姿态角，表示所述机身姿态角速度，φ_d，θ_d，ψ_d表示理想机身姿态角，表示理想机身姿态角速度，φ_fb表示足端支撑平面相对于机身平面的横滚角度，θ_fb表示足端支撑平面相对于机身平面的俯仰角度，k_φ，k_θ，k_ψ和表示控制参数量。

2.根据权利要求1所述的方法，其特征在于，实时获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标包括：

通过设置于所述待控制四足机器人上的检测设备获取所述机身姿态角，其中，所述检测设备包括以下任一种：惯性测量单元IMU，陀螺仪；

当所述检测设备为所述IMU时，通过所述IMU获取所述机身姿态角速度；

当所述检测设备为所述陀螺仪时，对所述待控制四足机器人的机身姿态角进行差分计算，得到所述机身姿态角速度；

通过正运动学对所述待控制四足机器人的各关节角进行计算，得到各足端在机身坐标系中的位置坐标，并将所述各足端在机身坐标系中的位置坐标作为所述各足端的位置坐标。

3.根据权利要求1所述的方法，其特征在于，基于所述各足端的位置坐标计算所述待控制四足机器人的足端支撑平面相对于机身平面的角度包括：

根据第一算式和第二算式计算所述待控制四足机器人的足端支撑平面相对于机身平面的角度，

其中，φ_fb表示所述足端支撑平面相对于机身平面的横滚角度，θ_fb表示所述足端支撑平面相对于机身平面的俯仰角度，i可取f或h，j可取l或r，f表示前腿，h表示后腿，l表示左侧腿，r表示右侧腿，p_liz，p_riz，p_jfz，p_jhz分别表示对应的足端的位置坐标中Z轴方向坐标值，p_liy，p_riy分别表示对应的足端的位置坐标中Y轴方向坐标值，p_jfx，p_jhx分别表示对应的足端的位置坐标中X轴方向坐标值。

4.根据权利要求1所述的方法，其特征在于，基于所述机身姿态角控制量计算所述待控制四足机器人的各关节目标位置包括：

基于所述机身姿态角控制量计算旋转矩阵；

基于所述旋转矩阵计算足端控制位置值；

根据逆运动学对所述足端控制位置值进行计算，得到各关节目标位置，以根据所述各关节目标位置控制所述待控制四足机器人的各关节进行运动。

5.根据权利要求4所述的方法，其特征在于，基于所述旋转矩阵计算足端控制位置值包括：

根据足端控制位置计算算式计算所述待控制四足机器人的足端控制位置值，其中，表示所述待控制四足机器人的足端控制位置值，表示足端规划位置值，R表示所述旋转矩阵。