基于三次贝塞尔曲线的轨迹规划方法

2022-12-05厦门大学航空航天学院江浩彭侠夫

数字技术与应用 2022年11期

厦门大学航空航天学院江浩彭侠夫

四足机器人在复杂环境下既能够实现静态行走，又能够以动态方式高速运动，具有较高的研究价值。而足端轨迹研究则是四足机器人运动的基础。对四足机器人建立了简化模型，并对单腿的运动学和动力学进行建模。然后在运动学上根据弹簧负载倒立摆模型选取落脚点，轨迹上选取三次贝塞尔曲线进行改进由弹簧负载倒立摆模型和三次贝塞尔曲线生成足端轨迹，弹簧负载倒立摆模型获取足端落地平衡点保证机器人支撑相与摆动相切换时的平稳。三次贝塞尔曲线结合地形生成规避障碍物的足端轨迹，对曲线生成方式进行改进使其符合零冲击步态的规划，在动力学上应用拉格朗日方程获得了驱动力矩与足端轨迹的映射关系，最后通过传感器计算实际足端轨迹与规划足端轨迹的误差建立虚拟动力学方程实现反馈控制，提高了四足机器人抗干扰能力，降低了单腿高速摆动及落地冲击对机身运动状态的影响。

腿足仿生机器人根据腿的数量又分为双足、四足、六足等多足机器人。参照哺乳类四足动物设计的仿生四足机器人相较于双足仿生机器人更稳定、灵活、负载高，相较于六足等多足仿生机器人结构复杂度低、易于控制，在复杂环境下既能够实现静态行走，又同时能够以动态方式高速运动。综合运动特性及结构复杂度，四足机器人的适应性更广，可研究价值更高[1，2]。

1 四足机器人模型

1.1 四足机器人简化模型

根据四足机器人简化模型确定世界坐标系和机身坐标系。机身质心位置为机身坐标系原点，设计足端轨迹时，足端轨迹坐标以初始站立姿态足端相对机身坐标系的固定位置为足端轨迹坐标原点，整机的运动方向为正方向。单腿可视作具有三自由度的三连杆，四足机器人的简化模型及坐标系定义如图1所示。

1.2 D-H 法运动学方程

对连杆系统进行建模的一种通用方法为D-H 法，在各个连杆上设置坐标系，根据连杆的连接结构与空间关系建立变换矩阵，从连杆一端根据连杆间的变换矩阵逐步计算下一连杆一端的空间矩阵，最终计算出连杆两端间的空间变换矩阵，即四足机器人的运动学方程。

坐标系的变换矩阵T 由坐标系旋转变换矩阵R 和坐标系位移矩阵P 组成，旋转矩阵R 是指两坐标系原点重合对应的坐标系、三轴的方向余弦，位移矩阵P 是变换后的坐标系相对于参考坐标系的三个方向位移。其变换矩阵构成如公式（1）所示。

连杆间的坐标系变换由连杆偏距di、连杆长度li、连杆扭转角φi、连杆间旋转角θi四个运动学模型参数来描述，连杆长度和连杆扭转角为连杆自身的物理参数，连杆偏距和连杆间夹角为连杆连接物理参数。在四足机器人单腿模型中，腿部连杆均不存在扭转，由此可以确定单腿从机身和大腿连接处到足端的坐标变换矩阵。各物理参数如图1所示，机身与单腿连接处即臀关节为初始坐标系，臀、大腿、小腿长度分别为l1、l2、l3，臀部与机身关节、臀部与大腿关节、大腿与小腿关节角度分别为θ1、θ2、θ3，D-H 法的参数表如表1所示。

图1 四足机器人简化模型Fig.1 Simplified model of quadruped robot

表1 四足机器人机身到足端的D-H 参数Tab.1 D-H parameters from the quadruped robot body to the foot end

根据D-H 参数法建立关节角度、角速度、角加速度与足端位移、速度、加速度间的映射关系，为动力学模型计算提供基础。

2 足端轨迹规划

2.1 虚拟弹簧负载倒立摆

倒立摆模型最早用于双足机器人步态控制，该理论提出以倒立摆接触点为核心，根据落点与平衡位置的关系来控制弹簧倒立摆的加速、匀速、减速运动。若落足点位于平衡点后方则为减速状态；若落足点位于平衡点前方则为加速状态；若落足点位于平衡点则为匀速运动状态[3]。在四足机器人运动过程中，分为支撑相与摆动相，可将摆动相的腿虚拟为弹簧负载倒立摆，根据机身状态及指令计算足端轨迹摆动相落点控制四足机器人机身跟踪状态指令运动。弹簧负载倒立摆如图2所示。

图2 弹簧负载倒立摆模型Fig.2 Spring-loaded inverted pendulum model

四足机器人摆动腿虚拟为弹簧负载倒立摆，其平衡点取决于机身速度v、摆动周期Tsw、摆动腿的最大抬腿高度hsw、指令机身速度vref，此处计算速度机身x、y方向平衡点位置如公式（2）所示：

2.2 三次贝塞尔曲线

