基于视觉传感的全方位车的定位和路径规划研究

王立银，赵铁军

（沈阳工业大学 机械工程学院，沈阳 110870）

0 引言

随着制造业自动化水平的不断提高，以磁条导航、二维码及激光反光板的传统AGV具有成本较高、效率低和精度低等缺点，难以满足当代生产实际需求。一种依靠自主导航并且在空间环境相对复杂的环境下能够灵活运动的智能全方位车应运而生，而导航和定位技术是全方位车的核心技术之一，体现了全方位车的智能化和自动化程度[1]。

本文针对室内有光源的工厂对传统的AGV进行运动学建模和SLAM系统框架的算法改造及路径规划算法的改进，以实现对工厂内部各个零件的自动化运输工作。

1 全方位车的运动学模型的建立及其整体结构

1.1 全方位车的整体结构

全方位车的机械结构主要由深度相机、激光雷达、控制系统、底盘系统等4部分组成。由于工厂室内物件规格多样、高度不一，所以对小车建图的能力提出了一定的要求。为了保证小车的运行效率及兼顾小车的通过性，在系统视觉高度的拾取上进行改进，所用的传感器为安装在小车前上方深度相机和激光雷达，小车车体中间则放置了控制系统硅板，车体下方为其车轮驱动电动机、编码器及电池，小车底部前方为一副万向轮，后方两侧为一对由差速驱动的轮子，该设计可以使小车具有小范围运动能力较枪的优势。

1.2 全方位车的运动模型

以现实中的某一点作为原点，构建世界坐标系。假设两车轮始终保持平行，以小车底面中心点为小车参考点，同时假设小车和地面做无相对滑动摩擦的运动，则小车t时刻在世界坐标系中的位姿的计算公式为

式中：x、y为小车在世界坐标系中的坐标，θ为小车前进方向与X轴的夹角。

小车运动过程如图1所示。其运动过程可根据左右轮编码器的数据来估计小车的整体运动状态。

图1 小车运动模型

车轮的路径计算公式为

式中：r为车轮半径，k为编码器线数，n为某段时间内编码器的脉冲数，s为车轮的路径。

根据单位时间内轮子左右的编码器脉冲数量就可以得到短时间内左右轮的速度，继而可以得到小车的运动模型，其运用的公式如下：

式中：v为小车平均速度，vr为右轮的速度，vl为左轮的速度。

式中：w为小车平均角速度，wr为右轮的角速度，wl为左轮的角速度。

由于短时间内小车直线运动，故路程和位移做近似处理可以得到小车的运动方程为

式中：ut为该时刻小车的控制参数，wt为该时刻小车的数据误差项。

将式（5）展开得到：

根据式（6）便可由机器人上一时刻的位姿推算出下一时刻的位姿，但是在邻近较短时间内适用，时间较长后累计误差项将会对推算结果造成明显的影响，但利用电动机编码器与其他车载传感器进行配合则可以削弱累积误差。

2 小车的导航系统

不同的引导方式直接影响到全方位车的安全性、稳定性、准确性和经济性等各个方面[2]。本小车采用了SLAM框架的导航系统综合运用激光里程计和视觉里程计来保证小车导航的正确性和精准性。

2.1 小车的激光里程计

激光里程计所使用的激光传感器为二维激光雷达，测距核心由红外激光发射点与接收点组成，通过脉冲测量原理实现距离测量。其中激光雷达采集的数据具有一定的波动性，并且雷达每自转一周就会获得n个相对独立的测距数据，因此采用概率模型作为其观察模型：

式中：zt为雷达t时刻的观测量，Ct为小车的位姿，m为地图参数。

2.2 小车的视觉里程计

视觉里程计使用的是深度相机，与单目和双目相机相比，深度相机可以直接获得图片的深度值，大大减少了系统运行的时间，可提高系统的实时性。而且通过目前成熟的坐标系转换就可以将相机拍摄的实景坐标转化到最终成像的像素坐标下[3]。

