飛走巡線機器人多模式切換混雜控制方法

秦新燕，李惠東，馮天明，雷金，李博，李兆鈞，張杰，王艷琦，賈文興

（石河子大學 機械電氣工程學院, 新疆 石河子 832003）

目前，電力線巡線機器人主要有飛行式巡線機器人、攀爬式巡線機器人和混合式巡線機器人。飛行式巡線機器人是以無人機為載體，通過在機體上搭載巡檢設備，采用飛行方式對電力線進行巡檢。攀爬式巡線機器人的典型特征是有兩個或者多個與電力線垂直的機械臂，機械臂上安裝有行走輪，行走輪與電力線相接觸且在電機的驅動下沿電力線行走[1]。混合式巡線機器人是一種將攀爬式巡線機器人的行走功能和飛行式巡線機器人的飛行功能相結合的巡線機器人[2]，能夠利用飛行功能將機器人懸掛到電力線上，利用行走功能控制機器人沿電力線移動[1]。遇到障礙物時可以通過飛行模式或爬行模式穿越障礙物。而由于輸電線路走廊常常被架設在雪山、河谷、沙漠、戈壁等特殊地形，且伴隨風雪、沙塵、雨露等天氣的侵擾[3]，增加了混合式巡線機器人的巡檢難度。近年來，如何提高巡線機器人的穩定性和可靠性，是其面向實際應用的難點。

對于飛行式巡線機器人和攀爬式巡線機器人，它們的穩定性和可靠性控制方式均有所不同。其中飛行式巡線機器人主要是對飛行穩定性和可靠性進行控制。例如，諸多學者利用了滑模控制技術來應對飛行器控制過程中所受外部干擾問題[4-6]。文獻[7]采用積分反步法，應對風場效應對飛行器的干擾。文獻[8]針對飛行器軌跡跟蹤控制問題，提出了一種模糊自適應動態面軌跡跟蹤控制方法。攀爬式巡線機器人的穩定性和可靠性控制主要集中在行走控制。文獻[9]提出了一種模糊控制方法，控制機器人的轉向行為。文獻[10]采用了預編程和傳感器定位相結合的方法，實現機器人自主越障控制。文獻[11]提出了一種基于重力模型的PD 控制律和模糊打滑控制方法，實現巡線機器人打滑和越障控制。飛行式巡線機器人的飛行控制目前有較為廣泛的研究，但其存在續航時間短、無法有效克服強氣流干擾等缺點，穩定性和續航時間不及攀爬式巡線機器人。而攀爬式巡線機器人雖然依托電力線，具有續航時間長、穩定性高等優點，但結構設計復雜、無法自主上下電力線，成為其在電力線巡檢上的應用瓶頸。

混合式巡線機器人在多變的野外環境下，頻繁的飛走切換，使其需不斷地在呈柔索特性的電力線上完成起落，對機器人的穩定控制提出了較大挑戰。為了使混合式巡線機器人能有效應對復雜的電力線環境，采用單一控制模式已無法滿足需求，因此需采用多模式控制方法，旨在機器人能在不同環境下工作時都有一套合適的控制模式與之匹配。目前，多模式控制方法已經廣泛應用于車輛、機器人等領域，并取得了一定的成果。文獻[12] 針對復雜的交通環境，設計了定速巡航、穩態跟隨、接近前車、強加速、強減速和避撞6 種模式，并利用經驗型碰撞時間倒數來實現模式切換。文獻[13]設計了一個可用于野外地質探測的陸空兩棲平臺，劃分了飛行和陸地行走兩種控制模式，并采用差值判定法實現陸空兩種模式切換。文獻[14]針對四旋翼飛行器室外環境因素對飛行的影響，將控制模式劃分為起降、平穩、增穩和失控保護4 種模式，并以俯仰角、翻滾角和高度為模式切換變量進行模式切換。

為了提升巡線機器人在復雜電力線環境中的穩定性和可靠性，本文以FPLIR 為研究對象，提出了一種多模式切換混雜控制方法。針對飛走巡線機器人(flying-walking power line inspection robot，FPLIR)的工作原理建立了4 種模式的混雜自動機模型和模式切換監測器模型，并利用Lyapunov函數法和力角穩定性判據(force-angle stability margin，FASM)分別分析FPLIR 在行走狀態下多模式切換的穩定性和基于力學特性的穩定性。基于各種控制模式側重的控制目標，提出了滑模控制、模型預測控制、變論域模糊控制、EGO-Planer等控制策略，并進行仿真和實驗。仿真和實驗結果表明，所提出的控制方法取得了較好的控制效果。

1 FPLIR 機構描述

自主研發的FPLIR 主要由飛行機構和行走機構兩部分構成，其模型如圖1 所示。

圖1 FPLIR 機械結構圖Fig. 1 Mechanical structure of FPLIR

