基于参考轨迹的移动机器人避碰决策及轨迹跟踪方法

聂振邦,于海斌+

(1.中国科学院 沈阳自动化研究所机器人学国家重点实验室,辽宁 沈阳 110016;2.中国科学院 网络化控制系统重点实验室,辽宁 沈阳 110016;3.中国科学院 机器人与智能制造创新研究院,辽宁 沈阳 110169;4.中国科学院大学,北京 100049)

0 引言

移动机器人具有自主、灵活的特点,在车间自动化物流系统中得到了广泛的应用。但为了让移动机器人安全高效地完成物流运输任务,需要进行运动规划。运动规划一般分为环境地图建模、路径规划、轨迹规划、轨迹跟踪4个步骤。

通常,车间移动机器人轨迹跟踪是指在轨迹规划基础上对机器人进行反馈控制,以减小机器人实时位置与具有时间特征的参考轨迹之间的偏差。除了减小轨迹跟踪误差以外,近年来,根据实际应用的需要,基于实时信息动态地避免由时间和位置扰动带来的碰撞问题开始得到关注,也成为研究热点之一。

具有避碰能力的轨迹跟踪方法通常包括避碰触发条件和轨迹跟踪控制两个部分。已有的避碰触发条件主要基于当前相对位置和相对速度信息。文献[1]提出一种轨迹跟踪控制器,当障碍物进入冲突检测范围就切换到避碰模式;文献[2]～文献[4]提出基于相对位置和速度信息来切换避碰模式的反馈控制器,只有当机器人相对障碍物的运动速度可能导致碰撞时才会进行避碰。经典的轨迹跟踪控制,主要有基于滑模控制的方法[5]、基于反步法的方法[6]、基于动态反馈线性化的方法[7]等。近年来,随着机器人计算能力的增强,以及数学优化方法和工具的进步,研究者提出了基于模型预测控制(Model Predictive Control,MPC)的轨迹跟踪方法[8-9],通过在线求解包含机器人非线性约束的优化问题来计算最优的控制输入。文献[10]和文献[11]给出了几种基于MPC的实时避碰轨迹跟踪控制算法。

以上研究能够保证机器人运动的安全性,但这些方法在进行避碰决策时只使用了当前信息,并没有充分利用参考轨迹的全局信息,使得避碰算法过于保守,导致机器人的实际运动轨迹与参考轨迹偏差较大。为此,本文提出基于参考轨迹全局信息的避碰决策和轨迹跟踪方法。该方法包括基于参考轨迹全局信息的避碰触发条件构建,以及最小化跟踪误差的避碰轨迹跟踪控制算法。

1 基于当前信息的避碰及轨迹跟踪方法

现有基于当前信息进行避碰的移动机器人轨迹跟踪方法可分为两类：一类根据机器人与障碍物的相对距离进行避碰[1],另一类根据机器人与障碍物的相对位置和相对速度进行避碰[2-4]。

1.1 基于相对距离信息的方法

图1 机器人感知范围

(1)

(2)

1.2 基于相对位置和相对速度的方法

在基于相对距离信息的避碰与轨迹跟踪研究的基础上,人们进一步开展了根据当前的相对位置和相对速度信息进行避碰与轨迹跟踪的研究[2-4]。

如图2所示,Ri的感知区域被划分为冲突区、过渡区、安全区。

图2 感知区域划分

据此定义冲突区Cij、过渡区Tij、安全区Sij如下：

Cij={zij∈Rij||γij|≤π/2};

Tij={zij∈Rij|π/2<|γij|≤π/2+δ};

Sij={zij∈Rij||γij|>π/2+δ}。

(3)

其中δ∈(0,π/2)为可调参数。

基于上述分区,将基于相对距离信息的碰撞风险度量函数式(1)进行扩展,定义考虑相对速度信息的碰撞风险度量函数Wij：

(4)

考虑相对速度信息的避碰梯度向量为

(5)

其中：kij>0为可调参数,σij为分区权重系数。

2 基于参考轨迹全局信息的避碰决策与轨迹跟踪

本章在综合考虑参考轨迹全局信息的前提下,提出避碰决策与轨迹跟踪方法。

(6)

2.1 基于参考轨迹信息的避碰触发条件

本节在如图2所示的感知区域划分的基础上,对zij∈Cij的情况作进一步的筛选,提出一组基于参考轨迹信息的避碰触发条件,以减少不必要的避碰动作。

图3 存在碰撞风险的轨迹跟踪场景

(7)

