行星采樣機(jī)械臂的軌跡規(guī)劃研究

張冰蔚 戚永康 齊超 申阿強(qiáng) 張鵬

（江蘇科技大學(xué)機(jī)械工程學(xué)院， 江蘇 鎮(zhèn)江? 212003）

摘? 要： 為行星采樣機(jī)械臂做軌跡規(guī)劃，先根據(jù)D?H法建立機(jī)械臂的運(yùn)動(dòng)學(xué)模型，對(duì)其進(jìn)行了笛卡爾空間直線插補(bǔ)和圓弧插補(bǔ)兩種軌跡的規(guī)劃，并在Matlab中進(jìn)行了運(yùn)動(dòng)仿真。然后基于MFC和OpenGL設(shè)計(jì)一種針對(duì)采樣機(jī)械臂的運(yùn)動(dòng)仿真程序。最后結(jié)合一種運(yùn)動(dòng)控制器進(jìn)行直線插補(bǔ)和圓弧插補(bǔ)的軌跡規(guī)劃的實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明，采樣機(jī)械臂的實(shí)際運(yùn)動(dòng)軌跡與運(yùn)動(dòng)仿真程序中的軌跡一致，對(duì)其進(jìn)行的正、逆運(yùn)動(dòng)學(xué)分析和軌跡規(guī)劃是正確的，為行星采樣機(jī)械臂的軌跡設(shè)計(jì)和分析提供了參考依據(jù)。

關(guān)鍵詞： 采樣機(jī)械臂; 運(yùn)動(dòng)學(xué)模型; 笛卡爾空間; 軌跡規(guī)劃; 插補(bǔ)算法; Matlab仿真

中圖分類號(hào)： TN101?34; TP242.3? ? ? ? ? ? ? ? 文獻(xiàn)標(biāo)識(shí)碼： A? ? ? ? ? ? ? ? ? ? ? ?文章編號(hào)： 1004?373X（2020）14?0114?04

Research on trajectory planning of manipulator for sampling on planets

ZHANG Bingwei， QI Yongkang， QI Chao， SHEN Aqiang， ZHANG Peng

（College of Mechanical Engineering， Jiangsu University of Science and Technology， Zhenjiang 212003， China）

Abstract： In allusion to the trajectory planning of the manipulator for sampling on planets， the kinematics model of the manipulator is established according to the D?H method to perform the two kinds of trajectory planning of Cartesian space linear interpolation and circular interpolation， and its motion simulation is carried out with Matlab. A motion simulation program for the sampling manipulator is designed based on MFC and OpenGL. The experiment of trajectory planning of the linear interpolation and circular interpolation was conducted in combination with the motion controller. The experimental results show that the actual motion trajectory of the sampling manipulator is consistent with the trajectory in the motion simulation program， and the forward and inverse kinematics analysis and trajectory planning are correct， which provides a reference for the trajectory design and analysis of the manipulator for sampling on planets.

Keywords： sampling manipulator; trajectory planning; kinematical modeling; motion simulation; line interpolation; circular interpolation

0? 引? 言

人類在了解地球的同時(shí)也在不懈地探索外太空，近年來，中、美、俄等國家的航天機(jī)構(gòu)陸續(xù)地向外太空發(fā)射了許多探測(cè)器。行星探測(cè)車是探測(cè)行星環(huán)境及其資源的重要工具，包括許多重要部件，而采樣機(jī)械臂又是重中之重，用于對(duì)行星進(jìn)行采樣極其關(guān)鍵[1?3]。為此本文對(duì)實(shí)驗(yàn)室里的行星采樣機(jī)械臂進(jìn)行軌跡規(guī)劃的研究，建立機(jī)械臂的數(shù)學(xué)模型、規(guī)劃了機(jī)械臂的軌跡，然后設(shè)計(jì)程序進(jìn)行運(yùn)動(dòng)仿真，結(jié)合控制器進(jìn)行軌跡實(shí)驗(yàn)。結(jié)果表明，對(duì)機(jī)械臂進(jìn)行的軌跡規(guī)劃是符合預(yù)期的，為今后進(jìn)一步的實(shí)驗(yàn)和分析提供了重要參考。

1? 采樣機(jī)械臂的結(jié)構(gòu)

