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

权利要求书

1.一种基于多传感器标定与融合的六自由度位姿确定方法，其特征在于步骤如下：

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

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

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

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

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

2.根据权利要求1所述的一种基于多传感器标定与融合的六自由度位姿确定方法，其特征在于，所述步骤3)中，求解得到传感器位姿标定结果，具体包括以下步骤：

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

3-2)根据步骤3-1)得到的在各标定相对位置下各传感器的标定测量估计值结合传感器标定测量值计算各传感器的位姿标定误差函数；

3-3)根据各传感器的位姿标定误差函数求解得到传感器位姿标定结果。

3.根据权利要求2所述的一种基于多传感器标定与融合的六自由度位姿确定方法，其特征在于：所述步骤3-1)中，计算得到在各标定相对位置下各传感器的标定测量估计值，具体为：

构造表征传感器理论坐标系相对于合作目标理论坐标系的六自由度位姿参数的齐次变换矩阵：

其中，Q_ij为表征在第j标定相对位置第i传感器理论坐标系相对于合作目标理论坐标系的六自由度位姿参数的齐次变换矩阵，为第i传感器的理论坐标系相对基座刚体机械坐标系的六自由度位姿参数的假设值，为第i传感器合作目标的理论坐标系相对目标刚体机械坐标系的六自由度位姿参数的假设值，为在第j标定相对位置标定设备测量值，x、y、z、α、β、γ分别表示笛卡尔坐标系下沿x轴位移、沿y轴位移、沿z轴位移、绕x轴转角、绕y轴转角、绕z轴转角，i＝1,2...,n，j＝1,2...,m，

其中，

通过六自由度参数与齐次变换矩阵的转换关系，由Q_ij确定为第i传感器在第j标定相对位置下的标定测量估计值。

4.根据权利要求3所述的一种基于多传感器标定与融合的六自由度位姿确定方法，其特征在于：所述步骤3-2)中，传感器的位姿标定误差函数表达式为：

其中，J_i为第i传感器的标定误差函数，w_k为加权系数，为第i传感器在第j标定相对位置的传感器标定测量值，u表示第i传感器能够测量的自由度数。

5.根据权利要求4所述的一种基于多传感器标定与融合的六自由度位姿确定方法，其特征在于，所述步骤3-3)中，根据各传感器的标定误差函数求解得到传感器位姿标定结果，具体为：以各传感器标定误差函数值最小为目标，各传感器理论坐标系相对基座刚体机械坐标系的位姿参数和与其对应的传感器合作目标理论坐标系相对目标刚体机械坐标系的位姿参数作为变量，进行目标优化计算，求解得到变量的最优值。