CN113819904B - 一种基于天顶点矢量的偏振/vio三维姿态确定方法 - Google Patents

一种基于天顶点矢量的偏振/vio三维姿态确定方法 Download PDF

Info

Publication number
CN113819904B
CN113819904B CN202111381893.5A CN202111381893A CN113819904B CN 113819904 B CN113819904 B CN 113819904B CN 202111381893 A CN202111381893 A CN 202111381893A CN 113819904 B CN113819904 B CN 113819904B
Authority
CN
China
Prior art keywords
vector
polarization
zenith
coordinate system
sun
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111381893.5A
Other languages
English (en)
Other versions
CN113819904A (zh
Inventor
杨健
李晶
王善澎
郭雷
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN202111381893.5A priority Critical patent/CN113819904B/zh
Publication of CN113819904A publication Critical patent/CN113819904A/zh
Application granted granted Critical
Publication of CN113819904B publication Critical patent/CN113819904B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

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

Description

一种基于天顶点矢量的偏振/VIO三维姿态确定方法
技术领域
本发明属于偏振组合导航领域，具体涉及一种基于天顶点矢量的偏振/VIO三维姿态确定方法，为研究偏振导航三维定姿提供了新的思路。
背景技术
偏振导航技术作为一种受生物启发的导航手段，具有无源且误差不随时间积累的优点。根据瑞利散射模型得到的偏振信息大多为组合导航系统提供航向约束，无法仅依靠偏振信息解算三维姿态。偏振导航通常需要和惯性导航组合完成三维定姿，但该种组合方法水平姿态误差容易累积且动态性比较差。视觉惯性里程计（VIO）通过提取自然环境中特征点并结合惯性导航技术，完成对于载体自身的姿态估计，但是其对于航向的估计能力较差，同时在环境特征缺失的场景下易出现姿态估计不准的问题。
目前利用视觉导航、惯性导航和图像式偏振导航组合导航的方法有：专利“一种基于视觉惯性偏振光融合的无人机位姿估计方法”（申请号：202010623718.1），该方法仅利用偏振导航提供航向约束，且未考虑稀疏特征下视觉导航姿态估计不准确的问题；论文《基于微惯性/偏振视觉组合定向方法》，利用偏振导航为组合导航系统提供航向约束，无法对水平姿态进行修正；专利“一种基于双中性点矢量的天空偏振三维定姿方法”（申请号：201611078923 .4），该方法通过像空间坐标系和地理坐标系中性点矢量解算得到载体姿态，相比本发明提出的方法，依靠双中性点矢量的方法容易受到天气干扰，在一些天气条件下不能保证检测到两个中性点矢量；专利“基于大气偏振模式空间特征的三维姿态获取方法（申请号：201210005641.7）”，该方法通过采集显著特征点的空间位置完成三维定姿，该方法由于需要遍历所有采样点导致计算量较大，对于机载计算资源有限的无人机等载体无法满足导航解算频率要求；专利“一种基于地平线与偏振光的载体三维姿态获取方法”（申请号：201810062481.7）通过提取地平线后解算水平姿态，然后根据偏振解算航向信息组合提供三维姿态信息，并没有充分利用偏振导航信息解算三维姿态。专利“基于双偏振光矢量的三维姿态信息快速解算方法”（申请号：201711137596.X）仅利用偏振光信息完成三维姿态解算，所以动态性相对本专利提供的偏振/VIO三维定姿方法较差。
发明内容
本发明解决的技术问题是在GNSS拒止环境下载体动态三维定姿问题，克服现有大多数天空偏振光信息只能为组合导航系统提供航向约束的缺点，提供使用偏振信息进行三维定姿的新思路，保障稀疏特征环境VIO三维定姿的精度。
本发明的技术解决方案为：一种基于天顶点矢量的偏振/VIO三维姿态确定方法，其主要实现步骤如下：
步骤1、利用图像式偏振传感器获得图像像素坐标系下天空偏振度分布图像和偏振角分布图像，基于偏振度图像检测中性点N，获得在图像像素坐标系位置坐标(u N ,v N )；基于偏振角图像获得两个观测方向的偏振矢量
Figure 663768DEST_PATH_IMAGE001
和
Figure 383463DEST_PATH_IMAGE002
，计算偏振传感器坐标系下太阳矢量s b ，确定太阳在图像像素坐标系的位置坐标(u s ,v s )；
步骤2、根据偏振度分布图像中太阳、中性点和天顶点共线，且天顶角
Figure 693221DEST_PATH_IMAGE003
与偏振传感器坐标系下太阳矢量s b 和天顶矢量夹角z b 相同，利用太阳天文年历和地理经纬度计算得到导航系下太阳的天顶角
Figure 763945DEST_PATH_IMAGE003
，计算偏振传感器坐标系下天顶矢量z b ；
步骤3、选择惯导误差状态和相机位姿为状态，并建立结合偏振传感器坐标系下天顶矢量z b 、偏振传感器坐标系下太阳矢量s b 和视觉相机坐标系下视觉残差信息的三维姿态量测模型；
步骤4、设计与VIO中视觉两帧误差相关的自适应因子，对偏振传感器坐标系下天顶矢量、偏振传感器坐标系下太阳矢量s b 和视觉相机坐标系下视觉残差信息进行加权融合，解算得到三维姿态信息；
在步骤1中根据基于偏振度图像检测出中性点N位置坐标(u N ,v N )，并将其转为齐次坐标表示中性点矢量n b 。根据太阳矢量和偏振矢量垂直关系，利用两个观测方向的偏振矢量
Figure 755035DEST_PATH_IMAGE001
和
Figure 594815DEST_PATH_IMAGE002
解算得到太阳矢量s b ，进而确定图像像素坐标系下太阳位置坐标(u s ,v s )，转换为归一化齐次坐标为：
Figure 75475DEST_PATH_IMAGE004
其中，
Figure 367916DEST_PATH_IMAGE005
表示偏振传感器坐标系下太阳高度角，K表示相机内参矩阵。
在步骤2中根据步骤1中得到的偏振传感器坐标系下的中性点矢量n b 和太阳矢量s b 信息，确定在偏振传感器坐标系下过太阳、中性点和天顶点的平面，记为平面α。
导航系下太阳的天顶角
Figure 411964DEST_PATH_IMAGE003
通过太阳天文年历和地理纬度得到：
Figure 106251DEST_PATH_IMAGE006
其中，
Figure 757812DEST_PATH_IMAGE007
表示导航系下太阳高度角，L为地理纬度，δ为太阳赤纬，Ω为太阳时角。
以偏振传感器坐标系的s b 轴为转轴，天顶角
Figure 475232DEST_PATH_IMAGE003
作为半轴角得到圆锥面z，利用该圆锥面z与平面α的交线即可得到偏振传感器坐标系下天顶矢量z b ：
Figure 959806DEST_PATH_IMAGE009
其中，
Figure 695681DEST_PATH_IMAGE003
为天顶角，同时表示圆锥面z的半轴角，z b 表示待求解的偏振传感器坐标系下的天顶矢量。明显可知，该圆锥与平面有两条交线，即求解出的天顶矢量存在模糊性。
一般选择导航系下天顶矢量为
Figure 363423DEST_PATH_IMAGE010
,根据视觉惯性里程计提供的姿态信息可以得到一个预测的天顶矢量，记为：
Figure 766722DEST_PATH_IMAGE011
其中，
Figure 494507DEST_PATH_IMAGE012
表示视觉惯性里程计提供的导航系到偏振传感器坐标系姿态转换矩阵，可以利用该预测的天顶矢量去除求得的偏振传感器坐标系下天顶矢量模糊性，确定唯一的天顶矢量。
在步骤3中以惯导误差状态
Figure 966945DEST_PATH_IMAGE013
（R 6×1表示6×1向量）和不同时刻相机状态
Figure 703957DEST_PATH_IMAGE014
建立系统估计状态向量：
Figure 961763DEST_PATH_IMAGE015
其中，
Figure 860449DEST_PATH_IMAGE016
表示载体姿态角误差，
Figure 836495DEST_PATH_IMAGE017
表示陀螺仪的零偏，
Figure 111619DEST_PATH_IMAGE018
表示第i时刻相机姿态角误差，
Figure 223931DEST_PATH_IMAGE019
表示第i时刻相机位置误差。建立包括步骤2中得到的天顶矢量、太阳矢量和视觉测量残差信息的系统量测模型如下：
Figure 542786DEST_PATH_IMAGE020
其中，
Figure 740549DEST_PATH_IMAGE012
表示从导航系到偏振传感器坐标系姿态转换矩阵，z v 表示视觉残差量测，
Figure 553785DEST_PATH_IMAGE021
表示视觉量测雅克比矩阵，
Figure 786183DEST_PATH_IMAGE022
表示对应量测噪声，
Figure 26671DEST_PATH_IMAGE023
表示太阳矢量量测噪声，
Figure 977310DEST_PATH_IMAGE024
表示天顶矢量量测噪声。
系统整体量测方程为：
Figure 594236DEST_PATH_IMAGE025
其中，z表示系统量测，由太阳矢量量测z s 、天顶矢量量测z b 和视觉残差量测z v 组成，H表示三者组合的量测矩阵，v表示三者组合量测噪声。
在步骤4中设计自适应因子，以VIO两帧之间差值的变化来设计一个自适应因子，来提高系统的滤波精度。其具体步骤如下：
设计自适应因子如下：
Figure 930408DEST_PATH_IMAGE026
其中，b是遗忘因子，
Figure 76219DEST_PATH_IMAGE027
表示第k时刻的相机y方向位置误差，即无人机前进方向位置增量。
Figure 248574DEST_PATH_IMAGE028
表示根据视觉前后两帧的帧间误差的自适应因子。得到新的滤波过程为：
结合上面的状态方程和量测方程，记
Figure 669191DEST_PATH_IMAGE029
表示第k时刻过程噪声的均值。在扩展卡尔曼滤波过程中可以得到：
Figure 610602DEST_PATH_IMAGE030
Figure 458473DEST_PATH_IMAGE031
其中，
Figure 101812DEST_PATH_IMAGE032
为k+1时刻量测噪声方差阵的估计，
Figure 60541DEST_PATH_IMAGE033
为k+1时刻系统噪声方差阵的估计。
Figure 122038DEST_PATH_IMAGE034
为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 )；基于偏振角图像获得两个观测方向的偏振矢量
Figure 140810DEST_PATH_IMAGE001
和
Figure 756599DEST_PATH_IMAGE002
，计算偏振传感器坐标系下太阳矢量s b ，确定太阳在图像像素坐标系的位置坐标(u s ,v s )；
步骤2、如图2所示，根据偏振度分布图像中太阳、中性点和天顶点共线，且天顶角与偏振传感器坐标系下太阳矢量和天顶矢量夹角相同，利用太阳天文年历和地理经纬度计算得到导航系下太阳的天顶角
Figure 784598DEST_PATH_IMAGE003
，计算偏振传感器坐标系下天顶矢量z b ；
步骤3、选择惯导误差状态和相机位姿为状态，并建立结合偏振传感器坐标系下天顶矢量z b 、偏振传感器坐标系下太阳矢量s b 和相机坐标系下视觉残差信息的三维姿态量测模型；
步骤4、针对获取的天顶矢量、太阳矢量以及视觉残差信息，由于视觉惯性里程计会受到环境特征点以及光照的影响导致量测信息缺失以及精度下降，同时偏振图像传感器会由于太阳直射光、反射光等环境因素导致偏振量测精度下降，设计与VIO两帧之间误差相关的自适应因子及时调整避免某一种量测信息不准确导致误差累积。
具体实施步骤如下：
在步骤1中通过图像式偏振传感器获得图像像素坐标系下天空偏振度分布图像和偏振角分布图像，根据偏振度分布图像中性点处偏振度为0这一特性，确定图像像素坐标系中性点位置坐标(u N ,v N )，将该坐标归一化表示中性点矢量n b 。基于偏振角分布图像获得两个观测方向的偏振矢量
Figure 966180DEST_PATH_IMAGE001
和
Figure 421432DEST_PATH_IMAGE002
解算得到偏振传感器坐标系下太阳矢量s b 。根据投影关系到太阳在图像像素坐标系下位置坐标(u s ,v s )，转换为齐次坐标可以得到：
Figure 508206DEST_PATH_IMAGE004
其中，
Figure 74317DEST_PATH_IMAGE005
表示偏振传感器坐标系下太阳高度角，K表示相机内参矩阵。
在步骤2中根据步骤1中得到的偏振传感器坐标系下的中性点矢量n b 和太阳矢量s b ，确定在偏振传感器坐标系下过太阳、中性点和天顶点的平面，记为平面α。
导航系下太阳的天顶角
Figure 48089DEST_PATH_IMAGE003
通过太阳天文年历和地理纬度得到：
Figure 674242DEST_PATH_IMAGE006
其中，
Figure 795782DEST_PATH_IMAGE007
表示导航系下太阳高度角，L为地理纬度，δ为太阳赤纬，Ω为太阳时角。
以偏振传感器坐标系的s b 轴为转轴，天顶角
Figure 165583DEST_PATH_IMAGE003
作为半轴角得到圆锥面z，利用该圆锥面z与平面α的交线即可得到偏振传感器坐标系下天顶矢量z b ：
Figure 610543DEST_PATH_IMAGE009
其中，
Figure 953799DEST_PATH_IMAGE003
为天顶角，同时表示圆锥面z的半轴角，z b 表示待求解的偏振传感器坐标系下的天顶矢量。明显可知，该圆锥与平面有两条交线，即求解出的天顶矢量存在模糊性。
一般选择天顶矢量为
Figure 596133DEST_PATH_IMAGE010
,根据视觉惯性里程计提供的姿态信息得到一个预测的天顶矢量，记为：
Figure 75656DEST_PATH_IMAGE011
其中，
Figure 27300DEST_PATH_IMAGE012
表示视觉惯性里程计提供的导航系到偏振传感器坐标系姿态转换矩阵，可以利用该预测的天顶矢量去除求得的偏振传感器坐标系下天顶矢量模糊性，确定唯一的天顶矢量。
在步骤3中以惯导误差状态
Figure 857853DEST_PATH_IMAGE035
（R 6×1表示6×1向量）和不同 时刻相机状态
Figure 303878DEST_PATH_IMAGE036
建立系统估计状态向量：
Figure 169066DEST_PATH_IMAGE015
其中，
Figure 776765DEST_PATH_IMAGE016
表示载体姿态角误差，
Figure 829034DEST_PATH_IMAGE037
表示陀螺仪的零偏，
Figure 78750DEST_PATH_IMAGE018
表示第i时刻相机姿态角误差，
Figure 47712DEST_PATH_IMAGE038
表示第i时刻相机位置误差。建立包括步骤2中得到的天顶矢量、太阳矢量和视觉测量残差信息的系统量测模型如下：
Figure 91891DEST_PATH_IMAGE020
其中，
Figure 365878DEST_PATH_IMAGE012
表示从导航系到偏振传感器坐标系姿态转换矩阵，z v 表示视觉残差量测，
Figure 419285DEST_PATH_IMAGE021
表示视觉量测雅克比矩阵，
Figure 259065DEST_PATH_IMAGE022
表示视觉量测噪声，
Figure 739725DEST_PATH_IMAGE023
表示太阳矢量量测噪声，
Figure 219116DEST_PATH_IMAGE039
表示天顶矢量量测噪声；
系统整体量测方程为：
Figure 545055DEST_PATH_IMAGE040
其中，z表示系统量测，由太阳矢量量测z s 、天顶矢量量测z b 和视觉残差量测z v 组成，H表示三者组合的量测矩阵，v表示三者组合量测噪声。
在步骤4中设计自适应因子，以VIO中视觉两帧之间差值的变化来设计一个自适应因子，来提高系统的滤波精度。其具体步骤如下：
设计自适应因子如下：
Figure 239342DEST_PATH_IMAGE041
其中，b是遗忘因子，
Figure 625324DEST_PATH_IMAGE027
表示第k时刻的相机y方向位置误差，即无人机前进方向位置增量。
Figure 873903DEST_PATH_IMAGE028
表示根据视觉前后两帧的帧间误差的自适应因子。
得到新的滤波过程为：
结合上面的状态方程和量测方程，记
Figure 3533DEST_PATH_IMAGE042
表示第k时刻过程噪声的均值。在扩展卡尔曼滤波过程中可以得到：
Figure 552326DEST_PATH_IMAGE043
Figure 358477DEST_PATH_IMAGE044
其中，
Figure 359931DEST_PATH_IMAGE045
为k+1时刻量测噪声方差阵的估计，
Figure 293252DEST_PATH_IMAGE033
为k+1时刻系统噪声方差阵的估计。
Figure 430972DEST_PATH_IMAGE034
为k+1时刻量测新息；H k+1为k+1时刻量测阵；K k+1表示k+1时刻卡尔曼滤波增益；Φ k+1,k 表示从k时刻到k+1时刻状态转移矩阵；P k 表示k时刻状态协方差。
自适应的物理意义在于当视觉惯性里程计两帧之间的误差大于某一个阈值时候，该阈值设为无人机位置增量的函数，即表示两帧间误差不大于无人机相应时间内移动过的距离，对系统量测噪声进行调节，进而达到最优的估计结果。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (4)

