CN111290272B - 基于多足机器人的姿态平稳性调整方法 - Google Patents

Abstract

本发明公开一种基于多足机器人的姿态平稳性调整方法，首先通过运动学分析建立多足机器人支撑腿关节角与机体姿态角的映射关系；基于所设计的映射模型，将姿态的平稳调整通过支撑腿的稳定调整来实现；其次，设定满足机器人姿态平稳调整要求的约束指标，基于S曲线加减速法规划机体姿态角变换曲线，再经过映射关系得到关节的控制轨迹要求，从而实现平稳调整；此外，还针对姿态变换时多足机器人支撑腿个数不同所产生的冗余问题给出了具体的选择和运动控制方法，达到多足机器人的姿态调整时机体平稳无晃动的目的，为多足机器人在崎岖地形下进行姿态调整时提供了一种平稳的规划方法，解决了传统规划方法机器人在变换机体姿态时产生的机身不稳定问题，为多足机器人提高地形适应能力提供了重要参考。

Description

基于多足机器人的姿态平稳性调整方法

技术领域

本发明涉及机器人姿态调整领域,具体涉及一种基于多足机器人的姿态平稳性调整方法。

背景技术

多足式机器人拥有较好的跨越障碍的能力和对复杂地形的适应能力，得益于机器人根据地形环境产生良好的适应性姿态，姿态之间的不断调整能够保证机器人完成移动任务时的高效性和稳定性，可以作为多自由度操作平台在崎岖复杂地形情况下完成移动任务。然而机器人在进行姿态调整时，由于机体加速度的不连续性易使机身产生晃动，尤其机器人在背负大负载工作时甚至产生机体倾翻。因此有必要对机器人姿态调整过程中平稳性进行研究，解决姿态调整时的机身晃动问题。

为提高六足机器人在崎岖地形的适应能力，许多学者从机器人的机械结构、力/位控制、改进算法和控制策略等角度对位姿调整方面展开大量研究。比如，在机械结构改进方面，Roennau等人提出一种反应姿态控制方法，通过增加一个额外的旋转关节实现机器人在严重崎岖地形下的稳定性；Marko Bjelonic等研究了一种新型的六足机器人，其每条腿有5个关节，其中两个冗余关节通过控制身体姿态和腿相对于地面的方向来提高机器人的地形适应力；从改进算法的角度，Jung-Yup Kim提出一种六足机器人压力中心控制算法，将机器人建立为带弹簧和阻尼器的倒立摆模型，利用传感反馈控制器来控制平衡位姿，实现了机器人以自由步态下的稳定行走；Dominik Belter以Messor机器人为研究对象，基于粒子群优化算法提出了姿态优化算法，并结合机体运动规划，实现了越障时机体姿态的控制；另外，在控制策略调整方面，Jan Faigl以价格较低无传感器的机器人为载体提出一种自适应控制方法，实现无任何额外传感器的情况下，减小机器人结构与伺服电机的应力和扭矩；Wen-June Wang提出一种六足机器人斜面行走的控制策略，机器人在斜面行走时通过惯性测量单元获得旋转矩阵，利用模糊控制器来调整支撑腿的电机角度实现姿态的调节等等。

然而上述这些对位姿的研究都没有考虑机器人在姿态调整时机身惯性力的问题，这样会使机器人运行时机身产生晃动，因此亟待提出一种技术方案，对机器人姿态平稳性调整进行规划，避免机身晃动问题，进一步提高机器人的地形适应能力。

发明内容

在进行姿态调整时，由于机体加速度的不连续性易使机体产生晃动，本发明为解决机器人在足端点位置固定、姿态调整时机身晃动的问题，提出一种基于多足机器人的姿态平稳性调整方法，达到姿态调整时机体平稳无晃动的目的。

本发明是采用以下的技术方案实现的：一种基于多足机器人的姿态平稳性调整方法，包括以下步骤：

步骤A、基于运动学分析构建多足机器人支撑腿关节角与机体姿态的映射关系，基于所设计的映射模型，将姿态的平稳性调整通过支撑腿的稳定调整来实现；

步骤B、多足机器人的姿态平稳性调整，包括：

步骤B1、设定满足多足机器人姿态平稳调节要求的约束指标；

步骤B2、基于S曲线加减速法规划机体姿态角变换曲线；

步骤B3、根据步骤A所得到的映射关系得到支撑腿关节的控制轨迹要求，从而实现多足机器人的姿态平稳性调整。

进一步的，由于多足机器人在姿态调整过程中可能会出现某条支撑腿关节角度限位的情况，因此在姿态调整前需先对所有支撑腿进行分析，故对多足机器人姿态平稳性调整前还包括步骤B’：多足机器人支撑腿判断，具体包括：

步骤B’1、支撑腿判别，确定多足机器人中的支撑腿和随动腿；

步骤B’2、对确定的支撑腿和随动腿进行规划：

(1)对确定的支撑腿进行规划，得到姿态角变换曲线所对应的支撑腿关节角变化曲线；

(2)对确定的随动腿进行规划，得到其足端规划曲线。

进一步的，所述步骤A具体包括以下步骤：

步骤A1、建立多足机器人的运动学模型；

(1)建立参考坐标系ΣW、机体坐标系ΣB和跟关节坐标系ΣG_i，并得到第i条腿足端位置

