CN106681341A - 基于多维度工作空间耦合算法的多足机器人步态优化控制方法 - Google Patents

Abstract

一种基于多维度工作空间耦合算法的多足机器人步态优化控制方法，包括如下步骤：步骤一、求解虚拟约束半径，根据虚拟约束半径解析机身摆动腿运动约束K空间；步骤二、求解摆动腿理想落足点约束R空间以及机身稳定性约束B空间，并对机身摆动腿运动约束K空间，理想落足点可达约束R空间，机身稳定性约束B空间进行耦合，解出多维度机身工作空间W，然后提出机身“死锁”的失稳情况及其解决措施；步骤三、根据步骤二求得的多维度机身工作空间，解析站立腿关节输出位置和机身工作空间之间的映射关系，最后，对关节转角进行多项式插补运算，完成机器人在非结构环境下的步态规划。本发明保证机器人在非结构环境下步态的稳定性以及步态的高效性。

Description

基于多维度工作空间耦合算法的多足机器人步态优化控制 方法

技术领域

本发明涉及多足机器人非结构环境下多维度机身工作空间耦合及步态优化领域，具体涉及基于多维度工作空间耦合算法的多足机器人步态优化控制方法。

背景技术

多足机器人具有丰富的驱动冗余和离散的步态点，能够实现复杂的步态方式，具有较强的地形适应能力。据统计，地球上有90％以上的地表属于崎岖表面，要使多足机器人在户外环境下进行作业，就必须对其在非结构环境下的步态进行规划，实现步态的稳定性和高效性。

在非结构环境下，多足机器人一个步态周期包括三个过程：(1)机器人机身重心移到工作空间内指定的点；(2)摆动腿抬起落到理想落足点；(3)摆动相和支撑相互换，新的摆动相落到理想落足点。其中，能否找到合理的机身工作空间是多足机器人能否在非结构环境下稳定步行的关键。

在多足机器人机身工作空间求解的方法中，较为常见的是解析法。机器人机身运动主要包括6个参数，分别是沿着空间坐标系三轴的移动和转动。解析法采用机身运动合成的方法，当机身处于理想姿态和理想位置时，利用机身坐标、世界坐标和关节坐标之间的坐标系姿态转换，分别求出足端相对于根关节的坐标表达式，然后合成求得足端最终的表达式。代入足端位置和关节角度约束，求出机器人重心在世界坐标系下的位置，改变机身姿态可得出重心的另外一种位置空间。这种算法的优点是求得的机身工作空间较为精确，但是，在计算过程中涉及到大量的坐标间姿态转换、机身和机器人摆动腿的正、逆运动学求解，算法的计算效率不高，在机器人步态实时计算中会影响机器人的步行效率。因此，目前迫切需要提出一种能够提高机器人步态效率和步态稳定性的机身工作空间求解算法，并将其应用到多足机器人在非结构环境下的步态规划中。

发明内容

鉴于现有解析法对多足机器人机身非结构环境下工作空间求解效率低的问题，本发明提出一种多维度耦合机身工作空间求解方法，该方法主要由三个约束子空间组成，这三个约束子空间分别是摆动腿运动约束K空间，摆动腿理想落足点约束R空间以及机身稳定性约束B空间，最终，这三个机身约束子空间耦合后的空间就是多足机器人机身非结构环境下的多维度工作空间；鉴于机身工作空间对于多足机器人在非结构环境下步态规划的重要性，将本文提出的多维度耦合机身工作空间求解算法应用于多足机器人非结构环境下的步态规划，最终，提出一种地形适应能力强，高效的步态规划方法。

本发明解决其技术问题所采用的技术方案是：

一种基于多维度工作空间耦合算法的多足机器人步态优化控制方法，包括以下步骤：

步骤一：求解虚拟约束半径，根据虚拟约束半径解析机身摆动腿运动约束K空间；

步骤二：求解摆动腿理想落足点约束R空间以及机身稳定性约束B空间，并对机身摆动腿运动约束K空间，理想落足点可达约束R空间，机身稳定性约束B空间进行耦合，解出多维度机身工作空间W；

