CN103363902B - 基于红外激光的粉尘环境中运动目标位姿检测装置及方法 - Google Patents

Abstract

基于红外激光的粉尘环境中运动目标位姿检测装置及方法，属于检测技术领域。双轴精密转台安装在运动目标上，多个传播方向互为一定角度的红外激光器组成阵列，安装在转台上。转台自动跟踪激光光斑检测单元，保证多束激光始终投射在光斑检测单元上。根据光斑检测单元上的光斑位置，通过计算获得运动目标的位姿信息。激光光源波长位于红外波段，具有穿透粉尘环境的能力；双轴精密转台自动跟踪光斑检测单元，检测范围大；多束激光的传播路径交汇于双轴精密转台与光斑检测单元之间的一个空间点，且光斑检测单元上的激光光斑相距较小，因此可使用视场范围较小的成像元件或尺寸较小的光敏元件阵列进行检测，以获得较高的位姿检测精度。

Description

基于红外激光的粉尘环境中运动目标位姿检测装置及方法

技术领域

本发明属于检测技术领域，特别涉及一种基于红外激光的粉尘环境中运动目标位姿检测装置及方法的设计。

背景技术

在煤炭开采、隧道掘进、矿石采选等行业中，作业环境粉尘浓度高，能见度极低。目前依靠工人主观经验操作作业机械的方法，难以保证良好的工作面质量与精确的巷道走向。粉尘环境中运动目标位姿自动检测技术，是实现作业机械自动控制的基础，是保证稳定、高精度作业质量的关键。

现有的位姿检测方法之一是使用全站仪等精密测量仪器，测量预设于运动目标上若干参考点的空间位置，确定运动目标的位姿信息。该方法的关键部件之一是激光测距传感器。激光测距传感器要求测距激光经运动目标反射后，沿入射方向的反方向返回传感器端，激光传播的总路程是传感器与运动目标距离的两倍，在粉尘环境中激光光强衰减严重。根据Lambert-Beer定律，激光光强与其传播路程呈指数衰减关系，因此激光测距传感器的反射激光光强极其微弱，容易导致激光测距传感器测量失效。

基于陀螺仪与加速度计等惯性导航元件的方法是另一种常见的运动目标位姿检测方法。该方法基本不受环境粉尘等因素的干扰，可达到很高的检测精度；其缺点是惯性导航元件普遍存在零点漂移问题，需反复标定以修正累积误差，而偏移量小的惯性导航元件价格昂贵。

中国专利文献公告号为CN101819036A公开的一种掘进机的空间位姿自动测量方法，在机身上布置特征点阵光源，使用工业相机拍摄特征点阵的图像，通过计算获得掘进机的位姿信息。要使工业相机能采集特征点阵的图像，必须要求特征点阵光源具有较大的发散角，且相机具有较大的视场范围，因此该方法的检测精度不高。

中国专利文献授权公告号为CN101738161B的一种测量运动物体六维位姿的设备和方法，在运动物体周围布置至少一个经纬仪，每个经纬仪上安装至少一个激光器，运动物体上固结至少一个投射面板和成像元件；改变经纬仪的方位角保证激光光斑始终位于投射面板上，根据成像元件图像上的光斑位置，计算运动物体相对于世界坐标系的六维位姿。由于经纬仪布置于地面上，且投射面板与运动物体固结，所以当运动物体航向角和俯仰角较大时，难以保证激光光斑被投射面板接收，因此该方法的角度检测范围有限。

综上，尚未有针对粉尘环境的低成本、高精度、检测范围大的运动目标位姿检测方法。

发明内容

本发明的目的是针对已有技术的不足之处，提出一种基于红外激光的粉尘环境中运动目标位姿检测装置及方法，该方法采用多个传播方向互成一定角度的红外激光器实现了粉尘环境中运动目标位姿检测。采用该方法可以在位姿检测时满足若干目标要求：能够检测运动目标的六维位姿，包括运动目标的航向角、俯仰角和横滚角，以及水平横向、水平纵向和竖直方向位移；系统具有较强的粉尘适应性，能够在粉尘环境中可靠运行；检测范围大，即使在运动目标航向角或俯仰角较大时，仍能检测运动目标的位姿信息；检测精度高，可达到检测量程千分之一的检测精度甚至更高。系统结构简单，检测范围大，检测精度高，粉尘穿透能力强，成本较低，适用于粉尘干扰环境中运动目标位姿检测。

本发明的技术方案如下：

一种基于红外激光的粉尘环境中运动目标位姿检测装置，其特征在于：包括双轴精密转台、激光阵列、光斑检测单元、运动目标、标定板和控制器；所述双轴精密转台包括转台底座、转台外框和转台内框；所述转台底座与所述运动目标固结；所述转台外框和转台内框在伺服电机驱动下绕各自的转轴转动；所述转台外框和转台内框均带有角度编码器；所述光斑检测单元包括光斑接收板和成像元件；所述激光阵列包括激光器底座和至少四个红外激光器；所述激光器底座与所述转台内框固结；红外激光器发出的激光均交汇于所述双轴精密转台与所述光斑检测单元之间的一个空间点；所述红外激光器发出的激光投射在所述光斑接收板上；所述标定板包括前标定板和后标定板；所述前标定板与后标定板相对固定；所述控制器与所述双轴精密转台通过无线传输方式通讯；所述控制器与所述光斑检测单元通过线路直接相连。

上述技术方案中，其特征在于：所述红外激光器发出的激光波长范围位于0.9-14.0μm之间。

本发明所述的光斑接收板、前标定板和后标定板均采用散射透光板，所述红外激光器入射到散射透光板上的激光波长与出射光波长一致；或均采用散射转换板，将入射到散射转换板上的红外激光转换为可见光出射；或均采用红外光敏元件阵列，直接获得所述光斑接收板上的光斑位置。

本发明所述的成像元件为电荷耦合器件、互补金属氧化物半导体成像器件、位置敏感器件或电荷注入器件；所述成像元件的敏感波长范围与所述光斑接收板、前标定板和后标定板的出射光波长范围一致。

