偏振光辅助的视觉惯性导航系统

为了提高组合导航系统的可靠性与位姿估计的精度，把偏振定向传感器引入同步定位与建图过程，提出并设计一种新的偏振光辅助的视觉惯性组合导航系统。采集偏振定向传感器、单目视觉相机及微惯性测量单元的数据，对多传感器数据进行时间戳对齐和预处理后，利用最小二乘优化方法建立目标方程，通过求解非线性方程组获取最佳的运动估计。该系统根据天空偏振分布实现了方位角的可观性，并融合了多传感器数据。基于上述组合导航系统进行户外车载实验，实验结果表明：在2 km的长距离运行中，相比原始视觉惯性系统，偏振光辅助的视觉惯性导航系统的位置误差降低了16.7%，航向角精度提升了23.4%。偏振定向传感器的接入能够抑制惯性器件测量值的漂移，改善导航系统的位置精度和姿态角精度，可满足卫星信号受到干扰等环境下的位姿估计精度和可靠性要求。

Abstract

To improve the reliability of the integrated navigation system and the accuracy of pose estimation, a novel visual inertial integrated navigation system assisted by polarized light is proposed and constructed by introducing a polarization orientation sensor into the process of simultaneous localization and mapping. The data from the polarization orientation sensor, monocular vision camera, and micro-inertial measurement unit are collected. The target equation is established using the least square optimization method after multi-sensor data has been time-stamped aligned and preprocessed, and the best motion estimation is then produced by solving nonlinear equations. In the suggested system, the observability of azimuth is achieved based on the polarization distribution of the sky, and multi-sensor data are fused. Based on the above-integrated navigation system, an outdoor vehicle-mounted experiment is carried out. The experimental findings indicate that, in the long-distance operation of 2 km, the location inaccuracy of the polarized light-assisted visual-inertial navigation system is 16.7% lower and the heading angle accuracy is 23.4% greater than the estimated value of the original visual-inertial system. The polarization orientation sensor can reduce the drift of inertial devices, enhance the position accuracy and attitude angle accuracy of the navigation system, and meet the needs of the position and pose estimation accuracy and reliability under the interference of satellite signals.

1　引言

导航定位技术对于机器人、无人驾驶、增强现实等作用重大。不同的应用领域对导航系统的精度、刷新频率、可靠性、成本、尺寸、质量等方面均有不同的要求。应用较为广泛的导航技术有天文导航、惯性导航、卫星导航、地磁导航、视觉导航等。为了提高导航系统的状态估计精度和鲁棒性，多传感器融合的组合导航解决方案近年来越来越受到学者们的关注和研究^［1-2］。

利用相机导航的同步定位与建图（Simultaneous location and mapping，SLAM）技术是机器人与自主导航领域的研究热点和关键技术。学者们尝试将惯性测量单元（Inertial measurement unit，IMU）等传感器与视觉相机融合，以提升导航性能。惯性器件和视觉器件的特性不同：对于短期高动态运动，惯性器件能够很好地保证运动导航精度，但长期工作会导致误差累积，并导致状态发散；视觉器件可获取高精度的定位信息，且可以在闭环状况下抑制误差，但其导航效果受到光照、采样频率等的限制。视觉惯性的组合导航系统能够在复杂环境下实现具有鲁棒性和高精度的组合导航，实现更好的导航效果^［3-6］。

Mourikis等^［7］首次提出利用MSCKF（Multi-state constraint Kalman filter）的视觉惯性导航滤波算法，该算法建立的状态模型无需包含特征点空间位置，但其可能产生错误的可观性信息。Li等^［8］提出的FEJ-EKF（First estimate Jacobian-extended Kalman filter）算法、Huang等^［9］提出的UKF（Unscented Kalman filter）算法，以及Castellanos等^［10］提出的Robocentric Mapping算法均致力于解决视觉惯性滤波估计不一致的问题。Lupton等^［11-12］提出了预积分理论，通过增量方法成功求解视觉惯性组合导航算法的优化方法；但此方法缺乏闭环检测，故无法用于长时间的精密导航。沈劭劼课题组提出的VINS（Visual-inertial navigation system）算法能够实现动态初始化、外参自动估计、闭环优化、重定位等功能^［13］。

