CN109945785A - 一种平台倾角和高度实时测量方法及系统 - Google Patents

Abstract

本发明公开了一种平台倾角和高度实时测量方法及系统，使用具有三点以上的同时测距能力的TOF距离传感器，朝向参考平面实施测量并获得不少于三点的距离数据，且其中至少有三个测量点在参考平面上不共线；利用以上数据使用RANSAC算法估计出参考平面的表达式方程；最终以该平面方程为参考解算出平台的倾角和高度数据。本发明测量结果不受运动平台自身加速度的影响，没有漂移，测量精度高。

Description

一种平台倾角和高度实时测量方法及系统

技术领域

本发明涉及光电测控技术领域，特别是一种平台倾角和高度实时测量方法及系统。

背景技术

动态平台在实际工作过程中，由于运动、机械振动、平台(或安装支架)形变等原因，其姿态和高度都会发生变化，平台上安装的传感器的姿态和高度也会随之变化，这种变化在许多高精度测量系统中都是必须实时测量并补偿的。比如，单目视觉定位系统就需要实时测量相机相对地面的拍摄姿态和高度，这是保证定位精度的关键技术之一。目前动态平台姿态的高精度实时测量主要使用基于陀螺和加速度计的惯性测量系统，并常与GPS(Global Position System，全球定位系统)组合以保证长时间的测量精度，但这类系统工作前的准备时间较长，价格昂贵，不适于在一些实时测量系统中应用。除此之外，传统低成本的姿态传感器同样通过测量重力加速度在其敏感轴上的分量，解算出传感器相对于水平面的姿态角，因此它得出的姿态数据全部是以重力场的方向作为参考系。而在很多情况下，我们需要的是平台相对于一个特定平面的姿态，比如在单目视觉定位系统应用中，得到相机相对于地面(可能是倾斜地面)的倾角至关重要，此时传统的姿态传感器均无法使用。另外，现有的倾角传感器主要用于相对平稳条件下载体的姿态测量，仅在载体姿态角变化速度较小时精度较高，动态条件下载体平台的姿态角的快速变化将导致测量精度显著下降。

鉴于以上分析，发明专利申请《一种动态平台姿态和高度测量方法》中(公开号：CN103090863A)，提出了一种基于正交激光扫描的方式实现该功能的方法和系统。有别于传统方法，该方法能够以指定的任意平面(不一定是水平的地面)作为参考基准来实现动态平台姿态和高度测量。但是，由于该方法需要同时使用两台二维激光扫描仪，不但导致系统体积庞大、成本昂贵，而且由于扫描仪每次获得扫描线都需要一定的时间，这使得系统在测量高动态平台倾角时难免出现较大的误差。为此，有必要采用一种体积小巧便携、成本低廉、实时性更好、精度更高的平台倾角和高度测量方法。

发明内容

本发明所要解决的技术问题是，针对现有技术不足，提供一种平台倾角和高度实时测量方法及系统，降低算法复杂度，提高测量精度，保证测量不受平台自身加速度变化的影响。

为解决上述技术问题，本发明所采用的技术方案是：一种平台倾角和高度实时测量方法：

第一步，采集数据。

假设TOF距离传感器在同一时刻T获得的n个点(n≥3)的点云数据集合为M,M＝{m₁,m₂,...m_n},n≥3。该点云的坐标值以TOF距离传感器自身默认的坐标参考系为参考标准。

第二步，获得T时刻参考平面的表达式方程。

利用数据集合M，使用RANSAC算法估计T时刻参考平面的表达式方程为：

Ax+By+Cz+D＝0 (公式一)

其中：A,B,C,D的值由RANSAC算法直接估计得到，该表达式方程同样以TOF距离传感器默认的坐标参考系为基准。RANSAC算法原理的介绍参见《计算机视觉中的数学方法》(科学出版社出版，吴福朝著)第338页至344页。

第三步，解算平台姿态角和待测点高度。

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

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

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

其中，(x_k,y_k,z_k)为待测点K的坐标，该坐标值以TOF距离传感器自身默认的坐标参考系为参考标准，它事先可用测量仪器测量得到。

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

使用具有三点以上的同时测距能力的TOF距离传感器，朝向参考平面实施测量并获得不少于三点的距离数据，且其中至少有三个测量点在参考平面上不共线；利用以上数据使用RANSAC算法估计出参考平面的表达式方程；最终以该平面方程为参考解算出平台的倾角和高度数据。

使用TOF距离传感器朝向参考平面实施测量。现有的TOF距离传感器有两种：第一种可同时实现大于三点的同时距离采样和测量，也称为TOF相机；另一种是只可以对一个点实施测量，称为单点TOF传感器。本方法中，两种传感器均可以使用，只是如果使用单点TOF距离传感器，则至少需要三个以上同步工作来采集数据。

