专利名称：激光跟踪惯性组合测量系统及其测量方法
技术领域：
本发明属于精密测量技术领域，涉及一种惯性仪表与激光跟踪仪器组合观测运动目标空间位置和姿态的测量方法和系统设计。
背景技术：
目前工业领域中常见的运动目标位置和姿态(简称位姿)的测量技术主要有三坐标测量机(CMM)、电子经纬仪跟踪系统、全站仪极坐标测量系统以及CCD摄像系统。它们的位姿计算原理分别为在位置坐标测量上，CMM采用测长系统直接输出，电子经纬仪采用空间三角法，全站仪极坐标测量系统采用极坐标法，立体摄像系统采用视觉变换和坐标变换。在姿态测量上，运动目标的姿态测量基本上仅限于机器人终端效应器的姿态标定，一般综合采用上述各种位置坐标测量技术跟踪测量多点坐标以解算姿态。近来有人采用立体CCD主动摄像方法测量运动目标的姿态，采用一个跟踪站，使用角隅棱镜的三个棱之间的夹角计算目标的姿态。上述运动目标位姿动态测量技术中，CMM的测量为接触式测量，范围有限；电子经纬仪的测量精度有限；全站仪的绝对精度差；而CCD摄像系统的测量精度受背景光及光源位置的影响；而且上述各方法均为静态或极低速的测量。近年来工业领域内的运动目标位姿测量研究主要活跃于激光跟踪测量系统的研究。目前通常采用激光干涉法和三角法测量技术。该方法采用激光作为光源，在被测目标上安装光学反射镜，通过激光束不间断地跟踪反射镜，然后采用多站纯距离法和单站角度距离法的解算方法实时测量被测目标的位置坐标并通过测量多点坐标解算目标姿态。激光跟踪测量系统具有动态测量精度高、无接触测量、动态响应性好的优点，但同时也存在着造价昂贵、易受外界干扰的缺点。
惯性测量技术是以军事需求为动力而发展成熟起来的，惯性技术主要应用于运载体的导航与制导。惯性测量技术具有自主测量、动态性能好、频带宽、输出数据可包含系统的速度、加速度信息的优点；但是惯性测量技术存在着定位精度随时间增加而降低、绝对精度差的缺点。
发明内容
本发明的目的在于克服传统位置及姿态测量装置的不足之处，提高现有激光动态跟踪测量系统的稳定性，提高现有动态跟踪测量系统的实时姿态测量精度，丰富现有动态跟踪测量系统的测量输出信息，提出一种目标空间位置和姿态的激光跟踪惯性组合测量系统及其测量方法，采用融合惯性测量技术与激光跟踪测量技术的组合测量三维运动目标的空间位姿，可以实现快速运动对象空间位置、速度、加速度以及姿态、角速率的动态实时跟踪测量。
本发明公开了一种激光跟踪惯性组合测量系统，其特征在于，该系统包括惯性测量分系统、激光跟踪测量分系统以及分别与惯性测量分系统和激光跟踪测量分系统相连接的计算机控制与数据采集处理分系统，所述的惯性测量分系统是指能够完成目标全姿态测量所需的按平台式工作方式或捷联式工作方式工作的惯性仪表组；所述的激光跟踪测量分系统包括激光跟踪站群和接受并反射所述激光跟踪站群发射的激光信号的目标反射镜组；所述的计算机控制与数据采集处理分系统包括存储多传感器数据融合算法软件程序的计算机，及其外围转换电路。
本发明所述惯性仪表组是包括三个单自由度陀螺或两个二自由度陀螺，以及三个加速度计；所述陀螺的三个敏感轴与加速度计的三个敏感轴保持方向一致，并构成一个平台坐标系(直角坐标系)。
本发明所述加速度计为挠性加速度计、振弦式加速度计或静电式加速度计中的一种；所述陀螺为静电陀螺、挠性陀螺或激光陀螺中的一种。
本发明所述目标反射镜组是由固定于被测运动目标上的一个以上反射镜组成的，所述反射镜是平面反射镜、实心角隅棱镜、空心角隅棱镜或“猫眼”反射镜中的一种。
本发明所述激光跟踪站群中的每个激光跟踪站包括激光测距子系统、二维伺服跟踪子系统、光电检测子系统和反馈控制跟踪子系统，以及连接所述各系统的计算机控制与数据采集子系统。
本发明所述外围转换电路包括并行或串行的硬件接口电路、A/D转换器和D/A转换器。
本发明还公开了一种动态跟踪测量的方法，其特征在于，该方法为目标全姿态的多传感器融合测量，包括以下步骤(1)预先标定出反射镜与运动目标之间的几何位置尺寸关系，定义运动目标固连坐标系，从而得到各反射镜中心点在目标固连坐标系内对应观测点在此固连坐标系内的坐标；(2)预先标定出惯性仪表组所构平台坐标系与运动目标之间的几何位置尺寸关系，从而得到平台坐标系与目标固连坐标系之间的变换关系；(3)预先标定出激光跟踪站群之间的位置关系，包括各跟踪站坐标系间以及各跟踪站与目标固连坐标系间初始关系；定义全局坐标系，从而得到各跟踪站坐标系与全局坐标系之间的变换关系以及目标固连坐标系相对全局坐标系的初始状态；(4)惯性仪表组测量出目标加速度、角速度在其敏感轴上的输出值，通过平台坐标系与目标自身固连坐标系的相应坐标变换得到目标固连坐标系内的加速度和角速度值；(5)激光跟踪站测量计算出反射镜中心点在对应跟踪站坐标系内的坐标信息，通过坐标转换得到反射镜中心点对应目标固连坐标系内观测点在全局坐标系下的坐标值；(6)根据卡尔曼滤波算法要求，建立目标固连坐标系相对全局坐标系运动的系统方程和观测方程；(7)分别将惯性仪表组所测的目标加速度、角速度信息以及激光跟踪站所测的反射镜中心点所对应目标固连观测点的坐标信息作为卡尔曼滤波算法的系统信息和观测信息，通过按步骤(6)中所建立的系统方程和观测方程编制的扩展卡尔曼滤波算法对各种误差信息进行最优修正，从而计算出目标固连坐标系相对于全局坐标系的位置和姿态信息。
本发明的测量原理如下测量系统采用惯性仪表组测量出目标运动的加速度以及角速度信息，通过对应系统方程积分得到目标运动的位置和姿态估计值；测量系统采用多个激光跟踪站分别跟踪多个反射镜组合，得到对目标系统的多个(满足冗余观测条件)观测点的位置观测信息；测量系统采用按照卡尔曼滤波算法要求的系统对应系统方程和观测方程编写相应的扩展卡尔曼滤波算法；测量系统分别将惯性仪表组所测的加速度和角速度信息以及激光跟踪站所测的多观测点位置观测信息作为卡尔曼滤波算法的系统观测信息和量测观测信息，经过对应扩展卡尔曼滤波算法对目标运动的位置和姿态的积分估计值进行误差修正，得到目标固连坐标系相对于全局坐标系的位置和姿态值。
利用状态χ2检验法或者残差χ2检测检验法对惯性测量分系统和激光跟踪测量分系统的测量信息进行检验，对系统硬件工作状态做出判断，视故障情况分别有效的隔离掉故障分系统，及时更改系统滤波算法，使无故障测量分系统执行测量任务，从而提高整个组合测量系统的稳定性。
本发明具有如下特点第一、惯性仪表组刚性固定在被测运动对象上，惯性仪表组所构建的平台坐标系与目标固连坐标系之间的转换精度由加工过程保证。
第二、目标反射镜组采用组合形式，置于被测运动对象上，它们之间的位置刚性固定，并由加工过程来保证精度。
第三、为保证测量的冗余性，激光跟踪站的数目至少为三个。
第四、定义了目标固连坐标系、全局坐标系以及各跟踪站坐标系。不同坐标系间需要进行坐标变换。
第五、计算机中存储的多传感器数据融合算法软件是采用扩展卡尔曼滤波算法、多采样率传感器数据关联、系统故障检测与隔离理论算法编制的对惯性测量分系统和激光跟踪测量分系统的测量信息进行处理的相应算法。
经过大量的仿真和试验研究表明，本发明测量范围最大可达50米，位置测量精度可达0.01～0.1mm，姿态测量精度可达2～20角秒，与单纯的激光动态跟踪系统测量系统相比，激光跟踪惯性组合测量系统排除了由于测量站位置布置不当引起的运动目标姿态解算精度降低，提高了姿态测量精度，降低了由于局部激光跟踪站出现故障而导致系统动态跟踪测量测量精度降低的可能性，大大提高了动态跟踪测量系统的自主性、信息丰富性。可用于火箭发射的初始姿态监测标定以及机器人终端效应器的、大型吊装设备的定位部分、大型机床上的运动部分等设备的测控。
图1为本发明的一种激光跟踪惯性组合测量系统实施例的系统构成图。
图2为图1实施例中坐标系间转换关系示意图。
图3为本发明的所述动态跟踪测量方法的流程图。
具体实施例方式
下面结合附图和实施例对本发明作进一步说明。
本发明提出一种运动目标空间位置与姿态的激光跟踪惯性组合测量方法与系统实施例，结合图1～3对系统的构成及测量方法详细描述如下图1为本发明测量系统的一种实施例的构成示意图，其激光跟踪方法采用角度距离法。
该系统包括惯性测量分系统、激光跟踪测量分系统以及分别与惯性测量分系统和激光跟踪测量分系统相连接的计算机控制与数据采集处理分系统，所述的惯性测量分系统是指能够完成目标全姿态测量所需的按平台式工作方式或捷联式工作方式工作的惯性仪表组4；所述的激光跟踪测量分系统包括激光跟踪站群1，2，3和接受并反射所述激光跟踪站群发射的激光信号的目标反射镜组5；所述的计算机控制与数据采集处理分系统包括存储多传感器数据融合算法软件程序的计算机7，及其外围转换电路。具体构成如下惯性仪表组4，装有三个单自由度陀螺(或装两个二自由度陀螺)和三个加速度计。陀螺的三个敏感轴与加速度计的三个敏感轴严格保持方向一致，并构成一个直角坐标系O′x′y′z′，即平台坐标系。在具体实施例中，可选用封装完整的成品惯性仪表组。它与运动对象通过螺栓刚性连接，并由加工过程精确保证惯性仪表组所建平台坐标系与运动目标固连坐标系间的转换精度。惯性仪表组输出模拟量形式的运动目标的加速度和角速度测量值。
目标反射镜组5，为三个空心角隅棱镜A、B、C，也可以为三个平面反射镜、实心角隅棱镜、或“猫眼”反射镜各种类型的反射镜的组合，它们与运动对象通过螺栓刚性连接，并由加工过程保证反射镜中心点在运动目标固连坐标系内的坐标精度。
激光跟踪站群，包括三个结构相同的激光跟踪站1、2、3，激光跟踪站的测量坐标的原理为单站角度距离法。每个跟踪站是由激光测距子系统、二维伺服跟踪子系统、光电检测子系统和反馈控制跟踪子系统，以及连接所述各系统的计算机控制与数据采集子系统组成；能够实现跟踪和测量这两项功能。在具体实施例中，可选用封装完整的激光跟踪站，如API公司的Laser tracker系列激光跟踪站，该系列激光跟踪站封装成激光跟踪头和控制器两部分。其中激光跟踪头11、21、31封装有激光测距子系统、二维伺服跟踪子系统和光电检测子系统，控制器12、22、32封装有反馈控制跟踪子系统，以及连接所述各系统的计算机控制与数据采集子系统。激光跟踪站通过控制器以网络输出所测反射镜中心点的在激光跟踪站坐标系内的极坐标形式的坐标值。
包括存储多传感器数据融合算法软件程序的计算机7，该计算机通过A/D转换器41或内置的A/D转换卡获得惯性仪表组的测量输出值；通过集线器6与激光跟踪站控制器共同搭建的局域网获得激光跟踪站群的测量输出值。
下面为了表述方便，将本实施例中使用的部分算法公式阐述如下1)目标固连坐标系O′x′y′z′在全局坐标系Oxyz内的位姿表示方法目标固连坐标系b(O′x′y′z′)相对全局坐标系n(Oxyz)的位姿用姿态四元数q和目标固连坐标系原点O′在全局坐标系Oxyz中的坐标Rnxyz]]>表示。
其中姿态四元数q‾=q0q→,]]>满足限制条件q‾Tq‾=q02+q→Tq→=1---(e1)]]>目标固连坐标系O′x′y′z′向全局坐标系Oxyz的转换矩阵 可以表示为
Cb→n(q‾)=1-2(q22+q32)2(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)1-2(q12+q32)2(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)1-2(q12+q22)---(e2)]]>2)利用惯性仪表组测量信息列写的系统方程惯性仪表组测量出的加速度比力信息、角速度信息分别为fb、ωb，观测系统的系统方程为 其中vn、b、gn、εb分别为系统的速度、加速度计的偏置误差、重力加速度、陀螺仪的随机游走误差分别在对应b、n坐标系中的投影值。wa、wR、wq、wε分别为对应方程中的随机白噪声。
3)利用激光跟踪站所测数据列写的量测方程三个跟踪测量站1、2、3分别实时跟踪测量A、B、C三个反射镜在各自测量系统中的坐标Rii(i＝1、2、3表示A、B、C三个子目标)。这些坐标Rii需要经过齐次变换以得到全局坐标系下的坐标Rin，由运动学关系可以计算出相应点的速度为vin=Rin(t)-Rin(t-Δt)Δt.]]>t、t-Δt、Δt分别表示当前采样时刻时间点、上一采样时刻时间点、采样时间间隔。
系统的量测方程为 wRl、wvl分别为对应方程的白噪声。[ωb]为ωb=ωxbωybωzb]]>的反对称矩阵。其表达式为[ωb]=0-ωzbωybωzb0-ωxb-ωybωxb0---(e5)]]>
用公式(e2)、(e3)的方程在其最优估计状态附近展开后，可通过扩展卡尔曼滤波最优估计得到目标的位姿坐标Rn、q。由上述方法可以编制计算机软件计算目标的位姿。
图2为实施例的坐标系变换原理示意图。在这里，只分析任意两个跟踪站测量坐标系之间的变换矩阵的方法，多个跟踪站之间实现坐标变换的方法与之相似。将跟踪站1的测量坐标系Oxyz定义为全局坐标系n，跟踪站2的测量坐标系O1x1y1z1定义为b，跟踪站1和跟踪站2同时测量多个目标P1、P2、…、Pn，并分别测量出各目标在各自坐标系下的空间坐标为{Rin}和{Rib}。那么{Rin}和{Rib}点对之间存在着一一对应的变换关系Rin=Cb→n(q‾)Rib+Rn+Vi---(e6)]]>式中，{Rin}=xinyinzin,]]>{Rib}=xibyibzib,]]>分别为各目标点Pi在跟踪站1、跟踪站2坐标系内的坐标，Vi为3×1的噪声矢量， 为用四元数q表示的从b坐标系向n坐标系转换的转换矩阵，Rn为b坐标系原点在n坐标系中的坐标。
由公式(e1)、(e6)联合求解最小二乘方程组，可以得到跟踪站1坐标系和跟踪站2坐标系间转换关系的最优解。
如图3所示，计算机采集惯性仪表组与激光跟踪站组的测量输出值，经过相应初步的数据变换处理后，送入扩展卡尔曼滤波器。相应的算法流程主要由以下几部分组成(L1)初始参数设定设定A/D转换器的端口地址、局域网内各终端的IP地址；预先标定出反射镜与运动目标之间的几何位置尺寸关系，定义运动目标固连坐标系，设定各反射镜中心点对应观测点在此固连坐标系内的坐标；预先标定出惯性仪表组所构平台坐标系与运动目标之间的几何位置尺寸关系，设定平台坐标系与目标固连坐标系之间的变换关系。
(L2)系统的自检与故障排除系统检查惯性仪表组以及激光跟踪站群的工作状态是否正常，如出现故障做相应的故障排除工作。
(L3)系统的自标定预先标定出激光跟踪站群之间以及激光跟踪站群与被测运动目标固连坐标系间的位置关系，定义全局坐标系；在满足信息冗余原则的条件下，通过自标定算法对运动对象进行自标定，得到各跟踪站坐标系与全局坐标系之间的转换关系以及目标固连坐标系在全局坐标系中的初始位置和姿态信息。
(L4)读取传感器读数以及激光跟踪站直角坐标值计算分别通过相应的A/D转换器和网卡读取惯性元器件组和激光跟踪站的测量输出值，其中，激光跟踪站采用极坐标法，需要转化为直角坐标系坐标，其空间坐标转换公式为 式中，R为目标到跟踪站的距离，α、β分别为激光跟踪仪对应垂直轴向和水平轴向的转角。
(L5)坐标转换坐标转换有两部分组成，一部分是将惯性元器件组测量的运动目标加速度和角速度在平台坐标系内的输出值通过流程(L1)中所得平台坐标系与目标固连坐标系之间的转换关系变换到目标固连坐标系中；另一部分是将激光跟踪站对应跟踪测量点在该跟踪站对应坐标系中的直角坐标值通过流程(L3)中所得跟踪站坐标系与全局坐标系之间的转换关系变换到全局坐标系中。
(L6)系统故障检测利用状态χ2检验法或者残差χ2检测检验法对惯性测量分系统和激光跟踪测量分系统的测量信息进行检验，对系统硬件工作状态做出判断。
(L7)扩展卡尔曼滤波器根据卡尔曼滤波算法的要求，建立目标固连坐标系相对全局坐标系运动的系统方程和观测方程，如前面事先阐述的公式(e3)和(e4)所示；通过对上述系统方程和观测方程在其最优估计状态附近的展开关系式，可以编制相应的扩展卡尔曼滤波器；以流程(L3)中所得的目标固连坐标系相对全局坐标系的位置和姿态为初始值，通过前述扩展卡尔曼滤波器对系统的各种误差信息进行最优修正，从而计算出目标固连坐标系相对于全局坐标系的位置和姿态信息。
权利要求
1.激光跟踪惯性组合测量系统，其特征在于，该系统包括惯性测量分系统、激光跟踪测量分系统以及分别与惯性测量分系统和激光跟踪测量分系统相连接的计算机控制与数据采集处理分系统，所述的惯性测量分系统是指能够完成目标全姿态测量所需的按平台式工作方式或捷联式工作方式工作的惯性仪表组；所述的激光跟踪测量分系统包括激光跟踪站群和接受并反射所述激光跟踪站群发射的激光信号的目标反射镜组；所述的计算机控制与数据采集处理分系统包括存储多传感器数据融合算法软件程序的计算机，及其外围转换电路。
2.根据权利要求1所述的激光跟踪惯性组合测量系统，其特征在于所述的惯性仪表组是包括三个单自由度陀螺或两个二自由度陀螺，以及三个加速度计；所述陀螺的三个敏感轴与加速度计的三个敏感轴保持方向一致，并构成一个平台坐标系。
3.根据权利要求2所述的激光跟踪惯性组合测量系统，其特征在于所述加速度计为挠性加速度计、振弦式加速度计或静电式加速度计中的一种；所述陀螺为静电陀螺、挠性陀螺或激光陀螺中的一种。
4.根据权利要求1所述的激光跟踪惯性组合测量系统，其特征在于所述目标反射镜组是由固定于被测运动目标上的一个以上反射镜组成的，所述反射镜是平面反射镜、实心角隅棱镜、空心角隅棱镜或“猫眼”反射镜中的一种。
5.根据权利要求1所述的激光跟踪惯性组合测量系统，其特征在于所述激光跟踪站群中的每个激光跟踪站包括激光测距子系统、二维伺服跟踪子系统、光电检测子系统和反馈控制跟踪子系统，以及连接所述各系统的计算机控制与数据采集子系统。
6.根据权利要求1所述的激光跟踪惯性组合测量系统，其特征在于所述外围转换电路包括并行或串行的硬件接口电路、A/D转换器和D/A转换器。
7.利用权利要求1所述激光跟踪惯性组合测量系统进行动态跟踪测量的方法，其特征在于，该方法为目标全姿态的多传感器融合测量，包括以下步骤(1)预先标定出反射镜与运动目标之间的几何位置尺寸关系，定义运动目标固连坐标系，从而得到各反射镜中心点在目标固连坐标系内对应观测点在此固连坐标系内的坐标；(2)预先标定出惯性仪表组所构平台坐标系与运动目标之间的几何位置尺寸关系，从而得到平台坐标系与目标固连坐标系之间的变换关系；(3)预先标定出激光跟踪站群之间的位置关系，包括各跟踪站坐标系间以及各跟踪站与目标固连坐标系间初始关系；定义全局坐标系，从而得到各跟踪站坐标系与全局坐标系之间的变换关系以及目标固连坐标系相对全局坐标系的初始状态；(4)惯性仪表组测量出目标加速度、角速度在其敏感轴上的输出值，通过平台坐标系与目标自身固连坐标系的相应坐标变换得到目标固连坐标系内的加速度和角速度值；(5)激光跟踪站测量计算出反射镜中心点在对应跟踪站坐标系内的坐标信息，通过坐标转换得到反射镜中心点对应目标固连坐标系内观测点在全局坐标系下的坐标值；(6)根据卡尔曼滤波算法要求，建立目标固连坐标系相对全局坐标系运动的系统方程和观测方程；(7)分别将惯性仪表组所测的目标加速度、角速度信息以及激光跟踪站所测的反射镜中心点所对应目标固连观测点的坐标信息作为卡尔曼滤波算法的系统信息和观测信息，通过按步骤(6)中所建立的系统方程和观测方程编制的扩展卡尔曼滤波算法对各种误差信息进行最优修正，从而计算出目标固连坐标系相对于全局坐标系的位置和姿态信息。
8.根据权利要求7所述的动态跟踪测量的方法，其特征在于在动态跟踪测量过程中，利用状态χ2检验法或者残差χ2检测检验法对惯性测量分系统和激光跟踪测量分系统的测量信息进行检验，对系统硬件工作状态做出判断，从而隔离故障分系统，提高整个组合测量系统的稳定性。
全文摘要
激光跟踪惯性组合测量系统及其测量方法，属于精密测量技术领域。为了提高现有激光动态跟踪测量系统的稳定性和实时姿态测量精度，丰富现有动态跟踪测量系统的测量输出信息，本发明公开一种目标空间位置和姿态的激光跟踪惯性组合测量系统，包括惯性测量分系统、激光跟踪测量分系统以及分别与惯性测量分系统和激光跟踪测量分系统相连接的计算机控制与数据采集处理分系统。本发明还公开了一种动态跟踪测量方法，该方法根据卡尔曼滤波算法要求，建立目标固连坐标系相对全局坐标系运动的系统方程和观测方程，并编制扩展卡尔曼滤波算法对各种误差信息进行最优修正，从而计算出目标固连坐标系相对于全局坐标系的位置和姿态信息。
文档编号G01B21/02GK1563889SQ20041000347
公开日2005年1月12日 申请日期2004年3月26日 优先权日2004年3月26日
发明者汪劲松, 綦麟, 任革学, 张嵘, 张辉, 南仁东, 朱文白, 王启明 申请人:清华大学, 中国科学院国家天文台