CN114967722B - 一种摇臂式机动平台自主越台阶障碍方法 - Google Patents

Abstract

本发明公开了一种摇臂式机动平台自主越台阶障碍方法，包括：构建用于跨越台阶障碍的摇臂式机动平台；对摇臂式机动平台设置激光雷达；采集障碍物的轮廓、高度、坡度、位置信息、俯仰角空间变量和摇臂关节空间变量；设计摇臂式机动平台的越障运动步态；对摇臂式机动平台进行跨越障碍的状态分析，获取状态分析结果；基于越障运动步态与状态分析结果进行障碍跨越。本发明推导提出了自主越障动作序列中关键姿态的概念，并以此为基础，设计了一种遵循最小运动代价准则的姿态规划方法，可以实现障碍物感知定位和分类、有序规划越障姿态、自适应调整越障动作幅度。

Description

一种摇臂式机动平台自主越台阶障碍方法

技术领域

本发明属于无人化军事机械技术领域，特别是涉及一种摇臂式机动平台自主越台阶障碍方法。

背景技术

地面无人平台具有优越的机动性和良好的地形适应能力，如今被广泛应用于军事行动、农林作业和太空探索等领域。军用地面移动机器人因其能适应恶劣的环境以及无条件服从战略指挥，适用于多种战术体系，能代替士兵完成各项高危任务，如潜伏侦查、搜救支援、扫雷破障、火力掩护等，完美契合了未来战场装备无人化的需求，是目前无人化军事技术发展的一大热点。除了军事目的，地面无人平台的重要应用还包括太空探索，监视，侦察危险地点，例如具有放射性或化学污染的区域等。

地面无人平台的行动系统的主要可以分为轮式、履带式和腿式，以及组合衍生出的四种复合类型：轮腿式、履带腿式、轮履式和轮腿履带式。

轮式运动具有移动速度快、能量效率高、机动性高的特点，但是车轮不适合攀爬障碍物且无法长期适应不规则地形，所以轮式运动在非结构环境中的移动能力较低；腿式运动是比较适合非结构化环境的行走系统，因为其能够以合理的步态克服各种障碍和复杂路面，但是腿式移动机构运动不连贯，所以平台移动速度相对更慢且能量效率低；履带式运动的综合性能介于轮式和腿式之间，平台通过采用履带增大了地面接触面积，提高了自身越野性能，但是整体结构复杂，运动惯性大，所以移动速度和机械效率有所降低。

复合式运动的主要特点是结合上述三种类型的移动系统的两个或多个移动执行机构形成复合行走结构。通过对比，综合了三种特征的轮腿履带式运动不能完全发挥各种机构的性能特点，这是由于复杂的执行机构很大程度上增加了平台的有效载荷，降低了整体性能；而轮腿式运动结合了轮式运动和腿式运动的双重优势，轮腿复合机构所具有的地面适应能力和越障能力可以大大提高平台在复杂、非结构环境中的机动性和通过性等地面环境适应能力，且控制系统相对简单。

发明内容

本发明的目的是提供一种摇臂式机动平台自主越台阶障碍方法，以解决上述现有技术存在的问题。

为实现上述目的，本发明提供了一种摇臂式机动平台自主越台阶障碍方法，包括：

构建用于跨越台阶障碍的摇臂式机动平台；

对所述摇臂式机动平台安装二维激光雷达；

基于所述激光雷达采集障碍物的高度、坡度、位置信息，进行台阶障碍特征检测与越障过程中的参考点检测，并在越障过程中，实时更新机体与障碍物参考点的距离，补全上一阶段传感器盲区位置的环境信息；

设计所述摇臂式机动平台的越障运动步态；

对所述摇臂式机动平台进行跨越障碍的状态分析，获取状态分析结果；

基于所述越障运动步态与所述状态分析结果进行障碍跨越。

可选地，对所述摇臂式机动平台设置激光雷达的过程包括：

将激光雷达安装于摇臂机动平台车体侧面，扫描平面与车身侧表面平行，并使激光雷达的扫面平面夹在摇臂与车身之间。

可选地，设计所述摇臂式机动平台的越障运动步态的过程包括：

构建越障步态规则与越障步态模型；

基于所述越障步态规则与所述越障步态模型进行越台阶过程的摇臂状态切换；

基于所述摇臂状态切换构建越台阶障碍动作序列。

可选地，对所述摇臂式机动平台进行跨越障碍的状态分析的过程包括：

对所述摇臂式机动平台进行质心位置估计；

计算越台阶过程中的车轮工作空间与过渡状态的摇臂工作空间；

分析越台阶过程中的稳定性约束与防碰撞约束；

基于最小运动代价准则对摇臂式机动平台越台阶的全过程进行分析，优化越台阶过程中的关键姿态。

可选地，所述越障步态规则包括：

每个摇臂包括三种状态：向前、腾空和向后；所述向前表示车轮接触地面，摇臂较初始位置向前旋转；所述腾空表示使用摇臂抬起车轮以允许车轮通过障碍物的状态；所述向后状态表示车轮接触地面，摇臂较初始位置向后旋转；

每个悬架的车轮只有在摇臂腾空状态下才可以通过障碍物；

每个摇臂在越障前后依次以向前、腾空、向后的顺序改变状态；最前方摇臂的状态按向前、腾空、向前的顺序变化，位于最后方摇臂的状态按向后、腾空、向后的顺序变化。

可选地，所述越台阶障碍动作序列包括：

前轮越障阶段：前摇臂由支撑相转变为摆动相，改变整车质心位置，扩大支撑边界，前轮抬升至台阶高度，驱动支撑相车轮控制车辆向前行驶，直至前轮接触台阶上平台，所有摇臂均为支撑相，前轮越障结束；