在跟关节坐标系ΣG_i中的描述；

(2)根据单腿逆运动学方程，通过多个足端位置得到关节角度矩阵θ_i＝[θ_i1,θ_i2,θ_i3]^T，i为多足机器人腿的数量；

其中，

θ_i＝[θ_i1,θ_i2,θ_i3]^T表示第i条腿从内而外，分别为跟关节、髋关节和膝关节的转角，l₁-l₃分别对应的表示基节长度、股节长度和胫节长度；

(3)足端运动时，机器人第i条腿足端速度与关节角速度之间的变换关系为：

其中，J_i为雅可比矩阵，通过建立正逆运动学模型，得到了腿部关节角度和足端位置的对应关系；

步骤A2、建立姿态角-关节角映射模型：

(1)定义多足机器人姿态矩阵E＝[α,β,γ]^T，α，β，γ分别表示机器人姿态的偏转角、俯仰角和滚动角；

足端点在跟关节坐标系下与在机体坐标系下的坐标存在关系：

式中，

为跟关节在ΣB中的位置，由机体结构决定；

足端点在机体坐标系下与在参考坐标系下的坐标存在关系：

^WP_i＝^WR_B ^BP_i (8)

式中，^BP_i为支撑腿足端在机体坐标系中的位置；^WP_i为支撑腿足端在参考坐标系中的位置；^WR_B为机体坐标系相对于参考坐标系的旋转矩阵；

(2)由式(6)和(8)，跟关节坐标系中的足端位置与姿态角的关系可表示为

当机器人支撑足端位置固定时，由式(9)建立各支撑腿在跟关节坐标系中的位置与姿态角的映射关系。

进一步的，所述步骤B1中，为使姿态调整时达到机体平稳无晃动的目标，多足机器人满足以下约束条件：

式中，E_f为初始姿态角度；E_a为目标姿态角度；E(t)为姿态角曲线；

为姿态角速度曲线；

为姿态角加速度曲线；θ_ij为第i条腿第j个关节的角度；θ_ijmin为该关节的角度最小值；θ_ijmax为该关节的角度最大值；v_ij为该关节的角速度；V_e为该关节的角速度最大值；T为调整周期。

进一步的，所述步骤B2中，基于S曲线加减速法规划机体姿态角变换曲线具体采用以下方式：

(1)为了清晰的描述E(t)在其值域内的运动，简化初始和目标角度姿态角改变时的计算量，令

姿态角度变化曲线描述为：

由于E(t)中的每一个元素值均不大于E_a中对应的元素值，所以有0≤s(τ)≤1，s(τ)为角度变化的比例系数和自变量，描述了角度的变化过程，通过设计s(τ)函数实现对姿态角的规划；

(2)基于上述多个平稳指标的约束条件，采用五段“S”曲线加减速的规划方法进行插补，该曲线分为加加速、加减速、匀速、减加速、减减速五段：由于除匀速段以外，其余四个阶段对称，设四个变速段斜率大小都为A，四段的时间都为T_a，加加速段位移L₁、加减速段位移L₂。

则：

则s(τ)的加速度分段函数为：

对加速度积分可得：

对速度积分可得到s(τ)的分段函数：

根据姿态平稳调节的目标，设计了加速度起止为零且连续的加减速规划方法，根据该方法实现平稳姿态调节。

进一步的，所述步骤B’1中，对支撑腿判别时具体采用以下方式；

1)将多足机器人所有的腿的标志位设置为“1”，由目标姿态E_a通过式(2)、(9)得到每条支撑腿的三个关节角，将关节角度对比关节角度约束范围，如果某支撑腿有任一关节角超出约束范围，则该腿标志位立即设置为“0”，作为随动腿进行后续规划；

2)对满足关节约束的标志位“1”的腿所构成支撑面的投影多边形进行稳定性约束判定，如果S_m＞0则证明该姿态角满足稳定性约束条件，则当前标志位为“1”的腿参与姿态角调节过程；否则机器人不稳定需重新设置E_a，直到同时满足关节角约束和稳定性约束指标。

进一步的，所述步骤B’2中对支撑腿进行规划时，具体采用以下方式：

对于经过步骤B’1所述判别方法后，标志位仍为“1”的腿，作为参与姿态调节的支撑腿，在支撑腿足端位置固定的前提下，根据规划的E(t)曲线，得到每时刻t的姿态角所对应的关节角θ_ij，由此可以得到姿态角规划曲线所对应的支撑腿关节角变化曲线θ_ij(t)。

进一步的，所述步骤B’2中，对随动腿进行规划时具体采用以下方式：

随动腿足端点从初始^WP_f点到目标落足点^WP_a,随动腿的规划周期T与姿态调整周期保持同步，设^WP_a选取为调整结束时刻随动腿足端点与地形有效接触面的中央位置；

假设末时刻随动腿足端可达范围与地面的所构成接触面的范围坐标分别为：x_min、x_max、y_min、y_max、z_min、z_max，则期望足端点坐标^GP_a(t)＝[x_a,y_a,z_a]为：

对于足端^WP_i(t)的轨迹需满足式(23)的约束条件

式中，^WP_f为随动腿初始足端点；^WP_a为随动腿目标落足点；^WP_i(t)、

和

为足端点在参考坐标系下的位置、速度和加速度曲线；

