✨ 长期致力于拖拉机自动导航系统、组合导航定位、液压比例控制、滑模控制、速度自适应研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》
（1）GNSS/MIMU/DR自适应扩展卡尔曼滤波组合导航定位：
采用微机械惯性测量单元（MIMU）和全球导航卫星系统（GNSS）构建组合导航系统，航位推算（DR）辅助弥补GNSS信号中断。拖拉机运动学模型基于阿克曼转向几何，状态向量包括位置（x,y）、航向角ψ、速度v和传感器偏差。自适应扩展卡尔曼滤波器中，量测噪声协方差矩阵R根据GNSS信号质量和速度实时调整，速度低于2km/h时增大伪距观测噪声，速度高于10km/h时减小。在田间试验中，组合导航系统在GNSS失锁30秒内定位误差小于0.35m，而纯DR误差达2.1m。直线导轨实验（100m距离）显示，系统横向偏差RMS为0.028m，纵向偏差0.05m。
（2）液压比例转向控制系统设计与滑模控制器：
在拖拉机原有全液压转向系统上并联自动转向阀块，阀块包含两个电磁比例减压阀和一个梭阀，适用于开心、闭心和负载传感三种液压系统。建立转向系统二阶数学模型，辨识得到转向阀增益0.12m³/s/A，转向油缸时间常数0.08s。滑模控制器设计以航向角误差为滑模面，采用指数趋近律，切换增益根据速度自适应调整：低速时降低增益避免超调，高速时提高增益保证响应速度。在8km/h速度下，阶跃转向响应超调量5.2%，调节时间0.35s，稳态误差小于0.3°。仿真和实车验证表明，滑模控制器对路面颠簸的抑制能力优于PID，横向误差标准差减少42%。
（3）速度自适应路径跟踪控制与实车试验：
路径跟踪采用基于预瞄距离的纯追踪模型与速度自适应横向控制器结合。预瞄距离随速度线性增加：Ld = 0.5 + 0.1*v (v单位km/h)。横向偏差控制采用线性二次型调节器，权重矩阵随速度调整，高速时增加航向偏差权重。开发了基于RTK-GNSS的自动导航试验平台（福田雷沃TG1254拖拉机），在田间进行直线路径和地头转弯测试。直线路径跟踪在6-12km/h速度范围内，横向误差平均值小于0.035m，标准差0.022m。点-位置路径跟踪（从A点至B点）在8km/h下，终点到达误差0.12m。系统已连续作业120小时，液压阀故障率为零，满足农业生产要求。
import numpy as np from filterpy.kalman import ExtendedKalmanFilter as EKF from scipy.linalg import solve_continuous_are import control as ct class TractorKinematics: def __init__(self, L=2.5): # wheelbase self.L = L def state_transition(self, x, dt, steering_angle, v): x[0] += v * np.cos(x[2]) * dt x[1] += v * np.sin(x[2]) * dt x[2] += v * np.tan(steering_angle) / self.L * dt x[3] = v return x class AdaptiveEKF: def __init__(self, dim_x=4): self.ekf = EKF(dim_x, dim_z=3) self.Q = np.diag([0.1, 0.1, 0.01, 0.5])**2 self.R_base = np.diag([0.05, 0.05, 0.01])**2 def update_R(self, speed, gnss_hdop): factor = 1.0 + 2.0 * (speed < 2) + 0.5 * (speed > 10) self.ekf.R = self.R_base * factor * gnss_hdop def predict(self, u, dt): self.ekf.predict() def update(self, z): self.ekf.update(z) class HydraulicSteeringSM: def __init__(self, K=0.12, tau=0.08): self.K = K self.tau = tau def sliding_control(self, e_psi, de_psi, speed): lambda_ = 2.0 s = e_psi + lambda_ * de_psi eta = 0.8 * (1 + 0.1 * speed) u_sw = -eta * np.tanh(s / 0.05) u_eq = -de_psi / (lambda_ * self.K) return u_eq + u_sw class PurePursuit: def __init__(self, lookahead_base=0.5, lookahead_coef=0.1): self.Ld_base = lookahead_base self.coef = lookahead_coef def compute_steering(self, lateral_error, heading_error, velocity, wheelbase=2.5): Ld = self.Ld_base + self.coef * velocity # curvature = 2 * lateral_error / Ld^2 curvature = 2 * lateral_error / (Ld**2 + 1e-6) steering = np.arctan(wheelbase * curvature) # add heading correction steering += 0.5 * heading_error return np.clip(steering, -0.5, 0.5) def lqr_path_tracking(A, B, Q, R): P = solve_continuous_are(A, B, Q, R) K = np.linalg.inv(R) @ B.T @ P return K def simulate_tractor_tracking(track_points, initial_state, dt=0.05): kinematics = TractorKinematics() pure = PurePursuit() states = [initial_state] for i, (ref_x, ref_y, ref_psi) in enumerate(track_points): x,y,psi,v = states[-1] lateral_err = (ref_x - x)*np.sin(ref_psi) - (ref_y - y)*np.cos(ref_psi) heading_err = ref_psi - psi delta = pure.compute_steering(lateral_err, heading_err, v) new_state = kinematics.state_transition(np.array(states[-1]), dt, delta, v) states.append(new_state) return np.array(states) if __name__ == '__main__': akf = AdaptiveEKF() akf.update_R(8.0, 1.2) print(f'EKF R matrix diagonal: {np.diag(akf.ekf.R)}') smc = HydraulicSteeringSM() u_sm = smc.sliding_control(0.05, -0.02, 8.0) print(f'Sliding mode control output: {u_sm:.3f} V') pure = PurePursuit() steer = pure.compute_steering(0.03, 0.01, 10.0) print(f'Pure pursuit steering angle: {np.degrees(steer):.1f} deg') # LQR weights velocity adaptive v = 8.0 Q = np.diag([10.0, 5.0/v, 2.0]) R = np.array([[0.5]]) A = np.array([[0, v, 0], [0,0,v], [0,0,0]]) B = np.array([[0],[0],[1]]) K_lqr = lqr_path_tracking(A, B, Q, R) print(f'LQR gain: {K_lqr[0]}') # simulation on straight line track = [(i*2, 0, 0) for i in range(50)] init = [0,0,0,2.0] traj = simulate_tractor_tracking(track, init) final_err = np.abs(traj[-1,1]) print(f'Final lateral error: {final_err:.3f} m')
)
