面向六關(guān)節(jié)機(jī)器人的位置域控制

崔旭東，鄧少豐，王平江?

1) 鞍山師范學(xué)院計(jì)算中心，鞍山 114007 2) 華中科技大學(xué)機(jī)械科學(xué)與工程學(xué)院，武漢 430074

制造業(yè)是國(guó)民經(jīng)濟(jì)的主要支柱，也是今后我國(guó)經(jīng)濟(jì)“創(chuàng)新驅(qū)動(dòng)、轉(zhuǎn)型升級(jí)”的主戰(zhàn)場(chǎng)[1].在此背景下，工業(yè)機(jī)器人領(lǐng)域成為各國(guó)提高制造業(yè)綜合實(shí)力，使制造業(yè)實(shí)現(xiàn)數(shù)字化、網(wǎng)絡(luò)化、智能化的必爭(zhēng)之地[2?5].PID控制是工業(yè)控制領(lǐng)域中最經(jīng)典也是最常用的算法，因其原理簡(jiǎn)單、容易實(shí)現(xiàn)，并且具有較強(qiáng)的魯棒性，而被廣泛應(yīng)用于各種工業(yè)裝備的實(shí)時(shí)控制[6].PID控制技術(shù)已經(jīng)在理論上被證明對(duì)于工業(yè)機(jī)器人的高耦合、非線性系統(tǒng)是有效的，且PID主要應(yīng)用于已解耦的關(guān)節(jié)臂單軸的時(shí)間域伺服控制層面.雖然解耦控制可以為具有串、并聯(lián)性質(zhì)的多軸機(jī)器人系統(tǒng)簡(jiǎn)單軌跡提供好的跟蹤性能，但是實(shí)踐證明，對(duì)于高精度輪廓跟蹤，由于末端的運(yùn)動(dòng)是所有關(guān)節(jié)同步運(yùn)動(dòng)的結(jié)果，分別確保每個(gè)關(guān)節(jié)在時(shí)間域的良好跟蹤性能，并不能保證理想的輪廓跟蹤誤差，相關(guān)運(yùn)動(dòng)關(guān)節(jié)的不良同步誤差，往往導(dǎo)致輪廓跟蹤的軌跡精度惡化[7?11].

近年來，針對(duì)工業(yè)機(jī)器人輪廓跟蹤中各關(guān)節(jié)的運(yùn)動(dòng)同步問題已經(jīng)進(jìn)行了大量研究.為了進(jìn)一步提高輪廓運(yùn)動(dòng)精度，外國(guó)學(xué)者Koren提出了一種交叉耦合控制（Cross-coupled control, CCC）方法[7].實(shí)際上，CCC方法是針對(duì)單個(gè)軸的PID控制和針對(duì)多個(gè)軸的耦合誤差反饋控制的一種組合.CCC的主要思想是實(shí)現(xiàn)兩個(gè)運(yùn)動(dòng)軸的位置同步.實(shí)驗(yàn)表明，通過選擇合適的耦合增益，CCC可以通過同步性能的改善來提高輪廓跟蹤精度.然而由于機(jī)器人系統(tǒng)中確定耦合增益的復(fù)雜性[12]，以及每個(gè)運(yùn)動(dòng)關(guān)節(jié)仍然需要單獨(dú)控制，且必然存在一定的跟蹤誤差等原因，CCC方法也同樣具有局限性.

針對(duì)上述問題，加拿大學(xué)者結(jié)合PID控制和CCC控制的優(yōu)勢(shì)，提出一個(gè)新的控制方法：位置域控制技術(shù)[13?17].與傳統(tǒng)時(shí)域PID控制中軌跡規(guī)劃被設(shè)計(jì)為時(shí)間t的函數(shù)不同，位置域控制方法是在位置域中構(gòu)建運(yùn)動(dòng)軌跡，稱之為位置域PID控制.

本文主要解決的問題為：在將位置域PID控制技術(shù)應(yīng)用于多自由度、非線性、串聯(lián)的關(guān)節(jié)機(jī)器人的輪廓運(yùn)動(dòng)中，如何在機(jī)器人軌跡代碼執(zhí)行中，實(shí)時(shí)將時(shí)間域表達(dá)的輪廓軌跡函數(shù)，轉(zhuǎn)變?yōu)槲恢糜虮磉_(dá)的輪廓軌跡函數(shù)，并采用基于動(dòng)力學(xué)的位置域PID控制方法來提高機(jī)器人末端的輪廓運(yùn)動(dòng)精度.