其中飛行機構采用X 型結構設計，6 個旋翼電機均勻分布在中心倉的四周序號為M1～M6。行走機構包括主行走輪、輔行走輪、主壓緊輪、編碼器等。在機器人運動空間中基于右手螺旋定則建立慣性坐標系 {E}和重心坐標系 {B}。其中重心坐標系的xb軸指向機器人正前方，yb軸指向機器人正左方，zb軸垂直于平面xbobyb豎直向上。慣性坐標系 {E}的xe軸指向正北方，ye軸指向正西方，ze軸方向鉛錘向上。

圖2 為FPLIR 的機構構型，圖中標號1～8 表示機器人的驅動器件，為8 個控制輸入。在飛行狀態下，飛行機構被激活，行走機構被關閉，該狀態下機器人具有6 個自由度。行走狀態下，飛行機構被關閉，行走機構被激活，此時機器人具有2 個自由度，分別為行走輪掛線行走時的沿線運動和壓緊組件（主壓緊輪、輔壓緊輪、絲杠、連桿等）在壓緊電機驅動下的上下運動。圖中， φ為沿xb軸旋轉的橫滾角， ψ為沿zb軸旋轉的偏航角， θ為沿yb軸旋轉的俯仰角。為方便對機器人進行飛行狀態下的動力學分析，定義重心坐標系 {B}為機體坐標系，重心與中心重合。

圖2 FPLIR 的機構構型Fig. 2 Mechanism diagram of FPLIR

設機體坐標系按照xyz旋轉順序得到慣性坐標系，則旋轉矩陣為

2 FPLIR 動力學模型

2.1 飛行機構動力學模型

2.1.1 位置動力學模型

根據牛頓第二定律，機器人運動學方程為

式中：m為機器人的質量，Ee為慣性坐標系下的位移量，G為重力，Te為慣性坐標系下機器人的升力，Fe為空氣阻力。

設機體坐標系下機器人升力為

式中：kF為升力系數， Ωi(i=1,2,···,6)為旋翼轉速。

空氣阻力表達式為

其中，kax、kay、kaz分別為xb、yb、zb3 個軸向的空氣阻尼系數。

聯立式(1)～(6)得到機器人位置動力學模型：

式中g為重力加速度。

2.1.2 姿態動力學模型

根據牛頓—歐拉方程建立機體坐標系下FPLIR的姿態動力學方程：

式中：Gb為旋翼產生的陀螺力矩，Γb=為旋翼在機體軸上產生的力矩，為FPLIR的轉動慣量。

如圖3 所示，r為機器人中心到機臂末端的距離，β=60°表示兩機臂之間的夾角，αn(n=1,2,···,6)為機臂與xb軸正方向的夾角，km為反扭矩系數，則旋翼在機體軸上產生的力矩為

圖3 FPLIR 機臂布局示意圖Fig. 3 Schematic diagram of FPLIR arm layout

對式(2)求逆矩陣，并采用小角度假設得：

聯立式(2)、(8)、(9)、(10)得到FPLIR 的姿態動力學方程為

2.1.3 模型線性化

2.2 行走機構動力學模型

機器人在掛線行走工況下旋翼電機停止工作對行走機構的影響可忽略，輔行走輪質量較輕且主要起導向作用忽略其對電力線的影響，因此在沿線行走工況下對機器人進行如圖4 所示的受力分析。

圖4 FPLIR 滾輪與電力線之間的受力分析Fig. 4 Force analysis between FPLIR roller and power line

在慣性坐標系下設電力線傾角為 φ，則機器人沿電力線勻速行走時受力平衡方程為

式中：FN為電力線對主行走輪的作用力，Nj為主壓緊輪 W2和輔壓緊輪 W1、 W3與電力線之間的相互作用力(j=1,2,3)，FS為主行走輪與電力線之間的靜摩擦力，FSi為主壓緊輪、輔壓緊輪和電力線之間的靜摩擦力(i=1,2,3)， ε為主行走輪、主壓緊輪和輔壓緊輪與電力線的摩擦系數，R為主行走輪的半徑，T為主行走輪的驅動力矩。

則機器人沿線行走時的動力學方程為

式中v為機器人沿線行走時的速度。

機器人沿線行走時除了受到滾輪（主壓緊輪、輔壓緊輪、主行走輪、輔行走輪）與電力線之間力的影響，亦會受到風力Fw和慣性力FI的影響，其受力如圖5(a)所示。

圖5 掛線行走時FPLIR 受力分析Fig. 5 Force analysis of FPLIR along the power line

圖中P1、P2、P3、P4、P5為滾輪與電力線的接觸點， ?為機器人擺角。機器人受風力影響的俯視圖如圖5(b)所示，風力Fw與yb軸的夾角為ξ，則機器人所受合力為

式中：FIG為G和FI的合力，FIGw為FIG和Fw的合力。

3 FPLIR 混雜自動機模型及多模式切換監督器設計

