[发明专利]一种对齐方法及对齐装置

权利要求书

1.一种对齐方法，其特征在于，包括：

确定IMU传感器进入稳定状态时定位目标的第一3D位置以及第一IMU计算3D位置，所述第一3D位置为所述IMU传感器进入稳定状态时根据定位装置采集的所述定位目标的3D位置信息确定所述定位目标的3D位置，所述第一IMU计算3D位置为所述IMU传感器进入稳定状态时根据所述IMU传感器采集的所述定位目标的IMU传感器信息计算出的所述定位目标的3D位置；

确定所述IMU传感器退出稳定状态时所述定位目标的第二3D位置以及第二IMU计算3D位置，所述第二3D位置为所述IMU传感器退出稳定状态时根据所述定位装置采集的所述定位目标的3D位置信息确定的所述定位目标的3D位置，所述第二IMU计算3D位置为所述IMU传感器退出稳定状态时根据所述IMU传感器采集的所述定位目标的IMU传感器信息计算出的所述定位目标的3D位置；

根据所述第一3D位置以及所述第二3D位置确定第一相对位置，所述第一相对位置为所述IMU传感器进入稳定状态时所述定位目标的3D位置以及所述IMU传感器退出稳定状态时的所述定位目标的3D位置间的距离；

根据所述第一IMU计算3D位置以及所述第二IMU计算3D位置信息确定第二相对位置，所述第二相对位置为所述IMU传感器进入稳定状态时计算出的所述定位目标的3D位置以及所述IMU传感器退出稳定状态时计算出的所述定位目标的3D位置间的距离；

判断所述第一相对位置以及所述第二相对位置是否符合预置的条件；

若是，则将所述第一相对位置以及所述第二相对位置进行向量归一化处理确定目标向量对；

将所述目标向量对保存至目标缓冲池中；

判断所述目标缓冲池中的向量对个数是否大于预设值；

若是，则通过刚性匹配算法对所述目标缓冲池中的所有向量对进行计算确定旋转矩阵；

根据所述旋转矩阵将所述定位装置的坐标系与所述IMU传感器的坐标系对齐。

2.根据权利要求1所述的方法，其特征在于，所述确定IMU传感器进入稳定状态时定位目标的第一3D位置以及第一IMU计算3D位置包括：

判断所述IMU传感器是否进入稳定状态；

若是，则采集的所述定位目标的第一3D位置信息以及第一IMU传感器信息，所述第一3D位置信息为所述IMU传感器进入稳定状态时所述定位装置采集的所述定位目标的3D位置信息，所述第一IMU传感器信息为所述IMU传感器进入稳定状态时所述IMU传感器采集的信息；

根据所述第一3D位置信息确定所述第一3D位置；

根据所述第一IMU传感器信息确定所述第一IMU计算3D位置。

3.根据权利要求2所述的方法，其特征在于，所述判断所述IMU传感器是否进入稳定状态包括：

获取任意两帧间的所述IMU传感器的yaw值的差值；

判断所述任意两帧间的所述IMU传感器的yaw值的差值是否小于第一预设的阈值；

若是，则确定所述IMU传感器进入稳定状态；

和/或，

获取任意两帧间的所述IMU传感器的pitch值的差值；

判断所述任意两帧间的所述IMU传感器的pitch值的差值是否小于第二预设的阈值；

若是，则确定所述IMU传感器进入稳定状态；

和/或，

获取任意两帧间的所述IMU传感器的roll值的差值；

判断所述任意两帧间的所述IMU传感器的roll值的差值是否小于第三预设的阈值；

若是，则确定所述IMU传感器进入稳定状态。

4.根据权利要求1所述的方法，其特征在于，所述确定所述IMU传感器退出稳定状态时所述定位目标的第二3D位置以及第二IMU计算3D位置包括：

判断所述IMU传感器是否退出所述稳定状态；

若是，则采集所述定位目标的第二3D位置信息以及第二IMU传感器信息，所述第二3D位置信息为所述IMU传感器退出稳定状态时所述定位装置采集的所述定位目标的3D位置信息，所述第二IMU传感器信息为所述IMU传感器退出稳定状态时所述IMU传感器采集的信息；

根据所述第二3D位置信息确定所述第二3D位置；

根据所述第二IMU传感器信息确定所述第二IMU计算3D位置。