張冰蔚 戚永康 齊超 申阿強 張鵬

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

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

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

中圖分類號： TN101?34; TP242.3? ? ? ? ? ? ? ? 文獻標(biāo)識碼： A? ? ? ? ? ? ? ? ? ? ? ?文章編號： 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? 引? 言

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

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

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

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

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

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

表2為行星采樣機械臂各連桿的4個運動學(xué)參數(shù)。

2.1? 正運動學(xué)建模

根據(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)出機械臂末端執(zhí)行器相對于基坐標(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? 逆運動學(xué)建模

給定一個期望的末端位姿齊次變換可得對應(yīng)的非線性方程[6]，通過代數(shù)求解法，可得采樣機械臂的逆解，結(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）

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

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

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

3.1? 笛卡爾空間直線插補

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

3.2? 笛卡爾空間圓弧插補

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

4? 采樣機械臂的運動仿真及軌跡規(guī)劃實驗

4.1? 基于OpenGL的機械臂運動仿真

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

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

4.2? 搭建實驗平臺

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

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

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

4.3? 軌跡規(guī)劃實驗

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

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

5? 結(jié)? 語

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

參考文獻

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

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

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

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

[7] 付榮，居鶴華.基于粒子群優(yōu)化的時間最優(yōu)機械臂軌跡規(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è)機器人軌跡規(guī)劃算法研究與仿真[D].南京：南京航空航天大學(xué)，2017.

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

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

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