Robotics, Vision and Control (Fundamental Algorithms in MATLAB)学习笔记 03_第三章 时间与运动

最近在学习机器人学相关知识，记录书中知识点，以便回顾复习。
请看这里：本文仅供个人学习交流使用~~

第三章时间与运动

本章对位姿为时间函数的物体进行研究。

轨迹

一条路径(path)是一个空间结构——空间中从初始位姿过渡到最终位姿的一个图形。轨迹(trajectory)是具有特定时间属性的一条路径。轨迹的一个重要特征是要平滑(smooth)，即位置和姿态随时间流畅地变化。

平滑一维轨迹

时间的标量函数的重要特征是其初始值和最终值是确定的，而且函数是光滑的（所谓光滑，在数学上是指其低阶时间导数是连续的）。这种函数的常见代表是时间多项式函数。
S ⟨ t ⟩ = A t 5 + B t 4 + C t 3 + D t 2 + E t + F S\langle t\rangle= A t^{5}+B t^{4}+C t^{3}+D t^{2}+E t+F S⟨t⟩=At5+Bt4+Ct3+Dt2+Et+F
其中，时间t ∈ [0, T]。其一阶导数和二阶导数也是光滑的多项式。
S ˙ ⟨ t ⟩ = 5 A t 4 + 4 B t 3 + 3 C t 2 + 2 D t + E S ¨ ⟨ t ⟩ = 20 A t 3 + 12 B t 2 + 6 C t + 2 D \begin{array}{l}{\dot{S}\langle t\rangle= 5 A t^{4}+4 B t^{3}+3 C t^{2}+2 D t+E} \\ {\ddot{S}\langle t\rangle= 20 A t^{3}+12 B t^{2}+6 C t+2 D}\end{array} S˙⟨t⟩=5At4+4Bt3+3Ct2+2Dt+ES¨⟨t⟩=20At3+12Bt2+6Ct+2D
轨迹上定义了位置、速度和加速度的边界条件，一般情况下速度和加速度的边界条件均为零。

在t = 0和t = T时分别将边界条件代入时间多项式函数，可以得到6个方程，其矩阵形式如下。
( s 0 s T s ˙ 0 s ˙ T s ¨ 0 s ¨ T ) = ( 0 0 0 0 0 1 T 5 T 4 T 3 T 2 T 1 0 0 0 0 1 0 5 T 4 4 T 3 3 T 2 2 T 1 0 0 0 0 2 0 0 20 T 3 12 T 2 6 T 2 0 0 ) ( A B C D E F ) \left(\begin{array}{l}{s_{0}} \\ {s_{T}} \\ {\dot{s}_{0}} \\ {\dot{s}_{T}} \\ {\ddot{s}_{0}} \\ {\ddot{s}_{T}}\end{array}\right)=\left(\begin{array}{cccccc}{0} & {0} & {0} & {0} & {0} & {1} \\ {T^{5}} & {T^{4}} & {T^{3}} & {T^{2}} & {T} & {1} \\ {0} & {0} & {0} & {0} & {1} & {0} \\ {5 T^{4}} & {4 T^{3}} & {3 T^{2}} & {2 T} & {1} & {0} \\ {0} & {0} & {0} & {2} & {0} & {0} \\ {20 T^{3}} & {12 T^{2}} & {6 T} & {2} & {0} & {0}\end{array}\right)\left(\begin{array}{l}{A} \\{B}\\ {C} \\ {D} \\ {E} \\ {F}\end{array}\right) ⎝⎜⎜⎜⎜⎜⎜⎛s0sTs˙0s˙Ts¨0s¨T⎠⎟⎟⎟⎟⎟⎟⎞=⎝⎜⎜⎜⎜⎜⎜⎛0T505T4020T30T404T3012T20T303T206T0T202T220T1100110000⎠⎟⎟⎟⎟⎟⎟⎞⎝⎜⎜⎜⎜⎜⎜⎛ABCDEF⎠⎟⎟⎟⎟⎟⎟⎞
工具箱中的函数 tpoly 可以返回一个五次时间多项式的轨迹、速度和加速度。其中初始速度和加速度的函数默认值都是零，但是也可以将初始速度和终点速度设置为非零值。由上式容易得到，机器人的初始位置为A，初始速度为C，初始加速度为E/2。

[s0,sd0,sdd0] = tpoly(0,1,50);			
%返回初始位置为0，终点位置为1，且分50个时间步平滑变化的轨迹、速度和加速度的列向量
[s1,sd1,sdd1] = tpoly(0,1,50,0.1,0);
%返回初始位置为0，终点位置为1，初始速度为0.1，终点速度为0，且分50个时间步平滑变化的轨迹、速度和加速度的列向量
subplot(3,1,1);hold on;grid on;plot(s0,'r');plot(s1,'b');
xlabel('time(s)');ylabel('s');
subplot(3,1,2);hold on;grid on;plot(sd0,'r');plot(sd1,'b');
xlabel('time(s)');ylabel('sd');
subplot(3,1,3);hold on;grid on;plot(sdd0,'r');plot(sdd1,'b');
xlabel('time(s)');ylabel('sdd');

机器人具有一个额定的最大速度，为了使运动时间最短，应使其运行在最大速度上的时间尽可能长。一种公认的较理想的选择是采用混合曲线轨迹，即由中间的恒速段平直线加上两侧的加速段和减速段多项式曲线构成的轨迹。工具箱中的函数 lspb 返回其混合轨迹、速度和加速度。这种类型的轨迹由于其速度-时间曲线的形状也被称作梯形轨迹，普遍应用于工业驱动马达中。
S ⟨ t ⟩ = { f 1 ⟨ t ⟩ , t ∈ [ 0 , t 1 ) v max ⁡ t , t ∈ [ t 1 , t 2 ) f 2 ⟨ t ⟩ , t ∈ [ t 2 , T ] {S}\langle t\rangle=\left\{\begin{array}{ll}{f_{1}\langle t\rangle} & {, t \in\left[0, t_{1}\right)} \\ {v_{\max } t} & {, t \in\left[t_{1}, t_{2}\right)} \\ {f_{2}\langle t\rangle} & {, t \in\left[t_{2}, T\right]}\end{array}\right. S⟨t⟩=⎩⎨⎧f1⟨t⟩vmaxtf2⟨t⟩,t∈[0,t1),t∈[t1,t2),t∈[t2,T]

f 1 ⟨ t ⟩ = A 1 t 5 + B 1 t 4 + C 1 t 3 + D 1 t 2 + E 1 t + F 1 f_{1}\langle t\rangle= A_{1} t^{5}+B_{1} t^{4}+C_{1} t^{3}+D_{1} t^{2}+E_{1} t+F_{1} f1⟨t⟩=A1t5+B1t4+C1t3+D1t2+E1t+F1

f 2 ⟨ t ⟩ = A 2 t 5 + B 2 t 4 + C 2 t 3 + D 2 t 2 + E 2 t + F 2 f_{2}\langle t\rangle= A_{2} t^{5}+B_{2} t^{4}+C_{2} t^{3}+D_{2} t^{2}+E_{2} t+F_{2} f2⟨t⟩=A2t5+B2t4+C2t3+D2t2+E2t+F2

[s0,sd0,sdd0] = lspb(0,1,50);
%返回初始位置为0，终点位置为1，恒定速度为默认值，且分50个时间步的混合曲线轨迹、速度和加速度的列向量
[s1,sd1,sdd1] = lspb(0,1,50,0.025);
%返回初始位置为0，终点位置为1，恒定速度为0.025，且分50个时间步的混合曲线轨迹、速度和加速度的列向量
[s2,sd2,sdd2] = lspb(0,1,50,0.035);
%返回初始位置为0，终点位置为1，恒定速度为0.035，且分50个时间步的混合曲线轨迹、速度和加速度的列向量
subplot(3,1,1);hold on;grid on;plot(s0,'r');plot(s1,'b');plot(s2,'g');
xlabel('time(s)');ylabel('s');
legend('nominal','0.0025','0.0035','location','SouthEast');
subplot(3,1,2);hold on;grid on;plot(sd0,'r');plot(sd1,'b');plot(sd2,'g');
xlabel('time(s)');ylabel('sd');
subplot(3,1,3);hold on;grid on;plot(sdd0,'r');plot(sdd1,'b');plot(sdd2,'g');
xlabel('time(s)');ylabel('sdd');

注意：速度值是不能任意选择的，过高或过低的最大速度都将产生一个不可行的轨迹，且函数返回错误。

多维的情况

实际情况中，机器人一般拥有一个以上的自由度，通过向量 x 表示。
x ∈ R M \boldsymbol{x} \in \mathbb{R}^{M} x∈RM
其中，M代表自由度的数目。例如，轮式移动机器人由其位置 (x, y) 或位姿 (x, y, θ) 描述。工具箱中通过函数 mtraj 将平滑的标量轨迹扩展成向量

clear
clc
s = mtraj(@tpoly,[3 1],[-8 5],50);
%返回初始位置为[3,1]，终点位置为[-8,5],且分50个时间步平滑变化轨迹的x和y的列向量
subplot(2,1,1);plot(s);
grid on
xlabel('time(s)');ylabel('x & y')
legend('x','y');
s = mtraj(@lspb,[5 2],[-2 6],50);
%返回初始位置为[5,2],终点位置为[-2,6]，且分50个时间步的混合曲线轨迹的x和y的列向量
subplot(2,1,2);plot(s);
grid on
xlabel('time(s)');ylabel('x & y');
legend('x','y');

在多段轨迹的情况（如下图）下，机器人从静止状态 x1 开始运动，到 x4 结束停止，但在通过（或接近）中间点时不能停顿。生成此运动轨迹的最大问题是过约束。为了实现速度连续，只能舍弃让轨迹能达到每个中间点。轨迹的第一段从速度为0的初始位姿 x1 开始加速，然后汇接入指向第二个位姿 x2 的直线段。汇接时间被设为常数 tacc ，在到达 x2 之前的 tacc/2 时间，直线轨迹汇接上一段多项式曲线，持续 tacc 时间后再次汇接从 x2 到 x3 的直线段，然后重复这一过程。

工具箱中通过函数 mstraj 可以基于中间点矩阵生成一个多段多轴轨迹。

clear
clc
via = [4,1; 4,4; 5,2; 2,5];				%中间点矩阵
s = mstraj(via, [2,1], [], [4,1], 0.1,1);
%返回轨迹s(包含x和y的轨迹列向量)，中间点矩阵为via，每轴最大速度向量为[2,1]，起点向量为[4,1]，采样时间间隔为0.1，加速时间为1
xlabel('time(s)');ylabel('s')
plot(s,'o')
legend('x','y','location','SouthEast')

figure
axis([0 5 0 5])
for i = 1:130
	plot_point(s(i,:)','*')
end

三维空间姿态插值

位姿由正交旋转矩阵表示时，插值后得到的通常不再是有效的正交矩阵，因为正交矩阵必须满足列向量范数为1以及列向量之间正交的条件。
一个可行且常用的选择是三角度表示法。
σ ( Γ 0 , Γ 1 , s ) = ( 1 − s ) Γ 0 + s Γ 1 \sigma\left(\boldsymbol{\Gamma}_{0}, \boldsymbol{\Gamma}_{1}, s\right)=(1-s) \boldsymbol{\Gamma}_{0}+s \boldsymbol{\Gamma}_{1} σ(Γ0,Γ1,s)=(1−s)Γ0+sΓ1

R0 = rotz(-1)*roty(-1);
R1 = rotz(1)*roty(1);

rpy0 = tr2rpy(R0);
rpy1 = tr2rpy(R1);

rpy = mtraj(@tpoly,rpy0,rpy1,50);
tranimate(rpy2tr(rpy))

注意在三角度系统中位姿接近奇异点时，就会出现问题。用单位四元数的插值法则会避免这一问题。

R0 = rotz(-1)*roty(-1);
R1 = rotz(1)*roty(1);

q0 = UnitQuaternion(R0);
q1 = UnitQuaternion(R1);

q = interp(q0,q1,[0:49]'/49);
%返回初始位姿为q0,终点位姿为q1,且时间步为50的单位四元数q的插值
about(q)
q.animate(q)

笛卡尔运动 Cartesian Motion

同时设计位置与姿态变化的运动在机器人学中称作笛卡尔运动。

clear
clc
T0 = transl(0.4,0.2,0)*trotx(pi);
T1 = transl(-0.4,-0.2,0.3)*troty(pi/2)*trotz(-pi/2);
Ts = trinterp(T0,T1,lspb(0,1,50));
%返回初始位姿为T0,终点位姿为T1,且时间步为50，运动为混合曲线轨迹的齐次变换矩阵Ts的插值
about(Ts)
tranimate(Ts)

figure
P = transl(Ts);
subplot(2,1,1);plot(P);
xlabel('time(s)');ylabel('x & y & z')
legend('x','y','z','location','SouthWest');

rpy = tr2rpy(Ts);
subplot(2,1,2);plot(rpy)
xlabel('time(s)');ylabel('angle');
legend('phi(x)','theta(y)','psi(z)')

时变坐标系

旋转坐标系
物体在三维空间中旋转时有一个角速度向量：
ω = ( ω x , ω y , ω z ) \omega=\left(\omega_{x}, \omega_{y}, \omega_{z}\right) ω=(ωx,ωy,ωz)
该向量的方向定义了瞬时转动轴，即在某个特定时间点坐标系旋转所绕的轴。通常情况下，这个轴是随时间改变的。
力学中的时变旋转矩阵微分表达式：
R ˙ ⟨ t ⟩ = S ( ω ) R ⟨ t ⟩ \dot{R}\langle t\rangle= S(\omega) R\langle t\rangle R˙⟨t⟩=S(ω)R⟨t⟩
其中 S(ω) 是一个斜对称矩阵。
S ( ω ) = ( 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 ) S(\omega)=\left(\begin{array}{ccc}{0} & {-\omega_{z}} & {\omega_{y}} \\ {\omega_{z}} & {0} & {-\omega_{x}} \\ {-\omega_{y}} & {\omega_{x}} & {0}\end{array}\right) S(ω)=⎝⎛0ωz−ωy−ωz0ωxωy−ωx0⎠⎞
对于时变旋转矩阵微分表达式，用微分近似得：
R ˙ ≈ R ⟨ t + δ t ⟩ − R ⟨ δ t ⟩ δ t \dot{R} \approx \frac{R\left\langle t+\delta_{t}\right\rangle- R\langle\delta t\rangle}{\delta_{t}} R˙≈δtR⟨t+δt⟩−R⟨δt⟩
整理上式
R ⟨ t + δ t ⟩ ≈ δ t R ˙ + R ⟨ t ⟩ \boldsymbol{R}\left\langle t+\delta_{t}\right\rangle \approx \delta_{t} \dot{\boldsymbol{R}}+\boldsymbol{R}\langle t\rangle R⟨t+δt⟩≈δtR˙+R⟨t⟩
代入时变旋转矩阵微分表达式
R ⟨ t + δ t ⟩ ≈ δ t S ( ω ) R ⟨ t ⟩ + R ⟨ t ⟩ ≈ ( δ t S ( ω ) + I 3 × 3 ) R ⟨ t ⟩ \boldsymbol{R}\left\langle t+\delta_{t}\right\rangle \approx \delta_{t} \boldsymbol{S}(\boldsymbol{\omega}) \boldsymbol{R}\langle t\rangle+\boldsymbol{R}\langle t\rangle \approx\left(\delta_{t} \boldsymbol{S}(\boldsymbol{\omega})+\boldsymbol{I}_{3 \times 3}\right) \boldsymbol{R}\langle t\rangle R⟨t+δt⟩≈δtS(ω)R⟨t⟩+R⟨t⟩≈(δtS(ω)+I3×3)R⟨t⟩
增量运动
考虑一个坐标系经微小旋转从R0变到R1，代入上节的表达式得到：
R 1 = ( δ t S ( ω ) + I 3 × 3 ) R 0 R_{1}=\left(\delta_{t} S(\omega)+I_{3 \times 3}\right) R_{0} R1=(δtS(ω)+I3×3)R0
整理上式
δ t S ( ω ) = R 1 R 0 T − I 3 × 3 \delta_{t} S(\omega)=R_{1} R_{0}^{T}-I_{3 \times 3} δtS(ω)=R1R0T−I3×3
两侧使用逆解运算符 vex，得到：
δ Θ = vex ⁡ ( R 1 R 0 T − I 3 × 3 ) \delta_{\Theta}=\operatorname{vex}\left(R_{1} R_{0}^{T}-I_{3 \times 3}\right) δΘ=vex(R1R0T−I3×3)
δ Θ = δ t ω \delta_{\Theta}=\delta_{t} \omega δΘ=δtω
在第二章中一再强调旋转变换的不可交换性，但对于无穷小角度变化的乘法却是允许的。