CN112130579A - 一种隧道无人机巡检方法及系统 - Google Patents

Abstract

本发明公开了一种隧道无人机巡检方法及系统，包括：采集无人机的姿态、方向、速度、加速度信息，无人机的高度信息，以及无人机的相对坐标信息；对上述数据进行融合，实时解算当前无人机在空间中的姿态角、速度以及空间坐标数据；对无人机航迹点路线周围环境进行实时感知，计算无人机与周围障碍物距离，实时修改航迹点，以实现无人机避障飞行；通过控制云台角度，以实现对隧道内设备的图像采集。本发明结合隧道的结构特点，在进行航迹点规划时，采用了全局固定坐标系作为基准航点坐标系，将曲线动坐标系作为航点规划的辅助参考，实现了隧道内航点即时修改，使无人机在隧道内能实现在固定飞行距离的情况下完成沿隧道通道的转向运动。

Description

一种隧道无人机巡检方法及系统

技术领域

本发明涉及无人机巡检技术领域，尤其涉及一种隧道无人机巡检方法及系统。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

目前，隧道内运维人员短缺尤其是结构性缺员问题日益突出，对隧道内的设备巡视、红外测温、表计抄录、缺陷跟踪、巡检数据录入等工作仍大量依赖于人工。人工巡检方式的巡检质量不易管控、巡检效率不高，难以适应新形势下的“精细化”发展需求。而目前公知的吊轨式巡检机器人实现的设备状态自动检测功能尚不完备，仍无法替代人工巡检。且吊轨机器人开发购买成本较高，难以实现在目前日益增长隧道中广泛使用。

随着无人机技术的提升，做为一种高效的巡检设备载体，通过搭载不同的传感设备无人机能实现在电网线路巡检、森林防火、测绘等不同领域自主巡检，因此在隧道内使用无人机进行巡查成为可能。

传统室外无人机普遍采用的是GPS定位方式，通过GPS信号获取无人机的位置信息实现位置控制，2.4G/5GHz射频信号、移动网络、WIFI网络都是较普遍的传输信号的方式，无人机的控制信号及载荷的图像信号基本都通过这三个途径传输，通过2.4G/5GHz射频信号、移动网络信号或WIFI信号传输控制及图像信号，通过气压计反馈高度信息实现定高飞行控制；

传统室内无人机通讯传输多采用WIFI或者移动网络信号的方式进行，虽无法接受GPS信号，但可采用激光或视觉SLAM的方式完成，并且由于不是完全密闭的空间，也可以采用气压计获得准确的高度信息。

然而，在隧道中由于地磁屏蔽无人机无法接受到GPS信号，同时在狭小密闭的隧道空间内气压不均匀，不会像地面开阔场景中一样可以采用气压计采集高度信息，又由于隧道中一旦有转角，无法满足无人机与遥控器间的通视距离，2.4G/5GHz的传输信号无法透过墙体，且隧道狭长，不方便布置WIFI，移动网络信号也无法接收，所以无法实现精准的定位及信号传输。

因此，受限于隧道内的物理及通讯环境，在地下无GPS、无移动通信网络的情况下，传统无人机很难实现精准的定位及信号传输。

发明内容

有鉴于此，本发明提出了一种隧道无人机巡检方法及系统，该方法不依赖GPS及移动通信网络，基于多种传感器数据融合实现自身位姿估计与避障功能，可在隧道环境下实现自主飞行及自主巡检。

为了实现上述目的，本发明采用如下技术方案：

根据本发明实施例的第一个方面，提供了一种隧道无人机巡检方法，包括：

采集无人机的姿态、方向、速度、加速度信息，无人机的高度信息，以及无人机的相对坐标信息；

对上述数据进行融合，实时解算当前无人机在空间中的姿态角、速度以及空间坐标数据；

对无人机航迹点路线周围环境进行实时感知，计算无人机与周围障碍物距离，实时修改航迹点，以实现无人机避障飞行；

无人机飞行过程中，通过控制云台角度，以实现对隧道内设备的图像采集。

根据本发明实施例的第二个方面，提供了一种隧道无人机巡检系统，包括：

数据采集模块，用于采集无人机的姿态、方向、速度、加速度信息，无人机的高度信息，以及无人机的相对坐标信息；

数据融合模块，用于对上述数据进行融合，实时解算当前无人机在空间中的姿态角、速度以及空间坐标数据；

避障模块，用于对无人机航迹点路线周围环境进行实时感知，计算无人机与周围障碍物距离，实时修改航迹点，以实现无人机避障飞行；

图像采集模块，用于无人机飞行过程中，通过控制云台角度，以实现对隧道内设备的图像采集。

根据本发明实施例的第三个方面，提供了一种隧道无人机，包括：无人机本体，所述机器人本体上分别搭载工业控制器、飞行控制器、激光雷达、UWB自解算标签以及传感器单元；其中，

传感器单元，用于采集无人机姿态、方向、速度及加速度信息，无人机的姿态角信息，以及无人机的高度信息；

工业控制器，用于基于采集到的数据计算无人机的空间位置及三轴加速度信息；

飞行控制器，用于根据工业控制器计算的数据控制无人机飞行；

激光雷达，用于通过扫描无人机周边环境，实时获取无人机距离周边障碍物的距离；

UWB自解算标签，用于与隧道中的UWB基站通信，实时计算无人机的空间全局坐标，以得到无人机当前位置。

与现有技术相比，本发明的有益效果是：

(1)本发明结合隧道的结构特点，在进行航迹点规划时，采用了全局固定坐标系作为基准航点坐标系，将曲线动坐标系作为航点规划的辅助参考，实现了隧道内航点即时修改，使无人机在隧道内能实现在固定飞行距离的情况下完成沿隧道通道的转向运动。

本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

图1为根据本发明实施例的一种隧道无人机巡检方法流程图；

图2为根据本发明实施例的一种隧道无人机巡检系统结构示意图；

其中，1.四轴无人机机体，2.工业控制计算机，3.飞行控制器，4.双目鱼眼摄像头，5.二维激光雷达，6.UWB自解算标签，7.红外可见光双光测温相机，8.单点激光测距仪。

具体实施方式

应该指出，以下详细说明都是示例性的，旨在对本申请提供进一步的说明。除非另有指明，本发明使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

实施例一

根据本发明实施例，提供了一种隧道无人机巡检方法的实施例，参照图1的一种隧道无人机巡检方法的流程图，包括以下步骤：

步骤1：采集无人机的姿态、方向速度、加速度信息，无人机的姿态角信息，无人机的高度信息，以及无人机的相对坐标信息；

具体地，利用双目鱼眼摄像头采集无人机的姿态及方向速度及加速度信息，利用飞控IMU惯性传感器采集无人机的姿态角信息，利用单点激光传感器采集无人机的高度，利用UWB采集无人机相对坐标信息。

步骤2：对上述数据进行融合，实时解算无人机在空间中的姿态角、速度以及空间坐标数据；

利用双目鱼眼摄像头图像，结合飞行控制器中的IMU惯性测量单元、UWB自解算标签以及单点激光测距仪得到的机体姿态数据进行融合，实时解算无人机在空间中的姿态角及位置坐标；

本实施例中，首先通过卡尔曼滤波，滤除噪声信息；然后采用最大似然估计方法，剔除明显失准的传感器信息；最后通过不同的传感器件分别获取无人机的姿态、方向、速度及加速度信息；对获得的信息进行差分处理，基于无人机的动力学模型，进行权重分配，推算出最精确的数据序列。

具体地，IMU惯性测量单元及双目鱼眼摄像头采集的无人机姿态角及加速度往往会包含很多的噪声信号，为保证采集数据的干净可信，首先通过卡尔曼滤波，利用动态信息，剔除噪声信号的影响，得到较为纯净的无人机位姿数据；由于单点激光传感器属于较为精确的测量器件，对单点激光传感器采集的距离信息采用较高的可信层级，无人机控制针对激光测出的距离将做出直接快速的响应；而场景中不可避免受到外界干扰的影响，如昆虫、纸屑等对无人机安全飞行没有影响但可能会由于瞬时遮挡导致激光测量数据出现偶发较大偏离，若不做处理，无人机飞行过程中将出项突然抖动或突然大幅度改变姿态的动作。因此，采用限幅滤波以T-1时刻数据变化速率及T时刻数据结合无人机设定的运动速率计算T时刻的估计值，并限制其增长幅度，若该T时刻值与T-1时刻值偏差超过设定的允许偏差值ΔT(该值通过实验测试效果得出)，则剔除T时刻值，并根据T-1时刻的数据及T-1时刻数据变化速率计算出T时刻的估计值。

