CN104422425A - 一种不规则外形物体空间姿态动态测量方法 - Google Patents

Abstract

本发明涉及运动物体空间姿态动态测量技术领域，具体公开了一种不规则外形物体空间姿态动态测量方法。该方法包括：1）在被测物上设置光学靶标，并利用激光跟踪仪对所有光学靶标进行全局校准；2）利用相机测量系统标定技术对测量系统进行标定，并控制左右两相机同步采集测量图像，并通过图像处理技术提取光学靶标的图像坐标；3）利用步骤1、2所获得的光学靶标在被测物坐标系下的三维坐标和在测量坐标系下的三维坐标，获得旋转矩阵，获得被测物的三维空间姿态角。该测量方法，可以测量非轴对称的不规则外形的空间物体的瞬时空间三维姿态角；在测量范围2m×2m×2m的空间中，测量频率1000Hz的测量条件下，测量精度可达到空间角合成均方根误差小于0.05°。

Description

一种不规则外形物体空间姿态动态测量方法

技术领域

本发明属于运动物体空间姿态动态测量技术领域，具体涉及一种不规则外形物体空间姿态动态测量方法。

背景技术

空间姿态动态测量指利用传感器获取空间物体三维运动姿态参数，包括物体自身坐标系相对于某固定参考坐标系的偏航角、俯仰角和滚转角。由于物体处于运动当中，接触式测量方法不能解决上述测量问题，需要采用非接触式测量方法。其中，光学测量是主流的空间姿态动态测量技术。现有的测量物体空间姿态的技术主要有利用光电经纬仪图像的中轴线法、利用多相机的二维平面测量方法和利用单站图像基于数学迭代优化方法等。

(1)利用光电经纬仪图像的中轴线法

利用光电经纬仪图像的中轴线法是首先假设被测物体是轴对称的，利用光电经纬仪获取被测物体清晰轮廓的图像，利用图像处理技术对图像进行分析处理，提取被测物的两条轮廓边界的直线方程，进而求出被测物体的中轴线方程，此直线与经纬仪照相系统的光心唯一确定了一个空间平面；利用两台经纬仪测量就能得到两个平面，两平面在空间相交，其交线就是被测物体的空间中轴线。得到物体的空间中轴线，便可进一步求出被测物体的偏航角和俯仰角。

(2)利用多相机的二维平面测量方法

利用多相机的二维平面测量方法是首先在被测物体上设置光学靶标，制造成像特征，利用多台相机在空间不同位置对被测物体成像，获取光学靶标的图像，利用两个光学靶标的质心构建一条直线，解算直线与相机坐标轴间的角度，进而求解被测物体的空间姿态角在与相机成像平面平行的平面内的角分量，利用多台相机便可以获得被测物体在指定方向上的姿态角变化。

(3)利用单站图像的数学迭代优化法

利用单站图像的数学迭代优化法是利用单个影像经纬仪或相机获取被测物体的图像，通过图像处理技术提取被测物轮廓上的边界角点，由摄影测量理论中共线条件约束可知，像面上一点a与相机光心O的连线确定空间一条直线Oa，像点对应的物点A必在直线Oa上。取被测目标上多于三个特征点，且已知任意两点间的距离，利用数学迭代优化的方法便可求解出被测物体的空间三维姿态。

现有的技术方法在面对不规则外形的物体空间三维姿态动态测量问题时，均表现出一定局限性：

(1)利用光电经纬仪图像的中轴线法仅适用于轮廓线是直线的轴对称物体，对被测物的形状规则性依赖性很强，对于非轴对称的不规则形体，算法稳定性和适应性受到很大影响，测量精度严重降低，甚至出现方法失效的情况；另外，该方法完全依靠物体自身的轮廓图像，由于轮廓提取精度是影响测量精度的关键因素之一，保证有效的测量精度对图像处理技术提出很高要求，相应的算法复杂性显著提高。

(2)利用多相机的二维平面测量方法本质上属于二维测量，获得的测量结果是被测物空间三维姿态角在某固定平面上的投影分量，投影平面是与成像相机像面平行的平面。因此，该方法的测量结果不能真实的反映被测物的三维姿态角，需要在不同位置设置测量相机获取感兴趣的方向上的姿态角，或通过多个方向的姿态角合成被测物体的三维空间姿态角数据。

(3)利用单站图像的数学迭代优化方法是基于相机成像模型结合空间距离约束建立数学方程，利用数学方法迭代优化求解被测物空间姿态参数。该方法的测量精度严重依赖相机内参数、畸变参数的标定精度和空间约束距离的精度，与图像处理被测物边界角点的精度密切相关。目前能够实现的精度不高，受图像质量影响显著。

发明内容

本发明的目的在于提供一种不规则外形物体空间姿态动态测量方法，能够适用于非轴对称的不规则外形的空间物体；通过设置测量特征，能够容易得到很高的特征提取精度，能够实现真正的三维姿态角测量，且该方法对图像处理算法的要求低，实现简单，精度高。