3.1 FPLIR 控制模式劃分及切換

FPLIR 的工作原理如圖6 所示：機器人從地面起飛，通過飛行的方式完成落線。安全落線后，機器人開始線上行走巡檢。在行走過程中遇到障礙物時，機器人通過飛行或行走穿越的方式越過障礙物。當完成單檔段的巡檢任務后，通過飛行的方式飛越桿塔到下一個檔段。當機器人完成巡檢任務后，通過飛行完成下線。

圖6 FPLIR 工作原理Fig. 6 Working principle of FPLIR

輸電線路因其循環往復的結構化分布特征，以及FPLIR 靈活切換的運行機制，將FPLIR 的工作劃分為如表1 所示的5 種控制模式。

表1 FPLIR 控制模式劃分Table 1 FPLIR control mode division

表中每種控制模式分別對應著FPLIR 的一種狀態，這些狀態通過離散事件的觸發實現相互切換[15]，對此本文引入一種如圖7 所示的多模式切換監控框架。FPLIR 巡檢過程中，設機器人任意時刻ti的運動狀態為q(ti)∈Q，當多模式切換監測器檢測到離散事件滿足觸發條件時，多模式切換監測器將機器人狀態信息，映射為系統的離散事件e∈E。離散事件控制器計算出離散控制策略s∈S，控制指令生成器將離散控制策略轉化為對機器人的連續操縱命令[16]u(ti)∈U，通過命令的執行，改變機器人的運動狀態。

圖7 FPLIR 多模式切換監控框架Fig. 7 FPLIR multi-mode switching monitoring framework

3.2 FPLIR 多模式切換混雜系統模型

由于FPLIR 控制系統是一個典型的混雜動態系統，無法單一地用離散或連續的數學模型來準確地描述，因此建立如下所示的混雜自動機模型。

式中：S={s1,s2,s3,s4}，s1為落線模式、s2為打滑模式、s3為行走越障模式、s4為飛越桿塔模式；E為4種控制模式切換的邏輯判決條件集，(i=1,2,3,4;j=1,2,3,4)；Q為n維連續變量狀態空間，Q?Rn；U為m維連續動態控制輸入空間，U?Rm；f為連續狀態向量x(t)的向量場，f∶Q×U→Q。

圖8 描述了控制模式的切換過程，在ti時刻控制模式為si，離散事件ei j發生之前，遵循連續動態行為q˙(ti)=f(q(ti),u(ti))，在tj時刻，離散事件eij發生，控制模式變遷為sj，此后系統將遵循新控制模式下的連續動態行為[17]從圖中可知，在具有結構化工況特點的電力線環境中機器人的運行具有一定的規律性，其模式的切換亦存在規律性。

圖8 混雜自動機切換模型Fig. 8 Switching model of hybrid automata

3.3 FPLIR 多模式切換監測器設計

FPLIR 多模式切換監測器用于監測機器人狀態的連續變化過程中產生的離散事件，依據控制模式的劃分規則，設計每種離散事件監測器模型，包括：

1) 落線模式監測器模型

落線模式需要機器人飛行到一定的高度后才能被觸發，則監測器模型為

式中：h為機器人的飛行高度；H為地線的高度；Δh為允許的高度偏差。

2) 打滑模式監測器模型

FPLIR 在掛線巡檢過程中，通過監測機器人擺角 ?是否小于安全閾值，以及主壓緊輪的線速度與主行走輪的線速度之比是否小于設定的打滑度 ηdes，從而判定是否打滑，則監測器模型為

其中：vt為主壓緊輪的速度，vref為主行走輪的速度，?max為擺角安全閾值。

3) 行走越障模式監測器模型

機器人掛線行走時，檢測到前方小型穿越型障礙物與機器人的距離為xd，觸發行走越障模式的設定距離為D，允許的檢測距離偏差為Δdmax，則：

4) 飛越桿塔模式監測器模型

機器人在掛線巡檢過程中，為保證機器人不與桿塔發生碰撞，需要與桿塔之間保持一個最小安全距離lmin。當機器人檢測到與桿塔的距離為lmin時，系統切換至飛越桿塔模式，則監測器模型為

其中：xl為機器人與桿塔之間的距離， Δl為允許的距離偏差。

4 FPLIR 多模式切換控制方法

4.1 FPLIR 穩定性分析

4.1.1 行走狀態下多模式切換穩定性分析

機器人沿線行走時的控制系統是一個非線性系統，其穩定性比通常的由連續或離散子系統組成的切換系統穩定性要復雜[18]。

定理1對于非線性切換系統[19]，如果存在正定Lyapunov 函數Vi(x(t),t)，K 類函數 αi，i=1,2,···,p滿足：

3)Vi+1(x(t),t)≤Vi(x(t),t)，i=1,2,···,p；那么平衡點是在Lyapunov 意義下穩定的。