本发明提供的一种基于红外激光的粉尘环境中运动目标位姿检测方法，其特征在于该方法包括如下步骤：

1)在所述光斑接收板上建立世界坐标系{W}，所述世界坐标系{W}的平面O_Wz_Wx_W与所述光斑接收板平面重合；在所述运动目标上建立机身坐标系{B}，所述机身坐标系{B}的原点O_B为所述转台底座中心，平面O_Bx_By_B与所述转台内框转轴平行，竖轴z_B与所述转台外框转轴重合；在所述转台内框上建立转台内框坐标系{L}，所述转台内框坐标系{L}的原点O_L为所述转台外框转轴与所述转台内框转轴的交点，横轴x_L与所述转台内框转轴重合，纵轴y_L与所述转台内框转轴垂直；在所述标定板上建立激光标定坐标系{C}；设所述红外激光器发出的激光交汇于所述双轴精密转台与所述光斑检测单元之间的一个空间点F；

2)在标定激光传播路径时，所述成像元件、标定板与转台底座保持相对静止，使用相机标定方法标定所述成像元件与所述激光标定坐标系{C}的相对位置关系；多次改变所述转台外框和转台内框的转角，并使用所述成像元件检测所述红外激光器发出的激光投射在所述前标定板和后标定板上的光斑位置；根据所述成像元件与所述激光标定坐标系{C}的相对位置关系，计算所述前标定板和后标定板上的光斑相对于所述激光标定坐标系{C}的坐标；

设所述红外激光器包括n个激光器，n为不小于4的正整数，所述双轴精密转台转角改变次数为m，m为不小于3的正整数；在第j次改变所述转台外框和转台内框转角时，j是不小于1且不大于m的正整数，第i个红外激光器发出的激光投射在所述前标定板和后标定板上的光斑分别为D_ij,E_ij，i是不小于1且不大于n的正整数，光斑D_ij,E_ij相对于所述激光标定坐标系{C}的坐标分别为第i个红外激光器发出的激光的传播方向向量为S_ij；

激光传播方向向量S_ij相对于所述激光标定坐标系{C}系的坐标由下式确定：

$S_{\mathrm{ij}}^C=\frac{E_{\mathrm{ij}}^C-D_{\mathrm{ij}}^C}{\left|\right|E_{\mathrm{ij}}^C-D_{\mathrm{ij}}^C\left|\right|}$

激光传播路径交汇点F相对于所述激光标定坐标系{C}的坐标满足：

$S_{\mathrm{ij}}^C\×(F_j^C-D_{\mathrm{ij}}^C)=0$

使用线性最小二乘法获得

所述转台内框坐标系{L}与所述机身坐标系{B}的旋转转换矩阵和平移转换矩阵由所述双轴精密转台的尺寸，以及所述转台外框和转台内框的转角确定；

激光传播方向向量S_ij相对于所述转台内框坐标系{L}的坐标以及所述激光标定坐标系{C}与所述机身坐标系{B}之间的旋转转换矩阵满足：

$R_{L_j}^B{S}_i^L=R_C^B{S}_{\mathrm{ij}}^C$

建立目标函数：

$f(S_1^L,S_2^L,\·\·\·S_n^L,R_C^B)=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\left|\right|R_{L_j}^B{S}_i^L-R_C^B{S}_{\mathrm{ij}}^C\left|\right|}^2$

使用非线性最小二乘算法使目标函数f取得最小值，获得

激光传播路径交汇点F相对于所述转台内框坐标系{L}的坐标F^L，以及所述激光标定坐标系{C}与所述机身坐标系{B}之间的平移转换矩阵满足：

$R_{L_j}^B{F}^L+T_{L_j}^B=R_C^B{F}_j^C+T_C^B$

即：

$\left[\begin{array}{cc}{R}_{L_1}^B& -I\\ R_{L_2}^B& -I\\ \·& \·\\ \·& \·\\ \·& \·\\ R_{L_m}^B& -I\end{array}\right]\left[\begin{array}{c}{F}^L\\ T_C^B\end{array}\right]=\left[\begin{array}{c}{R}_C^B{F}_1^C-T_{L_1}^B\\ R_C^B{F}_2^C-T_{L_2}^B\\ \·\\ \·\\ \·\\ R_C^B{F}_m^C-T_{L_m}^B\end{array}\right]$

使用线性最小二乘法获得F^L，

3)在检测运动目标位姿时，使用相机标定方法标定所述成像元件与所述世界坐标系{W}的相对位置关系；点亮所有红外激光器，调节所述转台外框和转台内框的转角，保证初始时刻所有红外激光器发出的激光均投射在光斑接收板上；然后以一定的频率和次序交替点亮所述红外激光器，保证每个时刻有且仅有一个红外激光器被点亮，并使所述成像元件同步采集所述光斑接收板上的激光光斑位置；设第i个红外激光器发出的激光投射在所述光斑接收板上的光斑为A_i；根据所述成像元件与所述世界坐标系{W}的相对位置关系，计算光斑A_i相对于所述世界坐标系{W}的坐标激光传播路径交汇点F与光斑A_i的距离|FA_i|满足：

${\left|{\mathrm{FA}}_k\right|}^2+{\left|{\mathrm{FA}}_t\right|}^2-2\left|{\mathrm{FA}}_k\right|\&CenterDot;\left|{\mathrm{FA}}_t\right|\cos <S_k^L,S_t^L>={\left|\right|A_k^W-A_t^W\left|\right|}^2$

式中，k,t均为不小于1且不大于n的正整数；

建立目标函数：

$g\left(\right|{\mathrm{FA}}_1|,|{\mathrm{FA}}_2|,...,|{\mathrm{FA}}_n\left|\right)$

$=\underset{t=k+1}{\overset{n}{\mathrm{\&Sigma;}}}\underset{k=1}{\overset{n}{\mathrm{\&Sigma;}}}{({\left|{\mathrm{FA}}_k\right|}^2+{\left|{\mathrm{FA}}_t\right|}^2-2|{\mathrm{FA}}_k|\&CenterDot;|{\mathrm{FA}}_t|\cos <S_k^L,S_t^L>-{\left|\right|A_k^W-A_t^W\left|\right|}^2)}^2$

