三維環境下基于反步法的多機器人編隊控制

馮 磊，肖伸平

（湖南工業大學 電氣與信息工程學院，湖南 株洲 412007）

三維環境下基于反步法的多機器人編隊控制

馮 磊，肖伸平

（湖南工業大學 電氣與信息工程學院，湖南 株洲 412007）

針對兩輪式移動機器人在復雜環境下的編隊控制問題，提出一種基于虛構領航法和反步法，并結合人工勢場法策略的多機器人避障編隊算法。首先，詳細分析多機器人系統在三維空間下的編隊模型，并利用空間投影方法將其映射到二維平面進行分析。其次，將運動學模型轉化為鏈式形式，并通過正則坐標變換,將誤差系統形式轉換成串聯非線性系統。然后運用Backstepping方法構造輪式機器人追蹤系統的Lyapunov函數，設計出針對輪式機器人的軌跡跟蹤控制器。再結合人工勢場法避障策略，完成多機器人復雜環境下的編隊任務。最后，通過多機器人軌跡跟蹤的兩組仿真實驗，驗證了所提出方法的有效性。

三維空間；人工勢場法；反步法；李雅普諾夫函數；編隊控制

1 研究背景

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

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

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

本文基于輪式移動機器人三維空間下的編隊運動學模型，將其映射到二維平面進行分析，并結合虛構法和領航跟隨法，將領航機器人的位姿參數轉換生成虛擬機器人的位姿參數，再利用反步法和李雅普諾夫函數設計該虛擬位姿的軌跡跟蹤控制器，結合人工勢場法，以解決多機器人在復雜環境下的編隊避障控制問題。本文設計了2組仿真實驗，實現復雜環境下的經典三角形和動態變換隊形的編隊避障控制，驗證了控制器的性能及所提方法策略的有效性與穩定性。

2 人工勢場法原理

人工勢場法是將復雜環境信息轉化為斥力場和引力場模型，并通過此模型來找出一條從初始點到目標點的路徑的方法，是路徑規劃傳統算法中較高效且成熟的規劃方法。

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

引力勢場函數的表達式為

斥力函數的表達式為

引力函數的表達式為

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

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

kr為斥力正比例因子；

ka為引力比例系數。

3 總體方案設計

兩輪式移動機器人模型的三維坐標結構如圖1所示。其中，x, y, z為機器人在三維空間的X，Y和Z坐標；為機器人的俯仰角；為機器人的橫搖角；為機器人的方位角。

通過對機器人模型在三維空間中的分析與計算，并對x, y, z和求導，得到的運動學模型為

式中w和v為機器人的角速度和線速度。

再根據空間投影方法，將運動模型映射到x-y坐標系統。在本研究中，為便于設計和分析，將兩軸的中心和機器人的重心重合，因此輪式機器人的運動學模型可以簡化為

式中 v′=vcos，為機器人在x-y坐標下的線速度。

多機器人模型的三維坐標編隊結構如圖2所示。編隊隊伍的主要軌跡為領航機器人的運動軌跡，每個跟隨機器人的期望位姿參數決定其編隊隊形，機器人均為同樣的運動學模型。

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

Rv的前進速度和角速度與R1保持一致。為了簡化控制器的設計,通過坐標變換后，機器人R2的運動學模型表達式轉換成鏈式形式，并對其求導可得:

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

x1, x2, x3為R2變換后的狀態變量。

同理，虛擬機器人Rv的運動學模型表達式轉換成如下鏈式形式：

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

xv1, xv2, xv3為Rv變換后的狀態變量。

此時，R2控制輸入表達式的變換方程可表示為

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

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

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

4 控制器設計及控制策略

首先，由式（10）形成的控制系統通過坐標變換轉換成一個三角形式，形成如下控制律，并對其求導可得:

式中z1, z2, z3均為誤差狀態變量。

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

令虛擬控制量z2的期望值為f1，誤差變量可定義為=z2-f1(z1) ，通過觀察并計算得知，當z3=0時，f1(z1) =0是系統的穩定方程。

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

課題組采用的避障策略為人工勢場法，其核心思想是：將機器人在物理上的運動轉化為一種其在虛擬的人工受力場中的運動。目標點對機器人產生引力,障礙物對機器人產生斥力，將所有的引力和斥力的分力進行合并，最終合力作為機器人的控制力，從而實現機器人安全繞過障礙物到達目標位置。

課題組采用的編隊控制策略為：在該編隊控制算法基礎上，結合人工勢場法，實現多機器人在復雜環境下編隊控制的任務。首先將三維空間下的編隊運動學模型映射到二維平面進行分析；然后結合本編隊控制器算法完成三維環境下的編隊任務；當遇到外部障礙物的干擾時，智能切換為本文所提出的人工勢場法避障策略；避障策略區域范圍及障礙物影響范圍可依據具體情況設定；當機器人脫離避障策略區域范圍后，自動切換成編隊控制模式，繼續實現編隊任務。

5 控制系統仿真

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

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

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

6 結語

課題組在前期研究的基礎上，完成控制器在三維環境中，存在障礙物外部干擾的情況下的多機器人編隊避障控制的研究，并提出一種控制策略和控制算法，最后進行了2組仿真驗證實驗。仿真結果表明，提出的編隊控制算法和避障策略，能順利完成復雜環境下的動態編隊及避障控制，容錯性強，精度高，具有良好的可靠性。后繼將對在動態障礙物環境下如何實現優化路徑規劃和通信失敗對編隊控制的影響等內容進行進一步研究。

[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].湖南工業大學學報，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].控制與決策，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].機器人，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].東南大學學報（自然科學版），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]楊 帆，劉士榮，董德國.編隊控制中的機器人行為與基于服務的運動行為結構設計[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.

(責任編輯：申 劍)

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

國家自然科學基金資助項目（61672225），國家火炬計劃基金資助項目（2015GH712901），湖南省教育廳科研基金資助重點項目（14A038）

馮 磊（1991-）男，河南焦作人，湖南工業大學碩士生，主要研究方向為智能檢測與控制，E-mail：1341497188@qq.com

肖伸平（1965-）男，湖南東安人，湖南工業大學教授，主要從事時滯系統魯棒控制，電力系統穩定性分析及網絡化控制等方面的研究，E-mail：xsph_519@163.com