□ 嵇建成 □ 郭 帥 □ 薛 卉 □ 王志勇

1.上海大學(xué) 上海市智能制造及機(jī)器人重點(diǎn)實(shí)驗(yàn)室 上海 200072

2.上海電氣集團(tuán)股份有限公司 中央研究院 上海 200070

模塊化機(jī)械臂軌跡規(guī)劃算法仿真研究*

□ 嵇建成1□ 郭 帥1□ 薛 卉2□ 王志勇1

1.上海大學(xué) 上海市智能制造及機(jī)器人重點(diǎn)實(shí)驗(yàn)室 上海 200072

2.上海電氣集團(tuán)股份有限公司 中央研究院 上海 200070

基于OpenGL模塊化機(jī)械臂三維仿真模型，闡述了豎直六自由度、懸掛六自由度以及豎直七自由度等多種模塊化機(jī)械臂的運(yùn)動(dòng)學(xué)求解，實(shí)現(xiàn)了三次多項(xiàng)式、五次多項(xiàng)式、BDJP等多種插補(bǔ)算法，完成了在笛卡爾空間層中以直角坐標(biāo)路徑控制和以BDJP法實(shí)現(xiàn)圓弧軌跡插值控制。另外，在模塊化機(jī)械臂實(shí)驗(yàn)平臺(tái)上，針對(duì)各種運(yùn)動(dòng)學(xué)求解算法編寫了驗(yàn)算程序，完成了豎直六自由度同步在線控制、懸掛六自由度指令控制走“中”字和圓等實(shí)驗(yàn)，證明了算法的正確性和精確性。

軌跡規(guī)劃 OpenGL 模塊化機(jī)械臂

服務(wù)機(jī)械臂不僅在空間探測(cè)、軍事戰(zhàn)場(chǎng)、家庭服務(wù)領(lǐng)域有著重要的應(yīng)用價(jià)值和廣闊的發(fā)展前景，而且在工業(yè)生產(chǎn)、醫(yī)療保健領(lǐng)域也存在很大的市場(chǎng)潛力［1］。許多科研機(jī)構(gòu)競(jìng)相開展了對(duì)服務(wù)機(jī)械臂的研究，但由于服務(wù)機(jī)械臂涉及軌跡規(guī)劃、遙控操作等復(fù)雜技術(shù)，使服務(wù)機(jī)械臂的仿真控制技術(shù)發(fā)展緩慢。

筆者以德國(guó)Schunk模塊化服務(wù)機(jī)械臂為研究對(duì)象，基于OpenGL完成了服務(wù)機(jī)械臂仿真控制平臺(tái)的研發(fā)，實(shí)現(xiàn)了三維仿真模型和實(shí)物模型的軌跡規(guī)劃和同步動(dòng)作，為進(jìn)一步實(shí)現(xiàn)無線遙控操作技術(shù)打好基礎(chǔ)。

1 簡(jiǎn)介

平臺(tái)的硬件部分是根據(jù)德國(guó)Schunk模塊化服務(wù)機(jī)械臂電氣和硬件要求完成服務(wù)機(jī)械臂組裝，通過CAN總線和模塊API函數(shù)，實(shí)現(xiàn)硬件軟件控制［2～4］。軟件層面，上位計(jì)算機(jī)根據(jù)仿真模型和硬件模型結(jié)構(gòu)，完成運(yùn)動(dòng)學(xué)求解，如圖1所示。

軌跡規(guī)劃是實(shí)現(xiàn)PTP運(yùn)動(dòng)、過路徑點(diǎn)運(yùn)動(dòng)、直線運(yùn)動(dòng)、圓弧運(yùn)動(dòng)以及解析平臺(tái)指令的基礎(chǔ)，決定著服務(wù)機(jī)械臂運(yùn)動(dòng)的精度［5～7］。模塊化機(jī)械臂仿真軟件的軌跡規(guī)劃器主要在關(guān)節(jié)層采用拋物線過渡、三次多項(xiàng)式、過路徑點(diǎn)三次多項(xiàng)式、過路徑點(diǎn)五次多項(xiàng)式等插

值算法，實(shí)現(xiàn)關(guān)節(jié)運(yùn)動(dòng)軌跡規(guī)劃；在笛卡爾空間層采用直角坐標(biāo)路徑控制法、BDJP法等插補(bǔ)算法，實(shí)現(xiàn)走直線、圓弧等軌跡運(yùn)動(dòng)［8～12］；其它兩層實(shí)現(xiàn)硬件模型和仿真模型的運(yùn)動(dòng)控制功能。指令系統(tǒng)負(fù)責(zé)服務(wù)機(jī)械臂指令文件的編譯、調(diào)試及執(zhí)行，軟件交互界面負(fù)責(zé)顯示三維仿真模型的運(yùn)動(dòng)過程及用戶交互。通過模塊的光電編碼器采集模塊的位置和速度，驅(qū)動(dòng)仿真模型動(dòng)作，監(jiān)視模型末端位置情況［13～16］。本文重點(diǎn)闡述基于BDJP插值算法的軌跡規(guī)劃工作。

▲圖1 模塊化機(jī)械臂仿真平臺(tái)軟件結(jié)構(gòu)

2 基于多項(xiàng)式軌跡規(guī)劃算法

筆者設(shè)計(jì)的軌跡規(guī)劃器根據(jù)用戶指令，采用拋物線過渡、三次多項(xiàng)式、五次多項(xiàng)式、過路徑點(diǎn)三次多項(xiàng)式、過路徑點(diǎn)五次多項(xiàng)式插值算法，在關(guān)節(jié)空間實(shí)現(xiàn)了服務(wù)機(jī)械臂的PTP和過路徑點(diǎn)運(yùn)動(dòng)；采用直角坐標(biāo)路徑法和BDJP插值法，在笛卡爾空間實(shí)現(xiàn)服務(wù)機(jī)械臂的直線和圓弧功能。

2.1 拋物線過渡插值算法

Schunk運(yùn)動(dòng)模塊接口函數(shù)提供了一種梯形運(yùn)動(dòng)方式，其速度曲線如圖2所示。該接口函數(shù)能實(shí)現(xiàn)帶拋物線過渡域的線性軌跡（LSPB），即拋物線過渡插值算法，手臂運(yùn)行可分為3個(gè)過程：加速、勻速和減速，假設(shè)初始位置為θi，初始加速度為，手臂最大速度為，終止時(shí)加速度為，終止位置為θf。在［0，ta］時(shí)間段手臂處于加速過程，加速度為，加速時(shí)間為ta；在［ta，tf-td］時(shí)間段手臂各關(guān)節(jié)處于勻速過程，速度為，勻速運(yùn)行時(shí)間為tu；在［tf-td，tf］時(shí)間段內(nèi)手臂各關(guān)節(jié)減速運(yùn)行直至停止，減速時(shí)間為td。

▲圖2 梯形插值V-T圖

根據(jù)拋物線過渡運(yùn)行特點(diǎn)可得：

由式（1）得手臂插值函數(shù)為：

為保證各關(guān)節(jié)同時(shí)啟停，對(duì)各關(guān)節(jié)的運(yùn)行基于V-T圖進(jìn)行時(shí)間歸一化處理，其處理過程如圖3所示。

▲圖3 時(shí)間歸一化處理

以懸掛六自由度服務(wù)機(jī)械臂為例，設(shè)：初始位置各關(guān)節(jié)為0°，目標(biāo)位置關(guān)節(jié)角度為 （20，30，40，-26，-15，-23），速度為10°/s，加速度為20°/s2，進(jìn)行實(shí)驗(yàn)。用該算法插值時(shí)，服務(wù)機(jī)械臂各關(guān)節(jié)速度時(shí)間曲線如圖4所示。

由圖4可知，采用該算法能實(shí)現(xiàn)PTP運(yùn)動(dòng)，但在關(guān)節(jié)速度為零或加速度發(fā)生突變時(shí)，電機(jī)的速度時(shí)間曲線有一定幅度的振蕩，振蕩主要由加速度突變引起。