定理2對于非線性切換系統，若存在正定Lyapunov 函數Vi(x(t),t)，K 類函數 αi和 βi，滿足下面條件：

3)Vi+1(x(t),t)≤Vi(x(t),t)，i=1,2,···,p；則平衡點在Lyapunov 意義下是一致穩定的。

由定理1 和2 可知，Lyapunov 函數值既要求在每個連續子系統的時間區間上是非增，也要求在切換過程中是非增的。對于FPLIR 而言，機器人沿線行走時各個模式具有相同的數學模型，因此選用共同Lyapunov 函數法對多模式切換進行穩定性分析。FPLIR 協調控制器的控制律和Lyapunov 函數為

式中：Υ=ηdes(t)-η(t)，k為量化因子， sat為飽和函數。機器人在低速穿越障礙物過程中壓緊組件降下不輸出壓緊力，無法進行打滑控制，忽略此時的打滑情況，并設此時的打滑度η(t)=1。對V求導得：

當ηdes(t)-η(t)變號時變號，從而得出恒成立。

式中，2ηdes(t)-ηi+1(t)-ηi(t)＞0 ，ηi(t)-ηi+1(t)＜0，因此Vi+1(x(t),t)-Vi(x(t),t)＞0。

綜上分析，機器人在行走狀態下多模式切換是穩定的，這樣保證了機器人沿線行走時速度與主行走輪轉速的跟隨性。

4.1.2 行走狀態下基于力學特性的穩定性分析

對已有機器人穩定性判別法進行綜合分析[20]，并結合FPLIR 的機械結構特點，采用力—角穩定性判據法對FPLIR 線上行走進行穩定性分析。

如圖9 所示，機器人滾輪與電力線的5 個接觸點共線，所以5 個點組成傾翻軸線，向量記為

圖9 FASM 穩定裕度分析示意圖Fig. 9 Diagram of FASM stability margin analysis

式中，P5、P1為重心Ob到接觸點P5和P1的向量。

過重心Ob作與傾翻軸線b的法向量為

FIGw在以為法向量的平面A上的投影為

機器人邊線傾翻角為FIGw的投影L2與L1的夾角：

根據文獻[1] 和文獻[21] 可知：L1的長度為40.9 cm，當風力與yb軸的夾角為ξ=90°，風速為15.5 m/s（7 級風），壓緊輪（主、輔壓緊輪）沒有對電力線輸出壓緊力時，機器人沿xb軸方向的擺動位移最大為5.9 cm。由于輔行走輪在起到導向作用的同時，也能夠減小機器人沿yb軸方向的擺動，因此忽略機器人沿yb軸方向的慣性力，經計算得λ=8.29，Tfasm＞0，故機器人沿線行走時在風速為15.5 m/s 的情況下依舊具有穩定性。

4.1.3 飛行和行走狀態切換穩定性分析

對機器人飛行狀態和行走狀態的切換選用Lyapunov直接法進行穩定性分析。

Lyapunov直接法[22]：設系統狀態方程為平衡狀態為xe=0，若存在一個標量函數V(x)對所有x都有連續一階偏導，滿足：

1)穩定性判據1：①V(x)正定，負定，則系統在平衡狀態xe是漸近穩定的；②‖x‖→∞，V(x)→∞，則系統在平衡狀態xe大范圍漸近穩定。

2)穩定性判據2：①V(x)正定，半負定，在非零狀態，則系統在平衡狀態xe是漸近穩定的；②‖x‖→∞，V(x)→∞則系統在平衡狀態xe是大范圍漸近穩定的。

3)穩定性判據3：V(x)正定，半負定，在非零狀態，則系統在平衡狀xe是Lyapunov 意義下穩定的。

由圖1 所示的機器人的行走機構可知，為保證機器人能夠安全、平穩落線，需要機器人的主行走輪能夠準確落到電力線上。

當主行走輪位于電力線正上方時，機器人沿z軸勻速緩慢降落，此時忽略其x軸和y軸的微小位移，則z軸的加速度矢量為

機器人接觸到電力線上但未完全停穩時，其x軸和y軸速度為0，z軸加速度矢量為

由于當機器人落到線上時，旋翼提供的升力逐漸減小至0，而電力線對機器人提供的支持力逐漸增大，因此機器人將做加速度逐漸變小的減速運動。

4.2 控制模式設計

4.2.1 增穩模式

由于滑模變結構控制能有效處理系統信號通道中突然出現的干擾或者信號大幅變化的情況[6]，所以增穩模式采用滑模變結構控制方法。以俯仰通道為例進行控制器設計：

選取滑模面為線性滑模面：

選取指數趨近律：

令 θr為參考輸入， θ為實際俯仰角，則：

將式(32)、(33)代入式(30)、(31)中得：

將式(34)代入式(12)得：

同理可得其他通道的控制器。

4.2.2 落線模式

