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

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

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

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

$\dot{R}(t)=S(\omega)\cdot R(t)$
其中， $S(\omega)$为关于角速度 $\omega$的反对称矩阵：
$$S(\omega)=\left[ \begin{matrix} 0&-\omega_z&\omega_y \\ \omega_z &0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{matrix} \right]$$
　　考虑在 $R(t)$处进行微分：
$\dot{R}\approx\frac{R(t+\triangle t)-R(t)}{\zeta_t}$
　　经过整理后可得：
$R(t+\triangle t)\approx\zeta_t\cdot S(\omega)\cdot R(t)+R(t)\approx(\zeta_t S(\omega)+I_{3\times3}))\cdot R(t)$
　　因此，矩阵 $\triangle R=\zeta_t S(\omega)+I_{3\times3}$即为角速度 $\omega$对应的旋转矩阵。
　　对于线速度 $v$来说，在一定时间$t$内，所导致的坐标系的变化只有平移没有旋转。因此，速度参数（控制率） $v$和$\omega$所对应的位姿变换矩阵 $\triangle T$为：
$\triangle T=\left[ \begin{matrix} \zeta_t\cdot S(\omega) & \zeta_t v\\ 0_{1\times3}& 0 \end{matrix} \right]+I_{4\times4}$
　　当需要把位姿变换矩阵 $\triangle T$转换为速度参数 $v$和$\omega$时，按照上面的步骤反着来就行了。

二. 那么问题来了：

1. 问题描述

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

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

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

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

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

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

$^{world}T_B=\triangle T$
　　因此，通过连杆矩阵相乘，可以得到：
$T_1=^{world}T_B\cdot ^BT_0=\triangle T\cdot T_0$

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

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

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

对于例1 中，如果要求取相对于$T_0$位姿自身坐标系$C_{cam}$的速度$v1$，那么该如何求？
　　1. 计算$T_1$在$T_0$自身坐标系$C_{cam}$下的表示$T_1^*$；
　　2. 计算$T_1^*$与单位矩阵$I_{4\times4}$（$T_0$在自身坐标系下的位姿是单位阵$I_{4\times4}$）的所对应的。
　　利用视觉伺服工具箱简单验证一下：

>> 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.youkuaiyun.com/gh234505 未经允许请勿用于商业用途）