步骤三：根据步骤二求得的多维度机身工作空间W，解析站立腿关节输出位置和机身空间之间的映射关系，并对关节转角进行多项式插补运算，完成机器人在非结构环境下的步态规划。

进一步，所述的步骤一中，首先，求解足端在根关节坐标系下的位置，令摆动腿连杆长为L_i(i＝1,2,3)，关节转角为θ_si,则摆动腿足端在根关节坐标系下的位置为：

p_x＝[L₁+L₂cθ_s2+L₃c(θ_s2+θ_s3)]cθ_s1

p_y＝[L₁+L₂cθ_s2+L₃c(θ_s2+θ_s3)]sθ_s1

p_z＝L₂sθ_s2+L₃s(θ_s2+θ_s3)

式中，sθ＝sinθ，cθ＝cosθ；

其次，解析摆动腿足端虚拟约束半径：

然后，令世界坐标系为O_W-zyz，机身坐标系为O_C-xyz，求解机身运动约束K_i空间：

式中，^Wc_i为每条摆动腿的运动约束K_i空间中心在世界坐标系中的位置，其表达式为：

式中，^Wa_i为足端在世界坐标系中的位置，为机身坐标系与机身坐标系之间的位置转换矩阵，^Cb_i为根关节在世界坐标系中的位置；

最后，对机器人六条腿的约束K_i空间进行耦合，得出摆动腿约束K空间：

K＝ΣK_i(i＝1,2,3,4,5,6)。

再进一步，所述的步骤二通过以下方式实现：

首先，求解摆动腿理想落足点可达约束R空间：

R＝F₁∩F₂∩F₃

上式为机器人采用三角步态时的理想落足点约束R空间，其中F_i(i＝1,2,3)为假想摆动腿的运动约束K_i空间，其求解方式和步骤一相同；

其次，求解机身稳定约束B空间：

则机身稳定性约束为：

d_M＝min{d₁,d₂,d₃}

上式中的系数表达式如下：

a₁＝y₂-y₁ b₁＝x₁-x₂ c₁＝x₂y₁-x₁y₂

a₂＝y₃-y₁ b₂＝x₁-x₃ c₂＝x₃y₁-x₁y₃

a₃＝y₂-y₁ b₃＝x₂-x₃ c₃＝x₃y₂-x₂y₃

然后，耦合机身摆动腿运动约束K空间，摆动腿理想落足点可达约束R空间以及机身稳定性约束B空间，解析多维度机身工作W：

W＝K∩R∩B

由步骤二中的表达式看出摆动腿运动约束K空间和机身姿态有关，因此，通过改变机身姿态来改变K空间，从而改变机身工作空间W。

更进一步，所述的步骤三通过以下方式实现：

首先，在根关节坐标系下，首先，求解站立腿足端和各关节输出位置之间的映射关系：

θ_d2＝2arctan t₁

θ_d3＝2arctan t₂-2arctan t₁

式中，t₁＝tan(θ_d2/2)，t₂＝tan[(θ_d2-θ_d3)/2]，为站立腿足端在根关节坐标系下的位置；

其次，在机身位于理想姿态下时，解析站立腿足端在机身坐标系下的位置：

式中，为摆动腿足端在根关节坐标系下的位置，为机身处于理想目标位姿时机身坐标系与世界坐标系之间的位置转换矩阵，其表达式如下：

式中，α、β、γ分别表示机身处于目标位姿时相对于世界坐标系的偏航角、横滚角和俯仰角；

然后，在机身处于理想位姿下时，解析站立腿足端在机身坐标系下的位置：

式中，为机身处于理想位姿下时，根关节坐标系与世界坐标系之间的位置坐标转换，为机身处于初始位姿下时，根关节坐标系与世界坐标系之间的位置坐标转换；

则站立腿足端在根关节坐标系下的位置：

式中，为机身处于理想位姿下时，根关节坐标系和机身坐标系之间的位置转换矩阵，为机身处于理想位姿下时，机身坐标系与世界坐标系之间的位置坐标转换，为机身处于初始位姿下时，机身坐标系与世界坐标系之间的位置坐标转换；

根据上式，利用站立腿足端位置和关节输出位置的映射关系，解出在根关节下，机身工作空间和关节输出位置的映射关系：

