一種基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)裝置及方法
【專利摘要】一種基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)裝置及方法，裝置包括用于采集生豬個(gè)體信息的信息采集模塊；用于將信息采集模塊所采集信息進(jìn)行融合濾波并輸出姿態(tài)角的姿態(tài)解算模塊；以及用于根據(jù)姿態(tài)解算模塊的輸出結(jié)果對(duì)生豬姿態(tài)進(jìn)行識(shí)別的PC上位機(jī)，本發(fā)明節(jié)省封裝空間，并采用四階龍格庫(kù)塔優(yōu)化算法對(duì)姿態(tài)信息進(jìn)行更新，通過(guò)動(dòng)態(tài)Kalman濾波模型，利用加速度計(jì)和磁力計(jì)對(duì)陀螺儀漂移進(jìn)行補(bǔ)償，實(shí)現(xiàn)了陀螺儀姿態(tài)和加速度計(jì)、磁力計(jì)測(cè)姿態(tài)的優(yōu)勢(shì)互補(bǔ)，提高了系統(tǒng)動(dòng)態(tài)測(cè)量的精度和可靠性，不僅設(shè)備成本明顯降低，且適用性強(qiáng)，能夠滿足實(shí)時(shí)監(jiān)測(cè)的要求，可以代替養(yǎng)殖人員觀察生豬每個(gè)時(shí)刻的姿態(tài)，及早發(fā)現(xiàn)疑似發(fā)病生豬個(gè)體，降低經(jīng)濟(jì)損失。
【專利說(shuō)明】
一種基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)裝置及方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及生豬姿態(tài)監(jiān)測(cè)技術(shù)領(lǐng)域，特別涉及一種基于微慣性傳感器的生豬姿態(tài) 監(jiān)測(cè)裝置及方法。
【背景技術(shù)】
[0002] 生豬養(yǎng)殖在我國(guó)畜牧業(yè)中占有重要地位，近年來(lái)發(fā)展迅猛，產(chǎn)量、質(zhì)量穩(wěn)步提高， 養(yǎng)殖模式逐漸向集約化、工廠化方向發(fā)展。但整體養(yǎng)殖效益低，其主要原因是養(yǎng)殖場(chǎng)飼養(yǎng)密 度增加，依靠人工監(jiān)測(cè)生豬健康狀況、生豬行為的傳統(tǒng)方法，已經(jīng)不能滿足現(xiàn)代規(guī)?；B(yǎng)殖 的需求，導(dǎo)致飼養(yǎng)動(dòng)物處于亞健康狀態(tài)，未能及早發(fā)現(xiàn)和預(yù)警，進(jìn)而疾病頻繁發(fā)生，生豬死 亡率高、豬肉質(zhì)量不合格。動(dòng)物行為模式是發(fā)現(xiàn)動(dòng)物異常行為的基礎(chǔ)，是分析動(dòng)物生理健康 狀況的外在表現(xiàn)，能一定程度反應(yīng)動(dòng)物機(jī)體對(duì)環(huán)境的適應(yīng)狀況，是評(píng)價(jià)動(dòng)物福利重要指標(biāo) 之一，動(dòng)物行為監(jiān)測(cè)、分析有利于及早發(fā)現(xiàn)疑似發(fā)病牲畜個(gè)體，降低經(jīng)濟(jì)損失。
[0003] 目前動(dòng)物姿態(tài)監(jiān)測(cè)方法主要分為兩種：
[0004] 其一，機(jī)器視覺法，通過(guò)在養(yǎng)殖舍頂部架設(shè)連PC的攝像機(jī)實(shí)現(xiàn)視頻流信息采集，從 原始圖像中分割出監(jiān)測(cè)對(duì)象，進(jìn)而提取動(dòng)物姿態(tài)、運(yùn)動(dòng)速度等信息。
[0005] 其二，無(wú)線傳感技術(shù)法，把姿態(tài)傳感器節(jié)點(diǎn)穿戴在動(dòng)物軀體上，通過(guò)單片機(jī)數(shù)據(jù)融 合、低通濾波、數(shù)模轉(zhuǎn)換后送入姿態(tài)解算模塊，利用無(wú)線通信方式把動(dòng)物姿態(tài)信息發(fā)送到上 位機(jī)，顯示動(dòng)物運(yùn)動(dòng)行為。
[0006] 基于機(jī)器視覺采集視頻信息測(cè)量法是目前應(yīng)用最為廣泛的一種生豬姿態(tài)測(cè)量方 法，雖然它可以捕捉實(shí)時(shí)運(yùn)動(dòng)狀態(tài)，但后期運(yùn)動(dòng)目標(biāo)提取過(guò)程工作量大，其中包括標(biāo)記點(diǎn)的 識(shí)別、跟蹤、空間坐標(biāo)的計(jì)算，單一簡(jiǎn)單特征提取算法難以滿足算法對(duì)普適性的要求，且存 在攝像機(jī)視距、拍攝范圍有限，易受現(xiàn)場(chǎng)光照的影響。

