莫 毅，卿啟新

(1.廣西工業(yè)職業(yè)技術(shù)學(xué)院 電子信息學(xué)院，南寧 530001； 2.廣西科技大學(xué) 自動化學(xué)院，廣西 柳州 545006)

0 引言

激光雷達(dá)是一種特殊雷達(dá)系統(tǒng)，能夠借助激光束探測目標(biāo)對象所處位置，可以將速度、頻率等特征量測量指標(biāo)反饋回控制主機(jī)。與其他類型的雷達(dá)探測系統(tǒng)相比，激光雷達(dá)設(shè)備可以根據(jù)探測激光束與目標(biāo)回波之間的差異性，獲取與測量目標(biāo)相關(guān)的參數(shù)信息。無論目標(biāo)對象處于靜態(tài)或動態(tài)運(yùn)動態(tài)勢，控制主機(jī)都能對其進(jìn)行準(zhǔn)確地跟蹤與識別[1]。激光雷達(dá)探測系統(tǒng)由激光發(fā)射器、光學(xué)接收設(shè)備、信息處理平臺等多個(gè)結(jié)構(gòu)組成。激光發(fā)射器元件將輸入電脈沖信號轉(zhuǎn)換成光脈沖信號發(fā)射出去，光學(xué)接收設(shè)備再將所接收到的光脈沖信號還原成初始電脈沖信號，以便于顯示器設(shè)備可以完整表現(xiàn)出探測對象的存在狀態(tài)[2]。

多關(guān)節(jié)機(jī)器人也叫關(guān)節(jié)機(jī)器人或關(guān)節(jié)機(jī)械手臂，其各個(gè)連接關(guān)節(jié)的運(yùn)動形態(tài)都是轉(zhuǎn)動的，故而整體形態(tài)與人的手臂極為類似[3]。當(dāng)某個(gè)關(guān)節(jié)出現(xiàn)轉(zhuǎn)動時(shí)，其與核心連桿在空間方向上會產(chǎn)生一個(gè)相對移動量，若相對移動量的取值相同，則機(jī)器人關(guān)節(jié)的轉(zhuǎn)動角度數(shù)值也相同，由于機(jī)器人運(yùn)動姿態(tài)會隨著轉(zhuǎn)動角數(shù)值的變化而發(fā)生改變，所以一臺多關(guān)節(jié)機(jī)器人需要由多臺計(jì)算機(jī)設(shè)備進(jìn)行控制。為了保持機(jī)器人運(yùn)動行為的穩(wěn)定性利用模型降階與鏈?zhǔn)浇Y(jié)構(gòu)的機(jī)械臂位姿控制算法將整個(gè)控制過程劃分為多個(gè)階段，根據(jù)被動連桿姿態(tài)角的變化幅度確定中間姿態(tài)角的當(dāng)前轉(zhuǎn)動數(shù)值，再聯(lián)合目標(biāo)姿態(tài)角偏轉(zhuǎn)程度的求解結(jié)果，判斷多關(guān)節(jié)機(jī)器人是否出現(xiàn)明顯的運(yùn)動晃動情況[4]。然而此方法并不能有效控制機(jī)器人坡面行進(jìn)步長與平坦地形直行運(yùn)動步長之間的差值水平，在維持機(jī)器人運(yùn)動穩(wěn)定性方面的能力不足以滿足實(shí)際應(yīng)用需求。

為解決上述問題，設(shè)計(jì)基于激光雷達(dá)的多關(guān)節(jié)機(jī)器人姿態(tài)自動控制方法。

1 激光雷達(dá)定位導(dǎo)航技術(shù)