所述支撑相包括处于向前状态和向后状态的摇臂，所述摆动相包括处于腾空状态的摇臂；

中轮越障阶段：中摇臂由支撑相转变为摆动相，中摇臂在腾空状态下朝远离地面方向转动，使达到中间车轮达到台阶高度，同时，支撑相抬高整车质心直至超过台阶高度，中间车轮达到台阶高度，支撑相车轮驱动整车向前行驶，直至中间车轮接触台阶上平台且车身不与台阶发生干涉，中间摇臂转换为向后状态，所有摇臂均为支撑相，中轮越障结束；

后轮越障阶段：后摇臂由支撑相转变为摆动相，剩余支撑相调整车身姿态，扩大支撑边界，车身后端抬高，直至俯仰角为0；摆动相调整后车轮高度，所有车轮处于同一水平高度，支撑相车轮驱动整车向前行驶，直至后轮接触台阶上平台，所有摇臂均为支撑相，后轮越障结束；

支撑相调整车身姿态，使整车恢复至初始状态。

可选地，对所述摇臂式机动平台进行质心位置估计的过程包括：

基于摇臂平台的空载质量获取摇臂平台的整体质心在车体坐标系中的位置；

构建世界坐标系，设定世界坐标系原点位于所述车体坐标系的正下方；

测量摇臂平台自身的姿态角与各个摇臂关节的转动角度；

基于所述姿态角、所述转动角度与所述世界坐标系获取所述摇臂式机动平台的质心位置。

可选地，计算越台阶过程中的车轮工作空间与过渡状态的摇臂工作空间的过程包括：

基于所述激光雷达获取的俯仰角空间变量和摇臂关节空间变量，采用蒙特卡洛法估计平台越台阶过程中，车轮在车体坐标系下的工作空间；

基于蒙特卡洛法，计算摇臂关节在不同角度的高度位置和俯仰角，获取摇臂角度的变化范围，基于所述变化范围过渡状态的摇臂工作空间。

可选地，分析越台阶过程中的稳定性约束与防碰撞约束的过程包括：

所述稳定性约束包括：基于重心投影法、静态稳定边界法、能量稳定边界法判定摇臂平台稳定性，构建稳定性约束条件；

所述防碰撞约束包括：在前轮越障阶段，以前摇臂关节、中间摇臂关节和台阶棱角为顶点定义一个三角形A；在中轮越障阶段，以前摇臂关节、后摇臂关节和台阶棱角为顶点定义一个三角形B，分别计算所述三角型A与所述三角型B的面积；若满足：

则摇臂平台不与台阶产生碰撞。

可选地，优化越台阶过程中的关键姿态的过程包括：建立优化整车质心变化高度的目标函数；计算整车质心在世界坐标系下的高度；计算各个车轮轮心在世界坐标系下的高度；建立优化摇臂关节角度变化的目标函数；基于车轮工作空间计算车轮在跨越方向的可达位置；基于俯仰角空间变量、摇臂工作空间以及稳定性约束与防碰撞约束获取运动代价最小的关键姿态以及姿态参数，进行越台阶过程中关键姿态的优化。

本发明的技术效果为：

(1)设计了一套针对台阶障碍检测的传感器布置方案，开发了相应的感知定位算法，可实时获取障碍物的距离、高度和坡度等关键数据。

(2)提出了自主越障动作序列中关键姿态的概念，并以此为基础，设计了一种遵循最小运动代价准则的姿态规划方法。

(3)设计并开发了一种完全自主的越障运动规划方法，可以实现障碍物感知定位和分类、有序规划越障姿态、自适应调整越障动作幅度。

附图说明

构成本申请的一部分的附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1为本发明实施例中的台阶障碍位于摇臂平台斜前方向图；

图2为本发明实施例中的激光雷达安装位置图；

图3为本发明实施例中的车轮越台阶示意图，其中(a)为前轮越台阶状态切换，(b)为中轮越台阶状态切换，(c)为后轮越台阶状态切换；

图4为本发明实施例中的越障步态模型图；

图5为本发明实施例中的前轮越障示意图；

图6为本发明实施例中的中轮越障过程图；

图7为本发明实施例中的后轮越障过程图；

图8为本发明实施例中的摇臂平台车轮工作空间分析图，其中(a)为后轮工作空间，(b)为中轮工作空间，(c)为前轮工作空间；

图9为本发明实施例中的车轮工作空间的三维表示图；

图10为本发明实施例中的平台越台阶过渡状态图；

图11为本发明实施例中的摇臂过渡状态参数图，其中(a)为前摇臂过渡状态参数，(b)为中摇臂过渡状态参数；

图12为本发明实施例中的越400mm台阶摇臂关节工作空间图，其中(a)为前轮越障摇臂关节工作空间，(b)为前轮越障摇臂关节工作空间；

图13为本发明实施例中的NESM法稳定性判据图，其中(a)为前轮越障稳定性分析，(b)为中轮越障稳定性分析，(c)为后轮越障稳定性分析；

图14为本发明实施例中的平台越障防碰撞约束判据图，其中(a)为前轮越障防碰撞分析，(b)为中轮越障防碰撞分析；

图15为本发明实施例中的摇臂平台越障过程图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

实施例一

如图1-15所示，本实施例中提供一种摇臂式机动平台自主越台阶障碍方法

6×6摇臂式无人平台在越障过程中,除了需要感知车辆本体的有效信息，越障运动规划的准确性、实时性也对障碍物信息的感知提出了更高的要求。环境感知在摇臂平台越障过程的主要任务是：在越障准备阶段，采集障碍物轮廓、高度、坡度、位置等信息；在越障过程中，实时更新机体与障碍物参考点的距离，补全上一阶段传感器盲区位置的环境信息。

