一种基于UWB的改进LSE-EKF室内无人机定位方法

一种基于uwb的改进lse-ekf室内无人机定位方法
技术领域
1.本发明涉及无人机室内定位技术领域，具体为一种基于冗余uwb基站的室内无人机定位方法


背景技术：

2.当前中国无人机行业的总产值已达接近1000亿元，其中小型多旋翼无人机占整个无人机产业的主导地位。目前，业界对于小型旋翼无人机的应用主要集中在室外，这是由于在室外空旷环境下，传统小型多旋翼无人机能达到良好的定位精度和飞行控制稳定性，但在室内复杂环境中无人机的应用主要受到定位问题的限制。
3.无人机在室内飞行过程中由于受到建筑物和室内障碍物的遮挡，无人机依靠全球卫星导航定位系统所得到的定位精度已无法满足飞行要求，同时现有无人机室内定位方法的信号传输也存在着多路径效应和非视距传播等问题，造成无人机在室内复杂空间飞行的定位精度下降。
4.根据传感器处理定位数据的方式不同，当前无人机主流的室内定位技术主要分为：主动感知型、被动接收型和组合型三种。
5.主动感知型定位方法是指：无人机自带信号发射接收元器件或能够通过其他技术手段能够主动感知周围环境。其具有定位精度高，实时性好，无须预先架设基站等优点，但也存在设备成本高，可靠性较差，需要高性能机载处理终端等缺点。目前主动感知型定位技术主要有超声波、光流、激光slam和视觉slam等。
6.被动接收型定位方法是指：无人机直接通过信号收发元器件接收所处环境的位置信息，无须通过机载高性能处理终端运算，这样以来被动接收型定位技术具有运算量小，接收终端体积小重量轻，定位精准度高等优点，但也存在需要提前架设基站或标定锚点，在复杂环境中定位信号衰减等缺点。目前被动接收型定位技术主要有蓝牙、wifi、zigbee和uwb等
7.组合型室内定位方法是指：通过主动感知型和被动接收型中的多种技术组合完成无人机室内导航定位的方式，组合型无人机室内定位技术能够结合两种方式的优点，但也对机载高性能处理器和无人机的载重配置提出了更高的要求，也增加了无人机室内导航的成本
8.uwb技术作为室内无人机定位的热点技术，与传统采用正弦载波的通讯技术不同，超宽带技术采用纳秒级高频脉冲信号传输数据，所占通讯频谱范围很宽，因此与传统通讯定位技术相比超宽带具有穿透力强、抗干扰能力好，定位精度高等优点，但也存在需要预先架设定位基站，易受信号多路径效应和非视距传播干扰等缺点。


技术实现要素：

9.为解决现有无人机室内定位技术存在的设备成本高、稳定性差、易受多路径效应和非视距传播等缺点，本发明提出一种基于uwb的改进lse-ekf室内无人机定位方法，能够
有效提升无人机在室内复杂场景下的定位精度。
10.本发明基于uwb-toa定位系统提出一种改进lse-ekf定位方法，包括bp神经网络修正模块、最小二乘估计冗余基站数据处理模块、扩展卡尔曼滤波模块。
11.对于本发明所述的bp神经网络修正模块，通过样本训练建立bp神经网络，对基站测量数据进行优化修正，能够有效应对非视距传播和多路径效应对无人机定位造成的影响，本发明将bp神经网络进行三层架构由输入层、隐含层和输出层组成。在非视距条件下，我们以4基站组成冗余定位系统为例，分别测得四组数据，将k组toa法测量值作为样本，建立bp神经网络模型，对所建立的模型进行样本训练后得到bp神经网络修正模型。
12.对于本发明所述的最小二乘估计冗余基站数据处理模块，通过最小二乘估计算法进行数据融合优化，减小uwb定位设备的观测系统误差。在已知定位基站坐标的情况下，我们通过选取不同基站的组合，解得待测标签的位置坐标。同时为提高系统解算速度，优化算法运行速度，减少计算量，我们通过求解系数矩阵伪逆法，对冗余基站数据进行融合优化计算。
13.对于本发明所述的扩展卡尔曼滤波模块，引入扩展卡尔曼滤波算法对uwb定位信号中的高斯白噪声部分进行优化处理。首先对于无人机室内定位系统进行建模分析，对模型在参考点处进行一阶泰勒展开，可得到非线性系统在高斯白噪声的干扰下的一阶线性描述。最后通过扩展卡尔曼滤波算法，对定位信号中的高斯白噪声部分进行滤波处理。
14.本发明对比已有室内定位技术，能够有效提升无人机在室内复杂场景下的定位精度与稳定性。特别是在非视距场景下与传统室内定位技术相比，改进lse-ekf定位方法能够有效减少信号传播多径效应和信号中混叠的高斯白噪声造成的定位误差。
附图说明
15.为了更清晰地体现本发明的特点与技术方案，下面通过附图对本发明作更直观的解释：
16.图1为本发明中bp神经网络模型示意图；
17.图2为改进lse-ekf定位算法流程图；
18.图3为视距和非视距测试环境下定位基站布置与无人机预定运动轨迹示意图
19.图4为无人机室内定位过程中在视距条件下和非视距条件下采用不同定位方法的定位轨迹对比图；
20.图5为无人机室内定位过程中在视距条件下和非视距条件下采用不同定位方法在各坐标轴方向上的定位数据对比图。
具体实施方式
21.为了使与本发明相关的问题和技术方案更加清晰，下面将结合附图对本发明的技术方案做详细说明。基于本发明的实施案例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施案例，都属于本发明的权利保护范围之内。
22.根据图1可知，bp神经网络的其样本训练的迭代过程如下，对于该网络的toa值输入，我们通常对其进行归一化处理，神经网络的输入为：
23.p＝[toa
123
,toa
124
,toa
134
,toa
234
]
[0024]
此时，输入层的神经元数量为4；隐含层神经元数根据kolmogorov定理取9；经过bp神经网络迭代修正后输入层与输出层神经元数相同，所以输出层的神经元同为4，则以x轴输出为例：
[0025]
o＝[x
123
,x
124
,x
134
,x
234
]
[0026]
f1表示bp神经网络输入层与隐含层之间的激活函数，ω
ij
表示隐含层与输入层之间的权值，因此在隐含层中第i个神经元输出值可以表示为：
[0027][0028]
因为输出层神经元的个数为4，所以f2表示bp神经网络隐含层和输出层之间的激活函数，ω
ki
表示隐含层与输出层之间的权值，所以在输出层中第k个神经元输出值可以表示为：
[0029][0030]
则损失函数可以表示为：
[0031][0032]
因为e是关于权值ω
ij
和ω
ki
的函数，所以本文通过梯度下降法迭代计算最优解，调整权值以减小损失函数e，所以权值的变化为：
[0033][0034][0035]
则权值可以按照以下公式进行调整：
[0036]
ω
ki
(t+1)＝ω
ki
(t)+δω
ki
(t)
[0037]
ω
ij
(t+1)＝ω
ij
(t)+δω
ij
(t)
[0038]
这样通过迭代训练，我们得到面向冗余toa定位的bp修正神经网络模型，此模型可以对冗余基站得到的原始数据进行修正，以应对室内复杂环境中信号的非视距传播和多路径传播对无人机定位造成的影响，为改进lse-ekf算法中进一步对定位信号进行处理优化提供良好基础。
[0039]
本发明引入最小二乘估计对冗余基站的定位数据进行数据融合优化，以减小uwb设备的观测系统误差。下面我们以4基站冗余定位系统为例，冗余方程组如下：
[0040][0041]
提取因数并进行整理后可得伪逆形式的表达式：
[0042][0043]
令为超定方程组的系数矩阵，d'为提取因数后的已知常量，方程可以化简为：
[0044][0045]
最后通过求解系数矩阵的伪逆可得到最优解待测点坐标为(x,y,z)。这样利用最小二乘估计算法对冗余uwb基站定位系统进行数据融合优化后，得到的待测点位置精度与最小3基站系统相比，精度有了很大的提高。
[0046]
下面本发明将对无人机室内定位系统建模过程及扩展卡尔曼滤波器在无人机室内定位中的应用做详细介绍。
[0047]
首先我们对系统进行建模，在本文实验过程中，无人机的飞行过程基于航点任务飞行模式，在飞行过程做匀速运动，但在横向和纵向还是会受到扰动进而产生随机的加速度，无人机的运动状态可以归纳为受到随机加速影响的匀速运动。设无人机在某一时刻k，位置为(x,y,z)，速度为(v
x
,vy.vz)，在k时刻的状态向量可表示为x
t
＝[x v
x y v
y z vz]
t
,我们可得到此时的系统模型：
[0048][0049]
状态转移矩阵a和噪声驱动矩阵g分别为：
[0050]
[0051][0052]
令w
k-1
＝[a
xk-1 a
yk-1 a
zk-1
]
t
,表示k时刻的过程噪声，其协方差为q，所以状态方程为：
[0053]
xk＝ax
k-1
+gw
k-1
[0054]
观测方程以4基站冗余观测系统为例，无人机标签位置到某一基站的距离di可以表示为：
[0055]di
＝c
·
(t
i-ts)+μ
toa
[0056]
其中，μ
toa
是系统测量误差，由系统的实际情况确定，μ
toa
服从高斯分布，则观测方程可以表示为：
[0057]
zk＝h(xk)+vk[0058]
其中，vk是系统的观测噪声，其协方差为：
[0059][0060]
以上为面向室内复杂场景下的无人机定位模型，下面我们将以x轴为例推导以此系统模型为基础的扩展卡尔曼滤波器原理，考虑超宽带室内定位为非线性系统，其状态方程和状态方程分别为：
[0061]
xk＝f(x
k-1
,u
k-1
,w
k-1
)
[0062]
zk＝h(xk,vk)
[0063]
其中，w
k-1
为过程噪声，vk为测量噪声，由于高斯白噪声对定位的干扰影响最大，我们令w
k-1
和vk相互独立且服从高斯分布，分别表示为：
[0064]
p(ω)～n(0,q)
[0065]
p(ν)～n(0,r)
[0066]
其中，q和r分别为为过程噪声和观测噪声协方差矩阵。
[0067][0068][0069]
式中，是x
k-1
的后验值，我们利用一阶泰勒展开进行线性化处理得到：
[0070][0071]
式中a和w为f对x求偏导数后的雅克比矩阵。同理我们将在处线性化得到：
[0072][0073]
式中h和v是h对x求偏导数后的雅克比矩阵，据扩展卡尔曼滤波器原理，将非线性
系统线性化后的扩展卡尔曼滤波器原理如下所示：
[0074]
预测部分：
[0075][0076][0077]
校正部分：
[0078][0079][0080][0081]
根据图2可知，改进lse-ekf算法首先通过bp神经网络修正模型得到优化后的uwb冗余基站定位数据，再通过最小二乘估计算法进行冗余基站定位数据融合，从而得到待测的坐标初步优化值，最后经过扩展卡尔曼滤波器除去信号中混叠的高斯白噪声，最终得到精准的坐标值，因此改进lse-ekf算法能够使无人机室内定位更加精准，测量位置更加接近真实值。
[0082]
根据图3可知，为验证改进lse-ekf算法的有效性，以a0基站为原点建立坐标系，顺时针方向分别为a1，a2，a3基站；标签t0固定在无人机上，由无人机高倍率锂电池经过电流计降压后直接供电；控制台c0直接与笔记本电脑相连，直接显示实时轨迹并记录运轨迹数据。
[0083]
根据图3可知，在los条件下，采用边长为5m的正方形布局，基站架设相对地面高度为1.55m。在nlos条件下，基站位置与los条件相同，实验中我们将基站t2，t3布置在了木质隔墙后，木质隔墙的厚度为0.2m，uwb信号经过木质隔墙阻隔后无法直射传播，通过折射等方式传播通讯，营造nlos环境下的实验条件。
[0084]
图4表示无人机在los和nlos条件下两次实验中不同算法优化后的轨迹，通过轨迹对比我们可以发现，与扩展卡尔曼滤波算法的定位轨迹相比经过改进lse-ekf算法优化后的轨迹的稳定性有着较大的提升，信号中白噪音的干扰明显减少。
[0085]
根据图5可知(a1)(a2)(a3)与(b1)(b2)(b3)分别为los和nlos条件下两次实验x，y，z轴方向上坐标位置随系统采样时间的变化情况，系统的采样周期t＝20ms。每幅图中三种线型分别代表理想数据、使用扩展卡尔曼滤波算法的定位数据和经过改进lse-ekf算法滤波优化后的数据。
[0086]
根据图5可知，经过改进lse-ekf算法优化后的数据与使用扩展卡尔曼滤波算法的定位数据相比数据的波动更小，更加接近理想数据。在(a3)和(b3)中，由于z轴的数据始终是恒定不变的，当我们选择尺度更小的坐标轴时，能够更加明显地看出改进lse-ekf算法的有效性。
[0087]
根据图5可知，经过数据分析在nlos条件下，最大路径偏离优化百分比能够平均提高15％以上，说明经过bp神经网络修正模型优化后的改进lse-ekf算法能够有效处理nlos条件下的异常定位值，减小定位数据大范围波动，有效提高定位精度。
[0088]
以上所述仅为本发明的一种实施方式，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修
改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种基于uwb的改进lse-ekf室内无人机定位方法，其主要包括bp神经网络修正模块、最小二乘估计模块和扩展卡尔曼滤波模块。其主要特征在于：通过bp神经网络修正模型对于uwb基站的初始定位数据进行修正优化，有效解决信号非视距传播造成的误差；对于室内冗余基站定位场景，通过最小二乘估计模块对冗余基站的定位数据进行融合；最后对于定位信号混叠的高斯白噪声，通过卡尔曼滤波模块进行滤波处理。2.根据权利要求1所述的一种基于uwb的改进lse-ekf室内无人机定位方法，其特征在于，本发明将bp神经网络用于室内无人机定位的预处理过程，特别是对于非视距场景下无人机的定位问题。主要内容包括：bp神经网络修正模型的构造迭代过程；神经网络输入层与隐含层之间的表达关系；隐含层与输出层之间的表达关系；利用梯度下降法对表达式中的权值进行调整。3.根据权利要求1所述的一种基于uwb的改进lse-ekf室内无人机定位方法，其特征在于，对于无人机室内多基站定位场景，本发明通过最小二乘估计算法对冗余基站的定位数据进行融合优化。主要内容为：通过伪逆方法求解超定方程系数矩阵，对冗余基站数据进行融合，并提升运算速度。4.根据权利要求1所述的一种基于uwb的改进lse-ekf室内无人机定位方法，其特征在于，对于uwb定位信号多路径传播造成的高斯白噪声干扰，通过扩展卡尔曼滤波器进行滤波处理。主要内容为：室内无人机定位模型的建立过程；对系统模型进行滤波处理。

技术总结
本发明提出一种基于UWB的改进LSE-EKF室内无人机定位方法，能够有效解决传统室内定位方法中信号易受多路径效应干扰、在非视距环境下定位精度低等缺点。改进LSE-EKF定位方法由BP神经网络修正模块、最小二乘估计模块和扩展卡尔曼滤波模块组成，BP神经网络模型能够有效对基站的定位数据进行修正；对于冗余基站的无人机室内定位场景，通过最小二乘估计算法对冗余基站的定位数据进行融合优化处理；最后对于定位信号中混叠的高斯白噪声干扰，本发明通过扩展卡尔曼滤波模块对信号进行滤波处理，得到更加精准的定位信息。特别是在非视距环境下，与传统定位方法相比改进LSE-EKF定位方法的精度能够提升15％以上，为室内无人机的产业化应用提供了一种优秀的定位解决方案。用提供了一种优秀的定位解决方案。用提供了一种优秀的定位解决方案。


技术研发人员：陈国华 关冠童
受保护的技术使用者：北京化工大学
技术研发日：2023.05.16
技术公布日：2023/8/13