CN105180966B - 一种基于卡尔曼滤波对二次平台线阵ccd测量倾角进行优化的方法 - Google Patents

一种基于卡尔曼滤波对二次平台线阵ccd测量倾角进行优化的方法 Download PDF

Info

Publication number
CN105180966B
CN105180966B CN201510474959.3A CN201510474959A CN105180966B CN 105180966 B CN105180966 B CN 105180966B CN 201510474959 A CN201510474959 A CN 201510474959A CN 105180966 B CN105180966 B CN 105180966B
Authority
CN
China
Prior art keywords
msub
mrow
points
line array
array ccd
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
CN201510474959.3A
Other languages
English (en)
Other versions
CN105180966A (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.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of Technology
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 Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN201510474959.3A priority Critical patent/CN105180966B/zh
Publication of CN105180966A publication Critical patent/CN105180966A/zh
Application granted granted Critical
Publication of CN105180966B publication Critical patent/CN105180966B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
    • B64G7/00—Simulating cosmonautic conditions, e.g. for conditioning crews

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Navigation (AREA)
  • Image Analysis (AREA)

Abstract

一种基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的系统及方法，本发明涉及基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的系统及方法。本发明的目的是为了解决现有单纯采用线阵CCD对平台倾角进行测量时，测量结果精度低的问题。通过以下技术方案实现的：步骤一、高速旋转电机带动半导体激光器旋转，得到线阵CCD感光器件上光点的高度数据；步骤二、选取相邻的3个线阵CCD感光器件上光点的高度数据记，确定一个平面记为平面ABC，根据三点坐标可求得平面的法线方向矢量，求得θ；步骤三、采用卡尔曼滤波对夹角θ进行递推校正，得出t时刻的实际倾角。本发明应用于二次平台倾角测量领域。

Description

一种基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化 的方法
技术领域
本发明涉及基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的方法。
背景技术
在现代航天探索中，经常需要高平面度的基部平台，比如在空间交汇对接、地面测量、导航、网络通信和编队控制等全物理仿真试验中，需要支撑整个地面仿真器，为地面仿真实验提供基础平台，为编队卫星地面试验验证、控制算法验证分析等物理仿真试验提供平台支撑。考虑到大面积实验平台通常都是由若干小平面板块拼接而成，这种方法虽然易于加工、运输，但很难保证多块平板之间的平面度，拼接产生的缝隙会影响气足运行的平稳性，从而使仿真结果产生偏差，不能真实模拟航天器在真空和微重力模拟环境下的运行状态。负载移动、气足气膜高度变化、地面平整度等因素，平台的水平倾角会实时变化，因此需要平台在静止和运动状态都能够被快速、精确的调平，以保证平台足够的水平度。
二次平台和六自由度气浮台轨道器及六自由度气浮台上升器共同构成交会对接仿真试验的核心部分。其中，以气浮球轴承和重力平衡伺服运动机构为核心组成的六自由度气浮台轨道器用来模拟轨道器动力学仿真状态；二次平台用来支撑六自由度气浮台轨道器，实现高精度自动调平；与六自由度上升器配合，实现完整的交会对接动力学与控制全物理仿真试验。整个系统运行在大型花岗岩平台上，是地面全物理仿真试验的核心和基础平台；也为未来的编队卫星地面试验验证、控制算法验证分析等物理仿真试验提供平台支撑。倾角测量是整个自动调平系统的一部分，主要实现激光扫描平面倾角测量。
原方法是利用线阵CCD在扫过不同的三个线阵CCD时分别求出的目标在空间中的Z向坐标，再加上我们之前安装好已经知道的线阵CCD所在的空间的X，Y坐标，这样的话就知道了激光器所扫过的扇面在空间中的三个点，在空间中一直三个点的坐标可以确定三个点所在平面的法向量，由此我们可以知到此时激光所在扇面的法向量，由于激光器是安装在二次平台上以一个自由度转动，由此我们便求出了二次平台此时的水平度，但是问题在于，当选取的CCD相隔较远时，事实上也应该去相隔较远的三个线阵CCD来进行水平度的计算，在这时，其实这三个点并不是同时被扫到的，而是他们之间有一定的时间间隔，这个时间间隔与飞轮电机的角速度和线阵CCD所在的位置有关，设这个间隔为T0，事实上按照之前的计算方法，我们第一时刻所得到的平面其实更趋近于T0之前时刻的平面，因为在选取上这三个点其实是第一时刻面，T0时刻面，和两倍的T0时刻之前的面，虽然在实际系统中飞轮电机转速很快，使得T0很小，可以让这部分误差也随之变小，综上所述现有技术单纯采用线阵CCD对平台倾角进行测量时，测量结果精度低。
发明内容
本发明的目的是为了解决现有单纯采用线阵CCD对平台倾角进行测量时，测量结果精度低的问题，而提出了一种基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的系统及方法。
上述的发明目的是通过以下技术方案实现的：
步骤一、高速旋转电机带动半导体激光器旋转，扫描以高速旋转电机为中心，直径为1～5米范围内的线阵CCD，在线阵CCD感光器件上留下不同高度的光点，通过线阵CCD确定线阵CCD感光器件上光点的成像中心坐标，得到线阵CCD感光器件上光点的高度数据；
其中，所述高速为大于等于10000r/min；
步骤二、选取相邻的3个线阵CCD感光器件上光点的高度数据记为线阵CCD上3个点的空间坐标，即A(x1，y1，z1)，B(x2，y2，z2)，C(x3，y3，z3)，x1为A点在x轴上的坐标，y1为A点在y轴上的坐标，z1为A点在z轴上的坐标，x2为B点在x轴上的坐标，y2为B点在y轴上的坐标，z2为B点在z轴上的坐标，x3为C点在x轴上的坐标，y3为C点在y轴上的坐标，z3为C点在z轴上的坐标；
以A(x1，y1，z1)，B(x2，y2，z2)，C(x3，y3，z3)三点确定一个平面记为平面ABC，根据三点坐标可求得平面ABC的法线方向矢量(a,b,c)，进而可求得该平面与水平面的夹角θ；
步骤三、采用卡尔曼滤波对夹角θ进行递推校正，得出t时刻的实际倾角θ(t|t)。
发明效果
采用本发明的一种基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的系统及方法，本方法可以应用于二次平台系统中对平台的调整，保证了气浮台在大平台处于动态工作时也能达到微米级别的精度，对直接应用线阵CCD数据解算平台倾角的方案进行了优化，使六自由度气浮台轨道器实现高精度自动调平，实现完整的交会对接动力学与控制全物理仿真试验，测量结果精度提高了25％。
附图说明
图1二次平台线阵CCD的倾角测量系统示意图；
图2测量二次平台倾角原理示意图；
图3求解光点构成平面的倾角示意图；
图4卡尔曼滤波的在本系统模型的框图。
具体实施方式
具体实施方式一：结合图1说明本实施方式，一种基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的方法，其特征在于，一种基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的方法具体是按照以下步骤进行的：
所述基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的系统由二次平台(1)、大平台(2)、半导体激光器(3)、高速旋转轴电机(4)、线阵CCD(5)、工控机(6)组成；半导体激光器(3)固定在高速旋转轴电机(4)上，置于二次平台上表面，将线阵CCD(5)设置在整个大平台四周，设置线阵CCD(5)的感光面法线的交点在底部大平台中心；二次平台可在整个底部大平台安全范围内运动，线阵CCD信号经工控机处理得到二次平台倾角；
步骤一、高速旋转电机带动半导体激光器旋转，扫描以高速旋转电机为中心，直径为1～5米范围内的线阵CCD，在线阵CCD感光器件上留下不同高度的光点，通过线阵CCD确定线阵CCD感光器件上光点的成像中心坐标，得到线阵CCD感光器件上光点的高度数据；
其中，所述高速为大于等于10000r/min；
步骤二、选取相邻的3个线阵CCD感光器件上光点的高度数据记为线阵CCD上3个点的空间坐标，即A(x1，y1，z1)，B(x2，y2，z2)，C(x3，y3，z3)，x1为A点在x轴上的坐标，y1为A点在y轴上的坐标，z1为A点在z轴上的坐标，x2为B点在x轴上的坐标，y2为B点在y轴上的坐标，z2为B点在z轴上的坐标，x3为C点在x轴上的坐标，y3为C点在y轴上的坐标，z3为C点在z轴上的坐标；
以A(x1，y1，z1)，B(x2，y2，z2)，C(x3，y3，z3)三点确定一个平面记为平面ABC，根据三点坐标可求得平面ABC的法线方向矢量(a,b,c)，进而可求得该平面与水平面的夹角θ；
步骤三、采用卡尔曼滤波对夹角θ进行递推校正，得出t时刻的实际倾角θ(t|t)。
具体实施方式二、本实施方式与具体实施方式一不同的是，所述步骤二中选取相邻的3个线阵CCD感光器件上光点的高度数据记为线阵CCD上3个点的空间坐标，即A(x1，y1，z1)，B(x2，y2，z2)，C(x3，y3，z3)，x1为A点在x轴上的坐标，y1为A点在y轴上的坐标，z1为A点在z轴上的坐标，x2为B点在x轴上的坐标，y2为B点在y轴上的坐标，z2为B点在z轴上的坐标，x3为C点在x轴上的坐标，y3为C点在y轴上的坐标，z3为C点在z轴上的坐标；以A(x1，y1，z1)，B(x2，y2，z2)，C(x3，y3，z3)三点确定一个平面记为平面ABC，根据三点坐标可求得平面ABC的法线方向矢量(a,b,c)，进而可求得该平面与水平面的夹角θ；具体过程为：
如图3所示，
以平台某一位置定位空间坐标原点(0，0，0)，当激光器扫射一周并采集数据，
设分别在相邻的三个线阵CCD上取点A(x1,y1,z1)，点B(x2,y2,z2)和点C(x3,y3,z3)，三点确定一个平面记为平面ABC，根据三点坐标求得平面ABC的法线方向矢量(a,b,c)，
式中，i为x方向的单位法向量，k为z方向的单位法向量，j为y方向的单位法向量；
设水平面为XOY平面，XOY平面法向量为垂直于XOY平面的向量，即XOY平面法向量的xy方向分量为0，取z方向分量为1，即(0,0,1)；
得出平面ABC与X0Y平面的夹角的余弦为：
在实际应用中，我们还需要再把这个角度按照所设定的X，Y正方向进行分解，以方便调整。平台稳定调节的思想是：在稳定调节的过程中，将平台测量反馈的倾角信息进行解算后，得到各个电机调平所需的位移输出，控制系统通过对电机进行位置控制实现动态调平功能。
具体实施方式三、结合图4说明本实施方式，本实施方式与具体实施方式一或二不同的是，所述步骤三中采用卡尔曼滤波对夹角θ进行递推校正，得出t时刻的实际倾角θ(t|t)；具体过程为：
建立一个离散的过程控制动态系统，设t时刻的过程控制动态系统的状态空间模型为：
θ(t)＝aθ(t-1)+bU(t)+W(t) (1)
t时刻过程控制动态系统的测量值为：
θz(t)＝Hθ(t)+V(t) (2)
式中，θ(t-1)为t-1时刻的过程控制动态系统状态，t为整数；
θ(t)为t时刻的过程控制动态系统状态；
U(t)为t时刻的控制量；
a为线阵CCD水平测量系统的第一个参数；
b为线阵CCD水平测量系统的第二个参数；
θz(t)为t时刻过程控制动态系统的测量值；
W(t)为t时刻过程控制动态系统噪声，协方差阵为Q；
V(t)为t时刻过程控制动态观测噪声，协方差阵为R；
H为测量过程控制动态系统的参数；
公式(1)、(2)中，a，b的取值由系统的实际情况决定。H在直接测量的情况下,H＝1。W(t)和V(t)各自表示过程和测量中的噪声，应当近似于白噪声，其协方差各是Q和R，在本系统中Q和R不受着其他量变化影响。
利用过程控制动态系统的状态空间模型(1)预测t时刻的值，具体过程为：
根据t-1时刻的过程控制动态系统状态预测t时刻的值，见公式(3)：
θ(t|t-1)＝aθ(t-1|t-1)+bU(t) (3)
式中，θ(t|t-1)为根据t-1时刻预测的t时刻的预测值；
θ(t-1|t-1)为t-1时刻的状态最优的结果；
U(t)为t时刻的控制量；
若只有过程控制动态系统的控制量，则U(t)值为零；
用P(t|t-1)表示θ(t|t-1)对应的协方差
P(t|t-1)＝aP(t-1|t-1)aT+Q (4)
式中，P(t-1|t-1)为θ(t-1|t-1)对应的协方差；
aT为a的转置矩阵；
Q为过程控制动态系统过程的协方差；
根据公式(2)和公式(3)求出t时刻的实际倾角θ(t|t)，见式(5)
θ(t|t)＝θ(t|t-1)+Kg(t)[θz(t)-Hθ(t|t-1)] (5)
式中，Kg(t)为卡尔曼增益；
Kg(t)＝P(t|t-1)HT/[HP(t|t-1)HT+R] (6)
式中，HT为H的转置矩阵，R为噪声的协方差矩阵；
通过公式(5)，(6)得出了t时刻的实际倾角θ(t|t)，更新t时刻的实际倾角θ(t|t)的协方差，
P(t|t)＝[In-Kg(t)H]P(t|t-1) (7)
根据公式(3)(4)(5)(6)和(7)就是卡尔曼滤波器的五个基本公式。通过五个基本公式，可以利用计算机程序对系统滤波。之前的描述的7个公式过程就是建立过程系统的说明，卡尔曼滤波虽然称之为滤波，但实际是一种预测过程，将最终得到的最优值作为最终结果，该结果相对于测量值更加准确，因此称之为滤波；
设线阵CCD水平测量系统的第一个参数a＝1；线阵CCD水平测量系统的第二个参数b＝1，控制量U(t)＝ωt△t，将a＝1,b＝1带入公式(3)、(4)既得公式(8)、(9)：
θ(t|t-1)＝θ(t-1|t-1)+ωt△t (8)
P(t|t-1)＝P(t-1|t-1)+Q (9)
式中，ωt为二次平台转速；
P(t|t-1)为θ(t|t-1)对应的协方差；
P(t-1|t-1)为θ(t-1|t-1)对应的协方差；
取H值为1，并结合式(8)和式(9)，式(5)(6)(7)改为：
θ(t|t)＝θ(t|t-1)+Kg(t)[θz(t)-θ(t|t-1)]
(10)
Kg(t)＝P(t|t-1)/[P(t|t-1)+R] (11)
P(t|t)＝[In-Kg(t)]P(t|t-1) (12)
式中，θ(t|t)为t时刻的实际倾角，Kg(t)为卡尔曼增益，θz(t)为t时刻过程控制动态系统的测量值，R为V(t)的协方差阵，P(t|t)为θ(t|t)对应的协方差，In为单位矩阵。
实施例1：
一种基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的方法具体是按照以下步骤进行的：
所述基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的系统由二次平台(1)、大平台(2)、半导体激光器(3)、高速旋转轴电机(4)、线阵CCD(5)、工控机(6)组成；半导体激光器(3)固定在高速旋转轴电机(4)上，置于二次平台上表面，将线阵CCD(5)设置在整个大平台四周，设置线阵CCD(5)的感光面法线的交点在底部大平台中心；二次平台可在整个底部大平台安全范围内运动，线阵CCD信号经工控机处理得到二次平台倾角；
步骤一、高速旋转电机带动半导体激光器旋转，扫描以高速旋转电机为中心，直径为1～5米范围内的线阵CCD，在线阵CCD感光器件上留下不同高度的光点，通过线阵CCD确定线阵CCD感光器件上光点的成像中心坐标，得到线阵CCD感光器件上光点的高度数据；
其中，所述高速为大于等于10000r/min；
步骤二、选取相邻的3个线阵CCD感光器件上光点的高度数据记为线阵CCD上3个点的空间坐标，即A(x1，y1，z1)，B(x2，y2，z2)，C(x3，y3，z3)，以A(x1，y1，z1)，B(x2，y2，z2)，C(x3，y3，z3)三点确定一个平面记为平面ABC，根据三点坐标可求得平面ABC的法线方向矢量(a,b,c)，进而可求得该平面与水平面的夹角θ，如图2所示；
步骤三、采用卡尔曼滤波对夹角θ进行递推校正，将t时刻的最优估计值作为t时刻的实际倾角θ(t|t)；
涉及到具体的运算，我们以一步的递推为例，我们设上位机接到了上一刻的最优值θ(t-1|t-1)＝0.20，此时系统中ωt＝3，△t＝0.01，根据公式(3)得知我们此刻得到的预测值为θ(t|t-1)＝θ(t-1|t-1)+ωt△t＝0.23；
结合实际情况将系统中的参数代入公式，假设上一时刻得到的最优角度估计的不确定度为P(t-1|t-1)＝0.3，对此预测值的不确定度为Q＝0.4，根据协方差计算，可知P(t|t-1)＝P(t-1|t-1)+Q＝0.5。注意这里面协方差的加减是平方和的加减。假设在现在时刻t时刻，测量系统所得到的测量值为θz(t)＝0.25，对此测量值的不确定度是R＝0.4。在得到了现在时刻t时刻的预测值和测量值之后，利用协方差计算来求出Kg(t)，这里面Kg(t)决定了最优估计值跟偏向于预测值还是测量值。将参数带入公式(11)得再将Kg＝0.78带入到公式(10)中，可以得出现在时刻t时刻的最优估计值θ(t|t)＝θ(t-1|t-1)+Kg(t)[θz(t)-θ(t|t-1)]＝0.23+0.78×(0.25-0.23)＝0.2456。在这种情况下的我们认为测量系统的不确定度较小，所以最终最优估计值更偏向测量值。
在得到了现在时刻t时刻的最优倾角值θ(t|t)＝0.2456后，为了递推能够进行下去，需要更新现在现在时刻t时刻的系统预测值的不确定度P(t|t)，由式(12)得，此时P(t|t)＝0.235会进入下一时刻的最优估计，类似于本次推导中P(t-1|t-1)＝0.3的作用，当做下一时刻的已知条件完成递推。
在这个过程中，可以看出卡尔曼滤波器在递推的过程中根据最小协方差，估算出最优的倾角值。我们只需要上一刻的最优值，系统偏差，预测偏差，就能进行推进行此刻最优值的计算，不会在运算上对系统造成很大负担。

Claims (2)

1.一种基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的方法，其特征在于，一种基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的方法具体是按照以下步骤进行的：
步骤一、高速旋转电机带动半导体激光器旋转，扫描以高速旋转电机为中心，直径为1～5米范围内的线阵CCD，在线阵CCD感光器件上留下不同高度的光点，通过线阵CCD确定线阵CCD感光器件上光点的成像中心坐标，得到线阵CCD感光器件上光点的高度数据；
其中，所述高速为大于等于10000r/min；
步骤二、选取相邻的3个线阵CCD感光器件上光点的高度数据记为线阵CCD上3个点的空间坐标，即A(x1，y1，z1)，B(x2，y2，z2)，C(x3，y3，z3)，x1为A点在x轴上的坐标，y1为A点在y轴上的坐标，z1为A点在z轴上的坐标，x2为B点在x轴上的坐标，y2为B点在y轴上的坐标，z2为B点在z轴上的坐标，x3为C点在x轴上的坐标，y3为C点在y轴上的坐标，z3为C点在z轴上的坐标；
以A(x1，y1，z1)，B(x2，y2，z2)，C(x3，y3，z3)三点确定一个平面记为平面ABC，根据三点坐标可求得平面ABC的法线方向矢量(a,b,c)，进而可求得该平面与水平面的夹角θ；
步骤三、采用卡尔曼滤波对夹角θ进行递推校正，得出t时刻的实际倾角θ(t|t)；具体过程为：
建立一个离散的过程控制动态系统，设t时刻的过程控制动态系统的状态空间模型为：
θ(t)＝aθ(t-1)+bU(t)+W(t) (1)
t时刻过程控制动态系统的测量值为：
θz(t)＝Hθ(t)+V(t) (2)
式中，θ(t-1)为t-1时刻的过程控制动态系统状态，t为整数；
θ(t)为t时刻的过程控制动态系统状态；
U(t)为t时刻的控制量；
a为线阵CCD水平测量系统的第一个参数；
b为线阵CCD水平测量系统的第二个参数；
θz(t)为t时刻过程控制动态系统的测量值；W(t)为t时刻过程控制动态系统噪声，协方差阵为Q；
V(t)为t时刻过程控制动态观测噪声，协方差阵为R；
H为测量过程控制动态系统的参数；
利用过程控制动态系统的状态空间模型(1)预测t时刻的值，具体过程为：
根据t-1时刻的过程控制动态系统状态预测t时刻的值，见公式(3)：
θ(t|t-1)＝aθ(t-1|t-1)+bU(t) (3)
式中，θ(t|t-1)为根据t-1时刻预测的t时刻的预测值；θ(t-1|t-1)为t-1时刻的状态最优的结果；
U(t)为t时刻的控制量；
若只有过程控制动态系统的控制量，则U(t)值为零；
用P(t|t-1)表示θ(t|t-1)对应的协方差
P(t|t-1)＝aP(t-1|t-1)aT+Q (4)
式中，P(t-1|t-1)为θ(t-1|t-1)对应的协方差；
aT为a的转置矩阵；
Q为过程控制动态系统过程的协方差；
根据公式(2)和公式(3)求出t时刻的实际倾角θ(t|t)，见式(5)
θ(t|t)＝θ(t|t-1)+Kg(t)[θz(t)-Hθ(t|t-1)] (5)
式中，Kg(t)为卡尔曼增益；
Kg(t)＝P(t|t-1)HT/[HP(t|t-1)HT+R] (6)
式中，HT为H的转置矩阵，R为噪声的协方差矩阵；
通过公式(5)，(6)得出了t时刻的实际倾角θ(t|t)，更新t时刻的实际倾角θ(t|t)的协方差，
P(t|t)＝[In-Kg(t)H]P(t|t-1) (7)
设线阵CCD水平测量系统的第一个参数a＝1；线阵CCD水平测量系统的第二个参数b＝1，控制量U(t)＝ωtΔt，将a＝1,b＝1带入公式(3)、(4)既得公式(8)、(9)：
θ(t|t-1)＝θ(t-1|t-1)+ωtΔt (8)
P(t|t-1)＝P(t-1|t-1)+Q (9)
式中，ωt为二次平台转速；
P(t|t-1)为θ(t|t-1)对应的协方差；
P(t-1|t-1)为θ(t-1|t-1)对应的协方差；
取H值为1，并结合式(8)和式(9)，式(5)(6)(7)改为：
θ(t|t)＝θ(t|t-1)+Kg(t)[θz(t)-θ(t|t-1)] (10)
Kg(t)＝P(t|t-1)/[P(t|t-1)+R] (11)
P(t|t)＝[In-Kg(t)]P(t|t-1) (12)
式中，θ(t|t)为t时刻的实际倾角，Kg(t)为卡尔曼增益，θz(t)为t时刻过程控制动态系统的测量值，R为V(t)的协方差阵，P(t|t)为θ(t|t)对应的协方差，In为单位矩阵。
2.根据权利要求1所述一种基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的方法，其特征在于，所述步骤二中选取相邻的3个线阵CCD感光器件上光点的高度数据记为线阵CCD上3个点的空间坐标，即A(x1，y1，z1)，B(x2，y2，z2)，C(x3，y3，z3)，x1为A点在x轴上的坐标，y1为A点在y轴上的坐标，z1为A点在z轴上的坐标，x2为B点在x轴上的坐标，y2为B点在y轴上的坐标，z2为B点在z轴上的坐标，x3为C点在x轴上的坐标，y3为C点在y轴上的坐标，z3为C点在z轴上的坐标；以A(x1，y1，z1)，B(x2，y2，z2)，C(x3，y3，z3)三点确定一个平面记为平面ABC，根据三点坐标可求得平面ABC的法线方向矢量(a,b,c)，进而可求得该平面与水平面的夹角θ；具体过程为：
设分别在相邻的三个线阵CCD上取点A(x1,y1,z1)，点B(x2,y2,z2)和点C(x3,y3,z3)，三点确定一个平面记为平面ABC，根据三点坐标求得平面ABC的法线方向矢量(a,b,c)，
<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <mover> <mi>n</mi> <mo>&amp;RightArrow;</mo> </mover> <mo>=</mo> <mrow> <mo>(</mo> <mi>a</mi> <mo>,</mo> <mi>b</mi> <mo>,</mo> <mi>c</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mo>&amp;lsqb;</mo> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>,</mo> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>,</mo> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> <mo>&amp;times;</mo> <mo>&amp;lsqb;</mo> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>,</mo> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>,</mo> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mo>&amp;lsqb;</mo> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> <mi>i</mi> <mo>-</mo> <mo>&amp;lsqb;</mo> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> <mi>j</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>+</mo> <mo>&amp;lsqb;</mo> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> <mi>k</mi> </mrow> </mtd> </mtr> </mtable> </mfenced>
式中，i为x方向的单位法向量，k为z方向的单位法向量，j为y方向的单位法向量；
设水平面为XOY平面，XOY平面法向量为垂直于XOY平面的向量，即XOY平面法向量的xy方向分量为0，取z方向分量为1，即(0,0,1)；
得出平面ABC与X0Y平面的夹角的余弦为：
<mrow> <mi>cos</mi> <mi>&amp;theta;</mi> <mo>=</mo> <mfrac> <mrow> <mo>(</mo> <mi>a</mi> <mo>,</mo> <mi>b</mi> <mo>,</mo> <mi>c</mi> <mo>)</mo> <mo>(</mo> <mn>0</mn> <mo>,</mo> <mn>0</mn> <mo>,</mo> <mn>1</mn> <mo>)</mo> </mrow> <mrow> <mo>|</mo> <mrow> <mo>(</mo> <mi>a</mi> <mo>,</mo> <mi>b</mi> <mo>,</mo> <mi>c</mi> <mo>)</mo> </mrow> <mo>|</mo> </mrow> </mfrac> <mo>=</mo> <mfrac> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>)</mo> <mo>(</mo> <msub> <mi>y</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>)</mo> <mo>-</mo> <mo>(</mo> <msub> <mi>x</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>)</mo> <mo>(</mo> <msub> <mi>y</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mrow> <mo>|</mo> <mrow> <mo>(</mo> <mi>a</mi> <mo>,</mo> <mi>b</mi> <mo>,</mo> <mi>c</mi> <mo>)</mo> </mrow> <mo>|</mo> </mrow> </mfrac> <mo>.</mo> </mrow>
CN201510474959.3A 2015-08-05 2015-08-05 一种基于卡尔曼滤波对二次平台线阵ccd测量倾角进行优化的方法 Active CN105180966B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510474959.3A CN105180966B (zh) 2015-08-05 2015-08-05 一种基于卡尔曼滤波对二次平台线阵ccd测量倾角进行优化的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510474959.3A CN105180966B (zh) 2015-08-05 2015-08-05 一种基于卡尔曼滤波对二次平台线阵ccd测量倾角进行优化的方法

Publications (2)

Publication Number Publication Date
CN105180966A CN105180966A (zh) 2015-12-23
CN105180966B true CN105180966B (zh) 2018-03-30

Family

ID=54903215

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510474959.3A Active CN105180966B (zh) 2015-08-05 2015-08-05 一种基于卡尔曼滤波对二次平台线阵ccd测量倾角进行优化的方法

Country Status (1)

Country Link
CN (1) CN105180966B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109038471B (zh) * 2018-08-08 2024-04-02 刘晓东 一种基于曲线拟合的智能开关控制器快速跳闸算法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103335632A (zh) * 2013-06-14 2013-10-02 哈尔滨工业大学 一种高精度高频响的平台倾角测量装置及采用该装置测量平台倾角的方法
CN103954267A (zh) * 2014-05-14 2014-07-30 哈尔滨工业大学 基于线阵ccd的二次平台水平度测量系统及方法
CN103983247A (zh) * 2014-05-16 2014-08-13 哈尔滨工业大学 基于二次平台线阵ccd的倾角测量方法
CN103983189A (zh) * 2014-05-16 2014-08-13 哈尔滨工业大学 一种基于二次平台线阵ccd的水平位置测量方法
CN104006787A (zh) * 2014-05-01 2014-08-27 哈尔滨工业大学 空间飞行器姿态运动模拟平台高精度姿态确定方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
ITMO20010110A1 (it) * 2001-05-29 2002-11-29 G S Srl Unipersonale Procedimento per la calibrazione di un sensore per la misura dell'angolo di inclinazione di una ruota di un autoveicolo

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103335632A (zh) * 2013-06-14 2013-10-02 哈尔滨工业大学 一种高精度高频响的平台倾角测量装置及采用该装置测量平台倾角的方法
CN104006787A (zh) * 2014-05-01 2014-08-27 哈尔滨工业大学 空间飞行器姿态运动模拟平台高精度姿态确定方法
CN103954267A (zh) * 2014-05-14 2014-07-30 哈尔滨工业大学 基于线阵ccd的二次平台水平度测量系统及方法
CN103983247A (zh) * 2014-05-16 2014-08-13 哈尔滨工业大学 基于二次平台线阵ccd的倾角测量方法
CN103983189A (zh) * 2014-05-16 2014-08-13 哈尔滨工业大学 一种基于二次平台线阵ccd的水平位置测量方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
卡尔曼滤波在动态倾角传感器测量中的应用研究;王丹;《万方数据库》;20150401;第24-29页 *
卡尔曼滤波在动态测量系统中的应用;曾凡祥，李勤英;《北京测绘》;20141231(第3期);第47-51页 *
平台倾角虚拟测量技术研究;黄安贻等;《机床与液压》;20061231(第12期);第182-184页 *

Also Published As

Publication number Publication date
CN105180966A (zh) 2015-12-23

Similar Documents

Publication Publication Date Title
CN104006787B (zh) 空间飞行器姿态运动模拟平台高精度姿态确定方法
CN111099045B (zh) 双超卫星动力学与控制气浮平台全物理仿真方法
CN103234512B (zh) 三轴气浮台高精度姿态角度及角速度测量装置
CN103424114B (zh) 一种视觉导航/惯性导航的全组合方法
CN106501783B (zh) 一种交会对接微波雷达测角性能系统误差标定系统及方法
CN110940354B (zh) 一种光电跟踪系统捷联惯导安装姿态的标定方法
CN104990533B (zh) 卫星地面物理仿真系统超高精度姿态测量方法及装置
CN106502277B (zh) 基于跟踪技术的三轴气浮台超高精度测量装置及方法
CN105929836B (zh) 用于四旋翼飞行器的控制方法
CN103868648B (zh) 三轴气浮仿真实验平台的质心测量方法
CN103983247B (zh) 基于二次平台线阵ccd的倾角测量方法
CN103983189B (zh) 一种基于二次平台线阵ccd的水平位置测量方法
CN102879832A (zh) 用于地磁要素测量系统的非对准误差校正方法
US11226203B2 (en) Low cost INS
CN110285816A (zh) 一种小卫星星上设备高精度姿态测量系统及方法
US10297168B1 (en) Dynamically tilting flat table to impart a time-varying gravity-induced acceleration on a floating spacecraft simulator
CN105180940B (zh) 一种基于wMPS系统的室内目标天文坐标的确定方法
CN105737848B (zh) 一种系统级星敏感器观星系统及观星方法
CN105180966B (zh) 一种基于卡尔曼滤波对二次平台线阵ccd测量倾角进行优化的方法
CN107764268A (zh) 一种机载分布式pos传递对准的方法和装置
CN105627982A (zh) 一种远程运载器倾斜瞄准方法
CN111102918B (zh) 一种立方镜坐标系的自动化测量系统
CN113465570B (zh) 一种基于高精度imu的气浮台初始对准方法及系统
CN112611379B (zh) 一种惯导稳定平台及其标校、安装方法
CN114659709A (zh) 一种用于大型带翼航天飞行器的三维质心测量方法

Legal Events

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