[发明专利]一种机器人平衡控制方法、装置、可读存储介质及机器人

权利要求书

1.一种机器人平衡控制方法，其特征在于，包括：

将机器人的支撑足脚掌等效为飞轮模型的无质量长杆，将所述机器人的其它部位等效为所述飞轮模型的飞轮；

基于所述飞轮模型对所述支撑足脚掌的姿态进行控制。

2.根据权利要求1所述的机器人平衡控制方法，其特征在于，所述基于所述飞轮模型对所述支撑足脚掌的姿态进行控制，包括：

确定所述支撑足脚掌在世界坐标系下的第一旋转矩阵；

根据所述第一旋转矩阵计算所述支撑足脚掌在所述世界坐标系下的第一姿态角；

根据所述第一姿态角控制所述机器人对所述支撑足脚掌施加扭力。

3.根据权利要求2所述的机器人平衡控制方法，其特征在于，所述确定机器人的支撑足脚掌在世界坐标系下的第一旋转矩阵，包括：

获取所述机器人的躯干在所述世界坐标系下的第二姿态角；

根据所述第二姿态角计算所述机器人的躯干在所述世界坐标系下的第二旋转矩阵；

根据正向运动学确定所述机器人的支撑足脚掌相对于躯干的第三旋转矩阵；

根据所述第二旋转矩阵和所述第三旋转矩阵计算所述第一旋转矩阵。

4.根据权利要求3所述的机器人平衡控制方法，其特征在于，所述根据所述第二姿态角计算所述机器人的躯干在所述世界坐标系下的第二旋转矩阵，包括：

分别计算与所述第二姿态角中的横滚角、俯仰角、偏航角对应的横滚角旋转矩阵、俯仰角旋转矩阵、偏航角旋转矩阵；

根据所述横滚角旋转矩阵、所述俯仰角旋转和所述偏航角旋转矩阵计算所述第二旋转矩阵。

5.根据权利要求3所述的机器人平衡控制方法，其特征在于，所述根据所述第二旋转矩阵和所述第三旋转矩阵计算所述第一旋转矩阵，包括：

根据下式计算所述第一旋转矩阵：

Rfoot2world＝Rbody2world×Rfoot2body

其中，Rbody2world为所述第二旋转矩阵，Rfoot2body为所述第三旋转矩阵，Rfoot2world为所述第一旋转矩阵。

6.根据权利要求2所述的机器人平衡控制方法，其特征在于，所述根据所述第一旋转矩阵计算所述支撑足脚掌在所述世界坐标系下的第一姿态角，包括：

根据下式计算所述第一姿态角：

pitch_real＝atan2(-R₃₁，R₃₃)

yaw_real＝atan2(-R₁₂，R₂₂)

其中，R_ij为所述第一旋转矩阵中第i行第j列的元素，roll_real为所述第一姿态角中的横滚角，pitch_real为所述第一姿态角中的俯仰角，yaw_real为所述第一姿态角中的偏航角。

7.根据权利要求2至6中任一项所述的机器人平衡控制方法，其特征在于，所述根据所述第一姿态角控制所述机器人对所述支撑足脚掌施加扭力，包括：

根据下式计算所述机器人对所述支撑足脚掌施加的扭力：

torque_x＝kp_x×(roll_desired-roll_real)

torque_y＝kp_y×(pitch_desired-pitch_real)

其中，roll_real为所述第一姿态角中的横滚角，pitch_real为所述第一姿态角中的俯仰角，roll_desired为预设的横滚角预期值，pitch_desired为预设的俯仰角预期值，kp_x和kp_y为预设的比例系数，torque_x为所述扭力在x轴方向上的分量，torque_y为所述扭力在y轴方向上的分量。

8.一种机器人平衡控制装置，其特征在于，包括：

飞轮模型建立模块，用于将机器人的支撑足脚掌等效为飞轮模型的无质量长杆，将所述机器人的其它部位等效为所述飞轮模型的飞轮；

平衡控制模块，用于基于所述飞轮模型对所述支撑足脚掌的姿态进行控制。