俯视相机辅助的地面机器人导航实验平台

曹明宇，袁家遥，张志明，赖志杰，朱子祎

（同济大学电子与信息工程学院，上海 200092）

0 引言

自主移动机器人在现代工业、农业、医疗、服务等行业得到了广泛应用，其感知环境信息并依此实现定位和导航功能，是机器人实现自主运动的关键问题之一［1］。在城市安全、搜索救援等场景应用中，由于环境复杂难辨，机器人的环境感知能力受到极大限制，本体搭载的传感器提供的信息有限，往往无法合理规划路径并导航到特定地点［2-3］。近年来，由无人机搭载俯视相机跟踪拍摄，直接获得全局地貌信息后，为地面机器人规划路径并引导到达指定位置的联合作业方案越来越受到研究人员的关注［4-6］。但现阶段由于无人机成本高、续航时间短、操控复杂且需要专用场地或空域，在高校新工科工程教育实验和综合创新实践时受到限制，难以开展普遍性的实训教学。学生在动手实践过程中需要综合理解和掌握专业课程的基础理论和方法，完成实验教学任务与创新拓展训练［7-8］。本实验项目针对人工智能＋机器人专业方向人才培养需求，简化设计方案，在室内环境中搭建基于俯视相机辅助下的地面机器人导航系统实验平台，完成相关核心功能模块和控制算法的研究与实现，并通过软件模拟仿真与实际场景测试，验证其可行性。

1 系统功能架构设计

地面机器人导航系统的设计与开发是一个典型的复杂工程问题，涉及传感检测技术、电子信息处理、嵌入式系统、自动控制原理、计算机控制以及人工智能等多学科的理论知识和综合实践技能，完成方案论证、建模仿真、软硬件设计、算法编程、系统联调等研究工作。其所要实现的主要功能，是控制机器人在周边环境中沿事先规划好的最优或者次优可达路径运动，并保证过程中其位置偏差始终在一定精度范围之内。系统实验平台的硬件实体对象主要由俯视相机、计算机和地面机器人组成，组合完成周边环境感知单元、导航路径规划单元、任务控制执行单元和辅助监控计算机单元等功能模块。实验测试场地中地面机器人导航系统的硬件单元如图1（a）所示，整体功能架构如图1（b）所示。俯视相机安装于实验场地上方，实时采集地面机器人及其所处环境的图像信息，传送到计算机系统进行图像处理，检测周围障碍物完成全局路径规划，并对机器人进行实时目标跟踪定位，将解算得到的位置误差与预期姿态通过无线方式传送至移动机器人；地面机器人（内置处理器）利用所接收到的位置误差和期望姿态，对导航路径进行跟踪，先通过EKF 扩展卡尔曼滤波融合估计得到机器人当前姿态，再运行复合模糊PID轨迹跟踪算法，解算得到预期线速度与角速度信息，并发布给机器人AI-KIT 底层控制系统，控制机器人运动的速度与方向，从而使机器人沿预设导航路径行驶；监控计算机单元是实验平台的人机交互界面，主要功能包括系统参数设置、机器人运动状态可视化、路径规划和任务控制的状态监视等功能。

图1 俯视相机辅助地面机器人导航系统整体功能架构

2 实验平台构建实现

2.1 地面机器人实验对象

地面机器人的移动机构是完成任务的基本保障，考虑到轮式机器人机械结构较为简单，相对易于控制，且转弯控制精度高，实验对象地面机器人选用乐博士公司的AI-KIT 人工智能ROS 机器人通用教学平台［9］，其采用两轮差速式底盘，搭载有工控机、激光雷达、深度摄像头、IMU 等模块，核心处理器采用Intel NUC（Core i3）／4GB RAM／250GB固态硬盘，由外部锂电池供电，采用ROS 机器人操作系统［10］作为软件环境，提供人机交互友好的可视化界面，通过订阅相关话题可以显示机器人模型、地图、激光数据、轨迹等信息。机器人的两个动力轮对称分布于本体底盘的两侧，由两个电动机独立驱动，通过给定不同的速度实现转向控制；底盘同时配有前后两个万向轮，用于辅助支撑。由于移动机器人是一个复杂的非完整约束系统，其动力学模型较为复杂，受干扰情况不稳定，建模准确度不足，因此在当前应用中主要基于运动学模型进行理论分析、仿真与调试。地面机器人的两轮差速底盘基本结构及其运动学分析数学模型如图2中所示，车体速度为v，左动力轮速度为vl，右动力轮速度为vr，车体自转速度为ω，转弯半径为R，两轮之间距离为D，两轮到车中心的距离为d，右轮到圆心距离为L。将机器人整体运动的预期前向速度v和转动角速度ω解算为左／右动力轮的独立速度vl和vr后分别控制，满足约束方程：