根据式(23)的约束条件，利用高阶多项式法拟合足端点曲线可以描述为：

^WP_i(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵ (24)

所以随动腿^WP_i(t)＝[^Wx(t),^Wy(t),^Wz(t)]^T在参考坐标系下的运动轨迹可以描述为:

式中，^Wx(t)、^Wy(t)和^Wz(t)为足端点在参考坐标系下沿三个坐标轴方向的变化曲线，当随动腿足端点的初始和目标位置已知，规划周期T已知时，根据式(25)得到足端规划曲线。

与现有技术相比，本发明的优点和积极效果在于：

(1)针对机器人姿态调整时的机身晃动问题，设计了一种姿态角加速度从零变化且连续变化的规划方法；

(2)对机器人姿态调整的所有的支撑腿分为了两类(支撑腿和随动腿)，二者分别规划且同步完成；

多足机器人姿态调整时机身晃动主要是由于加速度的不连续导致惯性力突变产生的刚性冲击引起的，本发明方案中对加速度不连续问题，通过对姿态角曲线进行S曲线加减速规划，设计加速度起止为零且连续变化的姿态调整方法；然后根据机器人机械结构和地形的约束条件对姿态调整时支撑腿的数量做了判断并分为支撑腿和随动腿两类；由规划的姿态角曲线根据所建立的关节角-姿态角映射模型和运动学模型得到了支撑腿的关节角曲线，对不满足约束条件的随动腿采用高阶多项式法设计了足端轨迹，并且通过实验验证了该方法的有效性和正确性，解决了传统规划方法机器人在变换机体姿态时产生的机身不稳定问题，为多足机器人提高地形适应能力提供了重要参考。

附图说明

图1为本发明实施例所述六足机器人结构示意图；

图2为本发明实施例所述的多足机器人姿态平稳性调整原理示意图；

图3为本发明实施例机器人机体运动姿态描述简图；

图4为本发明实施例两种规范方法的姿态角速度变化图像，(a)为匀速规划法姿态角速度变化图像；(b)为平稳规划法姿态角速度变化图像；

图5为本发明实施例两种规划方法的姿态角加速度变化图像，(a)为匀速规划法姿态角加速度变化图像，(b)为平稳规划法姿态角加速度变化图像；

图6为本发明实施例支撑腿关节角实时变化图像，(a)为leg2关节角度变化图像，(b)为leg3关节角度变化图像，(c)为leg6关节角度变化图像；

图7为本发明实施例支撑腿关节角速度变化图像，(a)为leg2关节角速度变化图像，(b)为leg3关节角速度变化图像，(c)为leg6关节角速度变化图像；

图8为本发明实施例随动腿足端点变化图像，(a)为足端点位置变化图像，(b)为足端点速度变化图像，(c)为足端点加速度变化图像；

图9为本发明实施例匀速调整法角速度期望值与实际值变化曲线；

图10为本发明实施例平稳调整法角速度期望值与实际值变化曲线；

图11为本发明实施例匀速调整法过程图；

图12为本发明实施例平稳调整法过程图；

图13为本发明实施例随动腿规划对比图，(a)为未规划的随动腿轨迹截图；(b)为已规划的随动腿轨迹截图；

图14为本发明实施例所述的多足机器人姿态平稳性调整方法的流程示意图。

具体实施方式

为了能够更加清楚地理解本发明的上述目的、特征和优点，下面结合附图及实施例对本发明做进一步说明。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

本发明方案主要针对多足机器人姿态变换时在启停时刻出现的不稳定问题，提出了一种平稳的姿态调整方法：

首先通过运动学分析建立机器人支撑腿关节角与机体姿态的映射关系；其中腿部关节角度是可准确并能够实时测量的已知量，因此，支撑腿足端位置不变时，在某时刻随着机体姿态的变换可以得到唯一一组对应的关节角度，基于所设计的映射模型，可以将姿态的平稳调整通过支撑腿的稳定调整来实现。其次，设定满足机器人姿态平稳调节要求的约束指标，基于S曲线加减速法规划机体姿态角变换曲线，再经过映射关系得到关节的控制轨迹要求，从而实现平稳调整；此外，还针对姿态变换时多足机器人支撑腿个数不同所产生的冗余问题给出了具体的选择和运动控制方法，达到多足机器人的姿态调整时机体平稳无晃动的目的。

本实施例以六足机器人为例进行介绍，如图2所示，包括以下步骤：

步骤A、基于运动学分析构建多足机器人支撑腿关节角与机体姿态的映射关系；

步骤B、多足机器人的姿态平稳性调整：

步骤B1、设定满足多足机器人姿态平稳调节要求的约束指标；

步骤B2、基于S曲线加减速法规划机体姿态角变换曲线；

步骤B3、根据步骤A所得到的映射关系得到支撑腿关节的控制轨迹要求，从而实现多足机器人的姿态平稳性调整。

具体的：

步骤A、构建多足机器人模型，基于运动学分析构建多足机器人支撑腿关节角与机体姿态的映射关系；

六足机器人机体姿态的调整由支撑腿关节角的转动实现，本实施例通过建立机器人的运动学模型和姿态角-关节角映射模型，得到机体姿态角和支撑腿各关节角的关系，基于所设计的映射模型，将姿态的平稳性调整通过支撑腿的稳定调整来实现。

步骤A1、建立运动学模型

本实施例以仿昆虫型双三足结构六足机器人为研究对象，如图1所示，其中，机体为对称六边形，每条腿具有三个自由度，机器人自重集中在机体中心，并近似设定机器人重心和机体中心重合。根据图1所示的机器人简化结构建立运动学模型，结构参数如表1所示，坐标系设定如下：

参考坐标系ΣW：原点位于机器人质心，z_W与重力方向相反；y_W轴指向运动初始状态时的机器人机体正前方与水平方向平行；x_W轴按照右手定则确定；此坐标系用于简化描述机体的姿态。

机体坐标系ΣB：机器人质心为坐标原点，z_B轴垂直于机体向上；y_B指向机体正前方；x_B指向机器人机体正右方。

跟关节坐标系ΣG_i：原点为第i条腿部的跟关节，各坐标轴方向与机体坐标系各坐标轴一致。

表1机器人机构参数表

基于D-H法求解得到第i条腿足端位置

在跟关节坐标系ΣG_i中的描述：

其中，c_ij＝cosθ_ij，c_i23＝cos(θ_i2+θ_i3)，s_ij＝sinθ_ij，s_i23＝sin(θ_i2+θ_i3)，j＝1,2。

根据单腿逆运动学方程，由6个足端位置

求解得到关节角度矩阵θ_i＝[θ_i1,θ_i2,θ_i3]^T，(i＝1,2...6)，其中：θ_i＝[θ_i1,θ_i2,θ_i3]^T指的是第i条腿从内而外，分别为跟关节、髋关节和膝关节的转角；

其中，

足端运动时，机器人第i条腿足端速度与关节角速度之间的变换关系为：

J_i为雅可比矩阵：

式中，J₁₁＝-s_i1(l₁+l₂c_i2+l₃c_i23)，

J₁₂＝-c_i1(l₂s_i2+l₃s_i23)，J₁₃＝-l₃c_i1s_i23，

J₂₁＝c_i1(l₁+l₂c_i2+l₃c_i23)，

J₂₂＝-s_i1(l₂s_i2+l₃s_i23)，J₂₃＝-l₃s_i1s_i23，

J₃₁＝0，J₃₂＝l₂c_i2+l₃c_i23，J₃₃＝l₃c_i23。

通过建立正逆运动学模型，得到了腿部关节角度和足端位置的对应关系，通过式(3)的速度雅可比矩阵，为后面建立姿态角速度和关节角速度映射模型做了准备工作。

步骤A2、建立姿态角-关节角映射模型

定义六足机器人姿态矩阵E＝[α,β,γ]^T，α，β，γ分别表示机器人姿态的偏转角、俯仰角和滚动角,如图3所示。

相对于参考坐标系，机体坐标系各轴转动的角度关系采用旋转矩阵^WR_B描述：

上式中，cα＝cosα,sα＝sinα，以此类推，此种表示方式是常用简写方式；

足端点在跟关节坐标系下与在机体坐标系下的坐标存在关系：

式中，

为跟关节在ΣB中的位置，由机体结构决定：

足端点在机体坐标系下与在参考坐标系下的坐标存在关系：

^WP_i＝^WR_B ^BP_i (8)

式中，^BP_i为支撑腿足端在机体坐标系中的位置；^WP_i为支撑腿足端在参考坐标系中的位置；^WR_B为机体坐标系相对于参考坐标系的旋转矩阵；

由于^WR_B为正交矩阵，所以^WR_B ^-1＝^WR_B ^T。由式(6)和(8)，跟关节坐标系中的足端位置与姿态角的关系可表示为：

由此，当机器人支撑足端位置固定时，由式(9)建立各支撑腿在跟关节坐标系中的位置与姿态角的映射关系，根据该位置可进一步由逆运动学得到当前机体姿态角下的关节角度。式(2)为足端与关节的关系，式(9)包括了姿态和足端位置，则式(2)、(9)为足端点位置固时的机体姿态角和支撑腿关节角提供了映射模型。

步骤B、多足机器人姿态的平稳性调整

为使机器人能够进行平稳姿态调整，需确定平稳调整目标并规划姿态角曲线。由规划好的姿态角曲线根据姿态角-关节角映射模型和运动学模型可以得到固定足端位置下的支撑腿关节角变化曲线，具体的：

步骤B1、机体平稳调整目标

考虑到传统的梯形规划法在机体姿态调整的过程中一直存在加速度不连续的问题，导致机体的等效惯性力突变较大，这在机器人搭载负载或是机身自重较大时影响较大，会使机器人产生较大机体晃动，不利于机器人在崎岖地形下的适应。

由此，根据机器人现有规划方法的不足之处：①机体姿态规划时加速度曲线不连续，机体等效惯性力易突变；②位姿调整构成的初始和结束时刻，速度和加速度部位零，即对机器人有冲击；③传统机器人姿态调整方法，机器人在初始和结束阶段速度变化过快，不利于机器人的平稳启停。机器人姿态调整时机身如果振动不平稳，直接影响机器人的位姿调整精准度甚至随着时间的延续，累积误差变大会使机器人产生倾翻。这对于机器人在崎岖地形上执行任务时是非常严重的问题，所以针对以上三点不足，再结合机器人姿态调整时，支撑腿的关节转角必须在其合理约束范围的约束条件，本实施例为使姿态调整时达到机体平稳无晃动的目标，给出六足机器人应满足的约束指标：

①：姿态角在调整的始末不应变化过快；

②：姿态角速度在调整周期的始末为零，且曲线应连续无断点；

③：姿态角加速度在调整周期的始末为零，且曲线连续无断点；

④：姿态角调整过程中，支撑腿关节角度必须在转角范围内；关节角速度不能超过所允许的最大值。

以上目标等价为约束条件：

式中，E_f为初始姿态角度；E_a为目标姿态角度；E(t)为姿态角曲线；

为姿态角速度曲线；

为姿态角速度曲线；θ_ij为第i条腿第j个关节的角度；θ_ijmin为该关节的角度最小值；θ_ijmax为该关节的角度最大值；v_ij为该关节的角速度；V_e为该关节的角速度最大值；T为调整周期。

步骤B2、基于S曲线减加速的规划方法

现有技术中关于位姿调整多采用“梯形速度法”，该规划方法只保证了速度的连续性而未保证加速度的连续性，所以导致惯性力突变的问题机身不稳定。机身的不稳定容易引起机器人产生位姿偏差，影响其执行任务。

本实施例创造性的提出S曲线加减速的方式，为了清晰的描述E(t)在其值域内的运动，简化初始和目标角度姿态角改变时的计算量，令

姿态角度变化曲线可以描述为：

由于E(t)中的每一个元素值均不大于E_a中对应的元素值，所以有0≤s(τ)≤1，s(τ)为角度变化的比例系数和自变量，描述了角度的变化过程，可以通过设计s(τ)函数就可以实现对姿态角的规划，s(τ)为姿态曲线的比例系数变化率，通过规划s(τ)可以简化求取E(t)的过程，而且形式更简单。

基于以上多个平稳指标的约束条件，本实施例采用五段“S”曲线加减速的规划方法进行插补。该曲线分为加加速、加减速、匀速、减加速、减减速五段。

由于除匀速段以外，其余四个阶段对称。设四个变速段斜率大小都为A，四段的时间都为T_a，加加速段位移L₁、加减速段位移L₂。

则：

则s(τ)的加速度分段函数为：

对加速度积分可得：

对速度积分可得到s(τ)的分段函数：

根据姿态平稳调节的目标，设计了加速度起止为零且连续的加减速规划方法，机器人可以根据该方法实现平稳姿态调节。

另外，由于六足机器人在姿态调整过程中可能会出现某条支撑腿关节角度限位的情况。因此在姿态调整前需先对所有支撑腿进行分析，再进行姿态规划。故对姿态调整前的准备工作还包括步骤B’，对六足机器人支撑腿判断的步骤，如图14所示，以确定实际支撑腿和随动腿，并对其分别进行规划，具体包括：

步骤B’1、支撑腿判别算法

所有支撑腿需满足关节角约束(关节角约束指的是每条腿三个转角的最小最大的范围如表1中的数据，姿态的实现通过支撑腿转角实现，但支撑腿转角决不能超出其区间范围，否则这个姿态角不可实现)和稳定性约束条件。稳定性约束采用静态稳定裕度法(SSM)，即重心沿重力方向在支撑平面上的投影到支撑多边形各边的距离中的最小值作为稳定裕度值S_m来衡量机器人的稳定性。

支撑腿的选取过程：

1)将所有支撑腿(多足机器人实际的腿的数量)的标志位设置为“1”，由目标姿态E_a通过式(2)、(9)得到每条支撑腿的三个关节角，将关节角度对比关节角度约束范围；如果某支撑腿有任一关节角超出约束范围，则该腿标志位立即设置为“0”，作为随动腿进行后续规划。