本实施例获取台阶障碍特征信息所用的传感器为2D激光雷达。

台阶障碍特征检测

2D激光雷达通过高速旋转的反射镜将激光发射出去，激光遇到目标后，激光的部分能量从目标反射回到接收器上，激光雷达通过测量发射光和反射光之间的时间差解析得到距离信息。

激光雷达返回的目标数据是极坐标系下的角度和距离，通过坐标转换，可以从极坐标系转化到直角坐标系。在扫描区域中，激光雷达在每个角度分辨率对应位置解析出的距离值会被依次连接起来，这样，通过直角坐标系就能非常直观地看到周围物体的轮廓。

激光雷达通常有五个性能衡量指标：测距精度、探测距离、扫描频率、角度分辨率以及扫描范围。

2D激光雷达是在一个平面上扫描，获得一个平面的二维信息。根据该特性，2D激光雷达要采集台阶障碍的特征信息，需要在垂直平面内进行扫描。故将激光雷达安装在摇臂平台车体侧面，扫描平面与车身侧表面平行。摇臂与车体之间存在一定间隙，忽略车身在运动过程的形变以及摇臂工作时的横向位移，则激光雷达的扫面平面夹在摇臂与车身之间，可以将平台自身对探测的干扰降到最低，使激光雷达更全面地检测前后方障碍物。

建立激光雷达坐标系R(O_R-X_RY_RZ_R)，坐标系原点位于扫描中心，扫描范围位于X_RO_RY_R平面。系统坐标系定义与2.2.1节一致，X_R轴方向为激光雷达0°方向，与Z_B轴负方向相同，Y_R轴方向为激光雷达90°方向，指向摇臂平台长度方向。激光雷达扫描范围为-120°～120°，扫描频率为10Hz，角分辨率为0.1°，探测距离为20m。将每次采集到的样本点数据储存在集合P中，P中每个元素对应激光发射方向测量到的距离，如果激光射线没有被物体反射，则返回空值。

根据上述设定，可以计算激光雷达的第i条扫描射线在激光雷达坐标系下相对X_R轴的极坐标角度

式中，0.1为雷达的角分辨率，i为激光雷达扫描射线的编号，i＝0，1，…，1200。

将距离数据进行极坐标系到直角坐标系的变换，可以得到障碍点p_i在激光雷达坐标系下的位置坐标^Rp_i。

由于激光雷达坐标系方向与车体坐标系、世界坐标系均不相同，在激光雷达坐标系下表示的障碍物位置信息无法直观反映出障碍物特征，所以先根据激光雷达的安装位置得出车体坐标系R相对于车体坐标系B的齐次变换矩阵

然后得到障碍点p_i在车体坐标系下的齐次坐标^Bp_i，如式。

式中，x_R、y_R、z_R为激光雷达在车体坐标系下的位置坐标。

^Bp_i表示障碍物相对车体坐标系的位置描述，会随平台移动和姿态角的变化而改变。在实际情况下，障碍物为客观存在的物体，其形状、位置等信息不受观测者状态的影响。为了方便提取障碍物特征，将^Bp_i变换到世界坐标系下，得到表示台阶障碍点的集合P在世界坐标系下的描述^Op_i。

将世界坐标系原点设置在摇臂平台初始状态下车体坐标的正下方，与地面处于同一高度。假设摇臂平台初始状态正对台阶，对800mm高度的典型台阶进行检测。

将台阶障碍点的集合P分成三个子集：P_t、P_m、P_b，分别表示台阶上部平台、台阶坡面或台阶垂直面和台阶下部平台。

首先，将世界坐标系原点的高度z_O置为0，则理论上点集P_b中各点的z轴坐标值均为0，受激光雷达测距分辨率的影响，取z_p∈[-0.005,0.001]的点^Op_b置于子集P_b中。在集合P的各点中寻找最大的z轴坐标值z_{max_p}，取z_p∈[z_{max_p}-0.005,z_{max_p}]的点^Op_t置于子集P_t中。则剩余的点属于子集P_m，即P_m＝C_P(P_t∪P_b)P^l _t。

台阶高度等于集合P_t中所有点的z轴坐标的平均值。

台阶坡段的水平距离等于集合P_m中x轴坐标的最大值和最小值的差，则台阶坡度的正切值等于台阶高度除以坡段水平距离。

取集合P_b中x轴坐标的最大值

通过齐次变换可以得到车体坐标系原点在世界坐标系下的坐标，将

与^Ox_B相减，得到摇臂平台与台阶障碍的距离，如式。

在实际越野环境中，摇臂平台在探测到障碍时车头方向是随机的，所以台阶障碍的朝向与世界坐标系X_O轴方向往往存在一定夹角，那么，一个2D激光雷达的垂直扫描得到的台阶特征不具备代表性和可靠性。

为了解决这一问题，在车身两侧的对称位置安装2D激光雷达，获得从摇臂平台左右两侧识别到的障碍物信息，以此计算台阶坡度和平台与台阶障碍之间的距离，验证台阶高度，补全摇臂平台相对台阶障碍物的航向角信息。

两个激光雷达在车体坐标系下的坐标分别为

和

通过对左右两侧激光雷达同一时刻采集到的台阶障碍物点P^l、P^r进行齐次变换，得到其在世界坐标系下的坐标，绘制各自测得的台阶轮廓。

经过数据处理，分别取点集

和点集

中x轴坐标的最大值

以及点集

和点集

中x轴坐标的最小值

