激光导航小车入库规划

蒋如飞，张伟军

(上海交通大学机械与动力工程学院,上海 200240)

0 引言

导航小车是智能工厂柔性化重要的一环，通过导航小车在不同工位之间穿梭、配合，工厂内的生产线可以做到按需搭配，大大提高生产线的效率和灵活性[1]。根据原理，导航小车可分为路标导航和激光导航[2]，其中路标导航需要在工厂内铺设磁钉、二维码等路标[3]，而且小车的运动路线必须经过这些路标以矫正位置；激光导航则灵活许多，小车可以通过激光雷达实时确定自身的位置，而且不需要事先改造工厂，因此成为许多智能工厂的选择[4]。在导航小车结束任务后，一般需要进入事先规定好的停车位保管。然而，由于小车往往呈长方形，而目前的规划算法大多没有考虑到小车的形状和朝向，需要小车先走到位置，再旋转自身达到目标角度，所以算法目标位置为狭窄的停车位时，往往会规划失败。

在此，提出了一种导航小车，并针对入库问题改进了规划方法，可以让导航小车平稳地进入较狭窄的终点，使自动入库成为可能。

1 激光导航小车硬件设计

激光导航小车的硬件示意如图1所示，实物如图2所示。为了方便规划，小车采用了差速的控制方法，位于中间的一对主动轮是小车主要的动力提供手段。差速控制可以实现直行、原地旋转、固定半径转弯等功能，为更上层的路径规划提供了很好的基础。主动轮的速度控制与跟踪由电机驱动器完成，单片机只需要通过485通讯按格式发送固定加速度限制下的期望速度值即可。不只是控制电机，单片机还可以实时控制LED状态灯，收到主控电脑的设定值后可更改LED灯的颜色显示状态。在这种设计中，电机控制任务被分给3个单元执行，最需要实时性的速度跟踪部分由驱动器完成，需要稳定及时反馈设定速度值的任务由单片机完成，主控电脑只需要根据算法按周期发送速度值即可。

除了电机控制，传感数据的反馈也是导航小车正常工作的重要依靠。传感数据主要分为2个部分。一是周围障碍物跟小车的位置关系，由2个270°的旋转式激光雷达分布在对角感应。探测到障碍物后，得到的数据是障碍物相对雷达的角度和距离，需要合并到小车的中心坐标系，转换公式为：

图1 激光导航小车硬件示意

图2 激光导航小车实物

xagv=xbeam+ρcosθ

(1)

yagv=ybeam+ρsinθ

(2)

xbeam和ybeam表示雷达的位置；xagv，yagv表示障碍物在导航小车下的位置；ρ为障碍物到雷达的距离；θ为障碍物在雷达坐标系中的角度。

另一个重要的传感数据是主动轮的角度编码器，经由驱动器、单片机反馈到主控电脑。主控电脑通过2个主动轮的角度值可以得到小车实际行走的角速度和前进速度，由主动轮半径、主动轮距离决定，计算公式为：

(3)

(4)

ωwheel1为小车左轮角速度；ωwheel2为小车右轮的角速度；rwheel为车轮半径；dwheel12为左右轮距离；ωagv为导航小车角速度；vagv为导航小车的速度。

除了发送电机速度指令和接收传感数据，主控电脑还需要运行规划和导航算法，并从路由器里接收操作人员的运行命令。路由器是操作人员和导航小车交流的媒介，操作人员通过Wi-Fi连接到路由器后，通过TCP协议往指定的IP地址和端口发送命令，可实现手动控制小车行走或设定目标让小车自动导航。

2 动态窗口规划及改进

为了使小车平稳、无碰撞地到达指定的地点和角度，需要规划出一条满足小车动力学约束并避开障碍物的轨迹。常见的一种方法是动态窗口规划[5]，其主要由5个部分组成：

a.根据最大速度、最大角速度的限制离散采样目标速度，将小车尽量加速或减速到设定的速度。

b.根据每个采样的速度模拟一段时间，得到多条模拟的轨迹。

c.对每条轨迹打分，打分考虑的因素有离障碍物的距离、离目标的距离等，如发生碰撞则直接去除。

d.选出得分最高的轨迹，将当前速度和角速度下发到单片机执行。

e.每隔一段时间重新采样、打分；到达目标左右后停止，原地旋转到目标角度。

动态窗口规划兼顾了小车动力学限制、设定速度限制、障碍物限制等，而且计算量小，实时性强，是一种十分实用的基于采样的规划算法。然而，该算法仅把导航小车视为质点规划，规划轨迹在局部多为弧形。当小车不是圆形时，要经过狭窄的通道时往往需要较窄的边正对着通道口才能通过。因此对于长方形的小车而言，除非小车刚好在当前位置、当前角度可以以弧形轨迹或直线轨迹通过，否则很难用动态窗口规划的方法解决。所以，动态窗口规划难以处理导航小车入库的情况。对此情况，提出一种分段规划入库的改进方案，入库规划改进对比情况如图3所示。

图3 入库规划改进示意

对于狭窄通道，一个简单有效的方法是先走到正对通道的位置，原地调整角度，然后直接沿直线走进去即可。所以，本设计的改进将动态窗口规划分为2个阶段：第1阶段走到目标位置的直线可达且可以任意原地旋转调整方向的地方，然后调整角度到正对目标的角度；第2阶段沿直线规划走到目标点即可。由于目标直线可达位置有无数个，最佳的选择是直线上离目标位置最近的点。同时，考虑到车库周围的障碍物地图可能并不是一个长方形，而且最终目标角度的反向延长线可能会与障碍物发生碰撞，因此不宜直接以车库周围障碍物的对称轴或目标方向为直线，而是以最不可能发生碰撞的角度为准。基于以上考虑，本改进的具体步骤如下：

a.在障碍物地图中进行模拟，向左旋转目标位置处的小车，记录发生碰撞的角度；同理向右旋转得到右旋碰撞角度，取2个角度的均值为“退车”角度。

b.沿“退车”角度从近到远离散采样位置点，到某一点可以原地360°旋转仍无任何碰撞为止，记录该点为第1阶段目标点。

c.用动态窗口规划使小车走到第1阶段目标点。

d.沿直线规划走到最终目标点，调整角度到最终目标角度。

3 仿真实验结果

为了测试改进算法的有效性，本文针对性地设计了入库场景的仿真实验。实验中，目标点位于狭窄的通道中，通道两旁是虚拟的障碍物；仿真小车长1 m，宽0.5 m，最大速度为0.5 m/s，加速度为0.2 m/s2，最大角速度为40(°)/s，角加速度为1(°)/s2；小车要求离障碍物有0.2 m的安全距离，同时运行必须满足动力学的约束。改进前的动态窗口规划算法如图4所示，导航小车径直走向目标，但由于障碍物的环绕无法到达目标点，最终卡住无法前进。改进后的算法分2个阶段：第1阶段到达一个要求较低的目标点，如图5所示；在第2阶段，小车从第1个目标点出发重新规划，最终规划出一条接近直线的平滑曲线达到目标点，如图6所示。

图4 动态窗口规划轨迹

图5 改进算法第1阶段规划轨迹

图6 改进算法最终轨迹

经过改进，导航小车避免了因直接规划较困难的目标点所导致的失败，先走到较轻松的目标点；而第1阶段的目标点到最终目标点要比一开始的起点要容易许多，因此结合2个阶段可以较好地完成狭窄通道的路径规划。在障碍物不变的条件下，从目标左侧x坐标5～7 m，y坐标11～13 m的矩形范围内9个点出发多次仿真，改进前仅有1次成功，成功率为11.1%；而分阶段规划法改进后均能够到达目标点。

4 结束语

动态窗口规划是一种离散采样的规划方法，可根据各种条件灵活设置采样的范围、模拟的时间和模拟轨迹评价的方式等，因此可以满足激光导航小车各种动力学、速度的限制，同时可以很好地避障。然而，由于动态窗口规划没有考虑到模拟时间之后的轨迹情况，往往会在狭窄的地方规划失败。通过改进规划算法，本文提出了2个阶段的规划策略，其中第1阶段仍然使用动态窗口规划，第2阶段考虑到狭窄位置对导航小车角度的限制，将第1阶段的终点设置在直线可达且可任意旋转角度的地方。由于选用的第1个终点对于第1阶段规划难度低，同时也方便了第2阶段的规划，所以大大提高了规划成功的可能性。改进后，在原来动态窗口规划难以处理的入库场景中成功地完成了轨迹规划，实现了导航小车的自动入库。