基于多路线追踪的机器人局部路径规划与实验

牛作硕 宫金良 张彦斐

1(山东理工大学机械工程学院 山东 淄博 255000) 2(山东理工大学农业工程与食品科学学院 山东 淄博 255000)

0 引 言

路径规划是智能行驶机器人研究的重要问题之一，较好的路径规划方式可以让机器人平稳且快速地按照规划路线进行移动，以执行其要完成的相关任务。智能行驶机器人的路径规划可分为全局路径规划和局部路径规划，其中全局路径规划为机器人制定最优预设路线，并通过局部路径规划控制机器人的实时动作，完成机器人行走轨迹和预设路径的拟合工作。目前，国内外针对路径跟踪的方法已有大量研究，主要包括智能控制、最优控制、PID控制和纯追踪模型等方法。纯追踪模型作为其中较为简便的方法已被广泛研究使用。唐小涛等[1]针对自动导航插秧机路径跟踪系统，提出了一种利用模糊控制调整纯追踪模型前视距离的路径跟踪方法；汪小旵等[2]基于Kinect体感感应技术，结合纯追踪模型设计了一套温室果蔬采摘运输自动跟随平台；张攀等[3]采用偏差统计等方法对纯追踪算法进行改进，提高了飞机地面自动牵引滑行导航控制系统的精度。纯追踪模型算法的研究和改进可以提高机器人的控制精度，对智能机器人的发展具有重要的意义。

作为一种几何模型算法，纯追踪模型在使用时的路径拟合效果主要由前视距离决定。前视距离是指车辆在正常行驶过程中，车辆目前所在位置与要到达的目标点的直线距离。在建立数学模型时，前视距离需要根据车体与预设路径的航向角偏差和横向距离偏差选取合适的数值，受机器人启动时车体位置和姿态不同的影响[4-5]，如果设定的前视距离过小，机器人会因与预设路径航向角偏差较大而引起抖动现象，设定较大的前视距离虽然可以减小航向角偏差，但同时会引起机器人走捷径现象，导致路线的拟合程度降低。针对上述问题，本文提出一种双段式纯追踪模型，对已有的纯追踪模型的应用进行了改进，在机器人位于初始状态横向距离偏差和航向角偏差较大时，能够减小前视距离的取值，从而提高纯追踪模型的路径拟合程度，并以一款自行设计的智能送餐机器人为研究对象，结合嵌入式控制系统和MATLAB建模，对改进后的应用效果进行实地实验和对比仿真。

1 传统纯追踪模型原理

本文使用的智能送餐机器人采用后轮驱动、前轮转向的工作方式，通过矩形化转向梯形设计实现阿克曼转向功能[6]，机器人结构简化模型如图1所示。其中：A点为车体后轴中心点；l为机器人轴距；U点为机器人在转弯时后轮轴线延长线与前轮轴线延长线的交点，此时机器人以U点为圆心进行旋转前进；r为旋转圆弧半径，有lUA=r。

图1 智能送餐机器人简化模型

如图2所示，对符合阿克曼转向的送餐机器人车辆模型进行二轮车辆模型简化[7]。以车辆预设定路径为Y轴建立送餐机器人坐标系，其中：β角为机器人在运行过程中整体车身转向偏角；σ角为车体与预设路线走向的航向偏差角。以车身后轴中心点为车辆运动参考点M(x1,y1)，其横坐标值x1等于机器人车体距离预设定路径的横向偏差值d。由前视距离p(p>x1)取合适的长度后得到车体在偏离路线状态下要到达的预设路径上的目标点N。σ值根据机器人沿预设路径的航向逆时针旋转时取正值，顺时针旋转时取负值。根据图2中几何关系有：

图2 智能送餐机器人二轮车辆模型

(1)

如图3所示,以M点为原点，机器人车身行驶方向为Y轴建立坐标系，根据纯追踪模型原理，机器人的转弯半径可以由机器人与目标点的位置关系求得，有：

图3 智能送餐机器人纯追踪模型

y2=f(x2)

(2)

(3)

(4)

(5)

式(2)中函数f为预设定路径的几何函数，联立式(2)和式(3)可求得在前视距离为p时的N点坐标(x2,y2)，代入式(1)和式(4)，可得机器人此时需要的转向偏角为：

σ=arctan(2lp·cosγ)

(6)

即送餐机器人按照此偏角进行转向即可达到预定路径上的目标点。

2 纯追踪模型的基本原理与改进2.1 传统追踪模型的局限性分析

机器人在初始状态未处于预设路径的情况下，通过建立纯追踪模型，选取合适的前视距离并为机器人设定合适的转向角，机器人即可围绕固定圆心前进至目标点[8]。本文所设计的智能送餐机器人在送餐过程中，会涉及到躲避障碍物等多种任务执行情况，在执行完避障等任务后，机器人会重新置于路径追踪初始状态，并与预设路径会有一定的航向角偏差和横向距离偏差。

如图4所示，在常规情况下，机器人与预设路线航线角偏差和横向距离偏差较小或航向角偏差较大，但机器人当前航向呈靠近预设路线趋势时，使用传统纯追踪模型规划路线可快速拟合预设路径。

(a)常规情况 (b)初始航向角偏差过大 (c)初始横向偏差过大图4 不同初始状态追踪模型对比

当机器人与预设路线横向偏差距离大于两个车体转弯半径距离或车体呈偏离预设路线趋势且航向角介于0°～45°之间时，若前视距离取值较小，会出现到达目标点时机器人车体航向偏差角过大现象，继而导致机器人路线异常抖动现象；选取前视距离较大，航向角偏差减小的同时，却会出现机器人走捷径现象，路径拟合程度下降，此时传统纯追踪模型无法满足机器人能够良好追踪路径的要求，不再适用。当车体呈偏离预设路线趋势且航向角大于45°时，车体通过传统路径追踪到达预设路径时，航向角偏差将大于90°，与基准航向呈相反趋势，无法继续追踪路径。