相比于直角坐標(biāo)系下的常用的三軸銑床控制系統(tǒng)，由于機(jī)器人運(yùn)動(dòng)控制的復(fù)雜性，尤其是運(yùn)動(dòng)學(xué)正逆解的多解性，在實(shí)時(shí)控制環(huán)節(jié)構(gòu)建主從運(yùn)動(dòng)的以主動(dòng)軸位置為自變量的從動(dòng)軸位置函數(shù)，是極其復(fù)雜的，因此位置域的控制方式，在關(guān)節(jié)型機(jī)器人的應(yīng)用尚未深入開展.

1 機(jī)器人的位置域 PID 控制模型

對(duì)于多自由度機(jī)器人系統(tǒng)，時(shí)間域下的PID控制器標(biāo)準(zhǔn)形式可表示為：

式中，e(t)是系統(tǒng)的位置誤差矢量，e˙(t)是系統(tǒng)的速度誤差矢量，τ(t)是系統(tǒng)的控制轉(zhuǎn)矩矢量，t表示時(shí)間，Kp、Ki和Kd分別為比例增益、積分增益和微分增益的對(duì)角矩陣.在高速插補(bǔ)控制中，由于插補(bǔ)控制周期很短（1 ms），穩(wěn)態(tài)誤差比較小，一般令積分項(xiàng)的系數(shù)為零，將PID控制簡(jiǎn)化為PD控制，可以在不增加計(jì)算量的基礎(chǔ)上，獲得同樣的控制精度.因此，對(duì)于公式（1）所描述的從運(yùn)動(dòng)軸在時(shí)間域中PID控制模型下，可以表示為式（2）的位置域的PD控制：

式中，下標(biāo)m表示主運(yùn)動(dòng)master軸所在關(guān)節(jié)軸的序號(hào)，s表示從運(yùn)動(dòng)salve軸所在的關(guān)節(jié)軸的序號(hào)（下文同樣適用）.式（2）中，es(qm)表示以主動(dòng)軸位置qm為自變量，計(jì)算從動(dòng)軸的期望位置qsd(qm)與實(shí)際位置qs(qm)之差的矢量；es′(qm)表示從運(yùn)動(dòng)軸的相對(duì)速度（相對(duì)于主運(yùn)動(dòng)軸位置）之差的矢量；Kps0和Kds0則分別為具有恒定值的從動(dòng)軸的比例增益和微分增益對(duì)角矩陣.

注意：在任意時(shí)刻，對(duì)于六關(guān)節(jié)機(jī)器人的關(guān)節(jié)軸，只有一個(gè)主運(yùn)動(dòng)軸，可以有五個(gè)從運(yùn)動(dòng)軸；亦即公式（2）對(duì)應(yīng)于五個(gè)從關(guān)節(jié)軸的位置域PD控制方程.但是，不同的機(jī)器人的運(yùn)動(dòng)軌跡，主運(yùn)動(dòng)、從運(yùn)動(dòng)關(guān)節(jié)軸所對(duì)應(yīng)的序號(hào)是動(dòng)態(tài)變化的，既使是同一段機(jī)器人運(yùn)動(dòng)軌跡描述的指令代碼中，主運(yùn)動(dòng)軸、從運(yùn)動(dòng)軸的關(guān)系，也有可能隨時(shí)間或者位姿的變化而發(fā)生變化；而且在任意時(shí)刻，所有從動(dòng)軸的位置，都要實(shí)時(shí)表達(dá)為主動(dòng)軸位置的函數(shù).這就是為什么位置域控制算法在多關(guān)節(jié)機(jī)器人控制中，難以實(shí)際應(yīng)用的癥結(jié)所在.

