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

摘要

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

主权项

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}\phi =a\tan (B/C)\\ \theta =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在扫描仪系统坐标系中的坐标。