【發(fā)明內(nèi)容】

[0007] 為了克服上述現(xiàn)有技術(shù)的缺點(diǎn)，本發(fā)明的目的在于提供一種基于微慣性傳感器的 生豬姿態(tài)監(jiān)測(cè)裝置及方法，能夠?qū)崿F(xiàn)自動(dòng)監(jiān)測(cè)不同姿態(tài)角下對(duì)應(yīng)的生豬站、坐、躺、行走等 行為，有助于提高養(yǎng)殖場(chǎng)自動(dòng)化管理水平，更好預(yù)防各種疾病，提高豬場(chǎng)經(jīng)濟(jì)效益，裝置具 有佩戴簡(jiǎn)單、精確可靠的特點(diǎn)。
[0008] 為了實(shí)現(xiàn)上述目的，本發(fā)明采用的技術(shù)方案是：
[0009] -種基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)裝置，包括：
[0010] 用于采集生豬個(gè)體信息的信息采集模塊；
[0011] 用于將信息采集模塊所采集信息進(jìn)行融合濾波并輸出姿態(tài)角的姿態(tài)解算模塊；
[0012] 以及，用于根據(jù)姿態(tài)解算模塊的輸出結(jié)果對(duì)生豬姿態(tài)進(jìn)行識(shí)別的PC上位機(jī)。
[0013] 所述信息采集模塊包括三軸磁力計(jì)傳感器HMC5883、整合三軸加速度傳感器和三 軸陀螺儀的6軸微慣性傳感器MPU-6050,采集生豬運(yùn)動(dòng)時(shí)的三軸加速度和三軸角速度信息 以及磁力計(jì)輸出的地磁強(qiáng)度信息。
[0014] 所述基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)裝置還包括：
[0015] 用于實(shí)現(xiàn)PC上位機(jī)與姿態(tài)解算模塊之間通信的藍(lán)牙模塊;藍(lán)牙模塊為HC-06與微 慣性傳感器MPU-6050數(shù)據(jù)傳輸采用I 2C接口；
[0016] 用于實(shí)現(xiàn)供電的電源模塊，電源模塊采用1節(jié)5號(hào)電池供電。
[0017]所述信息采集模塊、姿態(tài)解算模塊、藍(lán)牙傳輸模塊、電池模塊均置入防水密封盒 中，所述防水密封盒外部設(shè)置有用于卡入松緊帶的溝槽。
[0018] 所述上位機(jī)PC配置有藍(lán)牙模塊和對(duì)生豬姿態(tài)進(jìn)行識(shí)別的Matlab軟件，以及顯示生 豬姿態(tài)的顯示模塊。
[0019] 本發(fā)明還提供了相應(yīng)的基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)方法，包括如下步驟：
[0020] 采集生豬個(gè)體信息；
[0021 ]將信息采集模塊所采集信息進(jìn)行融合濾波并輸出姿態(tài)角信息；
[0022]根據(jù)姿態(tài)解算模塊的輸出結(jié)果對(duì)生豬姿態(tài)進(jìn)行識(shí)別。
[0023]所述生豬個(gè)體信息包括其運(yùn)動(dòng)時(shí)的三軸加速度和三軸角速度信息以及對(duì)姿態(tài)角 進(jìn)行校準(zhǔn)的三軸磁力計(jì)信息，利用集成了陀螺儀、加速度計(jì)的6軸微慣性傳感器MPU-6050， 以及通過(guò)I2C接口與6軸微慣性傳感器MPU-6050相連的HMC5833磁力傳感器采集獲取。
[0024]所述將信息采集模塊所采集信息進(jìn)行融合濾波并輸出精確姿態(tài)角包括如下步驟： [0025]步驟1:系統(tǒng)初始校準(zhǔn);在初始校準(zhǔn)時(shí)刻to，系統(tǒng)利用三軸加速度測(cè)量分量測(cè)量俯 仰角9、滾轉(zhuǎn)角y，三軸磁力計(jì)測(cè)量航偏角，求得初始時(shí)刻的旋轉(zhuǎn)矩陣qW)和姿態(tài)四元數(shù) Q(t〇);具體求解公式如下：
[0027]設(shè)初始校準(zhǔn)，姿態(tài)陣初值為q [% 13，則姿態(tài)四元數(shù)的初始值Q(t0)由以下公式確 定：
[0029] b表示載體坐標(biāo)系，n表示地理坐標(biāo)系，q表示從載體坐標(biāo)系到地理坐標(biāo)系之間的 旋轉(zhuǎn)矩陣，Q( to)表示初始時(shí)刻to下對(duì)應(yīng)的姿態(tài)四元數(shù)；
[0030] 步驟2:按照設(shè)定周期T，采集三軸磁場(chǎng)數(shù)據(jù)、三軸加速度和三軸角速度數(shù)據(jù)；
[0031]步驟3:利用四元數(shù)的初始值Q(to)和三軸角速度信號(hào)co(t)，采用四階龍格庫(kù)塔優(yōu) 化算法，迭代解算姿態(tài)四元數(shù)Q(tk)和三個(gè)姿態(tài)角即俯仰角0、滾轉(zhuǎn)角y、航偏角的實(shí)時(shí)值； [0032]設(shè)時(shí)間間隔為t，四階龍格庫(kù)塔算法公式如下：
[0034]式中，T是四元數(shù)的更新周期，1^、1(2、1(3、1(4分別為 ：
[0036]設(shè)Q(tk) = [q0 qi q2 q3]T，由Q(tk)依次求出（？(匕）和姿態(tài)角9、f、y ;
[0039]采用動(dòng)態(tài)kalman濾波對(duì)三軸角速度長(zhǎng)時(shí)間的累積誤差進(jìn)行補(bǔ)償，提高檢測(cè)精度的 要求，構(gòu)造隨機(jī)線性離散系統(tǒng)方程如下：
[0040] Xk= 4)k/k-iXk-i+Tk-iffk-i [0041 ] Zk = HkXk+Vk
[0042] 其中，Xk為系統(tǒng)的n維狀態(tài)向量;Zk為系統(tǒng)的m維觀測(cè)序列；<i>k/k-i為系統(tǒng)的nXn維 狀態(tài)轉(zhuǎn)移矩陣;Hk為mX n維觀測(cè)矩陣；X p維系統(tǒng)噪聲輸入矩陣;Wk-i為p維系統(tǒng)過(guò)程 噪聲;Vk為m維系統(tǒng)觀測(cè)噪聲；
[0043] Kalman濾波增益矩陣：|丨1
[0044] 加速度計(jì)和磁力計(jì)的輸出作為觀測(cè)向量Zb，則離散的狀態(tài)方程和觀測(cè)方程分別 為： r ^
[0045] { + Rk
[0046] 式中，Rk為測(cè)量噪聲方差矩陣；利用離散型Kalman濾波基本方程進(jìn)行遞推計(jì)算得 到各時(shí)刻狀態(tài)的最優(yōu)估計(jì)
[0047] 利用全A對(duì)陀螺儀姿態(tài)更新的姿態(tài)角進(jìn)行前饋校正，如下式所示： rp _ -yj-
[0048] e <p Y n ￡x_ k ￡y±： ￡Lk
[0049] 所述PC上位機(jī)對(duì)生豬姿態(tài)進(jìn)行識(shí)別的方法是：
[0050] 顯示生豬的加速度和姿態(tài)波形曲線，根據(jù)設(shè)定的分類庫(kù)，對(duì)采集的生豬姿態(tài)進(jìn)行 分類，其具體方法如下：
[0051] 當(dāng)生豬處于站立姿態(tài)時(shí)，由于裝置傳感器近似水平綁在生豬背部，載體坐標(biāo)系Zb 與地理坐標(biāo)系Zn兩軸接近重合，夾角0接近0°C ;當(dāng)生豬處于坐姿時(shí)，載體坐標(biāo)系Zb與地理坐 標(biāo)系ZJi成一定的夾角0;當(dāng)生豬處于躺姿時(shí)，載體坐標(biāo)系Z b與地理坐標(biāo)系ZJi成夾角0接近 90 °C，因此根據(jù)不同姿態(tài)下載體坐標(biāo)系b系與地理坐標(biāo)系n系形成的不同夾角初步對(duì)姿態(tài)進(jìn) 行分類;生豬行走時(shí)其加速度波形具有明顯變化，當(dāng)加速度值大于設(shè)定閾值時(shí)此時(shí)可以判 斷生豬在行走，通過(guò)對(duì)采集的生豬姿態(tài)信息進(jìn)行匹配，得到不同姿態(tài)下生豬的行為信息。 [0052]與現(xiàn)有技術(shù)相比，本發(fā)明根據(jù)MEMS傳感器具有體積小、精度高、功耗低的特點(diǎn)，選 擇整合三軸MEMS加速度計(jì)、三軸陀螺儀、三軸磁力計(jì)的MPU6050傳感器，免除了組合陀螺儀 與加速度、磁力計(jì)之間軸間差的問(wèn)題，節(jié)省了大量封裝空間，同時(shí)采用四階龍格庫(kù)塔優(yōu)化算 法對(duì)姿態(tài)信息進(jìn)行更新。通過(guò)動(dòng)態(tài)Kalman濾波模型，利用加速度計(jì)和磁力計(jì)對(duì)陀螺儀漂移 進(jìn)行補(bǔ)償，實(shí)現(xiàn)了陀螺儀姿態(tài)和加速度計(jì)、磁力計(jì)測(cè)姿態(tài)的優(yōu)勢(shì)互補(bǔ)，提高了系統(tǒng)動(dòng)態(tài)測(cè)量 的精度和可靠性。本發(fā)明還完成了通過(guò)PC上位機(jī)對(duì)生豬姿態(tài)進(jìn)行實(shí)時(shí)判別，相比于采用機(jī) 器視覺的方法，不僅設(shè)備成本明顯降低，且適用性強(qiáng)，能夠滿足實(shí)時(shí)監(jiān)測(cè)的要求。
[0053]同時(shí)，本發(fā)明裝置體積小、質(zhì)量輕、功耗小、成本低、便于穿戴在生豬軀干上，生豬 無(wú)明顯應(yīng)激反應(yīng)，可以代替養(yǎng)殖人員觀察生豬每個(gè)時(shí)刻的姿態(tài)，及早發(fā)現(xiàn)疑似發(fā)病生豬個(gè) 體，降低經(jīng)濟(jì)損失。
【附圖說(shuō)明】
[0054]圖1是本發(fā)明原理框圖。
[0055]圖2是本發(fā)明結(jié)構(gòu)示意圖。
[0056] 圖3是本發(fā)明應(yīng)用在生豬軀體上的結(jié)構(gòu)示意圖。
[0057] 圖4是姿態(tài)解算對(duì)應(yīng)的算法流程圖。
【具體實(shí)施方式】
[0058] 下面結(jié)合附圖和實(shí)施例詳細(xì)說(shuō)明本發(fā)明的實(shí)施方式。
[0059] 微慣性傳感器是一種測(cè)量物體運(yùn)動(dòng)時(shí)產(chǎn)生加速度、角速度數(shù)據(jù)的電子元件，將測(cè) 量到的數(shù)據(jù)通過(guò)電流和電壓變化進(jìn)行數(shù)據(jù)輸出，因此能夠準(zhǔn)確檢測(cè)到物體的運(yùn)動(dòng)情況。基 于微慣性傳感器網(wǎng)絡(luò)技術(shù)是一種接觸式測(cè)量方法，但是由于傳感器本身的對(duì)動(dòng)物活動(dòng)產(chǎn)生 的影響較小，因此可以將體積較小、功耗低、穩(wěn)定性高的微慣性傳感器佩戴在生豬軀體上， 實(shí)時(shí)獲取其運(yùn)動(dòng)信息，并且通過(guò)利用無(wú)線通信技術(shù)進(jìn)行信息傳輸，解決了養(yǎng)殖場(chǎng)布線困難 的問(wèn)題。生豬的行為體現(xiàn)在姿態(tài)變化上，生豬在站、坐、躺不同行為方式下，傳感器各方向軸 輸出幅值各有不同，因此利用傳感器方向軸輸出的響應(yīng)差異，可以實(shí)現(xiàn)對(duì)生豬行為的有效 監(jiān)測(cè)。
[0060] 如圖1所示，一種基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)裝置，包括：
[0061] 用于采集生豬個(gè)體信息的信息采集模塊；
[0062] 用于將信息采集模塊所采集信息進(jìn)行融合濾波并輸出姿態(tài)角的姿態(tài)解算模塊；
[0063] 用于實(shí)現(xiàn)PC上位機(jī)與姿態(tài)解算模塊之間通信的藍(lán)牙模塊一；
[0064] 用于實(shí)現(xiàn)供電的電源模塊；
[0065] 以及，用于根據(jù)姿態(tài)解算模塊的輸出結(jié)果對(duì)生豬姿態(tài)進(jìn)行識(shí)別的PC上位機(jī)。上位 機(jī)PC配置有藍(lán)牙模塊二和對(duì)生豬姿態(tài)進(jìn)行識(shí)別的Matlab軟件，以及顯示生豬姿態(tài)的顯示模 塊。其中，信息采集模塊包括三軸磁力計(jì)傳感器HMC5883和整合三軸加速度傳感器和三軸陀 螺儀的6軸微慣性傳感器MPU-6050,采集生豬運(yùn)動(dòng)時(shí)的三軸磁場(chǎng)變化信息以及三軸加速度 和三軸角速度信息。藍(lán)牙模塊為HC-06與微慣性傳感器MPU-6050數(shù)據(jù)傳輸采用I 2C接口，電 源模塊采用1節(jié)5號(hào)電池供電。
[0066] 裝置的安裝如圖2和圖3所示，電源模塊2、信息采集模塊3和藍(lán)牙模塊一 4均置入防 水密封盒1中，防水密封盒1外部設(shè)置有用于卡入松緊帶的溝槽5;
[0067] 使用時(shí)，防水密封盒1通過(guò)松緊帶穿戴在生豬背部，溝槽5可使得松緊帶綁縛牢靠。 防水密封盒1體積僅為75mm X 25mm X 30mm。
[0068]信息采集模塊3中，三軸加速度的指向分別為x軸指向生豬右方，y軸指向生豬頭部 方向，z軸指向地面。賽爾通MPU-60 50可準(zhǔn)確追蹤快速與慢速動(dòng)作，用戶可程式控制的加速 度感測(cè)范圍為± 2g、± 4g、± 8g、± 16g，產(chǎn)品最高傳輸頻率為400KHz。
[0069] 本發(fā)明還提供了相應(yīng)的基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)方法，包括如下步驟：
[0070] 采集生豬個(gè)體信息；
[0071] 將信息采集模塊所采集信息進(jìn)行融合濾波并輸出姿態(tài)角信息；
[0072] 根據(jù)姿態(tài)解算模塊的輸出結(jié)果對(duì)生豬姿態(tài)進(jìn)行識(shí)別。
[0073] 圖4是姿態(tài)測(cè)量系統(tǒng)對(duì)應(yīng)的算法流程圖，介紹了該姿態(tài)測(cè)量系統(tǒng)。
[0074] 其中，圖4給出了從初始校準(zhǔn)到解算出精確姿態(tài)的整個(gè)算法流程，將信息采集模塊 所采集信息進(jìn)行融合濾波并輸出精確姿態(tài)角包括如下步驟：
[0075]步驟1:系統(tǒng)初始校準(zhǔn)。系統(tǒng)的初始姿態(tài)信息可以根據(jù)三軸加速度計(jì)、三軸磁力計(jì) 的測(cè)量值計(jì)算得到，初始校準(zhǔn)算法如下：
[0076]系統(tǒng)上電后，傳感器處于靜止?fàn)顟B(tài)，加速度計(jì)測(cè)量數(shù)據(jù)為重力在三軸坐標(biāo)上的分 量，地理坐標(biāo)系下重力分量為gn=[00g]T，載體坐標(biāo)系下重力分量為g gf， 有載體坐標(biāo)系轉(zhuǎn)換為地理坐標(biāo)系公式，可得： -gsin0
[0077] gfc = C^gn = gsinycos沒進(jìn)而可以得到俯仰角0、滾轉(zhuǎn)角 y，@ = arcsin(-S/g) g COS/COS 0 y = arctan(gf:/gf)，由于僅使用加速度計(jì)無(wú)法計(jì)算靜止?fàn)顟B(tài)下的航偏角P，因此利用磁力計(jì) 計(jì)算航偏角<r，磁力計(jì)傳感器輸出分量『=[1^ 0 Md]t，載體坐標(biāo)系下地磁傳感器分量為 M；： 載體坐標(biāo)系轉(zhuǎn)換為地理坐標(biāo)系公式，可得： Mfl Imm~
[0078] Mhr =Cl 0 M\ IMd
[0079] 由地磁量與姿態(tài)角的關(guān)系解得航偏角
[0081]由初始姿態(tài)角9、y、免，求得初始時(shí)刻的旋轉(zhuǎn)矩陣G仏)和姿態(tài)四元數(shù)Q(t〇);具體 求解公式如下：
[0083]設(shè)初始校準(zhǔn)，姿態(tài)陣初值為qfZ^3x3，則姿態(tài)四元數(shù)的初始值Q(t〇)由以下公式確 定：
[0085] b表示載體坐標(biāo)系，n表示地理坐標(biāo)系，q1表示從載體坐標(biāo)系到地理坐標(biāo)系之間的 旋轉(zhuǎn)矩陣，Q (to)表示初始時(shí)刻to對(duì)應(yīng)的姿態(tài)四元數(shù)；
[0086] 步驟2:信號(hào)采集:DMP數(shù)字運(yùn)算單元按照設(shè)定的采樣周期T，采集MEMS三軸加速度 計(jì)信號(hào)、三軸MEMS陀螺儀輸出的角速度信號(hào)和HMC5833磁力計(jì)輸出的地磁強(qiáng)度信號(hào)；
[0087]步驟3:陀螺儀姿態(tài)更新:利用四元數(shù)的初始值Q(to)和陀螺儀輸出的三軸角速度 信號(hào)《 (t)，采用四階龍格庫(kù)塔優(yōu)化算法，迭代解算姿態(tài)四元數(shù)Q(tk)和俯仰角0、滾轉(zhuǎn)角Y、 航偏角P的實(shí)時(shí)值；
[0088]設(shè)時(shí)間間隔為t，四階龍格庫(kù)塔算法公式如下：
[0090]式中，T是四元數(shù)的更新周期，1^、1(2、1(3、1(4分別為 ：
[0092]設(shè)Q(tk) = [qo qi q2 q3]T，由Q(tk)依次求出C,丨(k)和姿態(tài)角0、f、_y ;
[0095]步驟 4:Kalman 濾波
[0096]由于陀螺儀傳感器存在隨機(jī)漂移誤差，因此采用動(dòng)態(tài)kalman濾波對(duì)陀螺儀三軸角 速度長(zhǎng)時(shí)間的累積誤差進(jìn)行補(bǔ)償，以提高檢測(cè)精度的要求?？柭鼮V波算法流程，分為預(yù)估 與矯正兩部分，預(yù)估部分使用前一時(shí)刻數(shù)據(jù)對(duì)當(dāng)前時(shí)刻數(shù)據(jù)進(jìn)行估計(jì)，矯正部分是利用當(dāng) 前時(shí)刻的測(cè)量值對(duì)預(yù)估值進(jìn)行矯正，進(jìn)而得到較為準(zhǔn)確的數(shù)值。
[0097] Kalman濾波方程為：
[0098]狀態(tài)預(yù)測(cè)方程:Xk|k-i=AXk-i|k-i+BUk (7)
[0099]狀態(tài)估計(jì)方程:Xk|k=Xk|k-i+Kgk(Zk_HXk|k-i) (8)
[0100] 預(yù)測(cè)誤差方差矩陣:pk|k-FAPk-ilk-iA'+Q (9)
[0101] 濾波增益矩陣:Kk|k = Pk|k-iH/(HPk/k-iHt+R) (10)
[0102] 估計(jì)誤差方差舉證:Pk|k=(I-KgkH)Pk|k-i (11)
[0103] 變量說(shuō)明：
[0104] X(k)表示k時(shí)刻的系統(tǒng)狀態(tài)，U(k)表示k時(shí)刻對(duì)系統(tǒng)的控制量，A、B表示系統(tǒng)參數(shù)，Z (k)表示k時(shí)刻的測(cè)量值，H表示測(cè)量系統(tǒng)的參數(shù)，W(k)表示K時(shí)刻過(guò)程噪聲，Q表示過(guò)程噪聲 協(xié)方差，R表不測(cè)量噪聲協(xié)方差。
[0105] 設(shè)計(jì)Kalman濾波器時(shí)利用陀螺儀輸出數(shù)據(jù)作為預(yù)估數(shù)據(jù)Xk，陀螺儀更新的姿態(tài)誤 差角為e = (ex ey ez)T，加速度計(jì)和磁力計(jì)輸出作為矯正測(cè)量值Zk，構(gòu)造隨機(jī)線性離散系統(tǒng) 方程和矯正方程分別如下：
[0106] Xk= 4)k/k-iXk-i+Tk-iffk-i (12)
[0107] Zk = HkXk+Vk (13)
[0108] 其中，Xk為系統(tǒng)的n維狀態(tài)向量;Zk為系統(tǒng)的m維觀測(cè)序列；<i>k/k-i為系統(tǒng)的nXn維 狀態(tài)轉(zhuǎn)移矩陣;Hk為mX n維觀測(cè)矩陣；X p維系統(tǒng)噪聲輸入矩陣;Wk-i為p維系統(tǒng)過(guò)程 噪聲;Vk為m維系統(tǒng)觀測(cè)噪聲；
[0109] 公式（12)和公式（13)聯(lián)立構(gòu)成離散Kalman濾波的狀態(tài)空間模塊，利用離散型 Kalman濾波基本方程進(jìn)行遞推計(jì)算得到各時(shí)刻狀態(tài)的最優(yōu)估計(jì)t
[0110] 步驟5:輸出校準(zhǔn)：圖4給出了輸出校正原理流程圖。利用對(duì)陀螺儀姿態(tài)更新的 姿態(tài)角進(jìn)行前饋校正，如下式所示： -. .1 - T - -
[0111] ^ 9 7 =[^ <Pk nT + By,k e：Zik
[0112] 步驟6:返回步驟2;返回步驟2循環(huán)執(zhí)行步驟2~步驟6。
[0113] PC上位機(jī)利用藍(lán)牙模塊接收到生豬的姿態(tài)信息，并輸入Matlab軟件中，通過(guò)編寫 Matlab程序，顯示生豬的加速度和姿態(tài)波形曲線，根據(jù)設(shè)定的分類庫(kù)，對(duì)采集的生豬姿態(tài)進(jìn) 行分類。其具體方法如下：
[0114] 當(dāng)生豬處于站立姿態(tài)時(shí)，由于裝置傳感器近似水平綁在生豬背部，載體坐標(biāo)系Zb 與地理坐標(biāo)系Zn兩軸接近重合，夾角0接近0°C ;當(dāng)生豬處于坐姿時(shí)，載體坐標(biāo)系Zb與地理坐 標(biāo)系ZJi成一定的夾角0;當(dāng)生豬處于躺姿時(shí)，載體坐標(biāo)系Z b與地理坐標(biāo)系ZJi成夾角0接近 90 °C，因此根據(jù)不同姿態(tài)下載體坐標(biāo)系b系與地理坐標(biāo)系n系形成的不同夾角初步對(duì)姿態(tài)進(jìn) 行分類;生豬行走時(shí)其加速度波形具有明顯變化，當(dāng)加速度值大于設(shè)定閾值時(shí)此時(shí)可以判 斷生豬在行走，通過(guò)對(duì)采集的生豬姿態(tài)信息進(jìn)行匹配，得到不同姿態(tài)下生豬的行為信息。
[0115] 綜上，本裝置將微慣性傳感器技術(shù)，無(wú)線藍(lán)牙技術(shù)，姿態(tài)識(shí)別技術(shù)等多種信息融合 在一起，實(shí)現(xiàn)了對(duì)生豬運(yùn)動(dòng)狀態(tài)的監(jiān)測(cè)和識(shí)別，為生豬行為識(shí)別系統(tǒng)的設(shè)計(jì)提供了一種新 思路和方法。
【主權(quán)項(xiàng)】
1. 一種基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)裝置，其特征在于，包括： 用于采集生豬個(gè)體信息的信息采集模塊； 用于將信息采集模塊所采集信息進(jìn)行融合濾波并輸出姿態(tài)角的姿態(tài)解算模塊； 以及，用于根據(jù)姿態(tài)解算模塊的輸出結(jié)果對(duì)生豬姿態(tài)進(jìn)行識(shí)別的PC上位機(jī)。2. 根據(jù)權(quán)利要求1所述基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)裝置，其特征在于，還包括： 用于實(shí)現(xiàn)PC上位機(jī)與姿態(tài)解算模塊之間通信的藍(lán)牙模塊； 用于實(shí)現(xiàn)供電的電源模塊。3. 根據(jù)權(quán)利要求1所述基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)裝置，其特征在于，所述信息 采集模塊包括三軸磁力計(jì)傳感器HMC5883、整合三軸加速度傳感器和三軸陀螺儀的6軸微慣 性傳感器MPU-6050,采集生豬運(yùn)動(dòng)時(shí)的三軸加速度計(jì)信號(hào)、三軸陀螺儀輸出的角速度信號(hào) 和HMC5833磁力計(jì)輸出的地磁強(qiáng)度信號(hào)。4. 一種基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)方法，其特征在于，包括如下步驟： 采集生豬個(gè)體信息； 將信息采集模塊所采集信息進(jìn)行融合濾波并輸出姿態(tài)角信息； 根據(jù)姿態(tài)解算模塊的輸出結(jié)果對(duì)生豬姿態(tài)進(jìn)行識(shí)別。5. 根據(jù)權(quán)利要求4所述基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)方法，其特征在于，所述生豬 個(gè)體信息包括其運(yùn)動(dòng)時(shí)的三軸加速度和三軸角速度信息以及對(duì)姿態(tài)角進(jìn)行校準(zhǔn)的三軸磁 力計(jì)信息，利用集成了陀螺儀、加速度計(jì)的6軸微慣性傳感器MPU-6050,以及通過(guò)I 2C接口與 6軸微慣性傳感器MPU-6050相連的HMC5833磁力傳感器采集獲取。6. 根據(jù)權(quán)利要求5所述基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)方法，其特征在于，所述將信 息采集模塊所采集信息進(jìn)行融合濾波并輸出精確姿態(tài)角包括如下步驟： 步驟1:系統(tǒng)初始校準(zhǔn);系統(tǒng)利用三軸加速度分量計(jì)算得到初始狀態(tài)的俯仰角Θ、滾轉(zhuǎn)角 γ，三軸磁力計(jì)測(cè)量航偏角?5，求得初始時(shí)刻的旋轉(zhuǎn)矩陣和姿態(tài)四元數(shù)q(t〇);具體求 解公式如下：設(shè)初始校準(zhǔn)后，姿態(tài)陣初值為，則姿態(tài)四元數(shù)的初始值Q(t〇)由以下公式確 定：b表示載體坐標(biāo)系，η表示地理坐標(biāo)系，€Γ表示從載體坐標(biāo)系到地理坐標(biāo)系之間的旋轉(zhuǎn) 矩陣，Q(to)表示初始時(shí)刻to下對(duì)應(yīng)的姿態(tài)四元數(shù)； 步驟2:按照設(shè)定周期T，采集三軸磁場(chǎng)數(shù)據(jù)、三軸加速度和三軸角速度數(shù)據(jù)； 步驟3:陀螺儀姿態(tài)更新;利用四元數(shù)的初始值Q(to)和三軸角速度信號(hào)co(t)，采用四階 龍格庫(kù)塔優(yōu)化算法，迭代解算姿態(tài)四元數(shù)Q(tk)求出俯仰角Θ、滾轉(zhuǎn)角γ、航偏角0的實(shí)時(shí)值； 設(shè)時(shí)間間隔為t，四階龍格庫(kù)塔算法公式如下：式中，T是四元數(shù)的更新周期，M〇'）為載體坐標(biāo)系轉(zhuǎn)動(dòng)時(shí)角速度矢量矩陣， ⑴、三個(gè)量為不同時(shí)刻下載體坐標(biāo)(b)系三個(gè)軸轉(zhuǎn)動(dòng)的角速度，是L、 K2H4分別為：設(shè)Q(tk) = [qo qi q2 q3]T，式中qo qi q2 q3為四元數(shù)中的四個(gè)參數(shù)，由Q(tk)依次求出 CfK)和姿態(tài)角θ、9>、γ ;7.根據(jù)權(quán)利要求6所述基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)方法，其特征在于，采用動(dòng)態(tài) kalman濾波對(duì)三軸角速度長(zhǎng)時(shí)間的累積誤差進(jìn)行補(bǔ)償，提高檢測(cè)精度的要求，設(shè)計(jì)Kalman 濾波器時(shí)利用陀螺儀輸出數(shù)據(jù)作為預(yù)估數(shù)據(jù)Xk，陀螺儀更新的姿態(tài)誤差角為ε = (εχ ey εζ )Τ，加速度計(jì)和磁力計(jì)輸出作為矯正測(cè)量值Zk，構(gòu)造隨機(jī)線性離散系統(tǒng)方程和矯正方程分別 如下： Xk- Φ k/k-lXk-1+Tk-lWk-1 Zk = HkXk+Vk 其中，Xk為系統(tǒng)的n維狀態(tài)向量;Zk為系統(tǒng)的m維觀測(cè)序列；<i>k/k-i為系統(tǒng)的nXn維狀態(tài)轉(zhuǎn) 移矩陣;Hk為m X η維觀測(cè)矩陣；τΗ為η X p維系統(tǒng)噪聲輸入矩陣;Wk-i為p維系統(tǒng)過(guò)程噪聲;Vk 為m維系統(tǒng)觀測(cè)噪聲； 由系統(tǒng)離散方程和矯正方程、聯(lián)立構(gòu)成離散Ka lman濾波的狀態(tài)空間模塊，利用離散型 Ka lman濾波基本方程進(jìn)行遞推計(jì)算得到各時(shí)刻狀態(tài)的最優(yōu)估計(jì) 利用it對(duì)陀螺儀姿態(tài)更新的姿態(tài)角進(jìn)行前饋校正，如下式所示：8.根據(jù)權(quán)利要求4所述基于微慣性傳感器的生豬姿態(tài)監(jiān)測(cè)方法，其特征在于，所述PC上 位機(jī)對(duì)生豬姿態(tài)進(jìn)行識(shí)別的方法是： 顯示生豬的加速度和姿態(tài)波形曲線，根據(jù)設(shè)定的分類庫(kù)，對(duì)采集的生豬姿態(tài)進(jìn)行分類， 其具體方法如下： 通過(guò)松緊布帶把監(jiān)測(cè)裝置縛在生豬背部，其內(nèi)置監(jiān)測(cè)模塊近似與平面平起，當(dāng)生豬處 于站立姿態(tài)時(shí)，載體坐標(biāo)系Zb與地理坐標(biāo)系z(mì)n兩軸接近重合，夾角Θ接近〇°c;當(dāng)生豬處于坐 姿時(shí)，載體坐標(biāo)系Zb與地理坐標(biāo)系ZJi成一定的夾角Θ;當(dāng)生豬處于躺姿時(shí)，載體坐標(biāo)系Zb與 地理坐標(biāo)系4形成夾角Θ接近9(T C，因此根據(jù)不同姿態(tài)下載體坐標(biāo)系b系與地理坐標(biāo)系η系 形成的不同夾角大小可以實(shí)現(xiàn)對(duì)生豬站、坐、躺行為的初步分類;生豬行走時(shí)其加速度波形 具有明顯變化，當(dāng)加速度值大于設(shè)定閾值時(shí)此時(shí)可以判斷生豬在行走，通過(guò)對(duì)采集的生豬 姿態(tài)信息進(jìn)行匹配，得到不同姿態(tài)下生豬的行為信息。
【文檔編號(hào)】A01K29/00GK105850773SQ201610187813
【公開日】2016年8月17日
【申請(qǐng)日】2016年3月29日
【發(fā)明人】張海輝, 王傳哲, 王東, 張陽(yáng), 邵志成, 范奧華
【申請(qǐng)人】西北農(nóng)林科技大學(xué)