它们分别代表台阶坡底线、台阶坡顶线经激光雷达扫描在世界坐标系下的X_O轴坐标。将位于底线、顶线的扫描点的坐标做差，结合世界坐标系下两个平行的激光雷达扫描平面在Y_O轴方向的距离，计算台阶坡底线和顶线与X_O轴的夹角，取平均值，可以得到台阶障碍绕Z_O轴旋转，与X_O轴的夹角ξ，则台阶障碍物相对摇臂平台的航向角等于

由式可以得到两个激光雷达探测的台阶高度，对其取平均值，获得校准后的台阶高度H。

已知夹角ξ，则台阶坡段的水平距离等于

对左右两个激光雷达的计算结果取平均值，得到台阶坡度α。

在车体坐标系下，取^Bp_i计算摇臂平台距台阶障碍扫描点的平均距离^Bx_R，以及摇臂平台距台阶障碍的垂直距离^Bd_R。

越障过程中的参考点检测

当激光雷达未通过台阶时，台阶被识别为正障碍，激光雷达能够检测到台阶障碍的位置、高度等信息，但当激光雷达随车体移动，经过台阶棱的正上方，激光雷达返回的样本点集合P种将出现断点。

平台重心已经越过台阶，故此时平台的感知系统对台阶高度不再关注，平台需要获得雷达相对台阶棱的位置信息，以判断后轮越障是否结束。

在到达之后，平台相对台阶障碍的距离为负值。取集合P_t中x轴坐标的最小值

将

与^Ox_B相减，得到摇臂平台与台阶障碍的距离，如式。

越障步态规则

6×6摇臂式机动平台的外形与矩形六足机器人有相似之处，它们的机体平台均采用矩形布置形式，且与机体相连接的关节采用偏置布置形式。但摇臂平台运动和越障方式相比足式机器人有较大差异。足式运动的越障方法是利用周期性的腿的摆动和支撑运动，控制足端运动轨迹，朝目标方向行走，移动机体重心。

对于形状规则的障碍，足式机器人一般采用规则步态，又称固定步态。足式机器人实现固定步态的必要条件是能够控制足端按规划的空间轨迹运动，这需要保证机器人每条腿上至少具有3个自由度。然而，本文研究对象包含的摇臂行走系统为2自由度甚至更低，故摇臂平台无法遵循正常步态。因此，本文使用自由度小于等于2的摇臂机构设计一种摇臂平台的越障步态和相应规则。

摇臂平台的越障步态包括以下五条规则：

(1)摇臂摆角的定义；

(2)每个摇臂具有三种状态：向前(F)、腾空(O)和向后(R)，如图10所示。其中，“向前”状态表示车轮接触地面，摇臂较初始位置向前旋转；“腾空”状态表示使用摇臂抬起车轮以允许车轮通过障碍物的状态；“向后”状态表示车轮接触地面，摇臂较初始位置向后旋转。初始姿态下，摇臂不同于前面定义的三种状态，但受稳定性影响，摇臂平台越障的所有动作均不会维持在初始姿态，故不做更多讨论；

(3)单凭车轮滚动无法跨越障碍，所以每个悬架的车轮只有在摇臂“腾空”状态下才可以通过障碍物；

(4)每个摇臂在越障前后以F→O→R的顺序改变其状态。但是，最前方摇臂的状态按F→O→F的顺序变化，位于最后方摇臂的状态按R→O→R的顺序变化，以确保机器人车辆的静态稳定性；

(5)同轴两侧的摇臂按照上述四条规则从前轮依次通过障碍。

按照上述方法，摇臂平台的重心将随着每个车轮依次通过障碍区域而越过障碍物，并且机摇臂平台可以利用已经越过障碍物的摇臂有效地构造支撑多边形，以保持平台的最大静态稳定性。

越障步态模型

摇臂平台可以独立驱动同轴两侧的摇臂完成越障，然而，由于摇臂平台机构本身的限制，越障步态在独立驱动同轴的一对摇臂时获得的自由度有限。考虑摇臂平台同轴的左侧摇臂和右侧摇臂独立驱动的情况，设计一种越障步态。

步态节点的序号用C_A代替，L和R分别表示驱动左摇臂或右摇臂的中间步态，BOTH表示同时驱动左、右摇臂的中间步态。

在每个步态节点下，摇臂之间相对位置固定，平台姿态保持不变。越障步态将左右两侧一对摇臂按F→O→F或F→O→R或R→O→R的顺序抬高或降低划分为一个阶段。在越障步态模型中，摇臂平台可以依次驱动两侧摇臂，使它们逐个变换到目标状态，也可以同时驱动两侧摇臂，减少过渡时间，使它们同时到达目标状态。对于同侧具有3个摇臂机构的摇臂式机动平台，包括初始步态在内，越障步态模型可以定义19个步态。

越台阶过程摇臂状态切换

按照上述方法，摇臂平台使用步态运动通过单个台阶障碍的过程需要经过至少7个状态节点，完成三个阶段，即前摇臂F→O→F状态变换阶段，中摇臂F→O→R阶段状态变换阶段，后摇臂R→O→R状态变换阶段。将平台越台阶的三个阶段命名为：前轮越障阶段、中轮越障阶段和后轮越障阶段。在初始状态下，C_A为0，并且在越障过程，随着每对摇臂状态的改变而增加。

经过越障步态简化，摇臂平台的空间姿态求解问题可以在X_BO_BZ_B平面内求解，平台姿态正运动学方程见式、。

越台阶障碍动作序列

本段描述了平台在越台阶障碍过程各阶段的摇臂状态变化。在此基础上，要实现跨越可通过台阶障碍的目标，需要摇臂与车轮的相互配合。通过台阶障碍检测方法，在越障准备阶段，获得台阶特征、位置、相对航向角等信息，摇臂平台朝台阶方向转向，调整自身横摆角直到正对台阶，从台阶正前方的任意位置开始越障。

