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

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

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

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

图1 NuBot-Rescue 四摆臂履带机器人Fig.1 The 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)所示。

(a) 单侧履带简化模型(a) The single-side pose modelling

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

1.2 姿态预测问题定义

机器人在地形中的姿态,是由机器人自身构型以及与地形的接触点决定的。设地面参考系为[G],姿态预测问题可以描述为下列形式。

给定:

1)T:地形的点集。

2)xR:底盘中心在[G]中的x轴坐标。

3)θ1:机器人前摆臂角度,逆时针为正。

4)θ2:机器人后摆臂角度,顺时针为正。

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

1)yR:底盘中心在[G]中的y轴坐标。

2)θR:机器人在[G]中的俯仰角,逆时针为正。

约束条件:

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

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

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

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

1.3.1 机器人模型的数学描述

以[G]中的待预测位置(xR,0)为原点建立一个y轴与重力方向平行、x轴方向为机器人朝向的局部坐标系[L]。令机器人的底盘中心位置与原点重合,底盘与x轴平行,可以得到如图3(a)所示的模型。该模型可以用分段直线描述各部件:

(1)

1.3.2 接触关系的数学描述

地形点与机器人接触可以表示为点在分段直线模型上,即机器人R在[L]中首先绕原点旋转俯仰角φ,再沿y轴平移h变换为R′,并与地形点pc1(xc1,yc1)∈T、pc2(xc2,yc2)∈T接触,如图3(b)所示。

(a) 在局部坐标系中水平放置的机器人的分段直线模型(a) A segmented linear model of the robot placed horizontally in the local coordinate system

(b) 将机器人模型先旋转φ,再平移h,与接触点重合(b) The robot model is first rotated φ, then translated h to coincide with the contact points图3 机器人模型以及机器人姿态与接触点的关系Fig.3 Robot model and the relationship between robot pose and contact points

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

(2)

对于pc2,有式(3)成立:

(3)

其中:lr′i+1r′i(i=1,2,3)为以齐次坐标向量形式表示的变换后的机器人模型R′,其含义为经过r′i+1、r′两点的直线;pcj(j=1,2)为以齐次坐标向量形式表示的接触点,如式(4)所示。

(4)

式中,(xr′i,yr′i)为r′i的坐标,是图3(a)中初始状态下的ri经过旋转和平移变换后得到的,如式(5):

(5)

在约束条件1中,接触点还需要满足分布在质心前后的要求,即式(2)、式(3)中接触点的x轴坐标取值范围要远离质心。机器人模型质心在[L]中的坐标(xcom,ycom)可以表示为式(6):

(6)

其中,(xb,yb)是底盘质心的坐标,lf是摆臂质心到摆臂转轴的距离,mB和mF是机器人单侧底盘以及单个摆臂的质量。假设底盘质心与底盘形状中心重合即(xb,yb)=(0,0),则有式(7):

(7)

与此同时,所有位于变换之后机器人范围内的地形点p还需要满足约束条件2的要求。可以用lr′i+1r′i(i=1,2,3)与p的点积正负表示点相对直线的位置,因此该条件可以转化为式(8):

(8)

需要注意的是,变换后的机器人模型摆臂末端点r′1、r′4有可能位于底盘下方即xr′2>xr′1或xr′4>xr′3,此时式(8)中的符号取反。

最终满足所有条件的候选接触点pc1、pc2所对应的(h,φ)即为机器人在当前位置的预测姿态参数:(yR,θR)=(h,φ)。

联立式(2)～(5)、式(8)可以得到如下形式的姿态预测问题的数学描述:

求解:h,φ,pc1(xc1,yc1)∈T1,pc2(xc2,yc2)∈T2

条件:

(9)

(10)

(11)

(12)

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

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

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

首先从地形点集T1、T2中选取一对候选接触点(pc1,pc2),由于接触点既可能位于摆臂上也可能位于底盘上,因此式(2)、式(3)可以根据下列情况各自提出一个等式组成待求解的等式方程组:

1)pc1在前摆臂上,pc2在后摆臂上;

2)pc1在前摆臂上,pc2在底盘上;

