CN113819904B - 一种基于天顶点矢量的偏振/vio三维姿态确定方法 - Google Patents
一种基于天顶点矢量的偏振/vio三维姿态确定方法 Download PDFInfo
- 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
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)通过提取自然环境中特征点并结合惯性导航技术,完成对于载体自身的姿态估计,但是其对于航向的估计能力较差,同时在环境特征缺失的场景下易出现姿态估计不准的问题。
目前利用视觉导航、惯性导航和图像式偏振导航组合导航的方法有:专利“一种基于视觉惯性偏振光融合的无人机位姿估计方法”(申请号: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 ),转换为归一化齐次坐标为:
在步骤2中根据步骤1中得到的偏振传感器坐标系下的中性点矢量n b 和太阳矢量s b 信息,确定在偏振传感器坐标系下过太阳、中性点和天顶点的平面,记为平面α。
系统整体量测方程为:
其中,z表示系统量测,由太阳矢量量测z s 、天顶矢量量测z b 和视觉残差量测z v 组成,H表示三者组合的量测矩阵,v表示三者组合量测噪声。
在步骤4中设计自适应因子,以VIO两帧之间差值的变化来设计一个自适应因子,来提高系统的滤波精度。其具体步骤如下:
设计自适应因子如下:
其中,为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 ),转换为齐次坐标可以得到:
在步骤2中根据步骤1中得到的偏振传感器坐标系下的中性点矢量n b 和太阳矢量s b ,确定在偏振传感器坐标系下过太阳、中性点和天顶点的平面,记为平面α。
系统整体量测方程为:
其中,z表示系统量测,由太阳矢量量测z s 、天顶矢量量测z b 和视觉残差量测z v 组成,H表示三者组合的量测矩阵,v表示三者组合量测噪声。
在步骤4中设计自适应因子,以VIO中视觉两帧之间差值的变化来设计一个自适应因子,来提高系统的滤波精度。其具体步骤如下:
设计自适应因子如下:
得到新的滤波过程为:
其中,为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时刻状态协方差。
自适应的物理意义在于当视觉惯性里程计两帧之间的误差大于某一个阈值时候,该阈值设为无人机位置增量的函数,即表示两帧间误差不大于无人机相应时间内移动过的距离,对系统量测噪声进行调节,进而达到最优的估计结果。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。以上所述,仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到变化或替换,都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应以所述权利要求的保护范围为准。
Claims (4)
1.一种基于天顶矢量的偏振/VIO三维姿态确定方法,其特征在于,实现步骤如下:
步骤1、利用图像式偏振传感器获得图像像素坐标系下天空偏振度分布图像和偏振角分布图像,基于偏振度分布图像检测出中性点N,并求得在图像像素坐标系下位置坐标(u N ,v N ),得到偏振传感器坐标系下的中性点矢量n b ;基于偏振角分布图像计算两个观测方向的偏振矢量和,求得偏振传感器坐标系下太阳矢量s b ,确定太阳在图像像素坐标系下的位置坐标(u s ,v s );
步骤2、利用偏振度分布图像中太阳、中性点和天顶点共线的关系,且天顶角与偏振传感器坐标系下太阳矢量s b 和天顶矢量z b 的夹角相同,利用太阳天文年历和地理经纬度计算得到导航系下太阳的天顶角,计算偏振传感器坐标系下天顶矢量z b ;
步骤3、选择VIO中惯导误差状态和不同时刻相机位姿误差状态建立估计状态向量,建立包括偏振传感器坐标系下天顶矢量z b 、偏振传感器坐标系下太阳矢量s b 和视觉相机坐标系下视觉残差信息的三维姿态量测;
步骤4、设计与VIO中视觉两帧之间的误差相关的自适应因子,对偏振传感器坐标系下天顶矢量z b 、偏振传感器坐标系下太阳矢量s b 和视觉相机坐标系下视觉残差信息进行加权融合,解算三维姿态信息;
在所述步骤2中根据步骤1中得到的偏振传感器坐标系下的中性点矢量n b 和太阳矢量s b 信息,确定在偏振传感器坐标系下过太阳、中性点和天顶点的平面,记为平面α;
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)
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)
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 | 中国人民解放军国防科技大学 | 一种基于偏振光/地磁航向约束的位姿估计方法及系统 |
-
2021
- 2021-11-22 CN CN202111381893.5A patent/CN113819904B/zh active Active
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 |