為了能夠讓機器人安全、平穩落線，需要讓機器人以零速度到達期望的落線位置。由于模型預測控制(model predictive control，MPC)方法能夠預測被控對象的未來行為，通過對輸入和狀態變量的約束，可以保證機器人降落的安全性[23]，因此采用MPC 作為落線模式的控制方法。

設連續時間的動力學系統常微分方程為

其中：機器人狀態向量和輸入向量分別為x(t)=和則最優控制問題為

其中：Δu(t)表示控制增量，xr(t)表示參考狀態向量，Q 和F 表示正常數。在實際控制過程中，往往需要對系統狀態機控制量進行約束，一般如下：

因此，模型預測控制在的每一步的都帶約束優化求解問題都等價于求解二次規劃問題。當每個控制周期完成求解后，可以得到控制時域內的一系列控制輸入增量。

根據模型預測控制基本原理，將該控制序列中的第一個元素作為實際的控制輸入增量作用于系統，即：

當控制系統執行這一控制量直到下一時刻。在新的時刻，系統根據狀態信息重新預測下一段時域的輸出，通過優化過程得到一個新的控制增量序列。

4.2.3 打滑模式

機器人沿線巡檢過程中，主行走輪可能發生打滑主要有以下3 種情況：1) 電力線坡度較大，且壓緊力不足；2)在雨雪天氣等影響下，使電力線表面的摩擦系數變小；3)有較大風擾時會使機器人發生擺動。這些工況會對機器人的行走穩定性造成嚴重影響。因此，針對電力線坡度、風擾、磨損等諸多因素，本文采用一種基于多工況變論域模糊控制(variable universe fuzzy control，VUFC)方法[24]。該控制系統能夠根據機器人在不同工況下，改變輸出變量的論域從而獲得更好的控制效果。

由2.2 節分析可知，若機器人沿線行走時主行走輪不打滑應滿足以下條件：

線上行走打滑控制系統流程圖如圖10 所示。以電力線坡度、打滑度和機器人擺角作為控制器的輸入變量，以三者的值作為工況分級的判斷依據，壓緊電機接收的脈沖數Pnum作為輸出變量。輸入變量的論域如表2 所示，輸出變量的論域受伸縮因子yp的影響。

表2 輸入變量論域Table 2 Input variable domain

圖10 變論域模糊控制系統流程Fig. 10 Flow of variable domain fuzzycontrol system

電力線坡度采用高斯型隸屬度函數進行模糊化；打滑度主要采用三角形隸屬度函數進行模糊化，對于η ≥0.95（不打滑）和η ＜0.6（嚴重打滑）兩個臨界點，則選擇梯形隸屬度函數；擺動角程度分為小(0°＜ ? ≤2°)，中(2°＜? ≤12°)，大(? ＞12°)，并分別采用高斯型隸屬度函數、三角形隸屬度函數和梯形隸屬函數進行模糊化，其中擺動角度為小時認定機器人穩定。輸入和輸出變量的隸屬度函數如圖11 所示。

圖11 輸入和輸出隸屬度函數Fig. 11 Input and output membership functions

伸縮因子yp的隸屬度函數如圖12 所示。解模糊方法采用最大隸屬度函數法。

圖12 伸縮因子隸屬度函數Fig. 12 Membership function diagram of expansion factor

4.2.4 行走越障模式

機器人沿線巡檢過程中遇到可以通過行走越過的小型障礙物如防震錘等，控制系統切換至行走越障模式，其越障過程如圖13 所示。當機器人與防震錘的距離為xd＞D0時，機器人速度為v0；當機器人與防震錘的距離為D1＜xd≤D0時，觸發行走越障模式，機器人速度為v1；當機器人與防震錘的距離為xd≤D1時，機器人降下主、輔壓緊輪，速度為v2；當機器人越過防震錘之后，若xd≤D1，則機器人以速度v2前進，若D1＜xd≤D0，則機器人主、輔壓緊輪重新壓緊電力線，速度為v1，若xd＞D0，則機器人退出行走越障模式，以速度v0前進。

圖13 FPLIR 穿越小型障礙物(防震錘)示意Fig. 13 Schematic of FPLIR crossing a small obstacle(shockproof hammer)

機器人沿線行走時速度突變會對機器人的穩定性造成影響，為保證在越障過程中速度變化時的平滑性，采用一種基于插值規則的控制方法，如下式所示。該控制方法是以機器人與障礙物的距離xd為自變量，速度v為因變量。由于機器人在不斷接近或遠離障礙物的過程中距離xd是連續平滑變化的，因此可以保證機器人速度v變化的平滑性。

式中：D0是機器人越過防震錘之前觸發行走越障模式的最大距離，以及越過防震錘之后退出行走越障模式的最大距離；D1是機器人越過防震錘之前降下主、輔壓緊輪的最小距離，以及越過防震錘之后升起主、輔壓緊輪的最小距離。

