11. 机器人正运动学---姿态描述之四元数

目录

 

1. 引言

2. 四元数转旋转矩阵

3. 已知旋转矩阵求四元数

3.1 先求w

3.2 先求x

3.3 先求y

3.4 先求z

4. 四元数与旋转矩阵转换的C++实现

4. 总结


1. 引言

上一篇文章我们主要介绍了欧拉角与旋转矩阵之间的关系,这篇文章介绍旋转矩阵和四元数之间的关系。关于四元数的定义和工作原理这里就不详细介绍了,给大家推荐一篇文章Understanding Quaternions以及它的中文翻译版理解四元数。我认为这篇文章写得非常透彻,相信不管是谁再去写关于四元数原理的文章都无出其右了。

2. 四元数转旋转矩阵

四元数对空间中的点进行变换有固定的表达形式p_{1}=qpq^{-1}。我们这里只讨论归一化的四元数。设一个四元数q表达如下:

                                                                    q=[cos\frac{\theta}{2}, sin\frac{\theta}{2}\vec{n}]=[w,(x,y,z)]

这个四元数描述的就是绕轴\vec{n}旋转\theta。空间中一个点p在惯性系下的坐标为[p_x,p_y,p_z],在用四元数进行操作之前我们首先要把它变成纯四元数的形式即p=[0,(p_x,p_y,p_z)],运用四元数对这个点进行旋转,得到一个新的点p_1,如下式:

                         p_1=(w+x\vec{i}+y\vec{j}+z\vec{k})\cdot (p_x\vec{i}+p_y\vec{j}+p_z\vec{k})\cdot(w-x\vec{i}-y\vec{j}-z\vec{k})=\begin{bmatrix} 1-2y^2-2z^2 & 2(xy-wz) & 2(xz+wy)\\ 2(xy+wz) & 1-2x^2-2z^2 & 2(yz-wx)\\ 2(xz-wy) & 2(yz+wx) & 1-2x^2-2y^2 \end{bmatrix}\begin{bmatrix} p_x\\ p_y\\ p_z \end{bmatrix}

因此在已知四元数时,旋转矩阵的计算公式如下:

                                                    R=\begin{bmatrix} 1-2y^2-2z^2 & 2(xy-wz) & 2(xz+wy)\\ 2(xy+wz) & 1-2x^2-2z^2 & 2(yz-wx)\\ 2(xz-wy) & 2(yz+wx) & 1-2x^2-2y^2 \end{bmatrix}

3. 已知旋转矩阵求四元数

关于旋转矩阵转四元数有一个问题曾经困扰了我很久,就是关于开方的问题,大家都了解开方其实是有正负两个解的,为什么忽略一个解呢?不只是在这里,在很多其他地方,特别是后面会讲到的机器人逆解问题,也有类似的情况出现。为了避免大家踩坑,我们先把这个坑填上。先给一个命题:四元数和它的相反四元数描述相同的旋转。这是一个真命题,简单证明一下,我们知道四元数对点进行变换的公式如下:

                                                                                      p_1=qpq^{-1}

对于归一化四元数q,它的逆是q^{-1}(其实对于归一化四元数,它的逆就是它的共轭),那么四元数-q的逆呢?很显然是-q^{-1},如果让四元数-q作用于同一个点p呢:

                                                                     p_1=(-q)p(-q^{-1})=qpq^{-1}

负负得正,因此-qq作用效果是一样的。

接下来回归正题,旋转矩阵怎么求四元数呢,我们还是要从前面得到的旋转矩阵出发来进行推导,在这个过程中不要忘记我们讨论旋转对应的四元数始终是归一化的,即元素平方和为1。

需要注意的是,我们求解四元数有四种途径,从数学上讲他们的行为是一样的,除了在表达式奇异的位置(所谓表达式奇异在这里是指分母为0),但是从数值分析的角度上考虑,当分母太小时,计算结果对舍入误差非常敏感,因此需要尽量减小舍入误差的影响,所以才会根据实际情况选择一种解法来求四元数。接下来我们通过以下旋转矩阵求四元数:

                                          R=\begin{bmatrix} 1-2y^2-2z^2 & 2(xy-wz) & 2(xz+wy)\\ 2(xy+wz) & 1-2x^2-2z^2 & 2(yz-wx)\\ 2(xz-wy) & 2(yz+wx) & 1-2x^2-2y^2 \end{bmatrix}

3.1 先求w

我们详细讲一下先求w,后面的方法类似,把R对角线元素相加r_{11}+r_{22}+r_{33}=3-4x^2-4y^2-4z^2(注意r_{ij}代表R的第i行第j列的元素),前面也提到对于归一化四元数它的元素平方和为1,即x^2+y^2+z^2+w^2=1,所以r_{11}+r_{22}+r_{33}=3+(4w^2-4)=4w^2-1,所以怎么求w呢?很简单:

                                                                    w=\frac{\sqrt{1+r_{11}+r_{22}+r_{33}}}{2}

有了w,其他元素就比较容易了,请观察r_{21}r_{12},两个元素作差再除以4w就得到了z,同理r_{13}r_{31}作差再除以4w可以得到yr_{32}r_{23}作差再除以4w可以得到x,最终计算结果如下:

                                                                                x=\frac{r_{32}-r_{23}}{4w}

                                                                                y=\frac{r_{13}-r_{31}}{4w}

                                                                                z=\frac{r_{21}-r_{12}}{4w}

接下来踩一个坑,求w时有一个开方运算,为什么不取负值?你可能也看到了,如果w取负值,相应的xyz的求解分母都有w,他们也会取负值,所以得到的四元数就是我们在第三节开头提到的负四元数,它和我们公式得到的四元数描述相同的旋转,所以我们取w为正的一组解,后面三种情况也相同。

