一种基于天顶点矢量的偏振/vio三维姿态确定方法
阅读说明：本技术 一种基于天顶点矢量的偏振/vio三维姿态确定方法 (polarization/VIO three-dimensional attitude determination method based on zenith vector ) 是由 杨健 李晶 王善澎 郭雷 于 2021-11-22 设计创作，主要内容包括：本发明涉及一种基于天顶点矢量的偏振/VIO三维姿态确定方法,主要包括以下步骤：首先,利用图像式偏振传感器测量的偏振角信息和偏振度信息获得太阳矢量并确定太阳在图像像素坐标系下的位置；然后,根据太阳、中性点和天顶点三点共线的特点以及当前太阳天顶角信息,求解出偏振传感器坐标系下天顶矢量；再次,选择惯导误差状态和相机位姿为估计状态向量,建立包括天顶矢量、太阳矢量和视觉残差信息的量测模型；最终,设计视觉两帧误差相关自适应因子,对天顶矢量、太阳矢量和视觉残差信息进行加权融合,解算三维姿态信息。本发明基于偏振信息与天顶点检测可以同时提供水平姿态与航向的修正,保障稀疏特征环境下的三维姿态估计精度,提高环境适应性。(The invention relates to a polarization/VIO three-dimensional attitude determination method based on zenith vectors, which mainly comprises the following steps: firstly, obtaining a sun vector by utilizing polarization angle information and polarization degree information measured by an image type polarization sensor and determining the position of the sun under an image pixel coordinate system; then, solving a zenith vector under a polarization sensor coordinate system according to the collinear characteristics of three points of the sun, the neutral point and the zenith point and the information of the current sun zenith angle; thirdly, selecting an inertial navigation error state and a camera pose as estimated state vectors, and establishing a measurement model comprising a zenith vector, a sun vector and visual residual error information; finally, designing a visual two-frame error-related adaptive factor, performing weighted fusion on zenith vectors, solar vectors and visual residual error information, and calculating three-dimensional attitude information. The invention can simultaneously provide correction of horizontal attitude and course based on polarization information and zenith detection, guarantee the three-dimensional attitude estimation precision in the sparse characteristic environment and improve the environmental adaptability.)
技术领域
本发明属于偏振组合导航领域，具体涉及一种基于天顶点矢量的偏振/VIO三维姿态确定方法，为研究偏振导航三维定姿提供了新的思路。
背景技术
偏振导航技术作为一种受生物启发的导航手段，具有无源且误差不随时间积累的优点。根据瑞利散射模型得到的偏振信息大多为组合导航系统提供航向约束，无法仅依靠偏振信息解算三维姿态。偏振导航通常需要和惯性导航组合完成三维定姿，但该种组合方法水平姿态误差容易累积且动态性比较差。视觉惯性里程计（VIO）通过提取自然环境中特征点并结合惯性导航技术，完成对于载体自身的姿态估计，但是其对于航向的估计能力较差，同时在环境特征缺失的场景下易出现姿态估计不准的问题。
目前利用视觉导航、惯性导航和图像式偏振导航组合导航的方法有：专利“一种基于视觉惯性偏振光融合的无人机位姿估计方法”（申请号：202010623718.1），该方法仅利用偏振导航提供航向约束，且未考虑稀疏特征下视觉导航姿态估计不准确的问题；论文《基于微惯性/偏振视觉组合定向方法》，利用偏振导航为组合导航系统提供航向约束，无法对水平姿态进行修正；专利“一种基于双中性点矢量的天空偏振三维定姿方法”（申请号：201611078923 .4），该方法通过像空间坐标系和地理坐标系中性点矢量解算得到载体姿态，相比本发明提出的方法，依靠双中性点矢量的方法容易受到天气干扰，在一些天气条件下不能保证检测到两个中性点矢量；专利“基于大气偏振模式空间特征的三维姿态获取方法（申请号：201210005641.7）”，该方法通过采集显著特征点的空间位置完成三维定姿，该方法由于需要遍历所有采样点导致计算量较大，对于机载计算资源有限的无人机等载体无法满足导航解算频率要求；专利“一种基于地平线与偏振光的载体三维姿态获取方法”（申请号：201810062481.7）通过提取地平线后解算水平姿态，然后根据偏振解算航向信息组合提供三维姿态信息，并没有充分利用偏振导航信息解算三维姿态。专利“基于双偏振光矢量的三维姿态信息快速解算方法”（申请号：201711137596.X）仅利用偏振光信息完成三维姿态解算，所以动态性相对本专利提供的偏振/VIO三维定姿方法较差。
发明内容
本发明解决的技术问题是在GNSS拒止环境下载体动态三维定姿问题，克服现有大多数天空偏振光信息只能为组合导航系统提供航向约束的缺点，提供使用偏振信息进行三维定姿的新思路，保障稀疏特征环境VIO三维定姿的精度。
本发明的技术解决方案为：一种基于天顶点矢量的偏振/VIO三维姿态确定方法，其主要实现步骤如下：
步骤1、利用图像式偏振传感器获得图像像素坐标系下天空偏振度分布图像和偏振角分布图像，基于偏振度图像检测中性点N，获得在图像像素坐标系位置坐标(u N ,v N )；基于偏振角图像获得两个观测方向的偏振矢量和，计算偏振传感器坐标系下太阳矢量s b ，确定太阳在图像像素坐标系的位置坐标(u s ,v s )；
步骤2、根据偏振度分布图像中太阳、中性点和天顶点共线，且天顶角与偏振传感器坐标系下太阳矢量s b 和天顶矢量夹角z b 相同，利用太阳天文年历和地理经纬度计算得到导航系下太阳的天顶角，计算偏振传感器坐标系下天顶矢量z b ；
步骤3、选择惯导误差状态和相机位姿为状态，并建立结合偏振传感器坐标系下天顶矢量z b 、偏振传感器坐标系下太阳矢量s b 和视觉相机坐标系下视觉残差信息的三维姿态量测模型；
步骤4、设计与VIO中视觉两帧误差相关的自适应因子，对偏振传感器坐标系下天顶矢量、偏振传感器坐标系下太阳矢量s b 和视觉相机坐标系下视觉残差信息进行加权融合，解算得到三维姿态信息；
在步骤1中根据基于偏振度图像检测出中性点N位置坐标(u N ,v N )，并将其转为齐次坐标表示中性点矢量n b 。根据太阳矢量和偏振矢量垂直关系，利用两个观测方向的偏振矢量和解算得到太阳矢量s b ，进而确定图像像素坐标系下太阳位置坐标(u s ,v s )，转换为归一化齐次坐标为：
其中，表示偏振传感器坐标系下太阳高度角，K表示相机内参矩阵。
在步骤2中根据步骤1中得到的偏振传感器坐标系下的中性点矢量n b 和太阳矢量s b 信息，确定在偏振传感器坐标系下过太阳、中性点和天顶点的平面，记为平面α。
导航系下太阳的天顶角通过太阳天文年历和地理纬度得到：
其中，表示导航系下太阳高度角，L为地理纬度，δ为太阳赤纬，Ω为太阳时角。
以偏振传感器坐标系的s b 轴为转轴，天顶角作为半轴角得到圆锥面z，利用该圆锥面z与平面α的交线即可得到偏振传感器坐标系下天顶矢量z b ：
其中，为天顶角，同时表示圆锥面z的半轴角，z b 表示待求解的偏振传感器坐标系下的天顶矢量。明显可知，该圆锥与平面有两条交线，即求解出的天顶矢量存在模糊性。
一般选择导航系下天顶矢量为,根据视觉惯性里程计提供的姿态信息可以得到一个预测的天顶矢量，记为：
其中，表示视觉惯性里程计提供的导航系到偏振传感器坐标系姿态转换矩阵，可以利用该预测的天顶矢量去除求得的偏振传感器坐标系下天顶矢量模糊性，确定唯一的天顶矢量。
在步骤3中以惯导误差状态（R 6×1表示6×1向量）和不同时刻相机状态建立系统估计状态向量：
其中，表示载体姿态角误差，表示陀螺仪的零偏，表示第i时刻相机姿态角误差，表示第i时刻相机位置误差。建立包括步骤2中得到的天顶矢量、太阳矢量和视觉测量残差信息的系统量测模型如下：
其中，表示从导航系到偏振传感器坐标系姿态转换矩阵，z v 表示视觉残差量测，表示视觉量测雅克比矩阵，表示对应量测噪声，表示太阳矢量量测噪声，表示天顶矢量量测噪声。
系统整体量测方程为：
其中，z表示系统量测，由太阳矢量量测z s 、天顶矢量量测z b 和视觉残差量测z v 组成，H表示三者组合的量测矩阵，v表示三者组合量测噪声。
在步骤4中设计自适应因子，以VIO两帧之间差值的变化来设计一个自适应因子，来提高系统的滤波精度。其具体步骤如下：
设计自适应因子如下：
其中，b是遗忘因子，表示第k时刻的相机y方向位置误差，即无人机前进方向位置增量。表示根据视觉前后两帧的帧间误差的自适应因子。得到新的滤波过程为：
结合上面的状态方程和量测方程，记表示第k时刻过程噪声的均值。在扩展卡尔曼滤波过程中可以得到：
其中，为k+1时刻量测噪声方差阵的估计，为k+1时刻系统噪声方差阵的估计。为k+1时刻量测新息；H k+1为k+1时刻量测阵；K k+1表示k+1时刻卡尔曼滤波增益；Φ k+1,k 表示从k时刻到k+1时刻状态转移矩阵；P k 表示k时刻状态协方差。
本发明与现有技术相比优点在于：
（1）利用图像式偏振传感器基于天顶点和中性点检测，可以提供水平姿态约束，为偏振导航技术进行三维定姿提供新的思路，不再局限于偏振导航技术只能提供航向约束；
（2）由于视觉量测会受到环境特征点以及光照的影响导致量测信息缺失以及精度下降，同时偏振图像传感器会由于太阳直射光、反射光等环境因素导致偏振量测精度下降，根据视觉两帧间误差设计自适应因子完成三种姿态约束信息的融合，求得三维姿态估计结果。
附图说明
图1为本发明一种基于天顶点矢量的偏振/VIO三维姿态确定方法流程图；
图2为本发明中根据导航系下太阳的天顶角和偏振传感器坐标系下天顶矢量和太阳矢量夹角不变，利用偏振传感器坐标系下太阳矢量所在平面和以天顶角为半轴角的圆锥面构成约束求解偏振传感器坐标系下天顶矢量示意图。
具体实施方式
下面结合附图1，2以及实例对本发明的具体实现步骤说明如下：
步骤1、利用图像式偏振传感器获得图像像素坐标系下天空偏振度分布图像和偏振角分布图像，基于偏振度图像检测中性点N，获得在图像像素坐标系位置坐标(u N ,v N )；基于偏振角图像获得两个观测方向的偏振矢量和，计算偏振传感器坐标系下太阳矢量s b ，确定太阳在图像像素坐标系的位置坐标(u s ,v s )；
步骤2、如图2所示，根据偏振度分布图像中太阳、中性点和天顶点共线，且天顶角与偏振传感器坐标系下太阳矢量和天顶矢量夹角相同，利用太阳天文年历和地理经纬度计算得到导航系下太阳的天顶角，计算偏振传感器坐标系下天顶矢量z b ；
步骤3、选择惯导误差状态和相机位姿为状态，并建立结合偏振传感器坐标系下天顶矢量z b 、偏振传感器坐标系下太阳矢量s b 和相机坐标系下视觉残差信息的三维姿态量测模型；
步骤4、 针对获取的天顶矢量、太阳矢量以及视觉残差信息，由于视觉惯性里程计会受到环境特征点以及光照的影响导致量测信息缺失以及精度下降，同时偏振图像传感器会由于太阳直射光、反射光等环境因素导致偏振量测精度下降，设计与VIO两帧之间误差相关的自适应因子及时调整避免某一种量测信息不准确导致误差累积。
具体实施步骤如下：
在步骤1中通过图像式偏振传感器获得图像像素坐标系下天空偏振度分布图像和偏振角分布图像，根据偏振度分布图像中性点处偏振度为0这一特性，确定图像像素坐标系中性点位置坐标(u N ,v N )，将该坐标归一化表示中性点矢量n b 。基于偏振角分布图像获得两个观测方向的偏振矢量和解算得到偏振传感器坐标系下太阳矢量s b 。根据投影关系到太阳在图像像素坐标系下位置坐标(u s ,v s )，转换为齐次坐标可以得到：
其中，表示偏振传感器坐标系下太阳高度角，K表示相机内参矩阵。
在步骤2中根据步骤1中得到的偏振传感器坐标系下的中性点矢量n b 和太阳矢量s b ，确定在偏振传感器坐标系下过太阳、中性点和天顶点的平面，记为平面α。
导航系下太阳的天顶角通过太阳天文年历和地理纬度得到：
其中，表示导航系下太阳高度角，L为地理纬度，δ为太阳赤纬，Ω为太阳时角。
以偏振传感器坐标系的s b 轴为转轴，天顶角作为半轴角得到圆锥面z，利用该圆锥面z与平面α的交线即可得到偏振传感器坐标系下天顶矢量z b ：
其中，为天顶角，同时表示圆锥面z的半轴角，z b 表示待求解的偏振传感器坐标系下的天顶矢量。明显可知，该圆锥与平面有两条交线，即求解出的天顶矢量存在模糊性。
一般选择天顶矢量为,根据视觉惯性里程计提供的姿态信息得到一个预测的天顶矢量，记为：
其中，表示视觉惯性里程计提供的导航系到偏振传感器坐标系姿态转换矩阵，可以利用该预测的天顶矢量去除求得的偏振传感器坐标系下天顶矢量模糊性，确定唯一的天顶矢量。
在步骤3中以惯导误差状态（R 6×1表示6×1向量）和不同时 刻相机状态建立系统估计状态向量：
其中，表示载体姿态角误差，表示陀螺仪的零偏，表示第i时刻相机姿态角误差，表示第i时刻相机位置误差。建立包括步骤2中得到的天顶矢量、太阳矢量和视觉测量残差信息的系统量测模型如下：
其中，表示从导航系到偏振传感器坐标系姿态转换矩阵，z v 表示视觉残差量测，表示视觉量测雅克比矩阵，表示视觉量测噪声，表示太阳矢量量测噪声，表示天顶矢量量测噪声；
系统整体量测方程为：
其中，z表示系统量测，由太阳矢量量测z s 、天顶矢量量测z b 和视觉残差量测z v 组成，H表示三者组合的量测矩阵，v表示三者组合量测噪声。
在步骤4中设计自适应因子，以VIO中视觉两帧之间差值的变化来设计一个自适应因子，来提高系统的滤波精度。其具体步骤如下：
设计自适应因子如下：
其中，b是遗忘因子，表示第k时刻的相机y方向位置误差，即无人机前进方向位置增量。表示根据视觉前后两帧的帧间误差的自适应因子。
得到新的滤波过程为：
结合上面的状态方程和量测方程，记表示第k时刻过程噪声的均值。在扩展卡尔曼滤波过程中可以得到：
其中，为k+1时刻量测噪声方差阵的估计，为k+1时刻系统噪声方差阵的估计。为k+1时刻量测新息；H k+1为k+1时刻量测阵；K k+1表示k+1时刻卡尔曼滤波增益；Φ k+1,k 表示从k时刻到k+1时刻状态转移矩阵；P k 表示k时刻状态协方差。
自适应的物理意义在于当视觉惯性里程计两帧之间的误差大于某一个阈值时候，该阈值设为无人机位置增量的函数，即表示两帧间误差不大于无人机相应时间内移动过的距离，对系统量测噪声进行调节，进而达到最优的估计结果。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种基于多传感器融合的里程计方法及装置