1.一种基于天顶矢量的偏振/VIO三维姿态确定方法，其特征在于，实现步骤如下：
步骤1、利用图像式偏振传感器获得图像像素坐标系下天空偏振度分布图像和偏振角分布图像，基于偏振度分布图像检测出中性点N，并求得在图像像素坐标系下位置坐标(u N ,v N )，得到偏振传感器坐标系下的中性点矢量n b ；基于偏振角分布图像计算两个观测方向的偏振矢量
Figure 380551DEST_PATH_IMAGE001
和
Figure 500953DEST_PATH_IMAGE002
，求得偏振传感器坐标系下太阳矢量s b ，确定太阳在图像像素坐标系下的位置坐标(u s ,v s )；
步骤2、利用偏振度分布图像中太阳、中性点和天顶点共线的关系，且天顶角
Figure 749532DEST_PATH_IMAGE003
与偏振传感器坐标系下太阳矢量s b 和天顶矢量z b 的夹角相同，利用太阳天文年历和地理经纬度计算得到导航系下太阳的天顶角
Figure 410321DEST_PATH_IMAGE004
，计算偏振传感器坐标系下天顶矢量z b ；
步骤3、选择VIO中惯导误差状态和不同时刻相机位姿误差状态建立估计状态向量，建立包括偏振传感器坐标系下天顶矢量z b 、偏振传感器坐标系下太阳矢量s b 和视觉相机坐标系下视觉残差信息的三维姿态量测；
步骤4、设计与VIO中视觉两帧之间的误差相关的自适应因子，对偏振传感器坐标系下天顶矢量z b 、偏振传感器坐标系下太阳矢量s b 和视觉相机坐标系下视觉残差信息进行加权融合，解算三维姿态信息；
在所述步骤2中根据步骤1中得到的偏振传感器坐标系下的中性点矢量n b 和太阳矢量s b 信息，确定在偏振传感器坐标系下过太阳、中性点和天顶点的平面，记为平面α；
导航系下太阳的天顶角
Figure 427955DEST_PATH_IMAGE005
通过太阳天文年历和地理纬度得到：
Figure 719259DEST_PATH_IMAGE006
其中，
Figure 251872DEST_PATH_IMAGE007
表示导航系下太阳高度角，L为地理纬度，
Figure 654034DEST_PATH_IMAGE008
为太阳赤纬，Ω为太阳时 角；
以偏振传感器坐标系下的太阳矢量s b 为转轴，天顶角
Figure 791754DEST_PATH_IMAGE009
作为半轴角得到圆锥面z，利用该圆锥面z与平面α的交线得到偏振传感器坐标系下天顶矢量z b ：
Figure 8289DEST_PATH_IMAGE011
其中，
Figure 10880DEST_PATH_IMAGE012
为天顶角，同时表示圆锥面z的半轴角，z b 表示待求解的偏振传感器坐标系下的天顶矢量；
选择导航系下天顶矢量为
Figure 737528DEST_PATH_IMAGE013
,根据VIO提供的姿态信息得到一个预测的天顶矢量，记为：
Figure 432951DEST_PATH_IMAGE014
其中，
Figure 131700DEST_PATH_IMAGE015
表示VIO提供的导航系到偏振传感器坐标系的姿态转换矩阵，利用该预测的天顶矢量去除求得的偏振传感器坐标系下天顶矢量模糊性，确定唯一的天顶矢量。
2.根据权利要求1所述的一种基于天顶矢量的偏振/VIO三维姿态确定方法，其特征在于：在所述步骤1中，基于偏振度图像检测出中性点N在图像像素坐标系下位置坐标(u N ,v N )，并将其转为齐次坐标表示中性点矢量n b ，根据太阳矢量和偏振矢量垂直关系，利用两个观测方向的偏振矢量
Figure 610085DEST_PATH_IMAGE016
和
Figure 519136DEST_PATH_IMAGE017
解算得到太阳矢量s b ，进而确定偏振度图像像素坐标系中太阳的投影位置，假设太阳在图像像素坐标系中的位置坐标为(u s ,v s )，转换为归一化齐次坐标为：
Figure 57564DEST_PATH_IMAGE018
其中，
Figure 52065DEST_PATH_IMAGE019
表示偏振传感器坐标系下太阳高度角，K表示相机内参矩阵。
3.根据权利要求1所述的一种基于天顶矢量的偏振/VIO三维姿态确定方法，其特征在 于：在所述步骤3中以VIO中惯导误差状态
Figure 334142DEST_PATH_IMAGE020
，其中R 6×1表示6×1向 量，和不同时刻相机状态
Figure 300961DEST_PATH_IMAGE021
建立系统估计状态向量：
Figure 338187DEST_PATH_IMAGE022
其中，
Figure 492088DEST_PATH_IMAGE023
表示载体姿态角误差，
Figure 640172DEST_PATH_IMAGE024
表示陀螺仪的零偏，
Figure 195919DEST_PATH_IMAGE025
表示第i时刻相机姿态角误差，
Figure 607309DEST_PATH_IMAGE026
表示第i时刻相机位置误差；
建立包括步骤2中得到的天顶矢量、太阳矢量和视觉残差信息的系统量测模型如下：
Figure 310822DEST_PATH_IMAGE027
其中，
Figure 934702DEST_PATH_IMAGE028
表示从导航系到偏振传感器坐标系的姿态转换矩阵，z v 表示视觉残差量测，
Figure 672851DEST_PATH_IMAGE029
表示视觉量测雅克比矩阵，
Figure 255142DEST_PATH_IMAGE030
表示视觉量测噪声，
Figure 383635DEST_PATH_IMAGE031
表示太阳矢量量测噪声，
Figure 873522DEST_PATH_IMAGE032
表示天顶矢量量测噪声。
4.根据 权利要求1所述的一种基于天顶矢量的偏振/VIO三维姿态确定方法，其特征在于：在所述步骤4中依据VIO中视觉两帧之间差值的变化设计自适应因子调节系统的过程噪声方差阵Q阵和量测噪声方差阵R阵：
Figure 138281DEST_PATH_IMAGE033
其中，b是遗忘因子，b k+1表示遗忘因子的次方；
Figure 688211DEST_PATH_IMAGE034
表示第k时刻的相机y方向位置误差，即无人机前进方向位置增量，
Figure 569579DEST_PATH_IMAGE035
表示根据视觉前后两帧的帧间误差的自适应因子，Q阵和R阵调节过程如下：
Figure 800841DEST_PATH_IMAGE037
Figure 248002DEST_PATH_IMAGE039
其中，
Figure 640938DEST_PATH_IMAGE040
表示量测噪声方差阵的估计；
Figure 9602DEST_PATH_IMAGE041
表示过程噪声方差阵的估计；
Figure 841292DEST_PATH_IMAGE042
为量测新息；H k+1为量测阵；K k+1表示卡尔曼滤波增益；Φ k+1,k 表示状态转移矩阵；P k 表示k时刻状态协方差。
CN202111381893.5A 2021-11-22 2021-11-22 一种基于天顶点矢量的偏振/vio三维姿态确定方法 Active CN113819904B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111381893.5A CN113819904B (zh) 2021-11-22 2021-11-22 一种基于天顶点矢量的偏振/vio三维姿态确定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111381893.5A CN113819904B (zh) 2021-11-22 2021-11-22 一种基于天顶点矢量的偏振/vio三维姿态确定方法

