CN103033189A - 一种深空探测巡视器惯性/视觉组合导航方法 - Google Patents
一种深空探测巡视器惯性/视觉组合导航方法 Download PDFInfo
- Publication number
- CN103033189A CN103033189A CN2012105749474A CN201210574947A CN103033189A CN 103033189 A CN103033189 A CN 103033189A CN 2012105749474 A CN2012105749474 A CN 2012105749474A CN 201210574947 A CN201210574947 A CN 201210574947A CN 103033189 A CN103033189 A CN 103033189A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- coordinate
- unique point
- state
- navigation
- 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.)
- Granted
Links
Images
Landscapes
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明涉及一种深空探测巡视器惯性/视觉组合导航方法,根据行星固连坐标系下捷联惯导的力学编排,建立巡视器惯性/视觉组合导航的状态模型;利用双目视觉相机对巡视器周围环境进行拍摄,获得立体图像序列,通过图像特征提取算法得到特征点的像素坐标,建立以特征点像素坐标为量测向量的量测模型;并使用Unscented卡尔曼滤波估计深空探测巡视器的位置、速度和姿态。本发明能够有效修正惯导的位置误差,提高导航精度,非常适用于深空探测巡视器的自主导航,本发明属于航天导航技术领域,不仅可以为深空探测巡视器提供高精度导航参数,而且可为其自主导航***设计提供参考。
Description
技术领域
本发明涉及深空探测巡视器在行星表面巡视勘察时,基于图像特征点像素坐标信息的惯性/视觉组合导航方法,是一种非常适用于深空探测巡视器的自主导航方法。
背景技术
深空探测作为人类航天活动的重要领域,对一个国家的科学研究、经济发展和军事应用价值都有一般航天活动无法比拟的特殊作用。深空探测巡视器作为当前行星际探测的主要工具,在行星表面自动巡视勘测、图像获取、采样分析和返回等方面发挥着重要作用,是当前研究的热点之一。
巡视器要在行星表面陌生的非结构化环境中安全行驶和工作,有效的导航是其可以成功完成预定科学探测任务的前提。由于行星空间环境的特殊性,现有的大多数地面机器人导航方法往往不能用于深空探测巡视器,当前深空探测巡视器主要还是借助地面测控站进行遥测遥制,但我国受地理条件的限制,加之地球和各天体的自转和公转运动的影响,可测控时间较短,而且即使在可测控时段,从测量数据下传到控制信息上传之间也存在较长的时间延迟,如NASA火星探测任务中一个导航控制指令的延迟通常为一个火星日,因此为提高巡视器的安全性、可靠性以及工作效率,较先进的技术是机器人利用自己的传感和控制***实现自主导航。
目前可以用于深空探测巡视器的自主导航方式主要有:惯性导航(航位推算)、天文导航、视觉导航和组合导航等。惯性导航具有自主性强、短时精度高、导航参数完备等优点,但由于惯性器件存在漂移,其导航误差随时间积累。视觉导航***具有体积小、重量轻、寿命长、可靠性高、图像信息丰富、抗干扰能力强等优点,对于短距离的障碍检测和路径规划非常有效,但数据更新率低。惯性导航和视觉导航有其各自的特点,并具有互补的特性,将两种导航方法相结合,取长补短,利用信息融合技术进行组合导航,可以最大限度地提高导航的精度和可靠性。目前应用于巡视器的惯性/视觉组合导航方法主要是惯性导航(航位推算)和视觉里程计相结合的方法,在可能存在滑移的情况下,使用视觉里程计完全取代IMU和车轮里程计得到的位置信息,而没有使用卡尔曼滤波及相关算法来融合IMU,车轮编码器和视觉里程计的运动信息。该方法没有通过组合导航的方式来提高导航精度,而是分时使用两种导航方法,且由于视觉里程计是通过运动估计获得相邻时刻的相对导航参数,然后在前一时刻导航参数基础上进行叠加来更新导航结果,其本身误差亦会随时间积累,从而导致导航的精度不高。
发明内容
本发明要解决的技术问题是:克服传统的惯性导航和航位推算相结合的位置估计方法在产生滑移情况下的精度低、安全性差等缺点,弥补惯性导航与视觉里程计组合模式中运动估计误差对组合导航精度影响大这一不足,为深空探测巡视器在行星表面漫游提供一种高精度的惯性/视觉组合导航方法。
本发明解决其技术问题所采用的技术方案为:建立恰当的深空探测巡视器状态模型,通过双目视觉相机连续拍摄获得巡视器周围环境信息的立体图像序列,再通过图像特征提取获得图像中的特征点像素坐标,建立以特征点像素坐标为量测量的量测模型,使用Unscented卡尔曼滤波估计深空探测巡视器的导航参数。
具体包括以下步骤:
1.建立深空探测巡视器惯性/视觉组合导航***状态模型;
选取行星固连坐标系为全局参考坐标系,其定义为:原点与行星质心重合,Z轴指向行星北极,X轴指向行星赤道面与0°子午线的交点,Y轴在赤道平面里与XOZ构成右手坐标系,根据捷联惯导在行星固连坐标系下的导航解算方程,可得组合导航的状态模型:
式中,为从行星固连坐标系到巡视器本体坐标系旋转的单位四元数,为4×1维的向量,和分别为巡视器在行星固连坐标系下的位置和速度,均为3×1维的向量,bg和ba分别为巡视器本体坐标系下陀螺仪和加速度计的常值偏置,均为3×1维的向量。为巡视器本体坐标系相对惯性坐标系的旋转角速率在巡视器本体坐标系中的表示,由陀螺仪直接测得,fb为比力信息,由加速度计直接测得,为行星自转角速率在行星固连坐标系下的矢量表示,为巡视器本体坐标系相对行星固连坐标系的方向余弦矩阵,gp为重力矢量在行星固连坐标系下的表示,Ω(·)为四元数矩阵,wg和wa分别为陀螺仪和加速度计的随机误差,均为零均值的高斯白噪声;
式(1)中变量都是与t有关的函数,可简写为
2.建立基于特征点像素坐标为量测量的量测模型;
式中,k表示时间序号,k=1,2,...,为摄像机相对巡视器本体坐标系的安装矩阵,为摄像机在巡视器本体坐标系中的位置,为3×1维的向量,由标定可知,和分别为tk时刻和tk+1时刻巡视器在行星固连坐标系中的位置,均为3×1维的向量,和分别为tk时刻和tk+1时刻巡视器本体坐标系相对行星固连坐标系的姿态矩阵,均为3×3维,和可根据tk时刻组合导航的滤波结果获得,和可由tk+1时刻惯导的状态更新结果获得;
跟踪得到的第i个特征点在相邻时刻摄像机坐标系下的三维坐标关系式为:
式中,k=1,2,…,为第i个特征点在tk时刻左摄像机坐标系下的三维坐标,根据三角测量原理和双目视觉***参数可以获得,为第i个特征点在tk+1时刻左摄像机坐标系下三维坐标的估计值, 分别为从tk时刻到tk+1时刻摄像机坐标系之间的旋转矩阵和平移矢量,由式(3)获得;
式中,为第i个特征点在左摄像机中对应的量测信息,为第i个特征点tk+1时刻在左摄像机成像平面中的像素坐标预估值,dX,dY分别为每个像素在左摄像机成像平面横轴和纵轴方向的长和宽,(ul0,vl0)为左摄像机光轴与成像平面交点的像素坐标,为左摄像机焦距,为第i个特征点在tk+1时刻左摄像机坐标系下的三维坐标预估值,M为tk时刻到tk+1时刻之间跟踪到的特征点个数,为左图像中特征点提取误差,为2×1维列向量;
式中,R,T分别为右摄像机坐标系相对于左摄像机坐标系的旋转矩阵和平移矢量,由标定可知。
式中,为第i个特征点在右摄像机中对应的量测信息,为第i个特征点tk+1时刻在右摄像机成像平面中的像素坐标预估值,dX,dY分别为每个像素在右摄像机成像平面横轴和纵轴方向的长和宽,其值与左摄像机相同,(ur0,vr0)为右摄像机光轴与成像平面交点的像素坐标,为右摄像机焦距,为第i个特征点在tk+1时刻右摄像机坐标系下的三维坐标预估值,M为tk时刻到tk+1时刻之间跟踪到的特征点个数,为右图像中特征点提取误差,为2×1维列向量;
将全部特征点在左、右摄像机成像平面中的像素坐标构成一个列向量 即***量测量,共4M维,设量测噪声 分别为 的观测误差,由于各变量都是与时间t有关的变量,则可建立像素坐标信息量测模型的表达式为:
Z(t)=h(X(t),t)+v(t) (8)
3.对步骤1中式(2)所示的状态模型及步骤2中式(8)所示的量测模型进行离散化:
X(k+1)=F(X(k),k)+w(k) (9)
Z(k)=H(X(k),k)+v(k) (10)
式中,k=1,2,…,F(X(k),k)为f(X(t),t)离散后的非线性状态转移函数,H(X(k),k)为h(X(t),t)离散后的非线性量测函数,w(k)、v(k)分别为离散后的***噪声和量测噪声,它们互不相关;
4.Unscented卡尔曼滤波;
利用Unscented卡尔曼滤波算法,结合步骤1和步骤2所述的状态模型和量测模型进行滤波,利用双目视觉相机拍摄立体图像序列,再进行图像特征提取获得特征点的像素坐标信息,通过量测量与量测模型得到的量测预测值相减得到***量测残差,用***这一残差校正状态一步预测的误差;由步骤1可知,状态向量的维数n′为16,因此可得2n′+1即33个Unscented采样点,利用***状态模型,对采样点进行一步预测,并得出与上一时刻滤波得到的迭代状态值之间的协方差阵,以消除状态模型中模型误差的影响,最终估计出行星固连坐标系下的导航信息;
5.将步骤4中获得的状态量的估计值和状态估计方差用于下一时刻的滤波,并输出状态估计值和状态估计方差,同时,将步骤4中获得的行星固连坐标系中的位置、速度和姿态信息通过矩阵变换转换到以东北天坐标系表示的导航坐标系中并输出。
本发明的原理是:首先建立深空探测巡视器惯性/视觉组合导航模型,利用行星固连坐标系下的捷联惯导解算方程建立其状态模型,提高了计算效率,然后通过对双目视觉***拍摄的立体图像序列进行图像处理获得的特征点像素坐标信息,建立以图像特征点像素坐标为量测量的量测模型,在一定程度上避免了将视觉导航中的累积误差引入到组合导航***中,从而提高了最终的导航精度,同时也避免了视觉导航中繁杂的非线性位姿方程求解,有利于提高导航***的实时性和鲁棒性。由于受到惯性测量仪器精度和图像处理算法精度的制约,***状态模型和量测模型都存在模型误差,且组合导航***的状态方程和量测方程均为非线性,因此在组合导航滤波中使用Unscented卡尔曼滤波方法,用量测量与量测模型之间的残差校正状态更新的误差,并解决***的非线性问题,实现对位置、速度、姿态等导航参数的估计。
本发明与现有技术相比的优点在于:(1)将惯导***和视觉***构成组合导航***,并直接利用特征点的像素坐标作为量测量,避免后续的纯视觉导航运动估计算法引入附加的量测信息误差,提高了量测信息的精度,充分融合了惯性信息和视觉信息,从而提高了导航***的精度;(2)选取行星固连坐标系为全局坐标系,建立状态方程,简化了量测方程的推导,并在此基础上建立了准确的量测方程,提高了计算效率;(3)使用姿态四元数来描述姿态,避免了欧拉角表示的奇点问题,也提高了姿态解算的精度,加快了计算速度。
附图说明
图1为本发明深空探测巡视器惯性/视觉组合导航方法的流程图;
图2为本发明中双目立体视觉***成像原理图。
具体实施方式
本实施例所述的巡视器惯性/视觉组合导航方法在状态模型和量测模型中均涉及到许多不同的坐标系及相互之间的转换,坐标系定义如下:
①惯性坐标系(opxiyizi):其原点在行星质心,zi轴垂直于赤道平面指向北极,xi轴在赤道平面内指向春分点方向,yi轴与xi、zi轴构成右手正交坐标系。
②行星固连坐标系(opxpypzp):其原点和zp轴指向与惯性坐标系相同,xp轴在赤道平面内指向本初子午线,yp轴与xp、zp轴构成右手正交坐标系。
③行星大地坐标系(L,λ,h):坐标依次为纬度、经度和高度。纬度L为过巡视器的行星球面法线与赤道面的夹角,经度λ为巡视器所在的子午面与0°子午面之间的夹角,高度h为巡视器沿球面法线到球面的距离。
④导航坐标系(oxnynzn):选取东北天坐标系为导航坐标系,即地理坐标系,其原点位于巡视器质心,xn轴指向东,yn轴指向北,zn轴指向天。
⑤巡视器本体坐标系(oxbybzb):与巡视器刚性固连,其原点位于巡视器质心,xb轴沿巡视器横轴指右,yb轴沿巡视器纵轴指前,zb轴沿巡视器竖轴并与xb、yb轴构成右手直角坐标系。
⑥IMU坐标系:与巡视器本体坐标系重合。
⑦摄像机坐标系(ocxcyczc):分为左摄像机坐标系(Clxlylzl)和右摄像机坐标系(Crxryrzr),其相应于左、右摄像机的定义均为,原点为摄像机光心,zc轴沿光轴指向景物方向,xc轴与像素的行平行并指向右,yc轴与像素的列平行并指向下,构成右手直角坐标系。
⑧图像物理坐标系(o1xy):分为左摄像机图像物理坐标系(OlXlYl)和右摄像机图像物理坐标系(OrXrYr),其分别定义在左、右摄像机的成像平面内,原点为相应摄像机光轴与图像平面的交点,x轴和y轴分别与相应摄像机坐标系的xc轴和yc轴平行且方向相同,坐标具有长度单位。
⑨图像像素坐标系(o0uv):分为左摄像机图像像素坐标系(olulvl)和右摄像机图像像素坐标系(orurvr),其定义均为,坐标原点位于图像像素阵列的左上角,u轴和v轴分别平行于摄像机坐标系的xc轴和yc轴且方向相同,像素坐标是像点在像素阵列中的行数和列数。
如图1所示,本发明的具体实施方法如下:
1.建立深空探测巡视器惯性/视觉组合导航***状态模型
设状态向量 其中,为巡视器相对行星固连坐标系的姿态四元数,和为巡视器在行星固连坐标系中的三轴位置和速度,bg和ba为巡视器本体坐标系下的陀螺常值漂移和加速度计常值零偏,首先初始化巡视器的位置、速度和姿态,根据巡视器的运行特点和行驶环境,选取巡视器在导航坐标系下的位置、速度、姿态初值以及陀螺漂移和加计零偏的滤波初值为:
将位置、速度和姿态初值转换到行星固连坐标系中作为滤波初值,之后建立巡视器惯性/视觉组合导航的状态模型:选取行星固连坐标系为全局参考坐标系,根据捷联惯导在行星固连坐标系下的导航解算方程,可得组合导航***的状态模型为:
式中,为从行星固连坐标系到巡视器本体坐标系旋转的单位四元数,为4×1维的向量,和分别为巡视器在行星固连坐标系下的位置和速度,均为3×1维的向量,bg和ba分别为巡视器本体坐标系下陀螺仪和加速度计的常值偏置,均为3×1维的向量。为巡视器本体坐标系相对惯性坐标系的旋转角速率在巡视器本体坐标系中的表示,由陀螺仪直接测得,fb为比力信息,由加速度计直接测得,为行星自转角速率在行星固连坐标系下的矢量表示,为巡视器本体坐标系相对行星固连坐标系的方向余弦矩阵,gp为重力矢量在行星固连坐标系下的表示,Ω(·)为四元数矩阵,wg和wa分别为陀螺仪和加速度计的随机误差,均为零均值的高斯白噪声;
式(1)中变量都是与t有关的函数,可简写为:
2.建立基于特征点像素坐标为量测量的量测模型
图2描述了平行双目立体视觉***的三维模型,由于物体在像平面上一般成倒立的像,为了得到正立的像和方便计算,将摄像机的实际成像平面根据直线ClCr进行镜像投影为虚拟的成像平面,L和R分别表示左摄像机成像平面和右摄像机成像平面。OlXlYl和OrXrYr分别为左、右摄像机图像物理坐标系,olulvl和orurvr分别为左、右摄像机图像像素坐标系,Cl和Cr分别为左、右摄像机镜头透镜的光心,Clxlylzl和Crxryrzr分别为左摄像机坐标系和右摄像机坐标系,B为基线长度。对获取的立体图像进行SIFT特征提取和立体匹配可以获得同一空间点Di在左、右摄像机成像平面中像点的像素坐标,根据图像像素坐标与物理坐标之间的转换关系,进而得到特征点在左、右摄像机成像平面中的图像物理坐标,分别为Pl和Pr,图2中,(xl′,yl′)为Pl的坐标,然后根据图2所示的平行双目立体视觉***成像的几何关系进行三维重建可得空间点Di在左摄像机坐标系下的三维坐标
式中,k表示时间序号,k=1,2,…,为摄像机相对巡视器本体坐标系的安装矩阵,为摄像机在巡视器本体坐标系中的位置,为3×1维的向量,由标定可知,和分别为tk时刻和tk+1时刻巡视器在行星固连坐标系中的位置,均为3×1维的向量,和分别为tk时刻和tk+1时刻巡视器本体坐标系相对行星固连坐标系的姿态矩阵,均为3×3维,和可根据tk时刻组合导航的滤波结果获得,和可由tk+1时刻惯导的状态更新结果获得;
跟踪得到的第i个特征点在相邻时刻摄像机坐标系下的三维坐标关系式为:
式中,k=1,2,…,为第i个特征点在tk时刻左摄像机坐标系下的三维坐标,根据三角测量原理和双目视觉***参数可以获得,为第i个特征点在tk+1时刻左摄像机坐标系下三维坐标的估计值, 分别为从tk时刻到tk+1时刻摄像机坐标系之间的旋转矩阵和平移矢量,由式(13)获得;
式中,为第i个特征点在左摄像机中对应的量测信息,为第i个特征点tk+1时刻在左摄像机成像平面中的像素坐标预估值,dX,dY分别为每个像素在左摄像机成像平面横轴和纵轴方向的长和宽,(ul0,vl0)为左摄像机光轴与成像平面交点的像素坐标,为左摄像机焦距,为第i个特征点在tk+1时刻左摄像机坐标系下的三维坐标预估值,M为tk时刻到tk+1时刻之间跟踪到的特征点个数,为左图像中特征点提取误差,为2×1维列向量;
式中,R,T分别为右摄像机坐标系相对于左摄像机坐标系的旋转矩阵和平移矢量,由标定可知。
式中,为第i个特征点在右摄像机中对应的量测信息,为第i个特征点tk+1时刻在右摄像机成像平面中的像素坐标预估值,dX,dY分别为每个像素在右摄像机成像平面横轴和纵轴方向的长和宽,其值与左摄像机相同,(ur0,vr0)为右摄像机光轴与成像平面交点的像素坐标,为右摄像机焦距,为第i个特征点在tk+1时刻右摄像机坐标系下的三维坐标预估值,M为tk时刻到tk+1时刻之间跟踪到的特征点个数,为右图像中特征点提取误差,为2×1维列向量;
将全部特征点在左、右摄像机成像平面中的像素坐标构成一个列向量 即***量测量,共4M维,设量测噪声 分别为 的观测误差,由于各变量都是与时间t有关的变量,则可建立像素坐标信息量测模型的表达式为:
Z(t)=h(X(t),t)+v(t) (18)
3.对步骤1中式(12)所示的状态模型及步骤2中式(18)所示的量测模型进行离散化:
X(k+1)=F(X(k),k)+w(k) (19)
Z(k)=H(X(k),k)+v(k) (20)
式中,k=1,2,…,F(X(k),k)为f(X(t),t)离散后的非线性状态转移函数,H(X(k),k)为h(X(t),t)离散后的非线性量测函数,w(k)、v(k)分别为离散后的***噪声和量测噪声,它们互不相关;
4.Unscented卡尔曼滤波
利用Unscented卡尔曼滤波算法,结合步骤1和步骤2所述的状态模型和量测模型进行滤波,利用双目视觉相机拍摄立体图像序列,再进行图像特征提取获得特征点的像素坐标信息,通过量测量与量测模型得到的量测预测值相减得到***量测残差,用***这一残差校正状态一步预测的误差;由步骤1可知,状态向量的维数n′为16,因此可得2n′+1即33个Unscented采样点,利用***状态模型,对采样点进行一步预测,并得出与上一时刻滤波得到的迭代状态值之间的协方差阵,以消除状态模型中模型误差的影响,最终输出状态量的滤波估计结果,具体步骤为:
②计算状态量采样点
在附近选取一系列采样点,这些采样点的均值和协方差分别为和Pk-1|k-1,这里且Pk-1|k-1=Pk-1。状态变量为16×1维,那么33个样本点χ0,k,χ1,k,...,χ32,k及其权重W0,W1…,W32分别如下:
③时间更新
状态量的一步预测χk+1|k为:
χk|k-1=f(χk-1,k-1) (24)
式中,Wj为第j个采样点的权值;
式中,Qk为***噪声协方差;
采样点对应的量测估计值Zk|k=1
Zk|k-1=h(χk|k-1,k) (27)
④量测更新
式中,Rk为量测噪声协方差;
状态变量量测量方差阵
滤波增益Kk为:
将获得的状态量的估计值和估计方差Pk返回滤波器,用于k+1时刻,k=1,2,...,最终将状态估计值和状态估计方差Pk输出,其中状态估计值包括探测器的速度、位置、姿态四元数、陀螺漂移和加速度计零偏信息,输出的状态估计方差Pk表示了滤波估计的性能。
5.求解导航坐标系中的位置、速度和姿态矩阵
其中涉及的中间变量的表达式如下:
式中,a,b,e分别为大地坐标系对应椭圆的半长轴、半短轴和第一偏心率;n为椭球的卯酉圈曲率半径,L,λ,h分别为巡视器所在位置的纬度、经度和高度。
根据上述公式进行迭代即可获得巡视器的大地坐标(λ,L,h)。
地理坐标系相对行星固连坐标系的旋转矩阵为:
巡视器在导航坐标系下的速度为:
则姿态矩阵为:
最后,输出巡视器在导航坐标系中的位置、速度和姿态信息。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
Claims (1)
1.一种深空探测巡视器的惯性/视觉组合导航方法,其特征在于:首先根据行星固连坐标系下的捷联惯导力学编排方程,建立惯性/视觉组合导航的状态模型,再利用双目视觉相机对周围环境进行拍摄,获得立体图像序列,通过图像特征提取算法得到特征点的像素坐标,建立以特征点像素坐标为量测量的量测模型,并使用Unscented卡尔曼滤波估计深空探测巡视器的位置、速度和姿态;具体包括以下步骤:
①建立深空探测巡视器惯性/视觉组合导航***状态模型;
选取行星固连坐标系为全局参考坐标系,可得组合导航的状态模型:
式中,为从行星固连坐标系到巡视器本体坐标系旋转的单位四元数,为4×1维的向量,和分别为巡视器在行星固连坐标系下的位置和速度,均为3×1维的向量,bg和ba分别为巡视器本体坐标系下陀螺仪和加速度计的常值偏置,均为3×1维的向量,为巡视器本体坐标系相对惯性坐标系的旋转角速率在巡视器本体坐标系中的表示,由陀螺仪直接测得,fb为比力信息,由加速度计直接测得,为行星自转角速率在行星固连坐标系下的矢量表示,为巡视器本体坐标系相对行星固连坐标系的方向余弦矩阵,gp为重力矢量在行星固连坐标系下的表示,Ω(·)为四元数矩阵,wg和wa分别为陀螺仪和加速度计的随机误差,均为零均值的高斯白噪声;
式(1)中变量都是与t有关的函数,可简写为:
②建立基于特征点像素坐标为量测量的量测模型;
式中,k表示时间序号,k=1,2,…,为摄像机相对巡视器本体坐标系的安装矩阵,为摄像机在巡视器本体坐标系中的位置,为3×1维的向量,由标定可知,和分别为tk时刻和tk+1时刻巡视器在行星固连坐标系中的位置,均为3×1维的向量,和分别为tk时刻和tk+1时刻巡视器本体坐标系相对行星固连坐标系的姿态矩阵,均为3×3维,和根据tk时刻组合导航的滤波结果获得,和由tk+1时刻惯导的状态更新结果获得;
跟踪得到的第i个特征点在相邻时刻摄像机坐标系下的三维坐标关系式为:
式中,k=1,2,…,为第i个特征点在tk时刻左摄像机坐标系下的三维坐标,为第i个特征点在tk+1时刻左摄像机坐标系下三维坐标的估计值, 分别为从tk时刻到tk+1时刻摄像机坐标系之间的旋转矩阵和平移矢量,由式(3)获得;
式中,为第i个特征点在左摄像机中对应的量测信息,为第i个特征点tk+1时刻在左摄像机成像平面中的像素坐标预估值,dX,dY分别为每个像素在左摄像机成像平面横轴和纵轴方向的长和宽,(ul0,vl0)为左摄像机光轴与成像平面交点的像素坐标,为左摄像机焦距,为第i个特征点在tk+1时刻左摄像机坐标系下的三维坐标预估值,M为tk时刻到tk+1时刻之间跟踪到的特征点个数,为左图像中特征点提取误差,为2×1维列向量;
式中,R,T分别为右摄像机坐标系相对于左摄像机坐标系的旋转矩阵和平移矢量;
式中,为第i个特征点在右摄像机中对应的量测信息,为第i个特征点tk+1时刻在右摄像机成像平面中的像素坐标预估值,dX,dY分别为每个像素在右摄像机成像平面横轴和纵轴方向的长和宽,其值与左摄像机相同,(ur0,vr0)为右摄像机光轴与成像平面交点的像素坐标,为右摄像机焦距,为第i个特征点在tk+1时刻右摄像机坐标系下的三维坐标预估值,M为tk时刻到tk+1时刻之间跟踪到的特征点个数,为右图像中特征点提取误差,为2×1维列向量;
将全部特征点在左、右摄像机成像平面中的像素坐标构成一个列向量 即***量测量,共4M维,设量测噪声 分别为 的观测误差,由于各变量都是与时间t有关的变量,则可建立基于像素坐标量测信息的量测模型表达式为:
Z(t)=h(X(t),t)+v(t) (8)
③对步骤①中式(2)所示的状态模型及步骤②中式(8)所示的量测模型进行离散化:
X(k+1)=F(X(k),k)+w(k) (9)
Z(k)=H(X(k),k)+v(k) (10)
式中,k=1,2,…,F(X(k),k)为f(X(t),t)离散后的非线性状态转移函数,H(X(k),k)为h(X(t),t)离散后的非线性量测函数,w(k)、v(k)分别为离散后的***噪声和量测噪声,它们互不相关;
④利用Unscented卡尔曼滤波算法,结合步骤①和步骤②所述的状态模型和量测模型进行滤波,利用双目视觉相机拍摄立体图像序列,再进行特征提取获得特征点的像素坐标信息,通过量测量与量测模型得到的量测预测值相减得到***量测残差,用***这一残差校正状态一步预测的误差;根据状态向量可得相应的Unscented采样点,利用***状态模型,对采样点进行一步预测,并得出与上一时刻滤波得到的迭代状态值之间的协方差阵,以消除状态模型中模型误差的影响;
⑤输出导航信息;
将步骤④中获得的状态量的估计值和状态估计方差用于下一时刻的滤波,并输出状态估计值和状态估计方差,同时,将步骤④中获得的行星固连坐标系中的位置、速度和姿态信息通过矩阵变换转换到以东北天坐标系表示的导航坐标系中并输出。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201210574947.4A CN103033189B (zh) | 2012-12-26 | 2012-12-26 | 一种深空探测巡视器惯性/视觉组合导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201210574947.4A CN103033189B (zh) | 2012-12-26 | 2012-12-26 | 一种深空探测巡视器惯性/视觉组合导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103033189A true CN103033189A (zh) | 2013-04-10 |
CN103033189B CN103033189B (zh) | 2015-05-20 |
Family
ID=48020326
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201210574947.4A Active CN103033189B (zh) | 2012-12-26 | 2012-12-26 | 一种深空探测巡视器惯性/视觉组合导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103033189B (zh) |
Cited By (28)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103869820A (zh) * | 2014-03-18 | 2014-06-18 | 北京控制工程研究所 | 一种巡视器地面导航规划控制方法 |
CN103900577A (zh) * | 2014-04-14 | 2014-07-02 | 武汉科技大学 | 一种面向编队飞行的相对导航测速及组合导航方法 |
CN104176273A (zh) * | 2014-07-15 | 2014-12-03 | 北京航空航天大学 | 一种载人小行星探测的目标星选取方法 |
CN104596517A (zh) * | 2015-01-16 | 2015-05-06 | 西北工业大学 | 基于多平移信息的 vps/imu 组合导航数据的空间同步方法 |
CN105371840A (zh) * | 2015-10-30 | 2016-03-02 | 北京自动化控制设备研究所 | 一种惯性/视觉里程计/激光雷达的组合导航方法 |
CN105411490A (zh) * | 2015-10-26 | 2016-03-23 | 曾彦平 | 移动机器人的实时定位方法及移动机器人 |
CN106871902A (zh) * | 2017-02-16 | 2017-06-20 | 广东工业大学 | 一种无人机导航的方法、装置以及*** |
CN106885571A (zh) * | 2017-03-07 | 2017-06-23 | 辽宁工程技术大学 | 一种结合imu和导航影像的月面巡视器快速定位方法 |
CN107270900A (zh) * | 2017-07-25 | 2017-10-20 | 广州阿路比电子科技有限公司 | 一种6自由度空间位置和姿态的检测***和方法 |
CN107621266A (zh) * | 2017-08-14 | 2018-01-23 | 上海宇航***工程研究所 | 基于特征点跟踪的空间非合作目标相对导航方法 |
CN107796391A (zh) * | 2017-10-27 | 2018-03-13 | 哈尔滨工程大学 | 一种捷联惯性导航***/视觉里程计组合导航方法 |
CN108981693A (zh) * | 2018-03-22 | 2018-12-11 | 东南大学 | 基于单目相机的vio快速联合初始化方法 |
CN109186592A (zh) * | 2018-08-31 | 2019-01-11 | 腾讯科技(深圳)有限公司 | 用于视觉惯导信息融合的方法和装置以及存储介质 |
CN109360240A (zh) * | 2018-09-18 | 2019-02-19 | 华南理工大学 | 一种基于双目视觉的小型无人机定位方法 |
CN109387192A (zh) * | 2017-08-02 | 2019-02-26 | 湖南格纳微信息科技有限公司 | 一种室内外连续定位方法及装置 |
CN109443355A (zh) * | 2018-12-25 | 2019-03-08 | 中北大学 | 基于自适应高斯pf的视觉-惯性紧耦合组合导航方法 |
CN109443354A (zh) * | 2018-12-25 | 2019-03-08 | 中北大学 | 基于萤火虫群优化pf的视觉-惯性紧耦合组合导航方法 |
CN110542415A (zh) * | 2018-05-28 | 2019-12-06 | 北京京东尚科信息技术有限公司 | 用于导航***的导航方法和装置 |
CN111307176A (zh) * | 2020-03-02 | 2020-06-19 | 北京航空航天大学青岛研究院 | 一种vr头戴显示设备中视觉惯性里程计的在线标定方法 |
CN111947652A (zh) * | 2020-08-13 | 2020-11-17 | 北京航空航天大学 | 一种适用于月球着陆器的惯性/视觉/天文/激光测距组合导航方法 |
CN111947653A (zh) * | 2020-08-13 | 2020-11-17 | 北京航空航天大学 | 一种月面巡视探测器双模式惯性/视觉/天文导航方法 |
CN112004645A (zh) * | 2017-12-19 | 2020-11-27 | 卡内基梅隆大学 | 智能清洁机器人 |
CN113033462A (zh) * | 2021-04-09 | 2021-06-25 | 山东大学 | 基于火星尘卷风产率的火星着陆点确定方法及*** |
CN113985904A (zh) * | 2021-09-28 | 2022-01-28 | 哈尔滨工业大学 | 星球探测车运动模式生成及智能切换方法 |
CN114659526A (zh) * | 2022-02-11 | 2022-06-24 | 北京空间飞行器总体设计部 | 基于序列图像状态表达的航天器自主导航鲁棒滤波算法 |
US11421980B2 (en) * | 2017-11-22 | 2022-08-23 | Ford Global Technologies, Llc | Method for determining a position and orientation of an object using a profilometer |
CN117532604A (zh) * | 2023-11-08 | 2024-02-09 | 哈尔滨工业大学 | 一种基于立体视觉的目标位姿及高阶运动信息观测方法 |
US12038756B2 (en) | 2018-12-19 | 2024-07-16 | Carnegie Mellon University | Intelligent cleaning robot |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101598556A (zh) * | 2009-07-15 | 2009-12-09 | 北京航空航天大学 | 一种未知环境下无人机视觉/惯性组合导航方法 |
CN101762274A (zh) * | 2010-02-01 | 2010-06-30 | 北京理工大学 | 基于观测条件数的深空探测器自主定位路标选取方法 |
CN102168981A (zh) * | 2011-01-13 | 2011-08-31 | 北京航空航天大学 | 一种深空探测器火星捕获段自主天文导航方法 |
CN102175241A (zh) * | 2011-01-13 | 2011-09-07 | 北京航空航天大学 | 一种火星探测器巡航段自主天文导航方法 |
CN102519433A (zh) * | 2011-11-09 | 2012-06-27 | 中国测绘科学研究院 | 一种利用rpc反演星载线阵传感器几何定标参数方法 |
CN102538781A (zh) * | 2011-12-14 | 2012-07-04 | 浙江大学 | 基于机器视觉和惯导融合的移动机器人运动姿态估计方法 |
CN102636081A (zh) * | 2011-12-29 | 2012-08-15 | 南京航空航天大学 | 一种基于视觉运动建模的传递对准方法及装置 |
-
2012
- 2012-12-26 CN CN201210574947.4A patent/CN103033189B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101598556A (zh) * | 2009-07-15 | 2009-12-09 | 北京航空航天大学 | 一种未知环境下无人机视觉/惯性组合导航方法 |
CN101762274A (zh) * | 2010-02-01 | 2010-06-30 | 北京理工大学 | 基于观测条件数的深空探测器自主定位路标选取方法 |
CN102168981A (zh) * | 2011-01-13 | 2011-08-31 | 北京航空航天大学 | 一种深空探测器火星捕获段自主天文导航方法 |
CN102175241A (zh) * | 2011-01-13 | 2011-09-07 | 北京航空航天大学 | 一种火星探测器巡航段自主天文导航方法 |
CN102519433A (zh) * | 2011-11-09 | 2012-06-27 | 中国测绘科学研究院 | 一种利用rpc反演星载线阵传感器几何定标参数方法 |
CN102538781A (zh) * | 2011-12-14 | 2012-07-04 | 浙江大学 | 基于机器视觉和惯导融合的移动机器人运动姿态估计方法 |
CN102636081A (zh) * | 2011-12-29 | 2012-08-15 | 南京航空航天大学 | 一种基于视觉运动建模的传递对准方法及装置 |
Non-Patent Citations (3)
Title |
---|
李静: "对偶四元数在航天器相对导航中的应用", 《应用科学学报》, vol. 30, no. 3, 31 May 2012 (2012-05-31), pages 311 - 316 * |
王龙: "机器视觉辅助的无人机空中加油相对导航", 《应用科学学报》, vol. 30, no. 2, 31 March 2012 (2012-03-31), pages 209 - 214 * |
郭力: "基于单目视觉的微型飞行器移动目标定位方法", 《***工程与电子技术》, vol. 34, no. 5, 31 May 2012 (2012-05-31), pages 996 - 1000 * |
Cited By (38)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103869820B (zh) * | 2014-03-18 | 2015-10-21 | 北京控制工程研究所 | 一种巡视器地面导航规划控制方法 |
CN103869820A (zh) * | 2014-03-18 | 2014-06-18 | 北京控制工程研究所 | 一种巡视器地面导航规划控制方法 |
CN103900577B (zh) * | 2014-04-14 | 2016-08-17 | 武汉科技大学 | 一种面向编队飞行的相对导航测速及组合导航方法 |
CN103900577A (zh) * | 2014-04-14 | 2014-07-02 | 武汉科技大学 | 一种面向编队飞行的相对导航测速及组合导航方法 |
CN104176273A (zh) * | 2014-07-15 | 2014-12-03 | 北京航空航天大学 | 一种载人小行星探测的目标星选取方法 |
CN104596517A (zh) * | 2015-01-16 | 2015-05-06 | 西北工业大学 | 基于多平移信息的 vps/imu 组合导航数据的空间同步方法 |
CN105411490A (zh) * | 2015-10-26 | 2016-03-23 | 曾彦平 | 移动机器人的实时定位方法及移动机器人 |
CN105411490B (zh) * | 2015-10-26 | 2019-07-05 | 深圳市杉川机器人有限公司 | 移动机器人的实时定位方法及移动机器人 |
CN105371840A (zh) * | 2015-10-30 | 2016-03-02 | 北京自动化控制设备研究所 | 一种惯性/视觉里程计/激光雷达的组合导航方法 |
CN105371840B (zh) * | 2015-10-30 | 2019-03-22 | 北京自动化控制设备研究所 | 一种惯性/视觉里程计/激光雷达的组合导航方法 |
CN106871902A (zh) * | 2017-02-16 | 2017-06-20 | 广东工业大学 | 一种无人机导航的方法、装置以及*** |
CN106885571A (zh) * | 2017-03-07 | 2017-06-23 | 辽宁工程技术大学 | 一种结合imu和导航影像的月面巡视器快速定位方法 |
CN107270900A (zh) * | 2017-07-25 | 2017-10-20 | 广州阿路比电子科技有限公司 | 一种6自由度空间位置和姿态的检测***和方法 |
CN109387192A (zh) * | 2017-08-02 | 2019-02-26 | 湖南格纳微信息科技有限公司 | 一种室内外连续定位方法及装置 |
CN107621266B (zh) * | 2017-08-14 | 2020-12-15 | 上海宇航***工程研究所 | 基于特征点跟踪的空间非合作目标相对导航方法 |
CN107621266A (zh) * | 2017-08-14 | 2018-01-23 | 上海宇航***工程研究所 | 基于特征点跟踪的空间非合作目标相对导航方法 |
CN107796391A (zh) * | 2017-10-27 | 2018-03-13 | 哈尔滨工程大学 | 一种捷联惯性导航***/视觉里程计组合导航方法 |
US11421980B2 (en) * | 2017-11-22 | 2022-08-23 | Ford Global Technologies, Llc | Method for determining a position and orientation of an object using a profilometer |
CN112004645A (zh) * | 2017-12-19 | 2020-11-27 | 卡内基梅隆大学 | 智能清洁机器人 |
CN108981693A (zh) * | 2018-03-22 | 2018-12-11 | 东南大学 | 基于单目相机的vio快速联合初始化方法 |
CN110542415A (zh) * | 2018-05-28 | 2019-12-06 | 北京京东尚科信息技术有限公司 | 用于导航***的导航方法和装置 |
CN109186592A (zh) * | 2018-08-31 | 2019-01-11 | 腾讯科技(深圳)有限公司 | 用于视觉惯导信息融合的方法和装置以及存储介质 |
CN109360240B (zh) * | 2018-09-18 | 2022-04-22 | 华南理工大学 | 一种基于双目视觉的小型无人机定位方法 |
CN109360240A (zh) * | 2018-09-18 | 2019-02-19 | 华南理工大学 | 一种基于双目视觉的小型无人机定位方法 |
US12038756B2 (en) | 2018-12-19 | 2024-07-16 | Carnegie Mellon University | Intelligent cleaning robot |
CN109443355A (zh) * | 2018-12-25 | 2019-03-08 | 中北大学 | 基于自适应高斯pf的视觉-惯性紧耦合组合导航方法 |
CN109443355B (zh) * | 2018-12-25 | 2020-10-27 | 中北大学 | 基于自适应高斯pf的视觉-惯性紧耦合组合导航方法 |
CN109443354A (zh) * | 2018-12-25 | 2019-03-08 | 中北大学 | 基于萤火虫群优化pf的视觉-惯性紧耦合组合导航方法 |
CN111307176A (zh) * | 2020-03-02 | 2020-06-19 | 北京航空航天大学青岛研究院 | 一种vr头戴显示设备中视觉惯性里程计的在线标定方法 |
CN111307176B (zh) * | 2020-03-02 | 2023-06-16 | 北京航空航天大学青岛研究院 | 一种vr头戴显示设备中视觉惯性里程计的在线标定方法 |
CN111947653A (zh) * | 2020-08-13 | 2020-11-17 | 北京航空航天大学 | 一种月面巡视探测器双模式惯性/视觉/天文导航方法 |
CN111947652A (zh) * | 2020-08-13 | 2020-11-17 | 北京航空航天大学 | 一种适用于月球着陆器的惯性/视觉/天文/激光测距组合导航方法 |
CN113033462A (zh) * | 2021-04-09 | 2021-06-25 | 山东大学 | 基于火星尘卷风产率的火星着陆点确定方法及*** |
CN113985904A (zh) * | 2021-09-28 | 2022-01-28 | 哈尔滨工业大学 | 星球探测车运动模式生成及智能切换方法 |
CN113985904B (zh) * | 2021-09-28 | 2023-03-10 | 哈尔滨工业大学 | 星球探测车运动模式生成及智能切换方法 |
CN114659526A (zh) * | 2022-02-11 | 2022-06-24 | 北京空间飞行器总体设计部 | 基于序列图像状态表达的航天器自主导航鲁棒滤波算法 |
CN117532604A (zh) * | 2023-11-08 | 2024-02-09 | 哈尔滨工业大学 | 一种基于立体视觉的目标位姿及高阶运动信息观测方法 |
CN117532604B (zh) * | 2023-11-08 | 2024-05-10 | 哈尔滨工业大学 | 一种基于立体视觉的目标位姿及高阶运动信息观测方法 |
Also Published As
Publication number | Publication date |
---|---|
CN103033189B (zh) | 2015-05-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103033189B (zh) | 一种深空探测巡视器惯性/视觉组合导航方法 | |
CN101344391B (zh) | 基于全功能太阳罗盘的月球车位姿自主确定方法 | |
CN111947652B (zh) | 一种适用于月球着陆器的惯性/视觉/天文/激光测距组合导航方法 | |
CN101598556B (zh) | 一种未知环境下无人机视觉/惯性组合导航方法 | |
CN103323026B (zh) | 星敏感器和有效载荷的姿态基准偏差估计与修正方法 | |
CN106017463A (zh) | 一种基于定位传感装置的飞行器定位方法 | |
CN102175241B (zh) | 一种火星探测器巡航段自主天文导航方法 | |
CN103900611B (zh) | 一种惯导天文高精度复合两位置对准及误差标定方法 | |
CN105953796A (zh) | 智能手机单目和imu融合的稳定运动跟踪方法和装置 | |
CN104748751A (zh) | 姿态矩阵的计算方法及基于姿态矩阵的定位导航方法 | |
CN103076015A (zh) | 一种基于全面最优校正的sins/cns组合导航***及其导航方法 | |
CN102168981A (zh) | 一种深空探测器火星捕获段自主天文导航方法 | |
CN105352509A (zh) | 地理信息时空约束下的无人机运动目标跟踪与定位方法 | |
CN103674021A (zh) | 基于捷联惯导与星敏感器的组合导航***及方法 | |
CN103196445A (zh) | 基于匹配技术的地磁辅助惯性的载体姿态测量方法 | |
CN112880669B (zh) | 一种航天器星光折射和单轴旋转调制惯性组合导航方法 | |
CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
CN103968844B (zh) | 基于低轨平台跟踪测量的大椭圆机动航天器自主导航方法 | |
CN103335654A (zh) | 一种行星动力下降段的自主导航方法 | |
Lo et al. | The direct georeferencing application and performance analysis of uav helicopter in gcp-free area | |
CN101782392A (zh) | 基于观测矩阵的深空探测器自主导航路标选取方法 | |
CN104567881A (zh) | 一种基于太阳、地心及月表重力矢量的巡视器位置姿态确定方法 | |
CN103017773B (zh) | 一种基于天体表面特征和天然卫星路标的环绕段导航方法 | |
CN204255368U (zh) | 一种适用于火星车的sins/cns深组合导航*** | |
Sigel et al. | Star tracker celestial localization system for a lunar rover |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant |