蘇成志，王承運(yùn)，劉 響，郭光遠(yuǎn)

（長春理工大學(xué)機(jī)電工程學(xué)院，吉林 長春 130022）

1 引言

工業(yè)機(jī)器人控制器是機(jī)器人信息處理和控制的主體，其設(shè)計(jì)好壞將決定機(jī)器人系統(tǒng)的穩(wěn)定性和整體性能。在機(jī)器人控制器的設(shè)計(jì)中，機(jī)器人的控制系統(tǒng)大都是非線性系統(tǒng)，如輪式移動(dòng)機(jī)器人控制系統(tǒng)，它是一個(gè)典型的非完整約束、高度非線性、多變量耦合的系統(tǒng)，難以用精確的數(shù)學(xué)方程的方法對(duì)其進(jìn)行精確描述。對(duì)于移動(dòng)機(jī)器人非線性控制系統(tǒng)的非線性問題，文獻(xiàn)[1-2]采用對(duì)機(jī)器人軌跡的預(yù)測(cè)模型進(jìn)行線性化處理，并將其滾動(dòng)優(yōu)化過程用模糊控制器代替。

文獻(xiàn)[3-4]提出了基于PSD的機(jī)器人標(biāo)定系統(tǒng)，在整個(gè)標(biāo)定過程中使激光束照射到每個(gè)PSD，PSD實(shí)時(shí)發(fā)送入射光線位置信息給機(jī)器人控制器，通過信息反饋形成了運(yùn)動(dòng)學(xué)閉合鏈，控制器根據(jù)該位置信息操控激光線逼近PSD中心。關(guān)于此類標(biāo)定系統(tǒng)控制器的設(shè)計(jì)問題，文獻(xiàn)[5-6]分別將控制系統(tǒng)的數(shù)學(xué)模型分成已知和未知兩部分，并通過Lyapunov方程設(shè)計(jì)出自適應(yīng)控制律。以上設(shè)計(jì)機(jī)器人控制器的處理方法只能針對(duì)控制模型能夠分解或者能夠進(jìn)行線性化處理的問題，而基于雙PSD機(jī)器人標(biāo)定系統(tǒng)為非線性系統(tǒng)很難線性化，因而難于設(shè)計(jì)出相應(yīng)的自適應(yīng)控制律，如何處理模型的非線性問題成為設(shè)計(jì)自適應(yīng)控制律的關(guān)鍵。

為設(shè)計(jì)出基于雙PSD的機(jī)器人標(biāo)定系統(tǒng)自適應(yīng)控制器，文中采用非線性方程降維的方式降低系統(tǒng)的非線性，即把機(jī)器人末端執(zhí)行器的運(yùn)動(dòng)分為平移運(yùn)動(dòng)和旋轉(zhuǎn)運(yùn)動(dòng)。通過運(yùn)動(dòng)分解的方式把非線性方程組分解為兩個(gè)方程組，分別設(shè)計(jì)出相應(yīng)的平移控制律和旋轉(zhuǎn)控制律。通過這種方式對(duì)非線性方程進(jìn)行降維，降低了設(shè)計(jì)自適應(yīng)控制律和估計(jì)律的難度。

2 雙PSD標(biāo)定系統(tǒng)

該標(biāo)定裝置包括六自由度機(jī)械手、機(jī)器人控制器、激光器、攝像機(jī)夾具和位置傳感器夾具（PSCF），半導(dǎo)體激光器安裝在激光器夾具上，并且固定在機(jī)器人末端執(zhí)行器（TCP）上，兩個(gè)PSD安裝在位置傳感器夾具上的兩表面中心，如圖1所示。

圖1 雙PSD標(biāo)定原理圖Fig.1 The Dual-PSD Calibration Scheme

