一种基于MCC的EKF-SLAM后端导航路径优化方法与流程

一种基于mcc的ekf-slam后端导航路径优化方法
技术领域
1.本发明属于飞行器导航的技术领域，具体涉及一种基于mcc的ekf-slam后端导航路径优化方法。


背景技术：

2.飞行器在运动的过程中，通常是通过探测其经纬度、方位角、俯仰角、加速度、角速度等信息对飞行器的运动姿态与路径进行计算。这些信息是通过极坐标系转换得到，这使得利用这些数据得到的飞行器运动轨迹存在非线性的数据。现阶段飞行器大多数采用的非自主导航，具有高精度、全时段的特点。但无线电的抗干扰能力差，面对电子对抗技术的快速发展，使得无线电导航在军民飞行器领域存在极大的安全隐患，slam（simultaneous localization and mapping）同步定位与构图，指运动物体根据传感器探测到的数据，一边计算自身的位置，一边构建环境地图的过程。slam分为前端与后端，前端通常采用光流法、直接法、特征点法对运动物体进行探测得出数据。后端则用于处理前端数据并建图，其主要分为滤波算法和非线性优化法对前端的数据进行校正与提取。
3.slam后端中非线性优化中光束平差法（ba，bundle adjustment）其实时性高，但在优化求解中设计变量过多，计算量较大。卡尔曼滤波（kf, kalman filtering）可以在非平稳噪声环境下工作，实现实时数据处理。但是传统的kf为线性系统，因此提出扩展卡尔曼滤波（ekf, extend kalman filtering）算法对非线性系统数据进行处理。当系统的非线性程度很高时，ekf在泰勒展开式中被忽略掉的二阶及以上所带来的误差将导致滤波的发散。但当其与最大相关熵（mcc, maximum correntropy criterion）相结合时，因mcc具有捕获高阶信息的特性，会使得ekf的鲁棒性大幅提升。
4.mcc主要用于度量两个随机变量的相似性，假设x，y间的联合分布函数为它们间的熵定义为：其中，e代表期望因子，代表mercer核。
5.所有的核函数都是由高斯核函数给出的，其表达式如下：其中代表核带宽；为高斯核函数；为指数函数。
6.因为在雷达跟踪系统中，可使用的数据是有限的，因此采用样本均值的方式对相
关熵进行估算，具体如下：其中，；n为数据总数，i为当前数据，为第i个高斯核函数，为第i个x-y的值 ，为第i个x值 ，为第i个y值 ；对高斯核进行泰勒级数展开可得：其中，n为当前阶数；由上式可以看出相关熵是x-y所有偶阶矩的加权和，可以从中提取数据的高阶统计量。
7.当给定误差数据序列时，基于mcc准则的代价函数表示为：。


技术实现要素：