图2 两轮差速底盘基本结构及其运动学分析数学模型

2.2 周边环境感知单元

实验系统中，周边环境感知单元由俯视相机和计算机组成，利用图像处理技术模拟人类视觉处理功能，从地面环境事物的图像中提取足够丰富的信息，提供地面机器人的实时位置，以及识别机器人周边环境的特征。

2.2.1 视频图像预处理

由于相比彩色级和灰度级图像，原始图像合理二值化后，在压缩减少信息量的同时，可继续保留图像主要特征，做相关运算时有更好的去噪作用和相关性能［11］。为提高机器人导航系统处理速度，在实际操作中，首先对采集到的单帧周边环境图像进行灰度化、降维和滤波增强等预处理工作，然后参考图像的灰度直方图选取二值化阈值，最终实现二值化处理，也便于后期路径搜索与全局规划。

2.2.2 相关滤波器的跟踪定位

目标跟踪为地面机器人提供实时定位信息，本实验平台中选用基于相关滤波器的跟踪（Correlation Filter-based Tracking，CFT）算法，典型的CFT 算法有KCF、DSST、STC、SAMF 等［12］。使用相关滤波器进行目标跟踪的工作过程如图3（a）所示：在首帧地面环境图像中机器人位置处提取图像块特征，用余弦窗口平滑边缘后训练得到相关滤波器的结构参数；通过离散傅里叶变换（DFT）执行相关滤波操作；在后续到来的每1 帧中，根据前1 帧给出的机器人位置信息提取得到新的图像块，再通过傅里叶逆变换（IDFT）得到该帧图像的置信图，其最大值所对应的像素坐标即为地面机器人的新位置，并由此训练更新相关滤波器的参数用于下1 帧的目标检测。核相关滤波算法（Kernel Correlation Filter，KCF）引入该方法使得算法更具鲁棒性，能够处理非线性分类；训练过程则被转化为岭回归问题以避免训练时的过拟合，且具有速度快，效果相对较好，结构和实现简单的优点［13］，实际跟踪效果示例如图3（b）所示。

图3 基于相关滤波器的跟踪定位

2.3 导航路径规划单元

机器人的导航，即在外部传感器的辅助下沿规划出的路径安全地运动到达目的地，基于地图的路径规划是其核心技术之一［14］。为实现机器人的自主导航，该单元首先要获得环境相关的地图信息，然后进行路径规划，最后将结果数据（如多个途经点坐标等）发送给机器人，使其通过机器人本体中的任务控制执行单元实现自主导航的功能。

2.3.1 代价地图

环境代价地图是基于地图导航的基础，其表示方法可分为栅格、几何以及拓扑。常用的栅格地图表示法将整个环境分为若干相同大小的栅格单元［15］，每个栅格的权值表示机器人在此处会与障碍物发生碰撞的概率，即机器人运动到该栅格时需付出的代价。为简化地图的构建与维护，尽可能保留地面机器人导航所需的环境信息，实验设计采用俯视相机获得忽略环境高度信息的二维地图来近似表征静态环境，机器人可据此地图进行全局路径规划。

2.3.2 路径规划

路径规划要求机器人根据某种优化准则，在地图上寻找一条从起始点到目标点不经过障碍物的代价最小的路线。经典的全局路径规划算法有Dijkstra 算法和A*算法等［16］，可以静态地规划出最优或次优路线。其中A*算法是一种启发式搜索算法，规划每步路径的同时会搜索下一步可达的各个节点并进行评估，评估函数形式如下：

