一种基于微惯性传感器的生猪姿态监测装置及方法
【专利摘要】一种基于微惯性传感器的生猪姿态监测装置及方法，装置包括用于采集生猪个体信息的信息采集模块；用于将信息采集模块所采集信息进行融合滤波并输出姿态角的姿态解算模块；以及用于根据姿态解算模块的输出结果对生猪姿态进行识别的PC上位机，本发明节省封装空间，并采用四阶龙格库塔优化算法对姿态信息进行更新，通过动态Kalman滤波模型，利用加速度计和磁力计对陀螺仪漂移进行补偿，实现了陀螺仪姿态和加速度计、磁力计测姿态的优势互补，提高了系统动态测量的精度和可靠性，不仅设备成本明显降低，且适用性强，能够满足实时监测的要求，可以代替养殖人员观察生猪每个时刻的姿态，及早发现疑似发病生猪个体，降低经济损失。
【专利说明】
一种基于微惯性传感器的生猪姿态监测装置及方法
技术领域
[0001] 本发明涉及生猪姿态监测技术领域，特别涉及一种基于微惯性传感器的生猪姿态 监测装置及方法。
【背景技术】
[0002] 生猪养殖在我国畜牧业中占有重要地位，近年来发展迅猛，产量、质量稳步提高， 养殖模式逐渐向集约化、工厂化方向发展。但整体养殖效益低，其主要原因是养殖场饲养密 度增加，依靠人工监测生猪健康状况、生猪行为的传统方法，已经不能满足现代规模化养殖 的需求，导致饲养动物处于亚健康状态，未能及早发现和预警，进而疾病频繁发生，生猪死 亡率高、猪肉质量不合格。动物行为模式是发现动物异常行为的基础，是分析动物生理健康 状况的外在表现，能一定程度反应动物机体对环境的适应状况，是评价动物福利重要指标 之一，动物行为监测、分析有利于及早发现疑似发病牲畜个体，降低经济损失。
[0003] 目前动物姿态监测方法主要分为两种：
[0004] 其一，机器视觉法，通过在养殖舍顶部架设连PC的摄像机实现视频流信息采集，从 原始图像中分割出监测对象，进而提取动物姿态、运动速度等信息。
[0005] 其二，无线传感技术法，把姿态传感器节点穿戴在动物躯体上，通过单片机数据融 合、低通滤波、数模转换后送入姿态解算模块，利用无线通信方式把动物姿态信息发送到上 位机，显示动物运动行为。
[0006] 基于机器视觉采集视频信息测量法是目前应用最为广泛的一种生猪姿态测量方 法，虽然它可以捕捉实时运动状态，但后期运动目标提取过程工作量大，其中包括标记点的 识别、跟踪、空间坐标的计算，单一简单特征提取算法难以满足算法对普适性的要求，且存 在摄像机视距、拍摄范围有限，易受现场光照的影响。
【发明内容】
[0007] 为了克服上述现有技术的缺点，本发明的目的在于提供一种基于微惯性传感器的 生猪姿态监测装置及方法，能够实现自动监测不同姿态角下对应的生猪站、坐、躺、行走等 行为，有助于提高养殖场自动化管理水平，更好预防各种疾病，提高猪场经济效益，装置具 有佩戴简单、精确可靠的特点。
[0008] 为了实现上述目的，本发明采用的技术方案是：
[0009] -种基于微惯性传感器的生猪姿态监测装置，包括：
[0010] 用于采集生猪个体信息的信息采集模块；
[0011] 用于将信息采集模块所采集信息进行融合滤波并输出姿态角的姿态解算模块；
[0012] 以及，用于根据姿态解算模块的输出结果对生猪姿态进行识别的PC上位机。
[0013] 所述信息采集模块包括三轴磁力计传感器HMC5883、整合三轴加速度传感器和三 轴陀螺仪的6轴微惯性传感器MPU-6050,采集生猪运动时的三轴加速度和三轴角速度信息 以及磁力计输出的地磁强度信息。
[0014] 所述基于微惯性传感器的生猪姿态监测装置还包括：
[0015] 用于实现PC上位机与姿态解算模块之间通信的蓝牙模块;蓝牙模块为HC-06与微 惯性传感器MPU-6050数据传输采用I 2C接口；
[0016] 用于实现供电的电源模块，电源模块采用1节5号电池供电。
[0017]所述信息采集模块、姿态解算模块、蓝牙传输模块、电池模块均置入防水密封盒 中，所述防水密封盒外部设置有用于卡入松紧带的沟槽。
[0018] 所述上位机PC配置有蓝牙模块和对生猪姿态进行识别的Matlab软件，以及显示生 猪姿态的显示模块。
[0019] 本发明还提供了相应的基于微惯性传感器的生猪姿态监测方法，包括如下步骤：
[0020] 采集生猪个体信息；
[0021 ]将信息采集模块所采集信息进行融合滤波并输出姿态角信息；
[0022]根据姿态解算模块的输出结果对生猪姿态进行识别。
[0023]所述生猪个体信息包括其运动时的三轴加速度和三轴角速度信息以及对姿态角 进行校准的三轴磁力计信息，利用集成了陀螺仪、加速度计的6轴微惯性传感器MPU-6050， 以及通过I2C接口与6轴微惯性传感器MPU-6050相连的HMC5833磁力传感器采集获取。
[0024]所述将信息采集模块所采集信息进行融合滤波并输出精确姿态角包括如下步骤： [0025]步骤1:系统初始校准;在初始校准时刻to，系统利用三轴加速度测量分量测量俯 仰角9、滚转角y，三轴磁力计测量航偏角，求得初始时刻的旋转矩阵qW)和姿态四元数 Q(t〇);具体求解公式如下：
[0027]设初始校准，姿态阵初值为q [% 13，则姿态四元数的初始值Q(t0)由以下公式确 定：
[0029] b表示载体坐标系，n表示地理坐标系，q表示从载体坐标系到地理坐标系之间的 旋转矩阵，Q( to)表示初始时刻to下对应的姿态四元数；
[0030] 步骤2:按照设定周期T，采集三轴磁场数据、三轴加速度和三轴角速度数据；
[0031]步骤3:利用四元数的初始值Q(to)和三轴角速度信号co(t)，采用四阶龙格库塔优 化算法，迭代解算姿态四元数Q(tk)和三个姿态角即俯仰角0、滚转角y、航偏角的实时值； [0032]设时间间隔为t，四阶龙格库塔算法公式如下：
[0034]式中，T是四元数的更新周期，1^、1(2、1(3、1(4分别为 ：
[0036]设Q(tk) = [q0 qi q2 q3]T，由Q(tk)依次求出（？(匕）和姿态角9、f、y ;
[0039]采用动态kalman滤波对三轴角速度长时间的累积误差进行补偿，提高检测精度的 要求，构造随机线性离散系统方程如下：
[0040] Xk= 4)k/k-iXk-i+Tk-iffk-i [0041 ] Zk = HkXk+Vk
[0042] 其中，Xk为系统的n维状态向量;Zk为系统的m维观测序列；<i>k/k-i为系统的nXn维 状态转移矩阵;Hk为mX n维观测矩阵；X p维系统噪声输入矩阵;Wk-i为p维系统过程 噪声;Vk为m维系统观测噪声；
[0043] Kalman滤波增益矩阵：|丨1
[0044] 加速度计和磁力计的输出作为观测向量Zb，则离散的状态方程和观测方程分别 为： r ^
[0045] { + Rk
[0046] 式中，Rk为测量噪声方差矩阵；利用离散型Kalman滤波基本方程进行递推计算得 到各时刻状态的最优估计
[0047] 利用全A对陀螺仪姿态更新的姿态角进行前馈校正，如下式所示： rp _ -yj-
[0048] e <p Y n ￡x_ k ￡y±： ￡Lk
[0049] 所述PC上位机对生猪姿态进行识别的方法是：
[0050] 显示生猪的加速度和姿态波形曲线，根据设定的分类库，对采集的生猪姿态进行 分类，其具体方法如下：
[0051] 当生猪处于站立姿态时，由于装置传感器近似水平绑在生猪背部，载体坐标系Zb 与地理坐标系Zn两轴接近重合，夹角0接近0°C ;当生猪处于坐姿时，载体坐标系Zb与地理坐 标系ZJi成一定的夹角0;当生猪处于躺姿时，载体坐标系Z b与地理坐标系ZJi成夹角0接近 90 °C，因此根据不同姿态下载体坐标系b系与地理坐标系n系形成的不同夹角初步对姿态进 行分类;生猪行走时其加速度波形具有明显变化，当加速度值大于设定阈值时此时可以判 断生猪在行走，通过对采集的生猪姿态信息进行匹配，得到不同姿态下生猪的行为信息。 [0052]与现有技术相比，本发明根据MEMS传感器具有体积小、精度高、功耗低的特点，选 择整合三轴MEMS加速度计、三轴陀螺仪、三轴磁力计的MPU6050传感器，免除了组合陀螺仪 与加速度、磁力计之间轴间差的问题，节省了大量封装空间，同时采用四阶龙格库塔优化算 法对姿态信息进行更新。通过动态Kalman滤波模型，利用加速度计和磁力计对陀螺仪漂移 进行补偿，实现了陀螺仪姿态和加速度计、磁力计测姿态的优势互补，提高了系统动态测量 的精度和可靠性。本发明还完成了通过PC上位机对生猪姿态进行实时判别，相比于采用机 器视觉的方法，不仅设备成本明显降低，且适用性强，能够满足实时监测的要求。
[0053]同时，本发明装置体积小、质量轻、功耗小、成本低、便于穿戴在生猪躯干上，生猪 无明显应激反应，可以代替养殖人员观察生猪每个时刻的姿态，及早发现疑似发病生猪个 体，降低经济损失。
【附图说明】
[0054]图1是本发明原理框图。
[0055]图2是本发明结构示意图。
[0056] 图3是本发明应用在生猪躯体上的结构示意图。
[0057] 图4是姿态解算对应的算法流程图。
【具体实施方式】
[0058] 下面结合附图和实施例详细说明本发明的实施方式。
[0059] 微惯性传感器是一种测量物体运动时产生加速度、角速度数据的电子元件，将测 量到的数据通过电流和电压变化进行数据输出，因此能够准确检测到物体的运动情况。基 于微惯性传感器网络技术是一种接触式测量方法，但是由于传感器本身的对动物活动产生 的影响较小，因此可以将体积较小、功耗低、稳定性高的微惯性传感器佩戴在生猪躯体上， 实时获取其运动信息，并且通过利用无线通信技术进行信息传输，解决了养殖场布线困难 的问题。生猪的行为体现在姿态变化上，生猪在站、坐、躺不同行为方式下，传感器各方向轴 输出幅值各有不同，因此利用传感器方向轴输出的响应差异，可以实现对生猪行为的有效 监测。
[0060] 如图1所示，一种基于微惯性传感器的生猪姿态监测装置，包括：
[0061] 用于采集生猪个体信息的信息采集模块；
[0062] 用于将信息采集模块所采集信息进行融合滤波并输出姿态角的姿态解算模块；
[0063] 用于实现PC上位机与姿态解算模块之间通信的蓝牙模块一；
[0064] 用于实现供电的电源模块；
[0065] 以及，用于根据姿态解算模块的输出结果对生猪姿态进行识别的PC上位机。上位 机PC配置有蓝牙模块二和对生猪姿态进行识别的Matlab软件，以及显示生猪姿态的显示模 块。其中，信息采集模块包括三轴磁力计传感器HMC5883和整合三轴加速度传感器和三轴陀 螺仪的6轴微惯性传感器MPU-6050,采集生猪运动时的三轴磁场变化信息以及三轴加速度 和三轴角速度信息。蓝牙模块为HC-06与微惯性传感器MPU-6050数据传输采用I 2C接口，电 源模块采用1节5号电池供电。
[0066] 装置的安装如图2和图3所示，电源模块2、信息采集模块3和蓝牙模块一 4均置入防 水密封盒1中，防水密封盒1外部设置有用于卡入松紧带的沟槽5;
[0067] 使用时，防水密封盒1通过松紧带穿戴在生猪背部，沟槽5可使得松紧带绑缚牢靠。 防水密封盒1体积仅为75mm X 25mm X 30mm。
[0068]信息采集模块3中，三轴加速度的指向分别为x轴指向生猪右方，y轴指向生猪头部 方向，z轴指向地面。赛尔通MPU-60 50可准确追踪快速与慢速动作，用户可程式控制的加速 度感测范围为± 2g、± 4g、± 8g、± 16g，产品最高传输频率为400KHz。
[0069] 本发明还提供了相应的基于微惯性传感器的生猪姿态监测方法，包括如下步骤：
[0070] 采集生猪个体信息；
[0071] 将信息采集模块所采集信息进行融合滤波并输出姿态角信息；
[0072] 根据姿态解算模块的输出结果对生猪姿态进行识别。
[0073] 图4是姿态测量系统对应的算法流程图，介绍了该姿态测量系统。
[0074] 其中，图4给出了从初始校准到解算出精确姿态的整个算法流程，将信息采集模块 所采集信息进行融合滤波并输出精确姿态角包括如下步骤：
[0075]步骤1:系统初始校准。系统的初始姿态信息可以根据三轴加速度计、三轴磁力计 的测量值计算得到，初始校准算法如下：
[0076]系统上电后，传感器处于静止状态，加速度计测量数据为重力在三轴坐标上的分 量，地理坐标系下重力分量为gn=[00g]T，载体坐标系下重力分量为g gf， 有载体坐标系转换为地理坐标系公式，可得： -gsin0
[0077] gfc = C^gn = gsinycos沒进而可以得到俯仰角0、滚转角 y，@ = arcsin(-S/g) g COS/COS 0 y = arctan(gf:/gf)，由于仅使用加速度计无法计算静止状态下的航偏角P，因此利用磁力计 计算航偏角<r，磁力计传感器输出分量『=[1^ 0 Md]t，载体坐标系下地磁传感器分量为 M；： 载体坐标系转换为地理坐标系公式，可得： Mfl Imm~
[0078] Mhr =Cl 0 M\ IMd
[0079] 由地磁量与姿态角的关系解得航偏角
[0081]由初始姿态角9、y、免，求得初始时刻的旋转矩阵G仏)和姿态四元数Q(t〇);具体 求解公式如下：
[0083]设初始校准，姿态阵初值为qfZ^3x3，则姿态四元数的初始值Q(t〇)由以下公式确 定：
[0085] b表示载体坐标系，n表示地理坐标系，q1表示从载体坐标系到地理坐标系之间的 旋转矩阵，Q (to)表示初始时刻to对应的姿态四元数；
[0086] 步骤2:信号采集:DMP数字运算单元按照设定的采样周期T，采集MEMS三轴加速度 计信号、三轴MEMS陀螺仪输出的角速度信号和HMC5833磁力计输出的地磁强度信号；
[0087]步骤3:陀螺仪姿态更新:利用四元数的初始值Q(to)和陀螺仪输出的三轴角速度 信号《 (t)，采用四阶龙格库塔优化算法，迭代解算姿态四元数Q(tk)和俯仰角0、滚转角Y、 航偏角P的实时值；
[0088]设时间间隔为t，四阶龙格库塔算法公式如下：
[0090]式中，T是四元数的更新周期，1^、1(2、1(3、1(4分别为 ：
[0092]设Q(tk) = [qo qi q2 q3]T，由Q(tk)依次求出C,丨(k)和姿态角0、f、_y ;
[0095]步骤 4:Kalman 滤波
[0096]由于陀螺仪传感器存在随机漂移误差，因此采用动态kalman滤波对陀螺仪三轴角 速度长时间的累积误差进行补偿，以提高检测精度的要求。卡尔曼滤波算法流程，分为预估 与矫正两部分，预估部分使用前一时刻数据对当前时刻数据进行估计，矫正部分是利用当 前时刻的测量值对预估值进行矫正，进而得到较为准确的数值。
[0097] Kalman滤波方程为：
[0098]状态预测方程:Xk|k-i=AXk-i|k-i+BUk (7)
[0099]状态估计方程:Xk|k=Xk|k-i+Kgk(Zk_HXk|k-i) (8)
[0100] 预测误差方差矩阵:pk|k-FAPk-ilk-iA'+Q (9)
[0101] 滤波增益矩阵:Kk|k = Pk|k-iH/(HPk/k-iHt+R) (10)
[0102] 估计误差方差举证:Pk|k=(I-KgkH)Pk|k-i (11)
[0103] 变量说明：
[0104] X(k)表示k时刻的系统状态，U(k)表示k时刻对系统的控制量，A、B表示系统参数，Z (k)表示k时刻的测量值，H表示测量系统的参数，W(k)表示K时刻过程噪声，Q表示过程噪声 协方差，R表不测量噪声协方差。
[0105] 设计Kalman滤波器时利用陀螺仪输出数据作为预估数据Xk，陀螺仪更新的姿态误 差角为e = (ex ey ez)T，加速度计和磁力计输出作为矫正测量值Zk，构造随机线性离散系统 方程和矫正方程分别如下：
[0106] Xk= 4)k/k-iXk-i+Tk-iffk-i (12)
[0107] Zk = HkXk+Vk (13)
[0108] 其中，Xk为系统的n维状态向量;Zk为系统的m维观测序列；<i>k/k-i为系统的nXn维 状态转移矩阵;Hk为mX n维观测矩阵；X p维系统噪声输入矩阵;Wk-i为p维系统过程 噪声;Vk为m维系统观测噪声；
[0109] 公式（12)和公式（13)联立构成离散Kalman滤波的状态空间模块，利用离散型 Kalman滤波基本方程进行递推计算得到各时刻状态的最优估计t
[0110] 步骤5:输出校准：图4给出了输出校正原理流程图。利用对陀螺仪姿态更新的 姿态角进行前馈校正，如下式所示： -. .1 - T - -
[0111] ^ 9 7 =[^ <Pk nT + By,k e：Zik
[0112] 步骤6:返回步骤2;返回步骤2循环执行步骤2~步骤6。
[0113] PC上位机利用蓝牙模块接收到生猪的姿态信息，并输入Matlab软件中，通过编写 Matlab程序，显示生猪的加速度和姿态波形曲线，根据设定的分类库，对采集的生猪姿态进 行分类。其具体方法如下：
[0114] 当生猪处于站立姿态时，由于装置传感器近似水平绑在生猪背部，载体坐标系Zb 与地理坐标系Zn两轴接近重合，夹角0接近0°C ;当生猪处于坐姿时，载体坐标系Zb与地理坐 标系ZJi成一定的夹角0;当生猪处于躺姿时，载体坐标系Z b与地理坐标系ZJi成夹角0接近 90 °C，因此根据不同姿态下载体坐标系b系与地理坐标系n系形成的不同夹角初步对姿态进 行分类;生猪行走时其加速度波形具有明显变化，当加速度值大于设定阈值时此时可以判 断生猪在行走，通过对采集的生猪姿态信息进行匹配，得到不同姿态下生猪的行为信息。
[0115] 综上，本装置将微惯性传感器技术，无线蓝牙技术，姿态识别技术等多种信息融合 在一起，实现了对生猪运动状态的监测和识别，为生猪行为识别系统的设计提供了一种新 思路和方法。
【主权项】
1. 一种基于微惯性传感器的生猪姿态监测装置，其特征在于，包括： 用于采集生猪个体信息的信息采集模块； 用于将信息采集模块所采集信息进行融合滤波并输出姿态角的姿态解算模块； 以及，用于根据姿态解算模块的输出结果对生猪姿态进行识别的PC上位机。2. 根据权利要求1所述基于微惯性传感器的生猪姿态监测装置，其特征在于，还包括： 用于实现PC上位机与姿态解算模块之间通信的蓝牙模块； 用于实现供电的电源模块。3. 根据权利要求1所述基于微惯性传感器的生猪姿态监测装置，其特征在于，所述信息 采集模块包括三轴磁力计传感器HMC5883、整合三轴加速度传感器和三轴陀螺仪的6轴微惯 性传感器MPU-6050,采集生猪运动时的三轴加速度计信号、三轴陀螺仪输出的角速度信号 和HMC5833磁力计输出的地磁强度信号。4. 一种基于微惯性传感器的生猪姿态监测方法，其特征在于，包括如下步骤： 采集生猪个体信息； 将信息采集模块所采集信息进行融合滤波并输出姿态角信息； 根据姿态解算模块的输出结果对生猪姿态进行识别。5. 根据权利要求4所述基于微惯性传感器的生猪姿态监测方法，其特征在于，所述生猪 个体信息包括其运动时的三轴加速度和三轴角速度信息以及对姿态角进行校准的三轴磁 力计信息，利用集成了陀螺仪、加速度计的6轴微惯性传感器MPU-6050,以及通过I 2C接口与 6轴微惯性传感器MPU-6050相连的HMC5833磁力传感器采集获取。6. 根据权利要求5所述基于微惯性传感器的生猪姿态监测方法，其特征在于，所述将信 息采集模块所采集信息进行融合滤波并输出精确姿态角包括如下步骤： 步骤1:系统初始校准;系统利用三轴加速度分量计算得到初始状态的俯仰角Θ、滚转角 γ，三轴磁力计测量航偏角?5，求得初始时刻的旋转矩阵和姿态四元数q(t〇);具体求 解公式如下：设初始校准后，姿态阵初值为，则姿态四元数的初始值Q(t〇)由以下公式确 定：b表示载体坐标系，η表示地理坐标系，€Γ表示从载体坐标系到地理坐标系之间的旋转 矩阵，Q(to)表示初始时刻to下对应的姿态四元数； 步骤2:按照设定周期T，采集三轴磁场数据、三轴加速度和三轴角速度数据； 步骤3:陀螺仪姿态更新;利用四元数的初始值Q(to)和三轴角速度信号co(t)，采用四阶 龙格库塔优化算法，迭代解算姿态四元数Q(tk)求出俯仰角Θ、滚转角γ、航偏角0的实时值； 设时间间隔为t，四阶龙格库塔算法公式如下：式中，T是四元数的更新周期，M〇'）为载体坐标系转动时角速度矢量矩阵， ⑴、三个量为不同时刻下载体坐标(b)系三个轴转动的角速度，是L、 K2H4分别为：设Q(tk) = [qo qi q2 q3]T，式中qo qi q2 q3为四元数中的四个参数，由Q(tk)依次求出 CfK)和姿态角θ、9>、γ ;7.根据权利要求6所述基于微惯性传感器的生猪姿态监测方法，其特征在于，采用动态 kalman滤波对三轴角速度长时间的累积误差进行补偿，提高检测精度的要求，设计Kalman 滤波器时利用陀螺仪输出数据作为预估数据Xk，陀螺仪更新的姿态误差角为ε = (εχ ey εζ )Τ，加速度计和磁力计输出作为矫正测量值Zk，构造随机线性离散系统方程和矫正方程分别 如下： Xk- Φ k/k-lXk-1+Tk-lWk-1 Zk = HkXk+Vk 其中，Xk为系统的n维状态向量;Zk为系统的m维观测序列；<i>k/k-i为系统的nXn维状态转 移矩阵;Hk为m X η维观测矩阵；τΗ为η X p维系统噪声输入矩阵;Wk-i为p维系统过程噪声;Vk 为m维系统观测噪声； 由系统离散方程和矫正方程、联立构成离散Ka lman滤波的状态空间模块，利用离散型 Ka lman滤波基本方程进行递推计算得到各时刻状态的最优估计 利用it对陀螺仪姿态更新的姿态角进行前馈校正，如下式所示：8.根据权利要求4所述基于微惯性传感器的生猪姿态监测方法，其特征在于，所述PC上 位机对生猪姿态进行识别的方法是： 显示生猪的加速度和姿态波形曲线，根据设定的分类库，对采集的生猪姿态进行分类， 其具体方法如下： 通过松紧布带把监测装置缚在生猪背部，其内置监测模块近似与平面平起，当生猪处 于站立姿态时，载体坐标系Zb与地理坐标系zn两轴接近重合，夹角Θ接近〇°c;当生猪处于坐 姿时，载体坐标系Zb与地理坐标系ZJi成一定的夹角Θ;当生猪处于躺姿时，载体坐标系Zb与 地理坐标系4形成夹角Θ接近9(T C，因此根据不同姿态下载体坐标系b系与地理坐标系η系 形成的不同夹角大小可以实现对生猪站、坐、躺行为的初步分类;生猪行走时其加速度波形 具有明显变化，当加速度值大于设定阈值时此时可以判断生猪在行走，通过对采集的生猪 姿态信息进行匹配，得到不同姿态下生猪的行为信息。
【文档编号】A01K29/00GK105850773SQ201610187813
【公开日】2016年8月17日
【申请日】2016年3月29日
【发明人】张海辉, 王传哲, 王东, 张阳, 邵志成, 范奥华
【申请人】西北农林科技大学