[发明专利]履带式农业机械的综合自动控制装置

权利要求书

1.一种履带式农业机械的综合自动控制装置，由测向型卫星定位系统、导航控制器、ECU机箱、电磁液压集成阀、升降油缸的位移传感器、水平油缸的位移传感器、电动推杆调速装置和CAN网络组成，其特征在于：

导航控制器对履带式农机进行综合自动控制，软件系统启动CAN通信线程、控制线程；CAN通信线程一旦接收到位姿数据采集卡的数据，控制线程立即解析出由测向型卫星定位系统产生的车身的位置、航向角和俯仰角，以及由陀螺仪产生的车身绕X轴的角速率和俯仰角、绕Y轴的角速率和横滚角；把测向型卫星定位系统产生的车身的X坐标、Y坐标、航向角、俯仰角和陀螺仪产生的绕车身X轴的角速率、绕Y轴的角速率、俯仰角、横滚角构成观测量，把车身的X坐标、Y坐标、航向角、绕X轴的角速率、Y轴的角速率、俯仰角和横滚角构成状态量，进行Kalman滤波器的递推计算；状态变量为

X_k＝[x_k,y_k,z_k,v_k,ω_xk,ω_yk,γ_xk,γ_yk,ψ_zk]^T                (1)

状态转移矩阵是

观察量和观测矩阵是

式中，X_k是状态变量矩阵

x_k，y_k，z_k分别是履带式农业机械测向型卫星定位系统的前GPS天线在时刻k的x、y、z坐标的状态量,m

v_k是履带式农业机械在时刻k的车速的状态量,m/s

ω_xk，ω_yk分别是履带式农业机械在时刻k的车身绕X轴、Y轴旋转角速度的状态量,rad/s

γ_xk，γ_yk分别是履带式农业机械在时刻k的车身绕X轴俯仰角、绕Y轴横滚角的状态量,rad

ψ_zk是履带式农业机械在时刻k的车身绕Z轴航向角的状态量,rad

Φ_k是状态转移矩阵

T是采样的时间间隔,s

Z_k是观测量矩阵

H_k是观测矩阵

ω_x，ω_y分别是履带式农业机械车身绕X轴、Y轴旋转角速度的观测量,rad/s

γ_x，γ_y分别是履带式农业机械车身绕X轴俯仰角、绕Y轴横滚角的观测量,rad

γ_GPS是由测向型卫星定位系统获得的车身绕X轴俯仰角的观测量,rad

ψ_GPS是由测向型卫星定位系统获得的车身绕Z轴航向角的观测量,rad

导航控制器然后从履带机具板卡获取升降油缸和水平油缸的位移传感器的数据，计算此时升降油缸的推杆的伸出量、水平油缸的推杆的伸出量；由此计算出悬挂机具当前的作业深度与设置的作业深度之间的差值、当前的左右两端之间的高程差：

其中，ΔE_h是悬挂机具当前的作业深度与设置的作业深度之间的差值,m

ΔD_h是悬挂机具当前的左右两端之间的高程差,m

Δs₁是当前升降油缸伸出量与设定工作深度时的伸出量之间的差值产生的油缸行程量,m

Δs₂是当前水平油缸伸出量与悬挂机具水平状态时的伸出量之间的差值产生的油缸行程量,m

ΔE_GPS是前卫星定位天线的高程的改变量,m

l₁，l₁₁分别是上提升臂的总长、从机身支座到油缸铰接点之间的距离,m

l₂，l₂₁分别是下提升臂的总长、从机身支座到拉杆铰接点之间的距离,m

l₃，l₄分别是履带中点距离前卫星定位天线的水平距离、履带中点距离悬挂机具刀尖的水平距离,m

l₅，l₆分别是悬挂机具的拉杆铰接点之间的距离和悬挂机具的宽度,m

α是升降油缸与垂线之间的角度,rad

依据车身的横向偏差和航向角，按照导航的非线性PID控制算法产生改变履带运动状态的纠正路径偏差的控制量，按照机具控制非线性PID控制算法产生使升降油缸、水平油缸动作的控制量；鉴于控制履带、升降油缸、水平油缸的油路属于串联性质，因此尽管每一次数据周期均可产生式(1)、式(3)和式(4)的计算结果，但是输出控制需要按照一定的节拍，并考虑不同作业场景的差异；如果是挂接旋耕机进行耕地作业时，直行阶段，导航控制器需要兼顾导航、作业深度、水平控制，以5拍为一个周期，按照输出节拍1：2：2的方式进行控制；转弯阶段，导航控制器需要兼顾作业深度与水平控制，仍然以5拍为一个周期，按照输出节拍3：2的方式进行控制；如果是挂接开沟机进行开沟作业，直行阶段，导航控制器兼顾导航、作业深度，输出控制节拍为1：4；转弯阶段，开沟机被抬升后，导航控制器只控制航向。