CN113084799B - 一种基于六次多项式的六足机器人足端轨迹规划方法 - Google Patents

Abstract

本发明设计一种基于六次多项式的六足机器人足端轨迹规划方法，针对六足机器人爬行特性，设计提出一种包括足端垂直与地面抬起及下落阶段、抛物线摆动阶段、平稳支撑阶段的足端轨迹曲线；并且各阶段足端轨迹曲线在机体坐标系下，足端位姿、速度、加速度时刻连续，从而保证在关节空间坐标系下，各个关节电机角度、角速度、角加速度时刻连续，使得机器人爬行过程中步态始终保持稳定、连续。本方法仅需使用者给出爬行速度、爬行周期、足底抬起高度信息，就可得到精确平滑的足底轨迹曲线减少了机器人产生侧滑的可能性，增加了机体稳定性，并且在凹凸地面处理上，程序实现较为方便。

Description

一种基于六次多项式的六足机器人足端轨迹规划方法

技术领域

本发明属于机器人足端轨迹规划领域，特别涉及一种基于六次多项式的六足机器人足端轨迹规划方法。

背景技术

六足机器人在结构上具有较多冗余自由度，能够适应较为复杂地形，在路况复杂的野外行走，完成履带式或轮式所不能完成的非结构性环境中的运输作业任务。在海底环境下这一点尤为明显，海底洋流、视野浑浊、通讯受限、海底地形凹凸不平、淤泥沉积等因素，使得传统履带式或轮式机器人难以胜任水下作业任务。因此对六足机器人的研究具有广阔的应用前景。

在六足机器人进行工作方法的研究中，首先根据机器人机械结构进行运动学建模，得到机械腿的逆运动学解；之后对其进行运动轨迹进行规划，得到各关节随时间变化曲线；最后控制电机旋转完成工作任务。其中对机械腿的轨迹规划是至关重要的一步，它直接影响机器人爬行的方式，影响爬行过程中的机体的稳定性、关节电机的受力情况、腿臂动作的整体能耗，进一步限制机器人的工作性能及所能适应的环境。

然而在现有技术中，六足机器人爬行过程中的轨迹规划方法都是针对特定的腿臂机械结构，无法做到通法通解，并且编程复杂，在腿臂机械结构或作业环境发生改变时，很难快速准确的更改相应算法参数。例如，当六足机器人在爬行过程中遇到地面凹陷或者凸起时，按照现有技术的轨迹规划方法，很难快速准确的更改相应算法参数，使其自主适应腿臂机械结构或作业环境发生的改变。

综上所述，对六足机器人足端进行运动轨迹规划，使六足机器人既能满足爬行任务要求，能在爬行中遇到凹凸不平路面时保持机体平稳，有着重要的实践意义。

发明内容

本发明解决的技术问题是：为了解决现有技术中六足机器人运动轨迹规划方法无法在凹凸不平路面时保持机体平稳且可能会对腿部足端进行损坏的缺陷，本发明设计一种基于六次多项式的六足机器人足端轨迹规划方法。

本发明的技术方案是：一种基于六次多项式的六足机器人足端轨迹规划方法，包括以下步骤：

步骤1：在机体坐标系下，将六足机器人单腿足端的运动轨迹分为：足端垂直与地面抬起阶段、抛物线摆动阶段、足端垂直与地面下落阶段和平稳支撑阶段四个阶段，其中足端垂直与地面抬起阶段和足端垂直与地面下落阶段的运动轨迹控制方法相同，同时定义AB段为足端垂直与地面抬起阶段，BCD段为抛物线摆动阶段，C为抛物线最高点；DE阶段为足端垂直与地面下落阶段，EA阶段为平稳支撑阶段；

步骤2：采用六次多项式，建立几个阶段的轨迹曲线，包括以下步骤：

步骤2.1：建立足端位置轨迹曲线：

其中E(t)表示足端点E在t时刻的位置矩阵；

步骤2.2：根据上一步骤的轨迹曲线，得到足端速度轨迹曲线为：

足端加速度轨迹曲线为：

步骤2.3：根据步骤2.1和步骤2.2的多项式得到足端在某一时刻的某一方向的位置、速度及加速度的约束条件：

进一步得到A_i和B_i矩阵：

B_i＝[E_i(t_i)…V_i(t_i)…a_i(t_i)]^T

步骤2.4：确定A、B、C、D、E五个点具体参数，包括以下内容：

位置参数表达式：

速度参数表达式：

加速度参数表达式：

时间参数表达式：

其中，S为步幅，T为爬行周期，X₀为初始侧向位置，Z₀为触底垂向位置，H_max为最大抬起高度，H_tir为垂直抬起高度，T_tir为垂直抬起时间，V_Bz为B点z轴方向速度，V_Cy为C点y轴方向速度，a_Bz为B点z轴方向加速度，a_Cy为C点y轴方向加速度，a_Cz为C点z轴方向加速度，a_Dz为D点z轴方向加速度；

