一种基于半圆形仿生刀锋腿的六足机器人航向控制方法

摘要

本发明涉及足式机器人控制领域，提出了一种基于半圆形仿生刀锋腿的六足机器人航向控制方法，针对跟踪缓变期望航向角的控制需求，利用前向位移误差构造出处于支撑相的腿足髋关节角度的期望值，提出六足机器人的前向位移闭环控制方法，采用同向差动原理，通过改变左右处于支撑相的腿足步幅大小，实现了六足机器人的缓变航向控制；针对跟踪急变期望航向角的控制需求，利用航向角误差构造出处于摆动相的腿足髋关节角度期望值，提出六足机器人急变航向角的闭环控制方法，采用反向差动原理，通过改变左右处于摆动相的腿足髋关节转动方向，实现了六足机器人急变航向控制。该六足机器人较好地规避远距离和近距离障碍物，并能较好地跟踪上预设的行走路径。

权利要求

1.一种基于半圆形仿生刀锋腿的六足机器人航向控制方法，其特征在于，当六足机器人的航向角与期望的航向角差值的绝对值大于阈值时，如果六足机器人转向空间不受限，则根据六足机器人当前所处的步态，调整支撑足的步幅；如果六足机器人转向空间受限，则根据六足机器人所处的步态，调整摆动足的转动方向；其中，如果六足机器人转向空间不受限，根据六足机器人所处的步态，调整支撑足的步幅时，如果六足机器人的航向角大于期望的航向角且处于第一步态，则减小第四足的步幅，增大第一足以及第五足的步幅；如果六足机器人的航向角大于期望的航向角且处于第二步态，则增大第三足的步幅，减小第二足以及第六足的步幅；如果六足机器人的航向角小于期望的航向角且处于第一步态，则增大第四足的步幅，减小第一足以及第五足的步幅；如果六足机器人的航向角小于期望的航向角且处于第二步态，则减小第三足的步幅，增大第二足以及第六足的步幅。

2.根据权利要求1所述的六足机器人航向控制方法，其特征在于，如果六足机器人转向空间受限，根据六足机器人所处的步态，调整摆动足的转动方向时，如果六足机器人的航向角大于期望的航向角且处于第三步态，则保持第二足、第三足以及第六足不变，同时控制第一足以及第五足正向转动，第四足反向转动；如果六足机器人的航向角小于期望的航向角且处于第四步态，则保持第一足、第四足以及第五足不变，同时控制第二足以及第六足正向转动，第三足反向转动。

3.根据权利要求1所述的六足机器人航向控制方法，其特征在于，如果六足机器人转向空间不受限，则根据六足机器人所处的步态，调整支撑足的步幅具体为：由第一步态过渡到第二步态时处于摆动相的腿足髋关节角度规划为：其中，

由第二步态过渡到第一步态时处于摆动相的腿足髋关节角度规划为：其中，

参数M11，M12，M21，M22均为保证两组步态顺利交替的髋关节角比例系数，θiold为上一时刻第i条腿足的角度值，其中，i＝1,2,3,4,5,6；

其中θifd为当前处于支撑相的第i条腿足髋关节期望离地角，θitd为即将处于支撑相的第i条腿足髋关节期望落地角，Kp2，Ki2，Kd2为PID离地角控制中的待调参数，Kp3，Ki3，Kd3为PID落地角控制中的待调参数，航向角误差值Δψ＝ψ‑ψd；θif为预置的腿足髋关节离地角，θit为预置的髋关节落地角；

其中θir为关节角度编码器读出的实际关节角度，θid为处于支撑相的第i条腿足的期望髋关节角度，Δx＝x‑cx，x为机器人的实际前向位移，cx为机器人的期望前向位移，Kp1，Ki1，Kd1为PID前向位移控制中的待调参数。

4.根据权利要求2所述的六足机器人航向控制方法，其特征在于，如果六足机器人转向空间受限，则根据六足机器人所处的步态，调整处于摆动相的腿足转动方向具体为：当六足机器人需要绕机体坐标系z轴顺时针转动且处于第三步态时，各腿足关节角度之间的关系可表示为：

当六足机器人需要绕机体坐标系z轴逆时针转动且处于第四步态时，各腿足关节角度之间的关系可表示为：

其中，θ′i为第i条腿足的髋关节角度，θ′id为处于摆动相的第i条腿足的期望髋关节角度，其中，i＝1,2,3,4,5,6。

说明书

一种基于半圆形仿生刀锋腿的六足机器人航向控制方法

技术领域

[0001] 本发明涉及足式机器人控制领域，具体为一种基于半圆形仿生刀锋腿的六足机器人航向控制方法。

