CN113219506A - 一种多模融合无缝切换的定位方法 - Google Patents

一种多模融合无缝切换的定位方法 Download PDF

Info

Publication number
CN113219506A
CN113219506A CN202110494582.3A CN202110494582A CN113219506A CN 113219506 A CN113219506 A CN 113219506A CN 202110494582 A CN202110494582 A CN 202110494582A CN 113219506 A CN113219506 A CN 113219506A
Authority
CN
China
Prior art keywords
positioning
model
fusion
filtering
probability
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
Application number
CN202110494582.3A
Other languages
English (en)
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.)
Jiangsu Juwei Technology Co ltd
Original Assignee
Jiangsu Juwei Technology 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 Jiangsu Juwei Technology Co ltd filed Critical Jiangsu Juwei Technology Co ltd
Priority to CN202110494582.3A priority Critical patent/CN113219506A/zh
Publication of CN113219506A publication Critical patent/CN113219506A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • 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
    • G01S19/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type
    • 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
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种多模融合无缝切换的定位方法，其步骤包括：S1：构建多模融合定位的多模型集；S2：对多模型进行并行卡尔曼滤波分析，进行多模型集的各子模型的滤波计算；S3：通过导航定位系统阈值判断卫星导航信号的强弱，如果小于阈值，并转入S4；如果大于阈值，输出融合定位数据；S4：基于转移矩阵输入信息交互；S5：基于S4中得到的滤波状态量估计和状态协方差估计初值采用卡尔曼滤波执行模型滤波；S6：进行转移概率更新；S7：无缝定位融合计算输出，输出融合定位数据。实现不同组合模式的自适应调整，使转移矩阵主对角线元素变化准确反映当前模型准确度，提高滤波算法的适应能力和融合定位的精度。

Description