使用非线性最小二乘算法使目标函数g取得最小值，获得|FA_i|；

光斑A_i相对于所述转台内框坐标系{L}的坐标由下式确定：

$A_i^L=F^L+\left|{\mathrm{FA}}_i\right|S_i^L$

所述世界坐标系{W}与所述转台内框坐标系{L}的旋转转换矩阵和平移转换矩阵满足：

$A_i^L=R_W^L{A}_i^W+T_W^L$

建立目标函数：

$h(R_W^L,T_W^L)=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{\left|\right|R_W^L{A}_i^W+T_W^L-A_i^L\left|\right|}^2$

使用非线性最小二乘算法使目标函数h取得最小值，获得

所述转台内框坐标系{L}与所述机身坐标系{B}的旋转转换矩阵和平移转换矩阵由所述双轴精密转台的尺寸，以及所述转台外框和转台内框的转角确定；

所述世界坐标系{W}与所述机身坐标系{B}的旋转转换矩阵和平移转换矩阵由下式确定：

$R_W^B=R_L^B{R}_W^L$

$T_W^B=R_L^B{T}_W^L+T_L^B$

4)根据所述世界坐标系{W}与所述机身坐标系{B}的转换矩阵自动调整所述转台外框和转台内框的转角，实时跟踪所述光斑检测单元，保证所述光斑接收板始终接收到4个或4个以上激光光斑。

本发明与现有技术相比，具有以下优点及突出性效果：采用多个互成角度的红外激光器组成阵列，安装在双轴精密转台上，双轴精密转台安装于被测运动物体上；转台自动跟踪光斑检测单元，保证多束激光始终被光斑检测单元接收；根据光斑检测单元上的光斑位置以及转台转角，通过计算获得运动目标的位姿信息，包括其相对于世界坐标系的航向角、俯仰角和横滚角，以及水平横向、水平纵向和竖直方向位移；激光波长位于红外波段，具有较强的粉尘穿透能力；多束激光的传播路径交汇于双轴精密转台与光斑检测单元之间的一个空间点，交汇点位置可精确标定；光斑检测单元上的激光光斑相距较小，因此可使用视场范围较小的成像元件或尺寸较小的光敏元件阵列进行检测，以获得较高的位姿检测精度；激光阵列可随双轴精密转台转动，实时跟踪光斑检测单元，即使在运动目标航向角和俯仰角较大时，仍能保证激光光斑始终投射在光斑检测单元上；系统结构简单，检测范围大，检测精度高，粉尘穿透能力强，成本较低，适用于粉尘干扰环境中运动目标位姿检测。

附图说明

图1为基于红外激光的粉尘环境中运动目标位姿检测装置及方法的第一个实施例结构原理示意图。

图2为基于红外激光的粉尘环境中运动目标位姿检测装置及方法的第二个实施例结构原理示意图。

图3为本发明中激光传播路径标定原理示意图。

图4为本发明中双轴精密转台的结构示意图。

图5为本发明中激光传播路径与激光标定坐标系相对位置关系的示意图。

图6为本发明中激光传播路径与世界坐标系相对位置关系的示意图。

图7为本发明中运动目标位姿检测与跟踪方法流程图。

在图1至图7中：

1—双轴精密转台；11—转台底座；12—转台外框；13—转台内框；2—激光阵列；21—红外激光器；22—激光器底座；3—光斑检测单元；31—光斑接收板；32—成像元件；4—运动目标；5—标定板；51—前标定板；52—后标定板；6—控制器。

具体实施方式

下面结合附图对本发明的结构、原理和工作过程作进一步说明。

图1为本发明提出的基于红外激光的粉尘环境中运动目标位姿检测方法的第一个实施例结构原理示意图，包括双轴精密转台1、激光阵列2、光斑检测单元3、运动目标4、标定板5和控制器6；所述双轴精密转台1包括转台底座11、转台外框12和转台内框13；所述转台底座11与所述运动目标4固结；所述光斑检测单元3包括光斑接收板31和成像元件32；所述激光阵列2包括激光器底座22和至少四个红外激光器21；所述激光器底座22与所述转台内框13固结；红外激光器21发出的激光均交汇于所述双轴精密转台1与所述光斑检测单元3之间的一个空间点；所述红外激光器21发出的激光投射在所述光斑接收板31上；所述标定板5包括前标定板51和后标定板52；所述前标定板51与后标定板52相对固定；所述控制器6与所述双轴精密转台1通过无线传输方式通讯；所述控制器6与所述光斑检测单元3通过线路直接相连。

图3为本发明中激光传播路径标定原理图。在进行激光传播路径标定时，成像元件32、标定板5和转台底座11保持相对静止，使用张正友标定方法标定成像元件32与标定板5的相对位置关系。多次改变双轴精密转台1的姿态角，记录前标定板51和后标定板52上的多个光斑位置，以及转台外框12和转台内框13的相应转角。

图4为本发明中双轴精密转台的结构示意图。双轴精密转台具有水平转动和俯仰转动两个自由度，转台外框12和转台内框13在伺服电机的驱动下绕各自的转轴转动，且均带有角度编码器。转台内框13转轴与转台底座11底面（即平面O_Bx_By_B）的距离为H。记转台外框12的转角（即x_B轴与x_L轴的夹角）为α，转台内框的转角（即z_B轴与z_L轴的夹角）为β。

在所述光斑接收板31上建立世界坐标系{W}，所述世界坐标系{W}的平面O_Wz_Wx_W与所述光斑接收板31平面重合；在所述运动目标上的建立机身坐标系{B}，所述机身坐标系{B}的原点O_B为所述转台底座11中心，平面O_Bx_By_B与所述转台内框13转轴平行，竖轴z_B与所述转台外框12转轴重合；在所述转台内框13上建立转台内框坐标系{L}，所述转台内框坐标系{L}的原点O_L为所述转台外框12转轴与所述转台内框13转轴的交点，横轴x_L与所述转台内框13转轴重合，纵轴y_L与所述转台内框13转轴垂直；在所述标定板5上建立激光标定坐标系{C}，所述激光标定坐标系{C}的平面O_Cz_Cx_C与后标定板52平面重合；设所述红外激光器21发出的激光交汇于所述双轴精密转台1与所述光斑检测单元3之间的一个空间点F。