相应地，本发明还提供了一种平台倾角和高度实时测量系统，其包括TOF距离传感器；所述TOF距离传感器以向下的方式对参考平面实施测量；所述参考平面是指平台倾角和高度测量数据所参考的基准面；在获得T时刻的参考平面方程后，以参考平面的法线方向为参考获得T时刻的平台姿态，通过计算位于平台上任意某待测点K到参考平面的距离，得到T时刻平台上待测点K的高度值H。

与现有技术相比，本发明所具有的有益效果为：本发明利用TOF距离传感器实时获得的点云，可实时测量动态平台的高度和倾角。相对于传统姿态测量方法，测量结果不受运动平台自身加速度的影响，没有漂移，测量精度高。本发明方法采用的系统体积显著缩小、成本低廉，而且测量实时性更好，对动态平台的测量精度更高。

附图说明

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

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

图3为由TOF相机获得的地面点云分布结果

具体实施方式

图1为动态平台姿态和高度测量方法的原理示意图，X-Y-Z坐标系为TOF距离传感器默认的坐标参考系，其中原点O为TOF距离传感器的安装中心点，X轴和Y轴平行于平台所在平面，Z轴垂直于平台所在平面且向上为正向，X轴、Y轴和Z轴构成右手坐标系；TOF距离传感器以向下的方式对参考平面实施测量。所谓参考平面是指平台倾角和高度测量数据所参考的基准面，在实际应用中可以是地面，也可以是其它任意指定的平面。以位于平台上任意某固定点M作为待测点(即点K)，m点为固定点M在参考平面上的投影点，显然线段Mm垂直于参考平面，其长度H即为待测高度。在任意时刻T由TOF距离传感器得到相对参考平面的距离测量结果数组，利用RANSAC算法对数据实施平面估计，得到T时刻参考平面在X-Y-Z坐标系下的表达式方程。在获得T时刻的参考平面方程后，以参考平面的法线方向为参考获得T时刻的平台姿态，通过计算M点到参考平面的距离得到T时刻的固定点M高度。

图2给出了本发明具体实施的流程图。第一步，采集数据；第二步，利用RANSAC平面估计算法计算T时刻道路平面方程；第三步，解算平台姿态角和待测点高度。

本实施例采用的TOF距离传感器为TOF相机，TOF相机是瑞士Mesa公司的产品，型号为SR-4000，它一次拍摄可以获取目标176×144个点的三维坐标信息，最大帧率达到50Hz。而且TOF相机本身体积小巧，这样就显著降低了整个系统的复杂度。

图3为由TOF相机获得的地面点云分布结果。可见，点云密度足够用来估计地面平面坐标，因此可以获得很好的姿态和高度测量精度。

Claims (5)

1.一种平台倾角和高度实时测量方法，其特征在于，包括以下步骤：

1)设TOF距离传感器在同一时刻T获得的n个点的点云数据集合为M，M＝{m₁,m₂,...m_n},n≥3；点云的坐标值以TOF距离传感器自身默认的坐标参考系为参考标准；其中TOF距离传感器自身默认的坐标参考系是指：原点O为TOF距离传感器的安装中心点，X轴和Y轴平行于平台所在平面，Z轴垂直于平台所在平面且向上为正向，X轴、Y轴和Z轴构成右手坐标系；

2)利用数据集合M，使用RANSAC算法估计T时刻参考平面的表达式方程为：Ax+By+Cz+D＝0；其中：系数A,B,C,D的值由RANSAC算法直接估计得到；

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

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

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

其中，(x_k,y_k,z_k)为待测点K的坐标，该坐标值以TOF距离传感器自身默认的坐标参考系为参考标准。

2.一种平台倾角和高度实时测量系统，其特征在于，包括TOF距离传感器；所述TOF距离传感器以向下的方式对参考平面实施测量；所述参考平面是指平台倾角和高度测量数据所参考的基准面；在获得T时刻的参考平面方程后，以参考平面的法线方向为参考获得T时刻的平台姿态，通过计算位于平台上任意某待测点K到参考平面的距离，得到T时刻平台上待测点K的高度值H。

3.根据权利要求2所述的平台倾角和高度实时测量系统，其特征在于，T时刻的平台姿态由如下公式计算出：

其中φ和θ分别为平台的俯仰角和横滚角；A,B,C,D为使用RANSAC算法估计出的T时刻参考平面的表达式方程的系数。

4.根据权利要求2所述的平台倾角和高度实时测量系统，其特征在于，T时刻平台上待测点K的高度值H由如下公式计算出：

其中，(x_k,y_k,z_k)为待测点K的坐标，该坐标值以TOF距离传感器自身默认的坐标参考系为参考标准；A,B,C,D为使用RANSAC算法估计出的T时刻参考平面的表达式方程的系数。

5.根据权利要求2～4之一所述的平台倾角和高度实时测量系统，其特征在于，所述TOF距离传感器采用TOF相机；TOF相机型号为SR-4000。