在规划越障步态时，将处于“向前”状态和“向后”状态的摇臂统称为支撑相，处于“腾空”状态的摇臂称为摆动相。

根据车轮在越障过程是否被驱动，还可以将平台翻越台阶的动作划分为前进阶段和姿态变换阶段。前进阶段包括前进Ⅰ、前进Ⅱ和前进Ⅲ阶段。前进阶段开始时有四个支撑相，结束时所有摇臂均为支撑相。姿态变换阶段包括前轮抬升、中轮抬升和后轮抬升阶段。姿态变换阶段存在摆动相，但同时需要保持至少四个支撑相，以保证平台的稳定性。

在前轮越障阶段，前摇臂先从支撑相转变为摆动相，同时，剩余的支撑相为了保持整车平衡，需要改变整车质心位置，扩大支撑边界，受前摇臂摆角和质心高度的综合影响，前轮最终抬升至台阶高度，然后驱动支撑相车轮控制车辆向前行驶，直至前轮接触台阶上平台，所有摇臂均为支撑相，前轮越障结束。

前轮越障可以分为前轮抬升和平台前进Ⅰ两个阶段。前轮抬升阶段是从控制摇臂开始t＝0到t＝t_f0，前轮达到台阶高度的时刻。整车前进Ⅰ阶段从t＝t_f0到t＝t_f1，前轮接触台阶并停止前进(v_x＝0)的时刻。

中轮越障开始，中摇臂先从支撑相转变为摆动相，受车身姿态和地面的限制，中摇臂在“腾空”状态下需要先朝远离地面方向转动，再使达到中间车轮达到台阶高度，同时，支撑相需要抬高整车质心，直至超过台阶高度，此阶段支撑边界将缩小，在支撑相和摆动相的相互作用下，中间车轮达到台阶高度，然后支撑相车轮驱动整车向前行驶，直至中间车轮接触台阶上平台且车身不与台阶发生干涉，中间摇臂转换为“向后”状态，所有摇臂均为支撑相，中轮越障结束。

中轮越障可以分为中轮抬升和平台前进Ⅱ两个阶段。中轮抬升阶段是从控制摇臂开始t＝t_f1到t＝t_m0，中轮达到台阶高度且整车质心超过台阶高度的时刻。整车前进Ⅱ阶段是从t＝t_m0时刻到t＝t_m1，中轮接触台阶并停止前进(v_x＝0)的时刻。

在后轮越障阶段，后摇臂先从支撑相转变为摆动相，剩余支撑相调整车身姿态，扩大支撑边界，使车身后端抬高，直至俯仰角为0，同时，摆动相调整后车轮高度，使所有车轮处于同一水平高度，然后支撑相车轮驱动整车向前行驶，直至后轮接触台阶上平台，所有摇臂均为支撑相，后轮越障结束。最后，支撑相调整车身姿态，使整车恢复至初始状态。

后轮越障可以分为后轮抬升和整车前进Ⅲ两个阶段。后轮抬升阶段是从控制摇臂开始t＝t_m1到t＝t_r0，中轮达到台阶高度且车身俯仰角为0的时刻。整车前进Ⅲ阶段是从t＝t_r0时刻到t＝t_r1，后轮接触台阶并停止前进(v_x＝0)的时刻。

越台阶障碍过程状态分析

平台质心位置估计

在具有空间多自由度的摇臂平台运动时，需要时刻关注其整体质心变化，尤其是在越障过程中，质心的位置决定了平台是否能够越障成功。如果摇臂平台的质心落在了支撑区域以外，平台在运动过程中可能会发生倾覆。

摇臂平台的整体质心在车体坐标系中的位置用^BP_COM表示。

式中，m为摇臂平台空载质量，m＝m_B+6m_A+6m_W。

摇臂平台的整体质心在车体坐标系的坐标如式。

首先，确定越障过程中的世界坐标系O(O_O-X_OY_OZ_O)。平台保持初始姿态向台阶移动，当相对台阶一定距离时，平台开始越障。设定世界坐标系原点位于平台开始越障时车体坐标系的正下方，与轮-地接触点处于同一高度，X_O轴指向台阶，Z_O轴与重力方向相反。

由式可以得到和世界坐标系中的坐标^OP_COM。

其中，摇臂平台自身的姿态角可以通过陀螺仪配合加速度计测量得到，各个摇臂关节的转动角度可以通过倾角传感器测量得到，这样就可以得到机器人的质心在世界坐标系中的位置。

越台阶过程车轮工作空间计算

轮-地接触点作为车轮与大地交互的空间位置，是整车姿态控制的末端，但在摇臂平台的步态运动中，只有支撑相具有轮-地接触点，而摆动相不存在，故不能通过轮-地接触点直接描述摇臂运动情况。如此，将摇臂平台车轮的工作空间定义为摇臂末端可达的空间位置，以衡量车轮的工作范围。

当前分析连杆机构工作空间的方法主要有几何法、解析法与数值法三种方式。对于越台阶工况，各个关节均有特定的约束空间，几何法很难明确地表示出摇臂末端可达的空间位置，解析法的过程相对来说计算量很大，过程较为复杂。蒙特卡洛法是数值方法中的一种，实质是对关节变量通过均匀分布赋以一定数量的符合关节变化要求的随机量，并对各关节变量进行组合，利用连杆机构的正向运动学方程式计算出机构末端执行器端点的坐标值，应用更具有普遍性。

根据俯仰角空间变量Q_θ和摇臂关节空间变量

