機械臂仿真:使用Matlab機器人工具箱與標準DH法欧啤,實現(xiàn)6自由度機械臂的仿真運動

目錄
1 預備知識與代碼
1.1 標準DH建模法
1.2 正向運動學與逆向運動學
1.3 使用matlab實現(xiàn)軌跡規(guī)劃
2 控制六自由度機械臂寫名字
3 matlab機器人工具箱常用函數(shù)
# 參考

0 引言

在了解了如何描述末端執(zhí)行器的位姿之后极颓,接下來我們就可以實現(xiàn)對機械臂的數(shù)學建模报腔。建模的工具有很多種谎痢,本文介紹一種基于標準DH參數(shù)法與Matlab的Robotics Toolbox - Peter Corke庫來實現(xiàn)此過程终娃,并嘗試實現(xiàn)6自由度機器人的仿真運動棋返。

強烈推薦b站教學視頻MATLAB機器人工具箱10.4 機械臂仿真教學(未完結(jié))延都,并且強烈建議大家先提前了解機器人位姿相關知識。本文多數(shù)內(nèi)容基于自己的理解與參考中的鏈接睛竣,不妥之處多多指正晰房。


1 預備知識

不嚴格的說,機械臂大體上可以分成三種結(jié)構(gòu):連桿(link)酵颁,關節(jié)(又叫軸嫉你,axis)與基座(base)。直覺上說躏惋,我們會把坐標系固定在關節(jié)上幽污,而非連桿上;基座指的是機器人的底座簿姨,是不動的距误,它又被稱為連桿0。

每個坐標系(除了基座上的)我們都可以視為一個“自由度”扁位,這意味著這個關節(jié)是可以運動的准潭。一般來說一個關節(jié)只有一個自由度,要么是旋轉(zhuǎn)域仇,要么是移動刑然,也可能有兩個自由度。對于不同的關節(jié)暇务,建模方法是有所差異泼掠,但大致相同。


1.1 標準DH建模法

1.1.1 基本概念

建模的主要目的之一是為了保存兩個關節(jié)與他們之間連桿的信息(例如連桿長度垦细、旋轉(zhuǎn)角度等)择镇,有了這些描述,我們就可以從基座開始括改,一步步把機械臂重新畫出來腻豌。


圖1 標準DH建模法任意兩關節(jié)之間的坐標系構(gòu)建

標準DH法是一種常用的定義連桿坐標系的規(guī)則,由J Denavit,RS Hartenberg于1956年提出吝梅。其具體規(guī)則為圖2所示:


圖2 標準DH建模法步驟圖

PS:實際建模過程中虱疏,坐標系O_{i-1}與坐標系O_i' 重合,這意味著軸x_i與軸z_{i-1}總是垂直相交憔涉。這樣订框,我們就能用更少的參數(shù)來表達這些關節(jié)和連桿(我所指的信息是指此處的a析苫、\alpha兜叨、\theta);并且實際建模的過程衩侥,我們大多數(shù)情況只關心z軸與x軸上旋轉(zhuǎn)国旷、位移的信息。

與其叫做標準DH建模法茫死,筆者認為叫做提線木偶可能更好理解一些跪但。它的核心就是,需要把坐標系建立在連桿遠離基座方向的關節(jié)的那一端(例如圖1中的坐標系O_i和連桿i峦萎,以此“牽引著”這個連桿的旋轉(zhuǎn)或者移動屡久,就像使用提線木偶一樣。更形象一點:使用左手抓住右手指尖爱榔,然后拉著指尖來控制右手臂的運動被环,你的手臂就是連桿,你的右手指尖的位置就是關節(jié)坐標系的位置详幽,這就是標準DH建模法的“感覺”筛欢。

補充:改進DH建模法

還有一種方法叫做改進DH建模法,它的核心就和標準DH建模法相反唇聘,它把坐標系建在連桿的靠近基座的那一個關節(jié)上版姑,它的感覺就比如:用右手的手腕,來主動控制右手掌的旋轉(zhuǎn)迟郎。不過剥险,現(xiàn)實建模過程區(qū)分它們最嚴謹?shù)姆椒ǎ€是它們獲取數(shù)據(jù)的順序宪肖。


1.1.2 DH參數(shù)表格