由圖1可知：坐標(biāo)系Σl建立在激光器上并且定義激光束的方向?yàn)閤軸；坐標(biāo)系Σo1定義在PSD1上，坐標(biāo)系Σb表示機(jī)器人基坐標(biāo)系；其中坐標(biāo)系Σo1和Σb之間的位姿關(guān)系是未知的，因而需設(shè)計(jì)自適應(yīng)估計(jì)律對(duì)位姿關(guān)系進(jìn)行估計(jì)。在本標(biāo)定系統(tǒng)中激光束以某一個(gè)角度照射到其中一個(gè)PSD上，其反射光束照射到另一個(gè)PSD上，在PSD驅(qū)動(dòng)板得到光斑位置信息的原始數(shù)據(jù)后送達(dá)到機(jī)器人控制器，根據(jù)該位置信息，機(jī)器人操控激光線逼近雙PSD中心。論文中通過激光點(diǎn)在四個(gè)不同位置（位置1-4）的定位來實(shí)現(xiàn)機(jī)器人的標(biāo)定，確定機(jī)器人與PSD的位姿關(guān)系。

3 運(yùn)動(dòng)學(xué)模型的建立

3.1 正向運(yùn)動(dòng)學(xué)模型

PSD1上系Σo1和PSD2上系Σo2的定義，如圖2所示。

圖2 雙PSD標(biāo)定設(shè)備Fig.2 The Dual-PSD Calibration Device

圖中：γ—兩個(gè)PSD的夾角；d—PSD1和PSD2在y軸方向的長度；l1—入射光線；p2p1—光線在平面x1y1的反射光線；l2—平面x2y2上的反射光線。假設(shè)激光線的單位方向向量為=［m，n，w］T，并且激光器中心的坐標(biāo)為（x0，y0，z）0，根據(jù)光學(xué)反射定律=［-m，-n，w］T。根據(jù)文獻(xiàn)[7]可得到運(yùn)動(dòng)學(xué)模型為：

3.2 運(yùn)動(dòng)學(xué)方程的分解

由上述分析得出機(jī)器人控制對(duì)象框圖，如圖3所示。

圖3 控制對(duì)象框圖Fig.3 Control Object Block Diagram

圖中：A1，A2—從系b到o1系和從b系到系的4×4位姿變換矩陣，A?2—A2的估計(jì)值。

式（1）是6輸入4輸出的非線性方程，直接對(duì)其設(shè)計(jì)估計(jì)律和控制律難度比較大，根據(jù)點(diǎn)的運(yùn)動(dòng)分解定理，用齊次坐標(biāo)與矩陣形式將式（1）表達(dá)為：

末端執(zhí)行器運(yùn)動(dòng)分為平移運(yùn)動(dòng)和旋轉(zhuǎn)運(yùn)動(dòng)，則式q′=A1q″可以寫成：

由此得運(yùn)動(dòng)學(xué)方程：

由上述分析可知機(jī)器人末端執(zhí)行器的運(yùn)動(dòng)過程可以分為三步：

（1）先平移機(jī)器人末端執(zhí)行器使激光光斑照射到PSD1的中心；（2）機(jī)器人末端執(zhí)行器保持位置不變調(diào)整位姿使PSD1的反射光線照射到PSD2的中心；（3）重復(fù)1、2步驟直到激光光斑在雙PSD中心。由此可得平移運(yùn)動(dòng)和旋轉(zhuǎn)運(yùn)動(dòng)控制系統(tǒng)框圖分別如下。

4 平移運(yùn)動(dòng)控制器設(shè)計(jì)

對(duì)于式（3）的線性系統(tǒng)，為了保持系統(tǒng)的穩(wěn)定需要采用控制律對(duì)其進(jìn)行控制。平移向量q″1從b系到o1系的位姿變換矩陣A1中含有不確定性參數(shù)，所以采用自適應(yīng)估計(jì)律Δθt對(duì)其進(jìn)行估計(jì)，具體，如圖4所示。

圖4 平移運(yùn)動(dòng)和旋轉(zhuǎn)運(yùn)動(dòng)控制系統(tǒng)方框圖Fig.4 Block Diagram of Translational Motion and Rotary Motion Control System

圖 4 下中，Y1d=［x1dy1d0 0］T，ΔY1=［Δx1Δy10 0］T，Δθt=θ?t-θt式中：Δθt=［Δθ4Δθ80 0］T，θ?t=［θ?4θ?8z01］T，θt=［θ4θ8z01］T

式中z0是常數(shù)，即機(jī)器人末端執(zhí)行器僅在平面z=z0內(nèi)運(yùn)動(dòng)，控制律為：ττ（ΔY1）＝［ττ1，ττ2，z0，1］T