在视觉惯性导航系统中，由于偏航角的不可观，里程计漂移是不可避免的问题。通过观察昆虫等采用的导航方式，学者们提出了仿生偏振导航。这种模拟昆虫复眼的导航方式能够利用大气偏振分布模式获得航向角，具有抗电磁波干扰、自主性强、无累积误差等优点^［14-16］，目前已经有广泛的应用。Chu等^［17］将偏振光传感器与光电编码盘组合成航位推算系统，并将该系统用于移动机器人的室外导航。褚金奎等^［18］在四旋翼飞行器导航系统中加入仿生偏振定向传感器，通过EKF算法设计了融合导航算法，提高了导航系统的抗电磁干扰能力。王玉杰等^［19］、王道斌^［20］设计了偏振光组合导航方案，验证了偏振组合导航的可行性。学者们将偏振光与惯性导航、卫星导航等进行了信息融合，但目前偏振光与SLAM的组合局限于纯视觉SLAM的滤波松耦合，其解算精度不高、鲁棒性不强，有关偏振光与多传感器融合SLAM的研究较少。

本文在视觉惯性SLAM的基础上引入仿生偏振导航，设计了偏振光辅助的视觉惯性SLAM系统，利用非线性优化方法建立目标方程，获取载体的运动估计，实现了多传感器融合算法，并在户外开展了车载导航实验。与传统的SLAM导航相比，该系统能够测量方位角，使得偏航角和姿态估计的准确度得到了提高，可更好地估计地面载体的运动姿态，有着抗电磁干扰、自主性强和成本低的优势。

2　偏振光传感器原理及IMU数据模型

2.1　偏振定向传感器工作原理

研究表明，大气层中的浮游粒子会散射和吸收自然光。在晴朗无云的天气状况下，大气中主要为N₂、O₂、CO₂、O₃等小直径的原子和分子，天空中能够产生有规律且稳定分布的偏振光场，故可采用瑞利散射理论模型建立大气偏振光场分布模型。偏振光定向传感器可根据偏振分布场进行导航，其模拟沙漠蚂蚁头背部的偏振敏感复眼可确定自身体轴与太阳子午线的夹角，进而确认二维平面内的航向角信息^［21］。

导航系统选用东北天坐标系（ENU），右前上坐标系为载体坐标系（b）；载体上固连偏振光传感器，偏振光传感器的体坐标系与载体坐标系重合。3个姿态角为俯仰角（ $φ$ ）、横滚角（ $θ$ ）、航向角（ $ψ$ ）。根据偏振光传感器测量体轴与天顶点入射光E矢量方向的夹角，计算体轴与太阳子午线的夹角，从而获得航向角。图1中，S点表示太阳，O点表示地面观测点，P点表示偏振定向传感器的观测点， $α$ 表示偏振光传感器体轴与观测点E矢量的夹角， $h_{S}$ 表示太阳高度角， $f_{S}$ 表示太阳方位角。天空任意一点的偏振E矢量总是垂直于上述S、O、P 3点所组成的平面。

图 1. 偏振光定向原理

Fig. 1. Principle of polarized light orientation

下载图片查看所有图片

偏振E矢量在世界坐标系（n）中表示为 $E_{n} = {(\begin{array}{l} e_{x} & e_{y} & e_{z} \end{array})}^{T}$ ， $e_{x}$ 、 $e_{y}$ 、 $e_{z}$ 分别为入射光偏振矢量在世界坐标系各方向的分量。偏振光定向传感器测到的角度是入射光偏振方向与传感器体轴的夹角，故入射光 $E$ 矢量在载体坐标系中表示为 $E_{b} = {(\begin{array}{l} c o s α & s i n α & 0 \end{array})}^{T}$ 。通过方向余弦矩阵（ $C_{n b}$ ）实现坐标系的转换，完成 $E$ 矢量在载体坐标系和世界坐标系之间的转换，可表示为

E_{n} = C_{n b} \cdot E_{b}

，（1）

理想状况下，观测点即为天顶点，此时用于计算余弦矩阵的P点坐标为P_n=［0 0 1］，方向余弦矩阵 $C_{n b}$ 可简化为