θ_d1＝π-A tan2(^by_C,^bx_C)

θ_d2＝2arctan t₁

θ_d3＝2arctan t₂-2arctan t₁

式中，t₁＝tan(θ_d2/2)，t₂＝tan[(θ_d2-θ_d3)/2]，^BA_C(^b x_c,^by_c,^bz_c)为机身坐标系在根关节坐标系下的位置；

最后，对关节输出角进行多项式插补运算，以六足机器人为例，完成机器人在非结构环境下的步态优化。

本发明的有益效果主要表现在：

1、本发明提出了一种机身非结构环境下的多维度耦合求解算法，能够迅速求出机身在复杂地形下的有效工作空间，采用本发明的非结构环境下步态规划方法的步态消耗时间约为现有步态规划算法的1/5，并且，随着地形结构越发复杂，其算法效率的提高更为明显。

2、基于多维度工作空间耦合算法的多足非结构环境下步态优化方法，提高了多足机器人在非结构环境下步态的稳定性。

3、基于多维度工作空间耦合算法的多足非结构环境下步态优化方法，提高多足机器人对非结构环境的地形适应能力。

附图说明

图1是本发明的基于多维度工作空间耦合算法的多足机器人步态优化控制方法应用的六足机器人结构示意图；

图2是在根关节坐标系下，多足机器人单摆动腿足端位置和关节输出位置之间的映射关系；

图3是机身工作空间摆动腿运动约束K空间的示意图，图中：OC和OW分别代表机身坐标系和世界坐标系，向量li代表虚拟约束半径；

图4是六足机器人非结构环境下机身运动约束K空间在x-y坐标面上的投影图。图中：两个圆相交部分代表运动约束K空间在x-y坐标面上的投影。

图5是六足机器人非结构环境下机身工作空间W在x-y坐标面上的投影图。

图6是六足机器人非结构环境下机身改变姿态后的摆动腿运动约束运动约束K空间在x-y坐标面上的投影图。图中：虚线部分为机身在原先姿态下的运动约束K空间，而实线部分为机身在现有姿态下的运动约束K空间；

图7是基于多维度工作空间耦合求解算法的步态优化控制方法流程图；

图8是六足机器人机身重心沿世界坐标系x轴方向上的位移曲线图；

图9是六足机器人腿1、3、5足端沿世界坐标系x轴方向上的位移曲线图；

图10是六足机器腿2、4、6足端沿世界坐标系x轴方向上的位移曲线图；

图11是六足机器人机身重心沿世界坐标系z轴方向上的位移曲线图；

图12是六足机器人腿1、3、5腿足端沿世界坐标系z轴方向上的位移曲线图；

图13是六足机器人腿2、4、6端沿世界坐标系z轴方向上的位移曲线图；

图14是六足机器人在非结构环境下运动时机身的最小稳定裕度变化曲线图；

图15是采用本发明的非结构环境下步态优化方法的步态消耗时间与现有步态规划算法消耗时间对比图，图中：虚线部分为采用本发明步态优化控制算法跨越一次障碍所需要的时间，实线部分为现有算法跨越一次障碍所需要的时间，不难发现，采用本发明的非结构环境下步态优化控制算法的步态消耗时间约为现有步态规划算法的1/5，并且随着障碍距离d的不断增加，本发明所采用的步态规划方法的算法效率在不断地提高。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1～图15，一种基于多维度工作空间耦合算法的步态优化控制方法，以六足机器人为例，包括六足机器人机身和六条相互独立的步行腿，所述步态控制方法包括以下步骤：

步骤一：令摆动腿的连杆长为L_i，关节转角为θ_i，建立摆动腿足端和关节输出位置之间的映射关系图如图2所示，由此求得摆动腿足端在根关节坐标系下的位置：

p_x＝[L₁+L₂cθ_s2+L₃c(θ_s2+θ_s3)]cθ_s1

p_y＝[L₁+L₂cθ_s2+L₃c(θ_s2+θ_s3)]sθ_s1

p_z＝L₂sθ_s2+L₃s(θ_s2+θ_s3)

式中，sθ＝sinθ，cθ＝cosθ；