Publications (2)

Publication Number Publication Date
CN113819904A CN113819904A (zh) 2021-12-21
CN113819904B true CN113819904B (zh) 2022-02-22

Family

ID=78917935

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111381893.5A Active CN113819904B (zh) 2021-11-22 2021-11-22 一种基于天顶点矢量的偏振/vio三维姿态确定方法

Country Status (1)

Country Link
CN (1) CN113819904B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114894197B (zh) * 2022-07-08 2022-10-21 北京航空航天大学 一种基于天顶点实时跟踪的水下偏振自主航向计算方法
CN116182855B (zh) * 2023-04-28 2023-07-07 北京航空航天大学 一种弱光强环境下仿复眼偏振视觉无人机组合导航方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104613956A (zh) * 2015-01-28 2015-05-13 南昌大学 基于大气偏振中性点的导航定向方法
CN104880191B (zh) * 2015-06-02 2016-03-02 北京航空航天大学 一种基于太阳矢量的偏振辅助导航方法
JP6650848B2 (ja) * 2016-08-22 2020-02-19 株式会社ソニー・インタラクティブエンタテインメント 情報処理装置、情報処理システム、および情報処理方法
CN111220150B (zh) * 2019-12-09 2021-09-14 北京航空航天大学 一种基于水下偏振分布模式的太阳矢量解算方法
CN112066979B (zh) * 2020-08-27 2021-12-28 北京航空航天大学 一种基于偏振位姿信息耦合迭代自主导航定位方法
CN112129288B (zh) * 2020-11-24 2021-02-02 中国人民解放军国防科技大学 一种基于偏振光/地磁航向约束的位姿估计方法及系统

