一种基于偏振光/地磁航向约束的位姿估计方法及系统

摘要

本发明公开一种基于偏振光/地磁航向约束的位姿估计方法及系统，该方法在视觉惯性里程计的基础上，加入了偏振光罗盘和磁罗盘的双航向约束，偏振光罗盘通过模仿沙漠蚂蚁感知偏振光的结构和功能，获取自身航向信息，磁罗盘通过测量地磁矢量获取航向信息，偏振光罗盘和磁罗盘均具备误差不随时间积累的优点，但偏振光罗盘易受天气的影响，磁罗盘易受磁场干扰的影响，本发明通过偏振光罗盘和磁罗盘的双航向约束可以大幅提高系统的航向稳定性与抗干扰能力；同时，本发明中偏振光罗盘和磁罗盘测量得到的航向信息是相对于地理北向的，可以用于解决视觉惯性里程计没有绝对航向的问题，提高位姿估计的精度。

权利要求

1.一种基于偏振光/地磁航向约束的位姿估计方法，其特征在于，包括：利用视觉惯性里程计获取当前时刻载体的姿态和位置信息；

利用偏振光罗盘采集载体的偏振图像，在所述偏振图像的偏振角模式和偏振度模式下，获得载体的第一航向信息；

利用磁罗盘采集载体的三轴磁分量和水平姿态角，在所述三轴磁分量和水平姿态角下，获得载体的第二航向信息；

根据所述姿态和位置信息、第一航向信息和第二航向信息，优化载体系统状态矢量，获得载体的位置、速度和姿态，实现载体位姿估计，包括：定义全局优化的系统状态矢量为

式中， 为系统的全状态矢量； 为视觉惯性里程计输出的第k个状态，包括在世界坐标系下的位置 和姿态四元数 ； 为总的待优化状态数目；

通过对所述姿态和位置信息、第一航向信息和第二航向信息进行残差求和，优化载体系统状态矢量，获得载体的位置、速度和姿态，实现载体位姿估计，式中， 为相对测量的残差， 为视觉惯性里程计的输出集合， 为相邻状态的相对测量； 为磁罗盘的测量残差， 为磁罗盘航向测量的集合， 为磁罗盘测量； 为偏振光罗盘的测量残差， 为偏振光罗盘航向测量的集合， 为偏振光罗盘测量； 为载体系统的全状态矢量。

2.如权利要求1所述的基于偏振光/地磁航向约束的位姿估计方法，其特征在于，利用偏振光罗盘采集载体的偏振图像，在所述偏振图像的偏振角模式和偏振度模式下，获得载体的第一航向信息，包括：利用偏振光罗盘采集载体的偏振图像，根据所述偏振图像计算每个像素的偏振度和偏振角；

根据所述偏振度和所述偏振角，获得太阳方向矢量的最优估计；

根据天文年历和所述太阳方向矢量的最优估计，获得载体的第一航向信息。

3.如权利要求2所述的基于偏振光/地磁航向约束的位姿估计方法，其特征在于，利用偏振光罗盘采集载体的偏振图像，根据所述偏振图像计算每个像素的偏振度和偏振角，包括：利用偏振光罗盘采集载体的偏振图像，根据所述偏振图像计算每个像素的偏振度和偏振角为：其中，

式中， 为偏振度； 为偏振角； 、 、 为Stokes向量中的前三项，分别描述入射光的光强、入射光在0°偏振方向上的分量和45°偏振方向偏振分量； 为与某个方向的偏振片对应的CCD测量得到的入射光光强，x为0º、45º、90º或135º。

4.如权利要求1所述的基于偏振光/地磁航向约束的位姿估计方法，其特征在于，利用磁罗盘采集载体的三轴磁分量和水平姿态角，在所述三轴磁分量和水平姿态角下，获得载体的第二航向信息，包括：利用磁罗盘测量载体周围的磁场分布，通过视觉惯性里程计获取的加速度，得到载体的水平姿态角；所述水平姿态角包括滚动角和俯仰角；

根据所述载体的水平姿态角，计算载体的三轴磁分量；

根据所述载体的三轴磁分量，计算载体的地理航向角；

根据地理北向和地磁北向，获得地磁偏角；

利用所述地磁偏角对所述地理航向角进行修正，获得载体的第二航向信息。

5.如权利要求4所述的基于偏振光/地磁航向约束的位姿估计方法，其特征在于，根据所述载体的水平姿态角，计算载体的三轴磁分量，包括：根据所述载体的水平姿态角，计算载体的三轴磁分量为：式中， 、 和 为磁场在x、y、z三个方向的磁分量； 为三轴磁分量数据； 为滚动角； 为俯仰角。

6.如权利要求4或5所述的基于偏振光/地磁航向约束的位姿估计方法，其特征在于，根据所述载体的三轴磁分量，计算载体的地理航向角，包括：根据所述载体的三轴磁分量，计算载体的地理航向角为：式中， 和 为磁场在x和z方向的磁分量； 为地理航向角。

7.一种基于偏振光/地磁航向约束的位姿估计系统，其特征在于，包括：视觉惯性里程计模块，用于利用视觉惯性里程计获取当前时刻载体的姿态和位置信息；

偏振光罗盘模块，用于利用偏振光罗盘采集载体的偏振图像，在所述偏振图像的偏振角模式和偏振度模式下，获得载体的第一航向信息；

磁罗盘模块，用于利用磁罗盘采集载体的三轴磁分量和水平姿态角，在所述三轴磁分量和水平姿态角下，获得载体的第二航向信息；

位姿估计模块，用于根据所述姿态和位置信息、第一航向信息和第二航向信息，优化载体系统状态矢量，获得载体的位置、速度和姿态，实现载体位姿估计，包括：定义全局优化的系统状态矢量为

式中， 为系统的全状态矢量； 为视觉惯性里程计输出的第k个状态，包括在世界坐标系下的位置 和姿态四元数 ； 为总的待优化状态数目；

通过对所述姿态和位置信息、第一航向信息和第二航向信息进行残差求和，优化载体系统状态矢量，获得载体的位置、速度和姿态，实现载体位姿估计，式中， 为相对测量的残差， 为视觉惯性里程计的输出集合， 为相邻状态的相对测量； 为磁罗盘的测量残差， 为磁罗盘航向测量的集合， 为磁罗盘测量； 为偏振光罗盘的测量残差， 为偏振光罗盘航向测量的集合，为偏振光罗盘测量； 为载体系统的全状态矢量。

8.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1 6中任一项所述方法的步骤。

~

9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1 6中任一项所述的方法的步骤。

~

说明书

一种基于偏振光/地磁航向约束的位姿估计方法及系统

技术领域

[0001] 本发明涉及运动估计技术领域，尤其是一种基于偏振光/地磁航向约束的位姿估计方法及系统。

背景技术

[0002] 无人系统是完成现代化作业、抢险救灾等任务的重要装备，往往需要在复杂开放的环境下进行大范围、长航时的自主运行。目前以无人车和无人机为代表的无人系统对卫星导航系统的依赖性较大，研究高精度的自主导航系统是当前亟待解决的问题。

[0003] 视觉惯性里程计可实现视觉和惯性的组合导航，惯性和视觉之间具有很强的互补性，二者融合后可以提升定位定向的准确性和稳定性。然而，视觉惯性里程计输出的航向是相对于载体初始方位的，在长航时运行后，存在航向误差随时间积累的问题。

发明内容

[0004] 本发明提供一种基于偏振光/地磁航向约束的位姿估计方法及系统，用于克服现有技术中在长航时运行后存在航向误差随时间积累等缺陷。

[0005] 为实现上述目的，本发明提出一种基于偏振光/地磁航向约束的位姿估计方法，包括：

[0006] 利用视觉惯性里程计获取当前时刻载体的姿态和位置信息；

[0007] 利用偏振光罗盘采集载体的偏振图像，在所述偏振图像的偏振角模式和偏振度模式下，获得载体的第一航向信息；

