基于特征分割与匹配的智能叉车定位算法

丁松，周奎，李凯，王鑫，蔡营，梁黄黄，付勇智

（1.湖北汽车工业学院，湖北 十堰442002;2.东风汽车集团股份有限公司 技术中心，湖北 武汉430000）

定位技术是智能叉车自主导航功能中关键的技术之一。刘洋提出了位姿估算矫正法，实现了基于激光的AGV 定位［1］；肖载鸿使用二维码作为视觉路标，利用二维码的定位信息对AGV 进行位姿校正［2］；杨玉明基于惯性导航原理提出适用于全向AGV 的定位方法，仅使用惯性测量单元和编码器对AGV 进行定位［3］；魏焱提出了在单目视觉条件下实现对移动机器人进行定位的方法［4］；李飞等人提出了基于两级WiFi 指纹的定位算法［5］；黄百川等人充分利用了低功耗蓝牙（BLE）的3 个独立通道并提出了一种不确定的加权三边测量方法从而确定用户最佳位置［6］；廖凌俊提出了一种基于动态测距方程的ADS-TWR算法，修正了由运动产生的测距偏差，分析了影响动态测距的因素，最后结合扩展卡尔曼滤波给出了的目标动态定位算法［7］，但是智能叉车的计算资源无法达到要求；张涛提出一种基于视觉信标和里程计融合的室内定位方法［8］，但叉车由于成本原因未安装里程计。由于期望尽可能减少环境和叉车本身的改造难度和成本，故仓库中不会再额外安装WiFi、蓝牙等；且为了降低成本，叉车使用了算力较弱的低成本处理器，也不会增设其他传感器。在仓库光照条件较弱，仅依靠叉车原有的单线激光雷达和环境中部署的若干反光柱进行定位的情况下，上述算法均不能很好适用。文中基于现有仓库和叉车固有的硬件条件，设计了基于单线激光雷达的特征分割与特征匹配的智能叉车定位算法，并应用于智能叉车，效果良好。

1 环境建模

环境建模主要是建立合理的坐标系系统（世界坐标系、雷达坐标系），确立坐标转换关系及航向角计算公式，建立反光柱系统与特征地图。在整个定位过程中始终将智能叉车视为质点。

1.1 坐标系统

如图1所示，在环境中选定合适的位置作为世界坐标系原点并建立世界坐标系xoy，x'o'y'为以激x'光雷达为原点建立的雷达坐标系。环境中布置的反光柱的坐标、确定智能叉车的航向角与位置均基于世界坐标系。图1 中（x,y）为智能叉车在世界坐标中的坐标位置，θ为叉车的航向角，（x's,y's）为智能叉车在雷达坐标系下的坐标，α为反光柱与激光雷达扫描平面的x'轴之间的夹角。激光雷达信息点用极坐标形式表示，坐标系如图2所示。极坐标（r,α）为激光雷达的信息点，与雷达二维直角坐标系下的坐标转换关系为

图1 智能叉车坐标系统

图2 激光雷达极坐标系

式中：r为反射点与激光雷达中心之间的水平距离；α为信息点在激光雷达极坐标系中的角度。激光雷达采集到的信息点在雷达坐标系中的精确角度计算如式（2）所示：

式中：λ为激光雷达的某个信息点在雷达坐标系中的精确角度;θf为接收到的数据包中第1 个信息点的绝对角度（在雷达坐标系中的精确角度）；i为该信息点在数据包中的索引；n为激光雷达的采样率。智能叉车航向角的计算公式为

式中：θ为叉车航向角；（xa,ya）和（xb,yb）为反光柱a、b在世界坐标系中的坐标；θa和θb为反光柱a、b在雷达坐标系中的角度；Ra和Rb为反光柱a、b与激光雷达之间的距离。智能叉车世界坐标的计算公式为

1.2 反光柱系统

为了提高激光雷达接收反光柱信息的质量，反光柱的摆放应遵循以下规则：1）在AGV 行驶路径上每个位置至少能接收到3个反光柱；2）叉车行驶路径两侧的反光柱应该呈不对称安装；3）在有玻璃或者有较强反光效果的位置应该安装反光柱以提高定位精度；4）在叉车的卸货站点处应该多安装几个反光柱来提高定位精度。

精确测量安装的每个反光柱的世界坐标，在应用环境中一共安装了n个反光柱，第i个反光柱的世界坐标用（xi,yi）来表示，n个反光柱的世界坐标构成反光柱世界坐标向量R：

第i个反光柱与第j个反光柱之间的绝对距离为

每个反光柱跟其他反光柱的绝对距离构成该反光柱的距离向量M，第i个反光柱的距离向量为

在智能叉车行走区域内精准布置的所有反光柱在世界坐标系下的距离向量构成了环境特征地图，用n×（n-1）二维矩阵G表示环境特征地图：

除此之外还需外存储与G中的距离向量Mi一一对应的R。根据式（5）～（8）得到当前探测到的所有反光柱的距离向量M'i。此时所有反光柱的距离向量构成当前探测到的反光柱距离矩阵G'。

2 定位系统

文中设计了基于特征分割与特征匹配的定位算法并将其应用于智能叉车的定位系统，定位系统主要分为数据输入、反光柱信息点处理、叉车位姿计算等几个模块。

2.1 定位系统框架

文中选用基于单线激光雷达的定位技术，虽然难度较高，但定位精度高、稳定性好。定位系统框架如图3 所示。基于TCP/IP 协议接收激光雷达的数据包，筛选出数据包中的反光柱信息点，用特征分割算法对信息点进行聚类处理，再使用三点定位算法［9］处理聚类结果，计算出每个反光柱在雷达坐标系中的坐标。根据计算得到的雷达坐标系坐标计算反光柱距离矩阵，利用特征匹配算法匹配出若干组反光柱的世界坐标与雷达坐标的映射关系，最后计算叉车位姿。

图3 定位系统框架图

2.2 特征分割算法

对筛选出来的数据进行特征分割，即将属于不同反光柱的反射点分割开，将属于同一个反光柱的反射点聚集在一起。由于激光雷达扫描到的信息点有如下特点：同一反光柱的信息点两两之间总是相邻，且相互之间的差异不大。若信息点与激光雷达中心点的距离的差值小于设定的阈值时，则属于同一个反光柱。

特征分割算法输入为点云数据集合R'和距离阈值，输出为属于不同反光柱的信息点簇。步骤如下：1）取数据集合R'的第i个元素和第j个元素（j=i+1）；2）判断第i和第j个元素差值的绝对值是否小于阈值；3）若是小于阈值则存储第j个元素，继续取第（j+1）个元素，跳回到2），否则存下此时将j赋值给h；4）判断h与j的差值是否大于2，若不是则继续取第（j+1）个元素，跳回到2）；5）若差值大于2，则1个反光柱元素分割完整，令i=h-1，跳回1）；6）数据集合R'遍历完成，则结束。

如图4 所示，三角形P为激光雷达，PA为激光雷达到反光柱的最大距离（激光雷达最大探测距离为6.5 m）。PA与圆O 相切，OC、OA为反光柱半径（反光柱直径为9 cm）。PC为在满足上述情况时激光雷达到反光柱的最小距离。阈值φ计算公式为

图4 阈值计算示意图

2.3 特征匹配算法

以反光柱之间的距离作为特征，对G与G'进行匹配，获取若干组反光柱在世界坐标系中与雷达坐标系中的坐标映射关系。

定位系统预存R和G。在最理想情况下，第i个反光柱与其他所有反光柱的距离信息的全集为Mi，进而形成G。把Mj的全体二元及以上子集作为第i个反光柱的幂集，记为P（Mi）。某时刻扫描到的反光柱中的第j个反光柱的距离向量为Mj。匹配原则如下：若Mj∩P（Mi）=Mj，则雷达坐标系中的第j个反光柱与世界坐标系中的第i个反光柱匹配成功，输出该坐标映射关系；若Mj∩P（Mi）≠Mj，则雷达坐标系中第j个反光柱与世界坐标系中的第i个反光柱匹配失败，雷达坐标系中的第（j+1）个反光柱继续与世界坐标系中的其他反光柱匹配。直至与其他所有反光柱匹配失败，则第j个反光柱匹配失败，取下一个反光柱进行匹配。

特征匹配算法输入为R、G和G'，输出为反光柱世界坐标与雷达坐标映射关系。步骤如下：1）取G'第i行并判断是否为G第i行的子集（i=0,1,2，…），若是则输出反光柱世界坐标与雷达坐标系坐标映射关系，i加1；2）若不是，则继续与环境地图矩阵的下一行比较，直至遍历完环境矩阵所有行，无法与环境矩阵匹配则匹配失败；3）取G'下一行元素并跳回到1）；4）遍历完G'所有行，结束。

3 实验设置及结果分析

3.1 智能叉车平台搭建

选取德国倍加福OMD10M-R2000-B23 的单线激光雷达，扫描范围为-180°～180°，搭载激光雷达的叉车实物如图5 所示。由于仓库面积较小且环境简单，构建的反光柱系统复杂度较低。

图5 叉车实物图

3.2 测试及结果分析

反光柱世界坐标为手工测量，存在一定的但在可控范围内的误差。在设置了反光柱系统的应用环境中手动驾驶智能叉车，激光雷达对环境中的反光柱进行扫描，实时获取实际数据，分别记录存储静态时的数据和动态行走时的数据。

以实际获得的数据作为实验数据，利用MATLAB 对特征分割算法及特征匹配算法进行离线测试，测试次数设为1000，将离线测试的定位结果与预设的参考点/参考轨迹比较得到算法准确率见表1，路面不平整车体发生倾斜时，算法准确率下降。

表1 特征分割与特征匹配算法准确率

静态时，将智能叉车在反光柱系统区域的若干位置静止停放，进行定位测试，将其结果与手工测量的叉车理想坐标进行比较。动态时，在反光柱系统中按预定轨迹移动叉车的同时进行定位测试，将定位系统记录的叉车移动过程中的一系列位置坐标与叉车预定轨迹比较。定位横、纵误差的计算如式（10）～（11）所示：

式中：Δx和Δy为定位横、纵轴误差；Δs为距离偏差；xt和yt为实际计算的横纵坐标；X和Y为横纵坐标的理论值。静态定位坐标误差如图6所示，具体数据如表2 所示。方差较小，定位稳定性较好，且在过程中没有出现定位失败的情况。

图6 静态定位误差曲线图

表2 静态定位误差

智能叉车进行了速度分别为5 km·h⁻¹和10 km·h⁻¹的动态定位测试，如图7 所示，定位坐标曲线与理想坐标基本重合，误差较小。动态定位的误差如图8所示，具体数据如表3所示。经过统计，叉车速度分别为5 km·h⁻¹和10 km·h⁻¹时的定位距离偏差的标准差分别为1.19 和2.05，定位误差波动较小，叉车具有较好的定位性能。

表3 动态定位误差

图7 动态定位轨迹曲线图

图8 动态定位误差曲线图

由于存在随机误差且未考虑传输时延和计算时延，动态定位时的误差较静态有所增大，但均在可接受范围内，且定位误差波动较小。综上，定位系统的定位精度和稳定性较高。该定位也为后续的导航提供的技术基础的合理性。

文中系统应用环境为湖北十堰某公司仓库，场景相对简单且单一，因此并未进行多项不同条件下的测试，如不同光照、不同地面平整程度下的测试等。而叉车以5 km·h⁻¹速度进行测试已能够满足在仓库室内场景的应用需求，能保证较高的定位精度和稳定性。

4 结论

对智能叉车的运行环境进行建模，并搭建了智能叉车定位系统，对该定位系统进行实车测试，结果表明在精确度和稳定性等方面均满足实车运行的要求。系统针对某工厂应用环境和其所提供的叉车进行开发，不具备环境和设备条件设计实验与其他定位方法进行对比，但经过实车测试，可满足后续的应用需求。文中所采用的定位方法需要在应用环境中安装反光柱，存在一定的局限性，后续考虑采用如SLAM等无需信标的定位方法。