[發明專利]一種四旋翼變繩長吊掛系統的路徑規劃與減擺控制方法

有效

• 申請號: 202011631309.2
• 申請日: 2020-12-31
• 公開（公告）號: CN112817328B
• 公開（公告）日: 2022-08-02
• 發明（設計）人: 張棟;楊云霄;郗厚印;韓衡志;胡玉斌;張國慶;張學智
• 申請（專利權）人: 青島理工大學
• 主分類號: G05D1/10
• 分類號: G05D1/10;G05D1/08;G05D1/04
• 代理公司: 青島華慧澤專利代理事務所(普通合伙) 37247
• 代理人: 劉娜

1.一種四旋翼變繩長吊掛系統的路徑規劃與減擺控制方法，其特征在于，包括如下步驟：

步驟一，建立四旋翼的數學模型和所吊掛負載的擺動模型；

步驟二，基于所建立的模型，對四旋翼和負載的初始狀態、結束狀態，以及負載的擺動角度和擺動速度進行約束，利用5階參數未知的三角樣條曲線和4階參數未知的三角樣條曲線對四旋翼和負載的期望位置進行參數化，根據初始狀態和結束狀態的約束條件計算三角樣條曲線的參數，得到滿足狀態約束的規劃路徑；

步驟三，根據得到的規劃路徑，利用負載的擺角與規劃路徑之間的關系，利用二分法求出滿足擺動角度和擺動速度約束條件的最小運輸時間；

步驟四，采用基于積分反步的串級控制方法，外環控制四旋翼的水平位置，內環控制四旋翼的姿態和高度，通過跟蹤所規劃的路徑，實現了負載的精確定位和負載的擺動角度抑制，在最短的時間內將負載送達預定位置；

步驟一的具體方法如下：

(1)首先建立坐標系，包括慣性坐標系{I}＝{X_I,Y_I,Z_I}，四旋翼固定坐標系{B}＝{X_B,Y_B,Z_B}和負載坐標系{H}＝{X_H,Y_H,Z_H}，并假設負載坐標系原點與四旋翼固定坐標系重合，方向始終與慣性坐標系平行，所有坐標系的方向都遵循右手定則；

(2)建立四旋翼的數學模型：

其中，ω_x，ω_y，ω_z為四旋翼在x，y，z軸的姿態角速度，為四旋翼在x，y，z軸的姿態角加速度，J_x，J_y，J_z為四旋翼在x，y，z軸的轉動慣量，J_r為整個電機轉子和螺旋槳繞轉軸的總轉動慣量，Ω_r為螺旋槳角速度的矢量和，d為電機中心到四旋翼中心的距離，為四旋翼在x，y，z方向上的加速度，歐拉角Θ＝[φ,θ,ψ]^T表示四旋翼的滾轉角，俯仰角和偏航角，U₁,U₂,U₃,U₄為輸入控制量，f_ox,f_oy,f_oz為吊繩對四旋翼的拉力在坐標軸方向上的分量，M為四旋翼的質量，g為重力加速度；

(3)建立負載的擺動模型：

將負載看作一個運動的質點，將其擺角分解為投影到兩個平面內與豎直方向的夾角，其中，α為吊繩在X_HO_HZ_H平面內的投影與豎直方向的夾角，β為吊繩在Y_HO_HZ_H平面內的投影與豎直方向的夾角；此外，吊繩長度的變化被認為是均勻的，即其一階導數是一個常數，其二階導數為零；

空間中，負載在慣性坐標系下的位置表示為：

其中，為負載在慣性坐標系下的位置，為四旋翼在慣性坐標系下的位置，為負載在負載坐標系中的位置，R_P為負載坐標系到慣性坐標系的旋轉矩陣，其具體形式為

Rot(α),Rot(β)分別表示在負載坐標系中，負載圍繞X_H軸轉β度和圍繞Y_H轉α度的旋轉矩陣；

將公式(3)帶入到公式(2)中后，最終負載在慣性坐標系下的位置表示為

其中，s_i＝sin(i),c_i＝cos(i)，L表示吊繩的長度，(x，y，z)代表四旋翼在慣性坐標系下的位置；

對公式(4)求二階導數，可得

其中，表示負載在慣性坐標系x，y，z方向上的加速度，為α的擺動速度，為β的擺動速度，為α的加速度，為β的加速度，為繩長變化率；

設吊繩上的拉力為f，則在慣性坐標系下拉力f沿各軸的分量為[-fs_αc_β,fs_β,-fc_αc_β]；

根據牛頓第二定律：

其中，m為負載的質量；

消去中間變量f，可得吊繩長度變化時負載的擺動模型為：

步驟二的具體方法如下：

設四旋翼的初始水平位置為x(0)＝x_r0,y(0)＝y_r0,負載的初始水平位置為x_P(0)＝x_r0，y_P(0)＝y_r0，經過時間T后四旋翼到達期望位置x(T)＝x_r,y(T)＝y_r，負載到達期望位置x_P(T)＝x_r,y_P(T)＝y_r；

在初始狀態時，控制整個吊掛系統處于靜止狀態，即初始時刻的狀態約束：

在結束時刻，控制整個系統也處于靜止狀態，即結束時刻的狀態約束：

在運輸過程中，對負載的擺角以及擺角的擺動速度進行約束，即擺角的狀態約束：

其中，α_max為擺角α的最大值，為α的擺動速度，為擺角α擺動速度的最大值，β_max為擺角β的最大值，為β的擺動速度，為擺角β擺動速度的最大值；

選擇5階參數未知的三角樣條曲線和4階參數未知的三角樣條曲線對負載和四旋翼的期望位置進行參數化：

其中，τ₁＝πt/(2T)，0≤t≤T，a₀～a₅，b₁～b₄，m₀～m₅，n₁～n₄均為待定參數；

其中，τ₂＝πt/T，0≤t≤T，p₀～p₄，r₀～r₄ q₁～q₃，s₁～s₃均為待定參數；結合狀態約束公式(9)～(11)，將公式(16)～(17)改寫為如下：

以為例，其計算過程如下：

當t＝0時，帶入樣條曲線中可得

當t＝T時，帶入樣條曲線中可得

同樣，對樣條曲線x^*(t)，樣條曲線和樣條曲線y^*(t)采用同樣的方法進行計算；

利用matlab求解出公式(19)～(28)的解，得到滿足約束的樣條曲線參數為：

a₄＝m₄＝0,b₂＝n₂＝0,p₂＝r₂＝p₄＝r₄＝q₁＝s₁＝q₂＝s₂＝q₃＝s₃＝0；

將以上參數帶入到樣條曲線中，得到即可獲取樣條曲線的具體表達式；

至此，規劃的路徑已經滿足初始狀態和結束狀態的約束；

步驟三的具體方法如下：

負載的擺角與吊掛系統的路徑有如下的關系：

將樣條曲線(29)～(32)帶入到(33)中，求出滿足擺角約束條件的T值；

步驟四的具體方法如下：

將系統模型寫成狀態方程的形式：

其中，ζ₁,ζ₂為狀態變量，u_x＝cosφsinθcosψ+sinφsinψ，u_y＝cosφsinθsinψ-sinφcosψ為虛擬控制輸入；

ζ₃＝[φ θ ψ]^T,ζ₄＝[ω_x ω_y ω_z]^T (36)

采用積分反步控制，設計的位置控制率為：

所設計的姿態控制率為：

其中，e₁＝ζ_1d-ζ₁表示位置跟蹤誤差，ζ_1d＝[x^*,y^*,z_d]^T表示期望位置，e₂＝v₁-ζ₂表示虛擬控制誤差，表示誤差導數，同理，e₃＝ζ_3d-ζ₃表示姿態跟蹤誤差，e₄＝v₂-ζ₄表示虛擬控制誤差，表示誤差導數，p₁為位置誤差積分p₂為姿態誤差積分A₁,A₃均為3維單位矩陣，Λ₁,Λ₂,A₂,A₄為非負正定斜對稱矩陣；

2.根據權利要求1所述的一種四旋翼變繩長吊掛系統的路徑規劃與減擺控制方法，其特征在于，二分法求滿足擺動角度和擺動速度約束條件的最小運輸時間T值的方法如下：

(1)給定輸入：設定T的最大值和最小值、設定擺角及其擺速的最大值和最小值，設定精度；

(2)進行迭代：取T的最大值和最小值的平均值，帶入到樣條曲線中，判斷是否滿足角度約束條件，若滿足，說明T值仍可取小，令當前最大值等于平均值，繼續迭代；若不滿足，說明T需取較大值，令當前最小值等于平均值，繼續迭代；

(3)每次迭代后求出最大值與最小值之差，判斷是否滿足設定的精度，如滿足，則輸出結果，不滿足，則繼續迭代搜索；直至求出滿足精度的T值，輸出結果。