移动机器人平台低成本自主导航系统设计与实现

赵世鑫，王春彦，王佳楠，李春雨

（北京理工大学宇航学院，北京 100081）

1 引 言

地面移动机器人具有运动稳定性高、承载能力强、机械结构简单等特点，能够在复杂恶劣环境中代替人类执行特定任务，已经成为当前的研究热点[1-2]。移动机器人在楼宇巡逻、地面清洁、仓储货运等场景中都具有巨大的应用价值，可以极大丰富人们的生活体验。自主导航、自主规划以及地图构建共同构成了移动机器人系统的核心，是移动机器人实现智能化、自主化的前提。

常见的移动机器人通常采用多传感器组合导航的方式，Zheng等[3]采用激光雷达、惯性测量单元以及GPS 多传感器融合的方法解决轮式机器人的导航问题。陈智伟等[4]利用激光雷达、相机和惯性测量单元（Inertial Measurement Unit，IMU）组成车载多传感器系统获取环境信息，通过训练RBF 神经网络对环境进行建模。李中道等[5]提出一种基于超宽带与激光雷达的融合定位算法，算法基于粒子滤波对两个传感器的数据进行互补融合解算，以提高定位精度。上述方法使用多传感器进行组合导航虽然有助于提高系统定位精度，但同时也增加了系统的复杂性，对机载计算机的运算能力提出更高的要求。

根据使用传感器的类型，常用的定位方法大致可以分为两类，基于激光雷达的定位方法和基于视觉的定位方法[6]。Gmapping[7]、hector slam[8]等采用单线激光雷达作为输入，是一种二维SLAM 算法。LOAM[9]、LeGO-LOAM[10]、LIO-SAM[11]、LOAM_livox[12]等主要针对多线激光雷达的三维SLAM 算法，可以输出传感器在三维空间中的位姿。VINS-mono[13]、Open-VINS[14]等融合视觉信息与 IMU 信息实现定位。ORB-SLAM2[15]是一种基于ORB 特征点的几何SLAM 框架，支持单目、双目以及RGB-D 相机。

在移动机器人的路径规划中，A*算法具有广泛应用。许多学者基于A*算法提出了不同的改进方法，Ju 等[16]提出了一种基于人工势场法和A*算法的融合算法，提高了A*算法的动态避障能力。张志文等[17]使用JPS 算法提升A*算法节点扩展效率，利用贝塞尔曲线对规划路径进行平滑处理，提高了A*算法的规划能力。杨瑶等[18]将车辆的运动学模型融入A*算法的搜索过程，采用自由边界三次插值平滑路径，生成更加符合车辆运动特性的轨迹。上述方法都是在二维平面进行规划，没有充分利用三维环境信息。

本文提出了一种基于单一传感器的低成本方案解决移动机器人在室内未知环境中自主规划与地图构建问题。该方案选取廉价的RGB-D 相机作为环境感知传感器，利用RGB-D 相机提供的环境深度信息和RGB 图像信息实现自主定位，实时解算移动机器人的位置和姿态，无需额外的里程计(车轮编码器)或者IMU 等传感器。此外，利用RGB-D 相机采集的点云数据，可以构建未知环境的三维地图，完成对未知环境的探测。为提高移动机器人在未知环境中运动轨迹的安全性和平稳性，本文基于A*算法提出了一种适用于二维平面运动物体的三维规划方法。首先，根据RGB-D 相机提供的点云数据构建局部3D 占据栅格地图，在三维空间中进行碰撞检测，提高规划路径的安全性；其次，改进A*算法节点扩展方式，将节点的搜索过程转移到状态空间，提高规划路径的平滑性；最后，基于B 样条曲线构建优化方程，进一步提升初始路径的安全性和平滑性，保障移动机器人在未知的环境中平稳安全移动。

本文设计并实现了一种基于单一传感器的自主系统，该系统能够感知非结构化的环境，并且提供足够精度的自定位信息。系统采用体积小、重量轻、低功耗的Intel NUC 作为主计算单元，易于部署到小型自主移动平台。此外，NUC 的运算能力支持扩展更多的功能，如物体的检测与识别等计算机视觉任务。系统的软件框架基于机器人操作系统（Robot Operating System,ROS）搭建，具有较强的灵活性，可以兼容不同的硬件设备，使得系统支持不同的硬件解决方案成为可能。此外，本文提出的软件算法求解速度快，不需要消耗太多的计算资源，易于部署到不同的运算平台。本文提供了包括软件部分和硬件部分在内的完整系统框架，以及该系统在现实环境中的应用效果。

与现有的工作相比，本文提出的系统采用单一传感器作为输入，使用低成本的相机实现移动机器人自主系统的构建。为解决无人系统在未知环境的自主避障问题，改进了传统的A*算法，提高了系统的环境适应能力。此外，本文还提出了一些规划策略，以提高系统的实时性。本文的贡献总结如下。

（1）本文提出了一种可以在未知环境中实现自主规划与地图构建的低成本解决方案。其核心部件只有一个感知环境信息的RGB-D 相机以及一台提供计算服务的机载计算机。

（2）本文提出了一种适用于二维平面运动物体的三维规划方法，充分利用三维环境地图，实现三维避障。

（3）本文提供了一套基于开源软件的系统框架，包含软件架构和硬件架构，便于快速部署。

2 系统概况

系统的软件框架基于ROS 搭建，整套系统采用模块化的设计理念，按照功能划分为不同的模块单独进行开发，再按照相应的需求将不同的功能模块进行组合，完成整套系统开发，如图1所示。基于ROS 的特性，将每个功能模块设计为一个节点，节点的运行是由ROS Master 统一进行调度和分配，节点之间的通信采用ROS 的话题机制，发布者将消息发布到指定的话题中，接收方订阅相同的话题，双方即可建立通信链路，完成信息传输。

图1 系统软件框架Fig.1 System software framework

系统使用了部分基于 C++编写的开源代码包，驱动包 realsense-ros[19]用于处理 Realsense D455 RGB-D 相机的数据，将其转化为ROS 标准信息类型，通过ROS Topic 的形式发布。定位模块基于ORB-SLAM2 算法设计，定位模块订阅ROS 相关的话题，获取RGB 图像和深度图像，计算相机当前的位置和姿态，输出位姿矩阵Tcw∈SE(3)。Tcw包含旋转矩阵和平移向量，位姿转换模块将矩阵Tcw转换为ROS 标准格式的里程计信息（ nav_msgs/Odometry） 和位姿信息(geometry_msgs/PoseStamped)供系统其他模块使用。局部建图模块和轨迹规划模块基于Fast-Planner[20]框架搭建，局部建图模块接收RGB-D 相机的深度图像和里程计信息，生成局部3D 占据栅格地图用于路径搜索，同时生成ESDF地图用于轨迹优化。由于传感器的感知范围有限，因此采用局部规划的策略，系统只在局部地图中进行轨迹规划，到达局部地图的边缘则停止规划，系统间隔固定的时间周期调用规划器以更新轨迹，随着机器人的移动逐步规划到目标点。运动控制模块结合期望轨迹信息和当前里程计信息计算误差量，采用PD 控制器计算期望速度和角速度，实现轨迹跟踪，并将速度指令转换为Twist消息类型（geometry_msgs/Twist），以ROS Topic的方式发布，使得系统可以快速地移植到任意一种支持Twist 控制指令的移动平台上。

3 系统关键算法分析

3.1 路径规划算法

路径规划是指移动机器人按照某一性能指标（如距离、时间等）搜索一条从起始状态到目标状态的最优或次优路径。常用的搜索算法有Dijkstra、A*、RRT、RRT*等，但是利用这些算法得到的路径往往是非光滑的。此外，传统的规划算法仅在传感器扫描得到二维平面上搜索，因此只能避开搜索平面内的障碍物，对于平面上方或者下方的障碍物难以有效规避。本系统采用的环境感知传感器为RGB-D 相机，可以建立机器人周围环境的三维地图。因此，本文对传统的A*算法进行改进，优化节点扩展策略，使得搜索的路径更加平滑，同时在三维空间中判断路径是否发生碰撞，实现三维避障。

传统A*算法在扩展节点时，以当前节点为中心，向着8 个方向探索，寻找与当前节点在几何上相邻的节点，节点与节点之间以折线段相连，规划的路径缺乏平滑性，如图2(a)所示。针对上述问题，基于Dolgov 等[21]提出的方法改进A*算法的节点扩展方式。假设移动机器人的状态向量为x(t)=[P,V]，则状态空间模型为

图2 节点扩展方式对比Fig.2 Comparison of node expansion modes

其中，

