用于移动机器人导航的 MATLAB 示例 其中包括1.使用扩展卡尔曼滤波器进行定位的示例代码；2.使用 Unscented Kalman 滤波器进行定位的示例代码；3.用粒子过滤器进行定位的示例代码；使用网格地图进行映射的示例代码；5.用Dijkstra方法进行路径规划的示例代码；6.用Astar进行路径规划的示例代码；6.用梯度法进行路径平滑的示例代码；7.用动态窗口法进行局部路径规划的示例代码；8.用迭代最接近点（ICP）算法进行相对运动估计的示例代码；9.基于特征点的EKF SLAM的示例代码；10.用EM算法对混合高斯模型进行参数学习的示例代码；11.用Nelder-mead算法进行非线性优化的示例代码；12.用最陡峭下降法进行非线性优化的示例代码；用共轭梯度法进行非线性优化的示例

先说说定位。定位就是回答“我在哪儿”的问题，而环境里往往有噪声，所以滤波算法是常客。扩展卡尔曼滤波（EKF）算是经典款了，它把非线性系统在估计点附近线性化，然后套用标准卡尔曼滤波那套预测-更新流程。

% EKF 预测步骤片段
x_pred = f(x_est, u); % 非线性状态转移
F = jacobian(f, x_est); % 计算雅可比矩阵
P_pred = F * P_est * F' + Q;

这里 f 是运动模型，jacobian 求偏导得到线性近似的状态转移矩阵 F。EKF 的问题在于线性化误差，尤其在非线性强的时候容易飘。

于是 Unscented Kalman Filter (UKF) 来了。它不搞线性化，而是选一组叫 sigma 点的样本，让它们经过真实非线性模型传递，再用这些点重建高斯分布。

% UKF 生成 sigma 点
[sigma_points, weights] = unscentedTransform(x_est, P_est);
sigma_points_pred = zeros(size(sigma_points));
for i = 1:size(sigma_points,2)
    sigma_points_pred(:,i) = f(sigma_points(:,i), u);
end
x_pred = sigma_points_pred * weights';

UKF 通常比 EKF 更稳定，尤其面对强非线性。但两者都是基于高斯假设，如果分布是多峰的（比如机器人可能在两个相似走廊中的任意一个），粒子滤波（PF）就更合适。

粒子滤波用一堆粒子表示后验分布，每个粒子是一个状态假设，通过运动模型传播，再用传感器观测给粒子赋权重，重采样。

% 粒子滤波重采样核心
cumulative_sum = cumsum(weights);
new_particles = zeros(size(particles));
for i = 1:N
    r = rand();
    idx = find(cumulative_sum >= r, 1);
    new_particles(:,i) = particles(:,idx);
end

这方法计算量大，但能处理任意分布。定位搞定了，还得有地图。网格地图简单直接，把环境划成小格子，每个格子存占据概率。

% 更新占据栅格地图（对数几率形式）
log_odds = log(map_occupied ./ (1 - map_occupied));
log_odds = log_odds + log(sensor_model(z)) - log(1 - sensor_model(z));
map_occupied = 1 - 1./(1+exp(log_odds));

用对数几率形式可以避免数值问题，更新也快。

知道自己在哪，地图也有了，就该规划怎么走了。Dijkstra 是最短路径老将，它从起点开始，逐步扩展到所有可达节点。

% Dijkstra 核心循环
while ~isempty(unvisited)
    [~, idx] = min(dist(unvisited));
    current = unvisited(idx);
    if current == goal, break; end
    unvisited(idx) = [];
    for neighbor = neighbors(current)
        tentative = dist(current) + cost(current, neighbor);
        if tentative < dist(neighbor)
            dist(neighbor) = tentative;
            prev(neighbor) = current;
        end
    end
end

它保证找到最优解，但效率不高。A* 用启发函数 h(n) 引导搜索方向。

f_score = g_score + h(node, goal); % g_score 是实际代价，h 是启发函数

如果 h(n) 是可采纳的（从不超估真实代价），A* 也能找到最优路径，通常比 Dijkstra 快很多。

规划出的路径可能拐弯太急，机器人不好跟，所以需要平滑。梯度下降法可以优化路径点序列，让路径更平滑且远离障碍物。

