基于LC諧振電路的智能配送無人機設計*

吳建鋒，徐振宇，秦會斌

(1.浙江樹人大學信息科技學院，浙江 杭州 310015；2.杭州電子科技大學電子信息學院，浙江 杭州 310018)

隨著經濟的飛速發(fā)展與人民生活水平的提高，各種新型餐飲形式層出不窮[1]，與此同時人口老齡化比例不斷上升[2]，勞動力成本提升等問題也接踵而來[3]，這導致我國對服務型智能機器人的需求十分迫切，我國服務型機器人市場潛力巨大[4]。

近年來代替服務員的配送機器人逐漸進入市場，市場上餐機器人路徑的識別通常使用激光，攝像頭和電荷耦合器件(CCD)等傳感器，這些傳感器容易受到外界環(huán)境中的強光干擾，導致傳感器感應能力下降，給機器人也帶來許許多多的問題，比如路徑偏差[5]，與顧客發(fā)生碰撞等[6]。

為克服上述的一系列問題，此本文設計了一款基于電磁檢測的無人機，采用四軸飛行器作為載體，利用LC諧振電路檢測電磁信號。通過四軸飛行器作為載體的空中的方式可以避免影響餐廳內顧客的行走與就餐[7]。此外路徑上的諧振信號的頻率可以設定或改變，選擇一個合適的諧振信號可以避免外界干擾源對諧振信號的干擾，從而提高了循跡過程的穩(wěn)定性，保證了顧客的安全[8]。

1 配送無人機的總體架構

無人機采用四軸飛行器的形式，以STM32作為主控制器，同時搭載慣性傳感器、WiFi通信模塊、超聲波測距模塊以及電調和電機。慣性傳感器將采集到的數(shù)據傳送給微處理器，由姿態(tài)算法實時估計出無人機的飛行姿態(tài)。根據獲得的姿態(tài)信息，使用串級模糊PID控制算法，通過電調調節(jié)電機轉速從而控制無人機的飛行速度和方向[9]。RFID讀寫模塊，用于無人機的定位，通過讀取預先安置的RFID標簽來確定無人機的起點和終點。超聲波測距模塊用來測量檢測障礙物，防止無人機發(fā)生碰撞。WiFi模塊用于和控制中心通信，收發(fā)指令和數(shù)據。

無人機通過循跡模塊檢測預設的電磁信號，該信號為20 kHz的交流信號，根據檢測到信號的路徑，控制無人機飛往目標位置。循跡模塊主要由2部分組成，一個是LC諧振電路，另一個是信號處理電路。LC諧振電路用于檢測交流信號所參數(shù)電磁信號[10]，無人機上共裝有三路LC諧振電路，中間路的LC諧振電路用于檢測信號路徑的強度，判定無人機偏離軌道的程度，左右兩路分別用于判斷無人機偏離的方現(xiàn)和偏離方現(xiàn)的程度。此外，由于諧振電路產生的感生電動勢較小，且為交流信號，無法利用A/D采樣直接進行處理。因此，使用一個信號處理電路需要包括放大電路和檢波電路，通過放大電路對信號進行放大，再將放大后的交流信號轉換成直流信號由高精度的A/D進行采用并將信號傳輸至STM32微控制器進行處理[11]。

電源模塊由22.2 V的動力鋰電池提供總電源，轉換成系統(tǒng)所需的各種電壓，系統(tǒng)結構框圖如圖1所示。

圖1 系統(tǒng)結構框圖

2 軟件設計

2.1 姿態(tài)解算

無人機的姿態(tài)指的是無人機的機體坐標系與慣性坐標系之間的角位置關系，表示這種關系的方法有多種，其中四元數(shù)法比較適合用作姿態(tài)解算，不會發(fā)生萬向節(jié)死鎖，且計算量小，只需要求解4個參數(shù)，在實際應用中最為廣泛[12]，同時也是本文所采用的方法。

