[發明專利]一種基于李雅普諾夫函數的移動機器人視覺伺服控制方法

有效

• 申請號: 202110165675.1
• 申請日: 2021-02-06
• 公開（公告）號: CN113031590B
• 公開（公告）日: 2022-09-23
• 發明（設計）人: 林葉貴;邢科新;何德峰;邢明
• 申請（專利權）人: 浙江同筑科技有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02;G06F30/20
• 代理公司: 杭州浙科專利事務所(普通合伙) 33213
• 代理人: 周紅芳

1.一種基于李雅普諾夫函數的移動機器人視覺伺服控制方法，其特征在于，包括如下步驟：

步驟1)建立機器人運動學模型:

1.1)機器人運動學模型表示為:

式中q＝[x,y,θ]^T為機器人的狀態，x和y為機器人的中心坐標，θ為移動機器人的方向角，v(t)為移動機器人的線速度、ω(t)為移動機器人的角速度，這里選擇移動機器人的線速度v(t)和角速度ω(t)作為機器人的控制輸入；

1.2)假定以空間中的4個特征點作為目標點，根據投影模型，二維圖像坐標p＝[p_x,p_y]^T由下式得到：

P_x＝fx_c/y_c，P_y＝fz_c/y_c (2)

其中，f為相機的焦距，x_c、y_c、z_c表示特征點在攝像機下的三維空間坐標；通過上式，將三維歐幾里得空間中的目標點投影到圖像平面中對應的點；

1.3)視覺伺服穩定化是利用視覺信息驅動移動機器人到達目標圖像，當移動機器人穩定到期望目標時，當前誤差和期望圖像誤差收斂到零；定義目標特征點P(P_xd,P_yd)，θ_d為移動機器人到達目標位置的角度，那么二維圖像坐標下的誤差模型(s₁,s₂,s₃)表示為：

以及期望的坐標為s_d＝[P_xd/P_yd -f/P_ydθ_d]^T；

1.4)定義圖像坐標和姿態角的視覺伺服穩定誤差為：

以及變換矩陣：

式中θ_e＝θ-θ_d為移動機器人視覺伺服穩定化的角度誤差；e₁、e₂、e₃三個量表示二維圖像坐標下構建的誤差模型；s_1d、s_2d、s_3d三個量表示二維圖像坐標下期望的機器人位姿；

1.5)考慮帶攝像頭的機器人與目標點的關系，通過誤差向量e與相關坐標系的正交變換，將式(1)組合得到誤差系統的微分方程為：

其中，h＝1/z_c為非零常數，因為相機高度不為零，即z_c≠0，e(t)為一個誤差向量e(t)＝[e₁(t),e₂(t),e₃(t)]^T；l是移動機器人質心和兩個后輪軸線中心的距離；

1.6)根據式(4)和式(5)，當誤差e→0時，移動機器人將到達如式(7)所示的目標狀態；

s₁→s_1d,s₂→s_2d,θ→θ_d (7)

由式(3)和式(7)可以看出，圖像坐標P_x→P_xd,P_y→P_yd,以及θ_e→0，因此，需要設計一種移動機器人視覺伺服穩定控制器u(t)＝u(e(t))使lim_t→∞e(t)＝0以及lim_t→∞u(t)＝0；

步驟2)針對非完整移動機器人視覺伺服系統，設計控制器，過程如下：

2.1)將式(6)離散化得到NMR的離散時間非線性視覺伺服誤差模型如下：

T_s0是一個足夠小的采樣間隔，控制輸入u＝[v ω]^T；

2.2)輸入矩陣為：

2.3)引入定義1，考慮系統x⁺＝f(x)+g(x)u,這里x⁺為連續的x，如果存在一個反饋控制律K(x)，導致系統x⁺＝f(x)+g(x)K(x)是在原點漸近穩定,則認為系統在控制輸入u＝K(x)下在原點附近漸進穩定；

2.4)引入定義2，考慮一個光滑的、正定的正函數V:Rⁿ→R₊，以及系統x⁺＝f(x)+g(x)u,這里x⁺為連續的x，Rⁿ表示N維歐式空間、R₊正實數空間；如果函數V(x)滿足：

則V(x)是系統的一個可控的李亞普諾夫函數CLF；

定義一個正定矩陣P,使V(x)＝x^TPx,即存在一個二次的CLF非線性函數如下：

V(e)＝λ₁e₁²+λ₂e₂²+λ₃e₃²＝e^TPe (10)

其中對角矩陣P＝diag{λ₁,λ₂,λ₃},λ₁,λ₂,λ₃0.為簡單起見，設e⁺是當前誤差e的連續e⁺＝e(k+1)＝e(k)+g(e(k))u(k)；

2.5)引入定理1，考慮式(8)和式(10)中的函數V(e)，存在一個非空的,不變集和反饋控制律u＝[u₁,u₂]^T

這樣，對于任何的0μ₁,μ₂1，V(e)是局部李亞普諾夫函數CLF且系統是漸進穩定的；

步驟3)非完整移動機器人視覺伺服系統控制器穩定性證明，過程如下：

3.1)對V(e)的閉環軌跡進行微分運算,得到：

式中，g_ij為矩陣g的第(i,j)個元素，i,j＝1,2,3，P為一個正定的對角矩陣，λ_i為對角矩陣的表示值，e_i為式(4)里面的誤差量；為了清晰起見，令：

a₁＝∑λ_ie_ig_i1＝λ₂hT_se₂

a₂＝∑λ_ie_ig_i2＝[λ₁e₁lh+e₁e₂(λ₂-λ₁)+λ₃e₃]T_s

將式(13)代入式(12)可得

再將式(11)代入式(14)得到：

對于離散時間視覺伺服系統，定義一個集合：

原點在集合S的內部，因此，S是非空的，任意選擇一個r0使得盡可能的接近S，當k≥0,對于任意的V(e(k+1))-V(e(k))0，因此,是離散時間視覺伺服在閉環系統的不變集合，此外，由于V(e)是式(8)的二次CLF，因此式(8)和式(10)對于任何0μ₁,μ₂1，都是漸近穩定的；

3.2)由式(15)和式(13)，可知在集合中遞減的速度V(e)依賴于λ＝(λ₁,λ₂,λ₃)以及μ＝(μ₁,μ₂)，的值，如果V(e)看作是移動機器人的視覺伺服系統的能量函數,其下降速度可以反映成穩態誤差信號e(k)的收斂率，因此,可以找到一組最優參數λ和μ使收斂穩定誤差信號達到最大滿意度；然而,相互作用之間的這些參數值和V(e)的減少速度太過復雜獲得顯式條件由于強烈的非線性；此外，為了保證伺服過程中目標特征點在攝像機范圍內和移動機器人安全快速的驅動，在控制器的設計中必須考慮一些約束條件；

一個約束是可見性約束，它使可視特性始終可見；這里使用e₁與e₂的誤差限制來表示可見性約束，即：

其中，e_1,min和e_2,min分別為e₁和e₂的下界，e_1,max和e_2,max分別為e₁和e₂的上界，另一個重要的約束是執行機構在線速度和角速度上的約束，即：

其中，v_min和w_min分別為移動機器人線速度v和移動機器人角速度w的下界，v_max和w_max分別為移動機器人線速度v和移動機器人角速度w的上界；

為了找到一組最優參數并滿足約束條件式(17)和式(18)，一個有效的方法是根據預先定義的代價函數優化這些參數，為此，結合式(8)并定義在一個足夠大的時間水平窗口N上的代價函數為：

其中R0是控制輸入的加權矩陣，P＝diag{λ₁,λ₂,λ₃}可以看作是誤差的加權矩陣，然后對于任意e(0)，定義以下最優控制問題來確定參數μ：

其中，函數u(e,λ,μ)由式(11)給出，集合U由式(18)定義，集合D＝(0,1)×(0,1)；從定理1可知,存在至少一組μ滿足式(20)約束問題,也就是說,對于任何初始狀態問題始終是可行的；使用數值算法,可得式(20)的最優解。