CN113721649B - 一种基于虚拟腿的四足机器人多支撑腿力分配方法 - Google Patents

Abstract

本发明提出一种基于虚拟腿的四足机器人多支撑腿力分配方法，包括计算各支撑腿足端在胯关节坐标系下的三维位置；将各支撑腿足端在胯关节坐标系下的三维位置转化为机体坐标系下的三维位置；获取当前全局坐标系与机体坐标系间的转换矩阵，并将各支撑腿足端在机体坐标系下的三维位置转换为全局坐标系下的三维位置；计算前后两侧对应虚拟腿分配权重，并计算虚拟腿位置；构建全局坐标系下两点支撑的静力学平衡问题；求解虚拟腿对应的足底力；基于虚拟腿分配权重将虚拟腿足底力向具体支撑腿进行分配。本发明能够保证四足机器人在复杂步态相序切换中支撑控制量输出足底力能合成相应的位姿虚拟控制量，为机器人实现高动态步态提供可靠的控制分配机制。

Description

一种基于虚拟腿的四足机器人多支撑腿力分配方法

技术领域

本发明属于机器人运动控制技术领域，具体涉及一种基于虚拟腿的四足机器人多支撑腿力分配方法。

背景技术

四足机器人稳定控制需要将期望力足底力向支撑腿进行分解，从而产生相应足底力完成对机体的支撑。在四足机器人步态相序切换中会出现多腿着地的情况，以Bound、Trot、Pace步态为例，同一时刻仅有2条腿支撑，以Gallop和站立步态为例，同一时刻有3条以上腿支撑，对其足底力的分配需要求解超定方程难以获取解析解，因此为实现对期望足底力的准确分配，保证位姿闭环控制运算的有效性，需要提出一种快速、在线的力分配方法。

发明内容

(一)要解决的技术问题

本发明的目的是提出一种基于虚拟腿的四足机器人多支撑腿力分配方法，以解决四足机器人支撑相力平衡控制中对虚拟伺服力的分配，实现将位姿控制所需虚拟力分解为各支撑腿所需三维足底力的的技术问题。

(二)技术方案

为解决上述技术问题，本发明提出一种基于虚拟腿的四足机器人多支撑腿力分配方法，该方法包括如下步骤：

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₂

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

(三)技术效果

本发明提出一种基于虚拟腿的四足机器人多支撑腿力分配方法，包括基于机器人结构参数与单腿构型，使用关节角度反馈值，采用运动学正解计算各支撑腿足端在胯关节坐标系下的三维位置；基于机器人左右两侧支撑腿胯关节间距和前后两侧支撑腿胯关节间距，将各支撑腿足端在胯关节坐标系下的三维位置转化为机体坐标系下的三维位置；基于机器人机载IMU测量得到的姿态四元数，获取当前全局坐标系与机体坐标系间的转换矩阵，并将各支撑腿足端在机体坐标系下的三维位置转换为全局坐标系下的三维位置；基于当前支撑腿Z轴支撑力估计情况，计算前后两侧对应虚拟腿分配权重，并计算虚拟腿位置；构建全局坐标系下两点支撑的静力学平衡问题；求解虚拟腿对应的足底力；基于虚拟腿分配权重将虚拟腿足底力向具体支撑腿进行分配。

本发明构建支撑腿的静力学分析，基于线性回归方法求取两到四条腿支撑下的力分配问题，基于虚拟腿原理解决同侧腿支撑情况下的超定方程解，保证四足机器人在复杂步态相序切换中支撑控制量输出足底力能合成相应的位姿虚拟控制量，为机器人实现高动态步态提供可靠的控制分配机制。

附图说明

图1为本发明实施例的四足机器人多支撑腿力分配方法主要流程图；

图2为本发明实施例的四足机器人两腿支撑和坐标系示意图；

图3为本发明实施例：(a)机体坐标系下机器人右视图，(b)机体坐标系下机器人俯视图，(c)四腿支撑情况俯视图，(d)三腿支撑情况俯视图，(e)两腿支撑情况俯视图；

图4为本发明实施例四腿支撑下的力分配结果示意图：(a)参考结果，(b)分配结果；

图5为本发明实施例四腿支撑下的力分配结果示意图：(a)参考结果，(b)分配结果；

图6为本发明实施例Trot两腿支撑下的力分配结果示意图：(a)参考结果，(b)分配结果。

具体实施方式

为使本发明的目的、内容和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

本实施例提出一种基于虚拟腿的四足机器人多支撑腿力分配方法，该方法主要流程如图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轴支撑力估计情况，计算前后两侧对应虚拟腿分配权重，并计算虚拟腿位置：

根据当前支撑腿全局位置与支撑情况，基于虚拟腿原理将机体前后同侧的支撑腿简化为一条虚拟腿，构建如图2所示的两条虚拟腿支撑下的静力学平衡问题。基于当前支撑腿在全局坐标系{N}下的Z轴支撑力估计值

对两侧虚拟腿的位置进行计算。

以机器人机头方向为Y轴，侧向为X轴，Z轴满足右手关系，支撑腿顺序为左前为1号支撑腿，右前为2号支撑腿，右后为3号支撑腿，左后为4号支撑腿。

机体坐标系{B}下机器人右视图和俯视图，分别如图3(a)和(b)所示。

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

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

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

对于前侧有两条支撑腿着地情况，如图3(b)中的1、2号支撑腿，其虚拟腿位置分配权重分别为：

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

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

其中，

为前侧虚拟腿位置。

对于后侧两条支撑腿着地情况，如图3(b)中的3、4号支撑腿，其虚拟腿位置分配权重分别为：

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

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

其中，

为后侧虚拟腿位置。

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

如图3(c)所示，前侧虚拟腿F为1号支撑腿和2号支撑腿的分配权重，后侧虚拟腿H为3号支撑腿和4号支撑腿的分配权重，即：

图4(a)为四条支撑腿着地情况下的参考力分配结果，足端矢量表示该支撑腿的三轴足底力，质心处矢量表示由足底力合成出的合力，机体姿态表示由足底力合成的机器人扭矩，图4(b)为通过本发明分配的足底力，其与图4(a)的参考力分配结果一致。

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

如图3(d)所示，前侧虚拟腿F为1号支撑腿和2号支撑腿的分配权重，后侧虚拟腿H位置与3号支撑腿重合，即：

图5(a)为三条支撑腿着地情况下的参考力分配结果，足端矢量表示该支撑腿的三轴足底力，质心处矢量表示由足底力合成出的合力，机体姿态表示由足底力合成的机器人扭矩，图5(b)为通过本发明分配的足底力，其与图5(a)的参考力分配结果一致。

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

如图3(e)所示，前侧虚拟腿F位置与2号支撑腿重合，后侧虚拟腿H位置与4号支撑腿重合，即

图6为典型的Trot斜对侧两条支撑腿着地情况，图中足端矢量表示该支撑腿的三轴足底力，质心处矢量表示由足底力合成出的合力，机体姿态表示由足底力合成的机器人扭矩。图6(b)为Trot步态两腿支撑时通过本发明分配的足底力，其与图6(a)的参考力分配结果一致。

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

基于所得到的虚拟腿位置，进一步构建如图2所示两点支撑下的静力学支撑分配问题，两条虚拟腿在全局坐标系{N}下的Z轴支撑力应当抵消重力，从而产生期望Z轴支撑力完成机体高度控制，全局坐标系{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轴的期望扭矩。

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

由于上式矩阵不满秩即该方程组为超越方程形式，因此无法得到前/后侧虚拟腿分解足底力f_vF、f_vH的解析解。考虑四足机器人整体机体尺寸长度大于宽度，且机器人运动时主要以前向运动为主方向，因此本发明假设已知各虚拟腿在全局坐标系{N}下的X轴牵引力，并且Y轴牵引力仅用于控制机器人横滚轴姿态，基于上述假设进一步对虚拟腿在Y和Z轴上的足底力进行分解，则上式简化为{F,T}＝A·f，即：

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

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

其中，A^-1为矩阵

的逆矩阵。

S7.基于虚拟腿分配权重将虚拟腿足底力向具体支撑腿进行分配

在S4中通过将多条支撑腿以虚拟腿理论映射为两条虚拟腿对角支撑的情况，进一步再对虚拟腿足底力进行分解，最终在伺服控制前需要将各虚拟腿的足底力重新分配给该侧对应的支撑腿，以前侧1号、2号支撑腿为例，各支撑腿最终的足底力分配结果如下：

f₁＝f_vFW₁,f₂＝f_vFW₂

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

对于前侧仅一条支撑腿着地的情况，其足底力分配结果与虚拟腿足底力分解结果一致，因此对于同侧有两条支撑腿时采用如上方法进行分配，以下对四足机器人典型的支撑情况进行针对性分析。

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

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

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

基于分配权重，将虚拟腿足底力分解结果向三条支撑腿进行分配，假设单侧支撑腿为4号腿，则其分配结果与虚拟腿足底力分解结果一致，即：

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

基于分配权重，将虚拟腿足底力分解结果向两条支撑腿进行分配，以1号和4号腿脚腿支撑为例，即：

/>

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (1)

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条支撑腿最终分配的足底力。

Images