[0008] 利用磁罗盘采集载体的三轴磁分量和水平姿态角，在所述三轴磁分量和水平姿态角下，获得载体的第二航向信息；

[0009] 根据所述姿态和位置信息、第一航向信息和第二航向信息，优化载体系统状态矢量，获得载体的位置、速度和姿态，实现载体位姿估计。

[0010] 为实现上述目的，本发明还提出一种基于偏振光/地磁航向约束的位姿估计系统，包括：

[0011] 视觉惯性里程计模块，用于利用视觉惯性里程计获取当前时刻载体的姿态和位置信息；

[0012] 偏振光罗盘模块，用于利用偏振光罗盘采集载体的偏振图像，在所述偏振图像的偏振角模式和偏振度模式下，获得载体的第一航向信息；

[0013] 磁罗盘模块，用于利用磁罗盘采集载体的三轴磁分量和水平姿态角，在所述三轴磁分量和水平姿态角下，获得载体的第二航向信息；

[0014] 位姿估计模块，用于根据所述姿态和位置信息、第一航向信息和第二航向信息，优化载体系统状态矢量，获得载体的位置、速度和姿态，实现载体位姿估计。

[0015] 为实现上述目的，本发明还提出一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述所述方法的步骤。

[0016] 为实现上述目的，本发明还提出一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述所述的方法的步骤。

[0017] 与现有技术相比，本发明的有益效果有：

[0018] 本发明提供的基于偏振光/地磁航向约束的位姿估计方法，在视觉惯性里程计的基础上，加入了偏振光罗盘和磁罗盘的双航向约束，偏振光罗盘通过模仿沙漠蚂蚁感知偏振光的结构和功能，获取自身航向信息，磁罗盘通过测量地磁矢量获取航向信息，偏振光罗盘和磁罗盘均具备误差不随时间积累的优点，但偏振光罗盘易受天气的影响，磁罗盘易受磁场干扰的影响，本发明通过偏振光罗盘和磁罗盘的双航向约束可以大幅提高系统的航向稳定性与抗干扰能力；同时，本发明中偏振光罗盘和磁罗盘测量得到的航向信息是相对于地理北向的，可以用于解决视觉惯性里程计没有绝对航向的问题，提高位姿估计的精度。

附图说明

[0019] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。

[0020] 图1为本发明提供的基于偏振光/地磁航向约束的位姿估计方法的流程图；

[0021] 图2为本发明中偏振光罗盘上阵列式偏振片的结构示意图；

[0022] 图3为本发明中一阶瑞利散射模型原理图；

[0023] 图4为本发明中磁罗盘原理示意图。

[0024] 本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

[0025] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

[0026] 另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

[0027] 如图1所示，本发明提出一种基于偏振光/地磁航向约束的位姿估计方法，包括：

[0028] 101：利用视觉惯性里程计获取当前时刻载体的姿态和位置信息；

[0029] 视觉惯性里程计，包括惯性测量单元和视觉传感器。采用现有的视觉惯性里程计即可。

[0030] 载体，为由无人系统控制运行的装备，如无人车、无人机等。

[0031] 102：利用偏振光罗盘采集载体的偏振图像，在所述偏振图像的偏振角模式和偏振度模式下，获得载体的第一航向信息；

[0032] 偏振光罗盘通过模仿沙漠蚂蚁感知偏振光的结构和功能，获取自身航向信息，具有误差不随时间积累、稳定性高的优点，可以为载体的导航传感器提供稳定的航向约束。

[0033] 103：利用磁罗盘采集载体的三轴磁分量和水平姿态角，在所述三轴磁分量和水平姿态角下，获得载体的第二航向信息；

[0034] 磁罗盘通过测量地磁矢量获取航向信息。

[0035] 104：根据所述姿态和位置信息、第一航向信息和第二航向信息，优化载体系统状态矢量，获得载体的位置、速度和姿态，实现载体位姿估计。