我們建模過程獲得的信息是怎么樣的表制?如果我們有n個連桿,我們獲得的信息的排布可能是一個n\times 4(如果把基座看做連桿匈庭,例如此處的連桿0)大小的表格,就比如圖3:

圖3 某5自由度機器人的標準DH表格夭拌,參考鏈接見下文
這四個參數(shù)分別是:

1.:\theta表明從上一個關節(jié)所對應的坐標系的z軸,該旋轉(zhuǎn)多少角度(逆時針為正)鸽扁;

2.:d表明沿著上一個關節(jié)所對應的坐標系的z軸,該移動多少距離躲雅;

3.:a表明沿著上一個關節(jié)所對應的坐標系的x軸,該移動多少距離(這個時候就已經(jīng)到達)骡和;

4.:\alpha表明沿著上一個關節(jié)所對應的坐標系的x軸,該旋轉(zhuǎn)多少角度(它可能用來調(diào)整這個自由度的旋轉(zhuǎn)方向)钮科;

這個DH參數(shù)表格中包含了所有我們需要的描述機器人的完整信息绵脯,本質(zhì)上就是機器人的各個坐標系之間的“變換矩陣”休里。這個信息可以幫助我們從基座的坐標系一步步的從參考坐標系一直推導到末端執(zhí)行器的坐標系上妙黍,從而獲得末端執(zhí)行器的位姿废境,這也是正向運動學的核心。相關的推導會放到后續(xù)的章節(jié)中(劇透:就是四個齊次變換矩陣的矩陣乘法巴元,獲得相鄰兩坐標系的變換矩陣)逮刨。關于機器人位姿的理解修己,可以參考這篇文章:機械臂位姿描述睬愤。

1.1.3 使用matlab機器人工具箱通過標準DH參數(shù)表格建立機器人

本文使用機器人工具箱Robotics Toolbox - Peter Corke以及標準DH建模法來建立機器人尤辱。核心是先確定有多少個自由度光督,再確定機械臂的初始位姿,最后使用幾個函數(shù)生成我們的機器人筐摘,獲得一個對象咖熟。
對于每一個自由度球恤,使用link函數(shù)來獲取一個連桿對象,這個對象的屬性如下:

Link 的類屬性(讀/寫): 
theta:D-H參數(shù) 
d:D-H參數(shù) 
a:D-H參數(shù) 
alpha:D-H參數(shù) 
sigma: 默認0躬存,旋轉(zhuǎn)關節(jié)岭洲;1盾剩,移動關節(jié) 
mdh: 默認0告私,標準D-H驻粟;1蜀撑,改進D-H 
offset:關節(jié)變量偏移量 
qlim:關節(jié)變量范圍
m: 質(zhì)量 
r: 質(zhì)心 
I: 慣性張量 
B: 粘性摩擦 
Tc: 靜摩擦 
G: 減速比 
Jm: 轉(zhuǎn)子慣量

例如酷麦,我們拿圖3舉例沃饶,我們建立第1根連桿及其坐標系绍坝,代碼如下:

L(1) = Link('revolute', 'd', 0.216, 'a', 0, 'alpha', pi/2, 'qlim', [-150 ,150]/180*pi);

第一個參數(shù)表明它是一個旋轉(zhuǎn)自由度轩褐,后面的d把介、a拗踢、\alpha為標準DH參數(shù)表中的內(nèi)容巢墅。為什么沒有在這里定義\theta這個參數(shù)呢君纫?在matlab中蓄髓,如果是旋轉(zhuǎn)關節(jié)会喝,那么需要給offset這個屬性賦值\theta肢执。并且如果選擇改進DH建模法蔚万,需要在最后加上一個參數(shù)'modified'反璃。
我們一步步把所有圖3所有連桿及其對應的關節(jié)定義出來(可以有多種不同的定義方法):

L(1) = Link('revolute', 'd', 0.216, 'a', 0, 'alpha', pi/2, 'qlim', [-150 ,150]/180*pi);
L(2) = Link('revolute', 'd', 0, 'a', 0.5, 'alpha', 0, 'offset', pi/2, 'qlim', [-100 ,90]/180*pi);
L(3) = Link('revolute', 'd', 0, 'a', sqrt(0.145^2+0.42746^2), 'alpha', 0, 'offset', -atan(427.46/145), 'qlim', [-90 ,90]/180*pi);
L(4) = Link('revolute', 'd', 0, 'a', 0, 'alpha', pi/2, 'offset', atan(427.46/145), 'qlim', [-100 ,100]/180*pi);
L(5) = Link('revolute', 'd', 0.258, 'a', 0, 'alpha', 0, 'qlim', [-180 ,180]/180*pi);