图5为本发明中激光传播路径与激光标定坐标系{C}相对位置关系的示意图。红外激光器21发出的激光交汇于点F后继续传播，并依次投射在前标定板51和后标定板52上，记投射在前标定板51上的光斑分别为D₁,D₂,D₃,D₄，投射在后标定板52上的光斑分别为E₁,E₂,E₃,E₄。

图6为本发明中激光传播路径与世界坐标系{W}相对位置关系的示意图。红外激光器21发出的激光交汇于点F后继续传播，并投射在光斑接收板31上，记投射在光斑接收板31上的光斑分别为A₁,A₂,A₃,A₄。

图7为本发明中运动目标位姿检测与跟踪方法流程图。初始时，点亮所有红外激光器，调整转台外框12和转台内框13的转角，保证所有红外激光器发出的激光均投射在光斑接收板31上；然后使红外激光器以一定的频率和次序交替点亮，保证每个时刻有且仅有一个红外激光器点亮，并使成像元件32同步采集光斑接收板31上的激光光斑图像。红外激光器的交替点亮频率可根据运动目标速度进行选择，运动目标速度越大，则要求红外激光器的交替点亮频率越高。激光光斑图像由控制器6进行处理后，得到运动目标的位姿信息。根据此位姿信息，计算转台外框12和转台内框13的转角调整量，并自动调整转台外框12和转台内框13的转角，实时跟踪光斑检测单元3，保证至少4个红外激光器发出的激光投射在光斑接收板31上。

假设所述粉尘环境为粉尘浓度达500mg/m³的煤矿巷道，粉尘粒径在0.25-10μm之间，80%-90%的粉尘粒径小于5μm，46.5%-60%的粉尘粒径小于2μm；粉尘比重为0.72-2.20g/cm³，粉尘间的平均间距约为粒径的90-130倍；粉尘消光系数为0.9-2.3cm²/mg；要求检测20-30m距离范围内的运动目标。在此粉尘环境中，粉尘对激光的散射与吸收等因素使光斑接收板31接收到的激光能量约为原来的3%-41%，且激光会受到粉尘颗粒遮挡而产生衰减。据此，红外激光器21选取波长为1.06μm的4个红外激光器，激光波长与粉尘粒径相近，绕射能力好，具有较强的粉尘环境穿透能力；每个激光光源的功率为150mW，远场发散角为0.1mrad。成像元件32为工业CCD相机，图像大小为1024×1024，视场范围为200mm×200mm，图像上每个像素对应实际距离0.02mm。光斑接收板31、前标定板51和后标定板52均采用上转换玻璃，可将1.06μm红外激光转换为550μm可见光，前标定板51与后标定板52相互平行且相距300mm。转台外框12和转台内框13的转角范围均为0°-360°，重复定位精度均为0.01°，分辨率均为0.005°，转台底座11中心与转台内框13转轴的距离H=1010mm。

设红外激光器21包括n个激光器，其中n为正整数，且n≥4。

以下就激光传播路径标定、运动目标位姿检测和光斑检测单元跟踪方法等方面说明实施例的具体工作原理。

1.激光传播路径标定

激光传播路径交汇点F和第i个红外激光器发出的激光的传播方向向量S_i相对于转台内框坐标系{L}的坐标不随转台外框12和转台内框13的转角变化而变化，记其分别为F^L,其中i为正整数，且1≤i≤n；设激光标定坐标系{C}与机身坐标系{B}的旋转转换矩阵和平移转换矩阵分别为在标定激光传播路径时，成像元件32、标定板5与转台底座11保持相对静止，因此均为常量。激光传播路径标定是求解F^L,的过程。

在标定激光传播路径前，首先使用张正友标定方法标定成像元件32与激光标定坐标系{C}的相对位置关系，即：

P₁ ^C＝μ₁(p₁)(1)

$P_2^C={\mathrm{\&mu;}}_2\left(p_2\right)---\left(2\right)$

式中，P₁ ^C 分别为前标定板51和后标定板52上任意点P₁,P₂相对于激光标定坐标系{C}的坐标，p₁,p₂分别为点P₁,P₂在成像元件32图像中的像素坐标，函数μ₁,μ₂分别为P₁ ^C与p₁、与p₂之间的转换函数。函数μ₁,μ₂由张正友标定方法给出。

多次改变转台外框12和转台内框13的转角，并使用成像元件32检测红外激光器21发出的激光投射在前标定板51和后标定板52上的光斑位置。假设双轴精密转台1转角的改变次数为m，其中m为正整数，且m≥3。在第j次改变所述转台外框12和转台内框13转角时（j为正整数，且1≤j≤m），记第i个红外激光器发出的激光投射在所述前标定板51和后标定板52上的光斑分别为D_ij,E_ij，光斑D_ij,E_ij在成像元件32图像中的像素坐标分别为d_ij,e_ij；设光斑D_ij,E_ij相对于激光标定坐标系{C}的坐标分别为第i个红外激光器发出的激光的传播方向向量为S_ij，转台外框12转角为α_j，转台内框13转角为β_j。

根据成像元件32与激光标定坐标系{C}的相对位置关系，由式(1)和式(2)计算前标定板51和后标定板52上的光斑相对于所述激光标定坐标系{C}的坐标，即：

$D_{\mathrm{ij}}^C={\mathrm{\&mu;}}_1\left(d_{\mathrm{ij}}\right)---\left(3\right)$

$E_{\mathrm{ij}}^C={\mathrm{\&mu;}}_2\left(e_{\mathrm{ij}}\right)---\left(4\right)$

激光传播方向向量S_ij相对于激光标定坐标系{C}系的坐标由下式确定：

