[发明专利]一种基于虚拟腿的四足机器人多支撑腿力分配方法

权利要求书

1.一种基于虚拟腿的四足机器人多支撑腿力分配方法，其特征在于，所述方法包括如下步骤：

S1.基于机器人结构参数与单腿构型，使用关节角度反馈值，采用运动学正解计算各支撑腿足端在胯关节坐标系{H}下的三维位置：

P_i^H＝f(q),i＝1…4

其中，P_i^H为第i条支撑腿足端在胯关节坐标系{H}下的三维位置，q为关节角度反馈值，f(q)为对应单腿构型的运动学正解计算函数；

S2.基于机器人左右两侧支撑腿胯关节间距和前后两侧支撑腿胯关节间距，将各支撑腿足端在胯关节坐标系{H}下的三维位置转化为机体坐标系{B}下的三维位置：

其中，P_i^B为第i条支撑腿足端在机体坐标系{B}下的三维位置，W为机器人左右两侧支撑腿胯关节间距，H为机器人前后两侧支撑腿胯关节间距；

S3.基于机器人机载IMU测量得到的姿态四元数，获取当前全局坐标系{N}与机体坐标系{B}间的转换矩阵，并将各支撑腿足端在机体坐标系{B}下的三维位置转换为全局坐标系{N}下的三维位置：

其中，为全局坐标系{N}与机体坐标系{B}间的转换矩阵，q₀、q₁、q₂、q₃为机器人当前的姿态四元数，为的转置，P_i^N为空间中第i条支撑腿足端在全局坐标系{N}下的三维位置；

S4.基于当前支撑腿Z轴支撑力估计情况，计算前后两侧对应虚拟腿分配权重，并计算虚拟腿位置：

将机体前后同侧的支撑腿简化为一条虚拟腿，构建两条虚拟腿支撑下的静力学平衡问题；基于当前支撑腿在全局坐标系{N}下的Z轴支撑力估计值对两侧虚拟腿的位置进行计算，以机器人机头方向为Y轴，侧向为X轴，Z轴满足右手关系，支撑腿顺序为左前为1号支撑腿，右前为2号支撑腿，右后为3号支撑腿，左后为4号支撑腿；

(1)前侧或后侧仅一条支撑腿着地情况

机器人前侧或后侧只有一条腿支撑时，其虚拟腿位置等于该支撑腿位置；

(2)前侧或后侧两条支撑腿着地情况

对于前侧有两条支撑腿着地情况，其虚拟腿位置分配权重分别为：

其中，W₁为1号支撑腿的虚拟腿位置分配权重，W₂为2号支撑腿的虚拟腿位置分配权重；

前侧虚拟腿位置为：

其中，为前两条支撑腿对应的前侧虚拟腿位置；

对于后侧两条支撑腿着地情况，其虚拟腿位置分配权重分别为：

其中，W₃为3号支撑腿的虚拟腿位置分配权重，W₄为4号支撑腿的虚拟腿位置分配权重；

后侧两条支撑腿对应的虚拟腿位置为：

其中，为后侧虚拟腿位置；

(3)四条支撑腿着地情况

前侧虚拟腿为1号支撑腿和2号支撑腿的分配权重，后侧虚拟腿为3号支撑腿和4号支撑腿的分配权重，即：

(4)三条支撑腿着地情况

前侧虚拟腿为1号支撑腿和2号支撑腿的分配权重，后侧虚拟腿位置与3号支撑腿重合，即

(3)斜对侧两条支撑腿着地情况

前侧虚拟腿位置与2号支撑腿重合，后侧虚拟腿位置与4号支撑腿重合，即

S5.构建全局坐标系{N}下两点支撑的静力学平衡问题

基于所得到的虚拟腿位置，进一步构建两点支撑下的静力学支撑分配问题，全局坐标系{N}下Z轴支撑力平衡公式为：

F_z＝f_vF,z+f_vH,z+G

其中，F_z为质心处期望Z轴支撑力，f_vF,z为前侧虚拟腿在全局坐标系{N}下的Z轴支撑力，f_vH,z为后侧虚拟腿在全局坐标系{N}下的Z轴支撑力，G为重力前馈；

全局坐标系{N}下X和Y轴支撑力平衡公式为：

F_x＝f_vF,x+f_vH,x

F_y＝f_vF,y+f_vH,y

其中，F_x为质心处期望X轴支撑力，f_vF,x为前侧虚拟腿在全局坐标系{N}下的X轴牵引力，f_vH,x为后侧虚拟腿在全局坐标系{N}下的X轴牵引力；F_y为质心处期望Y轴牵引力，f_vF,y为前侧虚拟腿在全局坐标系{N}下的Y轴牵引力，f_vH,y为后侧虚拟腿在全局坐标系{N}下的Y轴牵引力；

引入支撑腿落足位置产生的力臂进行机体扭矩分解，绕Z轴的扭矩控制平衡公式为：

T_z＝-f_vF,yP_vF,x-f_vH,yP_vH,x+f_vF,xP_vH,y+f_vF,xP_vH,y

其中，T_z为绕Z轴的期望扭矩，P_vF,x为前侧虚拟腿在全局坐标系{N}下X轴位置，P_vF,y为前侧虚拟腿在全局坐标系{N}下Y轴位置，P_vH,x为后侧虚拟腿在全局坐标系{N}下X轴位置，P_vH,y为后侧虚拟腿在全局坐标系{N}下Y轴位置；

绕X轴与绕Y轴的扭矩控制平衡公式分别为：

T_x＝-f_vF,yP_vF,x+f_vF,xP_vF,y-f_vH,yP_vH,x+f_vH,xP_vH,y

T_y＝-f_vF,zP_vF,x+f_vF,yP_vF,x-f_vH,zP_vH,x+f_vH,zP_vH,x

其中，T_x为绕X轴的期望扭矩，T_y为绕Y轴的期望扭矩；

根据上述方程得到两条虚拟腿支撑下的静力学平衡矩阵方程：

假设已知各虚拟腿在全局坐标系{N}下的X轴牵引力，并且Y轴牵引力仅用于控制机器人横滚轴姿态，基于上述假设对虚拟腿在Y和Z轴上的足底力进行分解，则上式简化为{F,T}＝A·f，即：

S6.求解虚拟腿对应的足底力

基于上述简化方程得到分解得到虚拟腿Y和Z轴足底力，结合已知X轴牵引力，根据以下公式得到两条虚拟腿对应足底力：

其中，A^-1为矩阵的逆矩阵；

S7.基于虚拟腿分配权重将虚拟腿足底力向具体支撑腿进行分配对虚拟腿足底力进行分解，将各虚拟腿的足底力重新分配给对应的支撑腿：

(1)四条支撑腿着地情况

基于分配权重，将虚拟腿足底力分解结果向四条支撑腿进行分配：

(2)三条支撑腿着地情况

基于分配权重，将虚拟腿足底力分解结果向三条支撑腿进行分配：

(3)斜对侧两条支撑腿着地情况

基于分配权重，将虚拟腿足底力分解结果向两条支撑腿进行分配：

(4)同侧两条支撑腿着地情况

基于分配权重，将虚拟腿足底力分解结果向两条支撑腿进行分配：

f₁＝f_vFW₁,f₂＝f_vFW₂

其中，fi为第i条支撑腿最终分配的足底力。