其次，解析摆动腿足端虚拟约束半径：

然后，令世界坐标系为O_W-zyz，机身坐标系为O_C-xyz，如图3所示，求解机身运动约束K_i空间：

^Wc_i为每条摆动腿的运动约束Ki空间中心在世界坐标系中的位置，其表达式为：

式中，^Wa_i为足端在世界坐标系中的位置，为机身坐标系与机身坐标系之间的位置转换矩阵，^Cb_i为根关节在世界坐标系中的位置。

最后，对机器人六条腿的约束K_i空间进行耦合，得出摆动腿运动约束K空间：

K＝ΣK_i(i＝1,2,3,4,5,6)

摆动腿运动约束K空间在x-y坐标面上的投影如图4所示。

步骤二：求解摆动腿理想落足点约束R空间以及机身稳定性约束B空间，并对机身摆动腿运动约束K空间，理想落足点可达约束R空间，机身稳定性约束B空间进行耦合，解出多维度机身工作空间W，然后提出机身“死锁”的失稳情况及其解决措施，过程如下：

首先，求解摆动腿理想落足点可达约束R空间：

R＝F₁∩F₂∩F₃

上式为机器人采用三角步态时的理想落足点约束R空间，其中F_i(i＝1,2,3)为假想摆动腿的运动约束K_i空间，其求解方式和步骤一相同。

其次，求解机身稳定约束B空间：

则机身稳定性约束为：

d_M＝min{d₁,d₂,d₃}

上式中的系数表达式如下：

a₁＝y₂-y₁ b₁＝x₁-x₂ c₁＝x₂y₁-x₁y₂

a₂＝y₃-y₁ b₂＝x₁-x₃ c₂＝x₃y₁-x₁y₃

a₃＝y₂-y₁ b₃＝x₂-x₃ c₃＝x₃y₂-x₂y₃

然后，耦合机身摆动腿运动约束K空间，摆动腿理想落足点可达约束R空间以及机身稳定性约束B空间，得到多维度机身工作空间W：

W＝K∩R∩B

非结构环境下机身多维度耦合工作空间W如图5所示，图中：实线同心圆相交部分为机身运动约束K空间，虚线同心圆相交部分为摆动腿理想落足点可达约束F空间，实线三角形为稳定性约束B空间，三个子空间交集部分为机身空间W。

在非结构环境下，多足机器人在步行过程中可能随时会出现无论哪条腿摆动都会引起失稳的现象，这种现象称为机身“死锁”，产生这种现象的原因是机器人机身工作空间不存在。要实现多足机器人在非结构环境下的稳定步态，必须提出解决机身“死锁”的措施。

由步骤一中的表达式可以看出摆动腿运动约束K空间和机身姿态有关，因此，可以通过改变机身姿态来改变K空间，从而改变机身工作空间W。如图6所示，图中：虚线部分为机身在原先姿态下的运动约束K空间，而实现部分为机身在现有姿态下的运动约束K空间。

同理，可以通过改变机身的另外两个子空间来解决机身“锁死”，比如，可以通过改变站立腿数量来改变机身稳定约束B空间，或者改变理想落足点的位置来改变摆动腿理想落足点可达F空间。一般情况下，当机身工作空间不存在时，首先改变站立腿的数量，其次是改变理想落足点的位置，最后才是改变机身的姿态，因为这种方法具有一定的不确定性和随机性。

步骤三：在根关节坐标系下，首先，求解站立腿足端和各关节输出位置之间的映射关系：

θ_d2＝2arctan t₁

θ_d3＝2arctan t₂-2arctan t₁

式中，t₁＝tan(θ_d2/2)，t₂＝tan[(θ_d2-θ_d3)/2]，为站立腿足端在根关节坐标系下的位置。

其次，在机身位于理想姿态下时，解析站立腿足端在机身坐标系下的位置：

式中，为摆动腿足端在根关节坐标系下的位置，为机身处于理想目标位姿时机身坐标系与世界坐标系之间的位置转换矩阵，其表达式如下：

式中，α、β、γ分别表示机身处于目标位姿时相对于世界坐标系的偏航角、横滚角和俯仰角。