▲圖4 拋物線插值V-T圖

2.2 三次多項(xiàng)式插值算法

為克服加速度突變導(dǎo)致的電機(jī)運(yùn)動(dòng)振蕩，采用了三次多項(xiàng)式插值算法來實(shí)現(xiàn)關(guān)節(jié)空間的軌跡規(guī)劃。設(shè)

關(guān)節(jié)空間運(yùn)動(dòng)的三次多項(xiàng)式為：

由于關(guān)節(jié)運(yùn)動(dòng)的目標(biāo)位置和運(yùn)動(dòng)時(shí)間tf由指令系統(tǒng)指定，并且起始點(diǎn)和終止點(diǎn)速度為零，因此可得4個(gè)約束方程。設(shè)起始點(diǎn)位置為θ0、終止點(diǎn)位置為θf，則約束方程組為：

求解上述線性方程組可得：

各組關(guān)節(jié)根據(jù)式（5）求出三次多項(xiàng)式因子，然后按照式（3）進(jìn)行關(guān)節(jié)插值運(yùn)動(dòng)。需要注意的是在完成插值運(yùn)動(dòng)后要用軟件限幅濾波法濾波，以保證關(guān)節(jié)運(yùn)動(dòng)在允許的范圍內(nèi)。

以懸掛六自由度服務(wù)機(jī)械臂為例進(jìn)行實(shí)驗(yàn)，讓其從初始位置（0，0，0，0，0，0）運(yùn)動(dòng)到（30，25，60，42，-34，-28），運(yùn)動(dòng)時(shí)間為5 s。用該算法實(shí)驗(yàn)時(shí)，服務(wù)機(jī)械臂各關(guān)節(jié)速度時(shí)間曲線如圖5所示?？梢?，在采用三次多項(xiàng)式插值算法的情況下，服務(wù)機(jī)械臂能實(shí)現(xiàn)PTP運(yùn)動(dòng)，而且運(yùn)動(dòng)更加平穩(wěn)，振蕩明顯減弱。

▲圖5 三次多項(xiàng)式插值V-T圖

2.3 過路徑點(diǎn)連續(xù)三次多項(xiàng)式插值算法

為實(shí)現(xiàn)關(guān)節(jié)空間的過路徑點(diǎn)連續(xù)運(yùn)動(dòng)，筆者設(shè)計(jì)了過路徑點(diǎn)連續(xù)三次多項(xiàng)式插值算法。算法的思想是在多組關(guān)節(jié)目標(biāo)位置和各目標(biāo)位置運(yùn)動(dòng)之間的時(shí)間值由指令系統(tǒng)指定后，采用啟發(fā)模式設(shè)定關(guān)節(jié)各路徑點(diǎn)的速度，再按照三次多項(xiàng)式插值算法完成插值運(yùn)算，最后采用限幅濾波法保證其在關(guān)節(jié)工作范圍內(nèi)。

采用啟發(fā)模式設(shè)定路徑點(diǎn)速度，如圖6所示。圖中，θ0為起始點(diǎn)，θD為終止點(diǎn)，θA、θB和θC為路徑點(diǎn)。假設(shè)用直線段把這些路徑點(diǎn)依次連接起來，如果相鄰線段的斜率在路徑點(diǎn)處改變符號(hào)，則把速度設(shè)定為零；如果相鄰線段不改變符號(hào)，則選取路徑點(diǎn)兩側(cè)線段斜率的平均值作為該點(diǎn)的速度。因此，根據(jù)指令，系統(tǒng)就能夠按此規(guī)則自動(dòng)生成相應(yīng)的路徑點(diǎn)速度。

▲圖6 路徑點(diǎn)速度啟發(fā)原理圖

通過速度啟發(fā)模式獲得路徑點(diǎn)的速度后，過路徑點(diǎn)連續(xù)三次多項(xiàng)式插值問題轉(zhuǎn)換為已知起始點(diǎn)位置和速度、終止點(diǎn)位置和速度、運(yùn)動(dòng)時(shí)間的三次多項(xiàng)式插值問題。設(shè)起點(diǎn)位置為θ0，起點(diǎn)速度為，終止點(diǎn)位置為θf，終止點(diǎn)速度為，起點(diǎn)運(yùn)動(dòng)到終點(diǎn)的運(yùn)動(dòng)時(shí)間為tf。

