CN117346768B - 同时适用于室内外的多传感器融合感知定位方法 - Google Patents

同时适用于室内外的多传感器融合感知定位方法 Download PDF

Info

Publication number
CN117346768B
CN117346768B CN202311455852.5A CN202311455852A CN117346768B CN 117346768 B CN117346768 B CN 117346768B CN 202311455852 A CN202311455852 A CN 202311455852A CN 117346768 B CN117346768 B CN 117346768B
Authority
CN
China
Prior art keywords
pose
predicted
representing
sensor
observation
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.)
Active
Application number
CN202311455852.5A
Other languages
English (en)
Other versions
CN117346768A (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.)
Kunming University of Science and Technology
Original Assignee
Kunming University of Science and Technology
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 Kunming University of Science and Technology filed Critical Kunming University of Science and Technology
Priority to CN202311455852.5A priority Critical patent/CN117346768B/zh
Publication of CN117346768A publication Critical patent/CN117346768A/zh
Application granted granted Critical
Publication of CN117346768B publication Critical patent/CN117346768B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • 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/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • 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)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明公开同时适用于室内外的多传感器融合感知定位方法，属于融合感知定位技术领域。本发明首先根据车辆或机器人初始位置进行位姿初始化，车辆或机器进行运动后，根据位姿传感器和里程计解算的结果进行卡尔曼滤波得到融合后的预测位姿；之后根据车辆或机器人所处的环境，判断3D激光雷达和全球导航卫星系统的观测数据是否可用，如果可用，计算出对应的观测位姿，并通过卡尔曼滤波计算出融合后的观测位姿；将得到的融合后的预测位姿与融合后的观测位姿进行卡尔曼滤波融合，得到最终位姿。该方法可同时适用于室内外场景，通过卡尔曼滤波让多种定位方法相关纠正，实现较高的定位精度。

Description