通过蒙特卡洛法估计平台越台阶过程中，车轮在车体坐标系下的工作空间。首先，在区间[0,1]中生成n个随机点Rand，然后根据θ的活动范围生成等差数列(θ)_m＝θ^d+(θ^u-θ^d)Rand，其中m表示n中第m个随机值。同理，对a_f、a_m、a_r赋予相同数目的随机值(a_f)_i、(a_m)_j、(a_b)_k。计算在(θ)_m和(a_f)_i、(a_m)_j、(a_b)_k时摇臂的末端位置，生成点云图。

三维空间中，在平台的初始姿态下描述以上计算得到的所有车轮的工作空间。

过渡状态摇臂工作空间计算

已知所有车轮的轮心高度，且轮心高度不完全一样的情况是平台越台阶的过渡状态。该状态下摇臂的关节角度决定了平台能否越障成功，故需要定义平台越台阶过渡状态下摇臂的工作空间，以衡量摇臂在该阶段的工作能力范围。

图10表示了平台越台阶要经历的2个过渡状态，分别为前轮越障结束和中轮越障结束时的状态，将其命名为前轮过渡状态和中轮过渡状态。

在已知每个车轮轮心高度的情况下，摇臂关节的可达位置将受到更多限制，由

表示的摇臂关节工作范围不再准确。摇臂工作空间以关节转角为度量，4.2.2节得到的轮工作空间无法直观表示摇臂的工作范围。通过车轮工作空间计算摇臂工作空间，需要在已知车轮高度的情况下进行逆运动学求解，如此，将间接获得摇臂的工作空间，但这与数值法直接求解工作域的原理相违背。

综上所述，需要采用新的求解方式，直接计算平台越台阶过渡状态下摇臂的工作空间。

将摇臂工作空间的计算过程进行简化，不考虑摇臂工作时车轮的纵向位移，将摇臂旋转关节的高度{z_A}_i作为摇臂工作的状态量之一。

列出摇臂关节高度与摇臂转角相关的正运动学方程，如式，。

使用蒙特卡洛法，根据摇臂关节空间变量

对a_f、a_m、a_r赋予数目相同的随机值，计算在角度(a_f)_i、(a_m)_j、(a_b)_k时摇臂关节的高度位置和俯仰角θ，应用车身俯仰角空间变量Q_θ的限制，排除

的计算结果，能够得到a_f、a_m、a_r的变化范围。

以台阶高度H＝400mm为例，按照上述方法计算得到的摇臂角度的变化范围。

摇臂平台越台阶姿态约束分析

稳定性约束

目前，静态稳定性判定方法主要有：重心投影法(CG projection Method)，静态稳定边界法(Static Stability Margin，SSM)，能量稳定边界方法(Energy StabilityMargin，ESM)，类似的还有纵向稳定边界法(Longitudinal Stability Margin，LSM)，偏转纵向稳定边界方法(Crab Longitudinal Stability Margin，CLSM)，规范化的能量稳定边界法(Normalized Energy Stability Margin，NESM)等。下面就常用重心投影法、静态稳定边界法和能量稳定边界法做简要描述。

重心投影法：无论何种结构形式的地面移动平台，当处于复杂环境中，按顺序连接行驶系统与地面接触点，从而组成一个封闭的区域，该区域称为地面移动平台的支撑区域，通过判断其重心投影是否在支撑区域中来判定是否稳定。

静态稳定边界法：将支撑区域和重心投影在水平面上，通过判断重心在对于静态稳定系统水平面上的投影距各边界距离的最小值来判定其稳定性。

能量稳定边界法：静态稳定边界法忽略了移动平台的高度和质量，而随着质心高度的增加或质量的减少，移动平台抵抗外界扰动的能力相应减弱，稳定性也会变差，为了弥补这一缺陷，ESM方法旨在测量移动平台沿支撑区域的任一边界发生倾覆所增加的最小势能。NESM将ESM标准化为与质量无关的量，是目前在非结构化路面且无惯性力和外部载荷的影响下最准确的分析方法之一。

使用NESM方法判定摇臂平台稳定性，定义摇臂平台越障运动的稳定裕度S_NEf、S_NEb，即在全局坐标系下，摇臂平台倾翻极限位置时的质心和当前位置质心的高度差。

当S_f或S_b等于0时，整车重心高度达到了平台绕支撑前边界或后边界旋转时的最大高度，是摇臂平台保持稳定的极限情况，故摇臂平台越台阶过程的稳定性约束条件如式，。

S_NEf≥0   (4-12)

S_NEb≥0   (4-13)

防碰撞约束

在越台阶过程中，如果平台与台阶产生碰撞，可能使平台陷入难以恢复的状态，导致自主控制失效，越障失败，故在越障算法设计中，需要考虑碰撞检测，避免摇臂和车身与台阶发生干涉。通过分析摇臂平台越台阶障碍过程，台阶坡顶线是最先与平台产生碰撞的区域，所以绕过台阶坡顶线是平台成功越障的必要条件。确保平台不与台阶产生碰撞的约束见图14。

在前轮越障阶段，以前摇臂关节、中间摇臂关节和台阶棱角为顶点定义一个三角形，代号为A。在后轮越障阶段，以中间摇臂关节、后摇臂关节和台阶棱角为顶点定义一个三角形，代号为B。三角形A、B的面积如式，。

由于越障过程中，台阶棱角必先与摆动相的轮-地接触点重合，故上式可以写作式，。

若台阶棱角与车身底面发生干涉，即台阶坡顶线在车体坐标系X_BO_BZ_B平面的投影与车身底边相交，三角形A、B的面积分别等于l₁h_d/2、l₂h_d/2。平台不与台阶产生碰撞的约束条件如式，。

摇臂平台越台阶姿态规划

最小运动代价准则