然后淮蜈,我們可以使用SerialLink類來獲得一個機械臂對象:

Five_dof = SerialLink(L, 'name', '5-dof');

其次梧田,我們需要確定0號連桿及其關節(jié)坐標系的位置。連桿0也就是基座讳癌,對應于圖3第一行信息(Link:0)晌坤。這個連桿0是固定的骤菠,所以在matlab中單獨定義成機械臂對象的一個屬性:

% transl函數(shù)獲得一個4*4變換矩陣
% transl(a, b, c) = [1  0  0  a
%                    0  1  0  b
%                    0  0  1  c
%                    0  0  0  1]
Five_dof.base = transl(0, 0, 0.28);

這個變換矩陣是針對于世界坐標系的商乎。在matlab中鹉戚,默認世界坐標系在(0, 0, 0)處崩瓤,因此可以通過base這個屬性,從世界坐標系處獲得0號坐標系以及連桿0(也就是基座)蔗牡。
想要查看這個機械臂辩越,使用teach屬性:

Five_dof.teach;
圖4 名字叫做 5-dof 機器人,參考鏈接見下文

1.2 正向運動學與逆向運動學

1.2.1 正向運動學

簡單且不嚴謹?shù)卣f督惰,正向運動學是指通過機械臂的關節(jié)運動的參數(shù)推導出末端執(zhí)行器的位置赏胚。推導需要幾個基本條件:第一觉阅,我們需要完整的DH參數(shù)表格典勇;其次割笙,我們需要知道關節(jié)究竟發(fā)生了怎么樣的運動咳蔚。我們最終得到的是末端執(zhí)行器的坐標系與參考坐標系(基坐標系)的變換矩陣谈火。

標準DH參數(shù)表獲得的變換矩陣

拿圖3舉例糯耍。Link0與Link1對應的兩個坐標系之間的變換矩陣應該等于:
^0T_1 =R(z, \theta) \times D(z, d) \times D(x, a) \times R(x, \alpha)\tag{1}
其中的四個矩陣的形狀都應該為4\times 4的大小革为。矩陣中元素的具體取值震檩,請參考最后一節(jié)的第4個鏈接抛虏。不過迂猴,^0T_1僅僅只是相對關系沸毁。我們要使用這個變化矩陣必須提前知道0號坐標系的位置息尺。
假定我們擁有一個六自由度機械臂掷倔,并且每個關節(jié)只有一個繞z軸旋轉(zhuǎn)自由度勒葱。最終死遭,我們通過一系列矩陣乘法呀潭,就可以獲得從0號坐標系到末端執(zhí)行器的變換矩陣:
^0T_6=^0T_1(\theta _1)\times ^1T_2(\theta _2)\times ^2T_3(\theta _3)\times ^3T_4(\theta _4)\times ^4T_5(\theta _5) \times ^5T_6(\theta _6) \tag{2}
\theta_i的含義就是指的是每個關節(jié)DH參數(shù)的第一個信息值钠署,也就是繞z軸旋轉(zhuǎn)的角度。
有了標準DH參數(shù)表狸棍,我們可以獲得末端執(zhí)行器的位置草戈,也可以獲得每個關節(jié)的位置唐片。

1.2.2 逆向運動學

簡單來說牵触,逆向運動學能獲得末端執(zhí)行器到達某個位姿所需的各個關節(jié)的角度。大多數(shù)情況下见擦,末端執(zhí)行器到達某個位姿的方式可以有很多種鲤屡,也可能根本沒有解。獲得解的方式便是反向求解公式2中的\theta_i參數(shù)堰汉。
具體求解方法涉及較復雜的矩陣運算和線性代數(shù)知識翘鸭,可以參考網(wǎng)上資料就乓,或者此鏈接:建模+正逆運動學+雅克比矩陣

1.2.3 matlab代碼實現(xiàn)正逆運動學

正向運動學