本发明的技术方案如下：一种不规则外形物体空间姿态动态测量方法，该方法具体包括如下步骤：

步骤1、在被测物上设置光学靶标，并利用激光跟踪仪对所有光学靶标进行全局校准；

在被测物体表面的明显位置设有光学靶标，并保证被测物体运动过程中左右两台相机均能够同时看到至少三个光学靶标；

步骤2、利用相机测量系统标定技术对测量系统进行标定，并利用计算机控制左右两相机同步采集测量图像，并通过图像处理技术提取光学靶标的图像坐标；

步骤2.1、利用相机测量系统标定技术标定出左右两台测量相机的内参数和外参数；

步骤2.2、通过同步控制器控制两台测量相机以固定测量频率采集测量图像，并利用质心法提取光学靶标的图像坐标；

步骤2.3、获取光学靶标在两台相机的测量图像中对应的质心坐标后，对成像镜头的畸变进行校正补偿；

步骤2.4、完成畸变校正后，获得光学靶标在测量坐标系下的三维坐标；

步骤3、利用步骤1、步骤2所获得的光学靶标在被测物坐标系下的三维坐标和在测量坐标系下的三维坐标，获得被测物坐标系相对测量坐标系的旋转矩阵，从而获得被测物的三维空间姿态角。

所述的步骤1中利用激光跟踪仪对光学靶标进行全局校准的具体步骤为：

利用激光跟踪仪测量三个光学靶标的空间三维坐标，并将激光跟踪仪坐标系设为标定坐标系O_c-X_cY_cZ_c，光学靶标在标定坐标系下的三维坐标为（X_c（i）,Y_c（i），Z_c（i）），（i=1，2，3）；利用激光跟踪仪测量被测物体的坐标系基准，建立被测物坐标系与标定坐标系的转换关系，将光学靶标在标定坐标系下的三维坐标转换为在被测物坐标系下的三维坐标（X_t（i）,Y_t（i）,Z_t（i）），（i=1，2，3）。

所述的步骤2.1中标定左右两台测量相机的内参数和外参数分别为：

利用平面标定法标定出左右两台测量相机的内参数（C_x ⁽ⁱ⁾,C_y ⁽ⁱ⁾,f_x ⁽ⁱ⁾,f_y ⁽ⁱ⁾,k₁ ⁽ⁱ⁾,k₂ ⁽ⁱ⁾,p₁ ⁽ⁱ⁾,p₂ ⁽ⁱ⁾）（i=1,2；1代表左相机，2代表右相机）和外参数R、T，其中，C_x ⁽ⁱ⁾,C_y ⁽ⁱ⁾为i相机的镜头主点位置；f_x ⁽ⁱ⁾,f_y ⁽ⁱ⁾为i相机的有效焦距；k₁ ⁽ⁱ⁾,k₂ ⁽ⁱ⁾为i相机的镜头径向畸变系数；p₁ ⁽ⁱ⁾,p₂ ⁽ⁱ⁾为i相机镜头切向畸变系数；R为旋转矩阵；T为平移矩阵。

所述的步骤2.2的具体步骤为：

通过同步控制器控制两台测量相机以固定测量频率采集测量图像，通过直方图分析、阈值分割图像处理手段滤除图像背景噪声，获得仅含有光学靶标的理想图像，利用质心法提取光学靶标的图像坐标（x_l ⁽ⁱ⁾,y_l ⁽ⁱ⁾）和（x_r ⁽ⁱ⁾,y_r ⁽ⁱ⁾）（i=1,2,3），其中，x_l ⁽ⁱ⁾和y_l ⁽ⁱ⁾为左相机测量的第i个光学靶标的图像坐标，x_r ⁽ⁱ⁾,y_r ⁽ⁱ⁾为右相机测量的第i个光学靶标的图像坐标。

所述的步骤2.3的具体步骤为：

获取光学靶标在两台相机的测量图像中对应的质心坐标后，对成像镜头的畸变进行校正补偿，校正模型为：

