基于多传感融合的丘陵山地农机姿态检测单元

蒿晟昆，霍静琦，张 宇，候 璞，张 琛，李志伟

（山西农业大学农业工程学院，山西晋中 030801）

国内对于丘陵山地农机的研究主要集中在其车身调节机构及调整算法[1]，对于农机作业姿态检测研究较少，多数研究只采用单轴或双轴倾角传感器采集农机车身姿态信号，但是拖拉机在作业时抖动噪音干扰极大，仅依靠倾角传感器很难获得农机真实的姿态信息，发动机工作噪音、路面噪音、车身部件抖动噪音均会影响姿态检测值[2]。该文通过多传感器融合，利用双轴倾角传感器及三轴加速度计实时对丘陵作业时的农机进行姿态检测，分别采用不同的滤波算法对采样信号进行处理得到两组倾角信号，通过信号融合算法将两组倾角信号融合，进而获得更加精确、稳定、全面的农机姿态信息。

1 丘陵山地拖拉机作业安全性分析

丘陵山地农机的安全性主要从横向稳定性和纵向稳定性两个方面考虑[3]；提高横纵向稳定性是为防止农机在丘陵山地作业时出现倾翻或者滑移等危险情况。爬坡作业时的受力分析如图1 所示，通常农机缓慢匀速爬坡作业，空气阻力对农机纵向稳定性的影响忽略不计，其中，农机受力情况如下：自身重力G、地面给予的垂直支撑力N、履带驱动轮的牵引力P1、履带所受到的地面阻力Pf。设此时斜坡角度为α1，力矩平衡公式[4]如式（1）所示：

图1 农机爬坡作业的受力图

根据式（1）可知，随着坡度加大，地面对履带的垂直支撑力N逐渐沿着履带从接地中点向接地后缘点A移动，所以农机爬坡作业时不发生纵向倾翻的临界条件为：

当农机处于另外一种作业状态，设农机以较低匀速进行横坡作业，其整机受力如图2 所示，其中，农机质心高度为h、质心偏移为e、N1和N2分别为地面对左右履带的支撑合力、Z1和Z2分别为地面对履带的横向摩擦力、Fi为绕M点的惯性力、B和b分别为履带轨距和接地宽度。农机匀速行驶时惯性力忽略不计。根据受力平衡方程，当右侧履带支撑合力N2的作用点移至M点的横向倾翻临界条件为[5]：

图2 农机横坡作业的受力图

2 姿态检测传感器

该文采用三轴加速度传感器和双轴倾角传感器，传统研究利用单轴倾角传感器检测农机姿态不够全面，仅能检测农机坡地作业时的横向倾角[6]；将单轴倾角传感器置于农机车身质心处虽然可以检测出倾角，但是实际作业中很难将农机的运动理想化为一个质点[7]，因此在车身前后安装倾角传感器和加速度传感器测出对应的倾角信号，结合传感器各自的特点，采用不同的滤波算法最大化消除农机自身抖动和路面引入的噪音信号[8]，通过信号融合算法将两者融合后便能更准确地呈现农机姿态。

倾角传感器选用迈科传感公司的LVG726T 型动态倾角传感器，该款传感器工作温度范围为-40～85 ℃，IP67 防护等级，可用于工业、农业等领域。

其工作原理为电容微型摆锤原理[9]，将倾角传感器置于倾斜面时，地球重力在电容微型摆锤上产生分量，电容值也会发生变化；摆锤与左极板之间以及右极板之间各自产生一个电压，当发生倾斜时，极板电压会按照一定规律变化；通过解算得到倾角α关于极板电压的函数[10]。

加速度传感器选用迈科传感公司的MK926 型三轴加速度计，全量程范围内偏差典型值和比例因子稳定性小于0.1%。

其工作原理是利用硅的机械特性设计可移动机构，该机构由固定和运动的两片硅梳齿组成（Siliconfingers）；前者视为固定的电极，后者视为可移动电极。当硅梳齿发生相对运动后，电容值和运动的相对位移存在线性关系；ASIC 接口芯片将电容差值转换为电压信号值，内置微处理器将电压信号转换为重力加速度g在X、Y、Z轴的加速度分量Ax、Ay和Az[11]。如图3 所示，根据三角函数关系可以解得其纵向倾斜角β2和横向倾斜角α2，如式（4）所示：

图3 三轴加速度转倾角示意图

3 丘陵山地农机姿态信号检测单元的设计

丘陵山地农机姿态信号检测单元的微控制芯片使用STM32F103ZET6，其外设拓展能力强、低功耗、总线时钟频率高，可用于数据处理能力要求不高、外界环境复杂、制作成本较低的农机自动化设计中。由于农机的驾驶台通常只能提供12 V 或者24 V 的电压，利用12 V 多路DC-DC 模块将12 V 降压至3.3 V后向单片机供电；12 V 降压至5 V 后向三轴加速度计和动态倾角传感器供电。通过拓展JLINK-08 模块使其只需要USB 线即可修改程序，便于现场调试。此外还搭载一块0.96 寸的OLED 数显模块，实时显示姿态检测数据。