2)对满足关节约束的标志位“1”的腿所构成支撑面的投影多边形进行稳定性约束判定，如果S_m＞0则证明该姿态角满足稳定性约束条件，则当前标志位为“1”的腿参与姿态角调节过程；否则机器人不稳定需重新设置E_a，直到同时满足关节角约束和稳定性约束指标，判断结束。

步骤B’2、支撑腿和随动腿规划

(1)支撑腿规划

对于经过上述判别算法流程，标志位仍为“1”的腿，作为参与姿态调节的支撑腿。在支撑腿足端位置固定的前提下，根据上述规划的E(t)曲线，得到每时刻t的姿态角所对应的关节角θ_ij，由此可以得到姿态角规划曲线所对应的支撑腿关节角变化曲线θ_ij(t)。

当支撑腿足端无滑动时，将式(8)对时间t求导，可得：

由于存在关系：

式中，S(ω)为姿态角速度矢量的反对称矩阵

故式(16)可写作：

所以足端点在机体坐标系中的速度可表示为：

对式(6)求导可得，

联立式(3)可以得到姿态角变化时支撑腿关节角速度的变化曲线：

(2)随动腿规划

随动腿是不能配合实现当前姿态调整范围的支撑腿，因此选取好支撑腿以后需对随动腿进行新的落足点规划，即需要跟随姿态变换同时移动到新位置的腿，以保证姿态调整后腿部关节在允许的运动范围内。