這里使用圖3所定義的機械臂來舉例噩翠。我們只需要給出每個關節(jié)的角度即可伤锚。返回一個變換矩陣(也就是^0T_6见芹,對于圖3而言是^0T_5玄呛,因為它只有5個自由度)。

q = [pi/2 pi/2 0 0 0];
T = Five_dof.fkine(q);

在學習的時候產(chǎn)生了一些疑惑惕它。例如我這里直接給出的角度q向量和我定義機械臂(一堆Link對象)的時候給出的\theta之間是什么關系呢淹魄?如果是旋轉(zhuǎn)關節(jié),q是在原來標準DH參數(shù)表格上,直接對\theta參數(shù)加上或減去某個角度易稠;如果是移動關節(jié),類比下來應該是對某個移動參數(shù)作出改變测萎,但筆者還沒有驗證這一點绳泉。

逆向運動學

我們給出一個變換矩陣^0T_6(代表末端執(zhí)行器的位姿)零酪,傳入機器人對象中的ikunc函數(shù)就可以快速求解各個關節(jié)角度:

T = transl(0, 0, 0.28) * trotx(180);
q = Five_dof.ikunc(T);

1.3 使用matlab完成軌跡規(guī)劃

想象一個簡單的場景:我們想讓一個機械臂的末端執(zhí)行器四苇,不發(fā)生任何旋轉(zhuǎn)的方式從某一個點直線移動到另一個點月腋。這個過程該怎么描述呢?
首先就是位置妓肢,我們有一個起點以及一個終點碉钠,它們可以分別使用一個變化矩陣來描述喊废。在這里由于沒有發(fā)生任何旋轉(zhuǎn),我們就使用兩個坐標來描述(相對于世界坐標系)颓屑。然后就是此過程中的速度v和加速度a遍搞,同時我們要引入一個時間t這個參數(shù)方便描述溪猿。
總之,我們簡化這個過程為4個參數(shù):x(位置)措左、t(時間)、v(速度)凉逛、a(加速度)。軌跡規(guī)劃的含義簡單來說诬辈,就是在確定位置后焙糟,設計幾個良好的速度和加速度酬荞,保證運行過程穩(wěn)定、柔和瞧哟。
如果在我們假想場景中混巧,我們運行速度保持不變,那么機器人在開始運動的時候勤揩,速度在短時間內(nèi)從0突變到v咧党,結(jié)束運動時速度又突變?yōu)?img class="math-inline" src="https://math.jianshu.com/math?formula=0" alt="0" mathimg="1">,在開始和結(jié)束的加速度很大傍衡,也就對機械臂的動態(tài)沖擊很大,這顯然是不利于機械臂的長期運作的负蠕。
在matlab提供了很多種方法蛙埂,這里介紹一個5次多項式的插值方式獲得軌跡的方法。簡化一下遮糖,我們只看x坐標的變化并且從x=0的位置移動到x=3的位置绣的,時間設置為2s

t = linspace(0, 2, 51);
[P, dP, ddP] = tpoly(0, 3, t);
figure
subplot(1,3,1);
plot(t, P, '.-', 'LineWidth',1);
ylabel('x');
subplot(1,3,2);
plot(t, dP, '.-', 'LineWidth',1);
ylabel('v');
subplot(1,3,3);
plot(t, ddP, '.-', 'LineWidth',1);
ylabel('a');

圖5 五次多項式插值方式獲得的x,v屡江,a關于時間t的圖像
相比于速度恒定的軌跡芭概,五次多項式插值后的軌跡明顯平緩且加速度沒有突變情況,也就更加柔和一些惩嘉。
我們同時考慮x罢洲、y、z坐標文黎,相當于把我們原來的假設增加兩個維度惹苗。在matlab中,實現(xiàn)也是非常方便的臊诊。假設在2s內(nèi)鸽粉,我們要從坐標(1, 2, 3)移動到(2, 3, 4)
為了方便繪制,我們寫一個函數(shù)抓艳,能一次性在同一幅圖中畫出三個坐標的變化:

function plot_traj(Traj, t)
    hold on
    plot(t, Traj(:, 1), '.-', 'LineWidth',1);
    plot(t, Traj(:, 2), '.-', 'LineWidth',1);
    plot(t, Traj(:, 3), '.-', 'LineWidth',1);
    grid on
    legend('x', 'y', 'z');
    xlabel('times')
    ylabel('position')
end