然后，在机身处于理想位姿下时，解析站立腿足端在机身坐标系下的位置：

则站立腿足端在根关节坐标系下的位置：

式中，为机身处于理想位姿下时，根关节坐标系和机身坐标系之间的位置转换矩阵，为机身处于理想位姿下时，根关节坐标系与世界坐标系之间的位置坐标转换，为机身处于初始位姿下时，根关节坐标系与世界坐标系之间的位置坐标转换。

根据上式，利用站立腿足端位置和关节输出位置的映射关系，解出在根关节下，机身工作空间和关节输出位置的映射关系：

θ_d1＝π-A tan2(^by_C,^bx_C)

θ_d2＝2arctan t₁

θ_d3＝2arctan t₂-2arctan t₁

式中，t₁＝tan(θ_d2/2)，t₂＝tan[(θ_d2-θ_d3)/2]，式中，^BA_C(^bx_c,^by_c,^bz_c)为机身坐标系在根关节坐标系下的位置。

最后，对关节输出角进行多项式插补运算，完成多足机器人在非结构环境下的步态优化。

本发明基于多维度工作空间耦合算法的步态优化控制方法流程框图如图7所示，图中，要实现多足机器人在非结构环境下的稳定、高效步态，最终需要求得多足机器人在非结构环境下摆动腿和站立腿关节的输出位置，即关节转角θ_s和θ_d。站立腿的关节转角主要由机身工作空间和机身逆运动学来确定，而摆动腿的关节转角主要由理想落足点的位置和摆动腿运动学决定。

采用本发明的基于多维度工作空间耦合算法的步态优化控制方法能够提高多足机器人在非结构环境下步态的稳定性和地形适应能力，以六足机器人为例，采用本发明的步态过程图如图8-13所示。图中：不难发现，在0-4s这段时间中，机器人采用二步态，也叫四边形步态，这是因为与三角步态相比，二步态具有更高的稳定性，机器人在一个环境中刚开始步行时，采用二步态更能保证机器人的稳定步行。

根据图8-13所得的数据，得出六足机器人在非结构环境下最小稳定裕度如图14所示，图中：机器人在起始迈步阶段具有较高的稳定裕度，经过一个周期的二步态后，采用步态效率更高的三角步态，但在这之前，机器人首先得调整腿部姿态(4-5s)，然后在5-11s采用两个周期的三角步态，腿1、3、5为一组，腿2、4、6为另一组。在11s左右，机身工作空间持续减小，最小稳定裕度也随之减小，此时，必须调整机器人的摆动腿数量，不然将会出现机身“锁死”的现象，于是在12-22s采用非周期步态，即机器人腿的运动顺序和足端的运动轨迹是非固定的。从图中还可以看出，改变步态策略之后，机器人的最小稳定裕度保持在较高的水平，避免了机器人失稳。

采用本发明的非结构环境下步态优化方法的步态消耗时间约为现有步态规划算法的1/5，并且，随着地形结构越发复杂，其算法效率的提高更为明显。如图15所示，图中，虚线部分为采用本发明步态优化控制算法跨越一次障碍所需要的时间，实线部分为采用现有算法跨越一次障碍所需要的时间。从图中可以看出，采用本发明算法的六足机器人能够在较短时间内迅速跨越障碍，并且随着障碍距离d的不断增加，本发明所采用的步态规划方法的算法效率不断地提高。

Claims (4)

1.一种基于多维度工作空间耦合算法的多足机器人步态优化控制方法，其特征在于：包括以下步骤：

步骤一：求解虚拟约束半径，根据虚拟约束半径解析机身摆动腿运动约束K空间；

步骤二：求解摆动腿理想落足点约束R空间以及机身稳定性约束B空间，并对机身摆动腿运动约束K空间，理想落足点可达约束R空间，机身稳定性约束B空间进行耦合，解出多维度机身工作空间W；

步骤三：根据步骤二求得的多维度机身工作空间W，解析站立腿关节输出位置和机身空间之间的映射关系，并对关节转角进行多项式插补运算，完成机器人在非结构环境下的步态规划。

2.如权利要求1所述的基于多维度工作空间耦合算法的多足机器人步态优化控制方法，其特征在于：所述的步骤一中，首先，求解足端在根关节坐标系下的位置，令摆动腿连杆长为L_i(i＝1,2,3)，关节转角为θ_si,则摆动腿足端在根关节坐标系下的位置为：

p_x＝[L₁+L₂cθ_s2+L₃c(θ_s2+θ_s3)]cθ_s1

p_y＝[L₁+L₂cθ_s2+L₃c(θ_s2+θ_s3)]sθ_s1

p_z＝L₂sθ_s2+L₃s(θ_s2+θ_s3)

式中，sθ＝sinθ，cθ＝cosθ；

其次，解析摆动腿足端虚拟约束半径：

$l_i=\sqrt{{p_x}^2+{p_y}^2+{p_z}^2}$

然后，令世界坐标系为O_W-zyz，机身坐标系为O_C-xyz，求解机身运动约束K_i空间：

$l_i^2={(p_x-c_x^i)}^2+{(p_y-c_y^i)}^2+{(p_z-c_z^i)}^2$

式中，为每条摆动腿的运动约束K_i空间中心在世界坐标系中的位置，其表达式为：

${c_W}_i=(c_x^i,c_y^i,c_z^i)={a_W}_i-A_C^W{b_C}_i$

式中，为足端在世界坐标系中的位置，为机身坐标系与机身坐标系之间的位置转换矩阵，为根关节在世界坐标系中的位置；

最后，对机器人六条腿的约束K_i空间进行耦合，得出摆动腿约束K空间：

K＝ΣK_i(i＝1,2,3,4,5,6)。

3.如权利要求1或2所述的基于多维度工作空间耦合算法的多足机器人步态优化控制方法，其特征在于：所述的步骤二通过以下方式实现：

首先，求解摆动腿理想落足点可达约束R空间：

R＝F₁∩F₂∩F₃

上式为机器人采用三角步态时的理想落足点约束R空间，其中F_i(i＝1,2,3)为假想摆动腿的运动约束K_i空间，其求解方式和步骤一相同；

其次，求解机身稳定约束B空间：

$d_i=\frac{|a_i{x}_C+b_i{y}_C+c_i|}{\sqrt{a_i^2+b_i^2}},(i=1,2,3)$

则机身稳定性约束为：

d_M＝min{d₁,d₂,d₃}

上式中的系数表达式如下：

a₁＝y₂-y₁ b₁＝x₁-x₂ c₁＝x₂y₁-x₁y₂

a₂＝y₃-y₁ b₂＝x₁-x₃ c₂＝x₃y₁-x₁y₃

a₃＝y₂-y₁ b₃＝x₂-x₃ c₃＝x₃y₂-x₂y₃

然后，耦合机身摆动腿运动约束K空间，摆动腿理想落足点可达约束R空间以及机身稳定性约束B空间，解析多维度机身工作W：

W＝K∩R∩B

由步骤二中的表达式看出摆动腿运动约束K空间和机身姿态有关，因此，通过改变机身姿态来改变K空间，从而改变机身工作空间W。

4.如权利要求1或2所述的基于多维度工作空间耦合算法的多足机器人步态优化控制方法，其特征在于：所述的步骤三通过以下方式实现：

首先，在根关节坐标系下，首先，求解站立腿足端和各关节输出位置之间的映射关系：

${\mathrm{\θ}}_{d1}=\mathrm{\π}-Atan2({y_b}_{A_i},{x_b}_{A_i})$

θ_d2＝2arctan t₁

θ_d3＝2arctant₂-2arctan t₁

式中，t₁＝tan(θ_d2/2)，t₂＝tan[(θ_d2-θ_d3)/2]，为站立腿足端在根关节坐标系下的位置；

其次，在机身位于理想姿态下时，解析站立腿足端在机身坐标系下的位置：

$\left[\begin{array}{c}x_{A_i}^s{}_C\\ y_{A_i}^s{}_C\\ z_{A_i}^s{}_C\end{array}\right]={\left({A_W}_{C_d}\right)}^T\left[\begin{array}{c}{x_W}_{A_i}\\ {y_W}_{A_i}\\ {z_W}_{A_i}\end{array}\right]$

