[发明专利]一种基于多传感器标定与融合的六自由度位姿确定方法

说明书

本发明提供了一种基于多传感器标定与融合的六自由度位姿确定方法，其步骤包括：将n个传感器安装在基座刚体上，将相应的n个传感器合作目标安装在目标刚体上；将基座刚体与目标刚体置于m个不同的标定相对位置，进行标定数据采集；根据标定数据求解各传感器相对于基座刚体的相对位姿参数和各传感器合作目标相对于目标刚体的相对位姿参数；根据各传感器位姿标定结果和各传感器测量结果分别计算目标刚体相对于基座刚体的相对位姿参数；对冗余位姿计算结果进行融合，确定目标刚体相对于基座刚体的相对位姿参数。本发明解决了多传感器测量六自由度位姿时，基座刚体、目标刚体、各传感器及合作目标坐标系不统一，以及冗余测量信息未充分利用的问题。

技术领域

本发明属于位姿测量技术领域，特别涉及一种基于多传感器标定与融合的六自由度位姿确定方法。

背景技术

对目标的六自由度位置和姿态进行高精度测量，在工业生产和航空航天领域都有广泛需求，在一些特殊的场合，考虑被测目标对象的复杂性，单独一个或一种传感器不容易满足对目标六自由度的高精度测量需求，采用多个或多种传感器，相互配合开展目标六自由度测量，是解决这一问题的可行方案；

传统的采用多传感器进行六自由度位姿进行测量的方法存在以下不足：

(1)未考虑多个传感器坐标系与基座刚体机械坐标系不重合，相应的合作目标坐标系与目标刚体机械坐标系不重合的情况；

(2)对冗余测量的位姿信息进行融合时，未充分考虑对测量精度的影响。

发明内容

本发明解决的技术问题是：克服现有技术的不足，提供了一种基于多传感器标定与融合的六自由度位姿确定方法，充分发挥了利用多传感器进行六自由度位姿测量的精度。

本发明的技术解决方案是：

一种基于多传感器标定与融合的六自由度位姿确定方法，包括以下步骤：

1)将n个传感器安装在基座刚体上，将相应的n个传感器合作目标安装在目标刚体上，在所述基座刚体上建立基座刚体机械坐标系，在所述目标刚体上建立目标刚体机械坐标系，在所述n个传感器和n个传感器合作目标上分别建立各自的理论坐标系，n为大于等于2的正整数；

2)将基座刚体与目标刚体置于m个不同的标定相对位置，采用六自由度标定设备进行测量，获取标定设备测量值，同时采用n个传感器进行测量，获取相同时刻下n个传感器标定测量值；所述标定设备测量值为在各标定相对位置下，目标刚体机械坐标系相对于基座刚体机械坐标系的六自由度位姿参数，所述传感器标定测量值为在标定相对位置下，各传感器的理论坐标系相对于与其对应的传感器合作目标的理论坐标系的六自由度位姿参数中的一个或多个；m为正整数；

3)根据步骤2)获取的m组标定设备测量值和传感器标定测量值求解得到传感器位姿标定结果，所述传感器位姿标定结果为各传感器的理论坐标系相对于基座刚体机械坐标系的六自由度位姿参数和各传感器合作目标的理论坐标系相对于目标刚体机械坐标系的六自由度位姿参数；

4)采用n个传感器同时进行测量，根据各传感器的测量值和由步骤3)获取的传感器位姿标定结果，求解得到目标刚体机械坐标系相对基座刚体机械坐标系的位姿参数；

5)根据步骤4)得到的n个传感器的测量结果，确定目标刚体机械坐标系相对于基座刚体机械坐标系的六自由度位姿参数。

优选的，所述步骤3)中，求解得到传感器位姿标定结果，具体包括以下步骤：

3-1)假设在各标定相对位置下各传感器的理论坐标系相对于基座刚体机械坐标系的位姿参数和相应传感器合作目标的理论坐标系相对于目标刚体机械坐标系的位姿参数，结合在各标定相对位置下的标定设备测量值，计算得到在各标定相对位置下各传感器的标定测量估计值；