背景技术

[0002] 不同于履带式机器人和轮式机器人，足式机器人利用行走机构以非连续的点接触形式在地面行走，其具备在非结构环境中稳定行走的能力。相较于普通六足机器人或其他
足端点接触机器人，基于半圆形仿生刀锋腿的六足机器人的不同之处在于：六条腿全部为
半圆弧型设计，以滚动代替转动，以面接触代替点接触。这使得该类型的六足机器人兼备足
式机器人的越障能力和轮式机器人的高速运动能力，因此该机器人可以胜任抗险救灾、物
资运输、地质勘探等面向复杂环境的特殊作业任务。

[0003] 为了能够在复杂多变的地形环境中高效作业，基于刀锋腿的六足机器人除了需要具备稳定行走的能力外，还需具备一定的航向控制能力。六足机器人的航向控制技术是实
现行走路径规划和自主决策的关键基础，是保证机器人高效作业的重要前提。具体地，当机
器人在外界不确定干扰下偏离预定行走路径时，可以通过恰当的航向角控制方法矫正其行
走路径，进而使其沿期望路径行走。因此，面向半圆形刀锋腿的六足机器人航向角控制是一
个很有理论和实际意义的研究问题。

[0004] 根据障碍物的远近，这类基于半圆形刀锋腿的六足机器人航向角控制策略大致可分成两大类：缓变航向角控制策略和急变航向角控制策略。当障碍物距离六足机器人较远
时，认为机器人的转向空间不受限制，可以控制机器人以一定速度行走的同时缓慢改变其
航向角从而避开该类障碍物；当遇到的障碍物的距离较近时，此时认为机器人转向空间受
到限制，可以放缓机器人行进速度的同时急速改变航向角从而避开近距离障碍物。

[0005] 综上所述，基于仿生刀锋腿的六足机器人实现航向控制存在的两个难点问题有：1)外界干扰力、运动学模型等不精确等不利因素将导致六足机器人支撑腿关节角度反解精
度难以保证；2)不同于传统机器人连续驱动特性，这类足式机器人的欠驱动和变拓扑离散
驱动特性将导致其航向角难以控制。

发明内容

[0006] (1)技术问题

[0007] 针对跟踪缓变期望航向角的控制需求，考虑不确定干扰力和模型不精确导致的六足机器人在行走过程中难以跟踪上预设行走速度的问题，利用前向位移误差构造出支撑腿
关节角度的期望值，提出六足机器人的前向位移闭环控制方法，同时结合六足机器人的结
构特性，采用同向差动原理，通过改变左右支撑足的步幅大小，实现了六足机器人的缓变航
向控制；针对跟踪急变期望航向角的控制需求，考虑不确定干扰力和模型不精确导致的六
足机器人在转向过程中难以跟踪上预设期望航向角的问题，利用航向角误差构造出摆动足
髋关节角度期望值，提出六足机器人急变航向角的闭环控制方法，同时结合六足机器人的
结构特性，采用反向差动原理，通过改变左右摆动足的髋关节转动方向，实现了六足机器人
急变航向控制。

[0008] (2)技术方案

[0009] 根据本发明的一方面提供了一种基于半圆形仿生刀锋腿的六足机器人航向控制方法，当六足机器人的航向角与期望的航向角差值的绝对值大于阈值时，如果六足机器人
转向空间不受限，则根据六足机器人所处的步态，调整支撑足的步幅；如果六足机器人转向
空间受限，则根据六足机器人所处的步态，调整摆动足的转动方向。

[0010] 根据本发明的示例性实施例，根据六足机器人所处的步态，如果六足机器人转向空间不受限，调整支撑足的步幅时，如果六足机器人的航向角大于期望的航向角且处于第
一步态，则减小第四足的步幅，增大第一足以及第五足的步幅；如果六足机器人的航向角大
于期望的航向角且处于第二步态，则增大第三足的步幅，减小第二足以及第六足的步幅；如
果六足机器人的航向角小于期望的航向角且处于第一步态，则增大第四足的步幅，减小第
一足以及第五足的步幅；如果六足机器人的航向角小于期望的航向角且处于第二步态，则
减小第三足的步幅，增大第二足以及第六足的步幅。

[0011] 根据本发明的示例性实施例，根据六足机器人所处的步态，如果六足机器人转向空间受限，调整摆动足的转动方向时，如果六足机器人的航向角大于期望的航向角且处于
第三步态，则保持第二足、第三足以及第六足不变，同时控制第一足以及第五足正向转动，
第四足反向转动；如果六足机器人的航向角小于期望的航向角且处于第四步态，则保持第
一足、第四足以及第五足不变，同时控制第二足以及第六足正向转动，第三足反向转动。

