四旋翼的定點(diǎn)目標(biāo)追蹤及避障

劉偉男 郭英利 李媛媛

摘要：針對(duì)室內(nèi)飛行器自主飛行無法采用衛(wèi)星定位的問題，采用慣導(dǎo)測量單元、視覺攝像頭的組合導(dǎo)航方法，實(shí)現(xiàn)四旋翼對(duì)地面智能小車的檢測跟蹤與避障。本設(shè)計(jì)選用STM32F103c8t6芯片作為飛控核心，硬件有航態(tài)采集模塊、激光測距模塊、攝像頭模塊、超聲波測距模塊和以STM32F407vgt6芯片為核心的攝像頭數(shù)據(jù)計(jì)算模塊等。ICM20602芯片為四旋翼提供航姿數(shù)據(jù)，實(shí)現(xiàn)四旋翼的穩(wěn)定飛行。攝像頭模塊為四旋翼提供導(dǎo)航數(shù)據(jù)，使四旋翼檢測到智能小車且定高飛行。系統(tǒng)增設(shè)STM32F407vgt6芯片處理攝像頭數(shù)據(jù)以確保攝像頭數(shù)據(jù)計(jì)算的同步性，通過超聲波測距模塊對(duì)障礙物進(jìn)行檢測并傳輸距離數(shù)據(jù)給飛控，使四旋翼實(shí)現(xiàn)定高跟隨智能小車飛行，任務(wù)中對(duì)面積型障礙物實(shí)現(xiàn)避障。

關(guān)鍵詞：四旋翼；攝像頭；避障；自主飛行

1 系統(tǒng)硬件設(shè)計(jì)

飛控核心采用STM32F103c8t6芯片，由航態(tài)采集模塊、激光測距模塊、超聲波測距模塊、攝像頭模塊、攝像頭數(shù)據(jù)計(jì)算模塊組成，如圖1所示。

1.1 飛行控制模塊選擇

針對(duì)四旋翼飛行過程中需要極高的數(shù)據(jù)實(shí)時(shí)性，因此對(duì)芯片的時(shí)鐘頻率有較高的要求。本設(shè)計(jì)采用STM32F103c8t6處理器，最高工作頻率可達(dá)72MHz，內(nèi)置有64KB Flash和20KB RAM。該芯片實(shí)時(shí)性高、功耗低，擁有豐富的外設(shè)，為二次開發(fā)用戶提供硬件接口。

1.2航態(tài)采集模塊選擇

本設(shè)計(jì)采用ICM20602模塊，該模塊通過3.3V-5.0V的電源供電，使用標(biāo)準(zhǔn)SPI通信協(xié)議實(shí)現(xiàn)與單片機(jī)的通信。芯片內(nèi)置有AD轉(zhuǎn)換器，可輸出16位數(shù)據(jù)，具有優(yōu)越的數(shù)據(jù)實(shí)時(shí)性和測量范圍可調(diào)等優(yōu)點(diǎn)。

1.3 測距模塊選擇

在本設(shè)計(jì)中四旋翼實(shí)現(xiàn)定高飛行采用VL53L0X激光測距模塊，用US-100超聲波測距模塊實(shí)現(xiàn)對(duì)障礙物距離的檢測。VL53L0X具有封裝尺寸小、靈敏度高等特點(diǎn)，可測量真實(shí)距離范圍為0.02mm-2000mm。US-100可采用串口通信讀取距離數(shù)據(jù)，可測量真實(shí)距離范圍為2cm-200cm。

1.4機(jī)器視覺系統(tǒng)選擇

機(jī)器視覺系統(tǒng)為四旋翼提供導(dǎo)航參數(shù)是系統(tǒng)的重要組成部分，它由視覺攝像頭OV7670和數(shù)據(jù)計(jì)算芯片STM32F407vgt6組成。OV7670模塊具有體積小、工作電壓低、靈敏度高等優(yōu)點(diǎn)。系統(tǒng)還增設(shè)由STM32F407vgt6芯片處理圖像數(shù)據(jù)，其擁有高達(dá)168MHz的工作頻率可以高速處理來自攝像頭的圖像數(shù)據(jù)，經(jīng)過測試可以1s處理8幀的圖像。

2 系統(tǒng)軟件設(shè)計(jì)

2.1 飛控算法分析

分析四旋翼的飛行原理可知，飛控獲取四旋翼的姿態(tài)信息，經(jīng)過控制器輸出不同占空比的PWM信號(hào)控制電機(jī)的轉(zhuǎn)速來控制四旋翼的姿態(tài)。利用ICM20602芯片讀取飛行姿態(tài)的原始數(shù)據(jù)，并將原始數(shù)據(jù)經(jīng)計(jì)算轉(zhuǎn)換為歐拉角，通過采用串級(jí)PID控制器算法控制飛行姿態(tài)。實(shí)測角度與期望角度作差得到角度誤差，角度誤差輸入到角度控制器中控制誤差為0；角度誤差作為角速度控制器的輸入值計(jì)算角速度，期望的角速度與當(dāng)前角速度作差得到角速度誤差，乘以角速度誤差系數(shù)k_p得到KP，將角速度誤差限幅后累加到KI中，前后兩次的角速度誤差的差值存儲(chǔ)到KD中，最后將KP、KI、KD三者相加并限幅后得到PID輸出。串級(jí)PID相比單級(jí)PID具有更優(yōu)的穩(wěn)定性，經(jīng)過測試，證明該飛控算法滿足設(shè)計(jì)要求。

2.2 機(jī)器視覺攝像頭算法分析

本設(shè)計(jì)采用OV7670攝像頭識(shí)別小車位置信息，實(shí)現(xiàn)目標(biāo)是讓四旋翼時(shí)刻處于小車的正上方。將攝像頭安裝在四旋翼底部中軸線中心的位置，并且裝有VL53L0X激光測距模塊。飛控通過讀取高度信息，經(jīng)過油門控制器輸出油門值控制四旋翼的飛行高度，保證圖像數(shù)據(jù)不會(huì)因四旋翼飛行高度變化而受到影響。系統(tǒng)還增設(shè)一片STM32F407圖像處理芯片來保證圖像數(shù)據(jù)計(jì)算的實(shí)時(shí)性，攝像頭采集實(shí)時(shí)圖像得到小車的位置信息，圖像處理芯片對(duì)當(dāng)前采集到的圖像信息進(jìn)行判斷，把計(jì)算后得到的小車實(shí)際位置通過串口發(fā)送給飛控控制四旋翼飛行線路實(shí)現(xiàn)跟隨小車飛行。

測試場景由黑色小車和白色背景構(gòu)成，圖像數(shù)據(jù)簡單且對(duì)比度明顯。在圖像數(shù)據(jù)計(jì)算時(shí)將一幀圖像轉(zhuǎn)換成“0”、“1”（0 為背景，1為小車）信息保存，然后提取小車中心以得到小車的位置。

本文設(shè)計(jì)的圖像識(shí)別算法分為3步：

（1）對(duì)一幀圖像進(jìn)行中值濾波。將攝像頭采集的圖像通過中值濾波去除噪聲，防止噪聲干擾。

（2）提取圖像每一行變量為“1”的中心點(diǎn)。依次判斷每一行中每一個(gè)像素點(diǎn)是否為黑點(diǎn)（數(shù)據(jù)信息“1”），當(dāng)遇到黑點(diǎn)時(shí)則統(tǒng)計(jì)黑點(diǎn)個(gè)數(shù)（設(shè)計(jì)數(shù)為Y），白點(diǎn)不予記錄。記錄黑點(diǎn)在該行的位置并與之前黑點(diǎn)的位置相加保存到X，X值是該行所有黑點(diǎn)位置的總和，用X/Y得到該行黑點(diǎn)的中心點(diǎn)，對(duì)中心點(diǎn)數(shù)據(jù)求均值，即將所有點(diǎn)值求和后除以行數(shù)。

（3）對(duì)比前后兩幀計(jì)算后的中心點(diǎn)位置差值，判斷當(dāng)前四旋翼的位置并根據(jù)偏移量調(diào)整四旋翼的位置。圖像識(shí)別算法的流程圖如圖2所示。

經(jīng)過測試，由于測試場景對(duì)比度高，可以準(zhǔn)確的確定小車的位置。因?yàn)轱w控處理能力有限，若增加圖像處理任務(wù)實(shí)時(shí)性差，容易導(dǎo)致四旋翼飛出攝像頭識(shí)別小車的范圍，添加了一片STM32F407圖像處理芯片后實(shí)現(xiàn) 1s處理5幀圖像，大大提高了實(shí)時(shí)性。

3 結(jié)論

本文提出一種基于機(jī)器視覺和雙控制芯片的自主飛行四旋翼的設(shè)計(jì)思路，在簡單背景與小車對(duì)比度大的情況下，證明圖像處理算法的可靠性。經(jīng)測試證明四旋翼飛行穩(wěn)定，跟隨小車飛行效果優(yōu)秀，由于采用了雙處理器計(jì)算數(shù)據(jù)，極大程度上提高了四旋翼的實(shí)時(shí)性。

參考文獻(xiàn)：

[1]李文超.X70四旋翼飛行器建模與飛行控制研究：[D].南京：南京理工大學(xué)，2014

[2]章志誠.基于STM32嵌入式四旋翼飛行器控制系統(tǒng)設(shè)計(jì)[D].杭州：浙江工業(yè)大學(xué)，2014

[3]姜強(qiáng)，曾勇，劉強(qiáng).四旋翼飛行器姿態(tài)航向參考系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)[J].控制工程，2013，20：167-169

基金項(xiàng)目：沈陽航空航天大學(xué)大學(xué)生創(chuàng)新創(chuàng)業(yè)訓(xùn)練計(jì)劃項(xiàng)目資助