CN106338286A - 一种动基座测量方法 - Google Patents

Abstract

本发明提供了一种动基座测量方法，采用动基座测量系统测量机动目标的位置和姿态，该系统包括定位定向装置、照相机、转台和测距仪；照相机和测距仪安装在转台的台体上，转台和定位定向装置安装在载体的同一安装面上。测量步骤包括：1、在目标上选取跟踪特征点，控制转台转动，使照相机的光轴对准跟踪特征点；记录光轴对准时的转台转动角度；2、测距仪测量载体与目标之间的距离，定位定向装置测量载体的位置和姿态角度；3、根据载体的位置和姿态、载体与目标之间的距离，以及转台转动角度，计算目标位置；4、在目标上选取M个特征点，利用照相机对目标进行成像，根据照片中M个特征点对应的像素点与跟踪特征点对应像素点的位置关系，计算目标姿态。

Description

一种动基座测量方法

技术领域

本发明涉及定位测姿技术领域，特别涉及一种动基座测量方法，用于对机动远距离机动目标进行位置和姿态测量。

背景技术

在现有技术里，可以采用惯性导航方法获取物体的位置和姿态信息，这种测量方法需要将惯性导航装置安装在被测物体上，具有实时提供被测物体的位置和姿态信息，以及独立自主不受外界影响的优点。然而，由于惯性导航装置存在误差随时间发散的缺点，很难保证长时间工作时的精度要求。

光学测量是近年来随着半导体技术和计算机技术的发展新兴出来的测量技术，其具有非接触、测量精度高误差不随时间发散的优点，在位置和姿态测量领域发挥作用。目前，已经出现由陀螺寻北仪、电子水平仪、经纬仪和激光跟踪测量系统组成的光学测量系统。通过陀螺寻北仪、电子水平仪和经纬仪提供基座的位置和姿态基准，通过激光跟踪测量系统跟踪被测物体的运动位置并测量其姿态角。然而，这种测量系统必须安装在固定基座上，使其不易运输和安装，同时限制了测量范围和应用范围。

发明内容

本发明的目的在于克服现有技术的缺点，提供了一种动基座测量方法，可以将测量系统安装在运动载体上，实现了对机动目标的位置和姿态的实时测量，扩大了测量距离。

本发明的上述目的通过以下方案实现：

一种动基座测量方法，用于实时测量目标的位置和姿态，基于动基座测量系统实现，所述系统包括定位定向装置、照相机、转台和测距仪；照相机和测距仪安装在转台的台体上，转台和定位定向装置安装在载体的同一安装面上；其中，转台的台体坐标系o_sx_sy_sz_s的原点o_s位于照相机的镜头中心，o_sx_sy_s平面与台体上表面平行，y_s轴位于照相机光轴方向；载体坐标系o_bx_by_bz_b的原点o_b位于定位定向装置的几何中心；

上述动基座测量方法包括以下步骤：

(1)、在目标上选取跟踪特征点，并控制转台转动，使照相机的光轴对准所述跟踪特征点；记录光轴对准时的转台转动方位角α_n和俯仰角α_w；

(2)、利用测距仪测量载体与目标之间的距离，并利用定位定向装置对载体的位置和姿态角度进行测量；

(3)、根据载体的位置和姿态角度、载体与目标之间的距离，以及转台转动方位角α_n和俯仰角α_w，计算得到目标的位置坐标；

(4)、在目标上选取M个特征点，并利用照相机对目标进行成像，根据成像照片中M个特征点对应的像素点与跟踪特征点对应像素点的位置关系，计算得到目标的姿态角度；M为正整数且M≥3。

上述的动基座测量方法，在步骤(1)中，控制转台转动，使得照相机的光轴对准跟踪特征点，具体实现过程如下：

(1a)、利用照相机对跟踪特征点进行拍照，获得照片；

(1b)、计算照片中跟踪特征点对应像素点在台体坐标系中的俯仰角φ_p和方位角θ_p；

(1c)、控制转台转动，并重复步骤(1a)～(1b)，当所述俯仰角φ_p和方位角θ_p都为零时，则判断照相机的光轴已对准跟踪特征点。

上述的动基座测量方法，在步骤(1b)中，俯仰角φ_p和方位角θ_p的计算公式如下：

${\mathrm{\φ}}_p=arctan\frac{z}{\sqrt{f^2+x^2}},{\mathrm{\θ}}_p=-arctan\frac{x}{f};$

其中：f为照相机的焦距；m_X和m_Y是跟踪特征点对应像素点在照片中的纵向排序序号和横向排序序号；M_X和M_Y分别为照片中纵向像素个数和横向像素个数；Δm是照相机感光元件上单个像素的边长。

上述的动基座测量方法，在步骤(3)中，计算得到的目标位置坐标包括目标在地理坐标系下经度λ_p、纬度φ_p和高度h_p，具体计算公式如下：