[0036] 本发明提供的基于偏振光/地磁航向约束的位姿估计方法，在视觉惯性里程计的基础上，加入了偏振光罗盘和磁罗盘的双航向约束，偏振光罗盘通过模仿沙漠蚂蚁感知偏振光的结构和功能，获取自身航向信息，磁罗盘通过测量地磁矢量获取航向信息，偏振光罗盘和磁罗盘均具备误差不随时间积累的优点，但偏振光罗盘易受天气的影响，磁罗盘易受磁场干扰的影响，本发明通过偏振光罗盘和磁罗盘的双航向约束可以大幅提高系统的航向稳定性与抗干扰能力；同时，本发明中偏振光罗盘和磁罗盘测量得到的航向信息是相对于地理北向的，可以用于解决视觉惯性里程计没有绝对航向的问题，提高位姿估计的精度。

[0037] 在其中一个实施例中，对于步骤101，参考论文（VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator）进行具体的操作，从而获取当前时刻载体的姿态和位置信息。

[0038] 在下一个实施例中，对于步骤102，利用偏振光罗盘采集载体的偏振图像，在所述偏振图像的偏振角模式和偏振度模式下，获得载体的第一航向信息，包括：

[0039] 201：利用偏振光罗盘采集载体的偏振图像，根据所述偏振图像计算每个像素的偏振度和偏振角；

[0040] 202：根据所述偏振度和所述偏振角，获得太阳方向矢量的最优估计；

[0041] 203：根据天文年历和所述太阳方向矢量的最优估计，获得载体的第一航向信息。

[0042] 在某个实施例中，步骤102具体为：

[0043] 偏振光罗盘为图像式偏振光传感器，主要由镜头、像素化偏振芯片和后端电路组成。像素化偏振芯片在CCD的每一个像元上刻蚀金属光栅，实现光栅与感光芯片的一体化集成。如图2所示，像素化偏振芯片包含若干个偏振测量单元，每个单元的光栅按照0º、45º、90º、135º的方向排列，采集天空偏振图像后，微阵列式偏振光罗盘每个偏振测量单元的偏振度 和偏振角 的计算方法为：

[0044] （1）

[0045] 其中，

[0046]   （2）

[0047] 式中， 、 、 为Stokes向量中的前三项，分别描述入射光的光强、入射光在0°偏振方向上的分量和45°偏振方向偏振分量； 为与某个方向的偏振片对应的CCD测量得到的入射光光强，x为0º、45º、90º或135º。

[0048] 如图3所示，点O表示视觉传感器（相机）坐标系的原点；点P为观测点，表示入射光与天球的交点，每一束入射光对应的观测点P与观测图像中一个偏振测量单元的中心像素对应，因此在已知相机的焦距 的前提下，

[0049] （8）

[0050] 式中，(xc,yc)表示相机的光轴在观测图像中的坐标； 与 为中间变量，无特殊含义。

[0051] 图3中， ， ；

[0052] 根据一阶瑞利散射模型，入射光 的E矢量方向 垂直于散射面 ，即：

[0053] （9）

[0054] 式中， 为E矢量方向 ；为太阳方向矢量 ；T为转置符号；

[0055] 太阳方向矢量 可以通过两个不相关的E矢量估计得到，定义 ，其中 为第 个偏振测量单元的E矢量方向，可以得到：

[0056] （10）

[0057] 则太阳方向矢量 的最优估计可通过求解如下的优化问题得到：

[0058] （11）

[0059] 太阳方向矢量 的最优估计是与 最小特征值相对应的特征向量。

[0060] 从而，获得最优太阳子午线方向αs：

[0061] （12）

[0062] 式中， 和 分别为 中第一个元素和第二个元素， 是公式（11）求解得到的最优太阳方向矢量。

[0063] 偏航角的计算过程为：

[0064] （13）

[0065] 式中， 为载体在导航坐标系中的航向角； 为太阳方位角，可根据天文年历计算得到； 为最优太阳子午线方向。

