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

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

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

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

中圖分類號： TN101?34; TP242.3? ? ? ? ? ? ? ? 文獻標識碼： 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? 引? 言

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

1? 采樣機械臂的結構

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

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

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

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

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

2.1? 正運動學建模

根據(jù)機械臂的坐標變換法則，可得相鄰坐標系之間的變換關系，如下[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），可推導出機械臂末端執(zhí)行器相對于基坐標系的變換矩陣：

[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? 逆運動學建模

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

[θ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）

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

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

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

3.1? 笛卡爾空間直線插補

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

3.2? 笛卡爾空間圓弧插補

假設三點P1，P2，P3不共線，通過逆運動學將需要插值點轉(zhuǎn)換為對應的關節(jié)坐標值，對應點的關節(jié)角，驅(qū)動各個關節(jié)在關節(jié)空間的運動。假設機械臂的末端執(zhí)行器在基坐標系下沿著圓弧路徑從起點A（0，3，2），經(jīng)過中間點B（7，0，5），到達終點C（5，0，7），這里插補總次數(shù)N取30。運用Matlab進行空間圓弧插補仿真，結果如圖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ù)軌跡算法得出各關節(jié)的角度變量，最后調(diào)用MFC框架類實現(xiàn)軌跡規(guī)劃的仿真，程序界面如圖6所示[12?14]。

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

4.2? 搭建實驗平臺

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

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

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

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

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

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

5? 結? 語

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

參考文獻

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

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

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

[6] 馮瑤，公茂震.6R機器人笛卡爾空間軌跡規(guī)劃中的逆運動學[J].自動化技術與應用，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].南京：南京航空航天大學，2017.

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

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

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