吳建鋒，徐振宇，秦會(huì)斌

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

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

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

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

1 配送無(wú)人機(jī)的總體架構(gòu)

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

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

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

圖1 系統(tǒng)結(jié)構(gòu)框圖

2 軟件設(shè)計(jì)

2.1 姿態(tài)解算

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

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

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

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

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

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

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

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

式中:ωx、ωy和ωz是經(jīng)過(guò)加速度計(jì)補(bǔ)償?shù)耐勇輧x的測(cè)量值，T為采樣間隔時(shí)間。

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

式中:φ表示滾轉(zhuǎn)角、θ表示俯仰角、φ表示偏航角。

2.2 串級(jí)模糊PID控制算法

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

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

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

圖3 模糊PID控制算法

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

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

誤差e和誤差變化量Δe作為模糊控制器輸入，ΔKp、ΔKi和ΔKd、α和β作為輸出。e、Δe、ΔKp、ΔKi和ΔKd的模糊子集為{NB(負(fù)大)、NM(負(fù)中)、NS(負(fù)小)、ZO(零)、PS(正小)、PM(正中)、PB(正大)}，采用7個(gè)量化等級(jí)，論域?yàn)閧-3，-2，-1，0，1，2，3}。α和β的模糊子集為{ZO(零)，PS(正大)，PM(正中)，PB(正大)}，采用4個(gè)量化等級(jí)，α的論域?yàn)閧1，1.5，2，2.5}，β的論域?yàn)閧0，0.33，0.67，1}。其中e、Δe、ΔKp、ΔKi和ΔKd隸屬度函數(shù)如圖4所示，α和β的隸屬度函數(shù)與之類(lèi)似。根據(jù)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ī)則表

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

圖5 串級(jí)PID控制算法

2.3 數(shù)字濾波算法

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

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

圖7 濾波后的波形圖

4 實(shí)驗(yàn)測(cè)試

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

圖8 俯仰角

圖9 橫滾角

5 結(jié)論

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