則根據(jù)約束條件可得：

解方程組（6）得：

由式（7）求得三次插值多項(xiàng)式，并結(jié)合三次多項(xiàng)式式（6）循環(huán)完成過路徑點(diǎn)的連續(xù)插值，最后利用限幅濾波法進(jìn)行濾波。

以懸掛六自由度服務(wù)機(jī)械臂為例進(jìn)行實(shí)驗(yàn)，初始位置為（0，0，0，0，0，0），第一個(gè)路徑點(diǎn)為（20，30，40，23，-34，-28），時(shí)間是5 s，第二個(gè)路徑點(diǎn)為（35，23，46，31，0，13），時(shí)間是5 s，第三個(gè)路徑點(diǎn)為（41，21，52，20，15，26），時(shí)間是4s，終 止 點(diǎn) 為（60，31，62，31，36，30），時(shí)間是3 s。用該算法進(jìn)行軌跡規(guī)劃時(shí)，服務(wù)機(jī)械臂各關(guān)節(jié)速度時(shí)間曲線如圖7所示。說明：由于模塊返回速度信息不具有方向性，因此圖7中部分關(guān)節(jié)速度曲線發(fā)生了翻折。

▲圖7 過路徑點(diǎn)三次多項(xiàng)式插值V-T圖

由圖7可知，過路徑點(diǎn)三次多項(xiàng)式插值算法，能實(shí)現(xiàn)服務(wù)機(jī)械臂關(guān)節(jié)空間的過路徑點(diǎn)連續(xù)軌跡運(yùn)動(dòng)，但由于路徑點(diǎn)速度、加速度不連續(xù)，導(dǎo)致路徑點(diǎn)連接處的振蕩幅度偏大。

2.4 過路徑點(diǎn)五次多項(xiàng)式實(shí)現(xiàn)過路徑點(diǎn)連續(xù)運(yùn)動(dòng)

為克服路徑點(diǎn)連接處振蕩較大問題，采用過路徑點(diǎn)五次多項(xiàng)式實(shí)現(xiàn)過路徑點(diǎn)連續(xù)運(yùn)動(dòng)。算法采用速度和加速度啟發(fā)模式，設(shè)定各個(gè)路徑點(diǎn)關(guān)節(jié)的速度和加速度，啟發(fā)規(guī)則與方式，如圖6所示。完成速度和加速啟發(fā)后，問題轉(zhuǎn)化為：已知起始點(diǎn)和終止點(diǎn)的位置、速度、加速度及運(yùn)動(dòng)時(shí)間的三次多項(xiàng)式插值問題。設(shè)起始點(diǎn)位置為θ0，起始點(diǎn)速度為，起始點(diǎn)的加速度為，終止點(diǎn)位置θf，終止點(diǎn)速度為，終止點(diǎn)的加速度為，運(yùn)動(dòng)時(shí)間為tf。設(shè)關(guān)節(jié)五次插值多項(xiàng)式為：

根據(jù)式（10），求解出各路徑點(diǎn)五次多項(xiàng)式參數(shù)進(jìn)行插值，再利用限幅濾波法進(jìn)行濾波。

以懸掛六自由度服務(wù)機(jī)械臂為例進(jìn)行試驗(yàn)，初始位置為（0，0，0，0，0，0），第一個(gè)路徑點(diǎn)為（20，30，40，23，-34，-28）、時(shí)間是5 s，第二個(gè)路徑點(diǎn)為（35，23，46，31，0，13）、時(shí)間是5 s，第三個(gè)路徑點(diǎn)為（41，21，52，20，15，26）、時(shí) 間是 4 s，終止點(diǎn)為（60，31，62，31，36，30）、時(shí)間是3 s。用該算法時(shí)，服務(wù)機(jī)械臂各關(guān)節(jié)速度時(shí)間曲線如圖8所示。

