在机器人的控制和轨迹规划等领域,算法结果常常能够得到机器人执行器的速度,如移动机器人的角速度和线速度。在有些仿真情况下,或是其它需要实时更新机器人的运动状态时,我们常常需要根据这些计算出的速度参数(控制率)对机器人的位姿姿态进行更新。这就涉及到了角速度与旋转矩阵之间的对应转换关系。本文将简要给出一种它们之间的转换关系,并在后半部分着重介绍转换出的姿态矩阵在不同情况下的左乘、右乘的不同意义,这一问题也曾经让作者混淆了很久。
一. 控制率与位姿变换矩阵( 4×4 )的转换
这里主要以Peter corke的视觉伺服工具箱中给出的转换方法为例,事实上,更为详尽的转换方式可以去了解一下指数映射关系,可以参见白老师的文章《 lie group and computer vision : 李群、李代数在计算机视觉中的应用》。
我们知道,一个变量的导数等于它本身再乘以一个系数。对于三维空间中的旋转矩阵来说,有如下表达式:
其中, S(ω) 为关于角速度 ω 的反对称矩阵:
考虑在 R(t) 处进行微分:
经过整理后可得:
因此,矩阵 △R=ζtS(ω)+I3×3 即为角速度 ω 对应的旋转矩阵。
对于线速度 v 来说,在一定时间
当需要把位姿变换矩阵 △T 转换为速度参数 v 和
二. 那么问题来了:
1. 问题描述
上述控制率与位姿变换矩阵的转换关系在视觉伺服工具箱中对应于 delta2tr(
v
)和tr2delta(
这个示例我们记为例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
也是表示在这个坐标系下的。
所以,速度参数
位姿
T0
和位姿
T1
都是在世界坐标系下表示的,在
T0
位姿时,使其以速度
v
进行运动,其对应的姿态转换矩阵为
因此,通过连杆矩阵相乘,可以得到:
其次再说右乘。例2中算法所求出的摄像机运动速度
v
均是相对于当时摄像机自身坐标系来说的。因此,根据连杆矩阵变换,可以得到:
此外,右乘这种情况也可以这样理解:
摄像机的运行速度对应的位姿转换矩阵 △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
<script type="math/tex" id="MathJax-Element-1449">v</script>。
利用视觉伺服工具箱简单验证一下:
>> 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:
- 对于位姿变换矩阵左乘和右乘,不同的人有不同的理解。比如有些情况下,目标从位姿运动到位姿时,可以把速度首先取负号,表达从状态运动到状态的控制率,这种情况下右乘的 还需要先对其求逆才能相乘。
- 理解来源于研一机器人学课件中“右乘联体左乘基”这句话,所以说学过的基础知识一定要学扎实,不然之后就有可能被一些不起眼的细节知识绊住☺。
个人理解,如有错误请指出