以支撑面是平面为例给出规划,随动腿足端点从初始^WP_f点到目标落足点^WP_a,随动腿的规划周期T与姿态调整周期保持同步。为计算简便，^WP_a选取为调整结束时刻随动腿足端点与地形有效接触面的中央位置。假设末时刻随动腿足端可达范围与地面的所构成接触面的范围坐标分别为：x_min、x_max、y_min、y_max、z_min、z_max。则期望足端点坐标^GP_a(t)＝[x_a,y_a,z_a]为：

对于足端^WP_i(t)的轨迹需满足式(23)的约束条件

式中，^WP_f为随动腿初始足端点；^WP_a为随动腿目标落足点；^WP_i(t)、

和

为足端点在参考坐标系下的位置、速度和加速度曲线。

根据式(23)的约束条件，利用高阶多项式法拟合足端点曲线可以描述为：

^WP_i(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵ (24)

所以随动腿^WP_i(t)＝[^Wx(t),^Wy(t),^Wz(t)]^T在参考坐标系下的运动轨迹可以描述为:

式中，^Wx(t)、^Wy(t)和^Wz(t)为足端点在参考坐标系下沿三个坐标轴方向的变化曲线。

当随动腿足端点的初始和目标位置已知，规划周期T已知时，可以根据式(25)得到足端规划曲线。

基于以上方案的描述，对平稳姿态调整时的支撑腿和随动腿分别作了运动规划，为了进一步验证本发明方法的有效性，下面对机体姿态平稳调整策略进行仿真实验，具体的实验流程参考图14。

仿真试验

机器人的仿真环境为5°的斜坡地形，支撑腿为leg1、leg2、leg3和leg6，leg4和leg5为不接触地面的工作状态。实验参数如表2所示。支撑腿足端相对地面无滑动的前提下，机体姿态从初始姿态E_f＝[0°，5°，0°]^T到目标姿态E_a＝[5°，18°，10°]^T进行变换。首先得到leg1的膝关节在调节中存在突破约束关节角限制的时刻，该腿状态位置被设置为“0”，作为“随动腿”进行运动。而leg2，leg3，leg6关节角在整个姿态范围内所有关节角度都在约束范围且构成支撑平面满足稳定条件，所以作为支撑腿。

表2试验参数表

设定仿真参数，平稳调整四个变速段斜率大小都为A＝8°/s³，T_a＝0.3s。由规划的姿态角E(t)曲线和姿态角速度

曲线根据式(2)、(9)和(21)，得到支撑腿关节角度曲线θ_ij(t)和角速度曲线

由于连续可导函数的极值总是可在其导函数为零的点取得，所以关节角速度曲线可以在t＝2T_a或t＝T-2T_a处取得极值,支撑腿三个关节角的速度极值为表2中的V_e。对每一关节都能求得对应的调整周期T_ij,取所有T_ij中最大者作为调整周期，此调整周期T既满足所有关节角速度约束，又是实现这个调整过程的最快时间。经计算leg3的膝关节对应的需要的调整时间最大，所以调整周期T＝T₃₃＝1.98s。

通过MATLAB对姿态角常用的匀速调整规划方法和本发明所述的平稳调整规划方法进行对比实验，两者的姿态角速度图像分别如图4(a)、(b)所示；姿态角加速度变化图像分别如图5(a)、(b)所示。其中，采用平稳调整方法规划时的支撑腿关节角度和角速度变化图像如图6、图7所示。

由图4两种规划方法的姿态角速度图像对比，匀速规划方法相对于平稳规划方法在整个调整周期的角速度曲线规划不够平滑。由图5可知平稳规划方法姿态角从初始姿态到目标姿态的整个调整周期中加速度曲线连续无断点。在调整周期的初始和结束阶段速度变化平缓且无突变，满足式(10)平稳姿态调整的约束指标。而匀速调整法的加速度曲线曲线变化不连续且存在突变。

图6、图7为支撑腿leg2，leg3，leg6的关节角和角速度变化图像。由图像可知各支撑腿满足式(10)中的关节角度约束和关节角速度约束条件。支撑腿关节角和角速度曲线随时间连续变化，舵机能够平稳转动。

对于随动腿leg1，经式(22)计算，在姿态调整末时刻其足端可达空间与地面的接触面的中心位置^WP_a＝[-18.8,10.46,-10.96]，因此将^WP_a点作为末时刻随动腿的落足点。然后根据式(23)的约束条件，结合式(25)得到随动腿leg1足端的规划曲线。足端点在x、y、z方向的坐标、线速度、线加速度图像分别如图8所示。

根据图8随动腿足端的变化图像，随动腿可以实现从初始足端点到目标足端点的规划。其速度和加速度初始时刻从零开始变化，结束时刻也为零，符合式(22)的要求。实现了随动腿与姿态调整过程的同步，保证了足端点的平稳规划。

通过仿真实验对比了匀速规划方法和平稳规划方法姿态角速度和角加速度图像，仿真图像表明平稳规划方法的姿态曲线在连续性和平稳性上要优于匀速调整策略，接下来将通过物理样机实验进一步验证两种规划策略。

样机试验

(1)两种规划方法目标值与实际值对比实验

为验证理论的可靠性，采用六足机器人装载陀螺仪设备为平台环境进行对比试验。实验环境与实验验证的仿真环境相同。机器人在5°斜坡地形下以leg2，leg3，leg6三条腿支撑进行姿态调整。机体姿态从初始姿态E_f＝[0°，5°，0°]^T到目标姿态E_a＝[5°，18°，10°]^T，调整周期T＝1.98s。本实验采用ALUBI公司的HI219M型九轴陀螺仪为测量装置，其安装在机器人上盖的中心位置。通过使用该传感器可以实时采集出机器人在X,Y,Z三个方向的姿态角度、角速度、角加速度数据，并通过无线传输模块传到配套上位机进行实时绘图。以采集姿态调整时的角速度信息为例，分析匀速调整法角速度期望值与实际值的贴合程度(如图9所示)，平稳调整法角速度期望值与实际值的贴合程度(如图10所示)。图中黑色粗线条表示传感器采集的实际值，黑色细线条为仿真的目标值。

从图像可以看出匀速调整法t＝0.2，1.5S时出现了较强的波动，这是由于匀速调整加速度不连续性造成的。而本发明的平稳调整法目标值与实际值曲线贴合紧密，跟随性高于匀速调整法。图中用a、b和c点表示匀速调整法中偏转角、俯仰角和滚动角的角速度实际值与目标值偏差最大的点；用a'、b'和c'点表示平稳调整法中偏转角、俯仰角和滚动角的角速度实际值与目标值偏差最大的点。经计算匀速调整法在三个方向实际与目标的最大偏差分别达62.5％、8.1％、30.1％；而匀速调整法在三个方向实际与目标的最大偏差分别达5.5％、3.2％、2.7％。从代表点的数据分析，平稳调整法相比匀速调整法的姿态角速度目标值与实际值的贴合程度提高了57.0％。实验证明采用本发明所提出的平稳姿态调整方法，在姿态调整时姿态角度、角速度和角加速度实际值与目标值曲线的贴合程度较好，波动较小，跟随性及稳定性都有很大提高。

(2)两种规划方法负载状态下稳定性对比实验

为了更直观的展示两种规划方法在负载条件下的对比，使机器人搭载红墨水进行对比试验。本实验环境以机器人的俯仰角单方向对两种姿态规划方法进行对比。机器人在10°斜坡地形下以三足支撑状态进行姿态调整，姿态角从初始姿态E_f＝[0°，10°，0°]^T到目标姿态E_a＝[0°，40°，0°]^T，调整周期T＝2.5s。机器人的中心位置处固定有一个容量30ml装有红墨水的量杯，初始状态量杯液面无波动。实验过程匀速调整法的状态截图如图11所示，平稳调整法的状态截图如图12所示。

如图11所示，由于匀速调整法姿态角速度、角加速度的不连续性，姿态调整过程中惯性力突变严重导致机身出现晃动，量杯中的红墨水溢出并沿机体外壳滴落至支撑面。图12为平稳调整法的状态截图，由于平稳调整法姿态角速度、角加速度曲线连续无突变，在整个调整周期内量杯中的红墨水液面变化平缓无溢出，机身在整个调整周期内无较大晃动。

通过仿真与样机实验对所提出的方法进行了验证和分析。通过对比实验的结果表明，本发明所提出的姿态调节方法，加速度连续无断点，在姿态变换的启停阶段速度变化平缓且整个过程无突变，提高了姿态规划时实际值与目标值的贴合程度。物理样机实验显示，姿态角速度实际值与目标值的最大偏差由62.5％变为5.5％，贴合程度提高了57.0％。研究成果解决了机器人在变换机体姿态时产生的机身不稳定问题，为多足机器人提高地形适应能力提供了重要参考。

崎岖地形下六足机器人姿态的变换对机器人的运行高效性和稳定性具有重要意义。本发明针对六足机器人在姿态调整时容易产生机身晃动的问题，提出了一种姿态平稳调整策略。分析了机器人在进行姿态调整时支撑腿对其影响，给出了一种平稳调整规划方法并与传统的匀速调整方法做了仿真对比。通过仿真和实验，分别验证了两种调整方法的理论值与实际值偏差程度和负载下的机身稳定程度，实验表明平稳调整方法目标值与实际值贴合程度更高。在进行姿态调整时机身晃动更小，提高了机器人在崎岖地形上适应能力。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例应用于其它领域，但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。

Claims (8)

1.基于多足机器人的姿态平稳性调整方法，其特征在于：包括以下步骤：

步骤A、基于运动学分析构建多足机器人支撑腿关节角与机体姿态的映射关系；

步骤B、多足机器人的姿态平稳性调整，包括：

步骤B1、设定满足多足机器人姿态平稳调节要求的约束指标；

步骤B2、基于S曲线加减速法规划机体姿态角变换曲线；

步骤B3、根据步骤A所得到的映射关系得到支撑腿关节的控制轨迹要求，从而实现多足机器人的姿态平稳性调整。

2.根据权利要求1所述的基于多足机器人的姿态平稳性调整方法，其特征在于：对多足机器人姿态平稳性调整前还包括步骤B’：多足机器人支撑腿判断，具体包括：

步骤B’1、支撑腿判别，确定多足机器人中的支撑腿和随动腿；

步骤B’2、对确定的支撑腿和随动腿进行规划：

(1)对确定的支撑腿进行规划，得到姿态角变换曲线所对应的支撑腿关节角变化曲线；

(2)对确定的随动腿进行规划，得到其足端规划曲线。

3.根据权利要求2所述的基于多足机器人的姿态平稳性调整方法，其特征在于：所述步骤A具体包括以下步骤：

步骤A1、建立多足机器人的运动学模型；

(1)建立参考坐标系∑W、机体坐标系∑B和跟关节坐标系∑G_i，并得到第i条腿足端位置

在跟关节坐标系∑G_i中的描述；

(2)根据单腿逆运动学方程，通过多个足端位置得到关节角度矩阵θ_i＝[θ_i1,θ_i2,θ_i3]^T，i为多足机器人的腿的数量；

其中，θ_i＝[θ_i1,θ_i2,θ_i3]^T表示第i条腿从内而外，分别为跟关节转角、髋关节转角和膝关节转角；

(3)进而确定足端运动时，机器人第i条腿足端速度与关节角速度之间的变换关系；

步骤A2、建立姿态角-关节角映射模型：

(1)定义多足机器人姿态矩阵E＝[α,β,γ]^T，α，β，γ分别表示机器人姿态的偏转角、俯仰角和滚动角；

确定足端点在跟关节坐标系下与在机体坐标系下的坐标关系以及足端点在机体坐标系下与在参考坐标系下的坐标关系；

(2)进而可以得到跟关节坐标系中的足端位置与姿态角的关系，当机器人支撑足端位置固定时，建立各支撑腿在跟关节坐标系中的位置与姿态角的映射关系。

4.根据权利要求3所述的基于多足机器人的姿态平稳性调整方法，其特征在于：所述步骤B1中，多足机器人满足以下约束条件：

式中，E_f为初始姿态角度；E_a为目标姿态角度；E(t)为姿态角曲线；

为姿态角速度曲线；

为姿态角加速度曲线；θ_ij为第i条腿第j个关节的角度；θ_ijmin为该关节的角度最小值；θ_ijmax为该关节的角度最大值；v_ij为该关节的角速度；V_e为该关节的角速度最大值；T为调整周期。

5.根据权利要求4所述的基于多足机器人的姿态平稳性调整方法，其特征在于：所述步骤B2中，基于S曲线加减速法规划机体姿态角变换曲线具体采用以下方式：

(1)令

将姿态角度变化曲线描述为：

由于E(t)中的每一个元素值均不大于E_a中对应的元素值，所以有0≤s(τ)≤1，s(τ)为角度变化的比例系数和自变量，通过设计s(τ)函数实现对姿态角的规划；

(2)基于上述约束条件，采用五段“S”曲线加减速的规划方法进行插补，该曲线分为加加速、加减速、匀速、减加速、减减速五段，设四个变速段斜率大小均为A，四段的时间都为T_a，加加速段位移L₁、加减速段位移L₂；

则：

则s(τ)的加速度分段函数为：

进而通过积分得到s(τ)的分段函数：

实现平稳姿态调节。

6.根据权利要求5所述的基于多足机器人的姿态平稳性调整方法，其特征在于：所述步骤B’1中，对支撑腿判别时具体采用以下方式；

1)将多足机器人所有的支撑腿的标志位设置为“1”，由目标姿态E_a通过映射关系得到每条支撑腿的三个关节角，将关节角度对比关节角度约束范围，如果某支撑腿有任一关节角超出约束范围，则该腿标志位立即设置为“0”，作为随动腿进行后续规划；

