张隽赓，张提升，唐海亮，牛小骥

(武汉大学卫星导航定位技术研究中心，湖北武汉 430079)

1 引言

随着机器人技术的发展，低速移动机器人作为一种特殊的地面移动机器人，如扫地机器人[1]、自主导航农业拖拉机[2]、建筑机器人[3]等，越来越多地影响着人们的生活。

为实现低速移动机器人自主导航，现有研究大多采用在全局路径的基础上进行局部避障[1-3]的方式，上述导航方式无法对机器人导航过程的时间进行规划。轨迹跟踪作为机器人的导航方式之一，能够保证机器人在规定时间运动在指定的状态附近。然而，移动机器人轨迹跟踪误差系统往往是一类耦合非线性系统，不满足Brockett镇定的必要条件[4]。因此，传统的线性方法不能直接应用于移动机器人系统。目前，解决轮式机器人轨迹跟踪问题的方法[5-9]大多假设导航场景为全局已知，难以直接应用于复杂多变的现实环境。经典的避障方法[10-14]的主要思路是设计局部目标点并在每个时刻寻找最优航向，缺乏轨迹跟踪方面的考虑。因此，能够同时满足轨迹跟踪与避障的方法对机器人应用必不可少。

文献[15]针对轨迹跟踪与避障两种情形分别设计了两种控制律，并根据机器人所处环境决定使用何种控制律。由于两种控制律差异较大，因此在控制律切换过程中机器人会存在能量损失。文献[16-17]设计了能够同时进行轨迹跟踪与避障的控制律，但相应的控制系统复杂度高，不易实现。文献[3]使用动态窗口法(DWA)跟踪由A*算法规划的路径点，但是这种做法无法预测机器人完成导航的时间，也没有解决障碍物占据路径点使得局部目标点不可达的问题。文献[18]设计的基于速度空间的控制律VSTTM (velocity space based concurrent obstacle avoidance and trajectory tracking method)能够使机器人在跟踪参考轨迹的同时考虑避障，但基于VSTTM的机器人低速行驶时无法躲避直线轨迹上的障碍物，这限制了低速移动机器人的工作场景。

本文对VSTTM进行改进。首先分析基于速度空间的同时跟踪与避障方法VSTTM的缺陷问题。针对VSTTM的缺陷改进了原目标函数，并设计了跟踪状态与避障状态的切换机制。通过Ubuntu 16.04+ROS Kinetic+Gazebo 7.0仿真比较改进前后的实验结果。相较于其它同时跟踪与避障的方法，改进算法保留VSTTM算法的优势：结构简单、轨迹跟踪效果明显、避障方式高效。此外，有效解决VSTTM算法的固有避障缺陷，跟踪与避障的状态切换过程考虑了机器人状态的渐变，能量损失极小，相同速度条件下拥有更高的跟踪精度。

2 基于速度空间的同步轨迹跟踪与避障方法(VSTTM)

同时适用于轨迹跟踪和避障的方法一般分为两类：根据是否应该避障设计两种不同的控制律[15]，或设计一种控制律[16-18]同时解决避障与跟踪问题。下面将分析同步轨迹跟踪与避障方法VSTTM[18]的设计原理及存在的不足。

2.1 构建速度空间

首先构建非完整约束的轮式移动机器人轨迹跟踪误差方程。尽管不同低速移动机器人拥有不同的运动执行机构，但它们拥有共同的可控制变量：前进速度v与转弯速度ω。因此本文的机器人运动学模型建立如式(1)-(2)。

图1中，当前时刻t机器人在全局坐标系XgOgYg的位姿表示为q(t)=[xt，yt，θt]T，相应的速度向量表示为u(t)=[vt，ωt]T；时刻t对应的参考位姿记为qr(t)=[xr(t)，yr(t)，θr(t)]T，对应的参考速度记为ur(t)=[vr(t)，ωr(t)]T。运动学方程如式(1)所示

图1 移动机器人轨迹跟踪示意图

(1)