[0066] 在另一个实施例中，对于步骤103，利用磁罗盘采集载体的三轴磁分量和水平姿态角，在所述三轴磁分量和水平姿态角下，获得载体的第二航向信息，包括：

[0067] 301：利用磁罗盘测量载体周围的磁场分布，通过视觉惯性里程计获取的加速度，得到载体的水平姿态角；所述水平姿态角包括滚动角和俯仰角；

[0068] 302：根据所述载体的水平姿态角，计算载体的三轴磁分量；

[0069] 303：根据所述载体的三轴磁分量，计算载体的地理航向角；

[0070] 304：根据地理北向和地磁北向，获得地磁偏角；

[0071] 305：利用所述地磁偏角对所述地理航向角进行修正，获得载体的第二航向信息。

[0072] 在某个实施例中，步骤103具体为：

[0073] 将三轴磁强计捷联在载体上，测量载体周围的磁场分布，设三轴磁分量数据为，通过视觉惯性里程计获取的加速度信息，可以得到载体的滚动角 和俯仰角 ，如图4所示，有：

[0074] （14）

[0075] （3）

[0076] 式中， 、 和 为磁场在x、y、z三个方向的磁分量； 为三轴磁分量数据； 为滚动角； 为俯仰角。

[0077] 然后，根据所述载体的三轴磁分量，计算载体的地理航向角为：

[0078] （4）

[0079] （5）

[0080]  式中， 和 为磁场在x和z方向的磁分量； 为地理航向角。

[0081] 地理北向与地磁北向之间存在着一个小角度的夹角，称为地磁偏角，地磁偏角是由于地球自转轴与地磁轴不重合而存在的。利用所述地磁偏角对所述地理航向角进行修正，获得载体的第二航向信息， 为磁偏角，利用 对 修正后可以得到载体真实的地理航向角，即：

[0082] （15）

[0083] 在另一个实施例中，对于步骤104，根据所述姿态和位置信息、第一航向信息和第二航向信息，优化载体系统状态矢量，获得载体的位置、速度和姿态，实现载体位姿估计，包括：

[0084] 401：定义全局优化的系统状态矢量为

[0085] （6）

[0086] 式中， 为系统的全状态矢量； 为视觉惯性里程计输出的第k个状态，包括在世界坐标系下的位置 和姿态四元数 ； 为总的待优化状态数目；

[0087] 402：通过对所述姿态和位置信息、第一航向信息和第二航向信息进行残差求和，优化载体系统状态矢量，获得载体的位置、速度和姿态，实现载体位姿估计，[0088] （7）

[0089] 式中， 为相对测量的残差，为视觉惯性里程计的输出集合， 为相邻状态的相对测量； 为磁罗盘的测量残差，为磁罗盘航向测量的集合， 为磁罗盘测量； 为偏振光罗盘的测量残差， 为偏振光罗盘航向测量的集合， 为偏振光罗盘测量； 为载体系统的全状态矢量。

[0090] 本实施例中，考虑第t时刻和t-1时刻的状态，公式（7）中相对测量的残差为：

[0091] （16）

[0092] 式中， 和 分别为t-1和t时刻视觉惯性里程计输出的位置； 和 分别为t-1和t时刻视觉惯性里程计输出的姿态四元数；

[0093] 考虑第j时刻，系统获取了磁罗盘的航向信息，磁罗盘的测量残差 为：

[0094] （17）

[0095] 式中， 为第j时刻世界坐标系下载体的姿态四元数； 为第j时刻磁罗盘航向对应的姿态四元数， 和 的计算方法如下：

[0096]                          （18）

[0097] 其中， 表示四元数到欧拉角的变换； 、 和 分别为 对应的滚动角、俯仰角和航向角； 为磁罗盘航向。

[0098]  公式（7）中偏振光罗盘的测量残差 为：

[0099] （19）

[0100] 式中， 为第k时刻世界坐标系下载体的姿态四元数； 为光罗盘航向对应的姿态四元数， 和 的计算方法如下：

[0101] （20）