$\left[\begin{array}{c}(N+h_p){\mathrm{cos\φ}}_p{\mathrm{cos\λ}}_p\\ (N+h_p){\mathrm{cos\φ}}_p{\mathrm{sin\λ}}_p\\ \[N(1-e^2)+h_p\]{\mathrm{sin\φ}}_p\end{array}\right]=\left[\begin{array}{c}(N+h_o){\mathrm{cos\φ}}_o{\mathrm{cos\λ}}_o\\ (N+h_o){\mathrm{cos\φ}}_o{\mathrm{sin\λ}}_o\\ \[N(1-e^2)+h_o\]{\mathrm{sin\φ}}_o\end{array}\right]+R_L^E{R}_b^L\left(R_s^b\left[\begin{array}{c}0\\ r_z\\ 0\end{array}\right]+{\stackrel{\→}{r}}_{bs}^b\right);$

其中：N为地球长轴半径；e为地球的椭球率；λ_o、φ_o、h_o分别为定位定向装置测量得到的载体在地理坐标系下的经度、纬度和高度；r_z为测距仪测量得到的载体与目标之间的距离；为载体坐标系内原点o_b到原点o_s的位置矢量；为台体坐标系到载体坐标系的坐标变换矩阵，为载体坐标系到地理坐标系的坐标变换矩阵，为地理坐标系到地球坐标系的坐标变换矩阵。

上述的动基座测量方法，坐标变换矩阵和的计算公式如下：

$R_s^b=\left[\begin{array}{ccc}{\mathrm{cos\α}}_w& -{\mathrm{cos\α}}_n{\mathrm{sin\α}}_w& {\mathrm{sin\α}}_n{\mathrm{sin\α}}_w\\ {\mathrm{sin\α}}_w& {\mathrm{cos\α}}_n{\mathrm{cos\α}}_w& -{\mathrm{cos\α}}_w{\mathrm{sin\α}}_n\\ 0& {\mathrm{sin\α}}_n& {\mathrm{cos\α}}_n\end{array}\right];$

$R_b^L=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\β}\sin \mathrm{\γ}\sin \mathrm{\ψ}& -\cos \mathrm{\β}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\β}\cos \mathrm{\γ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\β}\sin \mathrm{\γ}\cos \mathrm{\ψ}& \cos \mathrm{\β}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\sin \mathrm{\β}\cos \mathrm{\γ}\cos \mathrm{\ψ}\\ -\cos \mathrm{\β}\sin \mathrm{\γ}& \sin \mathrm{\β}& \cos \mathrm{\β}\cos \mathrm{\γ}\end{array}\right];$

$R_L^E=\left[\begin{array}{ccc}-{\mathrm{sin\λ}}_o& -{\mathrm{sin\φ}}_o{\mathrm{cos\λ}}_o& {\mathrm{cos\φ}}_o{\mathrm{cos\λ}}_o\\ {\mathrm{cos\λ}}_o& -{\mathrm{sin\φ}}_o{\mathrm{sin\λ}}_o& {\mathrm{cos\φ}}_o{\mathrm{sin\λ}}_o\\ 0& {\mathrm{cos\φ}}_o& {\mathrm{sin\φ}}_o\end{array}\right];$

其中，β、γ、ψ分别为定位定向装置测量得到的载体在地理坐标系下的俯仰角、横滚角和偏航角。

上述的动基座测量方法，在步骤(4)中，目标的姿态角度包括地理坐标系下的俯仰角γ_m、横滚角β_m和偏航角ψ_m，具体计算过程如下：

(4a)、在目标上以跟踪特征点为球心建立球体坐标系，选取的第i个特征点在所述球体坐标系中的半径为r_i、方位角为β_i、俯仰角为γ_i，i＝1、2、…、M；

(4b)、利用照相机对目标成像；在成像照片中，第i个特征点对应的像素点在台体坐标系中的俯仰角为φ_pi、方位角为θ_pi；

(4c)、计算目标本体坐标系到地理坐标系的变换矩阵具体计算过程如下：

(4ca)、根据M个特征点的球体坐标，以及所述M个特征点在照片中对应像素点在台体坐标系中的俯仰角和方位角，建立方程组；其中第i个特征点对应的方程为：

