最优控制与轨迹规划：从理论到MATLAB实践

1. 最优控制与轨迹规划概述

最优控制和轨迹规划是现代控制理论和机器人学中的核心课题，广泛应用于自动驾驶、机器人导航、航空航天等领域。简单来说，最优控制研究的是如何在满足系统动力学约束的前提下，找到使某个性能指标最优的控制策略；而轨迹规划则专注于生成一条从起点到终点的最优路径，同时避开障碍物并满足运动学约束。

在实际工程中，这两个问题往往密不可分。以自动驾驶汽车为例，它既需要规划一条从A点到B点的最优路径（轨迹规划），又需要在行驶过程中实时调整方向盘和油门刹车（最优控制），以确保车辆能够准确跟踪规划的路径，同时保证乘坐舒适性和安全性。

2. 最速降线问题求解

2.1 问题背景与数学模型

最速降线问题(Brachistochrone problem)是历史上著名的数学问题，由约翰·伯努利在1696年提出。问题的核心是：在重力场中，给定两点A和B，求出一条连接这两点的曲线，使得一个质点仅在重力作用下从A滑到B所需的时间最短。

从数学角度看，这是一个典型的变分问题。设曲线的方程为y(x)，则下滑时间T可以表示为：

T = ∫√(1+y'²)/√(2gy) dx

其中g是重力加速度，y'=dy/dx。我们的目标是通过选择适当的y(x)来最小化T。

2.2 数值求解方法

虽然这个问题有解析解（摆线），但为了展示数值方法，我们采用离散化+优化的思路来求解：

1. 离散化处理：将路径分成N段，每段用直线近似
2. 变量定义：中间点的y坐标作为优化变量
3. 目标函数：计算总下滑时间
4. 约束条件：起点和终点固定
5. 优化求解：使用拟牛顿法寻找最优解

2.3 MATLAB实现详解

matlab复制function [xs, ys] = Brachistochrone_curve(xy0, xyf, N)
    % 参数设置
    if nargin < 3
        N = 200; % 默认分段数
    end
    if nargin < 2
        xyf = [10, 10]; % 默认终点
    end
    if nargin < 1
        xy0 = [0, 0]; % 默认起点
    end
    
    % 初始猜测：直线连接
    x = linspace(xy0(1), xyf(1), N+2)';
    y = linspace(xy0(2), xyf(2), N+2)';
    y0 = y(2:end-1); % 优化变量是中间点的y坐标
    
    % 优化选项设置
    options = optimoptions('fminunc', 'Algorithm', 'quasi-newton',...
        'Display', 'iter', 'MaxIterations', 1000);
    
    % 调用优化器
    [y_opt, ~] = fminunc(@(y) CostTime(xy0, xyf, x, y), y0, options);
    
    % 整理结果
    ys = [xy0(2); y_opt; xyf(2)];
    xs = x;
    
    % 绘制结果
    figure;
    plot(xs, ys, 'r*'); hold on;
    
    % 计算并绘制解析解（摆线）
    theta = linspace(0, pi, 100);
    R = (xyf(2)-xy0(2))/(1-cos(theta(end)));
    x_cycloid = R*(theta-sin(theta)) + xy0(1);
    y_cycloid = -R*(1-cos(theta)) + xy0(2) + R;
    plot(x_cycloid, y_cycloid, 'b-');
    
    legend('数值解', '解析解(摆线)');
    xlabel('x'); ylabel('y');
    title('最速降线问题求解结果');
end

function T = CostTime(xy0, xyf, x, y)
    % 计算总下滑时间
    y_full = [xy0(2); y; xyf(2)];
    T = 0;
    g = 9.8; % 重力加速度
    
    for i = 1:length(x)-1
        dx = x(i+1) - x(i);
        dy = y_full(i+1) - y_full(i);
        segment_length = sqrt(dx^2 + dy^2);
        avg_height = (y_full(i) + y_full(i+1))/2;
        if avg_height <= 0
            T = inf; % 无效解
            return;
        end
        velocity = sqrt(2*g*avg_height);
        T = T + segment_length/velocity;
    end
end

2.4 关键点解析

1. 离散化精度：分段数N直接影响求解精度，一般需要200以上才能获得较好结果
2. 初始猜测：直线作为初始猜测虽然简单，但可能导致优化陷入局部最优