三維環(huán)境下基于反步法的多機(jī)器人編隊(duì)控制

馮 磊，肖伸平

（湖南工業(yè)大學(xué) 電氣與信息工程學(xué)院，湖南 株洲 412007）

三維環(huán)境下基于反步法的多機(jī)器人編隊(duì)控制

馮 磊，肖伸平

（湖南工業(yè)大學(xué) 電氣與信息工程學(xué)院，湖南 株洲 412007）

針對兩輪式移動機(jī)器人在復(fù)雜環(huán)境下的編隊(duì)控制問題，提出一種基于虛構(gòu)領(lǐng)航法和反步法，并結(jié)合人工勢場法策略的多機(jī)器人避障編隊(duì)算法。首先，詳細(xì)分析多機(jī)器人系統(tǒng)在三維空間下的編隊(duì)模型，并利用空間投影方法將其映射到二維平面進(jìn)行分析。其次，將運(yùn)動學(xué)模型轉(zhuǎn)化為鏈?zhǔn)叫问剑⑼ㄟ^正則坐標(biāo)變換,將誤差系統(tǒng)形式轉(zhuǎn)換成串聯(lián)非線性系統(tǒng)。然后運(yùn)用Backstepping方法構(gòu)造輪式機(jī)器人追蹤系統(tǒng)的Lyapunov函數(shù)，設(shè)計(jì)出針對輪式機(jī)器人的軌跡跟蹤控制器。再結(jié)合人工勢場法避障策略，完成多機(jī)器人復(fù)雜環(huán)境下的編隊(duì)任務(wù)。最后，通過多機(jī)器人軌跡跟蹤的兩組仿真實(shí)驗(yàn)，驗(yàn)證了所提出方法的有效性。

三維空間；人工勢場法；反步法；李雅普諾夫函數(shù)；編隊(duì)控制

1 研究背景

近年來，隨著機(jī)器人技術(shù)的發(fā)展，多機(jī)器人的穩(wěn)定控制和軌跡跟蹤問題越來越受到國內(nèi)外學(xué)者的關(guān)注[1]。相對于穩(wěn)定問題，軌跡跟蹤是一個(gè)更實(shí)際的控制問題。而編隊(duì)往往將面對復(fù)雜的環(huán)境，因此，在障礙物環(huán)境下，迫切需要尋找一條從起始位置到達(dá)目標(biāo)位置的避障路徑。而路徑規(guī)劃中的人工勢場法以其數(shù)學(xué)計(jì)算簡單明了而被廣泛應(yīng)用。目前，國內(nèi)外學(xué)者進(jìn)行編隊(duì)研究的機(jī)器人主要有地面自主移動機(jī)器人、水下自主式機(jī)器人、衛(wèi)星和無人飛行器等[2-3]；多機(jī)器人的控制算法主要包括虛擬結(jié)構(gòu)法、領(lǐng)航跟隨法、圖論法和基于行為的方法[4-6]。

在當(dāng)前的研究中，文獻(xiàn)[7]綜合路徑跟蹤法和虛擬結(jié)構(gòu)法，實(shí)現(xiàn)了多機(jī)器人系統(tǒng)的動態(tài)編隊(duì)控制。其缺點(diǎn)是其虛擬結(jié)構(gòu)運(yùn)動的隊(duì)形要求限制了該方法的應(yīng)用范圍，難以實(shí)現(xiàn)靈活的隊(duì)形控制。文獻(xiàn)[8]根據(jù)領(lǐng)航機(jī)器人和跟隨機(jī)器人之間的相對運(yùn)動模型，構(gòu)造了一種魯棒編隊(duì)控制器，有效地解決了多機(jī)器人對象模型中的參數(shù)不確定性問題。文獻(xiàn)[9]中基于圖論法和李雅普諾夫函數(shù)提出一種多智能體分布式編隊(duì)控制的理論框架，通過分布式控制所有機(jī)器人來實(shí)現(xiàn)整個(gè)系統(tǒng)的編隊(duì)控制。文獻(xiàn)[10]提出一種帶隊(duì)形反饋的分布式動態(tài)編隊(duì)控制算法，該算法將虛結(jié)構(gòu)法和行為法相融合，通過分段設(shè)定編隊(duì)的虛構(gòu)位置達(dá)到編隊(duì)控制的目的。但未明確指出達(dá)到整體行為的局部控制規(guī)則，很難保證隊(duì)形控制的穩(wěn)定性。文獻(xiàn)[11]基于人工勢場法提出一種編隊(duì)控制器，可在不完整的傳感器信息或者機(jī)器人出現(xiàn)故障的情況下應(yīng)用。但是其勢場函數(shù)的設(shè)計(jì)比較困難，而且存在局部極值點(diǎn)的問題。

