[发明专利]基于四元数的两个独立系统坐标系之间的转换方法

权利要求书

1.基于四元数的两个独立系统坐标系之间的转换方法，其特征在于：包括转换参数的标定过程和信息转换过程；

所述的转换参数的标定过程包括以下步骤：

S11：系统A的标志物为a，系统A测量标志物a的位置信息和空间姿态信息；

系统B的标志物为b，系统B测量标志物b的位置信息和空间姿态信息；

标志物a和标志物b固定在一个固定座上；

S12：保持标志物a、b相对不动，转动固定座即同时转动标志物a和标志物b，获取n组运动过程的两个标志物的位置信息和空间姿态信息；

S13：以位置信息中的三维空间坐标为四元数的虚部，把位置信息转化为实部为0的四元数：

p_mi＝(0,px_mi,py_mi,pz_mi),m＝a,b；i＝1,2…n；

如果获取的空间姿态信息是单位四元数，则不用做处理，否则把获取的空间姿态信息转为描述旋转的单位四元数；

q_mi＝(qw_mi,qx_mi,qy_mi,qz_mi),m＝a,b；i＝1,2…n；

表示空间姿态的单位四元数两两组合，得出n(n-1)/2个相对旋转值：

S14：设从系统A到系统B的转换参数为旋转参数q_AB、原点偏移p_AB和标志物偏移参数p_ab，系统A到系统B的信息转换公式为：

把步骤S13的n(n-1)/2个表示相对空间姿态的单位四元数和n个表示位置的四元数代入公式中，最小化均方差：

采用高斯-牛顿迭代法来求解出最优参数q_AB、p_AB、p_ab；

所述的信息转换过程包含以下步骤：

S21：通过系统A获取标志物a的位置信息和空间姿态信息；

S22：以位置信息中的三维空间坐标为四元数的虚部，把位置信息转化为实部为0的四元数p_a＝(0,px_a,py_a,pz_a)；如果获取的空间姿态信息是单位四元数，则不用做处理，否则把获取的空间姿态信息转为描述旋转的单位四元数q_a＝(qw_a,qx_a,qy_a,qz_a)；

S23：根据信息转化公式和标定后的转换参数，计算出该标志物在系统b下的位置信息p_B和空间姿态信息q_B：

按照使用的需要，把位置信息p_B等价地转化为平面直角坐标系的三维空间坐标、极坐标系的坐标等，把空间姿态信息q_B等价地转化为旋转矩阵、欧拉角、单位四元数。

2.根据权利要求1所述的基于四元数的两个独立系统坐标系之间的转换方法，其特征在于：所述的旋转参数q_AB、原点偏移p_AB和标志物偏移参数p_ab能够通过高斯-牛顿迭代法或LM阻尼法或者Dog-Leg法求出。