$\left\{\begin{array}{c}{\mathrm{\δ}}_x=k_1(x^2+y^2)+k_2{(x^2+y^2)}^2+p_1({3x}^2+y^2)+{2p}_2\mathrm{xy}\\ {\mathrm{\δ}}_y=k_1(x^2+y^2)+k_2{(x^2+y^2)}^2+p_2({3x}^2+y^2)+{2p}_1\mathrm{xy}\end{array}\right.$

$\left\{\begin{array}{c}{x}_u=x-C_x-{\mathrm{\δ}}_x\\ y_u=y-C_y-{\mathrm{\δ}}_y\end{array}\right.$

式中：x_u、y_u为目标质心校正后的图像坐标，x、y为目标质心校正前的图像坐标，δ_x、δ_y为镜头畸变；k₁、k₂为镜头径向畸变系数，p₁、p₂为镜头切向畸变系数，C_x、C_y为镜头主点位置，此6项参数通过相机系统标定后为已知量。

所述的步骤2.4的具体步骤为：

完成畸变校正后，基于下式模型求解靶标在测量坐标系O_m-X_mY_mZ_m下的三维坐标：

$\left\{\begin{array}{c}{X}_m=Z_m{x}_{\mathrm{ul}}/f_{\mathrm{xl}}\\ Y_m=Z_m{y}_{\mathrm{ul}}/f_{\mathrm{yl}}\\ Z_m=\frac{f_{\mathrm{xl}}(f_{\mathrm{xr}}{t}_x-x_{\mathrm{ul}}{t}_z)}{x_{\mathrm{ur}}(r_7{x}_{\mathrm{ul}}+r_8{y}_{\mathrm{ul}}+r_9{f}_{\mathrm{xl}})-f_{\mathrm{xr}}(r_1{x}_{\mathrm{ul}}+r_2{y}_{\mathrm{ul}}+r_3{f}_{\mathrm{xl}})}\\ =\frac{f_{\mathrm{yl}}(f_{\mathrm{yr}}{t}_y-t_{\mathrm{ul}}{t}_z)}{y_{\mathrm{ur}}(r_7{x}_{\mathrm{ul}}+r_8{y}_{\mathrm{ul}}+r_9{f}_{\mathrm{xl}})-f_{\mathrm{yr}}(r_4{x}_{\mathrm{ul}}+r_5{y}_{\mathrm{ul}}+r_6{f}_{\mathrm{yl}})}\end{array}\right.$

式中：X_m、Y_m、Z_m为光学靶标在测量坐标系下的三维坐标，x_ul、y_ul、x_ur、y_ur为校正后的左、右图像坐标，f_xl、f_yl、f_xr、f_yr分别表示左、右相机的有效焦距；矩阵R和T为：

$R=\left[\begin{array}{ccc}{r}_1& r_2& r_3\\ r_4& r_5& r_6\\ r_7& r_8& r_9\end{array}\right]$ $T=\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]$

f_xl、f_yl、f_xr、f_yr和R、T通过标定后为已知量。

所述的步骤3的具体步骤为：

步骤3.1、利用步骤2获得光学靶标在测量坐标系下的三维坐标（X_m ⁽ⁱ⁾,Y_m ⁽ⁱ⁾,Z_m ⁽ⁱ⁾）（i=1,2,3）后，结合步骤1获得的光学靶标在被测物坐标系下的三维坐标（X_t ⁽ⁱ⁾,Y_t ⁽ⁱ⁾,Z_t ⁽ⁱ⁾）（i=1,2,3），求解被测物坐标系相对测量坐标系的旋转矩阵；

考虑坐标系的平移，将坐标系原点由点P₁平移到P₂点，有关系式：

$P_2=P_1+t=\left[\begin{array}{c}{x}_{p1}+x_t\\ y_{p1}+y_t\\ z_{p1}+z_t\end{array}\right]$

考虑坐标系的旋转，坐标系的旋转可通过旋转矩阵R描述，绕X、Y、Z轴的旋转矩阵为：

$R_X=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\α}& -\sin \mathrm{\α}\\ 0& \sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right]$ $R_Y=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& \sin \mathrm{\β}\\ 0& 1& 0\\ -\sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right]$ $R_Z=\left[\begin{array}{ccc}\cos \mathrm{\γ}& -\sin \mathrm{\γ}& 0\\ \sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right]$

旋转矩阵

$R=R_z{R}_y{R}_x=\left[\begin{array}{ccc}\cos \mathrm{\β}\cos \mathrm{\γ}& -\cos \mathrm{\α}\sin \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}& \sin \mathrm{\α}\sin +\cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}\\ \cos \mathrm{\β}\sin \mathrm{\γ}& \cos \mathrm{\α}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}& -\sin \mathrm{\α}\cos \mathrm{\γ}+\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}\\ -\sin \mathrm{\β}& \sin \mathrm{\α}\cos \mathrm{\β}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]$

光学靶标在被测物坐标系与测量坐标系中三维坐标有如下关系：

p₂＝Rp₁+t

通过3个光学靶标，通过去中心化处理，得到只含旋转分量的坐标，列出如下参数方程，通过优化迭代可解算出旋转矩阵R；

$\left[\begin{array}{c}{x}_{11}\\ y_{11}\\ z_{11}\\ x_{12}\\ y_{12}\\ z_{12}\\ .\\ .\\ .\\ x_{1i}\\ y_{1i}\\ z_{1i}\end{array}\right]=\left[\begin{array}{ccccccccc}{x}_{01}& y_{01}& z_{01}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& x_{01}& y_{01}& z_{01}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& x_{01}& y_{01}& z_{01}\\ x_{02}& y_{02}& z_{02}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& x_{02}& y_{02}& z_{02}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& x_{02}& y_{02}& z_{02}\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ x_{03}& y_3& z_{03}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& x_{03}& y_{03}& z_{03}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& x_{03}& y_{03}& z_{03}\end{array}\right]\·\left[\begin{array}{c}{r}_1\\ r_2\\ r_3\\ r_4\\ r_5\\ r_6\\ r_7\\ r_8\\ r_9\end{array}\right]$