[0012] 根据本发明的示例性实施例，如果六足机器人转向空间不受限，则根据六足机器人所处的步态，调整处于支撑相的腿足的步幅具体为：

[0013] 由第一步态过渡到第二步态时处于摆动相的腿足髋关节角度规划为：

[0014]

[0015] 其中，

[0016]

[0017] 由第二步态过渡到第一步态时处于摆动相的腿足髋关节角度规划为：

[0018]

[0019] 其中，

[0020]

[0021] 参数M11，M12，M21，M22均为保证两组步态顺利交替的髋关节角比例系数，θiold(i＝1,2,3,4,5,6)为上一时刻第i条腿足的角度值；

[0022]

[0023]

[0024] 其中θifd为当前处于支撑相的第i条腿足髋关节期望离地角，θitd为即将处于支撑相的第i条腿足髋关节期望落地角，Kp2，Ki2，Kd2为PID(即比例(P)、积分(I)和微分(D)控制算
法)离地角控制中的待调参数，Kp3，Ki3，Kd3为PID落地角控制中的待调参数，航向角误差值Δ
ψ＝ψ‑ψd；θif为预置的腿足髋关节离地角，θit为预置的髋关节落地角；

[0025]

[0026] 其中θir(i＝1,2,3,4,5,6)为关节角度编码器读出的实际关节角度，θid(i＝1,2,3,4,5,6)为处于支撑相的第i条腿足的期望髋关节角度，Δx＝x‑cx，x为机器人的实际前向
位移，cx为机器人的期望前向位移，Kp1，Ki1，Kd1为PID前向位移控制中的待调参数。

[0027] 根据本发明的示例性实施例，如果六足机器人转向空间受限，则根据六足机器人所处的步态，调整处于摆动相的腿足的转动方向具体为：

[0028] 当六足机器人需要绕机体坐标系z轴顺时针转动且处于第三步态时，各腿足髋关节角度之间的关系可表示为：

[0029]

[0030] 当六足机器人需要绕机体坐标系z轴逆时针转动且处于第四步态时，各腿足髋关节角度之间的关系可表示为：

[0031]

[0032] (3)有益效果

[0033] 本发明的有益效果主要体现在以下方面：

[0034] 1)通过六足机器人上的定位系统/姿态传感器实时获取机器人的位姿，将其与期望的位移/期望的航向角求误差，并将这些误差引入到腿足髋关节角度的反解中，旨在解决
在外界不确定干扰力矩和模型不精确等情况下，六足机器人髋关节角度反解精度难于保证
的问题，进而实现机器人缓变航向行走过程中的位移闭环控制和急变航向行走过程中航向
角闭环控制。

[0035] 2)利用六足机器人的航向角误差，修正腿足髋关节的落地角和离地角，通过改变左右支撑足的步幅大小，自适应调节机器人两侧的支撑足在地面的滚动距离，从而实现基
于仿生刀锋腿驱动的六足机器人缓变航向控制。

[0036] 3)采用反向差动原理进行步态规划，通过改变左右摆动足的髋关节的转动方向，实现了六足机器人急变航向控制。

附图说明

[0037] 附图1为根据本发明实施例的基于半圆形仿生刀锋腿的六足机器人腿足编号示意图。

[0038] 附图2为根据本发明实施例的六足机器人坐标系示意图。

[0039] 附图3为根据本发明实施例的六足机器人腿足坐标系示意图。

[0040] 附图4为根据本发明实施例的六足机器人急变航向角时的转向原理示意图。

[0041] 附图5为根据本发明实施例的六足机器人在摆动腿一个周期内航向角变化示意图。

[0042] 附图6为根据本发明实施例的六足机器人腿足髋关节旋转时落地角和离地角示意图。

[0043] 附图7为根据本发明实施例的基于半圆形仿生刀锋腿的六足机器人航向控制系统框图。

[0044] 附图8为根据本发明实施例的基于半圆形仿生刀锋腿的六足机器人缓变航向角跟踪控制时的步态规划方案流程图。

[0045] 附图9为根据本发明实施例的基于半圆形仿生刀锋腿的六足机器人急变航向角跟踪控制时的步态规划方案流程图。

[0046] 附图10(a)和(b)分别为根据本发明实施例的基于半圆形仿生刀锋腿的六足机器人缓变航向行走时前向位移变化曲线对比图和前向位移误差曲线对比图。

[0047] 附图11(a)和(b)分别为根据本发明实施例的基于半圆形仿生刀锋腿的六足机器人缓变航向行走时航向角跟踪曲线图和航向角误差曲线图。

[0048] 附图12(a)、(b)、(c)和(d)分别为根据本发明实施例的基于半圆形仿生刀锋腿的六足机器人急变航向行走时质心在YZ、XZ、XY和XYZ坐标系上的运动曲线图。

[0049] 附图13(a)、(b)分别为根据本发明实施例的基于半圆形仿生刀锋腿的六足机器急变航向行走时航向角曲线对比图和航向角误差曲线对比图。

具体实施方式

[0050] 根据本发明实施例的一种基于半圆形仿生刀锋腿的六足机器人航向控制方法，当六足机器人的航向角与期望的航向角差值的绝对值大于阈值时，如果六足机器人转向空间
不受限(对应缓变航向控制)，则根据六足机器人所处的步态，调整处于支撑相的腿足的步
幅；如果六足机器人转向空间受限(对应急变航向控制)，则根据六足机器人所处的步态，调
整处于摆动相的腿足的转动方向。

[0051] 如下将对六足机器人跟踪缓变期望航向角以及跟踪急变期望航向角的控制方法的推导过程做详细的阐述。

[0052] 进一步地，六足机器人跟踪缓变期望航向角的控制方法，包括以下步骤：

[0053] 步骤1：根据实际需求预置六足机器人行走参数指令(vd,ψd)。其中，vd为期望行走速度，ψd为期望的航向角。根据期望行走参数规划出六足机器人在机体坐标系下沿期望航
向角方向的质心运动轨迹，质心运动轨迹表达式为

[0054]

[0055] 其中，px为机器人质心零力矩参考点(ZMP)沿机体坐标系x轴方向的坐标，1(t‑kT0)是起点为kT0的单位阶跃函数，k为行走的步数，T0为单步行走的周期，B为质心零力矩参考点
的单步距离增量，vd＝B/T0，cx为机器人沿机体坐标系x轴方向的质心运动轨迹，cz为机器人
质心距离地面的期望高度且为常数，g为重力加速度。

[0056] 步骤2：六足机器人采用三足步态行走，即编号为1、4、5腿足为一组，编号为2、3、6的腿足为另一组。为了沿期望的轨迹行走，机器人在步态A(腿足1、4、5为支撑相；腿足2、3、6
为腾空相)和步态B(腿足2、3、6为支撑相；腿足1、4、5为腾空相)之间交替摆动，其中第i条腿
足的支撑相和腾空相的临界点是由预置的腿足髋关节离地角θif与髋关节落地角θit进行划
分的。机器人为了实现步态的交替，需要控制机器人各个腿足髋关节的角度。令lir(i＝1,2,
3,4,5,6)为机器人第i条腿足的髋关节与腿着地点之间的距离，由于摆动腿足与地面发生
接触的弧长较小，因此有 故可将lir看作常量。令θi表示为第i条腿足的髋关节竖直方
向与lr之间的夹角。根据步骤1规划出的质心运动轨迹，采用六足机器人逆运动学方程反解
出六足机器人在步态A时支撑相的腿足髋关节角度的反解表达式为

[0057]

[0058] 腾空相的腿足髋关节表达式为

[0059]

[0060] 六足机器人在步态B时支撑相的腿足髋关节角度的反解表达式为

[0061]

[0062] 腾空相的腿足髋关节表达式为

[0063]

[0064] 将反解出的腿足髋关节角度θi(i＝1,2,3,4,5,6)作用于机器人的腿足髋关节，机器人开始运动。

[0065] 步骤3：为了减小前向干扰力和运动学模型不精确等不利因素对支撑腿足髋关节角度反解精度的影响，实时读取六足机器人定位系统测量到的前向位移信息x，将其与规划
的质心前向位移信息作差，得到前向位移误差Δx＝x‑cx，并将Δx引入到腿足髋关节角度
的反解中，实现机器人前向位移的闭环控制。所述控制律可表示为

[0066]

[0067] 其中θir(i＝1,2,3,4,5,6)为关节角度编码器读出的实际关节角度，θid(i＝1,2,3,4,5,6)为处于支撑相的第i条腿足的期望髋关节角度，Kp1，Ki1，Kd1为PID前向位移控制中
的待调参数。

[0068] 步骤4：为使机器人按期望航向角行走，将六足机器人航向角误差Δψ引入腿足髋关节离地角θif与髋关节落地角θit的规划中，具体地，将机器人姿态传感器测量的航向角ψ
与预置的期望航向角ψd作差，得到航向角误差值Δψ＝ψ‑ψd。使用Δψ修正腿足髋关节落地角
和离地角，自适应调节机器人两侧处于支撑相的腿足在地面的滚动距离，从而保证机器人
按预定航向角行走。所述的航向角控制律可表示为