C_{n b} = (\begin{matrix} c o s ψ & - s i n ψ & 0 \\ s i n ψ & c o s ψ & 0 \\ 0 & 0 & 1 \end{matrix})

。（2）

太阳视线的投影坐标表示为

S_{n} = {(\begin{array}{l} c o s h_{S} c o s f_{S} & c o s h_{S} s i n f_{S} & s i n h_{S} \end{array})}^{T}

，（3）

由瑞利散射规律可以得到

E_{n} = S_{n} \times P_{n}

，（4）

联立上述各式即可得到航向角

ψ = α + f_{S} + \frac{π}{2}

。（5）

2.2　IMU数据模型

IMU可测量运动载体瞬时的线性加速度和旋转角速度，进而计算载体的位置、速度和方向，其由加速度计和陀螺仪组成。IMU通过积分采集的相邻时刻数据求解位姿信息，载体在第i时刻到第j时刻之间位姿变换的求解方法为

\{\begin{array}{l} p_{n b_{j}} = p_{n b_{i}} + v_{n_{i}} t + \iint_{t \in [i, j]}^{} (r_{n b_{t}} a_{b_{t}} - g_{n}) δ t^{2} \\ v_{n_{j}} = v_{n_{i}} + \int_{t \in [i, j]} (r_{n b_{t}} a_{b_{t}} - g_{n}) δ t \\ r_{n b_{j}} = r_{n b_{i}} e x p (ω_{t} δ t) \end{array}

，（6）

式中：t代表时间间隔；δ代表微分； $a$ 代表采集的加速度，其需去除重力加速度g的影响；ω代表采集的角速度；p、v、r分别代表载体的位置、速度以及载体所在的载体坐标系与世界坐标系之间的转换关系。

陀螺仪和加速度计存在着零偏（Bias）、白噪声（Noise）、比例因子（Scale）等误差，其中的确定性误差可以通过采用标定手段进行补偿^［22］。考虑高斯白噪声N和零偏B，IMU的模型可表示为

\{\begin{array}{l} {\tilde{ω}}_{b} = ω_{b} + B_{g} + N_{g} \\ {\tilde{a}}_{b} = q_{n b} (a_{n} + g_{n}) + B_{a} + N_{a} \end{array}

，（7）

式中：下标g代表陀螺仪，下标a代表加速度计； ${\tilde{ω}}_{b}$ 和 ${\tilde{a}}_{b}$ 代表IMU测量的角速度和加速度；q代表不同坐标之间的姿态转换矩阵。IMU每次启动时，零偏B都会发生变化，这会对系统的运动估计值产生影响，因此需要在初始化阶段对其进行估计，并利用偏振定向传感器和视觉相机的测量值对其进行优化。

3　传感器融合导航关键算法

组合导航核心算法的流程图如图2所示。相机通过视觉特征点和SFM（Struct from motion）技术跟踪求解得到载体的位姿信息，但无法得到尺度信息；IMU积分得到的位姿存在漂移大的问题；仿生偏振传感器可解算得到航向角，进而修正航向漂移和累积误差。导航系统运行流程包括传感器数据输入采集、传感器时间戳同步及数据帧发布、数据预处理、初始化、非线性优化及闭环优化等部分。

图 2. 偏振光辅助的组合导航系统框架图

Fig. 2. Block diagram of integrated navigation system assisted by polarized light

下载图片查看所有图片

3.1　系统的数据采集和初始化

由于多个传感器的频率不同，需在对系统采集的传感器数据添加时间戳和序列号后重新发布，并在初始化阶段同步各个传感器的时间戳，集中存储和批量处理属于同一时间间隔内的传感器数据。良好的初始化对于系统的运动估计精度和稳定性至关重要。初始化阶段，需估计系统正常运行时所需的状态变量。由于系统的非线性，一个错误的初始化可能会损害整个估计过程，易使系统运行后期出现跟踪丢失、重定位以及估计误差过大等情况。相邻时刻k与k+1视觉测量的姿态变化 $q_{c_{k} c_{k + 1}}$ （下标c代表相机坐标系）与陀螺仪旋转积分 $q_{b_{k} b_{k + 1}}$ 可建立的标定方程为

q_{b_{k} b_{k + 1}} \otimes q_{b c} = q_{b c} \otimes q_{c_{k} c_{k + 1}}

，（8）

根据式（8）采集多组数据并建立线性方程，可求解得到相机与IMU之间的外部参数 $q_{b c}$ 。

为了估计载体坐标系的速度、视觉估计的尺度因子和相机坐标系下的重力矢量，需要利用相机和IMU的测量值进行联合初始化，建立约束，求解出相机与IMU之间的外部参数后，估计陀螺仪零偏B，进而估算图像帧的速度、视觉估计的尺度因子、重力矢量在相机坐标系下的测量值。

完成重力矢量的估计后，通过将重力加速度g在初始时刻相机坐标系下的表达式 ${(g_{x}, g_{y}, g_{z})}^{T}$ 旋转为在世界坐标系下的表达式 ${(0,0, - 9.81)}^{T}$ ，得到载体坐标系与世界坐标系之间的俯仰角和横滚角。当一种状态不可观察时，通常会有无穷多种初始状态，它们会重现完全相同的输入和输出，因此变换存在无穷多组解，即系统启动时，姿态偏航角为随机值。在单目视觉惯性导航系统中，三自由度位置与航向角均不可观，而俯仰角和横滚角由于重力矢量的测量而可观，尺度因子可通过加速度计测量确定。本文的导航方案通过加入偏振定向传感器实现了航向角的可观性，使得导航系统的位置和航向角误差都有较大程度的减小，后端估计的一致性得到提高。

3.2　系统的位姿估计

由于各传感器存在误差和缺陷，对于视觉跟踪和IMU初步估计的运动位姿与相关状态量还需进行最小二乘优化估计。利用多传感器数据进行融合优化以获取高精度的运动估计，记优化的状态量为

X = (\begin{matrix} \begin{matrix} \begin{matrix} p_{n b_{m}} & v_{n b_{m}} \end{matrix} & q_{n b_{m}} \end{matrix} & B_{a} & \begin{matrix} B_{g} & x_{b c} \end{matrix} \end{matrix})

，（9）

式中：各变量分别表示m时刻运动载体的载体坐标系相对于世界坐标系的位置、速度、姿态，以及惯性器件中加速度计与陀螺仪的零偏； $x_{b c} = (p_{b c}, q_{b c})$ ，为相机坐标系和载体坐标系之间平移和旋转的变换关系。计算目标函数关于各状态量的雅克比矩阵，求解方程式取最小值时变量X状态量为

X^{*} = a r g m i n_{X} \{\sum_{m \in I} {‖E_{I} ({\hat{z}}_{b_{m} b_{m + 1}}, X)‖}^{2} + \sum_{(l, j) \in C} {‖E_{C} ({\hat{z}}_{l}^{C_{j}}, X)‖}^{2}\}

，（10）

式中：I代表IMU的测量范围；l和j代表相机相邻观测时刻；C代表相机的测量范围； ${\hat{z}}_{l}^{C_{j}}$ 代表根据相机估算的l、j时刻之间的位姿变化； $E_{I}$ 代表IMU的积分误差； $E_{C}$ 代表IMU的视觉重投影误差； ${\hat{z}}_{b_{m} b_{m + 1}}$ 代表位姿更新过程中IMU在相邻时刻之间估计的位置、速度、旋转和偏置变化量的残差，其中相邻时刻的旋转残差可表示为

δ θ_{b_{m} b_{m + 1}} = 2 (γ_{b_{m} b_{m + 1}} \otimes {q_{n b_{m}}}^{- 1} \otimes q_{n b_{m + 1}})

，（11）

式中： $q_{n b_{m}}$ 和 $q_{n b_{m + 1}}$ 为IMU测量得到的m时刻与m+1时刻载体相对于世界坐标系的姿态的四元数表示，即优化前的姿态初始值； $γ_{b_{m} b_{m + 1}}$ 为估计的此时间间隔内的姿态变化值。由于存在零偏和其他测量误差，旋转残差不为0。通过对 $q_{n b_{m}}$ 、 $q_{n b_{m + 1}}$ 以及陀螺仪零偏的优化调整，将残差量优化到最小，能够得到各个优化变量以及载体姿态的最佳值。设四元数表示的姿态变化 ${(q_{t}, q_{x}, q_{y}, q_{z})}^{T}$ 与欧拉角 ${(θ, φ, ψ)}^{T}$ 的转换关系为

(\begin{array}{l} q_{t} \\ q_{x} \\ q_{y} \\ q_{z} \end{array}) = (\begin{array}{l} c o s \frac{θ}{2} c o s \frac{φ}{2} c o s \frac{ψ}{2} + s i n \frac{θ}{2} s i n \frac{φ}{2} s i n \frac{ψ}{2} \\ s i n \frac{θ}{2} c o s \frac{φ}{2} c o s \frac{ψ}{2} - c o s \frac{θ}{2} s i n \frac{φ}{2} s i n \frac{ψ}{2} \\ c o s \frac{θ}{2} s i n \frac{φ}{2} c o s \frac{ψ}{2} + s i n \frac{θ}{2} c o s \frac{φ}{2} s i n \frac{ψ}{2} \\ c o s \frac{θ}{2} c o s \frac{φ}{2} s i n \frac{ψ}{2} - s i n \frac{θ}{2} s i n \frac{φ}{2} c o s \frac{ψ}{2} \end{array})

。（12）

由相邻2个数据帧的IMU数据积分值得到的航向角的变化量为 $Δ ψ$ ，偏振定向传感器在对应时间间隔的测量值分别为 $P_{m}$ 和 $P_{m + 1}$ ，角度变化值为 $Δ P = P_{m + 1} - P_{m}$ 。根据IMU预积分测量值与偏振定向传感器测量值，可构建相对残差量为

ε = \frac{| Δ P - Δ ψ|}{Δ ψ}

。（13）

$ε$ 值的大小反映着IMU测量与偏振光传感器测量差异的大小，综合考虑数据精度和非线性优化过程的收敛效果，制定数据融合策略为：当 $ε$ 大于设定值时，在构建残差项时采用IMU测量的姿态变化值 $γ_{b_{m} b_{m + 1}}$ ；当 $ε$ 小于设定值时，利用 $Δ P$ 构建姿态变化值 $γ_{b_{m} b_{m + 1}}$ 以修正测量误差，并估计IMU的零偏。完成残差构建后，使用高斯-牛顿法对优化函数进行迭代优化求解，当达到误差收敛条件或迭代次数足够时取消迭代操作，求得载体导航信息和优化时所需的中间变量，实现多传感器信息融合。

4　导航平台设计及实验

4.1　导航平台的结构

搭建的组合导航实验平台如图3所示，系统的硬件部分主体为承载传感器、电源等设备的双层铝合金结构。传感器部分包括偏振光定向传感器、内置双目与IMU的MYNTEYE相机，以及高精度的SPAN-CPT光纤惯性导航系统，内置全球导航卫星系统（Global navigation satellite system，GNSS）。计算处理部分为配置机器人操作系统（ROS）的笔记本电脑，在ROS环境下撰写并启动各传感器的驱动和数据收发等程序。实验时将各传感器数据和时间戳等有效信息录制为数据集，同时记录SPAN-CPT的数据作为参考基准。录制的数据集可复现实时实验的传感器数据信息，可在算法调试时多次播放。组合导航系统所选用的传感器具体参数如表1所示。

图 3. 组合导航系统实验平台

Fig. 3. Integrated navigation system experimental platform

下载图片查看所有图片

表 1. 组合导航系统传感器信息

Table 1. Information of integrated navigation system sensor

Sensor	Type	Sampling frequency /Hz
Polarized light sensor	Self-developed	20
Binocular camera	MYNTEYE S1000	15
Accelerometer	BMI088	100
Gyroscope	BMI088	100
GNSS and inertial navigation system（INS）	SPAN-CPT	100

查看所有表

4.2　实验流程

实验平台内各传感器的坐标系朝向以及平台在车顶的固定位置如图4所示。对提出的导航方法进行多次室外实验。实验前首先使用惯性导航参数标定工具对IMU进行标定，以去除确定性误差，得到较为准确的加速度计和陀螺仪的噪声偏置初始值，最后配置IMU相关参数的初始值。

图 4. 车载导航示意图

Fig. 4. Vehicle-mounted navigation diagram

下载图片查看所有图片

实验一于辽宁省大连市甘井子区高新园小平岛别墅中心广场（北纬38.831°，东经121.499°）开展，实验实际轨迹为一个半径为70 m的近似圆形，其周长约为377 m，航向角随时间的变化接近线性。实验当日天空无云，天气晴朗。偏振定向传感器的测量精度与天空偏振分布模式有关，为了实现较为良好的实验效果，选择下午进行测试。实验平台没有障碍物的遮挡，能够较好地接收自然光。载体行进轨迹如图5所示，其中点划线为SPAN-CPT记录的基准轨迹，另外2条轨迹分别为改进后的偏振光辅助的组合导航系统估计的轨迹和原始的视觉惯性导航系统估计的轨迹。

图 5. 导航轨迹对比（实验一）

Fig. 5. Navigation trajectory comparison (experiment 1)

下载图片查看所有图片

使用误差分析工具EVO对导航结果进行分析，评估估计轨迹的相对位姿误差（Relative pose error，RPE；记为R_RPE），计算方法为：每隔相同的时间，计算基准轨迹与算法估计的位姿的变化量，对同一区间内的理论值与估计值的变化量作差，从而获得误差值。此计算方法也可用于估计系统漂移。对算法估计的位姿与基准轨迹的记录值进行时间戳对齐和比例自动缩放后，计算其误差。图6、图7分别展示了偏振光辅助方案与原始解决方案估计的位姿的RPE。

图 6. 组合导航系统的RPE（实验一）

Fig. 6. RPE of integrated navigation system (experiment 1)

下载图片查看所有图片

图 7. 原始导航系统的RPE（实验一）

Fig. 7. RPE of original navigation system (experiment 1)

下载图片查看所有图片

实验一的行驶距离较短，但载体在行驶过程中，航向角持续变化，且实验时光线较强，故系统的视觉特征提取与跟踪效果一般，最终计算得出的平均位置误差和角度误差结果如表2所示。提出的组合导航系统的轨迹估计值与SPAN-CPT记录之间的轨迹偏差均值为1.10 m，航向角偏差均值为0.69°；普通的视觉惯性SLAM的轨迹偏差均值为1.29 m，航向角偏差均值为0.81°。提出的组合导航系统在运行过程中更好地估计了航向角和轨迹，位置误差降低了14.7%，航向角精度提升了16.7%。

表 2. 导航方式误差对比（实验一）

Table 2. Navigation mode error comparison (experiment 1)

Plan	Average position error /m	Average angle error /（°）
Integrated-navigation	1.10	0.69
SLAM	1.29	0.83

查看所有表

实验二地点位于辽宁省大连市博广路锦绣华城附近（北纬38.898°，东经121.513°），实验当日天气良好，各传感器工作正常。载体行驶距离大于2 km，轨迹相对第一次实验更加复杂。图8展示了SPAN-CPT记录的实际行驶轨迹，以及改进前后的导航系统的移动轨迹估计图。提出的组合导航系统估计的轨迹明显更加贴近真实轨迹。

图 8. 导航轨迹对比（实验二）

Fig. 8. Navigation trajectory comparison (experiment 2)

下载图片查看所有图片

图9对比了各方案求解的航向角。在位置和角度求解中，偏振光辅助的导航系统都有着更好的表现。同样地，评估计算本次实验导航结果的RPE。图10、图11是进行时间戳对齐和比例自动缩放后，根据计算的误差结果绘制的2种方案的RPE。

图 9. 导航航向角对比（实验二）

Fig. 9. Navigation course angle comparison (experiment 2)

下载图片查看所有图片

图 10. 组合导航系统的RPE（实验二）

Fig. 10. RPE of integrated navigation system (experiment 2)

下载图片查看所有图片

图 11. 原始导航系统的RPE（实验二）

Fig. 11. RPE of original navigation system (experiment 2)

下载图片查看所有图片

本次实验最终得到如表3所示的位置误差和角度误差结果，其中本文方案的轨迹估计值与SPAN-CPT记录之间的轨迹偏差均值为0.75 m，航向角偏差均值为0.36°；未改进的SLAM的轨迹偏差均值为0.90 m，航向角偏差均值为0.47°。相比传统的SLAM，本文提出的组合导航系统能够确定载体初始时刻的绝对朝向，实现了良好的初始化；在转弯和长时间运行时，组合导航系统有着更好的表现：其位置误差降低了16.7%，航向角精度提升了23.4%。

表 3. 导航方式误差对比（实验二）

Table 3. Navigation mode error comparison (experiment 2)

Plan	Average position error /m	Average angle error /（°）
Integrated-navigation	0.75	0.36
SLAM	0.90	0.47

查看所有表

5　结论

为了实现偏振光传感器在SLAM中的应用，并改善组合导航系统的导航性能与抗电磁干扰能力，提出了一种仿生偏振定向传感器辅助的视觉惯性导航方法，完成了导航实验平台的搭建，并进行了户外车载实验。与传统的视觉惯性SLAM的位姿估计结果相比，在2 km的长距离运动中，提出的导航方案的位置误差降低了 16.7%，航向角精度提升了23.4%，证明该组合导航系统可应用于机器人户外导航和建图。

组合导航系统位姿估计的精度和鲁棒性受到多个因素的影响，包括传感器特性、解算过程、系统初始化效果和载体运动过程中的速度和角度变化。在方案设计和实现过程中，已尽量考虑各影响因素，但该方案仍然存在缺陷。目前，在相机过曝、光线不佳时，导航系统存在提取和跟踪特征点不稳定的情况。后续还需考虑在阴雨天气下偏振光传感器工作状态不良的情况，研究天气适应性更强的成像式偏振定向传感器的应用，并针对地图复用环节等展开研究。

参考文献

[1] 段国文, 何庆顺, 刘伟. 国外导航系统发展现状与趋势[J]. 电子技术与软件工程, 2021(19): 29-31.

Duan G W, He Q S, Liu W. Development status and trend of navigation system abroad[J]. Electronic Technology & Software Engineering, 2021(19): 29-31.

[2] GrovesPaul D.. GNSS与惯性及多传感器组合导航系统原理. 练军想, 译[M]. 2版. 北京: 国防工业出版社, 2015. 10.1109/maes.2014.14110

GrovesP D. Principles of GNSS, inertial, and multisensor integrated navigation systems. Liang J X, Transl[M]. 2nd ed. Beijing: National Defense Industry Press, 2015.

[3] 高翔, 张涛, 刘毅. 视觉SLAM十四讲从理论到实践[M]. 北京: 电子工业出版社, 2017.

GaoX, ZhangT, LiuY. Lecture 14 of visual SLAM: from theory to practice[M]. Beijing: Publishing House of Electronics Industry, 2017.

[4] 吴建清, 宋修广. 同步定位与建图技术发展综述[J]. 山东大学学报(工学版), 2021, 51(5): 16-31.

Wu J Q, Song X G. Review on development of simultaneous localization and mapping technology[J]. Journal of Shandong University (Engineering Science), 2021, 51(5): 16-31.

[5] 宋薇, 梁晶, 张海桥, 等. 基于楼宇场景分类的激光导航与地图构建[J]. 激光与光电子学进展, 2021, 58(14): 1404001.

Song W, Liang J, Zhang H Q, et al. Laser navigation and mapping based on building environment classification[J]. Laser & Optoelectronics Progress, 2021, 58(14): 1404001.

[6] Cadena C, Carlone L, Carrillo H, et al. Past, present, and future of simultaneous localization and mapping: toward the robust-perception age[J]. IEEE Transactions on Robotics, 2016, 32(6): 1309-1332.

[7] MourikisA I, RoumeliotisS I. A multi-state constraint Kalman filter for vision-aided inertial navigation[C]‍∥Proceedings 2007 IEEE International Conference on Robotics and Automation, April 10-14, 2007, Rome, Italy. New York: IEEE Press, 2007: 3565-3572.

[8] Li M Y, Mourikis A I. High-precision, consistent EKF-based visual-inertial odometry[J]. The International Journal of Robotics Research, 2013, 32(6): 690-711.

[9] Huang G P, Mourikis A I, Roumeliotis S I. A quadratic-complexity observability-constrained unscented Kalman filter for SLAM[J]. IEEE Transactions on Robotics, 2013, 29(5): 1226-1243.

[10] Castellanos J A, Martinez-Cantin R, Tardós J D, et al. Robocentric map joining: improving the consistency of EKF-SLAM[J]. Robotics and Autonomous Systems, 2007, 55(1): 21-29.

[11] Lupton T, Sukkarieh S. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions[J]. IEEE Transactions on Robotics, 2012, 28(1): 61-76.

[12] Forster C, Carlone L, Dellaert F, et al. On-manifold preintegration for real-time visual: inertial odometry[J]. IEEE Transactions on Robotics, 2017, 33(1): 1-21.

[13] Qin T, Li P L, Shen S J. VINS-mono: a robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020.

[14] 朱志飞, 杨江涛, 王晨光, 等. 基于瑞利大气偏振模式的太阳自主定位方法及其实现[J]. 激光与光电子学进展, 2017, 54(2): 022901.

Zhu Z F, Yang J T, Wang C G, et al. Solar autonomous-positioning method based on Rayleigh atmosphere polarization pattern and its implementation[J]. Laser & Optoelectronics Progress, 2017, 54(2): 022901.

[15] 褚金奎, 时超, 王寅龙, 等. 偏振光实时定位系统的设计[J]. 中国激光, 2018, 45(3): 0310002.

Chu J K, Shi C, Wang Y L, et al. Design of polarized light real-time positioning system[J]. Chinese Journal of Lasers, 2018, 45(3): 0310002.

[16] 褚金奎, 田连标, 成昊远, 等. 天空光主导的波浪水面下偏振分布模型仿真[J]. 光学学报, 2020, 40(20): 2001002.

Chu J K, Tian L B, Cheng H Y, et al. Simulation of polarization distribution model under wavy water surfaces dominated by skylight[J]. Acta Optica Sinica, 2020, 40(20): 2001002.

[17] ChuJ K, WangH Q, ChenW J, et al. Application of a novel polarization sensor to mobile robot navigation[C]‍∥2009 International Conference on Mechatronics and Automation, August 9-12, 2009, Changchun, China. New York: IEEE Press, 2009: 3763-3768.

[18] 褚金奎, 武进, 李金山, 等. 偏振光传感器在四旋翼飞行器中的应用[J]. 电子学报, 2020, 48(1): 198-203.

Chu J K, Wu J, Li J S, et al. Application of polarization sensor in quadrotor helicopter[J]. Acta Electronica Sinica, 2020, 48(1): 198-203.

[19] 王玉杰, 胡小平, 练军想, 等. 仿生偏振视觉定位定向机理与实验[J]. 光学精密工程, 2016, 24(9): 2109-2116.

Wang Y J, Hu X P, Lian J X, et al. Mechanisms of bionic positioning and orientation based on polarization vision and corresponding experiments[J]. Optics and Precision Engineering, 2016, 24(9): 2109-2116.

[20] 王道斌. 基于天空偏振光的SLAM方法的研究[D]. 合肥: 中国科学技术大学, 2014.

WangD B. The research of SLAM algorithm based on the sky polarized light[D]. Hefei: University of Science and Technology of China, 2014.

[21] 褚金奎, 张慧霞, 王寅龙, 等. 多方向偏振光实时定位样机的设计与搭建[J]. 光学精密工程, 2017, 25(2): 312-318.

Chu J K, Zhang H X, Wang Y L, et al. Design and construction of autonomous real-time position prototype based on multi-polarized skylight[J]. Optics and Precision Engineering, 2017, 25(2): 312-318.

[22] 汪志刚, 郭杭, 敖龙辉. 利用Vicon系统进行IMU标定[J]. 测绘通报, 2021(7): 121-125.

Wang Z G, Guo H, Ao L H. IMU calibration using Vicon system[J]. Bulletin of Surveying and Mapping, 2021(7): 121-125.

褚金奎, 胡瀚珮, 万振华, 李金山. 偏振光辅助的视觉惯性导航系统[J]. 激光与光电子学进展, 2023, 60(7): 0726002. Jinkui Chu, Hanpei Hu, Zhenhua Wan, Jinshan Li. Visual Inertial Navigation System Aided by Polarized Light[J]. Laser & Optoelectronics Progress, 2023, 60(7): 0726002.