基于卡爾曼濾波數(shù)據(jù)融合算法的智能釣魚竿系統(tǒng)①

陳小磊, 岳俊峰, 李秀梅

(杭州師范大學(xué) 信息科學(xué)與工程學(xué)院, 杭州 311121)

釣魚作為一項考驗技術(shù)、耐力、經(jīng)驗的戶外運動,已經(jīng)被越來越多人所接受, 而釣魚活動中最重要的工具就是釣魚竿, 傳統(tǒng)釣魚竿往往需要通過長時間的釣魚活動訓(xùn)練和長時間盯著魚漂才能看漂識魚, 極大地降低了釣魚的樂趣.目前市場上加裝傳感器的魚竿一定程度上彌補了傳統(tǒng)釣魚竿的不足, 但此類釣魚竿依然存在很多缺點: (1)檢測裝置單一, 缺乏水域監(jiān)測能力.(2)抗干擾能力較差, 有魚咬鉤時, 報警信號誤報率較高, 咬鉤時不能準(zhǔn)確報警.(3)不具備自動遛魚功能,斷線跑魚概率較大.

針對上述情況, 本文利用多種傳感器和MCU 控制器及外圍硬件電路, 通過嵌入式技術(shù)和自動化控制技術(shù), 研究一種具有智能選釣位、智能報警及自動遛魚等功能的新型智能釣魚竿系統(tǒng).該智能釣魚竿系統(tǒng)將有助于提高釣魚的精確度, 同時提供一種自動化釣魚方案, 為在惡劣環(huán)境下完成自主釣魚活動提供可能.本系統(tǒng)基于卡爾曼濾波的數(shù)據(jù)融合算法對四元數(shù)法解算得到的魚竿姿態(tài)角數(shù)據(jù)進一步修正, 最優(yōu)魚竿姿態(tài)角是智能報警和智能遛魚功能實現(xiàn)的關(guān)鍵.因此本文主要研究魚竿姿態(tài)解算過程、姿態(tài)數(shù)據(jù)濾波算法及魚竿自動遛魚控制算法等內(nèi)容.魚竿姿態(tài)解算、控制系統(tǒng)數(shù)學(xué)模型以及串級PID 控制算法的仿真測試結(jié)果表明, 控制系統(tǒng)響應(yīng)良好.在uC/OS-II 操作系統(tǒng)下, 環(huán)境傳感器及姿態(tài)傳感器的數(shù)據(jù)解算實時性良好, 例如魚竿報警功能, 具有較高靈敏度和準(zhǔn)確率, 符合預(yù)期目標(biāo).

1 智能釣魚竿系統(tǒng)總體方案設(shè)計

智能釣魚竿主要由8 個部分組成, 如圖1 所示, 其中① 為魚竿桿體, ② 為負(fù)責(zé)收放線的紡線輪, ③ 為環(huán)境檢測部分, 該部分主要由溫度傳感器、光線傳感器、溶解氧傳感器及深度傳感器所組成, 該部分將負(fù)責(zé)測量待釣水域的環(huán)境信息以作為釣位選擇的依據(jù), ④ 為姿態(tài)傳感器部分該部分主要由陀螺儀傳感器、加速度計傳感器及磁力計傳感器所組成, 該部分主要功能為獲取魚竿姿態(tài), ⑤ 為魚竿手柄該部分, ⑥ 為魚竿控制部分, 該部分由STM32 控制器、電機驅(qū)動、電機及報警模塊組成, 為智能釣魚竿控制部分的核心, 其通過魚竿姿態(tài)的變化來實現(xiàn)智能釣魚竿的報警及遛魚功能, ⑦ 為電源模塊.該模塊主要負(fù)責(zé)為整個系統(tǒng)供電.⑧ 為魚線.

釣魚竿智能選釣位功能的實現(xiàn)是通過將溫濕度傳感器、光線傳感器、深度傳感器、溶氧量傳感器及微控制器集成于一個防水球體內(nèi), 在釣魚時, 將此可拆卸的球體隨釣魚鉤一起投入待釣水域, 通過球體內(nèi)的微控制器及傳感器采集待釣水域的溫度、光線、深度及含氧量等數(shù)據(jù), 并通過數(shù)據(jù)分析預(yù)判該水域是否適宜魚逗留, 進而實現(xiàn)智能選釣位的功能.

