智能搜救机器人在障碍地形的自主构型规划

陈柏良，黄开宏，潘海南，肖军浩，吴文启，卢惠民; CHEN Bailiang; HUANG Kaihong; PAN Hainan; XIAO Junhao; WU Wenqi; LU Huimin

智能搜救机器人在障碍地形的自主构型规划^*

doi: 10.11887/j.cn.202306015

陈柏良，黄开宏，潘海南，肖军浩，吴文启，卢惠民

国防科技大学智能科学学院, 湖南长沙 410073

基金项目: 国家自然科学基金资助项目（U1913202，U22A2059，62203460）；湖南省自然科学基金资助项目（2021JC0004）

详细信息

作者简介

陈柏良（1994—），男，湖南长沙人，博士研究生，E-mail: chenbailiang12@nudt.edu.cn；

通讯作者

卢惠民，男，福建南平人，教授，博士，博士生导师，E-mail: lhmnew@nudt.edu.cn

中图分类号: TP242.6

文献标识码: A

文章编号: 1001-2486(2023)06-132-11

Autonomous configuration planning for intelligent search and rescue robots in rough terrains

CHEN Bailiang ， HUANG Kaihong ， PAN Hainan ， XIAO Junhao ， WU Wenqi ， LU Huimin

College of Intelligence Science and Technology, National University of Defense Technology, Changsha 410073 , China

摘要

为了解决带有辅助摆臂的智能搜救机器人自动规划构型以实现自主越障的难题，提出一种能够适应复杂地面形状的搜救机器人越障构型规划新方法，其核心是一种高适应性、高效率的机器人姿态预测算法。通过将地形表示为离散的点集，建立了搜救机器人的单侧姿态预测数学模型；进一步提出了快速求解该问题的算法，每秒可预测1000~1500个姿态。基于此，设计了机器人越障过程中状态、动作的评价指标，运用动态规划算法与滚动优化思想构建了具有优化能力的、能够实时运行的构型规划器。仿真与实物实验的结果表明，该方法能够使机器人自主调整构型穿越复杂地形，且相较强化学习算法和人工操作具有更平稳的越障效果。

关键词

搜救机器人 / 障碍地形 / 姿态预测 / 动作规划

Abstract

In order to solve the configuration planning problem for intelligent search and rescue robots with assisted flippers to achieve autonomous obstacle crossing, a novel method for planning robot configuration during obstacle crossing was proposed that can be applied to complex terrains. The core of the proposed method is an adaptable and efficient robot pose prediction algorithm. By representing the terrain as a series of discrete point sets, a mathematical model for predicting the one-sided pose of the tracked robot was established; further, a fast solver for this model was proposed, which can predict 1000~1500 poses per second. Based on this, the evaluation metrics of the robot′s state and action in the obstacle-crossing process were established, and an optimization-based real-time flippers action planner was realized by using the dynamic programming algorithm and rolling optimization. The simulation and real-robot experiments show that the proposed approach enables the robot to control the flippers to cross rough terrains autonomously. It performs more smoothly than the reinforcement-learning method and manual operation when crossing obstacles.

Keywords

search and rescue robots / rough terrains / pose prediction / motion planning

1 姿态预测 1.1 机器人与地形建模 1.2 姿态预测问题定义 1.3 姿态预测问题的数学描述 1.3.1 机器人模型的数学描述 1.3.2 接触关系的数学描述 1.4 姿态预测问题的解析求解方法 2 构型规划 2.1 机器人状态与动作评价 2.2 动态规划 2.3 基于滚动优化的构型序列更新 2.4 非对称地形构型规划策略 3 实验与讨论 3.1 实验设置与对比指标 3.1.1 参数设置 3.1.2 对比指标 3.2 仿真实验 3.2.1 台阶场景 3.2.2 陡峭楼梯场景 3.3 实物实验 3.3.1 单侧台阶场景 3.3.2 斜向楼梯场景 3.3.3 废墟场景 4 结论

智能搜救机器人是一种可变构型机器人，其利用摆臂结构或子履带结构调整自身构型并提高越障能力，更好地适应城市废墟、户外林地等障碍地形，如图1所示。其代价是操作自由度较多（6个），如果完全依赖远程人工遥控，会给操作人员带来很大的认知负担，容易导致控制失误^[1]。因此，越障过程中机器人构型的自主规划是当前一个急需解决的问题。

根据实现方法，现有的构型规划算法主要分为三类：底盘姿态控制法、简化模型分析法和机器学习法。

图1NuBot-Rescue 四摆臂履带机器人

Fig.1The NuBot-Rescue tracked robot with four flippers

底盘姿态控制法是根据地形起伏给出期望的底盘姿态（包括底盘俯仰角、摆臂角度等）并以此为摆臂控制目标。文献^[2]根据机器人三维全局路径的起伏曲率来调整摆臂角度，使底盘与摆臂的整体构型接近路径的包络线。底盘姿态控制法可以实现基本的越障功能，但仍需要专门的越障构型来应对较为复杂的障碍，而且这些方法较少考虑越障动作的优化问题，存在局限性。

简化模型分析法是针对摆臂履带机器人与特定地形的接触过程进行运动学与动力学分析^[3]，得到最优的越障过程。特定的地形通常指单个台阶^[4-5]、楼梯^[6]等结构化场景。文献^[4]中建立了机器人翻越单个台阶过程的运动学和动力学模型，并总结出使摆臂力矩最小的控制规律。基于简化模型的方法分析详尽，但不能应对复杂、不规则、非结构化的地形等情况。

机器学习法既考虑了越障动作优化问题，也对不规则的地形具有一定适应性。其主要采用深度学习或强化学习方法获得机器人在当前状态下越障效果最好的机器人期望姿态或摆臂动作^[7-10]。文献^[9-10]采用强化学习方法，以地形起伏信息和机器人姿态等为状态量，以危险姿态、颠簸程度等指标为惩罚项，训练出能够自主上下楼梯的摆臂控制策略。文献^[8]将模仿学习与强化学习相结合，使机器人面对多种不同障碍时能够快速地训练出适应性较好的越障策略。然而，此类方法在未训练过的地形以及极易倾覆的高难度地形中的适应性尚未得到充分验证。