Also Published As

Publication number Publication date
CN113819904A (zh) 2021-12-21

Similar Documents

Publication Publication Date Title
CN109931926B (zh) 一种基于站心坐标系的小型无人机无缝自主式导航方法
CN107063170B (zh) 复杂环境下基于大气偏振角模式的航向角估计方法
CN105865454B (zh) 一种基于实时在线地图生成的无人机导航方法
CN105678754B (zh) 一种无人机实时地图重建方法
CN103674021B (zh) 基于捷联惯导与星敏感器的组合导航系统及方法
CN104880191B (zh) 一种基于太阳矢量的偏振辅助导航方法
CN110231025B (zh) 一种基于捷联式偏振光罗盘的动态定向方法及系统
CN108387206B (zh) 一种基于地平线与偏振光的载体三维姿态获取方法
CN101344391A (zh) 基于全功能太阳罗盘的月球车位姿自主确定方法
CN103575274B (zh) 高性能星图匹配自主导航定位系统
CN115574816A (zh) 仿生视觉多源信息智能感知无人平台
CN113819904B (zh) 一种基于天顶点矢量的偏振/vio三维姿态确定方法
CN111595330B (zh) 一种基于概率密度函数估计的夜间偏振航向计算方法
CN106289250A (zh) 一种航向信息采集系统
CN111238540A (zh) 一种基于恒星拍摄的珞珈一号相机-星敏安装标定方法
CN110887473A (zh) 一种基于偏振度加权的仿生偏振自主组合导航方法
CN113686299B (zh) 一种海上动态目标定位与移速预测方法
CN117268373B (zh) 一种多传感器信息融合的自主导航方法及系统
CN112444245A (zh) 一种基于偏振光、光流矢量、双目视觉传感器的仿昆虫视觉组合导航方法
CN115597586A (zh) 一种基于大气偏振模式对称性的航向角提取方法
CN112129288B (zh) 一种基于偏振光/地磁航向约束的位姿估计方法及系统
CN103365302A (zh) 三相限太阳光线传感器与光斑图像传感器融合的太阳光线跟踪算法
CN110887477B (zh) 一种基于偏振北极点及偏振太阳矢量的自主定位方法
CN114509071B (zh) 一种风洞试验模型姿态测量方法
CN116088012A (zh) 适用于城市峡谷环境的gnss nlos数据去污与加权优化定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant