文档标题：移动机器人路径规划及控制系统设计

文档介绍：

一. 绪论

1.1 研究背景与意义

伴随着工业4.0的全面推进以及人工智能、大数据、传感器融合等前沿技术的深入发展，全球制造业正在迅速地向智能化、柔性化、无人化转变，移动机器人是智能制造领域的主要执行工具，也是无人驾驶、智能物流、特种作业等场景的主要支撑设备，它的使用范围越来越广，由原来的结构化车间场景延伸到仓储物流、户外巡检、应急救援、医疗辅助等各种复杂的、动态变化的环境中。移动机器人自主作业能力不再只是走路径、做基础工作，而是在复杂的、不确定的环境中对它的实时感知、动态决策、精确控制有更高的要求，是目前学术界和工程界共同研究的重要领域。

1.2 国内外研究现状深度综述

1.2.1 国外研究现状

国外对于移动机器人方面的研究开始的比较早，在数学建模、容错控制以及动态避障等方面一直保持领先的水平。Sivanthan等（2024）提出一种用代价地图的快速随机抽样算法（RRT），用采样次数评价复杂环境下所得采样数据是否可以减少代价地图中每个节点的计算成本从而提高性能。Yahia 等人（2025）用Lyapunov理论做故障检测和估计，能够处理执行器部分失效或者传感器漂移造成的路径追踪稳定问题。强化学习、Sim-to-Real迁移学习也成为了新的研究热点，使复杂动态规避的能力得以提高。

1.2.2 国内研究现状

国内学者主要对算法改进和工程实际应用进行分析，对小空间适应、多目标优化、ROS系统整合等有所建树。岳凤发等人提出了融合贝塞尔曲线和稀疏采样的自适应RRT*移动机器人路径规划方法，与本文移动机器人路径规划算法的平滑性和采样效率优化有关[1]。赵晨晖等(2025)用分布式宽度脉冲神经网络WNN和马尔可夫决策过程MDP相结合的方式，来达到多目标路径优化的目的。杨洁等人提出了用改进的蚁群算法做机器人路径规划的研究方法，这和本系统中移动机器人路径规划算法的全局搜索能力以及收敛速度优化有关[2]。韩龙岗等(2025)提出了一种新的算法CDBA-RRT，该算法使用变步长以及多种策略来进行扩展，从而使迷宫环境的规划速度比以前的方法快30%左右。赵琳等人提出了全自主移动机器人全局最优路径智能规划的方法，和本系统中移动机器人在复杂环境中全局路径规划、智能决策优化有关[3]。何晨昊等（2025）用ROS做SLAM建图和路径规划，把底层驱动模块解耦开，系统可以被移植，复用也变的更好。

1.2.3 核心算法对比分析

康燕萍等人提出了基于改进蜣螂优化算法的移动机器人路径规划方法，与本系统中移动机器人路径规划算法的启发式优化和全局搜索能力的改进有关[4]。家恺等人提出了基于改进阿奎拉鹰算法的移动机器人路径规划方法，该方法同本系统中移动机器人路径规划算法的仿生智能优化和对复杂环境的适应有关[5]。算法名称，算法逻辑分类，搜索效率，路径最优性，狭窄空间表现，动态环境自适应性算法，启发式搜索，静态环境下速度快，最优在网格精度范围内，优秀，如果存在解，则快速找到，较差，需要重新对整个图像进行搜索。

基础RRT算法通过随机采样的方式运行，速度快，初值搜索较好，在亚优情况下会产生冗余折线，性能一般，采样点难以进入狭窄通道，但是用重规划后计算代价较小。RRT、改进采样速度最快、渐进最优，一般、优秀。CDBA-RRT用变步长的多个策略共同工作，取得了良好的效果，逐步达到了最优，轨迹比较平滑，表现较好，变步长策略表现最佳，多个策略协同工作。

1.3 本文研究主要内容

本文试图构建出一种完整的移动机器人自主导航与控制方案，主要包括如下四个方面的研究内容。

运动学建模、数学仿真，本文采用差速驱动模型，在非完整约束下建立机器人的状态空间方程，用MATLAB对运动学正逆解进行计算，分析Jacobian矩阵在不同运动状态下奇点及线性度。

本文将树莓派4B作为中心，设计出具有高带宽的硬件通讯链路。本文介绍激光雷达、RGBD相机、超声波传感器的结合方式，设计电机驱动的反向电动势及电磁干扰隔离电路。

本文根据韩龙岗等人工作的基础上，对CDBA策略的数学实现进行了分析。本文主要研究怎样使用启发式采样函数来减小采样空间，并且提出平滑化处理算子，使生成的路径更加符合机器人的最小转弯半径要求。

双环PID控制器设计与集成，本文对移动机器人轨迹跟踪误差进行分析，并设计出线速度环、角速度环双重环PID控制结构。本文在ROS环境下完成了算法的集成以及硬件的软硬件联调，用实际仓库环境对避障成功率和跟踪精度进行了检验。

二. 基于 MATLAB 的机器人运动学建模与仿真

2.1 移动机器人物理结构建模

本文所设计的移动机器人采用经典的后两轮差速驱动、前一万个方向轮支撑的物理结构。本文在使用SolidWorks进行3D建模时，将轮径R设为0.065m，同时将轮距L设为0.32m。本文为了保持运动的稳定性，用重心平衡分析法把电池组、树莓派主控安置在底盘几何中心偏后一点的位置上。系统在得到模型之后，从质量属性、转动惯量中提取出来并输入MATLAB以及Simulink的Simscape模块中，成为运动学仿真时的物理引擎。图2-1为本文所设计的移动机器人三维结构模型。该模型用SolidWorks软件建立，整体是后两轮差速驱动和前一万向轮支撑的物理结构。图中可以清楚地看出两个驱动轮对称安装在底盘后面，前部有一个万向支撑轮来保持平衡。底盘中部留有电池组和树莓派主控安装的位置，重心平衡分析将电池组和树莓派主控设在几何中心稍后的地方来提高运动稳定性。轮径取0.065m，轮距取0.32m，见图中相应位置。

 图 21 移动机器人三维结构模型

2.2 运动学模型建立

1.模型假设与坐标系定义

本文研究的移动机器人采用两轮差速驱动方式，前部装有一个万向支撑轮。为了简化运动学建模，做出以下假设：

机器人质心和几何中心重合，车轮和地面之间满足纯滚动、无滑移条件，机器人运动平面为水平面，忽略垂直方向运动，机器人结构为刚体，忽略弹性变形。

定义坐标系如下：

世界坐标系 "O" _"w"  "-" "X" _"w"  "-" "Y" _"w"  " " ：固定于地面，作为全局参考系；

机器人坐标系 "O" _"r"  "-" "X" _"r"  "-" "Y" _"r"  " " ：固连于机器人本体，原点位于两驱动轮轴中心，"X" _"r"  轴指向机器人前进方向，"Y" _"r"  轴垂直于左侧指向车身。

机器人位姿用广义坐标向量表示：

"q=" [■("x" &"y" &"θ" )]^"T" 

其中，(x, y) 为机器人在世界坐标系中的位置坐标，"θ " 为机器人航向角。

2.运动学约束条件

（1）非完整约束

差速驱动机器人属于典型的非完整约束系统，在任何瞬时，机器人不能沿侧向独立运动。可以写成约束为：

"x"  ̇ "sin" ⁡"θ"  "-" "y"  ̇ "cos" ⁡"θ"  "=0" 

式子表明，机器人速度方向始终和它朝向一致，没有横向滑移。这个约束不可积，是系统非完整性的一种体现。

（2）驱动轮转速与车体速度关系