▲圖8 過路徑點(diǎn)五次多項(xiàng)式插值V-T圖

由圖8可知，過路徑點(diǎn)五次多項(xiàng)式插值算法能實(shí)現(xiàn)過路徑點(diǎn)連續(xù)運(yùn)動(dòng)，而且路徑點(diǎn)連接處速度、加速度連續(xù)，整個(gè)運(yùn)動(dòng)過程振蕩極小。

2.5 直角坐標(biāo)路徑控制法實(shí)現(xiàn)直線運(yùn)動(dòng)

為實(shí)現(xiàn)服務(wù)機(jī)械臂末端的直線運(yùn)動(dòng)，軌跡規(guī)劃器實(shí)現(xiàn)了直角坐標(biāo)路徑控制插值算法，采用拋物線過渡離散規(guī)則，根據(jù)指令系統(tǒng)傳入目標(biāo)位姿、位置運(yùn)動(dòng)速度、姿態(tài)運(yùn)動(dòng)速度、運(yùn)動(dòng)時(shí)間，利用四元數(shù)進(jìn)行位姿離散。設(shè)當(dāng)前位姿為P0，目標(biāo)位姿為P1，位置直線運(yùn)動(dòng)速度為V1，運(yùn)動(dòng)時(shí)間為T，則：

（1）根據(jù)當(dāng)前位置P0=（x0，y0，z0）、目標(biāo)位置P1=（x1，y1，z1）、直線運(yùn)動(dòng)速度V1和運(yùn)動(dòng)時(shí)間T，按照拋物線過渡運(yùn)動(dòng)，計(jì)算出從初始位姿P0直線插值運(yùn)動(dòng)到P1的位置直線運(yùn)動(dòng)加速度A1：

（2）由姿態(tài)矩陣R0和目標(biāo)姿態(tài)矩陣R1，計(jì)算出兩姿態(tài)矩陣對(duì)應(yīng)的四元數(shù)，然后利用四元數(shù)的共軛性質(zhì)求解出，利用四元數(shù)乘法性質(zhì)計(jì)算出工具姿態(tài)由R0轉(zhuǎn)為R1的姿態(tài)旋轉(zhuǎn)矩陣R為：

最后將姿態(tài)旋轉(zhuǎn)矩陣R，利用旋轉(zhuǎn)變換式（13），計(jì)算出等效旋轉(zhuǎn)軸n的等效旋轉(zhuǎn)角度θ。

（3）由上步計(jì)算得等效旋轉(zhuǎn)角度θ，根據(jù)拋物線過

渡運(yùn)動(dòng)，計(jì)算出姿態(tài)運(yùn)動(dòng)速度V2和加速度A2，即：

（4）根據(jù)計(jì)算出來的拋物線過渡參數(shù)，采用拋物線過渡法計(jì)算離散位姿。

（5）用運(yùn)動(dòng)學(xué)求解器求解出各離散位姿對(duì)應(yīng)的關(guān)節(jié)位置。

（6）采用過路徑點(diǎn)五次多項(xiàng)式插值算法，控制服務(wù)機(jī)械臂和仿真模型按離散關(guān)節(jié)位置實(shí)現(xiàn)過路徑點(diǎn)的連續(xù)運(yùn)動(dòng)，實(shí)現(xiàn)直線運(yùn)動(dòng)軌跡。

以懸掛六自由度服務(wù)機(jī)械臂為例進(jìn)行實(shí)驗(yàn)，設(shè)：初始位姿為 （-190，25，-500，0，180，0），末端位姿為（-70，25，-500，0，120，0），位置運(yùn)動(dòng)速度為20 mm/s，運(yùn)動(dòng)時(shí)間為8 s，用該算法規(guī)劃時(shí)，服務(wù)機(jī)械臂末端運(yùn)動(dòng)軌跡如圖9所示，與理想位置比較所得偏差如圖10所示。