8.本发明的目的在于针对现有技术中的上述不足，提供一种基于mcc的ekf-slam后端导航路径优化方法，以解决复杂空间环境下，面对不同的地域下飞行器巡逻时通过自主运动情况和空间进行准确的位置姿态参数获取以及路径优化的问题。
9.为达到上述目的，本发明采取的技术方案是：一种基于mcc的ekf-slam后端导航路径优化方法，其包括以下步骤：s1、slam前端通过飞行器自身传感器获取飞行器姿态、位置和路标点信息；s2、slam后端对获取的飞行器姿态、位置和路标点信息的数据进行融合，包括：s2.1、根据融合后的飞行器姿态、位置和路标点信息的数据，构建状态空间模型；s2.2、根据激光雷达获取的观测数据，构建量测模型；s2.3、系统初始化；s2.4、将步骤s1中飞行器自身传感器获取的数据信息与激光雷达获取到的观测数据通过ekf-mcc算法进行融合，以估计当前飞行器的姿态、速度、位置和路标点信息；s2.5、对飞行器巡视的系统信息进行更新。
10.进一步地，步骤s1中位置和路标点为系统状态，以飞行器的起始位置作为其世界坐标系w系的原点；以正北方向作为世界坐标系x轴的正向，以正西方向作为世界坐标系y轴的正向，同时根据右手准则确定世界坐标系z轴的正向；载体坐标系b系的原点为飞机机体正中心，平行于机身纵轴指向机头方向为载体坐标系x轴的正向，平行于机身横轴指向左方的为载体坐标系y轴的正向，同时根据右手准则确定载体坐标系z轴的正向。
11.进一步地，步骤s2.1中构建的状态空间模型，其在世界坐标系下表示为：
其中，为k时刻系统的状态，为k-1时刻系统的状态，为控制向量，即为传感器获得的加速度和角速度；为高斯白噪声组成的系统噪声，其构成协方差为；；由矩阵构成；为k-1时刻飞行器在世界坐标系下的经纬高；为k-1时刻飞行器在世界坐标系下的速度由x,y,z三个轴向速度构成；为k-1时刻飞行器的姿态，其由俯仰，横滚，偏航三个参数够成；为世界坐标系下的路标点坐标；为k-1时刻下载体坐标系与世界坐标系间的转换矩阵；为k-1时刻载体坐标系下的比力加速度；为k-1时刻世界坐标系下的重力加速度；代表采样时间间隔；为k-1时刻载体坐标系下的角速度。
12.进一步地，步骤s2.2中构建的量测模型为：其中，为k时刻系统的量测值，为k时刻激光雷达量测的飞行器质心与第i个路标点间的距离；为k时刻激光雷达量测的飞行器质心与第i个路标点间的距离俯仰角；为k时刻激光雷达量测的飞行器质心与第i个路标点间的方位角；为激光雷达自身量测噪声，设定为高斯白噪声，其构成协方差为；为k时刻与路标点构成的量测方程非线性函数。
13.进一步地，步骤s2.3中系统初始化为：使用预设的高斯核带宽，构建k 1时刻高斯核函数：
其中，为k 1时刻的高斯核函数，为选取的高斯核带宽，为k 1时刻的迭代参数：其中，为k 1时刻状态值的迭代参数，为k 1时刻观测值的迭代参数，为世界坐标系下根据k时刻获得的k 1时刻的状态值，为状态转移矩阵， 为k时刻世界坐标系下的状态估计值，为k 1时刻系统的量测值，；对ekf-mcc进行迭代获得k 1时刻世界坐标系下的状态估计值及其对应的协方差矩阵：其中，为世界坐标系下k时刻状态估值构成的观测方程，为世界坐标系下k时刻的误差协方差矩阵，为k时刻状态转移矩阵的转置，为量测噪声协方差。
14.进一步地，步骤s2.4中将步骤s1中飞行器自身传感器获取的数据信息与激光雷达获取到的观测数据通过ekf-mcc算法进行融合，以估计当前飞行器的姿态、速度、位置和路标点信息为：其中，为由状态，观测构成的高斯核函数比值，为误差协方差矩
阵的逆；为卡尔曼增益；为k时刻下观测矩阵的转置；为系统噪声协方差的逆。
15.进一步地，步骤s2.5中对飞行器巡视的系统信息进行更新为：进一步地，步骤s2.5中对飞行器巡视的系统信息进行更新为：其中，i为单位矩阵；为系统噪声协方差；为卡尔曼增益的转置。
16.本发明提供的基于mcc的ekf-slam后端导航路径优化方法，具有以下有益效果：本发明基于mcc，并将其作为实现准则，对slam后端数据进行ekf优化处理；针对slam后端的非线性、非高斯滤波问题，本发明将mcc-ekf算法应用到航空飞行器既定航线巡逻路径修正中；相较于传统的ekf算法只能得到误差二阶项的信息，本发明基于mcc的ekf算法可得到误差二阶项及更高阶的统计量，从而使得系统性能得到极大的改善，降低了飞行器系统在非高斯噪声下严重恶化的影响，提高了飞行器高空飞行中循迹定位的稳定性与可靠性。
17.本发明相比传统的ekf自主导航方式，本发明提出的算法提高了数据的鲁棒性，基于mcc的ekf算法作为slam后端可以用于更大规模的数据提取并校正，使得飞行器在面对更复杂的飞行环境时，具有更高的导航精度、完好性与连续性。
附图说明
18.图1为本发明系统示意图。
19.图2为本发明流程图。
20.图3为本发明ekf-mcc与ekf角度均方根误差值对比图。
21.图4为本发明ekf-mcc与ekf位置均方根误差值对比图。
具体实施方式
22.下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
23.实施例1，参考图2，本实施提供一种基于mcc的ekf-slam后端导航路径优化方法，其利用mcc作为代价函数以解决ekf算法作为slam后端抗噪能力不足的缺点，提高了其鲁棒性，具体包括以下步骤：步骤s1、slam前端通过飞行器自身传感器获取飞行器姿态、位置和路标点信息、同时激光雷达获取路标点；步骤s2、slam后端对前端获取的飞行器姿态、位置和路标点信息的数据通过ekf-mcc算法进行融合，包括：
步骤s2.1、根据融合后的飞行器姿态、位置和路标点信息的数据，构建状态空间模型；步骤s2.2、根据激光雷达获取的观测数据，构建激光雷达的量测模型；步骤s2.3、系统初始化、地图初始化；步骤s2.4、将步骤s1中飞行器自身传感器获取的数据信息与激光雷达获取到的观测数据通过ekf-mcc算法进行融合，以估计当前飞行器的姿态、速度、位置和路标点信息进行匹配，将数据进行关联；步骤s2.5、对飞行器巡视的系统信息进行更新。
24.如图1所示，飞行器通过地面路标点及探测器探测数据，进行数据融合slam后端mcc-ekf算法，进行下一位置目标循迹定位优化飞行。
25.实施例2，本实施例为对实施例1步骤进一步的描述，在建立飞行器状态空间模型的同时借助全球定位系统创建观测模型并利用改进后的ekf算法实现路径规划和循迹，与传统ekf相比mcc-ekf极大的提高了飞行器实时定位与建图精度，其具体包括以下步骤：步骤s1、slam前端通过飞行器自身传感器获取飞行器姿态、位置和路标点信息；位置和路标点为系统状态，以飞行器的起始位置作为其世界坐标系w系的原点；以正北方向作为世界坐标系x轴的正向，以正西方向作为世界坐标系y轴的正向，同时根据右手准则确定世界坐标系z轴的正向；载体坐标系b系的原点为飞机机体正中心，平行于机身纵轴指向机头方向为载体坐标系x轴的正向，平行于机身横轴指向左方的为载体坐标系y轴的正向同时根据右手准则确定载体坐标系z轴的正向。
26.步骤s2、slam后端对获取的飞行器姿态、位置和路标点信息的数据进行融合，包括：步骤s2.1、根据融合后的飞行器姿态、位置和路标点信息的数据，构建状态空间模型，其在世界坐标系下表示为：其中，为k时刻系统的状态，为k-1时刻系统的状态，为控制向量，即为传感器获得的加速度和角速度；为高斯白噪声组成的系统噪声，其构成协方差为；；由矩阵构成；为k-1时刻
飞行器在世界坐标系下的经纬高；为k-1时刻飞行器在世界坐标系下的速度由x,y,z三个轴向速度构成；为k-1时刻飞行器的姿态，其由俯仰，横滚，偏航三个参数够成；为世界坐标系下的路标点坐标；为k-1时刻下载体坐标系与世界坐标系间的转换矩阵；为k-1时刻载体坐标系下的比力加速度；为k-1时刻世界坐标系下的重力加速度；代表采样时间间隔；为k-1时刻载体坐标系下的角速度。
27.步骤s2.2、根据激光雷达获取的观测数据，构建量测模型为：其中，为k时刻系统的量测值，为k时刻激光雷达量测的飞行器质心与第i个路标点间的距离；为k时刻激光雷达量测的飞行器质心与第i个路标点间的距离俯仰角；为k时刻激光雷达量测的飞行器质心与第i个路标点间的方位角；为激光雷达自身量测噪声，设定为高斯白噪声，其构成协方差为；为k时刻与路标点构成的量测方程非线性函数。
28.步骤s2.3、系统初始化为：使用预设的高斯核带宽，构建k 1时刻高斯核函数：其中，为k 1时刻的高斯核函数，为选取的高斯核带宽，为k 1时刻的迭代参数：其中，为k 1时刻状态值的迭代参数，为k 1时刻观测值的迭代参数，为世界坐标系下根据k时刻获得的k 1时刻的状态值，为状态转移矩阵， 为k时刻世界坐标系下的状态估计值，为k 1时刻系统的量测值，。
29.对ekf-mcc进行迭代获得k 1时刻世界坐标系下的状态估计值及其对应的协方差矩阵：其中，为世界坐标系下k时刻状态估值构成的观测方程，为世界坐标系下k时刻的误差协方差矩阵，为k时刻状态转移矩阵的转置，为量测噪声协方差。
30.利用获取的信息进行初始化建图。
31.步骤s2.4，将步骤s1中飞行器自身传感器获取的数据信息与激光雷达获取到的观测数据通过ekf-mcc算法进行融合，以估计当前飞行器的姿态、速度、位置和路标点信息为：其中，为由状态，观测构成的高斯核函数比值，为误差协方差矩阵的逆；为卡尔曼增益；为k时刻下观测矩阵的转置；为系统噪声协方差的逆。
32.步骤s2.5，对飞行器巡视的系统信息进行更新为：视的系统信息进行更新为：其中，i为单位矩阵；为系统噪声协方差；为卡尔曼增益的转置。
33.根据以上步骤来判断每次迭代的结果是否在设定范围内，以此摒弃超界值保持算法精度，本实施例方法既提高了ekf在非线性环境下的跟踪性能同时还降低了高斯噪声环境下高阶信息对于算法的干扰，提高了ekf算法作为slam后端的鲁棒性和定位精度。
34.如图3和图4，其显示了本发明中ekf-mcc算法与ekf在相同数据条件下飞行器角度以位置的均方根误差值，其横坐标为时间系数，纵坐标为均方根差值，均方根误差越小表示
其性能越好，从滤波结果图中可以看出ekf-mcc算法作为slam后端的滤波效果明显优于传统的ekf算法。
35.虽然结合附图对发明的具体实施方式进行了详细地描述，但不应理解为对本专利的保护范围的限定。在权利要求书所描述的范围内，本领域技术人员不经创造性劳动即可做出的各种修改和变形仍属本专利的保护范围。