机器人局部坐标系：以机器人中心or为原点，xr轴与机器人航向相同，yr轴由xr轴逆时针方向旋转90°得到。跟踪误差方程如式(2)所示

(2)

文献[1]、[15]的测试结果表明：基于式(1)-(2)的简化运动学模型所设计的控制算法，应用在低速移动机器人上能够表现出不错的控制效果。因此本文的导航算法将在该运动学模型的基础上构建。

构建速度空间U[13]：设机器人能够达到的线速度最大值为vmax，最小值为vmin，角速度最大值为ωmax，得到机器人所有可能的速度范围构成一个速度空间Us

(3)

(4)

速度空间U(控制集)即两个速度空间Us和Ud的交集，满足式(5)

U={(vi，ωi)|(vi，ωi)∈Ud∩Us}

(5)

(6)

2.2 VSTTM避障缺陷分析

基于速度空间的同时避障与轨迹跟踪方法(VSTTM)[18]同样是基于速度空间的导航算法。对所有符合式(5)的控制集U，丢弃预测周期[t，t+Tf]内可能与障碍物碰撞的预测轨迹，从剩余可通行解中寻找最优控制量u*满足式(7)

(7)

式(7)中，Γ(u)评价控制量u在周期[t，t+Tf]内的预测轨迹与参考轨迹之间的偏差，P(u)评价末端状态q(t+Tf)与参考轨迹末端状态qr(t+Tf)的偏差。

图2 VSTTM跟踪直线轨迹情况

VSTTM存在如下缺陷：机器人在VSTTM控制下跟踪匀速直线轨迹，且参考速度vr较小时，将做减速直线运动。且当速度降低到一定程度后将发生避障失败的情况。图2展示了上述缺陷的过程。图2(a)表示机器人的预测轨迹受障碍物干扰初期的状态，此时机器人将做减速直线运动。随着时间推移，机器人从图2(a)代表的状态持续减速到图2(b)状态：由于速度减小，相应的预测轨迹探测范围宽度s′ 与障碍物大小相等。由于图2(b)状态下机器人的预测轨迹都不能成功避障，此处将该状态视为避障失败状态。

下面将分两步证明上述缺陷：1、证明预测轨迹受障碍物干扰初期(图2(a))，机器人做减速直线运动；2、通过证明图2(b)状态时机器人仍做减速直线运动，说明机器人从图2(a)状态持续减速到图2(b)代表的状态，进而导致避障失败结果。

首先，设当前时刻(t=0)机器人位姿为q0=[x0，y0，θ0]T，相应的参考轨迹位姿为qr0=[xr0，yr0，θr0]T。在预测周期t∈[0，Tf]内，参考轨迹以速度vr做直线运动，参考轨迹的坐标(xr(t)，yr(t))由式(8)决定

(8)

假设初始状态(t=0)机器人处于跟踪状态，即跟踪误差较小，有

(9)

在预测周期t∈[0，Tf]内，任意预测轨迹x(t)、y(t)为圆上的一段圆弧，满足式(10)

(10)

VSTTM的寻优过程由式(7)可简写为式(11)

JVSTTM(u)=(u)+(w)

(11)

为简化计算，同时也考虑到实际测试中参数q1与q2相近时算法轨迹跟踪效果最好，此处取q1=q2

(12)

F(u，t)=[xr(t)-x(t)]2+[yr(t)-y(t)]2

(13)

为获取最优控制量u*，分别考虑使R(ω)和Z(u)最小的控制量u。

首先分析使R(ω)最小的u。由式(12)可知，θe在定义域内趋近0时，R(ω)减小。θe随角速度绝对值|ω|减小而减小，且在ω=0取得最小值0。因此ω=0为R(ω)最小的最优解。

接着分析u对Z(u)的影响。使Δx=xr0-x0，Δy=yr0-y0。将式(8)-(10)代入式(13)：

F(u，t)=△x2+△y2+(vrt)2+2f(u，t)

(14)

(15)

由式(14)可知F(u，t)的可控变量仅有线速度v与角速度ω，即求最优速度u*仅需要考虑f(u，t)的函数图像。取vr=0.3m/s，t=Tf=2s，将式(15)函数f(u，2)画成图像。