圖1 智能釣魚竿示意圖

釣魚竿智能報警功能的實現(xiàn)是通過在釣竿頂部安裝姿態(tài)傳感器, 通過姿態(tài)傳感器中的三軸加速度計采集加速度數(shù)據(jù), 三軸磁力計采集磁場數(shù)據(jù), 三軸陀螺儀采集角速度數(shù)據(jù), 微控制器分別對三軸陀螺儀和三軸加速度計采集到的角速度數(shù)據(jù)和加速度數(shù)據(jù)通過卡爾曼濾波算法進行數(shù)據(jù)融合, 轉(zhuǎn)化為歐拉角進而獲取釣竿姿態(tài).當(dāng)魚有咬鉤信號時, 魚竿會有向下的瞬時加速度, 同時魚竿發(fā)生形變即會有一定的傾角變化, 此部分功能將通過瞬時加速度及魚竿姿態(tài)變化傾角作為報警信號, 通過高精度姿態(tài)傳感器獲取魚竿姿態(tài)變化, 能更加有效的捕捉到魚咬鉤的輕微信號, 這將極大地減少誤判率進而改進目前市場大多魚竿報警率低的問題.

釣魚竿智能遛魚功能通過自動化控制技術(shù)實現(xiàn),通過測量及預(yù)判魚線實時拉力值, 建立魚竿物理模型,利用魚竿姿態(tài)傾角變化來驅(qū)動電機, 可有效控制魚線收放過程, 使遛魚過程中魚線始終處于魚線的最佳拉力值附近, 從而減少斷線跑魚的概率, 實現(xiàn)智能遛魚功能.

2 智能釣魚竿系統(tǒng)硬件部分設(shè)計

智能釣魚竿系統(tǒng)硬件部分設(shè)計框圖如圖2 所示.本文選用STM32F103ZET6 微控制器作為智能釣魚竿核心控制器, 溫濕度傳感器DHT11 和光敏電阻模塊作為環(huán)境傳感器, MPU6050 六軸加速度/角速度傳感器作為魚竿姿態(tài)采集模塊, TB6612FNG 作為直流減速電機驅(qū)動模塊, 12 V 聚合物鋰電池作為電源模塊.

圖2 智能釣魚竿系統(tǒng)硬件部分設(shè)計框圖

3 智能釣魚竿系統(tǒng)軟件部分設(shè)計

本部分主要完成以下內(nèi)容: (1) 四元數(shù)法解算魚竿姿態(tài); (2) 通過建立卡爾曼濾波算法的狀態(tài)方程和觀測方程實現(xiàn)基于卡爾曼濾波的數(shù)據(jù)融合算法; (3) 建立串級PID 算法模型, 并通過Simulink 仿真整定PID 參數(shù),進而實現(xiàn)智能釣魚竿的控制.

為了實現(xiàn)智能遛魚功能, 魚竿電機控制流程如圖3,通過加速度和磁力計得到的姿態(tài)角作為卡爾曼數(shù)據(jù)融合算法的測量值, 將陀螺儀傳感器得出的姿態(tài)角作為預(yù)測值, 通過卡爾曼濾波數(shù)據(jù)融合得到精準(zhǔn)姿態(tài)角.將由數(shù)據(jù)融合的到的姿態(tài)角作為串級PID 算法的輸入量, 通過外環(huán)角度PID 和內(nèi)環(huán)角速度PID 算法求出電機控制調(diào)節(jié)量, 通過電機驅(qū)動來控制電機轉(zhuǎn)向和轉(zhuǎn)速.

圖3 魚竿電機控制流程圖

3.1 四元數(shù)姿態(tài)解算

四元數(shù)法是比較常用的姿態(tài)解算方法, 通過引入一個四維空間超復(fù)數(shù), 當(dāng)實部為零時, 建立了四維空間和三維空間的聯(lián)系, 由于角速度積分有累計誤差的特性, 因此需要借助加速度分量對四元數(shù)矩陣進行修正,再利用四元數(shù)和歐拉角的轉(zhuǎn)換公式即可計算出歐拉角.

