角速度与旋转矩阵的转换关系及思考

　　在机器人的控制和轨迹规划等领域，算法结果常常能够得到机器人执行器的速度，如移动机器人的角速度和线速度。在有些仿真情况下，或是其它需要实时更新机器人的运动状态时，我们常常需要根据这些计算出的速度参数（控制率）对机器人的位姿姿态进行更新。这就涉及到了角速度与旋转矩阵之间的对应转换关系。本文将简要给出一种它们之间的转换关系，并在后半部分着重介绍转换出的姿态矩阵在不同情况下的左乘、右乘的不同意义，这一问题也曾经让作者混淆了很久。

一. 控制率与位姿变换矩阵( 4×4 )的转换

　　这里主要以Peter corke的视觉伺服工具箱中给出的转换方法为例，事实上，更为详尽的转换方式可以去了解一下指数映射关系，可以参见白老师的文章《 lie group and computer vision : 李群、李代数在计算机视觉中的应用》。
　　我们知道，一个变量的导数等于它本身再乘以一个系数。对于三维空间中的旋转矩阵来说，有如下表达式：

R˙(t)=S(ω)⋅R(t)
其中， S(ω) 为关于角速度 ω 的反对称矩阵：

S(ω)=⎡⎣⎢0ωz−ωy−ωz0ωxωy−ωx0⎤⎦⎥

　　考虑在 R(t) 处进行微分：
R˙≈R(t+△t)−R(t)ζt
　　经过整理后可得：
R(t+△t)≈ζt⋅S(ω)⋅R(t)+R(t)≈(ζtS(ω)+I3×3))⋅R(t)
　　因此，矩阵 △R=ζtS(ω)+I3×3 即为角速度 ω 对应的旋转矩阵。
　　对于线速度 v 来说，在一定时间 t 内，所导致的坐标系的变化只有平移没有旋转。因此，速度参数（控制率） v 和 ω 所对应的位姿变换矩阵 △T 为：
△T=[ζt⋅S(ω)01×3ζtv0]+I4×4
　　当需要把位姿变换矩阵 △T 转换为速度参数 v 和 ω 时，按照上面的步骤反着来就行了。

二. 那么问题来了：

1. 问题描述

　　上述控制率与位姿变换矩阵的转换关系在视觉伺服工具箱中对应于 delta2tr( v )和tr2delta( T0 , T1 )这两个函数。书中给出了一个具体的例子：

　　这个示例我们记为例1。从上面我们可以看出，经过转换出的位姿变换矩阵 △T=delta2tr（d） 最终被 T0 左乘能够得到 T1 的值。 但是在具体的例程代码中，如下面工具箱中的视觉伺服仿真的代码：

　　我们看到，位姿变换矩阵 △T=delta2tr（d） 是被摄像机位姿 Tcam 右乘来实现位姿更新的。
　　除了这个视觉伺服工具箱外，在VISP开源库中，摄像机的姿态也是通过 右乘来实现的：

　　函数 direct(v_sat,delta_t)为利用指数映射将速度参数 v_sat 转换为位姿变换矩阵 △T 。
　　这两个例子我们记为例2。那么为什么有些情况下是左乘，有些情况下是右乘，这两者之间又有什么区分呢？

2. 左乘与右乘的不同情况及意义

　　首先来说例1中的左乘。对于左乘的情况，需要弄清楚的一点是，例1中 T0 、 T1 表示的是目标姿态在相同的参考坐标系（默认为世界坐标系）下的表示，因此，在这个例子中 tr2delta（d）求出的速度参数 v 也是表示在这个坐标系下的。
　　所以，速度参数 v 对应的姿态转换矩阵 △T=delta2tr（d） 也是相对于这个世界坐标系来表示的。因此，当从状态 T0 转换到状态 T1 时，需要被 T0 左乘（如果不理解为什么在世界坐标系下的速度对应的矩阵需要左乘，可以自己假想一下下面的场景）：
　　
　　位姿 T0 和位姿 T1 都是在世界坐标系下表示的，在 T0 位姿时，使其以速度 v 进行运动，其对应的姿态转换矩阵为 △T 。考虑一个虚拟的与世界坐标系 Cworld 重合的坐标系 B ，假定 T0 在坐标系 B 中固定不动，那么姿态变化矩阵 △T 相当于坐标系 B 在世界坐标系 Cworld 下的表示。即：

worldTB=△T
　　因此，通过连杆矩阵相乘，可以得到：
T1=worldTB⋅BT0=△T⋅T0

　　其次再说右乘。例2中算法所求出的摄像机运动速度 v 均是相对于当时摄像机自身坐标系来说的。因此，根据连杆矩阵变换，可以得到：

T1=T0⋅△T
　　此外，右乘这种情况也可以这样理解：
　　摄像机的运行速度对应的位姿转换矩阵 △T 可以理解成目标新的位姿 T1 在当前位姿 T0 所在的坐标系下的表示。比如，假定目标当前处于原点(0,0,0)的位置（自身坐标系），线速度为(5,0,0)，那么当一定时间过去后，目标（新的位姿）是不是位于(5,0,0)处（原自身坐标系）？

　　所以，同样的对于速度参数转换得到的位姿变换矩阵，要注意分清楚这个速度参数v是在哪个坐标系下表示的，才能决定我们是左乘/右乘这个矩阵。

对于例1 中，如果要求取相对于 T0 位姿自身坐标系 Ccam 的速度 v1 ，那么该如何求？
　　1. 计算 T1 在 T0 自身坐标系 Ccam 下的表示 T∗1 ；
　　2. 计算 T∗1 与单位矩阵 I4×4 （ T0 在自身坐标系下的位姿是单位阵 I4×4 ）的所对应的 v 。
　　利用视觉伺服工具箱简单验证一下：

>> T0 = transl(1,2,3)*trotx(1)*troty(1)*trotz(1);
>> T1 = T0*transl(0.01,0.02,0.03)*trotx(0.001)*troty(0.002)*trotz(0.003)

T1 =

    0.2889   -0.4547    0.8425    1.0191
    0.8372   -0.3069   -0.4527    1.9887
    0.4644    0.8361    0.2920    3.0301
         0         0         0    1.0000

>> T1_in_0=inv(T0)*T1;
>> I=eye(4);
>> v=tr2delta(I,T1_in_0);
>> v'

ans =
    0.0100    0.0200    0.0300    0.0010    0.0020    0.0030

三. Tips：

1. 对于位姿变换矩阵左乘和右乘，不同的人有不同的理解。比如有些情况下，目标从位姿运动到位姿时，可以把速度首先取负号，表达从状态运动到状态的控制率，这种情况下右乘的 还需要先对其求逆才能相乘。
2. 理解来源于研一机器人学课件中“右乘联体左乘基”这句话，所以说学过的基础知识一定要学扎实，不然之后就有可能被一些不起眼的细节知识绊住☺。

个人理解，如有错误请指出

　　　　　　　

（转载请注明作者和出处： http://blog.csdn.net/gh234505 未经允许请勿用于商业用途）