以机器人到达目标路径点时航向与预设路径航向相同为机器人进行路径追踪的最优规划结果[9-10]，针对上述现象，本文提出一种两段式纯追踪模型，通过两次追踪实现减小前视距离的取值和减小到达目标点时的航向角偏差，从而提高路径追踪的准确性。

2.2 纯追踪模型的应用改进

图5 追踪模型邻接状态示意图

图6 直线路段补偿示意图

由图5与图6中几何关系知：s2=p2-d2，当d固定时，机器人前向位移与前视距离正相关,即s为最小值时，机器人前视距离p达到最小。为便于对比和计算，使用前向位移p代替前视距离作为路径拟合程度参考对改进追踪模型进行分析。由图5中几何关系可知，s=r1+r2·(1+sinσ)，在满足车辆最小转弯要求情况下，令r1=r2=r，可得机器人当前初始姿态下s最小值为r·(2+sinσ)，若机器人处于临界位置，全局最小值为2r；当机器人初始航向与X轴同向时，可得s全局最大值为3r。

3 实 验

3.1 机器人实验平台

如图7所示，本文设计的送餐机器人为一款自行设计的后驱四轮机器人，车体前后轴距为1.2 m，横向宽度为0.7 m，最小转弯半径为1 m，采用前轮舵机转向系统，并安装角度传感器实时测量前轮转向角度。

图7 智能送餐机器人

机器人使用北斗/GNSS高精度差分定位系统与导航控制器对车辆整体进行位置信息采集。机载北斗/GNSS定位系统由上海思南卫星导航技术有限公司提供，组成部分包括基站和移动站两部分，其中基站由一个M300接收机、一个定位天线、一套电台发送设备组成，架设在校园空旷处；移动站由一台M600接收机、两个定位天线、一个电台信号接收天线组成，安装在移动机器人本体。在正常工作时，基站通过定位天线获取实时站点坐标，与预设固定参考坐标做差后获得实时差分信号，并通过电台设备广播发送，差分信号由移动站接收后与机器人坐标做误差补偿可得到机器人精准坐标，定位精度可达2 cm，从而求得机器人与预设路径的横向偏差；通过移动站两个定位天线接收信号对比即可获取机器人车体航向角,精度可达0.1°，数据传输频率设置为5 Hz，继而求得机器人与预设路径航向角偏差。

3.2 路径追踪实验

实验选取山东理工大学为测试环境进行路径规划，考虑到校园内路况比较平坦，忽略车体在运行过程中的水平落差。如图8所示，首先需要对实验路段进行全局路径规划，并采集路段关键节点坐标，制定预设路径，得到世界坐标系中预设路径位置以及预设路径的航向角，并建立数据库，机器人运行时将以数据库中路径信息为基准进行纯追踪模型路径追踪，通过北斗/GNSS高精度差分定位系统得到机器人实时位置与航向角后对比数据库数据，得到实时横向偏差与航向偏差角。为得到理论追踪轨迹与机器人实际运行路线偏差，将带有标号的纸质标签固定在道路理论路线特征点位置，同时使用墨盒漏斗装载在机器人后轴中心位置进行运行路径记录。

图8 全局送餐路径

根据校园交通状况设定机器人无障碍运行时速度为恒速v=1.5 m/s，当车体横向偏差稳定在0.2 m内时，机器人达到误差稳定状态。选取NVIDIA JETSON NANO B为实验的主控硬件，正点原子STM32单片机做底层控制，在Linux操作系统环境下，使用C++图形用户界面开发软件QT进行程序编写，通过机器人操作系统ROS(Robot Operating System)建立车体与工作环境模型，实时计算车体运行偏差状态并加以显示记录，以具有感知能力的人群为校园送餐的主要避障对象，设计机器人执行避障任务后的四种运行状态，路径追踪实验和实验结果如表1和图9-图12所示。

表1 实验数据

图9 实验一

图10 实验二

图11 实验三

图12 实验四

通过重复实验和采集整理数据后可以看出，由于机器人转向结构存在一定误差和转弯过程中会出现车体的侧滑、动作延迟等现象，理想路径和机器人实际路径存在一定的滞后性和一定的偏差，在车体到达预设路径时，航向角偏差在5°以内，横向偏差在0.1 m以内，前向位移在6 m以内，总体满足实际应用效果。可以看出改进后的纯追踪模型可以在机器人避障等原因偏离预设路线较大范围时，在较短的前向位移内使机器人迅速、准确地返回原路线继续执行任务。

4 结 语

为了解决智能送餐机器人使用纯追踪模型规划路径时，由于初始位置航向偏差角和横向偏差距离较大导致机器无法良好拟合预设路线的问题，本文对智能机器人进行了简化建模和路径规划分析，提出使用两次纯追踪模型的方式，对传统纯追踪模型进行了改进。改进后的追踪模型在使用先后两次相切路径追踪后，在保证机器人到达目标点后与预设路径航向相近的同时，减小了前视距离的取值，提高了路径追踪的拟合程度。使用自行设计的四轮智能送餐机器人在校园内进行实地实验，设计四种初始运行状态并进行多次实验，通过记录机器人运行路径和理论路径进行对比，得到机器人在使用改进纯追踪模型到达目标点航向角偏差在5°以内，横向偏差小于0.1 m，完全满足路径规划需要，从而证明了本文算法能够为机器人局部路径规划提供一种有效的追踪方法，提高了其路径追踪的稳定性和有效性。