一种基于时间节点的智能避障方法与流程

本发明属于uwb技术领域，具体涉及一种基于时间节点的智能避障方法。

背景技术：

uwb是一种无载波通信技术，利用纳秒至微秒级的非正弦波窄脉冲传输数据。通过在较宽的频谱上传送极低功率的信号，uwb能在10米左右的范围内实现数百mbit/s至数gbit/s的数据传输速率。其抗干扰性能强，传输速率高，系统容量大发送功率非常小。uwb系统发射功率非常小，通信设备可以用小于1mw的发射功率就能实现通信。低发射功率大大延长系统电源工作时间。而且，发射功率小，其电磁波辐射对人体的影响也会很小，应用面就广。将uwb技术应用于智能避障是一个有新意的创意，通过宏观的预判，调用所有资源，进行智能避障。

技术实现要素：

本发明的目的是提供了一种基于时间节点的智能避障方法，根据目标的行进路径进行风险预判，若存在风险则重新规划路径，可以避免目标在同一路口的拥堵情况，通过宏观的预判合理利用所有可通过路径，提高效率，减少等待现象与等待造成的瘫痪等问题。本发明的目的通过以下技术方案实现：

一种基于时间节点的智能避障方法，主要包括如下步骤，

步骤一、精准定位：对目标的实时位置进行精准定位，获得t时刻的目标坐标(x,y,t)；

步骤二、规划路径：

a)将平面划分成方形网格，划分坐标点，简化搜索区域；

b)1号目标要从a到b，画出所有从a到b的可通过路径，比较判断出一个最快路径，并将这个最快路径存到工作库中；

c)2号目标进场，重复1号目标的选择路径过程，并将该路径与工作库中的路径进行比较，如存在重合点，则判断时间差，如时间差过小则存在碰撞风险，则重新规划路线。

进一步地，上述步骤二的具体步骤为：

1)假设在t0时刻，1号目标从要起点a到终点b，在当前时刻，1号目标将做如下搜索：从点a开始，作为一个待处理点存入一个“开启列表”，在初始时刻，表中只有一个元素，但之后元素增多，形成一个待检查的方格列表；标记当前目标的行进方向；寻找起点上下左右四个方向中可通过的方格并加入开启列表，跳过无法通过的方格；从开启列表中删除点a，加入到一个“关闭列表”，列表中保存所有不需要再次检查的方格；接着，选择开启列表中的临近方格，重复前面的过程；

2)在t1时间，2号目标进入近地区域，将2号目标的最优三维数组与工作库中的数据进行比较，若存在相近点，那么该点被标记为2号目标不可行进点，重新规划路线直到新的路径生成；

3)重复上述1)和2)过程，对任意时刻的目标进行路径规划。

进一步地，上述步骤一具体包括如下步骤：

1)使用混合最小二乘法，直接处理由uwb标签发出的脉冲信号到基站接受脉冲信号的时间，得到定位初值；

2)用拉格朗日插值多项式处理近距离的点，提高坐标的准确度；

3)采用离群值检测算法将因为外界因素干扰所造成的坏数据进行剔除；

4)用滑动平均滤波得到各个离散标签的实时坐标，达到定位精度的优化效果。

进一步地，上述的智能避障方法，还包括路径检测，固定时间接收一次近地区域所有目标的二维坐标，将这些坐标加上时间信息，与工作库中各个目标的路线数据进行对比，当目标偏离路线时发出警报信息。

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

本发明的智能避障方法对目标进行准确定位，根据目标的行进路径进行风险预判，若存在风险则重新规划路径，可以避免目标在同一路口的拥堵情况，通过宏观的预判合理利用所有可通过路径，提高效率，减少等待现象与等待造成的瘫痪等问题。

附图说明

图1是本实施例定位基本原理框图。

图2是本实施例的精度随距离变化折线图。

图3是本实施例的插值函数拟合曲线。

图4是本实施例的离群值剔除算法流程图。

图5是本实施例的滑动平均滤波算法流程图。

图6是本实施例的无人机智能避障简化图。

图7是本实施例的系统框架示意图。

图8是本实施例的路径规划结果。

具体实施方式

下面结合具体实施例对本发明做进一步详细的描述，但本发明的实施方式不限于此。

本实施例以无人机为例，对该智能避障方法进行阐述。

dw1000一秒计数2～40次，因此其最小的时间单位为即16.65ps。dw1000将实时捕捉发送与接收时间戳，精准的时间戳是标签与基站之间点对点精准测距实现的前提，标签与基站间的距离是通过标签发送信号到达基站的飞行时间tof(timeofflignt)乘以光速来完成。即：