MK926 加速度计和LVG726T 动态倾角传感器的信号传输方式均为232 通信，该文通过两组RS232-TTL 转换模块连接两个传感器的数据接口并通过串口通信方式读入单片机内，其中，倾角传感器通过PA9、PA10 引脚的串口1 进行数据传输，加速度传感器通过PA2、PA3 引脚的串口2 进行数据传输。

两个传感器的读取数据包指令均相同，因此单片机在读取倾角和加速度信号时，只需通过串口1、串口2 同步发送读取全部数据的命令，其读取全部数据命令设置如下：

void tx_stack_init(void)//读取全部数据发送命令：68 04 00 04 08

对于传感器传回的数据还需要进行解算，MK926 加速度计测量X、Y、Z轴的加速度值由压缩BCD 码表示，每个轴的数据由三个字节组成，格式为XAA.BBB；A 为符号标志，1 表示-，0 表示+；AA 为两位加速度整数值；BBB 为三位加速度小数值。LVG726T-60 倾角传感器测量的X、Y轴倾角值由压缩BCD 码表示，每个轴的数据由三个字节组成，格式为XA.AA.BB；X为符号标志，1 表示-，0 表示+；AAA为三位角度整数值；BB 为三位角度小数值。

通常需要采集传感器检测的姿态数据，因此还设计了可供实时开启/关闭的数据采集模块，通过加装3.3 V 电源开关向单片机PA4 引脚发送高低电平，默认开关关闭时PA4 引脚保持低电平信号，不进行数据采集；当手动开启开关后，PA4 引脚置为高电平，单片机软件触发存储中断服务子函数，将此后采集的倾角、加速度数据通过SDIO 方式存入SD 卡内，便于实验数据分析及处理。

4 动态倾角传感器标准中值滤波算法

利用倾角传感器对农机姿态进行检测时，其采集的原始数据存在噪音干扰，影响对农机姿态的判断，由于LVG726 动态倾角传感器内部MCU 自带卡尔曼滤波算法，因此对原始数据采用标准中值滤波算法，提高数据的可读性；中值滤波算法是以离散数据统计理论为基础的非线性滤波算法，其特点为最大化抑制噪音干扰的同时还能保护信号边缘完整性。

标准中值滤波器（SMF）本质是将绝对误差最小估计[12]，作为非线性信号滤波技术，该算法的原理和运算对比其他滤波算法更简单，且有良好的噪音抑制效果。假设一组离散的时间序列T，设置一定宽度的滑动窗口后，按次序从数据序列取奇数长度的数据作为样本，对应该时刻x的样本为{T(x)}，如式（5）所示：

式中，ZR和ZL为非负整数，{T(x)}的取值需要对称分布，即ZR=ZL=Z1，则有：

式（6）中Z=2×Z1+1，Ti(x)=T(x-Z1+i-1)，i={1,2,3,4,…,Z}；利用Med 函数对采样数据取中值，即对序列T(x)进行大小排序处理，并选取中值赋给L(x)视为此次中值滤波输出值[13]。

如果对采样序列{T(x)}进行大小排序处理得到T1(x),T2(x),T3(x),…,TZ(x)，那么经过中值滤波后的输出值L(x)可用式（7）表示：

对于有限长度的离散序列{T1(x),T2(x),T3(x),…,TQ(x)}，中值滤波器的滑动窗口宽度为1+2×Z1，在滤波前还要对输入信号两端拓展Z1个额外数据，以保证信号不丢失，通常采用如下方法：

对称拓展，拓展的额外数据等于滑动滤波窗口两端信号值，如式（8）所示：

动态倾角传感器的标准中值滤波算法步骤如下：①输入采样的倾角信号；②对采样数据采用对称拓展；③设置中值滤波器的滤波窗口大小，以达到较好的滤波效果；④对处于滤波窗口中的序列T(x)进行大小排序；⑤通过输出序列中值L(x)作为本次倾角信号滤波值。

5 三轴加速度传感器卡尔曼滤波算法

三轴加速度传感器的数据包为X、Y、Z方向的加速度值Ax、Ay、Az，根据式（4）的反正切关系可求得农机姿态的横向倾角α2和纵向倾角β2。加速度传感器转化后的角度仍存在噪音干扰，导致信号波动性大、平稳性差，需要对其进行数字滤波，该文对加速度传感器的数据进行数字卡尔曼滤波[13]。

卡尔曼滤波算法基本原理：利用系统中的信号及干扰噪音建立状态空间模型，融合上一时刻的最优估计值和当前时刻测量值，进而解算出当前时刻最优估计值。该算法依照“预测值—测量值—估计值”的顺序反复运算[14]，从而达到实时滤波的效果。设随机线性离散系统的状态方程及测量方程分别为：

