CN116878497A - 一种神经网络辅助的鲁棒多传感器因子图融合定位方法 - Google Patents
一种神经网络辅助的鲁棒多传感器因子图融合定位方法 Download PDFInfo
- Publication number
- CN116878497A CN116878497A CN202310816699.8A CN202310816699A CN116878497A CN 116878497 A CN116878497 A CN 116878497A CN 202310816699 A CN202310816699 A CN 202310816699A CN 116878497 A CN116878497 A CN 116878497A
- Authority
- CN
- China
- Prior art keywords
- carrier
- sensor
- residual
- gnss
- coordinate system
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 32
- 230000004927 fusion Effects 0.000 title claims abstract description 28
- 238000013528 artificial neural network Methods 0.000 title claims abstract description 27
- 238000005259 measurement Methods 0.000 claims abstract description 24
- 230000010354 integration Effects 0.000 claims description 24
- 230000001133 acceleration Effects 0.000 claims description 12
- 239000011159 matrix material Substances 0.000 claims description 10
- 230000005484 gravity Effects 0.000 claims description 5
- 238000005457 optimization Methods 0.000 claims description 5
- 238000000605 extraction Methods 0.000 claims description 4
- 238000004422 calculation algorithm Methods 0.000 abstract description 5
- 230000000694 effects Effects 0.000 abstract description 4
- 230000002411 adverse Effects 0.000 abstract description 2
- 230000006870 function Effects 0.000 description 11
- 238000013527 convolutional neural network Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000011161 development Methods 0.000 description 3
- 238000007500 overflow downdraw method Methods 0.000 description 3
- 230000008447 perception Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000000470 constituent Substances 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000002955 isolation Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012549 training Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
- G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
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)
- Electromagnetism (AREA)
- Navigation (AREA)
Abstract
本发明公开一种神经网络辅助的鲁棒多传感器因子图融合定位方法。本发明包括以下步骤：首先获取传感器数据集；接着神经网络辅助激光雷达感知模块得到GNSS权重因子，同时建立IMU‑轮速的预积分模型；再构建各传感器对应的量测模型，利用各传感器的量测模型与对应约束关系以及权重因子、IMU‑轮速预积分模型建立传感器残差模型；最后根据传感器残差模型构建待优化的目标函数并求解后，获得载体最终的导航信息。本发明降低了故障时期GNSS所占量测比重。通过IMU‑轮速预积分模型减轻IMU的漂移，减少运算时间，提高算法实时性。利用所有传感器的量测信息并有效降低GNSS故障对导航系统的不良影响，提高导航精度。
Description
技术领域
本发明涉及了车载组合导航相关技术领域的一种定位方法，尤其涉及了一种神经网络辅助的鲁棒多传感器因子图融合定位方法。
背景技术
组合惯导系统通常以惯性导航系统作为主系统，其他导航系统，如天文导航、卫星导航、视觉里程计等，作为辅助导航系统，对组合系统的状态变量进行最优会，获得高精度的导航结果。随着无人机、无人车的发展，多源融合导航技术弥补了单一导航传感器带来的信息不足或信息不可用等缺陷，结合多个传感器信息提供更可靠、更准确、更能适应环境变化的组合导航系统是研究热点。由于传感器自身特性，例如全球导航卫星系统GNSS等外部传感器由于多路径与非视距等因素干扰很容易出现拒止、中断等情况，从而影响最终导航精度。目前多传感器融合技术多基于扩展卡尔曼滤波EKF的方法，在GNSS故障情况下直接剔除该信息完成融合，这样做会舍弃一部分导航信息。另外，基于滤波的方式由于一阶马尔科夫假设只能单步迭代，因此没有充分利用历史信息。因子图由于多次迭代、可重新线性化、利用历史信息等优势在SLAM领域得到广泛验证，但在导航定位领域应用较少，且现有系统多以视觉为中心，惯导在其中的作用并没有得到很好的发挥。IMU由于可自主导航的优势，稳定性高于其他传感器，且随着MEMS-IMU的快速发展，传感器本身精度也有了大幅提升，因此不考虑地球自转与哥氏力的传统多源融合算法浪费了传感器本身的精度。
与EKF一次迭代算法相比，因子图算法利用了历史时刻的多个量测信息同时对状态量进行估计以获得全局最优解。由于其多次迭代和可重新线性化的性质有效提高了系统对于非线性问题的求解准确度，其即插即用的特点也让增删传感器因子变得很方便。但传统因子图模型以IMU更新频率解算非常耗时，若采用层次化因子图的故障检测则建模计算量大。
发明内容
针对车载导航系统，GNSS信号极易受到高楼、林荫、隧道、高架遮挡从而引起定位不准影响最终导航结果，本发明的目的是提供一种实时性高、导航精度高、鲁棒性强的因子图融合方法，以解决现有方法的不足，有效解决由于传感器故障导致的导航精度下降问题与实时性差的问题，本发明所提出的方法可靠性高，可扩展性强。
为实现上述目的，本发明提供如下技术方案：
1)利用载体中所负载的多传感器分别获取位置、三轴角速度、三轴加速度、轮速以及点云信息，记为传感器数据集；
2)对点云信息进行特征提取与匹配后，得到关键帧位姿信息，根据载体所在环境选取目标神经网络，将关键帧位姿信息输入到目标神经网络中，输出获得GNSS故障概率并记为权重因子W；
3)基于三轴角速度和三轴加速度以及轮速，对预积分模型进行地球自转与哥氏力补偿后，获得考虑地球自转与哥氏力的IMU-轮速预积分模型；
4)构建各传感器对应的量测模型，利用各传感器的量测模型与对应约束关系以及权重因子W、考虑地球自转与哥氏力的IMU-轮速预积分模型建立传感器残差模型；
5)根据传感器残差模型构建待优化的目标函数，对待优化的目标函数求解后，获得载体的导航信息，从而实现载体的融合定位。
所述1)中，位置通过全球导航卫星系统获取，三轴角速度和三轴加速度通过惯性测量单元IMU采集获得，轮速通过轮速里程计采集获得，点云信息通过感知模块采集获得。
所述感知模块为激光雷达和毫米波雷达中的一种或多种。
所述考虑地球自转与哥氏力的IMU-轮速预积分模型的公式如下：
其中，分别为k-1时刻和k时刻的旋转四元数，/>表示克罗内克积，/>为k-1时刻的地球自转项，tk为时刻k，/>代表世界坐标系w下的载体速度，/>为世界坐标系w下k-1时刻速度，g为当地重力，Δtk-1，k为k-1到k时刻的时间增量，/>为k时刻世界坐标系w下的载体位置，/>为k-1时刻世界坐标系w下的载体位置，/>分别为速度和位置哥氏项，/>为k-1时刻从载体坐标系b到世界坐标系w的旋转姿态阵，为姿态预积分、速度预积分、位置预积分。
所述传感器残差模型包括先验残差、预积分残差、GNSS残差和LIDAR残差，具体地：
先验残差ep满足ep＝rp-HpX，其中，rp为先验因子残差，Hp为先验因子量测矩阵，X为导航状态量；
预积分残差ePre的公式如下：
其中，rPre()为预积分因子残差，为预积分量测值，T为转置，/>为位置预积分/>的一阶泰勒展开结果，/>为姿态预积分/>的一阶泰勒展开结果，/>为速度预积分/>的一阶泰勒展开结果，2[]v为小角度下的四元数转为的等效旋转矢量；/>分别为k和k-1时刻的陀螺零偏，/>分别为k和k-1时刻的加表零偏，sok，sok-1分别为k和k-1时刻的轮速比例因子；
GNSS残差eGNSS的公式如下：
其中，为天线杆臂，/>为实测经纬高值，W为权重因子，/>代表从世界坐标系w到地球坐标系e的旋转矩阵，rGNSS()为GNSS残差，/>为GNSS量测值，/>为预测的位置；
LIDAR残差eLIDAR的公式如下：
其中，rLIDAR()为激光雷达残差，为激光雷达量测值，/>代表平面特征点；/>代表与平面特征点匹配的并且与平面特征点距离最近的三个点，而且该三个点不在同一面上；||为取绝对值。
所述5)中，根据传感器残差模型构建待优化的目标函数，待优化的目标函数的公式如下：
其中，||||为平方马氏距离，为协方差为/>的平方马氏距离，/>为协方差为/>的平方马氏距离，/>为协方差为/>的平方马氏距离，n为预积分因子个数，m为GNSS定位因子个数，l为激光雷达因子个数。
所述5)中，使用L-M非线性优化方法对待优化的目标函数求解，获得载体的导航信息；不断获取传感器数据集，再基于传感器数据集更新待优化的目标函数并求解，将最新的载体的导航信息作为载体最终的导航信息，实现载体的融合定位。
所述载体的导航信息包括时间、载体位置、载体速度、载体姿态、陀螺零偏与加表零偏和轮速比例因子。
本发明具有的有益效果是：
本发明提出了一种适用于车载的IMU-OD、GNSS、LIDAR多传感器融合的鲁棒因子图方法，通过CNN辅助的激光雷达感知得到GNSS故障的概率作为权重因子调整GNSS量测占比，提高了算法的鲁棒性。通过预积分模型使系统不必在每一IMU量测时刻重新更新，从而降低解算频率，提高实时性，引入NHC约束的轮速模型加入IMU预积分减少IMU漂移，提高解算精度。基于因子图模型将GNSS数据、IMU-OD数据、LIDAR数据有效融合，同时避免了直接隔离故障因子导致的导航信息减少问题。
本发明首先提出了一种适用于车载情况下IMU-OD、GNSS、LIDAR多传感器融合的鲁棒因子图方法，有效解决了现有方法实时性差、计算量大、精度损失的问题。
随着自动驾驶技术的发展，通过如GNSS、LIDAR、惯导等多种类型传感器的数据融合定位成为可能，本发明在传统模型中引入轮速数据，基于NHC约束辅助惯导并通过预积分模型降低计算量。另外，针对GNSS拒止或遮挡情况下，利用激光雷达感知与神经网络辅助智能调整GNSS量测占比，降低故障对导航效果的不良影响。同时该神经网络辅助模型具有很好的泛化能力，本发明中主要针对了高楼、林荫、隧道、高架等场景，还可以继续泛化扩展输入与分类标签以适应更加复杂的环境。
值得指出的是，本发明并不是所有技术方案都能达到上述所有效果。
附图说明
图1为本发明一种神经网络辅助的鲁棒因子图多传感器融合方法整体框架图。
图2为本发明一种神经网络辅助的鲁棒因子图多传感器融合方法步骤流程图。
具体实施方式
下面结合本发明的目的与技术方案，对使用的附图作简单地介绍，并结合具体实施例及相关公式推导，对本发明做进一步详细说明。
如图1所示，本实施例提供了一种神经网络辅助的鲁棒因子图多传感器融合定位方法，整体框架分为数据输入部分、CNN辅助激光雷达获取GNSS故障权重因子部分、IMU-OD预积分部分、因子图构建与优化求解部分以及导航结果输出部分。
其中数据输入部分主要是获取各传感器数据，采集包括三轴陀螺仪与三轴加速度计的IMU数据、采集输出速度的轮速数据、采集输出位置信息的GNSS数据、采集输出点云信息的LIDAR的数据。具体实施中，系统噪声与系统状态量均需要初始化。导航系统状态量包括：载体位置、载体速度、载体姿态、陀螺与加表零偏、轮速比例因子。与传统方法相比，本发明将轮速比例因子也引入导航信息中以实现更全面的模型参数估计。系统噪声根据传感器本身特性选取，载体位置信息载体所在地理位置的经纬高度，初始化为GNSS起始经纬高值；速度信息为东向速度、北向速度、天向速度，初始化为0米/秒；姿态信息为俯仰角、横滚角、航向角，初始化为0。其中IMU输出为增量信息，分别为陀螺增量和加速度增量，轮速只使用前向速度，这点利用了NHC约束，NHC约束是指在载体行驶过程中假定侧向速度和天向速度为0，使用该约束能较好抑制惯导发散。LIDAR点云信息中蕴含着距离和角度信息，该信息经过处理不仅可用于后续位姿融合也能反映运动车体的周围环境信息，如距离某高楼有多远，树木有多高等；
CNN辅助激光雷达获取GNSS故障权重因子部分，主要是利用了上述对于激光雷达点云信息的处理，点云信息经过特征提取与匹配得到位姿信息，通过周围环境感知，分为两类：1.处于林荫、高楼区域；2.处于隧道、高架区域，根据不同的环境分类，位姿信息被输入到两个不同的CNN网络中训练，最终输出GNSS故障概率作为权重因子W；
IMU-OD预积分部分采用补偿地球旋转与哥氏力的模型，较好的利用了惯导的优势，尽最大可能保留惯导的精度。将轮速数据引入预积分中，抑制惯导的漂移，进一步提升导航的精度。
考虑地球旋转与哥氏力的惯导运动模型如下式所示：
其中，表示载体位置，/>表示载体速度，/>表示载体速度的微分，/>表示从载体坐标系b到世界坐标系w的旋转矩阵，fb表示加速度，/>表示姿态微分，/>表示姿态，w表示地理坐标系，e表示地球坐标系，i表示惯性坐标系。g是地理坐标系下的引力向量；/>是世界坐标系w相对于载体坐标系b在载体坐标系b下的投影，/>是w坐标系下的地球自转速率。
轮速模型如下：
其中，，表示实际轮速输出，/>表示i-1到i时刻载体坐标系b的转动，/>表示惯导与轮速间的安装角矩阵，/>为轮速杆臂，/>为载体坐标系b下轮速输出。
当新的IMU测量到来时，根据上述模型离散化结果更新运动状态，但每次量测都要重新线性化，计算量很大。为避免多次重复积分，引入预积分。同时为了抑制惯导漂移将轮速引入预积分，因此考虑考虑地球自转与哥氏力的IMU-轮速预积分模型的公式如下：
其中，分别为k-1时刻和k时刻的旋转四元数，/>表示克罗内克积，/>为k-1时刻的地球自转项，tk为时刻k，/>代表世界坐标系w下的载体速度，/>为世界坐标系w下k-1时刻速度，g为当地重力，Δtk-1，k为k-1到k时刻的时间增量，/>为k时刻世界坐标系w下的载体位置，/>为k-1时刻世界坐标系w下的载体位置，/>分别为速度和位置哥氏项，/>为k-1时刻从载体坐标系b到世界坐标系w的旋转姿态阵，为姿态预积分、速度预积分、位置预积分。
因子图构建与优化部分主要依据各传感器模型与约束关系，构建传感器因子，包括先验因子、预积分因子、GNSS因子、LIDRA因子。在因子图模型建立的时候，按照系统状态，将IMU零偏与轮速比例因子也加入模型之中提高导航精度，根据量测模型与约束关系建立各传感器因子与相应的残差方程。
载体的导航信息包括时间、载体位置、载体速度、载体姿态、陀螺零偏与加表零偏和轮速比例因子。载体位置信息包括载体所在地理位置的经纬高度；载体速度初始化为0m/s；载体姿态表示IMU坐标系到地理坐标系的旋转，用四元数表示。
导航信息表示为：
X＝[x0，x1，...，xn]
其中，w表示地理坐标系，b表示载体坐标系，坐标转化是从下标到上标的转化。xk代表k时刻实时状态，包括位置速度/>姿态/>陀螺零偏ba、加表零偏bg和轮速比例因子so。
传感器残差模型包括先验残差、预积分残差、GNSS残差和LIDAR残差，具体地：
先验残差ep满足ep＝rp-HpX，其中，rp为先验因子残差，Hp为先验因子量测矩阵，X为导航状态量；
预积分残差ePre的公式如下：
其中，rPre()为预积分因子残差，为预积分量测值，T为转置，/>为位置预积分/>的一阶泰勒展开结果，/>为姿态预积分/>的一阶泰勒展开结果，/>为速度预积分/>的一阶泰勒展开结果，2[]v为小角度下的四元数转为的等效旋转矢量；/>分别为k和k-1时刻的陀螺零偏，/>分别为k和k-1时刻的加表零偏，sok，sok-1分别为k和k-1时刻的轮速比例因子；
GNSS残差eGNSS的公式如下：
其中，为天线杆臂，/>为实测经纬高值，W为权重因子，/>代表从世界坐标系w到地球坐标系e的旋转矩阵，rGNSS()为GNSS残差，/>为GNSS量测值，/>为预测的位置；
LIDAR残差eLIDAR的公式如下：
其中，rLIDAR()为激光雷达残差，为激光雷达量测值，/>代表平面特征点；/>代表与平面特征点匹配的并且与平面特征点距离最近的三个点，而且该三个点不在同一面上；/>与/>构成向量ji，/>构成平面法向量/>的点乘，||为取绝对值。
本实施例涉及具体步骤如图2所示，步骤如下：
1)利用载体中所负载的多传感器分别获取位置、三轴角速度、三轴加速度、轮速以及点云信息，记为传感器数据集；具体实施中，载体为车辆。
1)中，位置通过全球导航卫星系统获取，三轴角速度和三轴加速度通过惯性测量单元IMU采集获得，IMU包括三轴陀螺仪与三轴加速度计，分别输出三轴角速度与加速度；轮速通过轮速里程计采集获得。点云信息通过感知模块采集获得，点云信息中蕴含距离角度信息。
感知模块为激光雷达和毫米波雷达中的一种或多种。
2)对点云信息进行特征提取与匹配后，得到关键帧位姿信息，根据载体所在环境选取目标神经网络，具体实施中，不同的环境分别设置有预训练的神经网络，神经网络的输入为关键帧位姿信息，输出为GNSS故障的概率。神经网络具体为CNN。按照环境不同分为两类：1.高楼、林荫场景；2.隧道、高架场景。将关键帧位姿信息输入到目标神经网络中，输出获得GNSS故障概率并记为权重因子W；
3)基于三轴角速度和三轴加速度以及轮速，对预积分模型进行地球自转与哥氏力补偿后，获得考虑地球自转与哥氏力的IMU-轮速预积分模型；角速度和加速度以及速度(即IMU-轮速数据)通过预积分模型得到导航信息，轮速加入预积分可以减少IMU漂移，只加入前向轮速是考虑了非完整性约束(NHC约束)，采用预积分能降低IMU更新频率，提高系统实时性。与此同时，模型考虑了地球自转与哥氏力的补偿，这点对于高精度惯导尤为重要。
4)构建各传感器对应的量测模型，利用各传感器的量测模型与对应约束关系以及权重因子W、考虑地球自转与哥氏力的IMU-轮速预积分模型建立传感器残差模型；
5)根据传感器残差模型构建待优化的目标函数，对待优化的目标函数求解后，获得载体最终的导航信息，从而实现载体的融合定位。
5)具体为：
首先，根据传感器残差模型构建待优化的目标函数，公式如下：
其中，||||为平方马氏距离，为协方差为/>的平方马氏距离，/>为协方差为/>的平方马氏距离，/>为协方差为/>的平方马氏距离，n为预积分因子个数，m为GNSS因子个数，l为激光雷达因子个数；
然后，使用L-M非线性优化方法(Levenberg-Marquardt算法)对待优化的目标函数求解，获得载体的导航信息。接着，不断获取下一时刻传感器数据集，重复执行步骤2)-4)直到结束，得到载体的最终的导航信息，实现载体的融合定位。
以上所述仅为本发明的较佳具体实施方式，但本发明保护范围不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可以轻易想到的变化或替换，都应涵盖在本发明保护范围内。
Claims (8)
1.一种神经网络辅助的鲁棒多传感器因子图融合定位方法，其特征在于，包括以下步骤：
1)利用载体中所负载的多传感器分别获取位置、三轴角速度、三轴加速度、轮速以及点云信息，记为传感器数据集；
2)对点云信息进行特征提取与匹配后，得到关键帧位姿信息，根据载体所在环境选取目标神经网络，将关键帧位姿信息输入到目标神经网络中，输出获得GNSS故障概率并记为权重因子W；
3)基于三轴角速度和三轴加速度以及轮速，对预积分模型进行地球自转与哥氏力补偿后，获得考虑地球自转与哥氏力的IMU-轮速预积分模型；
4)构建各传感器对应的量测模型，利用各传感器的量测模型与对应约束关系以及权重因子W、考虑地球自转与哥氏力的IMU-轮速预积分模型建立传感器残差模型；
5)根据传感器残差模型构建待优化的目标函数，对待优化的目标函数求解后，获得载体的导航信息，从而实现载体的融合定位。
2.根据权利要求1所述的一种神经网络辅助的鲁棒多传感器因子图融合定位方法，其特征在于，所述1)中，位置通过全球导航卫星系统获取，三轴角速度和三轴加速度通过惯性测量单元IMU采集获得，轮速通过轮速里程计采集获得，点云信息通过感知模块采集获得。
3.根据权利要求2所述的一种神经网络辅助的鲁棒多传感器因子图融合定位方法，其特征在于，所述感知模块为激光雷达和毫米波雷达中的一种或多种。
4.根据权利要求1所述的一种神经网络辅助的鲁棒多传感器因子图融合定位方法，其特征在于，所述考虑地球自转与哥氏力的IMU-轮速预积分模型的公式如下：
其中，分别为k-1时刻和k时刻的旋转四元数，/>表示克罗内克积，/>为k-1时刻的地球自转项，tk为时刻k，/>代表世界坐标系w下k时刻的载体速度，/>为世界坐标系w下k-1时刻速度，g为当地重力，Δtk-1，k为k-1到k时刻的时间增量，/>为k时刻世界坐标系w下的载体位置，/>为k-1时刻世界坐标系w下的载体位置，/>分别为速度和位置哥氏项，/>为k-1时刻从载体坐标系b到世界坐标系w的旋转姿态阵，为姿态预积分、速度预积分、位置预积分。
5.根据权利要求1所述的一种神经网络辅助的鲁棒多传感器因子图融合定位方法，其特征在于，所述传感器残差模型包括先验残差、预积分残差、GNSS残差和LIDAR残差，具体地：
先验残差ep满足ep＝rp-HpX，其中，rp为先验因子残差，Hp为先验因子量测矩阵，X为导航状态量；
预积分残差ePre的公式如下：
其中，rPre()为预积分因子残差，为预积分量测值，T为转置，/>为位置预积分/>的一阶泰勒展开结果，/>为姿态预积分/>的一阶泰勒展开结果，/>为速度预积分/>的一阶泰勒展开结果，2[ ]v为小角度下的四元数转为的等效旋转矢量；/>分别为k和k-1时刻的陀螺零偏，/>分别为k和k-1时刻的加表零偏，sok，sok-1分别为k和k-1时刻的轮速比例因子；/>为k-1时刻的旋转四元数，/>表示克罗内克积，为k-1时刻的地球自转项，/>代表世界坐标系w下k-1时刻的载体速度，/>为k时刻世界坐标系w下的载体位置，/>为k-1时刻世界坐标系w下的载体位置，/>和/>分别为世界坐标系w下k和k-1时刻速度，g为当地重力，Δtk-1，k为k-1到k时刻的时间增量，分别为速度和位置哥氏项，/>为k-1时刻从载体坐标系b到世界坐标系w的旋转姿态阵；
GNSS残差eGNSS的公式如下：
其中，为天线杆臂，/>为实测经纬高值，W为权重因子，/>代表从世界坐标系w到地球坐标系e的旋转矩阵，rGNSS( )为GNSS残差，/>为GNSS量测值，/>为预测的位置；
LIDAR残差eLIDAR的公式如下：
其中，rLIDAR( )为激光雷达残差，为激光雷达量测值，/>代表平面特征点；代表与平面特征点匹配的并且与平面特征点距离最近的三个点，而且该三个点不在同一面上；| |为取绝对值。
6.根据权利要求1所述的一种神经网络辅助的鲁棒多传感器因子图融合定位方法，其特征在于，所述5)中，根据传感器残差模型构建待优化的目标函数，待优化的目标函数的公式如下：
其中，rp为先验因子残差，Hp为先验因子量测矩阵，X为导航状态量，|| ||为平方马氏距离，rPre()为预积分因子残差，为预积分量测值，/>为协方差为/>的平方马氏距离，rGNSS( )为GNSS残差，/>为GNSS量测值，/>为协方差为/>的平方马氏距离，rLIDAR( )为激光雷达残差，/>为激光雷达量测值，/>为协方差为/>的平方马氏距离，n为预积分因子个数，m为GNSS定位因子个数，l为激光雷达因子个数。
7.根据权利要求1所述的一种神经网络辅助的鲁棒多传感器因子图融合定位方法，其特征在于，所述5)中，使用L-M非线性优化方法对待优化的目标函数求解，获得载体的导航信息；不断获取下一时刻传感器数据集，再基于传感器数据集更新待优化的目标函数并求解，将最新的载体的导航信息作为载体最终的导航信息，实现载体的融合定位。
8.根据权利要求1所述的一种神经网络辅助的鲁棒多传感器因子图融合定位方法，其特征在于，所述载体的导航信息包括时间、载体位置、载体速度、载体姿态、陀螺零偏与加表零偏和轮速比例因子。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310816699.8A CN116878497A (zh) | 2023-07-05 | 2023-07-05 | 一种神经网络辅助的鲁棒多传感器因子图融合定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310816699.8A CN116878497A (zh) | 2023-07-05 | 2023-07-05 | 一种神经网络辅助的鲁棒多传感器因子图融合定位方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116878497A true CN116878497A (zh) | 2023-10-13 |
Family
ID=88267286
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310816699.8A Pending CN116878497A (zh) | 2023-07-05 | 2023-07-05 | 一种神经网络辅助的鲁棒多传感器因子图融合定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116878497A (zh) |
-
2023
- 2023-07-05 CN CN202310816699.8A patent/CN116878497A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111426318B (zh) | 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法 | |
| CN110243358B (zh) | 多源融合的无人车室内外定位方法及系统 | |
| US11802769B2 (en) | Lane line positioning method and apparatus, and storage medium thereof | |
| CN108362288B (zh) | 一种基于无迹卡尔曼滤波的偏振光slam方法 | |
| CN112505737B (zh) | 一种gnss/ins组合导航方法 | |
| CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
| EP2187170A2 (en) | Method and system for estimation of inertial sensor errors in remote inertial measurement unit | |
| CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
| CN112697138B (zh) | 一种基于因子图优化的仿生偏振同步定位与构图的方法 | |
| CN112146655B (zh) | 一种BeiDou/SINS紧组合导航系统弹性模型设计方法 | |
| CN111189442B (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
| CN109506660B (zh) | 一种用于仿生导航的姿态最优化解算方法 | |
| CN108344413B (zh) | 一种水下滑翔器导航系统及其低精度与高精度转换方法 | |
| CN112197765B (zh) | 一种实现水下机器人精细导航的方法 | |
| CN114019552A (zh) | 一种基于贝叶斯多传感器误差约束的定位置信度优化方法 | |
| CN113503872B (zh) | 一种基于相机与消费级imu融合的低速无人车定位方法 | |
| CN112797985A (zh) | 基于加权扩展卡尔曼滤波的室内定位方法及室内定位系统 | |
| CN116222551A (zh) | 一种融合多种数据的水下导航方法及装置 | |
| CN114018254A (zh) | 一种激光雷达与旋转惯导一体化构架与信息融合的slam方法 | |
| CN111982126B (zh) | 一种全源BeiDou/SINS弹性状态观测器模型设计方法 | |
| CN113008229A (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
| CN116576849A (zh) | 一种基于gmm辅助的车辆融合定位方法及系统 | |
| CN114459474B (zh) | 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法 | |
| CN115542363A (zh) | 一种适用于垂直下视航空吊舱的姿态测量方法 | |
| CN116878497A (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 |