0. 軌跡
學機器人學前最想知道的，就是如何把機器人末端執行器平滑地從A點移動到B點，也許不太準確，應當說從位姿A移動到位姿B。
0.1 關節空間運動
考慮末端執行器在以下兩個笛卡爾位姿之間移動
下面兩種語法都可以表示（x,y,z,θ）的位姿
mdl_puma560 %載入提供的puma560機械臂模型
T1 = transl(0.4, 0.2, 0) * trotx(pi);
%T1 = SE3(0.4, 0.2, 0) * SE3.Rx(pi);
T2 = transl(0.4, -0.2, 0) * trotx(pi/2);
%T2 = SE3(0.4, -0.2 0) * SE3.Rx(pi/2);
那麼與之對應的關節座標就是
q1 = p560.ikine6s(T1);
q2 = p560.ikine6s(T2);
我們再給出時間向量
t = [0:0.05:2]';
通過對q1和q2兩個位姿之間進行平滑插值就可以得到一個關節空間軌跡，我們用如下函數
q = jtraj(q1, q2, qt);
%jtraj相當於具有tpoly插值的mtraj，
%但優化了多軸情況，且允許使用額外參數設置v0和vt
%以後就不用下面兩個插值函數
%q = mtraj(@tpoly, q1, q2, t);
%q = mtraj(@lspb, q1, q2, t);
%[q,qd,qdd] = jtraj(q1, q2, t);
%以下是SerialLink類提供的jtraj更簡便的用法
q = p560.jtraj(T1, T2, t);
最終得到的軌跡可以繪製出來觀察，包括關節角的變化都可以看到
p560.plot(q) %繪製關節角的變化如圖a
plot(t, q) %繪製關節角隨時間的變化，如圖a
T = p560.fkine(q); %得到三位笛卡爾軌跡
p = transl(T);%軌跡的位移部分
% plot(p(:,1),p(:,2)) %繪製xy平面內的軌跡，如圖c
% plot(t, tr2rpy(T)) %畫出另一種位姿表達，如圖d
從圖c可以看到，顯然我們做的這條軌跡不是一條直線。這個很好理解，我們只給機器人初始點和結束點兩個座標，那麼機器人在運動時，轉動其腰部關節，顯然會是一個弧線。那麼我們如何保證機器人末端在笛卡爾空間中也劃出一條直線軌跡呢？
其實很簡單，將原來的jtraj函數換成ctraj就可以，當然這裏的簡單隻得是用MATLAB仿真起來很簡單，並不是其他的。
Ts = ctraj(T1, T2, length(t));
%plot(t, transl(Ts) )
%plot(t, tr2rpy(Ts) )
qc = p560.ikine6s(Ts);
plot(t,q)
p560.plot(qc)
相比剛纔的圖我們可以看到，圖b中x現在是一條直線了，圖d中俯仰角爲0了，最直觀的是圖c，在xy平面內，軌跡是一條直線了！
仔細觀察圖a，與原來的有些差別，正是這些區別導致了直線笛卡爾運動！
1. 應用1——繪圖
這一部分我們講如何讓機器人畫出一個字母“E”，過程中實際上就是應用正逆運動學的知識！
首先我們給出字母E的一些途徑點，
path = [1 0 1
1 0 0
0 0 0
0 2 0
1 2 0
1 2 1
0 1 1
0 1 0
1 1 0
1 1 1];
分段路徑可繪製如下：
plot3(path(:,1), path(:,2), path(:,3), 'color', 'k', 'Linewidth', 2);
那麼如何轉化爲連續路徑呢？
p = mstraj(path, [10 10 10], [], [2 2 2], 0.02, 0.01);
%第二個參數規定了xyz三個方向上的最大速度
%第四個參數規定了初始座標、採樣間隔、加速時間
接下來我們給出字母的起點
Tp = transl(0.1 * p);
Tp = homtrans( transl(0.4, 0, 0), Tp);
%Tp是一系列的齊次變換矩陣，代表了路徑上每一個點的位姿
最後我們就可以通過逆運動學求出一系列的關節座標
p560.tool = trotx(pi); %考慮姿態，保證z軸向下
q = p560.ikine6s(Tp);
p560.plot(q) %繪製出動畫
如此，我們就完成了一個基本的機器人學小應用，原理就是我們學過的最基本的正逆運動學啦。
2. 應用2——步行機器人
walking部分其實仔細想想和寫字是差不的，
我們要建立好一條腿的模型，規定其走動的姿態變化，比如擡腿，前移，放下等等；完成一條腿的步行仿真後，再以此設置其他三條腿的變化，這裏我以Peter Corke給出的walking example爲例講解，實際上下載下來的原版有點小錯誤，我已經修改並驗證過好用了。
% set the dimensions of the two leg links
% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
function walking(varargin)
opt.niterations = 500;
opt.movie = [];
opt = tb_optparse(opt, varargin);
L1 = 0.1; L2 = 0.1; %這一句是單腿模型長度的設置，相當於設置大腿和小腿長度
fprintf('create leg model\n');
% create the leg links based on DH parameters 按照DH參數法設置腿足
% theta d a alpha
links(1) = Link([ 0 0 0 pi/2 ], 'standard');
links(2) = Link([ 0 0 L1 0 ], 'standard');
links(3) = Link([ 0 0 -L2 0 ], 'standard');
% now create a robot to represent a single leg 設置一條腿的模型
leg = SerialLink(links, 'name', 'leg', 'offset', [pi/2 0 -pi/2]);
% define the key parameters of the gait trajectory, walking in the
% x-direction 下面幾句是設置步行是擡起的高度限制之類的
xf = 5; xb = -xf; % forward and backward limits for foot on ground
y = 5; % distance of foot from body along y-axis
zu = 2; zd = 5; % height of foot when up and down
% define the rectangular path taken by the foot
segments = [xf y zd; xb y zd; xb y zu; xf y zu] * 0.01;
% build the gait. the points are:
% 1 start of walking stroke
% 2 end of walking stroke
% 3 end of foot raise
% 4 foot raised and forward
%
% The segments times are :
% 1->2 3s
% 2->3 0.5s
% 3->4 1s
% 4->1 0.5ss
%
% A total of 4s, of which 3s is walking and 1s is reset. At 0.01s sample
% time this is exactly 400 steps long.
%
% We use a finite acceleration time to get a nice smooth path, which means
% that the foot never actually goes through any of these points. This
% makes setting the initial robot pose and velocity difficult.
%
% Intead we create a longer cyclic path: 1, 2, 3, 4, 1, 2, 3, 4. The
% first 1->2 segment includes the initial ramp up, and the final 3->4
% has the slow down. However the middle 2->3->4->1 is smooth cyclic
% motion so we "cut it out" and use it.
fprintf('create trajectory\n');
segments = [segments; segments];
tseg = [3 0.25 0.5 0.25]';
tseg = [tseg; tseg];
x = mstraj(segments, [], tseg, segments(1,:), 0.01, 0.1); 設定好連續姿態
% pull out the cycle
fprintf('inverse kinematics (this will take a while)...');
xcycle = x(100:500,:);
qcycle = leg.ikine( transl(xcycle), 'mask', [1 1 1 0 0 0] );
% dimensions of the robot's rectangular body, width and height, the legs
% are at each corner.
W = 0.1; L = 0.2;
% a bit of optimization. We use a lot of plotting options to
% make the animation fast: turn off annotations like wrist axes, ground
% shadow, joint axes, no smooth shading. Rather than parse the switches
% each cycle we pre-digest them here into a plotopt struct.
% plotopt = leg.plot({'noraise', 'nobase', 'noshadow', ...
% 'nowrist', 'nojaxes'});
% plotopt = leg.plot({'noraise', 'norender', 'nobase', 'noshadow', ...
% 'nowrist', 'nojaxes', 'ortho'});
fprintf('\nanimate\n');
plotopt = {'noraise', 'nobase', 'noshadow', 'nowrist', 'nojaxes', 'delay', 0};
% create 4 leg robots. Each is a clone of the leg robot we built above,
% has a unique name, and a base transform to represent it's position
% on the body of the walking robot. 這裏是設置4條腿
legs(1) = SerialLink(leg, 'name', 'leg1');
legs(2) = SerialLink(leg, 'name', 'leg2', 'base', transl(-L, 0, 0));
legs(3) = SerialLink(leg, 'name', 'leg3', 'base', transl(-L, -W, 0)*trotz(pi));
legs(4) = SerialLink(leg, 'name', 'leg4', 'base', transl(0, -W, 0)*trotz(pi));
% create a fixed size axis for the robot, and set z positive downward
clf; axis([-0.3 0.1 -0.2 0.2 -0.15 0.05]); set(gca,'Zdir', 'reverse')
hold on
% draw the robot's body
patch([0 -L -L 0], [0 0 -W -W], [0 0 0 0], ...
'FaceColor', 'r', 'FaceAlpha', 0.5)
% instantiate each robot in the axes
for i=1:4
legs(i).plot(qcycle(1,:), plotopt{:});
end
hold off
% walk! 循環四條腿的動作，完整的步行
k = 1;
A = Animate(opt.movie);
for i=1:opt.niterations
legs(1).animate( gait(qcycle, k, 0, 0));
legs(2).animate( gait(qcycle, k, 100, 0));
legs(3).animate( gait(qcycle, k, 200, 1));
legs(4).animate( gait(qcycle, k, 300, 1));
drawnow
k = k+1;
A.add();
end
end
本篇的幾個例子通過MATLAB實現起來都比較容易，但是還是那句話，重要的不是會敲幾行代碼，而是理解背後的原理。