1 引言
随着自动化技术的不断发展，移动机器人技术逐步被应用在工业、农业、救援、交通等各个领域。移动机器人路径规划是集动态环境检测、动态规划与执行、行为决策与控制等数项功能于一体的综合化智能系统,近年来已经成功地应用到工农业生产的各个领域[1-2]，并在各个特定的工作空间下分别实现了有效的解决方案。本文中针对未知离散障碍物环境下移动机器人的环境感知、导航控制系统进一步设计、完善与测试。
移动机器人系统主要包含环境感知、路径规划和运动控制3个过程[3-4]。环境感知过程通常采用激光SLAM技术进行，常用的感知算法包括GMapping算法、Hector算法等，然而对于给定的未知离散障碍物环境，此类算法构建的感知地图无法直接用于规划得到满足约束条件的轨迹。故本文中在GMapping构建感知地图的基础上，进一步设计感知数据处理方法，从而约束机器人的规划。
路径规划过程包含全局规划器和局部规划器。全局规划器基于感知地图信息进行规划，由于移动机器人的实际工作空间存在动态变化，通常将全局路径分割为多个局部过程，由局部规划器基于动态点云信息进一步规划。丁皓等[5]基于A*算法，构建移动机器人的全局路径。然而其通常耗费大量算力用于扩展无用节点且存在所得轨迹不平滑等问题，本文中采用JPS算法代替A*算法从而提高算法的效率。同时采用模型预测控制(model predictive control，MPC)用于局部规划，预测控制用于移动机器人的运动过程调节，能够提升计算速度，加快移动机器人轨迹跟踪的速度，已经应用到移动机器人控制的各个领域[6-8]。
传统的移动机器人自主导航需提前获取感知地图信息，即在已知环境下规划导航与控制。针对未知的离散障碍物环境，本文中创新性地构建了神经网络控制器以实现移动机器人的基于视觉的主探索方案。并在感知环境后自主切换至导航功能，以实现路径规划与跟踪。
综上所述，本文章节安排及研究成果如下：第二部分进行系统的阿克曼运动学模型构建及整体工作流程设计；第三部分针对离散障碍物信息构建神经网络控制器，控制机器人自主感知未知环境，并将感知环境信息进行预处理，从而约束机器人运动；第四部分根据预处理感知得到的环境信息，构建路径规划器，实现机器人的自主导航；第五部分进行仿真与实践测试，验证机器人系统整体设计的合理性及稳定性；第六部分进行全文的总结。
2 模型构建
以世界坐标系XOY为参考坐标系，构建移动机器人的运动学模型，如图1所示。
图1 运动学模型示意图
Fig.1 Schematic diagram of kinematic model
机器人底盘前后两轴中心相对世界坐标分别为(Xf,Yf)和(Xr,Yr)。前轮偏向角为δf，机器人的航向角为φ，模型的其余参数如表1所示，其中采用下标r和f分别表示后轴和前轴。
为降低模型复杂度，对其进行条件假设如下：
1) 机器人的运动过程是刚体运动；
2) 机器人在运动过程中，前轴两轮胎的偏向角大小相等，由舵机控制；
3) 机器人在进行转向运动时，自身的转弯半径等同于道路的曲率半径。
表1 运动学模型参数
Table 1 Parameters of the kinematic model
参量含义(Xf,Yf)机器人底盘前轴中心坐标(Xr,Yr)机器人底盘后轴中心坐标δf机器人前轴轮胎偏向角φ机器人自身的航向角vr后轴线速度,即机器人运动的线速度vf前轴线速度ω机器人运动的角速度l轴距,即底盘前后轴之间的距离
2.1 运动学分析
对机器人底盘后轴进行分析，将其线速度分解至世界坐标系的两轴方向上：
(1)
对机器人前轴进行分析，可得如下约束条件：
(2)
根据机器人底盘的航向角φ及轴距l，可知前后轴轴心处坐标间的数学关系，对其求导得到如下约束：
(3)
将式(1)与式(3)联立，所得表达式代入式(2)并展开三角函数，可得机器人底盘的角速度表达式：
ω=vrtanδf/l
(4)
对航向角φ求导可得机器人底盘的角速度ω，进而由式(1)和式(4)可得如下状态空间表达式：
(5)
由此，构建一个输入量为
状态量为
的控制系统。
2.2 线性误差模型
对式(5)进行线性化，令ξref表示期望状态，则对于任意一个期望状态都满足如下条件：
(6)
对ξ在期望处进行一阶泰勒展开如下：
(7)
联立式(6)和式(7)，令参变量
表示ξ、u相对于期望值ξref、uref的偏差，可得如下表达式：
(8)
系数矩阵由雅可比矩阵求得，记为A、B，则得到线性误差模型如式(9)所示：
(9)
2.3 模型离散化
对式(9)近似离散化得到下式：
(10)
式中，参变量T为采样时间。定义系统输出方程如下：
(11)
令
代入式(10)和式(11)：
(12)
令矩阵系数为
则式(12)化简如下：
(13)
2.4 系统设计
本文中设计的移动机器人工作流程如图2所示，整体运作可分为探索、规划2部分。其中探索部分采用神经网络控制器避障行驶，同时采用激光雷达感知环境点云信息；规划部分根据感知得到的点云地图信息进行进一步的采用JPS全局规划器和MPC局部规划器规划，控制机器人在期望全局路径下自主导航行驶。
针对机器人行驶环境，做出如下假设：
1)机器人在初始运行时，周围环境特征信息处于未知状态;
2)机器人运行的环境中包含大量的离散的障碍物信息;
3)采用红色、蓝色锥桶模拟离散障碍物并规定机器人感知完成后自动进行导航。
图2 移动机器人系统工作流程框图
Fig.2 Working flow diagram of mobile robot system
3 探索模块设计
针对探索任务，设计思路如下：采用相机实时获取环境特征并进行目标信息提取；根据提取到的有效信息设计神经网络自主避障；将离散点云信息连续化处理，约束规划效果。
3.1 离散障碍物特征提取
使用OpenCV库对离散障碍物特征提取[9]，提取图像中符合条件的红色锥桶、蓝色锥桶的位置及像素面积特征。
离散障碍物目标检测算法的检测过程为：
1:如果相机节点VideoCapture开启:2:捕捉图像帧ImgInput3:对ImgInput进行高斯模糊4:将ImgInput由RGB通道转为LAB通道5:对ImgInput进行开运算得到ImgOpen6:获取边界信息bndscv2.findcontours(ImgOpen)7:初始化bndsRed表和bndsBlue表8:遍历bnds中所有item:9:如果bnds[i]满足蓝色阈值,且面积area>1 500 Pix:10:bndsBlue.append(bnds[i])11:如果bnds[i]满足红色阈值,且面积area>1 500Pix:12:bndsRed.append(bnds[i])13:计算红色和蓝色锥桶的中心坐标xRed和xBlue
对采集得到的环境特征图像进行高斯模糊并进行阈值分割，使得分割效果更为平滑。由于相机镜头边缘处图像与中心区域亮度差明显，故进行分区域阈值分割及开运算，有效提升边缘信息的提取效果，如图3所示。
图3 分区域阈值分割示意图
Fig.3 Schematic diagram of threshold segmentation in sub regions
对处理后的图像进行边界提取，并二次判定所得最大连通域是否为近似三角。二次判定通过的连通域，以面积最大锥桶为起点，从右向左(从左向右)对红色(蓝色)锥桶连线，如图4所示。
图4 目标检测示意图
Fig.4 Schematic diagram of target detection
3.2 神经网络控制器设计
常见的控制算法包括PID控制、神经网络控制等[10]。传统的视觉控制算法通常采用PID控制器实现，其设计原理如图5所示。
图5 PID控制器设计原理图
Fig.5 Design schematic diagram of PID controller
采用网格法调试PID控制器参数，设置P方向间隔为1,D方向间隔为0.05。多次试验并观察响应曲线，最终选择最优的PD参数为(5,0.2)。
基于“深层网络结构+单层计算复杂度低”的思路，本文中设计了“5层网络+限幅模块”的网络结构，用以平衡深层网络准确率高以及浅层网络计算速度快之间的矛盾。选择ReLU作为激活函数，相比其他激活函数，进一步降低了计算复杂度，且训练速度快，便于控制算法实时更新迭代。如图6所示为所构建的网络结构。
图6 神经网络控制器结构示意图
Fig.6 Structure diagram of neural network controller
通过多次实验，在推理速度和模型精度之间平衡，最终选择五层全ReLU的网络结构。神经网络参数如表2所示。
表2 神经网络参数
Table 2 Parameters of the neural network
网络层输出维数参量个数Linear-1[-1,1,60]780ReLU——Linear-2[-1,1,30]1 830ReLU——Linear-3[-1,1,12]372ReLU——Linear-4[-1,1,1]13总参量数:2 995
综上，机器人将依据感知得到的环境信息构建神经网络控制器，输出对应的期望控制量，控制车辆实现自主感知环境工作。
神经网络控制器的输入量为上述提取到的十二维度(红色、蓝色2种颜色面积最大的3个锥桶的中心坐标信息和像素面积信息)的图像特征信息。由于所得图像存在缺失特征的可能性，先对数据做预处理后，再将其输入神经网络进行控制，空缺特征填补情况为：红色的中心坐标为0，蓝色的中心坐标为1 280；面积均为0。
将人工采集的未知障碍物环境数据集进行测试，比较PID控制算法、神经网络控制算法和测试集的误差，如图7所示。在实际采集的数据中，PID控制器的RMSE(均方根误差)为15.01，神经网络控制器的RMSE为9.47，可得神经网络控制效果优于PID控制器。
图7 PID控制器同神经网络控制器的误差曲线
Fig.7 Error curve of PID controller and neural network controller
3.3 环境信息感知与处理
在神经网络控制器控制机器人自主感知环境的同时，采用2D激光雷达基于GMapping算法，实现对环境点云地图的构建。如图8所示为构建的初始感知地图。
传统的图搜索路径规划算法普遍用于连续障碍物环境，而对于离散障碍物环境则需要对地图进行处理。单纯提高障碍物膨胀无法达到良好的效果，故进行预处理作业。将离散的障碍物进行连线从而拟合为连续障碍物环境，确保路径规划的准确性，实现逻辑如图9所示。
图8 初始感知地图
Fig.8 Original perception map
图9 感知数据预处理的实现逻辑框图
Fig.9 Implementation logic block diagram of perceptual data preprocessing
在采集环境信息的同时，记录机器人里程计的位姿，从而获取行进的路径并区分内外离散障碍物，使其连接为2个闭环。图10所示为预处理后的感知地图。
图10 预处理后感知地图
Fig.10 Perceptual map after preprocessing
4 规划模块设计
环境感知得到地图后，根据给定的目标点与起点，机器人将自主完成规划任务。本文中设计采用JPS算法构建全局路径并使用MPC构建局部路径跟踪期望轨迹。
4.1 JPS全局规划器
JPS(jump point search)算法是一种基于图搜索的启发式全局路径规划算法，相比于A*算法，该算法通过裁剪与跳跃，降低搜索的范围[11]。其代价函数f(n)如下：
f(n)=g(n)+h(n)
(14)
式中: g(n)表示从起点至节点n的最小代价；h(n)表示从节点n至终点的预估代价，此处采用曼哈顿距离计算。
JPS的具体算法流程为：
1:初始化：创建用于放置节点的Open表和Closed表；
2:输入值：起点节点Start、目标节点Goal；
3:Start放置于Open表中，设置Start代价值为0；
4:遍历开始；
5:如果Open表非空；
6:弹出Open表中代价值f(n)最低的节点n；
7:将节点n加入Closed表，视为已探索节点；
8:如果节点n是目标节点Goal；
9:从节点n起追踪Parent节点，直至Start；
10:返回路径，算法结束；
11:遍历节点n所有不在Closed表中的邻居节点m；
12:设置节点n为节点m的Parent节点；
13:计算节点m的代价值f(n)；
14:将节点m加入Open表中；
15:遍历结束。
算法的实现逻辑同A*算法类似，其主要不同点在于对邻居节点的定义，如图11所示。A*算法在探索时以八联通方式进行，探索当前节点周围的八个节点；而JPS算法则采用跳跃规则横向、纵向、对角探索节点。
图11 邻居节点定义示意图
Fig.11 Schematic diagram of neighbor node definition
通过仿真相同环境，对比A*算法和JPS算法的效率以及所得路径的长度如图12所示。分析可知，前者计算耗时相对后者更长。
图12 全局路径规划器规划图
Fig.12 Global path planner plan
为将机器人移动的方向约束为指定方向而非反向规划，此处设计增加多个局部目标点，迫使机器人抵达后再次规划后续目标点。如图13所示，其中左下角箭头处为机器人当前位姿所在，红线为两点间由JPS规划得到的全局路径。
图13 局部目标点示意图
Fig.13 Schematic diagram of local target points
4.2 MPC局部规划器
针对JPS规划得到的轨迹，进一步设计MPC局部规划器用于跟踪期望轨迹，如图14为MPC局部规划器的规划过程框图。
图14 MPC规划器规划过程框图
Fig.14 MPC planner planning process block diagram
设预测时域为Np，控制时域为Nc(Nc≤Np)，对式多次迭代得到最终的预测模型：
Y(k)=Ψkχ(k)+ΘkΔU(k)
(15)
min J=ΔηTQΔη+ΔUTRΔU
(16)
由预测模型[式(15)]可知，若已知当前时刻机器人的状态量和控制时域Nc内的控制增量，即可预测得到预测时域Np的系统输出。
进一步采用平方误差模型设计目标函数[式(16)]用于迭代得到局部最优解[7]，其中矩阵参量Q和R为权重矩阵。对其做如下处理进行简化：
(17)
最后，考虑系统的约束条件。对于系统控制增量应满足如下不等式：
ΔUmin≤ΔU≤ΔUmax
(18)
由此得到预测时域Np的控制增量ΔU，用于规划轨迹跟踪所需的控制量u。
5 仿真与实践
针对上述设计系统，使用Matlab的CarSim模块进行仿真测试，随后设计实际机器人系统在室外环境下实验测试设计方案。
5.1 系统仿真测试
基于CarSim模块，构建如图15所示仿真模型。
图15 仿真模型框图
Fig.15 Structure block diagram of simulation model
此处模拟真实离散障碍物环境，构建仿真测试。设置机器人期望移动线速度为2 m/s；约束舵机转向角度小于80°；机器人初始移动时间T0=0；采样间隔时间T=0.1 s，如图16所示。
同时，根据期望轨迹计算机器人在移动时抵达期望轨迹的理论角度并记录实际控制实现的角度，如图17所示。0～20 s内，RMSE=7.05。在20～28.3 s，由于23 s时的延迟效应，RMSE=45.261。总体期望值与实际值拟合程度高，系统稳定。
综上所述，本文中所设计的移动机器人系统稳定性强，能适应于离散障碍物环境下的未知环境探索与导航。
图16 仿真测试结果图
Fig.16 Simulation test result diagram
图17 机器人角度控制效果曲线
Fig.17 Robot angle control effect curve
5.2 实际环境测试
本文中进一步搭建实体移动机器人系统，如图18所示，并基于ROS框架[12]实现对机器人的软件系统搭建。
图18 机器人实物图
Fig.18 Pictures of robot objects
综合考虑各个传感器的特性，对其置放位置进行规划。其中，相机安装至车辆前端用于采集未知环境的图像信息；IMU和激光雷达安装至车辆中部位置，用于检测车辆姿态信息及采集周围环境的点云信息；处理器及控制器安装至车辆后端位置，用于处理环境信息、决策车辆行驶状态及控制车辆运动；编码器计米轮安装至车辆后侧用于采集车辆的里程信息。
关键传感器型号如表3所示。
表3 关键传感器型号
Table 3 Keysensor models
传感器名称详细型号处理器Jetson Xavier NX控制器Stm32F427单片机激光雷达镭神LS01BIMUBW-IMU50相机USB相机,720P
启动程序后，机器人将自动依据视觉信息和神经网络控制器进行自主探索环境，获取到图8所示的初始感知地图后对其进行预处理，进而得到图10所示的预处理后感知地图。此后，采用JPS全局规划器规划全局路径，并由MPC局部规划器进行轨迹跟踪测试，如图19所示。实际运行效果表明，系统设计合理且运行稳定。
图19 实际跟踪测试结果图
Fig.19 Actual test result picture
6 结论
1) 使用Simulink和CarSim模块构建车辆模型进行仿真，验证控制系统的可实施性。
2) 设计并使用目标检测和神经网络控制算法，实现车辆自主建图和路径规划。
3) 根据获得轨迹信息，设计MPC轨迹跟踪器，在已知环境中有效地跟踪车辆的参考轨迹。该算法通过仿真测试和实际使用可有效实现车辆的稳定规划，轨迹跟踪效果显著。
[1] Raja P,Pugazhenthi S.Optimal path planning of mobile robots:A review[J].International Journal of Physical Sciences,2012,7(9):1314-1320.
[2] Senarathne P,Wang D.Incremental algorithms for safe and reachable frontier detection for robot exploration[J].Robotics & Autonomous Systems,2015,72(C):189-206.
[3] Tang L,Yuta S, Vision based navigation for mobile robots in indoor environment by teaching and playing-back scheme[J].IEEE International Conference on Robotics & Automation,2001(03):3072-3077.
[4] Li Z,Deng J,Lu R,et al.Trajectory-tracking control of mobile robot systems incorporating neural-dynamic optimized model predictive approach[J].IEEE Transactions on Systems Man & Cybernetics Systems,2017,46(6):740-749.
[5] 丁皓,刘浩宇,庄逸，等.基于四轮差速模型的多机器人路径规划[J/OL].控制工程,https://doi.org/10.14107/j.cnki.kzgc.20210058.
Ding H,Liu H Y,Zhuang Y,et al.Multi-robot path planning based on four-wheel differential model[J/OL].Control Engineering,https://doi.org/10.14107/j.cnki.kzgc.20210058.
[6] 陈勇,刘哲,乔健，等.重载轮式移动机器人路径跟踪滑模预测控制[J].兵器装备工程学报,2021,42(08):214-220.
Chen Y,Liu Z,Qiao J,et al.Sliding mode predictive control for path tracking of heavy wheeled mobile robot[J].Journal of Ordnance Equipment Engineering,2021,42(08):214-220.
[7] Bujarbaruah M,Rosolia U,Stürz Y R,et al.A simple robust MPC for linear systems with parametric and additive uncertainty[J].American Control Conference,2021(09):2108-2113.
[8] Li L,Lu Y,Wang R,et al.A three-dimensional dynamics control framework of vehicle lateral stability and roll- over prevention via active braking with MPC[J].IEEE Transactions on Industrial Electronics,2016,64(04):3389-3401.
[9] Senarathne P,Wang D.Incremental algorithms for safe and reachable frontier detection for robot exploration[J].Robotics & Autonomous Systems,2015,72(C):189-206.
[10] Sadeghi M S,Varzandian S,Barzegar A.Optimization of classical PID and fuzzy PID controllers of a nonlinear quarter car suspension system using PSO algorithm[C]//International Econference on Computer & Knowledge Engineering,IEEE,2011:172-176.
[11] Jia J,Pan J S,Xu H R,et al.An improved JPS algorithm in symmetric graph[C]//Third International Conference on Robot,IEEE,2016:208-211.
[12] Okada K.Robot operating system[J].Journal of the Robotics Society of Japan,2012,30(09):830-835.