$S_{\mathrm{ij}}^C=\frac{E_{\mathrm{ij}}^C-D_{\mathrm{ij}}^C}{\left|\right|E_{\mathrm{ij}}^C-D_{\mathrm{ij}}^C\left|\right|}---\left(5\right)$

由于第i个红外激光器发出的激光传播路径通过点F，因此激光传播路径交汇点F相对于激光标定坐标系{C}的坐标满足：

$S_{\mathrm{ij}}^C\&times;(F_j^C-D_{\mathrm{ij}}^C)=0---\left(6\right)$

令

$S_{\mathrm{ij}}^C={\left[\begin{array}{ccc}{s}_{{\mathrm{ij}}_1}^C& s_{{\mathrm{ij}}_2}^C& s_{{\mathrm{ij}}_3}^C\end{array}\right]}^T---\left(7\right)$

$M_{\mathrm{ij}}^C=\left[\begin{array}{ccc}0& -s_{{\mathrm{ij}}_3}^C& s_{{\mathrm{ij}}_2}^C\\ s_{{\mathrm{ij}}_3}^C& 0& {-s}_{{\mathrm{ij}}_1}^C\\ {-s}_{{\mathrm{ij}}_2}^C& s_{{\mathrm{ij}}_1}^C& 0\end{array}\right]---\left(8\right)$

式中，为向量的三个分量，则式(6)等价于：

$\left[\begin{array}{c}{M}_{1j}^C\\ M_{2j}^C\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ M_{\mathrm{nj}}^C\end{array}\right]F_j^C=\left[\begin{array}{c}{M}_{1j}^C{D}_{\mathrm{ij}}^C\\ M_{2j}^C{D}_{2j}^C\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ M_{\mathrm{nj}}^C{D}_{\mathrm{nj}}^C\end{array}\right]---\left(9\right)$

使用线性最小二乘法求解式(9)，获得

转台内框坐标系{L}与机身坐标系{B}的旋转转换矩阵和平移转换矩阵由双轴精密转台1的尺寸，以及转台外框12和转台内框13的转角确定，即：

$R_{L_j}^B=\left[\begin{array}{ccc}{\cos \mathrm{\&alpha;}}_j& {\sin \mathrm{\&alpha;}}_j\cos {\mathrm{\&beta;}}_j& {-\sin \mathrm{\&alpha;}}_j{\sin \mathrm{\&beta;}}_j\\ {-\sin \mathrm{\&alpha;}}_j& {\cos \mathrm{\&alpha;}}_j\cos {\mathrm{\&beta;}}_j& {-\cos \mathrm{\&alpha;}}_j{\sin \mathrm{\&beta;}}_j\\ 0& {\sin \mathrm{\&beta;}}_j& {\cos \mathrm{\&beta;}}_j\end{array}\right]---\left(10\right)$

$T_{L_j}^B=\left[\begin{array}{c}0\\ 0\\ H\end{array}\right]---\left(11\right)$

激光传播方向向量S_ij相对于转台内框坐标系{L}的坐标以及激光标定坐标系{C}与机身坐标系{B}之间的旋转转换矩阵满足：

$R_{L_j}^B{S}_i^L=R_C^B{S}_{\mathrm{ij}}^C---\left(12\right)$

只有当测量次数m≥3时，式(12)才可能有唯一解，此时建立目标函数：

$f(S_1^L,S_2^L,...S_n^L,R_C^B)=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{m}{\mathrm{\&Sigma;}}}{\left|\right|R_{L_j}^B{S}_i^L-R_C^B{S}_{\mathrm{ij}}^C\left|\right|}^2---\left(13\right)$

使用高斯-牛顿法、Levenberg-Marquardt或拟牛顿法等非线性最小二乘算法使目标函数f取得最小值，获得

激光传播路径交汇点F相对于转台内框坐标系{L}的坐标F^L，以及激光标定坐标系{C}与机身坐标系{B}之间的平移转换矩阵满足：

$R_{L_j}^B{F}^L+T_{L_j}^B=R_C^B{F}_j^C+T_C^B---\left(14\right)$

即：

$\left[\begin{array}{cc}{R}_{L_1}^B& -I\\ R_{L_2}^B& -I\\ \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;\\ R_{L_m}^B& -I\end{array}\right]\left[\begin{array}{c}{F}^L\\ T_C^B\end{array}\right]=\left[\begin{array}{c}{R}_C^B{F}_1^C-T_{L_1}^B\\ R_C^B{F}_2^C-T_{L_2}^B\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ R_C^B{F}_m^C-T_{L_m}^B\end{array}\right]---\left(15\right)$

使用线性最小二乘法求解式(15)，获得F^L,

至此F^L,均被求解出来，激光传播路径标定完毕。

2.运动目标位姿检测

运动目标位姿检测是求解世界坐标系{W}与机身坐标系{B}之间的转换矩阵的过程。

在检测运动目标位姿时，使用张正友标定方法标定成像元件32与世界坐标系{W}的相对位置关系，即：

P^W=μ₃(p)(16)

式中，P^W为光斑接收板31上任意一点P相对于世界坐标系{W}的坐标，p为点P在成像元件32图像中的像素坐标，函数μ₃为P^W与p之间的转换函数。函数μ₃由张正友标定方法获得。

首先点亮所有红外激光器，调节转台外框12和转台内框13的转角，保证初始时刻所有红外激光器发出的激光均投射在光斑接收板31上；然后以一定的频率和次序交替点亮红外激光器，保证每个时刻有且仅有一个红外激光器被点亮，并使成像元件32同步采集所述光斑接收板上的激光光斑位置；设第i个红外激光器发出的激光投射在光斑接收板31上的光斑为A_i，光斑A_i在成像元件32图像中的像素坐标为a_i；根据成像元件32与所述世界坐标系{W}的相对位置关系，计算光斑A_i相对于世界坐标系{W}的坐标即：

$A_i^W={\mathrm{\&mu;}}_3\left(a_i\right)---\left(17\right)$

激光传播路径交汇点F与光斑A_i的距离|FA_i|满足：

