CN105629970A - 一种基于超声波的机器人定位避障方法 - Google Patents

Abstract

本发明公开了一种融合双目视觉感知环境信息，精准矫正位姿态辅助超声波实时感知机器人环境信息，建立地图及根据地图避障行走的方法。本发明实现了基于超声波传感器的移动机器人定位，较快地实现了超声波与单目视觉下的机器人同时定位与地图创建，实现家庭环境下的自主导航和避障路径规划，实现了多感知的综合分析，融合了视觉、超声波传感器、红外传感器等信息，较好地实现了移动机器人的多模态避障路径规划和自主导航运动。

Description

一种基于超声波的机器人定位避障方法

技术领域

本发明涉及机器人自主定位及环境感知技术领域，尤其是一种基于超声波的机器人自主构建环境地图及避障方法。

背景技术

随着科技发展和社会进步，人们呆在家里的时间越来越少，因此产生了对家政服务的需求，但由于目前家政服务本身的问题，大多数人不愿意也不放心将财物及家人交与他人看管或照顾，交给可以信赖的机器人是一个不错的选择，有需求就有市场；在国外一些机器人己经实验性地进入了医院、家庭，从事部分辅助服务工作，相信在中国随着市场潜在需求的成熟，也将有更多的教育服务机器人走入寻常百姓家。例如家庭护理机器人、教育机器人、安防机器人、清洁机器人等，因此家庭服务机器人将大有可为。因为需求的不断发展，人们要求机器人在运动过程中能够根据周围环境的变化而自主的采取相应措施的能力也越来越强。新一代的家庭服务智能机器人就要求将机械、电子、传感器、计算机软件硬件、模式识别、人工智能、工业设计等众多先进技术融为一体，是典型机电一体化***。作为机器人智能的一个重要因素—避障与路径规划就显得尤为重要，它是机器人研究领域的关键技术之一，是机器人完成其它高级任务的前提。

移动机器人的避障与路径规划即移动机器人通过感知***检测外界环境信息，对这些信息进行处理后得到信息地图，再制定出行走路线，并在行进过程中躲避障碍物。而是否具备同时定位与地图创建的能力被许多人认为是机器人是否能够实现自主导航的关键的前提条件。在未知环境中，家庭服务机器人的自主导航实现将更加复杂。地图创建、定位和路径规划是移动机器人自主导航的三个关键要素。在环境未知的情况下，地图创建与定位的关系就像是先有蛋还是先有鸡的问题。

同时定位与地图创建是移动机器人导航领域的基本问题与研究热点，初始状态下，环境地图是不存在的，因此无法在地图中标定机器人的位置。另一方面，不掌握初始位姿，地图的创建也将没有基准点。正是由于这样的原因，未知环境下移动机器人的定位与地图创建将以同时定位与地图创建的方式来实现。即移动机器人随着对环境的探索，逐步扩大自身存储的地图的广度，并实时的将位置信息标定在新创建的地图中。这种技术一般称之为同时定位与地图生成（SLAM，Simultaneouslocalizationandmapping）。

在SLAM***中，包含两种信息：即移动机器人的当前位姿与已经探索区域的环境地图。一般而言，SLAM***将移动机器人位姿估计的内在不确定性转换成概率问题来解决。即通过求取机器人位姿与环境地图的联合后验概率分布的方法进行实现。假设***的状态服从高斯分布，那么SLAM可以采用卡尔曼滤波器来实现。其协方差矩阵包含了不确定信息，这些信息代表了移动机器人的位姿信息与地图信息。