图3 f(u，2)函数图像

取t=Tf/2=1s时，f(u，1)与f(u，2)的函数图像形状相似，都在ω=0附近取到最小值。但f(u，1)的数值比f(u，2)大约减小0.38倍，f(u，0.5)的数值比f(u，2)小3个数量级。因此t较大时对f(u，t)的数值影响较大。由于障碍物的存在，最优控制量只能取比vr更小的、可通行的线速度。综上可知，预测轨迹受障碍物干扰初期，VSTTM控制下机器人必定做减速直线运动。

下面证明图2(b)状态时机器人仍做减速直线运动。图2(b)中探测范围s′≈robs=0.15m，v′≈0.21m/s。计算机器人减速到v′ 相比参考轨迹点落后的距离Δy0≈0.027m。将Δy0、v′ 代入式(15)后画出f(u，t)的函数图像仍与图3相似：t较大时f(u，t)在ω=0附近取到最小值，命题得证。

3 VSTTM改进算法

由1.2小节内容可知，即使将预测周期Tf扩大到预测轨迹范围覆盖障碍物大小，也不能保证VSTTM算法能够避障成功。因此限制机器人避障时的速度是一个有效的解决方法。本文提出了对VSTTM进行改进： 在速度空间的基础上，从目标函数入手优化避障过程，并通过调整目标函数的权重更好兼顾避障效果与跟踪效果。

3.1 评价与优化

(16)

(17)

评价函数T(u)衡量在周期[t，t+Tf]内轨迹与参考轨迹之间的跟踪误差，在轨迹跟踪效果评估中起主要作用

(18)

式中xe，ye，θe由式(1)-(2)得到，q1，q2，q3为相应的权重系数。

根据文献[18]设计终端状态惩罚项P(u)，抑制机器人跟踪过程出现震荡现象：

(19)

设计评估机器人与障碍物之间距离的评价函数C(u)，且机器人越远离障碍物该项越小：

(20)

Dobs(t)为t时刻障碍物边缘到机器人中心的最小距离，rrob为机器人的半径，Δ为障碍物定位误差的上界。

速度评价函数V(u)调整机器人避障过程的线速度v(t)。该项抑制目标函数在满足式(20)后选择较小速度值。

V(u)=|v(t)-vr(t)|

(21)

3.2 跟踪与避障的切换

(22)

图4 跟踪与避障状态的判断示意图

本文提出的VSTTM改进方法将妨碍轨迹跟踪的障碍物视为扰动。根据控制系统稳定性定义，控制系统的稳定性由系统本身决定，即外界干扰消失后系统回到平衡状态的能力。由于算法在避障状态时受到障碍物的持续干扰，不存在讨论稳定性的场合。改进算法在跟踪状态时的参数与VSTTM完全相同，两者的稳定性有等效关系。VSTTM的原文献[18]已对该算法稳定性进行证明。又因为两种状态的切换是在速度空间的约束下进行，切换过程保持平稳运动。因此改进算法的稳定性得到保证。

图5 改进的VSTTM流程图

基于上述分析，VSTTM改进算法实现流程总结如下：

输入：机器人当前时刻t的状态q(t)=[xt，yt，θt]T和相应速度u(t)=[vt，ωt]T，同一时刻的参考轨迹qr(t)=[xr(t)，yr(t)，θr(t)]T，对应的参考速度记为ur(t)=[vr(t)，ωr(t)]T。传感器数据和环境地图。

输出：最优控制量u*。

算法流程如图5所示：VSTTM改进算法的一次迭代周期为执行周期δ。在一次迭代周期内，首先根据输入评估预测周期Tf内预测轨迹的优劣并输出u*。最后将输出u*作为命令送予机器人执行，完成一次迭代。当参考轨迹到达终点，则导航过程结束。

4 仿真测试分析

