資源簡(jiǎn)介
將末端姿態(tài)代入機(jī)械臂逆運(yùn)動(dòng)模型中,求出八組逆解角度,將八組逆解角度代入機(jī)械臂正運(yùn)動(dòng)學(xué)方程,判斷機(jī)械臂與障礙物是否發(fā)生碰撞,進(jìn)行碰撞檢測(cè),以實(shí)現(xiàn)機(jī)械臂避障路徑規(guī)劃。

代碼片段和文件信息
T?=?[452.12?149.09?-433.07;139.942?149.09?-320.595];?%路徑上各點(diǎn)坐標(biāo),路徑點(diǎn)為10個(gè),有蟻群算法得來(lái)
G?=?[];??%用來(lái)存儲(chǔ)新的障礙節(jié)點(diǎn),用于柵格地圖的更新
N?=?ndims(T);
Q?=?[];?%可行路徑上的最終各點(diǎn)的關(guān)節(jié)角
c?=?[0?0?0?0?0?0?0?0];?%不可行點(diǎn)判別數(shù)組??%?A為路徑可行判別數(shù),可行為1,不可行為0
sum?=?[];
for?i?=1:N????%分別對(duì)各點(diǎn)進(jìn)行碰撞檢測(cè)
????t?=?T(i:);
????q?=?transl(t);
????TH?=?niyundx_change(q);
????for?m?=?1:8??%根據(jù)轉(zhuǎn)角最小原則進(jìn)行排序,小的放前面,大的放后面
????????sum(m)?=?abs(TH(m1))+abs(TH(m2))+abs(TH(m3))+abs(TH(m4))+abs(TH(m5))+abs(TH(m6));
????end
????[sim?index]?=?sort(sum);
????THH?=?[TH(index(1):);TH(index(2):);TH(index(3):);TH(index(4):);TH(index(5):);
????????TH(index(6):);TH(index(7):);TH(index(8):)];
????for?j?=?1:8???%八組關(guān)節(jié)角分別進(jìn)行碰撞檢測(cè),若有可行的關(guān)節(jié)角,則后面的不繼續(xù)檢查
?????????B?=?pengzhuangjiance(THH(j:));
?????????if?B?==?0
????????????????Q(i:)?=?THH(j:);
????????????????break
?????????else
?????????????c(j)?=?1;?%把不可行的關(guān)節(jié)角相應(yīng)的判別數(shù)組里的位置置1,便于最后的關(guān)節(jié)可行檢測(cè)
?????????end
????end
????cc?=?1;
????for?n?=?1:8
????????cc?=?cc*c(n);
????end
????if?cc?==1??%判別i坐標(biāo)處是否所有關(guān)節(jié)角都不可行
????????G?=?[G;T(i:)]
????????A?=?0??%若都不可行,表示路徑不可行,該點(diǎn)納入障礙節(jié)點(diǎn),需重新規(guī)劃
????????break
????else?if?i?==?N??%若可行,判斷是否已經(jīng)最后一個(gè)路徑點(diǎn)
????????????A?=?1???%若是,則路徑可行,輸出各點(diǎn)的最優(yōu)關(guān)節(jié)角
????????????Q
????????????break
????????end
????end
end
????????
??
?????????????
???
????
?屬性????????????大小?????日期????時(shí)間???名稱(chēng)
-----------?---------??----------?-----??----
?????目錄???????????0??2020-07-15?22:35??路徑檢測(cè)?-?副本\
?????文件????????1531??2020-06-22?15:39??路徑檢測(cè)?-?副本\lujingjiance.m
?????文件????????6825??2020-06-29?19:45??路徑檢測(cè)?-?副本\niyundx_change.m
?????文件????????6679??2020-06-22?15:46??路徑檢測(cè)?-?副本\pengzhuangjiance.m
評(píng)論
共有 條評(píng)論