式中：f（n）为对当前点n 的评估函数；g（n）为从初始点到当前点的实际代价值；h（n）为当前点的启发值。

通过启发值的限制使得那些远离最优路径区域的栅格单元不被检索，与Dijkstra 遍历搜索算法相比具有较低的时间和空间复杂度。在启发值函数设计比较合理的情况下，A*算法所得近似最优路径与Dijkstra算法所得最优路径相差不大，故结合实际应用情况优先考虑使用A*算法用于寻路，实现基于地图的路径规划。

2.4 任务控制执行单元

路径规划的最终效果需要运动控制系统的执行体现，任务控制执行单元负责实现机器人的路径跟踪，根据机器人当前的运动状态及其与周围环境之间相对运动的关系，按照一定的策略综合控制机器人的运动速度和方向，使实际运动路径与理想规划路径能够满足指标要求［17］，实现真实路径与规划路径的良好复现。

2.4.1 导航路径跟踪控制架构

基于机器人运动学模型的导航路径跟踪控制一般由运动模型控制和电动机转速控制两层结构构成。其中外层的运动学模型控制器不需要考虑机器人的动力学特性，可以根据当前实际位姿与期望位姿的偏差计算得到地面机器人的期望角速度与线速度。经过解算转变为驱动电动机的预期转速，将其与当前实际转速的偏差输入内层的驱动电动机转速控制器，解算后输出电动机控制指令。路径跟踪控制的整体结构如图4（a）所示，AI-KIT 移动机器人的系统底层控制结构如图4（b）所示。具体实现过程中，机器人运动控制节点move＿controller 将路径跟踪控制中计算出的机器人运动速度信息robot＿vel 通过串口通信发送给下位机嵌入式控制板。下位机中根据机器人运动学模型进行解算，将移动机器人的预期速度转换为左／右两个动力轮的预期转速，然后通过相关电气接口将对应占空比的PWM驱动波形发送给电动机驱动板，控制机器人动力电动机转动，通过编码器反馈实际转速，通过外部传感器获取实际位姿，完成双电动机转速的闭环控制，使车轮转速跟上预期速度。

图4 基于机器人运动学模型的任务控制执行单元

2.4.2 双闭环模糊控制器

反馈闭环控制系统控制精度高，能够较好地抵抗干扰，但参数设定不合适时会存在稳定性问题。机器人底层的速度控制环结构比较简单，控制器采用常规PID算法即可达到较优的效果；但上层运动学模型控制器若采用传统PID控制算法，在轨迹曲率变化较大或连续变化处，机器人往往难以快速跟踪，甚至出现不稳定的情况。这是因为机器人是复杂的非线性系统，而传统PID控制必须依赖精确的模型进行参数整定，减弱了机器人对不同路径的适应能力。实验中围绕前述机器人运动学模型，将模糊控制等现代控制理论与方法与PID 控制相结合，实现基于复合模糊PID 算法［18］的路径跟踪，参数自整定模糊控制包括模糊化，制定模糊规则，清晰化等组成部分。在路径跟踪控制系统中，计算当前机器人位置与期望路径的偏差，以及偏差的变化率，根据给定的模糊规则进行模糊推理得到模糊参数，最后经过清晰化处理输出实时优化后的Kp，Ki，Kd3 个PID参数，完成上层运动学模型控制器。

2.4.3 姿态定位融合

实验中基于多传感器数据融合的方法测量位姿，根据系统物理模型以及噪声统计特性，利用一定的估计准则来获得系统的最优状态估计［19］，在一种传感器测量数据失真或者无效时，自动切换使用系统中其他可用传感器的测量数据；另一方面，多传感器数据融合处理后能得到更真实的测量结果。机器人通过编码器推算轨迹，直接对距离做分解，累积误差小，在航向角较准确的情况下做修正，最终精度可达到0.1%以内。为获取较准确的航向角θ，实验中调用ROS Navigation stack 中的robot＿pose＿ekf 包，通过扩展卡尔曼滤波器（EKF）对IMU、里程计odom的数据进行融合，估计地面移动机器人的真实姿态，输出odom＿combined消息。实际测试发现，IMU信息的协方差矩阵中代表机器人航向角分量的协方差数量级为10－6，而里程计信息的协方差矩阵中机器人姿态分量的协方差数量级为10－3，两者之间的数量级相差很大。故在进行EKF 融合时，会更“相信”IMU所提供的姿态信息。如机器人在转动过程中轮子发生了打滑，用编码器推算出的姿态一直在旋转，而其实际姿态并未发生太大变化，通过EKF 融合后的信息则可以纠正此种情形下的错误估计，使姿态信息更加可信，所以实验中订阅使用odom＿combined消息输出的姿态信息。