盡管機(jī)器人編隊(duì)控制研究近年來有了較大進(jìn)展，但仍有不少問題有待于進(jìn)一步研究。比如：沒有統(tǒng)一有效的框架描述機(jī)器人控制與編隊(duì)算法，缺乏適應(yīng)性和靈活性。穩(wěn)定性是編隊(duì)控制的重要因素，有些控制方法從理論上很難得到保證。實(shí)際運(yùn)動環(huán)境中機(jī)器人往往呈現(xiàn)動態(tài)的性質(zhì)，因此三維環(huán)境下移動機(jī)器人編隊(duì)避障控制問題的研究具有一定的迫切性。

本文基于輪式移動機(jī)器人三維空間下的編隊(duì)運(yùn)動學(xué)模型，將其映射到二維平面進(jìn)行分析，并結(jié)合虛構(gòu)法和領(lǐng)航跟隨法，將領(lǐng)航機(jī)器人的位姿參數(shù)轉(zhuǎn)換生成虛擬機(jī)器人的位姿參數(shù)，再利用反步法和李雅普諾夫函數(shù)設(shè)計(jì)該虛擬位姿的軌跡跟蹤控制器，結(jié)合人工勢場法，以解決多機(jī)器人在復(fù)雜環(huán)境下的編隊(duì)避障控制問題。本文設(shè)計(jì)了2組仿真實(shí)驗(yàn)，實(shí)現(xiàn)復(fù)雜環(huán)境下的經(jīng)典三角形和動態(tài)變換隊(duì)形的編隊(duì)避障控制，驗(yàn)證了控制器的性能及所提方法策略的有效性與穩(wěn)定性。

2 人工勢場法原理

人工勢場法是將復(fù)雜環(huán)境信息轉(zhuǎn)化為斥力場和引力場模型，并通過此模型來找出一條從初始點(diǎn)到目標(biāo)點(diǎn)的路徑的方法，是路徑規(guī)劃傳統(tǒng)算法中較高效且成熟的規(guī)劃方法。

設(shè)目標(biāo)的位置信息為qo=[xoyo]T，機(jī)器人的位置信息為q=[x y]T，第i障礙物的位置信息為qg(i)=[xg(i) yg(i)]T，則機(jī)器人的斥力勢場函數(shù)可表達(dá)為[12]：

引力勢場函數(shù)的表達(dá)式為

斥力函數(shù)的表達(dá)式為

引力函數(shù)的表達(dá)式為

式（1）～（4）中：d為機(jī)器人和障礙物間的最短距離；

d0為障礙物的影響距離范圍；

kr為斥力正比例因子；

ka為引力比例系數(shù)。

3 總體方案設(shè)計(jì)

兩輪式移動機(jī)器人模型的三維坐標(biāo)結(jié)構(gòu)如圖1所示。其中，x, y, z為機(jī)器人在三維空間的X，Y和Z坐標(biāo)；為機(jī)器人的俯仰角；為機(jī)器人的橫搖角；為機(jī)器人的方位角。

通過對機(jī)器人模型在三維空間中的分析與計(jì)算，并對x, y, z和求導(dǎo)，得到的運(yùn)動學(xué)模型為

式中w和v為機(jī)器人的角速度和線速度。

再根據(jù)空間投影方法，將運(yùn)動模型映射到x-y坐標(biāo)系統(tǒng)。在本研究中，為便于設(shè)計(jì)和分析，將兩軸的中心和機(jī)器人的重心重合，因此輪式機(jī)器人的運(yùn)動學(xué)模型可以簡化為

式中 v′=vcos，為機(jī)器人在x-y坐標(biāo)下的線速度。

多機(jī)器人模型的三維坐標(biāo)編隊(duì)結(jié)構(gòu)如圖2所示。編隊(duì)隊(duì)伍的主要軌跡為領(lǐng)航機(jī)器人的運(yùn)動軌跡，每個(gè)跟隨機(jī)器人的期望位姿參數(shù)決定其編隊(duì)隊(duì)形，機(jī)器人均為同樣的運(yùn)動學(xué)模型。

圖2中，R1是領(lǐng)航機(jī)器人，Rv為期望虛擬機(jī)器人，相對領(lǐng)航者R1的期望距離與角度分別為和，R2和R3為跟隨機(jī)器人。[x1y1z1α1β1γ1]T，[xfyfzfαfβfγf]T和[xvyvzvαvβvγv]T分別為R1，R2和Rv的構(gòu)成向量。