如圖1所示為位置域PD控制原理框圖[18]，首先根據(jù)理論輪廓運(yùn)用逆運(yùn)動(dòng)學(xué)分析，獲得時(shí)間域中某段輪廓軌跡中各個(gè)關(guān)節(jié)軸的期望位置，并分析在該段軌跡運(yùn)動(dòng)中，哪個(gè)關(guān)節(jié)軸的運(yùn)動(dòng)位置增量最大，則將哪個(gè)關(guān)節(jié)軸指定為主運(yùn)動(dòng)關(guān)節(jié)軸，其他的關(guān)節(jié)軸為從運(yùn)動(dòng)關(guān)節(jié)軸.然后通過軌跡規(guī)劃方法，獲得在位置域中表示的從運(yùn)動(dòng)軸的、以主動(dòng)軸位置q1為自變量的從動(dòng)軸期望位置qsd(q1)和期望位置相對(duì)導(dǎo)數(shù)qsd′(q1)（此處假設(shè)與關(guān)節(jié)軸1相對(duì)應(yīng)的運(yùn)動(dòng)為主運(yùn)動(dòng)），最后通過位置域PD控制，實(shí)現(xiàn)從運(yùn)動(dòng)相對(duì)于主運(yùn)動(dòng)位置的實(shí)時(shí)跟隨控制，以提高整個(gè)機(jī)器人末端件空間運(yùn)動(dòng)軌跡的精度.

圖1 位置域 PD 控制原理框圖Fig.1 Schematic of PD control in the position domain

綜上所述，位置域PD控制，從控制規(guī)律上可看作是一種通用的空間同步的位置和速度控制，其中從運(yùn)動(dòng)根據(jù)軌跡輪廓要求，相對(duì)于主運(yùn)動(dòng)的位置、速度，在位置和速度上進(jìn)行跟隨控制，使得主運(yùn)動(dòng)和從運(yùn)動(dòng)之間綜合的運(yùn)動(dòng)軌跡盡可能地精確.

另外，從實(shí)現(xiàn)上來說，位置域PD控制其實(shí)是以時(shí)間域控制和位置域控制的混合控制的形式進(jìn)行的[19?20].主運(yùn)動(dòng)在時(shí)域中受到公式（1）所示控制律的控制，同時(shí)從運(yùn)動(dòng)則在位置域中受到公式（2）所示控制律的控制.應(yīng)當(dāng)注意的是，主運(yùn)動(dòng)在時(shí)間域的控制過程中必須滿足單調(diào)遞增或遞減的條件，以便形成從時(shí)間域到位置域的映射是一對(duì)一的，才能使得主動(dòng)軸因時(shí)間域控制所產(chǎn)生的位置誤差，折算到從動(dòng)軸的位置誤差為減少的.

更進(jìn)一步地，采用非線性函數(shù)替換公式（2）中的誤差信號(hào)es(qm)、es′(qm)，形成一種基于多自由度機(jī)器人的位置域非線性PD控制律.該控制律用以下形式表示：

其中，Kps0和Kds0是系統(tǒng)的初始非線性PD控制增益矩陣，是恒定值的從動(dòng)軸的比例增益和微分增益對(duì)角矩陣；ψ是三元非線性函數(shù)；而 αp，δp，αd，δd則是定義非線性函數(shù)的附加參數(shù).

三元非線性函數(shù) ψ的一般表達(dá)式定義如下[21]：

從公式（4）可以看出，α為非線性函數(shù)中的冪指數(shù)部分，而δ則為線性與非線性分界的閾值.在閾值范圍內(nèi)，函數(shù)表現(xiàn)為線性；當(dāng)超過該閾值時(shí)，函數(shù)表現(xiàn)為非線性；此時(shí)非線性函數(shù)的數(shù)學(xué)屬性由參數(shù)α決定.在位置域PD控制中，參考位置qm是類似時(shí)域中時(shí)間t的自變量.es(qm)和(qm)是自變量qm的函數(shù).

將公式（4）代入公式（3）中，該控制律也可以采用式（5）表示：

其中，該控制律的增益系數(shù)定義如下：

其中，Kps和Kds表示位置域非線性PD控制的與系統(tǒng)誤差es(qm)和有關(guān)的增益矩陣.

公式（5）至公式（7）表明，位置域非線性PD控制器可視為變?cè)鲆娴目刂破?，其增益為系統(tǒng)誤差的函數(shù).

2 基于 LinuxCNC 系統(tǒng)的位置域控制系統(tǒng)設(shè)計(jì)

LinuxCNC系統(tǒng)具有以下優(yōu)點(diǎn)：

（1）開源：LinuxCNC系統(tǒng)的源碼完全公開，用戶可以方便地獲得其源碼并進(jìn)行有針對(duì)性的開發(fā)；

（2）實(shí)時(shí)性：LinuxCNC系統(tǒng)具有實(shí)時(shí)性，支持EtherCAT現(xiàn)場(chǎng)總線及1 ms的插補(bǔ)周期，可以滿足機(jī)器人運(yùn)動(dòng)控制對(duì)實(shí)時(shí)性的要求；

（3）可擴(kuò)展性：LinuxCNC系統(tǒng)的硬件抽象層提供了內(nèi)核模塊接口，用戶可以根據(jù)自身需要靈活設(shè)計(jì)內(nèi)核實(shí)時(shí)運(yùn)行的HAL組件，以實(shí)現(xiàn)特定的實(shí)時(shí)插補(bǔ)控制功能，如位置域控制功能.

因此，本文采用LinuxCNC數(shù)控系統(tǒng)作為多關(guān)節(jié)機(jī)器人位置域PD控制算法實(shí)現(xiàn)的軟件平臺(tái).

2.1 位置域控制軟件系統(tǒng)總體方案

經(jīng)過上述分析，本文所提出的位置域PD控制算法可以在LinuxCNC中的硬件抽象層中，以HAL組件的方式予以實(shí)現(xiàn)，由此設(shè)計(jì)出如圖2所示的基于LinuxCNC系統(tǒng)的位置域PD控制系統(tǒng)總體方案[22?25].

圖2 基于 LinuxCNC系統(tǒng)的位置域PD控制系統(tǒng)總體方案Fig.2 Overall scheme of the PD control system based on the LinuxCNC system in position domain

由圖2所示整個(gè)控制系統(tǒng)的控制流程如下：

（1）用戶首先向圖形用戶界面輸入軌跡運(yùn)動(dòng)命令（或軌跡運(yùn)動(dòng)的文件），將運(yùn)動(dòng)指令通過NML通信機(jī)制傳輸至任務(wù)執(zhí)行模塊（EMCTASK）；

（2）EMCTASK 主控模塊周期性地調(diào)用RS-274NGC解釋器，對(duì)類似于G代碼的機(jī)器人軌跡運(yùn)動(dòng)代碼進(jìn)行解析，并將轉(zhuǎn)換后的操作命令進(jìn)行后續(xù)處理；

（3）將處理后的NML軌跡控制命令發(fā)送到用戶態(tài)與內(nèi)核態(tài)的共享內(nèi)存，由此傳輸至運(yùn)行于內(nèi)核實(shí)時(shí)環(huán)境中的運(yùn)動(dòng)控制器（EMCMOT）；

（4）EMCMOT主控程序是實(shí)時(shí)線程，周期性執(zhí)行笛卡爾坐標(biāo)系下的運(yùn)動(dòng)插補(bǔ)程序及運(yùn)動(dòng)學(xué)逆解，并將各個(gè)關(guān)節(jié)伺服電機(jī)的運(yùn)動(dòng)指令，通過Ether CAT總線，發(fā)往各個(gè)伺服驅(qū)動(dòng)器.主控程序從共享內(nèi)存中讀取由任務(wù)控制器發(fā)送的運(yùn)動(dòng)軌跡命令，并進(jìn)行機(jī)器人的運(yùn)動(dòng)速度規(guī)劃（小線段擬合、速度規(guī)劃等）、運(yùn)動(dòng)學(xué)正逆變換（關(guān)鍵點(diǎn)：同時(shí)確定主運(yùn)動(dòng)軸、從運(yùn)動(dòng)軸所對(duì)應(yīng)的關(guān)節(jié)軸以及構(gòu)造以主運(yùn)動(dòng)關(guān)節(jié)軸位置為自變量的其他各個(gè)從動(dòng)關(guān)節(jié)軸位置的函數(shù)等），并實(shí)現(xiàn)時(shí)域/位置域的混合插補(bǔ)計(jì)算、動(dòng)力學(xué)前饋等運(yùn)動(dòng)控制功能；