圖4 為智能釣魚竿四元數(shù)姿態(tài)解算的流程圖.首先初始狀態(tài)下, 由四元數(shù)和歐拉角的關(guān)系確定初始四元數(shù), 姿態(tài)傳感器采集原始數(shù)據(jù)并通過均值濾波方法過濾掉高頻信號, 然后將四元數(shù)的等效余弦矩陣中的重力的分向量歸一化后進行向量的叉積運算, 進而求解出陀螺儀的積分誤差, 得到準(zhǔn)確的角速度后采用一階龍格庫塔法更新四元數(shù)和姿態(tài)矩陣, 將四元數(shù)歸一化后即可轉(zhuǎn)換出歐拉角.

圖4 四元數(shù)姿態(tài)解算流程圖

3.1.1 四元數(shù)定義

四元數(shù)是一個復(fù)數(shù)由實部和虛部組成, 四元數(shù)的虛部包含3 個虛數(shù)單位i、j、k, 即四元數(shù)可以表示為:

其中, 復(fù)數(shù)間有以下關(guān)系:

四元數(shù)的矩陣表示形式為:

在三維坐標(biāo)系中, 用四元數(shù)表示繞著某個軸的旋轉(zhuǎn)公式可用式(6)來表示, 其中, α 表示繞軸旋轉(zhuǎn)的實際角度, cos(βx), c os(βy)和 cos (βz)表示定位旋轉(zhuǎn)軸的方向余弦.當(dāng)四元數(shù)的范數(shù)滿足時, 稱該四元數(shù)為單位四元數(shù).該約束條件建立了三維和四維空間的聯(lián)系, 可以利用四維空間的四元數(shù)性質(zhì)和運算規(guī)則來解決三維空間中的旋轉(zhuǎn)問題.

3.1.2 四元數(shù)微分方程q0, q1, q2, q3是時間的函數(shù).可建立四元數(shù)微分方程如下:

由于對象是運動的, 所以四元數(shù)Q 是個變量, 即

其中, ω 為沿坐標(biāo)系的角速度.

將上述公式展開:

姿態(tài)矩陣角速度 ω= [ωx,ωy,ωz]T可通過陀螺儀測量值經(jīng)過補償后得到.

3.1.3 四元數(shù)姿態(tài)矩陣關(guān)系

四元數(shù)轉(zhuǎn)換成方向余弦矩陣: 設(shè)有參考坐標(biāo)系e,鋼體坐標(biāo)系為b, 假設(shè)初始狀態(tài)e 坐標(biāo)系與b 坐標(biāo)系重合, 則通過四元數(shù)可以得出b 坐標(biāo)系旋轉(zhuǎn)至e 坐標(biāo)系的轉(zhuǎn)換矩陣, 坐標(biāo)轉(zhuǎn)換矩陣公式見式(9)[1]:

通過式(10)可求出由b 坐標(biāo)系轉(zhuǎn)換至e 坐標(biāo)系的坐標(biāo)(Xe,Ye,Ze).

從方向余弦矩陣歐拉角的轉(zhuǎn)換: 設(shè)魚竿的偏航角為 ψ, 俯仰角為θ, 翻滾角為 φ, 假設(shè)導(dǎo)航坐標(biāo)系為g, 則剛體坐標(biāo)系b 與導(dǎo)航坐標(biāo)系e 的關(guān)系如下[2]:

我們認(rèn)為:

由于在坐標(biāo)系在旋轉(zhuǎn)過程中坐標(biāo)系一直保持正交關(guān)系, 所以有為正交矩陣:

由式(13)和(14)得:

由式(14)和式(15)通過上述公式變換即可解算出載體的姿態(tài)角得出:

3.1.4 初始四元數(shù)

設(shè)三軸加速度計和三軸磁力計在靜止條件下的初始姿態(tài)角為( ψ, θ , φ).有:

由式(13)和式(17)可得:

由式(18)即可得出初始四元數(shù).

3.1.5 更新四元數(shù)

四元數(shù)的更新常采用一階龍格-庫塔法算法來實現(xiàn)[3], 一階龍格-庫塔法的解為:

進而可推出:

設(shè) gx、 gy、 gz為陀螺儀傳感器測量的角速度, 設(shè)T 為姿態(tài)更新時間, 則由四元數(shù)微分方程的求解公式可得更新后的四元數(shù):