$\begin{array}{c}0=-r_i{\mathrm{cos\β}}^{\′}{\mathrm{cos\γ}}_i{\mathrm{sin\β}}_i{\mathrm{sin\ψ}}^{\′}+r_i{\mathrm{cos\γ}}^{\′}{\mathrm{sin\β}}^{\′}{\mathrm{sin\γ}}_i{\mathrm{sin\ψ}}^{\′}+r_z{\mathrm{tan\θ}}_{pi}+r_i{\mathrm{sin\γ}}^{\′}{\mathrm{sin\γ}}_i{\mathrm{sin\ψ}}^{\′}{\mathrm{tan\θ}}_{pi}+\\ r_i{\mathrm{cos\ψ}}^{\′}({\mathrm{sin\γ}}^{\′}{\mathrm{sin\γ}}_i+{\mathrm{cos\β}}^{\′}{\mathrm{cos\γ}}_i{\mathrm{sin\β}}_i{\mathrm{tan\θ}}_{pi}-{\mathrm{cos\γ}}^{\′}{\mathrm{sin\β}}^{\′}{\mathrm{sin\γ}}_i{\mathrm{tan\θ}}_{pi})+\\ r_i{\mathrm{cos\β}}_i{\mathrm{cos\γ}}_i\[\left({\mathrm{sec\θ}}_{pi}{\mathrm{sin\β}}^{\′}{\mathrm{sin\γ}}^{\′}\sin \right({\mathrm{\θ}}_{pi}-{\mathrm{\ψ}}^{\′})+{\mathrm{cos\γ}}^{\′}({\mathrm{cos\ψ}}^{\′}+{\mathrm{sin\ψ}}^{\′}{\mathrm{tan\θ}}_{pi})\]\end{array};$

其中，γ′、β′和ψ′为三个待求的变量；

(4cb)、对步骤(4ca)建立的方程组进行求解，得到变量γ′、β′和ψ′的取值；然后通过如下公式计算得到目标本体坐标系到台体坐标系的变换矩阵

$R_{bm}^s=\left[\begin{array}{ccc}{\mathrm{cos\γ}}^{\′}{\mathrm{cos\ψ}}^{\′}-{\mathrm{sin\β}}^{\′}{\mathrm{sin\γ}}^{\′}{\mathrm{sin\ψ}}^{\′}& -{\mathrm{cos\β}}^{\′}{\mathrm{sin\ψ}}^{\′}& {\mathrm{sin\γ}}^{\′}{\mathrm{cos\ψ}}^{\′}+{\mathrm{sin\β}}^{\′}{\mathrm{cos\γ}}^{\′}{\mathrm{sin\ψ}}^{\′}\\ {\mathrm{cos\γ}}^{\′}{\mathrm{sin\ψ}}^{\′}+{\mathrm{sin\β}}^{\′}{\mathrm{sin\γ}}^{\′}{\mathrm{cos\ψ}}^{\′}& {\mathrm{cos\β}}^{\′}{\mathrm{cos\ψ}}^{\′}& {\mathrm{sin\γ}}^{\′}{\mathrm{sin\ψ}}^{\′}-{\mathrm{sin\β}}^{\′}{\mathrm{cos\γ}}^{\′}{\mathrm{cos\ψ}}^{\′}\\ -{\mathrm{cos\β}}^{\′}{\mathrm{sin\γ}}^{\′}& {\mathrm{sin\β}}^{\′}& {\mathrm{cos\β}}^{\′}{\mathrm{cos\γ}}^{\′}\end{array}\right];$

(4cc)、计算得到目标本体坐标系到地理坐标系的变换矩阵其中，为台体坐标系到载体坐标系的坐标变换矩阵，为载体坐标系到地理坐标系的坐标变换矩阵；

(4d)、根据变换矩阵得到目标在地理坐标系下的俯仰角γ_m、横滚角β_m和偏航角ψ_m，其中：

${\mathrm{\γ}}_m=arcsin\[R_{bm}^L(3,2)\]$

${\mathrm{\β}}_m=-arctan\[\frac{R_{bm}^L(3,1)}{R_{bm}^L(3,3)}\]$

${\mathrm{\ψ}}_m=-arctan\[\frac{R_{bm}^L(1,2)}{R_{bm}^L(2,2)}\].$

上述的动基座测量方法，在步骤(4b)中，俯仰角φ_pi和方位角θ_pi的计算公式如下：

$\begin{array}{cc}{\mathrm{\φ}}_{pi}=arctan\frac{z_i}{\sqrt{f^2+{x_i}^2}},& {\mathrm{\θ}}_{pi}=-arctan\frac{x_i}{f}\end{array};$

其中：f为照相机的焦距；m_Xi和m_Yi是第i个特征点对应像素点在照片中的纵向排序序号和横向排序序号；M_X和M_Y分别为照片中纵向像素个数和横向像素个数；Δm是照相机感光元件上单个像素的边长。

上述的动基座测量方法，在步骤(4cb)中，对步骤(4ca)建立的方程组进行求解，其中，在方程组中选取3个方程建立子方程组，对所述子方程组进行求解，得到变量γ′、β′和ψ′的取值。

上述的动基座测量方法，在步骤(4cb)中，对步骤(4ca)建立的方程组进行求解，其中，在方程组中选取多个子方程组，每个子方程组中包括3个方程，然后对每个子方程组进行求解，得到多组解；最后对得到的多组取值进行平均计算，作为变量γ′、β′和ψ′的取值。

本发明与现有技术相比，具有以下优点：

(1)、本发明的动基座测量方法将测量系统安装在运动载体上，使测量系统具有机动性，可以随着目标一起移动，使被测目标始终位于测量系统的有效测量范围内，因此大大提高了测量系统的测量距离范围；

(2)、本发明的动基座测量方法，采用定位定向装置对载体位置和姿态进行测量，利用测距仪对载体与目标之间的距离进行测量，并利用光学测量系统对载体与目标之间的姿态相对关系进行测量，从而实现目标的位置和姿态测量，具有较高的测量精度。

附图说明

图1为本发明的动基座测量系统的结构示意图；

图2为本发明中照相机照片坐标示意图。

具体实施方式

下面结合附图和具体实例对本发明作进一步详细的描述：

在现有技术中，跟踪测量系统安装在固定基座上，对被测物体进行轨迹跟踪和姿态测量。由于采用固定安装方式，这种跟踪测量系统的测量距离范围受限于被测物体与测量系统的距离，因此其应用范围受到了限制。本发明针对该问题，提出了一种动基座测量方法。本发明的测量方法将测量系统安装在机动载体上，可以对目标进行动态跟踪，从而大大提高了测量范围，具有更为广阔的应用前景。

本发明的动基座测量方法，用于实时测量机动目标的位置和姿态。该方法采用的动基座测量系统，具体包括定位定向装置、照相机、转台和测距仪。其中，定位定向装置用于对安装动基座测量系统的载体进行定位和姿态测量，可以采用陀螺罗经与卫星导航组合，惯性导航与卫星导航组合等方法实现。照相机一般选用高速照相机，其每秒拍摄100幅以上的照片。

动基座测量系统的安装方式如图1所示。图中，1为定位定向装置，2为测距仪，3为照相机，4为转台，41为转台的台体。其中，照相机和测距仪安装在转台的台体上，而转台和定位定向装置安装在载体的同一安装面上。其中，转台的台体坐标系o_sx_sy_sz_s的原点o_s位于照相机的镜头中心，o_sx_sy_s平面与台体上表面平行，y_s轴位于照相机光轴方向；载体坐标系o_bx_by_bz_b的原点o_b位于定位定向装置的几何中心。在载体坐标系o_bx_by_bz_b中，原点o_b到原点o_s的位置矢量为

本发明的动基座测量方法包括以下步骤：

(1)、在目标上选取跟踪特征点，并控制转台转动，使照相机的光轴对准所述跟踪特征点；记录光轴对准时的转台转动方位角α_n和俯仰角α_w；具体跟踪对准实现过程如下：

(1a)、利用照相机对跟踪特征点进行拍照，获得照片；

(1b)、计算照片中跟踪特征点对应像素点在台体坐标系中的俯仰角φ_p和方位角θ_p，具体计算公式如下：

$\begin{array}{cc}{\mathrm{\φ}}_p=arctan\frac{z}{\sqrt{f^2+x^2}},& {\mathrm{\θ}}_p=-arctan\frac{x}{f}\end{array};$

其中：f为照相机的焦距；m_X和m_Y是跟踪特征点对应像素点在照片中的纵向排序序号和横向排序序号；M_X和M_Y分别为照片中纵向像素个数和横向像素个数；Δm是照相机感光元件上单个像素的边长。其中，根据照相机的成像原理可知，成像照片的二位坐标系OXY位于台体坐标系的y_s＝-f平面上，m_X为沿X轴方向上的像素位置序号，m_Y为沿Y轴方向上的像素位置序号。

(1c)、控制转台转动，并重复步骤(1a)～(1b)，当所述俯仰角φ_p和方位角θ_p都为零时，则判断照相机的光轴已对准跟踪特征点。

(2)、利用测距仪测量载体与目标之间的距离r_z；并利用定位定向装置对载体的位置和姿态角度进行测量。其中，测量得到载体在地理坐标系下的经度λ_o、纬度φ_o和高度h_o，以及载体在地理坐标系下的姿态角度分别为俯仰角β、横滚角γ、偏航角ψ；

(3)、根据载体的位置和姿态角度、载体与目标之间的距离，以及转台转动方位角α_n和俯仰角α_w，计算得到目标的位置坐标，具体包括目标在地理坐标系下经度λ_p、纬度φ_p和高度h_p，其计算公式如下：

$\left[\begin{array}{c}(N+h_p){\mathrm{cos\φ}}_p{\mathrm{cos\λ}}_p\\ (N+h_p){\mathrm{cos\φ}}_p{\mathrm{sin\λ}}_p\\ \[N(1-e^2)+h_p\]{\mathrm{sin\φ}}_p\end{array}\right]=\left[\begin{array}{c}(N+h_o){\mathrm{cos\φ}}_o{\mathrm{cos\λ}}_o\\ (N+h_o){\mathrm{cos\φ}}_o{\mathrm{sin\λ}}_o\\ \[N(1-e^2)+h_o\]{\mathrm{sin\φ}}_o\end{array}\right]+R_L^E{R}_b^L\left(R_s^b\left[\begin{array}{c}0\\ r_z\\ 0\end{array}\right]+{\stackrel{\→}{r}}_{bs}^b\right);$

其中：N为地球长轴半径；e为地球的椭球率；为台体坐标系到载体坐标系的坐标变换矩阵，为载体坐标系到地理坐标系的坐标变换矩阵，为地理坐标系到地球坐标系的坐标变换矩阵；各坐标变换矩阵的计算公式如下：

$R_s^b=\left[\begin{array}{ccc}{\mathrm{cos\α}}_w& -{\mathrm{cos\α}}_n{\mathrm{sin\α}}_w& {\mathrm{sin\α}}_n{\mathrm{sin\α}}_w\\ {\mathrm{sin\α}}_w& {\mathrm{cos\α}}_n{\mathrm{cos\α}}_w& -{\mathrm{cos\α}}_w{\mathrm{sin\α}}_n\\ 0& {\mathrm{sin\α}}_n& {\mathrm{cos\α}}_n\end{array}\right];$

$R_b^L=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\β}\sin \mathrm{\γ}\sin \mathrm{\ψ}& -\cos \mathrm{\β}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\β}\cos \mathrm{\γ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\β}\sin \mathrm{\γ}\cos \mathrm{\ψ}& \cos \mathrm{\β}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\sin \mathrm{\β}\cos \mathrm{\γ}\cos \mathrm{\ψ}\\ -\cos \mathrm{\β}\sin \mathrm{\γ}& \sin \mathrm{\β}& \cos \mathrm{\β}\cos \mathrm{\γ}\end{array}\right];$