一种多模融合无缝切换的定位方法
技术领域
本发明涉及机器人或自主行走的智能作业装备的导航定位方法，尤其涉及一种多模融合无缝切换的定位方法。
背景技术
自主导航定位系统通过北斗、视觉等传感器的获取环境信息、无人作业位姿信息，为机器人/智能作业装备任务规划、路径规划、自主行走、精准作业提供准确的环境、位置信息。精准定位传感器等技术发展，推动智能行走作业装备广泛应用于家居服务、农业、园林等室内/室外多环境全场景。
传统单一的传感器难以满足室内外混杂的机器人多环境全场景的自主行走的定位需求，北斗、IMU、视觉、激光雷达、UWB等多模融合成为自主导航定位的主要模式和重点方向。机器人或智能作业装备携带了多种传感器，依据不同的环境选择不同的传感器和定位数据，多环境作业的定位模式和算法无缝切换是自主导航定位的关键，现有融合算法对定位模式跳转切换自适应切换能力不足。
相比单一模式定位，两种以上多模融合定位系统多输入增加了信息冗余度，需要进行多重观测信息的分配选择、合理利用提高定位精度。由于非结构化场景复杂环境定位源信息随时跳变、观测噪声时变等原因引起滤波模型噪声与实际输入偏差可能导致估计精度下降甚至误差发散，同时室内外无缝定位面临的实际问题就是机器人等自主作业装备没有人为介入进行模式切换，自适应滤波算法实现不同组合定位模式的跳转是自主作业装备自主定位的理想解决方案，现有的滤波算法对滤波器性能评估方法、观测信息权重选择标准缺乏、观测噪声先验值与实际噪声偏差等原因自适应能力还不能满足应用的需求，精度降低。
因此，有必要提供一种新的多模融合无缝切换的定位方法。
发明内容
为了解决上述问题，本发明的目的在于提供一种多模融合无缝切换的定位方法，以提高了模型准确性和无缝多模定位的精度。
为了实现上述目的，本发明采用了如下技术方案：一种多模融合无缝切换的定位方法，其步骤包括：S1：构建多模融合定位的多模型集；S2：对多模型进行并行卡尔曼滤波分析，以获取各个传感器参数，进行多模型集的各子模型的滤波计算；S3：通过导航定位系统阈值判断卫星导航信号的强弱，如果小于阈值，并转入S4；如果大于阈值，输出融合定位数据；S4：输入S2中各子模型的滤波结果，依据模型交互概率，对导航定位系统以外的定位模型向当前时刻真实模型转移后的混合状态值及方差，完成信息交互；S5：基于S4中得到的滤波状态量估计和状态协方差估计初值采用卡尔曼滤波执行模型滤波；S6：基于有限采样时间内的可观测度计算进行滤波性能评估、计算似然函数、概率修正函数、马尔可夫转移概率，完成模型概率更新；S7：利用更新后的模型概率为权重，对S5中的所有模型状态估值及均方误差加权，进行当前时刻最终状态估值及其协方差计算，输出融合定位数据。
综上所述，由于采用了上述技术方案，本发明的有益效果是：基于对组合系统的特性分析，通过模型跳转时子滤波器可观测都分析判断滤波器噪声与实际噪声逼真度、当前实际情况适应性，并把可观测度指标作为权重调整子滤波器转移概率矩阵，实现不同组合模式的自适应调整，使转移矩阵主对角线元素变化准确反映当前模型准确度，提高滤波算法的适应能力和融合定位的精度。
附图说明
图1为本发明一种多模融合无缝切换的定位方法的流程示意图；
图2为本发明中完成模型概率更新的流程图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
参照图1所示，为本发明的多模融合无缝切换的定位方法的流程示意图。
本发明为一种多模融合无缝切换的定位方法，其包括：
S1：构建多模融合定位的多模型集；多模型集中子模型包括北斗导航系统、视觉、IMU、激光定位模型中任意模型；
具体的，依据智能作业装备非结构化自主行走作业的多场景环境特点确定各个环境的融合定位模式与滤波算法，构建无缝定位多模型集覆盖真实环境。
S2：对多模型进行并行卡尔曼滤波分析，以获取各个传感器参数，进行多模型集的各子模型的滤波计算；
S3：进行导航定位系统状态分析：通过导航定位系统阈值判断卫星导航信号的强弱，如果小于阈值，为室外信号遮挡或室内导航定位系统无效区域，以视觉/IMU、视觉/激光或视觉/UWB/IMU等其他融合模式，并转入S4；如果大于阈值，为户外强信号采用北斗/IMU模式，输出融合定位数据，同时融合视觉或雷达等作为避障传感器；
该实施例中，导航定位系统为北斗导航系统。
S4：基于转移矩阵输入信息交互：输入S2中各子模型的滤波结果，依据模型交互概率，对导航定位系统以外的定位模型向当前时刻真实模型转移后的混合状态值及方差，完成信息交互；
S5：基于输入交互进行模型滤波：基于S4中得到的滤波状态量估计和状态协方差估计初值采用卡尔曼滤波执行模型滤波；
S6：进行转移概率更新：滤波后模型状态估计值方差及估计残差均发生更新，模型与环境的匹配度发生了变化，基于有限采样时间内的可观测度计算进行滤波性能评估、计算似然函数、概率修正函数、马尔可夫转移概率，完成模型概率更新；
图2为本发明完成模型概率更新的流程图。其步骤包括：
S61：获取有限采样时间内的可观测度：
Figure BDA0003053840080000041
其中，
Figure BDA0003053840080000042
Figure BDA0003053840080000043
Figure BDA0003053840080000044
为伪逆矩阵
Figure BDA0003053840080000045
的第i行，O为局部可观测矩阵。
S62：获得似然数；
S63：依据可观测度、似然数，利用概率修正函数进行修正；
S64：依据修正结果，利用马尔可夫转移概率，得到更新后的转移概率矩阵。
S7：无缝定位融合计算输出：利用更新后的模型概率为权重，对S5中的所有模型状态估值及均方误差加权，进行当前时刻最终状态估值及其协方差计算，输出位置参数，获得融合定位数据。
在第二实施例中，还需要对室内/外转换的边缘地带，进行场景环境切换的过渡处理。
当在室内/外转换的边缘地带时，导航定位系统(北斗卫星导航)、视觉等定位方法的误差均较大，无法满足较高的定位精度要求，结合视觉采集特征点的信息进行匹配，获取其视觉位姿估计，并与惯导定位进行数据融合，保障在过渡区域的定位精度，并在信号达到一定的强度阈值之后，进行后续的多定位方法的融合定位。
本发明基于对组合系统的特性分析，通过模型跳转时子滤波器可观测都分析判断滤波器噪声与实际噪声逼真度、当前实际情况适应性，并把可观测度指标作为权重调整子滤波器转移概率矩阵，实现不同组合模式的自适应调整，使转移矩阵主对角线元素变化准确反映当前模型准确度，提高滤波算法的适应能力和融合定位的精度。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (3)

