A new method for double IRST cooperative passive detection and positioning for maneuvering target
红外搜索跟踪系统(Infrared Search and Track System, IRST)是一种采用被动方式工作的成像探测设备,具有隐蔽性好、不怕电磁干扰、测量精度高、低空探测性能好等多种优点。
红外搜索跟踪系统只能探测目标的角位置量,不能直接探测目标的距离。在现代空战中,无论是态势评估和瞄准攻击,都需要目标距离和速度信息,因此使用红外搜索跟踪系统探测、跟踪目标必须解决目标距离和速度的估计问题。
为了获得目标的距离信息,可采用的方法包括激光测距、单机单波段被动测距、单机双波段被动测距、双机或多机协同探测被动定位等。激光测距模式是IRST在对目标进行搜索、截获、跟踪后,对目标发射激光脉冲,然后从接收到的回波信号中提取目标信息,从而使红外搜索跟踪系统具有测角与测距的能力。
激光测距模式的优点是可靠稳定、测距精度高,缺点是受现有激光器件的制约,测距能力无法满足中远距火力攻击的需求。单机单波段被动测距即单机运动被动目标定位,其利用连续多个时刻测量的目标角度和相应的载机位置信息,结合空间几何关系建立单机运动被动目标定位方程从而完成求解。其优点是仅依靠载机采用一定策略的机动运动并完成对目标的角度测量即可实现被动测距,实现方式简单,但缺点也显而易见,收敛时间较长,收敛精度差且受角度测量精度、载机机动方式等因素影响大。
单机双波段被动测距方法即双波段IRST通过获取目标在2个波段的目标辐射信息,从中提取并建立与目标运动状态间的关系,采用非线性滤波方法完成对目标的测距。该方法的优点为测距理论精度较单机运动被动定位方式高,理论收敛时间更短,缺点为2个波段的目标辐射信息与大气透过率、目标距离间建模复杂,实际测量误差受天气、温度等影响难以控制。双机或多机协同探测被动定位即利用2架或多架飞机的相对位置和各自的IRST观测的目标角位置,根据交叉定位原理,采用一定的非线性滤波方法实现了对目标的定位,由于该方法工程上较易实现,且稳定性好、精度高,在实际中得到了广泛的应用。
双机协同探测被动定位的相对优势包括:①距离定位精度比单机被动定位精度高得多;②对红外搜索跟踪系统的测角精度和载机姿态的测量精度要求不高,角度空间测量误差可允许到2 mrad左右,工程上较易实现;③收敛时间短,双机定位由于不受可观测性的限制,其位置估计和速度估计的收敛速度较快。在实际工程应用中,双机编队飞机对匀速运动目标的定位往往能达到较为满意的效果,但对机动目标的定位精度往往效果不理想。究其原因:①采用笛卡尔直角坐标系的双机协同定位观测方程中,观测量与目标状态间存在着较强的非线性关系,选择不当的滤波方法将导致滤波器发散;②目标运动建模的准确性直接影响滤波器的性能,尤其是针对机动目标,选择一个能够适应目标机动运动的模型显得尤为必要。
机动目标跟踪包含了系统建模和状态估计2个问题,其中交互多模型(IMM)方法被广泛认为机动目标跟踪领域目标运动建模最有效的方法之一,但其在工程中使用时存在2个缺陷:一是模型集太少时,无法保证能覆盖目标的实际运动,将会导致跟踪性能下降;二是模型集密集时不仅会增加计算的复杂度,同时还会引起模型间的不必要竞争,导致滤波性能下降。
为了达到IMM方法模型集既能尽可能覆盖目标运动,又能兼顾计算量,近年来国内外学者提出了多种自适应交互多模型算法[1-7]。文献[8]提出了一种基于自适应转弯模型集合的IMM方法,中心滤波器的转弯率根据估计的目标速度和目标加速度计算得到,另外两个邻域滤波器的转弯率在中心滤波器的左右对称分布。该方法计算的转弯率精度较差,直接导致了该方法的模型适应性未能满足机动目标跟踪的要求。文献[9]在文献[8]的基础上提出了一种两层IMM跟踪方法,内层由“当前”统计模型和匀速模型组成,针对目标的速度方向角进行了滤波,从而得到了转弯角速度,作为外层的中心角速度;外层的角速度集合由中心角速度和对称角速度(交互输出的角速度与速度比)组成。该方法较文献[8]的机动目标适应性明显提高,但所采用的两层IMM跟踪方法计算量较大,影响了其在工程中的应用。由于匀速转弯模型、匀速模型、匀加速模型均可以看作是曲线模型的特例,可以说,曲线模型涵盖了目标运动的大部分模式,具有很强的模型适应性[10-12]。本文以自适应曲线模型作为主要研究对象,同时研究其在IRST双机协同被动探测定位中的应用情况。
1 系统建模
假设目标的状态方程和观测方程分别是
$
{\mathit{\boldsymbol{X}}_k} = f\left( {{\mathit{\boldsymbol{X}}_{k - 1}}} \right) + {\mathit{\boldsymbol{W}}_k}
$
|
(1)
|
|
$
{\mathit{\boldsymbol{Z}}_k} = h\left( {{\mathit{\boldsymbol{X}}_k}} \right) + {\mathit{\boldsymbol{V}}_k}
$
|
(2)
|
|
式中:$\boldsymbol{X}_{k}=\left[\begin{array}{llllll}x_{\mathrm{T}} & \dot{x}_{\mathrm{T}} & y_{\mathrm{T}} & \dot{y}_{\mathrm{T}} & z_{\mathrm{T}} & \dot{z}_{\mathrm{T}}\end{array}\right]$为k时刻的目标状态,包括北西天惯性坐标系下目标的北向位置和北向速度、西向位置和西向速度、天向位置和天向速度信息;f(Xk-1)为状态转移函数;h(Xk)为观测函数; Wk和Vk分别为状态噪声和观测噪声,且相互独立。
假设载机1的北西天位置坐标为[xz1, yz1, zz1],载机2的北西天位置坐标为[xz2, yz2, zz2],载机1的IRST(IRST1)和载机2的IRST(IRST2)对目标的方位角、俯仰角观测角度构成观测量,Zk=[α1 β1 α2 β2]。并满足:
$
{\alpha _i} = \arctan \frac{{{y_{\rm{T}}} - y_z^i}}{{{x_{\rm{T}}} - x_z^i}}
$
|
(3)
|
|
$
{\beta _i} = \arctan \frac{{{z_{\rm{T}}} - z_z^i}}{{\sqrt {{{\left( {{x_{\rm{T}}} - x_z^i} \right)}^2} + {{\left( {{y_{\rm{T}}} - y_z^i} \right)}^2}} }}
$
|
(4)
|
|
2 曲线模型状态方程的建立
根据第1节中建立的状态空间描述可知,观测方程中,状态变量与观测量之间存在较强的非线性关系。由于双机协同被动定位方式中,并无目标的测距量测信息,仅依靠目标的角度测量并采用交叉定位方法,因此,载机的位置测量误差以及对目标的角度测量误差都将不同程度上影响最终的双机定位精度。假设双机IRST探测目标角度的误差均为Δβmax,对于IRST1和IRST2来说,目标处于观测角β1、β2为中心,±Δβmax的扇形区域范围内,如图 1所示[13]。
在实际工程应用中,IRST跟踪近距机动目标时,为了能够稳定跟踪目标且使目标维持在视场中心,常常需要载机同时做机动,调整好两机相对态势。载机机动将会影响IRST对目标的角度观测精度,因此,笼统地说,对于机动目标跟踪来说,机动量越大,对应的定位模糊区就越大,定位精度将越低。双机协同被动探测机动目标定位的难点除了定位模糊区问题外,还存在着机动目标建模难度大的问题。在目标运动建模中,为了提高对目标运动的估计精度,本文将采用自适应曲线模型。
不失一般地,图 2中假设目标在二维平面内作转弯运动,在曲线模型中切向加速度和法向加速度分别记为αt和αn,转弯半径为r,转弯角速度为ω(t),转弯角加速度为αω(t),切向速度为v(t),方向角为ϕ(t)。方向角帧间变化量记作Δϕ,转弯角度变化量记作Δθ,可知Δϕ=Δθ,根据圆周运动线运动与角运动的关系,有
$
v(t) = r\omega (t)
$
|
(5)
|
|
$
{\alpha _{\rm{t}}} = \frac{{{\rm{d}}v(t)}}{{{\rm{d}}t}} = r{\alpha _\omega }(t)
$
|
(6)
|
|
$
{\alpha _{\rm{n}}} = r\omega {(t)^2} = v(t)\omega (t)
$
|
(7)
|
|
$
\dot x(t) = v(t)\sin \phi (t)
$
|
(8)
|
|
$
\dot y(t) = v(t)\cos \phi (t)
$
|
(9)
|
|
根据式(5)~式(9),文献[8-10]推导了状态方程的离散形式:
$
\begin{array}{l}
\left[ {\begin{array}{*{20}{l}}
{x(k + 1)}\\
{\dot x(k + 1)}\\
{y(k + 1)}\\
{\dot y(k + 1)}
\end{array}} \right] = \left[ {\begin{array}{*{20}{l}}
1&{\frac{{\sin \left( {T{\omega _k}} \right)}}{{{\omega _k}}}}&0&{\cos \left( {T{\omega _k}} \right)}\\
0&{\frac{{\cos \left( {T{\omega _k}} \right)}}{{{\omega _k}}}}&0&{ - \sin \left( {T{\omega _k}} \right)}\\
0&{\frac{{1 - \cos \left( {T{\omega _k}} \right)}}{{{\omega _k}}}}&0&{\sin \left( {T{\omega _k}} \right)}\\
0&{\frac{{\sin \left( {T{\omega _k}} \right)}}{{{\omega _k}}}}&0&{\cos \left( {T{\omega _k}} \right)}
\end{array}} \right]\left[ {\begin{array}{*{20}{l}}
{x(k)}\\
{x(k)}\\
{y(k)}\\
{\dot y(k)}
\end{array}} \right] + \left[ {\begin{array}{*{20}{c}}
{\frac{{T\cos {\phi _k}}}{{{\omega _k}}} + \frac{{\sin {\phi _k}}}{{\omega _k^2}} - \frac{{\sin \left( {{\phi _k} + {T_{{\omega _k}}}} \right)}}{{\omega _k^2}}}\\
{\frac{{\cos {\phi _k}}}{{{\omega _k}}} - \frac{{\cos \left( {{\phi _k} + T{\omega _k}} \right)}}{{{\omega _k}}}}\\
{\frac{{\cos {\phi _k}}}{{\omega _k^2}} - \frac{{T\sin {\phi _k}}}{{{\omega _k}}} - \frac{{\cos \left( {{\phi _k} + T{\omega _k}} \right)}}{{\omega _k^2}}}\\
{\frac{{\sin \left( {{\phi _k} + T{\omega _k}} \right)}}{{{\omega _k}}} - \frac{{\sin {\phi _k}}}{{{\omega _k}}}}
\end{array}} \right] \cdot \\
{\alpha _{\rm{t}}}\left( k \right) + \mathit{\boldsymbol{ \boldsymbol{\varGamma} W}}\left( k \right)
\end{array}
$
|
(10)
|
|
式中:T为采样周期;ωk为k时刻的转弯角速度;αt(k)为k时刻切向加速度;ϕk为k时刻方向角;Γ为噪声转移矩阵;W(k)为过程噪声。
式(10)描述的目标运动曲线模型包含了丰富的目标运动模式,覆盖了目标运动的大部分模式,对于跟踪机动目标具有很强的模型适应性。式(10)中目标的转弯角速度和切向加速度未知,因而问题的关键是通过采用一定的方法实时估计出每个采样时刻的转弯角速度和切向加速度,便可获得与目标实际运动模式相匹配的运动模型,从而实现对机动目标的跟踪。
对于转弯运动来说,
$
\theta \left( {k + 1} \right) = \theta \left( k \right) + T\omega \left( k \right) + \frac{1}{2}{\alpha _\omega }{T^2}
$
|
(11)
|
|
$
\omega \left( {k + 1} \right) = \omega \left( k \right) + {\alpha _\omega }T \cong \omega \left( k \right) + \frac{{\Delta \theta }}{{v\left( k \right)}}{\alpha _{\rm{t}}}\left( k \right)
$
|
(12)
|
|
一个采样周期内目标转过的角度记为Δθ,方向角记为Δϕ。由于$\phi_{k}=\arctan \frac{\dot{x}(k)}{\dot{y}(k)}$,
$
\begin{array}{l}
\omega \left( {k + 1} \right) = \omega \left( k \right) + \\
\;\;\;\;\;\;\frac{{\arctan \frac{{\dot x\left( k \right)}}{{\dot y\left( k \right)}} - \arctan \frac{{\dot x\left( {k - 1} \right)}}{{\dot y\left( {k - 1} \right)}}}}{{\sqrt {{{\dot x}^2}\left( k \right) + \dot y2\left( k \right)} }}{\alpha _{\rm{t}}}\left( k \right)
\end{array}
$
|
(13)
|
|
文献[10]通过状态扩维法,将转弯角速度作为状态进行了估计,式(10)的状态方程可转化为
$
\begin{array}{l}
\left[ {\begin{array}{*{20}{l}}
{x(k + 1)}\\
{\dot x(k + 1)}\\
{y(k + 1)}\\
{\dot y(k + 1)}\\
{\omega (k + 1)}
\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}
1&{\frac{{\sin \left( {T{\omega _k}} \right)}}{{{\omega _k}}}}&0&{\frac{{\cos \left( {T{\omega _k}} \right) - 1}}{{{\omega _k}}}}&0\\
0&{\cos \left( {T{\omega _k}} \right)}&0&{ - \sin \left( {T{\omega _k}} \right)}&0\\
0&{\frac{{1 - \cos \left( {T{\omega _k}} \right)}}{{{\omega _k}}}}&1&{\frac{{\sin \left( {T{\omega _k}} \right)}}{{{\omega _k}}}}&0\\
0&{\sin \left( {T{\omega _k}} \right)}&0&{\cos \left( {T{\omega _k}} \right)}&0\\
0&0&0&0&1
\end{array}} \right]\left[ {\begin{array}{*{20}{l}}
{x(k)}\\
{\dot x(k)}\\
{y(k)}\\
{\dot y(k)}\\
{\omega (k)}
\end{array}} \right] + \\
\left[ {\begin{array}{*{20}{c}}
{\frac{{T\cos {\phi _k}}}{{{\omega _k}}} + \frac{{\sin {\phi _k}}}{{\omega _k^2}} - \frac{{\sin \left( {{\phi _k} + T{\omega _k}} \right)}}{{\omega _k^2}}}\\
{\frac{{\cos {\phi _k}}}{{{\omega _k}}} - \frac{{\cos \left( {{\phi _k} + T{\omega _k}} \right)}}{{{\omega _k}}}}\\
{\frac{{\cos {\phi _k}}}{{\omega _k^2}} - \frac{{T\sin {\phi _k}}}{{{\omega _k}}} - \frac{{\cos \left( {{\phi _k} + T{\omega _k}} \right)}}{{\omega _k^2}}}\\
{\frac{{\sin \left( {{\phi _k} + T{\omega _k}} \right)}}{{{\omega _k}}} - \frac{{\sin {\phi _k}}}{{{\omega _k}}}}\\
\eta
\end{array}} \right]{\alpha _{\rm{t}}}\left( k \right) + \mathit{\boldsymbol{ \boldsymbol{\varGamma} W}}\left( k \right)
\end{array}
$
|
(14)
|
|
式中:
$
\eta = \frac{{\arctan \frac{{\dot x(k)}}{{\dot y(k)}} - \arctan \frac{{\dot x(k - 1)}}{{\dot y(k - 1)}}}}{{\sqrt {{{\dot x}^2}(k) + \dot y2(k)} }}
$
|
$
\mathit{\boldsymbol{ \boldsymbol{\varGamma} }} = \left[ {\begin{array}{*{20}{c}}
{\frac{{{T^2}}}{2}}&0\\
T&0\\
0&{\frac{{{T^2}}}{2}}\\
0&T\\
0&0
\end{array}} \right]
$
|
式中:转弯角速度ω(k)是在滤波过程中作为状态向量的一个分量进行估计的,因此,实现了对转弯角速度ω(k)的自适应调整。但切向加速度αt仍然未知。
文献[10]中通过对切向加速度αt采用固定的模型集,提出了一种基于曲线模型的模型集半自适应IMM方法,该方法存在的明显缺陷为模型集的设置需要覆盖实际切向加速度的分布范围,模型适应性相对有限。文献[9, 11]分别提出通过内嵌交互多模型估计转弯角速度的方法,其利用上一时刻的目标速度的估计值计算出当前时刻的方向角并作为转弯角速度的伪量测量,其在对切向加速度的估计中运用到了目标的加速度,然而所建立的转弯模型状态方程中并不包含加速度项,其采用了帧间插分的方法,然而插分误差极大影响了切向加速度的估计精度,从而导致自适应曲线模型的精度下降。
另外,文献[9, 14]中定义的方向角ϕk的连续区间为[0, 2π],当目标运动在第1象限和第2象限之间时存在方向角计算跳变问题。文献[11]在文献[9]基础上对方向角进行了重新定义,其定义了一个计数项[n(k)],当目标的方向角从第2象限过渡到第1象限时计数项加1,当从第1象限过渡到第2象限时计数项减1,从而实现方向角变化范围的扩展。然而该方法未能考虑到方向角在对角象限间的变化,如从第1象限变化到第3象限,另外,该方法在帧间x速度方向发生变化时,采用了简化处理方式:利用前一帧方向角叠加转弯角速度的方法作为当帧的方向角。因此,该方法的方向角计算精度有待提高,同时也并非实现了方向角(-∞, ∞)的扩展。
3 一种新的自适应曲线模型滤波方法
3.1 自适应曲线模型建模
针对本文第2节分析的文献[9, 11]存在的目标加速度估计不准导致的切向加速度误差大问题,本文在文献[10]曲线模型状态扩维的基础上,进一步对目标状态进行扩维,增加加速度维$\ddot{x}(k)$和$\ddot{y}(k)$。考虑到IRST观测目标运动的典型场景,假设目标在x-y平面作曲线运动,在z平面作匀速直线运动,则状态矢量可表示为
$
{\mathit{\boldsymbol{X}}_k} = {\left[ {\begin{array}{*{20}{l}}
{{x_{\rm{T}}}}&{{{\dot x}_{\rm{T}}}}&{{{\ddot x}_{\rm{T}}}}&{{y_{\rm{T}}}}&{{{\dot y}_{\rm{T}}}}&{{{\ddot y}_{\rm{T}}}}&{{z_{\rm{T}}}}&{{{\dot z}_{\rm{T}}}}&{{{\ddot z}_{\rm{T}}}}&\omega
\end{array}} \right]^{\rm{T}}}
$
|
本文推导了上述状态变量的状态转移矩阵,具体推导过程见附录A。
$
\mathit{\boldsymbol{F}}(k) = \left[ {\begin{array}{*{20}{c}}
{{f_{11}}}& \cdots &{{f_{110}}}\\
\vdots & \vdots & \vdots \\
{{f_{101}}}& \cdots &{{f_{1010}}}
\end{array}} \right]
$
|
(15)
|
|
式中:
$
{f_{11}} = 1,{f_{12}} = \frac{{\sin \left( {T{\omega _k}} \right)}}{{{\omega _k}}}
$
|
$
{f_{15}} = \frac{{\cos \left( {T{\omega _k}} \right) - 1}}{{{\omega _k}}},{f_{22}} = \cos \left( {T{\omega _k}} \right)
$
|
$
{f_{25}} = - \sin \left( {T{\omega _k}} \right),{f_{32}} = - {\omega _k} \cdot \sin \left( {T{\omega _k}} \right)
$
|
$
{f_{33}} = 1,{f_{35}} = - {\omega _k}\left( {\cos \left( {T{\omega _k}} \right) - 1} \right)
$
|
$
{f_{42}} = - \frac{{\cos \left( {T{\omega _k}} \right) - 1}}{{{\omega _k}}},{f_{44}} = 1
$
|
$
{f_{45}} = \frac{{\sin \left( {T{\omega _k}} \right)}}{{{\omega _k}}},{f_{52}} = \sin \left( {T{\omega _k}} \right)
$
|
$
{f_{55}} = \cos \left( {T{\omega _k}} \right),{f_{62}} = {\omega _k}\left( {\cos \left( {T{\omega _k}} \right) - 1} \right)
$
|
$
{f_{65}} = - {\omega _k}\sin \left( {T{\omega _k}} \right),{f_{66}} = 1
$
|
$
{f_{77}} = 1,{f_{77}} = 1,{f_{78}} = T
$
|
$
{f_{88}} = 1,{f_{1010}} = 1
$
|
其他元素为0。
扩维后的噪声转移矩阵记作
$
\mathit{\boldsymbol{ \boldsymbol{\varGamma} }} = \left[ {\begin{array}{*{20}{c}}
{0.167{T^3}}&0&0\\
{0.5{T^2}}&0&0\\
T&0&0\\
0&{0.167{T^3}}&0\\
0&{0.5{T^2}}&0\\
0&0&{0.167{T^3}}\\
0&0&{0.5{T^2}}\\
0&0&T\\
0&0&0
\end{array}} \right]
$
|
(16)
|
|
扩维后的切向加速度系数矩阵记作
$
\mathit{\boldsymbol{B}}\left( k \right) = \left[ {\begin{array}{*{20}{c}}
{\frac{{T\cos {\phi _k}}}{{{\omega _k}}} + \frac{{\sin {\phi _k}}}{{\omega _k^2}} - \frac{{\sin \left( {{\phi _k} + T{\omega _k}} \right)}}{{\omega _k^2}}}\\
{\frac{{\cos {\phi _k}}}{{{\omega _k}}} - \frac{{\cos \left( {{\phi _k} + T{\omega _k}} \right)}}{{{\omega _k}}}}\\
0\\
{\frac{{\cos {\phi _k}}}{{\omega _k^2}} - \frac{{T\sin {\phi _k}}}{{{\omega _k}}} - \frac{{\cos \left( {{\phi _k} + T{\omega _k}} \right)}}{{\omega _k^2}}}\\
{\frac{{\sin \left( {{\phi _k} + T{\omega _k}} \right)}}{{{\omega _k}}} - \frac{{\sin {\phi _k}}}{{{\omega _k}}}}\\
0\\
0\\
0\\
0\\
\eta
\end{array}} \right]
$
|
(17)
|
|
于是,式(14)更新为
$
\begin{array}{l}
\left[ {\begin{array}{*{20}{l}}
{x\left( {k + 1} \right)}\\
{\dot x\left( {k + 1} \right)}\\
{\ddot x\left( {k + 1} \right)}\\
{y\left( {k + 1} \right)}\\
{\dot y\left( {k + 1} \right)}\\
{\ddot y\left( {k + 1} \right)}\\
{z\left( {k + 1} \right)}\\
{\dot z\left( {k + 1} \right)}\\
{\ddot z\left( {k + 1} \right)}\\
{\omega \left( {k + 1} \right)}
\end{array}} \right] = \mathit{\boldsymbol{F}}\left( k \right)\left[ {\begin{array}{*{20}{l}}
{x\left( k \right)}\\
{\dot x\left( k \right)}\\
{\ddot x\left( k \right)}\\
{y\left( k \right)}\\
{\dot y\left( k \right)}\\
{\ddot y\left( k \right)}\\
{z\left( k \right)}\\
{\dot z\left( k \right)}\\
{\ddot z\left( k \right)}\\
{\omega \left( k \right)}
\end{array}} \right] + \mathit{\boldsymbol{B}}\left( k \right){\alpha _{\rm{t}}}\left( k \right) + \\
\;\;\;\;\;\;\;\;\mathit{\boldsymbol{ \boldsymbol{\varGamma} W}}\left( t \right)
\end{array}
$
|
(18)
|
|
根据过程噪声协方差的表达式:
$
\begin{array}{l}
\mathit{\boldsymbol{Q}} = \sigma _\omega ^2\int_0^T {\mathit{\boldsymbol{F}}\left[ {T,\omega } \right]{\mathit{\boldsymbol{F}}^{\rm{T}}}\left[ {T,\omega } \right]{\rm{d}}t} = \\
\;\;\;\;\;\;\;\sigma _\omega ^2\left[ {\begin{array}{*{20}{c}}
{{q_{00}}}& \cdots &{{q_{09}}}\\
\vdots & \cdots & \vdots \\
{{q_{90}}}&{}&{{q_{99}}}
\end{array}} \right]
\end{array}
$
|
(19)
|
|
式中:F为扩维后的状态转移矩阵;ω为转弯角速度;σω2为转弯率估计方差。
本文推导了式(19)中过程噪声协方差Q的表达式,具体推导过程见附录B,其元素qij分别为
$
{q_{00}} = \frac{{2{\omega ^5}T + {\omega ^3}T - {\omega ^2}\sin \left( {\omega T} \right)\cos \left( {\omega T} \right)}}{{2{\omega ^5}}} + \frac{{3\omega T - 4\sin \left( {\omega T} \right) + \sin \left( {\omega T} \right)\cos \left( {\omega T} \right)}}{{2{\omega ^5}}}
$
|
$
{q_{01}} = - \frac{{{\omega ^2}\cos {{(\omega T)}^2} + 2\cos (\omega T) - \cos {{(\omega T)}^2} - {\omega ^2} - 1}}{{2{\omega ^4}}}
$
|
$
{q_{02}} = \frac{{{\omega ^3}T - {\omega ^2}\sin (\omega T)\cos (\omega T) - 2\sin (\omega T) + \sin (\omega T)\cos (\omega T) + \omega T}}{{ - 2{\omega ^3}}}
$
|
$
{q_{11}} = \frac{{{\omega ^2}\sin (\omega T)\cos (\omega T) + {\omega ^3}T + \omega T - \sin (\omega T)\cos (\omega T)}}{{2{\omega ^3}}}
$
|
$
{q_{12}} = \frac{{{\omega ^2}\cos {{(\omega T)}^2} - \cos {{(\omega T)}^2} - {\omega ^2} + 1}}{{2{\omega ^2}}},{q_{20}} = {q_{02}},{q_{21}} = {q_{12}}
$
|
$
{q_{22}} = \frac{{{\omega ^3}T - {\omega ^2}\sin (\omega T)\cos (\omega T) + \sin (\omega T)\cos (\omega T) + \omega T}}{{2\omega }}
$
|
$
{q_{33}} = {q_{00}},{q_{34}} = {q_{01}},{q_{35}} = {q_{02}},{q_{43}} = {q_{10}}
$
|
$
{q_{44}} = {q_{11}},{q_{45}} = {q_{12}},{q_{53}} = {q_{20}},{q_{54}} = {q_{21}}
$
|
$
{q_{55}} = {q_{22}},{q_{66}} = T + \frac{{{T^3}}}{3},{q_{67}} = \frac{{{T^2}}}{2}
$
|
$
{q_{76}} = {q_{67}},{q_{77}} = T,{q_{99}} = \delta
$
|
Q阵的其他元素为0。
目标的切向加速度可表示为
$
{\alpha _{\rm{t}}}(k) = {\left[ {\alpha {{(k)}^2} - {\alpha _n}{{(k)}^2}} \right]^{1/2}}
$
|
(20)
|
|
式中:$\alpha(k)=\sqrt{\ddot{x}_{T}^{2}+\ddot{y}_{T}^{2}}$
$
{\alpha _{\rm{n}}}(k) = v(k)\omega (k),v(k) = \sqrt {\dot x_{\rm{T}}^2 + \dot y_{\rm{T}}^2}
$
|
将αt(k)代入式(18)即可确定k时刻曲线模型的状态方程表达式。
3.2 自适应滤波方法
采用笛卡尔直角坐标系的双机协同定位观测方程中,观测量与目标状态间存在着较强的非线性关系,若滤波方法选择不当,易导致滤波器发散。在实际工程应用中,比较了常用的非线性滤波方法,包括扩展卡尔曼滤波器、无迹卡尔曼滤波器和容积卡尔曼滤波器等,其中扩展卡尔曼滤波器的状态估计精度较后者差距明显,无迹卡尔曼滤波器存在着对机动目标状态估计时的非正定问题。总的来说,容积卡尔曼滤波器在状态估计精度和稳定性方面均有较好的表现,因此本文拟采用容积卡尔曼滤波器进行非线性滤波[12]。
在时间更新环节,
1) 因式分解
设k-1时刻协方差矩阵Pk-1|k-1正定,对其进行因式分解得到Sk-1|k-1。
$
{\mathit{\boldsymbol{P}}_{k - 1|k - 1}} = {\mathit{\boldsymbol{S}}_{k - 1|k - 1}}{\left( {{\mathit{\boldsymbol{S}}_{k - 1|k - 1}}} \right)^{\rm{T}}}
$
|
(21)
|
|
2) 容积点估计
$
\mathit{\boldsymbol{x}}_{k - 1|k - 1}^i = {\mathit{\boldsymbol{S}}_{k - 1|k - 1}}{\mathit{\boldsymbol{\xi }}_i} + {{\mathit{\boldsymbol{\hat x}}}_{k - 1|k - 1}}
$
|
(22)
|
|
式中:ξi表示第i个容积点, ξi=$\sqrt{\frac{L}{2}}$[Δ]i(i=1, 2, …, L),[Δ]i∈Rn×1为矩阵[In×n, -In×n]∈Rn×L的第i列元素,In×n为n维单位矩阵;L=2n为容积点的总数,n为状态向量维数。
3) 容积点传播
$
\mathit{\boldsymbol{x}}_{k|k - 1}^{ * ,i} = f\left( {\mathit{\boldsymbol{x}}_{k - 1|k - 1}^i} \right)
$
|
(23)
|
|
4) 一步预测
$
{\mathit{\boldsymbol{x}}_{k|k - 1}} = \sum\limits_{i = 1}^L {\mathit{\boldsymbol{x}}_{k|k - 1}^{ * ,i}} /L
$
|
(24)
|
|
$
\begin{array}{*{20}{c}}
{{\mathit{\boldsymbol{P}}_{k|k - 1}} = \sum\limits_{i = 1}^L {\mathit{\boldsymbol{x}}_{k|k - 1}^{ * ,i}} {{\left( {\mathit{\boldsymbol{x}}_{k|k - 1}^{ * ,i}} \right)}^{\rm{T}}}/L - }\\
{{{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}{{\left( {{{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}} \right)}^{\rm{T}}} + {\mathit{\boldsymbol{Q}}_k}}
\end{array}
$
|
(25)
|
|
在量测更新环节,
1) 因式分解
$
{\mathit{\boldsymbol{P}}_{k|k - 1}} = {\mathit{\boldsymbol{S}}_{k|k - 1}}{\left( {{\mathit{\boldsymbol{S}}_{k|k - 1}}} \right)^{\rm{T}}}
$
|
(26)
|
|
2) 容积点估计
$
\mathit{\boldsymbol{x}}_{k|k - 1}^i = {\mathit{\boldsymbol{S}}_{k|k - 1}}{\mathit{\boldsymbol{\xi }}_i} + {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}
$
|
(27)
|
|
3) 容积点传播
$
\mathit{\boldsymbol{z}}_{k|k - 1}^i = \mathit{\boldsymbol{h}}\left( {\mathit{\boldsymbol{x}}_{k|k - 1}^i} \right)
$
|
(28)
|
|
4) 一步预测
$
{{\mathit{\boldsymbol{\hat z}}}_{k|k - 1}} = \sum\limits_{i = 1}^L {\mathit{\boldsymbol{z}}_{k|k - 1}^i} /L
$
|
(29)
|
|
5) 协方差矩阵估计
新息方差,
$
\mathit{\boldsymbol{P}}_{k|k - 1}^{zz} = \sum\limits_{i = 1}^L {\frac{{\mathit{\boldsymbol{z}}_{k|k - 1}^i{{\left( {\mathit{\boldsymbol{z}}_{k|k - 1}^i} \right)}^{\rm{T}}}}}{L}} - {{\mathit{\boldsymbol{\hat z}}}_{k|k - 1}}{\left( {{{\mathit{\boldsymbol{\hat z}}}_{k|k - 1}}} \right)^{\rm{T}}}
$
|
(30)
|
|
协方差矩阵,
$
\mathit{\boldsymbol{P}}_{k|k - 1}^{xz} = \sum\limits_{i = 1}^L {\frac{{\mathit{\boldsymbol{x}}_{k|k - 1}^i{{\left( {\mathit{\boldsymbol{x}}_{k|k - 1}^i} \right)}^{\rm{T}}}}}{L}} - {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}{\left( {{{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}} \right)^{\rm{T}}}
$
|
(31)
|
|
6) 滤波增益计算
$
{\mathit{\boldsymbol{K}}_k} = \mathit{\boldsymbol{P}}_{k|k - 1}^{xz}{\left( {\mathit{\boldsymbol{P}}_{k|k - 1}^{zz}} \right)^{\rm{T}}}
$
|
(32)
|
|
7) 状态估计
$
{{\mathit{\boldsymbol{\hat x}}}_{k|k}} = {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}} + {\mathit{\boldsymbol{K}}_k}\left( {{\mathit{\boldsymbol{z}}_k} - {{\mathit{\boldsymbol{\hat z}}}_{k|k - 1}}} \right)
$
|
(33)
|
|
8) 估计误差协方差计算
$
{\mathit{\boldsymbol{P}}_{k|k}} = {\mathit{\boldsymbol{P}}_{k|k - 1}} - {\mathit{\boldsymbol{K}}_k}\mathit{\boldsymbol{P}}_{k|k - 1}^{xz}{\left( {{\mathit{\boldsymbol{K}}_k}} \right)^{\rm{T}}}
$
|
(34)
|
|
3.3 方向角模型建立
本文基于反正切函数atan(ẋ(k), ẏ(k))的值域[-π, π],结合4个象限之间的空间关系(如图 3所示),优化设计了方向角的计算方法。
$\phi_{k}=-\arctan \frac{\dot{x}(k)}{\dot{y}(k)}+\Delta \phi$,其中Δϕ表示方向角跨象限时的计算补偿量,记i时刻的补偿量记作
$
\Delta {\phi _i} = \left\{ \begin{array}{l}
- 2{\rm{ \mathsf{ π} }}{n_1}\;\;{Q_4}\;转\;{Q_3},{n_1} + 1\\
2{\rm{ \mathsf{ π} }}{n_2}\;\;\;\;{Q_3}\;转\;{Q_4},{n_2} + 1\\
- {\rm{ \mathsf{ π} }}{n_3}\;\;\;\;{Q_1}\;转\;{Q_3}\;或\;{Q_4}\;转\;{Q_2},{n_3} + 1\\
{\rm{ \mathsf{ π} }}{n_4}\;\;\;\;\;\;{Q_3}\;转\;{Q_1}\;或\;{Q_2}\;转\;{Q_4},{n_4} + 1\\
0\;\;\;\;\;\;\;\;\;\;其他
\end{array} \right.
$
|
(35)
|
|
式中:Qi表示第i象限;ni(i=1, 2, 3, 4)表示计算器。
则当前时刻的补偿量为$\Delta \phi=\sum\limits_{i=0}^{i=k} \Delta \phi_{i}$。
从式(35)可以看出,补偿量发生在速度矢量在第3象限和第4象限之间转移时,或者对角象限转移时。本文采用分类计数器的方法,当每一次a、b、c、d转移发生时,对应的计数器ni加1,最终对其求和作为方向角的补偿量,使目标的方向角由原来的连续区间[-π, π]扩展为(-∞, ∞),确保了方向角估计的平滑性,同时解决了文献[11]存在的问题。
4 仿真实验与分析
假设在北西天坐标系下载机1、载机2的初始位置位于[35 km, 20 km, 6 km]、[45 km, 10 km, 7 km],双机保持匀速编队飞行,速度为[0.7Ma, -0.7Ma, 0]。假设目标的初始状态为[40 km, 50 km, 8 km],初始速度为[0.7Ma, -0.5Ma, 0.005Ma],在1~29 s、49~69 s、89~119 s之间,作匀速直线运动,其他时刻做转弯运动,转弯半径为5 km,过载为5g。其中,在29~49 s以及119 s之后目标左转弯,在69~89 s目标右转弯,双机组网定位态势如图 4所示。双机探测目标的数据采样率为50 ms,角度测量误差标准差为1.8 mrad,载机位置测量误差为50 m,系统噪声方差中,设转弯角速度估计标准差为10 mrad,100次蒙特卡罗。
图 5为本文的方向角估计效果,从中可以看出方向角在4个象限间转移时可平顺过渡,实现了对方向角的准确计算。图 6为本文方法与文献[2, 4]方法对转弯率估计的比较,从图中可以看出,文献[4]方法在转弯率阶跃时的上升时间最短,而本文的超调时间最短。从模型选择上来看,本文将转弯角速度作为状态变量,因而其牺牲了一部分的对转弯率阶跃时响应率,导致上升时间比文献[4]略长。文献[2, 4]由于所建立的转弯模型状态方程中并不包含加速度项,采用了帧间插分的方法求加速度,然而插分误差极大影响了切向加速度的估计精度,从而导致自适应曲线模型的精度下降。图 7表明本文的目标状态估计精度较文献[2, 4]有明显的提高,尤其在目标机动期间尤为明显,这主要得益于本文方法在转弯率估计上的稳态误差较小,从而使本文建立的曲线模型更具有目标机动的适应性。
另外,在工程应用中,由于机载惯性测量器件误差的存在,导致了载机位置测量存在一定的误差。载机位置误差对目标状态估计的影响体现在:根据目标量测,即目标方位角、俯仰角、目标距离,可获得量测一步预测中目标相对于载机的三向位置,在此基础上结合载机的位置数据可获得目标的三向位置量测。因此,载机位置误差可影响目标的三向位置量测精度,进而影响量测一步预测精度及残差精度,从而对目标状态的估计精度产生不利影响。
5 结论
本文针对IRST双机协同被动探测定位中机动目标跟踪定位建模与实际目标运动不匹配的难题,研究了一种新的自适应曲线模型滤波算法,该方法相较于传统方法的创新性体现在:
1) 将转弯率和目标线运动加速度作为状态变量进行了状态扩维,提高了切向加速度的估计精度,从而增强了曲线模型的自适应性。
2) 针对方向角在4个象限间转移时存在波动的问题,基于反正切函数arctan ()的值域,结合四象限空间关系,优化了方向角的设计方法,从而提高了对转弯角速率的估计精度。
本文研究的自适应曲线模型滤波算法较其他机动目标滤波算法的优势在于不需要目标机动的先验信息,覆盖了目标运动的大部分模式,对于跟踪机动目标具有很强的模型适应性。仿真结果也表明本文的方法较传统方法对于机动目标的适应性更好。
附录A
考虑目标在x、y平面作转弯运动。在式(8)、式(9)的基础上,计算得到
$
\ddot x(t) = v\omega \cos \phi (t) = \omega \dot y
$
|
(A1)
|
|
$
\ddot y(t) = - v\omega \sin \phi (t) = - \omega \dot x
$
|
(A2)
|
|
继而可得
$
\dddot x(t) = - v{\omega ^2}\sin \phi (t) = - {\omega ^2}\dot x
$
|
(A3)
|
|
$
\ddot y(t) = - v{\omega ^2}\cos \phi (t) = - {\omega ^2}\dot y
$
|
(A4)
|
|
以此可得到目标转弯运动的连续时间系统状态方程为
$
\begin{gathered}
\left[ {\begin{array}{*{20}{l}}
{\dot x(t)} \\
{\ddot x(t)} \\
{\dddot x(t)} \\
{\dot y(t)} \\
{\ddot y(t)} \\
{\dddot y(t)}
\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}
0&1&0&0&0&0 \\
0&0&0&0&{ - \omega }&0 \\
0&{ - {\omega ^2}}&0&0&0&0 \\
0&0&0&0&1&0 \\
0&\omega &0&0&0&0 \\
0&0&0&0&{ - {\omega ^2}}&0
\end{array}} \right]\left[ {\begin{array}{*{20}{l}}
{x(t)} \\
{\dot x(t)} \\
{\ddot y(t)} \\
{y(t)} \\
{\dot y(t)} \\
{\ddot y(t)}
\end{array}} \right] + \hfill \\
\;\;\;\;\;\;\mathit{\boldsymbol{W}}\left( t \right) \hfill \\
\end{gathered}
$
|
(A5)
|
|
式中:W(t)为系统噪声,一般假定为高斯白噪声,令:
$
\mathit{\boldsymbol{A}} = \left[ {\begin{array}{*{20}{c}}
0&1&0&0&0&0\\
0&0&0&0&{ - \omega }&0\\
0&{ - {\omega ^2}}&0&0&0&0\\
0&0&0&0&1&0\\
0&\omega &0&0&0&0\\
0&0&0&0&{ - {\omega ^2}}&0
\end{array}} \right]
$
|
(A6)
|
|
经拉普拉斯逆变换,可得到转弯模型系统状态矩阵的离散化表达式。
$
s\mathit{\boldsymbol{I}} - \mathit{\boldsymbol{A}} = \left[ {\begin{array}{*{20}{c}}
s&{ - 1}&0&0&0&0\\
0&s&0&0&\omega &0\\
0&{{\omega ^2}}&s&0&0&0\\
0&0&0&s&{ - 1}&0\\
0&{ - \omega }&0&0&s&0\\
0&0&0&0&{{\omega ^2}}&s
\end{array}} \right]
$
|
(A7)
|
|
$
{\left( {s\mathit{\boldsymbol{I}} - \mathit{\boldsymbol{A}}} \right)^{ - 1}} = \left[ {\begin{array}{*{20}{c}}
{\frac{1}{s}}&{\frac{1}{{{s^2} + {\omega ^2}}}}&0&0&{ - \frac{\omega }{{s\left( {{s^2} + {\omega ^2}} \right)}}}&0\\
0&{\frac{s}{{{s^2} + {\omega ^2}}}}&0&0&{\frac{{ - \omega }}{{{s^2} + {\omega ^2}}}}&0\\
0&{\frac{{ - {\omega ^2}}}{{{s^2} + {\omega ^2}}}}&{\frac{1}{s}}&0&{\frac{{{\omega ^3}}}{{s\left( {{s^2} + {\omega ^2}} \right)}}}&0\\
0&{\frac{\omega }{{s\left( {{s^2} + {\omega ^2}} \right)}}}&0&{\frac{1}{s}}&{\frac{1}{{{s^2} + {\omega ^2}}}}&0\\
0&{\frac{\omega }{{{s^2} + {\omega ^2}}}}&0&0&{\frac{s}{{{s^2} + {\omega ^2}}}}&0\\
0&{\frac{{ - {\omega ^3}}}{{s\left( {{s^2} + {\omega ^2}} \right)}}}&0&0&{\frac{{ - {\omega ^2}}}{{{s^2} + {\omega ^2}}}}&{\frac{1}{s}}
\end{array}} \right]
$
|
(A8)
|
|
$
{\mathit{\boldsymbol{F}}_k} = {\mathit{\boldsymbol{L}}^{ - 1}}{\left( {s\mathit{\boldsymbol{I}} - \mathit{\boldsymbol{A}}} \right)^{ - 1}} = \left[ {\begin{array}{*{20}{c}}
1&{\frac{{\sin (\omega T)}}{\omega }}&0&0&{ - \frac{{1 - \cos (\omega T)}}{\omega }}&0\\
0&{\cos (\omega T)}&0&0&{ - \sin (\omega T)}&0\\
0&{ - \omega \sin (\omega T)}&1&0&{ - \omega (\cos (\omega T) - 1)}&0\\
0&{\frac{{1 - \cos (\omega T)}}{\omega }}&0&1&{\frac{{\sin (\omega T)}}{\omega }}&0\\
0&{\sin (\omega T)}&0&0&{\cos (\omega T)}&0\\
0&{\omega (\cos (\omega T) - 1)}&0&0&{ - \omega \sin (\omega T)}&1
\end{array}} \right]
$
|
(A9)
|
|
考虑到状态扩展,若目标状态为
$
{\mathit{\boldsymbol{X}}_k} = {\left[ {\begin{array}{*{20}{c}}
{{x_{\rm{T}}}}&{{{\dot x}_{\rm{T}}}}&{{{\ddot x}_{\rm{T}}}}&{{y_{\rm{T}}}}&{{{\dot y}_{\rm{T}}}}&{{{\ddot y}_{\rm{T}}}}&{{z_{\rm{T}}}}&{{{\dot z}_{\rm{T}}}}&{{{\ddot z}_{\rm{T}}}}&\omega
\end{array}} \right]^{\rm{T}}}
$
|
则状态转移矩阵为
$
\mathit{\boldsymbol{F}} = \left[ {\begin{array}{*{20}{c}}
{}&{{\mathit{\boldsymbol{F}}_k}}&{{{\bf{0}}_{6 \times 4}}}&{}&{}\\
{}&1&T&0&0\\
{{{\bf{0}}_{4 \times 6}}}&0&1&0&0\\
{}&0&0&0&1
\end{array}} \right]
$
|
(A10)
|
|
附录B
由式(A10)可获得状态扩维条件下的状态转移矩阵。记
$
\mathit{\boldsymbol{FF}} = \mathit{\boldsymbol{F}}\left[ {T,\omega } \right] \cdot {\mathit{\boldsymbol{F}}^{\rm{T}}}\left[ {T,\omega } \right] = \left[ {\begin{array}{*{20}{c}}
{{q_{00}}}& \cdots &{{q_{09}}}\\
\vdots & \cdots & \vdots \\
{{q_{90}}}& \cdots &{{q_{99}}}
\end{array}} \right]
$
|
(B1)
|
|
$
{q_{00}} = \frac{{{\omega ^2} + 2 - 2\cos \left( {\omega T} \right)}}{{{\omega ^2}}}
$
|
(B2)
|
|
$
{q_{01}} = \frac{{\sin \left( {\omega T} \right)}}{\omega }
$
|
(B3)
|
|
$
{q_{02}} = - 2 + 2\cos \left( {\omega T} \right)
$
|
(B4)
|
|
$
{q_{04}} = \frac{{1 - \cos \left( {\omega T} \right)}}{\omega }
$
|
(B5)
|
|
$
{q_{10}} = \frac{{\sin \left( {\omega T} \right)}}{\omega }
$
|
(B6)
|
|
$
{q_{12}} = - \omega \sin \left( {\omega T} \right)
$
|
(B8)
|
|
$
{q_{13}} = - \frac{{1 - \cos \left( {\omega T} \right)}}{\omega }
$
|
(B9)
|
|
$
{q_{15}} = - \omega \left( {\cos \left( {\omega T} \right) - 1} \right)
$
|
(B10)
|
|
$
{q_{20}} = - 2 + 2\cos \left( {\omega T} \right)
$
|
(B11)
|
|
$
{q_{21}} = - \omega \sin \left( {\omega T} \right)
$
|
(B12)
|
|
$
{q_{22}} = 1 + 2{\omega ^2} - 2{\omega ^2}\cos \left( {\omega T} \right)
$
|
(B13)
|
|
$
{q_{24}} = \omega \left( {\cos \left( {\omega T} \right) - 1} \right)
$
|
(B14)
|
|
$
{q_{31}} = - \frac{{1 - \cos \left( {\omega T} \right)}}{\omega }
$
|
(B15)
|
|
$
{q_{33}} = \frac{{{\omega ^2} + 2 - 2\cos \left( {\omega T} \right)}}{{{\omega ^2}}}
$
|
(B16)
|
|
$
{q_{34}} = \frac{{\sin (\omega T)}}{\omega }
$
|
(B17)
|
|
$
{q_{35}} = - 2 + 2\cos (\omega T)
$
|
(B18)
|
|
$
{q_{40}} = \frac{{1 - \cos (\omega T)}}{\omega }
$
|
(B19)
|
|
$
{q_{42}} = \omega (\cos (\omega T) - 1)
$
|
(B20)
|
|
$
{q_{43}} = \frac{{\sin (\omega T)}}{\omega }
$
|
(B21)
|
|
$
{q_{45}} = - \omega \sin (\omega T)
$
|
(B23)
|
|
$
{q_{51}} = - \omega (\cos (\omega T) - 1)
$
|
(B24)
|
|
$
{q_{53}} = - 2 + 2\cos (\omega T)
$
|
(B25)
|
|
$
{q_{54}} = - \omega \sin (\omega \cdot T)
$
|
(B26)
|
|
$
{q_{55}} = 1 + 2{\omega ^2} - 2{\omega ^2}\cos (\omega T)
$
|
(B27)
|
|
$
{q_{54}} = - \omega \sin (\omega \cdot T)
$
|
(B28)
|
|
$
{q_{66}} = 1 + {T^2}
$
|
(B29)
|
|
$
{q_{66}} = 1 + {T^2}
$
|
(B30)
|
|
则状态噪声协方差为
$
\mathit{\boldsymbol{Q}} = \sigma _\omega ^2\int_0^T {\mathit{\boldsymbol{FF}}{\rm{d}}t}
$
|
(B34)
|
|
$
= \sigma _\omega ^2\left[ {\begin{array}{*{20}{c}}
{{q_{00}}}& \cdots &{{q_{09}}}\\
\vdots & \cdots & \vdots \\
{{q_{90}}}& \cdots &{{q_{99}}}
\end{array}} \right]
$
|
(B35)
|
|
式中:
$
{q_{00}} = \frac{{{\omega ^3}T + 2\omega T - 2\omega \sin \left( {\omega T} \right)}}{{{\omega ^3}}}
$
|
(B36)
|
|
$
{q_{01}} = \frac{{\cos \left( {\omega T} \right)}}{{{\omega ^2}}}
$
|
(B37)
|
|
$
{q_{02}} = \frac{{ - 2\omega T + 2\sin \left( {\omega T} \right)}}{\omega }
$
|
(B38)
|
|
$
{q_{04}} = \frac{{\sin \left( {\omega T} \right) - \omega T}}{{{\omega ^2}}}
$
|
(B39)
|
|
$
{q_{20}} = \frac{{ - \cos \left( {\omega T} \right)}}{{{\omega ^2}}}
$
|
(B40)
|
|
$
{q_{21}} = \cos \left( {\omega T} \right)
$
|
(B41)
|
|
$
{q_{22}} = T + 2{\omega ^2}T - 2\omega \sin \left( {\omega T} \right)
$
|
(B42)
|
|
$
{q_{24}} = \sin \left( {\omega T} \right) - \omega T
$
|
(B43)
|
|
$
{q_{31}} = \frac{{\sin \left( {\omega T} \right) - \omega T}}{{{\omega ^2}}}
$
|
(B44)
|
|
$
{q_{33}} = \frac{{{\omega ^3}T + 2\omega T - 2\omega \sin \left( {\omega T} \right)}}{{{\omega ^3}}}
$
|
(B45)
|
|
$
{q_{34}} = \frac{{ - \cos \left( {\omega T} \right)}}{{{\omega ^2}}}
$
|
(B46)
|
|
$
{q_{35}} = \frac{{ - 2\omega T + 2\sin \left( {\omega T} \right)}}{\omega }
$
|
(B47)
|
|
$
{q_{40}} = \frac{{\sin \left( {\omega T} \right) - \omega T}}{{{\omega ^2}}}
$
|
(B48)
|
|
$
{q_{42}} = \sin \left( {\omega T} \right) - \omega T
$
|
(B49)
|
|
$
{q_{43}} = \frac{{ - \cos \left( {\omega T} \right)}}{{{\omega ^2}}}
$
|
(B50)
|
|
$
{q_{45}} = \cos \left( {\omega T} \right)
$
|
(B52)
|
|
$
{q_{51}} = - \sin \left( {\omega T} \right) + \omega T
$
|
(B53)
|
|
$
{q_{53}} = \frac{{ - 2\omega T + 2\sin \left( {\omega T} \right)}}{\omega }
$
|
(B54)
|
|
$
{q_{54}} = \cos \left( {\omega T} \right)
$
|
(B55)
|
|
$
{q_{55}} = T + 2{\omega ^2}T - 2\omega \sin \left( {\omega T} \right)
$
|
(B56)
|
|
$
{q_{66}} = \frac{{{T^3} + 3T}}{3}
$
|
(B57)
|
|
$
{q_{67}} = \frac{{{T^2}}}{2}
$
|
(B58)
|
|
$
{q_{76}} = \frac{{{T^2}}}{2}
$
|
(B59)
|
|