$R_L^E=\left[\begin{array}{ccc}-{\mathrm{sin\λ}}_o& -{\mathrm{sin\φ}}_o{\mathrm{cos\λ}}_o& {\mathrm{cos\φ}}_o{\mathrm{cos\λ}}_o\\ {\mathrm{cos\λ}}_o& -{\mathrm{sin\φ}}_o{\mathrm{sin\λ}}_o& {\mathrm{cos\φ}}_o{\mathrm{sin\λ}}_o\\ 0& {\mathrm{cos\φ}}_o& {\mathrm{sin\φ}}_o\end{array}\right].$

(4)、在目标上选取M个特征点，M为正整数且M≥3。然后，利用照相机对目标进行成像，根据成像照片中M个特征点对应的像素点与跟踪特征点对应像素点的位置关系，计算得到目标的姿态角度，具体包括目标在地理坐标系下的俯仰角γ_m、横滚角β_m和偏航角ψ_m，具体计算过程如下：

(4a)、在目标上以跟踪特征点为球心建立球体坐标系，选取的第i个特征点在所述球体坐标系中的半径为r_i、方位角为β_i、俯仰角为γ_i，i＝1、2、…、M；

(4b)、利用照相机对目标成像；在成像照片中，第i个特征点对应的像素点在台体坐标系中的俯仰角为φ_pi、方位角为θ_pi，其计算公式如下：

