目录
1. 引言
2. 四元数转旋转矩阵
3. 已知旋转矩阵求四元数
3.1 先求w
3.2 先求x
3.3 先求y
3.4 先求z
4. 四元数与旋转矩阵转换的C++实现
4. 总结
1. 引言
上一篇文章我们主要介绍了欧拉角与旋转矩阵之间的关系,这篇文章介绍旋转矩阵和四元数之间的关系。关于四元数的定义和工作原理这里就不详细介绍了,给大家推荐一篇文章Understanding Quaternions以及它的中文翻译版理解四元数。我认为这篇文章写得非常透彻,相信不管是谁再去写关于四元数原理的文章都无出其右了。
2. 四元数转旋转矩阵
3. 已知旋转矩阵求四元数
关于旋转矩阵转四元数有一个问题曾经困扰了我很久,就是关于开方的问题,大家都了解开方其实是有正负两个解的,为什么忽略一个解呢?不只是在这里,在很多其他地方,特别是后面会讲到的机器人逆解问题,也有类似的情况出现。为了避免大家踩坑,我们先把这个坑填上。先给一个命题:四元数和它的相反四元数描述相同的旋转。这是一个真命题,简单证明一下,我们知道四元数对点进行变换的公式如下:
对于归一化四元数
,它的逆是
(其实对于归一化四元数,它的逆就是它的共轭),那么四元数
的逆呢?很显然是
,如果让四元数
作用于同一个点
呢:
负负得正,因此
和
作用效果是一样的。 接下来回归正题,旋转矩阵怎么求四元数呢,我们还是要从前面得到的旋转矩阵出发来进行推导,在这个过程中不要忘记我们讨论旋转对应的四元数始终是归一化的,即元素平方和为1。 需要注意的是,我们求解四元数有四种途径,从数学上讲他们的行为是一样的,除了在表达式奇异的位置(所谓表达式奇异在这里是指分母为0),但是从数值分析的角度上考虑,当分母太小时,计算结果对舍入误差非常敏感,因此需要尽量减小舍入误差的影响,所以才会根据实际情况选择一种解法来求四元数。接下来我们通过以下旋转矩阵求四元数:
3.1 先求w
接下来踩一个坑,求
时有一个开方运算,为什么不取负值?你可能也看到了,如果
取负值,相应的
,
,
的求解分母都有
,他们也会取负值,所以得到的四元数就是我们在第三节开头提到的负四元数,它和我们公式得到的四元数描述相同的旋转,所以我们取
为正的一组解,后面三种情况也相同。
3.2 先求x
在3.1中如果我们求出的
过小,为了避免解的不稳定,需要采用其他的求解方法,我们可以从对角线元素中先求
,
或者
。具体先求哪一个呢?我们当然希望先求大的那一个,因为你会看到,通过对角线元素求出的那个值始终是作为另外三个等式的分母,这个值越大越不容易受到舍入误差的影响。 具体的比较方法就是比较三个对角线元素哪个大,如果
最大,说明
相对来说绝对值最大,为什么这么说呢?大概提一下:
3.3 先求y
3.4 先求z
4. 四元数与旋转矩阵转换的C++实现
代码是算法的载体,我始终觉得不写烂代码是算法工程师的基本素养,接下来我们把旋转矩阵与四元数的变换关系用C++实现一遍。 先说从四元数转旋转矩阵,这个过程是很直接的,我们直接把
对应的旋转矩阵写入即可。头文件中的描述如下:
/**
* @brief This class is used to represent rotation in 3d space.
* */
class Rotation {
public:
Rotation();
/**
* @brief Construct
* @param[in] Xx \ref data[0]
* @param[in] Yx \ref data[1]
* @param[in] Zx \ref data[2]
* @param[in] Xy \ref data[3]
* @param[in] Yy \ref data[4]
* @param[in] Zy \ref data[5]
* @param[in] Xz \ref data[6]
* @param[in] Yz \ref data[7]
* @param[in] Zz \ref data[8]
*/
Rotation(double Xx, double Yx, double Zx, double Xy, double Yy, double Zy,
double Xz, double Yz, double Zz);
/**
* @brief Convert quaternion to Rotation object.
* q=[w,(x,y,z)], w is the scalar, (x,y,z)is the direction vector.
* @param[in] x X of the quaternion
* @param[in] y Y of the quaternion
* @param[in] z Z of the quaternion
* @param[in] w W of the quaternion
*/
static Rotation quaternion(double x, double y, double z, double w);
/**
* @brief Get quaternion from the Rotation.
* @param[in] x Reference to quaternion x
* @param[in] y Reference to quaternion y
* @param[in] z Reference to quaternion z
* @param[in] w Reference to quaternion w
*/
void getQuaternion(double &x, double &y, double &z, double &w) const;
/**
* @brief data of the rotation matrix
* @note the rotation matrix is defined as:
* [data[0], data[1], data[2];
* data[3], data[4], data[5];
* data[6], data[7], data[8]]
* */
double data[9];
};
在头文件中我们定义了一个数组来保存旋转矩阵的9个元素。四元数转旋转矩阵的代码如下:
Rotation Rotation::quaternion(double x, double y, double z, double w) {
Rotation r;
r.data[0] = 1 - 2 * y * y - 2 * z * z;
r.data[1] = 2 * (x * y - w * z);
r.data[2] = 2 * (x * z + w * y);
r.data[3] = 2 * (x * y + w * z);
r.data[4] = 1 - 2 * x * x - 2 * z * z;
r.data[5] = 2 * (y * z - w * x);
r.data[6] = 2 * (x * z - w * y);
r.data[7] = 2 * (y * z + w * x);
r.data[8] = 1 - 2 * x * x - 2 * y * y;
return r;
}
旋转矩阵转四元数的代码如下:
void Rotation::getQuaternion(double &x, double &y, double &z, double &w) const {
double epsilon = 1E-12;
double r11 = data[0];
double r12 = data[1];
double r13 = data[2];
double r21 = data[3];
double r22 = data[4];
double r23 = data[5];
double r31 = data[6];
double r32 = data[7];
double r33 = data[8];
w = sqrt(1.0f + r11 + r22 + r33) / 2.0f;
if (fabs(w) > epsilon) {
// compute w first.
x = (r32 - r23) / (4 * w);
y = (r13 - r31) / (4 * w);
z = (r21 - r12) / (4 * w);
} else {
if (r11 >= r22 && r11 >= r33) {
// compute x first
x = sqrt(1.0f + r11 - r22 - r33) / 2.0f;
w = (r32 - r23) / (4 * x);
y = (r21 + r12) / (4 * x);
z = (r31 + r13) / (4 * x);
} else if (r22 >= r11 && r22 >= r33) {
// compute y first
y = sqrt(1.0f - r11 + r22 - r33) / 2.0f;
w = (r13 - r31) / (4 * y);
x = (r21 + r12) / (4 * y);
z = (r23 + r32) / (4 * y);
} else {
// compute z first
z = sqrt(1.0f - r11 - r22 + r33) / 2.0f;
w = (r21 - r12) / (4 * z);
x = (r13 + r31) / (4 * z);
y = (r23 + r32) / (4 * z);
}
}
}
原理讲清楚之后,代码其实十分简单,就是把公式套用一遍,这里就不过多介绍了,旋转矩阵与四元数的转换相关C++源代码我已经上传到github: https://github.com/hitgavin/rosws/tree/master/src/frames,感兴趣可以参考一下。
4. 总结
这篇文章主要介绍了旋转矩阵与四元数之间的关系,下一篇文章我们将介绍姿态描述的另一种方法:轴角/旋转向量。由于个人能力有限,所述内容难免存在疏漏,欢迎指出,欢迎讨论。