1.一种多模融合无缝切换的定位方法，其特征在于，其步骤包括：
S1：构建多模融合定位的多模型集；
S2：对多模型进行并行卡尔曼滤波分析，以获取各个传感器参数，进行多模型集的各子模型的滤波计算；
S3：通过导航定位系统阈值判断卫星导航信号的强弱，如果小于阈值，并转入S4；如果大于阈值，输出融合定位数据；
S4：输入S2中各子模型的滤波结果，依据模型交互概率，对导航定位系统以外的定位模型向当前时刻真实模型转移后的混合状态值及方差，完成信息交互；
S5：基于S4中得到的滤波状态量估计和状态协方差估计初值采用卡尔曼滤波执行模型滤波；
S6：基于有限采样时间内的可观测度计算进行滤波性能评估、计算似然函数、概率修正函数、马尔可夫转移概率，完成模型概率更新；
S7：利用更新后的模型概率为权重，对S5中的所有模型状态估值及均方误差加权，进行当前时刻最终状态估值及其协方差计算，输出融合定位数据。
2.如权利要求1所述的多模融合无缝切换的定位方法，其特征在于，上述S6步骤中，包括如下步骤：
S61：获取有限采样时间内的可观测度：
Figure FDA0003053840070000011
其中，
Figure FDA0003053840070000012
Figure FDA0003053840070000013
Figure FDA0003053840070000021
为伪逆矩阵
Figure FDA0003053840070000022
的第i行，O为局部可观测矩阵。
S62：获得似然数；
S63：依据可观测度、似然数，利用概率修正函数进行修正；
S64：依据修正结果，利用马尔可夫转移概率，得到更新后的转移概率矩阵。
3.如权利要求1所述的多模融合无缝切换的定位方法，其特征在于，当在室内/外转换的边缘地带时，结合视觉采集特征点的信息进行匹配，获取其视觉位姿估计，并与惯导定位进行数据融合。
CN202110494582.3A 2021-05-07 2021-05-07 一种多模融合无缝切换的定位方法 Pending CN113219506A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110494582.3A CN113219506A (zh) 2021-05-07 2021-05-07 一种多模融合无缝切换的定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110494582.3A CN113219506A (zh) 2021-05-07 2021-05-07 一种多模融合无缝切换的定位方法

Publications (1)

Publication Number Publication Date
CN113219506A true CN113219506A (zh) 2021-08-06

Family

ID=77091392

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110494582.3A Pending CN113219506A (zh) 2021-05-07 2021-05-07 一种多模融合无缝切换的定位方法

Country Status (1)