根据腿部传感器可以计算出四足机器人支撑相切换为摆动相时腿部足端的位置，结合弹簧负载倒立摆模型可计算得四足机器人落地平衡点。四足机器人为适用于复杂地形设计的机器人，在其结合三维地图传感器或激光雷达传感器生成三维地图时，四足机器人摆动相轨迹及落点需要灵活根据地图调整，保持机身平稳性。

三次贝塞尔曲线是应用于二维图形的数学曲线，通常是用于计算机矢量作图，n次贝塞尔曲线是由n+1 个点组合而成，曲线可以通过改变节点位置生成。在曲线生成过程中也可以通过拖动节点实时更改未完成的曲线轨迹，应用于四足机器人摆动相轨迹的生成，既能够结合弹簧负载倒立摆模型生成轨迹，还能够在运行过程中根据三维地图实时调整轨迹。三次贝塞尔曲线的基本方程如公式（3）所示：

其中P0、P1、P2、P4为轨迹生成的四个基础坐标，P0为支撑相切换为摆动相时足端所处位置，P4为根据弹簧负载倒立摆生成的落足点位置，通过调整P1、P2的位置可以控制足端摆动的高度等参数，以此来实现规避障碍物等功能。在原方程中t为每一段中点位置位于当前线段中的比例，若直接将其作为摆动周期时间的等比例缩放，在求导后获取摆动相轨迹的速度时，其速度初始不为零，此处将原式中的t替换为与时间有关的比例公式，则方程如公式（4）所示：

此处时间t为单腿从支撑相切换到摆动相开始到摆动相结束。四足机器人关节控制方式为力控制，因此加速度可突变，不考虑初始加速度为零的情况，仅考虑求导后两端点值均为零，且该函数为从0 到1 为递增函数，本文采用的比例函数如公式（5）所示：

将替换后的三次贝塞尔曲线对时间求导可以得到足端轨迹速度、加速度。如公式（6）、公式（7）所示：

由此获得初始速度和终点速度均为零的三次贝塞尔曲线方程及速度、加速度方程。该方程用于规划摆动相的足端轨迹，获取摆动腿末端的运动状态，结合运动学方程的逆解可得到关节转角所需的角度、角速度、角加速度作为关节控制的目标量，然后通过单腿动力学计算电机的按照规划的三次贝塞尔曲线运动所需的力矩。

3 实验

3.1 ADAMS 与Simulink 联合仿真

四足机器人为强非线性、强时变系统，该系统用力控制方法在地面行走时具有不确定性，四足机器人在行走中参数不断变化，与地面交互接触面小，系统必须能够准确及时的对四足机器人的四条腿、机身的所有状态共49 个状态量进行反馈并实现闭环控制。在进行实物样机运行前，必须由计算机仿真调整控制参数使得近似的建模模型具有理想的控制效果。建立整机模型的程序框图如图3所示。

图3 单腿摆动相控制框图Fig.3 Block diagram of single-leg swing phase control

目前常用的机器人力学控制方法是由Simulink 控制的数学模型和ADAMS 导入的四足机器人建模模型生成的动力学模型联合仿真验证效果。整体思路为：Simulink 中建立控制器中的运动学、动力学、模型预测控制等数学模型，搭建能够实现对四足机器人状态数据即规划轨迹完成力控输出的整体控制框架。SoildWorks 建立物理样机的3D 模型，将软件生成的3D 模型导入ADAMS，建立连接关系和驱动器，设置输入和输出量，构建控制系统。由Simulink 的模型于ADAMS 的物理样机动力学模型数据交互实现仿真控制。

轨迹生成器主要功能为接受上层算法或遥控的控制指令，其指令包括运动方向、旋转角、速度、预设步态类别。该运动方向指四足机器人不改变机体朝向，仅改变整机的运动方向，主要用于实现侧向行走。旋转角指机身朝向，但不影响四足机器人的运动方向，可以实现机身直线运动过程中的机身旋转，完成四足机器人作为移动平台需要完成的其他任务。速度仅指前进方向，通过运动方向分解为世界坐标系下x与y轴方向的运动速度。步摆动相控制器主要功能是接收到平衡控制器发送的腿部摆动指令、摆动腿的位置关系、四足机器人腿部编码器反馈的腿部状态数据。在摆动相控制器接受四足机器人腿部状态数据，该数据包括关节角度、角速度、角加速度，同时四足机器人轨迹中的速度，包括目标速度与实际速度，机身轨迹速度用于落足点计算。由于该方法用于优化负载控制，未考虑环境变化和地形识别方法，因此轨迹控制参数固定，并内置到摆动相控制器中，根据抬腿高度自动生成轨迹。然后将生成的轨迹包括足端位置、速度、加速度和关节传感器反馈的角度、角速度、角加速度带入动力学方程得出目标轨迹的力矩。再将传感器反馈的腿部关节角度、角速度、角加速度由运动学正解计算出四足机器人实际的足端状态即足端实际的位置、速度、加速度，根据列出的控制参数调整的动力学微分方程，计算反馈的足底力，再将反馈计算的足底力通过传感器反馈的关节角度映射为关节力矩作为反馈调节量。摆动相控制器输出的为目标轨迹的控制力矩与足端动力学微分方程计算的反馈力矩的求和。