机器人学：物体转动角度变化率与其世界坐标系下角速度的关系推导

引言

一个物体在空间中的六个自由度中有三个旋转角度，如下图所示所有坐标系遵循右手规则。我们定义该物体的欧拉角[r p y]表示：绕x轴的为r (roll)即翻滚角，绕y轴的为p (pitch)即俯仰角，绕y轴的为y (yaw)即偏航角。那么物体在世界坐标系中的角速度是不是：

w = [dr dp dy]

推导

我们定义物体欧拉角的旋转轴rax,pax,yax，通过物体在世界坐标系中的旋转矩阵R，即可以表示出物体角度变化率与角速度之间的关系。这里最要的一点是定义物体位姿传递给世界坐标系的顺序，我们规定物体先翻滚再俯仰最后偏航传递到世界坐标系。用到坐标变换的知识，我们通过matlab的符号解推导。

Matlab代码

clear
clc

syms p r y dp dr dy

rax = [1,0,0]'; pax = [0,1,0]';yax = [0,0,1]';

R1 = Ryaw(y);
R2 = Ryaw(y)*Rpitch(p);
R3 = Ryaw(y)*Rpitch(p)*Rroll(r);

w = R1*yax*dy+R2*pax*dp+R3*rax*dr

T = jacobian(w,[dr,dp,dy])

invT = simplify(inv(T))

输出结果

其结果表示为矩阵形式

相关函数

其中用到的函数分别为：

function R = Rroll(a)
    R = [1      0       0;
         0 cos(a) -sin(a);
         0 sin(a)  cos(a)];
end

function R = Ryaw(a)
    R = [cos(a) -sin(a)  0;
         sin(a)  cos(a)  0
              0       0  1];
end

function R = Rpitch(a)
    R = [cos(a)  0  sin(a);
              0  1       0;
         -sin(a) 0  cos(a)];
end

总结

上述即物体角度变化率与其在世界坐标系中的角速度的关系。用到的坐标变换关系你可以参考Kajita教授编著的《仿人机器人》一书，其中有对旋转关节旋转轴变换到其他坐标系的描述与推导，作者这里直接应用了这一知识点。