${\left|{\mathrm{FA}}_k\right|}^2+{\left|{\mathrm{FA}}_t\right|}^2-2\left|{\mathrm{FA}}_k\right|\&CenterDot;\left|{\mathrm{FA}}_t\right|\cos <S_k^L,S_t^L>={\left|\right|A_k^W-A_t^W\left|\right|}^2---\left(18\right)$

式中，k,t均为正整数，且1≤k≤n，1≤t≤n。

式(18)表示的方程等价于机器视觉中的PnP问题，其中激光传播路径交汇点F等价于PnP问题中待求的摄像机光心位置。根据PnP问题理论，当n=2时，式(18)有无穷多组解；当n=3时，式(18)一般有两组解；只有当n≥4时，式(18)才有唯一解，因此红外激光器的数量至少为4个。

建立目标函数：

$g\left(\right|{\mathrm{FA}}_1|,|{\mathrm{FA}}_2|,...,|{\mathrm{FA}}_n\left|\right)$

$=\underset{t=k+1}{\overset{n}{\mathrm{\&Sigma;}}}\underset{k=1}{\overset{n}{\mathrm{\&Sigma;}}}{({\left|{\mathrm{FA}}_k\right|}^2+{\left|{\mathrm{FA}}_t\right|}^2-2|{\mathrm{FA}}_k|\&CenterDot;|{\mathrm{FA}}_t|\cos <S_k^L,s_t^L>-{\left|\right|A_k^W-A_t^W\left|\right|}^2)}^2---\left(19\right)$

使用高斯-牛顿法、Levenberg-Marquardt或拟牛顿法等非线性最小二乘算法使目标函数g取得最小值，获得|FA_i|。

光斑A_i相对于所述转台内框坐标系{L}的坐标由下式确定：

$A_i^L=F^L+\left|{\mathrm{FA}}_i\right|S_i^L---\left(20\right)$

世界坐标系{W}与转台内框坐标系{L}的旋转转换矩阵和平移转换矩阵满足：

$A_i^L=R_W^L{A}_i^W+T_W^L---\left(21\right)$

建立目标函数：

$h(R_W^L,T_W^L)=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{\left|\right|R_W^L{A}_i^W+T_W^L-A_i^L\left|\right|}^2---\left(22\right)$

使用高斯-牛顿法、Levenberg-Marquardt或拟牛顿法等非线性最小二乘算法使目标函数h取得最小值，获得

转台内框坐标系{L}与所述机身坐标系{B}的旋转转换矩阵和平移转换矩阵由双轴精密转台1的尺寸，以及转台外框12和转台内框13的转角确定，即：

$R_L^B=\left[\begin{array}{ccc}\cos \mathrm{\&alpha;}& \sin \mathrm{\&alpha;}\cos \mathrm{\&beta;}& -\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\\ -\sin \mathrm{\&alpha;}& \cos \mathrm{\&alpha;}\cos \mathrm{\&beta;}& -\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\\ 0& \sin \mathrm{\&beta;}& \cos \mathrm{\&beta;}\end{array}\right]---\left(23\right)$

$T_L^B=\left[\begin{array}{c}0\\ 0\\ H\end{array}\right]---\left(24\right)$

式中，α为转台外框12的转角，β为转台内框13的转角。

世界坐标系{W}与机身坐标系{B}的旋转转换矩阵和平移转换矩阵由下式确定：

$R_W^B=R_L^B{R}_W^L---\left(25\right)$

$T_W^B=R_L^B{T}_W^L+T_L^B---\left(26\right)$

被检测的运动目标相对于世界坐标系{W}的横向位移、纵向偏移与竖直方向偏移分别等于矩阵的三个分量；被检测的运动目标相对于世界坐标系{W}的航向角ψ、俯仰角θ与横滚角γ满足式(27)：

$R_W^B=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}\\ \sin \mathrm{\&psi;}\cos \mathrm{\&theta;}& \cos \mathrm{\&psi;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\\ \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]---\left(27\right)$

令的第ξ行、第η列的元素为r_ξη，其中ξ,η在集合{1,2,3}中取值，则：

θ=sin^-1(r₂₃)(28)

$\mathrm{\&psi;}={\tan }^{-1}\left(\frac{r_{21}}{r_{22}}\right)---\left(29\right)$

$\mathrm{\&gamma;}={\tan }^{-1}(-\frac{r_{13}}{r_{33}})---\left(30\right)$

至此，被检测的运动目标相对于世界坐标系{W}的位姿检测完毕。

3.光斑检测单元跟踪方法

光斑接收板31平面在世界坐标系{W}中的方程为[010]x^W=0，在机身坐标系{B}中的方程为：

α^Tx^B=q(31)

式中，x^B为光斑接收板31平面上任意一点相对于机身坐标系{B}的坐标，且

$\mathrm{\&alpha;}=R_W^B\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]---\left(32\right)$

$q={\mathrm{\&alpha;}}^T{T}_W^B---\left(33\right)$

定义向量S^L：

$S^L=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{S}_i^L---\left(34\right)$

可见，S^L为n个激光器发出的激光传播方向向量的平均值。

在转台内框坐标系{L}中，经过激光传播路径交汇点F^L、且以S^L为方向向量的直线方程为（设该直线上任意一点相对于转台内框坐标系{L}中的坐标为x^L）：

x^L=F^L+lS^L(35)

式中，l为直线方程的参数。

设式(35)表示的直线上任意一点相对于机身坐标系{B}中的坐标为x^B′，则式(35)表示的直线在机身坐标系{B}中的方程为：

$x^{B\&prime;}=R_L^B{F}^L+T_L^B+l{R}_L^B{S}^L---\left(36\right)$

式(36)表示的直线与光斑接收板31平面的交点在机身坐标系{B}中的坐标G^B为：

$G^B=R_L^B{F}^L+T_L^B+\frac{q-{\mathrm{\&alpha;}}^T(R_L^B{F}^L+T_L^B)}{{\mathrm{\&alpha;}}^T{R}_L^B{S}^L}{R}_L^B{S}^L---\left(37\right)$

