我们首先研究关节坐标的微小变化如何影响末端执行器的位姿。使用齐次坐标变换表示位姿,我们可以通过一个一阶微分近似得到位姿相对于关节坐标的导数:
dqdT≈δqT(q+δq)−T(q)根据 T 的定义,可以得到
dqdT≈δq1⎝⎜⎜⎛R(q+δq)−R(q)000δxδyδz0⎠⎟⎟⎞(1)其中,(δx,δy,δz) 表示末端执行器的平移位移。下面我们举个例子
>> mdl_puma560
>> T0 = p560.fkine(qn);
>> dq = 1e-6;
>> Tp = p560.fkine(qn + [dq 0 0 0 0 0]);
>> dTdq1 = (Tp - T0) / dq
dTdq1 =
0.0000 -1.0000 -0.0000 0.1500
-0.0000 -0.0000 1.0000 0.5963
0 0 0 0
0 0 0 0
得到的 矩阵显然不是齐次变换矩阵,其左上角的 3×3 矩阵不是正交矩阵,右下角元素不为1。这说明了什么呢?
我们将dTdq1
中的第四列元素与方程 (1) 的矩阵中相应元素相等,得到
⎝⎛δxδyδz⎠⎞=⎝⎛0.15000.59630⎠⎞δq1
它代表了 q1 的变化引起的末端执行器位置的变化。
从图中我们能更容易地理解——腰关节(q1)的一个微小的旋转将使得末端执行器在世界坐标系水平的 xy 平面内移动,但却不产生垂直方向上的移动。
>> Tp = p560.fkine(qn + [0 dq 0 0 0 0]);
>> dTdq2 = (Tp - T0) / dq
dTdq2 =
1.0000 -0.0000 -0.0000 0.0144
0.0000 0 0.0000 0
0.0000 0.0000 1.0000 0.5963
0 0 0 0
同样可以写出
⎝⎛δxδyδz⎠⎞=⎝⎛0.014400.5963⎠⎞δq2
正如我们所预料的,肩关节 q2 的一个微小转动会引起末端执行器在垂直的 xz 平面内的运动,而没有 y 方向运动。等式两边同除以一个无限小的时间长度 δt,则得到关节角速度和末端执行器速度之间的一个关系:
⎝⎛x˙y˙z˙⎠⎞=⎝⎛0.014400.5963⎠⎞q˙2
现在考虑方程 (1) 的矩阵中左上方的 3×3 子矩阵。我们将其乘以 δq/δt ,从而得到 R 的一阶近似导数:
R˙≈
请发表评论