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

权利要求书

1.一种基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的方法，其特征在于，一种基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的方法具体是按照以下步骤进行的：

步骤一、高速旋转电机带动半导体激光器旋转，扫描以高速旋转电机为中心，直径为1～5米范围内的线阵CCD，在线阵CCD感光器件上留下不同高度的光点，通过线阵CCD确定线阵CCD感光器件上光点的成像中心坐标，得到线阵CCD感光器件上光点的高度数据；

其中，所述高速为大于等于10000r/min；

步骤二、选取相邻的3个线阵CCD感光器件上光点的高度数据记为线阵CCD上3个点的空间坐标，即A(x₁，y₁，z₁)，B(x₂，y₂，z₂)，C(x₃，y₃，z₃)，x₁为A点在x轴上的坐标，y₁为A点在y轴上的坐标，z₁为A点在z轴上的坐标，x₂为B点在x轴上的坐标，y₂为B点在y轴上的坐标，z₂为B点在z轴上的坐标，x₃为C点在x轴上的坐标，y₃为C点在y轴上的坐标，z₃为C点在z轴上的坐标；

以A(x₁，y₁，z₁)，B(x₂，y₂，z₂)，C(x₃，y₃，z₃)三点确定一个平面记为平面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)a^T+Q (4)

式中，P(t-1|t-1)为θ(t-1|t-1)对应的协方差；

a^T为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)H^T/[HP(t|t-1)H^T+R] (6)

式中，H^T为H的转置矩阵，R为噪声的协方差矩阵；

通过公式(5)，(6)得出了t时刻的实际倾角θ(t|t)，更新t时刻的实际倾角θ(t|t)的协方差，

P(t|t)＝[I_n-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)＝[I_n-Kg(t)]P(t|t-1) (12)

式中，θ(t|t)为t时刻的实际倾角，Kg(t)为卡尔曼增益，θ_z(t)为t时刻过程控制动态系统的测量值，R为V(t)的协方差阵，P(t|t)为θ(t|t)对应的协方差，I_n为单位矩阵。

2.根据权利要求1所述一种基于卡尔曼滤波对二次平台线阵CCD测量倾角进行优化的方法，其特征在于，所述步骤二中选取相邻的3个线阵CCD感光器件上光点的高度数据记为线阵CCD上3个点的空间坐标，即A(x₁，y₁，z₁)，B(x₂，y₂，z₂)，C(x₃，y₃，z₃)，x₁为A点在x轴上的坐标，y₁为A点在y轴上的坐标，z₁为A点在z轴上的坐标，x₂为B点在x轴上的坐标，y₂为B点在y轴上的坐标，z₂为B点在z轴上的坐标，x₃为C点在x轴上的坐标，y₃为C点在y轴上的坐标，z₃为C点在z轴上的坐标；以A(x₁，y₁，z₁)，B(x₂，y₂，z₂)，C(x₃，y₃，z₃)三点确定一个平面记为平面ABC，根据三点坐标可求得平面ABC的法线方向矢量(a,b,c)，进而可求得该平面与水平面的夹角θ；具体过程为：

设分别在相邻的三个线阵CCD上取点A(x₁,y₁,z₁)，点B(x₂,y₂,z₂)和点C(x₃,y₃,z₃)，三点确定一个平面记为平面ABC，根据三点坐标求得平面ABC的法线方向矢量(a,b,c)，

n→=(a,b,c)=[(x2-x1),(y2-y1),(z2-z1)]×[(x3-x1),(y3-y1),(z3-z1)]=[(y2-y1)(z3-z1)-(z2-z1)(y3-y1)]i-[(x2-x1)(z3-z1)-(x3-x1)(z2-z1)]j+[(x2-x1)(y3-y1)-(x3-x1)(y2-y1)]k]]>

式中，i为x方向的单位法向量，k为z方向的单位法向量，j为y方向的单位法向量；

设水平面为XOY平面，XOY平面法向量为垂直于XOY平面的向量，即XOY平面法向量的xy方向分量为0，取z方向分量为1，即(0,0,1)；

得出平面ABC与X0Y平面的夹角的余弦为：

cosθ=(a,b,c)(0,0,1)|(a,b,c)|=(x2-x1)(y3-y1)-(x3-x1)(y2-y1)|(a,b,c)|.]]>