[0069]

[0070]

[0071] 其中θifd为当前处于支撑相的第i条腿足髋关节期望离地角，θitd为即将处于支撑相的第i条腿足髋关节期望落地角，Kp2，Ki2，Kd2为PID离地角控制中的待调参数，Kp3，Ki3，Kd3
为PID落地角控制中的待调参数。

[0072] 步骤5：为实现步态A和步态B之间的顺利交替，避免步态重叠，由步态A过渡到步态B时处于摆动相的腿足髋关节角度规划为

[0073]

[0074] 其中，

[0075]

[0076] 同理，由步态B过渡到步态A时处于摆动相的腿足髋关节角度规划为

[0077]

[0078] 其中，

[0079]

[0080] 参数M11，M12、M21，M22均为保证两组步态顺利交替的关节角比例系数，其数值仅与期望离地角θitd(i＝1,2,3,4)和期望落地角θifd(i＝1,2,3,4)有关；θiold(i＝1,2,3,4,5,6)为
上一时刻第i条腿的角度值。

[0081] 进一步地，六足机器人跟踪急变期望航向角的控制方法，包括以下步骤：

[0082] 步骤1：根据实际需求预置六足机器人转向参数指令(ωd,ψt)，其中，ωd为期望航向角速度，ψt为目标航向角。根据期望转向参数指令规划出六足机器人急变期望航向角ψd，
急变期望航向角表达式为

[0083]

[0084] 其中，ωd为机器人期望角速度，ψ0为初始航向角。

[0085] 步骤2：六足机器人为了实现转向，采用三足步态，即编号为1、4、5腿足为一组，编号为2、3、6的腿足为另一组。其中一组处于支撑相，用于机器人的站立，另外一组处于摆动
T
相，用于机器人的转向。假设rBi＝[rBix rBiy rBiz] 为第i条腿足的髋关节到机器人质心的向
量，θ′i为第i条腿足的髋关节角度，根据步骤1预设的期望航向角速度，可以得到机器人转
向时处于摆动相腿足的切向速度分量为

[0086]

[0087] 步骤3：根据机器人转向的切向速度分量，可以得到机器人转向时处于摆动相腿足的足端速度向量为

[0088]

[0089] 式中，

[0090] 步骤4：当机器人转向时，需要控制机器人处于摆动相的各个腿足的髋关节角度。利用单腿的逆运动学方程反解出处于摆动相腿足的髋关节角度θ′i，其反解表达式为

[0091]

[0092] 其中，J+(θ′i)为矩阵J(θ′i)＝[lir  cosθ′i  0  ‑lir sinθ′i]T的伪逆，为第i条腿的转
弯半径向量。

[0093] 步骤5：为了减小干扰力矩和运动学模型不精确等不利因素对机体航向角的影响，实时读取安装在六足机器人质心处的姿态传感器所得到的航向角信息ψ，将其与期望航向
角信息ψd作差Δψ＝ψ‑ψd，并将航向角误差Δψ引入到处于摆动相的腿足髋关节角度反解中，
实现机器人急变期望航向角的闭环控制。所述控制律可表示为

[0094]

[0095] 其中θ′ir为关节角度编码器读出的实际髋关节角度，θ′id为处于摆动相的第i条腿足的期望髋关节角度，K′p1，K′i1，K′d1为PID航向角控制中的待调参数。

[0096] 步骤6：采用反向差动转向方式实现机器急变航向控制。当机器人需要绕机体坐标系z轴顺时针转动时，机器人采用步态C，各腿足髋关节角度之间的关系可表示为

[0097]

[0098] 即腿足2，3，6的髋关节以固定角度θ′2，θ′3，θ′6保持支撑相且不发生滚动；腿足1，5的髋关节沿正方向以角速度 转动，腿足4的髋关节沿反方向以角速度 转动。

[0099] 当机器人需要绕机体坐标系z轴逆时针转动时，机器人采用步态D，各腿足髋关节角度之间的关系可表示为

[0100]

[0101] 即腿足1，4，5的髋关节以固定角度θ′1，θ′4，θ′5保持支撑相且不发生滚动；腿足2，6的髋关节沿正方向以角速度 转动，腿足3的髋关节沿反方向以角速度 转动。

[0102] 下面结合说明书附图来详细描述本发明的实施例。

[0103] 附图1为基于半圆形仿生刀锋退的六足机器人腿足编号示意图。编号为1、3、5的腿足在机器人的左侧，编号为2、4、6的腿足在机器人的右侧。当机器人采用三足步态行走时，
则编号为1、4、5腿足为一组，编号为2、3、6的腿足为另一组。需要说明的是，编号为1、3、5的
腿足与编号为2、4、6的腿足对称设置在六足机器人两侧。腿足3、4分别位于两则中间位置，
腿足1、5与腿足3的距离相等，腿足2、6与腿足4的距离相等。编号1、2、3、4、5、6的腿足可以对
应称作第一足(左前足)、第二足(右前足)、第三足(左中足)、第四足(右中足)、第五足(左后
足)以及第六足(右后足)。相应地，当六足机器人处于缓变航向角控制时，编号为1、4、5腿足
为支撑足(也可称作支撑相)，编号为2、3、6的腿足为摆动足(也可称作摆动相)，此时六足机
器人处于步态A(也可称作第一步态)；编号为2、3、6腿足为支撑足(也可称作支撑相)，编号
为1、4、5的腿足为摆动足(也可称作摆动相)，此时六足机器人处于步态B(也可称作第二步
态)。当六足机器人需要绕机体坐标系(B)的z轴顺时针转动且编号为2、3、6腿足为支撑足
(也可称作支撑相)时，此时六足机器人处于步态C(也可称作第三步态)。当六足机器人需要
绕机体坐标系(B)的z轴逆时针转动且编号为1、4、5腿足为支撑足(也可称作支撑相)时，此
时六足机器人处于步态D(也可称作第四步态)。

[0104] 附图2为六足机器人建立的坐标系，包括全局坐标系(G)和机体坐标系(B)，其中，全局坐标系G和机体坐标系B的原点不一，二者的x、y、z轴的方向一致。

[0105] 附图3为六足机器人的腿足建立的坐标系，包括腿足的髋关节(P0)的坐标系和足(P1)的坐标系，其中，髋关节P0的坐标系原点为髋关节的中心点，足P1的坐标系原点为腿足
与水平面的切点，二者的x、y、z轴的方向一致。图3中，θ′为腿足的髋关节角度(即，髋关节P0
的坐标系和足P1的坐标系原点的连线与垂直线之间的夹角)，r为仿生刀锋腿半径，lr为腿足
髋关节与足之间的距离，当髋关节角度θ′为0°时，有lr＝2r。

[0106] 参照附图3，采用D‑H参数法构建由坐标系P0到坐标系P1的变换矩阵

[0107]

[0108] 其中，左侧 矩阵表示姿态矩阵，右侧 矩阵表示位置矩阵。由变换矩阵 知，在腿足髋关节旋转运动过程中足(P1)仅在x1z1平面上发生平移变
T
化。若机器人的足在腿足髋关节坐标系中坐标为P1＝[ax ay az] ,根据变换矩阵得到足端坐
标与腿足髋关节角度之间的转换为

[0109]

[0110] 对其对时间进行求导，得到足端的速度向量与腿足髋关节角速度之间的关系为

[0111]

[0112] 由于摆动腿与地面发生接触的弧长较小，可近似认为 因此上式可化简为

[0113]T

[0114] 令J(θ′)＝[lrcosθ′ 0 ‑lrsinθ′] ， 为足端的速度向量。

[0115] 附图4为六足机器人急变航向角时的转向原理示意图。为了实现机器人的转向，以编号为2、3、6的腿足为例，腿足2、6同步同向转向，腿足3和2、6同步反向转向。以腿足2为例，
令vl2为机器人转向时腿足2的前向速度，v′l2为机器人转向时腿足2的切向速度分量(其与
rB2x垂直)，并由腿足的切向速度分量来实现机器人的转向。

[0116] 令rBi＝[rBix  rBiy  rBiz]T为第i条腿足的髋关节到机器人质心的向量，为第i条腿足的足端速度向量；假设初始状态下，六足机器人腿足髋关
节角度均为θ′s，则机器人在转向过程中摆动腿足与地面接触的时间与摆动腿足一个运动
周期的比值为

[0117] 附图5为六足机器人在摆动腿足一个运动周期T内航向角变化示意图。由于机器人摆动腿足在一个运动周期内分别处于支撑相 和摆动相 机器人的航向
角发生改变只能在摆动腿足处于支撑相时才能实现。因此，机器人若要在一个运动周期T内
实现由航向角ψ0变化到ψT(如图中线段A所示)，则需要机器人在摆动腿足处于支撑相时航向
角从ψ0变化到ψT(如图中线段A’所示)，且在摆动腿足处于摆动相时保持航向角ψT不变(如图
中线段B’所示)。因此，根据期望的角速度指令ωd，机器人实际的转向角速度应为

[0118]

[0119] 由于lir＝2r·cosθ′s，根据v＝ω·r以及 可知，机器人转向时第i条腿足足端速度的切向分量为

[0120]

[0121] 其中，ωd为机器人的期望航向角速度。

[0122] 由附图4可知

[0123] v′li＝vlicosαi,i＝1,2,3,4,5,6

[0124] 式中，

[0125] 综上所述得到机器人急变航向角时，摆动腿足髋关节角度反解表达式为

[0126]

[0127] 其中，J+(θ′i)为矩阵J(θ′i)＝[lircosθ′i 0 ‑lirsinθ′i]T的伪逆，第i条腿足转弯半径向量为

[0128] 附图6为六足机器人腿部关节旋转时落地角和离地角示意图。图中，θt称为落地角，θf称为离地角。

[0129] 附图7为六足机器人航向控制系统框图。具体包括了跟踪缓变和急变两种期望航向角的控制方法。针对跟踪缓变期望航向角的控制需求，考虑不确定干扰力矩和模型不精
确导致的六足机器人在行走过程中难以跟踪上预设行走速度的问题，利用前向位移误差构
造出支撑腿关节角度的期望值，提出六足机器人的前向位移闭环控制方法，同时结合六足
机器人的结构特性，采用同向差动原理，通过改变左右处于支撑相的腿步幅大小，实现了六
足机器人的缓变航向控制；针对跟踪急变期望航向角的控制需求，考虑不确定干扰力和模
型不精确导致的六足机器人在转向过程中难以跟踪上预设期望航向角的问题，利用航向角
误差构造出处于摆动相的腿关节角度期望值，提出六足机器人急变航向角的闭环控制方
法，同时结合六足机器人的结构特性，采用反向差动原理，通过改变左右处于摆动相的腿关
节的转动方向，实现了六足机器人急变航向控制。

[0130] 附图8为六足机器人缓变航向角跟踪控制时的步态规划方案流程图。当六足机器人的航向需要进行修正时，根据当前航向角误差和机器人当前所处的步态A或B，通过改变
左右两侧处于支撑相腿足的步幅大小来实现六足机器人转向。

[0131] 当六足机器人的航向角与期望的航向角差值的绝对值大于阈值时，如果六足机器人的航向角大于期望的航向角且处于第一步态，则减小第四足的步幅，增大第一足以及第
五足的步幅；如果六足机器人的航向角大于期望的航向角且处于第二步态，则增大第三足
的步幅，减小第二足以及第六足的步幅；如果六足机器人的航向角小于期望的航向角且处
于第一步态，则增大第四足的步幅，减小第一足以及第五足的步幅；如果六足机器人的航向
角小于期望的航向角且处于第二步态，则减小第三足的步幅，增大第二足以及第六足的步
幅。

[0132] 附图9为六足机器人急变航向角跟踪控制时的步态规划方案流程图。当六足机器人需要急变航向角时，根据当前航向角误差，通过左右两侧处于支撑相的腿足来实现六足
机器人站立，同时改变左右两侧处于摆动相腿足的髋关节旋转方向来实现六足机器人转
向。

[0133] 当六足机器人的航向角与期望的航向角差值的绝对值大于阈值时，如果六足机器人的航向角大于期望的航向角且处于第三步态，则保持第二足、第三足以及第六足不变，同
时控制第一足以及第五足正向转动，第四足反向转动；如果六足机器人的航向角小于期望
的航向角且处于第四步态，则保持第一足、第四足以及第五足不变，同时控制第二足以及第
六足正向转动，第三足反向转动。

[0134] 为了验证六足机器人的缓变航向跟踪控制，假设六足机器人质心初始位置位于(0，0，0.147)处，预置机器人行走参数指令为(0.5，ψd)。即初始时刻机器人位置坐标x0＝0m、
y0＝0m、z0＝0.147m；初始速度v0＝0m/s，期望速度vd＝0.5m/s，初始航向角ψ0＝‑270°，本实
施例中期望航向角ψd呈阶梯形不断发生改变；六条腿关节处的初始角度均为θi＝0°；人为给
定的髋关节离地角θif＝15°、髋关节落地角θit＝345°；零力矩点规划参数为：B＝1、T0＝2；控
制参数为：Kp1＝‑0.08、Kd1＝0、Ki1＝0、Kp2＝‑0.4、Ki2＝0、Kd2＝0、Kp3＝‑0.4、Ki3＝0、Kd3＝0；
给机器人机体质心施加以F＝4N的干扰力，规定干扰力沿机体坐标系x轴正方向，仿真时长
为9.6s。

