一种基于相对定位的无人机自主穿越框型障碍物方法

1.本发明涉及无人机领域，具体涉及一种基于相对定位的无人机自主穿越框型障碍物方法。


背景技术：

2.无人机以其体积小、操作方便、机动灵活而著称，在军事和民用领域得到了广泛的应用。在室外，无人机依靠gps定位技术可以很好地获得定位信息。然而，在室内环境下，gps容易信号弱且不稳定，无法提供有效的实时准确的定位信息，室内环境下无人机容易出现飞行故障和碰撞等情况，极大地限制了无人机在室内环境下的应用，同时，近几年，越来越多的无人机被用于室内调查、室内救援等诸多室内场景，在这种进入室内的场景下，无人机穿越相似的框型障碍物成为了一种挑战。
3.常用的室内定位技术有基于wi-fi的室内定位，基于ibeacon的室内定位、基于uwb的室内定位、基于红外的室内定位、基于激光雷达的室内定位、基于 led的室内定位、基于zigbee的室内定位和基于超声波的室内定位等。不管采用哪种技术，都追求一种性价比较高的技术方案来解决实际需求。现有技术中技术，比较常见的是未知环境下，基于无人机自主定位、障碍物检测、障碍物定位、障碍物跟踪和障碍物穿越为主的无gps无人机穿越系统，但是这种方法对室内光线要求高，其次，其利用视觉slam确定自身对当前环境的位置和姿态计算较为复杂，其利用单目相机获取环境信息不够准确。还有双目相机配合多个毫米波雷达进行的障碍物检测，以及采用几何方法判断障碍物是否会阻碍无人机的飞行，从而进行避障穿越，但是这种方法复杂度较高，无人机在高速状态下反应不及时。
4.室内无人机定位要求定位精度较高、成本低且抗环境干扰(光、温度、湿度) 能力强等，但是当前的技术在应用中，当室内当无人机距离目标框型障碍物较远时，无人机无法通过摄像头对目标进行定位跟踪，自然无法实现自主穿越，同时，在光线薄弱的场景下，摄像头对于目标的检测的准确度不高，很难满足各种各样的室内场景，同时利用视觉slam确定自身对当前环境的位置和姿态计算较为复杂，其利用单目相机获取环境信息不够准确，多个毫米波雷达成本高，计算量大，信息融合需要时间长，无法满足无人机的高机动性，同时其采用的多传感器融合以及基于卡尔曼滤波的跟踪检测精度不高，实时性不强。所以无论是激光雷达进行定位还是红外传感，光流定位效果都不是很理想。


技术实现要素：