Rv的前進(jìn)速度和角速度與R1保持一致。為了簡化控制器的設(shè)計(jì),通過坐標(biāo)變換后，機(jī)器人R2的運(yùn)動學(xué)模型表達(dá)式轉(zhuǎn)換成鏈?zhǔn)叫问剑ζ淝髮?dǎo)可得:

式中：u1, u2為R2變換后的控制輸入變量；

x1, x2, x3為R2變換后的狀態(tài)變量。

同理，虛擬機(jī)器人Rv的運(yùn)動學(xué)模型表達(dá)式轉(zhuǎn)換成如下鏈?zhǔn)叫问剑?/p>

式中：uv1, uv2為Rv變換后的控制輸入；

xv1, xv2, xv3為Rv變換后的狀態(tài)變量。

此時(shí)，R2控制輸入表達(dá)式的變換方程可表示為

式中v, w為R2在x-y坐標(biāo)下的線速度和角速度。

變換后的跟蹤誤差可定義為xei=xi-xvi，i=1, 2, 3；并對xei進(jìn)行求導(dǎo)可以得出：

由此，將機(jī)器人的編隊(duì)問題轉(zhuǎn)化為跟隨機(jī)器人對虛擬機(jī)器人軌跡的跟蹤問題，因此需要尋找合適的控制律u1, u2，使得跟蹤誤差xei漸近收斂到0。

4 控制器設(shè)計(jì)及控制策略

首先，由式（10）形成的控制系統(tǒng)通過坐標(biāo)變換轉(zhuǎn)換成一個(gè)三角形式，形成如下控制律，并對其求導(dǎo)可得:

式中z1, z2, z3均為誤差狀態(tài)變量。

然后通過使用反步法，逐步遞推出各子系統(tǒng)的控制量。

令虛擬控制量z2的期望值為f1，誤差變量可定義為=z2-f1(z1) ，通過觀察并計(jì)算得知，當(dāng)z3=0時(shí)，f1(z1) =0是系統(tǒng)的穩(wěn)定方程。

綜合上述遞推出的各子系統(tǒng)控制量，可以得到如下控制律：

課題組采用的避障策略為人工勢場法，其核心思想是：將機(jī)器人在物理上的運(yùn)動轉(zhuǎn)化為一種其在虛擬的人工受力場中的運(yùn)動。目標(biāo)點(diǎn)對機(jī)器人產(chǎn)生引力,障礙物對機(jī)器人產(chǎn)生斥力，將所有的引力和斥力的分力進(jìn)行合并，最終合力作為機(jī)器人的控制力，從而實(shí)現(xiàn)機(jī)器人安全繞過障礙物到達(dá)目標(biāo)位置。

課題組采用的編隊(duì)控制策略為：在該編隊(duì)控制算法基礎(chǔ)上，結(jié)合人工勢場法，實(shí)現(xiàn)多機(jī)器人在復(fù)雜環(huán)境下編隊(duì)控制的任務(wù)。首先將三維空間下的編隊(duì)運(yùn)動學(xué)模型映射到二維平面進(jìn)行分析；然后結(jié)合本編隊(duì)控制器算法完成三維環(huán)境下的編隊(duì)任務(wù)；當(dāng)遇到外部障礙物的干擾時(shí)，智能切換為本文所提出的人工勢場法避障策略；避障策略區(qū)域范圍及障礙物影響范圍可依據(jù)具體情況設(shè)定；當(dāng)機(jī)器人脫離避障策略區(qū)域范圍后，自動切換成編隊(duì)控制模式，繼續(xù)實(shí)現(xiàn)編隊(duì)任務(wù)。

5 控制系統(tǒng)仿真

通過2個(gè)仿真實(shí)驗(yàn)來驗(yàn)證本文算法的有效性及可行性。仿真實(shí)驗(yàn)中，各輪式機(jī)器人控制器的參數(shù)為：k1=0.8，k2=1.2，控制周期T=50.0 ms。仿真機(jī)器人都裝配有虛擬紅外傳感器與無線傳感器，可以實(shí)時(shí)檢測障礙物的位置并獲得機(jī)器人的位置與角度等狀態(tài)信息，R1為領(lǐng)航機(jī)器人，Rv為虛擬機(jī)器人，R2～R5為跟隨機(jī)器人。