由圖10可以看出，各采樣點(diǎn)3個(gè)方向的目標(biāo)位置與理想位置的偏差都在2.5 mm范圍內(nèi)，則直線位置最大偏差為4.33 mm。由圖9可知，服務(wù)機(jī)械臂走出一條較精準(zhǔn)直線，但該算法無法顯示設(shè)定直線精度。

▲圖9 直角坐標(biāo)控制法末端軌跡

▲圖10 末端軌跡偏差

3 基于BDJP法軌跡規(guī)劃

3.1 BDJP法實(shí)現(xiàn)直線運(yùn)動(dòng)

為克服直線精度無法設(shè)定問題，筆者采用了BDJP（Bounded Deviation Joint Path）法實(shí)現(xiàn)直線插值。設(shè)：起始位姿為P0，末端位姿為P1，運(yùn)動(dòng)時(shí)間為T，位置最大偏差為δp，旋轉(zhuǎn)最大偏差為δR，其具體步驟如下。

（1）計(jì)算關(guān)節(jié)變量解，計(jì)算對(duì)應(yīng)于P0和P1的關(guān)節(jié)矢量q0和q1。

（2）求出關(guān)節(jié)空間的中間點(diǎn)，計(jì)算關(guān)節(jié)變量空間的中間點(diǎn)：

式（16）中，Δq1=q1-q0，再由qm計(jì)算相應(yīng)的工具坐標(biāo)系Pm。

（3）求出直角坐標(biāo)空間的中點(diǎn)，計(jì)算相應(yīng)的直角坐標(biāo)路徑的中點(diǎn)PC：

（4）求出偏離誤差，計(jì)算Pm和PC之間的偏差：

（6）根據(jù)總時(shí)間T和插值最大位移，將總時(shí)間進(jìn)行細(xì)分。

（7）然后按照過路徑點(diǎn)五次多項(xiàng)式插值算法，進(jìn)行關(guān)節(jié)插值控制。

以懸掛六自由度服務(wù)機(jī)械臂為例進(jìn)行實(shí)驗(yàn)，設(shè)：初始位姿為 （-190，25，-500，0，180，0），末端位姿為（-70，25，-500，0，120，0），位置最大偏差為2 mm，最大旋轉(zhuǎn)偏差為3°，運(yùn)動(dòng)時(shí)間為10 s，用該算法規(guī)劃時(shí)，服務(wù)機(jī)械臂末端運(yùn)動(dòng)軌跡如圖11所示，與理想位置比較所得偏差如圖12所示。

由圖12可知，采用BDJP法實(shí)現(xiàn)軌跡規(guī)劃時(shí)，各采樣點(diǎn)3個(gè)方向的目標(biāo)位置與理想位置的偏差都在1.1 mm范圍內(nèi)，則直線位置最大偏差為1.91 mm，滿足2 mm的設(shè)定要求。由圖11可知，該算法能夠?qū)崿F(xiàn)可控制精度的直線插值。

3.2 BDJP法實(shí)現(xiàn)圓弧運(yùn)動(dòng)

在BDJP直線插補(bǔ)算法的基礎(chǔ)上，實(shí)現(xiàn)末端軌跡圓弧插值，基本思想是首先根據(jù)指令系統(tǒng)指定的圓弧中間點(diǎn)位姿、圓弧終點(diǎn)位姿計(jì)算出圓弧中心和圓弧半徑；然后再根據(jù)最大位置精度計(jì)算出細(xì)分弧，如圖13所示，獲得細(xì)分點(diǎn)位姿；然后計(jì)算出圓弧在精度內(nèi)細(xì)分

的各個(gè)點(diǎn)，再按照BDJP法直線插值算法代替圓弧進(jìn)行插值運(yùn)動(dòng)控制。

以懸掛六自由度服務(wù)機(jī)械臂為例，設(shè)：圓弧初始位姿為（-20，0，-750，0，180，0），中間點(diǎn)位姿為（-120，-100，-750，0，180，0），圓弧終點(diǎn)位姿為（-220，0，-750，0，180，0），位置最大偏差為2 mm，旋轉(zhuǎn)最大偏差為3°，T為13 s，用該算法規(guī)劃時(shí)，服務(wù)機(jī)械臂末端運(yùn)動(dòng)軌跡如圖14所示，與理想位置比較所得偏差如圖15所示。