Country Link
CN (1) CN113219506A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114339595A (zh) * 2021-12-24 2022-04-12 北京理工大学重庆创新中心 一种基于多模型预测的超宽带动态反演定位方法
CN114779307A (zh) * 2022-06-17 2022-07-22 武汉理工大学 面向港区的uwb/ins/gnss的无缝定位方法
CN115932913A (zh) * 2022-11-24 2023-04-07 国网思极位置服务有限公司 一种卫星定位伪距修正方法及装置
CN117031521A (zh) * 2023-10-08 2023-11-10 山东大学 一种室内外无缝环境下的弹性融合定位方法及系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080303714A1 (en) * 2007-05-29 2008-12-11 Ezal Kenan O Compact single-aperture antenna and navigation system
CN109375248A (zh) * 2018-10-15 2019-02-22 北极星云空间技术股份有限公司 一种卡尔曼多模融合定位算法模型及其串行更新的方法
CN109407126A (zh) * 2018-10-15 2019-03-01 北极星云空间技术股份有限公司 一种多模接收机联合定位解算的方法
CN111780755A (zh) * 2020-06-30 2020-10-16 南京理工大学 一种基于因子图和可观测度分析的多源融合导航方法
CN112013842A (zh) * 2020-08-29 2020-12-01 桂林电子科技大学 一种基于图像地磁场和惯性传感器的多模态室内定位方法
CN112577496A (zh) * 2020-11-25 2021-03-30 哈尔滨工程大学 一种基于自适应选权的多源融合定位方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080303714A1 (en) * 2007-05-29 2008-12-11 Ezal Kenan O Compact single-aperture antenna and navigation system
CN109375248A (zh) * 2018-10-15 2019-02-22 北极星云空间技术股份有限公司 一种卡尔曼多模融合定位算法模型及其串行更新的方法
CN109407126A (zh) * 2018-10-15 2019-03-01 北极星云空间技术股份有限公司 一种多模接收机联合定位解算的方法
CN111780755A (zh) * 2020-06-30 2020-10-16 南京理工大学 一种基于因子图和可观测度分析的多源融合导航方法
CN112013842A (zh) * 2020-08-29 2020-12-01 桂林电子科技大学 一种基于图像地磁场和惯性传感器的多模态室内定位方法
CN112577496A (zh) * 2020-11-25 2021-03-30 哈尔滨工程大学 一种基于自适应选权的多源融合定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
文家富;张毅;陈红松;: "基于改进IMM-UKF算法的一种融合航迹推演的红外路标室内定位方法", 重庆邮电大学学报(自然科学版), no. 03, pages 403 - 408 *
江春红, 苏惠敏, 陈哲: "信息融合技术在INS/GPS/TAN/SMN四组合系统中的应用", 信息与控制, no. 06, pages 537 - 542 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114339595A (zh) * 2021-12-24 2022-04-12 北京理工大学重庆创新中心 一种基于多模型预测的超宽带动态反演定位方法
CN114339595B (zh) * 2021-12-24 2023-12-01 北京理工大学重庆创新中心 一种基于多模型预测的超宽带动态反演定位方法
CN114779307A (zh) * 2022-06-17 2022-07-22 武汉理工大学 面向港区的uwb/ins/gnss的无缝定位方法
CN115932913A (zh) * 2022-11-24 2023-04-07 国网思极位置服务有限公司 一种卫星定位伪距修正方法及装置
CN115932913B (zh) * 2022-11-24 2024-03-08 国网思极位置服务有限公司 一种卫星定位伪距修正方法及装置
CN117031521A (zh) * 2023-10-08 2023-11-10 山东大学 一种室内外无缝环境下的弹性融合定位方法及系统
CN117031521B (zh) * 2023-10-08 2024-01-30 山东大学 一种室内外无缝环境下的弹性融合定位方法及系统

Similar Documents

Publication Publication Date Title
CN113219506A (zh) 一种多模融合无缝切换的定位方法
Shen et al. Observability analysis and adaptive information fusion for integrated navigation of unmanned ground vehicles
CN110244715B (zh) 一种基于超宽带技术的多移动机器人高精度协同跟踪方法
CN110849369B (zh) 机器人跟踪方法、装置、设备及计算机可读存储介质
CN110524544A (zh) 一种机械臂运动的控制方法、终端和可读存储介质
CN105467382A (zh) 基于svm的多传感器的目标跟踪数据融合算法及其系统
Lin et al. Intelligent filter-based SLAM for mobile robots with improved localization performance
CN111291471A (zh) 一种基于l1正则无迹变换的约束多模型滤波方法
CN110763245A (zh) 一种基于流式计算的地图创建方法及其系统
Leung et al. An improved weighting strategy for rao-blackwellized probability hypothesis density simultaneous localization and mapping
Césić et al. Moving object tracking employing rigid body motion on matrix Lie groups
CN118020038A (zh) 两轮自平衡机器人
Barrile et al. Self-localization by laser scanner and GPS in automated surveys
CN116907510A (zh) 一种基于物联网技术的智能化运动识别方法
CN115932913B (zh) 一种卫星定位伪距修正方法及装置
CN116977434A (zh) 基于追踪相机的目标行为追踪方法及系统
Skoglar et al. On information measures based on particle mixture for optimal bearings-only tracking
Nilsson et al. A comparison of architectures for track fusion
Kong et al. Hybrid indoor positioning method of BLE and monocular VINS based smartphone
CN114047766A (zh) 面向室内外场景长期应用的移动机器人数据采集系统及方法
Gamallo et al. Combination of a low cost GPS with visual localization based on a previous map for outdoor navigation
Liang et al. Simultaneous Localization and Mapping in a Hybrid Robot and Camera Network System
Lidoris et al. Information-based gaze direction planning algorithm for SLAM
Jiang et al. High precise localization of mobile robot by three times pose correction
EP4198567A1 (en) Method for determining a beam configuration of a lidar system

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