式中，为摆动腿足端在根关节坐标系下的位置，为机身处于理想目标位姿时机身坐标系与世界坐标系之间的位置转换矩阵，其表达式如下：

$\begin{array}{c}{A_W}_{C_d}=\left[\begin{array}{ccc}c\mathrm{\α}& -s\mathrm{\α}& 0\\ s\mathrm{\α}& c\mathrm{\α}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}c\mathrm{\β}& 0& s\mathrm{\β}\\ 0& 1& 0\\ -s\mathrm{\β}& 0& c\mathrm{\β}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& c\mathrm{\γ}& -s\mathrm{\γ}\\ 0& s\mathrm{\γ}& c\mathrm{\γ}\end{array}\right]\\ =\left[\begin{array}{ccc}c\mathrm{\α}c\mathrm{\β}& -s\mathrm{\α}c\mathrm{\γ}+c\mathrm{\α}s\mathrm{\β}s\mathrm{\γ}& s\mathrm{\α}s\mathrm{\γ}+c\mathrm{\α}s\mathrm{\β}c\mathrm{\γ}\\ s\mathrm{\α}c\mathrm{\β}& c\mathrm{\α}c\mathrm{\γ}+s\mathrm{\α}s\mathrm{\β}s\mathrm{\γ}& -c\mathrm{\α}s\mathrm{\γ}+s\mathrm{\α}s\mathrm{\β}c\mathrm{\γ}\\ -s\mathrm{\β}& c\mathrm{\β}s\mathrm{\γ}& c\mathrm{\β}c\mathrm{\γ}\end{array}\right]\end{array}$

式中，α、β、γ分别表示机身处于目标位姿时相对于世界坐标系的偏航角、横滚角和俯仰角；

然后，在机身处于理想位姿下时，解析站立腿足端在机身坐标系下的位置：

$\left[\begin{array}{c}{x_C}_{A_i}\\ {y_C}_{A_i}\\ {z_C}_{A_i}\end{array}\right]={\left({A_W}_{B_d}\right)}^T(\left[\begin{array}{c}{x_W}_{A_i}\\ {y_W}_{A_i}\\ {z_W}_{A_i}\end{array}\right]+\left[\begin{array}{c}x_B^d{}_W-x_B^o{}_W\\ y_B^d{}_W-y_B^o{}_W\\ z_B^d{}_W-z_B^o{}_W\end{array}\right])$

式中，为机身处于理想位姿下时，根关节坐标系与世界坐标系之间的位置坐标转换，为机身处于初始位姿下时，根关节坐标系与世界坐标系之间的位置坐标转换；

则站立腿足端在根关节坐标系下的位置：

$\left[\begin{array}{c}{x_B}_{A_i}\\ {y_B}_{A_i}\\ {z_B}_{A_i}\end{array}\right]=\left({A_{C_d}}_{B_d}\right){\left({A_W}_{C_d}\right)}^T(\left[\begin{array}{c}{x_W}_{A_i}\\ {y_W}_{A_i}\\ {z_W}_{A_i}\end{array}\right]+\left[\begin{array}{c}x_C^d{}_W-x_C^o{}_W\\ y_C^d{}_W-y_C^o{}_W\\ z_C^d{}_W-z_C^o{}_W\end{array}\right])$

式中，为机身处于理想位姿下时，根关节坐标系和机身坐标系之间的位置转换矩阵，为机身处于理想位姿下时，机身坐标系与世界坐标系之间的位置坐标转换，为机身处于初始位姿下时，机身坐标系与世界坐标系之间的位置坐标转换；

根据上式，利用站立腿足端位置和关节输出位置的映射关系，解出在根关节下，机身工作空间和关节输出位置的映射关系：

θ_d1＝π-Atan2(^by_C,^bx_C)

θ_d2＝2arctant₁

θ_d3＝2arctan t₂-2arctan t₁

式中，t₁＝tan(θ_d2/2)，t₂＝tan[(θ_d2-θ_d3)/2]，^BA_C(^bx_c,^by_c,^bz_c)为机身坐标系在根关节坐标系下的位置；

最后，对关节输出角进行多项式插补运算，以六足机器人为例，完成机器人在非结构环境下的步态优化。