CN103090863A - 一种动态平台姿态和高度测量方法 - Google Patents

Abstract

本发明提供一种动态平台的姿态和高度测量方法。技术方案是利用两台正交安装、竖直向下扫描的二维激光扫描仪获得路面扫描线；使用RANSAC算法估计出扫描线的表达式方程，利用两扫描线的表达式方程得到道路平面方程；以道路平面方程为参考解算平台的姿态和高度。本发明使用的设备简单，成本低。测量结果不受运动平台自身加速度的影响，没有漂移，测量精度高。

Description

一种动态平台姿态和高度测量方法

技术领域

本发明属于光电测控技术领域，涉及一种姿态和高度测量方法，尤其是一种动态平台姿态和高度测量方法，进一步是涉及一种利用正交二维激光扫描原理进行路面上的动态平台姿态和高度测量的方法。

背景技术

目前动态平台姿态的高精度实时测量主要使用基于陀螺和加速度计的惯性测量***，并常与GPS（Global Position System，全球定位***）组合以保证长时间的测量精度，但这类***工作前的准备时间较长，价格昂贵，不适于在一些实时测量***中应用。基于GPS的姿态测量技术是当前研究的热点领域，但卫星信号在一些情况下可能无法正常接收，且姿态测量精度受限于接收机的基线长度。倾角传感器通过测量重力加速度在其敏感轴上的分量，解算出传感器相对于水平面的姿态角，主要用于静态条件下载体的姿态测量，仅在载体姿态角变化速度较小时精度较高，动态条件下载体平台的姿态角的快速变化将导致测量精度显著下降。

动态平台在实际工作过程中，由于运动、机械振动、平台（或安装支架）形变等原因，其姿态和高度都会发生变化，平台上安装的传感器的姿态和高度也会随之变化，这种变化在许多高精度测量***中都是必须实时测量并补偿的。比如，单目视觉定位***就需要实时测量相机的拍摄姿态和高度，这是保证定位精度的关键技术之一。

鉴于以上分析，有必要深入研究有别于传统方法且性价比更高的动态平台的姿态和高度测量方法。

发明内容

本发明要解决的技术问题是：提供一种动态平台的姿态和高度测量方法，实时测量出动态平台由于运动、机械振动、平台（或安装支架）形变等原因导致的姿态和高度变化。该方法算法简单、设备成本较为低廉、平台姿态和高度的测量频率和精度较高。

本发明的技术方案是：基于正交二维激光扫描的平台姿态和高度测量方法的基本思路为：由两台正交安装、竖直向下扫描的二维激光扫描仪获得路面扫描线；使用RANSAC算法估计出扫描线的表达式方程，利用两扫描线的表达式方程得到道路平面方程；以道路平面方程为参考解算平台的姿态和高度。

本发明所采用的具体技术方案如下：

将两台二维激光扫描仪固定在姿态和高度待测的平台上，两台扫描仪均以竖直向下的方式同步扫描路面，并且两台扫描仪的扫描平面相互正交。建立扫描仪***坐标系X_S-scanner，原点为两个扫描仪的扫描平面与平台所在平面三个平面的交点，X轴平行于平台所在平面但垂直于平台前进方向，Y轴平行于平台所在平面且与平台前进方向相反，Z轴垂直于平台所在平面且向上为正向，X轴、Y轴和Z轴构成右手坐标系。实施下述步骤：

第一步，采集数据。

平台在路面上移动，即平台处于运动状态时。两台扫描仪S₁和S₂同步扫描，采集数据。设S₁和S₂在某一时刻T分别扫描获得一条扫描线，令其分别为L₁和L₂。每条扫描线由n个扫描点组成，L₁在扫描仪***坐标系X_S-scanner中第i个扫描点的坐标为

(i=1,2,…,n)，L₂在扫描仪***坐标系X_S-scanner中第i个扫描点的坐标为

(i=1,2,…,n)。

第二步，计算T时刻扫描线L₁和L₂的表达式方程。

利用扫描线L₁和L₂所包含的扫描点坐标，使用RANSAC算法估计T时刻L₁和L₂的表达式方程，分别为：