$\begin{array}{cc}{\mathrm{\φ}}_{pi}=arctan\frac{z_i}{\sqrt{f^2+{x_i}^2}},& {\mathrm{\θ}}_{pi}=-arctan\frac{x_i}{f}\end{array};$

其中：f为照相机的焦距；m_Xi和m_Yi是第i个特征点对应像素点在照片中的纵向排序序号和横向排序序号；M_X和M_Y分别为照片中纵向像素个数和横向像素个数；Δm是照相机感光元件上单个像素的边长。

(4c)、计算目标本体坐标系到地理坐标系的变换矩阵具体计算过程如下：

(4ca)、根据M个特征点的球体坐标，以及所述M个特征点在照片中对应像素点在台体坐标系中的俯仰角和方位角，建立方程组；其中第i个特征点对应的方程为：

$\begin{array}{c}0=-r_i{\mathrm{cos\β}}^{\′}{\mathrm{cos\γ}}_i{\mathrm{sin\β}}_i{\mathrm{sin\ψ}}^{\′}+r_i{\mathrm{cos\γ}}^{\′}{\mathrm{sin\β}}^{\′}{\mathrm{sin\γ}}_i{\mathrm{sin\ψ}}^{\′}+r_z{\mathrm{tan\θ}}_{pi}+r_i{\mathrm{sin\γ}}^{\′}{\mathrm{sin\γ}}_i{\mathrm{sin\ψ}}^{\′}{\mathrm{tan\θ}}_{pi}+\\ r_i{\mathrm{cos\ψ}}^{\′}({\mathrm{sin\γ}}^{\′}+{\mathrm{cos\β}}^{\′}{\mathrm{cos\γ}}_i{\mathrm{sin\β}}_i{\mathrm{tan\θ}}_{pi}-{\mathrm{cos\γ}}^{\′}{\mathrm{sin\β}}^{\′}{\mathrm{sin\γ}}_i{\mathrm{tan\θ}}_{pi})+\\ {\mathrm{\γ}}_i{\mathrm{cos\β}}_i{\mathrm{cos\γ}}_i\[\left({\mathrm{sec\θ}}_{pi}{\mathrm{sin\β}}^{\′}{\mathrm{sin\γ}}^{\′}\sin \right({\mathrm{\θ}}_{pi}-{\mathrm{\ψ}}^{\′})+{\mathrm{cos\γ}}^{\′}({\mathrm{cos\ψ}}^{\′}+{\mathrm{sin\ψ}}^{\′}{\mathrm{tan\θ}}_{pi})\]\end{array};$

其中，γ′、β′和ψ′为三个待求的变量；

(4cb)、对步骤(4ca)建立的方程组进行求解，得到变量γ′、β′和ψ′的取值；然后通过如下公式计算得到目标本体坐标系到台体坐标系的变换矩阵R_b ^s _m：

$R_{bm}^s=\left[\begin{array}{ccc}{\mathrm{cos\γ}}^{\′}{\mathrm{cos\ψ}}^{\′}-{\mathrm{sin\β}}^{\′}{\mathrm{sin\γ}}^{\′}{\mathrm{sin\ψ}}^{\′}& -{\mathrm{cos\β}}^{\′}{\mathrm{sin\ψ}}^{\′}& {\mathrm{sin\γ}}^{\′}{\mathrm{cos\ψ}}^{\′}+{\mathrm{sin\β}}^{\′}{\mathrm{cos\γ}}^{\′}{\mathrm{sin\ψ}}^{\′}\\ {\mathrm{cos\γ}}^{\′}{\mathrm{sin\ψ}}^{\′}+{\mathrm{sin\β}}^{\′}{\mathrm{sin\γ}}^{\′}{\mathrm{cos\ψ}}^{\′}& {\mathrm{cos\β}}^{\′}{\mathrm{cos\ψ}}^{\′}& {\mathrm{sin\γ}}^{\′}{\mathrm{sin\ψ}}^{\′}-{\mathrm{sin\β}}^{\′}{\mathrm{cos\γ}}^{\′}{\mathrm{cos\ψ}}^{\′}\\ -{\mathrm{cos\β}}^{\′}{\mathrm{sin\γ}}^{\′}& {\mathrm{sin\β}}^{\′}& {\mathrm{cos\β}}^{\′}{\mathrm{cos\γ}}^{\′}\end{array}\right].$

在变量γ′、β′和ψ′的求解过程中，可以在步骤(4ca)建立的方程组中选取3个方程建立子方程组，对该子方程组进行求解，得到变量γ′、β′和ψ′的取值。也可以步骤(4ca)建立的方程组中选取多个子方程组，每个子方程组中包括3个方程，然后对每个子方程组进行求解，得到多组解；最后对得到的多组取值进行平均计算，作为变量γ′、β′和ψ′的取值。具体的方程求解方法可以采用非线性方程组的简单迭代法或牛顿迭代法进行计算。

(4cc)、计算得到目标本体坐标系到地理坐标系的变换矩阵其中，为台体坐标系到载体坐标系的坐标变换矩阵，为载体坐标系到地理坐标系的坐标变换矩阵；

(4d)、根据变换矩阵得到目标在地理坐标系下的俯仰角γ_m、横滚角β_m和偏航角ψ_m，其中：

${\mathrm{\γ}}_m=arcsin\[R_{bm}^L(3,2)\]$

${\mathrm{\β}}_m=-arctan\[\frac{R_{bm}^L(3,1)}{R_{bm}^L(3,3)}\]$

${\mathrm{\ψ}}_m=-arctan\[\frac{R_{bm}^L(1,2)}{R_{bm}^L(2,2)}\].$

其中，为变换矩阵中第1行第2列的元素；为变换矩阵中第2行第2列的元素；变换矩阵中第3行第1列的元素；变换矩阵中第3行第2列的元素；变换矩阵中第3行第3列的元素。

上述的本发明的测量方法在步骤(3)中计算得到目标的位置，在步骤(4)中计算得到目标的姿态角度，从而实现机动目标的位置和姿态测量。由于测量系统安装在机动载体上(如飞机)，当目标飞离测量系统的测量距离范围时，机动载体可以随着目标一起移动，以确保目标始终在测量系统的测量距离范围之内，从而大大提高了测量系统的测量距离和应用范围。

以上所述，仅为本发明一个具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。

Claims (9)

1.一种动基座测量方法，用于实时测量目标的位置和姿态，其特征在于：基于动基座测量系统实现，所述系统包括定位定向装置、照相机、转台和测距仪；照相机和测距仪安装在转台的台体上，转台和定位定向装置安装在载体的同一安装面上；其中，转台的台体坐标系o_sx_sy_sz_s的原点o_s位于照相机的镜头中心，o_sx_sy_s平面与台体上表面平行，y_s轴位于照相机光轴方向；载体坐标系o_bx_by_bz_b的原点o_b位于定位定向装置的几何中心；

所述动基座测量方法包括以下步骤：

(1)、在目标上选取跟踪特征点，并控制转台转动，使照相机的光轴对准所述跟踪特征点；记录光轴对准时的转台转动方位角α_n和俯仰角α_w；

(2)、利用测距仪测量载体与目标之间的距离，并利用定位定向装置对载体的位置和姿态角度进行测量；

(3)、根据载体的位置和姿态角度、载体与目标之间的距离，以及转台转动方位角α_n和俯仰角α_w，计算得到目标的位置坐标；

(4)、在目标上选取M个特征点，并利用照相机对目标进行成像，根据成像照片中M个特征点对应的像素点与跟踪特征点对应像素点的位置关系，计算得到目标的姿态角度；M为正整数且M≥3。

2.根据权利要求1所述的一种动基座测量方法，其特征在于：在步骤(1)中，控制转台转动，使得照相机的光轴对准跟踪特征点，具体实现过程如下：

(1a)、利用照相机对跟踪特征点进行拍照，获得照片；

(1b)、计算照片中跟踪特征点对应像素点在台体坐标系中的俯仰角φ_p和方位角θ_p；

(1c)、控制转台转动，并重复步骤(1a)～(1b)，当所述俯仰角φ_p和方位角θ_p都为零时，则判断照相机的光轴已对准跟踪特征点。

3.根据权利要求2所述的一种动基座测量方法，其特征在于：在步骤(1b)中，俯仰角φ_p和方位角θ_p的计算公式如下：

${\mathrm{\φ}}_p=\arctan \frac{z}{\sqrt{f^2+x^2}},{\mathrm{\θ}}_p=-\arctan \frac{x}{f};$

其中：f为照相机的焦距；m_X和m_Y是跟踪特征点对应像素点在照片中的纵向排序序号和横向排序序号；M_X和M_Y分别为照片中纵向像素个数和横向像素个数；Δm是照相机感光元件上单个像素的边长。

4.根据权利要求1所述的一种动基座测量方法，其特征在于：在步骤(3)中，计算得到的目标位置坐标包括目标在地理坐标系下经度λ_p、纬度φ_p和高度h_p，具体计算公式如下：

$\left[\begin{array}{c}\left(N+h_p\right){\mathrm{cos\φ}}_p{\mathrm{cos\λ}}_p\\ \left(N+h_p\right){\mathrm{cos\φ}}_p{\mathrm{sin\λ}}_p\\ \[N\left(1-e^2\right)+h_p\]{\mathrm{sin\φ}}_p\end{array}\right]=\left[\begin{array}{c}\left(N+h_o\right){\mathrm{cos\φ}}_o{\mathrm{cos\λ}}_o\\ \left(N+h_o\right){\mathrm{cos\φ}}_o{\mathrm{sin\λ}}_o\\ \[N\left(1-e^2\right)+h_o\]{\mathrm{sin\φ}}_o\end{array}\right]+R_L^E{R}_b^L\left(R_s^b\left[\begin{array}{c}0\\ r_z\\ 0\end{array}\right]+{\stackrel{\→}{r}}_{bs}^b\right);$

其中：N为地球长轴半径；e为地球的椭球率；λ_o、φ_o、h_o分别为定位定向装置测量得到的载体在地理坐标系下的经度、纬度和高度；r_z为测距仪测量得到的载体与目标之间的距离；为载体坐标系内原点o_b到原点o_s的位置矢量；为台体坐标系到载体坐标系的坐标变换矩阵，为载体坐标系到地理坐标系的坐标变换矩阵，为地理坐标系到地球坐标系的坐标变换矩阵。

5.根据权利要求4所述的一种动基座测量方法，其特征在于：坐标变换矩阵和的计算公式如下：

$R_s^b=\left[\begin{array}{ccc}{\mathrm{cos\α}}_w& -{\mathrm{cos\α}}_n{\mathrm{sin\α}}_w& {\mathrm{sin\α}}_n{\mathrm{sin\α}}_w\\ {\mathrm{sin\α}}_w& {\mathrm{cos\α}}_n{\mathrm{cos\α}}_w& -{\mathrm{cos\α}}_w{\mathrm{sin\α}}_n\\ 0& {\mathrm{sin\α}}_n& {\mathrm{cos\α}}_n\end{array}\right];$

$R_b^L=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\β}\sin \mathrm{\γ}\sin \mathrm{\ψ}& -\cos \mathrm{\β}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\β}\cos \mathrm{\γ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\β}\sin \mathrm{\γ}\cos \mathrm{\ψ}& \cos \mathrm{\β}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\sin \mathrm{\β}\cos \mathrm{\γ}\cos \mathrm{\ψ}\\ -\cos \mathrm{\β}\sin \mathrm{\γ}& \sin \mathrm{\β}& \cos \mathrm{\β}\cos \mathrm{\γ}\end{array}\right];$