% 梯度下降平滑迭代
for iter = 1:max_iter
    gradient = computeGradient(path, obstacles);
    path = path - alpha * gradient;
end

computeGradient 通常包含两项：一项让路径点靠近原始点，一项让相邻点间距均匀、角度变化小。

用于移动机器人导航的 MATLAB 示例 其中包括1.使用扩展卡尔曼滤波器进行定位的示例代码；2.使用 Unscented Kalman 滤波器进行定位的示例代码；3.用粒子过滤器进行定位的示例代码；使用网格地图进行映射的示例代码；5.用Dijkstra方法进行路径规划的示例代码；6.用Astar进行路径规划的示例代码；6.用梯度法进行路径平滑的示例代码；7.用动态窗口法进行局部路径规划的示例代码；8.用迭代最接近点（ICP）算法进行相对运动估计的示例代码；9.基于特征点的EKF SLAM的示例代码；10.用EM算法对混合高斯模型进行参数学习的示例代码；11.用Nelder-mead算法进行非线性优化的示例代码；12.用最陡峭下降法进行非线性优化的示例代码；用共轭梯度法进行非线性优化的示例

动态窗口法（DWA）则是局部规划，考虑机器人的动力学约束，在速度空间采样，模拟短时间轨迹，选最优的一条。

% DWA 轨迹评价
for v = v_range
    for w = w_range
        traj = simulateMotion(current_state, v, w, dt);
        score = evaluateTrajectory(traj, goal, obstacles);
        if score > best_score
            best_v = v; best_w = w;
        end
    end
end

evaluateTrajectory 会考虑朝向目标、远离障碍物、速度大小等因素。

移动中难免要用传感器估计自身运动，迭代最近点（ICP）就是用来对齐两帧点云，计算相对位姿变化的。

for iter = 1:max_iter
    [correspondences, dists] = findNearestNeighbors(source, target);
    T = estimateRigidTransform(source, target, correspondences);
    source = transformPoints(source, T);
    if norm(T - previous_T) < threshold, break; end
end

它反复找最近点对应，再计算最优刚体变换，直到收敛。不过 ICP 容易陷局部最优，初值得好点。

把这些拼起来，就是 SLAM（同时定位与建图）了。基于特征点的 EKF SLAM 把地图特征点也放进状态向量里一起估计。

state_vector = [robot_pose; feature1; feature2; ...];
covariance_matrix = blkdiag(P_robot, P_features);

每当观测到特征，就更新整个状态和协方差。但状态维数会随特征数增长，计算量平方级上升，所以大规模环境得用稀疏方法或图优化。

最后提几个底层优化工具。比如 EM 算法学高斯混合模型（GMM）参数，这可以用来对传感器数据聚类。

% EM 的 M 步更新高斯参数
for k = 1:K
    resp = responsibilities(:,k); % 每个数据点属于第 k 个高斯的后验概率
    mu(k,:) = sum(data .* resp) / sum(resp);
    diff = data - mu(k,:);
    sigma(:,:,k) = (diff' .* resp) * diff / sum(resp);
end

还有非线性优化，像 Nelder-Mead（单纯形法），它不需求导，靠比较顶点函数值来移动单纯形。

% 排序顶点，计算重心
[values, order] = sort(f(simplex));
simplex = simplex(:,order);
centroid = mean(simplex(:,1:end-1), 2);

然后根据反射、扩展、收缩等规则更新最差点。而最陡下降法沿着负梯度方向走，简单但可能 zigzag。

while norm(gradient) > tol
    direction = -gradient;
    alpha = lineSearch(f, x, direction);
    x = x + alpha * direction;
    gradient = computeGradient(f, x);
end

共轭梯度法则更聪明，它构造的搜索方向是共轭的，通常收敛更快。

这些代码片段虽然简短，但基本反映了每个算法的核心思想。MATLAB 示例里通常有更完整的实现，包括数据生成、可视化、参数调节。真正用的时候，你得根据自己机器人的运动模型、传感器特性去调整，甚至混合几种方法。比如定位可以用 UKF 提供初值给粒子滤波，路径规划可以 A* 全局规划再 DWA 局部避障。多试试，调参数的过程虽然头疼，但也是玩机器人导航的乐趣之一吧。