[发明专利]一种多足机器人平衡控制方法

权利要求书

1.一种多足机器人平衡控制方法，其特征在于，该方法包括下述步骤：

(1)获取当前的机器人各关节角度值和机身位置姿态值，以初始化机器人各关节角度、机身位姿参数；

(2)实时获取各关节角度、机身位姿信息，并利用这些信息对机器人进行正运动学计算，得到机身重心、腿部各部分重心在机器人世界坐标系中的坐标；

(3)利用获取的各足端脚力信息、机器人各关节角度信息和机身位姿信息，构建出当前运动时刻的支撑平面；

(4)使用零力矩点方法判断机器人的稳定性，根据零力矩点的位置判定机器人是否失稳并计算稳定裕度d_m；

(5)根据机器人当前的稳定性情况采取相应控制策略：若机器人稳定，则实施预设的正常行走步态，并转入步骤(2)；若机器人失稳，进入步骤(6)；

(6)计算调整腿末端点H_At的位置约束条件；

(7)取步骤(4)中计算的稳定裕度d_m与预设的稳定裕度阈值S进行比较：若稳定裕度为负值且|d_m|＜S，则判定为机器人失稳程度较小，并进入步骤(8)；若稳定裕度为负值且|d_m|≥S，则判定为机器人失稳程度较大，执行步骤(9)；

(8)根据调整腿末端点位于机器人所受合力在支撑面上的投影线上或者投影延长线上的原则，计算调整腿末端点H_At的位置坐标，并转入步骤(10)；

(9)根据髋关节到倾翻轴线的垂足与调整腿末端点的连线，垂直于倾翻轴线的原则，计算调整腿末端点H_At的位置坐标，并转入步骤(10)；

(10)计算出调整腿末端的位置后经机器人运动学反解得到机器人各关节角度值，然后向执行机构输出各关节角度序列，执行系统动作使机器人到达控制器要求达到的位置。

2.根据权利要求1所述的多足机器人平衡控制方法，其特征在于，步骤(7)中，稳定裕度用零力矩点到支撑多边形的各边距离的最小值来表示

3.根据权利要求1所述的多足机器人平衡控制方法，其特征在于，步骤(3)中，构建支撑平面的过程为：在机器人运动过程中，首先根据机器人传感系统获得的脚力信息确定脚力最大的三个着地足，假设为足k、足l、足m，由着地点的位置信息求出当前运动时刻的支撑平面的法向向量，所述支撑平面的法向向量^CV_A计算公式为：

^CV_A＝(x_l-k，y_l-k，z_l-k)×(x_m-k，y_m-k，z_m-k)            式I

   ＝(y_l-kz_m-k-z_l-ky_m-k，z_l-kx_m-k-x_l-kz_m-k，x_l-ky_m-k-y_l-kx_m-k)

其中x_l-k＝x_l-x_k，y_l-k＝y_l-y_k，z_l-k＝z_l-z_k；x_m-k＝x_m-x_k，y_m-k＝y_m-y_k，z_m-k＝z_m-z_k^CP_Ak＝(x_k，y_k，z_k)，^CP_Al＝(x_l，y_l，z_l)，^CP_Am＝(x_m，y_m，z_m)为脚力最大的三个着地足k、l、m末端点在机身坐标系∑C中的位置坐标。