视觉里程计的核心就是利用ORB特征点进行两张不同角度的图片的匹配工作，而ORB特征点由关键点（像素在图片里的位置）和描述子（该点的周围像素信息）组成，提取的主要依据就是该点周围的像素块明暗度是否达到要求的阈值[4]，但是传统的暴力匹配准确度差，而快速近似最邻近算法（FLANN）运行时间长，匹配的正确率不高的缺点制约着其应用，因此本文提出基于FLANN的新算法流程一定程度上缓解了当前的困境，该流程如图2所示。

图2 算法流程图

基于该算法改进后进行了程序的实际运行测试，对于小车运行过程拍摄的不同角度的图片流，通过图3、图4来模拟，运用算法后的匹配结果如图5所示，程序在Linux系统运行结果如图6所示。

图3 模拟小车拍摄图

图4 模拟小车拍摄图

图5 匹配结果图

图6 算法程序运行图

从原算法和本文改进算法针对同一组图片在本机运行得到的数据比较来看，本文改进的算法增加了配对的正确率，且在特征点的提取步骤上加快了速度，弥补了配对步骤中增加的时长，使得改进的算法整体运行时间也有所缩短。

3 小车的路径规划

小车的路径规划就是在前期小车通过导航系统和融合SLAM框架而建立的地图基础上对小车的行走路径做合理的规划，使小车能够在运行中尽量减少无用路径，从而提高小车的运行效率和降低能耗。传统的算法是随机数种子法（RRT），RRT算法以设定的起点为根节点，在采样空间中以随机采样的方式进行扩展，生成随机扩展树。当采样点到达目标点或在设定的目标点范围内，则停止扩展，随后随机拓展树快速将起点到终点的点连接起来，形成一条从起点到终点的路径[5]。

本文在RRT算法的基础上对其进行了路径的平滑处理，平滑处理就是在连接同路径点的基础上再对节点进行遍历同时继续生成新的节点[6]，并与原路径进行比较，如果符合转弯角度小和路径变短，在连接线上没有与障碍物接触则替换原有的节点，然后重复此步骤直到终结点，重复次数可以设置阈值。其示意图如图7所示。

图7 平滑处理RRT示意图

平滑处理改进的RRT算法基本步骤如下：

1）在工作空间初始化起始点XStart、目标点Xgoal、采样步长a、误差范围δ，先执行原RRT算法并生成路径；

2）从起始点XStart开始往外扩展，以已生成下一节点X2作为Xrand点并随机生成Xnew1节点；

3）比较Xnew1节点与节点X1到节点Xrand路径长度与弯折角度；

4）若沿着点Xnew1到点Xrand的比较项优于原节点，且连线没有经过障碍物，则将该节点替换原节点X1；

5）继续扩展并不断在其余点选定新的点Xrand，重复步骤2）到步骤4），直到达到设定Xrand迭代阈值次数。

将改进的算法在Matlab上进行模拟仿真，通过蓝色的物体模拟障碍物，黑色表示算法得到的路径，结果如图8、图9所示。

图8 平滑处理后RRT算法示意图

图9 原RRT算法示意图

通过Matlab软件对原算法和改进后的算法进行仿真后可以看到原算法路径弯折度较大，而改进后算法规划的路径弯折度大大降低，经过平滑处理后的算法规划的路径使小车在行进过程中由于转弯所带来的加减速和能耗将会大大降低，可以有效提高小车的运行效率。

4 结语

本文以全方位无人车定位与路径规划系统为研究对象，并构建了小车的运动学模型。小车的定位阶段利用ORB特征处理的图片流进行定位，本文针对传统FLANN算法匹配错误率和匹配速度进行改进，利用双向求交集的思想对其进行算法优化，经过实际运行程序和原算法比较，本文改进算法匹配准确率和匹配速度均有所提升。而在路径规划算法中对传统的RRT算法以弯折角度为目标进行优化改进，通过对比优化算法和原算法，发现改进算法使得路径距离减少同时增加了路径顺滑度，增加小车运行的平稳性，极大地提升了小车的运行效率。