基于關(guān)節(jié)空間軌跡規(guī)劃的機(jī)械臂遠(yuǎn)程控制

張 毅，游群霞，羅 元，周揮宇

（重慶郵電大學(xué)國(guó)家信息無障礙工程研發(fā)中心智能系統(tǒng)及機(jī)器人研究所，重慶 400065）

0 引言

在信息產(chǎn)業(yè)高度融合與快速發(fā)展的今天，機(jī)器人的遠(yuǎn)程控制技術(shù)為人們進(jìn)一步探索和改變世界帶來了極大的方便，機(jī)器人遠(yuǎn)程控制系統(tǒng)可以被應(yīng)用于危險(xiǎn)環(huán)境下的遠(yuǎn)程作業(yè)、遠(yuǎn)程醫(yī)療、遠(yuǎn)程教學(xué)、遠(yuǎn)程監(jiān)護(hù)以及傳統(tǒng)生產(chǎn)模式的改造等眾多方面，具有廣闊的應(yīng)用前景［1］。

隨著控制技術(shù)和網(wǎng)絡(luò)技術(shù)的不斷發(fā)展，人類對(duì)機(jī)器人遠(yuǎn)程控制技術(shù)的要求越來越高［2］。機(jī)械臂遠(yuǎn)程控制是建立在一個(gè)復(fù)雜控制環(huán)境基礎(chǔ)上的控制技術(shù)，要實(shí)現(xiàn)可靠的遠(yuǎn)程控制有很多問題需要解決，其中最重要的就是實(shí)時(shí)性和穩(wěn)定性。多數(shù)研究者對(duì)遠(yuǎn)程控制的實(shí)時(shí)性進(jìn)行了多方面的研究［3-4］，采用網(wǎng)絡(luò)建模、從控制的角度來分析和補(bǔ)償網(wǎng)絡(luò)傳輸延時(shí)等方法來彌補(bǔ)遠(yuǎn)程控制的實(shí)時(shí)性。對(duì)于穩(wěn)定性問題，可以采取可靠的網(wǎng)絡(luò)傳輸協(xié)議保障遠(yuǎn)程控制的穩(wěn)定性，在此基礎(chǔ)上，可從機(jī)械臂結(jié)構(gòu)本身出發(fā)以增強(qiáng)遠(yuǎn)程控制系統(tǒng)的穩(wěn)定性，即對(duì)機(jī)械臂運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃，使機(jī)械臂運(yùn)動(dòng)連續(xù)、平穩(wěn)、無抖動(dòng)。軌跡規(guī)劃既可在笛卡爾空間也可以在關(guān)節(jié)空間中進(jìn)行，鄭樺、叢爽等［5］研究者把基于笛卡爾空間的連續(xù)軌跡（continuous path，CP）控制方式引入了機(jī)械臂的遠(yuǎn)程控制中，CP控制比較復(fù)雜，涉及矩陣求逆等復(fù)雜運(yùn)算，需要通過各種插補(bǔ)運(yùn)算才能實(shí)現(xiàn)軌跡插補(bǔ)。本文中我們把運(yùn)算簡(jiǎn)化的關(guān)節(jié)空間軌跡插補(bǔ)控制方式引入了機(jī)械臂遠(yuǎn)程控制系統(tǒng)中，并對(duì)其進(jìn)行了研究和分析。