4.2.5 飛越桿塔模式

為保證機器人能夠安全飛越桿塔，本文采用基于無ESDF 梯度的局部規劃器(ESDF-free gradientbased local planner，EGO Planner)[25]作為飛越桿塔模式的控制方法。通過對機器人飛行路徑的規劃，以及設置膨脹半徑，能夠讓機器人沿著安全飛行路徑飛越桿塔，也可讓機器人與桿塔保持一定的安全距離，確保機器人飛越桿塔的安全性。

首先，生成一個滿足約束但忽略障礙物的B 樣條曲線 Φ，然后對曲線 Φ進行碰撞檢測，并用A*算法生成一條無碰撞路徑，對于每一條碰撞段中的控制點Qi都會分配一個{p,v}對。

為防止軌跡在被拉出當前障礙物前重復生成{p,v}對，則給出以下約束條件：

式中di j表示Qi到第j個障礙物的距離。若滿足dij＞0，則認為控制點Qi在新的障礙物中，反之則認為是已發現的障礙物。

使用均勻B 樣條曲線 Φ表示軌跡，每個節點向量與父節點向量有相同的時間間隔Δt=tm+1-tm。

根據B 樣條曲線 Φ的一階、二階、三階控制點分別為

式中：Vi表示控制點的速度，Ai表示加速度，Ji表示加加速度。

根據無人機的微分平坦特性，降低要規劃的變量，則目標函數為

其中：Js是平滑項懲罰，Jc是碰撞項懲罰，Jd是可行性懲罰， λs、 λc、 λd為懲罰項的權值。

根據上述得到的安全軌跡 Φs，需要通過合理的時間重新分配，重新生成均勻的B 樣條軌跡Φf。通過利用各向異性曲線擬合法，保持與 Φs幾乎相同的形狀，同時使 Φf可以自由優化控制點滿足高階導數的約束。

超限比例的計算方法為

式中，i∈{1,2,···,N-1}，j∈{1,2,···,Nc-2}，k∈{1,2,···,Nc-3}和r∈{x,y,z}軸。re表示相對于 Φs， Φf需要分配的時長。Vi，Ai和Jk與 Δt的一次、二次和三次成反比。則 Φf新的時間間隔為

通過求解閉式的最小二乘問題，在約束條件下初始生成時間間隔為 Δt′的軌跡 Φf，同時保持與Φs相同的控制點數和形狀，然后重新計算光滑項懲罰和動力學可行項懲罰得到新的目標函數：

其中： λf為擬合項權重，Jf為Φf(αT′)到Φs(αT)的各向異性位移的積分，α ∈[0,1]，T′、T為 Φs和 Φf的持續時間。

對于兩條曲線，用帶有低權重的軸向位移來放寬光滑調整限制，用高權重的徑向位移來防止碰撞。

5 仿真和實驗

為驗證多模式切換控制方法的有效性，本文以仿真和實驗相結合的方式進行驗證。仿真以Gazebo 等軟件為平臺，搭建FPLIR 飛行動力學模型，對FPLIR 飛行狀態下的多模式切換控制策略進行了仿真測試。實驗研究以FPLIR 為載體，在電力線實驗臺架上通過人為設置障礙物工況、打滑工況、改變地線表面摩擦系數等，驗證線上行走狀態下多模式切換控制策略的有效性。FPLIR 和實驗臺架的具體設計參數如表3 所示。

表3 FPLIR 和實驗臺架參數表Table 3 Parameters of FPLIR and test platform

5.1 仿真及結果分析

本文采用與PID 控制算法對比的方式驗證滑模變結構控制算法的魯棒性。結合前文對FPLIR 動力學模型的分析，利用仿真平臺對增穩模式進行仿真驗證，控制器模型如圖14 所示。該模型主要包括姿態指令模塊、姿態控制器模塊、電機混控模塊和飛行機構動力學模塊。姿態指令模塊用于對機器人施加姿態干擾；姿態控制器模塊包含PID 和滑模控制算法，用于對姿態進行調控；電機混控模塊用于將控制器的輸出解算為單個電機的控制指令；飛行機構動力學模塊包含機器人飛行狀態下的動力學模型，用于完成電機控制指令到轉速的轉換以及機器人飛行狀態的更新。

圖14 增穩模式仿真平臺Fig. 14 Simulation platform of stability mode

根據表3 中FPLIR 的模型參數在仿真平臺中進行參數設置，設g=9.8 m/s2。如圖15 所示，為模擬機器人在飛行過程中遇到陣風的情況，在7 s和13 s 時對機器人的俯仰角和橫滾角加入一個30°和-30°信號突變。為驗證控制方法的有效性，設置橫滾通道的滑模控制器參數為：kx=0.28，cx=0.01，εx=0.001，PID 控制器的參數為：Px=0.14，Ix=0.19，Dx=0.032；俯仰角通道的滑模控制器參數為：ky=0.53，cy=0.31， εy=0.003，PID 控制器的參數為：Py=0.35，Iy=0.21，Dy=0.035。

圖15 控制效果對比Fig. 15 Control effect comparison diagram

通過對比滑模變結構控制算法和PID 控制算法的控制效果可知，機器人在滑模變結構控制算法的控制下姿態角超調量小于在PID 控制算法控制下的超調量，避免了機器人在強干擾下因超調量過大導致機器人失穩的危險情況發生。

為有效驗證機器人落線模式和飛越桿塔模式，本文采用Gazebo 軟件對這兩種模式進行仿真模擬，仿真平臺如圖16 所示。仿真場景中使用的機器人模型與實際機器人相同，電力線桿塔模型略小于實際電力線桿塔，使用的飛行控制器與實際一致為Pixhawk4。

圖16 落線模式仿真平臺Fig. 16 Simulation platform of flying off line mode

落線模式仿真搭建：使用ACADO 工具箱建立了非線性控制問題，并使用qpOASES 作為求解器。通過使用代碼生成工具導出高效的C 代碼，該代碼集成到ROS 節點中。離散化步長被設置為dt=0.2 s，時間范圍為th=2 s。在精度和速度的權衡下，將預測時域設置為P=20，控制時域設置為M=10。對于權重矩陣，將位置的權重設置為5，速度的權重設置為2，其余的都設置為1。對于輸入權重矩陣R和輸入量u1、u2、u3、u4的權重設置為0.2，輸入量的變化率權重設置為0.1。為了保證對機器人的足夠敏捷性，控制回路頻率為設置為100 Hz。

落線模式仿真中，地線高度為10.2 m，設定的飛行高度為8.2 m。地線與機器人飛行高度差為2 m，旨在留下一定的調節范圍，減小精準落線過程中姿態的調整。圖17(a)顯示了機器人落線的飛行軌跡，當機器人飛行到空中高度為8.19 m 時，控制系統切換至落線模式。圖17(b)顯示了FPLIR 實際運行軌跡與航點軌跡誤差的對比效果，從圖中可以看出在20.3 s 時x、y、z3 個軸的誤差分別為0.0147 m、0.002 m 和0.046 m，兩點的距離差為0.048 m。圖17(c)顯示了機器人在靠近目標點時速度收斂于原點，在20.3 s 時，x、y、z3 個軸的速度分別為0.011 m/s，0.021 m/s 和0.0015 m/s，線速度為0.023 m/s。

圖17 FPLIR 飛行軌跡和速度Fig. 17 Flight path and speed of FPLIR

此外，本文落線系統與文獻[26]中的降落系統進行了比較，對比結果如表4 所示。由表4 可知，本文的落線系統讓機器人在完成降落后坐標誤差的絕對值以及x、y、z三軸方向上的速度均小于文獻[26]。而相較于本文的FPLIR 和降落目標而言，文獻[26]的中的機器人質量更輕(6.5 kg)，更容易控制，目標物更大，更容易識別，且在軟抓手降落裝置的輔助下，更容易實現穩定降落。因此，使用本文的落線系統可以實現更加準確和安全的降落。

表4 降落系統參數對比表Table 4 Comparison table of landing system parameters

對飛越桿塔模式下的仿真參數進行如下設置：將EGO-Planner 規劃器的建圖沿x、y、z3 個軸的大小分別設為50 m、50 m 和30 m，地圖分辨率設置為0.52。由于膨脹半徑不大于分辨率4 倍，因此將EGO-Planner 規劃器的膨脹半徑為設置為2 m。無人機的最大速度和最大加速度分別設置為1 m/s 和3 m/s2。

如圖18(a)所示，桿塔被化成長方體，其底面中心坐標為(7.3,0.4) m，長寬為1 m，高為15 m，規定機器人掛線行走時與桿塔保持的最小安全距離lmin=3 m。從圖18(b)中可知，在23 s 時機器人的坐標為(3.9,-0.3,10.7) m，與桿塔的距離為2.9 m，此時飛越桿塔監測器被觸發，控制系統切換至飛越桿塔模式，EGO-Planner 規劃器生成越障軌跡。飛越桿塔的過程中，在38 s 時機器人的坐標為(4.3,-2.7,11.3) m。此時機器人與桿塔的距離最近為3.2 m。通過仿真可以發現，飛越桿塔模式能夠保證機器人在與桿塔保持安全距離的情況下越過桿塔。

圖18 飛越桿塔軌跡Fig. 18 Trajectory over the tower

5.2 實驗及結果分析

實驗平臺如圖19 所示，包括電力線實驗臺架和FPLIR。其中FPLIR 包括傳感器（編碼器、壓力傳感器、相機、傾角傳感器），控制單元（NIVIDA、STM32F4、Pixhawk4），驅動單元（驅動電機、壓緊電機）。其中安裝在主壓緊輪上的編碼器用來檢測主壓緊輪的轉速，壓力傳感器用于檢測壓緊輪的附線壓力值。相機用于檢測機器人與目標物的距離，傾角傳感器安裝在輔行走輪上，用于檢測機器人所處電力線坡度。機器人的控制系統框圖如圖20 所示，其中3 個控制器彼此之間通過串行通信的方式實現數據交互。

圖19 實驗平臺Fig. 19 Test platform

圖20 機器人控制系統框圖Fig. 20 Control system block diagram of robot

實驗平臺參數如表3 所示。打滑實驗過程為機器人先下坡后爬坡，初始時機器人所處坡度為-18°，行走速度為35 cm/s，在電力線5 m 處減小其與主行走輪的靜摩擦系數至0.21。實驗結果如圖21 所示，其中圖(a)為變論域模糊控制的實驗結果圖，圖(b)為模糊控制和變論域模糊控制對機器人的控制效果對比圖。表5 為兩種控制方法重要實驗參數的對比數據表。

表5 實驗參數對比表Table 5 Comparison table of test parameters

圖21 打滑模式下的試驗曲線Fig. 21 Test curves in sliding mode

在坡度為-17.6°時，由于壓緊力偏小導致機器人在行走時出現了打滑現象，打滑度低于0.95，此時系統進入打滑控制模式。當壓緊力達到156 N時，打滑度大于0.95。坡度在15.8°～22.4°這一段中，由于機器人擺角大于2°打滑度低于0.95，此時壓緊力增量較大，旨在讓機器人在較短時間內不打滑，減小擺動。當機器人處于5.6°、-6.1°等坡度時，打滑度大于0.95 且擺角小于2°，此時壓緊力減小，當打滑度低于0.95 或擺角大于2°時，則壓緊力又開始增大。此過程是一個動態過程，旨在讓壓緊力輸出一個合適的值，避免壓緊力過大造成摩擦力變大，影響機器人的巡線速度，增加能耗，以及壓緊力不足，導致出現滑坡的現象。

通過表5 可知，采用變論域模糊控制時，平均打滑度為0.93 滿足實驗要求。變論域模糊控制相較于模糊控制而言機器人爬坡能力提升了6.92%，抑制打滑能力提升了9.44%，平均擺動角度降低了48.8%。由此可見，采用變論域模糊控制能明顯降低復雜工況對機器人的干擾，提高行走穩定性。

行走越障實驗是以防震錘為機器人的越障對象，實驗結果如圖22 所示。當機器人與防震錘的距離xd＞2 m，此時設定初始速度v=8.91 cm/s進行定速巡航。由于機器人慣性較大，在線上行走過程中速度存在±0.4 cm/s 的波動誤差。當機器人與防震錘的距離xd≤2 m時，行走越障模式被觸發，機器人進入行走越障模式。其中，當2 m ≥xd＞0.5 m時，機器人的速度采用式(44)的控制方法平滑遞減。當距離時xd≤0.5 m，此時機器人保持v=2.22 cm/s的恒定速度越過防震錘，NIVIDA 傳送回來的距離數據為0 cm。同理，當機器人越過防震錘后的距離2 m ≥xd＞0.5 m時，機器人的速度以同樣的控制方法平滑遞增；當機器人與防震錘的距離xd＞2 m時，機器人速度為v=8.91 cm/s。從圖中可以看出，機器人進入越障模式時模式切換過程平穩，行走速度變化平緩沒有出現陡增陡降的現象，保證了機器人在越障過程中的穩定性。

圖22 越障模式下速度與距離Fig. 22 Speed and distance in obstacle crossing mode

6 結束語

針對一種飛走巡線機器人，分析了機器人在結構化柔索環境下的多模式切換過程，設計了多模式切換監測器模型和各模式的控制策略。在Gazebo 等軟件平臺上建立了FPLIR 的仿真模型，并對FPLIR 線上行走進行了實驗，驗證了FPLIR 在飛行和行走狀態下監測器和控制策略的性能。

1) 仿真結果表明，針對所設計的增穩控制模式，有效地增強了FPLIR 飛行時的穩定性。落線模式保證了FPLIR 能以接近零速度和坐標誤差最大為0.048 m 的情況下完成落線，避免了FPLIR 在速度較大的情況下由于慣性導致落線失敗的情況發生。飛越桿塔模式則保證了FPLIR 能以最小3.2 m的安全距離避開電力線桿塔，完成檔段的切換。

2) 實驗研究發現，行走越障模式下通過對驅動電機的有效控制，保證了FPLIR 在線上行走越障過程中速度的平滑變換，保障了機器人在越障過程中的安全性和穩定性。打滑模式下，控制器根據變論域模糊控制規則，通過對壓緊力的合理輸出，將打滑度均值維持在0.93，擺角均值維持在1.27°，有效抑制了打滑和風擾的影響。