CN112539746B - 基于多频卡尔曼滤波的机器人视觉/ins组合定位方法及系统 - Google Patents

基于多频卡尔曼滤波的机器人视觉/ins组合定位方法及系统 Download PDF

Info

Publication number
CN112539746B
CN112539746B CN202011132559.1A CN202011132559A CN112539746B CN 112539746 B CN112539746 B CN 112539746B CN 202011132559 A CN202011132559 A CN 202011132559A CN 112539746 B CN112539746 B CN 112539746B
Authority
CN
China
Prior art keywords
robot
inertial navigation
navigation system
observation equation
frequency
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
CN202011132559.1A
Other languages
English (en)
Other versions
CN112539746A (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.)
University of Jinan
Original Assignee
University of Jinan
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 University of Jinan filed Critical University of Jinan
Priority to CN202011132559.1A priority Critical patent/CN112539746B/zh
Publication of CN112539746A publication Critical patent/CN112539746A/zh
Application granted granted Critical
Publication of CN112539746B publication Critical patent/CN112539746B/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/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/20—Instruments for performing navigational calculations
    • 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
    • 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)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于多频卡尔曼滤波的机器人视觉/INS组合定位方法及系统，包括：分别通过视觉单元和惯性导航系统获取机器人在东方向和北方向上的位置差，速度差值以及机器人在东方向和北方向速度和位置数据；构建第一观测方程和第二观测方程；通过第二观测方程估计出机器人在东方向和北方向上位置的最优误差值；利用所述最优位置误差值校正惯性导航系统输出的机器人位置，最终得到机器人位置的最优滤波值，实现对机器人的定位。本发明将视觉与惯性导航系统进行组合导航定位，相对于单一的视觉导航系统和单一的惯性导航功能系统，组合导航的定位精度得到提高。

Description

