✨ 长期致力于多传感器融合、同时定位与建图、路径跟踪、模型预测控制、模糊控制研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》

（1）多传感器融合SLAM系统构建：

以履带式喷雾机为载体，装配RoboSense 32线激光雷达、Intel RealSense D435i相机和BMI088惯性测量单元。首先对IMU内参进行Allan方差标定，陀螺仪零偏不稳定性降至8.2°/h，加速度计零偏不稳定性0.012 mg；激光雷达与相机的外参通过手眼标定求解，重投影误差0.35像素。SLAM前端包含三个并行里程计约束：激光里程计采用点面分割分割出地面平面点与树干边缘点，对非地面点按曲率提取200个角点和400个平面点，使用ICP迭代求解相邻帧位姿；视觉里程计采用Shi-Tomasi提取200个角点，稀疏光流跟踪，并通过激光点云投影为2D特征点赋予深度，构造视觉重投影误差约束；惯性里程计通过IMU预积分提供帧间高速率位姿约束。后端用iSAM2因子图优化，关键帧选取条件为平移超过0.5米或旋转超过5°。回环检测融合激光Scan Context描述子和视觉词袋模型，在检测到回环后，构建局部特征地图进行ICP精细匹配，将回环约束加入因子图进行全局优化。在梯状柑橘园采集的1.2公里数据上，绝对位置误差均值0.13米，建图一致性良好。

（2）履带底盘运动学建模与模糊MPC路径跟踪：

对履带式喷雾机建立考虑滑移的运动学模型：x_dot=v·cos(θ)/(1-i)，y_dot=v·sin(θ)/(1-i)，θ_dot = ω/(1-α)，其中i为纵向滑移率，α为转向滑移角，与地面附着和转弯半径相关。对模型在参考轨迹点处线性化，离散化周期0.05秒，状态量X=[x, y, θ, v]^T，控制量U=[v_cmd, ω_cmd]^T。MPC控制器预测时域Np=25，控制时域Nc=8，代价函数二次型权重Q=diag(1,1,0.5,0.1)，R=diag(0.1,0.2)。在线性二次型基础上，引入模糊调节机制：以横向误差ed和航向误差eθ的绝对值作为模糊输入，输出权重修正因子Δλed和ΔR。论域划分为小、中、大三个等级，以高斯隶属函数计算隶属度，模糊规则规定当ed大且eθ大时，增大Q中横向偏差权重、减小控制量惩罚以快速纠偏；当误差很小时则适当放松精度追求平稳。鱼钩路径仿真显示，模糊MPC的横向距离偏差最大值0.11米，航向角偏差最大值2.76°，质心侧偏角最大值1.12°，均优于普通MPC和纯跟踪控制器。

（3）丘陵果园实地试验验证：

在坡度5°至15°的丘陵柑橘园内铺设长约300米的S形作业路径，包含3处半径6米急弯。定位建图实验中，本算法定位的路径偏差最大值0.28米，平均值0.13米，显著优于LeGO-LOAM（最大值0.68米）和LINS（最大值0.52米），构建的地图在果树行间呈现清晰的行道边界，树干位置与人工测量值偏差在0.2米以内。路径跟踪实验中，模糊MPC控制器的横向距离偏差最大值0.43米、均值0.16米，航向角偏差最大值5.77°、均值2.55°，质心侧偏角最大值1.24°、均值0.48°。与纯跟踪控制器（最大横向偏差0.97米）和普通MPC（0.58米）相比，模糊MPC在急弯处的路径贴合能力和稳定性明显更优。全程喷雾机实际行驶速度保持在1.2至1.8米/秒，最高作业效率下每小时可覆盖0.8公顷果园，达到实用化水平。

import numpy as np
import cvxopt
from scipy.spatial import KDTree

# 点面分割提取
def extract_planar_edge_points(cloud):
    # cloud: Nx3 点云
    # 使用RANSAC提取地面平面
    best_plane = None
    max_inliers = 0
    for _ in range(50):
        sample_ids = np.random.choice(len(cloud), 3, replace=False)
        p1,p2,p3 = cloud[sample_ids]
        v1 = p2 - p1
        v2 = p3 - p1
        normal = np.cross(v1, v2)
        d = -normal.dot(p1)
        dists = np.abs(cloud.dot(normal) + d) / np.linalg.norm(normal)
        inliers = cloud[dists < 0.1]
        if len(inliers) > max_inliers:
            max_inliers = len(inliers)
            best_plane = (normal, d)
    planar_mask = dists < 0.1
    plane_points = cloud[planar_mask]
    other_points = cloud[~planar_mask]
    return plane_points, other_points

# 滑移运动学模型离散化
def kinematics_model(state, control, dt, i=0.05, alpha=0.03):
    x, y, theta, v = state
    v_cmd, omega_cmd = control
    v_actual = v_cmd / (1 - i)
    omega_actual = omega_cmd / (1 - alpha)
    x_new = x + v_actual * np.cos(theta) * dt
    y_new = y + v_actual * np.sin(theta) * dt
    theta_new = theta + omega_actual * dt
    return np.array([x_new, y_new, theta_new, v_cmd])

def fuzzy_mpc_weights_finetune(ed, e_theta):
    # 模糊调节因子计算
    mu_small = np.exp(-ed**2/0.01)
    mu_large = 1 - mu_small
    delta_q = 5.0 * mu_large
    delta_r = -0.5 * mu_large
    return delta_q, delta_r