式中，I2为二阶单位矩阵；P=[px,py]T表示移动机器人在惯性系下的位置；V=[vx,vy]T表示移动机器人在惯性系下的速度；输入u=[ax,ay]T为移动机器人的加速度，其中ax,ay∈[-amax,amax]。

节点扩展时，在每个坐标轴方向上分别离散化输入ud为将当前的状态x(0)以及输入ud∈U带入系统状态方程的解

可以得到在状态空间相邻的节点。改进后的A*算法节点扩展过程如图2(b)所示。

传统A*算法中常用的代价函数有曼哈顿距离、对角线距离以及欧几里得距离等，这些代价函数都是计算几何空间的距离，考虑到改进后的A*算法节点扩展是在状态空间中进行，因此采用文献[22]中的方法计算路径的代价，找到一条在时间和控制成本上最优的路径。定义代价函数为

式中，ρ为常值。

与传统A*算法类似，我们使用gc表示从起始状态xs到当前状态xc的实际代价，假设最优路径共包含J段，则gc的计算公式如下

式中，udj表示第j段路径的输入值，τ表示第j段路径的持续时间。

预估代价对于提升A*算法的搜索效率是非常重要的。根据当前节点的状态信息xc以及目标点的状态信息xg，采用庞特里亚金极小值原理求解式子(3)，可以得到最优控制输入u*(t)以及最小代价J*(T)。

式中，pμc,vμc表示当前节点的位置和速度，pμg,vμg表示目标节点的位置和速度。根据式(5)可以看出，J*(T)只与T有关，因此可以对T求导得到最优的代价。

将αμ,βμ的值带入式(6)，可以得到一系列极值点，挑选出符合运动学模型且代价最小的Topt，并将J*(Topt)作为预估代价hc。

传统规划算法只在二维平面进行搜索。如图3 所示，蓝色平面为传感器扫描平面，由于移动机器人自身有一定的高度且传感器的安装位置也有一定的高度，图3(a)中红色区域可能与机器人发生碰撞，因此二维平面的搜索难以保证机器人的安全。针对上述问题，本文改进了传统A*算法的碰撞检测部分。利用系统创建的三维环境地图，不仅在搜索平面内进行碰撞检测，还根据机器人自身高度在垂直方向上进行碰撞检测，如图3(b)所示。系统在垂直方向任一点检测到碰撞，则重新进行规划，以避开障碍物。

图3 不同规划方法对比示意图Fig.3 Comparison of different planning methods

3.2 轨迹优化算法

由于控制输入的离散化，搜索得到的路径不一定是最优的。此外，由于搜索过程没有考虑与障碍物之间的距离信息，得到的路径可能距离障碍物较近，因此需要对路径进行优化。基于文献[22]中提出的方法，利用B 样条曲线局部可调整的特性，结合约束条件，生成平滑、安全、动态可行的轨迹。

B 样条曲线是由它的阶次数pb、控制点{Q0,Q1,…,QN}以及节点向量 [t0,t1,…,tM]构成，其中Qi∈ℝ3,tm∈ℝ。首先将路径规划算法搜索到的几何路径转化为B 样条曲线，然后对B 样条曲线控制点的子集进行优化。总

代价函数定义为

式中，fs是平滑代价函数，fc是碰撞代价函数，fv是速度软约束函数，λs,λc,λv为权重系数。

平滑代价函数主要是利用轨迹的几何信息进行平滑，此处选择弹性带代价[23]，平滑代价函数为

碰撞代价函数为障碍物在每个控制点上的斥力的总和，轨迹碰撞代价函数定义如下

式中，M代表控制点Qi在Z轴上平移得到的点的集合，d(Qi,j)代表控制点Qi,j与其邻近障碍物之间的距离，可通过查询ESDF 地图[24]获得，Fc是一个可微分函数，其值与阈值dthr以及距离函数d(Qi,j)有关，表达式如下

速度软约束函数主要对轨迹中超过最大允许值vmax的速度项进行惩罚，速度软约束函数定义如下

其中

3.3 地图构建算法

3.3.1 局部地图

局部地图采用3D 占据栅格地图模型，如图4所示。占据栅格地图将环境分为一系列大小相同的立方体，每个立方体存储着环境的三种状态信息，分别为占用、空闲以及未知。整个地图在初始化时将所有的立方体的状态设置为未知，随着传感器的扫描进行更新，当环境中某处存在障碍物时，其对应的立方体的状态变为占用，此时代表移动机器人无法穿过此区域；环境中空旷的部分对应立方体的状态为空闲，表示移动机器人可以安全通过。地图的分辨率可以根据实际需求进行设置。立方体的状态信息采用概率值表示，概率值为0～1，概率值越接近0，空闲的可能性越大，概率值越接近1，占用的可能性越大；概率值为0.5 表示该单元的状态未知。

图4 3D 占据栅格地图Fig.4 3D occupancy grid map

3.3.2 全局地图

ORB-SLAM2 虽然具有构建全局地图的功能，但是构建的地图较为稀疏，且地图构建效率低。本文只使用ORB-SLAM2 的定位功能；局部地图采用的3D 占据栅格地图是安装设定的分辨率对环境均匀切分，优点为可以使用坐标索引的方法查询该单元的状态信息，提高轨迹规划的效率。缺点是对存储空间的消耗非常大，不适于构建大范围的地图。因此，全局地图构建采用OctoMap[25]。OctoMap 是一种高效表示3D 环境的方式，它可以根据激光雷达或者RGB-D 相机的测量值，实时构建并更新三维环境地图。OctoMap 本质上是一个基于Octree 数据结构的三维空间网格映射框架。Octree 是一种包含多个节点的分层结构，这些节点（也称为体素）是空间中的立方体，每个节点可以包含8 个或者零个子节点，当节点的8 个子节点都被占用或都空闲时，当前节点将不再划分子节点，仅对自身状态进行标记。其他情况下，每个节点被递归地划分为8个子节点，同时分别计算子节点的占用概率值，重复上述操作，直到达到预设的最小节点尺寸。

定义子节点n被占据的概率为P(n|z1:t)，传感器的测量值为z1:t，P(n|z1:t)的计算方法如下

式中，zt为传感器在t时刻的测量值；P(n) 为先验概率，统一设定为0.5；P(n|z1:t-1)为t-1时刻的估计值；P(n|zt)表示在给定测量值zt的条件下节点n被占据的概率。

为简化计算，对式(13)进行Logit 变换

其中

从式(14)可以看出，每次更新地图时只需计算当前测量值zt的对数几率值L(n|zt)，显著降低了计算量。为了应对传感器噪声和环境的变化带来的影响，设定占用率估计值的上下限

式中，lmin和lmax表示对数几率值的上限和下限。

修改后的更新公式限制了改变节点状态所需的更新次数，保证算法可以快速滤除传感器噪声以及环境变化带来的影响。

4 系统实现

图5为本文所设计系统的硬件框架，系统大致可以分为4 部分：具有环境感知能力的传感器；负责地图建立、导航规划的机载计算机；具备移动能力的差分轮式机器人以及用于远程监视、数据收集及显示的远程计算机。各个模块之间通过USB 以及局域网传输数据，本节将详细介绍系统的组成部分。

图5 系统硬件框架Fig.5 System hardware framework

机载计算机采用Intel NUC10i7FNH 微型计算机，如图6所示。NUC10i7FNH 搭载第10 代智能Intel®酷睿i7 处理器，拥有六核十二线程，基准频率1.1GHz，具有睿频加速技术，最大加速频率4.7GHz。系统中涉及的定位、规划以及建图算法都运行在机载计算机上，使得移动机器人成为一个不依赖外部资源的独立系统。机载计算机运行Ubuntu18 系统且安装了ROS，NUC 提供的强大计算能力以及ROS 的灵活性，使得现有系统易于实现功能扩展。

图6 NUC10i7FNH 迷你电脑Fig.6 NUC10i7FNH mini computer

环境感知传感器采用Realsense D455 RGB-D相机，如图7所示。Realsense D455 拥有一个红外投射器、一对双目相机以及一个RGB 相机，深度测量基于双目测距原理，红外投射器用于辅助测距，通过投射不可见的固定的红外纹理样式，提高相机在缺少纹理的环境中的测距精度。Realsense D455 输出的数据流通过USB 线传输到机载计算机。

图7 Realsense D455 RGB-D 相机Fig.7 Realsense D455 RGB-D camera

移动机器人拥有6 个独立驱动的轮子，如图8所示，6 个轮子分为左右两组，采用差速驱动模式运行。底盘控制器选用STM32F103ZET6 作为主控芯片，该芯片基于超低功耗的 ARM Cortex-M3 处理器内核，工作频率最高可达72MHz，而且具有丰富的硬件资源和外设接口，完全能够满足移动机器人底层控制需求。底层控制器利用自身的MAC 口接入局域网，基于UDP（User Datagram Protocol）协议与机载计算机进行数据交互。

图8 移动机器人底盘Fig.8 Mobile robot chassis

远程计算机用于实时监控系统的状态，其不参与移动机器人自主规划系统的任何运算，也不是系统的重要组成部分。远程计算机的主要任务是通过安全外壳协议（Secure Shell，SSH）与机载计算机建立通信链路，收集并显示系统数据。在本文设计的系统中，远程计算机为运行Linux系统的个人笔记本电脑。此外，系统还配备了乐迪T8FB 航模遥控器，遥控器可以取代机器人的自动规划系统，手动控制机器人的运动。遥控器作为一种安全措施，只在紧急情况下使用。

实验测试所用的移动机器人平台如图9所示，RGB-D 相机安装于机器人顶端，可以保持宽广的视野；机载计算机位于机器人中部，通过USB 线连接RGB-D 相机获取环境信息，通过网线连接通信模块接入局域网；通信模块采用TP-LINK AC1200 路由器，通信模块用于构建通信局域网，机器人底盘控制器、机载计算机以及远程计算机共同接入局域网以实现信息交互；机器人底盘控制器基于UDP 协议和机载计算机实现通信，接收机载计算机的控制指令，同时反馈移动机器人的状态信息；机载计算机与远程计算机基于ROS 分布式通信协议实现信息交互，远程计算机可向机载计算机发布指令，读取并显示系统创建的地图、移动机器人的运行轨迹等；电源模块稳定输出12V 直流电压，为机载计算机和通信模块提供稳定电源。表1列出了本文所提系统硬件设备的详细价格。

图9 系统验证平台Fig.9 System verification platform

表1 成本分析Table 1 Cost analysis

5 实验结果

5.1 规划算法实时性分析

机器人在未知环境中移动要求自主规划系统具有良好的实时性，而规划模块的实时性对于保障机器人的安全起着关键性的作用。为提高轨迹规划模块的实时性，系统并未使用全局地图进行轨迹规划，而是以移动机器人为中心构建局部地图，规划算法在局部地图中进行搜索，进而提升系统应对复杂环境的能力。为验证规划算法的实时性，在有大量障碍物的环境中进行测试，每次规划起点与终点的距离为3～8 m，轨迹规划的周期为20 ms，测试结果如图10所示。根据图10中的结果可以看出，路径搜索的时间基本在4 ms以内，轨迹优化所需的时间稳定在6 ms 以内，轨迹规划所需的总时间不超过10 ms，规划模块的性能完全满足机器人安全移动的需求。

图10 规划算法运行时间统计图Fig.10 Statistical chart of planning algorithm running time

5.2 三维规划实验

本文提出的规划算法基于三维环境地图，在立体空间中进行搜索，是一种三维空间搜索方法。相比于二维平面搜索，可以有效提升轨迹的安全性。为测试规划算法的三维避障能力，设置测试场景如图11所示，红色平面为传感器所在的平面，场景中有三种不同高度的障碍物，分别位于传感器平面的上方和下方，规划的起点和终点设置在障碍物的两侧，以保证规划的轨迹需要通过障碍物。由于RGD-D 相机观测能力有限，图11与图12中浅黄色区域为传感器观测盲区。设定搜索步长τ=1.2，权重系数λs=10.0，λc=5.0，λv=0.01，移动机器人的最大速度vmax=0.5，碰撞代价阈值dthr=0.4。

实验结果如图12所示。基于二维平面搜索得到的轨迹无法避开靠近搜索平面但分别位于平面下方的障碍物1 和平面上方的障碍物2，规划得到的轨迹近似为一条直线，如图12(a)所示。基于三维空间搜索的轨迹规划方法判断出障碍物1 和障碍物2 会与移动机器人的底部和顶部发生碰撞，因此选择绕开障碍物1 和障碍物2，规划出的轨迹如图12(b)所示。图12(b)中绿色曲线是基于改进的A*算法搜索得到的路径，红色曲线是优化后生成的轨迹。可以看出，相比于原始路径，优化后的轨迹在实现三维避障的同时，具有更优的平滑性，更适于机器人的运动。

5.3 路径规划算法对比实验

为验证改进后的A*算法的效果，本文对改进A*算法与传统A*算法进行对比测试，结果如图13所示。图中红色路径为改进A*算法的搜索结果，蓝色路径为传统A*算法的搜索结果，从图中可以看出改进的A*算法搜索的路径更加平滑。此外，由于改进A*算法考虑了移动机器人的运动学特征，因此在起点附近会沿着移动机器人的前进方向进行规划，保证机器人运动的平顺性。

为测试系统的实际效果，我们在实验室搭建了实验场地，如图14所示，地面上堆放大量的箱子以及挡板作为障碍物，系统的目标是在没有环境地图的情况下，实时规划出一条安全的轨迹，保证移动机器人不与障碍物发生碰撞，同时在移动的过程中创建实验场地的三维地图。为引导移动机器人走完整个实验场地，我们在起点和终点之间设定了两个中间点，以约束机器人的运动轨迹。系统的整个运行过程都是在没有人为参与的情况下自主完成的。

5.4 实时避障实验

轨迹规划子系统只依赖于传感器扫描得到的局部地图，无需预先加载环境地图，系统以20 ms间隔调用规划器，以50 ms 间隔进行碰撞检测，若轨迹与障碍物发生碰撞，则立即进行重规划。从图15可以看出，在移动机器人转弯前，由于Realsense D455 相机的视场角有限，无法感知前方可能存在的障碍物，规划出的轨迹比较平直；当移动机器人转过弯后，传感器检测到轨迹前方的障碍物（绿色部分），如图16所示，系统迅速调整轨迹偏离障碍物，保证移动机器人安全行驶。实验结果表明系统可以做到实时避障。

图15 转弯前未探测到障碍物Fig.15 No obstacles detected before turning

图16 转弯后探测到障碍物Fig.16 Obstacles detected after turning

5.5 全局地图效果

本文所提系统基于OctoMap 算法实现三维地图构建，R G B-D 相机的点云数据作为octomap_sever[26]的数据输入，地图分辨率设置为0.05 m，全局坐标系设置为“world”，点云数据以“camera_link”为参考坐标系，坐标系“world”与“camera_link”之间的坐标变换关系由预先配置的TF 树提供。移动机器人在实验场地中的移动轨迹以及实验生成的三维地图如图17所示，在楼宇走廊内实验得到的三维地图如图18所示。实验结果表明，系统可以较为高效地实现三维地图的实时构建，完成对未知环境的探测任务。

图17 实验场地三维地图Fig.17 3D map of the experimental site

图18 走廊三维地图Fig.18 3D map of the corridor

5.6 导航精度分析

为定量分析系统自主定位的精度，在移动机器人顶部放置反光球，记录OptiTrack 动作捕捉系统输出的位姿数据作为机器人运动的真实值，并与本文所述系统定位模块输出的位姿数据进行对比，实验结果如图19～20 所示。

图19 真实位姿与估计位姿对比Fig.19 Comparison of real pose and estimated pose

图20 真实位姿与估计位姿之间的误差Fig.20 Error between true pose and estimated pose

为定量分析3D 占据栅格地图的精度，将文中建图算法得到的三维环境地图与真实环境进行对比。通过ROS 平台下的Rviz 可视化软件的测量工具，对地图的多个位置进行测量，如图21所示。并与真实环境下相同位置测量数据进行对比，实验结果如表2所示。

图21 3D 占据栅格地图距离测量Fig.21 3D occupancy grid map distance measurement

表2 真实值与测量值对比Table 2 Comparison of real value and measured value

6 结 论

本文设计并实现了一种基于单一传感器的低成本自主系统，用于实现移动机器人的自主规划以及对未知环境的探测。系统的核心部件为一台提供计算服务的机载计算机和一个感知设备—Realsense D455 RGB-D 相机，硬件连接简单，便于部署。为保证机器人在未知环境中安全移动，提出了一种基于A*算法三维规划方法，改进了传统A*算法的节点扩展方式，扩大了碰撞检测的范围，提高了规划轨迹的平滑性和安全性，从而更好地保障机器人的运行安全。地图构建子系统基于OctoMap 算法，可以在机器人移动期间构建三维环境地图，完成环境探测任务。该系统在实际环境中进行了多次测试，测试结果表明该系统具有良好的稳定性，且软件算法运算速度快，可以实现实时避障，具有极强的环境适应性。在未来的研究中，可以优化系统的软件架构，使系统更加简洁；还可以考虑提升规划算法的性能，实现对动态障碍物的规避。此外，也可以将系统扩展为多机协同控制系统，实现多个移动机器人协同规划与探测。