因?yàn)閮H有θ4和θ8是未知的，所以：

根據(jù)上述分析可得到平移運(yùn)動(dòng)控制模型：

為了證明PSD1平面上光斑位置能夠趨近于PSD1中心，因而構(gòu)造Lyapunov函數(shù)。機(jī)器人末端執(zhí)行器只進(jìn)行平移運(yùn)動(dòng)時(shí)，標(biāo)定系統(tǒng)的目標(biāo)是PSD1上的光斑位置Y1→0，光斑速度Y˙1→0，同時(shí)坐標(biāo)轉(zhuǎn)換的估計(jì)值趨向于真實(shí)值，即Δθt→0?；诖耍鶕?jù)能量法構(gòu)造Lyapunov函數(shù)為：

式中：Kv1、Kp1、Bt—正常數(shù)。

由Lyapunov函數(shù)得自適應(yīng)估計(jì)律：

式中：Ct2、B—常數(shù)，Dt—4×4 的矩陣。

由Lyapunov函數(shù)得自適應(yīng)控制律為：

式中：Ct1—常數(shù)

式（7）為所設(shè)計(jì)的平移控制律。

5 旋轉(zhuǎn)運(yùn)動(dòng)控制器設(shè)計(jì)

對(duì)于式（4）的非線性系統(tǒng)，為了保持系統(tǒng)的穩(wěn)定需要采用控制律對(duì)其進(jìn)行控制；旋轉(zhuǎn)向量從b系到o1系的位姿變換矩陣R1中含有不確定性參數(shù)，所以采用自適應(yīng)估計(jì)律Δθr對(duì)其進(jìn)行估計(jì)，具體，如圖4所示。

圖 4 下中：Y2d=［x2dy2d］T，ΔY2=［Δx2Δy2］T

控制律：τr（ΔY2）=［τr1τr2τr3］T，Δθr=θ?r-θr因?yàn)閮H有R2是未知的，用R?2表示R2的估計(jì)值。

根據(jù)上述分析可得到旋轉(zhuǎn)運(yùn)動(dòng)控制模型：

為了證明PSD2平面上光斑位置能夠趨近于PSD2中心，因而構(gòu)造Lyapunov函數(shù)。機(jī)器人末端執(zhí)行器只進(jìn)行旋轉(zhuǎn)運(yùn)動(dòng)時(shí)，標(biāo)定系統(tǒng)的目標(biāo)是PSD2上的光斑位置Y2→0，光斑速度Y˙2→0，同時(shí)坐標(biāo)轉(zhuǎn)換的估計(jì)值趨向于真實(shí)值，即Δθr→0?；诖?，根據(jù)能量法構(gòu)造Lyapunov函數(shù)為：

Kv2、Kp2和 Br均為正常數(shù)，Δθr=θ?r-θr

由Lyapunov函數(shù)得自適應(yīng)估計(jì)律：

式中：Cr1—常數(shù) Dr為 3×9 矩陣。

由Lyapunov函數(shù)得自適應(yīng)控制律為：

式中：Cr1—常數(shù)。

所設(shè)計(jì)的旋轉(zhuǎn)控制律，如式（10）所示。

6 仿真實(shí)例

為了驗(yàn)證所設(shè)計(jì)的控制器能使激光光斑照射在PSD中心，采用Matlab/Simulink構(gòu)建模型進(jìn)行仿真實(shí)驗(yàn)，通過Level-1 S函數(shù)對(duì)各個(gè)模塊進(jìn)行編程，控制系統(tǒng)輸入部分是光斑理想位置坐標(biāo)向量，輸出是實(shí)際位置坐標(biāo)向量。（1）仿真展示了機(jī)器人末端執(zhí)行器先只進(jìn)行平移運(yùn)動(dòng)使激光光斑移動(dòng)到PSD1的中心，同時(shí)光斑在PSD2的光敏區(qū)內(nèi)；（2）以第一步結(jié)束時(shí)PSD2上的光斑點(diǎn)為初始點(diǎn)，機(jī)器人末端執(zhí)行器保持位置不變調(diào)整位姿使PSD1的反射光線照射到PSD2的中心；（3）重復(fù)多次1、2步驟直到激光光斑在雙PSD中心。