上述三种方法的共同特点都是针对越障过程中的机器人构型和姿态进行分析处理。机器人通过调整摆臂角度改变自身构型，从而控制自身姿态以翻越高难度的障碍，并减轻越障过程的颠簸。因此，机器人在越障时的构型变化取决于机器人当前的姿态以及在即将到达的地形上所期望的姿态。如果能够提前预知机器人在即将到达的地形上的状态，机器人就可以有效地规划越障构型。目前主要有仿真分析法和迭代几何法实现了履带机器人的姿态预测。前者主要利用物理仿真引擎，得到机器人在稳定状态下的仿真预测值^[11]。其适用于任意地形和机器人外形，但计算量大、实时性差。迭代几何法^[12]则是根据机器人以及地形的起伏形状，迭代寻找接触点以确定姿态。这种方法能够在复杂地形中预测姿态，但是现有研究并未考虑摆臂采用多种构型时的姿态预测问题。

因此，本文针对传统方法无法兼顾优化越障过程和适应复杂障碍的问题，以及强化学习方法训练过程不确定性强、高难度地形适应性存疑的不足，结合姿态预测对复杂地形的高适应性和动态规划算法寻找最优解的特点，提出一种既能应对复杂地形又具有优化能力的搜救机器人自主构型规划方法，该方法在仿真与实物实验中均展现出了优异的自动控制效果。

1 姿态预测

1.1 机器人与地形建模

研究对象为如图1所示的智能搜救机器人，其在一般履带底盘的基础上增加了四个可以独立转动的子履带（称为摆臂）。这种机器人的左侧与右侧结构是相同的，在建模时可以将机器人左侧与右侧分别使用同样的方法分析。当滚动角较小时，机器人模型可以简化为图2（a）所示的单侧履带模型：侧向投影机器人的左侧或右侧得到三段式模型，中间段为底盘，左右两段分别为后摆臂与前摆臂。在侧向投影中，当摆臂具有与主履带大致相同的厚度时，可以将地面向外膨胀履带的弯曲半径^[5]，得到进一步简化的机器人无厚度线段模型，如图2（b）所示。

图2单侧履带机器人模型简化过程与符号定义

Fig.2The tracked robot′s single-side pose modelling and notation definition

机器人下方的地形采用离散的点集表示，如图2（b）中的地形点。通过机载激光雷达或深度相机可以感知局部地形并结合建图算法^[13-14]获得原始点云；再将点云膨胀并重新采样得到膨胀后的地形点集，记为集合T，其中的点记为p∈T。

1.2 姿态预测问题定义

机器人在地形中的姿态，是由机器人自身构型以及与地形的接触点决定的。设地面参考系为

，姿态预测问题可以描述为下列形式。

给定：

1）T：地形的点集。

2）x_R：底盘中心在

中的x轴坐标。

3）θ₁：机器人前摆臂角度，逆时针为正。

4）θ₂：机器人后摆臂角度，顺时针为正。

求机器人在静态稳定状态下的：

1）y_R：底盘中心在

中的y轴坐标。

2）θ_R：机器人在

中的俯仰角，逆时针为正。

约束条件：

1）机器人质心的前后两侧各存在一个以上与地形的接触点；

2）由接触点确定的机器人姿态应当使机器人位于其范围内所有地形点的上方。

图2（b）展示了上述参数的具体意义。因此，求解姿态就是从点集T中找出满足约束条件的地形点即接触点，进而确定姿态参数。这里预测的是机器人在“稳定状态”下的姿态，基于此可以提出两个约束条件。条件1是机器人在地面上保持稳定的基本条件；条件2代表机器人的任一部位都不能与地面出现碰撞干涉的基本物理约束。

1.3 姿态预测问题的数学描述

1.3.1 机器人模型的数学描述

以

中的待预测位置（x_R，0）为原点建立一个y轴与重力方向平行、x轴方向为机器人朝向的局部坐标系

。令机器人的底盘中心位置与原点重合，底盘与x轴平行，可以得到如图3（a）所示的模型。该模型可以用分段直线描述各部件：

(1)

其中：前摆臂末端点r₁，坐标

(\frac{1}{2} B + F c o s θ_{1} ， F s i n θ_{1})

；前摆臂转轴点r₂，坐标

(\frac{1}{2} B ， 0)

；后摆臂转轴点r₃，坐标

(- \frac{1}{2} B ， 0)

；后摆臂末端点r₄，坐标

(- \frac{1}{2} B - F c o s θ_{2} ， F s i n θ_{2})

； B为底盘的长度，F为摆臂的长度。

1.3.2 接触关系的数学描述

地形点与机器人接触可以表示为点在分段直线模型上，即机器人

在

中首先绕原点旋转俯仰角φ，再沿y轴平移h变换为

′，并与地形点

p_{c 1} (x_{c 1} ， y_{c 1}) \in T 、 p_{c 2} (x_{c 2} ， y_{c 2}) \in T

接触，如图3（b）所示。

图3机器人模型以及机器人姿态与接触点的关系

Fig.3Robot model and the relationship between robot pose and contact points

为满足约束条件1，地形点需分别位于机器人质心前、后侧。点在直线上的几何关系可以用直线的齐次坐标向量与点的齐次坐标向量的点积为零表示。而p_c1、p_c2既可能位于摆臂上，也可能位于底盘上，因此对于p_c1，有式（2）成立：