利用激光雷達(dá)實(shí)現(xiàn)多關(guān)節(jié)機(jī)器人姿態(tài)的定位與導(dǎo)航涉及如下兩方面問題:1)如何掌握機(jī)器人在雷達(dá)地圖中的具體位置；2)根據(jù)激光脈沖光束的傳輸情況，制定合理的運(yùn)動路線。激光雷達(dá)定位導(dǎo)航技術(shù)要求在指定運(yùn)動空間內(nèi)設(shè)置多個(gè)目標(biāo)位置節(jié)點(diǎn)，在每個(gè)節(jié)點(diǎn)上裝配反射板結(jié)構(gòu)[5]。機(jī)器人在移動時(shí)，激光雷達(dá)設(shè)備對反射板進(jìn)行集中掃描，導(dǎo)航主機(jī)自主選擇并記錄三個(gè)以上反射板所處位置，再通過幾何計(jì)算的方法對相關(guān)運(yùn)動參數(shù)進(jìn)行解析，通常情況下的定位導(dǎo)航遵循“三角法”原則，具體參考圖1。

圖1 激光雷達(dá)定位導(dǎo)航原理

激光脈沖輸出裝置借助激光器元件將光波信號發(fā)射至被測機(jī)器人所處位置，而回波信號處理裝置則借助光電器件對電量信號進(jìn)行聚合處理，當(dāng)回收信號強(qiáng)度達(dá)到既定數(shù)值標(biāo)準(zhǔn)之后，計(jì)時(shí)單元、控制單元開始統(tǒng)計(jì)激光雷達(dá)傳輸回路中的信號輸出與回收總量，直至導(dǎo)航主機(jī)能夠準(zhǔn)確定位被測機(jī)器人的位姿形態(tài)[6-7]。

(1)

由于電脈沖輸出信號不可能被完全轉(zhuǎn)換成光脈沖回波信號，所以激光雷達(dá)傳輸回路中，光波信號的利用效率不會等于100%。

2 多關(guān)節(jié)機(jī)器人運(yùn)動姿態(tài)

在激光雷達(dá)定位導(dǎo)航技術(shù)的支持下，完善CPG單元振蕩器模型，再通過優(yōu)化足結(jié)構(gòu)參數(shù)的處理方式，生成完整的機(jī)器人行進(jìn)路線，以此開展機(jī)器人運(yùn)動姿態(tài)。

2.1 CPG單元振蕩器模型

CPG單元振蕩器為多關(guān)節(jié)機(jī)器人提供了運(yùn)動過程中的動力支持。在機(jī)器人行進(jìn)過程中，關(guān)節(jié)設(shè)備的變化狀態(tài)由彎曲、伸展兩種形式組成，故而CPG單元振蕩器模型的運(yùn)行狀態(tài)也由包含兩個(gè)部分。

2.1.1 關(guān)節(jié)設(shè)備彎曲

當(dāng)機(jī)器人關(guān)節(jié)設(shè)備呈現(xiàn)彎曲狀態(tài)時(shí)，激光信號快速輸入信號集成設(shè)備之中，此時(shí)電量信號由振蕩器回路“+”級傳輸至“-”級，振蕩設(shè)備中生成大量的雷達(dá)射線，且這些射頻信號的傳輸方向始終與電量信號的傳輸方向保持一致[8]。

2.1.2 關(guān)節(jié)設(shè)備伸展

當(dāng)機(jī)器人關(guān)節(jié)設(shè)備呈現(xiàn)伸展?fàn)顟B(tài)時(shí)，已存儲激光信號由集成設(shè)備繼續(xù)向外傳輸，此時(shí)電量信號傳輸方向雖然由振蕩器回路“+”級指向“-”級。由于所有傳輸信號的波動能力都相對較弱，故而振蕩設(shè)備中的雷達(dá)射線不能繼續(xù)向外傳輸，只能借助振蕩器模型底層的CPG單元回路反饋給機(jī)器人足結(jié)構(gòu)，從而使得機(jī)器人的運(yùn)動步頻與步長得到有效控制[9]。

完整的CPG單元振蕩器模型連接回路如圖2所示。

圖2 CPG單元振蕩器傳輸回路

CPG單元振蕩器模型表達(dá)式為：

(2)

2.2 足結(jié)構(gòu)參數(shù)優(yōu)化

多關(guān)節(jié)機(jī)器人的工作環(huán)境較為復(fù)雜，故而利用激光雷達(dá)設(shè)備實(shí)現(xiàn)對機(jī)器人運(yùn)動姿態(tài)的自動控制，此時(shí)要求機(jī)器人足結(jié)構(gòu)具有較好的地形適應(yīng)性與運(yùn)動穩(wěn)定性[10]。由于CPG單元振蕩器結(jié)構(gòu)是一個(gè)串并聯(lián)并存的混合應(yīng)用設(shè)備，所以當(dāng)機(jī)器人足結(jié)構(gòu)處于自由度較多的復(fù)雜運(yùn)動環(huán)境中時(shí)，要求關(guān)節(jié)設(shè)備必須滿足最基本的足結(jié)構(gòu)運(yùn)動性能。在CPG單元振蕩器模型的支持下，以多關(guān)節(jié)機(jī)器人足結(jié)構(gòu)作為研究對象，并對足結(jié)構(gòu)尺寸參數(shù)進(jìn)行優(yōu)化處理，從而使得控制主機(jī)能夠精準(zhǔn)捕獲處于同一運(yùn)動平面內(nèi)的機(jī)器人運(yùn)動姿態(tài)節(jié)點(diǎn)[11]。設(shè)r1,r2,…,rn表示n個(gè)處于同一運(yùn)動平面內(nèi)的機(jī)器人關(guān)節(jié)姿態(tài)節(jié)點(diǎn)，γ1,γ2,…,γn分別表示與r1,r2,…,rn匹配的運(yùn)動轉(zhuǎn)向系數(shù)，U表示復(fù)雜度標(biāo)記特征，聯(lián)立上述物理量，可將多關(guān)節(jié)機(jī)器人足結(jié)構(gòu)運(yùn)動參數(shù)求解表達(dá)式為：

(3)

聯(lián)立式(2)、式(3)，可將足結(jié)構(gòu)參數(shù)優(yōu)化結(jié)果表示為：

(4)

其中:umax表示激光波振蕩系數(shù)的最大取值，umin表示激光振蕩系數(shù)的最小取值，ΔT表示單位運(yùn)動周期。為避免復(fù)雜數(shù)據(jù)處理環(huán)節(jié)的出現(xiàn)，在劃分單位運(yùn)動周期時(shí)，要求機(jī)器人足結(jié)構(gòu)運(yùn)動形態(tài)應(yīng)保持一致。

2.3 運(yùn)動步態(tài)生成

(5)

式中，ι表示目標(biāo)姿態(tài)節(jié)點(diǎn)標(biāo)記系數(shù)，iι表示目標(biāo)位置處的機(jī)器人關(guān)節(jié)點(diǎn)坐標(biāo)，i0表示機(jī)器人關(guān)節(jié)點(diǎn)的初始坐標(biāo)，λ表示獨(dú)立步態(tài)周期內(nèi)的機(jī)器人運(yùn)動強(qiáng)度。在激光器、光電元件所處位置保持不變的情況下，激光雷達(dá)信號傳輸路徑也不會發(fā)生改變，這也是機(jī)器人運(yùn)動步態(tài)路線內(nèi)不會出現(xiàn)反向排布的姿態(tài)節(jié)點(diǎn)的主要原因。

3 機(jī)器人姿態(tài)自動控制實(shí)現(xiàn)

根據(jù)已生成的機(jī)器人運(yùn)動步態(tài)行進(jìn)路線，設(shè)定姿態(tài)參數(shù)指標(biāo)，通過轉(zhuǎn)換運(yùn)動坐標(biāo)的方式，求解動力學(xué)方程表達(dá)式，從而完善反饋控制器連接形式，實(shí)現(xiàn)基于激光雷達(dá)的多關(guān)節(jié)機(jī)器人姿態(tài)自動控制。

3.1 姿態(tài)參數(shù)設(shè)定

(6)

在式(6)的基礎(chǔ)上，假設(shè)gx表示機(jī)器人運(yùn)動姿態(tài)在X軸方向上的數(shù)值分量，gy表示Y軸方向上的數(shù)值分量，gz表示Z軸方向上的數(shù)值分量，且gx≠0、gy≠0、gz≠0的不等式條件同時(shí)成立，μ表示關(guān)節(jié)角標(biāo)記系數(shù)。

多關(guān)節(jié)機(jī)器人的姿態(tài)參數(shù)設(shè)定表達(dá)式為：

(7)

3.2 運(yùn)動坐標(biāo)轉(zhuǎn)換

(8)

當(dāng)待處理姿態(tài)節(jié)點(diǎn)不處于同一運(yùn)動平面內(nèi)時(shí)，需要根據(jù)平面轉(zhuǎn)向角取值，建立兩個(gè)關(guān)聯(lián)平面之間的映射關(guān)系，將原節(jié)點(diǎn)在另一運(yùn)動平面內(nèi)的對應(yīng)映射節(jié)點(diǎn)標(biāo)記出來。按照姿態(tài)參數(shù)設(shè)定原則，判斷當(dāng)前節(jié)點(diǎn)之間的映射關(guān)系是否能夠滿足激光雷達(dá)光波的穩(wěn)定傳輸需求。在此基礎(chǔ)上進(jìn)行運(yùn)動坐標(biāo)轉(zhuǎn)換處理，直至所得結(jié)果能夠清晰反映出機(jī)器人關(guān)節(jié)姿態(tài)節(jié)點(diǎn)的排列形式[17]。

3.3 動力學(xué)方程

機(jī)器人關(guān)節(jié)姿態(tài)動力學(xué)方程構(gòu)建包括簡化與分解、非線性耦合項(xiàng)參數(shù)化處理兩個(gè)部分，本章節(jié)研究將針對上述內(nèi)容展開。

3.3.1 簡化與分解

(9)

由于激光雷達(dá)光波在X軸、Y軸、Z軸方向上的投影長度不一定完全相同，所以當(dāng)定義姿態(tài)坐標(biāo)側(cè)重方向不同時(shí)，所得動力學(xué)方程的簡化與分解處理結(jié)果也會有所不同。

3.3.2 非線性耦合項(xiàng)參數(shù)化

非線性耦合項(xiàng)參數(shù)化注重對機(jī)器人運(yùn)動姿態(tài)的方向轉(zhuǎn)角進(jìn)行度量，可以在動力學(xué)方程簡化與分解表達(dá)式的基礎(chǔ)上，限定位姿角的偏轉(zhuǎn)情況，從而使得處理主機(jī)能夠?qū)ζD(zhuǎn)角參量進(jìn)行更為細(xì)致的劃分，也可有效抑制激光雷達(dá)光波變化[20-21]。設(shè)θx表示機(jī)器人運(yùn)動方向角的X軸轉(zhuǎn)向分量，θy表示Y軸轉(zhuǎn)向分量，θz標(biāo)準(zhǔn)Z軸轉(zhuǎn)向分量，ΔL表示機(jī)器人關(guān)節(jié)姿態(tài)的單位位移量，聯(lián)立上述物理量，可將動力學(xué)方程非線性耦合項(xiàng)參數(shù)化處理結(jié)果表示為：

(10)

其中:?x表示X軸方向上的運(yùn)動步長值，?y表示Y軸方向上的運(yùn)動步長值，?z表示Z軸方向上的運(yùn)動步長值。在機(jī)器人運(yùn)動過程中，若關(guān)節(jié)姿態(tài)節(jié)點(diǎn)不存在跨越激光雷達(dá)光波傳輸平面的動力學(xué)行為，則非線性耦合項(xiàng)參數(shù)化處理結(jié)果不會出現(xiàn)變化。

3.4 反饋控制器

反饋控制器是一個(gè)完整的信號閉環(huán)結(jié)構(gòu)，可以根據(jù)動力學(xué)方程表達(dá)式，標(biāo)記機(jī)器人關(guān)節(jié)所處實(shí)時(shí)運(yùn)動位置，將已記錄節(jié)點(diǎn)與激光雷達(dá)光波的入射節(jié)點(diǎn)進(jìn)行對比，以確定步長值向量的實(shí)際取值結(jié)果[22-23]。由于動力學(xué)方程非線性耦合項(xiàng)參數(shù)化結(jié)果并不能直接影響激光雷達(dá)光波的傳輸行為，所以建立反饋控制器閉環(huán)時(shí)，默認(rèn)激光雷達(dá)光波的傳輸方向只在X軸、Y軸、Z軸方向上存在波長分量。設(shè)m1,m2,…,mn表示n個(gè)不相等的運(yùn)動步幅向量，v表示關(guān)節(jié)角運(yùn)動速度，b表示位姿角定義系數(shù)，ζ表示關(guān)節(jié)角偏轉(zhuǎn)向量，σ表示位姿角偏轉(zhuǎn)向量，聯(lián)立式(10)，可將基于激光雷達(dá)的多關(guān)節(jié)機(jī)器人姿態(tài)反饋控制器閉環(huán)表達(dá)式定義為：