圖1所示為本文行星采樣機(jī)械臂的結(jié)構(gòu)簡(jiǎn)圖。它具有4個(gè)自由度，分別為腰、肩、肘和腕四個(gè)關(guān)節(jié)。腕關(guān)節(jié)的任務(wù)是挖掘行星表層的土壤，其余三個(gè)關(guān)節(jié)負(fù)責(zé)進(jìn)行定位。

本文的采樣機(jī)械臂主要應(yīng)用在30～50 kg的微型漫游車上，所以其結(jié)構(gòu)的設(shè)計(jì)有嚴(yán)格的標(biāo)準(zhǔn)，如表1所示為機(jī)械臂結(jié)構(gòu)中各參數(shù)的設(shè)計(jì)要求。

2? 采樣機(jī)械臂的數(shù)學(xué)模型建立

如圖2所示，依據(jù)經(jīng)典的D?H法[4]定義采樣機(jī)械臂的各關(guān)節(jié)坐標(biāo)系。

表2為行星采樣機(jī)械臂各連桿的4個(gè)運(yùn)動(dòng)學(xué)參數(shù)。

2.1? 正運(yùn)動(dòng)學(xué)建模

根據(jù)機(jī)械臂的坐標(biāo)變換法則，可得相鄰坐標(biāo)系之間的變換關(guān)系，如下[5]：

[i-1iT=cθi-sθi0? ? ? ? ? ? ? αi-1sθicαi-1cθicαi-1-sαi-1? ? -sαi-1disθisαi-1cθisαi-1cαi-1? ? ? ?cαi-1di? ? ?0? ? ? ? ? ? ? ? ? ? ?0? ? ? ? ? ? ? 0? ? ? ? ? ? ? ? ?1]? （1）

式中：[cθi=cos θi;sθi=sin θi;cαi=cos αi;sαi=][sin αi]。

根據(jù)表2與式（1），可推導(dǎo)出機(jī)械臂末端執(zhí)行器相對(duì)于基坐標(biāo)系的變換矩陣：

[04T=01T12T23T34T? ? ?=c1c234-c1s234s1? ? ? c1（l3c23+c2l2）s1c234-s1s234-c1? ? s1（l3c23+c2l2）s234c234 0? ? ? ?l3s23+s2l2+d1? ? 0? ? ? ? ? ? ? 0? ? ? ? ? ?0? ? ? ? ? ? ? ? ?1]? ? （2）

2.2? 逆運(yùn)動(dòng)學(xué)建模

給定一個(gè)期望的末端位姿齊次變換可得對(duì)應(yīng)的非線性方程[6]，通過代數(shù)求解法，可得采樣機(jī)械臂的逆解，結(jié)果如下：

[θ1=atan2（px，py）θ2=atan2pz-d1，pxc1-atan2（l3s3，l3c3+l2）θ3=atan2（sin θ3，cos θ3）θ4=atan2（a31，a32）-θ2-θ3]? （3）

由逆解計(jì)算的結(jié)果可知，θ3具有正負(fù)兩個(gè)解，如圖3所示。就采樣機(jī)械臂的末端執(zhí)行機(jī)構(gòu)需進(jìn)行挖掘作業(yè)，且避免碰撞地面，故選擇上肘位形式為最具有魯棒性的解。

3? 采樣機(jī)械臂的軌跡規(guī)劃

軌跡規(guī)劃是通過預(yù)先設(shè)定期望的軌跡，并從此路徑出發(fā)，從而生成相關(guān)變量的時(shí)間規(guī)律，如機(jī)械臂運(yùn)動(dòng)的位移、速度等[7]，本文對(duì)采樣機(jī)械臂采用笛卡爾空間軌跡規(guī)劃的方法[8?10]。

3.1? 笛卡爾空間直線插補(bǔ)

假設(shè)需要機(jī)械臂的末端執(zhí)行器從點(diǎn)A運(yùn)動(dòng)到點(diǎn)B，通過運(yùn)動(dòng)學(xué)的逆解，將所得的插值點(diǎn)坐標(biāo)映射到關(guān)節(jié)空間中各關(guān)節(jié)所對(duì)應(yīng)的角度。設(shè)從點(diǎn)A（0，3，2）運(yùn)動(dòng)到點(diǎn)B（7，0，5），插補(bǔ)點(diǎn)數(shù)N為30，可將具體的算法運(yùn)用M語言編寫，運(yùn)用Matlab進(jìn)行直線插補(bǔ)仿真。仿真結(jié)果如圖4所示。

3.2? 笛卡爾空間圓弧插補(bǔ)

假設(shè)三點(diǎn)P1，P2，P3不共線，通過逆運(yùn)動(dòng)學(xué)將需要插值點(diǎn)轉(zhuǎn)換為對(duì)應(yīng)的關(guān)節(jié)坐標(biāo)值，對(duì)應(yīng)點(diǎn)的關(guān)節(jié)角，驅(qū)動(dòng)各個(gè)關(guān)節(jié)在關(guān)節(jié)空間的運(yùn)動(dòng)。假設(shè)機(jī)械臂的末端執(zhí)行器在基坐標(biāo)系下沿著圓弧路徑從起點(diǎn)A（0，3，2），經(jīng)過中間點(diǎn)B（7，0，5），到達(dá)終點(diǎn)C（5，0，7），這里插補(bǔ)總次數(shù)N取30。運(yùn)用Matlab進(jìn)行空間圓弧插補(bǔ)仿真，結(jié)果如圖5所示[11]。

4? 采樣機(jī)械臂的運(yùn)動(dòng)仿真及軌跡規(guī)劃實(shí)驗(yàn)

4.1? 基于OpenGL的機(jī)械臂運(yùn)動(dòng)仿真

根據(jù)表2中各個(gè)連桿的具體參數(shù)，將采樣機(jī)械臂各個(gè)值等比例縮小80倍，取d1=2.187 5 mm，l2=6.125 mm，l3=5.25 mm，通過OpenGL函數(shù)建立采樣機(jī)械臂的簡(jiǎn)化三維模型，且根據(jù)軌跡算法得出各關(guān)節(jié)的角度變量，最后調(diào)用MFC框架類實(shí)現(xiàn)軌跡規(guī)劃的仿真，程序界面如圖6所示[12?14]。

圖6的程序界面包括三維顯示和運(yùn)動(dòng)控制兩部分。右邊的各個(gè)控制模塊可以進(jìn)行正、逆運(yùn)動(dòng)規(guī)劃，手動(dòng)輸入關(guān)節(jié)角值和通信，從而控制左邊界面中三維模型的各個(gè)關(guān)節(jié)的運(yùn)動(dòng)。本文僅對(duì)此機(jī)械臂進(jìn)行相關(guān)的直線規(guī)劃和圓弧規(guī)劃試驗(yàn)。

4.2? 搭建實(shí)驗(yàn)平臺(tái)

以EMAC?200四軸運(yùn)動(dòng)控制器作為采樣機(jī)械臂的下位機(jī)控制器，該控制器內(nèi)部集成了Magellan運(yùn)動(dòng)處理芯片，除能精確地控制多種電機(jī)外還能支持多種程序運(yùn)行方式，通過高速總線與通信端口、RMA等連接，且支持多種通信協(xié)議。

選用YZ?DSCD3605直流伺服驅(qū)動(dòng)器，并且搭配AM26LS31芯片轉(zhuǎn)換編碼器的單端信號(hào)到差分信號(hào)以供控制器讀取，采樣機(jī)械臂的硬件系統(tǒng)結(jié)構(gòu)如圖7所示。

此控制系統(tǒng)采用上、下控制器兩級(jí)分布式控制，上位機(jī)進(jìn)行正、逆運(yùn)動(dòng)學(xué)的求解，管理系統(tǒng)等，下位機(jī)則負(fù)責(zé)控制軌跡的運(yùn)動(dòng)。本方案搭建的采樣機(jī)械臂實(shí)驗(yàn)平臺(tái)如圖8所示。

4.3? 軌跡規(guī)劃實(shí)驗(yàn)

搭建采樣機(jī)械臂的實(shí)驗(yàn)平臺(tái)后，在仿真軟件中的通信模塊上，上位機(jī)電腦通過Ethernet以TCP協(xié)議與下位機(jī)實(shí)現(xiàn)連接。在軌跡規(guī)劃模塊輸入相應(yīng)的軌跡規(guī)劃坐標(biāo)，后臺(tái)程序即可計(jì)算路徑點(diǎn)在各個(gè)空間中的關(guān)節(jié)角度，通過控制器轉(zhuǎn)化為關(guān)節(jié)脈沖以控制電機(jī)的運(yùn)動(dòng)。

在軌跡規(guī)劃模塊設(shè)置好規(guī)劃軌跡的起始點(diǎn)及終點(diǎn)，進(jìn)行仿真軌跡實(shí)驗(yàn)和硬件的通信連接，仿真界面及實(shí)驗(yàn)的結(jié)果對(duì)比如圖9所示。

5? 結(jié)? 語

本文對(duì)一種行星采樣機(jī)械臂進(jìn)行軌跡規(guī)劃的研究。首先建立采樣機(jī)械臂運(yùn)動(dòng)學(xué)模型，接著利用Matlab工具進(jìn)行笛卡爾空間直線插補(bǔ)和圓弧插補(bǔ)軌跡規(guī)劃的仿真研究。然后根據(jù)OpenGL三維模型庫和Visual Studio/MFC模塊庫開發(fā)了一套針對(duì)于機(jī)械臂的運(yùn)動(dòng)仿真程序。最后基于以上研究搭建了機(jī)械臂的硬件平臺(tái)，用E?MAC200控制器作為機(jī)械臂的運(yùn)動(dòng)控制器，在笛卡爾空間進(jìn)行直線插補(bǔ)和圓弧插補(bǔ)的軌跡規(guī)劃實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明，各關(guān)節(jié)軌跡均能達(dá)到目標(biāo)品質(zhì)的要求，對(duì)此機(jī)械臂進(jìn)行的軌跡規(guī)劃是符合預(yù)期的，為今后進(jìn)一步的實(shí)驗(yàn)和分析提供了重要參考。

參考文獻(xiàn)

[1] 林楊挺.探索火星環(huán)境和生命[J].自然雜志，2016，38（1）：1?7.

[2] XU Lin， ZOU Yongliao， JIA Yingzhuo. China's planning for deep space exploration and lunar exploration before 2030 [J]. Chinese journal of space science， 2018， 38（5）： 591?592.

[3] 寧杭.六輪搖臂擺桿式行星探測(cè)車的結(jié)構(gòu)設(shè)計(jì)與分析[D].鎮(zhèn)江：江蘇科技大學(xué)，2016.

[4] 蔡自興，謝斌.機(jī)器人學(xué)[M].北京：清華大出版社，2015：35?49.

[5] 周霏，陳富林，沈金龍，等.基于Matlab的四自由度機(jī)械臂運(yùn)動(dòng)學(xué)仿真研究[J].機(jī)械制造與自動(dòng)化，2016，45（1）：115?119.

[6] 馮瑤，公茂震.6R機(jī)器人笛卡爾空間軌跡規(guī)劃中的逆運(yùn)動(dòng)學(xué)[J].自動(dòng)化技術(shù)與應(yīng)用，2018，37（6）：68?73.

[7] 付榮，居鶴華.基于粒子群優(yōu)化的時(shí)間最優(yōu)機(jī)械臂軌跡規(guī)劃算法[J].信息與控制，2011，40（6）：802?808.

[8] FANG Yi， HU Jie， LIU Wenhai， et al. Smooth and time?optimal S?curve trajectory planning for automated robots and machines [J]. Mechanism and machine theory， 2019， 137： 127?153.

[9] XUAN Guantao， SHAO Yuanyuan. Reverse?driving trajectory planning and simulation of joint robot [J]. IFAC PapersOnLine， 2018， 51（17）： 384?388.

[10] WANG He， WANG Heng， HUANG Jiahai， et al. Smooth point?to?point trajectory planning for industrial robots with kinematical constraints based on high?order polynomial curve [J]. Mechanism and machine theory， 2019， 139： 284?293.

[11] 葉政.六自由度工業(yè)機(jī)器人軌跡規(guī)劃算法研究與仿真[D].南京：南京航空航天大學(xué)，2017.

[12] 李泰國，李文新，王偉文，等.基于OpenGL空間機(jī)械臂三維重構(gòu)可視化研究[J].計(jì)算機(jī)技術(shù)與發(fā)展，2018，28（1）：179?181.

[13] 王銳.OpenGL編程指南[M].北京：機(jī)械工業(yè)出版社，2017.

[14] 杜必強(qiáng)，陳佳楠.基于以太網(wǎng)通信和OpenGL的庫卡機(jī)器人在線控制及仿真[J].現(xiàn)代制造技術(shù)與裝備，2018（12）：48?50.