基于AGV 的复杂场景视觉SLAM 惯导系统设计

冯悦鸣，吕 勤，李 岩

（国网浙江省电力有限公司嘉兴供电公司，浙江嘉兴 314033）

文献[1]基于图模型提出一种视觉SLAM 系统误差动态补偿方法，利用改进生成树遍历得到包含完整场景的最小关键帧子集，采用基于双窗口约束的平差策略限制计算量，通过四叉树均衡化算法进行三维地图点选取，可以得到更加精确的位姿估计与三维重建结果，提升位姿估计精度与重建模型的一致性。文献[2]提出一种同时定位与地图创建算法，利用RansacPnP 算法得到亚像素级的匹配点对，为初值进一步最小化重投影误差进行局部优化，基于匹配点对描述子距离提出一种不确定性模型，进行实地测试来判断其实际精度和鲁棒性，具有良好的综合性能，满足工业现场要求。

在传统的视觉SLAM 系统中，当载体运动速度很快、环境特征缺失、图像清晰度较低时，摄像机采集的图像可能存在相邻帧的异常匹配特征或出现采集图像模糊问题[3]，导致SLAM 系统的鲁棒性和精度下降，甚至导致整个系统的失效。基于AGV(Automated Guided Vehicle，自动引导机器人）的视觉SLAM 惯性导航系统在一定程度上缓解了上述问题，提高了系统的鲁棒性和精度，减小了光照等外界因素的影响。因此，在复杂的环境下，基于AGV 的SLAM 惯性导航系统将具有更重要的研究地位和广阔的发展前景。

1 硬件结构设计

硬件结构包括发射器、信号处理器、接收器、传感器、计算机与基准尺、相机设备、AGV 系统、CPU 芯片等[4]。该系统具有多任务并行功能，且可完成测量任务，发射站负责发射任务，接收器的数量没有限制；系统具有较强的抗干扰能力，前提是确保发射站和激光接收器的光敏设备对于光波的敏感性相同、光波相同[5-7]。硬件总体构架如图1 所示。

图1 硬件总体构架

完整的AGV 系统包括车载系统（即下位机）、外围设施（如充电站）、控制系统（即上位机）。在AGV系统体系结构中，上位机作为控制终端，功能是调度系统任务，是系统的中心构造。控制终端具体工作职责是对所有车载系统传送的数据进行接收，同时实时监控系统的运行状态。除此之外，还要负责搭建人机交互界面，使接收指令任务、解析任务能够正常顺利进行；还具备分配任务规划路径调度车辆的功能，亦可完成交通管理。外围设备的功能是保证系统的安全、正常运作，隶属辅助设备之一。外围设备有导引系统以及激光反射板、通信中转站、能量供给站等。下位机负责AGV 的管控，借助通讯技术与控制技术进行通讯，在对下达的指令进行接收任务后，能够完成AGV 的自主定位、追踪路径的规划、系统运动控制以及对障碍的检测等[8-9]。

1.1 AGV系统

AGV 系统的控制借助物流调控上位系统、AGV地面控制系统、AGV 车载控制系统共同协作[10-13]。在AGV 系统中，导航定位技术是整个系统的核心，内部构成有终端控制系统、车载控制系统。终端控制系统的主要构成包括对地图的管理、对任务的调度、对路径的规划等；车载控制系统的主要构成有导航系统、定位系统、驱动设备等，工作内容为无线通信的接收以及完成对接收数据信息的交互任务。AGV 总体结构如图2 所示。

图2 AGV总体结构

AGV 的工作原理：沿着磁条进行自主导航任务，到达指定区域后停止磁导航任务，自动切换为二维码导航，在执行完二维码的定位以及导航任务后，前往特定站点实现区域服务器与中央服务器的通信，接收指令信息并对路径进行规划。

1.2 接收器

其工作原理是将接收器固定在后交互式运行形式中，并将发射站固定在AGV 车体上，如工装房中的墙壁、地面等合适位置。在AGV 车体进行运动的过程中，发射站将保持对空间环境的持续扫描状态。一旦发出的激光信号被固定接收器接收后，便能够实时定位解算出发射站与全局导航坐标系相对位置关系、姿态信息。接收器是导航领域的地标位置点，三维坐标需要保证高精确度，并且需要进行统一化保存在导航坐标系之中。

综上所述，该线路的牵引供电系统在单边供电模式下可以满足2列AW3列车同时起动的负荷需求，在大双边供电模式下可以满足4列AW0列车同时起动的负荷需求，其牵引供电能力满足标准及设计的相关要求。通过对测试数据进行后期分析，可以发现既有设计单位的基本测试数据能满足线路初期和负载特性需求。

1.3 传感器

激光雷达工作原理类似于声呐，但是相比声呐系统激光雷达将激光作为外部发射以及接收的信号源，这保证了测距过程的速率有了明显的提高。而且激光雷达可以使测距的误差明显缩小，提高系统的精确度，减少了测距过程所造成的结果不准确现象的发生[14-16]。激光雷达将获取到的环境数据信息依据分散点云的形式予以展现。点云可以较详细地对信息进行描述，蕴含的信息量比较丰富。蕴含的信息包括比较准确的距离以及角度等数据信息。通过这些，激光雷达作为环境传感器能够直接利用点云状态信息的绝对不变性，将相邻的每一刻的激光雷达获取到的点云数据进行信息的匹配与描述，完成前后时刻机器人相对运动的运算主要包括旋转和平移的运动，借以完成对自身的定位。

2 软件功能设计

2.1 融合人工信标的多目SLAM算法流程

SLAM 系统是非线性优化框架，由前端的位姿跟踪和后端的全局优化构成。系统的后端以全局闭环检测为基础执行针对位姿的优化任务。与传统的纯人工SLAM 算法相比较，基于AGV 的复杂场景视觉SLAM 算法具有更强的光照变化适应性，并且在一定程度上能够减少人工信标的使用数量，降低了人力、物力、资源的投入量；除此之外，系统的鲁棒性和实用性也得到了明显的提高。SLAM 算法流程分为前端与后端，具体流程如图3 所示。

图3 SLAM算法总体流程图

在SLAM 系统跟踪过程中，以载体的运动模型为参考，与匀速模型类似，进行当前运动状态的位姿判别。在载体进行平稳运动时，模型能够达到较好的运行效果。然而，一旦载体运动速率超出适宜值，很容易出现跟踪失败的结果。视觉惯导系统的工作原理是以帧速率为参照标准进行传感器的姿态、差值以及速率的追踪，能够获取到可靠性比较高的位姿判别结果。

2.2 AGV驱动功能设计

AGV 的运输能力以及传动性能重点依靠驱动模块，所以驱动模块的设计在AGV 的设计中很重要。首先根据参考驱动模块的结构特点设计了差速驱动模式及形式构成；第二步将分析后得到的驱动模块主要性能参数作为基础对驱动模块的主要构成部件进行选取。

驱动模块的主要构成为电动同轴安装、电机交错安装。电机同轴安装的安装原理为:将驱动电机同减速器的输出设备进行连接。此种布局构成简易，需要的设计需求较低，然而对于驱动模块的横向尺寸的标准却比较高，比较适合应用在整体尺寸需求较低的AGV 中。电机交错安装是将驱动电机和减速器相连接后，借助传动机构同驱动轴相连。这种布局方式较同轴安装方式更能够实现两级减速的效果。该种方式对减速器具有较低的需求标准，并且驱动模块的横向尺寸不大，整体构架显得更加紧凑，驱动轮同轴安装的方式，更加适用于横向尺寸要求较高的AGV中。

2.3 SLAM惯导系统的姿态解算

姿态解算的作用是负责姿态矩阵的实时更新。姿态解算所得的结果可以作为速度、位置的解算所需依据，所以姿态解算的结果准确度在整个惯导系统解算中很重要。普遍的惯导系统的姿态解算的方法包括欧拉角法、四元数法、旋转矢量法等。欧拉角法的解算原理是借助欧拉角微分方程执行相关计算过程，进而能够直接得到运算结果。但是，由于欧拉角微分方程包含了三角函数分式，因此在计算时容易出现零分现象。姿态解算中经常运用四元数法，即将从b系到n系旋转的角速度用Wnb表示。

由此可知，能够借助一个四元数Q对b系相对于n系的旋转进行表示，那么这个四元数的微分方程可以表示为：

式中，Q=q0+q1iq2j+q3k，上式用矩阵可表示为：

四元数的实部由q表示，3 个虚部由i、j、k表示，与空间三维坐标系的3 个方向分别对应。四元数是对刚体定点转动运动描述的一种形式。Wnb代表b系到n系旋转的角速度，Q代表着四元数的微分。

在针对四元微分方程的求解运算中，若获取到每一个时刻的Wnb值，则可以计算出q0、q1、q2、q3的值，之后利用相关关系即可完成载体姿态的运算过程，分析出载体姿态的结果。

3 实 验

为验证基于AGV 的复杂场景视觉SLAM 惯导系统设计合理性进行了实验验证分析。

3.1 实验环境及条件设置

实验环境分为动态环境和静态环境。在动态环境中，AGV 系统是沿着铺设标志的路径进行匀速直线运动，那么当AGV 小车停止运动时，通过输出系统的定位信息来获得静态定位效果。由于试验中地面打标精度有限，且初始定位姿态的设定也包含一定的误差范围，因此很难获得定位数据的绝对误差。因此，测试实验中的变化量仅限于摄像机是否嵌入。将文中方法与文献[1]和文献[2]方法进行对比，其中，文中对摄像机引入前后定位数据的波动进行了研究和分析，得出了AGV 定位的比较效果。

在AGV 的运行过程中，同时记录摄像机引入前后的定位数据信息，然后将获取的信息导入Matlab，利用斜线进行动态定位的数据拟合。将拟合结果作为动态测量值的期望值，将静态定位数据的平均值作为静态测量值的期望值，然后进行测量，并将期望值绘制在图形中。需要注意的是，图像中的期望曲线是在摄像机融合后，即嵌入摄像机的定位数据拟合后形成的。在没有嵌入式摄像机的环境中，期望曲线与嵌入后的曲线基本吻合，因此在图像中没有显示。

3.2 实验结果与分析

数据波动曲线分为两部分。前50 组数据显示的是AGV 在恒速和静止状态下的定位数据，波动图像如图4 所示；后50 组数据是AGV 在匀速直线运动时的定位数据波动结果，对比如图5 所示。

图4 静态环境下实验对比结果

由图4 和图5 可得出实验结论：

图5 动态环境下实验对比结果

1）静态环境下

为确保观测更加明显、清晰，在绘制图像时将融合摄像头后的X轴坐标变化曲线沿Y轴向上平移了0.02 m 的距离，把期望曲线沿Y轴向上平移0.04 m 的距离。

2）动态环境下

在运动环境下，没有融合摄像头传统方式的定位结果数据波动比较大，融合了摄像头的基于AGV的复杂环境SLAM 算法系统的定位数据结果波动比较小，定位准确度很高。

实验显示，无论在静态环境中还是在运动环境下，嵌入摄像头的基于AGV 的复杂环境SLAM 惯导系统的定位数据波动程度均小于未嵌入摄像头的传统AGV 运动系统，即前者的定位准确性明显更高。

4 结束语

当运动载体的运动速率超过一定范围时，易使AGV 系统的鲁棒性和精度显著降低，而基于AGV 的复杂环境的SLAM 惯导系统可以在不受外界干扰的情况下，借助于高频的角速度和加速度特征，弥补视觉传感器的不足，此外，在获得稳定的观测数据的同时，还能有效地校正数据的波动，因此，基于AGV 的复杂环境的惯导系统的设计具有很大的市场需求和发展前景。

除此之外，在进一步探讨研究基于AGV 的复杂惯导系统时，切记要将光照、动态障碍物遮挡、噪声影响等外界因素纳入考虑之中，针对以上问题，可以尝试把惯导引入SLAM 系统中，通过这种方法将原系统前端出现的诸多问题加以改善与解决，尽量避免它们对运行效果产生影响，从而进一步提高该系统的鲁棒性以及准确度。