现有的与基于视觉的移动机器人同时定位与地图创建的相关专利主要集中在各个专用领域的子***实现，与本发明的研究内容相近的专利有我国在2012年授权的《一种实现移动机器人同时定位与地图构建的方法》[1]，该方法针对移动机器人在未知环境下利用航位推测传感器数据以及路标观测数据，借助于改进的强跟踪滤波技术实现移动机器人的自主定位,并同时构建出环境地图。该发明利用“强跟踪滤波器”,自适应调整卡尔曼增益；引入一种新的多重渐消因子,保证协方差矩阵的对称性,降低求解卡尔曼增益的计算复杂度；针对同时地位与地图构建问题中的观测不连续问题,提出了一种新的算法流程,并给出了一种新的多重渐消因子的计算方法。2012年授权的发明专利《一种智能机器人的定位与地图建模方法》[2]公开了一种智能移动机器人的定位与环境建模方法,首先形成修正迭代扩展卡尔曼滤波算法并确定迭代次数,然后建立移动机器人的运动模型和观测模型,初始化移动机器人的状态,计算位置雅可比矩阵和控制输入雅可比矩阵计算、观测雅可比矩阵等；最后通过求解卡尔曼增益矩阵，更新状态估计方程和协方差矩阵，并重复以上部分步骤。该发明以在移动机器人的同时定位与环境建模领域中使用最为广泛的扩展卡尔曼滤波算法为核心，通过对该算法进行改进，使得算法的性能得到极大提高，能够更好的满足在SLAM中的应用。2012年授权的发明专利《单目视觉SLAM中基于图像外观的闭环检测方法》，该发明包括:移动机器人前进过程中利用自身携带的单目摄像头采集当前场景图像，并提取当前场景图像的视觉词典特征；对图像进行预处理；利用图像权重向量之间的内积来衡量图像之间的相似度，将与前一帧历史图像相似度较大的当前图像剔除；利用贝叶斯滤波方法不断更新闭环假设状态的后验概率进行闭环检测，判断当前场景图像是否发生闭环；利用图像逆检索方法对上步中得到的闭环检测结果进行验证。进一步地,该发明在构建视觉词典过程中,利用TSC值作为聚类结果的评价标准来对聚类类别数进行动态调整。2010年授权的发明专利《实现室内服务机器人同时定位与地图创建的装置及机器人》[4]公开了一种实现室内服务机器人同时定位和地图创建的装置及带有该装置的室内服务机器人，该装置包括外部传感器、内部传感器和信息处理模块，通过所述机器人在外部环境中移动，记录所述外部传感器和所述内部传感器的测量数据，对环境进行特征提取，利用递推形式的预测和更新算法得出所述机器人的位姿和特征地图，在满足特征匹配的条件下,实现对相应的所述位姿和特征地图的更新。2013年授权的发明专利《一种家用服务机器人的定位***以及定位方法》[5]涉及一种家用服务机器人的定位***及定位方法，在一个区域内安装至少三个红外发射器，家用服务机器人通过自身的中央控制器，红外感应坐标定位模块、双目视觉定位模块、声纳辅助定位模块和红外探测器进行行进作业且在进行作业中能自动避障。

[1]一种实现移动机器人同时定位与地图构建的方法，公开号：CN102402225A，发明人：温丰，原魁，柴晓杰

[2]一种智能机器人的定位与地图建模方法，公开号：CN102706342A，发明人：张毅，罗元，谢颖，徐晓东，唐贤伦，李敏，蔡军，胡章芳

[3]单目视觉SLAM中基于图像外观的闭环检测方法，公开号：CN102831446A，发明人：梁志伟，陈燕燕，朱松豪，徐国政

[4]实现室内服务机器人同时定位与地图创建的装置及机器人，公开号：CN101920498A，发明人：钱东奇等。

[5]一种家用服务机器人的定位***以及定位方法，公开号：CN103170980A，发明人：曲东升，刘彦武，朱梓清，李长峰

[6]基于多特征融合的人脸定位与头部姿势识别方法，公开号：CN1601549A，发明人：谭铁牛，魏育成，周超

目前，较为常用的移动机器人的SLAM技术实现包括FastSLAM与vSLAM两大类。这两种实现均使用优化方法降低计算复杂度，使移动机器人SLAM成为可能。其中，FastSLAM***一般使用激光测距仪或声纳来实现，而vSLAM则使用视觉传感器来实现。FastSLAM由于使用了激光、声纳等传感器，对一些特殊的环境信息，如线段、拐角等并不能识别其特殊意义。这就在创建地图的工作中存在信息量偏小的缺点。而vSLAM则很大程度上解决了这个问题，原因是vSLAM使用视觉传感器，获取的图像包含了相对丰富的信息，使得一些特殊意义的信息得以识别。但vSLAM也存在一些问题，主要是视觉信息过于复杂，计算复杂度相对较高，在视觉传感器获取大分辨率图像的情况下很难实现实时性；并且针对移动机器人某一位姿状态下的地图信息数据量往往很大，当探索环境很大时，地图信息的存储也将面临挑战。

发明内容