在多自由度串聯(lián)機(jī)器臂的遠(yuǎn)程控制系統(tǒng)中，傳統(tǒng)的遠(yuǎn)程控制方式是直接向機(jī)器臂發(fā)送控制指令和指令參數(shù)來控制機(jī)械臂的運(yùn)動(dòng)。機(jī)械臂是一個(gè)多輸入多輸出、不穩(wěn)定、時(shí)變耦合的非線性動(dòng)態(tài)系統(tǒng)［5］，由于機(jī)械臂結(jié)構(gòu)的限制，直接向機(jī)械臂發(fā)送未軌跡規(guī)劃的控制指令，容易造成機(jī)械臂運(yùn)動(dòng)的不連續(xù)和不平滑，同時(shí)機(jī)器關(guān)節(jié)由于機(jī)械慣性會(huì)產(chǎn)生抖動(dòng)。為了克服這些問題，提高系統(tǒng)的穩(wěn)定性，本文對(duì)遠(yuǎn)程機(jī)械臂的運(yùn)動(dòng)軌跡進(jìn)行了基于關(guān)節(jié)空間的軌跡規(guī)劃，選取了結(jié)構(gòu)簡(jiǎn)單，計(jì)算量小的5次多項(xiàng)式插值函數(shù)對(duì)四自由度串聯(lián)機(jī)械臂的運(yùn)動(dòng)軌跡進(jìn)行軌跡插補(bǔ)，該插值函數(shù)能夠?qū)崟r(shí)計(jì)算機(jī)械臂各關(guān)節(jié)的運(yùn)動(dòng)參數(shù)，生成運(yùn)動(dòng)軌跡，可以確保各關(guān)節(jié)運(yùn)動(dòng)時(shí)的連續(xù)性和平滑性［6］，并提高軌跡規(guī)劃的實(shí)時(shí)性。把規(guī)劃后的控制指令通過遠(yuǎn)端客戶機(jī)傳送給本地服務(wù)器，可以使機(jī)械臂按照規(guī)劃后的軌跡，連續(xù)、光滑、無抖動(dòng)的運(yùn)動(dòng)，達(dá)到了連續(xù)、平穩(wěn)、可靠地控制遠(yuǎn)程機(jī)械臂的工作要求，提高了機(jī)械臂遠(yuǎn)程控制系統(tǒng)的穩(wěn)定性。

1 機(jī)械臂遠(yuǎn)程控制系統(tǒng)

本文中我們以四自由度串聯(lián)機(jī)械臂作為被控對(duì)象進(jìn)行實(shí)際的遠(yuǎn)程控制，機(jī)械臂控制系統(tǒng)實(shí)物圖如圖1所示。

圖1 機(jī)械臂控制系統(tǒng)實(shí)物圖Fig.1 Robot arm control system actual picture

本系統(tǒng)遠(yuǎn)程控制采用軟件實(shí)現(xiàn)的方式，遠(yuǎn)程控制機(jī)械臂采取常用的C/S（client/server）模式，遠(yuǎn)程控制計(jì)算機(jī)作為客戶端，主要完成控制器的功能，本地計(jì)算機(jī)作為服務(wù)器，主要完成執(zhí)行器的功能，四自由度串聯(lián)機(jī)械臂系統(tǒng)與本地服務(wù)器相連。系統(tǒng)采用了多模塊化思想進(jìn)行設(shè)計(jì)，構(gòu)建了基于Internet的機(jī)械臂遠(yuǎn)程控制系統(tǒng)，其結(jié)構(gòu)框圖如圖2所示。系統(tǒng)使用基于Winsock的進(jìn)程通信方法實(shí)現(xiàn)客戶端與服務(wù)器之間的數(shù)據(jù)連接與交換，為了增強(qiáng)系統(tǒng)的穩(wěn)定性，數(shù)據(jù)傳輸采用TCP（transmission control protocol）協(xié)議，TCP協(xié)議是面向連接的，提供無差錯(cuò)、無重復(fù)的順序數(shù)據(jù)傳輸?shù)目煽總鬏斂刂茀f(xié)議，TCP協(xié)議的主要特點(diǎn)是可靠性很高。

圖2 遠(yuǎn)程控制系統(tǒng)結(jié)構(gòu)框圖Fig.2 Romote control system strucrure diagram