$R_L^E=\left[\begin{array}{ccc}-{\mathrm{sin\λ}}_o& -{\mathrm{sin\φ}}_o{\mathrm{cos\λ}}_o& {\mathrm{cos\φ}}_o{\mathrm{cos\λ}}_o\\ {\mathrm{cos\λ}}_o& -{\mathrm{sin\φ}}_o{\mathrm{sin\λ}}_o& {\mathrm{cos\φ}}_o{\mathrm{sin\λ}}_o\\ 0& {\mathrm{cos\φ}}_o& {\mathrm{sin\φ}}_o\end{array}\right];$

其中，β、γ、ψ分别为定位定向装置测量得到的载体在地理坐标系下的俯仰角、横滚角和偏航角。

6.根据权利要求1所述的一种动基座测量方法，其特征在于：在步骤(4)中，目标的姿态角度包括地理坐标系下的俯仰角γ_m、横滚角β_m和偏航角ψ_m，具体计算过程如下：

(4a)、在目标上以跟踪特征点为球心建立球体坐标系，选取的第i个特征点在所述球体坐标系中的半径为r_i、方位角为β_i、俯仰角为γ_i，i＝1、2、…、M；

(4b)、利用照相机对目标成像；在成像照片中，第i个特征点对应的像素点在台体坐标系中的俯仰角为φ_pi、方位角为θ_pi；