为验证改进算法的有效性，本文在Ubuntu16.04+ROS Kinetic+Gazebo7.0环境下与VSTTM算法[18]进行对比。Gazebo是一个常与ROS系统搭配使用的带有物理引擎的开源机器人仿真工具，在模拟无人机和传感器[20]、竞赛[21]等领域均被广泛应用与认可。测试的机器人仿真模型来自Wikipedia公司的turtlebot3-burger机器人的开源模型。

图6 实验测试仿真环境

4.1 VSTTM避障性能测试

该测试验证VSTTM在应对直线轨迹时表现出的缺陷。测试场景为宽6m的狭长通道，障碍物正中心坐落于坐标(0，0)上。机器人初始位姿为q(0)=[0(m)，-8(m)，90°]T，速度矢量u(0)=[0，0]T。参考轨迹是以(0，-8)为起点，终点为(0，8)，速度为vr的匀速直线轨迹(图8、9中红色星型标记)。根据文献[18]中的圆形轨迹跟踪进行测试，并在算法稳定的情况下取VSTTM参数q1，q2，q3，对多组参数和参考速度vr在相同场景下测试，观察机器人避障效果。

图7 vr 分别为0.3与0.7时机器人导航情况

图7表示参数为q1=q2=3，q3=0.5，vr分别取0.3m/s和0.7m/s情况下机器人的导航情况。图8(a)中红色轨迹为vr=0.7m/s机器人的轨迹，蓝色轨迹为vr=0.3m/s的轨迹。结合图8(b)的速度曲线，可以观察到机器人在预测轨迹受障碍物干扰初期都开始减小速度。不同的是，vr=0.7m/s条件下机器人线速度减小到一定程度后开始避障，而在vr=0.3m/s的条件下机器人线速度直接减小到0。

表1 VSTTM避障情况一览表

表1展示了不同算法参数与参考速度vr情况下机器人的避障情况：横排表示vr具体数值，竖排表示不同的算法参数，√和×表示避障的成功或失败。如，表格左上第一个×表示机器人在q1=q2=3，q3=0.5和vr=0.3m/s条件下避障失败。

参数q1，q2，q3的调整导致不同避障结果的原因为：q1调大表明算法更看重前向距离偏差，对前向距离变化更为敏感，因此更容易避障。q2调大同理。但是q1与q2数值相差过大则不能保证算法的轨迹跟踪效果。但无论如何调整，VSTTM仍然无法满足turtlebot3-burger机器人(最大线速度0.3m/s)的避障需求。

4.2 改进的VSTTM避障性能测试

该测试对VSTTM与改进的VSTTM算法在同等条件(参考速度vr分别取0.3m/s和0.7m/s)下进行避障表现的对比。VSTTM改进算法参数q1，q2，q3与VSTTM相同，另取α=1，β=1，γ=3，λ=2。将相同条件下的VSTTM和改进后的轨迹进行比较，如图8(a)-(b)所示(图中改进算法使用VSTTM′ 表示)。图中黄色轨迹表示改进算法VSTTM′ 的轨迹，蓝色轨迹表示VSTTM轨迹。表2为VSTTM与改进算法同时成功避障(vr=0.7m/s)过程中的跟踪误差最大值的对比情况。

图8 VSTTM与VSTTM′ 轨迹对比

表2 VSTTM与改进算法跟踪误差最大值对比表

通过比较得出结论：

1) 改进算法在低速(vr=0.3m/s)情况能够成功避障，成功克服VSTTM算法的避障缺陷；

2) 在速度较大、与VSTTM算法同样成功避障的情况下，改进算法的跟踪误差更小。

5 结论

针对移动机器人对已有轨迹的跟踪过程探测到未知障碍物的避障问题，本文针对基于速度空间的同时避障与跟踪方法VSTTM的避障缺陷进行改进，设计了衡量跟踪误差和远离障碍物程度的目标函数。并根据机器人所处环境设计了跟踪状态与避障状态的切换机制。仿真测试表明，与原方法相比，改进后的VSTTM不仅解决跟踪直线轨迹时的避障缺陷，在同样成功避障的情况下拥有更好的跟踪精度。