（5）將位置域PD控制算法模塊，以HAL組件的方式加入到硬件抽象層（HAL）.HAL通過定義的引腳（HAL pin）獲取運(yùn)動(dòng)控制器中的各種實(shí)時(shí)數(shù)據(jù)，該數(shù)據(jù)經(jīng)過HAL中各種功能組件的運(yùn)算后，寫入到實(shí)時(shí)EtherCAT總線的硬件驅(qū)動(dòng)程序；

（6）EtherCAT總線將各個(gè)關(guān)節(jié)軸的運(yùn)動(dòng)控制指令（含位置增量、力矩前饋）發(fā)送到對(duì)應(yīng)的伺服驅(qū)動(dòng)器，實(shí)現(xiàn)對(duì)機(jī)器人各個(gè)關(guān)節(jié)軸的運(yùn)動(dòng)控制；同時(shí)EtherCAT總線將各個(gè)軸的實(shí)際位置反饋回位置域PD控制算法模塊，以便計(jì)算各個(gè)從動(dòng)關(guān)節(jié)軸的、以主動(dòng)軸位置為自變量的位置、相對(duì)速度誤差以及力矩前饋值，為下一個(gè)控制周期準(zhǔn)備好位置域PD控制相關(guān)的數(shù)據(jù).

2.2 位置域控制算法

如圖3所示，本文設(shè)計(jì)的位置域PD控制算法分解為以下四個(gè)功能模塊.

圖3 位置域 PD 控制算法模塊結(jié)構(gòu)圖Fig.3 Module structure of the PD control algorithm in position domain

（1）主動(dòng)軸選擇模塊.

位置域控制算法的關(guān)鍵是，在某段軌跡上實(shí)時(shí)確定各個(gè)關(guān)節(jié)軸之間的主、從運(yùn)動(dòng)關(guān)系，即確定主動(dòng)關(guān)節(jié)軸與從動(dòng)關(guān)節(jié)軸的索引號(hào).本文采用將運(yùn)動(dòng)控制周期內(nèi)位移增量最大的關(guān)節(jié)軸，作為該周期的主運(yùn)動(dòng)軸，其他關(guān)節(jié)軸作為從運(yùn)動(dòng)軸.

（2）位置域PD控制補(bǔ)償量計(jì)算模塊.

對(duì)于主動(dòng)軸選擇模塊確定的從動(dòng)軸，本模塊將各個(gè)從動(dòng)軸的實(shí)際位置與理論位置的差值，從時(shí)間域轉(zhuǎn)化到位置域上，并將根據(jù)公式（5）分別求得各個(gè)從運(yùn)動(dòng)關(guān)節(jié)軸所需力矩的補(bǔ)償量.

（3）動(dòng)力學(xué)力矩前饋模塊.

通過力矩與電流間的轉(zhuǎn)化關(guān)系，根據(jù)各個(gè)關(guān)節(jié)所需的力矩補(bǔ)償量，求出各伺服電機(jī)所需的電流補(bǔ)償，采用電流/力矩前饋的方法，將其補(bǔ)償電流疊加到伺服控制器的電流環(huán)上，以改善伺服電機(jī)的動(dòng)力學(xué)特性，提高整個(gè)機(jī)器人運(yùn)動(dòng)軌跡的準(zhǔn)確性.

（4）驅(qū)動(dòng)電流限流調(diào)整模塊.

該模塊主要對(duì)各個(gè)關(guān)節(jié)軸的驅(qū)動(dòng)電流進(jìn)行限流，防止由于驅(qū)動(dòng)伺服電機(jī)運(yùn)動(dòng)的電流過大而造成的機(jī)器人抖動(dòng)等性能變差以及安全問題.

2.3 位置域控制程序的實(shí)現(xiàn)

位置域PD控制算法模塊是通過HAL組件實(shí)現(xiàn)的.由于該組件中所包含的動(dòng)力學(xué)力矩前饋模塊與驅(qū)動(dòng)電流限流模塊不是本文關(guān)注的重點(diǎn)，且動(dòng)力學(xué)力矩前饋模塊涉及慣性矩陣、摩擦阻尼矩陣、重力/戈氏力矩陣等基于旋量理論的動(dòng)力學(xué)建模和摩擦力模型建立等，較為冗長(zhǎng)，這里不再贅述.