(4c)、计算目标本体坐标系到地理坐标系的变换矩阵R_b ^L _m，具体计算过程如下：

(4ca)、根据M个特征点的球体坐标，以及所述M个特征点在照片中对应像素点在台体坐标系中的俯仰角和方位角，建立方程组；其中第i个特征点对应的方程为：

0＝-r_icosβ′cosγ_isinβ_isinψ′+r_icosγ′sinβ′sinγ_isinψ′+r_ztanθ_pi+r_isinγ′sinγ_isinψ′tanθ_pi+

r_icosψ′(sinγ′sinγ_i+cosβ′cosγ_isinβ_itanθ_pi-cosγ′sinβ′sinγ_itanθ_pi)+；

r_icosβ_icosγ_i[(secθ_pisinβ′sinγ′sin(θ_pi-ψ′)+cosγ′(cosψ′+sinψ′tanθ_pi)]

其中，γ′、β′和ψ′为三个待求的变量；

(4cb)、对步骤(4ca)建立的方程组进行求解，得到变量γ′、β′和ψ′的取值；然后通过如下公式计算得到目标本体坐标系到台体坐标系的变换矩阵

$R_{bm}^s=\left[\begin{array}{ccc}{\mathrm{cos\γ}}^{\′}{\mathrm{cos\ψ}}^{\′}-{\mathrm{sin\β}}^{\′}{\mathrm{sin\γ}}^{\′}{\mathrm{sin\ψ}}^{\′}& -{\mathrm{cos\β}}^{\′}{\mathrm{sin\ψ}}^{\′}& {\mathrm{sin\γ}}^{\′}{\mathrm{cos\ψ}}^{\′}+{\mathrm{sin\β}}^{\′}{\mathrm{cos\γ}}^{\′}{\mathrm{sin\ψ}}^{\′}\\ {\mathrm{cos\γ}}^{\′}{\mathrm{sin\ψ}}^{\′}+{\mathrm{sin\β}}^{\′}{\mathrm{sin\γ}}^{\′}{\mathrm{cos\ψ}}^{\′}& {\mathrm{cos\β}}^{\′}{\mathrm{cos\ψ}}^{\′}& {\mathrm{sin\γ}}^{\′}{\mathrm{sin\ψ}}^{\′}-{\mathrm{sin\β}}^{\′}{\mathrm{cos\γ}}^{\′}{\mathrm{cos\ψ}}^{\′}\\ -{\mathrm{cos\β}}^{\′}{\mathrm{sin\γ}}^{\′}& {\mathrm{sin\β}}^{\′}& {\mathrm{cos\β}}^{\′}{\mathrm{cos\γ}}^{\′}\end{array}\right];$

(4cc)、计算得到目标本体坐标系到地理坐标系的变换矩阵其中，为台体坐标系到载体坐标系的坐标变换矩阵，为载体坐标系到地理坐标系的坐标变换矩阵；

(4d)、根据变换矩阵得到目标在地理坐标系下的俯仰角γ_m、横滚角β_m和偏航角ψ_m，其中：

${\mathrm{\γ}}_m=\arcsin \[R_{bm}^L(3,2)\]$

${\mathrm{\β}}_m=-\arctan \[\frac{R_{bm}^L(3,1)}{R_{bm}^L(3,3)}\]$

${\mathrm{\ψ}}_m=-\arctan \[\frac{R_{bm}^L(1,2)}{R_{bm}^L(2,2)}\].$

7.根据权利要求6所述的一种动基座测量方法，其特征在于：在步骤(4b)中，俯仰角φ_pi和方位角θ_pi的计算公式如下：

${\mathrm{\φ}}_{pi}=\arctan \frac{z_i}{\sqrt{f^2+x_i^2}},{\mathrm{\θ}}_{pi}=-\arctan \frac{x_i}{f};$

其中：f为照相机的焦距；m_Xi和m_Yi是第i个特征点对应像素点在照片中的纵向排序序号和横向排序序号；M_X和M_Y分别为照片中纵向像素个数和横向像素个数；Δm是照相机感光元件上单个像素的边长。

8.根据权利要求6所述的一种动基座测量方法，其特征在于：在步骤(4cb)中，对步骤(4ca)建立的方程组进行求解，其中，在方程组中选取3个方程建立子方程组，对所述子方程组进行求解，得到变量γ′、β′和ψ′的取值。

9.根据权利要求6所述的一种动基座测量方法，其特征在于：在步骤(4cb)中，对步骤(4ca)建立的方程组进行求解，其中，在方程组中选取多个子方程组，每个子方程组中包括3个方程，然后对每个子方程组进行求解，得到多组解；最后对得到的多组取值进行平均计算，作为变量γ′、β′和ψ′的取值。