本发明要解决的技术问题：针对上述SLAM技术存在的问题，提供了一种融合双目视觉感知环境信息，精准矫正位姿态辅助超声波实时感知机器人环境信息，建立地图及根据地图避障行走的方法。

本发明技术方案：

一种基于超声波的机器人定位避障方法，包括以下步骤：

步骤1，设定工作环境的大小，明确机器人工作的区域，机器人的初始起点及机器人的方向，把整个工作环境分成X*Y个栅格，建立一个数组用于存储X*Y个矩形栅格内有没有障碍物，把数组元素导入txt；

步骤2，移动机器人感知环境，具体包括:a已知规定的初始点、机器人正前方向，可得到各个超声波方向；b根据超声波的回值，通过极坐标的转换可知障碍物与机器人的方向和距离；c通过双目摄像头得到的值来与同向超声波回值比较，得到机器人与障碍物相对准确的距离；d已知初设或当前机器人在地图中的位置，计算得到障碍物在地图的位置；e行进过程中，由编码器回馈值，得到机器人的当前位置；f重复a、b、c步骤，可得当前位置时超声波探测的障碍物在图上的位置；g重复e；h最后将已知的机器人工作环境转换为数据存入txt中；

步骤3，调出地图，并通过超声波和摄像头感知机器人所处位置，在地图上先行判断，是否能够到达机器人当前的子目标点，把能到达的子目标点用特殊的颜色标记，并从能够到达的子目标点继续判断，到达的子目标点的下一个子目标点能否到达，能则标记，直到没有可以到达的子目标点或到达目标点的一定区域，完成避障结束标记，启动机器人有目标避障工作状态；

步骤4，机器人按照规划线路上的各个目标航迹点进行有目标避障：

a1机器人处于初始起点时，直接导出txt中的数据，显示当前的工作环境地图；

b1根据双目摄像头结构得到的数据，时刻刷新机器人的当前位置，确保机器人本身位置的准确性不受误差积累影响；

c1判断目标点与机器人本身的距离，到达一定范围，则控制模块对底盘控制模块发送停止行进指令，到达目标点范围，否则进行下面的步骤

d1若前方无障碍物时，通过计算，知道目标点与机器人本身的角度差异，如差异不大，则控制模块对底盘控制模块发送相应的指令，前进并返回；如差异大，则控制模块对底盘控制模块发送相应的指令，是左转还是右转并返回；

e1若前方有障碍物左右无障碍物时，则判断目标点在机器人的前进方向上,控制模块对底盘控制模块发送相应的指令，左边向左转，右边向右转并返回；

f1若前方有障碍物左右两方一方无障碍物一方有障碍时，控制模块对底盘控制模块发送相应的指令，左边无障碍物向左转，右边无障碍物向右转并返回；

g1若前方有障碍物左右都有障碍物时，通过后面的超声波回馈值，判断后方是否有障碍物，有：控制模块对底盘控制模块发送相应的指令，停止并返回；无：控制模块对底盘控制模块发送相应的指令，则控制模块对底盘控制模块发送相应的指令，是左转还是右转并返回；

h1重复进行b1判断；

在步骤5中，若超声波检测到地图中没有的障碍物，则进入自由避障状态，跟据超声波的回值，控制模块对底盘控制器发送命令，使机器人进行相应的躲避响应，并对重新对当前目标点的线路进行规划。

在步骤5中，每一次在b1中选择目标点时，若目标点在局部规划窗口内，直接以目标点作为当前局部窗口的子目标；若目标点不在局部规划窗口内，依次选择原路径规划中的航迹点作为局部规划窗口中的子目标点，若该航迹点出现在局部窗口内时，则选择下一个航迹点作为子目标点，当有多个航迹点出现在局部窗口内时，选择未出现在局部窗口内的最近航迹点作为子目标点。

本发明有益效果：