仿真實(shí)驗(yàn)1。圖3中，5個(gè)機(jī)器人從不同的初始位置出發(fā)，在三維空間中，途中受到障礙物外部干擾的情況下，進(jìn)行三角形編隊(duì),并且保持隊(duì)形進(jìn)行直線運(yùn)動。每個(gè)機(jī)器人的初始位置、運(yùn)動軌跡和編隊(duì)參數(shù)如圖3a所示，R2～R5機(jī)器人的編隊(duì)參數(shù)分別是[0.5, -π/6]，[0.5, π/6]，[1.0, -π/6]和[1.0, π/6]。各跟隨機(jī)器人的編隊(duì)位置誤差和角度誤差如圖3b和3c所示。R1領(lǐng)航機(jī)器人的前進(jìn)速度和轉(zhuǎn)向角速度為常量，進(jìn)入編隊(duì)穩(wěn)定狀態(tài)時(shí)，各機(jī)器人的前進(jìn)速度和轉(zhuǎn)向角速度都相同。從仿真圖中可以看出，多機(jī)器人在三維空間中受到障礙物外部干擾的情況下，能快速回歸編隊(duì)狀態(tài)，且保持極小誤差，驗(yàn)證出跟隨機(jī)器人的跟蹤性能的有效性、穩(wěn)定性和容錯(cuò)性。

仿真2。圖4中，4個(gè)機(jī)器人從不同的初始位置出發(fā)，在三維空間中，途中受到障礙物外部干擾的情況下，機(jī)器人 R1做圓周運(yùn)動，其余跟隨機(jī)器人完成編隊(duì)及變換隊(duì)形的任務(wù)。首先進(jìn)行菱形編隊(duì)，然后切換為并排一字隊(duì)形，最后切換為正方形隊(duì)形。每個(gè)機(jī)器人的初始位置、運(yùn)動軌跡和編隊(duì)參數(shù)如圖4a所示，R2, R3和R4機(jī)器人的編隊(duì)參數(shù)分別為[0.5, -π/4]，[0.5, π/4]，[0.707, 0]。各跟隨機(jī)器人的編隊(duì)位置誤差和角度誤差如圖4b和圖4c所示。從圖中可以看出，三維空間中，多機(jī)器人不僅在切換隊(duì)形時(shí)，即使當(dāng)前位姿相對期望位姿會有較大的誤差，都能快速平滑地完成變換隊(duì)形的編隊(duì)控制。而且在存在外部干擾的情況下，依然能完成動態(tài)編隊(duì)的任務(wù)，并保持較小的誤差。由實(shí)驗(yàn)結(jié)果可知，該控制器可以實(shí)現(xiàn)復(fù)雜環(huán)境下的動態(tài)變換隊(duì)形的編隊(duì)控制。

6 結(jié)語

課題組在前期研究的基礎(chǔ)上，完成控制器在三維環(huán)境中，存在障礙物外部干擾的情況下的多機(jī)器人編隊(duì)避障控制的研究，并提出一種控制策略和控制算法，最后進(jìn)行了2組仿真驗(yàn)證實(shí)驗(yàn)。仿真結(jié)果表明，提出的編隊(duì)控制算法和避障策略，能順利完成復(fù)雜環(huán)境下的動態(tài)編隊(duì)及避障控制，容錯(cuò)性強(qiáng)，精度高，具有良好的可靠性。后繼將對在動態(tài)障礙物環(huán)境下如何實(shí)現(xiàn)優(yōu)化路徑規(guī)劃和通信失敗對編隊(duì)控制的影響等內(nèi)容進(jìn)行進(jìn)一步研究。

[1]GANGULI A，CORTES J，BULLO F.Multirobot Rendezvous with Visibility Sensors in Nonconvex Environments[J].IEEE Trans on Robotics，2009，25(2)：340-352.

[2]楊 維，朱文球，張長隆.基于RGB-D相機(jī)的無人機(jī)快速自主避障[J].湖南工業(yè)大學(xué)學(xué)報(bào)，2015，29(6)：74-79.

YANG Wei，ZHU Wenqiu，ZHANG Changlong.UAV Autonomous Obstacle Avoidance Based on RGB-D Camera[J].Journal of Hunan University of Technology，2015，29(6)：74-79.

[3]王保防，張瑞雷，李 勝，等.基于軌跡跟蹤車式移動機(jī)器人編隊(duì)控制[J].控制與決策，2015，30(1)：176-180.

WANG Baofang，ZHANG Ruilei，LI Sheng，et al.Based on the Trajectory Tracking Vehicle Type Mobile Robot Formation Control[J].Control and Decision，2015，30(1)：176-180.

[4]WANG X，YU C，LIN Z.A Dual Quaternion Solution to Attitude and Position Control for Rigid-Body Coordination[J].IEEE Trans on Robotics，2012，28(5)：1162-1170.

[5]張瑞雷，李 勝，陳慶偉.車式移動機(jī)器人動態(tài)編隊(duì)控制方法[J].機(jī)器人，2013，35(6)：651-656.

ZHANG Ruilei，LI Sheng，CHEN Qingwei.Dynamic Formation Control Method for Vehicle Type Mobile Robot[J].Robot，2013，35(6)：651-656.

[6]李 勝，馬國梁，胡維禮.基于 Backstepping 方法的輪式移動機(jī)器人軌跡追蹤控制[J].東南大學(xué)學(xué)報(bào)（自然科學(xué)版），2005，35(2)：248-252.

LI Sheng，MA Guoliang，HU Weili.Trajectory Tracking Control of Wheeled Mobile Robot Based on Backstepping Method[J].Journal of Southeast University（Natural Science Edition），2005，35(2)：248-252.

[7]TIAN Y，ZHANG Y.High-Order Consensus of Heterogeneous Multi-Agent Systems with Unknown Communication Delays[J].Automatica，2012，48(6)：1205-1212.

[8]楊 帆，劉士榮，董德國.編隊(duì)控制中的機(jī)器人行為與基于服務(wù)的運(yùn)動行為結(jié)構(gòu)設(shè)計(jì)[J].機(jī)器人，2012，34(1)：120-128.

YANG Fan，LIU Shirong，DONG Deguo.Robot Formation Control Based on Motion Behavior and Structure Design Services[J].Robot，2012，34(1)：120-128.

[9]OLFATI S R，MURRAY R M.Distributed Structural Stabilization and Tracking for Formations of Dynamics Multi-Agents[C]// Proceedings of the 41st IEEE Conference on Decision and Control.Las Vegas：[s.n.]，2002：209-215.

[10]SHEN Kuihua，TAN Yuegang.A Solving Approach of Partial Minimum Problem on Artificial Potential Field Theory[J].Transaction of University of Science and Technology of Wuhan，2005，27(8)：77-79.

[11]DUDENHOEFFER D D，JONES M P.A Formation Behavior for Large-Scale Micro-Robot Force Deployment[C]// Proceedings of the 2000 Winter Simulation Conference.Orlando：[s.n.]，2000：972-982.

(責(zé)任編輯：申 劍)

Multi-Robot Formation Control Based on Backstepping Under the 3D Environment

FENG Lei，XIAO Shenping
（School of Electrical and Information Engineering，Hunan University of Technology，Zhuzhou Hunan 412007，China）

In view of the existing problems in the formation of dual-wheeled mobile robots under threedimensional environment, an obstacle avoidance multi-robot formation algorithm, based on methods of fictitious navigation and backstepping, and combined with the artificial potential field method, has thus been proposed.First, an analysis has been made of the formation model of the multi-robot system in three-dimensional environment, followed by an analysis of the spatial projection of the model onto a two-dimensional plane.Second, the kinematics model will be transformed into a chained form, where the error system will be transformed into a series nonlinear system through a canonical coordinate transformation.Third, Lyapunov function of the wheeled robot tracking system will be constructed by using backstepping method and a design of the trajectory tracking controller for wheeled robots will be obtained.The multi-robot formation in complicated environments, combined with the artificial potential field obstacle avoidance strategy, will be accomplished.Finally, the effectiveness of the proposed method will be verified by two groups of simulation experiments of multi-robot trajectory tracking.

three-dimensional space；method of artificial potential field；backstepping；Lyapunov function；formation control

TP273

A

1673-9833(2017)01-0069-06

10.3969/j.issn.1673-9833.2017.01.013

2016-12-18

國家自然科學(xué)基金資助項(xiàng)目（61672225），國家火炬計(jì)劃基金資助項(xiàng)目（2015GH712901），湖南省教育廳科研基金資助重點(diǎn)項(xiàng)目（14A038）

馮 磊（1991-）男，河南焦作人，湖南工業(yè)大學(xué)碩士生，主要研究方向?yàn)橹悄軝z測與控制，E-mail：1341497188@qq.com

肖伸平（1965-）男，湖南東安人，湖南工業(yè)大學(xué)教授，主要從事時(shí)滯系統(tǒng)魯棒控制，電力系統(tǒng)穩(wěn)定性分析及網(wǎng)絡(luò)化控制等方面的研究，E-mail：xsph_519@163.com