3.1.6 四元數(shù)轉(zhuǎn)換歐拉角

計算當(dāng)前時刻的四元數(shù)Q (t+T), 并對其進行歸一化處理:

設(shè)四元數(shù)[q0q1q2q3]的模為mode, 則:

由式(24)可得姿態(tài)角為:

3.2 卡爾曼濾波算法

作為一種高效的自回歸濾波器, 卡爾曼濾波常用于解決離散的線性系統(tǒng)濾波問題.卡爾曼濾波通過最小均方差來衡量最佳估計的標(biāo)準(zhǔn), 算法的實現(xiàn)是通過求解狀態(tài)方程和觀測方程然后不斷的更新最小均方差,進而得出最優(yōu)估計.

3.2.1 狀態(tài)方程和觀測方程

一個常規(guī)的線性離散系統(tǒng)模型, 其狀態(tài)空間方程如式(26)所示:

其中, Fk-1為 k-1 時刻對應(yīng)狀態(tài)轉(zhuǎn)移系數(shù)矩陣; Gk-1為k-1 時刻對應(yīng)控制輸入的增益矩陣; w 為過程噪聲.

觀測方程如下:

其中, zk表示k 時刻的觀測值; H 表示測量系數(shù)矩陣;vk表示觀測噪聲.

狀態(tài)方程中的噪聲w 和測量噪聲v 默認(rèn)情況下認(rèn)為兩者是相互獨立且滿足高斯分布的白噪聲, 即:

為了在實際使用中簡化算法, 常假設(shè)噪聲協(xié)方差矩陣Q 和測量噪聲協(xié)方差矩陣R 均為定值.

3.2.2 卡爾曼濾波算法流程

卡爾曼濾波算法在數(shù)學(xué)方法中是一種遞歸預(yù)測法,以偏差的最小方差為標(biāo)準(zhǔn), 計算得到系統(tǒng)的最優(yōu)估計值, 具有較好的實時性和抗干擾性[4].卡爾曼濾波算法是通過測量過程和預(yù)測過程間的相互更新相互反饋并通過求解卡爾曼增益和更新狀態(tài)方程來實現(xiàn)系統(tǒng)的自回歸狀態(tài)估計, 進而求出系統(tǒng)的最優(yōu)估計.卡爾曼濾波器可分為時間更新方程和測量更新方程.

式(30)和式(31)為卡爾曼時間更新方程:

式(33)和式(34)為狀態(tài)更新方程:

狀態(tài)更新方程中, K 為濾波增益矩陣即卡爾曼增益, { zk-Hxkˉ}為觀測值和預(yù)測觀測的殘差, 和卡爾曼增益K 一起修正先驗預(yù)測, 更新狀態(tài)估計, 利用先驗估計誤差的協(xié)方差矩陣更新 Pk, 以備下次自回歸運算使用.

3.3 基于卡爾曼濾波的數(shù)據(jù)融合算法

由于三軸陀螺儀是通過角速度的積分獲得姿態(tài)角,存在隨機漂移誤差和常值誤差, 隨著時間的增加, 必然會使輸出誤差也積累增加, 因此單獨使用陀螺儀進行姿態(tài)解算無法獲得準(zhǔn)確的姿態(tài)角.加速度計只有在靜止或勻速狀態(tài)下通過測量重力場, 可以準(zhǔn)確計算出姿態(tài)角, 但是在實際情況中加速度計通常處于動態(tài)環(huán)境中, 因此單獨使用加速度計解算出的姿態(tài)角同樣不可靠.由于外界磁場的干擾, 單獨使用磁強計計算出的偏航角同樣存在誤差.為解決上述問題, 很多控制領(lǐng)域的專家做了大量研究, 并提出多種組合測量的方案[5], 結(jié)合本文需求及精確度要求, 本文結(jié)合卡爾曼濾波的遞歸預(yù)估特性, 采用一種基于卡爾曼濾波的數(shù)據(jù)融合算法.

3.3.1 狀態(tài)方程的建立

狀態(tài)方程的建立是將陀螺儀輸出的角速度通過四元數(shù)微分方程計算姿態(tài)角的過程, 作為當(dāng)前狀態(tài)的預(yù)測.通過四元數(shù)微分方程求出的姿態(tài)角, 將作為當(dāng)前預(yù)測值.姿態(tài)角的詳細(xì)計算過程已經(jīng)在3.1 節(jié)四元數(shù)姿態(tài)解算中詳細(xì)敘述, 所以這里只給出相關(guān)公式:

四元數(shù)微分方程:

其中, ω 為沿坐標(biāo)系的角速度.

將式(35)展開:

通過四元數(shù)歸一化可求出當(dāng)前時刻姿態(tài)角(俯仰角、翻滾角、偏航角).

由卡爾曼濾波經(jīng)典狀態(tài)方程可知預(yù)測值包含真實角度和誤差角度, 所以這里我們將所求的姿態(tài)角作為真實角度和誤差角度的合成, 用公式可表示為:

3.3.2 觀測方程的建立

將由加速度計和磁力計測量出的實際姿態(tài)角作為觀測量.

3.3.3 姿態(tài)角噪聲估計計算方法

在實際卡爾曼濾波器應(yīng)用過程中, 過程噪聲協(xié)方差Q, 即作為預(yù)測姿態(tài)角的陀螺儀獲取姿態(tài)角的過程誤差協(xié)方差矩陣, 通常不容易得到, 但可以通過實驗的方法計算得到.測量噪聲協(xié)方差R, 即作為測量姿態(tài)角的加速度計和磁力計的噪聲協(xié)方差矩陣, 通過分析得到包含測量噪聲的數(shù)據(jù), 測量噪聲協(xié)方差R 可以通過數(shù)據(jù)的融合更新得到, 具體計算過程如下:

將上面兩式相減, 即可得到兩時刻的融合誤差:

求融合誤差協(xié)方差:

由于陀螺儀、加速度計和磁力計的誤差為相互獨立, 即自相關(guān)性為0, 滿足:

則由式(41)～式(43)經(jīng)過化簡及近似取代可得誤差協(xié)方差的最終結(jié)果為:

則得加速度計和磁力計的協(xié)方差R:

通過以上計算得出的測量噪聲協(xié)方差R, 用于卡爾曼濾波的自回歸數(shù)據(jù)更新, 經(jīng)過反復(fù)優(yōu)化迭代, 進而得到最優(yōu)姿態(tài)角.

3.3.4 算法流程

(1)初始化參數(shù), 記初始狀態(tài)k=1, 在六軸MPU6050傳感器處于靜止?fàn)顟B(tài)下, 通過陀螺儀、加速度計和磁力計獲取初始角度.

(2)當(dāng)k=2 時, 根據(jù)陀螺儀的角速度數(shù)據(jù), 通過四元數(shù)法進行姿態(tài)解算預(yù)測當(dāng)前角度, 根據(jù)前期陀螺儀數(shù)據(jù)實驗求得先驗誤差協(xié)方差 Pkˉ.

(3)按照式(32)求出卡爾曼增益K.

(4)根據(jù)k=2 時刻的加速度計和磁力計求出的姿態(tài)角作為觀測值, 按式(33), 式(34)實現(xiàn)預(yù)測值更新.

(5)計算k+1 時刻的預(yù)測值.實現(xiàn)方法: 通過采集k+1 時刻的陀螺儀輸出的角速度, 代入四元數(shù)微分方程求出此時刻的姿態(tài)角, 并作為預(yù)測值.然后循環(huán)執(zhí)行步驟(3)和步驟(4).

算法流程如圖5 所示, 先對初始時刻采集的陀螺儀、加速度計和磁力計原始數(shù)據(jù)進行相應(yīng)的濾波處理,若是初始狀態(tài)下, 那么將陀螺儀和加速度計解算出的姿態(tài)角作為初始姿態(tài)角, 并根據(jù)初始姿態(tài)角求出初始四元數(shù), 然后將通過陀螺儀采集到的數(shù)據(jù)帶入四元數(shù)微分方程進而求出姿態(tài)角, 并作為估計角度, 通過采集加速度計和磁力計數(shù)據(jù)并通過計算求出的姿態(tài)角作為測量值, 可計算出過程噪聲協(xié)方差和測量噪聲協(xié)方差.利用卡爾曼濾波算法將預(yù)測角度和測量角度進行融合得到最優(yōu)估計角度, 由卡爾曼濾波公式可知, 兩個符合高斯分布的數(shù)據(jù)融合后仍然滿足高斯分布且最優(yōu)值處在融合后的高斯分布曲線, 因此可將每次計算出的姿態(tài)角作為下次融合的初始角, 反復(fù)的數(shù)據(jù)融合能夠得到一系列穩(wěn)定的最優(yōu)估計角度[7].