上述方程组中，r1,…,r9为旋转矩阵元素。x_0i,y_0i,z_0i为目标初始点第i点去重心坐标，x_1i,y_1i,z_1i为目标运动后对应第i点去重心坐标；

步骤3.2、加入约束条件，获得被测物是三维空间姿态角；

加入如下约束：

$\left\{\begin{array}{c}{r1}^2+{r2}^2+{r3}^3=1\\ {r4}^2+{r5}^2+{r6}^2=1\\ {r7}^2+{r8}^2+{r9}^2=1\\ r1r2+r4r5+r7r8=0\\ r2r3+r5r6+r8r9=0\\ r1r3+r4r6+r7r9=0\end{array}\right.$

优化迭代得到旋转矩阵R后，可反求出物体绕三轴的旋转角：

$\mathrm{\α}=\arctan \left(\frac{r8}{r9}\right)$

β＝-arcsin(r7)

$\mathrm{\γ}=\arctan \left(\frac{r4}{r1}\right)$

其中，α、β和γ为被测物的三维空间姿态角，即俯仰角、滚转角和偏航角。

本发明的显著效果在于：本发明所述的一种不规则外形物体空间姿态动态测量方法，可以测量非轴对称的不规则外形的空间物体的瞬时空间三维姿态角；在测量范围2m×2m×2m的空间中，测量频率1000Hz的测量条件下，测量精度可达到空间角合成均方根误差小于0.05°。

附图说明

图1为本发明所述的一种不规则外形物体空间姿态动态测量方法流程图；

具体实施方式

下面结合附图及具体实施例对本发明作进一步详细说明。

如图1所示，一种不规则外形物体空间姿态动态测量方法，该方法具体包括如下步骤：

步骤1、在被测物上设置光学靶标，并利用激光跟踪仪对所有光学靶标进行全局校准；

在被测物体表面的明显位置设有光学靶标，并保证被测物体运动过程中左右两台相机均能够同时看到至少三个光学靶标；

利用激光跟踪仪测量光学靶标的空间三维坐标，并将激光跟踪仪坐标系设为标定坐标系O_c-X_cY_cZ_c，光学靶标在标定坐标系下的三维坐标为（X_c（i）,Y_c（i），Z_c（i）），（i=1，2，3）；利用激光跟踪仪测量被测物体的坐标系基准，建立被测物坐标系与标定坐标系的转换关系，将光学靶标在标定坐标系下的三维坐标转换为在被测物坐标系下的三维坐标（X_t（i）,Y_t（i）,Z_t（i）），（i=1，2，3）。

步骤2、利用相机测量系统标定技术对测量系统进行标定，并利用计算机控制左右两相机同步采集测量图像，并通过图像处理技术提取光学靶标的图像坐标；

步骤2.1、利用相机测量系统标定技术标定出左右两台测量相机的内参数和外参数

利用平面标定法标定出左右两台测量相机的内参数（C_x ⁽ⁱ⁾,C_y ⁽ⁱ⁾,f_x ⁽ⁱ⁾,f_y ⁽ⁱ⁾,k₁ ⁽ⁱ⁾,k₂ ⁽ⁱ⁾,p₁ ⁽ⁱ⁾,p₂ ⁽ⁱ⁾）（i=1,2；1代表左相机，2代表右相机）和外参数R、T，其中，C_x ⁽ⁱ⁾,C_y ⁽ⁱ⁾为i相机的镜头主点位置；f_x ⁽ⁱ⁾,f_y ⁽ⁱ⁾为i相机的有效焦距；k₁ ⁽ⁱ⁾,k₂ ⁽ⁱ⁾为i相机的镜头径向畸变系数；p₁ ⁽ⁱ⁾,p₂ ⁽ⁱ⁾为i相机镜头切向畸变系数；R为旋转矩阵；T为平移矩阵。

步骤2.2、通过同步控制器控制两台测量相机以固定测量频率采集测量图像，并利用质心法提取光学靶标的图像坐标；

通过同步控制器控制两台测量相机以固定测量频率采集测量图像，通过直方图分析、阈值分割图像处理手段滤除图像背景噪声，获得仅含有光学靶标的理想图像，利用质心法提取光学靶标的图像坐标（x_l ⁽ⁱ⁾,y_l ⁽ⁱ⁾）和（x_r ⁽ⁱ⁾,y_r ⁽ⁱ⁾）（i=1,2,3），其中，x_l ⁽ⁱ⁾和y_l ⁽ⁱ⁾为左相机测量的第i个光学靶标的图像坐标，x_r ⁽ⁱ⁾,y_r ⁽ⁱ⁾为右相机测量的第i个光学靶标的图像坐标；

步骤2.3、获取光学靶标在两台相机的测量图像中对应的质心坐标后，对成像镜头的畸变进行校正补偿；

获取光学靶标在两台相机的测量图像中对应的质心坐标后，对成像镜头的畸变进行校正补偿，校正模型为：

$\left\{\begin{array}{c}{\mathrm{\δ}}_x=k_1(x^2+y^2)+k_2{(x^2+y^2)}^2+p_1({3x}^2+y^2)+{2p}_2\mathrm{xy}\\ {\mathrm{\δ}}_y=k_1(x^2+y^2)+k_2{(x^2+y^2)}^2+p_2({3x}^2+y^2)+{2p}_1\mathrm{xy}\end{array}\right.$

$\left\{\begin{array}{c}{x}_u=x-C_x-{\mathrm{\δ}}_x\\ y_u=y-C_y-{\mathrm{\δ}}_y\end{array}\right.$

式中：x_u、y_u为目标质心校正后的图像坐标，x、y为目标质心校正前的图像坐标，δ_x、δ_y为镜头畸变；k₁、k₂为镜头径向畸变系数，p₁、p₂为镜头切向畸变系数，C_x、C_y为镜头主点位置，此6项参数通过相机系统标定后为已知量。

步骤2.4、完成畸变校正后，获得光学靶标在测量坐标系下的三维坐标；

完成畸变校正后，基于下式模型求解靶标在左相机坐标系（即测量坐标系O_m-X_mY_mZ_m）下的三维坐标：

$\left\{\begin{array}{c}{X}_m=Z_m{x}_{\mathrm{ul}}/f_{\mathrm{xl}}\\ Y_m=Z_m{y}_{\mathrm{ul}}/f_{\mathrm{yl}}\\ Z_m=\frac{f_{\mathrm{xl}}(f_{\mathrm{xr}}{t}_x-x_{\mathrm{ul}}{t}_z)}{x_{\mathrm{ur}}(r_7{x}_{\mathrm{ul}}+r_8{y}_{\mathrm{ul}}+r_9{f}_{\mathrm{xl}})-f_{\mathrm{xr}}(r_1{x}_{\mathrm{ul}}+r_2{y}_{\mathrm{ul}}+r_3{f}_{\mathrm{xl}})}\\ =\frac{f_{\mathrm{yl}}(f_{\mathrm{yr}}{t}_y-t_{\mathrm{ul}}{t}_z)}{y_{\mathrm{ur}}(r_7{x}_{\mathrm{ul}}+r_8{y}_{\mathrm{ul}}+r_9{f}_{\mathrm{xl}})-f_{\mathrm{yr}}(r_4{x}_{\mathrm{ul}}+r_5{y}_{\mathrm{ul}}+r_6{f}_{\mathrm{yl}})}\end{array}\right.$

式中：X_m、Y_m、Z_m为光学靶标在测量坐标系下的三维坐标，x_ul、y_ul、x_ur、y_ur为校正后的左、右图像坐标，f_xl、f_yl、f_xr、f_yr分别表示左、右相机的有效焦距；矩阵R和T为：

$R=\left[\begin{array}{ccc}{r}_1& r_2& r_3\\ r_4& r_5& r_6\\ r_7& r_8& r_9\end{array}\right]$ $T=\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]$

f_xl、f_yl、f_xr、f_yr和R、T通过标定后为已知量。

步骤3、利用步骤1、步骤2所获得的光学靶标在被测物坐标系下的三维坐标和在测量坐标系下的三维坐标，获得被测物坐标系相对测量坐标系的旋转矩阵，从而获得被测物的三维空间姿态角；

步骤3.1、利用步骤2获得光学靶标在测量坐标系下的三维坐标（X_m ⁽ⁱ⁾,Y_m ⁽ⁱ⁾,Z_m ⁽ⁱ⁾）（i=1,2,3）后，结合步骤1获得的光学靶标在被测物坐标系下的三维坐标（X_t ⁽ⁱ⁾,Y_t ⁽ⁱ⁾,Z_t ⁽ⁱ⁾）（i=1,2,3），求解被测物坐标系相对测量坐标系的旋转矩阵；

考虑坐标系的平移，将坐标系原点由点P₁平移到P₂点，有关系式：

$P_2=P_1+t=\left[\begin{array}{c}{x}_{p1}+x_t\\ y_{p1}+y_t\\ z_{p1}+z_t\end{array}\right]$

考虑坐标系的旋转，坐标系的旋转可通过旋转矩阵R描述，绕X、Y、Z轴的旋转矩阵为：

$R_X=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\α}& -\sin \mathrm{\α}\\ 0& \sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right]$ $R_Y=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& \sin \mathrm{\β}\\ 0& 1& 0\\ -\sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right]$ $R_Z=\left[\begin{array}{ccc}\cos \mathrm{\γ}& -\sin \mathrm{\γ}& 0\\ \sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right]$