5.本发明的目的是提供一种基于相对定位的无人机自主穿越框型障碍物方法，用以克服现有方法存在的技术问题。
6.为了实现上述任务，本发明采用以下技术方案：
7.一种基于相对定位的无人机自主穿越框型障碍物方法，包括：
8.在无人机行进路径上需要穿越的框形障碍物上布置uwb传感器；无人机飞行过程中，利用障碍物上的uwb传感器与无人机自身的uwb传感器配合实时获取无人机与框形障碍
物的距离信息，并获取无人机的位移信息；
9.根据实时获取的无人机的位移信息，以及无人机与框形障碍物的距离信息构建数学参数模型；
10.利用所述数学参数模型，构建框形障碍物的位置估计器，进行框形障碍物位置的估计，从而可得到无人机与框形障碍物之间的相对位置估计值；
11.构建事件驱动穿越控制器，基于无人机相对位置的估计值，利用所述控制器得到无人机的速度控制指令，通过速度控制指令控制无人机穿越框形障碍物。
12.进一步地，所述根据实时获取的无人机的位移信息，以及无人机与框形障碍物的距离信息构建数学参数模型，包括：
13.利用所述距离信息与位移信息之间的数学关系构建数学参数模型：
[0014][0015][0016]
其中，ps是框形障碍物的位置，p0(k)是k时刻无人机的位置，满足 q0(k)＝p0(k)-ps；q0(k)表示k时刻无人机到框型障碍物的相对位置；p0(0)表示无人机初始时刻的位置，d0(k)表示k时刻无人机与框形障碍物之间的距离，表示无人机k时刻的位移，满足δ1是所构成的位移矩阵的最小特征值，n是本方法的维度。
[0017]
进一步地，所述利用所述数学参数模型，构建框形障碍物的位置估计器，包括：
[0018]
根据的数学参数模型构建目标函数，将位置估计问题等效成一个最优化问题，目标是使得j0最小：
[0019][0020]
其中e0(k)是参数模型∈s(k)的集合矩阵，φ0(k)是位移构成的位移矩阵，μ是阻尼系数，满足n是本方法的维度，δ1是位移矩阵的最小特征值，m(k)是构建的遗忘算子矩阵，满足是k时刻对框形障碍物位置ps的估计值，λ为遗忘因子；
[0021]
针对目标函数(1.3)，采用梯度下降法，对其求导得到：
[0022][0023]
对式(1.4)进行归纳，便设计了如下的位置估计器：
[0024][0025]
其中，i是单位矩阵，r0(k)是构建的协方差矩阵，是k+1时刻对障碍物的位置估计。
[0026]
进一步地，所述事件驱动穿越控制器具体推导如下：
[0027]
针对未知环境下的无人机位置，将无人机建模为速度有界的离散时间积分器：
[0028][0029]
其中p0(k)为无人机在k时刻的位置，t为采样周期，表明无人机为速度控制的方式，其大小即小于无人机的最大速度v0；
[0030]
将无人机的穿越问题描述成一个不断变化的跟踪问题，因此得到事件驱动穿越控制器：
[0031][0032]
其中，a0是控制器增益，是设计的激励信号，v0(k)是经过事件驱动穿越控制器所得到的速度控制指令，是对v0(k)进行了限速，使得最后的输出速度小于预设的最大速度v0；h0(k)是穿越控制信号开关。
[0033]
进一步地，穿越控制信号开关h0(k)满足：
[0034][0035]
其中表示无人机到框型障碍物的相对位置估计值，k1为初始时间，k4为运行时间，sup表示上确界；和是事件触发器的起始点和结束点，rot是框型障碍物相对坐标系的旋转矩阵，在二维情况下，其旋转矩阵为w是障碍物相对于的无人机坐标系的角度，α是穿越过程中的穿越步长。
[0036]
进一步地，当事件驱动穿越控制器满足的条件，穿越误差是指数收敛；
[0037]
其中，t是式(1.6)的采样时间，a0为制器增益。
[0038]
一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，处理器执行计算机程序时实现所述基于相对定位的无人机自主穿越框型障碍物方法的步骤。
[0039]
一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器执行时实现所述基于相对定位的无人机自主穿越框型障碍物方法的步骤。
[0040]
与现有技术相比，本发明具有以下技术特点：
[0041]
本发明是一种长距离，高适用性的无人机自主穿越框型障碍物方法，没有光线和短距离的限制，且具有较低的计算复杂度，不仅限于无人机方面的应用，在其他如无人车，无人艇等在可获得的距离和位移的相对定位的应用场景方面都有良好的应用前景。
附图说明
[0042]
图1为本发明方法的结构框架图；
[0043]
图2为本方案中的数据流向示意图；
[0044]
图3的(a)和(b)为本发明方法在gazebo上的实现效果图。
具体实施方式
[0045]
本发明提出了一种具有长距离，高适用性的无人机自主穿越框型障碍物方法，没有光线和短距离的限制，具体来讲，本方案基于无人机与障碍物的距离设计了位置估计器，在不知道障碍物的位置时利用位置估计器来估算障碍物的位置；之后，设计了一个事件驱动穿越控制器来达到无人机穿越的目的。
[0046]
参见图1，本发明的一种基于相对定位的无人机自主穿越框型障碍物方法，包括以下步骤：
[0047]
步骤1，在无人机行进路径上需要穿越的框形障碍物上布置uwb传感器，可通过标枪等方式将uwb传感器标射到目标框型障碍物上，其中uwb传感器布置的具体位置是在框型障碍物的任意位置上，以此与无人机上的uwb传感器交互进而获得两者间的距离d0。
[0048]
步骤2，无人机在自动飞行中实时进行传感器数据的采集，其中，uwb传感器的距离数据直接通过串口通信传输给机载电脑，用于测量无人机到框型障碍物的距离信息，而惯性测量单元imu和超声波传感器搭载在无人机的控制平台px4 上，imu可以通过积分获得无人机的位移信息，超声波传感器可以获得无人机的高度信息，通过控制平台px4与机载电脑的通信进行间接的获取。
[0049]
步骤3，机载电脑进行数据的ros话题包处理；具体的，针对imu和超声波传感器的位移信息、高度信息，可通过控制平台px4与ros机器人操作系统的mavros包得到对应的数据话题，uwb传感器通过串口通信，将距离信息由不同的uwb型号和通信协议进行预处理，同时将其发布到一个自己构造的话题包。
[0050]
步骤4，根据实时获取的无人机的位移信息，以及无人机与框形障碍物的距离信息构建数学参数模型，具体如下：
[0051]
uwb传感器获取的距离信息与imu传感器获得的无人机的位移信息这两者之间存在着数学关系，将其表示为数学参数模型∈s(k)：
[0052][0053][0054]
其中，ps是框形障碍物的位置，p0(k)是k时刻无人机的位置，满足 q0(k)＝p0(k)-ps；q0(k)表示k时刻无人机到框型障碍物的相对位置；p0(0)表示无人机初始时刻的位置，d0(k)表示k时刻无人机与框形障碍物之间的距离，在三维情况下，可通过uwb传感器测量，在二维情况下，可通过超宽带传感器测量三维距离信息结合超声波的高度信息由勾股定理二维的距离信息，表示无人机k时刻的位移，满足δ1是所构成的位移矩阵的最小特征值，n是本方法的维度。
[0055]
步骤5，利用所述数学参数模型，构建框形障碍物的位置估计器，进行框形障碍物位置的估计，从而可得到无人机与框形障碍物之间的相对位置估计值。
[0056]
其中，所述位置估计器具体推导如下：
[0057]
步骤5.1，根据步骤4所设计的数学参数模型(1.1)、(1.2)，构建目标函数，将位置估计问题等效成一个最优化问题，目标是使得j0最小。
[0058][0059]
其中e0(k)是参数模型∈s(k)的集合矩阵，φ0(k)是位移构成的位移矩阵，μ是阻尼系数，满足n是本方法的维度，若是二维则为2，若是三维则为3，δ1是位移矩阵的最小特征值，m(k)是构建的遗忘算子矩阵，满足是k时刻对框形障碍物位置ps的估计值，本方案中，参数上标λ表示该参数的估计值，下同；λ为遗忘因子。
[0060]
步骤5.2，针对目标函数(1.3)，采用了梯度下降法，对其求导得到：
[0061][0062]
步骤5.3，对步骤5.2的式(1.4)进行归纳，便设计了如下的位置估计器：
[0063][0064]
其中，i是单位矩阵，r0(k)是构建的协方差矩阵，是k+1时刻对障碍物的位置估计，λ是设计的遗忘算子，目的为了使得新数据在算法中权重比更大，其大小限制在0＜λ＜1，μ是设计的阻尼系数，目的为使得数据不发生爆炸现象。
[0065]
步骤6，构建事件驱动穿越控制器，基于无人机相对位置的估计值，利用所述控制器得到无人机的速度控制指令，通过速度控制指令控制无人机穿越框形障碍物。
[0066]
所述事件驱动穿越控制器具体推导如下：
[0067]
步骤6.1，针对未知环境下的无人机位置，将无人机建模为速度有界的离散时间积分器。
[0068][0069]
其中p0(k)为无人机在k时刻的位置，t为采样周期，表明无人机为速度控制的方式，其大小即小于无人机的最大速度，由式(1.7)控制器设计； v0为无人机的最大速度。
[0070]
步骤6.2，在设计步骤5的障碍物位置估计器(1.5)后，结合式(1.6)，将无人机的穿越问题描述成一个不断变化的跟踪问题，因此本发明设计一个事件驱动穿越控制器，如式1.7：
[0071][0072]
其中，a0是控制器增益，由控制器稳定性和收敛性分析后得到其范围，是设计的激励信号，目的是为了使得协方差矩阵r0(k+1)一直是持续激励状态， v0(k)是经过事件驱动穿越控制器所得到的速度控制指令，是对v0(k)进行了限速，使得最后的输出速度小于预设的最大速度v0；h0(k)是穿越控制信号开关，满足：
[0073][0074]
其中表示无人机到框型障碍物的相对位置估计值，满足式中k1为初始时间，k4为运行时间，
sup表示上确界。和是事件触发器的起始点和结束点，rot是框型障碍物相对坐标系的旋转矩阵。在二维情况下，其旋转矩阵为w是障碍物相对于的无人机坐标系的角度，可通过人为设置，也可通过附着在uwb传感器的里程计测出角度；α是穿越过程中的穿越步长。
[0075]
当事件驱动穿越控制器满足的条件，穿越误差是指数收敛。
[0076]
其中，t是式(1.6)的采样时间，因此，可以得到控制器增益a0的取值范围。
[0077]
本发明在具体应用时，首先根据步骤6的(1.7)、(1.8)解算出来的无人机速度控制数据，将通过ros机器人操作系统，发布到ros针对px4无人机的mavros 包下的速度控制节点话题，由于步骤4无人机实时获取的数据在不断变化，进而步骤5不断更新，进而得到无人机与框形障碍物相对位置的估计值，而根据估计的相对位置的不同，会由步骤6得到对应的速度控制指令，以此使得无人机穿越框形障碍物。图3的(a)和(b)为本发明方法在gazebo上的实现效果图，可以看到，利用本发明方法能使无人机顺利地穿越框形障碍物，验证了本方法对实际无人机模型的合理性。
[0078]
以上实施例仅用以说明本技术的技术方案，而非对其限制；尽管参照前述实施例对本技术进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本技术各实施例技术方案的精神和范围，均应包含在本技术的保护范围之内。