2)对满足关节约束的标志位“1”的腿所构成支撑面的投影多边形进行稳定性约束判定，若满足稳定性约束条件，则当前标志位为“1”的腿参与姿态角调节过程；否则机器人不稳定需重新设置E_a，直到同时满足关节角约束和稳定性约束指标。

7.根据权利要求6所述的基于多足机器人的姿态平稳性调整方法，其特征在于：所述步骤B’2中对支撑腿进行规划时，具体采用以下方式：

对于经过步骤B’1所述判别方法后，标志位仍为“1”的腿，作为参与姿态调节的支撑腿，在支撑腿足端位置固定的前提下，根据规划的E(t)曲线，得到每时刻t的姿态角所对应的关节角θ_ij，由此可以得到姿态角规划曲线所对应的支撑腿关节角变化曲线θ_ij(t)。

8.根据权利要求6所述的基于多足机器人的姿态平稳性调整方法，其特征在于：所述步骤B’2中，对随动腿进行规划时具体采用以下方式：

随动腿足端点从初始^WP_f点到目标落足点^WP_a,随动腿的规划周期T与姿态调整周期保持同步，设^WP_a选取为调整结束时刻随动腿足端点与地形有效接触面的中央位置；

假设末时刻随动腿足端可达范围与地面的所构成接触面的范围坐标分别为：x_min、x_max、y_min、y_max、z_min、z_max，则期望足端点坐标^GP_a(t)＝[x_a,y_a,z_a]为：

对于足端^WP_i(t)的轨迹需满足式(23)的约束条件

式中，^WP_f为随动腿初始足端点；^WP_a为随动腿目标落足点；^WP_i(t)、

和

为足端点在参考坐标系下的位置、速度和加速度曲线；

根据式(23)的约束条件，随动腿^WP_i(t)＝[^Wx(t),^Wy(t),^Wz(t)]^T在参考坐标系下的运动轨迹描述为:

式中，^Wx(t)、^Wy(t)和^Wz(t)为足端点在参考坐标系下沿三个坐标轴方向的变化曲线。

Images