$\left\{\begin{array}{ccc}{L}_1:& a_{1}x+b_{1}z+c_1=0,& y=0\\ L_2:& a_{2}x+b_{2}z+c_2=0,& x=0\end{array}\right.$ （公式一）

上式中，(x,y,z)表示在扫描仪***坐标系X_S-scanner中任意一点的坐标。

第三步，计算道路平面方程：

由扫描线L₁和L₂的直线方程，得到T时刻道路平面方程，如公式二所示：

$A(x-\frac{b_1{t}_1}{2})+B(y-\frac{b_2{t}_2}{2})+C(z+\frac{c_1/b_1+c_2/b_2+a_1{t}_1+a_2{t}_2}{2})=0$ （公式二）

其中：

$\left\{\begin{array}{ccc}A=a_1{b}_2& & \\ B=a_2{b}_1,& t_1=\frac{\frac{a_1{b}_2}{b_1}(c_2{b}_1-c_1{b}_2)}{a_1^2{b}_2^2+a_2^2{b}_1^2+b_1^2{b}_2^2},& t_2=\frac{\frac{a_2{b}_1}{b_2}(c_2{b}_1-c_1{b}_2)}{a_1^2{b}_2^2+a_2^2{b}_1^2+b_1^2{b}_2^2}\\ C=b_1{b}_2& & \end{array}\right.$ （公式三）

第四步，解算平台姿态角和高度。

T时刻平台姿态角由如下公式计算出：

$\left\{\begin{array}{c}\mathrm{\φ}=a\tan (B/C)\\ \mathrm{\θ}=a\sin \left[\mathrm{sign}(-\frac{A}{C})\sqrt{\frac{1}{1+{(B/A)}^2+{(C/A)}^2}}\right]\end{array}\right.$ （公式四）

其中φ和θ分别为平台的俯仰角和横滚角。

T时刻平台上待测点M的高度值H由如下公式计算出：

$H=\frac{|A(x_M-\frac{b_1{t}_1}{2})+B(y_M-\frac{b_2{t}_2}{2})+C(z_M+\frac{c_1/b_1+c_2/b_2+a_1{t}_1+a_2{t}_2}{2})|}{\sqrt{A^2+B^2+C^2}}$ （公式五）

其中，(x_M, _M,z_M)为待测点M在扫描仪***坐标系中的坐标，事先用测量仪器测量得到。

则利用上述方法可测量任意时刻平台的姿态和平台上任意待测点的高度。

本发明的有益效果是：利用两台扫描仪实时获得的扫描线，可实时测量动态平台的高度和姿态。相对于传统姿态测量方法，使用的设备简单，成本低。测量结果不受运动平台自身加速度的影响，没有漂移，测量精度高。

附图说明

图1为动态平台姿态和高度测量方法的原理示意图；

图2为本发明的具体实施流程图；

图3为采用本发明的平台俯仰角测量结果与ADU传感器测量结果的对比图；

图4为采用本发明的平台横滚角测量结果与ADU传感器测量结果的对比图；

图5为采用本发明的平台实时高度测量结果。

具体实施方式

以下将结合说明书附图对本发明做进一步详细说明。

图1为动态平台姿态和高度测量方法的原理示意图，表示出扫描仪***坐标系的定义以及设备安装方式，X-Y-Z坐标系为扫描仪***坐标系X_S-scaner，原点为两个扫描仪的扫描平面与平台所在平面三个平面的交点O，X轴平行于平台所在平面但垂直于平台前进方向，Y轴平行于平台所在平面且与平台前进方向相反，Z轴垂直于平台所在平面且向上为正向，X轴、Y轴和Z轴构成右手坐标系；将两台二维激光扫描仪S₁和S₂固定在姿态和高度待测的平台上，两台扫描仪均以竖直向下的方式同步扫描路面，并且两台扫描仪的扫描平面相互正交。以位于平台上任一高度的某固定点M作为待测点，m点为固定点M在路面上的投影点，显然线段Mm垂直于路面，其长度H即为待测高度。在任意时刻T由扫描仪S₁和S₂得到两条路面扫描线，利用RANSAC算法对两条路面扫描线实施直线估计，分别得到对应的直线L₁和L₂的方程表达式,RANSAC算法原理的介绍参见《计算机视觉中的数学方法》一书（科学出版社出版，吴福朝著）第338页至344页。由于L₁和L₂为位于路面上的直线，联立L₁和L₂的直线方程得到T时刻道路平面在X-Y-Z坐标系下的表达式方程。在获得T时刻的道路平面方程后，以道路平面的法线方向为参考获得T时刻的平台姿态，通过计算M点到道路平面的距离计算得到T时刻的相机高度。

图2给出了本发明具体实施的流程图。第一步，采集数据；第二步，计算T时刻扫描线L₁和L₂的表达式方程；第三步，计算道路平面方程；第四步，解算平台姿态角和待测点高度。

采用本发明的具体实施方式进行了实验，验证了本发明测量运动平台姿态和高度的精度。具体实施例中使用的主要设备说明如下：用于数据处理的计算机主要配置为：CPU为Intel Core2Duo Processor2.8GHz，使用4GB DDR2双通道内存；二维激光扫描仪采用的是德国SICK公司的LMS291-S05型激光扫描仪；姿态角传感器采用的是星网宇达公司的ADU5600传感器（以下简称ADU传感器），测量频率为100Hz，动态测量精度为1°。

基于加速度计的姿态传感器的测量精度受车辆加减速的影响较大，而本方法以道路平面为参考基准测量平台姿态，因此原理上车辆加减速不会影响其姿态测量结果。为验证该结论，实验中采用ADU传感器方法和本发明提供的方法进行对比。车辆在路面反复进行加速和减速运动，期间经历四次转弯，ADU传感器与扫描仪同步测量，获得的平台姿态和待测点高度测量结果分别如图3、图4和图5所示。如图3所示，虚线表示ADU传感器测得的平台俯仰角，实线表示采用本方法测得的平台俯仰角；车辆加减速对ADU传感器测量结果影响较大，获得的车辆俯仰角出现较大偏移，曲线在车辆停止后仍然有向上漂移的趋势，而本方法获得的车辆俯仰角波动较小，这与车辆行驶于路面的实验条件相符合。由于车辆在快速转弯过程中会出现明显的侧倾，如图4所示，虚线表示ADU传感器测得的平台横滚角，实线表示采用本方法测得的平台横滚角，两种方法均检测出了车辆侧倾导致的横滚角变化，曲线中出现的四个波峰对应着车辆的四次转弯运动，按常理推知：车辆在发生左右摆动倾斜时，其横滚角应以0°为中心波动，如图4所示，本文方法得到的曲线与该常理吻合较好，而由ADU传感器得到的曲线在后半段出现了明显的整体漂移误差。图5为运动平台行进过程中高度实时测量曲线，运动平台初始架设高度为1.45m（即平台上某一个固定点的高度），由于在行进过程中运载车辆加速、减速和转弯，平台高度不断变化，所以如图5所示，平台高度实时变化且围绕1.45m上下波动。

Claims (1)

1.一种动态平台姿态和高度测量方法，其特征在于，将两台二维激光扫描仪固定在姿态和高度待测的平台上，两台扫描仪均以竖直向下的方式同步扫描路面，并且两台扫描仪的扫描平面相互正交；建立扫描仪***坐标系X_S-scanner，原点为两个扫描仪的扫描平面与平台所在平面三个平面的交点，X轴平行于平台所在平面但垂直于平台前进方向，Y轴平行于平台所在平面且与平台前进方向相反，Z轴垂直于平台所在平面且向上为正向，X轴、Y轴和Z轴构成右手坐标系；实施下述步骤：

第一步，采集数据：

平台在路面上移动时，两台扫描仪S₁和S₂同步扫描，采集数据；设S₁和S₂在某一时刻T分别扫描获得一条扫描线，令其分别为L₁和L₂；每条扫描线由n个扫描点组成，L₁在扫描仪***坐标系X_S-scanner中第i个扫描点的坐标为

(i=1,2,…,n)，L₂在扫描仪***坐标系X_S-scanner中第i个扫描点的坐标为(i=1,2,…,n)；

第二步，计算T时刻扫描线L₁和L₂的表达式方程：

利用扫描线L₁和L₂所包含的扫描点坐标，使用RANSAC算法估计T时刻L₁和L₂的表达式方程，分别为：

$\left\{\begin{array}{ccc}{L}_1:& a_{1}x+b_{1}z+c_1=0,& y=0\\ L_2:& a_{2}x+b_{2}z+c_2=0,& x=0\end{array}\right.$ 公式一

上式中，(x,y,z)表示在扫描仪***坐标系X_S-scanner中任意一点的坐标；

第三步，计算道路平面方程：

由扫描线L₁和L₂的直线方程，得到T时刻道路平面方程，如公式二所示：

$A(x-\frac{b_1{t}_1}{2})+B(y-\frac{b_2{t}_2}{2})+C(z+\frac{c_1/b_1+c_2/b_2+a_1{t}_1+a_2{t}_2}{2})=0$ 公式二

其中：

$\left\{\begin{array}{ccc}A=a_1{b}_2& & \\ B=a_2{b}_1,& t_1=\frac{\frac{a_1{b}_2}{b_1}(c_2{b}_1-c_1{b}_2)}{a_1^2{b}_2^2+a_2^2{b}_1^2+b_1^2{b}_2^2},& t_2=\frac{\frac{a_2{b}_1}{b_2}(c_2{b}_1-c_1{b}_2)}{a_1^2{b}_2^2+a_2^2{b}_1^2+b_1^2{b}_2^2}\\ C=b_1{b}_2& & \end{array}\right.$ 公式三

第四步，解算平台姿态角和高度：

T时刻平台姿态角由如下公式计算出：

$\left\{\begin{array}{c}\mathrm{\φ}=a\tan (B/C)\\ \mathrm{\θ}=a\sin \left[\mathrm{sign}(-\frac{A}{C})\sqrt{\frac{1}{1+{(B/A)}^2+{(C/A)}^2}}\right]\end{array}\right.$ 公式四

其中φ和θ分别为平台的俯仰角和横滚角；

T时刻平台上待测点M的高度值H由如下公式计算出：

$H=\frac{|A(x_M-\frac{b_1{t}_1}{2})+B(y_M-\frac{b_2{t}_2}{2})+C(z_M+\frac{c_1/b_1+c_2/b_2+a_1{t}_1+a_2{t}_2}{2})|}{\sqrt{A^2+B^2+C^2}}$ 公式五

其中，(x_M,y_M,z_M)为待测点M在扫描仪***坐标系中的坐标。

Images