客戶端用戶從遠(yuǎn)端通過網(wǎng)絡(luò)訪問本地服務(wù)器，在連接建立以后，服務(wù)器端便開始向連接到本地服務(wù)器的遠(yuǎn)端客戶提供服務(wù)，首先接受客戶端通過網(wǎng)絡(luò)發(fā)出的命令，然后根據(jù)該命令控制本地機(jī)械臂運(yùn)動(dòng)，最后將運(yùn)動(dòng)的結(jié)果以及一些現(xiàn)場(chǎng)相關(guān)信息傳回給遠(yuǎn)端用戶，從而實(shí)現(xiàn)遠(yuǎn)端客戶通過網(wǎng)絡(luò)對(duì)機(jī)械臂的控制。

2 關(guān)節(jié)空間軌跡規(guī)劃

軌跡規(guī)劃既可在關(guān)節(jié)空間也可以在笛卡爾空間中進(jìn)行，但是要求所規(guī)劃的軌跡函數(shù)必須連續(xù)且平滑，使得機(jī)器人操作臂運(yùn)動(dòng)平穩(wěn)［7］。關(guān)節(jié)空間軌跡規(guī)劃是以關(guān)節(jié)角度函數(shù)來描述機(jī)器人軌跡，并進(jìn)行軌跡規(guī)劃，無需在笛卡爾坐標(biāo)系描述兩個(gè)路徑點(diǎn)之間的路徑形狀，計(jì)算簡(jiǎn)單、容易，不會(huì)發(fā)生機(jī)構(gòu)奇異性的問題。在關(guān)節(jié)空間中進(jìn)行軌跡規(guī)劃，是將關(guān)節(jié)變量映射成時(shí)間的函數(shù)，并規(guī)劃它的一階和二階時(shí)間導(dǎo)數(shù)，軌跡規(guī)劃的主要任務(wù)是選擇合理的多項(xiàng)式函數(shù)和其他線性函數(shù)進(jìn)行插補(bǔ)運(yùn)算，使關(guān)節(jié)運(yùn)動(dòng)平滑、穩(wěn)定，始終保持在關(guān)節(jié)運(yùn)動(dòng)容許的范圍內(nèi)［8］。

針對(duì)機(jī)械臂的運(yùn)動(dòng)特點(diǎn)，我們直接用機(jī)械臂運(yùn)動(dòng)時(shí)的關(guān)節(jié)變量來規(guī)劃軌跡，其優(yōu)點(diǎn)是軌跡規(guī)劃可近似實(shí)時(shí)地進(jìn)行，關(guān)節(jié)軌跡易于規(guī)劃。在機(jī)械臂運(yùn)動(dòng)過程中，已知機(jī)械臂在在t0=0時(shí)刻的某關(guān)節(jié)角為θ0，tn時(shí)刻的值是終止關(guān)節(jié)角度θn。其軌跡的描述由起始點(diǎn)關(guān)節(jié)角度與終止點(diǎn)關(guān)節(jié)角度的一個(gè)平滑插值函數(shù)θ（t）來表示，對(duì)于每個(gè)關(guān)節(jié)的平滑函數(shù)而言，為了實(shí)現(xiàn)關(guān)節(jié)的平穩(wěn)運(yùn)動(dòng)，有6個(gè)約束條件:兩端點(diǎn)的位置約束、兩端點(diǎn)的速度約束和兩端點(diǎn)的加速度約束。端點(diǎn)的位置約束是指起始位姿和終止位姿各自所對(duì)應(yīng)的關(guān)節(jié)角度，即

為了滿足關(guān)節(jié)運(yùn)動(dòng)速度的連續(xù)性要求，假設(shè)機(jī)器人初始點(diǎn)和終止點(diǎn)的關(guān)節(jié)角速度為零，用Vθ（0）和Vθ（tn）表示，即

同時(shí)為了滿足（1）式和（2）式，兩端點(diǎn)的關(guān)節(jié)角加速度用aθ（0）和aθ（tn）表示，滿足

