[发明专利]基于物联网数据融合的高精度姿态测量装置与方法

权利要求书

1.一种基于物联网数据融合的高精度姿态测量装置，其特征在于，由测量基准平面(1)、待测目标(2)和激光测距仪(3)组成；

所述测量基准平面(1)位于待测目标(2)的上方，三个激光测距仪(3)均垂直于待测目标(2)的上表面安装，激光测距仪(3)发射的激光线与测量基准平面(1)相交得到对应的光斑特征点，激光测距仪(3)的测量输出值为激光测距仪(3)的发射器到光斑特征点之间的距离，待测目标(2)做三自由度的转动，其转动的范围确保三个激光测距仪(3)对应的光斑特征点均位于测量基准平面(1)上；

具体的姿态解算过程如下：

根据三个激光测距仪的安装位置，可以得到点A、B、C在物体坐标系下的坐标，分别记作P_A、P_B、P_C；

由于三个激光测距仪均垂直于待测目标的上表面安装，可以得到激光测距仪发射的激光束与物体坐标系的Oz轴平行，因此，点A、B、C与点A₁、B₁、C₁之间仅有z轴坐标不同，且该坐标值恰好为对应的激光测距仪的测量值，可以得到点A₁、B₁、C₁在物体坐标系下的坐标，分别记作P_A′、P_B′、P_C′；

接下来，根据点A、B、C在物体坐标系下的坐标P_A、P_B、P_C以及点A₁、B₁、C₁在物体坐标系下的坐标P_A′、P_B′、P_C′求取参考坐标系W相对于物体坐标系T的旋转矩阵；

在平面A₁B₁C₁内，找到两个空间矢量其坐标分别为

可求得平面A₁B₁C₁在物体坐标T下的法向量

以及平面A₁B₁C₁的法向量

由此，可求得参考坐标系W相对于物体坐标系T的旋转轴和旋转角度θ；

根据轴角对和旋转四元数之间的变换关系，可求得参考坐标系W相对于物体坐标系T的旋转四元数q

其中，n_x、n_y和n_z分别为旋转轴的3个分量，q₀、q₁、q₂和q₃分别为旋转四元数q的4个分量；

根据旋转四元数和旋转矩阵之间的变换关系，可求得参考坐标系W相对于物体坐标系T的旋转矩阵

最后，根据参考坐标系W相对于物体坐标系T的旋转矩阵可求得物体坐标系T相对于参考坐标系W的旋转矩阵

物体坐标系T相对于参考坐标系W的旋转矩阵即为测得的姿态信息。

2.一种基于物联网数据融合的高精度姿态测量方法，其特征在于，包括以下步骤：

将三个激光测距仪的发射器分别记作A、B、C，将三个激光测距仪对应的光斑特征点分别记作A₁、B₁、C₁；

首先，建立姿态测量所必需的坐标系，建立与测量基准平面固连的参考坐标系W，其Oxy平面与测量基准平面重合，Oz轴与测量基准平面垂直，类似地，建立与待测目标上表面固连的物体坐标系T，其Oxy平面与待测目标上表面重合，Oz轴与待测目标上表面垂直；

之后，三个激光测距仪开始测量自身到测量基准平面之间的距离，分别记作h_A、h_B、h_C，得到距离的测量值之后，将其上传至云端姿态解算算法的入口处，通过空间矢量运算计算得到待测目标的姿态；

在云端完成姿态解算之后，根据其他模块对姿态测量结果的请求指令，将姿态测量结果通过无线网络分发给其他模块；

具体的姿态解算过程如下：

根据三个激光测距仪的安装位置，可以得到点A、B、C在物体坐标系下的坐标，分别记作P_A、P_B、P_C；

由于三个激光测距仪均垂直于待测目标的上表面安装，可以得到激光测距仪发射的激光束与物体坐标系的Oz轴平行，因此，点A、B、C与点A₁、B₁、C₁之间仅有z轴坐标不同，且该坐标值恰好为对应的激光测距仪的测量值，可以得到点A₁、B₁、C₁在物体坐标系下的坐标，分别记作P_A′、P_B′、P_C′；

接下来，根据点A、B、C在物体坐标系下的坐标P_A、P_B、P_C以及点A₁、B₁、C₁在物体坐标系下的坐标P_A′、P_B′、P_C′求取参考坐标系W相对于物体坐标系T的旋转矩阵；

在平面A₁B₁C₁内，找到两个空间矢量其坐标分别为

可求得平面A₁B₁C₁在物体坐标T下的法向量

以及平面A₁B₁C₁的法向量

由此，可求得参考坐标系W相对于物体坐标系T的旋转轴和旋转角度θ；

根据轴角对和旋转四元数之间的变换关系，可求得参考坐标系W相对于物体坐标系T的旋转四元数q

其中，n_x、n_y和n_z分别为旋转轴的3个分量，q₀、q₁、q₂和q₃分别为旋转四元数q的4个分量；

根据旋转四元数和旋转矩阵之间的变换关系，可求得参考坐标系W相对于物体坐标系T的旋转矩阵

最后，根据参考坐标系W相对于物体坐标系T的旋转矩阵可求得物体坐标系T相对于参考坐标系W的旋转矩阵

物体坐标系T相对于参考坐标系W的旋转矩阵即为测得的姿态信息。