CN116086442A - 包含惯性传感器的多个定位传感器的融合定位装置 - Google Patents
包含惯性传感器的多个定位传感器的融合定位装置 Download PDFInfo
- Publication number
- CN116086442A CN116086442A CN202211707072.0A CN202211707072A CN116086442A CN 116086442 A CN116086442 A CN 116086442A CN 202211707072 A CN202211707072 A CN 202211707072A CN 116086442 A CN116086442 A CN 116086442A
- Authority
- CN
- China
- Prior art keywords
- positioning
- sensor
- fusion
- positioning sensor
- pose
- 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
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
Abstract
本申请涉及一种包含惯性传感器的多个定位传感器的融合定位装置,其中包括:至少一个惯性传感器、第一定位传感器及第二定位传感器,以分别采集目标的惯性数据,并获取目标的第一和第二定位位姿;此外,融合定位组件,用于根据惯性数据预测当前时刻状态量,并基于第一定位位姿和/或第二定位位姿得到当前预测状态量,以通过卡尔曼滤波原理计算卡尔曼增益,进而得到当前预测状态量的修正值,且根据当前预测状态量和修正值生成当前时刻真正状态量,得到目标融合定位结果。由此,解决了相关技术在标记点被长时间遮挡时,融合定位易出现偏差,鲁棒性较差,且大场地定位精度较低,无法避免光学运动捕捉出现异常突变等情况,定位稳定性较差等问题。
Description
技术领域
本申请涉及多传感器融合定位技术领域,特别涉及一种包含惯性传感器的多个定位传感器的融合定位装置。
背景技术
光学运动捕捉是实现室内刚体跟踪定位常用定位技术之一,光学运动捕捉系统可以快速精确定位主动发光或被动反光标记点,提供标记点位置坐标,三个固定标记点若组成一个非等腰三角形,或三个以上固定标记点组成非对称结构,可以形成一个刚体,光学运动捕捉系统可以计算得到刚体位姿。通常可以把固定上标记点的刚体看作一个定位传感器,可以用于定位任何固定上定位传感器的设备,如无人机、VR(Virtual Reality,虚拟现实)头盔、XR(Extended Reality,扩展现实)相机等。但不可避免的是存在定位传感器遮挡问题,导致定位失效。
目前,相关技术可通过引入惯性传感器,即惯性测量单元,该惯性传感器是一种内感受型传感器,不受遮挡、工作空间大小等影响,测量精度仅与传感器本身精度有关,将惯性传感器与光学定位传感器绑定,通过一定数据融合算法,融合两个传感器定位数据,从而弥补光学定位的一些缺点,如遮挡问题等。
然而,相关技术在标记点被长时间遮挡时,融合定位易出现偏差,鲁棒性较差,且大场地定位精度较低,无法避免光学运动捕捉出现异常突变等情况,定位稳定性较差,亟待解决。
发明内容
本申请提供一种包含惯性传感器的多个定位传感器的融合定位装置,以解决相关技术在标记点被长时间遮挡时,融合定位易出现偏差,鲁棒性较差,且大场地定位精度较低,无法避免光学运动捕捉出现异常突变等情况,定位稳定性较差等问题。
本申请第一方面实施例提供一种包含惯性传感器的多个定位传感器的融合定位装置,包括:至少一个惯性传感器,用于基于所述至少一个惯性传感器的采样帧率,采集目标的惯性数据;至少一个第一定位传感器,用于基于所述至少一个第一定位传感器的定位更新帧率,获取所述目标的第一定位位姿;至少一个第二定位传感器,用于基于所述至少一个第二定位传感器的定位更新帧率,获取所述目标的第二定位位姿,其中,第一定位传感器和第二定位传感器的类型不同;以及融合定位组件,用于根据所述惯性数据和所述第一定位位姿和/或所述第二定位位姿进行融合计算。
可选地,在本申请的一个实施例中,所述融合定位组件具体用于根据所述惯性数据预测当前时刻状态量,并基于所述第一定位位姿和/或所述第二定位位姿得到当前时刻观测量,通过加权计算得到所述目标的融合定位结果。
可选地,在本申请的一个实施例中,所述融合定位组件进一步用于基于卡尔曼滤器计算所述目标的融合定位结果。
可选地,在本申请的一个实施例中,所述融合定位组件还用于在根据当前预测状态量和修正值生成所述当前时刻真正状态量之前,判断所述至少一个第一定位传感器和/或至少一个第二定位传感器是否满足预设异常波动条件,并在所述至少一个第一定位传感器和/或所述至少一个第二定位传感器满足所述预设异常波动条件时,禁止修正所述当前预测状态量。
可选地,在本申请的一个实施例中,所述融合定位组件进一步用于根据当前时刻的惯性传感器世界坐标系分别与所述至少一个第一定位传感器的定位传感器世界坐标系和所述至少一个第二定位传感器的定位传感器世界坐标系之间的相对姿态判断是否满足所述预设异常波动条件。
可选地,在本申请的一个实施例中,还包括:供电组件,所述供电组件为所述至少一个惯性传感器、所述至少一个第一定位传感器和/或所述至少一个第二定位传感器供电。
可选地,在本申请的一个实施例中,还包括:通信组件,用于将所述融合定位组件的工作指令发送至所述至少一个惯性传感器、所述至少一个第一定位传感器和所述至少一个第二定位传感器,并接收所述至少一个惯性传感器的惯性数据、所述至少一个第一定位传感器和所述至少一个第二定位传感器的定位位姿,以将所述惯性数据和所述定位位姿发送至所述融合定位组件。
可选地,在本申请的一个实施例中,所述至少一个第一定位传感器为光学定位传感器或视觉定位传感器中的一种。
可选地,在本申请的一个实施例中,所述至少一个第二定位传感器为机械摇臂定位传感器或视觉定位传感器中的一种。
可选地,在本申请的一个实施例中,还包括:显示组件,用于显示所述至少一个惯性传感器的当前工作状态。
本申请第二方面实施例提供一种融合定位方法,包括以下步骤:基于所述至少一个第一定位传感器的定位更新帧率,获取所述目标的第一定位位姿;基于所述至少一个第一定位传感器的定位更新帧率,获取所述目标的第一定位位姿;基于所述至少一个第二定位传感器的定位更新帧率,获取所述目标的第二定位位姿,其中,第一定位传感器和第二定位传感器的类型不同;以及根据所述惯性数据和所述第一定位位姿和/或所述第二定位位姿进行融合计算。
可选地,在本申请的一个实施例中,根据所述惯性数据预测当前时刻状态量,并基于所述第一定位位姿和/或所述第二定位位姿得到当前时刻观测量,通过加权计算得到所述目标的融合定位结果。
可选地,在本申请的一个实施例中,基于卡尔曼滤器计算所述目标的融合定位结果。
可选地,在本申请的一个实施例中,在根据当前预测状态量和修正值生成所述当前时刻真正状态量之前,还包括:判断所述至少一个第一定位传感器和/或至少一个第二定位传感器是否满足预设异常波动条件;在所述至少一个第一定位传感器和/或所述至少一个第二定位传感器满足所述预设异常波动条件时,禁止修正所述当前预测状态量。
可选地,在本申请的一个实施例中,所述判断所述至少一个第一定位传感器和/或至少一个第二定位传感器是否满足预设异常波动条件,包括:根据当前时刻的惯性传感器世界坐标系分别与所述至少一个第一定位传感器的定位传感器世界坐标系和所述至少一个第二定位传感器的定位传感器世界坐标系之间的相对姿态判断是否满足所述预设异常波动条件。
本申请第三方面实施例提供一种电子设备,包括:存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序,所述处理器执行所述程序,以实现如上述实施例所述的融合定位方法。
本申请第四方面实施例提供一种计算机可读存储介质,所述计算机可读存储介质存储计算机程序,该程序被处理器执行时实现如上的融合定位方法。
由此,本申请的实施例具有以下有益效果:
本申请的实施例可包括至少一个惯性传感器,用于基于至少一个惯性传感器的采样帧率,采集目标的惯性数据;至少一个第一定位传感器,用于基于至少一个第一定位传感器的定位更新帧率,获取目标的第一定位位姿;至少一个第二定位传感器,用于基于至少一个第二定位传感器的定位更新帧率,获取目标的第二定位位姿,其中,第一定位传感器和第二定位传感器的类型不同;融合定位组件,用于根据惯性数据和第一定位位姿和/或第二定位位姿进行融合计算。本申请通过惯性传感器与光学定位传感器及机械摇臂定位传感器融合定位,持续产生刚体定位数据,从而有效提高了刚体定位的精度和抗干扰性,避免了长时间光学运动捕捉刚体运动定位数据丢失情况的发生。由此,解决了相关技术在标记点被长时间遮挡时,融合定位易出现偏差,鲁棒性较差,且大场地定位精度较低,无法避免光学运动捕捉出现异常突变等情况,定位稳定性较差等问题。
本申请附加的方面和优点将在下面的描述中部分给出,部分将从下面的描述中变得明显,或通过本申请的实践了解到。
附图说明
本申请上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解,其中:
图1为根据本申请实施例的包含惯性传感器的多个定位传感器的融合定位装置的示例图;
图2为根据本申请实施例提供的一种融合定位方法的流程图;
图3为本申请实施例提供的电子设备的结构示意图。
具体实施方式
下面详细描述本申请的实施例,所述实施例的示例在附图中示出,其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的,旨在用于解释本申请,而不能理解为对本申请的限制。
下面参考附图描述本申请实施例的包含惯性传感器的多个定位传感器的融合定位装置。针对上述背景技术中提到的问题,本申请提供了一种包含惯性传感器的多个定位传感器的融合定位装置,包括,至少一个惯性传感器,用于基于至少一个惯性传感器的采样帧率,采集目标的惯性数据;至少一个第一定位传感器,用于基于至少一个第一定位传感器的定位更新帧率,获取目标的第一定位位姿;至少一个第二定位传感器,用于基于至少一个第二定位传感器的定位更新帧率,获取目标的第二定位位姿,其中,第一定位传感器和第二定位传感器的类型不同;融合定位组件,用于根据惯性数据和第一定位位姿和/或第二定位位姿进行融合计算。本申请通过惯性传感器与光学定位传感器及机械摇臂定位传感器融合定位,持续产生刚体定位数据,从而有效提高了刚体定位的精度和抗干扰性,避免了长时间光学运动捕捉刚体运动定位数据丢失情况的发生。由此,解决了相关技术在标记点被长时间遮挡时,融合定位易出现偏差,鲁棒性较差,且大场地定位精度较低,无法避免光学运动捕捉出现异常突变等情况,定位稳定性较差等问题。
具体而言,图1是本申请实施例的包含惯性传感器的多个定位传感器的融合定位装置的方框示意图。
如图1所示,该包含惯性传感器的多个定位传感器的融合定位装置10包括:惯性传感器101、第一定位传感器102、第二定位传感器103以及融合定位组件104。
其中,至少一个惯性传感器101,用于基于至少一个惯性传感器的采样帧率,采集目标的惯性数据。
在本申请的实施例中,可利用惯性传感器的加速度计和陀螺仪,基于采样频率分别获取当前时刻三轴加速度数据以及三轴角速度数据,其采样频率可达上千赫兹,远大于定位传感器定位数据频率,本申请实施例中的惯性传感器的采样频率一般可设定为400Hz。
由此,本申请通过引入惯性传感器,从而为后续光惯融合定位的实现提供了可靠的硬件支撑。
可选地,在本申请的一个实施例中,本申请实施例的包含惯性传感器的多个定位传感器的融合定位装置10还包括:显示组件,用于显示至少一个惯性传感器的当前工作状态。
本申请的实施例可通过显示组件,以显示惯性传感器的工作状态,从而使用户能够更为直观地了解惯性传感器的实时状态,提高多个定位传感器的融合定位装置的人性化程度和可靠性水平。
至少一个第一定位传感器102,用于基于至少一个第一定位传感器的定位更新帧率,获取目标的第一定位位姿。
本申请的实施例可利用第一定位传感器的定位更新帧率进行计算,获取当前时刻第一定位传感器的定位位姿数据,从而为多传感器融合提供数据依据和指导。
可选地,在本申请的一个实施例中,至少一个第一定位传感器为光学定位传感器或视觉定位传感器中的一种。
需要说明的是,本申请的实施例可利用光学运动捕捉定位传感器作为第一定位传感器,以实现传感器的融合定位,其可通过一定的定位频率(默认为50Hz)获取目标的位姿数据。
举例而言,本申请的实施例可利用光学运动捕捉系统的光学定位传感器,通过在刚体上绑定不少于三个的主动发光或被动反光标识单元,且被动标识单元不组成对称结构,如球型主动发光点或被动反光球等,空间中的多相机定位标识单元组成的定位传感器可以跟踪定位到刚体标识单元,进而得到刚体定位位姿数据,由此,本申请的实施例通过设置合适的定位传感器,从而可得到可靠地刚体位姿等数据,有效保障了传感器融合定位的实现。
需要注意的是,在实际执行过程中,除上述光学运动捕捉定位传感器以外,本领域技术人员还可根据实际情况,将其他可提供刚体位姿的设备作为第一定位传感器,如SLAM(Simultaneous Localization And Mapping,即时定位与地图构建)系统中的相机传感器等,于此不做具体限制。
至少一个第二定位传感器103,用于基于至少一个第二定位传感器的定位更新帧率,获取目标的第二定位位姿,其中,第一定位传感器和第二定位传感器的类型不同。
此外,本申请的实施例还可利用第二定位传感器的定位更新帧率(默认为50Hz)进行计算,获取当前时刻第二定位传感器的定位位姿数据,从而进一步保障了多传感器融合定位功能的实现。
可选地,在本申请的一个实施例中,至少一个第二定位传感器为机械摇臂定位传感器或视觉定位传感器中的一种。
需要说明的是,在本申请的实施例中,可将机械摇臂定位传感器作为第二定位传感器,从而根据内置机械编码器获取机械摇臂旋转角度,进而计算得到摇臂末端位置,以及末端固定的相机位置姿态;此外,上述第二定位传感器还可为其他视觉定位传感器,本领域技术人员在具体实现过程中,可根据实际情况进行选择,于此不做具体限定。
举例而言,在XR场景中,XR相机可固定在机械摇臂上,包含惯性传感器的定位装置固定在相机上,同时其上固定光学定位传感器,从而可以实时获取绑定在XR相机上的三个传感器的实时定位数据,即惯性传感器的加速度角速度数据、第一定位传感器的光学定位数据以及第二定位传感器的机械摇臂定位数据。
由此,本申请的实施例通过机械摇臂定位传感器获取搭载在摇臂上的刚体位姿,从而满足了后续传感器的融合定位所需的原始数据需求,保障了多传感器融合定位的顺利实现。
融合定位组件104,用于根据惯性数据和第一定位位姿和/或第二定位位姿进行融合计算。
在通过惯性传感器获取加速度、角速度,以及前一时刻惯性传感器的位置、姿态、速度等状态量等惯性数据后,进一步地,本申请的实施例可根据惯性传感器获取的惯性数据结合目标的第一和第二定位位姿进行融合计算,从而得到可靠的目标定位数据。
可选地,在本申请的一个实施例中,融合定位组件具体用于根据惯性数据预测当前时刻状态量,并基于第一定位位姿和/或第二定位位姿得到当前时刻观测量,通过加权计算得到目标的融合定位结果。
具体地,本申请的实施例可将惯性传感器前一时刻状态量和当前时刻加速度与角速度代入动力学方程进行计算,通过动力学方程建立加速度与角速度和惯性传感器位置、姿态、速度之间的关系,即对加速度进行一次积分得到速度,再次进行积分得到位置数据,对角速度进行一次积分得到旋转角度,以获取当前时刻位置、姿态、速度等状态量的预测值及当前状态的协方差;在未得到定位传感器数据时,本申请的实施例可持续通过动力学方程进行计算,预测当前时刻状态量。
此外,本申请实施例可分别获取第一定位传感器和第一定位传感器的当前预测状态量,包括惯性传感器本身的加速度与角速度零偏、惯性传感器与第一定位传感器的相对位姿、惯性传感器世界坐标系与第一定位传感器世界坐标系的相对姿态、惯性传感器与第一定位传感器的尺度因子、惯性传感器与第二定位传感器的相对位姿、惯性传感器世界坐标系与第二定位传感器世界坐标系的相对姿态以及惯性传感器与第二定位传感器的尺度因子等;其中,获取的当前预测状态量通过惯性传感器与第一定位传感器相对位姿,可以得到当前第一定位传感器预测位姿;获取的当前预测状态量通过惯性传感器与第二定位传感器相对位姿,可以得到当前第二定位传感器预测位姿。
由此,本申请的实施例通过结合惯性数据、第一定位位姿和第二定位位姿分别进行预测,以得到的当前时刻状态量和观测量,并利用加权计算得到目标的融合定位结果,进一步提高了定位数据的可靠性和鲁棒性。
可选地,在本申请的一个实施例中,融合定位组件进一步用于基于卡尔曼滤器计算目标的融合定位结果。
本申请的实施例可通过融合定位组件,利用卡尔曼滤波原理,即利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计,以计算卡尔曼增益,得到当前预测状态量的修正,并根据预测状态量的修正,得到当前时刻真正状态量的估计。
需要注意的是,本领域技术人员在具体实现过程中,可根据实际情况选择合适的卡尔曼滤波原理得滤波技术,例如扩展卡尔曼滤波、无迹卡尔曼滤波等,于此不作具体限制。
由此,本申请的实施例通过融合定位组件,利用卡尔曼滤波原理实现了惯性传感器与第一定位传感器和第二定位传感器的融合定位,提高了定位数据的稳定性和定位精度。
可选地,在本申请的一个实施例中,融合定位组件进一步用于根据当前时刻的惯性传感器世界坐标系分别与至少一个第一定位传感器的定位传感器世界坐标系和至少一个第二定位传感器的定位传感器世界坐标系之间的相对姿态判断是否满足预设异常波动条件。
需要说明的是,本申请的实施例可通过融合定位组件获取状态量中的惯性传感器世界坐标系与第一定位传感器世界坐标系相对姿态,及与第二定位传感器世界坐标系相对姿态,保存一定时域窗口大小的数据,其理想情况下数据保持不变。
继而,获取时域窗口中惯性传感器世界坐标系与第一定位传感器世界坐标系和第二定位传感器世界坐标系的相对姿态的中值滤波结果。
之后,分别获取当前时刻惯性传感器世界坐标系与第一定位传感器和第二定位传感器世界坐标系的相对姿态,与上述中值滤波结果取逆,理想情况下取逆结果为单位阵,分析取逆结果与阈值大小比对结果,以判断当前定位传感器是否出现异常波动。
由此,本申请的实施例能够有效解决光学运动捕捉定位传感器的长时间遮挡问题,避免光学运动捕捉异常突变情况对融合定位性能的影响,提升了大场地光学运动捕捉定位精度以及定位鲁棒性。
可选地,在本申请的一个实施例中,融合定位组件还用于在根据当前预测状态量和修正值生成当前时刻真正状态量之前,判断至少一个第一定位传感器和/或至少一个第二定位传感器是否满足预设异常波动条件,并在至少一个第一定位传感器和/或至少一个第二定位传感器满足预设异常波动条件时,禁止修正当前预测状态量。
在根据当前预测状态量和修正值生成当前时刻真正状态量之前,本申请的实施例还可通过融合定位组件根据融合状态量波动,判断当前第一定位传感器和第二定位传感器是否出现异常波动,若任一定位传感器出现异常波动,则状态量不进行修正,保持原有结果,避免融合异常波动。
需要说明的是,在定位传感器出现波动异常时,上述惯性传感器本身的加速度与角速度零偏、惯性传感器与第一定位传感器的相对位姿、惯性传感器世界坐标系与第一定位传感器世界坐标系的相对姿态、惯性传感器与第一定位传感器的尺度因子、惯性传感器与第二定位传感器的相对位姿、惯性传感器世界坐标系与第二定位传感器世界坐标系的相对姿态以及惯性传感器与第二定位传感器的尺度因子等状态量不进行状态量的更新。
由此,本申请的实施例通过融合定位组件对第一定位传感器和第二定位传感器进行异常波动检测,从而使得光学定位传感器数据长时间丢失,融合数据仍能够稳定输出,提高了异常突变的稳定性和静态精度,极大改善了融合定位的性能。
可选地,在本申请的一个实施例中,本申请实施例的包含惯性传感器的多个定位传感器的融合定位装置10还包括:供电组件,供电组件为至少一个惯性传感器、至少一个第一定位传感器和/或至少一个第二定位传感器供电。
在本申请的实施例中,还可通过有线或无线供电组件对惯性传感器、第二定位传感器和第二定位传感器等持续进行供电,从而保证各传感器能正常稳定工作。
可选地,在本申请的一个实施例中,本申请实施例的包含惯性传感器的多个定位传感器的融合定位装置10还包括:通信组件,用于将融合定位组件的工作指令发送至至少一个惯性传感器、至少一个第一定位传感器和至少一个第二定位传感器,并接收至少一个惯性传感器的惯性数据、至少一个第一定位传感器和至少一个第二定位传感器的定位位姿,以将惯性数据和定位位姿发送至融合定位组件。
需要说明的是,本申请的实施例还可对上述融合定位组件的工作指令、惯性传感器的惯性数据以及第一和第二定位传感器的定位位姿数据通过有线或无线的通信组件进行准确实时的数据传输,从而保障融合定位的实时性和可靠性。
此外,本申请的实施例还基础结构单元固定惯性传感器、各个定位传感器、供电组件以及通信组件等,并提供可以连接固定到SLAM系统定位相机等其他定位设备其他设备上的连接接口,从而在实现了惯性传感器在内的多个传感器等设备的一体性,保证了定位数据的稳定性,提高装置通用性的同时,还可避免惯性传感器等设备裸露,具有良好的保护作用。
根据本申请实施例提出的包含惯性传感器的多个定位传感器的融合定位装置,包括至少一个惯性传感器、第一定位传感器及第二定位传感器,以分别采集目标的惯性数据,并获取目标的第一和第二定位位姿;此外,融合定位组件,用于根据惯性数据预测当前时刻状态量,并基于第一定位位姿和/或第二定位位姿得到当前预测状态量,以通过卡尔曼滤波原理计算卡尔曼增益,进而得到当前预测状态量的修正值,且根据当前预测状态量和修正值生成当前时刻真正状态量,得到目标融合定位结果。本申请通过卡尔曼滤波原理实现惯性传感器融合光学定位和机械定位,并通过异常检测算法,避免光学运动捕捉异常突变,提升了定位鲁棒性及大场地光学运动捕捉的定位精度。
其次参照附图描述根据本申请实施例提出的融合定位方法。
图2是本申请实施例的融合定位方法的流程图。
如图2所示,该融合定位方法包括以下步骤:
在步骤S201中,基于至少一个第一定位传感器的定位更新帧率,获取目标的第一定位位姿。
在步骤S202中,基于至少一个第一定位传感器的定位更新帧率,获取目标的第一定位位姿。
在步骤S203中,基于至少一个第二定位传感器的定位更新帧率,获取目标的第二定位位姿,其中,第一定位传感器和第二定位传感器的类型不同。
在步骤S204中,根据惯性数据和第一定位位姿和/或第二定位位姿进行融合计算。
可选地,在本申请的一个实施例中,根据惯性数据预测当前时刻状态量,并基于第一定位位姿和/或第二定位位姿得到当前时刻观测量,通过加权计算得到所述目标的融合定位结果。
可选地,在本申请的一个实施例中,基于卡尔曼滤器计算目标的融合定位结果。
可选地,在本申请的一个实施例中,在根据当前预测状态量和修正值生成当前时刻真正状态量之前,还包括:判断至少一个第一定位传感器和/或至少一个第二定位传感器是否满足预设异常波动条件;在至少一个第一定位传感器和/或至少一个第二定位传感器满足预设异常波动条件时,禁止修正当前预测状态量。
可选地,在本申请的一个实施例中,判断至少一个第一定位传感器和/或至少一个第二定位传感器是否满足预设异常波动条件,包括:根据当前时刻的惯性传感器世界坐标系分别与至少一个第一定位传感器的定位传感器世界坐标系和至少一个第二定位传感器的定位传感器世界坐标系之间的相对姿态判断是否满足预设异常波动条件。
需要说明的是,前述对包含惯性传感器的多个定位传感器的融合定位装置实施例的解释说明也适用于该实施例的融合定位方法,此处不再赘述。
根据本申请实施例提出的融合定位方法,通过基于至少一个第一定位传感器的定位更新帧率,获取目标的第一定位位姿;基于至少一个第一定位传感器的定位更新帧率,获取目标的第一定位位姿;基于至少一个第二定位传感器的定位更新帧率,获取目标的第二定位位姿,其中,第一定位传感器和第二定位传感器的类型不同;根据惯性数据和第一定位位姿和/或第二定位位姿进行融合计算,从而有效解决了光学运动捕捉异常突变问题,极大提升了定位鲁棒性以及大场地光学运动捕捉定位的精度。
图3为本申请实施例提供的电子设备的结构示意图。该电子设备可以包括:
存储器301、处理器302及存储在存储器301上并可在处理器302上运行的计算机程序。
处理器302执行程序时实现上述实施例中提供的融合定位方法。
进一步地,电子设备还包括:
通信接口303,用于存储器301和处理器302之间的通信。
存储器301,用于存放可在处理器302上运行的计算机程序。
存储器301可能包含高速RAM存储器,也可能还包括非易失性存储器(non-volatile memory),例如至少一个磁盘存储器。
如果存储器301、处理器302和通信接口303独立实现,则通信接口303、存储器301和处理器302可以通过总线相互连接并完成相互间的通信。总线可以是工业标准体系结构(Industry Standard Architecture,简称为ISA)总线、外部设备互连(PeripheralComponent,简称为PCI)总线或扩展工业标准体系结构(Extended Industry StandardArchitecture,简称为EISA)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示,图3中仅用一条粗线表示,但并不表示仅有一根总线或一种类型的总线。
可选地,在具体实现上,如果存储器301、处理器302及通信接口303,集成在一块芯片上实现,则存储器301、处理器302及通信接口303可以通过内部接口完成相互间的通信。
处理器302可能是一个中央处理器(Central Processing Unit,简称为CPU),或者是特定集成电路(Application Specific Integrated Circuit,简称为ASIC),或者是被配置成实施本申请实施例的一个或多个集成电路。
本申请实施例还提供一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现如上的融合定位方法。
在本说明书的描述中,参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中,对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且,描述的具体特征、结构、材料或者特点可以在任一个或N个实施例或示例中以合适的方式结合。此外,在不相互矛盾的情况下,本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
此外,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中,“N个”的含义是至少两个,例如两个,三个等,除非另有明确具体的限定。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为,表示包括一个或N个用于实现定制逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分,并且本申请的优选实施方式的范围包括另外的实现,其中可以不按所示出或讨论的顺序,包括根据所涉及的功能按基本同时的方式或按相反的顺序,来执行功能,这应被本申请的实施例所属技术领域的技术人员所理解。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤,例如,可以被认为是用于实现逻辑功能的可执行指令的定序列表,可以具体实现在任何计算机可读介质中,以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用,或结合这些指令执行系统、装置或设备而使用。就本说明书而言,"计算机可读介质"可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下:具有一个或N个布线的电连接部(电子装置),便携式计算机盘盒(磁装置),随机存取存储器(RAM),只读存储器(ROM),可擦除可编辑只读存储器(EPROM或闪速存储器),光纤装置,以及便携式光盘只读存储器(CDROM)。另外,计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质,因为可以通过对纸或其他介质进行光学扫描,接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序,然后将其存储在计算机存储器中。
应当理解,本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中,N个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。如果用硬件来实现和在另一实施方式中一样,可用本领域公知的下列技术中的任一项或他们的组合来实现:具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路,具有合适的组合逻辑门电路的专用集成电路,可编程门阵列(PGA),现场可编程门阵列(FPGA)等。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成,所述的程序可以存储于一种计算机可读存储介质中,该程序在执行时,包括方法实施例的步骤之一或其组合。
此外,在本申请各个实施例中的各功能单元可以集成在一个处理模块中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现,也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时,也可以存储在一个计算机可读取存储介质中。
上述提到的存储介质可以是只读存储器,磁盘或光盘等。尽管上面已经示出和描述了本申请的实施例,可以理解的是,上述实施例是示例性的,不能理解为对本申请的限制,本领域的普通技术人员在本申请的范围内可以对上述实施例进行变化、修改、替换和变型。
Claims (17)
1.一种包含惯性传感器的多个定位传感器的融合定位装置,其特征在于,包括:
至少一个惯性传感器,用于基于所述至少一个惯性传感器的采样帧率,采集目标的惯性数据;
至少一个第一定位传感器,用于基于所述至少一个第一定位传感器的定位更新帧率,获取所述目标的第一定位位姿;
至少一个第二定位传感器,用于基于所述至少一个第二定位传感器的定位更新帧率,获取所述目标的第二定位位姿,其中,第一定位传感器和第二定位传感器的类型不同;以及
融合定位组件,用于根据所述惯性数据和所述第一定位位姿和/或所述第二定位位姿进行融合计算。
2.根据权利要求1所述的装置,其特征在于,所述融合定位组件具体用于根据所述惯性数据预测当前时刻状态量,并基于所述第一定位位姿和/或所述第二定位位姿得到当前时刻观测量,通过加权计算得到所述目标的融合定位结果。
3.根据权利要求2所述的装置,其特征在于,所述融合定位组件进一步用于基于卡尔曼滤器计算所述目标的融合定位结果。
4.根据权利要求1所述的装置,其特征在于,所述融合定位组件还用于在根据当前预测状态量和修正值生成所述当前时刻真正状态量之前,判断所述至少一个第一定位传感器和/或至少一个第二定位传感器是否满足预设异常波动条件,并在所述至少一个第一定位传感器和/或所述至少一个第二定位传感器满足所述预设异常波动条件时,禁止修正所述当前预测状态量。
5.根据权利要求4所述的装置,其特征在于,所述融合定位组件进一步用于根据当前时刻的惯性传感器世界坐标系分别与所述至少一个第一定位传感器的定位传感器世界坐标系和所述至少一个第二定位传感器的定位传感器世界坐标系之间的相对姿态判断是否满足所述预设异常波动条件。
6.根据权利要求1所述的装置,其特征在于,还包括:
供电组件,所述供电组件为所述至少一个惯性传感器、所述至少一个第一定位传感器和/或所述至少一个第二定位传感器供电。
7.根据权利要求1所述的装置,其特征在于,还包括:
通信组件,用于将所述融合定位组件的工作指令发送至所述至少一个惯性传感器、所述至少一个第一定位传感器和所述至少一个第二定位传感器,并接收所述至少一个惯性传感器的惯性数据、所述至少一个第一定位传感器和所述至少一个第二定位传感器的定位位姿,以将所述惯性数据和所述定位位姿发送至所述融合定位组件。
8.根据权利要求1所述的装置,其特征在于,所述至少一个第一定位传感器为光学定位传感器或视觉定位传感器中的一种。
9.根据权利要求1所述的装置,其特征在于,所述至少一个第二定位传感器为机械摇臂定位传感器或视觉定位传感器中的一种。
10.根据权利要求1所述的装置,其特征在于,还包括:
显示组件,用于显示所述至少一个惯性传感器的当前工作状态。
11.一种融合定位方法,其特征在于,采用如权利要求1-8任一项所述的包含惯性传感器的多个定位传感器的融合定位装置,其中,所述方法包括以下步骤:
基于所述至少一个第一定位传感器的定位更新帧率,获取所述目标的第一定位位姿;
基于所述至少一个第一定位传感器的定位更新帧率,获取所述目标的第一定位位姿;
基于所述至少一个第二定位传感器的定位更新帧率,获取所述目标的第二定位位姿,其中,第一定位传感器和第二定位传感器的类型不同;以及
根据所述惯性数据和所述第一定位位姿和/或所述第二定位位姿进行融合计算。
12.根据权利要求11所述的方法,其特征在于,根据所述惯性数据预测当前时刻状态量,并基于所述第一定位位姿和/或所述第二定位位姿得到当前时刻观测量,通过加权计算得到所述目标的融合定位结果。
13.根据权利要求12所述的方法,其特征在于,基于卡尔曼滤器计算所述目标的融合定位结果。
14.根据权利要求11所述的方法,其特征在于,在根据当前预测状态量和修正值生成所述当前时刻真正状态量之前,还包括:
判断所述至少一个第一定位传感器和/或至少一个第二定位传感器是否满足预设异常波动条件;
在所述至少一个第一定位传感器和/或所述至少一个第二定位传感器满足所述预设异常波动条件时,禁止修正所述当前预测状态量。
15.根据权利要求14所述的方法,其特征在于,所述判断所述至少一个第一定位传感器和/或至少一个第二定位传感器是否满足预设异常波动条件,包括:
根据当前时刻的惯性传感器世界坐标系分别与所述至少一个第一定位传感器的定位传感器世界坐标系和所述至少一个第二定位传感器的定位传感器世界坐标系之间的相对姿态判断是否满足所述预设异常波动条件。
16.一种电子设备,其特征在于,包括:存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序,所述处理器执行所述程序,以实现如权利要求11-15任一项所述的融合定位方法。
17.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,该程序被处理器执行,以用于实现如权利要求11-15任一项所述的融合定位方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211707072.0A CN116086442A (zh) | 2022-12-29 | 2022-12-29 | 包含惯性传感器的多个定位传感器的融合定位装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211707072.0A CN116086442A (zh) | 2022-12-29 | 2022-12-29 | 包含惯性传感器的多个定位传感器的融合定位装置 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116086442A true CN116086442A (zh) | 2023-05-09 |
Family
ID=86198509
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211707072.0A Pending CN116086442A (zh) | 2022-12-29 | 2022-12-29 | 包含惯性传感器的多个定位传感器的融合定位装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116086442A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117452448A (zh) * | 2023-12-25 | 2024-01-26 | 合众新能源汽车股份有限公司 | 高精定位跳变联合故障诊断方法和车辆定位控制系统 |
-
2022
- 2022-12-29 CN CN202211707072.0A patent/CN116086442A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117452448A (zh) * | 2023-12-25 | 2024-01-26 | 合众新能源汽车股份有限公司 | 高精定位跳变联合故障诊断方法和车辆定位控制系统 |
CN117452448B (zh) * | 2023-12-25 | 2024-04-09 | 合众新能源汽车股份有限公司 | 高精定位跳变联合故障诊断方法和车辆定位控制系统 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6734940B2 (ja) | 三次元計測装置 | |
CN109668553A (zh) | 基于惯性的导航设备和基于相对预积分的惯性导航方法 | |
JP2000097637A5 (ja) | 姿勢位置検出装置及び移動体姿勢検出装置 | |
WO2015123774A1 (en) | System and method for augmented reality and virtual reality applications | |
EP3343242B1 (en) | Tracking system, tracking device and tracking method | |
US9584768B2 (en) | Information processing apparatus, information processing method and computer-readable storage medium | |
JP2009031295A (ja) | 移動体姿勢検出装置 | |
CN116086442A (zh) | 包含惯性传感器的多个定位传感器的融合定位装置 | |
KR20180076441A (ko) | 적응적 관심영역 및 탐색창을 이용한 객체 검출 방법 및 장치 | |
CN111721305B (zh) | 定位方法和装置、自动驾驶车辆、电子设备和存储介质 | |
JPWO2020184013A1 (ja) | 車両制御装置 | |
CN111595342B (zh) | 一种可大规模部署的室内定位方法及系统 | |
CN112362044A (zh) | 室内定位方法、装置、设备和系统 | |
EP3392748B1 (en) | System and method for position tracking in a virtual reality system | |
CN113158779A (zh) | 一种行走方法、装置和计算机存储介质 | |
CN112729294A (zh) | 适用于机器人的视觉和惯性融合的位姿估计方法及系统 | |
CN108322698B (zh) | 基于多摄像机和惯性测量单元融合的系统和方法 | |
CN116222554A (zh) | 包含惯性传感器的多个定位传感器的融合定位装置 | |
US20150142310A1 (en) | Self-position measuring terminal | |
CN111798489B (zh) | 一种特征点跟踪方法、设备、介质及无人设备 | |
CN111522441B (zh) | 空间定位方法、装置、电子设备及存储介质 | |
CN113065572B (zh) | 多传感器融合的数据处理方法、定位装置及虚拟现实设备 | |
JP6653151B2 (ja) | 進行方向推定システム | |
CN113074751B (zh) | 一种视觉定位误差检测方法及装置 | |
CN111078489B (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 |