根據(jù)上文控制器設(shè)計(jì)中正定增益參數(shù)應(yīng)為正常數(shù)，實(shí)驗(yàn)仿真系統(tǒng)參數(shù)如下：

正定增益：Kv1=1.00，Kp1=0.50，Bt=0.80，Kv2=0.60，Kp2=1.00，Br=0.60，Ct1=1.00，Ct2=1.00，Cr1=1.00，Cr2=1.00

為了保證激光光斑初始位置在PSD的光敏區(qū)內(nèi)，激光器初始位置設(shè)為：（x0，y0，z）0=（100，100，100）

激光器初始位姿R1設(shè)為：

按照如上系統(tǒng)參數(shù)進(jìn)行實(shí)驗(yàn)仿真，仿真時(shí)間為7s。仿真結(jié)果為：平移估計(jì)律和旋轉(zhuǎn)估計(jì)律變化趨勢(shì)，如圖5所示。激光光斑在雙PSD上的位置誤差，如圖6所示。

圖5 平移估計(jì)律和旋轉(zhuǎn)估計(jì)律變化趨勢(shì)Fig.5 Variation Trend of Position Estimation Law and Orientation Estimation Law

圖6 機(jī)器人控制器仿真結(jié)果Fig.6 Robot Control Simulation Results for PSD1 and PSD2

由圖5可知平移估計(jì)律和旋轉(zhuǎn)估計(jì)律的值隨著時(shí)間的增加都趨向于0。其中平移估計(jì)律估計(jì)出的位置誤差最大為0.004mm，旋轉(zhuǎn)估計(jì)律估計(jì)出的姿態(tài)誤差最大為2.25×10-5。由此可證明設(shè)計(jì)的估計(jì)律能夠精確的估算出雙PSD與機(jī)器人之間的位姿關(guān)系。精確的計(jì)算出雙PSD與機(jī)器人的位姿關(guān)系保證了激光能照射到雙PSD中心。對(duì)于平移控制器，伺服控制器的仿真結(jié)果顯示了平移控制器能夠驅(qū)使激光光斑照射到PSD1的中心，如圖6（a）所示。同理，如圖6（b）所示，仿真結(jié)果顯示了旋轉(zhuǎn)控制器能夠驅(qū)使光斑照射到PSD2的中心。激光光斑在雙PSD上的位置穩(wěn)態(tài)誤差最大為0.0066mm，能夠滿足控制系統(tǒng)要求。由此可證明設(shè)計(jì)的平移控制律和旋轉(zhuǎn)控制律能驅(qū)使激光光斑照射到雙PSD中心。

7 結(jié)論

在本論文中使用了運(yùn)動(dòng)學(xué)方程降維的方法降低設(shè)計(jì)控制器的難度，即把激光器的運(yùn)動(dòng)分為平移運(yùn)動(dòng)和旋轉(zhuǎn)運(yùn)動(dòng)，設(shè)計(jì)相應(yīng)的自適應(yīng)控制律驅(qū)使激光點(diǎn)分別照射向PSD1和PSD2的中心，仿真結(jié)果驗(yàn)證了所提方法的有效性。與非線性方程未降維設(shè)計(jì)的控制器[9]相比，通過運(yùn)動(dòng)分解的方法所設(shè)計(jì)的運(yùn)動(dòng)控制器具有較小的運(yùn)算量，收斂速度快，控制精度高。雙PSD的激光光斑的位置穩(wěn)態(tài)誤差為0.0066 mm，這非常適合機(jī)器人運(yùn)動(dòng)控制的應(yīng)用。由仿真實(shí)例可得出如下規(guī)律：（1）激光照射到雙PSD中心的速度與自適應(yīng)估計(jì)律趨向于0的速度有關(guān)，即自適應(yīng)估計(jì)律趨向于0的速度越快，光斑趨向于雙PSD中心的速度越快。（2）設(shè)計(jì)的自適應(yīng)控制律抵抗外界干擾的能力越強(qiáng)光斑越能夠穩(wěn)定在雙PSD中心，即控制系統(tǒng)的穩(wěn)定性越好。