姿態(tài)解算的數(shù)據由慣性傳感器提供，無人機采用MPU6050傳感器芯片，它是一款6軸傳感器芯片，集成了一個3軸陀螺儀和一個3軸加速度計。陀螺儀用來測量無人機的3軸旋轉角速度，可以通過角速度的積分來估計當前的姿態(tài)角，短時間內比較準確，但誤差會隨時間累積。加速度計測量無人機的3軸加速度，測量值不會受時間變化影響，但由于加速度采集到的信號噪聲比較大，單純的使用加速度計同樣也會存在誤差[13]。因此必須將兩者的數(shù)據結合起來，才能得到較為準確的姿態(tài)信息。本文采用一種基于四元數(shù)的互補濾波進行姿態(tài)解算，基本框圖如圖2所示。

圖2 四元數(shù)互補濾波

首先將慣性坐標系中的重力加速gn歸一化后，轉換到機體坐標系上得到gb，如式(1)所示，其中為方向余弦矩陣，如式(2)所示。

將加速度計的測量值與gb進行矢量叉乘，得到修正誤差，即從加速度計測量矢量到由方向余弦推算到的矢量之間需要旋轉修正的補償誤差。q0、q1、q2和q3為四元數(shù)的四個元素。

為了實現(xiàn)穩(wěn)態(tài)控制并消除長時間累積誤差，采用比例-積分控制器來對該誤差進行處理，如式(3)所示:

式中:ω0(t)為陀螺儀測量值，ω(t)為修正后的值，e(t)為誤差，Kp和Ki分別為比例和積分系數(shù)，用來調節(jié)加速度計信息和陀螺儀信息之間的權重。

四元數(shù)采用龍格庫塔法進行更新，如式(4)所示:

式中:ωx、ωy和ωz是經過加速度計補償?shù)耐勇輧x的測量值，T為采樣間隔時間。

將得到的新四元數(shù)進行歸一化處理，計算出歐拉角和方向余弦矩陣并進入下一次迭代，計算方式如式(5)和式(1)所示:

式中:φ表示滾轉角、θ表示俯仰角、φ表示偏航角。

2.2 串級模糊PID控制算法

PID控制算法是經典控制理論中常用的一種控制方法，它可以分成3個部分，分別是比例、積分和微分控制。比例控制根據輸出反饋值和輸入值之間的差，按照一定的比例進行調節(jié)，但是純比例控制會存在靜態(tài)誤差，因此增加積分控制部分，提高控制精度，微分控制則可以實現(xiàn)系統(tǒng)的超前調節(jié)，提前減小控制輸入量，減少甚至消除超調現(xiàn)象[14]。位置式PID控制算法計算表達式如式(7)所示:

式中:Kp、Ki和Kd分別為比例、積分、微分系數(shù)，e(t)為輸入和輸出反饋的誤差，u(t)為調節(jié)量。

為了提高PID控制器的穩(wěn)定性和響應速度，對經典PID控制器進行改進，增加模糊控制器來調節(jié)PID系數(shù)，改進的模糊PID控制器[15]控制原理如圖3所示。

圖3 模糊PID控制算法

PID參數(shù)Kp、Ki和Kd的計算方式如式(7)所示:

Kp0、Ki0和Kd0為初始值，經過模糊控制系統(tǒng)后得到實際參數(shù)，ΔKp、ΔKi和ΔKd能夠在一定程度上對參數(shù)進行調節(jié)，但調節(jié)的范圍受限，不夠靈活，因此在此基礎上，引入系數(shù)α和β，提高調節(jié)范圍，并實現(xiàn)積分分離，防止發(fā)生積分飽和。

誤差e和誤差變化量Δe作為模糊控制器輸入，ΔKp、ΔKi和ΔKd、α和β作為輸出。e、Δe、ΔKp、ΔKi和ΔKd的模糊子集為{NB(負大)、NM(負中)、NS(負小)、ZO(零)、PS(正小)、PM(正中)、PB(正大)}，采用7個量化等級，論域為{-3，-2，-1，0，1，2，3}。α和β的模糊子集為{ZO(零)，PS(正大)，PM(正中)，PB(正大)}，采用4個量化等級，α的論域為{1，1.5，2，2.5}，β的論域為{0，0.33，0.67，1}。其中e、Δe、ΔKp、ΔKi和ΔKd隸屬度函數(shù)如圖4所示，α和β的隸屬度函數(shù)與之類似。根據PID參數(shù)整定原理和方法，制定的模糊控制規(guī)則表如表1~表5所示。

