引言

在机器人控制中,关于旋转的表示上,有好几种不同的表示方式,今天我们来一一讲解,并通过pybullet验证他们之间的关系。

不同表示方式

旋转的表示方式,可以大致分为RPY角,欧拉角,四元数和轴角。

RPY角

首先解释字母:

R:Roll  横滚  

P:pitch 俯仰

Y:Yaw  偏航

从字母的顺序就可以知道旋转的次序:首先绕着x轴转,再绕着y轴转,最后绕着z轴转。但是需要注意的是RPY角中的旋转都是绕着最开始的坐标系旋转的,这是与欧拉角不同的地方。

欧拉角

欧拉角和RPY角很像,但是其旋转都是绕着旋转后的坐标系转的,而且旋转的次序是先z-y-x。比如说绕着在绕着y转时:

先绕着开始的坐标系转z,再绕着旋转后的坐标系转y。

经过这样旋转后,欧拉角旋转后的姿态和RPY角旋转后的姿态是一样的。

我们以欧拉角来分析,其实欧拉角存在一些问题:

1、死锁问题:

死锁,说的是当第二次旋转为90度的时候(宽泛来讲,第二次旋转后坐标系发生重合),第三次旋转和第一次旋转的旋转方向一样,从整体来看就是失去了一个自由度的控制。无法实现球面平滑插值。

上图就是发生在死锁情况下,且坐标系仍然对齐原坐标但方向变化的情况。

可以发现,由于机械臂的轴向为y轴,因此当代码中的zyx欧拉角转完后,机械臂轴向仍然不变;而盆模型的姿态并不只体现在y轴一个方向上,因此由平着转为竖直:

        init_position = [0.355,0,0.22]
        init_posture = p.getQuaternionFromEuler([ -math.pi/2, 0,-math.pi / 2.])
        a = p.getQuaternionFromEuler([1, math.pi/2, 1])
        rr = np.array(p.getMatrixFromQuaternion(a)).reshape(3,3)
        print(rr)
        init_posture = self.quaternion_rotation(init_posture,a)
        p.loadURDF("tray/tray.urdf", basePosition=[0, 0.9, 0.25], baseOrientation=a)
        jointposes = p.calculateInverseKinematics(self.UR5, 6, init_position,init_posture, maxNumIterations=100)
        for j in range(6):
            p.resetJointState(self.UR5, j + 1, jointposes[j])
        p.stepSimulation()

2、表示不唯一

由于角度的周期性变化,导致在表示的形式上也不唯一。且死锁问题的存在更增加了解的数目。当然这一点可以通过限制范围在[-180,180]解决。

3、插值后死锁引发震荡

当插值后的值正好为90度附近时,会出现相同姿态的欧拉角差距很大的情况(第二条)。

轴角

轴角形式的旋转,理解起来能够容易一些。如果说欧拉角是绕着坐标系进行三次顺序进行的旋转,那么轴角实现的就是将这三次旋转后得到复合旋转,找对其对应的旋转轴和转角。由于轴角形式的旋转只进行一次,因此可以避免欧拉角带来的死锁问题。

其表示形式为一个用于表示符合旋转中转轴的单位三维空间矢量,加一个标量的转角。

在进行插值的时候,由于单位的三维空间矢量和标量的转角并不能归一化处理,因此会造成插值不稳定。

四元数

四元数是另一种旋转的表示方式,用于旋转的四元数,每个分量的范围都在(-1,1)。其将单位化统一到四维空间中,可以方便得进行平滑插值,并避免死锁问题。

在进行线性插值,一般可以用这个式子进行表示:

为了保证单位性,需要对其进行归一化:

但是我们实际期望的是四元数中的转角线性变化:

因此需要球面插值:

不同表示之间的转换

在这里,主要介绍两种转换,四元数相乘与旋转矩阵;另一种是轴角到四元数的转换。

四元数---旋转矩阵

四元数相乘的公式如下:

代码实现:

其中的np.cross实现了向量叉乘。

我们测试了两个四元数相乘后得到结果转换为旋转矩阵的值,以及旋转矩阵直接相乘的值。发现在使用中,使用四元数右乘和矩阵右乘实现的效果相同。

        init_posture = p.getQuaternionFromEuler([ -math.pi/2, 0,-math.pi / 2.])
        r = np.array(p.getMatrixFromQuaternion(init_posture)).reshape(3,3)
        a = p.getQuaternionFromEuler([1, math.pi/2, 1])
        rr = np.array(p.getMatrixFromQuaternion(a)).reshape(3,3)
        print(np.dot(r,rr))
        init_posture = self.quaternion_rotation(init_posture,a)
        print(np.array(p.getMatrixFromQuaternion(init_posture)).reshape(3,3))

轴角---四元数

使用pybullet中自带的接口函数就可以:

...    
def axial_angle_2_quaternion(self,axis,alpha):
        q = p.getQuaternionFromAxisAngle(axis,alpha)
        print(np.array(p.getMatrixFromQuaternion(q)).reshape(3,3))
        return q
...
...
U.axial_angle_2_quaternion([1,0,0],math.pi/2)

可以看到,结果还是正确的。

参考链接

四元数(Quaternions)

欧拉角、轴角与四元数

May the force be with you!