设左右驱动轮转速分别为"ω" _"L"  和"ω" _"R" ，轮半径均为 r，轮距为 L。则机器人线速度 v 和角速度"ω " 满足：

■("v" &"="  "r" /"2"  ("ω" _"R"  "+" "ω" _"L"  )@"ω" &"="  "r" /"L"  ("ω" _"R"  "-" "ω" _"L"  ) )

机器人在世界坐标系中的速度分量可写为：

"x"  ̇"=v"  "cos" ⁡"θ"  "="  "r" /"2"  ("ω" _"R"  "+" "ω" _"L"  )  "cos" ⁡"θ" 

"y"  ̇"=v"  "sin" ⁡"θ"  "="  "r" /"2"  ("ω" _"R"  "+" "ω" _"L"  )  "sin" ⁡"θ" 

"θ"  ̇"=ω="  "r" /"L"  ("ω" _"R"  "-" "ω" _"L"  )

3.运动学状态空间方程

将上述方程统一写成矩阵形式的状态空间模型：

{█("x"  ̇"="  "r" /"2"  ("ω" _"R"  "+" "ω" _"L"  )  "cos" ⁡"θ" @"y"  ̇"="  "r" /"2"  ("ω" _"R"  "+" "ω" _"L"  )  "sin" ⁡"θ" @"θ"  ̇"="  "r" /"L"  ("ω" _"R"  "-" "ω" _"L"  ) )┤"'" 

该方程组可以完全表示机器人位姿随着轮速的变化而发生的变化，是后面仿真和控制设计的基础。

4.正运动学与逆运动学

（1）正运动学

已知左右轮转速 "ω" _"R"  "、" "ω" _"L" ，求解机器人在世界坐标系中的位姿变化。将连续运动学模型离散化，得到递推公式：

"x" _"k+1"  "=" "x" _"k"  "+"  "r" /"2"  ("ω" _"R"  "+" "ω" _"L"  )  "cos" ⁡〖"θ" _"k"  〗 "⋅Δt" 

"y" _"k+1"  "=" "y" _"k"  "+"  "r" /"2"  ("ω" _"R"  "+" "ω" _"L"  )  "sin" ⁡〖"θ" _"k"  〗 "⋅Δt" 

"θ" _"k+1"  "=" "θ" _"k"  "+"  "r" /"L"  ("ω" _"R"  "-" "ω" _"L"  )"⋅Δt" 

其中"Δt" 为采样周期，k 为离散时刻索引。

（2）逆运动学

已知期望线速度 v 和角速度"ω" ，求解所需轮速：

"ω" _"R"  "="  "2v+ωL" /"2r" 

"ω" _"L"  "="  "2v-ωL" /"2r" 

物理意义分析：

当"ω" _"R"  "=" "ω" _"L" 时，"ω=0" ，机器人直线运动；

当"ω" _"R"  "=-" "ω" _"L" 时，v = 0，机器人原地旋转；

当"ω" _"R"  "≠" "ω" _"L" 时，机器人沿圆弧轨迹运动，转弯半径"R=v/ω" 。

5.雅可比矩阵

雅可比矩阵"J" 描述了轮速输入到位姿速度输出的线性映射关系：

[■("x"  ̇@"y"  ̇@"θ"  ̇ )]"=J" [■("ω" _"R" @"ω" _"L"  )]

其中雅可比矩阵的具体形式为：

"r" /"2"   "cos" ⁡"θ"   "r" /"2"   "cos" ⁡"θ" 

"r" /"2"   "sin" ⁡"θ"   "r" /"2"   "sin" ⁡"θ" 

"r" /"L"  "-"  "r" /"L" 

矩阵秩分析：

在任意"θ下" ，"rank(J)=2" ，表明系统具有2个可控自由度；

差速驱动机器人不存在传统上所谓的奇异位形，具有较好的可控性。

本节建立了差速驱动机器人的完整运动学模型，主要成果有以下几点：

推导了非完整约束条件下的运动学约束方程；

建立了轮速与车体速度之间的线性转换关系；

给出了正运动学与逆运动学的显式计算公式；

构建了雅可比矩阵并分析了系统的可控性；

确定了模型参数之后，就为后面MATLAB仿真验证奠定了数学基础。

2.3 MATLAB 仿真与验证

本文用MATLAB R2024b软件作为仿真环境，将步长设为0.01秒，用ODE45求解器求解。

仿真内容：S 型步态、原地旋转、急停工况。

结果说明，质心轨迹和理论值之间的误差在0.005m之内，并没有因为完整约束而出现数值漂移的现象，模型是有效的。图2-2为用MATLAB仿真平台制作的机器人运动动画截图。动画模拟了机器人在平面上完成S型步态、原地旋转和紧急停止这三种典型工况的运动轨迹。图中蓝色圆点是机器人的实时位置，红色轨迹线是质心的过去运动路径。仿真步长取0.01秒，用ODE45求解器做数值计算。由动画输出结果可知，质心轨迹同理论值之差一直维持在0.005m之内，显示出了运动学模型是正确的且具备数值稳定性的特征。

图2-2 机器人运动动画 

三. 基于树莓派的硬件架构设计

移动机器人硬件架构属于支持上层算法稳定工作和准确完成运动控制的物理支撑。本章主要针对核心计算单元、多源感知系统、驱动执行机构和通讯电路这四个主要模块，对以树莓派4B为基础的移动机器人硬件系统设计进行详细的说明。

3.1 核心控制单元

本文用树莓派4B作为机器人的主控核心。该板卡搭载Broadcom BCM2711四核Cortex-A72处理器，主频达1.5 GHz，配备4GB LPDDR4运行内存，提供千兆以太网、双频Wi-Fi及蓝牙5.0等无线通讯接口。相比于传统的嵌入式平台来说，树莓派4B有如下一些明显的优点。

（1）算力强，用到4颗CPU，64位，有硬件浮点运算和NEON指令集加速度，可以并行完成ROS系统里的多个节点工作。相比于STM32F4系列来说，树莓派算力提高了大约8到10倍，可以满足SLAM实时建图、点云数据处理和路径规划算法对高负载计算的要求。

（2）树莓派支持的ubuntu 20.04 LTS系统版本可以和ROS Noetic相兼容，极大的降低算法开发、部署的难度。开发者可以直接使用ROS丰富的功能包库来实现功能，不需要自己去开发底层驱动。

（3）接口丰富性：板载40针GPIO接口、2×USB 3.0、2×USB 2.0、全尺寸HDMI、MIPI CSI摄像头接口及MicroSD卡槽，为多传感器扩展提供了充足的外部连接资源。

树莓派4B主要是对ROS Master节点进行管理，即协调激光雷达、相机、超声波等各个传感器节点之间的Topic通信以及消息转发；激光雷达数据处理上用2D占据栅格表示每秒钟大概8000多个点云数据，并且给出一个地图；路径规划算法是在毫秒甚至百毫秒级别内完成CDBA-RRT的采样、扩展和重连优化；运动控制解算是根据PID控制器输出的速度指令通过I2C总线向PWM驱动板发送控制信号。

3.2 多源感知系统

为了达到机器人的复杂环境鲁棒感知的目的，本文提出使用激光雷达、RGBD相机、超声波传感器三者的融合方式。主测距传感器为RPLIDAR A2激光雷达，扫描范围为360°、测距精度为12m、扫描频率为10Hz，一圈可以得到大约800个激光点，用来建立2D占据栅格地图。为了弥补激光雷达垂直探测盲区，使用了Intel RealSense D435i深度相机，它给出640×480深度图像和1920×1080彩色图像，内置IMU可以对视觉里程计进行定位校正并检测低矮的障碍物。另外在机器人前后左右安装了四个HC-SR04超声波传感器来构成360度近场防护网络，检测距离2～400cm，可以检测到玻璃、吸光物体等激光雷达漏检的目标。三种传感器形成了一种由远及近的多层感知结构，即“远距离激光扫描、中距离视觉识别、近场超声防护”。

3.3 驱动与执行机构

移动机器人执行部分用差速驱动方式，左右两个轮子独立工作，靠转速差来实现转向。本文选用JGA25-371型直流减速电机，额定电压12V，减速比为1:30，内置霍尔编码器，每转产生330个脉冲，经倍频后得到1320个脉冲/转，可以实现电机转速的闭环检测。由于树莓派GPIO引脚数量少且不能实现独立高频PWM输出，本文使用了PCA968516路PWM控制板来扩展IO接口，并且采用I2C协议（400kHz）和树莓派进行通信，得到12位分辨率（4096级）的PWM占空比调节。系统利用逆运动学公式将期望的线速度v和角速度ω求得左右轮的目标转速，再通过PID控制器调节PWM来达到速度闭环跟踪的目的，保证机器人可以准确地按照预定的轨迹运动。

3.4 硬件通讯与电路设计

系统遵照“强弱电分离开来”的规划理念，控制电路用5V、3A供电，驱动电路用7.4V、10A供电，两路电源分开接地，较好地削减了耦合噪声。Yahia S等人提出了基于Lyapunov的随机切换系统容错控制以及K近邻切换检测与联合故障估计的方法，与本系统中移动机器人的控制系统的容错控制和故障检测设计有关[6]。为了防止电机启停时产生的反向电动势（几十伏）干扰主控，本文在PWM输出端和电机驱动器之间加装了6N137高速光耦隔离模块，数据传输速度能达到10MBd，隔离电压2500Vrms，可以很好地阻止尖峰干扰。

3.5 基于 MATLAB 的硬件系统仿真验证

为了检验硬件电路和供电系统是否可靠，本文用MATLAB/Simulink建立仿真模型，对电源模块、光耦隔离电路、通信时序进行仿真分析。

（1）双路 DC-DC 降压电源仿真

在 MATLAB/Simulink 中搭建双路独立输出 DC-DC 降压仿真模型，分别模拟 5V/3A 与 7.4V/10A 供电支路的动态响应特性。仿真设置电机启动冲击、负载突变等典型工况，结果为两路电源输出电压稳定，电压纹波小于2%，负载突变时电压跌落小于5%，满足主控和执行机构的供电要求。图3-1为MATLAB/Simulink仿真得到的双路DC-DC降压电源输出电压波形图。图上上面红色曲线是5V/3A供电支路的输出电压波形，下面蓝色曲线是7.4V/10A供电支路的输出电压波形。仿真设置电机启动冲击（0.1s）和负载突变（0.3s）。从波形可知，两路电源稳态时输出电压稳定，电压纹波小于2%；负载突变时电压跌落小于5%，约0.02s内恢复稳定。结果说明电源模块可以给主控单元(树莓派)、执行机构(舵机)提供不同的供电要求。

图3-1双路电压波形图

（2）6N137 光耦隔离电路仿真

采用 MATLAB/Simulink建立高速光耦信号隔离的仿真模型，仿真电机侧的干扰信号对控制电路产生的影响。仿真结果表明6N137光耦可以很好地阻断尖峰干扰和噪声信号，树莓派端控制信号没有失真、没有延迟畸变，隔离效果好，可以保护主控硬件安全。图3-2为6N137高速光耦隔离电路前后信号波形对比图。图中上方波形是电机侧输入的原始PWM控制信号，带有明显的尖峰干扰和高频噪声（峰值电压大约8V，噪声幅值约2V）。下方为经过光耦隔离之后输入树莓派端的信号，可以看见尖峰干扰已经被有效地去除掉了，信号依然保持着标准的5V方波，没有失真以及延迟畸变。对比结果说明6N137光耦能很好地阻止电机启动或者换向时产生的反向电动势以及电磁干扰对主控硬件造成影响。

图3-2对比波形图

（3）I2C 与串口通信时序仿真

用MATLAB建立I2C和UART通信时序仿真模型来检验多个设备同时通信是否稳定。经由仿真得知，在系统工作状况下各传感器均正常工作时不会出现数据通信冲突、无丢包的情况。图3-3为I2C、串口通信时序仿真波形图。图中上半部分为400kHz频率下I2C通信波形，即时钟线SCL、数据线SDA的信号变化情况，下半部分为115200b/s波特率下UART串口通信波形，即发送线TX、接收线RX数据帧格式。仿真模拟出多个传感器同时工作的情况。时序图显示各个通信通道之间没有数据冲突的现象，数据帧是完整的、没有丢失的，信号跳变沿很清楚，传输延迟小于1ms，可以满足ROS节点对于传感器数据实时性的要求。

图3-3时序图

（4）仿真结果表明，用MATLAB仿真的结果可以证明本文提出的硬件系统，在供电稳定度、信号抗干扰性以及通信实时性方面均能满足移动机器人导航系统的要求，结构合理。 

四．多传感器测试

4.1 多传感器融合标定

本文在构建地图时要将相机数据转换到雷达坐标系中，完成数据融合，需要获取两个传感器之间的外参，即标定。另外，由于相机和激光雷达的采样频率不一样，因此还要对二者的时间进行同步，以保证融合采样的同时性。本文的实验场景为室内环境，移动服务机器人的工作区域都是在室内平面上完成，IMU坐标的系和机器人的本体坐标系是重合的，激光雷达坐标系和机器人本体坐标系已经存在变换关系，所以不标定IMU与其它传感器的外参，只标定激光雷达和相机的外参。

4.1.1 相机标定原理

在进行相机雷达标定前，首先要对相机的内参进行标定，即畸变消除。相机的成像过程要经过这三个主要坐标系之间的转换。理想情况下相机成像过程中的三个主要坐标系之间的转换。图4-1给出了相机成像过程中三个主要坐标系之间变换的关系。图中分为三行，第一行是世界坐标系到相机坐标系，第二行是相机坐标系到图像坐标系，第三行是图像坐标系到像素坐标系。每一个变换步骤都给出了相应的变换矩阵类型。该图很好地展示了三维空间点最后映射到二维图像像素的过程，给相机内参标定和外参联合标定提供了理论依据。

图 4-1 相机成像坐标系变换过程

首先，将世界坐标系转换至相机坐标系，世界分布坐标系到相机分布坐标系的变换，是属于刚体转换的，经过旋转运动和平行转换移动来获取，如图4-2 所示，其中，世界坐标系是"O" _"w" -"X" _"w" -"Y" _"w" -"Z" _"w" ，"O" _"c" 为相机光心，"Z" _"c" 轴为垂直于像平面的光轴，相机坐标系表示为"O" _"c" -"X" _"c" -"Y" _"c" -"Z" _"c" 。图4-2具体展示了世界坐标系"-" "X" _"w"  "-" "Y" _"w"  "-" "Z" _"w"  " " 与相机坐标系"O" _"c"  "-" "X" _"c"  "-" "Y" _"c"  "-" "Z" _"c" 之间的刚体变换关系。图中左侧为世界坐标系，右侧为以相机光心 O_c 为原点的相机坐标系，"Z" _"c"  " " 轴垂直于像平面。从世界坐标系到相机坐标系的变换由一个3×3旋转矩阵 R 和一个3×1平移向量 T 共同描述。图中用圆弧箭头表示旋转自由度，用带箭头直线表示平移自由度，直观展示了两坐标系之间的六个自由度变换关系。

图 4-2 世界坐标系到相机坐标系的变换

两个坐标系之间的转换方程如下式所示，旋转矩阵是坐标系1到坐标系2的旋转矩阵，变换矩阵是坐标系1到坐标系2的变换矩阵。

[■("X" _"c" @"Y" _"c" @"Z" _"c" @"1" )]"=" [■("R" &"T" @"0" &"1" )][■("X" _"w" @"Y" _"w" @"Z" _"w" @"1" )]

将相机坐标系转换到图像坐标系。从相机坐标系到图像坐标系是属于小孔成像原理中的透视投影关系，从三维到二维的变换，其透视投影变换如图4-3所示图4-3展示了小孔成像模型下的透视投影关系。图中左侧为三维空间中的物体点"X" _"c"  "," "Y" _"c"  "," "Z" _"c"  ")" ，右侧为像平面上的投影点 p(x, y)。相机光心 "O" _"c"  " " 到像平面的距离称为焦距 f。从图中可以看出：空间点 P、光心 "O" _"c"  " " 与像点 p 三点共线，满足相似三角形关系，即"x=f⋅" "X" _"c"  "/" "Z" _"c"  "，y=f⋅" "Y" _"c"  "/" "Z" _"c" 。该图解释了三维空间坐标向二维图像坐标转换的几何本质。

图 4-3 透视投影

上图中，点p位于相机的像平面，相机光心到像平面的距离称为焦距，大小为f，"O" _"c" -"X" _"c" -"Y" _"c" -"Z" _"c" 是相机坐标系，o-x-y是图像坐标系。在转换过程中，通过下式可以从三维的相机坐标系变换到二维的图像坐标系：

"Z" _"c"  [■("x" @"y" @"1" )]"=" [■("f" &"0" &"0" &"0" @"0" &"f" &"0" &"0" @"0" &"0" &"1" &"0" )][■("X" _"c" @"Y" _"c" @"Z" _"c" @"1" )]

其中，"Z" _"C" 为比例因子,[x y 1]为像点 p在图像平面上的齐次坐标,["X" _"c"  "Y" _"c"  "Z" _"c"  1]是空间点P在相机坐标系下的齐次坐标。可通过下式计算出图像上点P：

"x=f"  "X" _"c" /"Z" _"c"   ", y=f"  "Y" _"c" /"Z" _"c"  

（3）把图像坐标系变换为像素坐标系。像素坐标系是用二维直角坐标系来表示相机中像素分布的一种方式，可以定位相机上每一个像素的位置。它把图像的左上角设为原点，水平方向用x轴表示，垂直方向用y轴表示。像素坐标系和图像坐标系在同一直线上，但原点位置及计量单位不同。图像坐标系一般用毫米做单位，像素坐标系用像素做计量单位。二者之间的关系如图 4-4 所示。图4-4为图像坐标系和像素坐标系的关系图。图像坐标系原点o在像平面中心，用毫米表示；像素坐标系原点在图像左上角，用像素表示。图中标注出的是图像坐标系的原点在像素坐标系中的坐标，dx 和 dy 分别是每一个像素在x轴、y轴上所占的物理大小。两坐标系之间转换包含平移、缩放等操作，图中用箭头标出坐标轴对应情况。

图 4-4 图像坐标系与像素坐标系

图中，（"u" _"0" ，"v" _"0" ）为图像坐标系原点，图像坐标与像素坐标之间的转换方程

{█("u="  "x" /"dx"  "+" "u" _"0" @"v="  "y" /"dy"  "+" "v" _"0"  )┤

上式写为矩阵形式如下：

[■("u" @"v" @"1" )]"=" [■("f" _"x" &"0" &"c" _"x" @"0" &"f" _"y" &"c" _"y" @"0" &"0" &"1" )][■("x" @"y" @"1" )]

其中，u和v为尺度系数。

结合以上所有的公式整理出将世界坐标系中的一点转换到像素坐标系中方程为方程1。

"Z" _"c"  [■("u" @"v" @"1" )]"=" [■("f" _"x" &"0" &"u" _"0" &"0" @"0" &"f" _"y" &"v" _"0" &"0" @"0" &"0" &"1" &"0" )][■("R" &"T" @"0" &"1" )][■("X" _"w" @"Y" _"w" @"Z" _"w" @"1" )]

其中，"f" _"x" ="f" /"dx" ，"f" _"y" ="f" /"dy" ， 为内参矩阵，由相机自身的内部参数决定，[■("R" &"T" @"0" &"1" )]为外参矩阵，取决于相机坐标系和世界坐标系的位置关系。

上述公式推导都是基于理想相机模型，真实的相机获取图像时，由于透镜形状的影响，光线传播发生改变，产生成像变形，称为径向畸变。透镜安装设置以及成像平面不平行引起的变化被称为切向畸变。径向畸变可以分为桶形畸变和枕形畸变，如图4-5所示。图4-5为相机成像中常见的两种径向畸变。左侧是原始无畸变图像；中间是桶形畸变，即图像中心向外凸出，网格线呈向外弯曲的弧形，常出现在广角镜头下；右侧是枕形畸变，即图像四角向外拉伸，网格线向内凹陷，多在长焦镜头下出现。图中用蓝色虚线标出了畸变前后的网格边界变化趋势，使读者对径向畸变的几何特点一目了然。

图 4-5 两种径向畸变

4.1.2 相机雷达空间标定

本文采用一种基于 3D-3D 匹配的方法对相机与二维雷达进行外参标定，其标定原理如图 4-6 所示。图4-6展示了相机与二维激光雷达联合外参标定的基本原理。图中左侧为棋盘格标定板，右侧为深度相机，中间为单线激光雷达发出的激光束。标定过程中，相机采集标定板图像并建立其在相机坐标系下的平面方程"n" _"c" ^"T"  "P" _"c"  "+d=0" ；激光束打在标定板上形成离散激光点，记录其在激光雷达坐标系下的坐标 "P" _"l" 。通过求解点到平面的约束关系，可以计算出雷达坐标系到相机坐标系的旋转矩阵"R" _"l2c"  和平移向量"t" _"l2c" 。图中用不同颜色的箭头标注了两种传感器各自采集的标定信息流向。

图 4-6标定原理图

首先用深度相机采集标定板图像，得到该标定板在相机坐标系下的平面方程。第二，用激光点在标定板上的投射位置来建立激光雷达坐标系下标定板的平面方程。最后，根据点到平面的约束条件得到激光雷达和相机之间的外参变换矩阵。假定标定板在相机坐标系中平面参数为：

"π" _"c"  "=" [" " "n" _"c"  ", d " ]"∈" "R" ^"4"  "," "n" _"c"  "∈" "R" ^"3" 

其中,"n" _"c" 为标定板平面的三维法向量，d表示标定板和相机平面的距离。标定板上的三维点在相机坐标系下坐标为"P" _"C" ，激光雷达的激光点在激光坐标系下的坐标为"P" _"I" 。任何三维空间点只要落在标定板上都满足公式所示：

"n" _"c" ^"T"  "P" _"c" +d=0

激光坐标系下激光点变换到相机坐标系下用公式表示

"P" _"C" ="R" _"I2c" ^"T" ("P" _"I" -"t" _"I2C" )

其中，"R" _"I2c" 和"t" _"I2C" 为激光坐标系到相机坐标系之间的旋转和平移。

本文使用的激光雷达是单线激光雷达，其只能感知一个平面，所以激光束的扫描平面为X-Y平面，Z为0。激光点在空间中的坐标可以写成"P" _"I" =["x" ┤，y，├ "0" ]^"T" 。则上述变换矩阵可以改写为

"P" _"c"  "=" "R" _"12c" ^"T"  ([■("x" @"y" @"0" )]"-" "t" _"12c"  )"=" "R" _"12c" ^"T"  [■("1" &"0" &"-" "t" _"12cx" @"0" &"1" &"-" "t" _"12cy" @"0" &"0" &"1" )][■("x" @"y" @"1" )]"=H" "P"  ̄_"1" 

其中，"R" _"I2c" 和"t" _"I2C" 为雷达坐标系到相机坐标系的旋转和平移，因此由下式可以构建两个坐标系的外参方程，

"n" _"c" ^"T" （"R" _"I2c"  "P" _"I" +"t" _"I2C" )+"d" _"C" =0

将上式整合得

"n" _"c" ^"T" H"P"  ̅=－"d" _"C" 

由于H矩阵存在9个新的未知量，因此可以对H矩阵整体进行估计，将上式变为一个线性最小二乘问题来进行求解。外参"R" _"I2c" 和 "t" _"I2C" 可以通过多帧激光雷达构建的约束求解获得。通过联合优化外部参数求解来获得最优的估计值。假设第i帧激光束打在标定板上的个数为"N" _"i" 个，同时第i帧激光对应的标定板平面方程在相机坐标系下的表示为                         ["n" _"ic"  ┤，├ "d" _"ic"  ]^"T" :

"R"  ̂_"12c"  "," "t"  ̂_"12c"  "="  "arg" ⁡"min"  ∑_"i=1" ^"N" ▒"1" /"N" _"i"   ∑_"j=1" ^("N" _"i" )▒‖"n" _"ic" ^"T"  ("R" _"12c"  "P" _"ij"  "+" "t" _"12c"  )"+" "d" _"ic"  ‖^"2"  

在求解优化的过程中用到了四元数的参数化方法，优化方法为LM优化法。

4.1.3 相机雷达时间同步

对相机和激光雷达进行数据融合除了需要获得两者的外参参数外，还需要对其时间戳进行同步，也就是获取相机雷达同一时刻的数据，但相机和雷达的频率不同，本文使用的雷达频率为10Hz,相机为30Hz，因此需要进行标定，使时间戳对齐。本文的实验使用的是 ROS 操作系统中基于 C++与 python 编程语言实现的滤波程序库 "messag" "e" _"f"  "ilters" 里的时间同步器组件来实现数据采集的时间同步。该同步器具备双重时间对齐能力，可灵活选择精确时间戳对齐或近似时间戳对齐方案。针对移动机器人平台的资源约束特性，实际采用计算开销更低的近似同步模式。如图 4-7 所示，系统在获取激光雷达数据时，会自动匹配时间戳最接近的相机图像帧，实现跨模态传感器数据的时间对齐。这个过程涉及比较相机数据和雷达数据的时间戳。当时间戳的差异小于设定的阈值时，同步器将执行匹配操作，将相机数据和雷达数据对应起来，如果时间差超过阈值时，匹配不会执行。通过这种匹配，我们可以获得在相同时间点上的相机图像和激光雷达点云数据,从而进行传感器融合。图4-7展示了相机与激光雷达数据时间戳同步的工作原理。图中左侧为激光雷达数据流（10 Hz），右侧为相机数据流（30 Hz），中间为ROS message_filters时间同步器。同步器分别订阅两个传感器的话题，比较两帧数据的时间戳差异"Δt" 。当"|Δt| " 小于预设阈值时，同步器将激光雷达帧与相机帧进行配对，输出时间对齐的融合数据；当时间差超过阈值时，不执行匹配。图中用不同颜色标注了同步成功与失败的匹配案例，直观体现了近似时间戳同步策略的工作流程。

图 4- 7 时间戳同步原理图

本文中相机和雷达的时间同步是用ROS来实现的。首先，二维激光雷达节点和相机节点把采集到的点云数据和图像数据发布到 ROS的话题（Topic）上。时间同步器节点同时订阅这两个话题，实时接收到激光雷达、相机的传感器数据。时间同步器点检测到激光雷达数据和相机数据时间戳的差异小于一个事先设定好的阈值时，就会触发回调函数来执行后面的融合处理。图4-8、图4-9为同步前后的时间戳，图中可以看见时间戳大体上是同步的。图4-8用散点图的形式展示时间同步前激光雷达数据和相机数据的时间戳分布。横坐标是数据帧的序列号，纵坐标是时间戳值（秒）到数据帧之间的距离。图中蓝色圆点为激光雷达数据，红色叉号为相机数据（30Hz）。从图中可以看出，两种传感器数据的时间轴上是不均匀的，没有对齐关系，如果直接融合的话就会产生时间错位，从而造成感知结果出错。

图 4-8 时间戳同步前

图4-9为经过message_filters时间同步器处理后的时间戳对齐结果。绿色圆点是被正确配准好的激光雷达和相机的数据对。从图可知每对匹配数据的时间戳差都是在设定阈值之内的，在时间轴上是均匀的分布的。该结果说明时间同步器把10Hz激光雷达数据和30Hz相机数据近似时间对准，给后面的多传感器融合提供了一个可信的时间基准。

图 4-9 时间戳同步后

4.2 多传感器标定实验 

4.2.1 相机内参标定实验

在进行相机雷达联合标定之前，首先要对相机的内参进行标定，相机标定的质量会对后面的目标检测结果造成重大影响。本文内参标定实验使用的是张正友标定法。张氏标定法比其他标定方法只需要一块棋盘格标定板，能够满足大部分场景的标定需求。

本文实验用到的是8行11列的棋盘格标定板，每一个黑白格子的边长是30mm。标定时主要用固定相机的方法，使相机的位姿不变，拍摄不同角度、不同位置的棋盘格图片。一共收集了24张图片，图4-10给出了用于相机内参标定的24个棋盘格图像。棋盘格为8行11列，黑白格的边长是30mm。拍摄时保持相机位姿不变，移动棋盘格标定板到不同的角度（俯仰、偏航、滚转变化），不同的位置（图像中心、四角、边缘），不同的距离（0.5m~1.5m）。图中用缩略图阵列的形式给出了部分标定需要的几何信息丰富的图像，保证标定结果的鲁棒性及精度。

图4-10相机标定图片

本文主要使用的是 OpenCV 中的相机标定程序，对每张图片中的角点进行提取，然后将角点标示出来，进行标定，最终对结果进行评定，如果成功的图片少于10张，则会被认定为标定失败。识别的棋盘格角点。图4-11展示了OpenCV相机标定程序对棋盘格图像进行角点检测的结果。图中棋盘格图像上叠加了绿色圆圈标记，每个圆圈对应一个检测到的棋盘格内角点。本实验中每张图像可检测到（8-1）×（11-1）= 70个角点。程序通过亚像素精确定位算法进一步提升角点坐标精度。图中右下角显示了成功检测角点的图像序号及角点数量，成功识别的图像用于后续内参矩阵与畸变系数的计算。 

图4-11角点提取

相 机 标 定 实 验 获 得 的 内 参 为

[■("453.616" &"0" &"326.510" @"0" &"453.863" &"241.558" @"0" &"0" &"1" )]

畸 变 系 数 为

[0.0950 0.19240 0.00063 0.00023 0.06845]

4.2.2 相机雷达外参标定实验

完成相机内参标定之后，接下来进行相机和雷达的外参标定。标定板还是用的棋盘格标定板。首先将棋盘格参数输入到代码中，手持标定板到相机和二维激光雷达前方0.7m左右处，在手持过程中需要保证标定板在相机视野范围内，同时保证二维激光雷达激光束能打在标定板上。开启 rosbag 录制功能包，从不同的角度、不同的位置对相机和二维激光雷达数据进行采集，并保存起来。

得到采集的数据后，将录制的 bag 文件路径写入，运行标定代码。在标定过程中，发现激光束自动检测的效果较为一般，会出现误检测，导致标定效果较差。因此，本文通过手动分割的方法，将一帧激光中打在标定板上的激光束分割出来，然后与相机数据进行标定。分割得到的激光束。图4-12展示了二维激光雷达一帧数据中，通过手动分割方法提取出的打标定板上的激光束点云。图中背景为灰度化的激光雷达强度图，红色高亮点云表示被手动框选分割出的有效激光点。这些激光点均落在标定板平面区域内，坐标在激光雷达坐标系下表示为 "P" _"l"  "=[x,y,0" "]" ^"T" 。通过手动分割，避免了自动检测中可能出现的误检问题，从而提高了后续外参标定的精度。

图4-12手动分割后的激光束

图4-13展示了相机与二维激光雷达联合外参标定的投影验证效果。图中左侧为相机采集的棋盘格标定板彩色图像，右侧为将激光雷达点云数据经外参变换矩阵"T" _"l2c"  投影至图像平面后的叠加效果。红色圆点表示激光束投影点，绿色方框表示标定板检测区域。从图中可以看出：红色激光点云完整且准确地落在棋盘格标定板的区域内，边缘与棋盘格边界基本重合，无明显偏移或发散。该结果表明求得的变换矩阵 "T" _"l2c"  具有较高的标定精度，能够满足多传感器数据融合的实际需求。

图4-13标定结果

将分割后的二维激光雷达数据保存下来，并将其路径写入，运行标定程序，程序结束后终端会输出相机与二维激光雷达之间的变换矩阵。相机雷达外参标定实验获得的变换矩阵"T" _"I2C" 结果如下：

"T" _"12c"  "=" [■("0.00532" &"-0.69993" &"0.71419" &"-0.01867" @"-0.999996" &"0.00166" &"0.00908" &"-0.01102" @"-0.00753" &"-0.71421" &"-0.69990" &"0.40894" @"0" &"0" &"0" &"1" )]

得到外参之后，用可视化的代码将二维激光雷达数据投影到图像上以验证标定的效果。图4-13为标定验证的效果图。由投影结果可知，得到的激光束可以完全投影到标定板上，说明相机雷达联合标定结果的效果较好。

五. 路径规划算法深度解析

5.1 传统算法局限性分析

移动机器人领域中A算法可以保证最优性，但是需要先验地图的完整性和搜索空间会随着地图的规模呈指数级增长。基础快速搜索随机树RRT利用随机采样技术可以快速地覆盖整个空间，但是存在两个主要缺陷，即生成的轨迹不是最优的，轨迹中包含了很多随机折线，从而使得驱动电机能耗和机械磨损增大。第二条是狭窄通道瓶颈，在穿过窄门或者迷宫的时候，因为采样点处在窄道内所占的概率较小，算法一般会在有限时间内不能收敛。表5-1用表格的方式对传统RRT算法、传统A算法以及改进CDBA-RRT算法在四个方面进行了比较。表格的行标题为三种算法类型，列标题分别为平均规划时间（ms）、路径长度（m）、动态避障成功率（%）、狭窄通道通过率（%）。从数据可以看出：CDBA-RRT算法平均规划时间为95.7 ms，较RRT算法（328.6 ms）提升约71%；狭窄通道通过率为94.3%，较RRT算法（65.7%）提升约28个百分点。通过比较可以看出CDBA-RRT算法在规划效率以及复杂环境适应性方面都有明显的优势。

表5-1算法对比

算法类型 平均规划时间 路径长度 动态避障成功率 狭窄通道通过率

传统RRT算法 328.6 12.35 82.4 65.7

传统A算法 186.2 10.87 91.3 78.2

改进CDBA-RRT 95.7 10.21 98.6 94.3

5.2 CDBA-RRT 算法原理

CDBA-RRT算法是由韩龙岗等人在2025年提出的，它的主要思想就是用代价引导的双向搜索、变步长自适应扩展和多策略协同采样来解决传统RRT算法的路径次优性和狭窄通道瓶颈问题。赵晨晖等人提出了基于分布式宽度脉冲神经网络和马尔可夫决策的路径规划多目标优化方法，和本文中移动机器人路径规划的多目标优化以及智能决策协同相关[7]。本节从三个关键技术进行说明。

变步长自适应扩展策略传统的RRT算法使用固定步长，在空旷地区会造成采样点的重复，而在狭窄地区由于步长太大而造成扩展失败。CDBA-RRT用环境感知变步长的方法。韩龙岗等人提出的多策略协同扩展变步长CDBA-RRT机器人路径规划算法与本文移动机器人路径规划算法的变步长和多策略协同扩展机制设计有联系[8]。

"λ=" "λ" _"max"  "⋅" "e" ^"-κ⋅ρ(o)" 

其中"λ" _"max" 为预设最大步长，"ρ(o) " 为以当前节点为中心的局部区域内的障碍物密度，"κ" 为衰减系数。该公式的物理意义是：在空旷区域，"λ→" "λ" _"max" ，大步长快速收敛；在障碍物密集区域或狭窄通道内，步长呈指数衰减至极小值，精细化探索通道内部。这一机制实现了探索效率与通过精度的自适应平衡。

2.目标引力与协同扩展策略

为解决随机采样的盲目性问题，CDBA-RRT采用双树双向搜索结构：一棵树从起点向目标扩展，另一棵树从目标向起点扩展。每棵树的采样点不再完全随机，而是以一定概率 "p" _"g"  直接选择目标点作为采样目标：

"p" _"g"  "="  "d" _"minn" /("d" _"min"  "+" "d" _"rand"  )

其中"d" _"min" 为当前树节点距目标点的最小距离，"d" _"rand"  " " 为随机采样点与节点的平均距离。当"p" _"g"  " " 较大时，树被“拉”向对方树的方向，加速两树相遇。同时，两棵树在扩展过程中定期交换信息，当两棵树的节点间距小于步长时，尝试直接连接，形成完整路径。该策略显著降低了随机采样的盲目性，使搜索树更具“目的性”地向目标延伸。

3.重连优化和代价函数CDBA-RRT是继承RRT的重连机制，并用更加细致的代价函数评价路径的好坏。路径总代价表示为：

"C" _"total"  "=" "C" _"length"  "+α⋅" "C" _"smooth"  "+β⋅" "C" _"clearance" 

三个代价分量分别定义如下：

路径长度代价：

"C" _"length"  "=" ∑_"i=1" ^"n-1" ▒‖"q" _"i+1"  "-" "q" _"i"  ‖ 

其中"q" _"i"  " " 为路径点，n为路径点总数。该分量使算法倾向于寻找最短路径。

平滑性代价：

"C" _"smooth"  "=" ∑_"i=2" ^"n-1" ▒("q" _"i-1"  "-2" "q" _"i"  "+" "q" _"i+1"  )^"2" /‖"q" _"i"  "-" "q" _"i-1"  ‖^"2"  

该分量是路径曲率的离散近似，惩罚急转弯，使得轨迹连续可导，满足机器人的运动学约束。

安全裕度代价：

"C" _"clearance"  "=" ∑_"i=1" ^"n" ▒"max"  ("0," "d" _"safe"  "-" "d" _"obs"  "(" "q" _"i"  ")" )^"2" 

其中"d" _"safe" 为预设安全距离，"d" _"obs"  "(" "q" _"i"  ") " 为路径点"q_i " 到最近障碍物距离。该分量惩罚过于贴近障碍物的路径点，提高系统安全性。

三个分量的加权系数设置为 "α=0.5、β=1.5" ，体现对安全性的更高重视。

重连优化的具体流程为：每次向树中添加新节点 "q" _"new"  后，搜索其邻域半径 r 内的所有现有节点：

"r=γ⋅" √("log" ⁡"N" /"N" )

图5-1是用相同狭窄通道障碍物环境，传统的RRT算法和CDBA-RRT算法生成的路径规划效果对比图。左侧子图中的绿色线为随机扩展采样树，红色线为最后规划出来的路径。可以得出结论，由于RRT算法所形成的路径上有很多折线、冗余转向，在狭窄通道的入口处采样点较少，造成路径不能顺畅地通过。在右侧子图中，使用变步长和目标引力采样的策略，采样树比较紧密，最终得到的路径比较平滑，长度较短。两图对比可知CDBA-RRT算法在轨迹平滑性、路径最优性和狭窄空间过能力强方面都比传统RRT算法好。

  图 51 RRT 与 CDBARRT  路径规划效果对比

5.3 动态避障逻辑

静态环境下预先规划出的路径，在实际运行过程中由于突然出现的动态障碍物（行人、移动的小车）而失效。CDBA-RRT算法在ROS框架里把分层避障策略集成起来。

1.局部代价地图实时更新

系统维护两层代价地图：全局代价地图基于静态SLAM地图构建，用于全局规划；局部代价地图以机器人为中心动态更新，尺寸为5 m × 5 m，网格分辨率为0.05 m。激光雷达以10 Hz频率扫描环境，将新检测到的障碍物信息实时融入局部代价地图，障碍物点对应网格的代价增加值"Δc"  与距离 d 的关系为。

"Δc=" "C" _"max"  "⋅" "e" ^("-" "d" ^"2"  "/" "σ" ^"2"  )

其中 "c" _"maax" 为最大代价，"σ" 为影响半径。该高斯衰减模型使障碍物周围形成“软约束”区域，避免硬边界导致的路径振荡。

2.局部避障重规划机制

当预定轨迹上出现瞬时动态障碍物的时候，系统不会执行耗时的全局重规划，而会使用局部切向避障来处理。

步骤一：将激光雷达检测到的动态障碍物位置"o" 作为圆心，以安全半径 "r" _"obs"  "=0.25m"  划定禁行区域。

步骤二：在机器人当前位置"qrobot " 沿原全局路径方向搜索，找到第一个未被障碍物遮挡的路径点作为临时子目标 "qsub" 。

步骤三：以 "qrobot " 为起点、"qsub" 为目标，在局部代价地图的子区域内调用CDBA-RRT算法快速生成切向避障弧线。

步骤四，把局部避障路径和原来的全局路径用B样条曲线平滑连接起来，消除连接处的曲率突然变化。

该分层避障机制在100到200ms之间就完成重规划的反应，避免由于频繁做全局重规划而造成的计算成本增加。

3.算法收敛性分析

CDBA-RRT在保持RRT概率完备性的基础上，通过目标引力采样提高了有限时间内的搜索效率。变步长策略确保了在狭窄通道内的精细化探索；代价函数驱动的重连机制保证了渐进最优性随着采样次数"N→∞" ，算法生成的路径代价" C_N " 以概率1收敛至全局最优值 C：

("lim" )┬"N→∞" ⁡"P"  ("C" _"N"  "-" "C" ^"*"  "<ε" )"=1" 

因此CDBA-RRT算法依靠变步长自适应、双向目标引力和重连优化三者之间互相影响，从而在规划效率、路径平滑性和狭窄空间通过能力这三个方面都比传统的RRT、RRT要好。

六. 基于 PID 控制的路径跟踪与运动控制

移动机器人路径跟踪控制属于完成高精度自主导航的重要部分。规划算法所给出的理想路径一般是由许多离散路径点组成的，但是由于机器人自身运动学限制、轮地摩擦非线性、传感器噪声以及行驶过程中细微的扰动等原因，开环控制方式很难使机器人准确地复现规划轨迹。

需要设计一个具有反馈校正功能的运动控制器，对机器人实际位姿和期望路径之间的差异进行及时比较，然后根据比较结果改变驱动轮的转速，以达到高精度路径跟随的效果。本文在对差速驱动机器人运动学特性进行充分分析的基础上，设计了一个采用双环PID控制结构、结合前瞻点跟踪策略的路径跟踪系统，在复杂的环境里可以使机器人准确、平稳地跟随规划的路径。

6.1 双环 PID 控制器结构

本文使用角速度环和线速度环相结合的方式，对机器人航向角偏差和横向跟踪误差做独立的修正。Mostafa J等人提出了使用较小时延的轮式移动机器人学习模型预测控制法，该模型与本文所述的移动机器人路径跟踪控制器模型预测优化以及延迟补偿设计有联系[9]。角速度环属于内环，它的输入量是机器人当前航向角和期望路径切线方向之间的偏差。线速度环属于外环，它的输入量是机器人质心到期望路径的最短距离，也就是横向跟踪误差。机器人偏离路线太远的时候，线速度环会立刻减小行驶速度，给角速度环足够多的时间去修正，从而防止由于速度太快造成的大幅度摆动或者失稳。

两个控制环一起工作，内环是航向变化的快速响应环，外环是宏观路径的纠偏环，二者结合使用可以得到响应速度快、稳态精度高的控制结构。经过多次参数整定实验，本文得到的角速度环PID参数为比例系数1.2、积分系数0.05、微分系数0.08，线速度环PID参数为比例系数0.8、积分系数0.02、微分系数0.05。这组参数可以取得良好的响应速度和抗干扰效果。

6.2 前瞻点跟踪策略

传统路径跟踪控制的控制器一般把机器人当前位置最接近的一个路径点当作跟踪目标。但是该控制策略很容易造成机器人出现追逐振荡的现象，即机器人每经过一个路径点就会立刻转到下一个路径点上，从而使运动轨迹变得非常尖锐，行驶起来十分颠簸且能量消耗过多。Youngmin Y等给出城市环境下全尺寸自动驾驶巴士使用扫掠区域的换道路径规划方法，该类方法同本文中的移动机器人在结构化环境中路线规划以及换道策略改进相关联[10]。为此本文采用了前瞻点的跟踪策略。前瞻距离的选取会对控制性能产生较大的影响，当前瞻距离过短的时候，机器人对于路径局部的曲率变化非常敏感，容易出现频繁转向的情况，当前瞻距离太长的时候，机器人就会“绕远道而行”，进而陷入窄小通道中。

因此本文设计了一个以车速为依据的前瞻距离自适应机制，使得前瞻距离随着当前的线速度而改变。高速行驶时前瞻距离会变大，让机器人提前对前方的弯道进行感知并慢慢转向；低速行驶或者穿过狭小的地方的时候，前瞻距离就会减小，使得机器人可以准确地跟上复杂的轨迹。本策略在平滑性以及贴合精度两个方面达到了自适应的效果。

6.3 控制闭环稳定性分析

为了检验双环PID控制器实际的跟踪性能，在MATLAB仿真环境里建立了完整的闭环控制系统模型，用S型蜿蜒路径作参考轨迹来测试。仿真时在均值为0、方差为0.02米的高斯噪声上加上噪声，代表了真实情况下，测量装置无法完全准确地给出位置。另外在机器人运动模型里加入±5%轮速执行偏差来代表电机响应不同、地面摩擦不均等非理想情况。

仿真实验分为开环控制和闭环控制两种模式的对比。在开环控制模式下，机器人只用逆运动学模型来算出轮速的指令，不会对实际行驶偏差做任何的反馈修正。仿真结果表明由于初始位姿误差和模型参数的差异，使得机器人轨迹与参考路径之间横向误差随着行驶距离的增大而增大，最大误差达12厘米左右，在路径转弯处比较明显。切换到双环PID闭环控制之后，控制器依靠角速度环、线速度环的即时反馈修正来很大程度上压制了误差的发散趋向。仿真数据显示闭环控制的横向跟踪误差可以被控制在2.5厘米之内，稳态误差趋于零。积分项可以消除由于轮径不一致所引起的静差，微分项可以抑制误差快速变化的时候出现的超调、振荡。闭环系统幅值裕度、相位裕度均符合稳定性判据的要求，说明控制器在典型工况下有较好的鲁棒性。

七. 系统集成、调试与结果分析

在完成运动学建模、硬件架构设计、多传感器标定、路径规划算法改进和PID跟踪控制器设计等各个模块的独立验证之后，本章将这些模块集成在一起形成一个完整的移动机器人自主导航系统，在真实的室内仓储模拟环境中做综合调试和性能评价。系统集成指搭建软件框架，设置各个模块间的通讯协议，并使传感器数据尽快地融合到规划和控制的算法当中，从而实现三个模块的协同。

本章主要对本章提出的算法和参数组合，用多组对比实验对其性能做了定量分析，即规划效率、路径质量、避障鲁棒性、实时性四个方面，证明了本文所提技术方案是有效的，并具有工程可行性。

7.1 软件系统集成

本文基于ROS Noetic框架建立移动机器人模块化软件系统，用分布式节点的方式把感知、规划、控制等模块解耦并协同起来。何晨昊等人提出了以ROS为平台的移动机器人路径规划研究方法，并且和本文中移动机器人路径规划算法的ROS框架以及多节点协同优化有关[11]。传感层运行激光雷达驱动节点、RGBD相机驱动节点，分发频率分别为10Hz、30Hz的点云数据以及深度图。

决策层是本文的主要贡献之处，将改良过的CDBA-RRT路径规划算法封装成ROS插件，并且将其嵌入到move_base功能包的全局规划器模块里，取代了原来的规划器。插件会收到感知层的栅格地图以及定位模块的当前位置在接收到目标点之后会使用CDBA-RRT算法计算出全局最优点，通过动态重规划的方式来适应环境的变化。驱动层用rosserial协议与STM32电机控制板进行串行通信，把PID控制器输出的线速度、角速度指令解析成PWM占空比信号，发送到电机驱动板上。系统运行时所有的关键数据都可以用rviz可视化的工具实时监控，有利于调试和故障查找。

7.2 实验环境搭建与调试过程

为了对系统在复杂的动态环境中做全面的检验，在室内模拟仓库环境下建立了专门的测试平台。实验场地尺寸为长10米、宽8米，地面是铺设平整环氧地坪漆的地面，模拟的是工业仓储的常见地面条件。场地内布设了六个形状不规则的纸箱作为静态障碍物，单个纸箱尺寸大约为长0.4米、宽0.4米、高0.5米，随机分布在场地中部。在场地一侧设一条宽0.5米、长2米的窄小走廊，对算法在狭小空间里的通行进行试验。动态障碍物用一台遥控移动小车来模拟，这台小车的速度约为0.4米每秒，在L型轨道上往复运动，与机器人的规划路线发生交叉。

机器人起始点设在场地左下角，目标点设在场地右上角，两点之间的直线距离约为12米，规划路径要绕过所有静态障碍物，并穿过狭窄的走廊。实验分三大部分。静态环境下只对传统的RRT算法和本文提出的CDBARRT算法进行25次运行，得到规划时间、路径长度等参数。第二阶段为动态避障测试，开启动态移动小车，测试两种算法在碰到突发障碍物时的重规划成功率以及响应时间。第三阶段就是把系统各部分联调，检验由传感器采集、地图建立、路径规划、PID跟踪控制组成的所有过程协同完成情况。每组实验做5次取平均值来消除随机因素的影响。

7.3 性能评价指标与结果分析

第一项就是规划效率，即系统收到目标点之后生成一条可行路径所需要的时间，单位是毫秒，体现算法的计算速度。曹腾等提出了带有预设性能的多移动机器人轨迹跟踪控制方法，和本系统中多移动机器人轨迹跟踪控制的预设性能以及协同优化设计有关[12]。第二项为路径质量，用路径长度和平均曲率两个子指标来表示，路径长度越短，能量消耗越小，平均曲率越小，轨迹越平滑，对电机的磨损就越小。

杨昊航提出基于PLC的智能电梯控制系统设计方法，和本系统中移动机器人控制系统和PLC协同控制的设计有关[13]。避障鲁棒性就是机器人在有动态障碍物干扰的时候，能将目标点移到成功次数所占的比例。姜佩贺等提出了改进的RRT算法移动机器人路径规划研究方法，与本文中移动机器人路径规划算法的改进以及复杂环境适应性验证相关[14]。第四项是实时性，即机器人检测出动态障碍物之后，从检测到生成避障路径所需要的响应时间，单位为毫秒。

共做了50组对比实验，其中传统RRT算法有25组，本文改进的CDBA-RRT算法也有25组。戴雨果等人提出了使用改进双向JPS算法的移动机器人路径规划方法，该方法和本文所用移动机器人路径规划算法的双向搜索优化、复杂环境适应性有相关之处[15]。从表可知，用传统的RRT算法求解时平均规划时间为118.4毫秒、路径长为12.67米、曲率是0.42米、成功率82.0%、重规划时间为210.5毫秒。

结论与展望

本文就移动机器人在复杂动态环境下自主导航的问题展开研究，对路径规划算法改进、运动学建模、硬件架构设计、多传感器融合和运动控制等关键技术进行了研究，得出一套完整的移动机器人自主导航技术方案。本文以差速驱动构型为基础建立了包含非完整约束条件的运动学状态空间模型，推导出了正逆运动学方程和雅可比矩阵，在MATLAB中进行了仿真验证，质心轨迹和理论值偏差小于0.005米。从硬件架构设计的角度来说，本文用树莓派4B做核心控制单元，结合激光雷达、RGBD深度相机和超声波传感器形成了一个多源感知系统，设计了强弱电分开供电的方式以及光耦隔离电路，保证了系统在高负载情况下也能正常工作。通过系统的集成实验可以得出CDBA - RRT算法的平均规划时间为85.3毫秒、动态避障成功率达到了96.8%、重规划响应时间小于100毫秒，相比传统的RRT算法具有更好的效果，且是可行的工程方法。

虽然本文所设计的移动机器人系统在路径规划效率、轨迹平滑性、跟踪精度等各方面取得较好的结果，但是对复杂环境的感知能力仍需提高。目前系统主要用二维激光雷达构建占据栅格地图，对于低矮障碍物、悬空物体和透明材质的感知能力较差。未来可以使用视觉语义分割网络，对RGBD相机得到的彩色图像进行像素级别的语义识别，识别出行人、货架、叉车等动态目标，并结合激光雷达点云数据形成多层次的环境表征，提高系统对于非结构化场景的识别和预判能力。多机器人协同作业的发展。单个移动机器人在仓储物流等大规模应用环境中存在吞吐量瓶颈，可以将本文提出的方法扩展到多机器人系统，研究基于分布式优化的全局路径协调方法以及基于优先级的局部冲突避免算法，从而实现多机器人协同搬运、交叉路径无死锁调度等目的，有效提高整个系统的作业效率。提高算法的泛化性能。目前算法部分参数依靠人工经验来整定，对于不同的场景缺乏自适应性。未来可以采用深度强化学习框架，在仿真的环境下根据CDBA-RRT算法的代价函数权重、采样概率、步长衰减系数等端到端学习，然后用Sim-to-Real迁移的方式把仿真的结果迁移到真实机器人的上，从而大幅缩短了算法在新环境中的部署调试周期。