CN115235475A - 一种基于mcc的ekf-slam后端导航路径优化方法 - Google Patents

一种基于mcc的ekf-slam后端导航路径优化方法 Download PDF

Info

Publication number
CN115235475A
CN115235475A CN202211162292.XA CN202211162292A CN115235475A CN 115235475 A CN115235475 A CN 115235475A CN 202211162292 A CN202211162292 A CN 202211162292A CN 115235475 A CN115235475 A CN 115235475A
Authority
CN
China
Prior art keywords
aircraft
coordinate system
moment
world coordinate
ekf
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202211162292.XA
Other languages
English (en)
Other versions
CN115235475B (zh
Inventor
张啟明
孙巧娜
黄超
林萍
胡青云
郭朋飞
马学胜
聂坤宏
胡雨婷
李果村
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chengdu CAIC Electronics Co Ltd
Original Assignee
Chengdu CAIC Electronics Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Chengdu CAIC Electronics Co Ltd filed Critical Chengdu CAIC Electronics Co Ltd
Priority to CN202211162292.XA priority Critical patent/CN115235475B/zh
Publication of CN115235475A publication Critical patent/CN115235475A/zh
Application granted granted Critical
Publication of CN115235475B publication Critical patent/CN115235475B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于MCC的EKF‑SLAM后端导航路径优化方法，包括S1、SLAM前端通过飞行器自身传感器获取飞行器姿态、位置和路标点信息；S2、SLAM后端对获取的飞行器姿态、位置和路标点信息的数据进行融合。相比传统的EKF自主导航方式，本发明提高了数据的鲁棒性。基于MCC的EKF作为SLAM后端可以用于更大规模的数据提取与校正，使得飞行器在面对更为复杂的飞行环境时，具有更高的导航精度、完好性与连续性，可实现在复杂空间环境下面对不同的地域，飞行器巡逻时能够通过自主运动情况和空间环境进行准确的位置姿态参数获取及路径优化。

Description

一种基于MCC的EKF-SLAM后端导航路径优化方法
技术领域
本发明属于飞行器导航的技术领域，具体涉及一种基于MCC的EKF-SLAM后端导航路径优化方法。
背景技术
飞行器在运动的过程中，通常是通过探测其经纬度、方位角、俯仰角、加速度、角速度等信息对飞行器的运动姿态与路径进行计算。这些信息是通过极坐标系转换得到，这使得利用这些数据得到的飞行器运动轨迹存在非线性的数据。现阶段飞行器大多数采用的非自主导航，具有高精度、全时段的特点。但无线电的抗干扰能力差，面对电子对抗技术的快速发展，使得无线电导航在军民飞行器领域存在极大的安全隐患，SLAM（SimultaneousLocalization and Mapping）同步定位与构图，指运动物体根据传感器探测到的数据，一边计算自身的位置，一边构建环境地图的过程。SLAM分为前端与后端，前端通常采用光流法、直接法、特征点法对运动物体进行探测得出数据。后端则用于处理前端数据并建图，其主要分为滤波算法和非线性优化法对前端的数据进行校正与提取。
SLAM后端中非线性优化中光束平差法（BA，Bundle Adjustment）其实时性高，但在优化求解中设计变量过多，计算量较大。卡尔曼滤波（KF, Kalman Filtering）可以在非平稳噪声环境下工作，实现实时数据处理。但是传统的KF为线性系统，因此提出扩展卡尔曼滤波（EKF, Extend Kalman Filtering）算法对非线性系统数据进行处理。当系统的非线性程度很高时，EKF在泰勒展开式中被忽略掉的二阶及以上所带来的误差将导致滤波的发散。但当其与最大相关熵（MCC, Maximum Correntropy Criterion）相结合时，因MCC具有捕获高阶信息的特性，会使得EKF的鲁棒性大幅提升。
MCC主要用于度量两个随机变量
Figure 860503DEST_PATH_IMAGE001
的相似性，假设X，Y间的联合分布函数为
Figure 526977DEST_PATH_IMAGE002
它们间的熵定义
Figure 291670DEST_PATH_IMAGE003
为：
Figure 794327DEST_PATH_IMAGE004
其中，E代表期望因子，
Figure 663188DEST_PATH_IMAGE005
代表Mercer核。
所有的核函数都是由高斯核函数给出的，其表达式如下：
Figure 325114DEST_PATH_IMAGE006
其中
Figure 870496DEST_PATH_IMAGE007
代表核带宽；
Figure 250661DEST_PATH_IMAGE008
为高斯核函数；
Figure 156169DEST_PATH_IMAGE009
为指数函数。
因为在雷达跟踪系统中，可使用的数据是有限的，因此采用样本均值的方式对相关熵
Figure 672601DEST_PATH_IMAGE010
进行估算，具体如下：
Figure 654464DEST_PATH_IMAGE011
其中，
Figure 882445DEST_PATH_IMAGE012
；N为数据总数，i为当前数据，
Figure 467010DEST_PATH_IMAGE013
为第i个高斯核函数，
Figure 978894DEST_PATH_IMAGE014
为第i个x-y的值 ，
Figure 725133DEST_PATH_IMAGE015
为第i个x值 ，
Figure 673367DEST_PATH_IMAGE016
为第i个y值 ；
对高斯核进行泰勒级数展开可得：
Figure 61623DEST_PATH_IMAGE017
其中，n为当前阶数；
由上式可以看出相关熵是X-Y所有偶阶矩的加权和，可以从中提取数据的高阶统计量。
当给定误差数据序列时，基于MCC准则的代价函数
Figure 162434DEST_PATH_IMAGE018
表示为：
Figure 345153DEST_PATH_IMAGE019
。
发明内容
本发明的目的在于针对现有技术中的上述不足，提供一种基于MCC的EKF-SLAM后端导航路径优化方法，以解决复杂空间环境下，面对不同的地域下飞行器巡逻时通过自主运动情况和空间进行准确的位置姿态参数获取以及路径优化的问题。
为达到上述目的，本发明采取的技术方案是：
一种基于MCC的EKF-SLAM后端导航路径优化方法，其包括以下步骤：
S1、SLAM前端通过飞行器自身传感器获取飞行器姿态、位置和路标点信息；
S2、SLAM后端对获取的飞行器姿态、位置和路标点信息的数据进行融合，包括：
S2.1、根据融合后的飞行器姿态、位置和路标点信息的数据，构建状态空间模型；
S2.2、根据激光雷达获取的观测数据，构建量测模型；
S2.3、系统初始化；
S2.4、将步骤S1中飞行器自身传感器获取的数据信息与激光雷达获取到的观测数据通过EKF-MCC算法进行融合，以估计当前飞行器的姿态、速度、位置和路标点信息；
S2.5、对飞行器巡视的系统信息进行更新。
进一步地，步骤S1中位置和路标点为系统状态，以飞行器的起始位置作为其世界坐标系w系的原点；以正北方向作为世界坐标系x轴的正向，以正西方向作为世界坐标系y轴的正向，同时根据右手准则确定世界坐标系z轴的正向；载体坐标系b系的原点为飞机机体正中心，平行于机身纵轴指向机头方向为载体坐标系x轴的正向，平行于机身横轴指向左方的为载体坐标系y轴的正向，同时根据右手准则确定载体坐标系z轴的正向。
进一步地，步骤S2.1中构建的状态空间模型，其在世界坐标系下表示为：
Figure 547727DEST_PATH_IMAGE020
其中，
Figure 474095DEST_PATH_IMAGE021
为k时刻系统的状态，
Figure 694992DEST_PATH_IMAGE022
为k-1时刻系统的状态，
Figure 314192DEST_PATH_IMAGE023
为控制向量，即为传感器获得的加速度和角速度；
Figure 237017DEST_PATH_IMAGE024
为高斯白噪声组成的系统噪声，其构成协方差为
Figure 701497DEST_PATH_IMAGE025
；
Figure 776900DEST_PATH_IMAGE026
Figure 567001DEST_PATH_IMAGE022
由矩阵
Figure 478588DEST_PATH_IMAGE027
构成；
Figure 746758DEST_PATH_IMAGE028
为k-1时刻飞行器在世界坐标系下的经纬高；
Figure 942247DEST_PATH_IMAGE029
为k-1时刻飞行器在世界坐标系下的速度由x,y,z三个轴向速度构成；
Figure 762305DEST_PATH_IMAGE030
为k-1时刻飞行器的姿态，其由俯仰，横滚，偏航三个参数够成；
Figure 535089DEST_PATH_IMAGE031
为世界坐标系下的路标点坐标；
Figure 482316DEST_PATH_IMAGE032
为k-1时刻下载体坐标系与世界坐标系间的转换矩阵；
Figure 391366DEST_PATH_IMAGE033
为k-1时刻载体坐标系下的比力加速度；
Figure 149369DEST_PATH_IMAGE034
为k-1时刻世界坐标系下的重力加速度；
Figure 143870DEST_PATH_IMAGE035
代表采样时间间隔；
Figure 894788DEST_PATH_IMAGE036
为k-1时刻载体坐标系下的角速度。
进一步地，步骤S2.2中构建的量测模型为：
Figure 189503DEST_PATH_IMAGE037
其中，
Figure 351363DEST_PATH_IMAGE038
为k时刻系统的量测值，
Figure 833160DEST_PATH_IMAGE039
为k时刻激光雷达量测的飞行器质心与第i个路标点间的距离；
Figure 122190DEST_PATH_IMAGE040
为k时刻激光雷达量测的飞行器质心与第i个路标点间的距离俯仰角；
Figure 5832DEST_PATH_IMAGE041
为k时刻激光雷达量测的飞行器质心与第i个路标点间的方位角；
Figure 105638DEST_PATH_IMAGE042
为激光雷达自身量测噪声，设定为高斯白噪声，其构成协方差为
Figure 809151DEST_PATH_IMAGE043
；
Figure 901872DEST_PATH_IMAGE044
为k时刻与路标点构成的量测方程非线性函数。
进一步地，步骤S2.3中系统初始化为：
使用预设的高斯核带宽，构建k+1时刻高斯核函数：
Figure 905600DEST_PATH_IMAGE045
其中，
Figure 409263DEST_PATH_IMAGE046
为k+1时刻的高斯核函数，
Figure 865652DEST_PATH_IMAGE047
为选取的高斯核带宽，
Figure 762064DEST_PATH_IMAGE048
为k+1时刻的迭代参数：
Figure 620299DEST_PATH_IMAGE049
其中，
Figure 61906DEST_PATH_IMAGE050
为k+1时刻状态值的迭代参数，
Figure 740012DEST_PATH_IMAGE051
为k+1时刻观测值的迭代参数，
Figure 174536DEST_PATH_IMAGE052
为世界坐标系下根据k时刻获得的k+1时刻的状态值，
Figure 887277DEST_PATH_IMAGE053
为状态转移矩阵，
Figure 998321DEST_PATH_IMAGE054
为k时刻世界坐标系下的状态估计值，
Figure 429303DEST_PATH_IMAGE055
为k+1时刻系统的量测值，
Figure 401938DEST_PATH_IMAGE056
；
对EKF-MCC进行迭代获得k+1时刻世界坐标系下的状态估计值
Figure 969186DEST_PATH_IMAGE057
及其对应的协方差矩阵
Figure 740877DEST_PATH_IMAGE058
：
Figure 393575DEST_PATH_IMAGE059
其中，
Figure 169902DEST_PATH_IMAGE060
为世界坐标系下k时刻状态估值构成的观测方程，
Figure 857235DEST_PATH_IMAGE061
为世界坐标系下k时刻的误差协方差矩阵，
Figure 310082DEST_PATH_IMAGE062
为k时刻状态转移矩阵的转置，
Figure 450076DEST_PATH_IMAGE063
为量测噪声协方差。
进一步地，步骤S2.4中将步骤S1中飞行器自身传感器获取的数据信息与激光雷达获取到的观测数据通过EKF-MCC算法进行融合，以估计当前飞行器的姿态、速度、位置和路标点信息为：
Figure 764514DEST_PATH_IMAGE064
其中，
Figure 306354DEST_PATH_IMAGE065
为由状态，观测构成的高斯核函数比值，
Figure 431567DEST_PATH_IMAGE066
为误差协方差矩阵的逆；
Figure 58857DEST_PATH_IMAGE067
为卡尔曼增益；
Figure 176986DEST_PATH_IMAGE068
为k时刻下观测矩阵的转置；
Figure 838911DEST_PATH_IMAGE069
为系统噪声协方差的逆。
进一步地，步骤S2.5中对飞行器巡视的系统信息进行更新为：
Figure 633561DEST_PATH_IMAGE070
Figure 482568DEST_PATH_IMAGE071
其中，I为单位矩阵；
Figure 404388DEST_PATH_IMAGE072
为系统噪声协方差；
Figure 920820DEST_PATH_IMAGE073
为卡尔曼增益的转置。
本发明提供的基于MCC的EKF-SLAM后端导航路径优化方法，具有以下有益效果：
本发明基于MCC，并将其作为实现准则，对SLAM后端数据进行EKF优化处理；针对SLAM后端的非线性、非高斯滤波问题，本发明将MCC-EKF算法应用到航空飞行器既定航线巡逻路径修正中；相较于传统的EKF算法只能得到误差二阶项的信息，本发明基于MCC的EKF算法可得到误差二阶项及更高阶的统计量，从而使得系统性能得到极大的改善，降低了飞行器系统在非高斯噪声下严重恶化的影响，提高了飞行器高空飞行中循迹定位的稳定性与可靠性。
本发明相比传统的EKF自主导航方式，本发明提出的算法提高了数据的鲁棒性，基于MCC的EKF算法作为SLAM后端可以用于更大规模的数据提取并校正，使得飞行器在面对更复杂的飞行环境时，具有更高的导航精度、完好性与连续性。
附图说明
图1为本发明系统示意图。
图2为本发明流程图。
图3为本发明EKF-MCC与EKF角度均方根误差值对比图。
图4为本发明EKF-MCC与EKF位置均方根误差值对比图。
具体实施方式
下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
实施例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、对飞行器巡视的系统信息进行更新。
如图1所示，飞行器通过地面路标点及探测器探测数据，进行数据融合SLAM后端MCC-EKF算法，进行下一位置目标循迹定位优化飞行。
实施例2，本实施例为对实施例1步骤进一步的描述，在建立飞行器状态空间模型的同时借助全球定位系统创建观测模型并利用改进后的EKF算法实现路径规划和循迹，与传统EKF相比MCC-EKF极大的提高了飞行器实时定位与建图精度，其具体包括以下步骤：
步骤S1、SLAM前端通过飞行器自身传感器获取飞行器姿态、位置和路标点信息；
位置和路标点为系统状态，以飞行器的起始位置作为其世界坐标系w系的原点；以正北方向作为世界坐标系x轴的正向，以正西方向作为世界坐标系y轴的正向，同时根据右手准则确定世界坐标系z轴的正向；载体坐标系b系的原点为飞机机体正中心，平行于机身纵轴指向机头方向为载体坐标系x轴的正向，平行于机身横轴指向左方的为载体坐标系y轴的正向同时根据右手准则确定载体坐标系z轴的正向。
步骤S2、SLAM后端对获取的飞行器姿态、位置和路标点信息的数据进行融合，包括：
步骤S2.1、根据融合后的飞行器姿态、位置和路标点信息的数据，构建状态空间模型，其在世界坐标系下表示为：
Figure 653415DEST_PATH_IMAGE020
其中，
Figure 865084DEST_PATH_IMAGE021
为k时刻系统的状态，
Figure 449650DEST_PATH_IMAGE022
为k-1时刻系统的状态，
Figure 555009DEST_PATH_IMAGE023
为控制向量，即为传感器获得的加速度和角速度；
Figure 691461DEST_PATH_IMAGE024
为高斯白噪声组成的系统噪声，其构成协方差为
Figure 921585DEST_PATH_IMAGE025
；
Figure 44262DEST_PATH_IMAGE026
Figure 161385DEST_PATH_IMAGE022
由矩阵
Figure 344104DEST_PATH_IMAGE027
构成；
Figure 530366DEST_PATH_IMAGE028
为k-1时刻飞行器在世界坐标系下的经纬高；
Figure 456734DEST_PATH_IMAGE029
为k-1时刻飞行器在世界坐标系下的速度由x,y,z三个轴向速度构成；
Figure 926898DEST_PATH_IMAGE030
为k-1时刻飞行器的姿态，其由俯仰，横滚，偏航三个参数够成；
Figure 546099DEST_PATH_IMAGE031
为世界坐标系下的路标点坐标；
Figure 954077DEST_PATH_IMAGE032
为k-1时刻下载体坐标系与世界坐标系间的转换矩阵；
Figure 684136DEST_PATH_IMAGE033
为k-1时刻载体坐标系下的比力加速度；
Figure 775851DEST_PATH_IMAGE034
为k-1时刻世界坐标系下的重力加速度；
Figure 300373DEST_PATH_IMAGE035
代表采样时间间隔；
Figure 461227DEST_PATH_IMAGE036
为k-1时刻载体坐标系下的角速度。
步骤S2.2、根据激光雷达获取的观测数据，构建量测模型为：
Figure 994977DEST_PATH_IMAGE037
其中，
Figure 174154DEST_PATH_IMAGE038
为k时刻系统的量测值，
Figure 869578DEST_PATH_IMAGE039
为k时刻激光雷达量测的飞行器质心与第i个路标点间的距离；
Figure 517728DEST_PATH_IMAGE040
为k时刻激光雷达量测的飞行器质心与第i个路标点间的距离俯仰角；
Figure 589589DEST_PATH_IMAGE041
为k时刻激光雷达量测的飞行器质心与第i个路标点间的方位角；
Figure 124738DEST_PATH_IMAGE042
为激光雷达自身量测噪声，设定为高斯白噪声，其构成协方差为
Figure 256642DEST_PATH_IMAGE043
；
Figure 392088DEST_PATH_IMAGE044
为k时刻与路标点构成的量测方程非线性函数。
步骤S2.3、系统初始化为：
使用预设的高斯核带宽，构建k+1时刻高斯核函数：
Figure 392274DEST_PATH_IMAGE045
其中，
Figure 155831DEST_PATH_IMAGE046
为k+1时刻的高斯核函数，
Figure 334002DEST_PATH_IMAGE047
为选取的高斯核带宽，
Figure 815799DEST_PATH_IMAGE048
为k+1时刻的迭代参数：
Figure 855562DEST_PATH_IMAGE049
其中，
Figure 4783DEST_PATH_IMAGE050
为k+1时刻状态值的迭代参数，
Figure 353856DEST_PATH_IMAGE051
为k+1时刻观测值的迭代参数，
Figure 322949DEST_PATH_IMAGE052
为世界坐标系下根据k时刻获得的k+1时刻的状态值，
Figure 664938DEST_PATH_IMAGE053
为状态转移矩阵，
Figure 403087DEST_PATH_IMAGE054
为k时刻世界坐标系下的状态估计值，
Figure 923061DEST_PATH_IMAGE055
为k+1时刻系统的量测值，
Figure 379450DEST_PATH_IMAGE056
。
对EKF-MCC进行迭代获得k+1时刻世界坐标系下的状态估计值
Figure 761015DEST_PATH_IMAGE057
及其对应的协方差矩阵
Figure 619250DEST_PATH_IMAGE058
：
Figure 44546DEST_PATH_IMAGE059
其中，
Figure 988231DEST_PATH_IMAGE060
为世界坐标系下k时刻状态估值构成的观测方程，
Figure 672022DEST_PATH_IMAGE061
为世界坐标系下k时刻的误差协方差矩阵，
Figure 384763DEST_PATH_IMAGE062
为k时刻状态转移矩阵的转置，
Figure 246540DEST_PATH_IMAGE063
为量测噪声协方差。
利用获取的信息进行初始化建图。
步骤S2.4，将步骤S1中飞行器自身传感器获取的数据信息与激光雷达获取到的观测数据通过EKF-MCC算法进行融合，以估计当前飞行器的姿态、速度、位置和路标点信息为：
Figure 411942DEST_PATH_IMAGE064
其中，
Figure 412608DEST_PATH_IMAGE065
为由状态，观测构成的高斯核函数比值，
Figure 120801DEST_PATH_IMAGE066
为误差协方差矩阵的逆；
Figure 278113DEST_PATH_IMAGE067
为卡尔曼增益；
Figure 321024DEST_PATH_IMAGE068
为k时刻下观测矩阵的转置；
Figure 221984DEST_PATH_IMAGE069
为系统噪声协方差的逆。
步骤S2.5，对飞行器巡视的系统信息进行更新为：
Figure 784683DEST_PATH_IMAGE070
Figure 112896DEST_PATH_IMAGE071
其中，I为单位矩阵；
Figure 144569DEST_PATH_IMAGE072
为系统噪声协方差；
Figure 459006DEST_PATH_IMAGE073
为卡尔曼增益的转置。
根据以上步骤来判断每次迭代的结果是否在设定范围内，以此摒弃超界值保持算法精度，本实施例方法既提高了EKF在非线性环境下的跟踪性能同时还降低了高斯噪声环境下高阶信息对于算法的干扰，提高了EKF算法作为SLAM后端的鲁棒性和定位精度。
如图3和图4，其显示了本发明中EKF-MCC算法与EKF在相同数据条件下飞行器角度以位置的均方根误差值，其横坐标为时间系数，纵坐标为均方根差值，均方根误差越小表示其性能越好，从滤波结果图中可以看出EKF-MCC算法作为SLAM后端的滤波效果明显优于传统的EKF算法。
虽然结合附图对发明的具体实施方式进行了详细地描述，但不应理解为对本专利的保护范围的限定。在权利要求书所描述的范围内，本领域技术人员不经创造性劳动即可做出的各种修改和变形仍属本专利的保护范围。

Claims (7)

1.一种基于MCC的EKF-SLAM后端导航路径优化方法，其特征包括以下步骤：
S1、SLAM前端通过飞行器自身传感器获取飞行器姿态、位置和路标点信息；
S2、SLAM后端对获取的飞行器姿态、位置和路标点信息的数据进行融合，包括：
S2.1、根据融合后的飞行器姿态、位置和路标点信息的数据，构建状态空间模型；
S2.2、根据激光雷达获取的观测数据，构建量测模型；
S2.3、系统初始化；
S2.4、将步骤S1中飞行器自身传感器获取的数据信息与激光雷达获取到的观测数据通过EKF-MCC算法进行融合，以估计当前飞行器的姿态、速度、位置和路标点信息；
S2.5、对飞行器巡视的系统信息进行更新。
2.根据权利要求1所述的基于MCC的EKF-SLAM后端导航路径优化方法，其特征在于：所述步骤S1中位置和路标点为系统状态，以飞行器的起始位置作为其世界坐标系w系的原点；以正北方向作为世界坐标系x轴的正向，以正西方向作为世界坐标系y轴的正向，同时根据右手准则确定世界坐标系z轴的正向；载体坐标系b系的原点为飞机机体正中心，平行于机身纵轴指向机头方向为载体坐标系x轴的正向，平行于机身横轴指向左方的为载体坐标系y轴的正向，同时根据右手准则确定载体坐标系z轴的正向。
3.根据权利要求2所述的基于MCC的EKF-SLAM后端导航路径优化方法，其特征在于，所述步骤S2.1中构建的状态空间模型，其在世界坐标系下表示为：
Figure 983205DEST_PATH_IMAGE001
其中，
Figure 699489DEST_PATH_IMAGE002
为k时刻系统的状态，
Figure 301371DEST_PATH_IMAGE003
为k-1时刻系统的状态，
Figure 777614DEST_PATH_IMAGE004
为控制向量，即为传感器获得的加速度和角速度；
Figure 148553DEST_PATH_IMAGE005
为高斯白噪声组成的系统噪声，其构成协方差为
Figure 35737DEST_PATH_IMAGE006
；
Figure 249550DEST_PATH_IMAGE007
Figure 637806DEST_PATH_IMAGE003
由矩阵
Figure 738617DEST_PATH_IMAGE008
构成；
Figure 921337DEST_PATH_IMAGE009
为k-1时刻飞行器在世界坐标系下的经纬高；
Figure 123910DEST_PATH_IMAGE010
为k-1时刻飞行器在世界坐标系下的速度由x,y,z三个轴向速度构成；
Figure 50278DEST_PATH_IMAGE011
为k-1时刻飞行器的姿态，其由俯仰，横滚，偏航三个参数够成；
Figure 5596DEST_PATH_IMAGE012
为世界坐标系下的路标点坐标；
Figure 890375DEST_PATH_IMAGE013
为k-1时刻下载体坐标系与世界坐标系间的转换矩阵；
Figure 813201DEST_PATH_IMAGE014
为k-1时刻载体坐标系下的比力加速度；
Figure 543259DEST_PATH_IMAGE015
为k-1时刻世界坐标系下的重力加速度；
Figure 353083DEST_PATH_IMAGE016
代表采样时间间隔；
Figure 143185DEST_PATH_IMAGE017
为k-1时刻载体坐标系下的角速度。
4.根据权利要求3所述的基于MCC的EKF-SLAM后端导航路径优化方法，其特征在于，所述步骤S2.2中构建的量测模型为：
Figure 54771DEST_PATH_IMAGE018
其中，
Figure 322942DEST_PATH_IMAGE019
为k时刻系统的量测值，
Figure 252851DEST_PATH_IMAGE020
为k时刻激光雷达量测的飞行器质心与第i个路标点间的距离；
Figure 213854DEST_PATH_IMAGE021
为k时刻激光雷达量测的飞行器质心与第i个路标点间的距离俯仰角；
Figure 845693DEST_PATH_IMAGE022
为k时刻激光雷达量测的飞行器质心与第i个路标点间的方位角；
Figure 58499DEST_PATH_IMAGE023
为激光雷达自身量测噪声，设定为高斯白噪声，其构成协方差为
Figure 233129DEST_PATH_IMAGE024
；
Figure 725552DEST_PATH_IMAGE025
为k时刻与路标点构成的量测方程非线性函数。
5.根据权利要求4所述的基于MCC的EKF-SLAM后端导航路径优化方法，其特征在于，所述步骤S2.3中系统初始化为：
使用预设的高斯核带宽，构建k+1时刻高斯核函数：
Figure 985632DEST_PATH_IMAGE026
其中，
Figure 330026DEST_PATH_IMAGE027
为k+1时刻的高斯核函数，
Figure 500107DEST_PATH_IMAGE028
为选取的高斯核带宽，
Figure 661967DEST_PATH_IMAGE029
为k+1时刻的迭代参数：
Figure 409343DEST_PATH_IMAGE030
其中，
Figure 698373DEST_PATH_IMAGE031
为k+1时刻状态值的迭代参数，
Figure 582016DEST_PATH_IMAGE032
为k+1时刻观测值的迭代参数，
Figure 681821DEST_PATH_IMAGE033
为世界坐标系下根据k时刻获得的k+1时刻的状态值，
Figure 916493DEST_PATH_IMAGE034
为状态转移矩阵，
Figure 743635DEST_PATH_IMAGE035
为k时刻世界坐标系下的状态估计值，
Figure 747363DEST_PATH_IMAGE036
为k+1时刻系统的量测值，
Figure 251026DEST_PATH_IMAGE037
；
对EKF-MCC进行迭代获得k+1时刻世界坐标系下的状态估计值
Figure 707415DEST_PATH_IMAGE038
及其对应的协方差矩阵
Figure 338247DEST_PATH_IMAGE039
：
Figure 196482DEST_PATH_IMAGE040
其中，
Figure 638090DEST_PATH_IMAGE041
为世界坐标系下k时刻状态估值构成的观测方程，
Figure 581775DEST_PATH_IMAGE042
为世界坐标系下k时刻的误差协方差矩阵，
Figure 16299DEST_PATH_IMAGE043
为k时刻状态转移矩阵的转置，
Figure 853673DEST_PATH_IMAGE044
为量测噪声协方差。
6.根据权利要求5所述的基于MCC的EKF-SLAM后端导航路径优化方法，其特征在于，所述步骤S2.4中将步骤S1中飞行器自身传感器获取的数据信息与激光雷达获取到的观测数据通过EKF-MCC算法进行融合，以估计当前飞行器的姿态、速度、位置和路标点信息为：
Figure 840084DEST_PATH_IMAGE045
其中，
Figure 146432DEST_PATH_IMAGE046
为由状态，观测构成的高斯核函数比值，
Figure 243701DEST_PATH_IMAGE047
为误差协方差矩阵的逆；
Figure 448765DEST_PATH_IMAGE048
为卡尔曼增益；
Figure 606077DEST_PATH_IMAGE049
为k时刻下观测矩阵的转置；
Figure 399721DEST_PATH_IMAGE050
为系统噪声协方差的逆。
7.根据权利要求6所述的基于MCC的EKF-SLAM后端导航路径优化方法，其特征在于，所述步骤S2.5中对飞行器巡视的系统信息进行更新为：
Figure 35102DEST_PATH_IMAGE051
Figure 847069DEST_PATH_IMAGE052
其中，I为单位矩阵；
Figure 175282DEST_PATH_IMAGE053
为系统噪声协方差；
Figure 190642DEST_PATH_IMAGE054
为卡尔曼增益的转置。
CN202211162292.XA 2022-09-23 2022-09-23 一种基于mcc的ekf-slam后端导航路径优化方法 Active CN115235475B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211162292.XA CN115235475B (zh) 2022-09-23 2022-09-23 一种基于mcc的ekf-slam后端导航路径优化方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211162292.XA CN115235475B (zh) 2022-09-23 2022-09-23 一种基于mcc的ekf-slam后端导航路径优化方法

Publications (2)

Publication Number Publication Date
CN115235475A true CN115235475A (zh) 2022-10-25
CN115235475B CN115235475B (zh) 2023-01-03

Family

ID=83667284

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211162292.XA Active CN115235475B (zh) 2022-09-23 2022-09-23 一种基于mcc的ekf-slam后端导航路径优化方法

Country Status (1)

Country Link
CN (1) CN115235475B (zh)

Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20160014713A1 (en) * 2010-09-30 2016-01-14 Echo Ridge Llc System and method for robust navigation and geolocation using measurements of opportunity
CN106197432A (zh) * 2016-08-30 2016-12-07 北京航空航天大学 一种基于FastSLAM算法的无人机着陆方法
US20170212529A1 (en) * 2013-11-27 2017-07-27 The Trustees Of The University Of Pennsylvania Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (mav)
CN107643762A (zh) * 2017-08-07 2018-01-30 中国兵器工业计算机应用技术研究所 自主导航的无人机系统及其导航方法
CN108387236A (zh) * 2018-02-08 2018-08-10 北方工业大学 一种基于扩展卡尔曼滤波的偏振光slam方法
CN109682373A (zh) * 2018-12-28 2019-04-26 中国兵器工业计算机应用技术研究所 一种无人平台的感知系统
CN109974707A (zh) * 2019-03-19 2019-07-05 重庆邮电大学 一种基于改进点云匹配算法的室内移动机器人视觉导航方法
US20210072770A1 (en) * 2019-09-06 2021-03-11 Samsung Electronics Co., Ltd. Multi-sensor-based unmanned aerial vehicle and method for controlling same
US20210110484A1 (en) * 2017-08-16 2021-04-15 Mobileye Vision Technologies Ltd. Navigation Based on Liability Constraints
CN112747750A (zh) * 2020-12-30 2021-05-04 电子科技大学 一种基于单目视觉里程计和imu融合的定位方法
CN112904396A (zh) * 2021-02-03 2021-06-04 深圳亿嘉和科技研发有限公司 一种基于多传感器融合的高精度定位方法及系统
WO2021194590A1 (en) * 2020-03-25 2021-09-30 Intel Corporation Dynamic contextual road occupancy map perception for vulnerable road user safety in intelligent transportation systems
CN113485441A (zh) * 2021-08-03 2021-10-08 国网江苏省电力有限公司泰州供电分公司 结合无人机高精度定位和视觉跟踪技术的配网巡检方法
US20210319568A1 (en) * 2020-04-10 2021-10-14 Brigham Young University Cooperative Aircraft Navigation
CN113660681A (zh) * 2021-05-31 2021-11-16 西北工业大学 一种应用于无人机集群辅助传输的多智能体资源优化方法

Patent Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20160014713A1 (en) * 2010-09-30 2016-01-14 Echo Ridge Llc System and method for robust navigation and geolocation using measurements of opportunity
US20170212529A1 (en) * 2013-11-27 2017-07-27 The Trustees Of The University Of Pennsylvania Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (mav)
CN106197432A (zh) * 2016-08-30 2016-12-07 北京航空航天大学 一种基于FastSLAM算法的无人机着陆方法
CN107643762A (zh) * 2017-08-07 2018-01-30 中国兵器工业计算机应用技术研究所 自主导航的无人机系统及其导航方法
US20210110484A1 (en) * 2017-08-16 2021-04-15 Mobileye Vision Technologies Ltd. Navigation Based on Liability Constraints
CN108387236A (zh) * 2018-02-08 2018-08-10 北方工业大学 一种基于扩展卡尔曼滤波的偏振光slam方法
CN109682373A (zh) * 2018-12-28 2019-04-26 中国兵器工业计算机应用技术研究所 一种无人平台的感知系统
CN109974707A (zh) * 2019-03-19 2019-07-05 重庆邮电大学 一种基于改进点云匹配算法的室内移动机器人视觉导航方法
US20210072770A1 (en) * 2019-09-06 2021-03-11 Samsung Electronics Co., Ltd. Multi-sensor-based unmanned aerial vehicle and method for controlling same
WO2021194590A1 (en) * 2020-03-25 2021-09-30 Intel Corporation Dynamic contextual road occupancy map perception for vulnerable road user safety in intelligent transportation systems
US20210319568A1 (en) * 2020-04-10 2021-10-14 Brigham Young University Cooperative Aircraft Navigation
CN112747750A (zh) * 2020-12-30 2021-05-04 电子科技大学 一种基于单目视觉里程计和imu融合的定位方法
CN112904396A (zh) * 2021-02-03 2021-06-04 深圳亿嘉和科技研发有限公司 一种基于多传感器融合的高精度定位方法及系统
CN113660681A (zh) * 2021-05-31 2021-11-16 西北工业大学 一种应用于无人机集群辅助传输的多智能体资源优化方法
CN113485441A (zh) * 2021-08-03 2021-10-08 国网江苏省电力有限公司泰州供电分公司 结合无人机高精度定位和视觉跟踪技术的配网巡检方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
ASHUTOSH SINGANDHUPE等: "MCC-EKF for Autonomous Car Security", 《2020 FOURTH IEEE INTERNATIONAL CONFERENCE ON ROBOTIC COMPUTING (IRC)》 *
MATHEUS F. REIS等: "On the Use of a Maximum Correntropy Criterion in Kalman Filtering Based Strategies for Robot Localization and Mapping", 《APCA INTERNATIONAL CONFERENCE ON AUTOMATIC CONTROL AND SOFT COMPUTING》 *
孙巧娜: "基于最大相关熵滤波的SLAM后端算法研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
蓝天保: "基于单目视觉与IMU融合的室内机器人定位系统研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
雷碧波: "基于CEKF的SLAM算法研究与分析", 《工业控制计算机》 *

Also Published As

Publication number Publication date
CN115235475B (zh) 2023-01-03

Similar Documents

Publication Publication Date Title
CN111522043B (zh) 一种无人车激光雷达快速重新匹配定位方法
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN111288989B (zh) 一种小型无人机视觉定位方法
CN112346104B (zh) 一种无人机信息融合定位方法
Cai et al. Mobile robot localization using gps, imu and visual odometry
CN110702091A (zh) 一种沿地铁轨道移动机器人的高精度定位方法
CN113124856A (zh) 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN113920198B (zh) 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法
CN111366148A (zh) 适用于机载光电观瞄系统多次观察的目标定位方法
CN113238072B (zh) 一种适用于车载光电平台的运动目标解算方法
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
CN112414413A (zh) 一种基于相对角动量的仅测角机动检测及跟踪方法
CN114018270A (zh) 一种中远程空间非合作目标轨道机动检测方法
CN115930977A (zh) 特征退化场景的定位方法、系统、电子设备和可读存介质
CN115183762A (zh) 一种机场仓库内外建图方法、系统、电子设备及介质
CN113740864B (zh) 基于激光三维点云的探测器软着陆末段自主位姿估计方法
CN114608568A (zh) 一种基于多传感器信息即时融合定位方法
CN113359167A (zh) 一种通过惯性测量参数将gps与激光雷达融合定位的方法
CN115235475B (zh) 一种基于mcc的ekf-slam后端导航路径优化方法
CN117387604A (zh) 基于4d毫米波雷达和imu融合的定位与建图方法及系统
CN114459474B (zh) 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法
CN115014321A (zh) 一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法
Yang Research on key technologies of UAV navigation and positioning system
CN112762960A (zh) 一种飞行器所处风场的在线计算方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant