[发明专利]一种基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的方法

摘要

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

主权项

一种基于卡尔曼滤波对二次平台线阵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为单位矩阵。