引言
在机器人控制中,关于旋转的表示上,有好几种不同的表示方式,今天我们来一一讲解,并通过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)
可以看到,结果还是正确的。
参考链接
May the force be with you!
评论(0)
您还未登录,请登录后发表或查看评论