由圖14可知，服務(wù)機(jī)械臂按照該算法能夠?qū)崿F(xiàn)圓弧軌跡插值運(yùn)動(dòng)，而且由圖15可知，各方向最大偏差值都小于1.5 mm，圓弧位置偏差最大小于2 mm，滿足算法設(shè)定精度。

▲圖11 BDJP法末端軌跡

▲圖12 末端軌跡偏差

▲圖13 圓弧細(xì)分作直線處理方法

▲圖14 圓弧插值實(shí)驗(yàn)?zāi)┒塑壽E

▲圖15 末端軌跡偏差

4 機(jī)械臂軌跡規(guī)劃實(shí)驗(yàn)

該平臺(tái)已完成多種配置模塊化服務(wù)機(jī)械臂的同步在線控制測(cè)試，以豎直六自由度服務(wù)機(jī)械臂進(jìn)行在線模型控制實(shí)驗(yàn)為例，其測(cè)試如圖16所示。

為測(cè)定BDJP直線插補(bǔ)算法，用直線指令LINE，設(shè)：位置插值誤差為5 mm，旋轉(zhuǎn)插值誤差為1°，編寫指令文件，完成服務(wù)機(jī)械臂走“中”字的實(shí)驗(yàn)，如圖17所示。

為驗(yàn)證BDJP圓弧插補(bǔ)算法，用圓弧指令CIR3P指令，設(shè)：位置偏差為5 mm，旋轉(zhuǎn)偏差為1°，編寫指令文件，完成懸掛六自由度服務(wù)機(jī)械臂走圓實(shí)驗(yàn)，如圖18所示。

經(jīng)過多次實(shí)驗(yàn)，算法的穩(wěn)定性、可靠性得到了檢驗(yàn)，平臺(tái)的功能和性能也得到了完善和提高，驗(yàn)證了仿真控制平臺(tái)在模塊化服務(wù)機(jī)器人的模型構(gòu)建、仿真控制、在線同步控制以及軌跡規(guī)劃、指令控制等方面的功能。

▲圖16 豎直六自由度機(jī)械臂同步在線測(cè)試

5 結(jié)論

本文以Schunk模塊化服務(wù)機(jī)器人為研究對(duì)象，基于BDJP插值算法完成了服務(wù)機(jī)器人的軌跡規(guī)劃工作，完成模塊化服務(wù)機(jī)器人仿真控制平臺(tái)的搭建。在該仿真平臺(tái)上，完成了在線同步測(cè)試、懸掛六自由度服務(wù)機(jī)械臂走 “中”字和走圓實(shí)驗(yàn)，驗(yàn)證了平臺(tái)的功能和性能。

作為模塊化機(jī)器人仿真控制平臺(tái)，本文的研究工作在實(shí)時(shí)性、場(chǎng)景精細(xì)等方面還存在較多不足，整個(gè)仿真控制軟件主要從服務(wù)機(jī)械臂的建模和運(yùn)動(dòng)控制出發(fā)，未考慮服務(wù)機(jī)械臂的工作環(huán)境、手臂受

力情況等因素，下一步將考慮把增加服務(wù)機(jī)械臂作業(yè)環(huán)境的建模作為后續(xù)研究?jī)?nèi)容，并考慮把實(shí)現(xiàn)服務(wù)機(jī)械臂運(yùn)動(dòng)過程中各模塊受力情況的仿真分析作為研究重點(diǎn)。另外，設(shè)計(jì)的服務(wù)機(jī)械臂運(yùn)動(dòng)學(xué)求解器還具有一定的局限性，不能夠適應(yīng)建立的所有模型。下一步將考慮從服務(wù)機(jī)械臂機(jī)構(gòu)學(xué)的角度分析其運(yùn)動(dòng)學(xué)求解過程，建立具有普遍適應(yīng)性的統(tǒng)一運(yùn)動(dòng)學(xué)求解算法。最后，設(shè)計(jì)的軌跡規(guī)劃器實(shí)現(xiàn)的軌跡規(guī)劃算法還非常有限，下一步將考慮研究具有避障、指定任務(wù)軌跡自動(dòng)生成等更加高級(jí)的軌跡規(guī)劃算法。