旋转矩阵

$R=R_z{R}_y{R}_x=\left[\begin{array}{ccc}\cos \mathrm{\β}\cos \mathrm{\γ}& -\cos \mathrm{\α}\sin \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}& \sin \mathrm{\α}\sin +\cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}\\ \cos \mathrm{\β}\sin \mathrm{\γ}& \cos \mathrm{\α}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}& -\sin \mathrm{\α}\cos \mathrm{\γ}+\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}\\ -\sin \mathrm{\β}& \sin \mathrm{\α}\cos \mathrm{\β}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]$

光学靶标在被测物坐标系与测量坐标系中三维坐标有如下关系：

p₂＝Rp₁+t

通过3个光学靶标，通过去中心化处理，得到只含旋转分量的坐标，列出如下参数方程，通过优化迭代可解算出旋转矩阵R；

$\left[\begin{array}{c}{x}_{11}\\ y_{11}\\ z_{11}\\ x_{12}\\ y_{12}\\ z_{12}\\ .\\ .\\ .\\ x_{1i}\\ y_{1i}\\ z_{1i}\end{array}\right]=\left[\begin{array}{ccccccccc}{x}_{01}& y_{01}& z_{01}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& x_{01}& y_{01}& z_{01}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& x_{01}& y_{01}& z_{01}\\ x_{02}& y_{02}& z_{02}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& x_{02}& y_{02}& z_{02}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& x_{02}& y_{02}& z_{02}\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ x_{03}& y_3& z_{03}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& x_{03}& y_{03}& z_{03}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& x_{03}& y_{03}& z_{03}\end{array}\right]\·\left[\begin{array}{c}{r}_1\\ r_2\\ r_3\\ r_4\\ r_5\\ r_6\\ r_7\\ r_8\\ r_9\end{array}\right]$