基于多频卡尔曼滤波的机器人视觉/INS组合定位方法及系统
技术领域
本发明涉及复杂环境下组合定位技术领域，尤其涉及一种基于多频卡尔曼滤波的机器人视觉/INS(惯性导航系统)组合定位方法及系统。
背景技术
本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。
随着物联网技术、无线通信技术的应用和发展，基于用户位置信息的相关技术日趋成熟，人们在日常生活与工作环境中对位置服务也提出了更高的要求。如何提高室内定位技术的精确性、稳定性和通用性已成为当今的研究热点。目前，比较成熟的室内导航定位技术有：基于惯性传感器的室内导航定位技术、基于地磁的定位技术、基于二维码的定位技术以及基于视觉传感器的定位导航技术等。
然而，上述单一的导航方法均存在一定的不足，比如：惯性器件会产生随时间累积的导航误差，难以长时间独立工作；视觉导航系统受敏感器作用距离的限制，且导航数据更新频率低。
随着SLAM(即时定位与地图构建)技术的成熟，越来越多的学者和研究人员将惯性测量单元与视觉传感器相结合。视觉单元融合惯性测量单元(Inertial MeasurementUnit,IMU)进行位姿估计的问题，按照是否把图像特征信息加入状态向量来进行分类，可以分为松耦合和紧耦合两大类。松耦合将视觉传感器和IMU作为两个单独的模块，两个模块均可以计算得到机器人位姿信息，然后一般通过卡尔曼滤波器(Kalman Filter,KF)，扩展卡尔曼滤波器(Extended Kalman Filter,EKF)或无迹卡尔曼(Unscented Kalman Filter,UKF)等方法进行融合；紧耦合则是指将视觉和IMU得到的中间数据通过一个优化滤波器进行处理，紧耦合需要把图像特征加入到滤波器要输入的特征向量中，最终得到位姿信息的过程，由于这个原因，系统状态向量最终的维度也会非常高，同时计算量也很大。总体来看，松耦合算法相对简单，且扩展性很强，可以加入多种传感器信息进行融合。
发明人发现，现有技术对于视觉/INS组合进行定位的研究，往往采用单速率滤波方法，传统单速率滤波方法是将滤波周期选取为慢速率采样周期或者采样周期的最小公倍数；这样虽然使系统具有处理多速率问题的能力，但降低了数据更新率，进而可能导致滤波性能的下降。
发明内容
有鉴于此，本发明提出了一种基于多频卡尔曼滤波的机器人视觉/INS组合定位方法及系统，为了提高滤波数据的更新率，把滤波周期选取为IMU的采样周期，在滤波时刻对是否出现慢速率视觉导航量测信息进行判断。
在一些实施方式中，采用如下技术方案：
基于多频卡尔曼滤波的机器人视觉/INS组合定位方法，包括：
分别通过视觉单元和惯性导航系统获取机器人在东方向和北方向上的位置差，速度差值以及机器人在东方向和北方向速度和位置数据；
将惯性导航系统测量得到的机器人在东方向和北方向的位置作为多频卡尔曼滤波器的观测量，构建第一观测方程；将惯性导航系统和视觉单元在东方向和北方向上的机器人位置差作为多频卡尔曼滤波器的观测量，构建第二观测方程；
通过第二观测方程估计出机器人在东方向和北方向上位置的最优误差值；
利用所述最优位置误差值校正惯性导航系统输出的机器人位置，最终得到机器人位置的最优滤波值，实现对机器人的定位。
在另一些实施方式中，采用如下技术方案：
基于多频卡尔曼滤波的机器人视觉/INS组合定位系统，包括：
视觉单元，用于获取机器人在东方向和北方向上的位置差和速度差值；
惯性导航系统，用于获取机器人在东方向和北方向的速度和位置数据；
观测方程构建模块，用于将惯性导航系统测量得到的机器人在东方向和北方向的位置作为多频卡尔曼滤波器的观测量，构建第一观测方程；将惯性导航系统和视觉单元在东方向和北方向上的机器人位置差作为多频卡尔曼滤波器的观测量，构建第二观测方程；
最优误差估计模块，用于通过第二观测方程估计出机器人在东方向和北方向上位置的最优误差值；
机器人定位模块，利用所述最优位置误差值校正惯性导航系统输出的机器人位置，最终得到机器人位置的最优滤波值，实现对机器人的定位。
在另一些实施方式中，采用如下技术方案：
一种终端设备，其包括处理器和计算机可读存储介质，处理器用于实现各指令；计算机可读存储介质用于存储多条指令，所述指令适于由处理器加载并执行上述的基于多频卡尔曼滤波的机器人视觉/INS组合定位方法。
在另一些实施方式中，采用如下技术方案：
一种计算机可读存储介质，其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行上述的基于多频卡尔曼滤波的机器人视觉/INS组合定位方法。
与现有技术相比，本发明的有益效果是：
(1)本发明选取所有导航子系统中最小滤波周期作为滤波周期，多频卡尔曼滤波器始终以最快的采样频率fI进行滤波，使系统在具有处理多速率问题能力的同时，提高了数据的更新率,能够获取更多的有效数据；同时，提高了采样数据的利用率，有助于提高滤波性能，提高机器人的定位精度。
(2)本发明将视觉与惯性导航系统进行组合导航定位，相对于单一的视觉导航系统和单一的惯性导航功能系统，组合导航的定位精度得到提高。
(3)本发明可用于室内环境下的移动机器人的中高精度定位。
附图说明
图1为本发明实施例中机器人视觉/INS组合定位系统示意图；
图2为本发明实施例中基于多频卡尔曼滤波的机器人视觉/INS组合定位方法示意图；
图3为多频卡尔曼滤波算法的实现示意图。
具体实施方式
应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本发明使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
实施例一
在一个或多个实施方式中，公开了一种基于多频卡尔曼滤波的机器人视觉/INS组合定位方法，该方法基于图1所示的机器人视觉/INS组合定位系统实现。具体地，参照图1，机器人视觉/INS组合定位系统包括：分别固定在移动机器人上的视觉相机、电子罗盘以及数据处理单元；其中，视觉相机和电子罗盘分别通过串口RS232与数据处理单元连接。
本实施例中，视觉相机用于测量移动机器人位置和姿态角等相对运动参数；电子罗盘用于测量移动机器人的航向角；数据处理单元用于对采集到的传感器数据进行数据融合。
需要说明的是，IMU是惯性测量单元，用于测量数据；INS是惯性导航系统，对IMU测得的数据进行处理可以得到INS系统下的机器人的位置、速度等信息。
参照图2和图3，本实施例基于多频卡尔曼滤波的机器人视觉/INS组合定位方法，包括如下过程：
以视觉和惯性导航系统在东方向和北方向两个方向的位置差值、速度差值，以及惯导系统在东方向和北方向的位置、速度作为多频卡尔曼滤波器状态向量，构建相应的状态方程；
将视觉相机测量得到的机器人在东方向和北方向的位置和惯性器件测量得到的机器人在东方向和北方向的位置作为滤波器的观测向量进行数据融合，在此基础上，视觉/惯性导航系统分别对相对速度和位置进行测量，将其差值作为量测量输入给多速率卡尔曼滤波器进行最优滤波估计，估计出各种状态量的误差值。用惯性导航系统误差的估计值去校正惯性导航的输出参数，实现输出校正，以得到导航参数的最优滤波值；
具体地，将惯性导航系统测量得到的机器人在东方向和北方向的位置作为多频卡尔曼滤波器的观测量，构建第一观测方程；将惯性导航系统和视觉单元在东方向和北方向上的机器人位置差作为多频卡尔曼滤波器的观测量，构建第二观测方程；
通过第二观测方程估计出机器人在东方向和北方向上位置的最优误差值；
利用所述最优位置误差值校正惯性导航系统输出的机器人位置(惯性导航系统进入滤波器前输出的位置，即图2中的Po I)
最终得到机器人位置的最优滤波值，实现对机器人的定位。
本实施例中，多频卡尔曼滤波器的状态方程为：
Figure GDA0004178869440000061
其中，δPE,k，δPN,k分别为视觉单元和惯性导航系统在东方向位置的差值和北方向位置的差值；δVE,k，δVN,k分别为视觉单元和惯性导航系统东方向速度的差值和北方向速度的差值；
Figure GDA0004178869440000062
分别为k时刻惯性导航系统在东方向和北方向的位置；/>
Figure GDA0004178869440000063
分别为K时刻惯性导航系统在东方向和北方向的速度；T为惯导滤波周期；ωk-1为系统噪声，其协方差阵为Q。
第一观测方程具体为:
Figure GDA0004178869440000071
其中，νk为白色噪声，
Figure GDA0004178869440000072
表示只有INS导航信息输入时，k时刻INS在东方向上机器人位置的观测输出值；/>
Figure GDA0004178869440000073
表示只有INS导航信息输入时，k时刻INS在北方向上机器人位置的观测输出值。
第二观测方程具体为:
Figure GDA0004178869440000074
其中，
Figure GDA0004178869440000075
是机器人在东方向上视觉导航系统和惯性导航系统输出位置差值的观测值，/>
Figure GDA0004178869440000076
是机器人在北方向上视觉导航系统和惯性导航系统输出位置差值的观测值；/>
Figure GDA0004178869440000077
分别为k时刻惯性导航系统测量得到机器人在东方向和北方向的位置；wk为白色噪声，/>
Figure GDA0004178869440000078
分别为分别是k时刻视觉导航系统测量得到机器人在东方向和北方向的位置。
多频卡尔曼滤波器的滤波频率f为惯性导航系统采样频率fI。
本实施例设定相机采样周期为IMU采样周期的整数倍，即IMU的采样周期相较于相机采样周期较小。滤波周期选取系统中较小的采样周期，即选取IMU的采样周期。
通过判断某一时刻是否有视觉信息的输入，确定多频卡尔曼滤波器采用第一观测方程还是第二观测方程。
采样周期为固定的，慢率视觉导航量测信息与快速率惯性导航系统的测量值会按照固定的间隔时间输入。
当频率为IMU采样频率fI时，选择第一量测方程；当频率为视觉单元采样频率fV时，选择第二量测方程；即：在只有惯性导航信息输入时，执行第一观测方程；在两种导航信息都存在时执行第二观测方程；这样可以使用所有的信号进行滤波，提高数据的利用率，提高导航系统的定位精度。
本实施例中，多频卡尔曼滤波器的迭代方程为：
(1)线性卡尔曼滤波器时间更新过程
状态一步预测：
Figure GDA0004178869440000081
一步预测误差方差阵：
Figure GDA0004178869440000082
(2)线性卡尔曼滤波器量测更新过程
滤波增益矩阵：
Figure GDA0004178869440000083
状态估计：
Figure GDA0004178869440000084
估计误差方差阵：
Pk＝[I-KkHk]Pk,k-1 (5)
当没有量测信息输出时只进行时间更新；反之，有量测信息输出时，同时进行卡尔曼滤波器的时间更新和量测更新。
在滤波时刻k时，若没有视觉导航的量测信息输入而只有惯性导航的量测信息，滤波器则认为此时系统出现了任意粗大误差，系统观测噪声方差阵Rk会趋向于无穷大，使得式(3)中的项
Figure GDA0004178869440000091
会趋向于零，则滤波增益矩阵Kk也趋近于零。把式(3)代入式(4)和式(5)，则有：
Figure GDA0004178869440000092
Pk＝Pk,k-1 (7)
在此基础上本文对多频卡尔曼滤波进一步进行了改进,通过判断某一时刻是否有视觉信息的输入，来选择是使用第一观测方程还是使用第二观测方程。当有视觉导航信息输入时，选取第二观测方程，选择东方向上INS、视觉导航系统输出的位置差值和西方向上INS、视觉导航系统输出的位置差值作为观测量，即将视觉输出的导航信息与惯导计算得到的导航信息作差作为系统观测量的方法；当只有惯性导航信息输入时，选取第一观测方程，选取INS输出的位置作为观测量的方法。
记惯性导航解算周期为TINS，视觉导航解算周期为TVision，滤波周期为TFilter，且有:
TFilter＝N×TINS (8)
TVision＝M×TFilter (9)
其中，假设视觉导航解算周期和滤波周期都是惯性导航的整数倍，即N和M都是整数。取N＝1,M＝2～n(当视觉导航解算周期不固定时n为一个不确定的整数值)。
实施例二
在一个或多个实施方式中，公开了一种基于多频卡尔曼滤波的机器人视觉/INS组合定位系统，包括：
视觉单元，用于获取机器人在东方向和北方向上的位置差和速度差值；
惯性导航系统，用于获取机器人在东方向和北方向的速度和位置数据；
观测方程构建模块，用于将惯性导航系统测量得到的机器人在东方向和北方向的位置作为多频卡尔曼滤波器的观测量，构建第一观测方程；将惯性导航系统和视觉单元在东方向和北方向上的机器人位置差作为多频卡尔曼滤波器的观测量，构建第二观测方程；
最优误差估计模块，用于通过第二观测方程估计出机器人在东方向和北方向上位置的最优误差值；
机器人定位模块，利用所述最优位置误差值校正惯性导航系统输出的机器人位置，最终得到机器人位置的最优滤波值，实现对机器人的定位。
需要说明的是，上述各模块的具体实现方式对应实施例一中的实现方式，具体不再赘述。
实施例三
在一个或多个实施方式中，公开了一种终端设备，包括服务器，所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现实施例一中的方法。为了简洁，在此不再赘述。
应理解，本实施例中，处理器可以是中央处理单元CPU，处理器还可以是其他通用处理器、数字信号处理器DSP、专用集成电路ASIC，现成可编程门阵列FPGA或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
存储器可以包括只读存储器和随机存取存储器，并向处理器提供指令和数据、存储器的一部分还可以包括非易失性随机存储器。例如，存储器还可以存储设备类型的信息。
在实现过程中，上述方法的各步骤可以通过处理器中的硬件的集成逻辑电路或者软件形式的指令完成。
实施例一中的方法可以直接体现为硬件处理器执行完成，或者用处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器、闪存、只读存储器、可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器，处理器读取存储器中的信息，结合其硬件完成上述方法的步骤。为避免重复，这里不再详细描述。
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (7)