(3)避碰触发条件 当存在Rj或Oj,使得zij∈Cij,且满足以下条件之一时,Ri需要进行避碰：

其中：τp>0,θth>0为可调参数,符号×为叉积算子,sign为符号函数。

2.2 最小化跟踪误差的避碰轨迹跟踪控制

2.2.1 常规MPC轨迹跟踪控制算法

考虑如图4所示的车式移动机器人的轨迹跟踪控制。将时间按控制周期进行离散化,k∈对应时刻tk=kT。

图4 车式移动机器人等效模型

机器人的速度上限vm,加速度上限am,转向角φ∈[-φm,φm],φ的变化率上限wm,轴距为l,后轮轴中心pa=[xa,ya],pa=p-0.5l,其中l=[lcosθ,lsinθ]T。

在时刻tk,参照文献[12],以最小化跟踪误差为优化目标,构造MPC轨迹跟踪问题如下：

(8)

(9)

|f1[n]|≤b1。

(10)

(11)

其中：

(12)

(13)

f1[n]=[u[n]T,u[n+1]T-u[n]T]T;

(14)

b1=[vm,φm,am,wm]T。

(15)

以上MPC轨迹跟踪控制算法未考虑避碰的要求,不能保证轨迹跟踪过程中对障碍物的避碰。为此,还需要研究轨迹跟踪误差优化的避碰策略,以构建机器人避碰速度的约束空间Vs,作为MPC轨迹跟踪优化的约束条件。

2.2.2 面向轨迹跟踪误差优化的避碰策略

为了减小避碰对轨迹跟踪精度的影响,针对机器人之间的避碰、机器人与障碍物间的避碰,本文提出一组基于速度向量调节的避碰策略。

为有效描述基于速度调节的避碰机制,利用文献[13]提出的碰撞锥(Collision Cone,CC)概念。具体如图5所示,l1,l2为从pi出发,与圆o(pj,rij)相切的直线,l1,l2所夹的区域为碰撞锥,以下记为CCij。记碰撞锥的边界l1,l2的方向为θl1,θl2。

图5 碰撞锥示意

不难看出,在不考虑跟踪误差的情况下,只需调节vi,vj,使相对速度vij脱离碰撞锥,即vij∉CCij,就可以避免Ri与Rj之间的碰撞。但是,如果对相对速度调节方向不作必要的约束,有可能对轨迹跟踪性能产生不利的影响。

GF Machining Solutions（又名“GF加工方案”）是一家为工模具和精密零件加工行业提供机床和自动化解决方案的世界领先的系统供应商。其产品范围从放电加工机床、高速铣床和高性能铣床、夹具和托盘系统、三维激光纹理机，到提供客户服务、备品备件、易损件、耗品以及自动化的解决方案。作为一个全球化的集团，GF Machining Solutions隶属于Georg Fischer集团（瑞士），管理着全球范围内50个自己的组织架构。

基于以上认识,本文提出基于参考轨迹信息确定相对速度vij调节方向的原则,以保证机器人按照参考轨迹规定的方式来协调运动。

以如图6所示情况为例,为保持按参考轨迹规定的Rj先通过交叉口,此时,vij应向碰撞锥CCij的逆时针方向调节。

图6 存在碰撞风险的场景

根据上述分析,就移动障碍物与固定障碍物的避碰,分4种情况给出构造Ri在速度平面vx-vy中避碰速度约束空间Vs的方法。

情况1避免与移动机器人Rj碰撞,vij(tk)在CCij(tk)内。

(16)

(17)

情况3避免与固定障碍物Oj碰撞,vi(tk)在CCij(tk)内。

(18)

其中ξc∈(0,π/2)为可调参数。

情况4避免与固定障碍物Oj碰撞,vi(tk)不在CCij(tk)内。

(19)

其中ξs∈(0,π/2)为可调参数。

不难验证,以上得到的避碰速度约束空间Vs是凸的多边形空间。因此,基于Vs可以使用线性代数的方法构造出速度v=[vx,vy]对应的一组半平面(Half Planes,HP)线性不等式约束,其中的每个约束Avx+Bvy+C≤0都对应Vs的一条边界Avx+Bvy+C=0。

对于HP中的每个约束Avx+Bvy+C≤0,构造MPC控制变量u[k]的避碰约束：

Av[k]cosθp[k]+Bv[k]sinθp[k]+C≤0。

(20)

显然,将式(20)并入问题(8)进行求解,就可以在优化跟踪误差的同时避免碰撞。

3 实验验证与分析

