[發(fā)明專利]一種兩輪自平衡車集中式編隊(duì)控制方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202010060834.7
• 申請(qǐng)日: 2020-01-19
• 公開（公告）號(hào): CN111399500B
• 公開（公告）日: 2023-06-13
• 發(fā)明（設(shè)計(jì)）人: 董輝;袁登鵬;董浩;吳祥;吳宇航;田叮;童濤;錢學(xué)成;夏啟劍
• 申請(qǐng)（專利權(quán)）人: 浙江工業(yè)大學(xué)
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02
• 代理公司: 杭州君度專利代理事務(wù)所(特殊普通合伙) 33240
• 代理人: 楊天嬌

1.一種兩輪自平衡車集中式編隊(duì)控制方法，其特征在于，所述兩輪自平衡車集中式編隊(duì)控制方法，包括：

步驟S1、根據(jù)領(lǐng)航平衡車的位置信息和目標(biāo)隊(duì)形結(jié)構(gòu)，計(jì)算跟隨平衡車的理想位置信息，所述位置信息包括坐標(biāo)和方位角，包括：

步驟S1.1、定義(x?y?θ)^T為一階非完整性輪式平衡車在世界坐標(biāo)系下的橫坐標(biāo)、縱坐標(biāo)和方位角，則平衡車的運(yùn)動(dòng)學(xué)方程為：

其中，v為平衡車當(dāng)前的實(shí)際線速度，ω為平衡車當(dāng)前的實(shí)際角速度；

步驟S1.2、獲取領(lǐng)航平衡車R_i的位置信息為P_i＝[x_i?y_i?θ_i]^T，跟隨平衡車R_j當(dāng)前的實(shí)際位置信息為P_j＝[x_j?y_j?θ_j]^T，則根據(jù)控制法，計(jì)算跟隨平衡車R_j的理想位置信息P_j^d為：

其中，跟隨平衡車R_j當(dāng)前的實(shí)際位置信息為P_j滿足以下關(guān)系式：

其中，L_ij^d和分別表示跟隨平衡車R_j和領(lǐng)航平衡車R_i的理想距離和理想相對(duì)轉(zhuǎn)角，L_ij和分別表示跟隨平衡車R_j和領(lǐng)航平衡車R_i的實(shí)際距離和實(shí)際相對(duì)轉(zhuǎn)角；

步驟S2、由跟隨平衡車當(dāng)前的實(shí)際位置信息和所述理想位置信息，計(jì)算跟隨平衡車的位置誤差，所述位置誤差包括坐標(biāo)誤差和方位角誤差，包括：

步驟S2.1、根據(jù)跟隨平衡車當(dāng)前的實(shí)際位置信息和理想位置信息，得到跟隨平衡車R_j在世界坐標(biāo)系下的位置誤差為：

其中，為世界坐標(biāo)系下坐標(biāo)x方向的誤差，為世界坐標(biāo)系下坐標(biāo)y方向的誤差，為世界坐標(biāo)系下方位角誤差；

步驟S2.2、將世界坐標(biāo)系下的位置誤差轉(zhuǎn)換至平衡車坐標(biāo)系下，得到平衡車坐標(biāo)系下跟隨平衡車的位置誤差為：

其中，e_j1表示平衡車坐標(biāo)系下的坐標(biāo)x方向的誤差，e_j2表示平衡車坐標(biāo)系下的坐標(biāo)y方向的誤差，e_j3表示平衡車坐標(biāo)系下的方位角誤差；

步驟S2.3、對(duì)平衡車坐標(biāo)系下跟隨平衡車的位置誤差進(jìn)行求導(dǎo)，得到：

其中，為e_j1的導(dǎo)數(shù)，為e_j2的導(dǎo)數(shù)，為e_j3的導(dǎo)數(shù)，v_j為跟隨平衡車R_j當(dāng)前的實(shí)際線速度，v_i為領(lǐng)航平衡車R_i當(dāng)前的實(shí)際線速度，ω_j為跟隨平衡車R_j當(dāng)前的實(shí)際角速度，ω_i為領(lǐng)航平衡車R_i當(dāng)前的實(shí)際角速度，d_j表示跟隨平衡車R_j的重心到輪軸中心的距離；

步驟S3、構(gòu)建運(yùn)動(dòng)學(xué)控制器，采用運(yùn)動(dòng)學(xué)控制器根據(jù)位置誤差輸出控制跟隨平衡車的理想線速度和理想角速度，包括：

構(gòu)建的運(yùn)動(dòng)學(xué)控制器如下：

其中，k_x、k_y、k_θ分別表示x、y、θ的正向量常數(shù)；

運(yùn)動(dòng)學(xué)控制器輸出的控制跟隨平衡車的理想線速度為v_jc、理想角速度為ω_jc；

步驟S4、判斷采樣時(shí)間內(nèi)按照理想角速度旋轉(zhuǎn)的角度與跟隨平衡車實(shí)際旋轉(zhuǎn)的角度是否相同，若相同則向跟隨平衡車輸出理想角速度；否則構(gòu)建增量式PD控制器，根據(jù)采樣時(shí)間內(nèi)旋轉(zhuǎn)的角度誤差輸出實(shí)際角速度至跟隨平衡車，包括：

步驟S4.1、設(shè)置采樣時(shí)間為ΔT；

步驟S4.2、則在采樣時(shí)間內(nèi)跟隨平衡車按照理想角速度ω_jc旋轉(zhuǎn)的角度為：θ₁＝ω_jc*ΔT；

步驟S4.3、獲取在采樣時(shí)間內(nèi)跟隨平衡車實(shí)際旋轉(zhuǎn)的角度為θ₂，則采樣時(shí)間內(nèi)旋轉(zhuǎn)的角度誤差為e_θ＝θ₁-θ₂；

步驟S4.4、若e_θ＝0，則向跟隨平衡車輸出理想角速度ω_jc，并結(jié)束本次的角度調(diào)控；否則執(zhí)行下一步；

步驟S4.5、構(gòu)建增量式PD控制器如下：

Δθ＝λe_θ(k)+βe_θ(k-2)

其中，Δθ為增量式PD控制器輸出的實(shí)際角速度，e_θ(k)為k時(shí)刻的角度誤差，e_θ(k-2)為k-2時(shí)刻的角度誤差，k_p為比例系數(shù)，T_d為微分系數(shù)，T為采樣周期；并將增量式PD控制器輸出的實(shí)際角速度Δθ發(fā)送至跟隨平衡車；

步驟S5、判斷采樣時(shí)間內(nèi)按照理想線速度計(jì)算的坐標(biāo)與跟隨平衡車實(shí)際運(yùn)行的坐標(biāo)是否相同，若相同則向跟隨平衡車輸出理想線速度；否則構(gòu)建預(yù)測(cè)控制器，根據(jù)采樣時(shí)間內(nèi)運(yùn)行的坐標(biāo)誤差輸出實(shí)際線速度至跟隨平衡車，包括：

步驟S5.1、設(shè)置采樣時(shí)間為ΔT；

步驟S5.2、則在采樣時(shí)間內(nèi)跟隨平衡車按照理想線速度v_jc計(jì)算得到的對(duì)應(yīng)時(shí)刻的坐標(biāo)為(x_d,y_d)；

步驟S5.3、獲取在采樣時(shí)間內(nèi)跟隨平衡車運(yùn)行的對(duì)應(yīng)時(shí)刻的實(shí)際坐標(biāo)為(x_r,y_r)，則采樣時(shí)間內(nèi)運(yùn)行的坐標(biāo)誤差如下：

其中，x_e,y_e分別為x、y方向的誤差；

步驟S5.4、若x_e＝0且y_e＝0，則取所述理想線速度v_jc作為待發(fā)送線速度，并執(zhí)行步驟S5.7；否則執(zhí)行步驟S5.5；

步驟S5.5、計(jì)算采樣時(shí)間內(nèi)運(yùn)行的坐標(biāo)誤差的微分值為：

其中，v為跟隨平衡車當(dāng)前的實(shí)際線速度，θ為跟隨平衡車當(dāng)前的實(shí)際方位角；

設(shè)置中間虛擬控制規(guī)律，得到虛擬的x、y方向的線速度如下：

其中，u₁為虛擬的y方向的線速度，u₂為虛擬的y方向的線速度，k₁為常數(shù)，k₂為常數(shù)；

步驟S5.6、僅考慮x方向的位置誤差，即設(shè)置y_e＝0，構(gòu)建預(yù)測(cè)控制器為：

將預(yù)測(cè)控制器輸出的實(shí)際線速度v_r作為待發(fā)送線速度；

步驟S5.7、取所述待發(fā)送線速度作為當(dāng)前k時(shí)刻的線速度v(k)，根據(jù)以下公式判斷線速度v(k)是否滿足限幅要求：

其中，v(k-1)為k-1時(shí)刻的線速度，Δt為前后兩次速度計(jì)算的時(shí)間間隔，a_tv為預(yù)設(shè)的速度變化率閾值，v_tv為預(yù)設(shè)的速度閾值；

若線速度v(k)滿足限幅要求則向跟隨平衡車輸出線速度v(k)；否則向跟隨平衡車輸出速度閾值v_tv。