那么這三條軌跡的圖像是:

t = linspace(0, 2, 51);
% @tpoly表明選擇某個插值方式
[P ,dP ,ddP] = mtraj(@tpoly, [1, 2, 3], [2, 3, 4], t);
figure
subplot(1,3,1);
plot_traj(P, t);
ylabel('x');
subplot(1,3,2);
plot_traj(dP, t);
ylabel('v');
subplot(1,3,3);
plot_traj(ddP, t);
ylabel('a');

圖6 多維軌跡繪制
我們使用這些函數(shù)獲得的是一系列坐標值触机,例如圖6情形所代表的矩陣P,其長度應該是51\times 3玷或。51行是因為我們選取的時間t向量的長度為51段儡首,3的含義是指x,y偏友,z三個坐標蔬胯。我們要把這些坐標轉(zhuǎn)化成變化矩陣(也就是末端執(zhí)行器的位姿):

T = zeros(4, 4, 51);
for i = 1:51
% 不帶任何旋轉(zhuǎn)
  T(:, :, i) = transl(P(i, :));
end
% 逆向運動學
q = Five_dof.ikunc(T);

使用matlab還可以實現(xiàn)更多不同的插值方式(例如lspb)、多維多段的軌跡規(guī)劃(mstraj)位他,在此不多贅述氛濒。

旋轉(zhuǎn)運動軌跡規(guī)劃

旋轉(zhuǎn)矩陣的優(yōu)點就是坐標系轉(zhuǎn)換非常簡單,但是不容易直接通過它去規(guī)劃(插值)機械臂的運動鹅髓。

現(xiàn)在我們已知起點和終點末端執(zhí)行器的變換矩陣T_0舞竿、T_1,我們可以由此獲得它們的旋轉(zhuǎn)矩陣窿冯。旋轉(zhuǎn)矩陣本質(zhì)上就是坐標系的旋轉(zhuǎn)變換骗奖,不過它是一個相對關系(相對于基坐標系),T_0T_1都可以由另外三次旋轉(zhuǎn)來完成醒串。怎么旋轉(zhuǎn)的方式有很多種执桌,比如歐拉角和橫滾-俯仰-偏航角:
T_i(eul) = R(z, \theta_1)\times R(y, \theta_2) \times R(z, \theta_3)\tag{3}
T_i(rpy) = R(z, \theta_1)\times R(y, \theta_2) \times R(x, \theta_3)\tag{4}
這里的\theta_i角就是我們想要的。例如公式3芜赌,我們將獲得T_0仰挣、T_1分別對應的\theta_{11}、\theta_{12}缠沈、\theta_{13}\theta_{21}椎木、\theta_{22}违柏、\theta_{23},我們類比前面對于三維坐標的插值香椎,把這兩對角度看做兩對坐標,用法就和前文完全一致禽篱。使用tr2rpy或者tr2eul函數(shù)來實現(xiàn)轉(zhuǎn)化成“兩對角度”畜伐,使用rpy2tr或者eul2tr重新獲得一系列變換矩陣:

T0 = transl(1, 2, 3) * trotx(180);
% 繞x軸旋轉(zhuǎn)-180度
T1 = transl(1, 2, 3);
rpy1 = tr2rpy(T0)/pi*180;
rpy2 = tr2rpy(T1)/pi*180;

% 時間
t = linspace(0, 2, 51);

% 多維五次多項式插值(規(guī)劃)
rpy_traj = mtraj(@tpoly, rpy1, rpy2, t);

%重新轉(zhuǎn)化成變換矩陣,指導機械臂運動
T_traj_rot = rpy2tr(rpy_traj);

值得注意的是躺率,轉(zhuǎn)換成角度后玛界,我們丟失了“位姿”中的位置參數(shù)。所以我們在規(guī)劃機械臂運動的時候悼吱,對于位置和姿態(tài)先分別規(guī)劃慎框,最后把它們獲得的變化矩陣重新整合。例如如下代碼:

% 起點和終點
T0 = transl(1, 2, 3) * trotx(180);
T1 = transl(2, 3, 4);
% 設置運動時間
t = linspace(0, 2, 51);

% 對于姿態(tài)
rpy1 = tr2rpy(T0)/pi*180;
rpy2 = tr2rpy(T1)/pi*180;
rpy_traj = mtraj(@tpoly, rpy1, rpy2, t);
T_traj_rot = rpy2tr(rpy_traj);