同时适用于室内外的多传感器融合感知定位方法
技术领域
本发明属于融合感知定位技术领域，具体涉及同时适用于室内外的多传感器融合感知定位方法。
背景技术
定位技术是用于确定物体或机器人在空间中位姿的技术。定位技术是机器人和自动驾驶汽车实现自主导航、路径规划、位置查找等功能的重要一环。为了提高定位效率与定位精度，室内外定位功能同时覆盖成为定位技术领域迫切解决的问题。
为了实现室内外精确定位，人们在不同角度做出了大量努力。室内外定位方法目前有：(1)基于双目相机、2D激光雷达、超声测距传感器、位姿传感器的融合感知定位。该方法在室内定位效果良好，但是在室外大场景或者空旷场景进行定位时，容易出现2D激光雷达和相机丢定位的情况。(2)基于GPS和激光slam技术的融合感知定位。该方法在室外时，若GPS有信号，则只采用RTK实时差分定位，若机器人或自动驾驶车辆速度突然变化过大，会对RTK的定位精度造成影响。(3)基于室内无线信号RSSI值和室外卫星定位数据的融合感知定位；该方法实现室内外定位，但是得到的定位数据单一且存在信号中断导致丢定位的情况，精确度不高。
尽管做出了这些努力，但室内外定位精度的提升还任重道远。因此同时提高室内外定位精度是亟需解决的问题。
发明内容
针对上述现有技术的缺点，本发明提出同时适用于室内外的多传感器融合感知定位方法。
一种同时适用于室内外的多传感器融合感知定位方法，包括如下步骤：
(1)位姿初始化和卡尔曼滤波器初始化；
(2)车辆或机器进行运动后，对位姿传感器进行惯性导航解算，计算出基于位姿传感器预测的位姿；对里程计进行惯性导航解算，计算出基于里程计预测的位姿；
(3)将基于位姿传感器的预测位姿和基于里程计的预测位姿，进行卡尔曼滤波的融合，计算出融合后的预测位姿；
(4)根据融合后的预测位姿，判断该预测状态下3D激光雷达和全球导航卫星系统的观测数据是否可用，并将室内外观测状态划分为四种情况，计算出3D激光雷达和全球导航卫星系统对应的观测位姿；
(5)根据3D激光雷达和全球导航卫星系统对应的观测位姿，将其进行卡尔曼滤波融合，计算出融合后的观测位姿；
(6)根据融合后的预测位姿和融合后的观测位姿，再次通过卡尔曼滤波，计算出最终位姿，根据最终位姿对位姿传感器和里程计进行校准。
作为本发明的优选实施方案，所述位姿初始化和卡尔曼滤波器初始化具体包括如下步骤：
S1-1：3D激光雷达在点云地图中实现初始定位，并将初始位姿赋值，进行位姿初始化：
初始时刻位置；
初始时刻速度；
初始时刻姿态。
S1-2：卡尔曼滤波器初始化包括状态量初始化、协方差初始化、过程噪声与观测噪声初始化，具体赋值如下：
状态量初始化：
初始化状态量中包括初始姿态/>初始速度/>和初始位置/>
协方差初始化：
Pδp表示初始位置方差，Pδv表示初始速度方差，Pδθ表示初始姿态方差，表示加速度计的零偏不确定性方差，/>表示陀螺仪的零偏不确定性方差。
过程噪声与观测噪声初始化：
Qa代表加速度计的噪声，Qw代表陀螺仪的噪声，和Qb分别代表加速度计的噪声协方差矩阵和陀螺仪的噪声协方差矩阵，Rδp表示位置测量噪声，Rδθ表示姿态测量噪声，Q表示过程噪声，R0表示观测噪声，过程噪声与观测噪声一般在卡尔曼滤波迭代过程中保持不变。
作为本发明的优选实施方案，所述步骤(2)中，计算出基于位姿传感器预测的位姿具体包括：
S2-1：根据位姿传感器测量到的围绕位姿传感器三个轴的加速度与角速度，计算出基于位姿传感器的预测姿态：
其中，
Riwb＝(Rx(α)Ry(-β)Rz(γ))T；
表示当前基于位姿传感器的预测姿态，/>表示上一时刻后验姿态，Riwb表示位姿传感器旋转矩阵，α表示横滚角，β表示俯仰角，γ表示航向角；Rx(α)表示围绕x轴的旋转矩阵，Ry(-β)表示围绕y轴的旋转矩阵，Rz(γ)表示围绕z轴的旋转矩阵，T表示矩阵的转秩符号。
S2-2：根据位姿传感器的测量到的加速度，计算出基于位姿传感器的预测速度：
表示当前基于位姿传感器的预测速度，/>表示上一时刻后验速度，/>表示k时刻的先验姿态，/>表示上一时刻的后验姿态，aik表示k时刻的加速度，ai(k-1)表示上一时刻的加速度，g表示重力加速度，tk表示当前时间、tk-1表示上一时刻的时间。
S2-3：根据基于位姿传感器的预测速度和位姿传感器测量得到的加速度，计算出基于位姿传感器的预测位置：
表示上一时刻的后验位置，/>表示当前基于位姿传感器的预测位置。
S2-4：根据基于位姿传感器的预测姿态、预测速度、预测位置公式，得到位姿传感器的预测位姿为：
表示当前基于位姿传感器的预测位姿。
作为本发明的优选实施方案，所述步骤(2)中，计算出里程计预测的位姿具体包括：
S1：根据里程计测量到的加速度，通过加速度对时间积分，计算出基于里程计的预测速度：
表示当前的预测速度，/>表示上一时刻的后验速度，aok表示里程计测量到的加速度，g表示重力加速度，(tk-tk-1)表示时间差。
S2：根据当前的预测速度，通过速度对时间积分，计算出基于里程计的预测位置：
表示当前的预测位置，/>表示上一时刻的后验位置，/>表示此时的先验速度。
S3：根据基于里程计的预测位置，计算基于里程计的预测姿态：
表示上一时刻姿态，/>表示当前的预测姿态，Rowb表示里程计的旋转矩阵，/>其中γo为里程计航向角，其计算过程如下：其中/>则出航向角为： 表示基于里程计当前时刻预测的位置相对于上一时刻预测位姿的变化量，/>表示基于里程计当前预测的位置相对于上一时刻预测位置在y轴的变化量，/>表示基于里程计当前预测的位置相对于上一时刻预测位置在x轴的变化量；
S4：根据基于里程计的预测速度、预测位置、预测姿态，因此里程计的预测位姿为：
作为本发明的优选实施方案，所述步骤(3)中，融合后的预测位姿为：
其中，表示融合后的预测位姿，Kk表示卡尔曼滤波增益，/>表示基于位姿传感器的预测位姿，/>表示基于里程计的预测位姿，/>表示基于位姿传感器的位姿估计值与初始位姿的差值，
表示融合后的预测姿态，/>表示融合后的预测速度，/>表示融合后的预测位置。
作为本发明的优选实施方案，所述步骤(4)中，分别判断3D激光雷达和全球导航卫星系统的观测数据是否可用，具体包括：
如果观测传感器测量到的位置与步骤(3)得到的融合后的预测位置直线距离相差大于3米，视为观测数据不可用；若观测传感器测量到的位置与步骤(3)得到的融合后的预测位置直线距离相差小于或等于3米，视为观测数据可用。
作为本发明的优选实施方案，所述步骤(4)中，四种情况分别为：
情况1：3D激光雷达观测数据可用，全球导航卫星系统观测数据也可用时，计算3D激光雷达对应的观测位姿ylk：
Gk表示噪声增益矩阵，表示融合后的预测位姿与上一时刻最终位姿的差值，Clknlk表示3D激光雷达观测偏差，/>表示3D激光雷达对应的观测位姿；
计算全球导航卫星系统对应的观测位姿ygk：
Hgk表示观测矩阵，表示融合后的预测位姿，Cgkngk表示全球导航卫星系统观测偏差，ygk表示全球导航卫星系统对应的观测位姿；
情况2：3D激光雷达观测数据可用，全球导航卫星系统观测数据不可用时，
ygk＝0；
情况3：3D激光雷达观测数据不可用，全球导航卫星系统观测数据可用时，
ylk＝0；
情况4：3D激光雷达观测数据不可用，全球导航卫星系统观测数据也不可用时，
ylk＝0；
ygk＝0。
作为本发明的优选实施方案，所述步骤(5)中，融合后的观测位姿的表达式具体如下：
zk＝ylk+Kk(yak-g*ylk，0))；
其中，zk表示融合后的观测位姿，ylk表示3D激光雷达对应的观测位姿，ygk表示全球导航卫星系统对应的观测位姿，Kk表示卡尔曼滤波增益，g(ylk，0)表示基于3D激光雷达的位姿观测值与初始位姿的差值。
作为本发明的优选实施方案，所述步骤(6)中：最终位姿的表达式具体如下：
其中，表示最终位姿，/>表示融合后的预测位姿，zk表示融合后的观测位姿，K表示卡尔曼滤波增益，Ck表示观测矩阵，/>表示最终得到的姿态，/>表示最终得到的速度，表示最终得到的位置；
最终输出最终位姿，即输出
与现有技术相比，本发明的有益效果为：本发明在室内外定位过程中，使用多个传感器进行融合感知定位，参与融合的定位数据包含了基于3D激光雷达提供的相对定位、全球导航卫星系统提供的绝对定位、位姿传感器和里程计经过导航解算的惯性定位，将多种定位方法进行相互纠正，达到更加精确的定位效果。
附图说明
图1为本发明所述同时适用于室内外的多传感器融合感知定位方法的流程图。
图2为本发明同时适用于室内外的多传感器融合感知定位原理图。
具体实施方式
为更好地说明本发明的目的、技术方案和优点，下面将结合具体实施例对本发明作进一步说明。
实施例1
一种同时适用于室内外的多传感器融合感知定位方法，实现流程如图1所示。本发明以一辆无人物流车进行室内外多传感器融合感知定位为例，包括以下步骤：
(1)对无人物流车进行位姿初始化和对卡尔曼滤波器初始化，具体包括：
S1-1：无人物流车位姿初始化：
3D激光雷达在点云地图中实现初始定位，并给初始位姿赋值，进行位姿初始化：
初始时刻位置；
初始时刻速度；
初始时刻姿态。
S1-2：卡尔曼滤波器初始化包括状态量初始化、协方差初始化、过程噪声与观测噪声初始化，具体赋值如下：
状态量初始化：
初始化状态量中包括初始姿态/>初始速度/>和初始位置/>
协方差初始化：
Pδp表示初始位置方差，Pδv表示初始速度方差，Pδθ表示初始姿态方差，表示加速度计的零偏不确定性方差，/>表示陀螺仪的零偏不确定性方差。
过程噪声与观测噪声初始化：
Qa代表加速度计的噪声，Qω代表陀螺仪的噪声，和Qb分别代表加速度计的噪声协方差矩阵和陀螺仪的噪声协方差矩阵，Rδp表示位置测量噪声，Rδθ表示姿态测量噪声，Q表示过程噪声，R0表示观测噪声，过程噪声与观测噪声一般在卡尔曼滤波迭代过程中保持不变。
(2)无人物流车进行运动后，对位姿传感器和里程计数据进行坐标转换，之后分别进行惯性导航解算的表达式具体如下：
S2-1：根据位姿传感器测量到的围绕三个轴的加速度与角速度，计算出基于位姿传感器的预测姿态：
其中，
Riwb＝(Rx(α)Ry(-β)Rz(γ))T；
表示当前基于位姿传感器的预测姿态，/>表示上一时刻后验姿态，Riwb表示位姿传感器旋转矩阵，α表示横滚角，β表示俯仰角，γ表示航向角；Rx(α)表示围绕x轴的旋转矩阵，Ry(-β)表示围绕y轴的旋转矩阵，Rz(γ)表示围绕z轴的旋转矩阵，T表示矩阵的转秩符号。
S2-2：根据位姿传感器的测量到的加速度，计算出基于位姿传感器的预测速度：
表示当前基于位姿传感器预的测速度，/>表示上一时刻后验速度，/>表示k时刻的先验姿态，/>表示上一时刻的后验姿态，aik表示k时刻的加速度，ai(k-1)表示上一时刻的加速度，g表示重力加速度，tk表示当前时间、tk-1表示上一时刻的时间。
S2-3：根据基于位姿传感器的预测速度和位姿传感器测量得到的加速度，计算出基于位姿传感器的预测位置：
表示上一时刻的后验位置，/>表示当前基于位姿传感器的预测位置。
S2-4：根据基于位姿传感器的预测姿态、预测速度、预测位置公式，得到位姿传感器的预测位姿为：
其中为图2中的姿态1，/>为图2中的速度1，/>为图2中的位置1。
S2-5：根据在无人物流车的运动学约束和高精地图的道路约束下里程计测量到的加速度，通过加速度对时间积分，计算出基于里程计的预测速度：
表示当前的预测速度，/>表示上一时刻的后验速度，aok表示里程计测量到的加速度，g表示重力加速度，(tk-tk-1)表示时间差；
S2-6：根据当前的预测速度，通过速度对时间积分，计算出基于里程计的预测位置：
表示当前的预测位置，/>表示上一时刻的后验位置，/>表示此时的先验速度；
S2-7：根据基于里程计的预测位置，计算基于里程计的预测姿态：
里程计只要是针对航向角进行预测，里程计航向角γo计算如下：
其中/>则可计算出航向角：
里程计的旋转矩阵为
因此计算出里程计姿态为：
表示上一时刻姿态，/>表示当前的预测姿态，Rowb表示里程计的旋转矩阵。
S2-8：根据基于里程计的预测速度、预测位置、预测姿态，因此里程计的预测位姿为：
其中为图2中的姿态2，/>为图2中的速度2，/>为图2中的位置2。
(3)将得到的基于位姿传感器的预测位姿和基于里程计的预测位姿，进行基于卡尔曼滤波的融合，得到融合后的预测位姿，具体表达式如下：
其中，表示融合后的预测位姿，Kk表示卡尔曼滤波增益，/>表示基于位姿传感器的预测位姿，/>表示基于里程计的预测位姿，/>表示基于位姿传感器的位姿估计值与初始位姿的差值，/>表示融合后的预测姿态，/>表示融合后的预测速度，/>表示融合后的预测位置。
其中为图2中的姿态3，/>为图2中的速度3，/>为图2中的位置3。
(4)根据步骤(3)得到的融合后的预测位姿，判断该预测状态下3D激光雷达和全球导航卫星系统的观测数据是否可用，并将室内外观测状态划分为四种情况，计算3D激光雷达和全球导航卫星系统对应的观测位姿，具体表达式如下：
S4-1：根据车辆所处的环境，分别判断3D激光雷达和全球导航卫星系统的观测数据是否可用：3D激光雷达通过与点云地图进行点云特征匹配得到相对定位位姿，全球导航卫星系统进行决定定位提供绝对定位位姿，其中基于3D激光雷达和全球导航卫星系统测量到的位姿中的位置信息，如果观测传感器测量到的位置与步骤(3)得到的融合后的预测位置直线距离相差大于3米，即观测传感器观测置信度低，视为观测数据不可用，不可用的观测数据进入NA模块进行删除；若观测传感器测量到的位置与步骤(3)得到的融合后的预测位置直线距离相差小于或等于3米，即观测传感器置信度足够，视为观测数据可用。
S4-2：根据S4-1的方法判断观测数据是否可用，分别根据3D激光雷达和全球导航卫星系统的观测数据的可用情况，将室内外观测状态划分为四种情况，四种情况具体包括：
情况1：3D激光雷达观测数据可用，全球导航卫星系统观测数据也可用时，该情况主要出现在室外定位的时候，此时，计算3D激光雷达对应的观测位姿ylk：
Gk表示噪声增益矩阵，表示融合后的预测位姿与上一时刻最终位姿的差值，Clknlk表示3D激光雷达观测偏差，/>表示3D激光雷达对应的观测位姿；ylk表示图2中的姿态4、位置4。
计算全球导航卫星系统对应的观测位姿ygk：
Hgk表示观测矩阵，表示融合后的预测位姿，Cgkngk表示全球导航卫星系统观测偏差，ygk表示全球导航卫星系统对应的观测位姿；ygk表示图2中姿态5、速度5和位置5。
情况2：3D激光雷达观测数据可用，全球导航卫星系统观测数据不可用时，该情况主要出现在室内定位或者室外定位时全球导航卫星系统被遮挡的时候，此时：
ygk＝0；
此时图2中输出姿态4、位置4，而姿态5、速度5和位置5为0。
情况3：3D激光雷达观测数据不可用，全球导航卫星系统观测数据可用时，该情况主要出现在室外定位时，外界环境突然变化过大或环境过于空旷导致雷达丢定位的时候，此时：
ylk＝0；
此时图2中输出姿态5、速度5和位置5，而图2中的姿态4、位置4均为0。
情况4：3D激光雷达观测数据不可用，全球导航卫星系统观测数据也不可用时，该情况出现在3D激光雷达和全球导航卫星系统都丢定位的时候，此时：
ylk＝0；
ygk＝0；
此时图2中的姿态4、位置4和姿态5、速度5、位置5均为0。
(5)将得到的3D激光雷达和全球导航卫星系统对应的观测位姿，将其进行卡尔曼滤波融合，计算出融合后的观测位姿，其表达式具体如下：
zk＝ylk+Hk(ygk-g(ylk，0))；
其中，zk表示融合后的观测位姿，ylk表示3D激光雷达对应的观测位姿，ygk表示全球导航卫星系统对应的观测位姿，Kk表示卡尔曼滤波增益，g(ylk，0)表示基于3D激光雷达的位姿观测值与初始位姿的差值。
zk中包含图2中的姿态6、速度6、位置6。
(6)根据融合后的预测位姿与融合后的观测位姿，再次通过卡尔曼滤波，计算出最终位姿，其表达式具体如下：
其中，表示最终位姿，/>表示融合后的预测位姿，zk表示融合后的观测位姿，K表示卡尔曼滤波增益，Ck表示观测矩阵，/>表示最终得到的姿态，/>表示最终得到的速度，表示最终得到的位置；
最终输出无人物流车的最终位姿，即输出图2中的姿态、速度和位置。
与基于双目相机、2D激光雷达、超声测距传感器、位姿传感器的多传感器融合感知定位方法相比：针对室外定位，本发明增加了全球导航卫星系统提供的绝对定位。在室外大场景或者空旷场景导致3D激光雷达丢定位时，还能够通过全球导航卫星系统、位姿传感器和里程计进行多传感器融合实现精确定位。
与基于GPS和激光slam技术的融合感知定位方法相比：针对室外定位，发明除了使用全球导航卫星系统进行绝对定位外，还在此基础上融合了3D激光雷达进行点云特征匹配的相对定位、位姿传感器和里程计进行惯性解算得到的定位。
与基于室内无线信号RSSI值和室外卫星定位数据的融合感知定位方法相比：本发明使用3D激光雷达、位姿传感器、里程计和全球导航卫星系统进行多传感器融合感知定位，在自动驾驶与机器人领域具有更大的实用性。
本发明所述采用同时适用于室内外的多传感器融合感知定位方法进行定位的误差在±5cm，小于上述双目相机、2D激光雷达、超声测距传感器、位姿传感器的多传感器融合感知定位方法、基于GPS和激光slam技术的融合感知定位方法、基于室内无线信号RSSI值和室外卫星定位数据的融合感知定位方法。
最后所应当说明的是，以上实施例仅用以说明本发明的技术方案而非对本发明保护范围的限制，尽管参照较佳实施例对本发明作了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的实质和范围。

