本申请涉及自动驾驶领域，尤其涉及一种位姿融合估计方法、装置和存储介质。
背景技术：
在实时动态载波相位差分惯性测量仪real-timekinematic-inertialmeasurementunit，rtk-imu，的组合导航方面，无论是松组合还是紧组合，目前的方案均须假设全球导航卫星系统gnss接收天线与imu之间是相对固定的刚性连接。
然而，在矿车应用中，全车惯性测量仪imu(inertialmeasurementunit)的安装位置从惯导设备安全性及底盘控制的需求考虑，应当安装在矿车的底盘上。而卫星导航天线实时动态载波相位差分rtk(real-timekinematic)天线需要安装在不易被遮挡的位置，如车顶。由于矿车为典型的承载式车身，因而底盘与车身间为柔性连接将发生相对晃动，在矿山这类恶劣地形下晃动将更为明显，在位姿融合中就会因为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的转移矩阵为：
其中，ri为旋转矩阵
ti为平移向量ti＝t0i τ0i τsi，t0i位移为初始标定值，τ0i为在线标定值，τsi为相对晃动值。
优选的，通过下列代价函数确定相对晃动值的最优估计值：
f(φsi)表示姿态晃动代价函数，exp()表示自然指数运算，
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，inertialmeasurementunit的简称，即惯性测量仪，或者惯性测量单元，是测量物体三轴姿态角(或角速率)以及加速度的装置。通常，一个imu包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度。
4、mcu，microcontrollerunit，即微控制单元，或微计算单元，或单片机，是把中央处理器(centralprocessunit，cpu)的频率与规格做适当缩减，并将内存(memory)、计数器(timer)、usb、a/d转换、uart、plc、dma等外围接口驱动电路都整合在单一芯片上，形成芯片级的计算机。
5、gnss，全球卫星导航系统。
6、rtk，real-timekinematic的简称，即实时载波相位差分技术。
矿车在作业过程中，由于地形恶劣，矿车需要高速实时采集车身自身姿态信息(例如矿车的速度，加速度等)，还要高速采集自身的位置信息(例如矿车的经纬度信息，高度信息等)，然后根据自身姿态信息和位置信息，快速做出导航决策。如图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的相对位姿值。
作为一种优选示例，本发明实施例中，柔性连接因子转移矩阵为：
ri为旋转矩阵，旋转矩阵为：
其中，r0i为姿态初始标定值，
ti为平移向量，平移向量为：ti＝t0i τ0i τsi，其中，t0i为位移初始标定值，τ0i为在线标定值，τsi为相对晃动值。
作为一种优选示例，通过下列代价函数确定相对晃动值的最优估计值：
其中，
f(τsi)表示位移振动的代价函数，exp()表示自然指数运算，cτ是位移xxx，γ是位移异常振动的惩罚参数。
需要说明的是，上述代价函数，可用于图3中ti的因子构建。作为一种优选示例，因子图框架由gtsam直接提供，只需构建因子中的代价函数以及代价函数相对
作为一种优选示例，姿态晃动代价函数
其中，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存储的计算机程序时，实现实施例一中的任一位姿融合估计方法。
需要说明的是，实施例三提供的装置与实施例一提供的方法属于同一个发明构思，解决相同的技术问题，达到相同的技术效果，实施例三提供的装置能实现实施例一的所有方法，相同之处不再赘述。
本申请还提出一种处理器可读存储介质。其中，该处理器可读存储介质存储有计算机程序，所述处理器执行所述计算机程序时实现实施例一中的任一位姿融合估计方法。
需要说明的是，本申请实施例中对单元的划分是示意性的，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。
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的转移矩阵为：
其中，ri为旋转矩阵
ti为平移向量ti＝t0i τ0i τsi，t0i为位移初始标定值，τ0i为位移在线标定值，τsi为位移相对振动值。
8.根据权利要求7所述的方法，其特征在于，包括：
通过下列代价函数确定相对晃动值的最优估计值：
f(φsi)表示姿态晃动代价函数，exp()表示自然指数运算，
f(τsi)表示位移振动的代价函数，exp()表示自然指数运算，cτ是位移，γ是位移异常晃动的惩罚参数。
9.一种位姿融合估计装置，其特征在于，包括：
车身惯导模块，被配置用于确定车身惯导位姿状态；
车顶天线模块，被配置用于确定车顶天线模块位姿状态；
算法模块，被配置用于确定所述车身惯导位姿状态和所述车顶天线模块位姿状态之间的柔性连接因子，根据所述车身惯导位姿状态、所述车顶天线模块位姿状态和所述柔性连接因子，确定车顶天线的定位因子。
10.一种位姿融合估计装置，其特征在于，包括存储器、处理器和用户接口；
所述存储器，用于存储计算机程序；
所述用户接口，用于与用户实现交互；
所述处理器，用于读取所述存储器中的计算机程序，所述处理器执行所述计算机程序时，实现如权利要求1到8之一所述的位姿融合估计方法。
11.一种处理器可读存储介质，其特征在于，所述处理器可读存储介质存储有计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至8之一所述的位姿融合估计方法。
技术总结