滿足上述任意給定約束條件的平滑函數(shù)有許多，考慮到要在遠(yuǎn)程控制系統(tǒng)中實(shí)現(xiàn)軌跡規(guī)劃，應(yīng)選取計(jì)算簡(jiǎn)化和形式簡(jiǎn)單，可以提高軌跡規(guī)劃實(shí)時(shí)性的插值函數(shù)，實(shí)現(xiàn)快速性要求。綜合以上因素考慮，本文選取了5次多項(xiàng)式插值函數(shù)進(jìn)行軌跡插補(bǔ)，便于實(shí)時(shí)計(jì)算和實(shí)際應(yīng)用。

由（1）－（3）式6個(gè)約束條件可以唯一確定一個(gè)5次多項(xiàng)式

設(shè)該路徑的關(guān)節(jié)角速度為Vθ（t），關(guān)節(jié)角加速度為aθ（t），則

由（1）－（3）式分別代入（4）－（6）式可解得

所以，對(duì)于初速度及終速度為零的關(guān)節(jié)運(yùn)動(dòng)，滿足約束條件的5次多項(xiàng)式插值函數(shù)為

對(duì)于給定約束條件的軌跡，由（8）式的多項(xiàng)式插值函數(shù)可迅速得到，從而避免對(duì)運(yùn)動(dòng)過程中的每一點(diǎn)進(jìn)行實(shí)時(shí)運(yùn)動(dòng)學(xué)逆解的繁雜計(jì)算，提高了運(yùn)算速度與效率，可以有效地提高軌跡規(guī)劃的精確性，實(shí)時(shí)性。

3 軌跡規(guī)劃下的機(jī)械臂遠(yuǎn)程控制

在傳統(tǒng)的機(jī)械臂遠(yuǎn)程控制系統(tǒng)中加入軌跡規(guī)劃模塊，使機(jī)械臂按照規(guī)劃后的軌跡運(yùn)動(dòng)，最終實(shí)現(xiàn)軌跡規(guī)劃下的機(jī)械臂遠(yuǎn)程控制。系統(tǒng)控制軟件中，軌跡規(guī)劃工作放在個(gè)客戶端獨(dú)立處理，系統(tǒng)創(chuàng)建了軌跡規(guī)劃模塊，構(gòu)建了Cplanner規(guī)劃類來負(fù)責(zé)軌跡的規(guī)劃工作，加入軌跡規(guī)劃模塊后的客戶端流程圖［9］如圖3所示。

圖3 客戶端程序流程圖Fig.3 Client program flow diagram

當(dāng)客戶端與服務(wù)器端建立連接后，客戶端主線程開啟后向指定的服務(wù)器發(fā)送控制機(jī)械臂的請(qǐng)求，請(qǐng)求獲得允許后初始化，等待用戶的軌跡規(guī)劃。有任務(wù)時(shí)由軌跡規(guī)劃模塊生成軌跡點(diǎn)指令序列，以事件觸發(fā)的方式按順序交由網(wǎng)絡(luò)傳輸模塊發(fā)送至服務(wù)器，服務(wù)器端開啟網(wǎng)絡(luò)控制后循環(huán)等待客戶機(jī)發(fā)送網(wǎng)絡(luò)字節(jié)形式的參數(shù)指令序列，并翻譯成本地指令，轉(zhuǎn)化成機(jī)械臂控制字以實(shí)現(xiàn)對(duì)機(jī)械臂的控制，使機(jī)械臂按照規(guī)劃后的軌跡，從起始點(diǎn)開始，依次光滑、連續(xù)、平穩(wěn)地通過所有路徑點(diǎn)，最后到達(dá)目標(biāo)位置。

4 實(shí)驗(yàn)及分析