(11)

多關(guān)節(jié)機(jī)器人姿態(tài)反饋控制器閉環(huán)設(shè)計(jì)過程中，除了要考慮激光雷達(dá)輸出光波的定位能力外，還要確保反饋控制器結(jié)構(gòu)呈現(xiàn)出完整的閉環(huán)連接狀態(tài)，以此實(shí)現(xiàn)對于多關(guān)節(jié)機(jī)器人姿態(tài)自動控制。

4 實(shí)驗(yàn)結(jié)果與分析

4.1 實(shí)驗(yàn)準(zhǔn)備

選擇R6-0205型號的多關(guān)節(jié)機(jī)器人作為實(shí)驗(yàn)對象，如圖3所示。

圖3 多關(guān)節(jié)機(jī)器人實(shí)驗(yàn)設(shè)備

將其置于5.0～34.0 V的電壓環(huán)境中，調(diào)節(jié)陀螺儀測量設(shè)備，使得MTi-G-700姿態(tài)傳感器對于機(jī)器人關(guān)節(jié)角節(jié)點(diǎn)的控制精度保持為1。

表1記錄了相關(guān)技術(shù)指標(biāo)的實(shí)際取值范圍。

表1 技術(shù)參數(shù)表

實(shí)驗(yàn)組為基于激光雷達(dá)的多關(guān)節(jié)機(jī)器人姿態(tài)自動控制算法方法，對照組為基于模型降階與鏈?zhǔn)浇Y(jié)構(gòu)的機(jī)械臂位姿控制方法，分別利用上述方法監(jiān)測多關(guān)節(jié)機(jī)器人的坡面行進(jìn)姿態(tài)，再將所得步長值數(shù)據(jù)與機(jī)器人平坦直行步長進(jìn)行對比。機(jī)器人運(yùn)動行為的激光雷達(dá)圖像如圖4所示。

圖4 機(jī)器人運(yùn)動行為的激光雷達(dá)圖像

監(jiān)測機(jī)器人坡面行進(jìn)姿態(tài)時(shí)，為避免關(guān)節(jié)角出現(xiàn)過度偏轉(zhuǎn)的情況，應(yīng)控制機(jī)器人運(yùn)動速率保持較為穩(wěn)定的數(shù)值狀態(tài)。

4.2 數(shù)據(jù)處理與結(jié)果分析

機(jī)器人坡面步長與平坦直行步長之間的差值可以用來描述機(jī)器人運(yùn)動行為的穩(wěn)定性。一般來說，步長差值越小，就表示機(jī)器人的運(yùn)動行為越穩(wěn)定。對于多關(guān)節(jié)機(jī)器人設(shè)備來說，當(dāng)其關(guān)節(jié)偏轉(zhuǎn)角達(dá)到90°時(shí)，整個(gè)機(jī)械臂呈現(xiàn)水平狀態(tài)，此情況下激光雷達(dá)的傳輸光波容易被主臂體結(jié)構(gòu)遮擋，故而本次實(shí)驗(yàn)過程中，只記錄關(guān)節(jié)角處于15～75°時(shí)，機(jī)器人坡面步長的數(shù)值變化情況。

圖5反映了機(jī)器人上、下坡步長與平坦直行步長的數(shù)值對比情況。

圖5 機(jī)器人上坡步長