Claims (7)

1.一种同时适用于室内外的多传感器融合感知定位方法，其特征在于，包括如下步骤：
(1)位姿初始化和卡尔曼滤波器初始化；
(2)车辆或机器进行运动后，对位姿传感器进行惯性导航解算，计算出基于位姿传感器预测的位姿，具体包括：
S2-1：根据位姿传感器测量到的围绕位姿传感器三个轴的加速度与角速度，计算出基于位姿传感器的预测姿态：
其中，
Riwb＝(Rx(α)Ry(-β)Rz(γ))T；
表示当前基于位姿传感器的预测姿态/>表示上一时刻后验姿态，Riwb表示位姿传感器旋转矩阵，α表示横滚角，β表示俯仰角，γ表示航向角；Rx(α)表示围绕x轴的旋转矩阵，Ry(-β)表示围绕y轴的旋转矩阵，Rz(γ)表示围绕z轴的旋转矩阵，T表示矩阵的转秩符号；
S2-2：根据位姿传感器的测量到的加速度，计算出基于位姿传感器的预测速度：
表示当前基于位姿传感器的预测速度，/>表示上一时刻后验速度，/>表示k时刻的先验姿态，/>表示上一时刻的后验姿态，aik表示k时刻的加速度，ai(k-1)表示上一时刻的加速度，g表示重力加速度，tk表示当前时间、tk-1表示上一时刻的时间；
S2-3：根据基于位姿传感器的预测速度，计算出基于位姿传感器的预测位置：
表示上一时刻的后验位置，/>表示当前基于位姿传感器的预测位置，/>表示融合后的预测速度；
S2-4：根据基于位姿传感器的预测姿态、预测速度、预测位置公式，得到位姿传感器的预测位姿为：
表示当前基于位姿传感器的预测位姿；
然后，对里程计进行惯性导航解算，计算出基于里程计预测的位姿；
(3)将基于位姿传感器的预测位姿和基于里程计的预测位姿，进行卡尔曼滤波的融合，计算出融合后的预测位姿；
(4)根据融合后的预测位姿，分别判断该预测状态下3D激光雷达和全球导航卫星系统的观测数据是否可用，根据3D激光雷达和全球导航卫星系统的观测数据的可用情况，将室内外观测状态划分为四种情况，并分别计算出3D激光雷达和全球导航卫星系统对应的观测位姿；所述四种情况分别为：情况1：3D激光雷达观测数据可用，全球导航卫星系统观测数据也可用时，计算3D激光雷达对应的观测位姿ylk：
Gk表示噪声增益矩阵，表示融合后的预测位姿与上一时刻最终位姿的差值，Clknlk表示3D激光雷达观测偏差，/>表示3D激光雷达对应的观测位姿；
计算全球导航卫星系统对应的观测位姿ygk：
Hgk表示观测矩阵，表示融合后的预测位姿，Cgkngk表示全球导航卫星系统观测偏差，ygk表示全球导航卫星系统对应的观测位姿
情况2：3D激光雷达观测数据可用，全球导航卫星系统观测数据不可用时，
ygk＝0；
情况3：3D激光雷达观测数据不可用，全球导航卫星系统观测数据可用时，
ylk＝0；
情况4：3D激光雷达观测数据不可用，全球导航卫星系统观测数据也不可用时，
ylk＝0；
ygk＝0；
(5)根据3D激光雷达和全球导航卫星系统对应的观测位姿，将其进行卡尔曼滤波融合，计算出融合后的观测位姿；
(6)根据融合后的预测位姿和融合后的观测位姿，再次通过卡尔曼滤波，计算出最终位姿，根据最终位姿对位姿传感器和里程计进行校准。
2.权利要求1所述同时适用于室内外的多传感器融合感知定位方法，其特征在于，所述位姿初始化和卡尔曼滤波器初始化具体包括如下步骤：
S1-1：3D激光雷达在点云地图中实现初始定位，并将初始位姿赋值，进行位姿初始化：
初始时刻位置；
初始时刻速度；
初始时刻姿态；
S1-2：卡尔曼滤波器初始化包括状态量初始化、协方差初始化、过程噪声与观测噪声初始化，具体赋值如下：
状态量初始化：
初始化状态量中包括初始姿态/>初始速度/>和初始位置/>
协方差初始化：
Pδp表示初始位置方差，Pδv表示初始速度方差，Pδθ表示初始姿态方差，表示加速度计的零偏不确定性方差，/>表示陀螺仪的零偏不确定性方差；
过程噪声与观测噪声初始化：
Qa代表加速度计的噪声，Qω代表陀螺仪的噪声，和Qb分别代表加速度计的噪声协方差矩阵和陀螺仪的噪声协方差矩阵，Pδp表示位置测量噪声，Rδθ表示姿态测量噪声，Q表示过程噪声，R0表示观测噪声。
3.如权利要求1所述同时适用于室内外的多传感器融合感知定位方法，其特征在于，所述步骤(2)中，计算出里程计预测的位姿具体包括：
S1：根据里程计测量到的加速度，通过加速度对时间积分，计算出基于里程计的预测速度：
表示当前的预测速度，/>表示上一时刻的后验速度，aok表示里程计测量到的加速度，g表示重力加速度，tk-tk-1表示时间差；
S2：根据当前的预测速度，通过速度对时间积分，计算出基于里程计的预测位置：
表示当前的预测位置，/>表示上一时刻的后验位置，/>表示当前的预测速度；
S3：根据基于里程计的预测位置，计算基于里程计的预测姿态：
表示上一时刻姿态，/>表示当前的预测姿态，Rowb表示里程计的旋转矩阵，其中γo为里程计航向角，其计算过程如下：其中/>则航向角为： 表示基于里程计当前时刻预测的位置相对于上一时刻预测位置的变化量，/>表示基于里程计当前预测的位置相对于上一时刻预测位置在y轴的变化量，/>表示基于里程计当前预测的位置相对于上一时刻预测位置在x轴的变化量；
S4：根据基于里程计的预测速度、预测位置、预测姿态，因此里程计的预测位姿为：
4.如权利要求3所述同时适用于室内外的多传感器融合感知定位方法，其特征在于，所述步骤(3)中，融合后的预测位姿为：
其中，表示融合后的预测位姿，Kk表示卡尔曼滤波增益，/>表示基于位姿传感器的预测位姿，/>表示基于里程计的预测位姿，/>表示基于位姿传感器的位姿估计值与初始位姿的差值，/>表示融合后的预测姿态，/>表示融合后的预测速度，/>表示融合后的预测位置。
5.如权利要求1所述同时适用于室内外的多传感器融合感知定位方法，其特征在于，所述步骤(4)中，分别判断3D激光雷达和全球导航卫星系统的观测数据是否可用，具体包括：
3D激光雷达通过与点云地图进行点云特征匹配得到相对定位位姿，全球导航卫星系统进行绝对定位提供绝对定位位姿；如果观测传感器测量到的位置与步骤(3)得到的融合后的预测位置直线距离相差大于3米，视为观测数据不可用；若观测传感器测量到的位置与步骤(3)得到的融合后的预测位置直线距离相差小于或等于3米，视为观测数据可用。
6.权利要求1所述同时适用于室内外的多传感器融合感知定位方法，其特征在于，所述步骤(5)中，融合后的观测位姿的表达式具体如下：
zk＝ylk+Kk(ygk-g(ylk，0))；
其中，zk表示融合后的观测位姿，ylk表示3D激光雷达对应的观测位姿，ygk表示全球导航卫星系统对应的观测位姿，Kk表示卡尔曼滤波增益，g(ylk，0)表示基于3D激光雷达的位姿观测值与初始位姿的差值。
7.如权利要求6所述同时适用于室内外的多传感器融合感知定位方法，其特征在于，所述步骤(6)中：最终位姿的表达式具体如下：
其中，表示最终位姿，/>表示融合后的预测位姿，zk表示融合后的观测位姿，K表示卡尔曼滤波增益，Ck表示观测矩阵，/>表示最终得到的姿态，/>表示最终得到的速度，/>表示最终得到的位置；
最终输出最终位姿，即输出
CN202311455852.5A 2023-11-03 2023-11-03 同时适用于室内外的多传感器融合感知定位方法 Active CN117346768B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311455852.5A CN117346768B (zh) 2023-11-03 2023-11-03 同时适用于室内外的多传感器融合感知定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311455852.5A CN117346768B (zh) 2023-11-03 2023-11-03 同时适用于室内外的多传感器融合感知定位方法