[0135] 当定位传感器测得的机器人位移信息x小于该仿真时刻时的质心期望位移cx时，此时通过计算前向位移反馈值Δx＝x‑cx将其用于机器人髋关节角的反解中，使得髋关节
角速度 增大，进而使机器人位移反馈误差Δx减小；当定位传感器测得的机器人位移信息x
大于该仿真时刻时的质心期望位移cx时，将前向位移反馈值Δx引入机器人髋关节角的反
解中，使得髋关节角速度 减小，进而使机器人位移反馈值Δx增大，最终使其沿期望运动
轨迹航行。

[0136] 当姿态传感器测得的航向角ψ小于期望航向角ψd时，此时意味着机器人航向角向右侧偏移，通过实时计算姿态角误差值Δψ＝ψ‑ψd，规划机器人腿足髋关节落地角θit和离地
角θif，使得位于六足机器人左侧的支撑相腿足1、5(步态A)或者腿3(步态B)步幅减小，右侧
支撑相腿足4(步态A)或者腿2、6(步态B)步幅增大，机器人向左侧纠正航向；当姿态传感器
测得的航向角ψ大于期望航向角ψd时，此时意味着机器人航向角向左侧偏移，通过实时计算
姿态角误差值Δψ＝ψ‑ψd，规划机器人腿足髋关节落地角θit和离地角θif，使得位于六足机器
人左侧的支撑相腿足1、5(步态A)或者腿3(步态B)步幅增大，右侧的支撑相腿足4(步态A)或
者腿足2、6(步态B)步幅减小，机器人向右侧纠正航向，最终实现沿期望航向行走。

[0137] 由图10(a)、(b)可以看出机器人在干扰力的作用下，有前向位移反馈的机体位移能很好的跟踪期望运动轨迹，航行速度基本保持在0.5m/s，最大位移偏差为20cm；而没有前
向位移反馈的机体前向位移在x轴正方向干扰力的作用下前向位移误差逐渐增大，运动轨
迹逐渐偏离期望质心运动轨迹呈发散趋势。由图11(a)、(b)可以看出机器人机体航向角能
够实现稳定跟随不断发生变化的期望航向角，航向角误差基本保持在±2°。

[0138] 为了验证六足机器人的急变航向跟踪控制，六足机器人质心初始位置位于(0.068，0.212，0.147)处，即初始时刻机器人位置坐标x0＝0.068m、y0＝0.212m、z0＝
0.147m，假设初始角速度ω0＝0m/s，初始航向角ψ0＝‑270°，六条腿关节处的初始角度均为
θ′s＝30°。期望角速度ωd为9.4°/s，目标航向角ψt＝‑180°。给机体质心施加以M＝2N·m的
干扰力矩，干扰力矩沿z轴正方向，控制参数为：K′p1＝‑0.12、K′d1＝0、K′i1＝0，仿真时长为
9.6s。

[0139] 当姿态传感器测得的机器人航向角信息ψ小于该仿真时刻时的期望航向角ψd时，此时通过计算航向角反馈值Δψ＝ψ‑ψd，将其引入机器人关节角的反解中，使得关节角速度
增大，进而使机器人航向角反馈值Δψ减小；当姿态传感器测得的机器人位移信息ψ大于该
仿真时刻时的期望航向角ψd时，将航向角反馈值Δψ引入机器人关节角的反解中，使得关节
角速度 减小进而使机器人航向角反馈值Δψ增大，最终使其沿期望航向角ψd航行。

[0140] 腿足1，4，5以固定髋关节角度θ′1，θ′4，θ′5保持支撑相且不发生滚动；腿足2，6沿正方向以期望髋关节角速度 转动，腿足3沿反方向以期望髋关节角速度 转动。

[0141]

[0142] 根据目标航向角误差Δψ＝ψ‑ψt＝90°可知，机器人绕机体坐标系z轴顺时针旋转90°。

[0143] 由图12(a)、(b)、(c)和(d)可以看出机器人沿急变期望航向角转向时，机体沿z轴方向波动较小，转弯半径小于1m，机体质心位移为0.74m，符合预期急变航向转向要求；由图
13(a)和(b)可以看出机器人在干扰力矩的作用下，有航向角反馈的机体航向角能稳定地跟
随期望航向角，转向角速度基本保持在19°/s，航向角误差基本保持在±5°。而没有航向角
反馈的机体航向角误差逐渐增大，转向角速度逐渐增大，航向角误差呈发散趋势，最终提前
到达目标航向角。该实例符合控制要求。

[0144] 以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，在不脱离本发明技术方案的范围内，当可利用上述揭示的技术内容做出些许更动或修饰为等
同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以
上实施例所作任何的简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。