1．本发明的运动***主要由两轮驱动单元和一个万向轮组成，这种经典结构可以使得机器人能够实现零半径转向，前进后退左右转动等各种运动功能。机器人行进时利用编码器可将电机控制的两个驱动轮运转圈数传给上位机，上位机通过计算，得出机器人的当前位置；已知当前位置，根据超声波角度以及探测的障碍物回给上位机的值，上位机计算出当前位置时9个超声波探测到的障碍物的位置。因为超声波探测到的距离回馈值、编码器的回馈值、计算所不可避免的误差，机器人行进的时间越长，误差就越大，对周边环境的感知失真就越大，所以需要用两个单目摄像头构成双目视觉***，感知周边环境中的已知物体，确定机器人当前较为准确的位置来矫正将要失去的准确性，同时计算出来的机器人位置；从而达成成服务机器人的自主定位，使机器人能够在家庭环境中自主导航并完成更多的辅助人类生活的功能，本发明算法相对简单，适用于家庭服务机器人对周边环境感知要求。

2．单目视觉传感器具有价格低廉、结构简单、易于标定等优点，已经成为服务机器人必配的传感器，使用单目视觉解决服务机器人的SLAM问题也成为服务机器人的研究者的选择；由于超声波测距模块比较便宜，且能较为精确测量到障碍物的距离，配合电机可以实现转向测距，灵活避障。在机器人自主导航中应用甚广;而超声波模块和视觉传感器的结合,其优势显而易见。

3.本发明实现了基于超声波传感器的移动机器人定位，较快地实现了超声波与单目视觉下的机器人同时定位与地图创建，实现家庭环境下的自主导航和避障路径规划，实现了多感知的综合分析，融合了视觉、超声波传感器、红外传感器等信息，较好地实现了移动机器人的多模态避障路径规划和自主导航运动。

附图说明：

图1为本发明避障算法流程图。

图2为局部窗口中路径选择示意图。

具体实施方式：

一种用于家庭服务机器人，具有环境感知，同时定位与地图创建、避障路径规划的自主移动机器人，包括：

（1）用于机器人移动的运动装置，具有自主移动功能，能够在多种模式下工作，包括跟随模式，巡逻模式和异常行为处理模式；运动装置由2个直流伺服电机、1个万向轮构成、1个电机驱动和控制电路构成；其中，2个直流电机通过所述的直流电机驱动电路与所述装置连接，万向轮独立安装在家庭服务机器人底座的正前方。

（2）感知装置，在普通家庭的环境内感知一项或多项如下信息：感知周围环境的视觉信息、检测到行走路线周围的障碍；环境感知装置包括两个单目摄像头及9个超声波传感器，用于环境感知和同时定位与地图创建的多感知信息融合***，上述超声波传感器、视觉传感器与作为上位机的PC主机连接。单目摄像头安装于机器人头部，并可由舵机控制转动（俯仰与摇摆控制），舵机与头部控制器连接。

（3）基于超声波的SLAM技术，通过不时使用单目摄像头提取数据对机器人位置进行校正，再生成相应的超声波探测出的障碍物，保证获取的环境图像具有旋转不变性。

（4）基于家庭服务机器人的工作环境，在基本不降低可靠性的前提下可以显著的降低计算复杂度，达到了提高校正机器人位置的目的。

（5）在条件变化并不剧烈的情况下，单目摄像头提取数据对机器人位置进行校正可以保持较高的重复率；提高校正机器人位置频率，使超声波探测出的障碍物保持较高的相似度；

（6）控制装置，包括头部控制装置、底盘控制装置和主控装置；对所述感知装置接收到的多种感知信号进行综合分析处理，发出指令控制所述的执行装置完成相应的工作；底盘控制器、头部控制器分别与作为主控制器的上位机连接，上位机中装载有基于超声波的同时定位与地图创建、避障路径规划等核心算法，主控制装置通过串口向底盘控制器发送控制命令，控制电机进行相应动作，并对障碍信号进行处理。

（7）执行装置，接收智能感知装置的信号和指令输入，完成机器人的避障路径规划、自主导航等任务。

基于超声波的家庭服务机器人SLAM算法基本步骤如图1所示：

（1）设定工作环境的大小；明确机器人工作的区域，机器人的初始起点（x，y）及机器人的方向。使得机器人创建地图方便快捷，具体建图的方法如下：

a)首先把整个工作环境分成X*Y个小的栅格，X，Y的大小根据工作环境来定；

b)建立一个数组，存储X*Y个小矩形内有没有障碍物；

c)在工作前的环境输入时(即机器人第一次探索环境时),每发现障碍物时,把障碍物所在的矩形------数组元素置为1,否则为0;

d)其中0代表没有障碍物,1代表有障碍物,并把数组元素导入txt,用时导出到数组。数组就代表了障碍物;同样也构成工作环境的数据化地图；