機(jī)械臂控制系統(tǒng)通過串口與服務(wù)器連接，機(jī)械臂的關(guān)節(jié)都是由舵機(jī)驅(qū)動(dòng)的，表1是機(jī)械臂舵機(jī)控制的通信協(xié)議。服務(wù)器接受到客戶端傳來的各關(guān)節(jié)的控制指令后，利用其強(qiáng)大的處理能力把各關(guān)節(jié)的參數(shù)信息轉(zhuǎn)換成控制字，通過串口向AVR（automatic voltage regulator）單片機(jī)發(fā)送控制字命令，單片機(jī)程序初始化后，用查詢的方式從串口接收數(shù)據(jù)，當(dāng)接收到數(shù)據(jù)后，計(jì)算PWM（pulse width modulation）脈寬，控制舵機(jī)的轉(zhuǎn)動(dòng)角度和速度，最終實(shí)現(xiàn)對(duì)機(jī)械臂各關(guān)節(jié)的控制。

表1 舵機(jī)控制協(xié)議格式Tab.1 Steering engine control protocol format

為了驗(yàn)證遠(yuǎn)程機(jī)械臂加入軌跡規(guī)劃模塊以后運(yùn)動(dòng)的連續(xù)性和平穩(wěn)性，給定機(jī)械臂起始點(diǎn)和目標(biāo)點(diǎn)，對(duì)同一任務(wù)采用兩種控制方式（即傳統(tǒng)的機(jī)械臂遠(yuǎn)程控制方式和軌跡規(guī)劃后的機(jī)械臂遠(yuǎn)程控制方式）實(shí)現(xiàn)對(duì)機(jī)械臂的遠(yuǎn)程控制。從服務(wù)器端獲取2種控制方式的各個(gè)關(guān)節(jié)的運(yùn)動(dòng)參數(shù)的數(shù)據(jù)信息，將其繪制成曲線，得到的軌跡規(guī)劃前和軌跡規(guī)劃后的各關(guān)節(jié)的角度和速度變化曲線對(duì)比圖如圖4所示。圖4中:1為軌跡規(guī)劃前角度曲線;2為規(guī)劃后角度曲線;3為軌跡規(guī)劃前速度曲線;4為規(guī)劃后速度曲線?？梢姡壽E規(guī)劃前各關(guān)節(jié)的角度和速度變化曲線波動(dòng)大，不平滑，給定約束條件對(duì)機(jī)械臂運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃后各關(guān)節(jié)的角度和速度變化曲線是光滑、連續(xù)、平穩(wěn)的。

對(duì)于增加的加速度約束條件，把服務(wù)器端得到的加速度數(shù)據(jù)信息繪制成曲線，得到4個(gè)關(guān)節(jié)的加速度曲線，如圖5所示。圖5中，1－4分別為關(guān)節(jié)1－4的加速度曲線。由圖5可知，各關(guān)節(jié)的加速曲線連續(xù)，表明機(jī)械臂末端執(zhí)行器不會(huì)產(chǎn)生抖動(dòng)。

為了更好的驗(yàn)證傳統(tǒng)的機(jī)械臂遠(yuǎn)程控制系統(tǒng)加入軌跡規(guī)劃模塊以后的有效性，本文對(duì)軌跡規(guī)劃后的機(jī)械臂做了實(shí)際的遠(yuǎn)程控制實(shí)驗(yàn)。在服務(wù)器端把接收到的規(guī)劃后各關(guān)節(jié)的指令序列轉(zhuǎn)換成機(jī)械臂的控制字，通過串口把控制字發(fā)送給AVR單片機(jī)后，使遠(yuǎn)程機(jī)械臂按照規(guī)劃后的軌跡完成運(yùn)動(dòng)，達(dá)到了連續(xù)、平穩(wěn)、無抖動(dòng)的控制機(jī)械臂的工作要求。

5 結(jié)束語

