[发明专利]一种移动平台位姿状态监测装置及方法

权利要求书

1.一种移动平台位姿状态监测装置，其特征在于，包括布置在地面的标定板和设置在整体平台底部的：

激光测距位移传感器，布置在整体平台的四角处，用于获取整体平台的四角与水平标定板之间的距离，以判断整体平台在空间上是否发生翻转；

视觉相机，布置有两个，位于整体平台前后两侧成对角布置，通过拍摄标定板上的标记点，判断整体平台在水平方向上是否发生移动和旋转。

2.根据权利要求1所述的移动平台位姿状态监测装置，其特征在于，所述的标定板上设有与每个视觉相机对应的五孔阵列，包括四个均匀布置的小孔和设置在四个小孔中心位置的大孔。

3.根据权利要求1所述的移动平台位姿状态监测装置，其特征在于，所述的标定板包括安装在站位底座的同一参考面内的四个。

4.根据权利要求1所述的移动平台位姿状态监测装置，其特征在于，所述的激光测距位移传感器和所述的视觉相机安装在同一水平面上。

5.根据权利要求1所述的移动平台位姿状态监测装置，其特征在于，所述的视觉相机配有环形光源。

6.一种移动平台位姿状态监测方法，基于权利要求1～5任一权利要求所述的移动平台位姿状态监测装置实现，其特征在于，包括以下步骤：

1)通过激光测距位移传感器测量整体平台四角至水平地面的距离，获得四个当前距离值，与期望姿态时的四个期望距离值相比，得到四个距离偏差值；

2)视觉相机的焦平面被配置在标定板表面，获取标记点图像，通过数字图像处理算法提取标记点坐标值，得到标记点的位置偏差；

3)结合距离偏差和位置偏差，通过匹配当前特征点和期望特征点的坐标值计算当前位姿偏差，获取整体平台的当前位姿。

7.根据权利要求1所述的移动平台位姿状态监测方法，其特征在于，步骤1)中，设置与所述视觉相机对应的五孔阵列，五孔阵列包括四个均匀布置的小孔和设置在四个小孔中心位置的大孔，大孔的圆心作为所述的标记点。

8.根据权利要求7所述的移动平台位姿状态监测方法，其特征在于，步骤1)中四个当前距离值为(d₁，d₂，d₃，d₄)，四个距离偏差值为(Δd₁，Δd₂，Δd₃，Δd₄)，标定板上两个大孔的位置偏差为(Δx₁，Δy₁，Δx₂，Δy₂)，则六个监测点的初始坐标值分别为：

激光测距点：

大孔中心点：

X_Goal、Y_Goal、Z_Goal，分别表示监测点在目标坐标系下的X、Y、Z三个方向上的理论值；

采用奇异值分解求变换矩阵，将目标坐标值变换至当前坐标系：

激光测距点：

大孔中心点：

X′_Cur、Y′_Cur、Z′_Cur，分别表示监测点在当前坐标系下的X、Y、Z三个方向上的坐标值；

迭代上述过程求解变换矩阵，直至匹配误差收敛，获得当前的姿态偏差。