[發(fā)明專利]一種基于分離式超聲波的移動機器人編隊跟蹤控制方法

有效

• 申請?zhí)枺?/td>: 201910146815.3
• 申請日: 2019-02-27
• 公開（公告）號: CN109828580B
• 公開（公告）日: 2022-05-24
• 發(fā)明（設(shè)計）人: 戴詩陸;歐建永;王敏;李烈軍
• 申請（專利權(quán)）人: 華南理工大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 廣州粵高專利商標代理有限公司 44102
• 代理人: 何淑珍;黃海波

1.一種基于分離式超聲波的移動機器人編隊跟蹤控制方法，其特征在于，包含步驟：

步驟(1)：建立單個移動機器人的運動學模型；

步驟(2)：建立n個移動機器人編隊動態(tài)數(shù)學模型，并考慮實際系統(tǒng)中存在的相對位姿獲取和相對位姿保持的問題；

步驟(3)：建立分離式超聲波獲取相對位姿的模型，并推導出相對位姿的計算公式；

步驟(4)：從編隊跟蹤控制的實際問題出發(fā)，結(jié)合推導出的所述相對位姿的計算公式設(shè)計出使編隊跟蹤誤差漸近收斂的控制規(guī)律；

步驟(5)：選取移動機器人參考路徑，設(shè)置移動機器人參數(shù)與控制器參數(shù)，根據(jù)所述控制規(guī)律實現(xiàn)移動機器人的編隊跟蹤控制；

步驟(1)中，所述的建立單個機器人平面坐標的運動學模型具體包括：

假設(shè)移動機器人在運動過程中不發(fā)生側(cè)滑，故滿足如下條件：

得出移動機器人的運動學模型為：

其中，來描述移動機器人R_i當前所處的位姿狀態(tài)，x_i與y_i是二維坐標系統(tǒng)橫軸縱軸的坐標位置，是機器人R_i運動方向與橫軸的夾角；(v_i,w_i)來描述移動機器人R_i當前所處運動狀態(tài)，其中v_i是移動機器人R_i相對于全局坐標系的線速度，w_i是移動機器人R_i相對于全局坐標系的角速度；

步驟(2)所述建立n個移動機器人編隊動態(tài)數(shù)學模型具體包括：

定義移動機器人跟隨者與領(lǐng)導者的跟隨位姿偏差：

其中，d_i是跟隨者機器人R_i與領(lǐng)導者機器人R_i-1間的距離，θ_i是跟隨者機器人R_i與領(lǐng)導者機器人R_i-1的運動方向角度的偏差；x_i-1，y_i-1是領(lǐng)導者機器人R_i-1當前所處的全局坐標值，x_i，y_i是跟隨者機器人R_i當前所處的全局坐標值，為兩中間變量定義如下公式所示：

然后求取偏差的動態(tài)方程：

其中v_i,w_i是跟隨機器人R_i當前的線速度與角速度，v_i-1,w_i-1是領(lǐng)導者機器人R_i-1當前的線速度與角速度；

步驟(3)中，每個機器人都安裝有一個發(fā)射模塊并放置在機器人中心；每個機器人都會在中心的兩端安裝兩個接收模塊，所述接收模塊之間的距離為2b_i，接收模塊的連線方向與車前進的方向垂直；

所述發(fā)射模塊中同時設(shè)置有紅外發(fā)送器和超聲波發(fā)射器；所述接收模塊中同時設(shè)置有紅外接受器和超聲波接收器，所述紅外發(fā)送、接收器是用于實現(xiàn)時鐘同步，所述發(fā)射模塊同時控制紅外發(fā)送器發(fā)射紅外線以及超聲波發(fā)射器發(fā)射超聲波；

步驟(3)中建立分離式超聲波獲取相對位姿的模型、并推導出相對位姿的計算公式具體包括：

首先通過與常見的集成式超聲波的測距對比，可得知分離式超聲波的優(yōu)點，通過數(shù)學建模后，故確定分離式超聲波測距的計算公式為：

d_measure＝v_sound*t₃≈v_sound*(t₃-t₄)

其中d_measure是測量距離，v_sound是聲音在空氣中的傳播速度，為一個常數(shù)，t₃是整個過程中聲音傳播的時間，t₄是整個過程中輔助的紅外光傳播的時間，t₃與t₄的差值可由超聲波模組中的CPU計算獲得；根據(jù)領(lǐng)導者跟隨者超聲波位置排布進行建模，基于過程中的圖形關(guān)系，由余弦定理得到如下關(guān)系：

結(jié)合上述關(guān)于d_i與θ_i的定義，計算出d_i與θ_i為：

其中，2b_i為跟隨者機器人R_i左右超聲波接收模塊的距離，d_1i和d_2i分別為跟隨者機器人R_i左右超聲波接收模塊測量獲得的與領(lǐng)導機器人R_i-1中心的距離，β₁，β₂為推導過程中的中間變量。

2.根據(jù)權(quán)利要求1所述的方法，其特征在于，步驟(4)具體包括：

設(shè)置移動機器人編隊過程中距離約束與角度約束：

其中，d_max為最大約束距離，d_min為最小距離約束，d_max、d_min都是常數(shù)，由實際的傳感器參數(shù)以及機器人的機械尺寸決定；θ_max為最大角度誤差約束，θ_min為最小角度誤差約束，其中θ_max、θ_min都是常數(shù)且θ_max＝-θ_min；

根據(jù)上述移動機器人跟隨者與領(lǐng)導者的跟隨位姿偏差，推導出動態(tài)誤差方程為：

其中，和分別是機器人R_i與R_i-1運行方向與x軸的夾角；

采用障礙(barrier)李雅普諾函數(shù)的方法進行約束，從穩(wěn)定性理論出發(fā)，設(shè)計編隊跟蹤控制誤差漸近收斂的控制規(guī)律，用于保證跟蹤誤差全局漸近收斂，保證d_i收斂在d_min～d_max范圍內(nèi)，以及θ_i收斂在θ_min～θ_max范圍內(nèi)，故設(shè)定如下所示的李雅普諾夫函數(shù)V_d(d_i)與V_θ(θ_i)為：

其中，β_d與β_θ是一個設(shè)置函數(shù)性能的參數(shù)，d_d與θ_d是d_i與θ_i的期望值，設(shè)置距離偏差與角度偏差的李雅普諾夫函數(shù)分別為V_d,V_θ，通過求解V_d，V_θ對d_i，θ_i的導數(shù)與分別為：

通過估計速度上界的方法，設(shè)置如下所示的控制規(guī)律：

其中，k_d，k_θ，δ_d，δ_θ都是正參數(shù)，是對于領(lǐng)導者機器人速度上界的估計，tanh是雙曲正切函數(shù)，結(jié)合控制規(guī)律以及上述方程，可得：

結(jié)合上述控制規(guī)律與動態(tài)方程，即可得：

通過設(shè)計k_d、k_θ、δ_d、δ_θ都是正參數(shù)，即可使編隊跟蹤誤差漸近收斂。