式中，Q(k)为k时刻系统状态阵；P(k)为k时刻的测量值；E(k)为系统噪音阵；R(k)为观测噪音阵；H为系统测量参数阵；A、B为参数阵。其递推过程如下：

1）状态预测如式（11）所示：

2）k时刻协方差估计过程如式（12）、（13）所示：

其中，S为该系统噪音的协方差，V为测量噪音协方差，δ(k)为滤波增益矩阵。

3）对系统进行状态估计；

4）修正k时刻该系统的最优解协方差，如下：

通过上述公式，设定任意初值和k时刻的测量值P(k)，通过卡尔曼滤波算法便可递推估计出下一时刻状态。该文需要对加速度计转换后的横向倾角α2和纵向倾角β2分别进行卡尔曼滤波；输入的倾角信号为农机作业时的姿态信号，该系统真值无法确定，因此进行卡尔曼滤波处理时应该调整系数尽可能抑制噪音对真实倾角信号的干扰，在信号不失真的情况下减小信号波动。

6 多传感器平均加权融合算法

图4 所示为加权数据融合算法的多传感器测量值融合模型[15]；加权融合算法模型如下：同时对一个目标进行测量时，n个传感器相对应的测量数据为X1,X2,X3,…,Xn。则第k个传感器的测量值为Xk且每个传感器的测量值互相独立，互不干扰。每个传感器的加权系数表示为W1,W2,W3,…,Wn，先将Xn进行加权融合[16]。平均加权、最优加权和自适应加权三种融合算法对于每个传感器测量值的加权系数W的求解方法存在区别。式（15）表示加权融合后的取值。

图4 加权数据融合算法模型

加权融合算法通常利用测量值Xn和真实值X计算出总均方差，当均方差取最小值时，每个传感器的加权系数W为最优值，但是农机在丘陵山地坡地作业时，无法获取姿态的真实值X，仅能通过传感器的噪音滤波和真实信号融合尽可能描述农机的作业姿态；因此该文对三轴加速度计转换后的横向倾角α2、纵向倾角β2和倾角传感器采集的横向倾角α1和纵向倾角β1进行加权平均滤波处理。

加权平均融合算法（MVFA）基于数学平均式求取的思想对各传感器的加权系数Wi取相等的值；n个传感器在同一时刻对同一对象进行测量，各传感器方差分别为

由于该文采用双传感器数据融合，其总均方差可以简化为式（16）：

双传感器经过MVFA 算法融合后的姿态估计值可以表示为：

结合上文描述的倾角滤波及倾角融合算法，图5所示为农机姿态检测的主要步骤。

7 试验与结果分析

试验选用一台小型履带柴油动力农机进行丘陵山地作业。如图6（a）、（b）所示为农机在横坡上作业时采集的数据，此时农机存在横向侧翻的隐患，分别绘制加速度计采集的加速度信号转化为倾角信号后进行卡尔曼滤波、倾角传感器采集的倾角信号进行标准中值滤波的曲线图。根据对比分析可知，三轴加速度计所测得的倾角信号波动范围为0～20.4°，倾角信号传感器所测得的倾角信号波动范围为0～51.2°。进行滤波处理的曲线均表现平滑，排除意外噪音干扰，波动范围均为0～3°，符合滤波预期效果。

图6 横向倾角数据图

在三轴加速度计所测得的倾角信号波动范围为0～6.3°，倾角信号传感器所测得的倾角信号波动范围为0～6.7°的基础上，图7 所示为农机在纵坡上作业时采集的数据，此时农机存在纵向侧翻的隐患，分别绘制加速度计采集的加速度信号转化为倾角信号后进行卡尔曼滤波、倾角传感器采集的倾角信号进行标准中值滤波的曲线图。进行滤波处理的曲线均表现平滑，排除意外噪音干扰，波动范围均为0～2.5°，符合滤波预期效果。

图7 纵向倾角数据图

多传感融合结果如图8、9 所示，由于三轴加速度计传感器和倾角传感器安装在农机中心不同位置处，因此农机车体前后测量值存在差异，为了更好地描述农机整体的作业姿态，将两种传感器测量值分别滤波后再采用平均加权融合算法处理。处理后的曲线更加平滑，数值波动范围为0～2.2°，几乎无噪音干扰，对于整车姿态变化过程描述更为准确。

图8 横向倾角多传感融合结果图

图9 纵向倾角多传感融合结果图

8 结论

该文通过设计一种基于多传感融合的丘陵山地农机姿态检测单元，优化现有的单轴倾角传感器测量的缺陷，选择三轴加速度与双轴倾角相融合的姿态检测策略，将农机不再理想化为一个质点，而是分别测量农机车体前后位置的横向、纵向倾角信号，经过不同的滤波算法对其达到最优处理后再次进行多传感器融合，保证尽可能排除农机自身抖动和外界干扰，最大程度还原农机真实作业时姿态的变化，为后续研发丘陵山地农机坡地自适应机构和控制系统提供了思路与参考。