分析圖5可知，隨著關(guān)節(jié)轉(zhuǎn)角數(shù)值的增大，機(jī)器人平坦直行步長呈現(xiàn)出不斷增大的數(shù)值變化狀態(tài)。當(dāng)關(guān)節(jié)轉(zhuǎn)角等于75°時(shí)，實(shí)驗(yàn)組上坡步長與當(dāng)前情況下的平坦直行步長完全相等，二者之差為0；當(dāng)關(guān)節(jié)轉(zhuǎn)角等于45°時(shí)，實(shí)驗(yàn)組上坡步長與當(dāng)前情況下的平坦直行步長差值最大，達(dá)到了0.02 m，即實(shí)驗(yàn)組上坡步長與平坦直行步長之間的數(shù)值最大誤差為5%。當(dāng)關(guān)節(jié)轉(zhuǎn)角等于75°時(shí)，對照組上坡步長與當(dāng)前情況下的平坦直行步長差值最大，達(dá)到了0.07 m，即對照組上坡步長與平坦直行步長之間的數(shù)值最大誤差為15.6%，高于實(shí)驗(yàn)組誤差水平。

分析圖6可知，當(dāng)關(guān)節(jié)轉(zhuǎn)角等于15°、30°、45°、75°時(shí)，實(shí)驗(yàn)組下坡步長與當(dāng)前情況下的平坦直行步長完全相等，二者之差為0；當(dāng)關(guān)節(jié)轉(zhuǎn)角等于60°時(shí)，實(shí)驗(yàn)組下坡步長與當(dāng)前情況下的平坦直行步長差值最大，達(dá)到了0.03 m，即實(shí)驗(yàn)組下坡步長與平坦直行步長之間的數(shù)值最大誤差為7.3%。當(dāng)關(guān)節(jié)轉(zhuǎn)角等于60°時(shí)，對照組下坡步長與當(dāng)前情況下的平坦直行步長差值最大，達(dá)到了0.26 m，即對照組下坡步長與平坦直行步長之間的數(shù)值最大誤差為61.9%，遠(yuǎn)高于實(shí)驗(yàn)組誤差水平。

圖6 機(jī)器人下坡步長

整個(gè)實(shí)驗(yàn)過程中，實(shí)驗(yàn)組機(jī)器人步長的數(shù)值狀態(tài)明顯更加穩(wěn)定，這就表示在該控制算法作用下，機(jī)器人不會出現(xiàn)明顯的晃動情況。

綜上可知，本次實(shí)驗(yàn)結(jié)論如下：

1)基于模型降階與鏈?zhǔn)浇Y(jié)構(gòu)的機(jī)械臂位姿控制方法在坡面運(yùn)動狀態(tài)下，不能有效控制實(shí)際步長與平坦直行步長之間的差值，不滿足維持機(jī)器人穩(wěn)定運(yùn)動行為的實(shí)際應(yīng)用需求；

2)基于激光雷達(dá)的多關(guān)節(jié)機(jī)器人姿態(tài)自動控制方法能夠較好控制上、下坡步長與平坦直行步長之間的差值水平，對于維持機(jī)器人穩(wěn)定運(yùn)動行為能夠起到明顯的促進(jìn)作用。

5 結(jié)束語

本文在激光雷達(dá)定位導(dǎo)航技術(shù)的基礎(chǔ)上，建立CPG單元振蕩器模型，又根據(jù)運(yùn)動步態(tài)路線表達(dá)式，求解動力學(xué)方程，再按照運(yùn)動坐標(biāo)轉(zhuǎn)換原則，調(diào)節(jié)反饋控制器閉環(huán)的連接狀態(tài)。與基于模型降階與鏈?zhǔn)浇Y(jié)構(gòu)的機(jī)械臂位姿控制方法相比，本文所設(shè)計(jì)的方法能夠避免機(jī)器人坡面行進(jìn)姿態(tài)與平坦地形直行姿態(tài)出現(xiàn)較大偏差，更符合維持機(jī)器人穩(wěn)定運(yùn)動行為的實(shí)際應(yīng)用需求。此外，這種新型控制算法還能夠準(zhǔn)確辨別出機(jī)器人的上坡與下坡運(yùn)動行為，并對其進(jìn)行分別控制，這也是機(jī)器人在整個(gè)運(yùn)動過程中不出現(xiàn)明顯晃動行為的原因。