（2）人工操作机器人，在工作区感知环境；具体方法为：

a)已知规定的初始点、机器人正前方向，可得到各个超声波方向；

b)根据超声波的回值，通过极坐标的转换可知障碍物与机器人的方向和距离；

c)通过双目摄像头得到的值来与同向超声波回值比较，得到机器人与障碍物相对准确的距离

d)已知初设或当前机器人在地图中的位置，计算得到障碍物在地图的位置；

e)行进过程中，由编码器回馈值，得到机器人的当前位置；

f)重复（a、b、c）步骤，可得当前位置时超声波探测的障碍物在图上的位置；

g)重复e；

h)最后将已知的机器人工作环境转换为数据存入txt中；

i)可以人工比对现实环境，对txt中数据进行修订；

获得环境地图以后，根据在地图上机器人当前位置和目标点位置，规划出全局的行进路线，进行避障移动，包括有目标避障和自由避障：

（3）自由避障：移动机器人能根据采集的障碍物的状态信息安全有效的避开行进中的障碍物朝目标方向前进。只采用（2）中a、b两步即可，跟据超声波的回值，控制模块对底盘控制器发送命令，使机器人进行相应的躲避（前进，左转，右转，后退，停止）响应。

（4）有目标避障方法如下：

a)机器人处于初始起点时，直接导出txt中的数据，显示当前的工作环境地图；

b)根据双目摄像头结构得到的数据，时刻刷新机器人的当前位置，确保机器人本身位置的准确性不受误差积累影响；

c)判断目标点与机器人本身的距离，到达一定范围，则控制模块对底盘控制模块发送停止行进指令，到达目标点范围，否则进行下面的步骤

d)若前方无障碍物时，通过计算，知道目标点与机器人本身的角度差异，如差异不大，则控制模块对底盘控制模块发送相应的指令，前进并返回；如差异大，则控制模块对底盘控制模块发送相应的指令，是左转还是右转并返回；

e)若前方有障碍物左右无障碍物时，则判断目标点在机器人的前进方向上。控制模块对底盘控制模块发送相应的指令，左边向左转，右边向右转并返回；

f)若前方有障碍物左右两方一方无障碍物一方有障碍时，控制模块对底盘控制模块发送相应的指令，左边无障碍物向左转，右边无障碍物向右转并返回；

g)若前方有障碍物左右都有障碍物时，通过后面的超声波回馈值，判断后方是否有障碍物，有：控制模块对底盘控制模块发送相应的指令，停止并返回；无：控制模块对底盘控制模块发送相应的指令，则控制模块对底盘控制模块发送相应的指令，是左转还是右转并返回；

h)重复进行b判断；

（5）在移动机器人避障路径规划过程中，在每一个局部窗口内都是一个局部路径规划问题，与全局避障路径规划不同的是，规划前需要确定局部窗口的目标点（作为临时目标点），而局部目标点的选取会直接影响最终路径规划的性能。因此利用全局粗粒度路径规划的航迹点作为局部路径规划的子目标点，可以为局部避障路径规划提供全局优化要求信息，提高***的优化性能。使用全局粗粒度航迹点作为局部目标点包括以下三种情况：1）目标点G在局部规划窗口内，直接以目标点作为当前局部窗口的子目标（临时目标）如图2（a）所示；如G不在局部规划窗口内，依次选择粗粒度路径规划中的航迹点作为局部规划窗口中的子目标点，如图2(b)所示，若该航迹点出现在局部窗口内时，则选择下一个航迹点作为子目标点。当有多个航迹点出现在局部窗口内时，如图2(c)所示，选择未出现在局部窗口内的最近航迹点作为子目标点。

（6）无目标避障规划时，即根据自由避障的方法，首先采用（5）中的方法选择子目标点，在地图上先行判断，机器人是否能够到达当前的子目标点，能的话，把能到达的子目标点用特殊的颜色标记，并从到达的子目标点继续判断，能够到达的子目标点的下一个子目标点能否到达，能则标记，不能不标记；

（7）有目标避障规划时，就是依据已知的地图，和有目标避障的方法，采用（5）的方法选择子目标点，在地图上先行判断，是否能够到达机器人当前的子目标点，能的话，把能到达的子目标点用特殊的颜色标记，并从能够到达的子目标点继续判断，到达的子目标点的下一个子目标点能否到达，能则标记，不能不标记，直到没有可以到达的子目标点或到达目标点的一定区域——符合有目标避障算法，成功完成避障；结束标记，启动机器人有目标避障工作状态；

（8）无论有无目标点避障路径规划，如果有动态的物体或人存在时，机器人所走的路线均不一定是规划的路线；

本方法是通过VS2010中的MFC编写的控件窗口来完成服务机器人的避障路径规划功能，经过实验检验，尽管存在相应的误差，但均在家庭服务机器人的预计适用要求范围内。

Claims (2)

1.一种基于超声波的机器人定位避障方法，其特征在于，包括以下步骤：

步骤1，设定工作环境的大小，明确机器人工作的区域，机器人的初始起点及机器人的方向，把整个工作环境分成X*Y个栅格，建立一个数组用于存储X*Y个矩形栅格内有没有障碍物，把数组元素导入txt；

步骤2，移动机器人感知环境，具体包括:a已知规定的初始点、机器人正前方向，可得到各个超声波方向；b根据超声波的回值，通过极坐标的转换可知障碍物与机器人的方向和距离；c通过双目摄像头得到的值来与同向超声波回值比较，得到机器人与障碍物相对准确的距离；d已知初设或当前机器人在地图中的位置，计算得到障碍物在地图的位置；e行进过程中，由编码器回馈值，得到机器人的当前位置；f重复a、b、c步骤，可得当前位置时超声波探测的障碍物在图上的位置；g重复e；h最后将已知的机器人工作环境转换为数据存入txt中；

步骤3，调出地图，并通过超声波和摄像头感知机器人所处位置，在地图上先行判断，是否能够到达机器人当前的子目标点，把能到达的子目标点用特殊的颜色标记，并从能够到达的子目标点继续判断，到达的子目标点的下一个子目标点能否到达，能则标记，直到没有可以到达的子目标点或到达目标点的一定区域，完成避障结束标记，启动机器人有目标避障工作状态；

步骤4，机器人按照规划线路上的各个目标航迹点进行有目标避障：

a1机器人处于初始起点时，直接导出txt中的数据，显示当前的工作环境地图；

b1根据双目摄像头结构得到的数据，时刻刷新机器人的当前位置，确保机器人本身位置的准确性不受误差积累影响；

c1判断目标点与机器人本身的距离，到达一定范围，则控制模块对底盘控制模块发送停止行进指令，到达目标点范围，否则进行下面的步骤

d1若前方无障碍物时，通过计算，知道目标点与机器人本身的角度差异，如差异不大，则控制模块对底盘控制模块发送相应的指令，前进并返回；如差异大，则控制模块对底盘控制模块发送相应的指令，是左转还是右转并返回；

e1若前方有障碍物左右无障碍物时，则判断目标点在机器人的前进方向上,控制模块对底盘控制模块发送相应的指令，左边向左转，右边向右转并返回；

f1若前方有障碍物左右两方一方无障碍物一方有障碍时，控制模块对底盘控制模块发送相应的指令，左边无障碍物向左转，右边无障碍物向右转并返回；

g1若前方有障碍物左右都有障碍物时，通过后面的超声波回馈值，判断后方是否有障碍物，有：控制模块对底盘控制模块发送相应的指令，停止并返回；无：控制模块对底盘控制模块发送相应的指令，则控制模块对底盘控制模块发送相应的指令，是左转还是右转并返回；

h1重复进行b1判断；

根据权利要求1所述的基于超声波的机器人定位避障方法，其特征在于：在步骤5中，若超声波检测到地图中没有的障碍物，则进入自由避障状态，跟据超声波的回值，控制模块对底盘控制器发送命令，使机器人进行相应的躲避响应，并对重新对当前目标点的线路进行规划。

2.根据权利要求1所述的基于超声波的机器人定位避障方法，其特征在于：在步骤5中，每一次在b1中选择目标点时，若目标点在局部规划窗口内，直接以目标点作为当前局部窗口的子目标；若目标点不在局部规划窗口内，依次选择原路径规划中的航迹点作为局部规划窗口中的子目标点，若该航迹点出现在局部窗口内时，则选择下一个航迹点作为子目标点，当有多个航迹点出现在局部窗口内时，选择未出现在局部窗口内的最近航迹点作为子目标点。