d＝tof×c

由于自身的超宽带特性，uwb的时间分辨率非常高，乘以光速，其测距精度基本可有效保持在很小的范围内，为实现三维定位，需要求出被定位设备的xyz三维坐标，在基站架设的时候，需要特别拉开z轴的高度差，以确保在z轴上的精确度。使用tof的方式，三个基站就可以完成三维定位。地面架设可利用架设安装杆的方式，为获得精准的三维定位数据，可使用加装基站无人机进行定点悬停的方式进行对z轴高度的测量。

精准的时间是获得目标精准坐标的关键因素，现有的dw1000能够将误差控制在30cm以内，通过数据的进一步处理可将误差进一步缩小。在这一部分，首先使用混合最小二乘法，直接处理由uwb标签发出的脉冲信号到基站接受脉冲信号的时间，得到定位初值；然后用拉格朗日插值多项式处理近距离的点，提高坐标的准确度；再采用离群值检测算法将因为外界因素干扰所造成的坏数据进行剔除；最后用滑动平均滤波得到各个离散标签的实时坐标，达到定位精度的优化效果。本定位优化模型使定位精度进一步提高，且能够大大提高定位系统的抗干扰效果，有效提高了定位效率。具体框图如图1所示。

取四个点的数据绘制折线图如图2所示。观察图2，利用插值法求出距离和误差之间的映射关系。通过以上4个节点求3次拉格朗日插值多项式p3(x)。计算公式为

其中

在节点处有

l3(xj)＝f(xj)(j＝0，1，2，3)

而在其他点上，均是f(x)的近似值。记

r3(x)＝f(x)-ln(x)

称r3(x)为插值多项式的余项。

经过计算得出的3次拉格朗日插值多项式为：

y＝-5.96×10^-13·x³+3.175×10^-9·x³-50824×10^-6·x²+0.004034·x+0.4263

拟合曲线如图3所示。观察图3可以发现，3次拉格朗日插值多项式法可以达到很好的拟合效果。因此，对于在基站两米内的标签点，只需将接收到的距离经过上面的公式进行转换，即可得到精确度大大提升的位置坐标。下面将这些点跟远距离的标签一起进行进一步的处理。

dw1000一秒可以计数40次，即每个标签每秒有40个动态坐标。这些坐标均在准确值左右浮动。经过实际观察发现，部分坐标与真实值相差较大，分析原因可知，标签与基站之间的信息传递是电磁波，当外界有较强干扰时(比如，距离近时，有人在来回走动)，传播时间会受到影响，进而所测距离与真实值相比会产生较大的偏差。对于这些偏差较大的数据，需要进行剔除，主要思想为删除离平均值较远的数据。设定一个临界值k，当数据与平均值的差距超过临界值k时，判定为离群值并剔除。具体公式如下：

其中ri＝1表示该数据不会被剔除，ri＝0表示该数据会被剔除。为了确定k的具体值，要借助最小二乘法的思想，具体流程图参照图4。

目前可供选择的滤波算法有很多种，如限幅滤波、中位值滤波法、算术平均滤波法、滑动平均滤波法、防脉冲干扰平均滤波法等。选择了滑动平均滤波，它能够很好的适应于本文的高频振荡系统，而且平滑都高，对周期性干扰有良好的抑制作用。

滑动平均滤波的具体方法即为将连续取n个采样值看成一个队列，队列的长度固定为n。每次采样到一个新数据放入队尾，并扔掉原来队首的一次数据，(先进先出原则)，把队列中的n个数据进行算术平均运算，即获得新的滤波结果算法的流程图参照图5。

无人机在高空飞行时不需要壁障，但uwb在无人机飞行在较低的高度下时能达到更精准的定位。在这种前提下，对无人机的飞行路线进行路径规划。

基本思想为：在离目标降落点还有一定的水平距离时，无人机直接在高空进行直线飞行，当进入目标点的范围内时，垂直下降到距离地面8m，然后进行智能壁障，并最终到达目标点的8m上方(下文中称为近地区域)，然后垂直降落。简化的示意图如图6所示。

观察图6，可以看出，三维坐标被简化为二维坐标。利用无人机的位置和时间的三维坐标(x1,x2,x3)，借鉴a*智能寻路算法来规划无人机在低空中的飞行路径。

无人机的所有指示都由总控系统发出，总控系统分为两个子系统：寻路和避障系统、监测系统。二者各自独立工作，系统框图如图7。