\{\begin{matrix} l_{r_{2}^{'} r_{1}^{'}}^{T} \cdot p_{c 1} = 0 p_{c 1} 位 于 前 摆 臂 上, x_{c 1} \in (x_{r_{2}^{'}}, x_{r_{1}^{'}}] \\ l_{r_{3}^{'} r_{2}^{'}}^{T} \cdot p_{c 1} = 0 p_{c 1} 位 于 底 盘 上, x_{c 1} \in (x_{c o m}, x_{r_{2}^{'}}] \end{matrix}

(2)

对于p_c2，有式（3）成立：

\{\begin{matrix} l_{r_{3}^{'} r_{2}^{'}}^{T} \cdot p_{c 2} = 0 p_{c 2} 位 于 底 盘 上, x_{c 2} \in [x_{r_{3}^{'}}, x_{c o m}) \\ l_{r_{4}^{'} r_{3}^{'}}^{T} \cdot p_{c 2} = 0 p_{c 2} 位 于 后 摆 臂 上, x_{c 2} \in [x_{r_{4}^{'}}, x_{r_{3}^{'}}) \end{matrix}

(3)

其中：

l_{r_{i + 1}^{'} r_{i}^{'}}

（i=1，2，3）为以齐次坐标向量形式表示的变换后的机器人模型

′，其含义为经过r′_i₊₁、r′两点的直线；p_cj（j=1，2）为以齐次坐标向量形式表示的接触点，如式（4）所示。

\{\begin{matrix} l_{r_{i + 1}^{'} r_{i}^{'}} = [\begin{matrix} y_{r_{i + 1}^{'}} - y_{r_{i}^{'}} \\ - (x_{r_{i + 1}^{'}} - x_{r_{i}^{'}}) \\ x_{r_{i + 1}^{'}} y_{r_{i}^{'}} - x_{r_{i}^{'}} y_{r_{i + 1}^{'}} \end{matrix}] \\ p_{c j} = [\begin{matrix} x_{c j} \\ y_{c j} \\ 1 \end{matrix}] \end{matrix}

(4)

式中，（

x_{r_{i}^{'}}

，

y_{r_{i}^{'}}

）为r′_i的坐标，是图3（a）中初始状态下的r_i经过旋转和平移变换后得到的，如式（5）：

[\begin{matrix} x_{r_{i}^{'}} (h, ϕ) \\ y_{r_{i}^{'}} (h, ϕ) \end{matrix}] = [\begin{matrix} c o s ϕ & - s i n ϕ \\ s i n ϕ & c o s ϕ \end{matrix}] [\begin{matrix} x_{r_{i}} \\ y_{r_{i}} \end{matrix}] + [\begin{matrix} 0 \\ h \end{matrix}]

(5)

在约束条件1中，接触点还需要满足分布在质心前后的要求，即式（2）、式（3）中接触点的x轴坐标取值范围要远离质心。机器人模型质心在

中的坐标（x_com，y_com）可以表示为式（6）：

\begin{matrix} \{\begin{matrix} x_{com} = \frac{1}{m_{B} + 2 m_{F}} [m_{F} l_{f} (c o s θ_{1} - c o s θ_{2}) + m_{B} x_{b}] \\ y_{com} = \frac{1}{m_{B} + 2 m_{F}} [m_{F} l_{f} (s i n θ_{1} + s i n θ_{2}) + m_{B} y_{b}] \end{matrix} \\ \Rightarrow {(x_{com} - \frac{m_{B} x_{b}}{m_{B} + 2 m_{F}})}^{2} + {(y_{com} - \frac{m_{B} y_{b}}{m_{B} + 2 m_{F}})}^{2} \\ = 2 [1 - c o s (θ_{1} + θ_{2})] {(\frac{m_{F} l_{f}}{m_{B} + 2 m_{F}})}^{2} \\ ⩽ {(\frac{2 m_{F} l_{f}}{m_{B} + 2 m_{F}})}^{2} \end{matrix}

(6)

其中，（x_b，y_b）是底盘质心的坐标，lf是摆臂质心到摆臂转轴的距离，m_B和m_F是机器人单侧底盘以及单个摆臂的质量。假设底盘质心与底盘形状中心重合即（x_b，y_b）=（0，0），则有式（7）：

x_{com} \in [- \frac{2 m_{F} l_{f}}{m_{B} + 2 m_{F}}, \frac{2 m_{F} l_{f}}{m_{B} + 2 m_{F}}]

(7)

在实际中，m_B通常比m_F大8~10倍，因此（x_com，y_com）只会在半径约

\frac{1}{5} l_{f}

的圆内变化，只需使接触点不在该范围内即可满足稳定性要求。因此，可以初步将所有地形点在

下以x_com的上下边界为界分成T₁、T₂前后两个子集，并设定候选接触点分别位于其中，即：p_c1∈T₁，p_c2∈T₂。

与此同时，所有位于变换之后机器人范围内的地形点p还需要满足约束条件2的要求。可以用

l_{r_{i + 1}^{'} r_{i}^{'}}

（i=1，2，3）与p的点积正负表示点相对直线的位置，因此该条件可以转化为式（8）：

\{\begin{matrix} l_{r_{2}^{'} r_{1}^{'}}^{T} \cdot p ⩽ 0 x_{p} \in (x_{r_{2}^{'}}, x_{r_{1}^{'}}] \\ l_{r_{3}^{'} r_{2}^{'}}^{T} \cdot p ⩽ 0 x_{p} \in [x_{r_{3}^{'}}, x_{r_{2}^{'}}], p (x_{p}, y_{p}) \in T \\ l_{r_{4}^{'} r_{3}^{'}}^{T} \cdot p ⩽ 0 x_{p} \in [x_{r_{4}^{'}}, x_{r_{3}^{'}}) \end{matrix}

(8)

需要注意的是，变换后的机器人模型摆臂末端点r′₁、r′₄有可能位于底盘下方即

x_{r_{2}^{'}}

＞

x_{r_{1}^{'}}

或

x_{r_{4}^{'}}

＞

x_{r_{3}^{'}}

，此时式（8）中的符号取反。

最终满足所有条件的候选接触点p_c1、p_c2所对应的（h，φ）即为机器人在当前位置的预测姿态参数：（y_R，θ_R）=（h，φ）。

联立式（2）~（5）、式（8）可以得到如下形式的姿态预测问题的数学描述：

求解：

h ， ϕ ， p_{c 1} (x_{c 1} ， y_{c 1}) \in T_{1} ， p_{c 2} (x_{c 2} ， y_{c 2}) \in T_{2}

条件：

\{\begin{matrix} l_{r_{2}^{'} r_{1}^{'}}^{T} \cdot p_{c 1} = 0 p_{c 1} 位 于 前 摆 臂 上, x_{c 1} \in (x_{r_{2}^{'}}, x_{r_{1}^{'}}] \\ l_{r_{3}^{'} r_{2}^{'}}^{T} \cdot p_{c 1} = 0 p_{c 1} 位 于 底 盘 上, x_{c 1} \in (\frac{2 m_{F} l_{f}}{m_{B} + 2 m_{F}}, x_{r_{2}^{'}}] \end{matrix}

(9)

\{\begin{matrix} l_{r_{3}^{'} r_{2}^{'}}^{T} \cdot p_{c 2} = 0 p_{c 2} 位 于 底 盘 上, x_{c 2} \in (x_{r_{3}^{'}}, - \frac{2 m_{F} l_{f}}{m_{B} + 2 m_{F}}] \\ l_{r_{4}^{'} r_{3}^{'}}^{T} \cdot p_{c 2} = 0 p_{c 2} 位 于 后 摆 臂 上, x_{c 2} \in [x_{r_{4}^{'}}, x_{r_{3}^{'}}) \end{matrix}

(10)

\{\begin{matrix} l_{r_{2}^{'} r_{1}^{'}}^{T} \cdot p ⩽ 0 x_{p} \in (x_{r_{2}^{'}}, x_{r_{1}^{'}}] \\ l_{r_{3}^{'} r_{2}^{'}}^{T} \cdot p ⩽ 0 x_{p} \in [x_{r_{3}^{'}}, x_{r_{2}^{'}}], p (x_{p}, y_{p}) \in T \\ l_{r_{4}^{'} r_{3}^{'}}^{T} \cdot p ⩽ 0 x_{p} \in [x_{r_{4}^{'}}, x_{r_{3}^{'}}) \end{matrix}

(11)

\begin{matrix} [\begin{matrix} x_{r_{i}^{'}} \\ y_{r_{i}^{'}} \end{matrix}] = [\begin{matrix} c o s ϕ & - s i n ϕ \\ s i n ϕ & c o s ϕ \end{matrix}] [\begin{matrix} x_{r_{i}} \\ y_{r_{i}} \end{matrix}] + [\begin{matrix} 0 \\ h \end{matrix}] \\ i = 1,2, 3,4 \end{matrix}

(12)

该问题并不总是有可行解，而无效解对应的机器人构型与地形位置，将在后续的构型规划中被视为危险状态进行规避。

1.4 姿态预测问题的解析求解方法

至此，姿态预测问题的数学模型已经建立，实际运用时需要高效的求解算法。当地形点总数量有限时，可以利用式（2）、式（3）组成的等式方程组快速地遍历求解局部地形点集中所有候选接触点对（p_c1，p_c2）对应的机器人姿态解析解，并根据区间范围条件、不等式条件筛选得到满足所有约束条件的姿态。基于这种思路，设计了一种可以适用于复杂地形的履带机器人姿态预测方法。

首先从地形点集T₁、T₂中选取一对候选接触点（p_c1，p_c2），由于接触点既可能位于摆臂上也可能位于底盘上，因此式（2）、式（3）可以根据下列情况各自提出一个等式组成待求解的等式方程组：

1）p_c1在前摆臂上，p_c2在后摆臂上；

2）p_c1在前摆臂上，p_c2在底盘上；

3）p_c1在底盘上，p_c2在后摆臂上；

4）p_c1在底盘上，p_c2在底盘上。

这些方程组的基本形式是一致的，接下来以第二种情况为例，其他三种情况将对应参数替换即可。将式（4）、式（5）代入由式（2）第一行等式、式（3）第一行等式组成的方程组，令z=tan（φ/2），则sinφ=2z/（1+z²），cosφ=（1-z²）/（1+z²）。整理得到关于h和z的方程组如式（13）：

\{\begin{matrix} (A_{1} - X_{1} h) z^{2} + (B_{1} - 2 Y_{1} h) z + (C_{1} + X_{1} h) = 0 \\ (A_{2} - X_{2} h) z^{2} + (B_{2} - 2 Y_{2} h) z + (C_{2} + X_{1} h) = 0 \end{matrix}

(13)

其中的系数A_i、B_i、C_i、X_i、Y_i（i=1，2）为只与机器人模型的原坐标

(x_{r_{i}} ， y_{r_{i}})

、候选接触点坐标（x_c_i，y_c_i）有关的常数。将该方程组视为关于z的多项式，如果z的解存在，则这两个多项式之间必然存在公因式，即两者的结式为零。假设式（13）的最高项系数（A_i-X_ih）均不为零，展开结式可以得到关于h的四次方程，利用多项式求根算法可以求出h的解；进一步利用二次方程求根公式与反正切函数得到h对应的φ。采用相同的方法，可以求出其他三种情况下的解。根据接触点x轴坐标的范围条件以及等式约束条件，可从这四种情况中找到一组满足接触点条件的姿态参数（h，φ）。

基于上述解析求解方法，在给定一对候选接触点坐标后，可以计算出满足约束条件1的姿态参数。在此之后，根据式（8）判断是否满足约束条件2。如果不满足，则重新从T₁、T₂中选取一对候选接触点，最终找到机器人在给定地形上采取指定前后摆臂角度（构型）时的单侧机器人姿态。

姿态预测算法的整体流程如图4所示。当T₁、T₂中地形点总数为30~50时，指定x_R以及θ₁、θ₂时的单个姿态的平均求解时间约为0.75 ms，即每秒预测1 000~1 500个姿态，能够满足实时进行构型规划的需求；采用点集的形式描述地面，使算法能够应对各种上下起伏不规则的地形。

图4姿态预测算法框图

Fig.4Framework of pose prediction algorithm

2 构型规划

基于快速的姿态预测算法，进一步设计越障构型规划算法。算法以传统的动态规划方法为基础，寻找局部感知范围内的最优越障构型序列；结合滚动优化的思想设计不断更新构型序列的规划器，实现机器人越障整体过程的优化。

2.1 机器人状态与动作评价

动态规划算法需要首先对越障过程中的机器人状态以及状态之间的转移过程进行合理的评价。

如图5所示，左侧为地形与机器人在当前位置

P_{1}^{i}

的状态，地形中红色的点表示当前时刻用于姿态预测的局部地形点。姿态预测算法以

P_{1}^{i}

为起点沿前进方向设定路径点

P_{2}^{i} 、 P_{3}^{i} ， P_{4}^{i}

；在每个路径点处按照预设的机器人构型（θ₁，θ₂）进行姿态预测，即可得到由路径点位置、前后摆臂角度、机器人俯仰角、离地高度等元素构成的离散状态空间。机器人在指定位置与摆臂角度时的姿态定义为离散的状态

S_{P ， θ_{1} ， θ_{2}} = (y_{R} (P ， θ_{1} ， θ_{2}) ， θ_{R} (P ， θ_{1} ， θ_{2}))

，其中P代表路径点。前、后摆臂在相邻路径点之间变化的角度分别记为a₁、a₂，范围为{-bk，···，0，···，bk}，

b \in Z

，又称摆臂动作；其中k为摆臂角度离散化分辨率，b为每当机器人前进一个路径点时摆臂角度最多可以变化的单位分辨率个数。

图5机器人下壕沟过程的构型规划示意图

Fig.5Diagram of robot configuration planning when going down the trench

根据人类操作员的经验，设计了代价函数

来评价在状态

S_{P ， θ_{1} ， θ_{2}}

时采取动作（a₁，a₂）的代价：

(14)

其中，

_θ

(S_{P ， θ_{1} ， θ_{2}} ， S_{P + 1 ， θ_{1} + a_{1} ， θ_{2} + a_{2}})

为前后两个状态间俯仰角变化的代价，

_y（

S_{P ， θ_{1} ， θ_{2}}

）为当前状态下质心高度与底盘范围内地面平均高度的偏差的代价，

_s（

S_{P ， θ_{1} ， θ_{2}}

）为当前状态下俯仰方向稳定性的代价，

_t（

S_{P ， θ_{1} ， θ_{2}}

）为当前状态下摆臂力矩的代价，

_r（a₁，a₂）为摆臂调整幅度的代价。

利用式（14）对离散状态空间中所有的状态与动作进行评价，作为构型规划的依据。

2.2 动态规划

在获得每个路径点处机器人各种构型时的状态以及采取各种动作时的代价值后，运用动态规划算法寻找机器人状态空间中连接起始状态和目标状态的最优状态转移路径，使得式（15）的累计代价值最小。

(15)

首先指定机器人的起始状态（如图5中位置

P_{1}^{i}

和此时机器人的构型），以及期望的目标点位置

P_{4}^{i}

；随后计算从起始状态开始每一次状态转移的累计最小代价

_all，依次迭代直到

P_{4}^{i}

，得到如图5右侧所示的各路径点处的相对累计代价图

^′_all（减去了当前路径点中最小的

_all，对应图中蓝点位置的状态）。选取

P_{4}^{i}

位置

_all最小的状态作为目标状态，并回溯累计代价最低的状态转移过程，得到最优的越障构型变换序列，即图5中蓝点的连线。

2.3 基于滚动优化的构型序列更新

机器人的感知范围有限导致构型规划的距离有限，因此运用滚动优化的方式在前进的同时不断更新构型序列，如图5中的3个阶段所示。

机器人在当前位置

P_{1}^{i}

采集相应信息，预测前方

P_{2}^{i} 、 P_{3}^{i} ， P_{4}^{i}

路径点处的姿态并进行构型规划。随着机器人前进，摆臂执行规划的动作并到达

P_{3}^{i}

路径点。此时机器人被视为到达了新的位置

P_{1}^{i + 1}

，根据当前的状态对路径点

P_{2}^{i +}

、

P_{3}^{i + 1}

、

P_{4}^{i + 1}

进行新一轮姿态预测与规划，得到更新后的构型序列并执行，如此循环。基于此，算法持续输出在机器人前方局部范围内的构型序列，通过不断滚动向前实现整体越障效果的优化。

2.4 非对称地形构型规划策略

左右非对称的地形会使机器人倾斜，导致姿态预测算法的基本条件“滚动角度较小”无法满足，同时也将影响滚转方向的稳定性，使机器人更容易侧翻。图6中机器人滚转方向的稳定性可以使用力角稳定性指标^[15]分析，如式（16）：

β = m i n \{θ_{s l} \cdot ∥d_{s l}∥ \cdot ∥ f ∥, θ_{s r} \cdot ∥d_{s r}∥ \cdot ∥ f ∥\}

(16)

其中：θ_sl、θ_sr分别指机器人左侧与右侧的接触点形成的翻转轴法向与合力f（不包括支持力，静态稳定时主要是重力）的夹角；d_sl、d_sr指左侧与右侧的翻转轴到合力方向的距离。

当所处地形相对机器人左右高度差H较大时，如图6（a）所示，机器人滚转角θ_Rr很大，极易侧翻。此时将地形较低一侧的摆臂下压可以提高机器人稳定性，如图6（b）所示；且机器人被抬起使滚转角接近0°时，能满足姿态预测算法的假设。

图6机器人在单侧台阶上的滚转稳定性分析示意图

Fig.6Diagram of the rolling stability analysis of the robot on single-side steps

因此，设计了一种辅助侧摆臂角度计算方法，以减少机器人在面对不对称障碍时的滚转角变化，在保持稳定的同时让姿态预测算法能够有效运行。首先分析局部地形中不同方位的起伏程度，选择地形较高的方位中的点云数据作为越障侧姿态预测与构型规划的输入，地形较低的方位中的点云数据作为辅助侧摆臂角度计算的输入。如图7所示，当灰色圆点T_tl表示的左侧地形比红色方形T_tr表示的右侧地形平均值高时，黑色线段表示的机器人左侧为越障侧，淡蓝色表示的机器人右侧为辅助侧。利用T_tl进行姿态预测与构型规划，可以获得在当前位置处期望的机器人左侧姿态（y_R，θ_R），接触点p_cl1、p_cl2，以及左侧摆臂角度θ_fl1、θ_fl2。此时若要使滚转角接近0°，则右侧的摆臂需要下压将车体撑起，使右侧摆臂转轴r_r1、r_r2与左侧摆臂转轴的侧向投影位置相近。因此右侧（辅助侧）摆臂角度θ_fr1、θ_fr2的计算只需在给定摆臂转轴位置的条件下，求得满足摆臂与地形接触、摆臂不与地形碰撞干涉、摆臂角度在允许范围内三个条件的摆臂角度即可，此时辅助侧摆臂与地形的接触点为p_cr1、p_cr2。

图7机器人攀爬左侧台阶时的辅助侧摆臂动作示意图

Fig.7Diagram of the assistant side flipper action when the robot is climbing the left side step

综上所述，以姿态预测为基础，设计了基于动态规划并能滚动更新的机器人构型规划器，并提出了一种能够适应非对称地形的规划策略。

3 实验与讨论

为了测试所提出的机器人自主构型规划算法（以下简称构型规划算法）的效果，在2个Gazebo仿真场景与3个真实场景中进行了实验，并与基于深度强化学习的方法^[10]进行了对比。

3.1 实验设置与对比指标

3.1.1 参数设置

如图1所示，实验使用的NuBot-Rescue机器人的基本物理参数为：底盘轴距B=0.552 m，摆臂轴距F=0.336 m。通过机载激光雷达结合建图算法^[16]输出局部地形点云；通过惯性测量单元（inertial measurement unit，IMU）感知机器人姿态。Gazebo仿真中的机器人与实物机器人具有相同的物理参数和传感器配置。仿真履带运动控制方法采用文献^[17]提出的“不可变形履带的快速仿真方法”。

系统软硬件架构如图8所示。采用机器人操作系统（ROS）将所有程序运行在机载电脑（Intel i9-9900T，16 GB）中，主要包括：同步定位与建图节点（5 Hz）、姿态预测与构型规划节点（1~2 Hz）、电机控制节点（100 Hz）、人工控制节点（100 Hz）、交互界面。机载电脑从激光雷达、IMU、相机、电机驱动器等机载设备获得环境感知信息与机器人状态感知信息；通过无线通信将感知信息传递到遥控端，并接收前进方向、速度等人工控制指令信息。在电机控制节点中将人工控制指令与规划的构型序列融合，输出摆臂、履带的控制指令。

图8机器人软硬件架构

Fig.8Framework of the robot′s software and hardware

在算法的参数方面，局部地形感知域为以机器人底盘中心向前1.5 m、向后0.5 m，左右各0.5 m的长方形范围；预测路径点间距0.2 m，路径点数量至多4个；摆臂角度分辨率k=10°，摆臂可调整单位数b=3，摆臂活动范围-70°~70°。

3.1.2 对比指标

用于量化对比的指标主要有：单次任务机器人姿态角变化率绝对值的总和

\hat{θ}

（俯仰

{\hat{θ}}_{R}

、滚转

{\hat{θ}}_{roll}

）^[10]、单次任务过程中的姿态角速度最大值

| \dot{θ} |_{m a x}

（俯仰

{|{\dot{θ}}_{R}|}_{m a x}

、滚转

{|{\dot{θ}}_{roll}|}_{m a x}

）、单次任务耗时t_cost等三种。其中

\hat{θ}

由式（17）计算：

\hat{θ} = \int_{0}^{t_{cost}} | \dot{θ} | d t = \sum_{t = 1}^{T - 1} | θ (t + 1) - θ (t) |

(17)

式中，T表示单次任务的时间总步数。

\hat{θ}

能够衡量机器人运行时的整体摇晃程度，

| \dot{θ} |_{m a x}

能够衡量瞬时摇晃程度。除废墟场景外，每个场景各实验10次，取上述指标的均值进行分析。实验的起点设置为机器人中心距离障碍物边缘1 m处，终点设置为机器人中心离开障碍物后1 m处。

3.2 仿真实验

3.2.1 台阶场景

0.4 m高的单台阶是机器人能够通过的极限高度，用于测试算法对高难度对称地形的适应性。主要包括上、下台阶两个过程，如图9所示。

图90.4 m单台阶场景越障过程

Fig.9Processes of the robot moving through the 0.4 m single-step scenarios in simulation

量化指标如表1所示，对比强化学习方法和人工操作^[10]，构型规划算法在单台阶越障场景中具有比强化学习方法更好的通行平稳程度和相近的通行时间，且都明显优于人工操作。

3.2.2 陡峭楼梯场景

陡峭楼梯场景由上33.7°楼梯和下45°楼梯两部分组成，其中上楼梯时台阶高度0.2 m，台阶宽度0.3 m，共6级；下楼梯时台阶高度0.2 m，台阶宽度0.2 m，共6级。整体过程如图10所示。

图10陡峭楼梯场景越障过程

Fig.10Processes of the robot moving through the steep staircase scenarios in simulation

运行中的量化指标如表1所示。其中

{\hat{θ}}_{R}

较深度强化学习方法稍大，因为下楼梯的过程中算法并未将摆臂完全展平，机器人随着楼梯起伏而轻微摇摆。但是，构型规划算法的第二项指标（

{\hat{θ}}_{Rmax} - {\hat{θ}}_{Rmin}

）更小，表明

{\hat{θ}}_{R}

的上限与深度强化学习方法的上限几乎相同。并且，构型规划算法的任务完成时间更短，间接表明越障过程更流畅。

表1仿真台阶与楼梯场景中的不同算法指标对比

Table1Quantitative evaluation results in the single-step and steep staircase scenarios in simulation

3.3 实物实验

3.3.1 单侧台阶场景

在单侧台阶场景实物实验中，机器人需要攀爬位于左侧的高0.3 m台阶，如图11（a）所示。运行中的量化指标如表2所示。结果表明构型规划算法能够使机器人稳定地攀爬具有较高难度的单侧台阶，且滚转方向的摇晃程度较小。

3.3.2 斜向楼梯场景

在斜向楼梯场景中，机器人需要斜向通过楼梯地形，每级楼梯长0.38 m、高0.12 m，共8级。具体效果如图11（b）~（c）所示。

量化指标如表2所示，相比上楼梯的过程，下楼梯的过程中

{\hat{θ}}_{R}

、

{\hat{θ}}_{roll}

更接近，但

{|{\dot{θ}}_{R}|}_{m a x}

、

{|{\dot{θ}}_{roll}|}_{m a x}

更大，说明瞬时晃动更大。这是由于机器人质心从平台下到楼梯上时会向左前方倾转；而上台阶时右后摆臂下压使底盘较平，在上到台阶顶部时倾转较小，从而晃动较小。

3.3.3 废墟场景

废墟场景由水泥砖块、家具、树枝、麻袋等物体堆砌而成。机器人需要正向、反向通过废墟，整体水平移动距离约8 m。具体效果如图12所示。

整体而言，机器人流畅而平稳地通过了废墟。在上坡时前后摆臂放平以提高稳定性；在下坡时提前将前摆臂下压以降低俯仰角晃动。在局部地形的左右高差较大时，左右摆臂采取不同的动作，虽然幅度较小但有效地防止了机器人侧翻。每次实验时的量化指标单独列出，如表3所示。废墟场景整体起伏大、距离长，使得

{\hat{θ}}_{R}

、

{\hat{θ}}_{roll}

较高。但借助构型规划算法，瞬时晃动

{|{\dot{θ}}_{R}|}_{m a x}

、

{|{\dot{θ}}_{roll}|}_{m a x}

相对较小，越障过程较平稳。

综上所述，在仿真实验中，构型规划算法能够使机器人顺利通过高难度的障碍地形，且相比深度强化学习算法具有更好的效果；在实物实验中，进一步测试了算法的实际运行性能，结果表明算法能够有效适应难度较高的单侧台阶，轻松地斜向上下楼梯，平稳地通过复杂的废墟地形。

图11机器人在真实单侧台阶与斜向楼梯场景的越障过程

Fig.11Processes of the robot moving through the single-side-step and oblique stairs scenarios in real-world

表2实物单侧台阶与斜向楼梯场景的量化指标

Table2Quantitative evaluation results in the single-side-step and oblique stairs scenarios in real-world

图12机器人在废墟场景的越障过程

Fig.12Processes of the robot moving through the ruins scenario in real-world

表3实物废墟场景的量化指标

Table3Quantitative evaluation results in the ruins scenario in real-world

4 结论

本文提出了一种适用于障碍地形的搜救机器人越障构型规划方法。该方法以高效的姿态预测算法为核心，将不规则的地形剖面描述为离散的点云，并建立了履带机器人单侧姿态预测问题模型；进一步设计了该问题的解析求解算法，使得求解时间大为缩短；结合动态规划方法与滚动优化思想，构建了姿态预测—状态评价—构型规划三个步骤滚动进行的越障构型规划算法，并针对非对称地形设计了构型规划策略，获得了优化的机器人越障构型序列和对应的摆臂动作。最后在多个场景中进行了实验，并与深度强化学习方法进行了对比，仿真与实物实验结果表明该算法能够使机器人在复杂地形上实现优异的越障效果。在未来的工作中，将研究滚转角较大时机器人整体姿态预测算法，以更好地适应非对称地形；进一步提高姿态预测算法与构型规划算法的协同程度，探索效率更高、适用性更强的自主越障方法。

图1NuBot-Rescue 四摆臂履带机器人

Fig.1The NuBot-Rescue tracked robot with four flippers

下载: 全尺寸图片

图2单侧履带机器人模型简化过程与符号定义

Fig.2The tracked robot′s single-side pose modelling and notation definition

下载: 全尺寸图片

图3机器人模型以及机器人姿态与接触点的关系

Fig.3Robot model and the relationship between robot pose and contact points

下载: 全尺寸图片

图4姿态预测算法框图

Fig.4Framework of pose prediction algorithm

下载: 全尺寸图片

图5机器人下壕沟过程的构型规划示意图

Fig.5Diagram of robot configuration planning when going down the trench

下载: 全尺寸图片

图6机器人在单侧台阶上的滚转稳定性分析示意图

Fig.6Diagram of the rolling stability analysis of the robot on single-side steps

下载: 全尺寸图片

图7机器人攀爬左侧台阶时的辅助侧摆臂动作示意图

Fig.7Diagram of the assistant side flipper action when the robot is climbing the left side step

下载: 全尺寸图片

图8机器人软硬件架构

Fig.8Framework of the robot′s software and hardware

下载: 全尺寸图片

图90.4 m单台阶场景越障过程

Fig.9Processes of the robot moving through the 0.4 m single-step scenarios in simulation

下载: 全尺寸图片

图10陡峭楼梯场景越障过程

Fig.10Processes of the robot moving through the steep staircase scenarios in simulation

下载: 全尺寸图片

图11机器人在真实单侧台阶与斜向楼梯场景的越障过程

Fig.11Processes of the robot moving through the single-side-step and oblique stairs scenarios in real-world

下载: 全尺寸图片

图12机器人在废墟场景的越障过程

Fig.12Processes of the robot moving through the ruins scenario in real-world

下载: 全尺寸图片

表1仿真台阶与楼梯场景中的不同算法指标对比

Table1Quantitative evaluation results in the single-step and steep staircase scenarios in simulation

下载: 全尺寸图片

表2实物单侧台阶与斜向楼梯场景的量化指标

Table2Quantitative evaluation results in the single-side-step and oblique stairs scenarios in real-world

下载: 全尺寸图片

表3实物废墟场景的量化指标

Table3Quantitative evaluation results in the ruins scenario in real-world

下载: 全尺寸图片

图1NuBot-Rescue 四摆臂履带机器人

Fig.1The NuBot-Rescue tracked robot with four flippers

图2单侧履带机器人模型简化过程与符号定义

Fig.2The tracked robot′s single-side pose modelling and notation definition

图3机器人模型以及机器人姿态与接触点的关系

Fig.3Robot model and the relationship between robot pose and contact points

图4姿态预测算法框图

Fig.4Framework of pose prediction algorithm

图5机器人下壕沟过程的构型规划示意图

Fig.5Diagram of robot configuration planning when going down the trench

图6机器人在单侧台阶上的滚转稳定性分析示意图

Fig.6Diagram of the rolling stability analysis of the robot on single-side steps

图7机器人攀爬左侧台阶时的辅助侧摆臂动作示意图

Fig.7Diagram of the assistant side flipper action when the robot is climbing the left side step

图8机器人软硬件架构

Fig.8Framework of the robot′s software and hardware

图90.4 m单台阶场景越障过程

Fig.9Processes of the robot moving through the 0.4 m single-step scenarios in simulation

图10陡峭楼梯场景越障过程

Fig.10Processes of the robot moving through the steep staircase scenarios in simulation

图11机器人在真实单侧台阶与斜向楼梯场景的越障过程

Fig.11Processes of the robot moving through the single-side-step and oblique stairs scenarios in real-world

图12机器人在废墟场景的越障过程

Fig.12Processes of the robot moving through the ruins scenario in real-world

表1仿真台阶与楼梯场景中的不同算法指标对比

Table1Quantitative evaluation results in the single-step and steep staircase scenarios in simulation

表2实物单侧台阶与斜向楼梯场景的量化指标

Table2Quantitative evaluation results in the single-side-step and oblique stairs scenarios in real-world

表3实物废墟场景的量化指标

Table3Quantitative evaluation results in the ruins scenario in real-world

图1NuBot-Rescue 四摆臂履带机器人

Fig.1The NuBot-Rescue tracked robot with four flippers

图2单侧履带机器人模型简化过程与符号定义

Fig.2The tracked robot′s single-side pose modelling and notation definition

图3机器人模型以及机器人姿态与接触点的关系

Fig.3Robot model and the relationship between robot pose and contact points

图4姿态预测算法框图

Fig.4Framework of pose prediction algorithm

图5机器人下壕沟过程的构型规划示意图

Fig.5Diagram of robot configuration planning when going down the trench

图6机器人在单侧台阶上的滚转稳定性分析示意图

Fig.6Diagram of the rolling stability analysis of the robot on single-side steps

图7机器人攀爬左侧台阶时的辅助侧摆臂动作示意图

Fig.7Diagram of the assistant side flipper action when the robot is climbing the left side step

图8机器人软硬件架构

Fig.8Framework of the robot′s software and hardware

图90.4 m单台阶场景越障过程

Fig.9Processes of the robot moving through the 0.4 m single-step scenarios in simulation

图10陡峭楼梯场景越障过程

Fig.10Processes of the robot moving through the steep staircase scenarios in simulation

图11机器人在真实单侧台阶与斜向楼梯场景的越障过程

Fig.11Processes of the robot moving through the single-side-step and oblique stairs scenarios in real-world

图12机器人在废墟场景的越障过程

Fig.12Processes of the robot moving through the ruins scenario in real-world

表1仿真台阶与楼梯场景中的不同算法指标对比

Table1Quantitative evaluation results in the single-step and steep staircase scenarios in simulation

表2实物单侧台阶与斜向楼梯场景的量化指标

Table2Quantitative evaluation results in the single-side-step and oblique stairs scenarios in real-world

表3实物废墟场景的量化指标

Table3Quantitative evaluation results in the ruins scenario in real-world

YANCO H A, NORTON A, OBER W,et al. Analysis of human-robot interaction at the DARPA robotics challenge trials[J]. Journal of Field Robotics,2015,32(3):420-444.

GIANNI M, FERRI F, MENNA M,et al. Adaptive robust three-dimensional trajectory tracking for actively articulated tracked vehicles[J]. Journal of Field Robotics,2016,33(7):901-930.

HOWARD T, PIVTORAIKO M, KNEPPER R A,et al. Model-predictive motion planning:several key developments for autonomous mobile robots[J]. IEEE Robotics & Automation Magazine,2014,21(1):64-73.

SINGH S, JADHAV B D, KRISHNA K M. Posture control of a three-segmented tracked robot with torque minimization during step climbing[C]//Proceedings of 2014 IEEE International Conference on Robotics and Automation(ICRA),2014:4200-4207.

YUAN Y J, XU Q W, SCHWERTFEGER S. Configuration-space flipper planning on 3D terrain[C]//Proceedings of 2020 IEEE International Symposium on Safety, Security,and Rescue Robotics(SSRR),2020:318-325.

ENDO D, WATANABE A, NAGATANI K. Stair climbing control of 4-degrees-of-freedom tracked vehicle based on internal sensors[C]//Proceedings of 2016 IEEE International Symposium on Safety, Security,and Rescue Robotics(SSRR),2016:112-117.

PECKA M, ZIMMERMANN K, PETRLÍK M,et al. Data-driven policy transfer with imprecise perception simulation[J]. IEEE Robotics and Automation Letters,2018,3(4):3916-3921.

AZAYEV T, ZIMMERMANN K. Autonomous state-based flipper control for articulated tracked robots in urban environments[J]. IEEE Robotics and Automation Letters,2022,7(3):7794-7801.

MITRIAKOV A, PAPADAKIS P, KERDREUX J,et al. Reinforcement learning based,staircase negotiation learning:simulation and transfer to reality for articulated tracked robots[J]. IEEE Robotics & Automation Magazine,2021,28(4):10-20.

潘海南, 陈柏良, 黄开宏, 等. 基于深度强化学习的履带机器人摆臂控制方法[J/OL]. 系统仿真学报,2023:1-11(2023-02-16)[2023-02-25].https://doi-org-s.libyc.nudt.edu.cn:443/10.16182/j.issn100473x.joss.22-1105. PAN H N, CHEN B L, HUANG K H,et al. Flipper control method based on deep reinforcement learning for tracked robot[J]. Journal of System Simulation,2023:1-11(2023-02-16)[2023-02-25].https://doi-org-s.libyc.nudt.edu.cn:443/10.16182/j.issn100473x.joss.22-1105.(in Chinese)

NOROUZI M, MIRO J V, DISSANAYAKE G. Planning stable and efficient paths for reconfigurable robots on uneven terrain[J]. Journal of Intelligent & Robotic Systems,2017,87(2):291-312.

FABIAN S, KOHLBRECHER S, VON STRYK O. Pose prediction for mobile ground robots in uneven terrain based on difference of heightmaps[C]//Proceedings of 2020 IEEE International Symposium on Safety, Security,and Rescue Robotics(SSRR),2020:49-56.

SHI C H, HUANG K H, YU Q H,et al. Extrinsic calibration and odometry for camera-LiDAR systems[J]. IEEE Access,2019,7:120106-120116.

DENG W B, HUANG K H, CHEN X,et al. Semantic RGB-D SLAM for rescue robot navigation[J]. IEEE Access,2020,8:221320-221329.

PAPADOPOULOS E, REY D A. The force-angle measure of tipover stability margin for mobile manipulators[J]. Vehicle System Dynamics,2000,33(1):29-48.

XU W, CAI Y X, HE D J,et al. FAST-LIO2:fast direct LiDAR-inertial odometry[J]. IEEE Transactions on Robotics,2022,38(4):2053-2073.

PECKA M, ZIMMERMANN K, SVOBODA T. Fast simulation of vehicles with non-deformable tracks[C]//Proceedings of 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS),2017:6414-6419.

Home

1 姿态预测

2 构型规划

3 实验与讨论

4 结论