时间与运动

3.1 轨迹

轨迹是具有特定时间属性的一条路径。

3.1.1 平滑一维轨迹

一个真正的机器人关节都有一个额定的最大速度，而且为了使关节运动时间最短，应使其运行在最大速度上的时间尽可能长。因此我们希望速度曲线的顶部是一条直线。

混合曲线轨迹：延长最大速度运行时间，使得速度曲线顶部为一平直线，两侧为加减速段。

3.1.2 多维的情况

大多数实用的机器人都有一个以上的运动轴或自由度。我们将其用向量形式表示为X∈RM,M代表自由度的数目。轮式移动机器人由它的位置(x, y)或位姿(x,y,θ)来描述，而关节臂式机器人的末端工具则有位置(x,y,z)，姿态(θr,θp ,θy)，或位姿(x, y, z, θr,θp ,θy)。因此我们需要从初始位姿向量到最终位姿向量的多维平滑运动。

3.1.3 多段轨迹

机器人应用中经常需要机器人平滑地沿一条路径运动，并不停顿地经过一个或多个中间节点。为了实现速度连续，舍弃中间点位置，采用直线和曲线拟合：

3.1.4 三维空间姿态插值

线性插值法：

3.2 时变坐标系

3.2.1 旋转坐标系

物体在三维空间中旋转时有一个角速度向量ω=(ωx,ωy,ωz)\omega=(\omega_x, \omega_y, \omega_z)ω=(ωx,ωy,ωz)。这个向量的方向定义了瞬时转动轴，即在某个特定时间点坐标系旋转所绕的轴。通常情况下，这个轴是随时间变化而改变的。向量长度代表绕该轴的转速，这有点类似于旋转的角轴表示法。力学中有一个众所周知的时变旋转矩阵微分表达式(理论力学)：

 R˙(t)=S(ω)R(t)\dot{R}(t)=S(\omega)R(t)R˙(t)=S(ω)R(t)式中：R(t)R(t)R(t) 为空间旋转矩阵；S(w)S(w)S(w) 为角速度矩阵，其具体形式如下：

R(t)=(nxoxaxnyoyaynzozaz)S(ω)=(0−ωzωyωz0−ωx−ωyωx0)\begin{matrix}

 R\text{(}t\text{)}=\left( \begin{matrix}

 n_x&o_x&a_x\\

 n_y&o_y&a_y\\

 n_z&o_z&a_z\\

 \end{matrix} \right)&S\left( \omega \right) =\left( \begin{matrix}

 0&-\omega _z&\omega _y\\

 \omega _z&0&-\omega _x\\

 -\omega _y&\omega _x&0\\

 \end{matrix} \right)\\

 \end{matrix}R(t)=⎝⎛nxnynzoxoyozaxayaz⎠⎞S(ω)=⎝⎛0ωz−ωy−ωz0ωxωy−ωx0⎠⎞利用机器人工具箱，可以得到

 >> S = skew([1 2 3])

 S =

 0 -3 2

 3 0 -1

 -2 1 0

 工具箱函数vex具有逆解的功能：将斜对称矩阵转换成一个向量：

 >> vex(S)

 ans =123

那么方程中的 R˙\dot RR˙ 是什么意思呢？我们用微分近似法可得 

R˙≈R(t+δt)−R(t)δt\dot{R}\approx\dfrac{R(t+\delta_t)-R(t)}{\delta_t}R˙≈δtR(t+δt)−R(t)即

 R(t+δt)≈δtR˙+R(t)R(t+\delta_t)\approx\delta_t\dot{R}+R(t)R(t+δt)≈δtR˙+R(t)将其代入方程中，得到

R(t+δt)≈δtS(ω)R(t)+R(t)=(δtS(ω)+I3×3)R(t)R(t+\delta_t)\approx\delta_tS(\omega)R(t)+R(t)=(\delta_tS(\omega)+I_{3\times 3})R(t)R(t+δt)≈δtS(ω)R(t)+R(t)=(δtS(ω)+I3×3)R(t)它描述了标准正交旋转矩阵是如何作为一个角速度的函数变化的。

3.2.2 增量运动

现在考虑一个坐标系经微小旋转从 R0R_0R0 变到 R1R_1R1。这时可以将上个方程写为 

R1=(δtS(ω)+I3×3)R0R_1=(\delta_tS(\omega)+I_{3\times3})R_0R1=(δtS(ω)+I3×3)R0将上式重新整理后，得

δtS(ω)=R1R0T−I3×3\delta_tS(\omega)=R_1R_0^T-I_{3\times3}δtS(ω)=R1R0T−I3×3再对其两侧均使用运算符vex，即求 S(ω)S(\omega)S(ω)的逆，得到

δθ=vex(R1R0T−I3×3)\delta_\theta=\text{vex}(R_1R_0^T-I_{3\times3})δθ=vex(R1R0T−I3×3)其中，δθ=δtω\delta_\theta = \delta_t\omegaδθ=δtω 是一个三维向量，单位是角度，它表示一个绕世界坐标系的 xxx、yyy 和 zzz 三轴的无穷小转动。

我们之前强调过旋转变换的不可交换性，但对于无穷小角度变化的乘法来说却是可交换的。可以用具体数字的例子说明如下：

>> Rdelta = rotx(0.001) * roty(0.002) * rotz(0.003)

Rdelta =

1.0000 -0.0030 0.0020

0.0030 1.0000 -0.0010

-0.0020 0.0010 1.0000

以上结果是与下面的计算相同的(取4位有效数字)：

>> Rdelta = roty(0.002) * rotx(0.001) *rotz(0.003)

Rdelta =

1.0000 -0.0030 0.0020

0.0030 1.0000 -0.0010

-0.0020 0.0010 1.0000

使用前面推导的方程，可以重新获得上面矩阵对应的微小旋转角 δθ\delta_\thetaδθ：

>> vex(Rdelta - eye(3, 3))

ans =

0.0010

0.0020

0.0030

现在给出两个差异极小的位姿 ξ0\xi_0ξ0 和 ξ1\xi_1ξ1，可以用一个六维向量来表示它们间的差异：

δ=Δ(ξ0,ξ1)=(δd,δθ)\delta=\Delta(\xi_0, \xi_1)=(\delta_d, \delta_\theta)δ=Δ(ξ0,ξ1)=(δd,δθ)它由位移增量和旋转增量两部分组成。δ∈R6\delta\in\mathbb{R}^6δ∈R6 的值实际上是用空间速度乘以 δt\delta_tδt 得到的。如果位姿都用齐次变换矩阵的形式表示，那么位姿差异是

δ=Δ(T0,T1)=(t1−t0vex(R1R0T−I3×3))\delta=\Delta(T_0, T_1)=\left( \begin{array}{c}

t_1-t_0\\[1em]

\text{vex}\left( R_1R_0^T-I_{3\times 3} \right)\\

\end{array} \right)δ=Δ(T0,T1)=⎝⎛t1−t0vex(R1R0T−I3×3)⎠⎞其中，T0=(R0,t0)T_0=(R_0, t_0)T0=(R0,t0)，T1=(R1,t1)T_1=(R_1, t_1)T1=(R1,t1)。在工具箱中它可以用函数tr2delta求得。

方程的逆运算为

ξ=Δ−1(δ)\xi=\Delta^{-1}(\delta)ξ=Δ−1(δ)而齐次变换表示法的逆运算是

T=(Sδθδd03×10)+I4×4T=\left( \begin{matrix}

S\delta _{\theta}&\delta _d\\

0_{3\times 1}&0\\

\end{matrix} \right) +I_{4\times4}T=(Sδθ03×1δd0)+I4×4在工具箱里它用函数delta2tr求解。

>> 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

函数 Δ(⋅)\Delta(\cdot)Δ(⋅) 的计算是由工具箱函数tr2delta完成的： 

>> d = tr2delta(T0, T1); 

>> d' 

ans =

0.0191 -0.0113 0.0301 0.0019 -0.0011 0.0030

该位姿变化(位移)由相对于世界坐标系的平移增量和旋转增量组成。给定这个位移量以及初始位姿，可求得最终位姿为 

>> delta2tr(d) * T0

ans =

0.2889 -0.4547 0.8425 1.0096

0.8372 -0.3069 -0.4527 1.9859 

0.4644 0.8361 0.2920 3.0351 

0 0 0 1.0000

它非常接近于上面T1给出的真实值，误差是由于位移量不是无穷小这一事实所致。