上述方程组中，r1,…,r9为旋转矩阵元素。x_0i,y_0i,z_0i为目标初始点第i点去重心坐标，x_1i,y_1i,z_1i为目标运动后对应第i点去重心坐标。

步骤3.2、加入约束条件，获得被测物是三维空间姿态角；

加入如下约束：

$\left\{\begin{array}{c}{r1}^2+{r2}^2+{r3}^3=1\\ {r4}^2+{r5}^2+{r6}^2=1\\ {r7}^2+{r8}^2+{r9}^2=1\\ r1r2+r4r5+r7r8=0\\ r2r3+r5r6+r8r9=0\\ r1r3+r4r6+r7r9=0\end{array}\right.$

优化迭代得到旋转矩阵R后，可反求出物体绕三轴的旋转角：

$\mathrm{\α}=\arctan \left(\frac{r8}{r9}\right)$

β＝-arcsin(r7)

$\mathrm{\γ}=\arctan \left(\frac{r4}{r1}\right)$

其中，α、β和γ为被测物的三维空间姿态角，即俯仰角、滚转角和偏航角。

Claims (7)

1.一种不规则外形物体空间姿态动态测量方法，其特征在于：该方法具体包括如下步骤：

步骤1、在被测物上设置光学靶标，并利用激光跟踪仪对所有光学靶标进行全局校准；

在被测物体表面的明显位置设有光学靶标，并保证被测物体运动过程中左右两台相机均能够同时看到至少三个光学靶标；

步骤2、利用相机测量系统标定技术对测量系统进行标定，并利用计算机控制左右两相机同步采集测量图像，并通过图像处理技术提取光学靶标的图像坐标；

步骤2.1、利用相机测量系统标定技术标定出左右两台测量相机的内参数和外参数；

步骤2.2、通过同步控制器控制两台测量相机以固定测量频率采集测量图像，并利用质心法提取光学靶标的图像坐标；

步骤2.3、获取光学靶标在两台相机的测量图像中对应的质心坐标后，对成像镜头的畸变进行校正补偿；

步骤2.4、完成畸变校正后，获得光学靶标在测量坐标系下的三维坐标；

步骤3、利用步骤1、步骤2所获得的光学靶标在被测物坐标系下的三维坐标和在测量坐标系下的三维坐标，获得被测物坐标系相对测量坐标系的旋转矩阵，从而获得被测物的三维空间姿态角。

2.根据权利要求1所述的一种不规则外形物体空间姿态动态测量方法，其特征在于：所述的步骤1中利用激光跟踪仪对光学靶标进行全局校准的具体步骤为：

利用激光跟踪仪测量三个光学靶标的空间三维坐标，并将激光跟踪仪坐标系设为标定坐标系O_c-X_cY_cZ_c，光学靶标在标定坐标系下的三维坐标为（X_c（i）,Y_c（i），Z_c（i）），（i=1，2，3）；利用激光跟踪仪测量被测物体的坐标系基准，建立被测物坐标系与标定坐标系的转换关系，将光学靶标在标定坐标系下的三维坐标转换为在被测物坐标系下的三维坐标（X_t（i）,Y_t（i）,Z_t（i）），（i=1，2，3）。

3.根据权利要求1所述的一种不规则外形物体空间姿态动态测量方法，其特征在于：所述的步骤2.1中标定左右两台测量相机的内参数和外参数分别为：

利用平面标定法标定出左右两台测量相机的内参数（C_x ⁽ⁱ⁾,C_y ⁽ⁱ⁾,f_x ⁽ⁱ⁾,f_y ⁽ⁱ⁾,k₁ ⁽ⁱ⁾,k₂ ⁽ⁱ⁾,p₁ ⁽ⁱ⁾,p₂ ⁽ⁱ⁾）（i=1,2；1代表左相机，2代表右相机）和外参数R、T，其中，C_x ⁽ⁱ⁾,C_y ⁽ⁱ⁾为i相机的镜头主点位置；f_x ⁽ⁱ⁾,f_y ⁽ⁱ⁾为i相机的有效焦距；k₁ ⁽ⁱ⁾,k₂ ⁽ⁱ⁾为i相机的镜头径向畸变系数；p₁ ⁽ⁱ⁾,p₂ ⁽ⁱ⁾为i相机镜头切向畸变系数；R为旋转矩阵；T为平移矩阵。

4.根据权利要求1述的一种不规则外形物体空间姿态动态测量方法，其特征在于：所述的步骤2.2的具体步骤为：

通过同步控制器控制两台测量相机以固定测量频率采集测量图像，通过直方图分析、阈值分割图像处理手段滤除图像背景噪声，获得仅含有光学靶标的理想图像，利用质心法提取光学靶标的图像坐标（x_l ⁽ⁱ⁾,y_l ⁽ⁱ⁾）和（x_r ⁽ⁱ⁾,y_r ⁽ⁱ⁾）（i=1,2,3），其中，x_l ⁽ⁱ⁾和y_l ⁽ⁱ⁾为左相机测量的第i个光学靶标的图像坐标，x_r ⁽ⁱ⁾,y_r ⁽ⁱ⁾为右相机测量的第i个光学靶标的图像坐标。

5.根据权利要求1述的一种不规则外形物体空间姿态动态测量方法，其特征在于：所述的步骤2.3的具体步骤为：

获取光学靶标在两台相机的测量图像中对应的质心坐标后，对成像镜头的畸变进行校正补偿，校正模型为：

$\left\{\begin{array}{c}{\mathrm{\δ}}_x=k_1(x^2+y^2)+k_2{(x^2+y^2)}^2+p_1({3x}^2+y^2)+{2p}_2\mathrm{xy}\\ {\mathrm{\δ}}_y=k_1(x^2+y^2)+k_2{(x^2+y^2)}^2+p_2({3x}^2+y^2)+{2p}_1\mathrm{xy}\end{array}\right.$

$\left\{\begin{array}{c}{x}_u=x-C_x-{\mathrm{\δ}}_x\\ y_u=y-C_y-{\mathrm{\δ}}_y\end{array}\right.$

式中：x_u、y_u为目标质心校正后的图像坐标，x、y为目标质心校正前的图像坐标，δ_x、δ_y为镜头畸变；k₁、k₂为镜头径向畸变系数，p₁、p₂为镜头切向畸变系数，C_x、C_y为镜头主点位置，此6项参数通过相机系统标定后为已知量。

6.根据权利要求1述的一种不规则外形物体空间姿态动态测量方法，其特征在于：所述的步骤2.4的具体步骤为：

完成畸变校正后，基于下式模型求解靶标在测量坐标系O_m-X_mY_mZ_m下的三维坐标：

$\left\{\begin{array}{c}{X}_m=Z_m{x}_{\mathrm{ul}}/f_{\mathrm{xl}}\\ Y_m=Z_m{y}_{\mathrm{ul}}/f_{\mathrm{yl}}\\ Z_m=\frac{f_{\mathrm{xl}}(f_{\mathrm{xr}}{t}_x-x_{\mathrm{ul}}{t}_z)}{x_{\mathrm{ur}}(r_7{x}_{\mathrm{ul}}+r_8{y}_{\mathrm{ul}}+r_9{f}_{\mathrm{xl}})-f_{\mathrm{xr}}(r_1{x}_{\mathrm{ul}}+r_2{y}_{\mathrm{ul}}+r_3{f}_{\mathrm{xl}})}\\ =\frac{f_{\mathrm{yl}}(f_{\mathrm{yr}}{t}_y-t_{\mathrm{ul}}{t}_z)}{y_{\mathrm{ur}}(r_7{x}_{\mathrm{ul}}+r_8{y}_{\mathrm{ul}}+r_9{f}_{\mathrm{xl}})-f_{\mathrm{yr}}(r_4{x}_{\mathrm{ul}}+r_5{y}_{\mathrm{ul}}+r_6{f}_{\mathrm{yl}})}\end{array}\right.$

式中：X_m、Y_m、Z_m为光学靶标在测量坐标系下的三维坐标，x_ul、y_ul、x_ur、y_ur为校正后的左、右图像坐标，f_xl、f_yl、f_xr、f_yr分别表示左、右相机的有效焦距；矩阵R和T为：

$R=\left[\begin{array}{ccc}{r}_1& r_2& r_3\\ r_4& r_5& r_6\\ r_7& r_8& r_9\end{array}\right]$ $T=\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]$

f_xl、f_yl、f_xr、f_yr和R、T通过标定后为已知量。

7.根据权利要求1述的一种不规则外形物体空间姿态动态测量方法，其特征在于：所述的步骤3的具体步骤为：

步骤3.1、利用步骤2获得光学靶标在测量坐标系下的三维坐标（X_m ⁽ⁱ⁾,Y_m ⁽ⁱ⁾,Z_m ⁽ⁱ⁾）（i=1,2,3）后，结合步骤1获得的光学靶标在被测物坐标系下的三维坐标（X_t ⁽ⁱ⁾,Y_t ⁽ⁱ⁾,Z_t ⁽ⁱ⁾）（i=1,2,3），求解被测物坐标系相对测量坐标系的旋转矩阵；

考虑坐标系的平移，将坐标系原点由点P₁平移到P₂点，有关系式：

$P_2=P_1+t=\left[\begin{array}{c}{x}_{p1}+x_t\\ y_{p1}+y_t\\ z_{p1}+z_t\end{array}\right]$

考虑坐标系的旋转，坐标系的旋转可通过旋转矩阵R描述，绕X、Y、Z轴的旋转矩阵为：

$R_X=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\α}& -\sin \mathrm{\α}\\ 0& \sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right]$ $R_Y=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& \sin \mathrm{\β}\\ 0& 1& 0\\ -\sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right]$ $R_Z=\left[\begin{array}{ccc}\cos \mathrm{\γ}& -\sin \mathrm{\γ}& 0\\ \sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right]$