若保证式(36)表示的直线始终经过成像元件32的视场中心，则可保证尽可能多的激光光斑被光斑检测单元3接收，设成像元件32的视场中心在世界坐标系{W}中的坐标为则：

$G^B=R_W^B{G}_{\mathrm{mid}}^W+T_W^B---\left(38\right)$

据式(31)至式(38)可计算在式(36)表示的直线经过成像元件32视场中心时，转台外框12和转台内框13的转角，设计相应的控制器，保证红外激光器21发出的激光始终投射在光斑接收板31上。

图2为本发明提出的基于红外激光的粉尘环境中运动目标位姿检测方法的第二个实施例结构原理示意图。与第一个实施例不同的是，第二个实施例光斑接收板31为红外光敏三极管阵列，且不包括成像元件32。红外光敏三极管阵列可直接获得激光光斑的位置坐标，因此无需进行成像元件标定、图像处理等步骤，系统组成更为简单，激光传播路径标定、运动目标检测与光斑检测单元跟踪方法与实施例基本相同。

应当说明的是，以上实施例仅用于说明本发明而并非限制本发明描述的方案；因此，尽管本说明书参照以上的各个实施例对本发明进行了详细的说明，但是本领域的普通技术人员应当理解，仍然可以对本发明进行修改或等同替换，如将光斑接收板替换为散射透光玻璃、成像元件替换为红外摄像机、前标定板与后标定板相对固定且相互成一定角度等；而一切不脱离发明的精神和范围的技术方案及其改进，其均应涵盖在本发明的权利要求范围当中。

本发明采用多个互成角度的红外激光器组成阵列，安装在双轴精密转台上，双轴精密转台安装于被测运动物体上；转台自动跟踪光斑检测单元，保证多束激光始终被光斑检测单元接收；根据光斑检测单元上的光斑位置以及转台转角，通过计算获得运动目标的位姿信息，包括其相对于世界坐标系的航向角、俯仰角和横滚角，以及水平横向、水平纵向和竖直方向位移；激光波长位于红外波段，具有较强的粉尘穿透能力；多束激光的传播路径交汇于双轴精密转台与光斑检测单元之间的一个空间点，交汇点位置可精确标定；光斑检测单元上的激光光斑相距较小，因此可使用视场范围较小的成像元件或尺寸较小的光敏元件阵列进行检测，以获得较高的位姿检测精度；激光阵列可随双轴精密转台转动，实时跟踪光斑检测单元，即使在运动目标航向角和俯仰角较大时，仍能保证激光光斑始终投射在光斑检测单元上；系统结构简单，检测范围大，检测精度高，粉尘穿透能力强，成本较低，适用于粉尘干扰环境中运动目标位姿检测。

Claims (4)

1.一种基于红外激光的粉尘环境中运动目标位姿检测方法，其特征在于，该方法采用的装置包括双轴精密转台(1)、激光阵列(2)、光斑检测单元(3)、运动目标(4)、标定板(5)和控制器(6)；所述双轴精密转台(1)包括转台底座(11)、转台外框(12)和转台内框(13)；所述转台底座与所述运动目标固结；所述转台外框和转台内框在伺服电机驱动下绕各自的转轴转动；所述转台外框和转台内框均带有角度编码器；所述光斑检测单元(3)包括光斑接收板(31)和成像元件(32)；所述激光阵列(2)包括激光器底座(22)和至少四个红外激光器(21)；所述激光器底座与所述转台内框固结；红外激光器发出的激光均交汇于所述双轴精密转台与所述光斑检测单元之间的一个空间点；所述红外激光器发出的激光投射在所述光斑接收板上；所述标定板(5)包括前标定板(51)和后标定板(52)；所述前标定板与后标定板相对固定；所述控制器(6)与所述双轴精密转台通过无线传输方式通讯；所述控制器与所述光斑检测单元通过线路直接相连；

所述方法包括如下步骤：

1)在所述光斑接收板上建立世界坐标系{W}，所述世界坐标系{W}的平面O_Wz_Wx_W与所述光斑接收板平面重合；在所述运动目标上建立机身坐标系{B}，所述机身坐标系{B}的原点O_B为所述转台底座中心，平面O_Bx_By_B与所述转台内框转轴平行，竖轴z_B与所述转台外框转轴重合；在所述转台内框上建立转台内框坐标系{L}，所述转台内框坐标系{L}的原点O_L为所述转台外框转轴与所述转台内框转轴的交点，横轴x_L与所述转台内框转轴重合，纵轴y_L与所述转台内框转轴垂直；在所述标定板上建立激光标定坐标系{C}；设所述红外激光器发出的激光交汇于所述双轴精密转台与所述光斑检测单元之间的一个空间点F；

2)在标定激光传播路径时，所述成像元件、标定板与转台底座保持相对静止，使用相机标定方法标定所述成像元件与所述激光标定坐标系{C}的相对位置关系；多次改变所述转台外框和转台内框的转角，并使用所述成像元件检测所述红外激光器发出的激光投射在所述前标定板和后标定板上的光斑位置；根据所述成像元件与所述激光标定坐标系{C}的相对位置关系，计算所述前标定板和后标定板上的光斑相对于所述激光标定坐标系{C}的坐标；

设所述红外激光器包括n个激光器，n为不小于4的正整数，所述双轴精密转台转角改变次数为m，m为不小于3的正整数；在第j次改变所述转台外框和转台内框转角时，j是不小于1且不大于m的正整数，第i个红外激光器发出的激光投射在所述前标定板和后标定板上的光斑分别为D_ij,E_ij，i是不小于1且不大于n的正整数，光斑D_ij,E_ij相对于所述激光标定坐标系{C}的坐标分别为第i个红外激光器发出的激光的传播方向向量为S_ij；

激光传播方向向量S_ij相对于所述激光标定坐标系{C}系的坐标由下式确定：

$S_{ij}^C=\frac{E_{ij}^C-D_{ij}^C}{\left|\right|E_{ij}^C-D_{ij}^C\left|\right|}$

激光传播路径交汇点F相对于所述激光标定坐标系{C}的坐标满足：