本文所設(shè)計(jì)的位置域PD控制算法模塊，首先定義了機(jī)器人各關(guān)節(jié)的期望角位移、期望角速度、期望角加速度、實(shí)際角位移、實(shí)際角速度等引腳；然后通過HAL引腳之間的連接，將LinuxCNC系統(tǒng)中的各類數(shù)據(jù)傳遞到對(duì)應(yīng)的引腳中；隨后進(jìn)行位置域PD控制補(bǔ)償量計(jì)算；最終將計(jì)算得到的力矩補(bǔ)償值以伺服電機(jī)的力矩前饋的方式傳遞給各關(guān)節(jié)對(duì)應(yīng)的伺服電機(jī)驅(qū)動(dòng)器，實(shí)現(xiàn)對(duì)六關(guān)節(jié)機(jī)器人的位置域控制.

按照HAL編程語(yǔ)法，位置域PD控制算法模塊的comp文件（如pdc605.comp）編寫完成后，可以通過halcompile命令對(duì)該文件進(jìn)行編譯，編譯步驟如下：

（1）首先將 pdc605.comp 放置在 src 文件夾下；

（2）然后在src/scrips目錄下配置HAL程序的編譯環(huán)境source./rip-environment；

（3）在Linux 的終端上輸入halcompile pdc605.comp，則src文件夾中會(huì)生成一個(gè)pdc605.c文件；

（4）隨后在 Linux 的終端上輸入 halcompile -install pdc605，則rtlib文件夾中會(huì)生成一個(gè)pdc605.so內(nèi)核實(shí)時(shí)運(yùn)行的動(dòng)態(tài)庫(kù)文件；

（5）最后在configs文件夾中的puma605.hal文件里添加 loadrt pdc605（加載 HAL 組件 pdc605）以及addf pdc605.0.pdc servo-thread（添加函數(shù) pdc 到 1 ms實(shí)時(shí)線程）；

（6）利用HAL的net命令和setp命令進(jìn)行相關(guān)引腳的連接和設(shè)置.

編譯過程的終端命令窗口如圖4所示.

圖4 終端命令窗口Fig.4 Terminal command window

3 實(shí)驗(yàn)與分析

本文所研究的對(duì)象，允許有一定的穩(wěn)態(tài)誤差，所以實(shí)際使用時(shí)，PID參數(shù)中的 I 參數(shù)為0，亦即實(shí)際為PD調(diào)節(jié).PD參數(shù)的整定，采用的是4∶1的衰減曲線法.對(duì)于本文所作的研究中，PD控制周期為1 ms，也就是在PD參數(shù)整定的時(shí)候，采樣觀察的周期為1 ms.各個(gè)關(guān)節(jié)軸在位置域控制中，經(jīng)過整定后的PID參數(shù)如表1所示.

表1 PID 參數(shù)表Table 1 PID parameters

每個(gè)關(guān)節(jié)軸的慣量以及電氣、機(jī)械傳動(dòng)的屬性不一樣，所以各個(gè)軸的PD參數(shù)不一樣.

3.1 圓形軌跡控制實(shí)驗(yàn)及軌跡精度分析

為驗(yàn)證本文所提出的位置域PD控制系統(tǒng)在改善機(jī)器人末端輪廓軌跡精度的有效性，以某公司HSR-JR605工業(yè)機(jī)器人為實(shí)驗(yàn)對(duì)象，進(jìn)行平面圓形軌跡跟蹤實(shí)驗(yàn).本實(shí)驗(yàn)設(shè)計(jì)的平面圓形輪廓的理論軌跡如下：在機(jī)器人基坐標(biāo)系下圓心為O(450,0,250)，半徑R為100，軌跡運(yùn)行起點(diǎn)為A(450, ?100, 250)，單位為 mm，運(yùn)動(dòng)方向?yàn)轫槙r(shí)針方向，進(jìn)給速率F分別為30000和15000，單位為mm·min?1，下同.

為驗(yàn)證位置域PD控制算法的有效性，在運(yùn)行相同軌跡的情況下，分別進(jìn)行兩組實(shí)驗(yàn)以形成對(duì)照：第一組實(shí)驗(yàn)為未加入位置域PD控制算法模塊，在原有的時(shí)域控制下進(jìn)行軌跡實(shí)時(shí)采集；第二組為加入位置域PD控制算法模塊后進(jìn)行軌跡跟蹤與實(shí)時(shí)采集.