步骤2.5：将步骤2.4中的各个表达式代入A_i和B_i矩阵，得到各个阶段的变化曲线的参数矩阵，其中：

足端垂直与地面抬起阶段和足端垂直与地面下落阶段的z轴变化曲线的参数矩阵为：

B₁＝[Z₀ H_tir H_max Z₀ 0 0 0]^T

抛物线摆动阶段y轴变化曲线的参数矩阵：

抛物线摆动阶段z轴变化曲线：

抛物线摆动阶段z轴变化曲线的参数矩阵与足端垂直与地面抬起阶段和足端垂直与地面下落阶段的z轴变化曲线的参数矩阵相同。

平稳支撑阶段y轴变化曲线的参数矩阵：

步骤3：通过步骤2得到ABCDE五个点的变化曲线的参数矩阵，根据公式c_i'＝A_i'B_i计算c_i'，最终得到六足机器人足端的运动轨迹。

发明效果

本发明的技术效果在于：

(1)提供了一种较为通用的六足机器人足端轨迹规划算法，仅需使用者给出爬行速度、爬行周期、足底抬起高度信息，就可得到精确平滑的足底轨迹曲线。

(2)将运动过程分为四个阶段，利于分时段改变算法参数，提高算法灵活性，使其适应多种地形。

(3)由于垂直落地的存在，减少了机器人产生侧滑的可能性，增加了机体稳定性，并且在凹凸地面处理上，程序实现较为方便。

(4)以六次方程作为轨迹曲线，减少了足底坐标运算的复杂度，提高了工程实现的实时性，且易于根据腿臂机械结构和作业环境的改变而修改。

附图说明

图1为本发明提出的机体坐标系下单腿足端空间运动轨迹示意图

图2为单纯抛物线突起地面足端空间运动轨迹示意图

图3为单纯抛物线凹陷地面足端空间运动轨迹示意图

具体实施方式

在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“顺时针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

参见图1-图3，本发明目的是提供一种，满足爬行任务要求,包括爬行过程中机体平稳、爬行速度、平稳踏过凹凸不平路面等；又满足工程实现的通用六足机器人轨迹规划算法。

如图1所示为机体坐标系O-XYZ下，一个周期内单腿足端空间运动轨迹示意图。前进爬行一步动作过程分为足端垂直与地面抬起阶段、抛物线摆动阶段、足端垂直与地面下落阶段、平稳支撑阶段。各个过程设计不同的六次方程，由同一计时器按照同一周期进行曲线插补，解算出的笛卡尔系下坐标再由运动学逆解解算出具体各关节电机角度，进而完成爬行动作。具体过程介绍如下：

第一步：足端垂直与地面抬起、下落阶段

若摆动相采用单纯抛物线方式，当遇到地面突起或地面凹陷，如图2、图3所示，在大地坐标系O’-X’Y’Z’下就会出现Y轴方向的偏移dY和相对于地面的夹角。即在足底产生水平于Y轴的力，造成机体不稳且损伤足底力觉传感器；又对编程实现造成困难。

此阶段为实现平稳踏过凹凸不平路面，提供条件。在腿臂下落过程中，根据足端力觉传感器可实时判断是否提前落地(凸出地面，规划z轴方向停止运动，记录此时z轴方向坐标z_stop,y轴方向继续运动，进入支撑阶段，保证支撑过程继续平稳进行；之后进入垂直抬起阶段，z轴方向坐标继续保持在z_stop，直到由垂直抬起阶段的六次方程计算出的z轴方向坐标大于等于z_stop，继续执行六次方程规划的曲线)或未能落地(凹陷地面，规划y轴方向停止运动，记录此时计时器时刻t_hang,z轴方向继续向下探底，直到足端完全触地，并保持在此z轴方向坐标，进入支撑阶段，y轴方向继续运动，之后进入垂直抬起阶段；垂直抬起至规划的地面高度，继续执行六次方程规划的曲线)。

第二步：抛物线摆动阶段

此阶段为腿臂向机体前方(y轴正方向)摆动，足端在平行与ZOY的平面内做抛物线运动，该腿在机体坐标系下的z轴方向、y轴方向坐标同时发生变化。

在此阶段六次方程规划中，确定了足底的最大抬起高度、跨越的长度、运动的时间，从而确定了爬行的速度。

第三步：平稳支撑阶段

此阶段为腿臂向机体后方(y轴后方向)移动，将机体向前推动，足端在平行与y轴线段上做先加速后减速直线运动，该腿在机体坐标系下仅在z轴y轴方向坐标发生变化。

各阶段运动轨迹算法：

各阶段足端的运动轨迹，即在机体坐标系下，某一时刻足端经过某一规定的点，并且在处于这一规定点时，满足规定的速度和加速度约束条件。

本发明采用六次多项式作为轨迹曲线，数学表达为：