表5 β模糊控制規(guī)則表

圖4 e、Δe、ΔKp、ΔKi和ΔKd隸屬度函數(shù)

表1 ΔKp模糊控制規(guī)則表

表2 ΔKi模糊控制規(guī)則表

表3 ΔKd模糊控制規(guī)則表

表4 α模糊控制規(guī)則表

無人機采用串級PID控制算法，其控制原理如圖5所示，在無人機進行工作前，無人機將被擺放在鋪設諧振電路的路徑上，并使中間路的LC諧振電路獲取到一個最大的輸入信號作為期望信號，左右兩側的LC諧振電路分別獲取兩個信號用于輔助控制無人機進行飛行方向控制。圖5中位置環(huán)用于控制無人機的路徑追蹤，防止偏移設定路徑。當無人機移動路徑發(fā)生偏差時，中間路的LC諧振電路信號變小，兩側的LC諧振電路信號分別增大和減小，從而可以判斷偏移的方向和偏移的程度，將當前的中間路的LC諧振電路信號和期望信號的差值輸入到位置環(huán)的PID控制，并將輸出信號輸入到角度環(huán)進行進一步處理。無人機的角度環(huán)用于保持無人機的角度，并控制無人機的移動方向，無人機的角速度環(huán)用于操控無人機的速度和平衡度。因此位置環(huán)的輸出和當前角度的反饋信息共同控制無人機的移動方向并使無人機保持平衡，在此過程中通過角速度環(huán)控制電機的轉速，使無人機平穩(wěn)的返回路徑，并按照一定速度按照路徑進行移動。

圖5 串級PID控制算法

2.3 數(shù)字濾波算法

由于無人機在飛行過程中機體震動比較大，受其影響，傳感器將會輸出帶有高頻噪聲的原始數(shù)據，這種噪聲對姿態(tài)解算和飛行控制都有一定的影響[16]。因此必須通過濾波算法將引入的噪聲進行濾除[17]。移動平滑濾波是一種簡單的低通濾波器，它是一種從時域方面進行設計的濾波算法[18]。一般采用簡單平均的方式實現(xiàn)，即對時域相鄰采樣點的數(shù)值求平均值，本系統(tǒng)為了避免信號采集中混入誤差信號，在同一時間節(jié)點內進行20次的信號采集，默認最大的5組信號和最小的5組信號為誤差信號。去除10組誤差信號后面對剩余10組信號進行求平均值獲得最終信號。針對原始加速度數(shù)據，采用平滑濾波算法前后對比如圖6和圖7所示，其中圖6是原始數(shù)據，圖7是經過濾波后的數(shù)據，對比圖6和圖7可得，圖7的加速度曲線更為平滑，無人機在飛行過程中能夠避免頻繁的加速/減速，從而保證配送過程的穩(wěn)定性。

圖6 濾波前的原始數(shù)據波形圖

圖7 濾波后的波形圖

4 實驗測試

為了驗證姿態(tài)解算算法以及串級模糊PID控制算法的控制效果，進行了一段時間的飛行實驗，采集了無人機的俯仰角和滾轉角數(shù)據如圖8和圖9所示。在測試的30 s內，俯仰角在(-13，15)°，橫滾角在(-11，15)°，無人機在飛行過程中保持平衡。實驗表明，無人機能夠獲取比較準確的姿態(tài)數(shù)據(俯仰角，橫滾角，加速度)，實現(xiàn)穩(wěn)定飛行，具有較好的應用和推廣價值。

圖8 俯仰角

圖9 橫滾角

5 結論

針對傳統(tǒng)配送機器人在配送過程中出現(xiàn)路徑偏差，碰撞顧客等問題，提出了一種基于電磁檢測的無人機，通過LC諧振電路對電磁信號進行檢測，能夠獲得正確的路徑信息，并利用一種基于四元數(shù)的互補濾波算法進行姿態(tài)解算，改進的模糊PID控制算法進一步提高了可無人機的穩(wěn)定性和響應速度。實驗表明該無人機能夠穩(wěn)定、可靠的完成任務。