[发明专利]一种四足仿生机器人的身体姿态斜坡自适应控制方法

权利要求书

1.一种四足仿生机器人的身体姿态斜坡自适应控制方法，其特征是：包括以下步骤：

搭建仿生机器人机体与世界坐标系，将机器人建模为单个刚体，且在接触点处受力，建立机器人动力学模型；

计算并跟踪仿生机器人腿部在世界坐标系中的轨迹，并计算腿部关节力矩，考虑地面力控制过程中的关节力矩，利用模型预测控制确定最优控制轨迹；

根据得到的最优控制轨迹控制仿生机器人的运动；

获取仿生机器人行走过程中此时刻的姿态参数，确定足端位置的坐标映射，调整机器人在斜坡上的重心位置；具体过程包括：

基于零力矩点的稳定条件，通过调节机器人足端位置，以实现机器人质心的调整，在动力学基础上，根据机体坐标系、世界坐标系、轨迹规划坐标系和斜面坐标系的关系，将依据躯干姿态信息进行足端位置的坐标映射，实现斜坡足端位置的调整适应；具体步骤为：

机体坐标系∑_A：原点G_A位于机器人机体重心，X_A轴指向机器人运动前方，Y_A轴指向机器人的左方，Z_A轴垂直于机体向上；

世界坐标系∑_B：Z_B轴垂直于水平面向上，X_B轴为X_A轴在水平面的垂直投影，Y_B的方向遵循右手螺旋准则；

轨迹规划坐标系∑_M：其原点与∑_A原点重合，∑_M由∑_A旋转而成；

斜面坐标系∑_C：原点为机器人重心在斜面的投影，∑_C由∑_M旋转和平移得到；

由各坐标系间的关系得到：

^PHIPp_TOE＝R_y(θ_ad)R_x(ψ_ad)(^CHIPp_TOE+^Cp_HIP)+^MP_C-^MP_HIP (1)

其中^CHIPp_TOE为坐标系∑_C中足端相对于髋关节的位置，^PHIPp_TOE为坐标系∑_M中足端相对于髋关节的位置，^Cp_HIP和^MP_HIP为坐标系∑_C和∑_M中髋关节相对于原点的位置，^MP_C为坐标系∑_C在坐标系∑_M的位置；θ_ad与ψ_ad为坐标系∑_M到∑_C旋转中分别绕y轴、x轴的调整角；由坐标系间的关系可得：

其中，^AHIPp_TOE为坐标系∑_A中足端相对于髋关节的位置，^MP_B为坐标系∑_B在坐标系∑_M的位置，^AP_HIP为坐标系∑_A中髋关节的位置；θ_ref与ψ_ref分别表示坐标系∑_M到∑_A旋转中绕z轴、y轴和x轴的扭转角、俯仰角与横滚角；

四足机器人爬坡时，支撑位置调整和躯干

姿态调整相结合，令

θ_ad＝θ_ref＝θ_IMU

ψ_ad＝ψ_ref＝ψ_IMU

其中，θ_IMU为固定于躯干的惯性测量单元(IMU)测得的俯仰角，ψ_IMU为测得的横滚角；

式(1)和式(2)结合并简化为：

^AHIPp_TOE＝^CHIPp_TOE+R_x(-ψ_IMU)R_y(-θ_IMU)^MP_C

其中，^Mp_C＝[0 0 -H]^T，是保持∑_C原点在∑_M中的坐标系为常向量，H为站立时的质心高度，^CHIPP_TOE为坐标系∑_C中足端相对于髋关节的位置；

设置一虚拟斜坡，根据机器人此时刻姿态参数中躯干俯仰角度，计算当前状态下虚拟斜坡的角度，将此角度作为下一时刻机器人俯仰角的调整值，不断调整机器人的姿态，直到虚拟斜坡角度、躯干俯仰角和实际斜坡角度三者相等具体算法为：

p＝(L+Δ_F+Δ_H)cosα (31)

β’＝α＝β(33)

式(31)中，p为前后两足端在水平地面上的投影，L为机器人躯干长度，Δ_F为机器人前腿足端相对于前腿髋关节的位移，α为IMU实时测得的机体俯仰角；式(32)中，β’为前后足端所成虚拟斜坡的角度，h为机器人前后足端的高度差，由状态估计器得到；

其中参数k与机器人的关节力控、状态估计值、行走速度和地面材料特性有关；式(32)重写为