使用HAL示波器實(shí)時(shí)采集軌跡運(yùn)行過程中機(jī)器人各關(guān)節(jié)軸的相關(guān)數(shù)據(jù)，HAL示波器設(shè)置8個(gè)采樣通道，分別為關(guān)節(jié)1、2、3、5的理論位置與實(shí)際位置（關(guān)節(jié)4、6在軌跡運(yùn)行過程中未發(fā)生運(yùn)動(dòng)，相關(guān)數(shù)據(jù)均設(shè)為0），采樣周期為6 ms，采樣數(shù)量為2000個(gè).數(shù)據(jù)采集完成后，根據(jù)運(yùn)動(dòng)學(xué)正變換，可得到在機(jī)器人笛卡爾基坐標(biāo)系下機(jī)器人末端的運(yùn)動(dòng)軌跡，并對(duì)兩組實(shí)驗(yàn)的末端軌跡進(jìn)行對(duì)比.

設(shè)定進(jìn)給速率為 30000 mm·min?1時(shí)，普通時(shí)域控制下末端的三維運(yùn)動(dòng)軌跡及其三個(gè)方向投影與加入位置域PD控制算法模塊后的情況對(duì)比如圖5所示.

圖5 進(jìn)給速率為 30000 mm·min?1時(shí)不同控制下末端的平面圓形跟蹤軌跡對(duì)比圖.（a）三維圖；（b）x?y 平面投影；（c）x?z平面投影；（d）y?z平面投影Fig.5 Comparison of the planar circular tracking trajectories at the ends under different controls when the feed rate is 30000 mm·min?1: (a) threedimensional figure; (b) x?y plane projection; (c) x?z plane projection; (d) y?z plane projection

注意：圖5至圖8中，黑色線條為理論軌跡，紅色為時(shí)域控制下的軌跡，藍(lán)色為位置域控制下的軌跡.本文所有的位置域控制采用的是非線性的位置域控制方式.

設(shè)定進(jìn)給速率為15000，時(shí)域控制下末端的三維運(yùn)動(dòng)軌跡及其三個(gè)方向投影與加入位置域PD控制算法模塊后的情況對(duì)比如圖6所示.

圖6 進(jìn)給速率為 15000 mm·min?1時(shí)不同控制下末端的平面圓形跟蹤軌跡對(duì)比圖.（a）三維圖；（b）x?y 平面投影；（c）x?z平面投影；（d）y?z平面投影Fig.6 Comparison of the planar circular tracking trajectories at ends under different controls when the feed rate is 15000 mm·min?1: (a) threedimensional figure; (b) x–y plane projection; (c) x–z plane projection; (d) y–z plane projection

通過圖5、圖6可以明顯地看出，位置域PD控制下的平面圓形軌跡具有更好的輪廓跟蹤精度，證明了本文所提出的位置域PD控制系統(tǒng)，能夠有效提高六關(guān)節(jié)機(jī)器人末端的輪廓軌跡精度.

3.2 矩形軌跡控制實(shí)驗(yàn)及軌跡精度分析

如圖7、圖8所示，為不同運(yùn)動(dòng)速度下平面矩形軌跡運(yùn)動(dòng)精度檢驗(yàn)的實(shí)驗(yàn).本實(shí)驗(yàn)設(shè)計(jì)的平面矩形理論軌跡如下：在機(jī)器人基坐標(biāo)系下，軌跡運(yùn)行起點(diǎn)為A(350, ?100, 250)，分別經(jīng)過B(550, ?100,250)，C(550, 100, 250)，D(350, 100, 250)三個(gè)點(diǎn)后回到A，單位為mm；運(yùn)動(dòng)方向?yàn)槟鏁r(shí)針方向，進(jìn)給速率F分別為15000和10000.

圖7 進(jìn)給速率為 15000 mm·min?1時(shí)不同控制下末端的平面矩形跟蹤軌跡對(duì)比圖.（a）三維圖；（b）x?y 平面投影；（c）x?z平面投影；（d）y?z平面投影Fig.7 Comparison of the planar rectangular tracking tracks at ends under different controls at a feed rate of 15000 mm·min?1: (a) three-dimensional figure; (b) x?y plane projection; (c) x?z plane projection; (d) y?z plane projection