3.3.5 卡爾曼濾波數(shù)據(jù)融合算法的適用性

本文通過卡爾曼濾波數(shù)據(jù)融合算法輸出姿態(tài)角,而實際系統(tǒng)中總是存在不同程度的非線性因素, 利用線性化卡爾曼濾波方法, 通過不斷地統(tǒng)計和計算數(shù)據(jù)方差來自動改變卡爾曼增益系數(shù)K, 因此當(dāng)系統(tǒng)存在噪聲或者預(yù)測量的累積誤差較大時, 也能提高系統(tǒng)精度, 誤差能夠通過快速迭代而減少.

智能釣魚竿中采用高性能單片機獲取高頻的姿態(tài)數(shù)據(jù), 自動整定得到的卡爾曼增益系數(shù)具有較強的通用性, 同時計算量相對較大.智能釣魚竿系統(tǒng)中使用的STM32 微處理器能夠保證數(shù)據(jù)融合線程周期不超過100 ms, 較高的姿態(tài)融合頻率可以保證預(yù)測量誤差較小且收斂速度較快, 因而基于線性卡爾曼濾波的姿態(tài)數(shù)據(jù)融合能較好地完成對目標(biāo)的濾波估計處理.

圖5 算法流程圖

3.4 基于PID 控制算法的智能遛魚控制方案

本文中所述魚竿是以電機轉(zhuǎn)動帶動魚竿紡線輪實現(xiàn)魚線自動收放線的智能釣魚竿.所謂遛魚, 指的是一種釣魚方法.釣魚時如果釣到個體較大的魚一般都不能直接將魚提上來, 魚勢必在被勾住后掙扎逃竄, 此時我們需要根據(jù)魚的力氣大小來選擇收線和放線, 在收線和放線的來回拉扯中, 逐漸消耗掉魚的體力, 當(dāng)魚精疲力竭停止反抗時, 將魚拉回岸邊的這個過程, 形象地稱為“遛魚”.

通過上文中的姿態(tài)解算及卡爾曼濾波數(shù)據(jù)融合可得魚竿的實時姿態(tài)角, 結(jié)合圖6 對魚竿模型進行分析:當(dāng)檢測到魚竿的俯仰角的傾角偏差大于或等于 θ1時判斷為有魚咬鉤, 此時系統(tǒng)開啟智能報警功能, 同時檢測遛魚功能是否開啟, 若開啟遛魚功能, 則將當(dāng)前魚竿傾角偏差與偏差角 θ2做 比較, θ2為控制電機正反轉(zhuǎn)的臨界值, 當(dāng)傾角偏差小于 θ2, 則需要控制器控制電機正轉(zhuǎn), 達到收線的目的, 對魚竿和魚組成的力學(xué)模型分析可知,當(dāng)魚竿加速收線時, 由于魚的重力及向反方向的拉力會使得魚竿傾角偏差不斷加大, 當(dāng)傾角大于 θ2時, 則應(yīng)控制電機反轉(zhuǎn), 實現(xiàn)放線的目的, 當(dāng)魚竿快速放線時,魚竿的傾角偏差會逐漸減小, 在魚竿的不斷循環(huán)往復(fù)的收放線的過程中實現(xiàn)消耗魚的體力的目的.θ3為魚線處于極限拉力值時魚竿所對應(yīng)的偏差角, 當(dāng)拉力值大于該魚線的極限拉力值時, 就會導(dǎo)致斷線跑魚, 所以在遛魚過程中對于收放線的時間和魚線拉力值的控制都有較高要求, 基于以上分析, 該控制系統(tǒng)需要通過角度的偏差來實現(xiàn)系統(tǒng)的控制, 因此本文采用串級PID 算法來實現(xiàn)智能遛魚功能.

圖6 魚竿傾角示意圖

3.4.1 串級PID 控制模型的建立