则足端速度的多项式表达为：

足端加速度的多项式表达为：

其中E(t)表示足端点E在t时刻的位置矩阵，E(t)＝[x_E(t) y_E(t) z_E(t)]^T；v(t)表示足端点E在t时刻的速度矩阵，v(t)＝[v_xE(t) v_yE(t) v_zE(t)]^T；a(t)表示足端点E在t时刻的位置矩阵，a(t)＝[a_xE(t) a_yE(t) a_zE(t)]^T；c_i表示时间t的多项式系数矩阵，c_i＝[c_ix c_iyc_iz]^T。

各方向分量展开式为：

由此得到某一时刻某一方向的三个对应的约束条件。对于六次多项而言，仅需确定七个非线性相关的方程，即可确定唯一的一组：

c_i'＝[c₀' c₁' c₂' c₃' c₄' c₅' c₆']^T (5)

整理为矩阵得到：

A_ic_i'＝B_i (6)

c_i'＝A_i'B_i (7)

其中：

B_i＝[E_i(t_i)…V_i(t_i)…a_i(t_i)]^T (8)

根据预先设计的轨迹规划示意，首先，确定轨迹规划的各项参数，步幅(S)、爬行周期(T)、初始侧向位置(X0)、触地垂向位置(Z0)、最大抬起高度(H_max)、垂直抬起高度(H_tir)、垂直抬起时间(T_tir)。其次，确定在各阶段各关键位置的坐标，以及相对应的时刻。在本发明中，需确定起始点A(t0)、垂直抬起终点B(t1)、抬起顶点C(t2)、垂直下落起点D(t3)、垂直下落终点E(t4)，五个点的时间、位置、速度及加速度。

其中：

将确定的参数带入六次多项式得到各阶段具体A_i、B_i矩阵分别如下：

垂直抬起、抛物线摆动、垂直落下z轴变化曲线：

B₁＝[Z₀ H_tir H_max Z₀ 0 0 0]^T (14)

抛物线摆动y轴变化曲线：

平稳支撑y轴变化曲线：

Claims (1)

1.一种基于六次多项式的六足机器人足端轨迹规划方法，其特征在于，包括以下步骤：

步骤1：在机体坐标系下，将六足机器人单腿足端的运动轨迹分为：足端垂直与地面抬起阶段、抛物线摆动阶段、足端垂直与地面下落阶段和平稳支撑阶段四个阶段，其中足端垂直与地面抬起阶段和足端垂直与地面下落阶段的运动轨迹控制方法相同，同时定义AB段为足端垂直与地面抬起阶段，BCD段为抛物线摆动阶段，C为抛物线最高点；DE阶段为足端垂直与地面下落阶段，EA阶段为平稳支撑阶段；

步骤2：采用六次多项式，建立几个阶段的轨迹曲线，包括以下步骤：

步骤2.1：建立足端位置轨迹曲线：

其中E(t)表示足端点E在t时刻的位置矩阵；

步骤2.2：根据上一步骤的轨迹曲线，得到足端速度轨迹曲线为：

足端加速度轨迹曲线为：

步骤2.3：根据步骤2.1和步骤2.2的多项式得到足端在某一时刻的某一方向的位置、速度及加速度的约束条件：

进一步得到A_i和B_i矩阵：

B_i＝[E_i(t_i) … V_i(t_i) … a_i(t_i)]^T

步骤2.4：确定A、B、C、D、E五个点具体参数，包括以下内容：

位置参数表达式：

速度参数表达式：

加速度参数表达式：

时间参数表达式：

其中，S为步幅，T为爬行周期，X₀为初始侧向位置，Z₀为触底垂向位置，H_max为最大抬起高度，H_tir为垂直抬起高度，T_tir为垂直抬起时间，V_Bz为B点z轴方向速度，V_Cy为C点y轴方向速度，a_Bz为B点z轴方向加速度，a_Cy为C点y轴方向加速度，a_Cz为C点z轴方向加速度，a_Dz为D点z轴方向加速度；

步骤2.5：将步骤2.4中的各个表达式代入A_i和B_i矩阵，得到各个阶段的变化曲线的参数矩阵，其中：

足端垂直与地面抬起阶段和足端垂直与地面下落阶段的z轴变化曲线的参数矩阵为：

B₁＝[Z₀ H_tir H_max Z₀ 0 0 0]^T

抛物线摆动阶段y轴变化曲线的参数矩阵：

抛物线摆动阶段z轴变化曲线：

抛物线摆动阶段z轴变化曲线的参数矩阵与足端垂直与地面抬起阶段和足端垂直与地面下落阶段的z轴变化曲线的参数矩阵相同。

平稳支撑阶段y轴变化曲线的参数矩阵：

步骤3：通过步骤2得到ABCDE五个点的变化曲线的参数矩阵，根据公式c_i'＝A_i'B_i计算c_i'，最终得到六足机器人足端的运动轨迹。

Images