[0102] 其中， 表示四元数到欧拉角的变换， 、 和 分别为 对应的滚动角、俯仰角和航向角， 为偏振光罗盘航向。

[0103] 本发明还提出一种基于偏振光/地磁航向约束的位姿估计系统，包括：

[0104] 视觉惯性里程计模块，用于利用视觉惯性里程计获取当前时刻载体的姿态和位置信息；

[0105] 偏振光罗盘模块，用于利用偏振光罗盘采集载体的偏振图像，在所述偏振图像的偏振角模式和偏振度模式下，获得载体的第一航向信息；

[0106] 磁罗盘模块，用于利用磁罗盘采集载体的三轴磁分量和水平姿态角，在所述三轴磁分量和水平姿态角下，获得载体的第二航向信息；

[0107] 位姿估计模块，用于根据所述姿态和位置信息、第一航向信息和第二航向信息，优化载体系统状态矢量，获得载体的位置、速度和姿态，实现载体位姿估计。

[0108] 在其中一个实施例中，对于视觉惯性里程计模块，参考论文（VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator）进行具体的操作，从而获取当前时刻载体的姿态和位置信息。

[0109] 在下一个实施例中，偏振光罗盘模块还包括：

[0110] 201：利用偏振光罗盘采集载体的偏振图像，根据所述偏振图像计算每个像素的偏振度和偏振角；

[0111] 202：根据所述偏振度和所述偏振角，获得太阳方向矢量的最优估计；

[0112] 203：根据天文年历和所述太阳方向矢量的最优估计，获得载体的第一航向信息。

[0113] 在某个实施例中，偏振光罗盘模块具体工作过程为：

[0114] 偏振光罗盘为图像式偏振光传感器，主要由镜头、像素化偏振芯片和后端电路组成。像素化偏振芯片在CCD的每一个像元上刻蚀金属光栅，实现光栅与感光芯片的一体化集成。如图2所示，像素化偏振芯片包含若干个偏振测量单元，每个单元的光栅按照0º、45º、90º、135º的方向排列，采集天空偏振图像后，微阵列式偏振光罗盘每个偏振测量单元的偏振度 和偏振角 的计算方法为：

[0115] （1）

[0116] 其中，

[0117] （2）

[0118] 式中， 、 、 为Stokes向量中的前三项，分别描述入射光的光强、 入射光在0°偏振方向上的分量和45°偏振方向偏振分量； 为与某个方向的偏振片对应的CCD测量得到的入射光光强，x为0º、45º、90º或135º。

[0119] 如图3所示，点O表示视觉传感器（相机）坐标系的原点；点P为观测点，表示入射光与天球的交点，每一束入射光对应的观测点P与观测图像中一个偏振测量单元的中心像素对应，因此在已知相机的焦距 的前提下，

[0120] （8）

[0121] 式中，(xc,yc)表示相机的光轴在观测图像中的坐标； 与 为中间变量，无特殊含义。

[0122] 图3中， ， ；

[0123] 根据一阶瑞利散射模型，入射光 的E矢量方向 垂直于散射面 ，即：

[0124] （9）

[0125] 式中， 为E矢量方向 ；为太阳方向矢量 ；T为转置符号；

[0126] 太阳方向矢量 可以通过两个不相关的E矢量估计得到，定义，其中 为第 个偏振测量单元的E矢量方向，可以得到：

[0127] （10）

[0128] 则太阳方向矢量 的最优估计可通过求解如下的优化问题得到：

[0129] （11）

[0130] 太阳方向矢量 的最优估计是与 最小特征值相对应的特征向量。

[0131] 从而，获得最优太阳子午线方向αs：

[0132] （12）

[0133] 式中， 和 分别为 中第一个元素和第二个元素， 是公式（11）求解得到的最优太阳方向矢量。

[0134] 偏航角的计算过程为：

[0135] （13）

[0136] 式中， 为载体在导航坐标系中的航向角； 为太阳方位角，可根据天文年历计算得到； 为最优太阳子午线方向。

[0137] 在另一个实施例中，磁罗盘模块还包括：