1.基于多频卡尔曼滤波的机器人视觉/INS组合定位方法，其特征在于，包括：
分别通过视觉单元和惯性导航系统获取机器人在东方向和北方向上的位置差，速度差值以及机器人在东方向和北方向速度和位置数据；
将惯性导航系统测量得到的机器人在东方向和北方向的位置作为多频卡尔曼滤波器的观测量，构建第一观测方程；将惯性导航系统和视觉单元在东方向和北方向上的机器人位置差作为多频卡尔曼滤波器的观测量，构建第二观测方程；
所述多频卡尔曼滤波器的滤波频率为惯性导航系统采样频率；设定相机采样周期为IMU采样周期的整数倍，滤波周期选取系统中较小的采样周期，即选取IMU的采样周期；
通过判断某一时刻是否有视觉信息的输入，确定多频卡尔曼滤波器采用第一观测方程还是第二观测方程，包括：
在只有惯性导航信息输入时，执行第一观测方程；在两种导航信息都存在时执行第二观测方程；
通过第二观测方程估计出机器人在东方向和北方向上位置的最优误差值；
利用所述最优位置误差值校正惯性导航系统输出的机器人位置，最终得到机器人位置的最优滤波值，实现对机器人的定位。
2.如权利要求1所述的基于多频卡尔曼滤波的机器人视觉/INS组合定位方法，其特征在于，所述多频卡尔曼滤波器的状态方程为：
Figure FDA0004178869430000021
其中，δPE,k，δPN,k分别为视觉单元和惯性导航系统在东方向位置的差值和北方向位置的差值；δVE,k，δVN,k分别为视觉单元和惯性导航系统东方向速度的差值和北方向速度的差值；
Figure FDA0004178869430000022
分别为k时刻惯性导航系统在东方向和北方向的位置；/>
Figure FDA0004178869430000023
分别为K时刻惯性导航系统在东方向和北方向的速度；T为惯导滤波周期；ωk-1为系统噪声，其协方差阵为Q。
3.如权利要求1所述的基于多频卡尔曼滤波的机器人视觉/INS组合定位方法，其特征在于，所述第一观测方程具体为:
Figure FDA0004178869430000024
其中，νk为白色噪声，
Figure FDA0004178869430000025
表F示只有INS导航信息输入时，k时刻INS在东方向上机器人位置的观测输出值；/>
Figure FDA0004178869430000026
表F示只有INS导航信息输入时，k时刻INS在北方向上机器人位置的观测输出值。
4.如权利要求1所述的基于多频卡尔曼滤波的机器人视觉/INS组合定位方法，其特征在于，所述第二观测方程具体为:
Figure FDA0004178869430000031
其中，
Figure FDA0004178869430000032
是机器人在东方向上视觉导航系统和惯性导航系统输出位置差值的观测值，/>
Figure FDA0004178869430000033
是机器人在北方向上视觉导航系统和惯性导航系统输出位置差值的观测值；
Figure FDA0004178869430000034
分别为k时刻惯性导航系统测量得到机器人在东方向和北方向的位置；wk为白色噪声，/>
Figure FDA0004178869430000035
Figure FDA0004178869430000036
分别为分别是k时刻视觉导航系统测量得到机器人在东方向和北方向的位置。
5.基于多频卡尔曼滤波的机器人视觉/INS组合定位系统，其特征在于，包括：
视觉单元，用于获取机器人在东方向和北方向上的位置差和速度差值；
惯性导航系统，用于获取机器人在东方向和北方向的速度和位置数据；
观测方程构建模块，用于将惯性导航系统测量得到的机器人在东方向和北方向的位置作为多频卡尔曼滤波器的观测量，构建第一观测方程；将惯性导航系统和视觉单元在东方向和北方向上的机器人位置差作为多频卡尔曼滤波器的观测量，构建第二观测方程；
所述多频卡尔曼滤波器的滤波频率为惯性导航系统采样频率；设定相机采样周期为IMU采样周期的整数倍，滤波周期选取系统中较小的采样周期，即选取IMU的采样周期；
通过判断某一时刻是否有视觉信息的输入，确定多频卡尔曼滤波器采用第一观测方程还是第二观测方程，包括：
在只有惯性导航信息输入时，执行第一观测方程；在两种导航信息都存在时执行第二观测方程；
最优误差估计模块，用于通过第二观测方程估计出机器人在东方向和北方向上位置的最优误差值；
机器人定位模块，利用所述最优位置误差值校正惯性导航系统输出的机器人位置，最终得到机器人位置的最优滤波值，实现对机器人的定位。
6.一种终端设备，其包括处理器和计算机可读存储介质，处理器用于实现各指令；计算机可读存储介质用于存储多条指令，其特征在于，所述指令适于由处理器加载并执行权利要求1-4任一项所述的基于多频卡尔曼滤波的机器人视觉/INS组合定位方法。
7.一种计算机可读存储介质，其中存储有多条指令，其特征在于，所述指令适于由终端设备的处理器加载并执行权利要求1-4任一项所述的基于多频卡尔曼滤波的机器人视觉/INS组合定位方法。
CN202011132559.1A 2020-10-21 2020-10-21 基于多频卡尔曼滤波的机器人视觉/ins组合定位方法及系统 Active CN112539746B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011132559.1A CN112539746B (zh) 2020-10-21 2020-10-21 基于多频卡尔曼滤波的机器人视觉/ins组合定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011132559.1A CN112539746B (zh) 2020-10-21 2020-10-21 基于多频卡尔曼滤波的机器人视觉/ins组合定位方法及系统

Publications (2)

Publication Number Publication Date
CN112539746A CN112539746A (zh) 2021-03-23
CN112539746B true CN112539746B (zh) 2023-05-26

Family

ID=75013890

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011132559.1A Active CN112539746B (zh) 2020-10-21 2020-10-21 基于多频卡尔曼滤波的机器人视觉/ins组合定位方法及系统

Country Status (1)

Country Link
CN (1) CN112539746B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113566827A (zh) * 2021-07-09 2021-10-29 中国能源建设集团安徽省电力设计院有限公司 一种基于信息融合的变电站巡检机器人室内定位方法
CN114643598B (zh) * 2022-05-13 2022-09-13 北京科技大学 一种基于多信息融合的机械臂末端位置估计方法
CN115127554B (zh) * 2022-08-31 2022-11-15 中国人民解放军国防科技大学 一种基于多源视觉辅助的无人机自主导航方法与系统

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109900265A (zh) * 2019-03-15 2019-06-18 武汉大学 一种camera/mems辅助北斗的机器人定位算法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103471595B (zh) * 2013-09-26 2016-04-06 东南大学 一种面向ins/wsn室内移动机器人紧组合导航的迭代扩展rts均值滤波方法
CN206132067U (zh) * 2016-06-29 2017-04-26 陕西国际商贸学院 一种融合导航处理系统
CN106647784A (zh) * 2016-11-15 2017-05-10 天津大学 基于北斗导航系统的微小型无人飞行器定位与导航方法
CN109387198B (zh) * 2017-08-03 2022-07-15 北京自动化控制设备研究所 一种基于序贯检测的惯性/视觉里程计组合导航方法
CN108802770B (zh) * 2018-06-01 2022-05-13 东南大学 一种ins增强gnss的高精度动态定位检定基准
CN111578939B (zh) * 2020-03-23 2021-11-02 济南大学 考虑采样周期随机变化的机器人紧组合导航方法及系统

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109900265A (zh) * 2019-03-15 2019-06-18 武汉大学 一种camera/mems辅助北斗的机器人定位算法

