[发明专利]一种平台倾角和高度实时测量方法及系统

摘要

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

主权项

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距离传感器自身默认的坐标参考系为参考标准。