CN115371650B - 一种六自由度激光标靶测量***及其动态性能提升方法 - Google Patents
一种六自由度激光标靶测量***及其动态性能提升方法 Download PDFInfo
- Publication number
- CN115371650B CN115371650B CN202211010946.7A CN202211010946A CN115371650B CN 115371650 B CN115371650 B CN 115371650B CN 202211010946 A CN202211010946 A CN 202211010946A CN 115371650 B CN115371650 B CN 115371650B
- Authority
- CN
- China
- Prior art keywords
- state
- vector
- unit
- inertial navigation
- matrix
- 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
- 238000000034 method Methods 0.000 title claims abstract description 33
- 238000005259 measurement Methods 0.000 title claims description 76
- 238000001914 filtration Methods 0.000 claims abstract description 43
- 238000001514 detection method Methods 0.000 claims abstract description 41
- 238000005562 fading Methods 0.000 claims abstract description 39
- 238000003384 imaging method Methods 0.000 claims abstract description 24
- 230000000007 visual effect Effects 0.000 claims abstract description 19
- 239000013598 vector Substances 0.000 claims description 60
- 239000011159 matrix material Substances 0.000 claims description 58
- 230000003044 adaptive effect Effects 0.000 claims description 16
- 238000012545 processing Methods 0.000 claims description 12
- 238000001228 spectrum Methods 0.000 claims description 7
- 230000003068 static effect Effects 0.000 claims description 7
- 238000007476 Maximum Likelihood Methods 0.000 claims description 4
- 238000009826 distribution Methods 0.000 claims description 4
- 238000005096 rolling process Methods 0.000 claims description 4
- 230000002159 abnormal effect Effects 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims description 3
- 238000012423 maintenance Methods 0.000 claims description 3
- 238000012360 testing method Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 2
- 230000002708 enhancing effect Effects 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 4
- 238000004519 manufacturing process Methods 0.000 description 4
- 238000011160 research Methods 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 230000004927 fusion Effects 0.000 description 3
- 238000002955 isolation Methods 0.000 description 3
- 239000002689 soil Substances 0.000 description 3
- 241001497337 Euscorpius gamma Species 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 239000000835 fiber Substances 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000001788 irregular Effects 0.000 description 2
- 238000000691 measurement method Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000007500 overflow downdraw method Methods 0.000 description 2
- 230000005641 tunneling Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 239000011435 rock Substances 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 238000003860 storage Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C15/00—Surveying instruments or accessories not provided for in groups G01C1/00 - G01C13/00
- G01C15/002—Active optical surveying means
-
- 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)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明公开一种六自由度激光标靶测量***及其动态性能提升方法，所述***包括激光跟踪仪和六自由度姿态激光标靶，所述标靶中包括倾角传感单元和捷联惯导测量单元。所述动态性能提升方法将将捷联惯导测量单元的量测信息比对所述倾角传感单元和视觉成像单元的量测信息获得捷联惯导测量单元的误差，引入故障检测算法实时判定倾角传感单元工作状态，并在倾角传感单元正常和失准时使用多重次优渐消自适应容错卡尔曼滤波算法进行自适应容错滤波对捷联惯导测量单元误差进行预测与补偿。
Description
技术领域
本发明涉及分布式测量***，具体涉及一种六自由度激光标靶测量***及其动态性能提升方法。
背景技术
激光标靶六自由度测量***以其测量精度稳定，量程广，灵活装卸的特点在我国工业测量与生产诸如船舶制造、盾构导向、轨道检测、大型仪器装调等工程领域中有着广泛应用。其主要工作原理是测量基站发射测量光经激光标靶针孔棱镜部分反射并由测量基站接收获取其位置信息，并通过倾角仪测角并结合视觉映射模型解算待测目标姿态值。但由于激光标靶内部的倾角传感器件通过测量重力加速度在其相应惯性轴的比力从而获取其惯性轴与水平面的倾角，因此该方法主要适用于无非引力加速度场景的静态或准静态环境的测量，然而随着我国工业发展，对测量及装配环节的动态性能及工程效率需求日益提升，典型的在我国隧道工程中，激光标靶用以实时获取盾构机(TBM)的位姿信息，然而在TBM进行盾构掘进时，由于地质中硬岩、软土、空洞分布不均，将给倾角传感单元引入各种不规则的干扰加速度从而影响盾构导向中的姿态测量精度，降低工程制造的效率及质量，因此提升六自由度激光标靶***在动态应用场景下的精度与鲁棒性是亟待解决的问题。
目前，对于激光标靶的动态提升算法主要包括：隔振补偿法与传感器融合法。其中，隔振补偿法([1]Li H X,Pan M H,Wang L,et al.Research of vibration errorcompensation for inclinometers[C],Advanced Materials Research,2012,590:377-384.)主要通过对其不同振动环境下的输出特性进行分析，发现其共振规律，从而进行隔振并通过数字滤波技术平稳输出信号以减小振动误差，然而，在盾构场景下由于土质不均引入的振动具有不规则性，并且土质结构还会随着不同地域发生改变，因此此类方法应用范围有限，并且在盾构等不规则振动的应用场景效果较差。传感器融合法通过引入其他高精度抗振单元从而补偿倾角仪的动态性能，文献([2]肖兴维,马国鹭,曾国英,等.正交视觉与倾角仪组合空间位姿测量方法研究[J].激光技术,2020,44(3):278-282.)中提出正交双目视觉与倾角仪相结合的位姿测量方法，利用双目视觉求解的位姿信息结合倾角仪输出的角度信息实现相对位姿的测量，然而由于视觉测量精度随测量距离下降，并且易受工作环境等噪声影响，并不适用于盾构等大型室外作业工程之中。文献([3]肖涛.盾构姿态测量中倾角仪和陀螺仪的组合应用研究[D].武汉:华中科技大学,2013.)中利用光纤陀螺仪与激光标靶的融合进行六自由度位姿测量，提出基于陀螺仪零漂估计的传感器融合算法，利用稳态环境下激光标靶高精度输出对陀螺仪零漂进行估计与预测，从而提升组合***的动态性能与测量精度，然而该方法中缺少对倾角仪失准的有效监测，将导致在动态情况下倾角仪的输出“野值”污染滤波器，并且在盾构等高动态振动情况下，缺失了激光标靶的静态基准，组合***缺少有效的误差补偿方法，陀螺仪的输出将会产生较为严重的误差累积，此外光纤陀螺成本较高且温度系数、可靠性、长储尚不理想。
六自由度激光标靶测量***在我国盾构等大型工程领域的实时位姿测量中有着重要应用并对其动态性能提出了更高需求，然而目前对其动态性能提升方法尚有不足，寻找一种高精度、高鲁棒性的六自由度激光标靶动态性能提升方法，进一步满足我国大型工程中装配与制造需求，具有重要的应用价值。
发明内容
本发明的目的在于提供一种六自由度激光标靶测量***及其动态性能提升方法，能够在动态应用场景下提供高精度、稳定的六自由度位姿测量值。本发明在激光标靶内设置倾角传感单元和捷联惯导测量单元(即SINS***)，由于捷联惯导测量单元的量测值是恒定的，因此将倾角传感单元的测量值与其对比估算误差。随后使用多重次优渐消自适应容错卡尔曼滤波(SMFAFKF)的自适应容错滤波算法可以在倾角传感单元正常时结合***工作状态自适应调整，在其失常时自适应调整滤波量测方程，利用可供观测的量测信息进行姿态误差的预测与补偿。
本发明的目的是通过以下技术方案实现的：
一种六自由度激光标靶测量***，包括激光跟踪仪和六自由度姿态激光标靶，所述激光跟踪仪作为基站用于通过高频测点提供冗余量测信息，所述激光标靶固定在待测目标上，包括角锥棱镜、倾角传感单元、视觉成像单元、捷联惯导测量单元和数据处理单元；所述数据处理单元与倾角传感单元、视觉成像单元和捷联惯导测量单元电连接；
所述视觉成像单元与所述角锥棱镜配合测量待测目标的方位角，所述倾角传感单元用于测量滚转角和俯仰角信息，所述捷联惯导测量单元用于测量待测目标的姿态、速度与位置信息；所述数据处理单元接收来自所述倾角传感单元、视觉成像单元和捷联惯导测量单元的量测信息并传送至上位机；
所述上位机建立组合滤波***的状态空间方程,将捷联惯导测量单元的量测信息比对所述倾角传感单元和视觉成像单元的量测信息获得捷联惯导测量单元的误差，引入故障检测算法实时判定倾角传感单元工作状态，并在倾角传感单元正常和失准时使用多重次优渐消自适应容错卡尔曼滤波算法进行自适应容错滤波对捷联惯导测量单元误差进行预测与补偿。
一种六自由度激光标靶测量***的动态性能提升方法，包括：
步骤一：根据倾角传感单元、视觉成像单元获取的姿态信息与激光跟踪仪获取的冗余量测信息结合捷联惯导测量单元获取的姿态、速度与位置信息，确定***状态变量并建立组合滤波***的状态空间方程；其中，所述冗余量测信息是指测量的速度、位置信息；
步骤二：提取单个滤波新息向量(innovation vector)构建残差卡方故障检测量进行检测，识别检测量判断***是否故障，如判断***故障，执行步骤三；否则，提取状态递推器中的伪新息向量所构建残差矩阵的特征谱峰值对所述六自由度激光标靶测量***的倾角传感单元进行进一步故障检测；
步骤三：当步骤二判断为六自由度激光标靶测量***工作正常时，使用多重次优渐消自适应容错卡尔曼滤波算法进行自适应容错滤波获取最优的***状态误差估计值；其中，利用步骤一中所构建的状态空间方程结合自适应调节算法构造渐消因子，根据***当前状态对渐消因子自适应调整；
当步骤二判断为六自由度激光标靶测量***工作失常时，即当倾角传感单元失准时，***调整步骤一建立的状态空间方程中量测矩阵结构，再使用多重次优渐消自适应容错卡尔曼滤波算法对***误差进行预测，获取最优的***状态误差估计值；
步骤四：根据获取的***状态量误差估计值对当前***状态进行补偿获取当前***位姿信息。
进一步的，所述步骤一具体包括如下步骤：
S101:建立倾角传感器坐标系YsOZs，获取俯仰角与滚转角信息：所述倾角传感单元输出两惯性轴与水平面的夹角θ与η，其中俯仰角γ定义为OYs轴与水平面的夹角，即γ＝η，滚转角β根据如下几何关系求解：
S102:获取方位角信息：根据获取的俯仰角与滚转角信息，经过单目视觉成像单元获得的入射光图像坐标经双矢量定姿模型求解方位角信息；
S103:根据S101-102获得的信息以及结合激光跟踪仪接收激光标靶的返回光获取速度与位置信息，和捷联惯导测量单元获取姿态、速度与位置信息，建立组合滤波***状态空间方程：
其中，状态向量
φx,φy,φz分别表示捷联惯导测量单元的滚转角误差、俯仰角误差与方位角误差；δvx，δvy，δvz分别表示捷联惯导测量单元在X轴、Y轴、Z轴上的速度误差；δpx，δpy，δpz分别表示捷联惯导测量单元在X轴、Y轴、Z轴上的位置误差；εgb，εgr分别表示捷联惯导测量单元的陀螺仪的静态偏差与动态偏差，εab，εar分别表示加速度计的静态偏差与动态偏差；量测向量
其中，/>
δvS，δpS分别表示捷联惯导测量单元的获取的姿态向量、速度向量与位置向量；φP表示激光标靶获取的姿态向量，vP、pP是通过激光跟踪仪接收激光标靶返回激光获取的速度向量与位置向量。
进一步的，所述步骤二中的故障检测算法是一种改进的残差卡方-特征值的检测算法，采用状态递推器构建伪新息序列，提取以当前递推器内的归一化伪新息向量所构建残差矩阵的特征谱峰值作为故障检测指标，具体包括：
构建如下状态递推器：
其中，
为k-1时刻的状态递推器内的状态向量，/>
为k-1时刻的状态递推器内的状态协方差阵，/>
为状态初值，/>
为状态协方差阵初值，为避免状态发散，状态递推器设置长度为N，即递推N个时刻则以当前原滤波器的状态量及状态协方差阵重新赋状态递推器的初值开始新的状态递推。由此构建伪新息向量：/>
将状态递推器内的归一化伪新息向量构建矩阵：
其中n为当前状态递推器的伪新息向量数，
为对应的伪新息协方差阵，定义残差矩阵Dk为：
提取Dk特征谱的极大值λk＝max{δ{Dk}}，构建如下判定准则：
当λk＞λd时，***发生故障；当λk≤λd时，***工作正常；其中，λd为故障检测阈值。
进一步的，步骤三中“使用多重次优渐消自适应容错卡尔曼滤波算法进行自适应容错滤波获取最优的***状态误差估计值”具体包括：
构建渐消矩阵Lk+1实时对中间状态协方差阵进行修正，其滤波递推式为：
其中，Xk/k为k时刻的状态向量，Pk/k为k时刻的状态协方差阵，Φk+1/k为状态转移矩阵，Γk+1/k为控制矩阵，uk为k时刻的控制变量，Qk+1为控制量协方差阵，Hk+1为量测矩阵，Rk为量测方差阵，Zk+1为量测向量，rk+1为新息向量，Kk+1为卡尔曼增益。定义渐消矩阵Lk+1＝diag(λ1,λ2,...,λm)，其中m为状态向量维数，渐消因子λi按照如下规则进行自适应调整：
构造如下统计量：
可知γk+1服从自由度为m的卡方分布，即γk+1～χ2(m)，令Sk+1＝Hk+1Lk+，
Nk+1＝Rk+1，其中Sk+1即为表示渐消矩阵Lk+1中可被观测的部分，则统计量γk+1可表示为：
依据极大似然检验的原理，当***正常时，则有
α为置信度水平，分别对其每一分量进行检测，则有：
其中εi为检测阈值，
由此获取维持***正常渐消因子的取值范围为：/>
考虑到滤波器的收敛性，取
对于不可观测的状态量对应的渐消因子，由于缺少对应的量测约束，可确定为1。
与现有技术相比，本发明的技术方案所带来的有益效果是：
1、本发明采用激光跟踪仪为测量基站并通过捷联惯导测量单元(SINS)与激光标靶结合的方式提升六自由度激光标靶的动态性能，充分利用***中可观测的位置、速度冗余信息提升***的容错能力，可在倾角传感单元产生动态失准时使组合***提供稳定高精度的姿态测量值。
2、本发明提出一种多重次优渐消自适应容错卡尔曼滤波(SMFAFKF)的自适应容错滤波算法，可在倾角传感单元正常工作时引入次优渐消因子调整状态量记忆长度，以增强可靠量测值对于状态量最优估计的影响比重，提升状态估计精度；在倾角单元发生故障时，自适应调整滤波量测方程，利用可供观测的量测信息进行姿态误差的预测与补偿，从而提升了***的测量精度与鲁棒性。
3、本发明的步骤二提出一种改进残差卡方-特征值的故障检测算法，克服传统故障检测算法检测灵敏度低，对“软故障”检测性能差的问题，在复杂工业场景下的应用范围与价值得到了提升。
附图说明
图1是本发明所述的六自由度激光标靶动态性能提升方法流程图；
图2是六自由度激光标靶测量***整体结构示意图；
图3是激光标靶内部结构示意图；
图4是倾角传感单元姿态测量原理图；
图5是步骤二中故障检测方法的原理图。
其中，
201：激光跟踪仪，202：激光标靶，301：角锥棱镜，302：倾角传感单元，303：视觉成像单元，304：捷联惯导测量单元，305：数据处理单元。
具体实施方式
为使本发明实施例的目的、技术方案、有益效果及显著进步更加清楚，下面结合本发明实施例中所提供的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所有描述的这些实施例仅是本发明的部分实施例，而不是全部的实施例；基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图1所示，一种六自由度激光标靶测量***的动态性能提升方法，所述方法利用如图2-3所示的一种六自由度激光标靶测量***，包括激光跟踪仪201和六自由度姿态激光标靶202，所述激光跟踪仪201作为基站用于通过高频测点提供冗余量测信息，所述激光标靶202固定在待测目标上，包括角锥棱镜301、倾角传感单元302、视觉成像单元303、捷联惯导测量单元304和数据处理单元305。所述角锥棱镜301选用针孔棱镜，所述倾角传感单元302选用双轴倾角仪，所述捷联惯导测量单元304包括陀螺仪和加速度计，所述视觉成像单元303选用单目视觉成像单元。如图3所示，所述视觉成像单元303与所述角锥棱镜配合测量方位角，所述倾角传感单元302将测得的滚转角和俯仰角信息传送至所述数据处理单元305，所述捷联惯导测量单元304与所述数据处理单元305电连接，用于将测得的姿态、速度与位置信息传送至所述数据处理单元。所述数据处理单元305接收来自所述倾角传感单元302、视觉成像单元303和捷联惯导测量单元304的数据并传送至上位机。
所述上位机引入自适应容错滤波算法以进行数据融合并通过激光跟踪仪的高频测点提供冗余量测信息，将捷联惯导测量单元304的量测信息比对所述倾角传感单元302和视觉成像单元303的量测信息获得捷联惯导测量单元的误差，引入故障检测算法实时判定倾角传感单元302工作状态，并在倾角传感单元302失准时利用激光跟踪仪的高频测点提供的冗余量测信息对捷联惯导测量单元误差进行预测与补偿。
所述六自由度激光标靶测量***的动态性能提升方法具体包括如下步骤：
步骤一：根据倾角传感单元302、视觉成像单元303和捷联惯导测量单元304的量测信息，确定***状态变量并建立组合滤波***的状态空间方程；
建立倾角传感器坐标系YsOZs；其中倾角传感单元302获取滚转角与俯仰角信息，如图4所示，所述倾角传感单元302输出两惯性轴与水平面的夹角θ与η，其中俯仰角γ定义为OYs轴与水平面的夹角，即γ＝η，滚转角β可通过几何关系直接进行如下求解：
结合倾角传感单元302获取的俯仰角与滚转角，经过单目视觉成像单元303获得的入射光图像坐标经双矢量定姿模型求解方位角信息，并且激光跟踪仪201接收激光标靶201的返回光获取速度与位置信息，再结合捷联惯导测量单元304获取的姿态、速度与位置信息，可建立如下组合滤波***状态方程与量测方程：
其中，状态向量
φx,φy,φz分别表示捷联惯导测量单元304的滚转角误差、俯仰角误差与方位角误差；δvx，δvy，δvz分别表示捷联惯导测量单元304在X轴、Y轴、Z轴上的速度误差；δpx，δpy，δpz分别表示捷联惯导测量单元304在X轴、Y轴、Z轴上的位置误差；εgb，εgr分别表示捷联惯导测量单元304的陀螺仪的静态偏差与动态偏差，εab，εar分别表示加速度计的静态偏差与动态偏差。量测向量/>
其中，/>
δvS，δpS分别表示捷联惯导测量单元304的获取的姿态向量、速度向量与位置向量；φP表示激光标靶202获取的姿态向量，vP、pP是通过激光跟踪仪201接收激光标靶202返回激光获取的速度向量与位置向量。
步骤二：提取单个滤波新息向量(innovation vector)构建残差卡方故障检测量进行检测，识别检测量判断***是否故障，如判断***故障，执行步骤三；否则，提取状态递推器中的伪新息向量所构建残差矩阵的特征谱峰值对所述六自由度激光标靶测量***的倾角传感单元进行进一步故障检测；
其中，故障检测的算法为残差卡方故障检测算法结合本发明所提供的改进残差卡方-特征值检测算法，如图5所示，首先提取当前k时刻滤波新息向量构建残差卡方检测量进行故障检测，当检测量超出检测阈值，则认定***故障；否则，则采用改进残差卡方-特征值的故障检测算法进行检测，构建如下状态递推器：
其中，
为k-1时刻的状态递推器内的状态向量，/>
为k-1时刻的状态递推器内的状态协方差阵，/>
为状态初值，/>
为状态协方差阵初值，为避免状态发散，状态递推器设置长度为N，即递推N个时刻则以当前原滤波器的状态量及状态协方差阵重新赋状态递推器的初值开始新的状态递推。由此构建伪新息向量：
将状态递推器内的归一化伪新息向量构建矩阵：
其中n为当前状态递推器的伪新息向量数，
为对应的伪新息协方差阵，定义残差矩阵Dk为：
提取Dk特征谱的极大值λk＝max{δ{Dk}}，构建如下判定准则：
当λk＞λd时，***发生故障；
当λk≤λd时，***工作正常。
其中，λd为故障检测阈值。
步骤三：当步骤二判断为六自由度激光标靶测量***工作正常时，即当倾角传感单元正常时，根据***当前状态对渐消因子自适应调整并结合渐消矩阵进行自适应容错滤波获取***状态估计；本专利将倾角传感单元正常的状态即视为六自由度激光标靶测量***工作正常，因为倾角传感单元的故障会引起所述六自由度激光标靶测量***的故障；
所述自适应容错滤波算法为本发明所提供的一种多重次优渐消自适应容错卡尔曼滤波(SMFAFKF)算法，即在步骤一建立的滤波***状态方程与量测方程(即状态空间方程)的基础之上的状态递推算法的改进，其通过引入渐消矩阵并依据极大似然检测原理进行多重渐消因子自适应调整，从而实时调整状态协方差阵，限制卡尔曼滤波器(KF)的记忆长度，提升对残差序列中的有效信息的利用比重，用于提升***检测精度与滤波精度；
当步骤二检测所述倾角传感单元302数据未发生失准时，定义步骤一中建立的状态空间方程的量测矩阵Hk+1：
其中，I为3×3单位阵；
所述SMFAFKF的滤波递推式为：
其中，Lk+1为渐消矩阵，Xk/k为k时刻的状态向量，Pk/k为k时刻的状态协方差阵，Φk+1/k为状态转移矩阵，Γk+1/k为控制矩阵，uk为k时刻的控制变量，Qk+1为控制量协方差阵，Hk+1为量测矩阵，Rk为量测方差阵，Zk+1为量测向量，rk+1为新息向量，Kk+1为卡尔曼增益。定义渐消矩阵Lk+1＝diag(λ1,λ2,...,λm)，其中m为状态向量维数，渐消因子λi按照如下规则进行自适应调整：
构造如下统计量：
可知γk+1服从自由度为m的卡方分布，即γk+1～χ2(m)，令Sk+1＝Hk+1Lk-，
Nk+1＝Rk+1，其中Sk+1即为表示渐消矩阵Lk+1中可被观测的部分，则统计量γk+1可表示为：
依据极大似然检验的原理，当***正常时，则有
α为置信度水平，分别对其每一分量进行检测，则有：
其中εi为检测阈值，
由此获取维持***正常渐消因子的取值范围为：
考虑到滤波器的收敛性，取
对于不可观测的状态量对应的渐消因子，由于缺少对应的量测约束，可确定为1。
当步骤二判断为六自由度激光标靶测量***故障时，即当倾角传感单元失准时，***调整量测结构，即当步骤二检测倾角传感单元302数据发生失准时，定义步骤一建立的状态空间方程的量测矩阵Hk+1：
充分利用可观测冗余量测信息，使用多重次优渐消自适应容错卡尔曼滤波算法对***误差进行预测从而获取最优的***状态误差估计值，实现在缺失姿态量测条件下的SINS姿态误差预测与补偿。当***出现故障的时候，那么姿态便是不可观测量，所以认为渐消因子就认为是1，因此本步骤中涉及的多重次优渐消自适应容错卡尔曼滤波算法就和步骤三判断当六自由度激光标靶测量***正常时的算法相同，在此不再赘述。
步骤四：根据获取的***状态误差估计值对当前***状态进行补偿获取当前***位姿信息。
以上实施例仅用以说明本发明的技术方案，而非是对其的限制，尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换，而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围，本领域技术人员根据本说明书内容所做出的非本质改进和调整或者替换，均属本发明所要求保护的范围。
Claims (4)
1.一种六自由度激光标靶测量***，包括激光跟踪仪(201)和六自由度姿态激光标靶(202)，其特征在于，所述激光跟踪仪(201)作为基站用于通过高频测点提供冗余量测信息，所述激光标靶(202)固定在待测目标上，包括角锥棱镜(301)、倾角传感单元(302)、视觉成像单元(303)、捷联惯导测量单元(304)和数据处理单元(305)；所述数据处理单元(305)与倾角传感单元(302)、视觉成像单元(303)和捷联惯导测量单元(304)电连接；
所述视觉成像单元(303)与所述角锥棱镜配合测量待测目标的方位角，所述倾角传感单元(302)用于测量滚转角和俯仰角信息，所述捷联惯导测量单元(304)用于测量待测目标的姿态、速度与位置信息；所述数据处理单元(305)接收来自所述倾角传感单元(302)、视觉成像单元(303)和捷联惯导测量单元(304)的量测信息并传送至上位机；
所述上位机建立组合滤波***的状态空间方程,将捷联惯导测量单元(304)的量测信息比对所述倾角传感单元(302)和视觉成像单元(303)的量测信息获得捷联惯导测量单元的误差，引入故障检测算法实时判定倾角传感单元(302)工作状态，并在倾角传感单元(302)正常和失准时使用多重次优渐消自适应容错卡尔曼滤波算法进行自适应容错滤波对捷联惯导测量单元误差进行预测与补偿。
2.如权利要求1所述六自由度激光标靶测量***的动态性能提升方法，包括：
步骤一：根据倾角传感单元(302)、视觉成像单元(303)获取的姿态信息与激光跟踪仪(201)获取的冗余量测信息结合捷联惯导测量单元(304)获取的姿态、速度与位置信息，确定***状态变量并建立组合滤波***的状态空间方程；其中，所述冗余量测信息是指测量的速度、位置信息；
步骤二：提取单个滤波新息向量构建残差卡方故障检测量进行检测，识别检测量判断***是否故障，如判断***故障，执行步骤三；否则，提取状态递推器中的伪新息向量所构建残差矩阵的特征谱峰值对所述六自由度激光标靶测量***的倾角传感单元进行进一步故障检测；
步骤三：当步骤二判断为六自由度激光标靶测量***工作正常时，使用多重次优渐消自适应容错卡尔曼滤波算法进行自适应容错滤波获取最优的***状态误差估计值；其中，利用步骤一中所构建的状态空间方程结合自适应调节算法构造渐消因子，根据***当前状态对渐消因子自适应调整；
当步骤二判断为六自由度激光标靶测量***工作失常时，即当倾角传感单元失准时，***调整步骤一建立的状态空间方程中量测矩阵结构，再使用多重次优渐消自适应容错卡尔曼滤波算法对***误差进行预测，获取最优的***状态误差估计值；
步骤四：根据获取的***状态量误差估计值对当前***状态进行补偿获取当前***位姿信息；
其中，所述步骤二中的故障检测算法是一种改进的残差卡方-特征值的检测算法，采用状态递推器构建伪新息序列，提取以当前递推器内的归一化伪新息向量所构建残差矩阵的特征谱峰值作为故障检测指标，具体包括：
构建如下状态递推器：
其中，
为k-1时刻的状态递推器内的状态向量，/>
为k-1时刻的状态递推器内的状态协方差阵，/>
为状态初值，/>
为状态协方差阵初值，为避免状态发散，状态递推器设置长度为N，即递推N个时刻则以当前原滤波器的状态量及状态协方差阵重新赋状态递推器的初值开始新的状态递推；由此构建伪新息向量：
将状态递推器内的归一化伪新息向量构建矩阵：
其中n为当前状态递推器的伪新息向量数，
为对应的伪新息协方差阵，定义残差矩阵Dk为：
提取Dk特征谱的极大值λk＝max{δ{Dk}}，构建如下判定准则：
当λk>λd时，***发生故障；
当λk≤λd时，***工作正常；
其中，λd为故障检测阈值。
3.根据权利要求2所述的动态性能提升方法，其特征在于，所述步骤一具体包括如下步骤：
S101:建立倾角传感器坐标系YsOZs，获取俯仰角与滚转角信息：所述倾角传感单元(302)输出两惯性轴与水平面的夹角θ与η，其中俯仰角γ定义为OYs轴与水平面的夹角，即γ＝η，滚转角β根据如下几何关系求解：
S102:获取方位角信息：根据获取的俯仰角与滚转角信息，经过单目视觉成像单元(303)获得的入射光图像坐标经双矢量定姿模型求解方位角信息；
S103:根据S101-102获得的信息以及结合激光跟踪仪(201)接收激光标靶(202)的返回光获取速度与位置信息，和捷联惯导测量单元(304)获取姿态、速度与位置信息，建立组合滤波***状态空间方程：
其中，状态向量
φx,φy,φz分别表示捷联惯导测量单元(304)的滚转角误差、俯仰角误差与方位角误差；δvx，δvy，δvz分别表示捷联惯导测量单元(304)在X轴、Y轴、Z轴上的速度误差；δpx，δpy，δpz分别表示捷联惯导测量单元(304)在X轴、Y轴、Z轴上的位置误差；εgb，εgr分别表示捷联惯导测量单元(304)的陀螺仪的静态偏差与动态偏差，εab，εar分别表示加速度计的静态偏差与动态偏差；量测向量/>
其中，/>
δvS，δpS分别表示捷联惯导测量单元304的获取的姿态向量、速度向量与位置向量；φP表示激光标靶(202)获取的姿态向量，vP、pP是通过激光跟踪仪(201)接收激光标靶(202)返回激光获取的速度向量与位置向量。
4.根据权利要求2所述的动态性能提升方法，其特征在于，步骤三中“使用多重次优渐消自适应容错卡尔曼滤波算法进行自适应容错滤波获取最优的***状态误差估计值”具体包括：
构建渐消矩阵Lk+1实时对中间状态协方差阵进行修正，其滤波递推式为：
其中，Xk/k为k时刻的状态向量，Pk/k为k时刻的状态协方差阵，Φk+1/k为状态转移矩阵，Γk+1/k为控制矩阵，uk为k时刻的控制变量，Qk+1为控制量协方差阵，Hk+1为量测矩阵，Rk为量测方差阵，Zk+1为量测向量，rk+1为新息向量，Kk+1为卡尔曼增益；定义渐消矩阵Lk+1＝diag(λ1,λ2,...,λm)，其中m为状态向量维数，渐消因子λi按照如下规则进行自适应调整：
构造如下统计量：
可知γk+1服从自由度为m的卡方分布，即
令Sk+1＝Hk+1Lk+1，
Nk+1＝Rk+1，其中Sk+1即为表示渐消矩阵Lk+1中可被观测的部分，则统计量γk+1可表示为：
依据极大似然检验的原理，当***正常时，则有
α为置信度水平，分别对其每一分量进行检测，则有：
其中εi为检测阈值，
由此获取维持***正常渐消因子的取值范围为：
考虑到滤波器的收敛性，取
对于不可观测的状态量对应的渐消因子，由于缺少对应的量测约束，确定为1。/>
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211010946.7A CN115371650B (zh) | 2022-08-23 | 2022-08-23 | 一种六自由度激光标靶测量***及其动态性能提升方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211010946.7A CN115371650B (zh) | 2022-08-23 | 2022-08-23 | 一种六自由度激光标靶测量***及其动态性能提升方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115371650A CN115371650A (zh) | 2022-11-22 |
| CN115371650B true CN115371650B (zh) | 2023-06-02 |
Family
ID=84067268
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211010946.7A Active CN115371650B (zh) | 2022-08-23 | 2022-08-23 | 一种六自由度激光标靶测量***及其动态性能提升方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115371650B (zh) |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7142981B2 (en) * | 2003-08-05 | 2006-11-28 | The Boeing Company | Laser range finder closed-loop pointing technology of relative navigation, attitude determination, pointing and tracking for spacecraft rendezvous |
| CN111722295B (zh) * | 2020-07-04 | 2021-04-23 | 东南大学 | 一种水下捷联式重力测量数据处理方法 |
| CN114111771A (zh) * | 2021-11-25 | 2022-03-01 | 九江中船仪表有限责任公司（四四一厂） | 一种双轴稳定平台的动态姿态测量方法 |
-
2022
- 2022-08-23 CN CN202211010946.7A patent/CN115371650B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN115371650A (zh) | 2022-11-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111207774B (zh) | 一种用于激光-imu外参标定的方法及*** | |
| CN106647791B (zh) | 三维姿态测控装置、机械设备及三维姿态的测控方法 | |
| CN109737913B (zh) | 一种激光跟踪姿态角测量***及方法 | |
| CN109781099A (zh) | 一种自适应ukf算法的导航方法及*** | |
| CN108225258A (zh) | 基于惯性单元和激光跟踪仪动态位姿测量装置和方法 | |
| CN112611380B (zh) | 基于多imu融合的姿态检测方法及其姿态检测装置 | |
| KR100898169B1 (ko) | 관성항법시스템의 초기정렬 방법 | |
| CN105973268B (zh) | 一种基于共基座安装的传递对准精度定量评估方法 | |
| CN112697131B (zh) | 基于视觉和惯性导航***的井下移动装备定位方法及*** | |
| CN113804185A (zh) | 基于mems阵列式的新型惯性导航*** | |
| CN113739795A (zh) | 一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法 | |
| CN111141286A (zh) | 一种无人机飞控多传感器姿态置信解算方法 | |
| CN115371650B (zh) | 一种六自由度激光标靶测量***及其动态性能提升方法 | |
| CN110672103A (zh) | 一种多传感器目标跟踪滤波方法及*** | |
| CN114252077A (zh) | 基于联邦滤波器的双gps/sins的组合导航方法及*** | |
| CN117739972A (zh) | 一种无全球卫星定位***的无人机进近阶段定位方法 | |
| CN107888289B (zh) | 基于可见光通信与惯性传感器融合的室内定位方法及平台 | |
| CN112229392B (zh) | 一种高冗余的室内煤场导航方法及*** | |
| CN109443333B (zh) | 一种陀螺阵列反馈加权融合方法 | |
| CN115096294B (zh) | 一种多参量水下磁目标体定位方法 | |
| CN110779549A (zh) | 一种水下组合导航***突变型故障诊断方法 | |
| US20210201011A1 (en) | Data processing method for multi-sensor fusion, positioning apparatus and virtual reality device | |
| CN112344966B (zh) | 一种定位失效检测方法、装置、存储介质及电子设备 | |
| CN114526729A (zh) | 一种基于冗余技术的mems惯性定位***航向优化方法 | |
| CN114705223A (zh) | 多移动智能体在目标跟踪中的惯导误差补偿方法及*** |
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 |