基于时间点的路径规划与避障方法，优点：更宏观，因为所有的无人机的位置都已知，所以可以算出无人机通过某个点的时间，都通过路径调整提前做出避让。类似高德导航时的路线推荐。避免拥堵避让。

首先将平面划分成方形网格，人为划分坐标点，简化搜索区域。结合实时定位，系统简化成了一个由坐标和时间构成的三维动态数组a＝[a1(x1,x2,x3),a2(x1,x2,x3),…,am(x1,x2,x3)]，其中m是整数，随无人机的数量和路线动态变化；x1,x2表示无人机的位置坐标，x3表示时间坐标。数组的每一个元素是网格的一个方块，方块有两种状态：可通过和不可通过。路径被描述为从a到b经过的方块的集合。一旦路径被找到，无人机就从方格的中心(我们称为“节点”)走向另一个，直到到达目的地，此系统的特点在于无人机的整个行进过程无需停顿。

假设在t0时刻，1号无人机从要起点a到终点b，在当前时刻，1号将做如下搜索：

从点a开始，并且把它作为一个待处理点存入一个“开启列表”。在初始时刻，表中只有一个元素，但之后就会多起来。1号的最终路径可能会通过它包含的方格，也可能不会。基本上，这是一个待检查的方格列表。

标记当前无人机的行进方向，行进方向为上下左右其中的一个，设无人机旋转90度的耗费为c1,旋转180度的耗费为c2(c1，c2为固定值，根据实际情况设定)。

寻找起点上下左右四个方向中可通过的方格并把他们加入开启列表，跳过无法通过的方格。所有这些方格保存点作为a的父方格，用于后面路径的描述。

从开启列表中删除点a，把他加入到一个“关闭列表”，列表中保存所有不需要再次检查的方格。

接着，选择开启列表中的临近方格，大致重复前面的过程。目标方格的选择是主要关心的问题。规定路径评分值

f＝g+h+c1+c2

这里，g表示从起点a，沿着产生的路径，移动到网格上指定方格的移动耗费,这个过程无人机只走一步，所以g＝1.h表示从指定方格移动到最终的目标点b的耗费。

引入欧式距离和曼哈顿距离，其中欧式距离为：

曼哈顿距离为：

可以看出，h是一个预判的过程，它能尽量减小无人机的路径耗费。

经过以上步骤生成1号无人机最优行进路径后，得到一个三维数组，每个点包含了1号无人机行进至该点的位置和时间。将数组中的所有元素存进一个新数组中，称为“工作库”。其中时间的计算公式为：

其中，t0代表无人机出发时间，n90代表该条路径中直角转弯的次数，n180代表路径中180度转弯的次数，τt代表直角转弯耗费时间，s代表路径总长度，v代表无人机平均速度。

在t1时间，2号无人机进入近地区域，此时重复上述算法，当得出2号无人机的最优三维数组后，与工作库中的数据进行比较，若存在相近点，那么该点被标记为2号无人机不可行进点，重新规划路线直到新的路径生成。规定，在位置坐标相同的前提下，当两个点的时间差小于2秒时，这两个点被认为是相近点。

重复上述过程，对任意时刻将要降落的无人机进行路径规划。

实时路径监测系统：

充分考虑现实中可能出现的无人机续航不足，个别无人机连接中断等突发情况。必须设计一个监测系统能够对整片近地区域的无人机进行实时的位置测定，当发现问题无人机时，监测系统报告目标无人机的位置信息，以便于及时纠正来保证总控系统能够继续有条不紊的运行。

具体为：以总控系统的时间为基准，监测系统每5秒接收一次近地区域所有无人机的二维坐标，将这些坐标加上此时的时间信息，和工作库中各无人机的路线数据进行对比，当目标偏离路线时发出警报信息。偏离检测算法的具体公式如下：

其中，k1＝1,k2＝0.2，为比例系数。x1,x2,x3为当前监测系统收到的某无人机坐标信息。n表示该架无人机的路线经过的方格个数。当warning＝1时，系统发出警报，否则不发出警报。经过实际场景的测试，设定当无人机偏离路线超过1米，或时间偏差超过五秒，认为无人机出现故障，通过调整比例系数的数值可以自行设定。

结果展示：采用上述算法，随机取两个路径规划的结果如图8所示(图中2表示无法通过，0表示可以通过)。从图8可以看出，整个系统都能有条不紊的运行，图中不仅能看到无人机的路线，还能预测出路线上的时刻信息，在地图障碍物不是很多的情况下，任意时刻任意路线无人机均能找到合适路线无停顿的抵达降落点。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。