旋转矩阵

$R=R_z{R}_y{R}_x=\left[\begin{array}{ccc}\cos \mathrm{\β}\cos \mathrm{\γ}& -\cos \mathrm{\α}\sin \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}& \sin \mathrm{\α}\sin +\cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}\\ \cos \mathrm{\β}\sin \mathrm{\γ}& \cos \mathrm{\α}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}& -\sin \mathrm{\α}\cos \mathrm{\γ}+\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}\\ -\sin \mathrm{\β}& \sin \mathrm{\α}\cos \mathrm{\β}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]$

光学靶标在被测物坐标系与测量坐标系中三维坐标有如下关系：

p₂＝Rp₁+t

通过3个光学靶标，通过去中心化处理，得到只含旋转分量的坐标，列出如下参数方程，通过优化迭代可解算出旋转矩阵R；

$\left[\begin{array}{c}{x}_{11}\\ y_{11}\\ z_{11}\\ x_{12}\\ y_{12}\\ z_{12}\\ .\\ .\\ .\\ x_{1i}\\ y_{1i}\\ z_{1i}\end{array}\right]=\left[\begin{array}{ccccccccc}{x}_{01}& y_{01}& z_{01}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& x_{01}& y_{01}& z_{01}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& x_{01}& y_{01}& z_{01}\\ x_{02}& y_{02}& z_{02}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& x_{02}& y_{02}& z_{02}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& x_{02}& y_{02}& z_{02}\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ x_{03}& y_3& z_{03}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& x_{03}& y_{03}& z_{03}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& x_{03}& y_{03}& z_{03}\end{array}\right]\·\left[\begin{array}{c}{r}_1\\ r_2\\ r_3\\ r_4\\ r_5\\ r_6\\ r_7\\ r_8\\ r_9\end{array}\right]$

上述方程组中，r1,…,r9为旋转矩阵元素。x_0i,y_0i,z_0i为目标初始点第i点去重心坐标，x_1i,y_1i,z_1i为目标运动后对应第i点去重心坐标；

步骤3.2、加入约束条件，获得被测物是三维空间姿态角；

加入如下约束：

$\left\{\begin{array}{c}{r1}^2+{r2}^2+{r3}^3=1\\ {r4}^2+{r5}^2+{r6}^2=1\\ {r7}^2+{r8}^2+{r9}^2=1\\ r1r2+r4r5+r7r8=0\\ r2r3+r5r6+r8r9=0\\ r1r3+r4r6+r7r9=0\end{array}\right.$

优化迭代得到旋转矩阵R后，可反求出物体绕三轴的旋转角：

$\mathrm{\α}=\arctan \left(\frac{r8}{r9}\right)$

β＝-arcsin(r7)

$\mathrm{\γ}=\arctan \left(\frac{r4}{r1}\right)$

其中，α、β和γ为被测物的三维空间姿态角，即俯仰角、滚转角和偏航角。