本文采用雙環(huán)PID 控制系統(tǒng), 即外環(huán)采用角度環(huán),內(nèi)環(huán)采用角速度環(huán), 角速度環(huán)的加入將更加準(zhǔn)確的表示魚竿姿態(tài), 使控制系統(tǒng)更加穩(wěn)定.與傳統(tǒng)的單環(huán)PID 控制器相比, 增強了控制系統(tǒng)對外界的抗干擾能力, 提高了控制精度.智能釣魚竿串級PID 控制流程圖如圖7 所示.

3.4.2 串級PID 控制系統(tǒng)Simulink 仿真

本文中采用以角度作為外環(huán)控制, 角速度作為內(nèi)環(huán)控制的串級PID 與以角度作為控制量的單環(huán)PID 控制作比較.從而驗證串級PID 控制的優(yōu)越性.在該系統(tǒng)仿真中輸入量為指定的角度, 輸出為經(jīng)過串級PID 控制輸出的角度, 該系統(tǒng)將以階躍信號為輸入進行系統(tǒng)仿真, 通過參考飛行器的數(shù)學(xué)模型的推導(dǎo), 在這里假設(shè)該俯仰角、翻滾角及偏航角的傳遞函數(shù)分別為:

可建立單級和串級PID 下俯仰角和翻滾角Simulink仿真結(jié)構(gòu)如圖8.

圖7 串級PID 控制流程圖

圖8 單級和串級PID 下俯仰角和翻滾角仿真結(jié)構(gòu)圖

可建立單級和串級PID 下偏航角Simulink 仿真結(jié)構(gòu)如圖9.

俯仰角、翻滾角的單極與串級PID 仿真結(jié)果對比如圖10 所示, 圖中紅線為單級PID 仿真波形, 藍線為串級PID 仿真波形:

偏航角的單極與串級PID 仿真結(jié)果對比如圖11所示.

由圖10 和圖11 仿真結(jié)果圖可見, 在輸入信號為階躍信號的控制系統(tǒng)中, 單極PID 控制下系統(tǒng)的超調(diào)量和振蕩次數(shù)明顯大于串級PID 控制下系統(tǒng)的作用,因此串級PID 控制系統(tǒng)下控制信號更穩(wěn)定且響應(yīng)速度更快.

3.4.3 PID 參數(shù)整定

本節(jié)采用試湊法對串級PID 參數(shù)進行整定.試湊法是根據(jù)以往經(jīng)驗依照先內(nèi)環(huán)、后外環(huán)、先比例、后積分再微分的步驟來實現(xiàn)參數(shù)整定.由于參數(shù)整定過程類似, 本節(jié)以俯仰角為例進行調(diào)試.

首先整定內(nèi)環(huán)PID 參數(shù), 比例參數(shù)Kp=4 時, 此時系統(tǒng)振蕩劇烈, 系統(tǒng)嚴(yán)重超調(diào), 此時應(yīng)減小Kp 的值.Kp=1 時系統(tǒng)需要很長時間才能恢復(fù)到1 值, 說明Kp 值偏小, 因此可初步判斷Kp=3 時較為合適.但是單純的比例調(diào)節(jié)無法使系統(tǒng)快速穩(wěn)定, 還應(yīng)加入積分調(diào)節(jié), 調(diào)節(jié)依據(jù)是若系統(tǒng)誤差消除較慢, 此時應(yīng)當(dāng)適當(dāng)減少積分時間, 增強積分作用.此時系統(tǒng)可以較快的消除誤差, 但系統(tǒng)超調(diào)量較大, 此時應(yīng)加入微分作用減少超調(diào)量, 加入微分控制量能一定程度上抑制超調(diào)量.

圖9 單級和串級PID 下偏航角仿真結(jié)構(gòu)圖

圖10 俯仰角和翻滾角仿真結(jié)果對比圖

圖11 偏航角仿真結(jié)果對比圖

由于PID 參數(shù)的調(diào)試是一個參數(shù)間相互耦合的過程, 所以在調(diào)試中要不斷的嘗試和試湊才能得出最終理想的參數(shù).通過反復(fù)試湊的方法, 最終確定智能魚竿PID 控制參數(shù)如表1 所示, 最終俯仰角的PID 仿真結(jié)果圖如圖12 所示.