对摇臂平台越台阶的全过程进行分析，假设在姿态变换阶段，所有车轮均为从动轮，则该阶段只有摇臂电机处于驱动状态，又因为摇臂平台越台阶动作被简化为准静态越障过程，所以在摇臂末端目标高度不变的情况下，摇臂电机的能耗主要取决于摇臂平台质心高度的变化和摇臂关节角度的变化。在前进阶段，如果忽略平台在相同支撑相下采用不同姿态驱动车轮直线行驶耗能的差别，那么摇臂平台在越台阶过程的运动代价可以等价于，在姿态变换各阶段摇臂平台整车质心高度的变化量和各摇臂摆角的变化量。

最小运动代价包括以下四条准则：

(1)姿态变换各个阶段的起始姿态和终止姿态分别为s_si、s_ei(i＝1，2，3)，平台向前行驶后，姿态没有发生变化，s_s1与初始姿态s₀相同，s_ei-1与s_si相同，将s_ei命名为平台越台阶的关键姿态；

(2)当前关键姿态s_ei较上一关键姿态s_ei-1或初始姿态s₀的整车质心变化高度Δh_i(i＝1，2，3)最小；

(3)在s_ei状态下，已知所有车轮轮心高度z_wi，整车质心高度H_COM主要取决于车身质心高度z₀，假设当车身质心达到某一高度z_0i时，无论姿态如何变化，Δh_i保持最小；

(4)当前关键姿态s_ei较上一关键姿态s_ei-1或初始姿态u₀的摇臂转角变化Δα_fi、Δα_mi、Δα_bi(i＝1，2，3)最小。

按照上述方法，按最小运动代价准则规划摇臂平台姿态，可以看作对平台关键姿态s_ei进行优化。

关键姿态优化过程

建立优化整车质心变化高度的目标函数。

minJ_i＝Δh_i   (4-20)

计算整车质心在世界坐标系下的高度。

计算各个车轮轮心在世界坐标系下的高度。

在s_ei状态下，已知车轮轮心高度{z_w}_i，结合车身俯仰角空间变量Q_θ，摇臂工作空间，即摇臂关节{α}_i的变化范围，以及姿态约束，对Δh_i进行优化，取优化后的车身质心高度z_0i。

建立优化摇臂关节角度变化的目标函数。

minT_i＝w₁Δα_fi+w₂Δα_mi+w₃Δα_bi   (4-23)

式中，T_i为优化的目标函数，第一项w₁最小化前摇臂摆角变化，第二项w₂最小化中间摇臂摆角变化，第三项w₃最小化后摇臂摆角变化。

式表示各个车轮轮心在世界坐标系下的纵向位置。

已知z_0i和{z_w}_i，通过车轮工作空间可以计算得到车轮在x方向的可达位置，即{x_w}_i的变化范围，结合车身俯仰角空间变量Q_θ，摇臂工作空间，即摇臂关节{α}_i的变化范围，以及姿态约束，对{Δα}_i进行优化，获得该阶段运动代价最小的关键姿态s_ei，以及对应的姿态参数{θ,a_f,a_m,a_r}。

确定了越台阶过程的关键姿态，平台就可以根据预定义的越台阶动作序列，自主完成越台阶任务。

以上所述，仅为本申请较佳的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应该以权利要求的保护范围为准。

Claims (7)

1.一种摇臂式机动平台自主越台阶障碍方法，其特征在于，包括以下步骤：

构建用于跨越台阶障碍的摇臂式机动平台；

对所述摇臂式机动平台安装二维激光雷达；

基于所述激光雷达采集障碍物的高度、坡度、位置信息，进行台阶障碍特征检测与越障过程中的参考点检测，并在越障过程中，实时更新机体与障碍物参考点的距离，补全上一阶段传感器盲区位置的环境信息；

设计所述摇臂式机动平台的越障运动步态；

对所述摇臂式机动平台进行跨越障碍的状态分析，获取状态分析结果；

对所述摇臂式机动平台进行跨越障碍的状态分析的过程包括：

对所述摇臂式机动平台进行质心位置估计；

计算越台阶过程中的车轮工作空间与过渡状态的摇臂工作空间；

分析越台阶过程中的稳定性约束与防碰撞约束；

基于最小运动代价准则对摇臂式机动平台越台阶的全过程进行分析，优化越台阶过程中的关键姿态；

计算越台阶过程中的车轮工作空间与过渡状态的摇臂工作空间的过程包括：

基于所述激光雷达获取的俯仰角空间变量和摇臂关节空间变量，采用蒙特卡洛法估计平台越台阶过程中，车轮在车体坐标系下的工作空间；

基于蒙特卡洛法，计算摇臂关节在不同角度的高度位置和俯仰角，获取摇臂角度的变化范围，基于所述变化范围计算过渡状态的摇臂工作空间；

将摇臂工作空间的计算过程进行简化，不考虑摇臂工作时车轮的纵向位移，将摇臂旋转关节的高度{z_A}_i作为摇臂工作的状态量之一；

列出摇臂关节高度与摇臂转角相关的正运动学方程，如式，

使用蒙特卡洛法，根据摇臂关节空间变量

对a_f、a_m、a_r赋予数目相同的随机值，计算在角度(a_f)_i、(a_m)_j、(a_b)_k时摇臂关节的高度位置和俯仰角θ，应用车身俯仰角空间变量Q_θ的限制，排除

的计算结果，得到a_f、a_m、a_r的变化范围；

优化越台阶过程中的关键姿态的过程包括：建立优化整车质心变化高度的目标函数；计算整车质心在世界坐标系下的高度；计算各个车轮轮心在世界坐标系下的高度；建立优化摇臂关节角度变化的目标函数；基于车轮工作空间计算车轮在跨越方向的可达位置；基于俯仰角空间变量、摇臂工作空间以及稳定性约束与防碰撞约束获取运动代价最小的关键姿态以及姿态参数，进行越台阶过程中关键姿态的优化；