具体地，首先以T-1时刻数据的变化速率(该速率由T-2及T-1时刻数据及数据采集周期计算得出)计算出一个T时刻估计数据(称为T1)，采集到T时刻的采集值(称为T2)，将T1与T2做平均值得到T3，此时将T3数据结合无人机运动速率(由于激光传感器固定安装于无人机上，无人机旋转(无人机在运动过程中会有沿各个轴不停的旋转调整以保持其稳定飞行及空间位置的固定)时激光将会发生偏移，此时的测量值与实际值有一个角度偏差，需将测量值乘一个旋转角度的COS值才是正确结果)，计算得出T时刻一个较精确的估计值(称为T4)，然后我们设定一个偏差允许值ΔT，若T4与T-1数据差值小于ΔT，则认为T4即为T时刻的估计值，若偏差大于ΔT，则以T1为T时刻的估计值。

为保证无人机位姿数据的准确性，采用了冗余的传感器设计，尤其是无人机的姿态角、方向速度及加速度信息分别可从IMU惯性测量单元、双目鱼眼摄像头(视觉测量器件)、UWB自解算标签获得；而IMU惯性测量单元存在累积误差和零点漂移，视觉测量器件受环境关照变化影响较大，UWB位姿解算精度相对较低，通过对三组数据进行差分处理，并通过微积分计算并根据分配权重，基于无人机的动力学模型，推算出当前时刻最可能精确的数据序列，以在最大程度上克服各传感器各自的缺点，提高数据精度，这样能够避免单一传感器由于光照条件或累积误差造成的定位失准。

具体地，对三组数据进行差分运算并求加权平均，比如加速度数据，实际采用三个传感器采集到的速度数据，通过差分运算得到三组计算的加速度数据，再把该计算加速度与采集到的加速度信号相比较，剔除异常增或异常小值，然后进行加权平均(加权方式通过实测经验获取)得到；速度数据采用采集到的加速的数据进行积分运算，先得到一组计算速度数据，然后与采集到的速度数据进行比较，剔除异常大或异常小值或不连续的值(因为速度是连续的，如果出现跳变或与同组数据中有明显大小差异的即可剔除)，然后进行加权平均得到；当通过差分及微积分运算得到数据序列时，通过无人机的动力学模型(现有模型)对其加速度、速度及角度值进行滤波融合。

步骤3：对无人机航迹点路线周围环境进行实时感知，计算无人机与周围障碍物距离，以实现无人机避障飞行；

具体地，根据解算机体位置坐标，结合由所述工业控制计算机输入的巡检长度及预设高度，生成巡检线路并生成若干航迹点空间坐标，无人机采用航点飞行模式实现在隧道内的定位和精准的导航飞行及自动起降；

在该步骤中，工业控制计算机采用双目鱼眼摄像头开机时的姿态点作为系统空间原点建立全局固定坐标系，全局固定坐标是在开机后以无人机为坐标原点生成的坐标系；X轴为无人机正向移动轴，Y轴为无人机侧向移动轴，Z轴为无人机高度移动轴，使用时输入预巡检距离L及飞行高度H，自动在空间中沿直线间距飞行距离1米设定若干个空间点作为航迹点，并拟合航线覆盖所述航迹点作为飞行航线，无人机启动自动飞行将延航线依次飞向下一航迹点直至达到最后航迹点并自动降落。

巡检高度和长度是在起飞前设定，航迹点在设定完高度和长度后生成，航迹点为以起飞点为原点，起飞点沿隧道方向为X轴，起飞点竖直方向为Z轴，起飞点垂直于隧道通道方向为Y轴的固定空间坐标系下自动生成的初始航迹点，此时的航迹点巡检长度与设定巡检长度相同即默认初始路径为直线路径。

利用所述二维激光雷达对航迹点路线周围环境进行实时感知，计算无人机机体与周围障碍物距离，进行无人机飞行过程的避障飞行及航点实时更新；

无人机在执行航迹点自动飞行时，二维激光雷达实时返回航迹路线周围障碍物距离信息，

当左右均有障碍物且间隔距离大于安全阈值时，修改航点Y坐标保证无人机处于隧道中间位置，并以Y坐标为参考重新设定下一航点坐标与当前航点坐标使其空间间距为1米。这样能够保证无人机在左右空间相对空旷的条件下，在隧道中央飞行，便于无人机上监控图像设备能同时捕捉到隧道左右两边的设备状态。

当无人机前方出现障碍物且一侧间距减少或不变，一次间距增加或在探测距离内没有障碍物，即出现隧道转向时，则以有障碍物一侧为基准，保持间隔距离重建下一航迹点以完成原地转向或弧度转向。

以无人机竖直方向为Z轴，沿隧道路径方向为X轴，选取无人机一侧隧道墙壁做为参考面，以其切线的垂直线方向为Y轴，以起飞点作为动坐标系原点坐标建立曲线动坐标系，曲线动坐标系是指一个随着无人机运动位置改变而随时变化的坐标系；坐标轴是曲线，且非固定曲线，坐标Y轴坐标原点变化而X轴坐标原点为起飞点。

若无人机前方出现障碍物，无人机与一侧障碍物间距减少或不变，与另一侧障碍物间距增加时，判断出现隧道转向，此时由于下一航点坐标建立于固定航点坐标系且是直线航点，因此需以此时动坐标轴三轴方向作为参考，间隔1米设立新航点，并通过坐标转换将该动坐标下的新航点坐标转换到固定坐标系下；当然，具体间隔的距离可以根据实际需要自行设定。

当两侧均出现障碍物且障碍物间距小于无人机安全阈值时，则无人机原定下降高度，若下降后距离满足通过要求则修改下一航点Z坐标值并飞行通过，若仍未满足通过要求，则无人机原地悬停并向地面站返回告警信息选择返航或原地降落，若没有做出指令，则无人机在悬停3分钟后自动降落。

当无人机一侧出现障碍物且小于安全阈值，另一侧不变且大于安全阈值时，则无人机一侧出现障碍物，或无人机本身飞行状态受到地磁或由于桨叶形成的风，因为此时特定隧道外形结构形成的涡流造成的干扰导致无人机侧漂，此时不修改航点，对无人机发出水平横滚指令，无人机做出平移动作，拉大间隔距离，直至大于等于安全距离，若过程中另一侧亦出现小于安全距离情况，则处理方式同两侧均出现障碍物且障碍物间距小于无人机安全阈值时的处理方式相同。

步骤4：无人机飞行过程中，通过控制云台角度，以实现对隧道内设备的图像采集。

云台相机自自动起飞开始即打开相机录制，操作人员可通过地面站实时观察巡检线路中的设备信息，并手动完成云台相机俯仰偏航轴控制以对准监控目标，当发现异常设备时，可向无人机发送暂停任务指令，此时无人机将维持当前状态悬停飞行，操作人员可控制相机完成拍照取证操作。

本实施例中，全局坐标系是固定的，其作用是给无人机一个标准的空间直角坐标标准，为无人机航点飞行提供标准；

动坐标系是在运动过程中，为了使无人机在隧道这种具有约束的未知空间运动时能描述此时无人机在当前位置附近的空间状态，以实现对航点修改的目的而建立的坐标轴随实际空间情况改变的坐标，它能够反映无人机当前位置周边的情况。

比如：全局坐标系设定两个坐标(忽略高度Z轴)，(1m.0m)(2m.0m)表示无人机将在全局固定坐标系下飞行沿X轴直线飞行2m，但实际隧道情况是在前方1m的地方有一个直角弯道，如果按照固定坐标下的坐标，无人机将撞墙，此时就需要参考动坐标系，调整无人机姿态，旋转机身90°保证动坐标Y轴为0，根据旋转角度以及动坐标系下下一航点坐标(2m，0m)进行坐标转换从得到而实际转弯后第二个航点在固定坐标系下坐标是

无人机继续从固定坐标系下(1m，0m)飞往固定坐标系下

实施例二

根据本发明实施例，提供了一种隧道无人机巡检系统的实施例，包括：

数据采集模块，用于采集无人机的姿态、方向、速度、加速度信息，无人机的高度信息，以及无人机的相对坐标信息；

数据融合模块，用于对上述数据进行融合，实时解算当前无人机在空间中的姿态角、速度以及空间坐标数据；

避障模块，用于对无人机航迹点路线周围环境进行实时感知，计算无人机与周围障碍物距离，实时修改航迹点，以实现无人机避障飞行；

图像采集模块，用于无人机飞行过程中，通过控制云台角度，以实现对隧道内设备的图像采集。

需要说明的是，上述各模块的具体工作方式采用与实施例一中相同的方式实现，为了简洁，不再赘述。

实施例三

根据本发明实施例，还提供了一种隧道无人机巡检系统的实施例，参照图2，具体包括：

四轴无人机机体1、工业控制计算机2、飞行控制器3、双目鱼眼摄像头4、二维激光雷达5、UWB自解算标签6、UWB基站、单点激光测距仪8、红外可见光双光测温相机7；

本实施例中，四轴无人机机体1采用460mm轴距无人机，但这并不构成对本发明的限制，本领域技术人员可以根据需要选择合适的无人机。

工业控制计算机2、飞行控制器3、二维激光雷达5、IMU惯性测量单元、UWB自解算标签6固定设置于所述四轴无人机机体1上方，单点激光测距仪8固定设置于所述四轴无人机机体1下方，红外可见光双光测温云台相机、双目鱼眼摄像头4固定设置于四轴无人机机体1正前方，UWB基站设置于巡检隧道场景中。

利用双目鱼眼摄像头4采集无人机的姿态及方向速度及加速度信息，利用IMU惯性测量单元采集无人机的姿态角信息，利用单点激光传感器采集无人机的高度。

工业控制器用于处理双目鱼眼摄像头4采集的数据图像，得到无人机的空间位置及三轴加速度信息并将其传送给飞行控制器3；

飞行控制器3用于计算各传感器采集的无人机位姿及运动信息并以此计算控制量以控制无人机飞行；

红外可见光双光测温云台相机在无人机巡检飞行时作为监控目标的视频/图像的采集设备；二维激光雷达5进行以无人机的周视扫描，以实时获取无人机距离周边的障碍物的距离，UWB自解算标签6可以从安装于隧道中的UWB基站实时计算无人机此时的空间全局坐标，用以在地面站地图显示无人机当前位置。

基于计算的无人机空间位置及三轴加速度信息；结合预设的巡检长度和高度信息，生成巡检线路并生成若干航迹点空间坐标；基于获取的无人机距离周边障碍物的距离，实时修正航迹点坐标。具体的实现方式参照实施例一中公开的方式，不再赘述。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.一种隧道无人机巡检方法，其特征在于，包括：

采集无人机的姿态、方向、速度、加速度信息，无人机的高度信息，以及无人机的相对坐标信息；

对上述数据进行融合，实时解算当前无人机在空间中的姿态角、速度以及空间坐标数据；

对无人机航迹点路线周围环境进行实时感知，计算无人机与周围障碍物距离，实时修改航迹点，以实现无人机避障飞行；

无人机飞行过程中，通过控制云台角度，以实现对隧道内设备的图像采集。

2.如权利要求1所述的一种隧道无人机巡检方法，其特征在于，通过无人机上搭载的双目视觉传感器采集无人机的姿态、方向速度及加速度信息，利用飞控IMU惯性传感器采集无人机的姿态角信息，利用激光传感器采集无人机的高度，利用UWB采集无人机相对坐标信息。