3.2 先求x

在3.1中如果我们求出的w过小,为了避免解的不稳定,需要采用其他的求解方法,我们可以从对角线元素中先求xy或者z。具体先求哪一个呢?我们当然希望先求大的那一个,因为你会看到,通过对角线元素求出的那个值始终是作为另外三个等式的分母,这个值越大越不容易受到舍入误差的影响。

具体的比较方法就是比较三个对角线元素哪个大,如果r_{11}最大,说明x相对来说绝对值最大,为什么这么说呢?大概提一下:

                        r_{11}=1-2y^2-2z^2=2-2y^2-2z^2-1=2(1-y^2-z^2)-1=2(w^2+x^2)-1

                        r_{22}=2(w^2+y^2)-1

                        r_{22}=2(w^2+z^2)-1

这样很明显可以看出当r_{11}最大时说明x绝对值最大,当r_{22}最大时说明y绝对值最大,当r_{33}最大时说明z绝对值最大。

当你发现r_{11}最大时,我们就先求x,根据归一化四元数元素平方和为1,利用对角线元素我们就可以求解x,如下:

                                                                    x=\frac{\sqrt{1+r_{11}-r_{22}-r_{33}}}{2}

后面的求解就类似了,找到矩阵主对角线对称的元素,作和或者作差然后除以4x即可。最终计算结果如下:

                                                                               w=\frac{r_{32}-r_{23}}{4x}

                                                                                y=\frac{r_{21}+r_{12}}{4x}

                                                                                z=\frac{r_{31}+r_{13}}{4x}

3.3 先求y

当你发现r_{22}最大时,我们就先求y,思路与3.1一样,结果如下:

                                                                   y=\frac{\sqrt{1-r_{11}+r_{22}-r_{33}}}{2}

                                                                                w=\frac{r_{13}-r_{31}}{4y}

                                                                                 x=\frac{r_{21}+r_{12}}{4y}

                                                                                 z=\frac{r_{23}+r_{32}}{4y}         

3.4 先求z

当你发现r_{33}最大时,我们就先求z,思路与3.1一样,结果如下:

                                                                  z=\frac{\sqrt{1-r_{11}-r_{22}+r_{33}}}{2}

                                                                               w=\frac{r_{21}-r_{12}}{4z}

                                                                                x=\frac{r_{13}+r_{31}}{4z}

                                                                                y=\frac{r_{23}+r_{32}}{4z}

4. 四元数与旋转矩阵转换的C++实现

代码是算法的载体,我始终觉得不写烂代码是算法工程师的基本素养,接下来我们把旋转矩阵与四元数的变换关系用C++实现一遍。

先说从四元数转旋转矩阵,这个过程是很直接的,我们直接把R对应的旋转矩阵写入即可。头文件中的描述如下:

/**
 * @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. 总结

这篇文章主要介绍了旋转矩阵与四元数之间的关系,下一篇文章我们将介绍姿态描述的另一种方法:轴角/旋转向量。由于个人能力有限,所述内容难免存在疏漏,欢迎指出,欢迎讨论。

### 回答1: 六轴机器人是一种具有六个自由度的机器人,其运动学逆解是指对机器人的末端执行器的位置和姿态进行求解,以实现机器人确运动。 六轴机器人运动学解是指已知各个关节的角度,求解出机器人末端执行器的位姿。根据六个关节的角度、长度以及关节之间的连接方式,可以使用解析法、几何法或矢量法等方法来求解机器人解。这样可以得到机器人末端的位置(三维坐标)和姿态姿态矩阵或四元数),从而实现末端的运动。 六轴机器人运动学逆解是指已知机器人末端执行器的位姿,求解出各个关节的角度。机器人的逆解是一个反向问题,通常使用数值方法(如牛顿法、雅克比转置法等)进行求解。逆解的目标是通过给定末端执行器的位姿来确定合适的关节角度,使机器人能够到达指定的位置和姿态。逆解可通过迭代算法逐步调整关节角度,直到满足末端执行器的位姿要求。 运动学逆解在机器人控制中起着重要的作用,它们是实现机器人精确运动控制和路径规划的基础。通过逆解,可以精确控制六轴机器人的末端执行器的位置和姿态,实现复杂的运动任务,如拾取、装配、焊接等。这对于自动化生产线、工业制造和航天航空等领域具有重要意义。 ### 回答2: 六轴机器人是一种由六个关节组成的机械臂,可以在三维空间内自由移动和执行各种工作任务。六轴机器人运动学逆解是指通过机械臂的关节角度计算出机械臂的末端执行器的空间位置和姿态,或者通过给定的末端执行器的目标空间位置和姿态计算出关节角度。 机器人运动学解是从机器人基座坐标系到末端执行器坐标系的过程。它通过利用机械结构和关节限制条件,将各个关节的角度转化为末端执行器的位置和姿态运动学解的目的是求解出机械臂末端执行器的位置和姿态,从而确定机器人姿态机器人运动学逆解是从末端执行器坐标系到机器人基座坐标系的过程。它是运动学解的逆运算,通过给定末端执行器的目标位置和姿态,计算出机器人各个关节的角度值。运动学逆解的目的是确定关节角度,从而实现机械臂从给定的位置到目标位置的移动。 六轴机器人运动学逆解是机器人的基本问题之一,能够帮助机器人完成各种任务和运动控制。在实际应用中,逆解通常利用数学方法和算法进行计算,通过求解运动学逆解,机器人能够自主地执行各种动作和任务。这对于工业自动化、物流和生产线等领域都具有重要的意义。
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值