$S_{ij}^C\&times;(F_j^C-D_{ij}^C)=0$

使用线性最小二乘法获得

所述转台内框坐标系{L}与所述机身坐标系{B}的旋转转换矩阵和平移转换矩阵由所述双轴精密转台的尺寸，以及所述转台外框和转台内框的转角确定；

激光传播方向向量S_ij相对于所述转台内框坐标系{L}的坐标以及所述激光标定坐标系{C}与所述机身坐标系{B}之间的旋转转换矩阵满足：

$R_{L_j}^B{S}_i^L=R_C^B{S}_{ij}^C$

建立目标函数：

$f(S_1^L,S_2^L,...S_n^L,R_C^B)=\underset{i=1}{\overset{n}{\&Sigma;}}\underset{j=1}{\overset{m}{\&Sigma;}}\left|\right|R_{L_j}^B{S}_i^L-R_C^B{S}_{ij}^C|{|}^2$

使用非线性最小二乘算法使目标函数f取得最小值，获得

激光传播路径交汇点F相对于所述转台内框坐标系{L}的坐标F^L，以及所述激光标定坐标系{C}与所述机身坐标系{B}之间的平移转换矩阵满足：

$R_{L_j}^B{F}^L+T_{L_j}^B=R_C^B{F}_j^C+T_C^B$

即：

$\left[\begin{array}{cc}{R}_{L_1}^B& -I\\ R_{L_2}^B& -I\\ \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;\\ R_{L_m}^B& -I\end{array}\right]\left[\begin{array}{c}{F}^L\\ T_C^B\end{array}\right]=\left[\begin{array}{c}{R}_C^B{F}_1^C-T_{L_1}^B\\ R_C^B{F}_2^C-T_{L_2}^B\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ R_C^B{F}_m^C-T_{L_m}^B\end{array}\right]$

使用线性最小二乘法获得F^L,

3)在检测运动目标位姿时，使用相机标定方法标定所述成像元件与所述世界坐标系{W}的相对位置关系；点亮所有红外激光器，调节所述转台外框和转台内框的转角，保证初始时刻所有红外激光器发出的激光均投射在光斑接收板上；然后以一定的频率和次序交替点亮所述红外激光器，保证每个时刻有且仅有一个红外激光器被点亮，并使所述成像元件同步采集所述光斑接收板上的激光光斑位置；设第i个红外激光器发出的激光投射在所述光斑接收板上的光斑为A_i；根据所述成像元件与所述世界坐标系{W}的相对位置关系，计算光斑A_i相对于所述世界坐标系{W}的坐标激光传播路径交汇点F与光斑A_i的距离|FA_i|满足：

$|{\mathrm{FA}}_k{|}^2+|{\mathrm{FA}}_t{|}^2-2\left|{\mathrm{FA}}_k\right|\&CenterDot;\left|{\mathrm{FA}}_t\right|cos<S_k^L,S_t^L>=\left|\right|A_k^W-A_t^W|{|}^2$

式中，k,t均为不小于1且不大于n的正整数；

建立目标函数：

$\begin{array}{c}g\left(\right|{\mathrm{FA}}_1|,|{\mathrm{FA}}_2|,...,|{\mathrm{FA}}_n\left|\right)\\ =\underset{t=k+1}{\overset{n}{\mathrm{\&Sigma;}}}\underset{k=1}{\overset{n}{\mathrm{\&Sigma;}}}{\left(\right|{\mathrm{FA}}_k{|}^2+|{\mathrm{FA}}_t{|}^2-2|{\mathrm{FA}}_k|\&CenterDot;|{\mathrm{FA}}_t|\cos <S_k^L,S_t^L>-||A_k^W-A_t^W|{|}^2)}^2\end{array}$

使用非线性最小二乘算法使目标函数g取得最小值，获得|FA_i|；

光斑A_i相对于所述转台内框坐标系{L}的坐标由下式确定：

$A_i^L=F^L+\left|{\mathrm{FA}}_i\right|S_i^L$

所述世界坐标系{W}与所述转台内框坐标系{L}的旋转转换矩阵和平移转换矩阵满足：

$A_i^L=R_W^L{A}_i^W+T_W^L$

建立目标函数：

$h(R_W^L,T_W^L)=\underset{i=1}{\overset{n}{\&Sigma;}}\left|\right|R_W^L{A}_i^W+T_W^L-A_i^L|{|}^2$

使用非线性最小二乘算法使目标函数h取得最小值，获得

所述转台内框坐标系{L}与所述机身坐标系{B}的旋转转换矩阵和平移转换矩阵由所述双轴精密转台的尺寸，以及所述转台外框和转台内框的转角确定；

所述世界坐标系{W}与所述机身坐标系{B}的旋转转换矩阵和平移转换矩阵由下式确定：

$R_W^B=R_L^B{R}_W^L$

$T_W^B=R_L^B{T}_W^L+T_L^B$

4)根据所述世界坐标系{W}与所述机身坐标系{B}的转换矩阵自动调整所述转台外框和转台内框的转角，实时跟踪所述光斑检测单元，保证所述光斑接收板始终接收到4个或4个以上激光光斑。

2.如权利要求1所述的一种基于红外激光的粉尘环境中运动目标位姿检测方法，其特征在于：所述红外激光器发出的激光波长范围位于0.9-14.0μm之间。

3.如权利要求1所述的一种基于红外激光的粉尘环境中运动目标位姿检测方法，其特征在于：所述光斑接收板、前标定板和后标定板均采用散射透光板，所述红外激光器入射到散射透光板上的激光波长与出射光波长一致；或均采用散射转换板，将入射到散射转换板上的红外激光转换为可见光出射；或均采用红外光敏元件阵列，直接获得所述光斑接收板上的光斑位置。

4.如权利要求1所述的一种基于红外激光的粉尘环境中运动目标位姿检测方法，其特征在于：所述成像元件为电荷耦合器件、互补金属氧化物半导体成像器件、位置敏感器件或电荷注入器件；所述成像元件的敏感波长范围与所述光斑接收板、前标定板和后标定板的出射光波长范围一致。