圖8 進(jìn)給速率為 10000 mm·min?1時(shí)不同控制下末端的平面矩形跟蹤軌跡對(duì)比圖.（a）三維圖；（b）x?y 平面投影；（c）x?z平面投影；（d）y?z平面投影Fig.8 Comparison of the planar rectangular tracking tracks at ends under different controls at a feed rate of 10000 mm·min?1: (a) three-dimensional figure; (b) x?y plane projection; (c) x?z plane projection; (d) y?z plane projection

設(shè)定進(jìn)給速率為15000，時(shí)域控制下末端的三維運(yùn)動(dòng)軌跡及其三個(gè)方向投影與加入位置域PD控制算法模塊后的情況對(duì)比如圖7所示.

設(shè)定進(jìn)給速率為10000，時(shí)域控制下末端的三維運(yùn)動(dòng)軌跡及其三個(gè)方向投影與加入位置域PD控制算法模塊后的情況對(duì)比如圖8所示.

通過圖7、圖8可以明顯地看出，位置域PD控制下的平面矩形軌跡具有更好的輪廓軌跡精度，亦證明了本文所提出的位置域PD控制系統(tǒng)，能夠有效提高六關(guān)節(jié)機(jī)器人末端的輪廓軌跡的精度.

4 結(jié)論

本文以時(shí)間域PID控制原理為依據(jù)，基于機(jī)器人動(dòng)力學(xué)模型推導(dǎo)出多自由度串聯(lián)關(guān)節(jié)型機(jī)器人的位置域PD控制模型，并設(shè)計(jì)了基于六關(guān)節(jié)機(jī)器人位置域PD的實(shí)時(shí)控制的整體方案；緊接著展開了基于LinuxCNC系統(tǒng)的六關(guān)節(jié)機(jī)器人位置域控制系統(tǒng)的研究，從位置域PD控制系統(tǒng)總體方案、位置域PD控制算法模塊以及位置域PD控制程序設(shè)計(jì)三個(gè)方面，將六關(guān)節(jié)機(jī)器人位置域PD控制算法在LinuxCNC控制系統(tǒng)中予以實(shí)現(xiàn)；在所搭建的六關(guān)節(jié)機(jī)器人實(shí)驗(yàn)平臺(tái)中，通過相關(guān)實(shí)驗(yàn)的設(shè)計(jì)、數(shù)據(jù)采集及分析，驗(yàn)證了本文所研究的位置域PD控制算法，在提高六關(guān)節(jié)機(jī)器人末端輪廓軌跡運(yùn)動(dòng)精度方面的有效性.

位置域控制算法，用在復(fù)雜的關(guān)節(jié)型機(jī)器人的實(shí)時(shí)控制中，其難點(diǎn)不在于如何實(shí)時(shí)確定關(guān)節(jié)軸的主從關(guān)系，而是在確定了主從關(guān)系后，如何實(shí)時(shí)地構(gòu)造以主動(dòng)關(guān)節(jié)軸位置為自變量、各個(gè)從動(dòng)關(guān)節(jié)軸位置為因變量的各自對(duì)應(yīng)的函數(shù).本文針對(duì)的是已知輪廓軌跡的方式，可以離線確定各個(gè)指令運(yùn)動(dòng)軌跡段中的主、從運(yùn)動(dòng)關(guān)系及從動(dòng)軸的位置函數(shù)，從而簡(jiǎn)化了實(shí)驗(yàn).如若真正地將位置域控制方式，引入到多軸聯(lián)動(dòng)的復(fù)雜機(jī)械結(jié)構(gòu)形式的機(jī)器人實(shí)時(shí)運(yùn)動(dòng)控制，則在運(yùn)動(dòng)軌跡代碼的解釋、小線段軌跡擬合、速度規(guī)劃階段，實(shí)時(shí)確定主、從關(guān)節(jié)軸及其關(guān)系，尤其是如何實(shí)時(shí)構(gòu)建從動(dòng)軸的位置函數(shù)方面，尚需做進(jìn)一步深入研究.