[0138] 301：利用磁罗盘测量载体周围的磁场分布，通过视觉惯性里程计获取的加速度，得到载体的水平姿态角；所述水平姿态角包括滚动角和俯仰角；

[0139] 302：根据所述载体的水平姿态角，计算载体的三轴磁分量；

[0140] 303：根据所述载体的三轴磁分量，计算载体的地理航向角；

[0141] 304：根据地理北向和地磁北向，获得地磁偏角；

[0142] 305：利用所述地磁偏角对所述地理航向角进行修正，获得载体的第二航向信息。

[0143] 在某个实施例中，磁罗盘模块具体的工作过程为：

[0144] 将三轴磁强计捷联在载体上，测量载体周围的磁场分布，设三轴磁分量数据为，通过视觉惯性里程计获取的加速度信息，

[0145] 可以得到载体的滚动角 和俯仰角 ，如图4所示，有：

[0146] （14）

[0147] （3）

[0148] 式中， 、 和 为磁场在x、y、z三个方向的磁分量； 为三轴磁分量数据； 为滚动角； 为俯仰角。

[0149] 然后，根据所述载体的三轴磁分量，计算载体的地理航向角为：

[0150] （4）

[0151] （5）

[0152] 式中， 和 为磁场在x和z方向的磁分量； 为地理航向角。

[0153] 地理北向与地磁北向之间存在着一个小角度的夹角，称为地磁偏角，地磁偏角是由于地球自转轴与地磁轴不重合而存在的。利用所述地磁偏角对所述地理航向角进行修正，获得载体的第二航向信息， 为磁偏角，利用 对 修正后可以得到载体真实的地理航向角 ，即：

[0154] （15）

[0155] 在另一个实施例中，位姿估计模块还包括：

[0156] 401：定义全局优化的系统状态矢量为

[0157] （6）

[0158] 式中， 为系统的全状态矢量； 为视觉惯性里程计输出的第k个状态，包括在世界坐标系下的位置 和姿态四元数 ； 为总的待优化状态数目；

[0159] 402：通过对所述姿态和位置信息、第一航向信息和第二航向信息进行残差求和，优化载体系统状态矢量，获得载体的位置、速度和姿态，实现载体位姿估计，[0160] （7）

[0161] 式中， 为相对测量的残差， 为视觉惯性里程计的输出集合， 为相邻状态的相对测量； 为磁罗盘的测量残差， 为磁罗盘航向测量的集合， 为磁罗盘测量； 为偏振光罗盘的测量残差， 为偏振光罗盘航向测量的集合，为偏振光罗盘测量； 为载体系统的全状态矢量。

[0162] 本实施例中，考虑第t时刻和t-1时刻的状态，公式（7）中相对测量的残差为：

[0163] （16）

[0164] 式中， 和 分别为t-1和t时刻视觉惯性里程计输出的位置； 和 分别为t-1和t时刻视觉惯性里程计输出的姿态四元数；

[0165] 考虑第j时刻，系统获取了磁罗盘的航向信息，磁罗盘的测量残差 为：

[0166] （17）

[0167] 式中， 为第j时刻世界坐标系下载体的姿态四元数； 为第j时刻磁罗盘航向对应的姿态四元数， 和 的计算方法如下：

[0168]                      （18）

[0169] 其中， 表示四元数到欧拉角的变换； 、 和 分别为 对应的滚动角、俯仰角和航向角； 为磁罗盘航向。

[0170] 公式（7）中偏振光罗盘的测量残差 为：

[0171] （19）

[0172] 式中， 为第k时刻世界坐标系下载体的姿态四元数； 为光罗盘航向对应的姿态四元数， 和 的计算方法如下：

[0173] （20）

[0174] 其中， 表示四元数到欧拉角的变换， 、 和 分别为 对应的滚动角、俯仰角和航向角， 为偏振光罗盘航向。

[0175] 本发明还提出一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述所述方法的步骤。

[0176] 本发明还提出一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述所述的方法的步骤。

[0177] 以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。