由最終的串級PID 仿真圖, 可知系統(tǒng)在階躍信號輸入的控制系統(tǒng)中, 響應(yīng)時間約為0.03 s, 系統(tǒng)穩(wěn)定時間約為0.1 s 且系統(tǒng)超調(diào)量在10%以內(nèi), 因此該串級PID 控制系統(tǒng)能夠快速響應(yīng)并快速趨于穩(wěn)定, 能較好地實現(xiàn)魚竿的控制需求.

表1 智能魚竿PID 控制參數(shù)表

圖12 俯仰角PID 最終仿真結(jié)果圖

4 智能釣魚竿系統(tǒng)測試結(jié)果

智能釣魚竿系統(tǒng)實物圖如圖13 所示, 考慮到魚竿對輕便的需求, 經(jīng)過調(diào)試, 實現(xiàn)了智能釣魚竿的相關(guān)基本功能, 如智能報警功能.該智能釣魚竿設(shè)計成本低廉且功能實用, 具有較好的實用價值和廣闊的市場前景.

4.1 姿態(tài)角測試結(jié)果

由姿態(tài)傳感器數(shù)據(jù)處理得到精確的姿態(tài)角是控制系統(tǒng)的基礎(chǔ), 其測量結(jié)果的準(zhǔn)確性對于實驗結(jié)果準(zhǔn)確性有很大的影響.

由于陀螺儀和加速度計均存在不同頻段的噪聲而導(dǎo)致在檢測過程中產(chǎn)生一定的誤差, 所以本系統(tǒng)采用了卡爾曼數(shù)據(jù)融合方式來降低誤差, 圖14 為傳感器靜止時, 最終解算出的歐拉角波形.

圖13 智能釣魚竿整體實物圖

圖14 靜止時解算的姿態(tài)角波形圖

由圖14 可知, 藍色曲線表示翻滾角, 黃色曲線表示俯仰角, 紅色曲線表示航向角, 傳感器水平靜止放置時, 姿態(tài)角的初始值曲線相對平滑且俯仰角和翻滾角角度均接近零, 說明本文中通過卡爾曼數(shù)據(jù)融合算法最終得出的姿態(tài)角相對精確且噪聲較小.

圖15 為智能釣魚竿實際運行時姿態(tài)角變化波形圖, 航向角的量程范圍為-180°到+180, 則從-180°變化到+180°時, 角度會突變.由圖可知在7-8 s 的時間段內(nèi)Yaw 出現(xiàn)了從-180°到+180°的突變, 因此在智能釣魚竿姿態(tài)控制時會對該處進行平滑處理.

圖15 角度突變時解算的姿態(tài)角波形圖

4.2 智能魚竿報警功能測試

智能魚竿的報警是依據(jù)俯仰角的變化作為咬鉤信號的判斷, 當(dāng)魚竿俯仰角的變化在10 度以上時, 則認(rèn)為此時有魚咬鉤, 且可以通過調(diào)節(jié)報警閾值來改變智能釣魚竿報警的靈敏度.對測試數(shù)據(jù)進行統(tǒng)計得出表2.

分析表2 可知, 當(dāng)翻滾角和偏航角保持不變時, 俯仰角度變化大于10 度時魚竿報警, 俯仰角變化小于10 度時, 魚竿不報警, 當(dāng)俯仰角度不發(fā)生變化時, 無論翻滾角和偏航角如何變化, 魚竿均不報警, 且由表中數(shù)據(jù)分析可知魚竿傾角變化在較接近10 度時依然可以很準(zhǔn)確的發(fā)出報警信號, 這也從側(cè)面證明姿態(tài)解算出的姿態(tài)角精度較高.

表2 魚竿報警數(shù)據(jù)分析表

5 結(jié)論與展望

本文以微處理器STM32 為核心搭建uC/OS-II 操作系統(tǒng), 通過四元數(shù)法對魚竿姿態(tài)解算, 提出基于卡爾曼濾波的數(shù)據(jù)融合算法, 并以串級PID 控制算法實現(xiàn)了魚竿控制系統(tǒng)的智能控制, 并通過Matlab 建立Simulink仿真, 驗證了串級PID 控制器的有效性.通過實驗測試最終達到了本設(shè)計的預(yù)期功能.