关键姿态优化过程：

建立优化整车质心变化高度的目标函数；

minJ_i＝Δh_i

计算整车质心在世界坐标系下的高度；

计算各个车轮轮心在世界坐标系下的高度；

在s_ei状态下，已知车轮轮心高度{z_w}_i，结合车身俯仰角空间变量Q_θ，摇臂工作空间，即摇臂关节{α}_i的变化范围，以及姿态约束，对Δh_i进行优化，取优化后的车身质心高度z_0i；

建立优化摇臂关节角度变化的目标函数；

minT_i＝w₁Δα_fi+w₂Δα_mi+w₃Δα_bi      (4-23)

式中，T_i为优化的目标函数，第一项w₁最小化前摇臂摆角变化，第二项w₂最小化中间摇臂摆角变化，第三项w₃最小化后摇臂摆角变化；

式表示各个车轮轮心在世界坐标系下的纵向位置；

已知z_0i和{z_w}_i，通过车轮工作空间可以计算得到车轮在x方向的可达位置，即{x_w}_i的变化范围，结合车身俯仰角空间变量Q_θ，摇臂工作空间，即摇臂关节{α}_i的变化范围，以及姿态约束，对{Δα}_i进行优化，获得该阶段运动代价最小的关键姿态s_ei，以及对应的姿态参数{θ,a_f,a_m,a_r}；

基于所述越障运动步态与所述状态分析结果进行障碍跨越。

2.根据权利要求1所述的摇臂式机动平台自主越台阶障碍方法，其特征在于，对所述摇臂式机动平台设置激光雷达的过程包括：

将激光雷达安装于摇臂机动平台车体侧面，扫描平面与车身侧表面平行，并使激光雷达的扫面平面夹在摇臂与车身之间。

3.根据权利要求1所述的摇臂式机动平台自主越台阶障碍方法，其特征在于，设计所述摇臂式机动平台的越障运动步态的过程包括：

构建越障步态规则与越障步态模型；

基于所述越障步态规则与所述越障步态模型进行越台阶过程的摇臂状态切换；

基于所述摇臂状态切换构建越台阶障碍动作序列。

4.根据权利要求3所述的摇臂式机动平台自主越台阶障碍方法，其特征在于，所述越障步态规则包括：

每个摇臂包括三种状态：向前、腾空和向后；所述向前表示车轮接触地面，摇臂较初始位置向前旋转；所述腾空表示使用摇臂抬起车轮以允许车轮通过障碍物的状态；所述向后状态表示车轮接触地面，摇臂较初始位置向后旋转；

每个悬架的车轮只有在摇臂腾空状态下才可以通过障碍物；

每个摇臂在越障前后依次以向前、腾空、向后的顺序改变状态；最前方摇臂的状态按向前、腾空、向前的顺序变化，位于最后方摇臂的状态按向后、腾空、向后的顺序变化。

5.根据权利要求3所述的摇臂式机动平台自主越台阶障碍方法，其特征在于，所述越台阶障碍动作序列包括：

前轮越障阶段：前摇臂由支撑相转变为摆动相，改变整车质心位置，扩大支撑边界，前轮抬升至台阶高度，驱动支撑相车轮控制车辆向前行驶，直至前轮接触台阶上平台，所有摇臂均为支撑相，前轮越障结束；

所述支撑相包括处于向前状态和向后状态的摇臂，所述摆动相包括处于腾空状态的摇臂；

中轮越障阶段：中摇臂由支撑相转变为摆动相，中摇臂在腾空状态下朝远离地面方向转动，使达到中间车轮达到台阶高度，同时，支撑相抬高整车质心直至超过台阶高度，中间车轮达到台阶高度，支撑相车轮驱动整车向前行驶，直至中间车轮接触台阶上平台且车身不与台阶发生干涉，中间摇臂转换为向后状态，所有摇臂均为支撑相，中轮越障结束；

后轮越障阶段：后摇臂由支撑相转变为摆动相，剩余支撑相调整车身姿态，扩大支撑边界，车身后端抬高，直至俯仰角为0；摆动相调整后车轮高度，所有车轮处于同一水平高度，支撑相车轮驱动整车向前行驶，直至后轮接触台阶上平台，所有摇臂均为支撑相，后轮越障结束；

支撑相调整车身姿态，使整车恢复至初始状态。

6.根据权利要求1所述的摇臂式机动平台自主越台阶障碍方法，其特征在于，对所述摇臂式机动平台进行质心位置估计的过程包括：

基于摇臂平台的空载质量获取摇臂平台的整体质心在车体坐标系中的位置；

构建世界坐标系，设定世界坐标系原点位于所述车体坐标系的正下方；

测量摇臂平台自身的姿态角与各个摇臂关节的转动角度；

基于所述姿态角、所述转动角度与所述世界坐标系获取所述摇臂式机动平台的质心位置。

7.根据权利要求1所述的摇臂式机动平台自主越台阶障碍方法，其特征在于，分析越台阶过程中的稳定性约束与防碰撞约束的过程包括：

所述稳定性约束包括：基于重心投影法、静态稳定边界法、能量稳定边界法判定摇臂平台稳定性，构建稳定性约束条件；

所述防碰撞约束包括：在前轮越障阶段，以前摇臂关节、中间摇臂关节和台阶棱角为顶点定义一个三角形A；在中轮越障阶段，以前摇臂关节、后摇臂关节和台阶棱角为顶点定义一个三角形B，分别计算所述三角型A与所述三角型B的面积；若满足：

则摇臂平台不与台阶产生碰撞。

Images