CN113203413A - 一种位姿融合估计方法、装置及存储介质 - Google Patents

一种位姿融合估计方法、装置及存储介质 Download PDF

Info

Publication number
CN113203413A
CN113203413A CN202110495613.7A CN202110495613A CN113203413A CN 113203413 A CN113203413 A CN 113203413A CN 202110495613 A CN202110495613 A CN 202110495613A CN 113203413 A CN113203413 A CN 113203413A
Authority
CN
China
Prior art keywords
pose
state
roof antenna
determining
inertial navigation
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.)
Granted
Application number
CN202110495613.7A
Other languages
English (en)
Other versions
CN113203413B (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.)
Shanghai Boonray Intelligent Technology Co Ltd
Original Assignee
Shanghai Boonray Intelligent 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 Shanghai Boonray Intelligent Technology Co Ltd filed Critical Shanghai Boonray Intelligent Technology Co Ltd
Priority to CN202110495613.7A priority Critical patent/CN113203413B/zh
Publication of CN113203413A publication Critical patent/CN113203413A/zh
Application granted granted Critical
Publication of CN113203413B publication Critical patent/CN113203413B/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
    • 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
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Data Mining & Analysis (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本申请公开了一种位姿融合估计方法、装置及存储介质，用于提高融合估计的精度。本申请公开的位姿融合估计方法包括：确定车身惯导位姿状态和车顶天线模块位姿状态；确定所述车身惯导位姿状态和所述车顶天线模块位姿状态之间的柔性连接因子；根据所述车身惯导位姿状态、所述车顶天线模块位姿状态和所述柔性连接因子，确定车顶天线的定位因子。本申请还提供了一种位姿融合估计装置及存储介质。

Description

一种位姿融合估计方法、装置及存储介质
技术领域
本申请涉及自动驾驶领域，尤其涉及一种位姿融合估计方法、装置和存储介质。
背景技术
在实时动态载波相位差分惯性测量仪Real-time kinematic-InertialMeasurement Unit，RTK-IMU，的组合导航方面，无论是松组合还是紧组合，目前的方案均须假设全球导航卫星系统GNSS接收天线与IMU之间是相对固定的刚性连接。
然而，在矿车应用中，全车惯性测量仪IMU(Inertial Measurement Unit)的安装位置从惯导设备安全性及底盘控制的需求考虑，应当安装在矿车的底盘上。而卫星导航天线实时动态载波相位差分RTK(Real-time kinematic)天线需要安装在不易被遮挡的位置，如车顶。由于矿车为典型的承载式车身，因而底盘与车身间为柔性连接将发生相对晃动，在矿山这类恶劣地形下晃动将更为明显，在位姿融合中就会因为GNSS接收天线与IMU之间是相对固定的刚性连接假设与实际的相对晃动不一致而引入计算误差。
发明内容
针对上述技术问题，本申请实施例提供了一种位姿融合估计方法、装置及存储介质，用以提高融合估计的精度。
第一方面，本申请实施例提供的一种位姿融合估计方法，包括：
确定车身惯导位姿状态和车顶天线模块位姿状态；
确定所述车身惯导位姿状态和所述车顶天线模块位姿状态之间的柔性连接因子；
根据所述车身惯导位姿状态、所述车顶天线模块位姿状态和所述柔性连接因子，确定车顶天线的定位因子。
优选的，通过安装在车辆主框架上的位姿传感器确定所述车身惯导位姿状态。
优选的，通过安装在车顶的全球导航卫星系统GNSS接收机确定所述车顶天线模块位姿状态。
进一步的，第i时刻的车身惯导位姿状态估计值xci通过第i-1时刻的车身惯导位姿状态估计值xci-1和所述xci和xci-1之间的预积分因子zIci确定。
进一步的，第i时刻的车顶天线模块位姿状态估计值xri通过第i-1时刻的车顶天线模块位姿状态估计值xri-1和所述xri和xri-1之间的车顶天线内置惯导预积分因子zIri确定。
优选的，所述确定所述车身惯导位姿状态和所述车顶天线模块位姿状态之间的柔性连接因子包括：
预先标定车顶天线到车身IMU的外参初值，并预先确定车顶天线到车身IMU的柔性约束代价函数，在线标定车顶天线与车身IMU的相对位姿值。
优选的，第i时刻的柔性连接因子Ti的转移矩阵为：
Figure BDA0003054288030000021
其中，Ri为旋转矩阵
Figure BDA0003054288030000022
和
Figure BDA0003054288030000023
均为3维列向量；R0i为姿态初始标定值，
Figure BDA0003054288030000024
为在线标定值，
Figure BDA0003054288030000025
为相对晃动值，
Figure BDA0003054288030000026
表示旋转叠加；
ti为平移向量ti＝t0i+τ0i+τsi，t0i位移为初始标定值，τ0i为在线标定值，τsi为相对晃动值。
优选的，通过下列代价函数确定相对晃动值的最优估计值：
Figure BDA0003054288030000031
Figure BDA0003054288030000032
f(φsi)表示姿态晃动代价函数，exp()表示自然指数运算，
Figure BDA0003054288030000033
是正常的姿态晃动范围，λ是姿态异常晃动的惩罚参数
f(τsi)表示位移振动的代价函数，exp()表示自然指数运算，cτ是位移，γ是位移异常晃动的惩罚参数。
使用本发明提供的位姿融合估计方法，充分利用晃动阈值对IMU的校正，其中的晃动/振动阈值(容许度)松弛式的设定不会影响对IMU漂移量的矫正。由于IMU因子对IMU修正的是量测值的漂移量，并且将其建立了随机游走(缓变)的动态模型，因而在限定阈值的晃动下依旧可以对IMU的漂移进行修正，并且不会发生在晃动阈值边缘滞留的问题，从而提高了位姿融合的精度。
第二方面，本申请实施例还提供一种位姿融合估计装置，包括：
车身惯导模块，被配置用于确定车身惯导位姿状态；
车顶天线模块，被配置用于确定车顶天线模块位姿状态；
算法模块，被配置用于确定所述车身惯导位姿状态和所述车顶天线模块位姿状态之间的柔性连接因子，根据所述车身惯导位姿状态、所述车顶天线模块位姿状态和所述柔性连接因子，确定车顶天线的定位因子。
第三方面，本申请实施例还提供一种位姿融合估计装置，包括：存储器、处理器和用户接口；
所述存储器，用于存储计算机程序；
所述用户接口，用于与用户实现交互；
所述处理器，用于读取所述存储器中的计算机程序，所述处理器执行所述计算机程序时，实现本发明提供的位姿融合估计方法。
第四方面，本申请实施例还提供一种处理器可读存储介质，所述处理器可读存储介质存储有计算机程序，所述处理器执行所述计算机程序时实现本发明提供的位姿融合估计方法。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简要介绍，显而易见地，下面描述中的附图仅是本申请的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为车身主IMU封装模块和GNSS接收天线安装示意图；
图2为本申请实施例提供的位姿融合估计方法示意图；
图3为本申请实施例提供的融合定位因子关系示意图；
图4为本申请实施例提供的位姿融合估计装置示意图；
图5为本申请实施例提供的另一种位姿融合估计装置结构示意图。
具体实施方式
为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部份实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
下面对文中出现的一些词语进行解释：
1、本发明实施例中术语“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。
2、本申请实施例中术语“多个”是指两个或两个以上，其它量词与之类似。
3、IMU，Inertial Measurement Unit的简称，即惯性测量仪，或者惯性测量单元，是测量物体三轴姿态角(或角速率)以及加速度的装置。通常，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度。
4、MCU，Microcontroller Unit，即微控制单元，或微计算单元，或单片机，是把中央处理器(Central Process Unit，CPU)的频率与规格做适当缩减，并将内存(memory)、计数器(Timer)、USB、A/D转换、UART、PLC、DMA等外围接口驱动电路都整合在单一芯片上，形成芯片级的计算机。
5、GNSS，全球卫星导航系统。
6、RTK，Real-time kinematic的简称，即实时载波相位差分技术。
矿车在作业过程中，由于地形恶劣，矿车需要高速实时采集车身自身姿态信息(例如矿车的速度，加速度等)，还要高速采集自身的位置信息(例如矿车的经纬度信息，高度信息等)，然后根据自身姿态信息和位置信息，快速做出导航决策。如图1所示为车身主IMU封装模块和GNSS接收天线安装示意图。由于激光雷达和毫米波雷达等传感器均安装在车辆主框架上，因而车身主IMU也应安装在主框架上；为了提高GNSS信号接收性能，须将GNSS接收天线安装在驾驶室外的顶部。驾驶室与主框架之间一般是柔性连接(例如，弹簧+阻尼器)，在矿山这类恶劣地形下晃动将更为明显，导致GNSS天线与IMU间的位姿估计误差加大。
针对上述技术问题，本发明提出了一种位姿融合方法、装置及存储介质，用于提高位姿融合估计的精度。
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，并不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
需要说明的是，本申请实施例的展示顺序仅代表实施例的先后顺序，并不代表实施例所提供的技术方案的优劣。
实施例一
参见图2，本申请实施例提供的一种位姿融合估计方法示意图，如图2所示，该方法包括步骤S201到S203：
S201、确定车身惯导位姿状态和车顶天线模块位姿状态；
S202、确定所述车身惯导位姿状态和所述车顶天线模块位姿状态之间的柔性连接因子；
S203、根据所述车身惯导位姿状态、所述车顶天线模块位姿状态和所述柔性连接因子，确定车顶天线的定位因子。
通过本实施例的方法，先确定车身惯导位姿状态和车顶天线模块位姿状态，然后根据车身惯导位姿状态和车顶天线模块位姿状态之间的柔性连接因子，对车顶天线的定位因子进行融合估计，从而提高位姿估计的精度。
如图3所示，为位姿融合估计关系示意图，其中，xci表示车身惯导位姿状态估计值，xri表示车顶天线模块位姿状态估计值。ZIci表示车身IMU预积分因子，ZIri表示车顶天线模块中的低成本IMU预积分因子，ZGi表示车顶天线的定位因子。柔性连接因子Ti表示xci到xri的转换关系。其中i表示时序采样编号。
需要说明的是，ZGi即为车顶GNSS天线的RTK定位因子，通过车顶天线及GNSS接收模块得到。
需要说明的是，第i时刻的车身惯导位姿状态估计值xci通过第i-1时刻的车身惯导位姿状态估计值xci-1和所述xci和xci-1之间的预积分因子zIci确定，即在时序上，当前时刻的车身惯导位姿，通过前一时刻的车身惯导位姿，通过预积分因子得到。
需要说明的是，第i时刻的车顶天线模块位姿状态估计值xri通过第i-1时刻的车顶天线模块位姿状态估计值xri-1和所述xri和xri-1之间的车顶天线内置惯导预积分因子zIri和上述ZGi因子确定。即在时序上，当前时刻的车顶天线模块位姿状态，通过前一时刻的车顶天线模块位姿状态，通过预积分因子和ZGi因子得到。
作为一种优选示例，车顶天线模块位姿状态可通过安装在车顶的全球导航卫星系统GNSS接收机确定。
作为一种优选示例，本实施例中，柔性连接因子Ti，通过预先标定的初值，在线标定叠加值和晃动范围的约束函数确定。即柔性连接因子Ti由一个初值、一个在线标定叠加值和一个限定晃动范围的约束函数组成。需要说明的是，初值可以是预先标定的。即预先标定车顶天线到车身IMU的外参初值，并预先确定车顶天线到车身IMU的柔性约束代价函数，在线标定车顶天线与车身IMU的相对位姿值。
作为一种优选示例，本发明实施例中，柔性连接因子转移矩阵为：
Figure BDA0003054288030000071
Ri为旋转矩阵，旋转矩阵为：
Figure BDA0003054288030000072
其中，
Figure BDA0003054288030000073
和
Figure BDA0003054288030000074
均为3维列向量。
其中，R0i为姿态初始标定值，
Figure BDA0003054288030000075
为在线标定值，
Figure BDA0003054288030000076
为相对晃动值，
Figure BDA0003054288030000077
表示旋转叠加。
ti为平移向量，平移向量为：ti＝t0i+τ0i+τsi，其中，t0i为位移初始标定值，τ0i为在线标定值，τsi为相对晃动值。
作为一种优选示例，通过下列代价函数确定相对晃动值的最优估计值：
Figure BDA0003054288030000078
Figure BDA0003054288030000079
其中，
Figure BDA0003054288030000081
表示姿态晃动代价函数，exp()表示自然指数运算，
Figure BDA0003054288030000082
是正常的姿态晃动范围，λ是姿态异常晃动的惩罚参数；
f(τsi)表示位移振动的代价函数，exp()表示自然指数运算，cτ是位移xxx，γ是位移异常振动的惩罚参数。
需要说明的是，上述代价函数，可用于图3中Ti的因子构建。作为一种优选示例，因子图框架由gtsam直接提供，只需构建因子中的代价函数以及代价函数相对
Figure BDA0003054288030000083
和τ0i的雅克比矩阵。
作为一种优选示例，姿态晃动代价函数
Figure BDA0003054288030000084
和位移振动的代价函数f(τsi)表现形式可为：
Figure BDA0003054288030000085
Figure BDA0003054288030000086
其中，Cfacror表示因子的代价。
作为一种优选示例，所述IMU预积分代价函数和车顶天线定位(GNSS+内置IMU)代价函数均参考gtsam中的IMU因子和GPS因子，在此不再赘述。
以上姿态与位移指车身IMU与车顶天线模块间的值。
本实施例中，充分利用晃动阈值对IMU的校正，其中的晃动/振动阈值(容许度)松弛式的设定不会影响对IMU漂移量的矫正。由于IMU因子对IMU修正的是量测值的漂移量，并且将其建立了随机游走(缓变)的动态模型，因而在限定阈值的晃动下依旧可以对IMU的漂移进行修正，并且不会发生在晃动阈值边缘滞留的问题，从而提高了位姿融合的精度。
实施例二
基于同一个发明构思，本发明实施例还提供了一种位姿融合估计装置，如图4所示，该装置包括：
车身惯导模块401，被配置用于确定车身惯导位姿状态；
车顶天线模块403，被配置用于确定车顶天线模块位姿状态；
柔性连接模块402，被配置用于确定所述车身惯导位姿状态和所述车顶天线模块位姿状态之间的柔性连接因子，根据所述车身惯导位姿状态、所述车顶天线模块位姿状态和所述柔性连接因子，确定车顶天线的定位因子。
作为一种优选示例，车身惯导模块401还被配置用于通过安装在车辆主框架上的位姿传感器确定所述车身惯导位姿状态。优选的，位姿传感器包括激光雷达和/或者毫米波雷达。优选的，第i时刻的车身惯导位姿状态估计值xci通过第i-1时刻的车身惯导位姿状态估计值xci-1和所述xci和xci-1之间的预积分因子zIci确定。
作为一种优选示例，车顶天线模块403还被配置用于通过安装在车顶的全球导航卫星系统GNSS接收机确定所述车顶天线模块位姿状态。优选的，第i时刻的车顶天线模块位姿状态估计值xri通过第i-1时刻的车顶天线模块位姿状态估计值xri-1和所述xri和xri-1之间的预积分因子zIri确定。
作为一种优选示例，柔性连接模块402还被配置用于通过预先标定的初值，在线标定叠加值和晃动范围的约束函数确定所述柔性连接因子。
需要说明的是，本实施例提供的车身惯导模块401，能实现实施例一中步骤S201包含的确定车身惯导位姿状态的全部功能，解决相同技术问题，达到相同技术效果，在此不再赘述；
需要说明的是，本实施例提供的柔性连接模块402，能实现实施例一中步骤S202和S203包含的全部功能，解决相同技术问题，达到相同技术效果，在此不再赘述；
需要说明的是，本实施例提供的车顶天线模块403，能实现实施例一中步骤S201包含的确定车顶天线模块位姿状态的全部功能，解决相同技术问题，达到相同技术效果，在此不再赘述；
需要说明的是，实施例二提供的装置与实施例一提供的方法属于同一个发明构思，解决相同的技术问题，达到相同的技术效果，实施例二提供的装置能实现实施例一的所有方法，相同之处不再赘述。
实施例三
基于同一个发明构思，本发明实施例还提供了一种位姿融合估计装置，如图5所示，该装置包括：
包括存储器502、处理器501和用户接口503；
所述存储器502，用于存储计算机程序；
所述用户接口503，用于与用户实现交互；
所述处理器501，用于读取所述存储器502中的计算机程序，所述处理器501执行所述计算机程序时，实现：
确定车身惯导位姿状态和车顶天线模块位姿状态；
确定所述车身惯导位姿状态和所述车顶天线模块位姿状态之间的柔性连接因子；
根据所述车身惯导位姿状态、所述车顶天线模块位姿状态和所述柔性连接因子，确定车顶天线的定位因子。
其中，在图5中，总线架构可以包括任意数量的互联的总线和桥，具体由处理器501代表的一个或多个处理器和存储器502代表的存储器的各种电路链接在一起。总线架构还可以将诸如外围设备、稳压器和功率管理电路等之类的各种其他电路链接在一起，这些都是本领域所公知的，因此，本文不再对其进行进一步描述。总线接口提供接口。处理器501负责管理总线架构和通常的处理，存储器502可以存储处理器501在执行操作时所使用的数据。
处理器501可以是CPU、ASIC、FPGA或CPLD，处理器501也可以采用多核架构。
处理器501执行存储器502存储的计算机程序时，实现实施例一中的任一位姿融合估计方法。
需要说明的是，实施例三提供的装置与实施例一提供的方法属于同一个发明构思，解决相同的技术问题，达到相同的技术效果，实施例三提供的装置能实现实施例一的所有方法，相同之处不再赘述。
本申请还提出一种处理器可读存储介质。其中，该处理器可读存储介质存储有计算机程序，所述处理器执行所述计算机程序时实现实施例一中的任一位姿融合估计方法。
需要说明的是，本申请实施例中对单元的划分是示意性的，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

Claims (11)

1.一种位姿融合估计方法，其特征在于，包括：
确定车身惯导位姿状态和车顶天线模块位姿状态；
确定所述车身惯导位姿状态和所述车顶天线模块位姿状态之间的柔性连接因子；
根据所述车身惯导位姿状态、所述车顶天线模块位姿状态和所述柔性连接因子，确定车顶天线的定位因子。
2.根据权利要求1所述的方法，其特征在于，所述确定车身惯导位姿状态包括：
通过安装在车辆主框架上的位姿传感器确定所述车身惯导位姿状态。
3.根据权利要求1到2之一所述的方法，其特征在于，包括：
第i时刻的车身惯导位姿状态估计值xci通过第i-1时刻的车身惯导位姿状态估计值xci-1和所述xci和xci-1之间的预积分因子zIci确定。
4.根据权利要求1所述的方法，其特征在于，所述确定车顶天线模块位姿状态包括：
通过安装在车顶的全球导航卫星系统GNSS接收机确定所述车顶天线模块位姿状态。
5.根据权利要求4所述的方法，其特征在于，包括：
第i时刻的车顶天线模块位姿状态估计值xri通过第i-1时刻的车顶天线模块位姿状态估计值xri-1和所述xri和xri-1之间的车顶天线内置惯导预积分因子zIri确定。
6.根据权利要求1所述的方法，其特征在于，所述确定所述车身惯导位姿状态和所述车顶天线模块位姿状态之间的柔性连接因子包括：
预先标定车顶天线到车身IMU的外参初值，并预先确定车顶天线到车身IMU的柔性约束代价函数，在线标定车顶天线与车身IMU的相对位姿值。
7.根据权利要求6所述的方法，其特征在于，包括：
第i时刻的柔性连接因子Ti的转移矩阵为：
Figure FDA0003054288020000021
其中，Ri为旋转矩阵
Figure FDA0003054288020000022
Figure FDA0003054288020000023
和
Figure FDA0003054288020000024
均为3维列向量；R0i为姿态初始标定值，
Figure FDA0003054288020000025
为姿态在线标定值，
Figure FDA0003054288020000026
为姿态相对晃动值，
Figure FDA00030542880200000210
表示姿态旋转叠加；
ti为平移向量ti＝t0i+τ0i+τsi，t0i为位移初始标定值，τ0i为位移在线标定值，τsi为位移相对振动值。
8.根据权利要求7所述的方法，其特征在于，包括：
通过下列代价函数确定相对晃动值的最优估计值：
Figure FDA0003054288020000027
Figure FDA0003054288020000028
f(φsi)表示姿态晃动代价函数，exp()表示自然指数运算，
Figure FDA0003054288020000029
是正常的姿态晃动范围，λ是姿态异常晃动的惩罚参数；
f(τsi)表示位移振动的代价函数，exp()表示自然指数运算，cτ是位移，γ是位移异常晃动的惩罚参数。
9.一种位姿融合估计装置，其特征在于，包括：
车身惯导模块，被配置用于确定车身惯导位姿状态；
车顶天线模块，被配置用于确定车顶天线模块位姿状态；
算法模块，被配置用于确定所述车身惯导位姿状态和所述车顶天线模块位姿状态之间的柔性连接因子，根据所述车身惯导位姿状态、所述车顶天线模块位姿状态和所述柔性连接因子，确定车顶天线的定位因子。
10.一种位姿融合估计装置，其特征在于，包括存储器、处理器和用户接口；
所述存储器，用于存储计算机程序；
所述用户接口，用于与用户实现交互；
所述处理器，用于读取所述存储器中的计算机程序，所述处理器执行所述计算机程序时，实现如权利要求1到8之一所述的位姿融合估计方法。
11.一种处理器可读存储介质，其特征在于，所述处理器可读存储介质存储有计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至8之一所述的位姿融合估计方法。
CN202110495613.7A 2021-05-07 2021-05-07 一种位姿融合估计方法、装置及存储介质 Active CN113203413B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110495613.7A CN113203413B (zh) 2021-05-07 2021-05-07 一种位姿融合估计方法、装置及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110495613.7A CN113203413B (zh) 2021-05-07 2021-05-07 一种位姿融合估计方法、装置及存储介质

Publications (2)

Publication Number Publication Date
CN113203413A true CN113203413A (zh) 2021-08-03
CN113203413B CN113203413B (zh) 2024-02-27

Family

ID=77029656

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110495613.7A Active CN113203413B (zh) 2021-05-07 2021-05-07 一种位姿融合估计方法、装置及存储介质

Country Status (1)

Country Link
CN (1) CN113203413B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE19939345A1 (de) * 1999-08-19 2001-03-29 Joerg F Wagner Verfahren und Vorrichtung zur Navigation und Bewegungssteuerung von nicht starren Gegenständen
US20170254877A1 (en) * 2016-03-07 2017-09-07 Raytheon Company Geolocation on a single platform having flexible portions
CN111551973A (zh) * 2020-04-16 2020-08-18 北京踏歌智行科技有限公司 一种露天矿无人驾驶惯导系统的故障检测和矫正方法
CN112344928A (zh) * 2020-11-14 2021-02-09 上海伯镭智能科技有限公司 一种用于矿车的导航系统

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE19939345A1 (de) * 1999-08-19 2001-03-29 Joerg F Wagner Verfahren und Vorrichtung zur Navigation und Bewegungssteuerung von nicht starren Gegenständen
US20170254877A1 (en) * 2016-03-07 2017-09-07 Raytheon Company Geolocation on a single platform having flexible portions
CN111551973A (zh) * 2020-04-16 2020-08-18 北京踏歌智行科技有限公司 一种露天矿无人驾驶惯导系统的故障检测和矫正方法
CN112344928A (zh) * 2020-11-14 2021-02-09 上海伯镭智能科技有限公司 一种用于矿车的导航系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
籍颖 等: "基于Kalman滤波农用车辆导航定位方法", 《农业机械学报》, vol. 40, no. 1, pages 13 - 17 *

Also Published As

Publication number Publication date
CN113203413B (zh) 2024-02-27

Similar Documents

Publication Publication Date Title
US9494428B2 (en) Attitude determination method, position calculation method, and attitude determination device
CN105300379B (zh) 一种基于加速度的卡尔曼滤波姿态估计方法及系统
JP5270184B2 (ja) 衛星航法／推測航法統合測位装置
EP1760431B1 (en) Inertial navigation system with a plurality of Kalman filters and vehicle equipped with such a system
CA1222153A (en) Inertial systems
Parsa et al. Design and implementation of a mechatronic, all-accelerometer inertial measurement unit
US8019542B2 (en) Heading stabilization for aided inertial navigation systems
CN110017837B (zh) 一种姿态抗磁干扰的组合导航方法
CN109540135B (zh) 水田拖拉机位姿检测和偏航角提取的方法及装置
CN105973243A (zh) 一种车载惯性导航系统
US10267638B2 (en) Method and system for adapting a navigation system
CN103512584A (zh) 导航姿态信息输出方法、装置及捷联航姿参考系统
US10126130B2 (en) Device for detecting the attitude of motor vehicles
JP3380404B2 (ja) 移動検出装置
CN106646569A (zh) 一种导航定位方法及设备
CN113203413B (zh) 一种位姿融合估计方法、装置及存储介质
EP4300042A1 (en) Navigation information processing method and apparatus, electronic device, and storage medium
CN115900705A (zh) 一种紧耦合陆地组合导航方法、装置、计算机设备和介质
CN115856922A (zh) 一种松耦合陆地组合导航方法、装置、计算机设备和介质
CN114001730B (zh) 融合定位方法、装置、计算机设备和存储介质
CN116380119A (zh) 组合导航的校准方法、装置和系统
CN108931247B (zh) 一种导航方法和装置
Luque et al. Low-cost monitoring system of sensors for evaluating dynamic solicitations of semitrailer structure
CN111002991B (zh) 一种车载导航信息处理的方法、装置及计算机存储介质
CN117076824B (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