3.如权利要求1所述的一种隧道无人机巡检方法，其特征在于，对上述数据进行融合，实时解算无人机在空间中的姿态角、速度以及空间坐标数据；具体过程包括：

通过卡尔曼滤波，滤除噪声信息；

采用最大似然估计方法，剔除明显失准的传感器信息；

通过不同的传感器件分别获取无人机的姿态、方向、速度及加速度信息；对获得的信息进行差分处理，基于无人机的动力学模型，进行权重分配，推算出最精确的数据序列。

4.如权利要求1所述的一种隧道无人机巡检方法，其特征在于，根据解算的无人机空间坐标数据，结合预设的巡检长度和高度信息，生成巡检线路并生成若干航迹点空间坐标；无人机沿着巡检线路依次飞向下一航迹点直至达到最后航迹点并自动降落。

5.如权利要求4所述的一种隧道无人机巡检方法，其特征在于，所述航迹点是在设定完巡检长度和高度信息后，在以起飞点为原点，起飞点沿隧道方向为X轴，起飞点竖直方向为Z轴，起飞点垂直于隧道通道方向为Y轴的固定空间坐标系下，自动生成的初始航迹点。

6.如权利要求1所述的一种隧道无人机巡检方法，其特征在于，对无人机航迹点路线周围环境进行实时感知，计算无人机与周围障碍物距离，以实现无人机避障飞行；具体过程包括：

通过激光雷达对航迹点路线周围环境进行实时感知，并返回航迹路线周围障碍物距离信息；

若无人机周围障碍物距离大于设定安全阈值时，按照预设轨迹飞行或者修改航迹点使得无人机处于中间位置；

若无人机前方出现障碍物，无人机与一侧障碍物间距减少或不变，与另一侧障碍物间距增加时，判断出现隧道转向；以间距减少或不变的一侧障碍物为基准，间隔设定距离重建下一航迹点，以完成原地转向或弧度转向；

若无人机周围障碍物距离小于安全阈值时，则判断过道变窄或当前飞行高度周边出现障碍，无人机下降设定高度，若下降高度后满足要求，则修改下一航迹点坐标高度值为当前安全高度并飞行通过；若仍不满足，则无人机发出告警信息等待进一步的控制指令。

7.如权利要求1所述的一种隧道无人机巡检方法，其特征在于，巡检过程中发现异常设备时，向无人机发送暂停任务指令，此时无人机将维持当前状态悬停飞行，以便于实现拍照。

8.一种隧道无人机巡检系统，其特征在于，包括：

数据采集模块，用于采集无人机的姿态、方向、速度、加速度信息，无人机的高度信息，以及无人机的相对坐标信息；

数据融合模块，用于对上述数据进行融合，实时解算当前无人机在空间中的姿态角、速度以及空间坐标数据；

避障模块，用于对无人机航迹点路线周围环境进行实时感知，计算无人机与周围障碍物距离，实时修改航迹点，以实现无人机避障飞行；

图像采集模块，用于无人机飞行过程中，通过控制云台角度，以实现对隧道内设备的图像采集。

9.一种隧道无人机，其特征在于，包括：无人机本体，所述机器人本体上分别搭载工业控制器、飞行控制器、激光雷达、UWB自解算标签以及传感器单元；其中，

传感器单元，用于采集无人机姿态、方向、速度及加速度信息，无人机的姿态角信息，以及无人机的高度信息；

工业控制器，用于基于采集到的数据计算无人机的空间位置及三轴加速度信息；

飞行控制器，用于根据工业控制器计算的数据控制无人机飞行；

激光雷达，用于通过扫描无人机周边环境，实时获取无人机距离周边障碍物的距离；

UWB自解算标签，用于与隧道中的UWB基站通信，实时计算无人机的空间全局坐标，以得到无人机当前位置。

10.如权利要求9所述的一种隧道无人机，其特征在于，

基于计算的无人机空间位置及三轴加速度信息；结合预设的巡检长度和高度信息，生成巡检线路并生成若干航迹点空间坐标；

基于获取的无人机距离周边障碍物的距离，实时修正航迹点坐标。

Images