Publications (2)

Publication Number Publication Date
CN117346768A CN117346768A (zh) 2024-01-05
CN117346768B true CN117346768B (zh) 2024-04-19

Family

ID=89367634

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311455852.5A Active CN117346768B (zh) 2023-11-03 2023-11-03 同时适用于室内外的多传感器融合感知定位方法

Country Status (1)

Country Link
CN (1) CN117346768B (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106123890A (zh) * 2016-06-14 2016-11-16 中国科学院合肥物质科学研究院 一种多传感器数据融合的机器人定位方法
CN112444246A (zh) * 2020-11-06 2021-03-05 北京易达恩能科技有限公司 高精度的数字孪生场景中的激光融合定位方法
CN113984044A (zh) * 2021-10-08 2022-01-28 杭州鸿泉物联网技术股份有限公司 一种基于车载多感知融合的车辆位姿获取方法及装置
CN114111775A (zh) * 2021-12-20 2022-03-01 国汽(北京)智能网联汽车研究院有限公司 一种多传感器融合定位方法、装置、存储介质及电子设备
CN114413894A (zh) * 2022-02-24 2022-04-29 山东新一代信息产业技术研究院有限公司 一种多传感器融合的机器人定位的方法
CN115388892A (zh) * 2022-08-22 2022-11-25 盐城工学院 一种基于改进rbpf-slam算法的多传感器融合slam方法
CN116592875A (zh) * 2023-06-02 2023-08-15 长沙理工大学 一种低成本多传感器融合的电缆沟机器人自适应建图方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106123890A (zh) * 2016-06-14 2016-11-16 中国科学院合肥物质科学研究院 一种多传感器数据融合的机器人定位方法
CN112444246A (zh) * 2020-11-06 2021-03-05 北京易达恩能科技有限公司 高精度的数字孪生场景中的激光融合定位方法
CN113984044A (zh) * 2021-10-08 2022-01-28 杭州鸿泉物联网技术股份有限公司 一种基于车载多感知融合的车辆位姿获取方法及装置
CN114111775A (zh) * 2021-12-20 2022-03-01 国汽(北京)智能网联汽车研究院有限公司 一种多传感器融合定位方法、装置、存储介质及电子设备
CN114413894A (zh) * 2022-02-24 2022-04-29 山东新一代信息产业技术研究院有限公司 一种多传感器融合的机器人定位的方法
CN115388892A (zh) * 2022-08-22 2022-11-25 盐城工学院 一种基于改进rbpf-slam算法的多传感器融合slam方法
CN116592875A (zh) * 2023-06-02 2023-08-15 长沙理工大学 一种低成本多传感器融合的电缆沟机器人自适应建图方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
基于多模态感知与融合的无人车韧性导航系统;仲训昱;武东杰;陈登龙;庄明溪;吴汶鸿;导航定位与授时;20211115;第8卷(第06期);第1-12页1-9 *
张胜宾 ; 赵祚喜 ; .多传感器融合的车辆自主定位试验.机电工程技术.2016,第45卷(第08期),第1-3页. *
王随阳 ; 蒋林 ; 雷斌 ; 郭永兴 ; .基于多传感器融合的室内机器人定位研究.组合机床与自动化加工技术.2020,(第10期),第72-75页. *

Also Published As

Publication number Publication date
CN117346768A (zh) 2024-01-05

Similar Documents

Publication Publication Date Title
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN111207774B (zh) 一种用于激光-imu外参标定的方法及系统
CN110706279B (zh) 基于全局地图与多传感器信息融合的全程位姿估计方法
CN109887057B (zh) 生成高精度地图的方法和装置
CN111156998B (zh) 一种基于rgb-d相机与imu信息融合的移动机器人定位方法
CN106767752B (zh) 一种基于偏振信息的组合导航方法
Xiong et al. G-VIDO: A vehicle dynamics and intermittent GNSS-aided visual-inertial state estimator for autonomous driving
CN113252033B (zh) 基于多传感器融合的定位方法、定位系统及机器人
CN112923919B (zh) 基于图优化的行人定位方法及系统
Zhang et al. Vision-aided localization for ground robots
CN112946681B (zh) 融合组合导航信息的激光雷达定位方法
CN109490931A (zh) 飞行定位方法、装置及无人机
CN114018254B (zh) 一种激光雷达与旋转惯导一体化构架与信息融合的slam方法
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
CN117387604A (zh) 基于4d毫米波雷达和imu融合的定位与建图方法及系统
CN117346768B (zh) 同时适用于室内外的多传感器融合感知定位方法
Emran et al. A cascaded approach for quadrotor's attitude estimation
CN116482735A (zh) 一种受限空间内外高精度定位方法
CN116380057B (zh) 一种gnss拒止环境下无人机自主着陆定位方法
Luo et al. An imu/visual odometry integrated navigation method based on measurement model optimization
CN115451958B (zh) 基于相对旋转角度的相机绝对姿态优化方法
Ruan et al. An Unmanned Vehicle Location Algorithm Based on UWb Combined TOF and TDOA
Do et al. An Adaptive Approach based on Multi-State Constraint Kalman Filter for UAVs
Xu et al. Research on Indoor AGV Fusion Localization Based on Adaptive Weight EKF Using Multi-sensor
CN114184194A (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