▲圖17 走"中"字實(shí)驗(yàn)

▲圖18 走圓實(shí)驗(yàn)

［1］姚玉峰，蘇衍宇，黃博.面向服務(wù)機(jī)械臂有效操作特征提取的示教方法［J］.機(jī)械工程學(xué)報(bào)，2013，49（3）：10-17.

［2］高美真.OpenGL在基于圖形建模技術(shù)中的應(yīng)用［J］.現(xiàn)代計(jì)算機(jī)，2005（12）：92-94.

［3］伍文偉，朱志杰，伍良偉.一種基于Lab Windows/CVI和OpenGL的六自由度機(jī)械手三維建模的方法［J］.電子測(cè)量技術(shù)，2007，30（8）：75-77.

［4］李憲華，郭永存，郭帥，等.基于CAN總線結(jié)構(gòu)的服務(wù)機(jī)器人模塊化雙臂系統(tǒng)構(gòu)建 ［J］.機(jī)床與液壓，2013，41（17）：22-23.

［5］趙建文，杜志江，孫立寧. 7自由度冗余手臂的自運(yùn)動(dòng)流形［J］.機(jī)械工程學(xué)報(bào)，2007，43（9）：132-137.

［6］封文春，林貴平.四元數(shù)在彈射座椅性能仿真中的應(yīng)用［J］.北京航空航天大學(xué)學(xué)報(bào)，2006，32（8）：881-884.

［7］李憲華，郭永存，張軍，等.模塊化六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)解算與驗(yàn)證［J］.農(nóng)業(yè)機(jī)械學(xué)報(bào)，2013，44（4）：246-251.

［8］孫亮，馬江，阮曉鋼.六自由度機(jī)械臂軌跡規(guī)劃與仿真研究［J］.控制工程，2010，17（3）：388-392.

［9］熊有倫.機(jī)器人技術(shù)基礎(chǔ)［M］.武漢：華中科技大學(xué)出版社，2008.

［10］張小江，高秀華.三次樣條插值在機(jī)器人軌跡規(guī)劃應(yīng)用中的改進(jìn)研究［J］.機(jī)械設(shè)計(jì)與制造，2012（9）：170-171.

［11］劉鵬，宋濤，贠超，等.焊接機(jī)器人運(yùn)動(dòng)學(xué)分析及軌跡規(guī)劃研究［J］.機(jī)電工程，2013，30（4）：390-394.

［12］Kubota N，Arakawa T.Trajectory Generation for Redundant Manipulator Using Virus Evolutionary Genetic Algorithm.［C］Robotics and Automation，1997 Proceedings，IEEE International Conference,Albuquerque USA，1997.

［13］劉鵬飛，楊孟興，宋科，等.‘S’型加減速曲線在機(jī)器人軌跡插補(bǔ)算法中的應(yīng)用研究［J］.制造業(yè)自動(dòng)化，2012，（20）：4-11.

［14］R H Taylor.The Planning and Execution of Straight Line Manipulator Trajectories［J］.IBM Journal of Research and Develapment，1979，23（4）：424-426.

［15］欒海英，劉學(xué)君.基于CANopen協(xié)議絕對(duì)值光電編碼器的伺服位置控制的研究［J］.制造業(yè)自動(dòng)化，2013,35（9）：151-153.

［16］卓揚(yáng)娃，白曉燦，陳永明.機(jī)器人的三種規(guī)則曲線插補(bǔ)算法［J］.裝備制造技術(shù)，2009（11）：27-29.

（編輯 丁 罡）

TH122；TP242.6

A

1000-4998（2015）10-0011-07

*上海市科學(xué)技術(shù)委員會(huì)科技支撐項(xiàng)目（編號(hào)：14111104502）

2015年3月