一种基于天顶点矢量的偏振/VIO三维姿态确定方法技术

本发明专利技术涉及一种基于天顶点矢量的偏振/VIO三维姿态确定方法，主要包括以下步骤：首先，利用图像式偏振传感器测量的偏振角信息和偏振度信息获得太阳矢量并确定太阳在图像像素坐标系下的位置；然后，根据太阳、中性点和天顶点三点共线的特点以及当前太阳天顶角信息，求解出偏振传感器坐标系下天顶矢量；再次，选择惯导误差状态和相机位姿为估计状态向量，建立包括天顶矢量、太阳矢量和视觉残差信息的量测模型；最终，设计视觉两帧误差相关自适应因子，对天顶矢量、太阳矢量和视觉残差信息进行加权融合，解算三维姿态信息。本发明专利技术基于偏振信息与天顶点检测可以同时提供水平姿态与航向的修正，保障稀疏特征环境下的三维姿态估计精度，提高环境适应性。提高环境适应性。提高环境适应性。

【技术实现步骤摘要】
一种基于天顶点矢量的偏振/VIO三维姿态确定方法


[0001]本专利技术属于偏振组合导航领域，具体涉及一种基于天顶点矢量的偏振/VIO三维姿态确定方法，为研究偏振导航三维定姿提供了新的思路。

技术介绍

[0002]偏振导航技术作为一种受生物启发的导航手段，具有无源且误差不随时间积累的优点。根据瑞利散射模型得到的偏振信息大多为组合导航系统提供航向约束，无法仅依靠偏振信息解算三维姿态。偏振导航通常需要和惯性导航组合完成三维定姿，但该种组合方法水平姿态误差容易累积且动态性比较差。视觉惯性里程计（VIO）通过提取自然环境中特征点并结合惯性导航技术，完成对于载体自身的姿态估计，但是其对于航向的估计能力较差，同时在环境特征缺失的场景下易出现姿态估计不准的问题。
[0003]目前利用视觉导航、惯性导航和图像式偏振导航组合导航的方法有：专利“一种基于视觉惯性偏振光融合的无人机位姿估计方法”（申请号：202010623718.1），该方法仅利用偏振导航提供航向约束，且未考虑稀疏特征下视觉导航姿态估计不准确的问题；论文《基于微惯性/偏振视觉组合定向方法》，利用偏振导航为组合导航系统提供航向约束，无法对水平姿态进行修正；专利“一种基于双中性点矢量的天空偏振三维定姿方法”（申请号：201611078923 .4），该方法通过像空间坐标系和地理坐标系中性点矢量解算得到载体姿态，相比本专利技术提出的方法，依靠双中性点矢量的方法容易受到天气干扰，在一些天气条件下不能保证检测到两个中性点矢量；专利“基于大气偏振模式空间特征的三维姿态获取方法（申请号：201210005641.7）”，该方法通过采集显著特征点的空间位置完成三维定姿，该方法由于需要遍历所有采样点导致计算量较大，对于机载计算资源有限的无人机等载体无法满足导航解算频率要求；专利“一种基于地平线与偏振光的载体三维姿态获取方法”（申请号：201810062481.7）通过提取地平线后解算水平姿态，然后根据偏振解算航向信息组合提供三维姿态信息，并没有充分利用偏振导航信息解算三维姿态。专利“基于双偏振光矢量的三维姿态信息快速解算方法”（申请号：201711137596.X）仅利用偏振光信息完成三维姿态解算，所以动态性相对本专利提供的偏振/VIO三维定姿方法较差。

技术实现思路

[0004]本专利技术解决的技术问题是在GNSS拒止环境下载体动态三维定姿问题，克服现有大多数天空偏振光信息只能为组合导航系统提供航向约束的缺点，提供使用偏振信息进行三维定姿的新思路，保障稀疏特征环境VIO三维定姿的精度。
[0005]本专利技术的技术解决方案为：一种基于天顶点矢量的偏振/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表示相机内参矩阵。
[0006]在步骤2中根据步骤1中得到的偏振传感器坐标系下的中性点矢量n
b
和太阳矢量s
b
信息，确定在偏振传感器坐标系下过太阳、中性点和天顶点的平面，记为平面α。
[0007]导航系下太阳的天顶角通过太阳天文年历和地理纬度得到：其中，表示导航系下太阳高度角，L为地理纬度，δ为太阳赤纬，Ω为太阳时角。
[0008]以偏振传感器坐标系的s
b
轴为转轴，天顶角作为半轴角得到圆锥面z，利用该圆锥面z与平面α的交线即可得到偏振传感器坐标系下天顶矢量z
b
：其中，为天顶角，同时表示圆锥面z的半轴角，z
b
表示待求解的偏振传感器坐标系下的天顶矢量。明显可知，该圆锥与平面有两条交线，即求解出的天顶矢量存在模糊性。
[0009]一般选择导航系下天顶矢量为,根据视觉惯性里程计提供的姿态信
息可以得到一个预测的天顶矢量，记为：其中，表示视觉惯性里程计提供的导航系到偏振传感器坐标系姿态转换矩阵，可以利用该预测的天顶矢量去除求得的偏振传感器坐标系下天顶矢量模糊性，确定唯一的天顶矢量。
[0010]在步骤3中以惯导误差状态（R6×1表示6
×
1向量）和不同时刻相机状态建立系统估计状态向量： 其中，表示载体姿态角误差，表示陀螺仪的零偏，表示第i时刻相机姿态角误差，表示第i时刻相机位置误差。建立包括步骤2中得到的天顶矢量、太阳矢量和视觉测量残差信息的系统量测模型如下：其中，表示从导航系到偏振传感器坐标系姿态转换矩阵，z
v
表示视觉残差量测，表示视觉量测雅克比矩阵，表示对应量测噪声，表示太阳矢量量测噪声，表示天顶矢量量测噪声。
[0011]系统整体量测方程为：其中，z表示系统量测，由太阳矢量量测z
s
、天顶矢量量测z
b
和视觉残差量测z
v
组成，H表示三者组合的量测矩阵，v表示三者组合量测噪声。
[0012]在步骤4中设计自适应因子，以VIO两帧之间差值的变化来设计一个自适应因子，来提高系统的滤波精度。其具体步骤如下：设计自适应因子如下：
其中，b是遗忘因子，表示第k时刻的相机y方向位置误差，即无人机前进方向位置增量。表示根据视觉前后两帧的帧间误差的自适应因子。得到新的滤波过程为：结合上面的状态方程和量测方程，记表示第k时刻过程噪声的均值。在扩展卡尔曼滤波过程中可以得到：扩展卡尔曼滤波过程中可以得到：其中，为k+本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于天顶点矢量的偏振/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中视觉两帧误差相关的自适应因子，对偏振传感器坐标系下天顶矢量z
b
、偏振传感器坐标系下太阳矢量s
b
和视觉相机坐标系下视觉残差信息进行加权融合，解算三维姿态信息。2.根据权利要求1所述的一种基于天顶点矢量的偏振/VIO三维姿态确定方法，其特征在于：在所述步骤1中，基于偏振度图像检测出中性点N在图像像素坐标系下位置坐标(u
N
,v
N
)，并将其转为齐次坐标表示中性点矢量n
b
，根据太阳矢量和偏振矢量垂直关系，利用两个观测方向的偏振矢量和解算得到太阳矢量s
b
，进而确定偏振度图像像素坐标系中太阳的投影位置，假设太阳在图像像素坐标系中的位置坐标为(u
s
,v
s
)，转换为归一化齐次坐标为：其中，表示偏振传感器坐标系下太阳高度角，K表示相机内参矩阵。3.根据权利要求1所述的一种基于天顶点矢量的偏振/VIO三维姿态确定方法，其特征在于：在所述步骤2中根据步骤1中得到的偏振传感器坐标系下的中性点矢量n
b<...

【专利技术属性】
技术研发人员：杨健，李晶，王善澎，郭雷，
申请(专利权)人：北京航空航天大学，
类型：发明
国别省市：