3)pc1在底盘上,pc2在后摆臂上;

4)pc1在底盘上,pc2在底盘上。

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

(13)

其中的系数Ai、Bi、Ci、Xi、Yi(i=1,2)为只与机器人模型的原坐标(xri,yri)、候选接触点坐标(xci,yci) 有关的常数。将该方程组视为关于z的多项式,如果z的解存在,则这两个多项式之间必然存在公因式,即两者的结式为零。假设式(13)的最高项系数(Ai-Xih)均不为零,展开结式可以得到关于h的四次方程,利用多项式求根算法可以求出h的解;进一步利用二次方程求根公式与反正切函数得到h对应的φ。采用相同的方法,可以求出其他三种情况下的解。根据接触点x轴坐标的范围条件以及等式约束条件,可从这四种情况中找到一组满足接触点条件的姿态参数(h,φ)。

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

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

图4 姿态预测算法框图Fig.4 Framework of pose prediction algorithm

2 构型规划

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

2.1 机器人状态与动作评价

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

图5 机器人下壕沟过程的构型规划示意图Fig.5 Diagram of robot configuration planning when going down the trench

根据人类操作员的经验,设计了代价函数C来评价在状态SP,θ1,θ2时采取动作(a1,a2)的代价:

(14)

其中,Cθ(SP,θ1,θ2,SP+1,θ1+a1,θ2+a2)为前后两个状态间俯仰角变化的代价,Cy(SP,θ1,θ2)为当前状态下质心高度与底盘范围内地面平均高度的偏差的代价,Cs(SP,θ1,θ2)为当前状态下俯仰方向稳定性的代价,Ct(SP,θ1,θ2)为当前状态下摆臂力矩的代价,Cr(a1,a2)为摆臂调整幅度的代价。

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

2.2 动态规划

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

Call(SP,θ1,θ2)=min{Call(SP-1,θ1-a1,θ2-a2)+

C(P-1,θ1-a1,θ2-a2)}

(15)

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

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

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

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

(16)

其中:θsl、θsr分别指机器人左侧与右侧的接触点形成的翻转轴法向与合力f(不包括支持力,静态稳定时主要是重力)的夹角;dsl、dsr指左侧与右侧的翻转轴到合力方向的距离。

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

图6 机器人在单侧台阶上的滚转稳定性分析示意图Fig.6 Diagram of the rolling stability analysis of the robot on single-side steps

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

图7 机器人攀爬左侧台阶时的辅助侧摆臂动作示意图Fig.7 Diagram 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、相机、电机驱动器等机载设备获得环境感知信息与机器人状态感知信息;通过无线通信将感知信息传递到遥控端,并接收前进方向、速度等人工控制指令信息。在电机控制节点中将人工控制指令与规划的构型序列融合,输出摆臂、履带的控制指令。

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

3.1.2 对比指标

(17)

3.2 仿真实验

3.2.1 台阶场景

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

图9 0.4 m单台阶场景越障过程Fig.9 Processes of the robot moving through the0.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.10 Processes of the robot moving through the steep staircase scenarios in simulation

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

3.3 实物实验

3.3.1 单侧台阶场景

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

3.3.2 斜向楼梯场景

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

3.3.3 废墟场景

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

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

(a) 0.3 m单侧台阶场景越障过程(a) Obstacle crossing process in the 0.3 m single-side-step scenario

(b) 斜向上楼梯场景越障过程(b) Obstacle crossing process in the up-side oblique stairs scenario

(c) 斜向下楼梯场景越障过程(c) Obstacle crossing process in the down-side oblique stairs scenario图11 机器人在真实单侧台阶与斜向楼梯场景的越障过程Fig.11 Processes of the robot moving through the single-side-step and oblique stairs scenarios in real-world

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

(a) 废墟场景正向越障过程(a) Obstacle crossing process along forward direction in ruins scenario

(b) 废墟场景反向越障过程(b) Obstacle crossing process along reverse direction in ruins scenario图12 机器人在废墟场景的越障过程Fig.12 Processes of the robot moving through the ruins scenario in real-world

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

4 结论

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