Also Published As

Publication number Publication date
CN112539746A (zh) 2021-03-23

Similar Documents

Publication Publication Date Title
CN112539746B (zh) 基于多频卡尔曼滤波的机器人视觉/ins组合定位方法及系统
CN110706279B (zh) 基于全局地图与多传感器信息融合的全程位姿估计方法
CN107255795B (zh) 基于ekf/efir混合滤波的室内移动机器人定位方法和装置
CN110057354B (zh) 一种基于磁偏角修正的地磁匹配导航方法
CN113074727A (zh) 基于蓝牙与slam的室内定位导航装置及其方法
CN107941211B (zh) 基于二阶级联的多元素融合定位方法、装置及电子设备
WO2019057173A1 (zh) 传感器数据处理的方法及装置
CN114013449B (zh) 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN110187375A (zh) 一种基于slam定位结果提高定位精度的方法及装置
CN113066127B (zh) 一种在线标定设备参数的视觉惯性里程计方法和系统
CN114136315B (zh) 一种基于单目视觉辅助惯性组合导航方法及系统
CN113029134B (zh) 视觉惯性系统的初始化方法、装置、电子设备及存储介质
CN111025366A (zh) 基于ins及gnss的网格slam的导航系统及方法
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
CN114119744B (zh) 一种构建点云地图的方法、装置、设备及存储介质
CN109737957B (zh) 一种采用级联FIR滤波的INS/LiDAR组合导航方法及系统
CN117388830A (zh) 激光雷达与惯性导航的外参标定方法、装置、设备及介质
CN109459769B (zh) 一种自主定位方法与系统
CN112461258A (zh) 一种参数修正的方法及装置
CN116242373A (zh) 一种融合多源数据的高精度导航定位方法及系统
CN115451946A (zh) 一种MEMS-IMU和Wi-Fi组合的室内行人定位方法
CN111123323B (zh) 一种提高便携设备定位精度的方法
CN113155156A (zh) 运行信息的确定方法及装置、存储介质、电子装置
CN114245460B (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
EE01 Entry into force of recordation of patent licensing contract

Application publication date: 20210323

Assignee: Shanxi Yu Yanxing micro nano IOT Technology Co.,Ltd.

Assignor: University of Jinan

Contract record no.: X2024980003208

Denomination of invention: A Robot Vision/INS Combination Localization Method and System Based on Multi frequency Kalman Filter

Granted publication date: 20230526

License type: Common License

Record date: 20240321

EE01 Entry into force of recordation of patent licensing contract