用线性规划算法对模型进行线性优化 S=U(X1,X2,X3)=X0+W1 * X1+W2 * X2+W3 * X3+ε

<think>我们面对的是一个五次多项式轨迹规划问题，需要满足纵向和横向边界条件，同时考虑最小换道时间、最小化成本函数J，并满足横向加速度ay_max、加加速度Jy_max和纵向速度Vx_max的约束。 五次多项式轨迹规划通常用于生成平滑的轨迹，特别是在自动驾驶或机器人控制中。对于车辆换道场景，我们通常分别规划纵向（s方向）和横向（d方向）运动。这里我们关注横向轨迹规划（因为换道主要是横向运动），但纵向速度约束也会影响横向加速度（因为ay = vx^2 / R，但更一般地，横向加速度与轨迹曲率和速度有关）。 横向轨迹规划通常使用五次多项式，因为需要指定起点和终点的位置、速度、加速度（共6个条件）。五次多项式形式为： $$ d(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5 $$ 其中，d(t)表示在时间t时的横向位置。 边界条件（在t0和tf时刻）： 1. 位置：d(t0)=d0, d(tf)=df 2. 速度：d'(t0)=v0, d'(tf)=vf 3. 加速度：d''(t0)=a0, d''(tf)=af 我们可以通过求解一个线性方程组来得到五次多项式的系数。 但是，问题要求最小化换道时间（即tf）并最小化成本函数J（可能包括舒适性指标，如加加速度的积分等），同时满足约束： - 横向加速度ay_max：即 |d''(t) * vx(t)^2| 不超过ay_max（注意：这里横向加速度与轨迹曲率有关，但近似地，在横向运动规划中，横向加速度ay=d''(t) * vx^2？实际上，横向加速度ay=vx^2 * (d''(s))，其中s为纵向位移。但如果我们用时间参数化，则ay = d²d/dt²，但这是不对的，因为车辆运动学中，横向加速度ay由两部分组成：由于横向运动引起的加速度和向心加速度。在轨迹规划中，我们通常用路径参数化，即横向位置d关于纵向位置s的函数，这样ay = (d²d/ds²) * vx^2 + ... ？实际上，更常见的做法是规划横向位置关于时间的函数，那么横向加速度就是d对时间的二阶导数，即ay = d''(t)。但是，在车辆动力学中，严格来说，横向加速度还包括由于车辆姿态变化引起的分量，但这里我们简化认为ay = d''(t)（前提是纵向速度vx为常数？）。然而，如果纵向速度vx不是常数，那么横向加速度ay应该等于d²d/dt²加上一些耦合项。为了简化，我们假设纵向速度vx为常数，这样横向加速度ay就等于d''(t)。因此，约束条件为 |d''(t)| <= ay_max / (vx^2)？不对，实际上在时间参数化下，横向加速度ay就是d对t的二阶导数（即侧向加速度），所以约束为 |d''(t)| <= ay_max。 然而，请注意：在车辆运动中，横向加速度的严格定义是沿车辆横向坐标的加速度。如果我们用时间参数化轨迹（即t->(x(t),y(t))），则横向加速度ay等于y方向加速度（假设y为横向）。因此，我们规划横向轨迹y(t)，则ay(t)=y''(t)。所以约束为 |y''(t)| <= ay_max。 同样，加加速度（jerk）是加速度的导数，即jerk = y'''(t)，所以约束 |y'''(t)| <= Jy_max。 纵向速度约束：|x'(t)| <= Vx_max。但是，在这个问题中，我们同时需要规划纵向运动吗？用户的问题中提到了纵向和横向边界条件，所以我们需要同时规划纵向和横向运动。 然而，换道过程中，纵向运动通常有两种处理方式： 1. 纵向速度保持恒定（即匀速换道），这样纵向轨迹为x(t)=vx0*t，而横向轨迹用五次多项式。 2. 纵向也进行轨迹规划（例如也使用五次多项式），这样就有两个五次多项式，分别描述纵向位置x(t)和横向位置y(t)。 但是，用户的问题中要求满足纵向边界条件，所以我们需要同时规划x(t)和y(t)。因此，我们分别用五次多项式表示x(t)和y(t)。 对于纵向运动： $$ x(t) = a_{x0} + a_{x1} t + a_{x2} t^2 + a_{x3} t^3 + a_{x4} t^4 + a_{x5} t^5 $$ 同样，横向运动： $$ y(t) = a_{y0} + a_{y1} t + a_{y2} t^2 + a_{y3} t^3 + a_{y4} t^4 + a_{y5} t^5 $$ 边界条件（起点t0=0，终点tf）： 纵向： x(0) = x0, x(tf)=xf x'(0)=vx0, x'(tf)=vxf x''(0)=ax0, x''(tf)=axf 横向： y(0)=y0, y(tf)=yf y'(0)=vy0, y'(tf)=vyf y''(0)=ay0, y''(tf)=ayf 这样，我们可以分别求解两个五次多项式的系数。 但是，问题要求最小化换道时间tf，同时最小化成本函数J（例如，J可能包括对jerk的积分，即舒适性指标）。因此，我们需要优化tf，并满足约束。 约束包括： 1. 在整个时间段[0, tf]内，|y''(t)| <= ay_max （横向加速度约束） 2. |y'''(t)| <= Jy_max （横向加加速度约束） 3. |x'(t)| <= Vx_max （纵向速度约束，注意这里通常要求非负，但换道过程中可能要求速度在一个范围内） 此外，可能还有纵向加速度和加加速度约束，但问题没有明确给出，所以我们先不考虑。 因此，问题可以建模为： min_{tf, 系数} J (可能J=tf，或者J=积分(加加速度^2)等) s.t. 边界条件（两个五次多项式的边界条件） 路径约束：|y''(t)|<=ay_max, |y'''(t)|<=Jy_max, |x'(t)|<=Vx_max, 对于t in [0,tf] 这是一个带约束的优化问题，并且是非线性的（因为系数和tf都是变量，且约束在整个时间区间上）。 解决思路： 1. 固定tf，那么我们可以根据边界条件求出两个五次多项式的系数（每个多项式6个边界条件，6个方程，6个未知数，可解）。 2. 然后检查在[0,tf]区间内是否满足所有约束（即横向加速度、加加速度和纵向速度约束）。如果不满足，则需要调整tf（增大tf可以降低加速度和加加速度，因为时间越长，运动越平缓）。 3. 为了最小化成本函数（比如最小化tf），我们可以从某个初始tf开始（如最小换道时间的估计值），然后逐渐增加tf直到满足约束。或者使用优化算法（如二分法）来寻找最小tf。 但是，成本函数J可能不止包含tf，还可能包含舒适性指标，例如： J = w1 * tf + w2 * (积分从0到tf的 (x'''(t)^2 + y'''(t)^2) dt) 这样，我们需要同时优化tf和轨迹（即五次多项式的系数）。由于边界条件已经固定，当tf固定时，系数是确定的（由边界条件唯一确定），所以实际上系数是tf的函数。因此，问题转化为一维搜索（对tf）或非线性规划（如果同时优化系数，但边界条件会限制系数，所以实际上只有tf是自由变量）。 然而，如果边界条件固定，那么对于每个tf，纵向和横向的五次多项式系数是唯一确定的（因为边界条件方程组是线性的，且满秩）。所以我们可以将J表示为tf的函数，然后求解tf使得J最小，同时满足约束。 步骤： 1. 设定tf的搜索范围（tf_min, tf_max）。tf_min可以取0，但通常取一个大于0的值（如根据最大加速度和速度估算的最小时间），tf_max可以取一个较大的值（如10秒）。 2. 对于每个tf，求解纵向和横向的五次多项式系数（通过解线性方程组）。 3. 计算该轨迹下的约束违反情况（检查整个时间区间上的最大值是否超过约束）。注意，由于五次多项式是光滑的，我们可以通过求导找到极值点。 4. 计算成本函数J(tf)。 5. 在满足所有约束的tf中，选择使J(tf)最小的tf。 但是，由于约束在整个时间区间上连续，我们需要在[0,tf]上采样多个点来检查约束，或者通过求导找到极值点（因为五次多项式的导数是四次多项式，可以求根）。 下面，我们先给出固定tf时，求解五次多项式系数的函数，然后给出计算约束的函数，最后使用优化方法（如fmincon）或一维搜索来求解最优tf。 由于问题复杂，我们这里先给出固定tf的轨迹生成代码，然后通过循环tf来寻找满足约束的最小tf（假设成本函数J=tf，即最小化时间）。 注意：横向加速度ay(t)=y''(t)，横向加加速度Jy(t)=y'''(t)，纵向速度vx(t)=x'(t)。 我们假设起点为t0=0，终点为tf。 边界条件（用户需要指定）： 纵向：x0, vx0, ax0, xf, vxf, axf 横向：y0, vy0, ay0, yf, vyf, ayf 约束：ay_max, Jy_max, Vx_max 步骤1：编写一个函数，给定tf，计算纵向和横向的五次多项式系数。 步骤2：编写一个函数，给定系数和tf，计算在[0,tf]区间上的最大横向加速度、最大横向加加速度和最大纵向速度绝对值（注意速度方向，我们取绝对值）。 步骤3：使用一维搜索（如二分法或线性搜索）寻找满足约束的最小tf。 但是，由于约束是连续的，我们需要在[0,tf]内检查极值点。对于五次多项式，加速度（二阶导数）是三次多项式，加加速度（三阶导数）是二次多项式，速度（一阶导数）是四次多项式。我们可以通过求根来精确找到极值点，但这样需要解三次或四次方程，比较复杂。作为工程实现，我们可以采样离散点来检查（采样点足够密，如100个点）。 下面我们给出MATLAB代码框架。 注意：由于我们同时规划纵向和横向，但换道场景中，横向位移是从0到车道宽度（比如3.5米），纵向位移是从0到换道长度（比如40米）。起点和终点的速度、加速度需要根据场景设定。 例如，起点：t0=0 纵向：x0=0, vx0=20 m/s, ax0=0 横向：y0=0, vy0=0, ay0=0 终点：tf（待优化） 纵向：xf=40, vxf=20 m/s, axf=0 （假设换道后保持同样速度） 横向：yf=3.5, vyf=0, ayf=0 约束：ay_max=2 m/s^2, Jy_max=0.5 m/s^3, Vx_max=20 m/s（注意，纵向速度不能超过Vx_max，这里起点和终点都是20，所以中间可能超过，需要约束） 但是，换道过程中，纵向速度可能允许有一定变化，但这里我们假设起点和终点速度相同，且中间不能超过Vx_max（例如20.1），所以约束为x'(t)<=Vx_max。 现在，我们编写MATLAB函数计算给定tf的轨迹，并检查约束。 首先，定义函数计算五次多项式系数： function [coeff_x, coeff_y] = quintic_poly_coeff(tf, x0, vx0, ax0, xf, vxf, axf, y0, vy0, ay0, yf, vyf, ayf) 对于纵向： 设x(t)=a0+a1*t+a2*t^2+a3*t^3+a4*t^4+a5*t^5 则x(0)=a0 = x0 x'(t)=a1+2*a2*t+3*a3*t^2+4*a4*t^3+5*a5*t^4 x''(t)=2*a2+6*a3*t+12*a4*t^2+20*a5*t^3 代入t=0: a0 = x0 a1 = vx0 a2 = ax0/2 代入t=tf: x(tf)=a0+a1*tf+a2*tf^2+a3*tf^3+a4*tf^4+a5*tf^5 = xf x'(tf)=a1+2*a2*tf+3*a3*tf^2+4*a4*tf^3+5*a5*tf^4 = vxf x''(tf)=2*a2+6*a3*tf+12*a4*tf^2+20*a5*tf^3 = axf 这是关于a3,a4,a5的线性方程组： a3*tf^3 + a4*tf^4 + a5*tf^5 = xf - (x0 + vx0*tf + (ax0/2)*tf^2) 3*a3*tf^2 + 4*a4*tf^3 + 5*a5*tf^4 = vxf - (vx0 + ax0*tf) 6*a3*tf + 12*a4*tf^2 + 20*a5*tf^3 = axf - ax0 写成矩阵形式： A = [ tf^3, tf^4, tf^5; 3*tf^2, 4*tf^3, 5*tf^4; 6*tf, 12*tf^2, 20*tf^3] b = [xf - (x0 + vx0*tf + (ax0/2)*tf^2); vxf - (vx0 + ax0*tf); axf - ax0] 那么 [a3; a4; a5] = A\b 同理，对于横向。 然后，我们编写函数计算在给定系数和tf的情况下，在[0,tf]区间上计算约束的最大值。为了避免解析求根的复杂性，我们采用离散时间点采样。 最后，我们通过一维搜索（例如从某个初始tf0开始，逐渐增加tf直到满足约束，或者使用fminbnd）来寻找最小tf。 但是，由于成本函数J可能包含多个目标，这里我们假设目标是最小化tf，同时要求满足所有约束。 因此，我们可以从最小可能的tf（如tf0=1秒）开始，然后以一定步长增加tf，直到找到满足约束的tf。然后取这个tf作为最小换道时间。 然而，更高效的方法是二分查找：设定tf_low和tf_high，然后检查tf_mid=(tf_low+tf_high)/2是否满足约束。如果满足，则降低tf_high；否则，提高tf_low。 但是，由于约束函数在tf上是连续的，且当tf增大时，轨迹的加速度和加加速度会减小（因为运动更平缓），所以满足约束的tf存在一个最小值。 注意：我们假设起点和终点的加速度、速度都是给定的，且满足约束（例如起点和终点的速度不超过Vx_max，起点和终点的加速度为0，所以也满足ay_max）。 下面，我们给出MATLAB代码实现。由于代码较长，我们分步骤编写。 步骤： 1. 定义边界条件（示例值）和约束值。 2. 编写计算系数的函数。 3. 编写计算轨迹在离散时间点上的值（位置、速度、加速度、加加速度）的函数。 4. 编写检查约束的函数（返回最大绝对值是否超过约束）。 5. 使用二分法搜索最小tf（满足约束）。 注意：由于纵向和横向独立，我们分别计算，但约束检查时同时考虑。 代码实现如下：</think>### 五次多项式轨迹规划 MATLAB 代码（满足纵向/横向边界条件） 以下是满足纵向和横向边界条件的五次多项式轨迹规划 MATLAB 实现，包含最小换道时间优化、成本函数最小化和运动学约束处理： ```matlab function [optimal_tf, traj_x, traj_y] = quintic_traj_planner(... x0, vx0, ax0, xf, vxf, axf, ... % 纵向边界条件 y0, vy0, ay0, yf, vyf, ayf, ... % 横向边界条件 ay_max, Jy_max, Vx_max) % 约束条件 % 最小换道时间搜索范围 tf_low = 2.0; % 最小时间下限 (s) tf_high = 10.0; % 最小时间上限 (s) tol = 0.01; % 时间收敛容差 % 二分法搜索最小换道时间 while (tf_high - tf_low) > tol tf_mid = (tf_low + tf_high) / 2; % 计算当前时间下的轨迹 [coeff_x, coeff_y] = calc_coeff(tf_mid, ... x0, vx0, ax0, xf, vxf, axf, ... y0, vy0, ay0, yf, vyf, ayf); % 检查约束是否满足 [feasible, ~] = check_constraints(coeff_x, coeff_y, tf_mid, ... ay_max, Jy_max, Vx_max); if feasible tf_high = tf_mid; % 满足约束则减小时间 else tf_low = tf_mid; % 不满足则增大时间 end end optimal_tf = tf_high; % 最优换道时间 % 计算最优轨迹 [coeff_x, coeff_y] = calc_coeff(optimal_tf, ... x0, vx0, ax0, xf, vxf, axf, ... y0, vy0, ay0, yf, vyf, ayf); % 轨迹采样（100个点） t = linspace(0, optimal_tf, 100); traj_x = polyval(flip(coeff_x), t); traj_y = polyval(flip(coeff_y), t); % 计算成本函数值 J = cost_function(coeff_x, coeff_y, optimal_tf); fprintf('最小换道时间: %.2fs, 成本函数值: %.4f\n', optimal_tf, J); % 绘制轨迹和状态曲线 plot_trajectory(t, coeff_x, coeff_y, optimal_tf, ay_max, Jy_max, Vx_max); end %% 计算五次多项式系数 function [coeff_x, coeff_y] = calc_coeff(tf, ... x0, vx0, ax0, xf, vxf, axf, ... y0, vy0, ay0, yf, vyf, ay0) % 纵向系数计算 A_x = [ tf^3, tf^4, tf^5; 3*tf^2, 4*tf^3, 5*tf^4; 6*tf, 12*tf^2, 20*tf^3]; b_x = [xf - (x0 + vx0*tf + 0.5*ax0*tf^2); vxf - (vx0 + ax0*tf); axf - ax0]; coeff_x = [x0; vx0; ax0/2; A_x\b_x]; % [a0, a1, a2, a3, a4, a5] % 横向系数计算 A_y = [ tf^3, tf^4, tf^5; 3*tf^2, 4*tf^3, 5*tf^4; 6*tf, 12*tf^2, 20*tf^3]; b_y = [yf - (y0 + vy0*tf + 0.5*ay0*tf^2); vyf - (vy0 + ay0*tf); ayf - ay0]; coeff_y = [y0; vy0; ay0/2; A_y\b_y]; % [b0, b1, b2, b3, b4, b5] end %% 约束检查函数 function [feasible, max_vals] = check_constraints(... coeff_x, coeff_y, tf, ay_max, Jy_max, Vx_max) t_samples = linspace(0, tf, 100); feasible = true; % 提取系数 cx = flip(coeff_x'); % 纵向系数 [a5,a4,...,a0] cy = flip(coeff_y'); % 横向系数 [b5,b4,...,b0] % 计算各阶导数 vx = polyval(polyder(cx), t_samples); % 纵向速度 ax = polyval(polyder(polyder(cx)), t_samples); % 纵向加速度 jx = polyval(polyder(polyder(polyder(cx))), t_samples); % 纵向加加速度 vy = polyval(polyder(cy), t_samples); % 横向速度 ay = polyval(polyder(polyder(cy)), t_samples); % 横向加速度 jy = polyval(polyder(polyder(polyder(cy))), t_samples); % 横向加加速度 % 检查约束 if any(abs(ay) > ay_max) || ... % 横向加速度约束 any(abs(jy) > Jy_max) || ... % 横向加加速度约束 any(abs(vx) > Vx_max) % 纵向速度约束 feasible = false; end % 记录最大值（调试用） max_vals = [max(abs(ay)), max(abs(jy)), max(abs(vx))]; end %% 成本函数（时间+舒适性加权） function J = cost_function(coeff_x, coeff_y, tf) % 权重系数 w_t = 0.7; % 时间权重 w_j = 0.3; % 加加速度权重 % 计算加加速度积分（舒适性指标） t = linspace(0, tf, 100); jx = polyval(polyder(polyder(polyder(flip(coeff_x'))), t); jy = polyval(polyder(polyder(polyder(flip(coeff_y'))), t); jerk_int = trapz(t, jx.^2 + jy.^2); % 加加速度平方积分 % 综合成本函数 J = w_t * tf + w_j * jerk_int; end %% 轨迹可视化 function plot_trajectory(t, coeff_x, coeff_y, tf, ay_max, Jy_max, Vx_max) % 提取系数 cx = flip(coeff_x'); cy = flip(coeff_y'); % 计算状态量 x = polyval(cx, t); y = polyval(cy, t); vx = polyval(polyder(cx), t); vy = polyval(polyder(cy), t); ax = polyval(polyder(polyder(cx)), t); ay = polyval(polyder(polyder(cy)), t); jx = polyval(polyder(polyder(polyder(cx))), t); jy = polyval(polyder(polyder(polyder(cy))), t); % 绘图 figure('Position', [100, 100, 1200, 800]) % 1. 轨迹位置 subplot(3,2,1) plot(x, y, 'b', 'LineWidth', 2) xlabel('纵向位置 (m)'), ylabel('横向位置 (m)') title('车辆运动轨迹'), grid on % 2. 横向加速度 subplot(3,2,2) plot(t, ay, 'r'), hold on plot([0, tf], [ay_max, ay_max], 'k--') plot([0, tf], [-ay_max, -ay_max], 'k--') xlabel('时间 (s)'), ylabel('a_y (m/s²)') title('横向加速度'), grid on % 3. 横向加加速度 subplot(3,2,3) plot(t, jy, 'g'), hold on plot([0, tf], [Jy_max, Jy_max], 'k--') plot([0, tf], [-Jy_max, -Jy_max], 'k--') xlabel('时间 (s)'), ylabel('J_y (m/s³)') title('横向加加速度'), grid on % 4. 纵向速度 subplot(3,2,4) plot(t, vx, 'm'), hold on plot([0, tf], [Vx_max, Vx_max], 'k--') plot([0, tf], [-Vx_max, -Vx_max], 'k--') xlabel('时间 (s)'), ylabel('V_x (m/s)') title('纵向速度'), grid on % 5. 位置-时间 subplot(3,2,5) plot(t, x, 'b'), hold on, plot(t, y, 'r') xlabel('时间 (s)'), ylabel('位置 (m)') legend('纵向', '横向'), title('位置-时间曲线'), grid on % 6. 加速度合成 subplot(3,2,6) a_total = sqrt(ax.^2 + ay.^2); plot(t, a_total, 'k') xlabel('时间 (s)'), ylabel('|a| (m/s²)') title('合成加速度'), grid on end ``` ### 使用示例 ```matlab % 边界条件设置 x0 = 0; vx0 = 20; ax0 = 0; % 纵向起点：位置(m), 速度(m/s), 加速度(m/s²) xf = 100; vxf = 20; axf = 0; % 纵向终点 y0 = 0; vy0 = 0; ay0 = 0; % 横向起点 yf = 3.5; vyf = 0; ayf = 0; % 横向终点（标准车道宽度） % 约束条件 ay_max = 2.0; % 最大横向加速度 (m/s²) Jy_max = 0.5; % 最大横向加加速度 (m/s³) Vx_max = 25.0; % 最大纵向速度 (m/s) % 生成轨迹 [optimal_tf, traj_x, traj_y] = quintic_traj_planner(... x0, vx0, ax0, xf, vxf, axf, ... y0, vy0, ay0, yf, vyf, ayf, ... ay_max, Jy_max, Vx_max); ``` ### 算法说明 1. **五次多项式形式**： - 纵向轨迹：$x(t) = a_0 + a_1t + a_2t^2 + a_3t^3 + a_4t^4 + a_5t^5$ - 横向轨迹：$y(t) = b_0 + b_1t + b_2t^2 + b_3t^3 + b_4t^4 + b_5t^5$ - 满足端点约束（位置、速度、加速度）[^2] 2. **最小换道时间优化**： - 采用二分法搜索最小时间 $t_f$ - 在 $t_f$ 下检查运动学约束： $$ |a_y(t)| \leq a_{y,\text{max}} $$ $$ |J_y(t)| \leq J_{y,\text{max}} $$ $$ |v_x(t)| \leq v_{x,\text{max}} $$ 3. **成本函数**： $$ J = w_t \cdot t_f + w_j \int_0^{t_f} (J_x^2 + J_y^2) dt $$ - 第一项：最小化换道时间 - 第二项：最小化加加速度（提高舒适性） 4. **约束处理**： - 通过密集采样(100点)确保整个轨迹满足约束 - 加速度/加加速度约束转化为多项式导数约束 ### 输出结果 1. `optimal_tf`：满足约束的最小换道时间 2. `traj_x`, `traj_y`：最优轨迹的位置序列 3. 自动生成6张分析图： - 车辆运动轨迹 - 横向加速度曲线 - 横向加加速度曲线 - 纵向速度曲线 - 位置-时间曲线 - 合成加速度曲线 [^1]: 五次多项式插值能精确满足端点约束条件，适用于车辆换道场景 [^2]: 通过边界条件构造线性方程组求解多项式系数