基于支撑线运动分解的四足机器人trot步态本体位姿控制方法
【技术领域】
[0001] 本发明主要涉及到机器人运动控制技术领域，特指一种基于支撑线运动分解的四 足机器人trot步态本体位姿控制方法。
【背景技术】
[0002] 四足机器人是一种模仿自然界中四足哺乳动物的骨骼结构和行走步态的机器人， 充分结合了仿生学和机器人学的先进技术，其最大的优点是对山地、陡坡、沙漠等非结构化 的地形环境有极强的适应能力，可以用于在危险环境下执行巡逻，物资运输等一些复杂任 务，拥有广阔的应用前景。
[0003] 四足机器人一般由一个本体和四条仿生腿构成，每条仿生腿由2~3个前向关节 和一个侧向关节组成，以保证足端点的运动有三个自由度。目前比较成熟的四足机器人的 关节驱动器分为两种：液压驱动器和电机驱动器。通过对四足哺乳动物的运动方式的学习、 分析、模仿，四足机器人的移动主要采用以下三种典型步态=WALK步态（爬行步态）、TROT 步态（对角小跑步态）和BOUNDING步态（奔跑步态）。
[0004] 在四足机器人以TROT步态行进过程中，对角两条腿的运动相同，机器人依靠两组 对角腿按照一定的规律在支撑腿模式和摆动腿模式之间周期性切换，完成对本体速度和姿 态的控制。目前，四足机器人运动控制普遍采用以下方法：
[0005] 1、CPG控制方法；即中枢模式发生器，借助神经元振荡器模型产生周期性信号，控 制腿部运动，这种方法的优点是实现简单，缺点是参数多，而且缺乏明确的物理含义，难以 准确调节。
[0006] 2、机器学习；借助机器学习的理论和方法，使机器人能够学习四足动物稳定的行 走步态，但是需要大量的训练，很难在短时间适应新的非结构化环境。
[0007] 3、在线规划；根据机器人当前的传感器信息，计算下一个或者几个步行周期足端 点期望运动轨迹，通过调整关节驱动力矩使得足端点跟踪期望运动轨迹。但是由于非结构 化环境的未知性，可能出现摆动腿与环境提前接触的情况，产生非预期冲击力，影响本体的 速度和姿态。
[0008] 4、足端接触力控制；通过力传感器反馈足端与环境的接触力信息，设计足端接触 力控制算法，进而控制机器人本体的速度和姿态。这种控制方法的缺点在于在两条支撑腿 之间可能产生内力，在支撑腿切换到摆动腿模式的时刻，内力瞬间释放，对摆动腿和本体的 姿态产生非预期冲击。因此，需要进一步设计控制策略，既能够控制机器人本体的速度和姿 态，又能够有效消除支撑腿之间的内力。
【发明内容】
[0009] 本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一 种通用性强、控制效果好、控制精度高的基于支撑线运动分解的四足机器人trot步态本体 位姿控制方法。
[0010] 为解决上述技术问题，本发明采用以下技术方案：
[0011] -种基于支撑线运动分解的四足机器人trot步态本体位姿控制方法，其步骤为：
[0012] Sl :将机器人分别投影到径向平面和法向平面上；所述径向平面为通过支撑腿足 端点连线并且与水平面垂直的平面；所述法向平面为通过本体质心并且垂直于径向平面的 平面；
[0013] S2 :将机器人本体的期望速度使用与步骤Sl相同的方法投影到径向平面和法向 平面上，得到两个投影平面上的本体期望速度；
[0014] S3:分别建立简化模型的运动学和动力学方程，通过采用控制器使得两个投影平 面上的本体速度跟踪各自的期望轨迹，再进行速度合成，完成对机器人本体的速度和姿态 的控制。
[0015] 作为本发明的进一步改进：所述步骤Sl和S2中，在所述径向平面上的投影简化为 平面七连杆模型；通过调整关节驱动力矩使得平面七连杆的质心位置、姿态跟踪期望轨迹。
[0016] 作为本发明的进一步改进：所述步骤Sl和S2中，在所述法向平面上的投影简化为 一维线形倒立摆模型。
[0017] 作为本发明的进一步改进：所述步骤S3中，通过计算、选择法向平面内摆动腿落 足点的位置，调整每个支撑期开始时刻机器人本体的速度、姿态，控制本体在支撑期结束时 刻法向平面内的速度，与径向平面内机器人本体的速度进行合成，完成对机器人速度的控 制。
[0018] 作为本发明的进一步改进：采用基于足端接触力控制的双环PID控制器，外环利 用本体质心和姿态误差计算期望的足端接触力，且加入支撑腿足端之间内力为零的约束条 件，计算出唯一的期望接触力；内环通过测量实际足端接触力和期望接触力之间的误差计 算关节驱动力矩。
[0019] 与现有技术相比，本发明的优点在于：
[0020] 1、本发明的基于支撑线运动分解的四足机器人trot步态本体位姿控制方法，在 四足机器人以TROT步态行进过程中，采用基于速度分解和合成的控制策略可以实现本体 的速度姿态准确跟踪期望轨迹。
[0021] 2、本发明的基于支撑线运动分解的四足机器人trot步态本体位姿控制方法，所 提出的控制策略能够有效减小四足机器人在行走过程中支撑腿足端之间的内力。
[0022] 3、本发明的基于支撑线运动分解的四足机器人trot步态本体位姿控制方法，能 够提高四足机器人对非结构化环境的适应能力，实现机器人在不平整地面上的行走。
[0023] 4、本发明的基于支撑线运动分解的四足机器人trot步态本体位姿控制方法，具 有较强的通用性。控制策略不依赖具体的四足机器人系统，只要构建合适的传感器系统，结 合机器人运动学、动力学模型相关参数，适当调整控制器参数，即可实现本发明所提出的方 法。
[0024] 5、本发明的基于支撑线运动分解的四足机器人trot步态本体位姿控制方法，结 构清晰、层次分明，具有较好的理论和应用价值。
【附图说明】
[0025] 图1是四足机器人的系统结构示意图。
[0026] 图2是本发明在具体应用时四足机器人的径向平面投影示意图。
[0027] 图3是本发明在具体应用时四足机器人的法向平面投影示意图。
[0028] 图4是本发明在具体应用实例中的控制流程示意图。
[0029] 图5是本发明在具体应用实例中的具体计算过程示意图。
[0030] 图6是本发明方法的流程示意图。
【具体实施方式】
[0031] 以下将结合说明书附图和具体实施例对本发明做进一步详细说明。
[0032] 本发明的方法主要适用于四足机器人，四足机器人的系统结构如图1所示，它包 括一个本体和四条仿生腿，即包括左前腿、右前腿、左后腿、右后腿。四条仿生腿的机械结 构、尺寸相同，左前腿与左后腿，右前腿与右后腿的安装方式相对于本体互为镜像。每条仿 生腿包含四个主动关节（踝关节、膝关节、髋关节、髋部侧向关节），各主动关节均采用液压 作动器驱动，足端与地面为点接触。每个主动关节装有位移传感器和力传感器，用于检测作 动器长度信息和关节力信息，在足端点通过三维力传感器反馈足端与地面的接触力信息， 机器人本体在惯性空间中的位置、姿态可通过安装在上体的IMU姿态传感器实时检测。
[0033] 在本发明中，行走步态默认为TROT步态。如图6所示，本发明的基于支撑线运动 分解的四足机器人trot步态本体位姿控制方法，其步骤为：
[0034] Sl :将机器人分别投影到径向平面和法向平面上；
[0035] 所述径向平面为通过支撑腿足端点连线并且与水平面垂直的平面；所述法向平面 为通过本体质心并且垂直于径向平面的平面；
[0036] 在所述径向平面上的投影简化为平面七连杆模型（如图2所示），在所述法向平面 上的投影简化为一维线形倒立摆模型（如图3所示）；
[0037] S2 :将机器人本体的期望速度使用与步骤Sl相同的方法投影到径向平面和法向 平面上，得到两个投影平面上的本体期望速度；
[0038] 基于本体的期望前向速度Vd(本体的侧向期望速度为0)，计算两个投影平面内本 体的期望速度。
[0039] Vsd= V dcos α
[0040] Vfd=Vdsina
[0041] 其中Vd为本体的前向期望速度，Vsd为本体在径向平面内的期望速度，V fdS本体在 法向平面内的期望速度，a为机器人前向速度与径向平面之间的夹角。
[0042] S3:分别建立简化模型的运动学和动力学方程，通过采用控制器使得两个投影平 面上的本体速度跟踪各自的期望轨迹，再进行速度合成，以完成对机器人本体的速度和姿 态的控制。
[0043] 以左前腿和右后腿处于支撑期，右前腿和左后腿处于摆动期为例：
[0044] TROT步态四足机器人的动力学模型为：
[0045] 径向平面：
[0046]
LiN 丄uoid/yoy a yJ^ u ·* 4/5 ^
[0047]
[0048] 其中，qi= [X，Y，β ] T表示本体在径向平面内的水平位置、高度和俯仰角，
[0049] q2= [X D Yc，Θ dl，XD，YD，Θ J T，[Xc，Yc]T ([XD，YD]τ)为右后腿（左前腿）足端相 对于髋关节安装点的位置，9 dl(0d2)为右后腿（左前腿）前向关节角线性约束，Fe = Fm2, FyJτ为支撑腿足端接触力向量，τ为支撑腿关节驱动力矩，V/，V/ %惯性矩 阵，为哥氏力矩阵，戈，％为重力项，JuJ5为雅克比矩阵。
[0050] 法向平面：
[0051]
[0052]
[0053]
[0054] 其中，I (t)为本体在法向平面内的水平位置，I (0)为支撑期初始时刻本体在法向 平面内的水平位置，/(〇)为支撑期初始时刻本体在法向平面内的水平速度。g为重力加速 度。
[0055] 控制器分为内环和外环两部分：
[0056] 外环控制器：
[0057]
[0058] 其中如为q i的期望轨迹，F ed为支撑腿足端期望接触力，Kp i，Kd1, Ki1为外环控制 参数。
[0059] 内环控制器：
[0060]
[0061] 其中，Kp2为内环控制参数。
[0062] h时刻线性倒立摆初始位置的计算公式为：
[0063]
[0064] 其中，T为单个支撑期时间。
[0065] 在上述过程中，将机器人投影到径向平面上，简化为一个平面七连杆后，通过调整 关节驱动力矩使得平面七连杆的质心位置（2维）、姿态（1维）跟踪期望轨迹。
[0066] 在上述步骤S3中，本发明采用基于足端接触力控制的双环PID控制器，外环利用 本体质心和姿态误差计算期望的足端接触力，由于控制量为4维，控制目标为3维，所以加 入支撑腿足端之间