% 對于位置
P1 = transl(T0);
P2 = transl(T1);
P_traj = mtraj(@tpoly, P1', P2', t);
T_traj_transl = transl(P_traj);

% 整合
T_traj = zeros(4, 4, 51);
for i = 1:51
  T_traj(:,:,i) = T_traj_rot(:,:,i)*T_traj_transl(:,:,i);
end

% 讓機械臂動起來
q_traj = Five_dof.ikunc(T_traj)
Five_dof.plot(q_traj);

但是對于角度的規(guī)劃后添,求逆解的過程有誤差笨枯。對于roll-pitch-yaw(橫滾-俯仰-行偏角)的求解出的三個角度被限制在了一定范圍。所以并不推薦以上的方法來獲取旋轉(zhuǎn)的插值(有可能產(chǎn)生不連續(xù)的情況)遇西。

更簡單的方法

matlab給了一個非常方便的函數(shù)trinterp馅精,能直接傳入兩個變化矩陣(起點和終點),求解一個指定插值方式的軌跡(默認是線性)粱檀;以及笛卡爾軌跡生成函數(shù)ctraj洲敢。trinterp用法如下:

% S可以是一個值,也可以是向量茄蚯,但必須所有的元素在[0, 1]范圍之內(nèi)
T = trinterp(T0, T1, S);

以下代碼生成與圖6一致的軌跡:

T0 = transl(1, 2, 3);
T1 = transl(2, 3, 4);
% 必須保證tpoly生成的向量中的元素都在[0, 1]之內(nèi)
T  = trinterp(T0, T1, tpoly(0, 2, 50)/2); 
p = transl(T);
% 此函數(shù)在上文中定義過
plot_traj(p, linspace(0, 2, 50));

ctraj的用法與trinterp基本一致压彭,它們的區(qū)別在于默認的插值方式不同。

當然渗常,還可以先對我們的起點和終點的變換矩陣直接求逆解獲得起點和終點的關節(jié)變量壮不,然后對這兩個關節(jié)變量進行軌跡規(guī)劃。筆者認為凳谦,這種方法更加穩(wěn)定忆畅。

q1 = Five_dof.ikunc(T0);
q2 = Five_dof.ikunc(T1);
t = linspace(0, 2, 51);

% 使用jtraj函數(shù)對關節(jié)角度插值
q_Traj = jtraj(q1, q2, t);

Five_dof.ploy(q_Traj);

2 控制六自由度機械臂寫名字

我們選擇名叫RB08的6自由度機械臂來實現(xiàn)上文中的所有內(nèi)容。它的標準DH參數(shù)表如下:

表1 RB08

它沒有給出任何的\theta_i角度尸执,因此實際上機器人的初始位姿可以自行定義家凯,此處就不定義了,給它默認值如失。

  • 建模
%% modeling
L(1) = Link('revolute', 'd', 0.45,  'a', 0.17,      'alpha', pi/2, 'qlim', [-180, 180]/180*pi);
L(2) = Link('revolute', 'a', 0.56,  'qlim', [-135, 180]/180*pi);
L(3) = Link('revolute', 'a', 0.166, 'alpha', pi/2,    'qlim', [-90, 90]/180*pi);
L(4) = Link('revolute', 'd', 0.63,  'alpha', -1*pi/2, 'qlim', [-180, 180]/180*pi);
L(5) = Link('revolute', 'a', 0.11,  'alpha', pi/2,    'qlim', [-135, 135]/180*pi);
L(6) = Link('revolute', 'd', 0.095, 'qlim', [-180, 180]/180*pi);
Six_dof = SerialLink(L, 'name', 'RB08');
Six_dof.teach;

圖7 RB08建模

雖然由于沒有\theta_i參數(shù)看上去比較奇怪绊诲,但是各個自由度是非常正常的。

  • 預設路徑
    簡單起見褪贵,讓機器人只寫兩個字母(H與L)掂之。這兩個字母非常簡單抗俄,只有橫和豎。
%% 軌跡
n = 20;
t = linspace(0, 2, 20)/2;
t(20) = 1;
% H
% H-1
T1_0 = transl(1.2, -0.5, 0.8)*troty(90);
T1_1 = transl(1.2, -0.5, -0.2)*troty(90);
T_traj_1 = trinterp(T1_0, T1_1, t);
q_traj_1 = Six_dof.ikunc(T_traj_1);
% Six_dof.plot(q_traj_1, 'trail', 'b');

% H-2
T2_0 = transl(1.2, -0.5, 0.3)*troty(90);
T2_1 = transl(1.34, 0.35, 0.3)*troty(90);
% 到達第二筆初始位置
T_traj2s = trinterp(T1_1, T2_0, t);
q_traj2s = Six_dof.ikunc(T_traj2s);

T_traj_2 = trinterp(T2_0, T2_1, t);
q_traj_2 = Six_dof.ikunc(T_traj_2);

% H-3
T3_0 = transl(1.34, 0.35, 0.8)*troty(90);
T3_1 = transl(1.34, 0.35, -0.2)*troty(90);
% 到達第三筆的初始位置
T_traj3s = trinterp(T2_1, T3_0, t);
q_traj3s = Six_dof.ikunc(T_traj3s);

T_traj_3 = trinterp(T3_0, T3_1, t);
q_traj_3 = Six_dof.ikunc(T_traj_3);

% L-1
T4_0 = transl(1, 0.83, 0.8)*troty(90);
T4_1 = transl(1, 0.83, -0.2)*troty(90)*trotx(-60);
% 到達第四筆的初始位置
T_traj4s = trinterp(T3_1, T4_0, t);
q_traj4s = Six_dof.ikunc(T_traj4s);

T_traj_4 = trinterp(T4_0, T4_1, t);
q_traj_4 = Six_dof.ikunc(T_traj_4);

% L-2
T5_0 = transl(1, 0.83, -0.2)*troty(90)*trotx(-60);
T5_1 = transl(0.5, 1.2, -0.2)*troty(90)*trotx(-60);

T_traj_5 = trinterp(T5_0, T5_1, t);
q_traj_5 = Six_dof.ikunc(T_traj_5);

%% 最終軌跡
q = zeros(n*8, 6);
q(1:n, :) = q_traj_1;
q(n+1:n*2, :) = q_traj2s;
q(n*2+1:n*3, :) = q_traj_2;
q(n*3+1:n*4, :) = q_traj3s;
q(n*4+1:n*5, :) = q_traj_3;
q(n*5+1:n*6, :) = q_traj4s;
q(n*6+1:n*7, :) = q_traj_4;
q(n*7+1:end, :) = q_traj_5;
Six_dof.plot(q,'trail', 'b');

代碼運行起來就像這樣:


圖8 成品圖

也許還有更好的方法繪制軌跡世舰,這里暫時先用屎山代碼放著动雹。


3 matlab機器人工具箱常用函數(shù)

請閱讀本文參考鏈接MATLAB機器人工具箱10.4 機械臂仿真教學(未完結(jié))

姿態(tài)的旋轉(zhuǎn)矩陣代碼和歐拉角和橫滾-俯仰-航偏角
旋轉(zhuǎn)與變換矩陣的轉(zhuǎn)換
變換矩陣與位置、旋轉(zhuǎn)互相轉(zhuǎn)換
正向運動學
逆向運動學

# 參考

參考鏈接:

  1. 運動學建模(標準DH法)
  2. https://blog.csdn.net/yaked/article/details/73030668
  3. https://blog.csdn.net/weixin_43332715/article/details/131347874
  4. 機械臂位姿描述
  5. 機器人工具箱常用函數(shù)跟压、代碼
  6. 建模+正逆運動學+雅克比矩陣
  7. 正運動學詳解
  8. MATLAB機器人工具箱10.4 機械臂仿真教學(未完結(jié))
最后編輯于
?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者
  • 序言:七十年代末胰蝠,一起剝皮案震驚了整個濱河市,隨后出現(xiàn)的幾起案子震蒋,更是在濱河造成了極大的恐慌茸塞,老刑警劉巖,帶你破解...
    沈念sama閱讀 219,188評論 6 508
  • 序言:濱河連續(xù)發(fā)生了三起死亡事件查剖,死亡現(xiàn)場離奇詭異钾虐,居然都是意外死亡,警方通過查閱死者的電腦和手機笋庄,發(fā)現(xiàn)死者居然都...
    沈念sama閱讀 93,464評論 3 395
  • 文/潘曉璐 我一進店門效扫,熙熙樓的掌柜王于貴愁眉苦臉地迎上來,“玉大人无切,你說我怎么就攤上這事荡短。” “怎么了哆键?”我有些...
    開封第一講書人閱讀 165,562評論 0 356
  • 文/不壞的土叔 我叫張陵掘托,是天一觀的道長。 經(jīng)常有香客問我籍嘹,道長闪盔,這世上最難降的妖魔是什么? 我笑而不...
    開封第一講書人閱讀 58,893評論 1 295
  • 正文 為了忘掉前任辱士,我火速辦了婚禮泪掀,結(jié)果婚禮上,老公的妹妹穿的比我還像新娘颂碘。我一直安慰自己异赫,他們只是感情好,可當我...
    茶點故事閱讀 67,917評論 6 392
  • 文/花漫 我一把揭開白布头岔。 她就那樣靜靜地躺著塔拳,像睡著了一般。 火紅的嫁衣襯著肌膚如雪峡竣。 梳的紋絲不亂的頭發(fā)上靠抑,一...
    開封第一講書人閱讀 51,708評論 1 305
  • 那天,我揣著相機與錄音适掰,去河邊找鬼颂碧。 笑死荠列,一個胖子當著我的面吹牛,可吹牛的內(nèi)容都是我干的载城。 我是一名探鬼主播肌似,決...
    沈念sama閱讀 40,430評論 3 420
  • 文/蒼蘭香墨 我猛地睜開眼,長吁一口氣:“原來是場噩夢啊……” “哼诉瓦!你這毒婦竟也來了锈嫩?” 一聲冷哼從身側(cè)響起,我...
    開封第一講書人閱讀 39,342評論 0 276
  • 序言:老撾萬榮一對情侶失蹤垦搬,失蹤者是張志新(化名)和其女友劉穎,沒想到半個月后艳汽,有當?shù)厝嗽跇淞掷锇l(fā)現(xiàn)了一具尸體猴贰,經(jīng)...
    沈念sama閱讀 45,801評論 1 317
  • 正文 獨居荒郊野嶺守林人離奇死亡,尸身上長有42處帶血的膿包…… 初始之章·張勛 以下內(nèi)容為張勛視角 年9月15日...
    茶點故事閱讀 37,976評論 3 337
  • 正文 我和宋清朗相戀三年河狐,在試婚紗的時候發(fā)現(xiàn)自己被綠了米绕。 大學時的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片。...
    茶點故事閱讀 40,115評論 1 351
  • 序言:一個原本活蹦亂跳的男人離奇死亡馋艺,死狀恐怖栅干,靈堂內(nèi)的尸體忽然破棺而出,到底是詐尸還是另有隱情捐祠,我是刑警寧澤碱鳞,帶...
    沈念sama閱讀 35,804評論 5 346
  • 正文 年R本政府宣布,位于F島的核電站踱蛀,受9級特大地震影響窿给,放射性物質(zhì)發(fā)生泄漏。R本人自食惡果不足惜率拒,卻給世界環(huán)境...
    茶點故事閱讀 41,458評論 3 331
  • 文/蒙蒙 一崩泡、第九天 我趴在偏房一處隱蔽的房頂上張望。 院中可真熱鬧猬膨,春花似錦角撞、人聲如沸。這莊子的主人今日做“春日...
    開封第一講書人閱讀 32,008評論 0 22
  • 文/蒼蘭香墨 我抬頭看了看天上的太陽。三九已至召耘,卻和暖如春百炬,著一層夾襖步出監(jiān)牢的瞬間,已是汗流浹背污它。 一陣腳步聲響...
    開封第一講書人閱讀 33,135評論 1 272
  • 我被黑心中介騙來泰國打工剖踊, 沒想到剛下飛機就差點兒被人妖公主榨干…… 1. 我叫王不留庶弃,地道東北人。 一個月前我還...
    沈念sama閱讀 48,365評論 3 373
  • 正文 我出身青樓德澈,卻偏偏與公主長得像歇攻,于是被迫代替她去往敵國和親。 傳聞我的和親對象是個殘疾皇子梆造,可洞房花燭夜當晚...
    茶點故事閱讀 45,055評論 2 355

推薦閱讀更多精彩內(nèi)容