本章设计了4个移动机器人跟踪预规划安全参考轨迹的实验场景。在轨迹跟踪过程中,对机器人施加位置扰动和时间扰动,引入碰撞风险,以验证所提方法的避碰和轨迹跟踪性能。

以下记本文所提基于参考轨迹全局信息的避碰轨迹跟踪方法为CABR(collision avoidance based on reference trajectory)。选取当前最具代表性的基于相对位置和相对速度信息的轨迹跟踪方法(Collision Avoidance Velocity-based Gradient,CAVG)[2]与本文所提的CABR进行比较。

同时,为了深度说明2.1节提出的避碰触发条件和2.2.2节提出的避碰策略的作用,也与基于当前信息的避碰轨迹跟踪(Collision Avoidance Not Based on Reference trajectory,CANBR)算法和未考虑避碰的轨迹跟踪(Trajectory tracKing No Collision Avoidance,TKNCA)算法进行了对比。

各方法的控制对象为车式移动机器人,安全半径r=0.5 m,轴距l=0.85 m,最大速度vm=1.5 m/s,最大加速度am=1.5m/s2,最大转向角φm=π/9 rad,转向角最大变化率wm=π rad/s。所有场景中的参考轨迹都满足式(6)。

各方法在仿真实验中的参数设定如下。TKNCA、CANBR、CABR求解MPC轨迹跟踪问题(8)时,控制周期T=0.1 s,预测时域NH=5。CANBR和CABR构造与固定障碍物避碰的速度方向θi的避碰区域SCij时,使用ξc=ξs=π/9rad。CABR避碰触发条件中的参数τp=1 s,θth=0.1 rad,CABR构造Ri的避碰速度约束空间Vs时的参数tl=6 s。CAVG的控制器参数γ=0 rad,其余与文献[2]的实验部分一致。CANBR、CABR、CAVG的感知范围半径Di相同。

3.1 参考轨迹不相交的场景

位置扰动的情况是：当R1、R2的间距降到5 m以下时,分别对R1、R2施加扰动,使机器人往路径左侧平移距离pe=0.5 m,为避免感知范围半径Di影响避碰效果,取Di=6 m。

各方法得到的运动轨迹如图7所示,图中显示了每0.2 s机器人的位置。可以看出,使用TKNCA时,R1、R2的安全区域存在交错重叠,说明R1、R2发生了碰撞。使用其余3种方法时,R1、R2没有发生碰撞,体现了避碰策略的作用。

图7 4种方法得到的运动轨迹(pe=0.5 m)

各方法得到的机器人跟踪误差如图8所示。在t=7.5 s扰动发生后,TKNCA的跟踪误差减小最快,随后产生大幅回升,说明机器人运动发生振荡,致使碰撞发生。使用CANBR与CABR时,跟踪误差一开始较大,在t=9 s后快速减小,之后出现小幅回升,说明机器人优先进行避碰,等到没有碰撞风险后迅速缩小跟踪误差。使用基于相对位置和相对速度信息的CAVG时,机器人的跟踪误差下降速度最慢。

图8 4种方法得到的机器人跟踪误差

3.2 参考轨迹相交的场景

感知半径Di决定了冲突区域Cij(如图2)的大小,增加Di可以让机器人提前发现危险,提高安全性,但也容易导致不必要的避碰动作。该场景验证所提方法在不同的感知半径Di下的有效性,以及在机器人受到不同幅度位置扰动时的避碰和轨迹跟踪效果。

图9 机器人的参考轨迹

图与障碍物的间距变化

为了模拟不同的碰撞风险,选择ct=4.5 m、3.5 m、2.5 m、1.5 m和pe=0.125 m、0.25 m、0.5 m的不同组合构造扰动。

实验发现,TKNCA在ct=1.5 m,pe=0.5 m的场景下发生了碰撞。分别在感知半径Di=2 m、3.5 m、6 m、8.5 m、11 m的情况下,使用CABR、CANBR、CAVG对机器人进行轨迹跟踪控制。加上ct、pe的不同取值,每种方法得到60组轨迹跟踪数据。

对每一组数据,计算平均跟踪误差(ATE)：

(19)

图11显示了各方法的相对误差指标RTE。在不同的ct下,CAVG和CANBR的RTE都随着感知半径的增加而增加。CABR得到的RTE受感知半径增加的影响不大。CABR的RTE比另两种方法都要小,并且在ct=4.5 m和3.5 m时几乎为0,说明CABR避免了不必要的避碰动作。

图11 不同ct下各方法的RTE

3.3 四向交叉路口场景

本场景验证了所提方法在机器人受到不同幅度的时间扰动时的避碰和轨迹跟踪效果。

图12 4个机器人的参考轨迹

在t=4 s时,对R1施加扰动,将其实际运动速度降为max{0,vc-vd},其中vc为各方法计算得到的速度控制量,vd>0为扰动幅度。扰动的持续时间记为Ddur,选择Ddur=1 s、2 s、3 s,以及vd=0.5 m/s,0.7 m/s进行测试。通过实验发现,在Ddur=3 s,vd=0.7 m/s时,使用TKNCA的机器人发生了碰撞。

分别在感知半径Di=2 m、4 m、6 m、8 m、10 m的情况下,对CABR、CANBR、CAVG进行轨迹跟踪实验。对每组轨迹跟踪数据,统计各方法从扰动开始的时刻t=4 s到机器人离开中心区域的时刻t=8.8 s这一时段内的ATE。计算CATE=ATE-ATEb,其中ATEb为TKNCA得到的ATE。在不同扰动参数下,各方法得到的CATE随Di的变化如图13所示。

图13 不同方法得到的CATE

对所有的6种扰动,CABR得到的CATE都是最小的,随Di的增加的变化不明显;并且,除了Ddur=3 s,vd=0.7 m/s的情况以外,CABR的CATE几乎为0。说明CABR能够避免不必要的避碰动作,而其余两种方法的CATE都随Di的增加而增加。

在Di=8 m和10 m时,CANBR的CATE有明显的上升。这是因为在这两种情况中,机器人较早地感知到了其他机器人,CANBR根据当前状态信息acw2构造的Vs使得R1减速让R3先通过交叉口,而不是按参考轨迹规定那样让R1先通过,由此产生了很大的跟踪误差。CABR则使用基于参考轨迹信息得到的acw1来构造Vs,保持了R1、R3通过交叉口的先后顺序,产生的跟踪误差较小。

3.4 多机器人交换位置场景

本场景验证所提方法在机器人数量较多时的效果,工作区域和机器人参考轨迹如图14所示。机器人的起点位于正方形工作区域的边界x=±25 m,y=±25 m上,终点为另一侧边界上其他机器人的起点。

图14 机器人的参考轨迹

选择ct=4.5 m、3.5 m、2.5 m、1.5 m和pe=0.125 m、0.25 m、0.5 m共12组扰动参数的组合进行测试。通过实验发现,TKNCA在其中8组扰动下发生了碰撞。分别在感知半径Di=2 m、3.5 m、6 m、8.5 m、11 m的情况下,对CABR、CANBR、CAVG进行轨迹跟踪实验。和前面的实验场景一样,以CATE作为性能指标。在计算ATE时,统计时段为整个轨迹跟踪过程。

若实验过程中,机器人与参考轨迹的偏差超过10 m,则认为跟踪失败,不统计该次的数据。CABR成功完成了所有60次实验。CANBR有2次跟踪失败,CAVG有20次跟踪失败。

在不同扰动下各方法CATE随Di的变化如图15所示。CABR得到的CATE比CANBR和CAVG都小,并且随Di的变化不明显。在大多数扰动场景中,CABR的CATE都接近于0。在ct=1.5 m,pe=0.5 m时,机器人的碰撞风险最高,CABR的CATE有所增加,但仍然小于CANBR。

通过以上4个场景的实验,可以得出以下结论：

(1)利用基于参考轨迹全局信息的避碰触发条件进行避碰决策,可以有效减少移动机器人非必要的避碰动作。

(2)基于参考轨迹信息推导避碰速度约束空间Vs,将其作为避碰约束条件,构造最小化跟踪误差的MPC避碰轨迹跟踪控制方法,可以实现位置扰动和时间扰动环境下对参考轨迹的高精度跟踪。

4 结束语

针对车间物流移动机器人轨迹跟踪的鲁棒控制问题,本文提出一种基于参考轨迹全局信息的避碰决策及轨迹跟踪控制方法。该方法在基于当前相对位置和相对速度划分机器人感知区域的基础上,构建了基于参考轨迹信息的避碰触发条件,对是否需要碰撞作进一步的判断,避免不必要的避碰动作;将机器人的轨迹跟踪控制建模成以最小化跟踪误差为目标的MPC问题,根据参考轨迹信息推导面向轨迹跟踪误差优化的避碰速度约束空间,将其作为MPC的避碰约束条件,可以实现机器人在位置扰动和时间扰动下的高精度轨迹跟踪。

下一步工作将致力于将所提方法应用到实际移动机器人系统。