本文中我們?cè)趥鹘y(tǒng)的機(jī)械臂遠(yuǎn)程控制系統(tǒng)中加入軌跡規(guī)劃模塊，采用5次多項(xiàng)式插值函數(shù)對(duì)機(jī)械臂運(yùn)動(dòng)軌跡進(jìn)行軌跡插補(bǔ)，軌跡規(guī)劃過程中，整個(gè)計(jì)算與插補(bǔ)是在關(guān)節(jié)變量空間進(jìn)行的，計(jì)算量小，避免對(duì)運(yùn)動(dòng)過程中的每一點(diǎn)進(jìn)行實(shí)時(shí)運(yùn)動(dòng)學(xué)逆解的繁雜計(jì)算，實(shí)時(shí)性好。軌跡規(guī)劃完成后，使遠(yuǎn)程機(jī)械臂按照規(guī)劃后的軌跡運(yùn)動(dòng)，機(jī)械臂能夠連續(xù)、平穩(wěn)的運(yùn)動(dòng)，克服了機(jī)械臂的抖動(dòng)，有效地提高了機(jī)械臂遠(yuǎn)程控制系統(tǒng)的實(shí)時(shí)性和穩(wěn)定性，提高了系統(tǒng)的使用性，

為遠(yuǎn)程機(jī)械臂的實(shí)際應(yīng)用奠定了基礎(chǔ)。

［1］馮華山.基于Internet的遠(yuǎn)程控制機(jī)器人系統(tǒng)［D］.西安:西安工業(yè)大學(xué)，2004.

FENG Hua-shan.Remote Control Robot System Based on Internet［D］.Xi'an:Xi'an Echnological University，2004.

［2］ 馮蘭勝.基于機(jī)械臂的遠(yuǎn)程控制系統(tǒng)研究［D］.西安:西安電子科技大學(xué)，2005.

FENG Lan-sheng.Remote Control Robot System Research Based on Robot Arm［D］.Xi'an:Xi'an Univeristy of E-lectronic Science and Technology，2005.

［3］ TIPSUWANY，CHOW M.Control methodologies in networked control systems［J］.2003，11（10）:1099-1111.

［4］WALSHG，YE H，BUSHNELL L.Stability analysis of networked control systems［J］.2002，13（5）:438-446.

［5］鄭樺，叢爽.絡(luò)遠(yuǎn)程控制中機(jī)械臂的連續(xù)軌跡控制［J］.中國(guó)工程科學(xué)，2008，10（10）:91-95.

ZHENG Hua，CONG Shuang.Continuous Path Control of Robot Arm in Network-based Remote Control［J］.Chinese Engineering Science，2002（5）:438-446.

［6］付西光，顏國(guó)正.7-DOF核工業(yè)機(jī)器人的軌跡規(guī)劃與仿真［J］.系統(tǒng)仿真學(xué)報(bào)，2005，8（8）:1948-1950.

FU Xi-guang，YAN Guo-zheng.Trajectory Planning and Simulating on 7-DOF Robot Applied to Nuclear Industry［J］.Journal of System Simulating，2005，8（8）:1948-1950.

［7］ 殷際英，何廣平.關(guān)節(jié)型機(jī)器人［M］.北京:化學(xué)工業(yè)出版社，2003:68-82.

YIN Ji-ying，HE Guang-ping.Joint Robot［M］.Beijing:Chemical Industry Press，2003:68-82.

［8］譚崗慧子，陳勁杰.三指仿人靈巧手軌跡規(guī)劃及仿真研究［J］.機(jī)械研究與應(yīng)用，2010，16（5）:44-47.

TANGANG Hui-zi，CHEN Jin-jie.Trajectory Planning and Simulation on three finger Dexterous Hand［J］.Mechanical Research and Application，2010，16（5）:44-47.

［9］魏子翔，從爽，鄭樺.基于網(wǎng)絡(luò)的機(jī)械臂遠(yuǎn)程控制平臺(tái)設(shè)計(jì)與時(shí)延補(bǔ)償［J］.機(jī)械設(shè)計(jì)，2007，24（6）:26-28.

WEIZi-xiang，CONG Shuang，ZHENG Hua.Research on Remote Controlled Platform Design of Mechanical Arm and Time Delayed Compensation Based on Network［J］.Mechanic Design，2007，24（6）:26-28.