3 实验测试与结果分析

在实验室环境中搭建实验操作场景，如图1 所示，包括俯视相机、计算机、地面机器人和试验场地。针对项目背景中所述问题，为验证实验平台方案的有效性，设计与实现基于俯视相机的图像采集与地图处理、基于A*算法的路径规划、基于KCF算法的定位、复合模糊PID路径跟踪等功能模块，并进行了Matlab 仿真测试与在AI-KIT移动机器人平台上的实际测试。部分综合评价指标、测试结果（平均值）和测试方法如表1所示。

3.1 基于地图的导航路径规划

实验示例中（见图5），地图中白色部分为可通行安全区域，黑色部分为障碍区域，灰色部分为障碍膨胀区域，黑色和灰色部分不可通行。在可通行区域内部选定起点（左上方）和终点（右下方），规划路径用黑色线条表示。如图5（a）所示，由于A*算法的贪婪性，所得路径离障碍边界过近（特别是转弯处），会因为机器人本身物理尺寸，在运行中发生碰撞影响通行性。对障碍边界进行膨胀处理后，效果如图5（b）所示。此时获得的路径基本位于可通行区域的中央，如图5（c）所示，避免了障碍碰撞，但仍存在一些角度变化较大的拐点，不利于路径跟踪控制。为消除这些拐点，对途经点进行插值采样，然后通过三次样条方法得到较为平滑的路径，如图5（d）所示。

表1 部分综合评价指标和测试结果

图5 基于地图的导航路径规划

3.2 定位测试结果及分析

测试实验地面场地为8 m×8 m，在俯视摄像头正下方区域约为4.12 m ×4.12 m，定位测试轨迹如图6（a）所示，图中绿色轨迹为使用激光雷达SLAM 测定的定位结果，红色轨迹为基于KCF 算法的定位结果，计算可得全程平均误差约为80 mm。由于俯视相机标定与坐标系转换存在一定的误差，再综合考虑机器人本体尺寸与地面范围大小的因素，该定位误差在修正后可进一步减小到50 mm的可接受范围内，能够满足机器人定位需求。

图6 地面机器人定位测试及路径跟踪控制

3.3 路径跟踪测试结果及分析

在测试实验地面场地中分别选取起始点和终止点，由导航路径规划单元计算得到任务路径，地面机器人从起始点出发，在任务控制执行单元控制下沿所设定的路径运动，抵达终止点后停止。机器人的路径跟踪控制移动轨迹如图6（b）所示，图中绿色轨迹1 和2为理想规划路径，红色轨迹1 和2 为融合后的实际运动路径，轨迹数据离线计算后可知，路径跟踪过程中的最大误差为73 mm，全程平均误差均在48 mm 以内。综合考虑机器人本体尺寸与场地范围大小等的因素，该误差同样在可接受范围内，能够较好地实现路径跟踪。

4 结语

Matlab仿真与实际场景测试的结果表明，由俯视观测器提供全局观测，可以辅助地面移动机器人的导航，方案有效可行。在此实验平台基础上实现的综合创新性实验，可吸引学生积极参与，培养创新思维和工程实践能力。进一步工作包括：①俯视相机换用RGD-D相机，提供深度信息，对障碍物的感知更加全面和直观；②将现在固定位置安装的俯视相机，升级为空中无人机航拍，对复杂环境中信息获取处理和地空协调等提出更高的要求；③结合实际需求开展针对性研究，如室内／室外环境中的障碍物探测及机器人主动避障技术等，优化算法以增强系统的鲁棒性和实时性。从而增强实验功能和效果，更好地营造大学生创新创业实践训练良好环境，服务教学实践和科研实践。