CN106969767A - 一种动平台传感器***偏差的估计方法 - Google Patents
一种动平台传感器***偏差的估计方法 Download PDFInfo
- Publication number
- CN106969767A CN106969767A CN201710202662.0A CN201710202662A CN106969767A CN 106969767 A CN106969767 A CN 106969767A CN 201710202662 A CN201710202662 A CN 201710202662A CN 106969767 A CN106969767 A CN 106969767A
- Authority
- CN
- China
- Prior art keywords
- sin
- gamma
- cos
- alpha
- beta
- 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
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/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
一种动平台传感器***偏差的估计方法,包括以下步骤:第一步、基于合作目标建立估计姿态***偏差的状态空间;第二步、基于非合作目标建立估计姿态***偏差的状态空间;第三步、实现基于合作目标和非合作目标的交替配准。本发明解决了动平台传感器***中,传感器量测无偏差而平台姿态角存在偏差的空间配准问题,考虑到地球曲率的影响,基于公共坐标系为地心地固坐标系的三维场景,分析传感器不存在量测偏差的前提下,平台姿态角偏差的估计问题,根据目标类型的不同,设计了基于合作目标为单个或多个的姿态角配准,基于非合作目标为单个或多个的姿态角配准,以及两类目标共存时的姿态角配准,能够精确估计姿态角偏差,实现目标的精确跟踪。
Description
技术领域
本发明涉及多平台多传感器数据融合领域,具体涉及一种动平台传感器***偏差的估计方法,从而协调好基于合作目标的信息和基于非合作目标的信息。
背景技术
目前已公开的空间配准文献所研究的问题是针对探测传感器(如雷达)的***偏差估计,很少提到平台姿态角的***偏差估计。在超视距作战过程中,由于目标距离很远,很小的姿态偏差都可能导致极大的多源数据探测误差。因此亟需研究平台的姿态配准算法。
在多平台多传感器目标跟踪***中,由于平台自身的姿态角***偏差在坐标转换过程中会严重的污染量测数据,使得后续的关联、滤波、融合等运算异常,甚至产生虚假目标,很难发挥出多传感器的优势[1]。目前,对***偏差估计的算法主要包括:实时质量控制法[2]、最小二乘类估计算法[3]、精确极大似然类法[4],以及基于kalman滤波的方法[5]、不敏滤波[6]、EM算法[7]等,这些算法均适用于平台姿态角偏差是固定值时的***偏差估计。
而在空间配准技术中,根据目标类型的不同,主要分为基于合作目标的空间配准和基于非合作目标的空间配准两类[8]。合作目标是指目标的真实位置是已知的,所以只需要一个传感器就能获取其位置。而非合作是指目标的真实位置是未知的,所以往往需要两个或两个以上的传感器对其进行探测。文献[9]研究了当同时出现合作目标与非合作目标时怎样协调好基于合作目标的空间配准结果与非合作目标信息,但其只适用于仅有一个传感器对合作目标有量测的场景,并且是针对仅传感器有***偏差的情况,而非平台姿态角有偏差。
因此,需要设计一种当合作目标与非合作目标同时存在,仅平台的姿态角存在***偏差场景下的空间配准方法,从而使得配准精度既优于单基于合作目标的空间配准,同时也优于单基于非合作目标的空间配准,进而实现对目标更加精确的跟踪。
以上内容所提及的参考文献如下:
[1]保铮.传感器信号的长时间积累[J].第七届全国传感器学术年会,南京,1999:9-15;
[2]Burke J.The SAGE real quality control fraction and its interfacewith BUIC II/BUIC III[R].[s.l.]:MITRE Corporation,1966;
[3]Sudano J J.A least square algorithm with covariance weighting forcomputing the translational and rotational errors between two radar sites[J].IEEE AES,1993,29(1):383-387;
[4]Zhou Yifeng,Henry L.An exact maximum likelihood registrationalgorithm for data fusion[J].IEEE Trans Signal Processing,1997,45(6):1560-1572;
[5]Kousuge Y,Okada T.Bias Estimation of Two 3-Dimensonal Radars UsingKalman Filter[J].4th Int.Workshop on Advanced Motion Control,1996,1(1):377-382;
[6]W.Li,H.Leung and Y.Zhou.Space-time registration of radar and ESMusing unscented Kalman filter.IEEE Trans.On AES.Vol.40(3),2004,824-836;
[7]Zhenhua Li and Henry Leung.An Expectation Maximization BasedSimultaneous Registration and Fusion Algorithm for Radar Networks.IEEE CCECE/CCGEI,Ottawa,May 2006;
[8]宋文彬.传感器数据空间配准算法研究进展[J].传感器与微***,2012,31(8):5-8;
[9]宋文彬.基于合作目标与非合作目标的一体化空间配准新算法[J].电讯技术,2013,53(11):1422-1427。
发明内容
本发明的目的在于针对上述现有技术中的问题,提供一种动平台传感器***偏差的估计方法,协调好基于合作目标的信息和基于非合作目标的信息,实现对目标的精确跟踪。
为了实现上述目的,本发明采用的技术方案包括以下步骤:
第一步、基于合作目标建立估计姿态***偏差的状态空间;
将传感器载于平台上,使二者的笛卡尔坐标系重合,取ECEF坐标系为配准的坐标系,得到:Xtrue=Xs+RtRl(α-Δα,β-Δβ,γ-Δγ)X1;式中,Xtrue为目标在ECEF坐标系下的真实位置,Xs为平台在ECEF坐标系下的真实坐标,Rt为东北天坐标系到ECEF坐标系的转换矩阵,Rl为机载平台笛卡尔坐标系到东北天坐标系的转换矩阵,X1为机载平台笛卡尔坐标系下平台对目标的量测,β,γ,α分别是平台俯仰角、偏航角、滚动角的量测值,Δβ,Δγ,Δα分别是平台俯仰角、偏航角、滚动角的***偏差;
第二步、基于非合作目标建立估计姿态***偏差的状态空间;
将传感器载于平台上,使二者的笛卡尔坐标系重合,取ECEF坐标系为配准的坐标系,对运动的非合作目标,在不考虑传感器的***偏差及平台的姿态角量测噪声的情况下,两平台A,B在同一时刻,对目标的不含偏差量测相同,得到:
XsA+RtARlA(αA-ΔαA,βA-ΔβA,γA-ΔγA)XpA
=XsB+RtBRlB(αB-ΔαB,βB-ΔβB,γB-ΔγB)XpB
式中,XsA,XsB分别为平台A,B在ECEF坐标系下的坐标,RtA,RtB分别为平台A,B东北天坐标系到ECEF坐标系的转换矩阵,RlA,RlB分别为平台A,B机载平台笛卡尔坐标系到东北天坐标系的转换矩阵,XpA,XpB分别为平台A,B在机载平台笛卡尔坐标系下传感器对目标的量测,(αA,βA,γA)和(αB,βB,γB)分别是平台A,B的滚动角、俯仰角、偏航角的量测值,(ΔαA,ΔβA,ΔγA)和(ΔαB,ΔβB,ΔγB)分别是平台A,B相应姿态角的***偏差;
第三步、实现基于合作目标和非合作目标的交替配准。
所述的第一步中:
1.1)得到东北天坐标系到ECEF坐标系的转换矩阵Rt为:
得到:
其中:
此处(xs,ys,zs)表示机载平台中心在ECEF坐标系中的位置,而(λs,ls,hs)则表示机载平台中心在大地坐标系的位置,分别表示经度,纬度和高度;
1.2)得到机载平台笛卡尔坐标系到东北天坐标系的转换矩阵Rl(α,β,γ)为:
其中,γ,α,β分别为平台的偏航角,滚动角和俯仰角;
1.3)得到机载平台笛卡尔坐标系到东北天坐标系的真实转换矩阵Rtl为:
其中,Δγ,Δα,Δβ分别为平台的偏航角***偏差,滚动角***偏差和俯仰角***偏差。
将步骤1.3)得到的矩阵在Δγ=Δβ=Δα=0处进行一阶泰勒展开,得到:
使得:
其中,Δε=(Δα,Δβ,Δγ)T是平台在机体坐标系的定姿误差,而Γ1是把机体坐标系的定姿误差转换到东北天坐标系的误差转换矩阵;
a.对ΔRl的各元素展开,得到:
b.根据机载平台笛卡尔坐标系到东北天坐标系的真实转换矩阵Rtl及其一阶泰勒展开式,计算得到各元素:
根据东北天坐标系到ECEF坐标系的转换矩阵Rt,得到:
结合上式以及东北天坐标系到ECEF坐标系的转换矩阵Rt,得到:
Xtrue=Xs+RtRl(α,β,γ)X1+ΔX3=Xs+RtRl(α,β,γ)X1+ΓΔε
其中,Γ=Rt·Γ1是平台定姿误差到ECEF坐标系的转换矩阵,是机体坐标系定姿误差转换到东北天坐标系的误差转换矩阵与旋转变换矩阵的复合。
所述的第二步中:
2.1)令i=A,B,则平台i东北天坐标系到ECEF坐标系的转换矩阵Rti为:
其中,(λsi,lsi,hsi)则表示机载平台i(i=A,B)中心在大地坐标系的位置,分别表示经度,纬度和高度;
2.2)机载平台i笛卡尔坐标系到东北天坐标系的转换矩阵Rli为:
其中,γi,αi,βi分别为平台i的偏航角,滚动角和俯仰角。
将步骤2.2)的等式在ΔαA=ΔβA=ΔγA=ΔαB=ΔβB=ΔγB=0处进行一阶泰勒展开,
得到:
其中,分别为:
整理得到:
简化后得到:
Γfhζ=Zfh;
其中,Γfh=[ΓA -ΓB]为线性化后的量测矩阵,具体为:
ζ=[ΔαA ΔβA ΔγA ΔαB ΔβB ΔγB]T为平台三个姿态角的***偏差,即状态向量,Zfh=(XsA+RtARlA(αA,βA,γA)XpA)-(XsB+RtBRlB(αB,βB,γB)XpB)为等效量测。
所述第三步实现交替配准的步骤为:
步骤3.1、若两平台A,B对同一个合作目标均有量测,则根据第一步关系式得到:
Xtrue=XsA+RtARlA(αA,βA,γA)X1A+ΓAΔεA=XsB+RtBRlB(αB,βB,γB)X1B+ΓBΔεB;
步骤3.2、将步骤3.1简化为:
其中,
ZA=Xtrue-XsA-RtARlA(αA,βA,γA)X1A,
ZB=Xtrue-XsB-RtBRlB(αB,βB,γB)X1B,
ζ=[ΔαA ΔβA ΔγA ΔαB ΔβB ΔγB]T;
步骤3.3、若两平台A,B中只有平台A对合作目标有量测,则根据第一步关系式得到:
Xtrue=XsA+RtARlA(αA,βA,γA)X1A+ΓAΔεA;
步骤3.4、将步骤3.3简化,使之能与基于非合作目标实现交替配准:
其中,
ZA=Xtrue-XsA-RtARlA(αA,βA,γA)X1A,
ZB=[0 0 0]T,
ζ=[ΔαA ΔβA ΔγA ΔαB ΔβB ΔγB]T;
步骤3.5、进行基于合作目标的k时刻卡尔曼滤波,基于合作目标的状态空间为:
步骤3.5.1、对状态进行一步预测和对状态的误差协方差阵进行一步预测:
步骤3.5.2、对状态的增益矩阵的计算:
步骤3.5.3、对状态进行一步更新:
步骤3.5.4、对状态误差协方差阵进行一步更新:
其中,cov(υh,k)=Rh,k+1。
步骤3.6、进行基于非合作目标的k时刻卡尔曼滤波时,等于Pkk等于Pk+1|k+1,则基于非合作目标的状态空间为:
步骤3.6.1、对状态进行一步预测和对状态的误差协方差阵进行一步预测;
步骤3.6.2、对状态的增益矩阵的计算:
步骤3.6.3、对状态进行一步更新:
步骤3.6.4、对状态误差协方差阵进行一步更新:
其中,cov(υfh,k)=Rfh,k+1;
步骤3.7、k=k+1,然后返回步骤3.1,但是此时的等于步骤3.6.2中的Pk|k等于步骤3.6.3中的Pk+1|k+1,如此实现所有时刻基于合作目标和基于非合作目标的交替配准。
若是多个合作目标,则先按照步骤3.5对所有的合作目标进行k时刻的参数估计;若是多个非合作目标,则先按照步骤3.6对所有的非合作目标进行k时刻的参数估计。
与现有技术相比,本发明具有如下的有益效果:解决了动平台传感器***中,传感器量测无偏差而平台姿态角存在偏差的空间配准问题,考虑到地球曲率的影响,本发明基于公共坐标系为地心地固坐标系的三维场景,分析传感器不存在量测偏差的前提下,平台姿态角偏差的估计问题,根据目标类型的不同,设计了基于合作目标为单个或多个的姿态角配准,基于非合作目标为单个或多个的姿态角配准,以及两类目标共存时的姿态角配准,从而得到同时用两类目标的量测信息进行配准,并且能够精确估计姿态角偏差,实现目标的精确跟踪。本发明首先建立以地心地固坐标系为公共坐标系,用于合作目标空间配准的状态空间,并针对一个平台对该合作目标有量测还是两个平台对该合作目标均有量测,然后建立以地心地固坐标系为公共坐标系,用于非合作目标空间配准的状态空间,最后建立基于合作目标配准和基于非合作配准的交替姿态角偏差估计,最终得到的估计精度优于单基于合作目标量测信息进行空间配准的方法以及单基于非合作目标量测信息进行空间配准的方法。
附图说明
图1合作目标、非合作目标及两平台A,B的运动轨迹;
图2平台A姿态角偏差RMSE图:(a)、(b)分别为不同程度放大示意图;
图3平台B姿态角偏差RMSE图:(a)、(b)分别为不同程度放大示意图;
图4本发明方法的整体操作框图;
图5本发明方法的具体流程图。
具体实施方式
下面结合附图对本发明做进一步的详细说明。
本发明动平台传感器***偏差的估计方法包括以下步骤:
步骤一:基于合作目标建立估计姿态***偏差的状态空间;
步骤二:基于非合作目标建立估计姿态***偏差的状态空间;
步骤三:实现基于合作目标和非合作目标的交替配准。
步骤一中基于合作目标建立估计姿态***偏差的状态空间,具体包括以下步骤:
步骤1.1:传感器载于平台之上,且二者的笛卡尔坐标系重合,考虑地球曲率影响,取地心地固坐标系(ECEF坐标系)作为配准的坐标系。在不考虑传感器量测***偏差以及姿态角量测噪声的情况下,平台对目标不含偏差的量测位置与合作目标汇报的真实位置在ECEF坐标系下相同,于是可列出等式
Xtrue=Xs+RtRl(α-Δα,β-Δβ,γ-Δγ)X1
其中,Xtrue为目标在ECEF坐标系下的真实位置,Xs为平台在ECEF坐标系下的真实坐标,Rt为东北天坐标系到ECEF坐标系的转换矩阵,Rl为机载平台笛卡尔坐标系到东北天坐标系的转换矩阵,X1为机载平台(传感器)笛卡尔坐标系下平台对目标的量测,β,γ,α分别是平台俯仰角、偏航角、滚动角的量测值,Δβ,Δγ,Δα分别是平台俯仰角、偏航角、滚动角的***偏差。
步骤1.1.1:东北天坐标系到ECEF坐标系的转换矩阵Rt为:
此时步骤1.1可表示为:
其中:
此处(xs,ys,zs)表示机载平台中心在ECEF坐标系中的位置,而(λs,ls,hs)则表示机载平台中心在大地坐标系的位置,分别表示经度,纬度和高度。
步骤1.1.2:机载平台(传感器)笛卡尔坐标系到东北天坐标系的转换矩阵Rl(α,β,γ)为
其中,γ,α,β分别为平台的偏航角,滚动角和俯仰角。
步骤1.1.3:参考步骤1.1.2,机载平台(传感器)笛卡尔坐标系到东北天坐标系的真实转换矩阵Rtl为
其中,Δγ,Δα,Δβ分别为平台的偏航角***偏差,滚动角***偏差和俯仰角***偏差。
步骤1.2:将步骤1.1.3在Δγ=Δβ=Δα=0处进行一阶泰勒展开,得到
使得
其中,Δε=(Δα,Δβ,Δγ)T是平台在机体坐标系的定姿误差,而Γ1是把机体坐标系的定姿误差转换到东北天坐标系的误差转换矩阵。
步骤1.2.1:根据步骤1.2,对ΔRl的各元素展开,得到
步骤1.2.2:根据步骤1.1.3和步骤1.2,计算得到各元素,得到
步骤1.3:根据步骤1.1.1,得
步骤1.4:根据步骤1.1.1和步骤1.3,得
Xtrue=Xs+RtRl(α,β,γ)X1+ΔX3=Xs+RtRl(α,β,γ)X1+ΓΔε
其中Γ=Rt·Γ1是平台定姿误差到ECEF坐标系的转换矩阵,是机体坐标系定姿误差转换到东北天坐标系的误差转换矩阵与旋转变换矩阵的复合。
步骤二中基于非合作目标建立估计姿态***偏差的状态空间,具体步骤如下:
步骤2.1:传感器载于平台之上,且二者的笛卡尔坐标系重合,考虑地球曲率影响,取地心地固坐标系(ECEF坐标系)作为配准的坐标系。在目标是运动的非合作目标的前提下对两个平台进行配准。配准的主要思路是,在不考虑传感器的***偏差及平台的姿态角量测噪声的情况下,两平台A,B在同一时刻,对目标的不含偏差量测相同,则可列出列出等式
其中,XsA,XsB分别为平台A,B在ECEF坐标系下的坐标,RtA,RtB分别为平台A,B东北天坐标系到ECEF坐标系的转换矩阵,RlA,RlB分别为平台A,B机载平台(传感器)笛卡尔坐标系到东北天坐标系的转换矩阵,XpA,XpB分别为平台A,B在机载平台(传感器)笛卡尔坐标系下传感器对目标的量测,(αA,βA,γA)和(αB,βB,γB)分别是平台A,B的滚动角、俯仰角、偏航角的量测值,(ΔαA,ΔβA,ΔγA)和(ΔαB,ΔβB,ΔγB)分别是平台A,B相应姿态角的***偏差。
步骤2.1.1:平台i(i=A,B)东北天坐标系到ECEF坐标系的转换矩阵Rti为
其中,(λsi,lsi,hsi)则表示机载平台i(i=A,B)中心在大地坐标系的位置,分别表示经度,纬度和高度。
步骤2.1.2:机载平台(传感器)i(i=A,B)笛卡尔坐标系到东北天坐标系的转换矩阵Rli为
其中,γi,αi,βi分别为平台i的偏航角,滚动角和俯仰角。
步骤2.2:由于平台的姿态偏差相对较小,参考步骤2.1.1和步骤2.1.2,将步骤2.1等式在ΔαA=ΔβA=ΔγA=ΔαB=ΔβB=ΔγB=0处进行一阶泰勒展开,得
其中,分别为
步骤2.3:将步骤2.2进行整理,得
步骤2.4:将步骤2.3进行简化,得
Γfhζ=Zfh
其中,Γfh=[ΓA -ΓB]为线性化后的量测矩阵,具体为
ζ=[ΔαA ΔβA ΔγA ΔαB ΔβB ΔγB]T为平台三个姿态角的***偏差,即状态向量,
Zfh=(XsA+RtARlA(αA,βA,γA)XpA)-(XsB+RtBRlB(αB,βB,γB)XpB)为等效量测。
步骤三中,对基于合作目标的量测信息和基于非合作目标的量测信息用卡尔曼滤波交替估计平台的姿态角偏差,具体步骤如下:
步骤3.1:若两平台(传感器)A,B对同一个合作目标均有量测,则根据步骤一的建模过程,可分别得到
Xtrue=XsA+RtARlA(αA,βA,γA)X1A+ΓAΔεA=XsB+RtBRlB(αB,βB,γB)X1B+ΓBΔεB
步骤3.2:步骤3.1可简化为
其中,
ZA=Xtrue-XsA-RtARlA(αA,βA,γA)X1A
ZB=Xtrue-XsB-RtBRlB(αB,βB,γB)X1B
ζ=[ΔαA ΔβA ΔγA ΔαB ΔβB ΔγB]T
步骤3.3:若两平台(传感器)A,B中只有平台(传感器)A对合作目标有量测,则根据步骤一的建模过程,可得到
Xtrue=XsA+RtARlA(αA,βA,γA)X1A+ΓAΔεA
步骤3.4:但是为了能与基于非合作目标实现交替配准,步骤3.3需简化为
其中,
ZA=Xtrue-XsA-RtARlA(αA,βA,γA)X1A
ZB=[0 0 0]T
ζ=[ΔαA ΔβA ΔγA ΔαB ΔβB ΔγB]T
步骤3.5:步骤一为了简便起见,忽略了平台传感器的量测噪声,下面在进行卡尔曼滤波的时候将噪声考虑在内。依据步骤3.1至3.4,进行基于合作目标的k时刻的卡尔曼滤波,基于合作目标的状态空间为
步骤3.5.1:对状态进行一步预测和对状态的误差协方差阵进行一步预测
步骤3.5.2:对状态的增益矩阵的计算
步骤3.5.3:对状态进行一步更新
步骤3.5.4:对状态误差协方差阵进行一步更新
其中,cov(υh,k)=Rh,k+1。
步骤3.6:步骤二为了简便起见,忽略了平台传感器的量测噪声,下面在进行卡尔曼滤波的时候将平台姿态角量测噪声考虑在内。依据步骤二,进行基于非合作目标的k时刻的卡尔曼滤波,但是此时的等于步骤3.5.2中的Pk|k等于步骤3.5.3中的Pk+1|k+1,基于非合作目标的状态空间为
步骤3.6.1:对状态进行一步预测和对状态的误差协方差阵进行一步预测
步骤3.6.2:对状态的增益矩阵的计算
步骤3.6.3:对状态进行一步更新
步骤3.6.4:对状态误差协方差阵进行一步更新
其中,cov(υfh,k)=Rfh,k+1
步骤3.7:k=k+1,然后返回步骤3.1,但是此时的等于步骤3.6.2中的Pk|k等于步骤3.6.3中的Pk+1|k+1。如此实现所有时刻的基于合作目标和基于非合作目标的交替配准。当然,若是多个合作目标,则先按照步骤3.5对所有的合作目标进行k时刻的参数估计,同样的,若是多个非合作目标,则先按照步骤3.6对所有的非合作目标进行k时刻的参数估计,具体参见图5。
参见图1-3,本发明利用PC平台仿真来实现对算法的验证。仿真软件为Windows 7旗舰版,MATLAB R2014a版本。仿真场景设置为:两平台对一个合作目标和一个非合作目标进行观测,其中两平台对该合作目标均有量测,平台A和B在大地坐标系下的初始位置(纬度,经度,高度)分别为(33.9°,108.2°,6000m)和(33.9°,107.9°,6000m),在纬度、经度、高度的速度向量均为(-0.12km/s,-0.12km/s,0),平台A和B的姿态角真值分别为(25°,10°,20°)和(30°,15°,25°),姿态角***偏差分别为(0.5°,-0.95°,0.55°)和(0.45°,-0.7°,0.4°),所有的角度量测噪声标准差均为0.1°,合作目标和非合作目标在在大地坐标系下的初始位置(纬度,经度,高度)分别为(33°,107°,6000m)和(33.4°,107.2°,6000m),在纬度、经度、高度的速度向量分别为(0.1km/s,0.1km/s,0)和(0.1km/s,0,0),采样周期为1s,整个仿真时间为1000s,蒙特卡洛仿真次数为100次。图1为两个目标和两个平台在大地坐标系下的运动轨迹,从图2(a)和图2(b)可以看出利用平台A对合作目标和非合作目标的量测信息可以得到最精确的姿态角偏差估计,从图3(a)和图3(b)可以看出利用平台B对合作目标和非合作目标的量测信息可以得到最精确的姿态角偏差估计,精确的姿态角偏差估计可以实现对目标更准确的跟踪。
Claims (8)
1.一种动平台传感器***偏差的估计方法,其特征在于,包括以下步骤:
第一步、基于合作目标建立估计姿态***偏差的状态空间;
将传感器载于平台上,使二者的笛卡尔坐标系重合,取ECEF坐标系为配准的坐标系,得到:Xtrue=Xs+RtRl(α-Δα,β-Δβ,γ-Δγ)X1;式中,Xtrue为目标在ECEF坐标系下的真实位置,Xs为平台在ECEF坐标系下的真实坐标,Rt为东北天坐标系到ECEF坐标系的转换矩阵,Rl为机载平台笛卡尔坐标系到东北天坐标系的转换矩阵,X1为机载平台笛卡尔坐标系下平台对目标的量测,β,γ,α分别是平台俯仰角、偏航角、滚动角的量测值,Δβ,Δγ,Δα分别是平台俯仰角、偏航角、滚动角的***偏差;
第二步、基于非合作目标建立估计姿态***偏差的状态空间;
将传感器载于平台上,使二者的笛卡尔坐标系重合,取ECEF坐标系为配准的坐标系,对运动的非合作目标,在不考虑传感器的***偏差及平台的姿态角量测噪声的情况下,两平台A,B在同一时刻,对目标的不含偏差量测相同,得到:
XsA+RtARlA(αA-ΔαA,βA-ΔβA,γA-ΔγA)XpA
=XsB+RtBRlB(αB-ΔαB,βB-ΔβB,γB-ΔγB)XpB
式中,XsA,XsB分别为平台A,B在ECEF坐标系下的坐标,RtA,RtB分别为平台A,B东北天坐标系到ECEF坐标系的转换矩阵,RlA,RlB分别为平台A,B机载平台笛卡尔坐标系到东北天坐标系的转换矩阵,XpA,XpB分别为平台A,B在机载平台笛卡尔坐标系下传感器对目标的量测,(αA,βA,γA)和(αB,βB,γB)分别是平台A,B的滚动角、俯仰角、偏航角的量测值,(ΔαA,ΔβA,ΔγA)和(ΔαB,ΔβB,ΔγB)分别是平台A,B相应姿态角的***偏差;
第三步、实现基于合作目标和非合作目标的交替配准。
2.根据权利要求1所述动平台传感器***偏差的估计方法,其特征在于,所述的第一步:
1.1)得到东北天坐标系到ECEF坐标系的转换矩阵Rt为:
得到:
其中:
此处(xs,ys,zs)表示机载平台中心在ECEF坐标系中的位置,而(λs,ls,hs)则表示机载平台中心在大地坐标系的位置,分别表示经度,纬度和高度;
1.2)得到机载平台笛卡尔坐标系到东北天坐标系的转换矩阵Rl(α,β,γ)为:
其中,γ,α,β分别为平台的偏航角,滚动角和俯仰角;
1.3)得到机载平台笛卡尔坐标系到东北天坐标系的真实转换矩阵Rtl为:
其中,Δγ,Δα,Δβ分别为平台的偏航角***偏差,滚动角***偏差和俯仰角***偏差。
3.根据权利要求2所述动平台传感器***偏差的估计方法,其特征在于,将步骤1.3)得到的矩阵在Δγ=Δβ=Δα=0处进行一阶泰勒展开,得到:
使得:
其中,Δε=(Δα,Δβ,Δγ)T是平台在机体坐标系的定姿误差,而Γ1是把机体坐标系的定姿误差转换到东北天坐标系的误差转换矩阵;
a.对ΔRl的各元素展开,得到:
b.根据机载平台笛卡尔坐标系到东北天坐标系的真实转换矩阵Rtl及其一阶泰勒展开式,计算得到各元素:
4.根据权利要求3所述动平台传感器***偏差的估计方法,其特征在于,根据东北天坐标系到ECEF坐标系的转换矩阵Rt,得到:
结合上式以及东北天坐标系到ECEF坐标系的转换矩阵Rt,得到:
Xtrue=Xs+RtRl(α,β,γ)X1+ΔX3=Xs+RtRl(α,β,γ)X1+ΓΔε
其中,Γ=Rt·Γ1是平台定姿误差到ECEF坐标系的转换矩阵,是机体坐标系定姿误差转换到东北天坐标系的误差转换矩阵与旋转变换矩阵的复合。
5.根据权利要求1所述动平台传感器***偏差的估计方法,其特征在于,所述的第二步:
2.1)令i=A,B,则平台i东北天坐标系到ECEF坐标系的转换矩阵Rti为:
其中,(λsi,lsi,hsi)则表示机载平台i(i=A,B)中心在大地坐标系的位置,分别表示经度,纬度和高度;
2.2)机载平台i笛卡尔坐标系到东北天坐标系的转换矩阵Rli为:
其中,γi,αi,βi分别为平台i的偏航角,滚动角和俯仰角。
6.根据权利要求5所述动平台传感器***偏差的估计方法,其特征在于,将步骤2.2)的等式在ΔαA=ΔβA=ΔγA=ΔαB=ΔβB=ΔγB=0处进行一阶泰勒展开,得到:
其中,分别为:
整理得到:
简化后得到:
Γfhζ=Zfh;
其中,Γfh=[ΓA -ΓB]为线性化后的量测矩阵,具体为:
ζ=[ΔαA ΔβA ΔγA ΔαB ΔβB ΔγB]T为平台三个姿态角的***偏差,即状态向量,Zfh=(XsA+RtARlA(αA,βA,γA)XpA)-(XsB+RtBRlB(αB,βB,γB)XpB)为等效量测。
7.根据权利要求1所述动平台传感器***偏差的估计方法,其特征在于,所述第三步实现交替配准的步骤为:
步骤3.1、若两平台A,B对同一个合作目标均有量测,则根据第一步关系式得到:
Xtrue=XsA+RtARlA(αA,βA,γA)X1A+ΓAΔεA=XsB+RtBRlB(αB,βB,γB)X1B+ΓBΔεB;
步骤3.2、将步骤3.1简化为:
其中,
ZA=Xtrue-XsA-RtARlA(αA,βA,γA)X1A,
ZB=Xtrue-XsB-RtBRlB(αB,βB,γB)X1B,
ζ=[ΔαA ΔβA ΔγA ΔαB ΔβB ΔγB]T;
步骤3.3、若两平台A,B中只有平台A对合作目标有量测,则根据第一步关系式得到:
Xtrue=XsA+RtARlA(αA,βA,γA)X1A+ΓAΔεA;
步骤3.4、将步骤3.3简化,使之能与基于非合作目标实现交替配准:
其中,
ZA=Xtrue-XsA-RtARlA(αA,βA,γA)X1A,
ZB=[0 0 0]T,
ζ=[ΔαA ΔβA ΔγA ΔαB ΔβB ΔγB]T;
步骤3.5、进行基于合作目标的k时刻卡尔曼滤波,基于合作目标的状态空间为:
步骤3.5.1、对状态进行一步预测和对状态的误差协方差阵进行一步预测:
Pk+1|k=Pk|k
步骤3.5.2、对状态的增益矩阵的计算:
步骤3.5.3、对状态进行一步更新:
步骤3.5.4、对状态误差协方差阵进行一步更新:
其中,cov(υh,k)=Rh,k+1;
步骤3.6、进行基于非合作目标的k时刻卡尔曼滤波时,等于Pk|k等于Pk+1|k+1,则基于非合作目标的状态空间为:
步骤3.6.1、对状态进行一步预测和对状态的误差协方差阵进行一步预测;
Pk+1|k=Pk|k
步骤3.6.2、对状态的增益矩阵的计算:
步骤3.6.3、对状态进行一步更新:
步骤3.6.4、对状态误差协方差阵进行一步更新:
其中,cov(υfh,k)=Rfh,k+1;
步骤3.7、k=k+1,然后返回步骤3.1,但是此时的等于步骤3.6.2中的Pk|k等于步骤3.6.3中的Pk+1|k+1,如此实现所有时刻基于合作目标和基于非合作目标的交替配准。
8.根据权利要求7所述动平台传感器***偏差的估计方法,其特征在于,若是多个合作目标,则先按照步骤3.5对所有的合作目标进行k时刻的参数估计;若是多个非合作目标,则先按照步骤3.6对所有的非合作目标进行k时刻的参数估计。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710202662.0A CN106969767B (zh) | 2017-03-30 | 2017-03-30 | 一种动平台传感器***偏差的估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710202662.0A CN106969767B (zh) | 2017-03-30 | 2017-03-30 | 一种动平台传感器***偏差的估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106969767A true CN106969767A (zh) | 2017-07-21 |
CN106969767B CN106969767B (zh) | 2020-03-17 |
Family
ID=59336314
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710202662.0A Active CN106969767B (zh) | 2017-03-30 | 2017-03-30 | 一种动平台传感器***偏差的估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106969767B (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108594193A (zh) * | 2018-04-24 | 2018-09-28 | 西安交通大学 | 一种基于固定目标与非合作目标的雷达***偏差估计方法 |
CN108876862A (zh) * | 2018-07-13 | 2018-11-23 | 北京控制工程研究所 | 一种非合作目标点云位置姿态计算方法 |
CN113310505A (zh) * | 2021-06-15 | 2021-08-27 | 苏州挚途科技有限公司 | 传感器***的外参标定方法、装置及电子设备 |
CN113484832A (zh) * | 2021-07-29 | 2021-10-08 | 西安电子科技大学 | 一种地基雷达组网的***误差配准方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP1307705B1 (en) * | 2000-08-07 | 2009-05-13 | BAE SYSTEMS plc | Height measurement apparatus |
DE202010012985U1 (de) * | 2010-11-25 | 2012-02-27 | Sick Ag | Sensoranordnung zur Objekterkennung |
CN103344946A (zh) * | 2013-07-08 | 2013-10-09 | 杭州电子科技大学 | 一种地基雷达与空中移动平台雷达的实时误差配准方法 |
CN103376447A (zh) * | 2012-04-24 | 2013-10-30 | 中国人民解放军海军航空工程学院 | 一种实现非合作双多基地雷达目标三维定位的方法 |
CN103616036A (zh) * | 2013-11-29 | 2014-03-05 | 中国航空无线电电子研究所 | 一种基于合作目标的机载传感器***误差估计与补偿方法 |
CN104406598A (zh) * | 2014-12-11 | 2015-03-11 | 南京航空航天大学 | 一种基于虚拟滑模控制的非合作航天器姿态估计方法 |
-
2017
- 2017-03-30 CN CN201710202662.0A patent/CN106969767B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP1307705B1 (en) * | 2000-08-07 | 2009-05-13 | BAE SYSTEMS plc | Height measurement apparatus |
DE202010012985U1 (de) * | 2010-11-25 | 2012-02-27 | Sick Ag | Sensoranordnung zur Objekterkennung |
CN103376447A (zh) * | 2012-04-24 | 2013-10-30 | 中国人民解放军海军航空工程学院 | 一种实现非合作双多基地雷达目标三维定位的方法 |
CN103344946A (zh) * | 2013-07-08 | 2013-10-09 | 杭州电子科技大学 | 一种地基雷达与空中移动平台雷达的实时误差配准方法 |
CN103616036A (zh) * | 2013-11-29 | 2014-03-05 | 中国航空无线电电子研究所 | 一种基于合作目标的机载传感器***误差估计与补偿方法 |
CN104406598A (zh) * | 2014-12-11 | 2015-03-11 | 南京航空航天大学 | 一种基于虚拟滑模控制的非合作航天器姿态估计方法 |
Non-Patent Citations (2)
Title |
---|
GUO,JUNJUN,ETC: "Bias change detection-based sensor selection approach for target tracking in large-scale distributed sensor networks", 《IET RADAR, SONAR & NAVIGATION》 * |
熊伟,等: "基于合作目标的动平台传感器偏差估计方法", 《***工程与电子技术》 * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108594193A (zh) * | 2018-04-24 | 2018-09-28 | 西安交通大学 | 一种基于固定目标与非合作目标的雷达***偏差估计方法 |
CN108876862A (zh) * | 2018-07-13 | 2018-11-23 | 北京控制工程研究所 | 一种非合作目标点云位置姿态计算方法 |
CN108876862B (zh) * | 2018-07-13 | 2021-12-07 | 北京控制工程研究所 | 一种非合作目标点云位置姿态计算方法 |
CN113310505A (zh) * | 2021-06-15 | 2021-08-27 | 苏州挚途科技有限公司 | 传感器***的外参标定方法、装置及电子设备 |
CN113310505B (zh) * | 2021-06-15 | 2024-04-09 | 苏州挚途科技有限公司 | 传感器***的外参标定方法、装置及电子设备 |
CN113484832A (zh) * | 2021-07-29 | 2021-10-08 | 西安电子科技大学 | 一种地基雷达组网的***误差配准方法 |
Also Published As
Publication number | Publication date |
---|---|
CN106969767B (zh) | 2020-03-17 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
WO2022206978A1 (zh) | 一种基于车载定位装置的路侧毫米波雷达校准方法 | |
CN113074739B (zh) | 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法 | |
CN106969767A (zh) | 一种动平台传感器***偏差的估计方法 | |
Wei et al. | Camera/laser/GPS fusion method for vehicle positioning under extended NIS-based sensor validation | |
CN103323855B (zh) | 一种基线动态测量***的精度获取方法 | |
EP3617749B1 (en) | Method and arrangement for sourcing of location information, generating and updating maps representing the location | |
CN107356947A (zh) | 基于单频导航卫星数据确定卫星差分伪距偏差的方法 | |
CN103616036B (zh) | 一种基于合作目标的机载传感器***误差估计与补偿方法 | |
CN106052691B (zh) | 激光测距移动制图中闭合环误差纠正方法 | |
CN101339244B (zh) | 一种机载sar图像自动目标定位方法 | |
CN107229037A (zh) | 移动平台传感器量测数据扩维空间配准方法 | |
EP3193187A1 (en) | Method for calibrating a local positioning system based on time-difference-of-arrival measurements | |
Gernhardt et al. | Persistent scatterers at building facades–evaluation of appearance and localization accuracy | |
KR20180000522A (ko) | 이동체의 위치 및 자세 결정 장치 및 방법 | |
Chang et al. | Railway infrastructure monitoring using satellite radar data | |
CN101126806A (zh) | 基于信息融合的修正极大似然配准方法 | |
CN109856616B (zh) | 一种雷达定位相对***误差修正方法 | |
CN112163052B (zh) | 一种信息缺失量测下的多传感器空间配准方法 | |
CN108919304A (zh) | 一种基于参考平面的移动测量***中pos误差补偿方法 | |
Chiang et al. | High-Definition-Map-Based LiDAR Localization Through Dynamic Time-Synchronized Normal Distribution Transform Scan Matching | |
CN110456360B (zh) | 一种针对非连续目标被动多阵列声呐检测跟踪方法 | |
Kuang et al. | Robust constrained Kalman filter algorithm considering time registration for GNSS/acoustic joint positioning | |
CN115880328A (zh) | 一种双星协同观测的空中运动目标实时定位方法 | |
Mounier et al. | High-precision positioning in GNSS-challenged environments: a LiDAR-based multi-sensor fusion approach with 3D digital maps registration | |
CN102645646B (zh) | 一种多信源不确定融合定位方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
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 |