[發明專利]基于信任域的井下礦用鉸接車軌跡規劃方法

有效

• 申請號: 202111254252.3
• 申請日: 2021-10-27
• 公開（公告）號: CN113885518B
• 公開（公告）日: 2023-08-11
• 發明（設計）人: 李柏;張坦探;歐陽亞坤;方勇;鐘翔
• 申請（專利權）人: 湖南大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 重慶一葉知秋專利代理事務所(普通合伙) 50277
• 代理人: 劉洪雨

1.一種基于信任域的井下礦用鉸接車軌跡規劃方法，其特征在于，包括以下步驟：

步驟一、生成一條粗略的行車軌跡：在明確了行駛的起點和終點前提下，形成一條銜接始末位置的避障軌跡；

步驟二、基于信任域技術求解精細的最優的運動軌跡：構建一個最優控制問題描述軌跡規劃任務，隨后對其進行數值求解，數值求解的過程中引入信任域技術輔助加速計算；

其中，構建一個最優控制問題描述軌跡規劃任務具體為：

拖掛車是由一節拖車頭和若干節掛車順次鉸接而成的多體車輛，為拖掛車的各組成單元分配唯一ID：拖車頭的ID為0，第i節掛車的ID為i，i＝1,...,N_V，N_V代表掛車的總數目，則完整的拖掛車包含N_V+1個組成單元；并設，L_N0為拖車前懸長度，L_M0為拖車后懸長度，L_W0為拖車軸距，φ₀為拖車的前輪轉向角，P₀＝(x₀,y₀)為拖車后輪軸中心點，每一節掛車參考點可指定為其輪軸中心點P_i＝(x_i,y_i)，L_Wi為第i節掛車參考點P_i與前一單元鉸接點H_i-1的距離，i＝1,...,N_V，θ_j為第j部分單元的姿態角，j＝0,...,N_V，H_j是指固定于第j個單元上的鉸接點，j＝0,...,N_V，線段P_jH_j的長度是固定的，反映了鉸接點與輪軸點的距離；L_H0為從拖車后輪中心點P₀到拖車與掛車1的鉸接點H₀之間的這段線段的長度；L_Hi表征的是掛車i的輪軸中心點P_i到掛車i與掛車i+1鉸接點H_i之間的距離；L_H(i-1)表征的是掛車i-1的輪軸中心點P_i-1到掛車i-1與掛車i鉸接點H_i-1之間的距離；

拖車頭在慣性坐標系X-Y中的運動過程受到以下微分方程組的制約：

其中t_f為車輛運動過程的終止時刻，v₀(t)及a₀(t)分別為拖車沿車身縱軸方向的速度及加速度，ω₀(t)為拖車前輪轉角的角速度，與拖車相關的變量存在容許作用區間：

-Φ_max≤φ₀(t)≤Φ_max，

v_min≤v₀(t)≤v_max，

a_min≤a₀(t)≤a_max，

-Ω_max≤ω₀(t)≤Ω_max，t∈[0，t_f]；

假設與拖車固定的鉸接點則可根據拖車姿態角θ₀以及P₀坐標確定H₀位置：

由掛車1的姿態角θ₁以及H₀坐標可確定P₁點坐標：

可使用上述方法逐一確定所有P_j位置：

x_j+1(t)＝x_j(t)-L_W(j+1)·cosθ_j+1(t)-L_Hj·cosθ_j(t),

y_j+1(t)＝y_j(t)-L_W(j+1)·sinθ_j+1(t)-L_Hj·sinθ_j(t),

j＝0,...,N_V

每一節掛車的姿態角θ_i(t)可由下式確定：

在拖掛車系統中，相鄰單元之間的姿態角夾角不宜超過90°，否則車輛系統會陷入彎折狀態(jackknife)而難以恢復，因此可設置以下約束條件：

其中buffer取值0.1rad；

每一節掛車的速度v_i(t)由下式確定：

在拖掛車運動的起始時刻t＝0，應顯示地指定車輛系統所處的運動狀態，即針對變量x₀(0)、y₀(0)、v₀(0)、φ₀(0)、a₀(0)、ω₀(0)、θ_j(0),j＝0,...,N_V賦值，在對上述變量施加點約束后，其余狀態變量，x_i(0)、y_i(0)、v_i(0)，i＝1,...,N_V，都可以唯一確定下來；

在終止時刻t_f，拖掛車系統應達到某一既定運動狀態，一般要求車輛最終穩定地停泊，因此可建立以下約束條件：

v₀(t_f)＝a₀(t_f)＝φ₀(t_f)＝ω₀(t_f)＝0；

為使拖掛車運動到某一指定區域，可以針對x_j(t_f)、y_j(t_f)，j＝0,...,N_V直接施加點約束，

設拖掛車系統中單元j的四個頂點分別為A_j(t)、B_j(t)、C_j(t)以及D_j(t)，則碰撞躲避約束可寫為：

VehicleOutOfPolygon(A_j(t)B_j(t)C_j(t)D_j(t),ο_r(t)),t∈[0,t_f],j＝0,...,N_V,r＝1,...,N_OBS

以及

VehicleOutOfPolygon(A_j(t)B_j(t)C_j(t)D_j(t),Ar(t)Br(t)Cr(t)Dr(t)),t∈[0,t_f],j,r＝0,...,N_V,r≠j；

其中，ο_r(t)記錄著第r個障礙物的頂點在t時刻的位置，N_OBS代表障礙物數目，VehicleOutOfPolygon函數的具體寫法在附件C給出；

拖掛車的全部動力源于其頭部的拖車，因此拖掛車決策規劃任務的代價函數設置方式與剛體車輛類似，可設置：J＝t_f；

將上述所列出的約束條件合并起來，即構成完整的用于描述拖掛車軌跡規劃任務的最優控制問題；

對構建的最優控制問題進行數值求解，最優控制問題可抽象地簡記為以下形式：

min?t_f，

G(x(t)，u(t))≤0，t∈[0，t_f]；

其中x(t)代表狀態變量，u(t)代表控制變量，求解命題上述最優控制問題即確定符合約束條件的控制變量u(t)以及時域長度t_f，使得代價函數t_f得以最小化；

首先，定義N_fe+1個采樣時刻{t_k|k＝0,...,N_fe}，要求這些采樣時刻均勻分布于時域0-t_f上，即：

第二步，引入一系列變量{u_k|k＝0,...,N_fe}、{x_k|k＝0,...,N_fe}來分別表征u(t)、x(t)，求解u(t)、x(t)這一原始任務轉化為求解它們在一系列采樣時刻的取值，即求解變量{u_k}、{x_k}的取值，由于在離散化意義下已不存在連續時間變量t，因此在最優控制問題的原始命題中與t相關的部分均需相應改造，代價函數變為代數等式/不等式G(x(t),u(t))≤0,t∈[0,t_f]變為G(u_k,x_k)≤0，k＝0,...,N_fe，微分等式的變化方式稍微復雜，將dx(t)/dt＝F(x(t),u(t))寫以下等價形式：

其中無論變量a如何選取，上式一定成立，現將上式中的t取為t_k，并令a＝t_k-1，則有：

為去除復雜的積分運算，可將被積函數整體近似為一個常值Const，即：

由于函數F(·，·)本身不可能是常值函數，上式的成立意味著x(t)、u(t)在子區間t∈(t_k-1，t_k)上取值恒定，即：

其中Const₁、Const₂為常值，上式可寫為：

x(t_k)＝x(t_k-1)+Const·(t_k-t_k-1)，

即：

x_k＝x_k-1+Const·h_k；

上式去除了積分運算以及連續變量t，最后還需明確常值Const究竟應該如何選取，這相當于明確x(t)、u(t)在子區間t∈(t_k-1，t_k)上取什么樣的常值，最常見的取值方式是選取子區間端點處的變量值，即：

或者：

以上兩式分別代表在子區間左右兩端取值，從中選定一種取值方式即可；以在左端取值為例，則可為：

x_k＝x_k-1+F(x_k-1，u_k-1)·h_k；

在所有子區間(t_k-1，t_k)上重復上述轉化，則可形成一個NLP問題：

s.t.x_k＝x_k-1+F(x_k-1，u_k-1)·(t_f/N_fe)，k＝1，...，N_fe；

G(u_k，x_k)≤0，k＝0，...，N_fe

首先，待求解的NLP問題可精煉地描述為：

s.t.g(x)＜0，

h(χ)＝0；

其中χ代表由優化變量組成的向量，即解向量，通過引入松弛向量s＞0，可將不等式約束g(χ)＜0轉化為等式約束，即：

min?J(x，s)，

s.tg(χ)+s＝0，

h(χ)＝0，

s＞0；

此時，如將上式中唯一的不等式約束條件s＞0轉化為內罰函數項補入代價函數J，則可構造僅包含等式約束的標準形式NLP問題：

min?J_μ(χ，s)＝J(χ)-μ_IPM·ln(s)，

s.t.g(χ)+s＝0，

h(χ)＝0；

其中，J_μ(χ,s)代表包含內罰函數項的代價函數；μ_IPM＞0是障礙因子，其取值越趨于0⁺則通過內罰函數μ_IPM·ln(s)描述不等式s＞0的精準程度越高。