MPC算法路径跟踪_Matlab实现

在机器人控制领域，模型预测控制（MPC）因其能够处理动态约束和多目标优化的特性，成为路径跟踪的热门方案。近期，我在 GitHub 上发现了 @Mr.Winter` 的MPC路径规划项目，其代码实现简洁且功能完整。本文将结合理论分析与代码解读，分享我的学习心得。
该项目实现了差分驱动机器人的 MPC 路径跟踪，主要功能包括：
动态前瞻点选择
角度优先的分层控制策略
约束条件下的二次规划优化
特别感谢作者Winter的开源贡献！

代码

function [pose, traj, flag] = mpc_plan(start, goal, varargin)% @file: mpc_plan.m
% @breif: Model Predicted Control (MPC) motion planning
% @author: Winter
% @update: 2023.2.7p = inputParser;           addParameter(p, 'path', "none");addParameter(p, 'map', "none");parse(p, varargin{:});if isstring(p.Results.path) || isstring(p.Results.map)exception = MException('MyErr:InvalidInput', 'parameter `path` or `map` must be set.');throw(exception);end% pathpath = flipud(p.Results.path);path = path_interpolation(path, 5);% initial robotic staterobot.x = start(1);robot.y = start(2);robot.theta = start(3);robot.v = 0;robot.w = 0;% common parametersparam.dt = 0.1;%每次控制更新的时间间隔param.max_iteration = 2000;%最大迭代次数param.goal_dist_tol = 1.0;%目标距离容差，当车辆与目标点的距离小于这个容差时，认为车辆已经到达目标点param.rotate_tol = 0.5;%旋转容差，当车辆的朝向与目标朝向的偏差小于这个容差时，认为车辆的朝向已经达到要求。param.lookahead_time = 1.0;%控制器向前预测的时间范围，用于确定前瞻距离param.min_lookahead_dist = 1.0;%最小前瞻距离param.max_lookahead_dist = 2.5;%最大前瞻距离param.max_v_inc = 0.5;%最大线速度增量param.max_v = 1.0;param.min_v = 0.0;param.max_w_inc = pi / 2;%最大角速度增量param.max_w = pi / 2;param.min_w = 0.0;% MPC parametersparam.Q = diag([1, 1, 1]);%状态权重矩阵param.R = diag([2, 2]);%控制输入权重矩阵param.p = 12;%预测时域长度。表示 MPC 算法向前预测的步数param.m = 8;%控制时域长度。表示 MPC 算法在每个控制周期内优化的控制输入步数param.u_min = [param.min_v; param.min_w];param.u_max = [param.max_v; param.max_w];param.du_min = [param.min_v; -param.max_w_inc];param.du_max = [param.max_v_inc; param.max_w_inc];% return valueflag = false;pose = [];traj = [];%轨迹% main loopiter = 0;u_p = [0, 0];%先前的控制误差while iter < param.max_iterationiter = iter + 1;% break until goal reachedif shouldRotateToGoal([robot.x, robot.y], goal, param)flag = true;break;end% get the particular point on the path at the lookahead distance%获取路径上前瞻距离处的特定点[lookahead_pt, theta_trj, kappa] = getLookaheadPoint(robot, path, param);% calculate velocity commande_theta = regularizeAngle(robot.theta - goal(3)) / 10;if shouldRotateToGoal([robot.x, robot.y], goal, param)%是否需要旋转到目标点if ~shouldRotateToPath(abs(e_theta), 0.0, param)u = [0, 0];elseu = [0, angularRegularization(robot, e_theta / param.dt, param)];%将线速度设为 0，角速度经过规范化处理后赋值给 u，表示机器人只进行旋转运动endelsee_theta = regularizeAngle( ...atan2(real(lookahead_pt(2)) - robot.y, real(lookahead_pt(1)) - robot.x) - robot.theta ...) / 10;%从机器人当前位置到前瞻点的方向角度，减去机器人当前的朝向角度 robot.theta，并进行角度规范化和除以 10 的操作if shouldRotateToPath(abs(e_theta), pi / 4, param)u = [0, angularRegularization(robot, e_theta / param.dt, param)];else%不需要旋转到路径方向，使用模型预测控制（MPC）计算控制指令% current states = [robot.x, robot.y, robot.theta];% desired states_d = [real(lookahead_pt), theta_trj];% refered input 参考输入u_r = [robot.v, theta_trj - robot.theta];% control[u, u_p] = mpcControl(s, s_d, u_r, u_p, robot, param);endend% input into robotic kinematicrobot = f(robot, u, param.dt);pose = [pose; robot.x, robot.y, robot.theta];end
end%%
function robot = f(robot, u, dt)% robotic kinematicF = [ 1 0 0 0 00 1 0 0 00 0 1 0 00 0 0 0 00 0 0 0 0];B = [dt * cos(robot.theta) 0dt * sin(robot.theta)  00                                dt1                                 00                                 1];x = [robot.x; robot.y; robot.theta; robot.v; robot.w];x_star = F * x + B * u';robot.x = x_star(1); robot.y = x_star(2); robot.theta = x_star(3);robot.v = x_star(4); robot.w = x_star(5);
endfunction theta = regularizeAngle(angle)theta = angle - 2.0 * pi * floor((angle + pi) / (2.0 * pi));
end
%比较机器人当前位置与目标位置的欧几里得距离和目标距离容差，来判断机器人是否需要执行旋转操作以到达目标位姿。如果距离小于容差，返回 true
function flag = shouldRotateToGoal(cur, goal, param)%{Whether to reach the target pose through rotation operationParameters----------cur: tuplecurrent pose of robotgoal: tuplegoal pose of robotReturn----------flag: booltrue if robot should perform rotation%}flag = hypot(cur(1) - goal(1), cur(2) - goal(2)) < param.goal_dist_tol;
end
%判断机器人是否需要通过旋转操作来纠正其跟踪路径，即判断机器人当前的朝向与路径期望朝向之间的角度偏差是否超出了可容忍的范围
function flag = shouldRotateToPath(angle_to_path, tol, param)%{Whether to correct the tracking path with rotation operationParameters----------angle_to_path: float the angle deviationtol: float[None]the angle deviation tolerenceReturn----------flag: booltrue if robot should perform rotation%}if tol == 0.0flag = angle_to_path > param.rotate_tol;elseflag = angle_to_path > tol;end
end
%对机器人的角速度进行正则化处理，确保角速度及其变化量在设定的允许范围内
function w = angularRegularization(robot, w_d, param)%{Angular velocity regularizationParameters----------w_d: floatreference angular velocity inputReturn----------w: floatcontrol angular velocity output%}w_inc = w_d - robot.w;if abs(w_inc) > param.max_w_incw_inc =param.max_w_inc * sign(w_inc);endw = robot.w + w_inc;if abs(w) > param.max_ww = param.max_w * sign(w);endif abs(w) < param.min_ww = param.min_w * sign(w)  ;end
endfunction v = linearRegularization(robot, v_d, param)%{Linear velocity regularizationParameters----------v_d: floatreference velocity inputReturn----------v: floatcontrol velocity output%}v_inc = v_d - robot.v;if abs(v_inc) > param.max_v_incv_inc = param.max_v_inc * sign(v_inc);endv = robot.v + v_inc;if abs(v) > param.max_vv = param.max_v * sign(v);endif abs(v) < param.min_vv = param.min_v * sign(v);end
end
%计算机器人的前瞻距离
function d = getLookaheadDistance(robot, param)d = robot.v * param.lookahead_time;if d < param.min_lookahead_distd = param.min_lookahead_dist;endif d > param.max_lookahead_distd = param.max_lookahead_dist;end
endfunction [pt, theta, kappa] = getLookaheadPoint(robot, path, param)%{Find the point on the path that is exactly the lookahead distance away from the robotReturn----------lookahead_pt: tuplelookahead pointtheta: floatthe angle on trajectorykappa: floatthe curvature on trajectory%}% Find the first pose which is at a distance greater than the lookahead distance、%找到第一个距离机器人大于等于前瞻距离的点dist_to_robot = [];[pts_num, ~] = size(path);% 使用 size 函数获取路径 `path` 的大小，`pts_num` 表示路径上点的数量，`~` 表示忽略路径点的维度信息（因为这里只关心点的数量）for i=1:pts_num% 使用 hypot 函数计算路径上第 `i` 个点到机器人当前位置的欧几里得距离dist_to_robot(end + 1) = hypot(path(i, 1) - robot.x, path(i, 2) - robot.y);end%%先找到路径上距离机器人最近的点，然后根据前瞻距离，从该最近点开始向后搜索，找到第一个距离机器人大于等于前瞻距离的点。[~, idx_closest] = min(dist_to_robot);idx_goal = pts_num - 1; idx_prev = idx_goal - 1;%初始化目标点和前一个点的索引lookahead_dist = getLookaheadDistance(robot, param);for i=idx_closest:pts_numif hypot(path(i, 1) - robot.x, path(i, 2) - robot.y) >= lookahead_distidx_goal = i;break;endendif idx_goal == pts_num - 1% If the no pose is not far enough, take the last pose%之前的搜索中没有找到距离机器人大于等于前瞻距离的点，可能路径上所有点距离机器人都不够远 将路径上倒数第二个点作为前瞻点pt = [path(idx_goal, 1), path(idx_goal, 2)];elseif idx_goal == 1idx_goal = idx_goal + 1;end% find the point on the line segment between the two poses% that is exactly the lookahead distance away from the robot pose (the origin)% This can be found with a closed form for the intersection of a segment and a circle%找到路径上距离机器人当前位置恰好为前瞻距离（lookahead_dist）的点，并计算该点所在路径线段的切线角度idx_prev = idx_goal - 1;px = path(idx_prev, 1); py = path(idx_prev, 2);gx = path(idx_goal, 1); gy = path(idx_goal, 2);% transform to the robot frame so that the circle centers at (0,0)%将线段的两个端点坐标从全局坐标系转换到以机器人位置为原点的局部坐标系prev_p = [px - robot.x, py - robot.y];goal_p = [gx - robot.x, gy - robot.y];i_points = circleSegmentIntersection(prev_p, goal_p, lookahead_dist);%计算线段与圆的交点pt = [i_points(1, 1) + robot.x, i_points(1, 2) + robot.y];%全局坐标系下的前瞻点坐标end% calculate the angle on trajectory%计算路径上 idx_prev 到 idx_goal 这两点所构成线段的切线角度theta = atan2(path(idx_goal, 2) - path(idx_prev, 2), path(idx_goal, 1) - path(idx_prev, 1));% calculate the curvature on trajectory 曲率if idx_goal == 2idx_goal = idx_goal + 1;endidx_prev = idx_goal - 1;idx_pprev = idx_prev - 1;%计算由这三个相邻点构成的三角形的三条边长a = hypot(path(idx_prev, 1) - path(idx_goal, 1), path(idx_prev, 2) - path(idx_goal, 2));b = hypot(path(idx_pprev, 1) - path(idx_goal, 1), path(idx_pprev, 2) - path(idx_goal, 2));c = hypot(path(idx_pprev, 1) - path(idx_prev, 1), path(idx_pprev, 2) - path(idx_prev, 2));cosB = (a * a + c * c - b * b) / (2 * a * c);sinB = sin(acos(cosB));%计算向量叉积来确定曲线的弯曲方向。叉积的正负可以表示曲线是向左弯曲还是向右弯曲cross = (path(idx_prev, 1) - path(idx_pprev, 1)) * ...(path(idx_goal, 2) - path(idx_pprev, 2)) - ...(path(idx_prev, 2) - path(idx_pprev, 2)) * ...(path(idx_goal, 1) - path(idx_pprev, 1));kappa = 2 * sinB / b *sign(cross); %曲率kappa= b/2sinB,sign(cross) 是为了给曲率加上方向信息，正曲率表示曲线向左弯曲，负曲率表示曲线向右弯曲。​
endfunction i_points = circleSegmentIntersection(p1, p2, r)x1 = p1(1); x2 = p2(1);y1 = p1(2); y2 = p2(2);dx = x2 - x1; dy = y2 - y1;dr2 = dx * dx + dy * dy;%线段长度的平方D = x1 * y2 - x2 * y1;% the first element is the point within segmentd1 = x1 * x1 + y1 * y1;d2 = x2 * x2 + y2 * y2;dd = d2 - d1;delta = sqrt(r * r * dr2 - D * D);%delta 的值决定了线段与圆的相交情况if delta >= 0if delta == 0%线段与圆相切i_points = [D * dy / dr2, -D * dx / dr2];else%线段与圆有两个交点i_points = [(D * dy + sign(dd) * dx * delta) / dr2, ...(-D * dx + sign(dd) * dy * delta) / dr2; ...(D * dy - sign(dd) * dx * delta) / dr2, ...(-D * dx - sign(dd) * dy * delta) / dr2];%sign(dd) 函数用于返回 dd 的符号（正为 1，负为 -1），用于确定交点的具体位置。endelsei_points = [];end
endfunction path_new = path_interpolation(path, n)for i=1:npath_new = path;path_inter = path_new(1:end - 1, :) + diff(path_new) / 2;path = zeros(length(path_new) + length(path_inter), 2);path(1:2:end, :) = path_new;path(2:2:end, :) = path_inter;end
endfunction [u, u_p_new] = mpcControl(s, s_d, u_r, u_p, robot, param)%{Execute MPC control process.Parameters----------s: tuplecurrent states_d: tupledesired stateu_r: tuplerefered controlu_p: tupleprevious control errorReturn----------u: np.ndarraycontrol vector%}dim_u = 2; dim_x = 3;dt = param.dt;% state vector (5 x 1)x = [s - s_d, u_p]';% original state matrixA = eye(3);A(1, 3) = -u_r(1) * sin(s_d(3)) * dt;A(2, 3) = u_r(1) * cos(s_d(3)) * dt;% original control matrixB = zeros(3, 2);B(1, 1) = cos(s_d(3)) * dt;B(2, 1) = sin(s_d(3)) * dt;B(3, 2) = dt;% discrete iteration Ricatti equationP = param.Q;% state matrix (5 x 5)A = [A, B; zeros(dim_u, dim_x), eye(dim_u)];% control matrix (5 x 2)B = [B; eye(dim_u)];% output matrix (3 x 5)C = [eye(dim_x), zeros(dim_x, dim_u)];% mpc state matrix (3p x 5)S_x_cell = cell(param.p, 1);%预测时域长度。表示 MPC 算法向前预测的步数for i=1:param.pS_x_cell{i, 1} = C * A ^ i;endS_x = cell2mat(S_x_cell);% mpc control matrix (3p x 2m)S_u_cell = cell(param.p, param.m);for i = 1:param.pfor j = 1:param.mif j <= iS_u_cell{i, j} = C * A ^ (i - j) * B;elseS_u_cell{i, j} = zeros(dim_x, dim_u);endendendS_u = cell2mat(S_u_cell);% optimizationYr = zeros(3 * param.p, 1);                  % (3p x 1)Q = kron(eye(param.p), param.Q);    % (3p x 3p)R = kron(eye(param.m), param.R);    % (2m x 2m)H = S_u' * Q * S_u + R;                        % (2m x 2m)g = S_u' * Q * (S_x * x - Yr);                 % (2m x 1)% constriantsA_I = kron(tril(ones(param.m, param.m)), diag([1, 1]));U_min = kron(ones(param.m, 1), param.u_min);U_max = kron(ones(param.m, 1), param.u_max);U_k_1 = kron(ones(param.m, 1), u_p');% boundarydU_min = kron(ones(param.m, 1), param.du_min);dU_max = kron(ones(param.m, 1), param.du_max);% solveoptions = optimoptions('quadprog', 'MaxIterations', 100, 'TolFun', 1e-16, 'Display','off');dU_opt = quadprog(H, g, [-A_I; A_I], [-U_min + U_k_1; U_max - U_k_1], [], [], dU_min, dU_max, [], options);% first elementdu = [dU_opt(1), dU_opt(2)];% real controlu = du + u_p + u_r;u = [linearRegularization(robot, u(1), param), angularRegularization(robot, u(2), param)];u_p_new = u - u_r;
end

这段代码是一个基于模型预测控制（MPC）的路径跟踪算法，用于控制机器人沿给定路径移动并到达目标点。以下是按代码执行顺序的详细解释：

1. 主函数入口 mpc_plan

function [pose, traj, flag] = mpc_plan(start, goal, varargin)

• 输入参数：
  • start: 机器人初始位姿 [x, y, theta]
  • goal: 目标位姿 [x, y, theta]
  • varargin: 可选参数（路径path和地图map）
• 输出：
  • pose: 机器人位姿序列
  • traj: 轨迹
  • flag: 是否成功到达目标

2. 参数解析与验证

p = inputParser;
addParameter(p, 'path', "none");
addParameter(p, 'map', "none");
parse(p, varargin{:});if isstring(p.Results.path) || isstring(p.Results.map)throw(MException('MyErr:InvalidInput', 'path/map must be matrix.'));
end

• 功能：检查输入参数是否为有效矩阵（非字符串），否则抛出异常。
• 这段代码的主要功能是对输入参数进行解析和验证，确保path和map参数为有效的非字符串类型，以下是详细解释：

1. 创建输入参数解析器

p = inputParser;

• inputParser 是 MATLAB 中用于解析函数输入参数的类。通过创建 inputParser 对象 p，可以方便地定义和管理函数的输入参数。这个对象就像是一个参数管理的容器，后续可以向其中添加不同类型的参数。

2. 添加参数到解析器

addParameter(p, 'path', "none");
addParameter(p, 'map', "none");

• addParameter 是 inputParser 对象的一个方法，用于向解析器中添加参数。
  • 第一个参数 p 是 inputParser 对象，指定要添加参数的解析器。
  • 第二个参数是参数的名称，这里分别是 'path' 和 'map'，表示路径和地图信息。
  • 第三个参数是参数的默认值，这里都设置为字符串 "none"。这意味着如果调用函数时没有显式提供 path 或 map 参数，它们将被默认设置为 "none"。

3. 解析输入参数

parse(p, varargin{:});

• parse 是 inputParser 对象的另一个方法，用于解析输入参数。
  • 第一个参数 p 是要执行解析操作的 inputParser 对象。
  • varargin 是 MATLAB 中用于处理可变数量输入参数的特殊变量。varargin{:} 表示将 varargin 中的所有元素展开作为 parse 方法的输入。通过调用 parse 方法，解析器会根据之前定义的参数规则，将输入的参数赋值给相应的参数名。

4. 验证参数类型

if isstring(p.Results.path) || isstring(p.Results.map)exception = MException('MyErr:InvalidInput', 'parameter `path` or `map` must be set.');throw(exception);
end

• isstring 是 MATLAB 的一个函数，用于判断一个变量是否为字符串类型。
  • p.Results.path 和 p.Results.map 分别表示解析器解析后得到的 path 和 map 参数的值。
  • 如果 path 或 map 中的任意一个参数是字符串类型，说明它们可能还是默认值 "none"，没有被正确设置为有效的数据类型，则会执行以下操作：
    • MException 是 MATLAB 中用于创建异常对象的函数。这里创建了一个异常对象 exception，其标识符为 'MyErr:InvalidInput'，错误消息为 'parameter pathormap must be set.'，表示 path 或 map 参数必须被正确设置。
    • throw 函数用于抛出异常，当调用该函数时，程序会停止当前的执行流程，并显示异常信息，提示用户输入的参数不符合要求。

3. 路径预处理

path = flipud(p.Results.path);  % 反转路径点顺序（假设原路径是终点到起点）
path = path_interpolation(path, 5);  % 插值5次，增加路径密度function path_new = path_interpolation(path, n)for i=1:npath_new = path;path_inter = path_new(1:end - 1, :) + diff(path_new) / 2;path = zeros(length(path_new) + length(path_inter), 2);path(1:2:end, :) = path_new;path(2:2:end, :) = path_inter;end
end

• path_interpolation：
  • 逻辑：每次将线段中点插入路径，例如初始路径长度为N，插值后变为N + (N-1)*2^n。
  • 作用：提高路径平滑度，便于后续跟踪。
    该函数通过 n 次循环，每次在原路径相邻点之间插入中点，从而增加路径的点数，使路径更加平滑。最终返回经过 n 次插值后的新路径 path_new。例如，假设原路径有 3 个点，经过 1 次插值后会变成 5 个点（在原有的 2 个间隔中各插入 1 个中点）；经过 2 次插值后会进一步增加点数，路径会更加平滑。

函数定义和参数说明

function path_new = path_interpolation(path, n)

• 函数名：path_interpolation
• 输入参数：
  • path：一个二维矩阵，每一行代表路径上一个点的坐标，通常为 [x, y] 形式。
  • n：一个正整数，表示插值的次数。
• 输出参数：
  • path_new：经过 n 次插值操作后的新路径，同样是一个二维矩阵，行数比原路径更多，路径更加平滑。

每次插值的具体步骤

1. 保存当前路径

   path_new = path;
   

   在每次插值开始时，将当前的路径 path 赋值给 path_new，方便后续操作。

2. 计算路径点之间的中点

   path_inter = path_new(1:end - 1, :) + diff(path_new) / 2;
   

   • path_new(1:end - 1, :)：选取 path_new 矩阵中除最后一行之外的所有行，也就是路径上除最后一个点之外的所有点。
   • diff(path_new)：计算 path_new 矩阵中相邻行之间的差值，得到相邻点之间的向量。
   • diff(path_new) / 2：将相邻点之间的向量除以 2，得到中点相对于前一个点的偏移量。
   • path_new(1:end - 1, :) + diff(path_new) / 2：将偏移量加到前一个点上，得到相邻两点之间的中点坐标。最终 path_inter 是一个矩阵，每一行代表相邻两点之间的中点坐标。

3. 创建新的路径矩阵

   path = zeros(length(path_new) + length(path_inter), 2);
   

   创建一个新的零矩阵 path，其行数为 path_new 的行数加上 path_inter 的行数，列数为 2，用于存储插值后的新路径。

4. 合并原路径点和中点

   path(1:2:end, :) = path_new;
   path(2:2:end, :) = path_inter;
   

   • path(1:2:end, :)：选取 path 矩阵中所有奇数行，将原路径 path_new 赋值给这些奇数行。
   • path(2:2:end, :)：选取 path 矩阵中所有偶数行，将计算得到的中点 path_inter 赋值给这些偶数行。

4. 机器人状态初始化

robot.x = start(1);
robot.y = start(2);
robot.theta = start(3);
robot.v = 0;  % 初始速度为0
robot.w = 0;  % 初始角速度为0

5. 参数设置

5.1 控制参数

param.dt = 0.1;          % 控制周期0.1秒
param.max_iteration = 2000;  % 最大迭代次数
param.goal_dist_tol = 1.0;  % 目标距离容差（米）当车辆与目标点的距离小于这个容差时，认为车辆已经到达目标点
param.rotate_tol = 0.5;  % 角度容差（弧度）当车辆的朝向与目标朝向的偏差小于这个容差时，认为车辆的朝向已经达到要求。
param.lookahead_time = 1.0;  % 前瞻时间，决定前瞻距离d = v*T
param.min_lookahead_dist = 1.0;  % 最小前瞻距离
param.max_lookahead_dist = 2.5;  % 最大前瞻距离
param.max_v_inc = 0.5;  % 线速度最大增量
param.max_v = 1.0;       % 线速度上限
param.min_v = 0.0;       % 线速度下限
param.max_w_inc = pi/2;  % 角速度最大增量
param.max_w = pi/2;      % 角速度上限
param.min_w = 0.0;       % 角速度下限

5.2 MPC参数

param.Q = diag([1, 1, 1]);  % 状态误差权重（x,y,theta权重相同）
param.R = diag([2, 2]);     % 控制输入权重（速度和角速度权重为2）
param.p = 12;               % 预测时域（未来12步）
param.m = 8;                % 控制时域（优化前8步控制输入）
param.u_min = [0; -pi/2];   % 控制输入下限（v≥0，w≥-pi/2）
param.u_max = [1; pi/2];     % 控制输入上限
param.du_min = [0; -pi/2];  % 控制增量下限（v增量≥0，w增量≥-pi/2）
param.du_max = [0.5; pi/2]; % 控制增量上限

6. 主循环

iter = 0;
u_p = [0, 0];  % 前一次控制误差
while iter < param.max_iterationiter = iter + 1;% 终止条件：到达目标点if shouldRotateToGoal([robot.x, robot.y], goal, param)flag = true;break;end% 步骤1：获取前瞻点[lookahead_pt, theta_trj, kappa] = getLookaheadPoint(robot, path, param);% 步骤2：计算角度误差e_theta = regularizeAngle(robot.theta - goal(3)) / 10;% 步骤3：控制决策if shouldRotateToGoal([robot.x, robot.y], goal, param)% 优先旋转到目标点if ~shouldRotateToPath(abs(e_theta), 0.0, param)u = [0, 0];elseu = [0, angularRegularization(robot, e_theta / param.dt, param)];endelse% 计算路径方向误差e_theta = regularizeAngle(atan2(lookahead_pt(2)-robot.y, lookahead_pt(1)-robot.x) - robot.theta) / 10;if shouldRotateToPath(abs(e_theta), pi/4, param)% 优先旋转到路径方向u = [0, angularRegularization(robot, e_theta / param.dt, param)];else% MPC计算控制输入s = [robot.x, robot.y, robot.theta];s_d = [lookahead_pt, theta_trj];u_r = [robot.v, theta_trj - robot.theta];[u, u_p] = mpcControl(s, s_d, u_r, u_p, robot, param);endend% 步骤4：更新机器人状态robot = f(robot, u, param.dt);pose = [pose; robot.x, robot.y, robot.theta];
end

这段代码是 mpc_plan 函数中的主循环部分，主要实现了机器人的路径跟踪和运动控制，通过不断迭代更新机器人的状态，使其朝着目标点移动。以下是对代码的详细解释：

1. 初始化变量

   iter = 0;
   u_p = [0, 0]; % 先前的控制误差
   

   • iter：迭代计数器，初始化为 0，用于记录当前的迭代次数。
   • u_p：先前的控制误差，初始化为 [0, 0]，在后续的 MPC 控制中会不断更新。

2. 主循环

   while iter < param.max_iterationiter = iter + 1;
   

   • while 循环：只要迭代次数 iter 小于最大迭代次数 param.max_iteration，就会继续执行循环体中的代码。
   • iter = iter + 1：每次迭代时，迭代计数器 iter 加 1。

3. 检查是否到达目标点

   if shouldRotateToGoal([robot.x, robot.y], goal, param)flag = true;break;
   end
   

   • shouldRotateToGoal 函数：用于判断机器人当前位置 [robot.x, robot.y] 与目标位置 goal 之间的距离是否小于目标距离容差 param.goal_dist_tol。如果满足条件，则认为机器人已经接近目标点，需要进行旋转操作以到达目标位姿。
   • flag = true：将标志变量 flag 设置为 true，表示机器人已经到达目标点。
   • break：跳出 while 循环，结束路径跟踪过程。

4. 获取前瞻点

   [lookahead_pt, theta_trj, kappa] = getLookaheadPoint(robot, path, param);
   

   • getLookaheadPoint 函数：根据机器人的当前状态 robot、路径 path 和参数 param，计算路径上前瞻距离处的特定点。
   • lookahead_pt：前瞻点的坐标。
   • theta_trj：前瞻点处路径的切线角度。
   • kappa：前瞻点处路径的曲率。

5. 计算角度误差

   e_theta = regularizeAngle(robot.theta - goal(3)) / 10;
   

   • regularizeAngle 函数：将角度归一化到 [-π, π] 范围内。
   • robot.theta - goal(3)：计算机器人当前朝向与目标朝向之间的角度误差。
   • / 10：对角度误差进行缩放，以避免过大的控制输入。

6. 控制决策
   接近目标点时的控制决策

   if shouldRotateToGoal([robot.x, robot.y], goal, param)if ~shouldRotateToPath(abs(e_theta), 0.0, param)u = [0, 0];elseu = [0, angularRegularization(robot, e_theta / param.dt, param)];end
   

   • shouldRotateToGoal 函数：再次判断机器人是否接近目标点。

   • shouldRotateToPath 函数：判断机器人当前的朝向与目标朝向之间的角度偏差是否超出了可容忍的范围。

     • 如果角度偏差小于容差（~shouldRotateToPath(abs(e_theta), 0.0, param)），则将控制输入 u 设置为 [0, 0]，表示机器人停止运动。
     • 否则，将线速度设置为 0，角速度经过规范化处理后赋值给 u，表示机器人只进行旋转运动。

     未接近目标点时的控制决策

     elsee_theta = regularizeAngle( ...atan2(real(lookahead_pt(2)) - robot.y, real(lookahead_pt(1)) - robot.x) - robot.theta ...) / 10;if shouldRotateToPath(abs(e_theta), pi / 4, param)u = [0, angularRegularization(robot, e_theta / param.dt, param)];elses = [robot.x, robot.y, robot.theta];s_d = [real(lookahead_pt), theta_trj];u_r = [robot.v, theta_trj - robot.theta];[u, u_p] = mpcControl(s, s_d, u_r, u_p, robot, param);end
     

     • 计算从机器人当前位置到前瞻点的方向角度，减去机器人当前的朝向角度，并进行角度规范化和缩放。
     • shouldRotateToPath 函数：判断机器人当前的朝向与路径期望朝向之间的角度偏差是否超出了 π/4 的范围。
       • 如果角度偏差大于 π/4，则将线速度设置为 0，角速度经过规范化处理后赋值给 u，表示机器人只进行旋转运动。
       • 否则，使用模型预测控制（MPC）计算控制指令。
         • s：机器人的当前状态，包括位置和朝向。
         • s_d：期望状态，即前瞻点的坐标和切线角度。
         • u_r：参考输入，包括机器人的当前线速度和期望的角速度变化。
         • mpcControl 函数：根据当前状态、期望状态、参考输入和先前的控制误差，计算最优的控制输入 u 和更新后的控制误差 u_p。

7. 更新机器人状态

   robot = f(robot, u, param.dt);
   pose = [pose; robot.x, robot.y, robot.theta];
   

   • f 函数：机器人的运动学模型，根据当前状态 robot、控制输入 u 和时间步长 param.dt，更新机器人的状态。
   • pose：记录机器人的位姿序列，将更新后的机器人位置和朝向添加到 pose 矩阵中。

7. 运动学模型 f

function robot = f(robot, u, dt)F = eye(5);  % 单位矩阵（前3行对应位置和角度，后两行固定为0）B = [dt*cos(robot.theta) 0;dt*sin(robot.theta) 0;0                dt;1                 0;0                 1];x = [robot.x; robot.y; robot.theta; robot.v; robot.w];x_star = F * x + B * u';robot.x = x_star(1);robot.y = x_star(2);robot.theta = x_star(3);robot.v = x_star(4);robot.w = x_star(5);
end

该函数接收三个输入参数：

• robot：一个结构体，包含机器人当前的状态信息，如位置 (x, y)、朝向 theta、线速度 v 和角速度 w。
• u：一个二维向量，包含控制输入，通常为 [v_d, w_d]，其中 v_d 是期望的线速度，w_d 是期望的角速度。
• dt：时间步长，即两次状态更新之间的时间间隔。

函数返回更新后的机器人状态 robot。

代码详细解释

1. 定义状态转移矩阵 F

   F = [ 1 0 0 0 00 1 0 0 00 0 1 0 00 0 0 0 00 0 0 0 0];
   

   矩阵 F 是一个 $5\times 5$ 的矩阵，用于描述机器人状态的自主演化部分。在这个模型中，位置 (x, y) 和朝向 theta 不受其他状态的直接影响（除了通过控制输入），而线速度 v 和角速度 w 在这里假设没有自主变化（即不考虑自身的加速度等因素），所以对应位置为 0。

2. 定义控制输入矩阵 B

   B = [dt * cos(robot.theta) 0dt * sin(robot.theta)  00                                dt1                                 00                                 1];
   

   矩阵 B 是一个 $5\times 2$ 的矩阵，用于描述控制输入 u 对机器人状态的影响。具体分析如下：

   • 第一行 dt * cos(robot.theta)：表示线速度输入 v_d 对机器人 x 坐标的影响。在时间步长 dt 内，机器人在 x 方向上的位移与线速度和当前朝向的余弦值有关。
   • 第二行 dt * sin(robot.theta)：表示线速度输入 v_d 对机器人 y 坐标的影响。在时间步长 dt 内，机器人在 y 方向上的位移与线速度和当前朝向的正弦值有关。
   • 第三行 dt：表示角速度输入 w_d 对机器人朝向 theta 的影响。在时间步长 dt 内，机器人的朝向变化与角速度成正比。
   • 第四行 1：表示线速度输入 v_d 直接影响机器人的线速度状态。
   • 第五行 1：表示角速度输入 w_d 直接影响机器人的角速度状态。

3. 构建状态向量 x

   x = [robot.x; robot.y; robot.theta; robot.v; robot.w];
   

   将机器人的当前状态 (x, y, theta, v, w) 组合成一个 $5\times 1$ 的列向量 x。

4. 计算更新后的状态向量 x_star

   x_star = F * x + B * u';
   

   这是离散时间线性系统的状态更新公式，其中 F * x 表示状态的自主演化部分，B * u' 表示控制输入对状态的影响。

5. 更新机器人状态

   robot.x = x_star(1); robot.y = x_star(2); robot.theta = x_star(3);
   robot.v = x_star(4); robot.w = x_star(5);
   

   将更新后的状态向量 x_star 中的元素分别赋值给机器人状态结构体 robot 中的对应字段。

数学表达式推导

设机器人在 $k$ 时刻的状态向量为 ${\mathbf{x}}_k=[x_k,y_k,{\theta }_k,v_k,w_k{]}^T$，控制输入向量为 ${\mathbf{u}}_k=[v_{d_k},w_{d_k}{]}^T$，时间步长为 $\Delta t$。

则在 $k+1$ 时刻的状态向量 ${\mathbf{x}}_{k+1}$ 可以通过以下公式计算：

${\mathbf{x}}_{k+1}=\mathbf{F}{\mathbf{x}}_k+\mathbf{B}{\mathbf{u}}_k$

其中，

$\mathbf{F}=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]$

$\mathbf{B}=\left[\begin{array}{cc}\Delta t\cos ({\theta }_k)& 0\\ \Delta t\sin ({\theta }_k)& 0\\ 0& \Delta t\\ 1& 0\\ 0& 1\end{array}\right]$

具体展开各个状态变量的更新公式如下：

• $x_{k+1}=x_k+\Delta t\cdot v_{d_k}\cos ({\theta }_k)$
• $y_{k+1}=y_k+\Delta t\cdot v_{d_k}\sin ({\theta }_k)$
• ${\theta }_{k+1}={\theta }_k+\Delta t\cdot w_{d_k}$
• $v_{k+1}=v_{d_k}$
• $w_{k+1}=w_{d_k}$

这些公式描述了在一个时间步长 $\Delta t$ 内，机器人的位置、朝向、线速度和角速度如何根据当前状态和控制输入进行更新。

8. 角度归一化 regularizeAngle

function theta = regularizeAngle(angle)theta = angle - 2*pi*floor((angle + pi)/(2*pi));
end

regularizeAngle 函数的主要作用是将输入的角度 angle 规范化到 $(-\pi ,\pi ]$ 的范围内。在机器人运动控制、导航等领域中，角度的表示通常需要保持在一个标准的范围内，这样可以避免因角度值过大或过小而导致的计算误差和逻辑混乱。这个函数就实现了这样的角度规范化功能。

• (angle + pi)：首先将输入的角度 angle 加上 $\pi$。这一步的目的是将角度的范围从原来的 $(-\infty ,+\infty )$ 平移到一个便于后续处理的区间，使得后续的取整操作能够正确地将角度映射到 $(-\pi ,\pi ]$ 范围内。

• (angle + pi)/(2*pi)：将上一步得到的结果除以 $2\pi$。因为 $2\pi$ 是一个完整的圆周角度，这样做的目的是计算输入角度相对于一个完整圆周的倍数。

• floor((angle + pi)/(2*pi))：使用 floor 函数对上述结果进行向下取整。floor 函数会返回不大于输入值的最大整数，这样就得到了输入角度包含的完整圆周的数量。

• 2*pi*floor((angle + pi)/(2*pi))：将取整后的结果乘以 $2\pi$，得到输入角度中包含的完整圆周的总角度。

• angle - 2*pi*floor((angle + pi)/(2*pi))：最后，用输入角度 angle 减去完整圆周的总角度，就得到了一个位于 $(-\pi ,\pi ]$ 范围内的规范化角度。

示例说明

• 假设输入角度 angle = 3*pi，则：

  • (angle + pi) = 4*pi
  • (angle + pi)/(2*pi) = 2
  • floor((angle + pi)/(2*pi)) = 2
  • 2*pi*floor((angle + pi)/(2*pi)) = 4*pi
  • theta = angle - 2*pi*floor((angle + pi)/(2*pi)) = 3*pi - 4*pi = -pi

• 假设输入角度 angle = -3*pi/2，则：

  • (angle + pi) = -pi/2
  • (angle + pi)/(2*pi) = -1/4
  • floor((angle + pi)/(2*pi)) = -1
  • 2*pi*floor((angle + pi)/(2*pi)) = -2*pi
  • theta = angle - 2*pi*floor((angle + pi)/(2*pi)) = -3*pi/2 - (-2*pi) = pi/2

通过这样的计算，无论输入的角度是多少，最终都能将其规范化到 $(-\pi ,\pi ]$ 的范围内。

9. 终止条件判断 shouldRotateToGoal

function flag = shouldRotateToGoal(cur, goal, param)flag = hypot(cur(1)-goal(1), cur(2)-goal(2)) < param.goal_dist_tol;
end

代码详细解释

function flag = shouldRotateToGoal(cur, goal, param)

• 此函数名为 shouldRotateToGoal，接收三个输入参数：
  • cur：这是一个包含两个元素的向量，通常表示机器人当前的二维位置 [x, y]。
  • goal：同样是一个包含两个元素的向量，代表目标位置 [x, y]。
  • param：这是一个结构体，包含了各种参数，其中 param.goal_dist_tol 表示目标距离容差，即当机器人与目标位置的距离小于该容差值时，认为机器人已经接近目标位置。
• 函数返回一个布尔值 flag，用于指示是否需要执行旋转操作。

hypot(cur(1)-goal(1), cur(2)-goal(2))

• hypot 是 MATLAB 中的一个函数，用于计算两个数的平方和的平方根，也就是欧几里得距离。
• cur(1)-goal(1) 计算的是机器人当前位置的 x 坐标与目标位置的 x 坐标之间的差值。
• cur(2)-goal(2) 计算的是机器人当前位置的 y 坐标与目标位置的 y 坐标之间的差值。
• hypot(cur(1)-goal(1), cur(2)-goal(2)) 最终得到的是机器人当前位置与目标位置之间的欧几里得距离。

flag = hypot(cur(1)-goal(1), cur(2)-goal(2)) < param.goal_dist_tol;

• 这行代码将计算得到的欧几里得距离与目标距离容差 param.goal_dist_tol 进行比较。
• 如果欧几里得距离小于目标距离容差，说明机器人已经足够接近目标位置，此时 flag 被赋值为 true，表示需要执行旋转操作以调整机器人的朝向，使其达到目标位姿。
• 如果欧几里得距离大于或等于目标距离容差，说明机器人还没有接近目标位置，flag 被赋值为 false，表示暂时不需要执行旋转操作，机器人可能需要继续向目标位置移动。

10. 角度偏差判断 shouldRotateToPath

function flag = shouldRotateToPath(angle_to_path, tol, param)if tol == 0.0flag = angle_to_path > param.rotate_tol;elseflag = angle_to_path > tol;end
end

shouldRotateToPath 函数的主要作用是判断机器人是否需要通过旋转操作来纠正其跟踪路径，即判断机器人当前的朝向与路径期望朝向之间的角度偏差是否超出了可容忍的范围。根据传入的容差参数 tol 的不同，使用不同的判断标准来确定是否需要进行旋转。

1. 函数定义

   function flag = shouldRotateToPath(angle_to_path, tol, param)
   

   • 该函数名为 shouldRotateToPath，接收三个输入参数：
     • angle_to_path：一个浮点数，表示机器人当前朝向与路径期望朝向之间的角度偏差（单位通常为弧度）。
     • tol：一个浮点数，是角度偏差的容差。它可以根据具体情况进行设置，如果传入 0.0，则会使用结构体 param 中的 rotate_tol 作为容差；如果传入其他值，则直接使用该值作为容差。
     • param：一个结构体，其中包含了各种控制参数，这里主要使用 param.rotate_tol，它表示默认的旋转容差。
   • 函数返回一个布尔值 flag，用于指示机器人是否应该执行旋转操作。

2. 判断逻辑

   if tol == 0.0flag = angle_to_path > param.rotate_tol;
   elseflag = angle_to_path > tol;
   end
   

   • if tol == 0.0：检查传入的容差 tol 是否为 0.0。
     • 如果 tol 等于 0.0：使用结构体 param 中的 rotate_tol 作为角度偏差的容差。将 angle_to_path 与 param.rotate_tol 进行比较，如果 angle_to_path 大于 param.rotate_tol，则将 flag 赋值为 true，表示机器人需要执行旋转操作来纠正朝向；否则，将 flag 赋值为 false，表示不需要旋转。
     • 如果 tol 不等于 0.0：直接使用传入的 tol 作为角度偏差的容差。将 angle_to_path 与 tol 进行比较，如果 angle_to_path 大于 tol，则将 flag 赋值为 true；否则，将 flag 赋值为 false。

11. 角速度约束 angularRegularization

function w = angularRegularization(robot, w_d, param)w_inc = w_d - robot.w;if abs(w_inc) > param.max_w_incw_inc = param.max_w_inc * sign(w_inc);endw = robot.w + w_inc;if abs(w) > param.max_w, w = param.max_w * sign(w); endif abs(w) < param.min_w, w = param.min_w * sign(w); end
end

• 约束：
  • 角速度变化量Δw ≤ pi/2。
  • 角速度范围w ∈ [-pi/2, pi/2]。
    函数的主要作用是对机器人的角速度进行正则化处理，确保角速度及其变化量在设定的允许范围内。在机器人运动控制中，为了保证机器人的安全和稳定运行，需要限制角速度的变化幅度以及角速度的取值范围，该函数就是实现这一功能的。

12. 线速度约束 linearRegularization

function v = linearRegularization(robot, v_d, param)v_inc = v_d - robot.v;if abs(v_inc) > param.max_v_incv_inc = param.max_v_inc * sign(v_inc);endv = robot.v + v_inc;if abs(v) > param.max_v, v = param.max_v * sign(v); endif abs(v) < param.min_v, v = param.min_v * sign(v); end
end

• 约束：
  • 线速度变化量Δv ≤ 0.5。
  • 线速度范围v ∈ [0, 1]。
    函数的主要作用是对机器人的线速度进行正则化处理，确保线速度及其变化量在设定的允许范围内。在机器人运动控制场景中，为了保障机器人运行的安全性和稳定性，需要对其线速度的变化幅度以及线速度的取值范围进行限制，此函数就实现了这样的功能。

13. 前瞻距离计算 getLookaheadDistance

function d = getLookaheadDistance(robot, param)d = robot.v * param.lookahead_time;d = max(min(d, param.max_lookahead_dist), param.min_lookahead_dist);
end

• 逻辑：根据当前线速度和前瞻时间动态调整前瞻距离，确保在[1, 2.5]米范围内。

14. 路径前瞻点计算 getLookaheadPoint

function [pt, theta, kappa] = getLookaheadPoint(robot, path, param)

步骤：

1. 搜索路径点：
   • 计算所有路径点到机器人的距离，找到第一个≥前瞻距离的点。
2. 线段与圆交点：
   • 若路径点不足，取最后一个点；否则计算线段与圆的交点。
3. 角度与曲率计算：
   • 切线角度theta：由路径线段方向确定。
   • 曲率kappa：通过三点法计算，符号由叉积决定（正负表示弯曲方向）。

getLookaheadPoint 函数的主要作用是在给定的路径 path 上找到距离机器人当前位置恰好为前瞻距离 lookahead_dist 的点，同时计算该点所在路径线段的切线角度 theta 以及路径在该点附近的曲率 kappa。这些信息对于机器人的路径跟踪和运动控制非常重要，例如可以帮助机器人确定下一步的行驶方向和速度。

1. 函数定义与参数说明

   function [pt, theta, kappa] = getLookaheadPoint(robot, path, param)
   

   • 函数接收三个输入参数：
     • robot：一个结构体，包含机器人的当前状态信息，如 robot.x 和 robot.y 表示机器人当前的位置坐标。
     • path：一个二维矩阵，每一行代表路径上一个点的坐标 [x, y]。
     • param：一个结构体，包含各种控制参数，其中 getLookaheadDistance 函数会使用 param 中的参数来计算前瞻距离。
   • 函数返回三个输出：
     • pt：一个二维向量，表示找到的前瞻点在全局坐标系下的坐标 [x, y]。
     • theta：一个浮点数，表示前瞻点所在路径线段的切线角度（单位为弧度）。
     • kappa：一个浮点数，表示路径在该点附近的曲率，正曲率表示曲线向左弯曲，负曲率表示曲线向右弯曲。

2. 计算路径上各点到机器人的距离

   dist_to_robot = [];
   [pts_num, ~] = size(path);
   for i=1:pts_numdist_to_robot(end + 1) = hypot(path(i, 1) - robot.x, path(i, 2) - robot.y);
   end
   

   • 初始化一个空数组 dist_to_robot，用于存储路径上每个点到机器人当前位置的欧几里得距离。
   • 使用 size 函数获取路径 path 的行数 pts_num，即路径上点的数量。
   • 通过 for 循环遍历路径上的每个点，使用 hypot 函数计算每个点到机器人当前位置的欧几里得距离，并将结果添加到 dist_to_robot 数组中。

3. 找到距离机器人最近的点

   [~, idx_closest] = min(dist_to_robot);
   

   • 使用 min 函数找到 dist_to_robot 数组中的最小值及其对应的索引 idx_closest，该索引表示路径上距离机器人最近的点的编号。

4. 初始化目标点和前一个点的索引

   idx_goal = pts_num - 1; idx_prev = idx_goal - 1;
   

   • 初始化目标点的索引 idx_goal 为路径上倒数第二个点的编号，前一个点的索引 idx_prev 为倒数第三个点的编号。

5. 计算前瞻距离

   lookahead_dist = getLookaheadDistance(robot, param);
   

   • 调用 getLookaheadDistance 函数，根据机器人的当前状态 robot 和参数 param 计算前瞻距离。

6. 找到第一个距离机器人大于等于前瞻距离的点

   for i=idx_closest:pts_numif hypot(path(i, 1) - robot.x, path(i, 2) - robot.y) >= lookahead_distidx_goal = i;break;end
   end
   

   • 从距离机器人最近的点开始，向后遍历路径上的点。
   • 使用 hypot 函数计算当前点到机器人的距离，如果该距离大于等于前瞻距离，则将该点的索引赋值给 idx_goal，并跳出循环。

7. 确定前瞻点的坐标

   if idx_goal == pts_num - 1pt = [path(idx_goal, 1), path(idx_goal, 2)];
   elseif idx_goal == 1idx_goal = idx_goal + 1;endidx_prev = idx_goal - 1;px = path(idx_prev, 1); py = path(idx_prev, 2);gx = path(idx_goal, 1); gy = path(idx_goal, 2);prev_p = [px - robot.x, py - robot.y];goal_p = [gx - robot.x, gy - robot.y];i_points = circleSegmentIntersection(prev_p, goal_p, lookahead_dist);pt = [i_points(1, 1) + robot.x, i_points(1, 2) + robot.y];
   end
   

   • 情况一：未找到距离大于等于前瞻距离的点：如果 idx_goal 仍然是路径上倒数第二个点的编号，说明路径上所有点距离机器人都不够远，此时将路径上倒数第二个点作为前瞻点。
   • 情况二：找到距离大于等于前瞻距离的点：
     • 确保 idx_goal 不小于 2，如果 idx_goal 等于 1，则将其加 1。
     • 获取目标点和前一个点的坐标 px, py 和 gx, gy。
     • 将这两个点的坐标从全局坐标系转换到以机器人位置为原点的局部坐标系，得到 prev_p 和 goal_p。
     • 调用 circleSegmentIntersection 函数，计算以机器人位置为圆心、前瞻距离为半径的圆与路径上目标点和前一个点所构成线段的交点。
     • 将交点的坐标从局部坐标系转换回全局坐标系，得到前瞻点的坐标 pt。

8. 计算路径上该线段的切线角度

   theta = atan2(path(idx_goal, 2) - path(idx_prev, 2), path(idx_goal, 1) - path(idx_prev, 1));
   

• 使用 atan2 函数计算路径上目标点和前一个点所构成线段的切线角度 theta，该角度表示路径在该线段处的方向。

9. 计算路径在该点附近的曲率

   if idx_goal == 2idx_goal = idx_goal + 1;
   end
   idx_prev = idx_goal - 1;
   idx_pprev = idx_prev - 1;
   a = hypot(path(idx_prev, 1) - path(idx_goal, 1), path(idx_prev, 2) - path(idx_goal, 2));
   b = hypot(path(idx_pprev, 1) - path(idx_goal, 1), path(idx_pprev, 2) - path(idx_goal, 2));
   c = hypot(path(idx_pprev, 1) - path(idx_prev, 1), path(idx_pprev, 2) - path(idx_prev, 2));
   cosB = (a * a + c * c - b * b) / (2 * a * c);
   sinB = sin(acos(cosB));
   cross = (path(idx_prev, 1) - path(idx_pprev, 1)) * ...(path(idx_goal, 2) - path(idx_pprev, 2)) - ...(path(idx_prev, 2) - path(idx_pprev, 2)) * ...(path(idx_goal, 1) - path(idx_pprev, 1));
   kappa = 2 * sinB / b * sign(cross);
   

   • 确保 idx_goal 不小于 3，如果 idx_goal 等于 2，则将其加 1。
   • 获取目标点、前一个点和前前一个点的索引 idx_goal、idx_prev 和 idx_pprev。
   • 使用 hypot 函数计算由这三个点构成的三角形的三条边长 a、b 和 c。
   • 根据余弦定理计算角 B 的余弦值 cosB，再使用 acos 函数计算其反余弦值得到角 B 的弧度值，最后使用 sin 函数计算其正弦值 sinB。
   • 计算向量叉积 cross，用于确定曲线的弯曲方向。
   • 根据公式 kappa = 2 * sinB / b * sign(cross) 计算路径在该点附近的曲率 kappa，其中 sign(cross) 用于给曲率加上方向信息，正曲率表示曲线向左弯曲，负曲率表示曲线向右弯曲。

15. 线段与圆交点 circleSegmentIntersection

function i_points = circleSegmentIntersection(p1, p2, r)

数学原理：

• 线段方程：(x-x1)(y2-y1) = (y-y1)(x2-x1)。
• 圆方程：x² + y² = r²。
• 联立方程求解交点，考虑相切、相交和不相交三种情况。

circleSegmentIntersection 函数的主要功能是计算以原点 (0, 0) 为圆心、半径为 r 的圆与由点 p1 和 p2 所构成的线段的交点。函数返回一个矩阵 i_points，其中包含了所有的交点坐标。如果线段与圆没有交点，i_points 为空矩阵。

1. 函数定义与参数说明

   function i_points = circleSegmentIntersection(p1, p2, r)
   

   • 函数接收三个输入参数：
     • p1：一个二维向量，表示线段的起点坐标 [x1, y1]。
     • p2：一个二维向量，表示线段的终点坐标 [x2, y2]。
     • r：一个浮点数，表示圆的半径。
   • 函数返回一个矩阵 i_points，每一行代表一个交点的坐标 [x, y]。

2. 提取点的坐标并计算相关参数

   x1 = p1(1); x2 = p2(1);
   y1 = p1(2); y2 = p2(2);dx = x2 - x1; dy = y2 - y1;
   dr2 = dx * dx + dy * dy;
   D = x1 * y2 - x2 * y1;
   

   • 提取 p1 和 p2 的 x 和 y 坐标。
   • 计算线段在 x 和 y 方向上的增量 dx 和 dy。
   • 计算线段长度的平方 dr2，即 $d{x}^2+d{y}^2$。
   • 计算 D，它是一个中间变量，在后续计算交点坐标时会用到，其几何意义与线段和原点所构成的三角形面积有关（D 的绝对值的一半就是该三角形的面积）。

3. 计算其他中间变量

   d1 = x1 * x1 + y1 * y1;
   d2 = x2 * x2 + y2 * y2;
   dd = d2 - d1;
   

   • 计算点 p1 到原点的距离的平方 d1 和点 p2 到原点的距离的平方 d2。
   • 计算 dd，即 d2 - d1，用于后续判断交点的具体位置。

4. 计算判别式 delta

   delta = sqrt(r * r * dr2 - D * D);
   

   • delta 是一个判别式，用于判断线段与圆的相交情况。其数学原理基于圆和直线的方程联立求解。圆的方程为 $x^2+y^2=r^2$，直线的方程可以表示为 $y=(dy/dx)\ast (x-x1)+y1$（当 dx != 0 时），将直线方程代入圆的方程，经过整理可以得到一个关于 x 的二次方程 $A{x}^2+Bx+C=0$，其中判别式 $Delta=B^2-4AC$。这里的 delta 是经过简化和变形后的判别式，其值决定了线段与圆的相交情况：
     • 当 delta < 0 时，二次方程无实数解，说明线段与圆没有交点。
     • 当 delta = 0 时，二次方程有一个实数解，说明线段与圆相切。
     • 当 delta > 0 时，二次方程有两个实数解，说明线段与圆有两个交点。

5. 根据 delta 的值计算交点坐标

   if delta >= 0if delta == 0i_points = [D * dy / dr2, -D * dx / dr2];elsei_points = [(D * dy + sign(dd) * dx * delta) / dr2, ...(-D * dx + sign(dd) * dy * delta) / dr2; ...(D * dy - sign(dd) * dx * delta) / dr2, ...(-D * dx - sign(dd) * dy * delta) / dr2];end
   elsei_points = [];
   end
   

   • delta < 0 的情况：线段与圆没有交点，将 i_points 赋值为空矩阵。
   • delta = 0 的情况：线段与圆相切，根据推导得到的公式计算切点的坐标 [D * dy / dr2, -D * dx / dr2]。
   • delta > 0 的情况：线段与圆有两个交点，根据推导得到的公式计算两个交点的坐标。sign(dd) 函数用于返回 dd 的符号（正为 1，负为 -1），用于确定交点的具体位置。

16. MPC控制核心 mpcControl

function [u, u_p_new] = mpcControl(s, s_d, u_r, u_p, robot, param)%{Execute MPC control process.Parameters----------s: tuplecurrent states_d: tupledesired stateu_r: tuplerefered controlu_p: tupleprevious control errorReturn----------u: np.ndarraycontrol vector%}dim_u = 2; dim_x = 3;dt = param.dt;% state vector (5 x 1)x = [s - s_d, u_p]';% original state matrixA = eye(3);A(1, 3) = -u_r(1) * sin(s_d(3)) * dt;A(2, 3) = u_r(1) * cos(s_d(3)) * dt;% original control matrixB = zeros(3, 2);B(1, 1) = cos(s_d(3)) * dt;B(2, 1) = sin(s_d(3)) * dt;B(3, 2) = dt;% discrete iteration Ricatti equationP = param.Q;% state matrix (5 x 5)A = [A, B; zeros(dim_u, dim_x), eye(dim_u)];% control matrix (5 x 2)B = [B; eye(dim_u)];% output matrix (3 x 5)C = [eye(dim_x), zeros(dim_x, dim_u)];% mpc state matrix (3p x 5)S_x_cell = cell(param.p, 1);%预测时域长度。表示 MPC 算法向前预测的步数for i=1:param.pS_x_cell{i, 1} = C * A ^ i;endS_x = cell2mat(S_x_cell);% mpc control matrix (3p x 2m)S_u_cell = cell(param.p, param.m);for i = 1:param.pfor j = 1:param.mif j <= iS_u_cell{i, j} = C * A ^ (i - j) * B;elseS_u_cell{i, j} = zeros(dim_x, dim_u);endendendS_u = cell2mat(S_u_cell);% optimizationYr = zeros(3 * param.p, 1);                  % (3p x 1)Q = kron(eye(param.p), param.Q);    % (3p x 3p)R = kron(eye(param.m), param.R);    % (2m x 2m)H = S_u' * Q * S_u + R;                        % (2m x 2m)g = S_u' * Q * (S_x * x - Yr);                 % (2m x 1)% constriantsA_I = kron(tril(ones(param.m, param.m)), diag([1, 1]));U_min = kron(ones(param.m, 1), param.u_min);U_max = kron(ones(param.m, 1), param.u_max);U_k_1 = kron(ones(param.m, 1), u_p');% boundarydU_min = kron(ones(param.m, 1), param.du_min);dU_max = kron(ones(param.m, 1), param.du_max);% solveoptions = optimoptions('quadprog', 'MaxIterations', 100, 'TolFun', 1e-16, 'Display','off');dU_opt = quadprog(H, g, [-A_I; A_I], [-U_min + U_k_1; U_max - U_k_1], [], [], dU_min, dU_max, [], options);% first elementdu = [dU_opt(1), dU_opt(2)];% real controlu = du + u_p + u_r;u = [linearRegularization(robot, u(1), param), angularRegularization(robot, u(2), param)];u_p_new = u - u_r;
end

mpcControl 函数实现了模型预测控制（MPC）的核心算法，用于计算机器人的最优控制输入 u，使其在约束条件下跟踪期望状态 s_d。该函数通过构建状态增广模型、预测矩阵、优化目标和约束条件，求解二次规划问题得到控制增量，并最终生成实际控制输入。

数学推导与矩阵分析

1. 状态增广与误差动态

状态向量：

x = [s - s_d; u_p]';  % 增广状态向量（5×1）

• 状态误差：s - s_d（位置、角度误差）。
• 控制误差：u_p = u_{k-1} - u_r（前一次控制输入与参考输入的偏差）。

增广的目的：

• 将控制误差作为状态的一部分，引入积分作用，消除稳态误差。
• 将系统转换为线性时变（LTV）模型，便于MPC优化。

2. 状态转移矩阵 A 与控制矩阵 B

原状态矩阵 A（3×3）：

A = eye(3);
A(1, 3) = -u_r(1) * sin(s_d(3)) * dt;
A(2, 3) = u_r(1) * cos(s_d(3)) * dt;

• 物理意义：在参考输入 u_r 附近线性化，考虑参考线速度对角度误差的影响。
• 数学推导：
  • 原连续时间模型：
    $$\begin{gathered} {\dot{e}}_x=v_r\cos ({\theta }_d)-v_r\sin ({\theta }_d)e_{\theta } \\ {\dot{e}}_y=v_r\sin ({\theta }_d)+v_r\cos ({\theta }_d)e_{\theta } \\ {\dot{e}}_{\theta }={\omega }_r-\omega \end{gathered}$$
  • 离散化后得到 A 的元素（线性化忽略高阶项）。

原控制矩阵 B（3×2）：

B(1, 1) = cos(s_d(3)) * dt;
B(2, 1) = sin(s_d(3)) * dt;
B(3, 2) = dt;

• 物理意义：控制输入 u = [v, omega] 对状态误差的影响。
• 数学推导：
  $$\begin{gathered} e_x[k+1]=e_x[k]+v\cos ({\theta }_d)\Delta t \\ {e}_y[k+1]=e_y[k]+v\sin ({\theta }_d)\Delta t \\ {e}_{\theta }[k+1]=e_{\theta }[k]+\omega \Delta t \end{gathered}$$

增广后的矩阵：

A = [A, B; zeros(2,3), eye(2)];  % 5×5
B = [B; eye(2)];                 % 5×2

• 状态转移方程：
  $${\mathbf{x}}_{k+1}=A{\mathbf{x}}_k+B\Delta u_k$$
  其中，Δu_k = u_k - u_r 为控制增量。

3. 预测模型矩阵 S_x 和 S_u

预测时域 p：预测未来 p 步的状态响应。
控制时域 m：优化前 m 步的控制增量 Δu。

预测矩阵构造：

S_x = C * A^i  % 3p×5
S_u = C * A^{i-j} * B  % 3p×2m

• 物理意义：
  • S_x：初始状态对未来 p 步状态的影响。
  • S_u：控制增量对未来 p 步状态的影响。
• 数学公式：
  $$\mathbf{y}=S_x{\mathbf{x}}_0+S_u\Delta \mathbf{U}$$
  其中，$\mathbf{y}$ 是预测状态误差向量。

4. 优化问题

目标函数：
$$min\frac{1}{2}\Delta U^{T}H\Delta U+g^T\Delta U$$

• Hessian矩阵 H：
  $$H=S_u^{T}Q{S}_u+R$$
  • Q = kron(eye(p), param.Q)：状态误差权重矩阵（分块对角，每块为 Q）。
  • R = kron(eye(m), param.R)：控制增量权重矩阵（分块对角，每块为 R）。
• 梯度向量 g：
  $$g=S_u^{T}Q(S_x{\mathbf{x}}_0-Y_r)$$
  • Y_r = 0：期望的预测状态误差为零。

约束条件：

1. 控制输入约束：
   $$U_{\text{min}}\le U_k\le U_{\text{max}}$$

   • 矩阵形式：
     $$A_I\Delta U\le U_{\text{max}}-U_{k-1};A_I\Delta U\ge U_{\text{min}}-U_{k-1}$$
     $A_I=kron(tril(ones(m)),diag([1,1]))$：下三角矩阵，确保控制增量的递推关系。

2. 控制增量约束：
   $$\Delta U_{\text{min}}\le \Delta U\le \Delta U_{\text{max}}$$

5. 二次规划求解

dU_opt = quadprog(H, g, [-A_I; A_I], [-U_min + U_k_1; U_max - U_k_1], [], [], dU_min, dU_max);

• 输入约束矩阵：
  $$\left[\begin{array}{c}-A_I\\ A_I\end{array}\right]\Delta U\le \left[\begin{array}{c}-U_{\text{min}}+U_{k-1}\\ U_{\text{max}}-U_{k-1}\end{array}\right]$$
• 增量约束：
  $$\Delta U_{\text{min}}\le \Delta U\le \Delta U_{\text{max}}$$

6. 控制输入计算

u = du + u_p + u_r;
u = [linearRegularization(...), angularRegularization(...)];

• 实际控制输入：
  $$u_k=\Delta u_0+u_{k-1}+u_r$$
  • $\Delta u_0$：优化得到的第一个控制增量。
  • u_p_new = u - u_r：更新后的控制误差。

关键矩阵与参数总结

矩阵/参数	物理意义
A, B	增广状态转移矩阵和控制矩阵
S_x, S_u	预测模型矩阵，描述状态和控制对未来的影响
Q, R	状态误差和控制增量的权重矩阵
A_I	控制输入约束矩阵（下三角结构确保增量递推）
U_min, U_max	控制输入的上下限
dU_min, dU_max	控制增量的变化范围

数学公式总结

1. 增广状态方程：
   $${\mathbf{x}}_{k+1}=A{\mathbf{x}}_k+B\Delta u_k$$

2. 预测模型：
   $$\mathbf{y}=S_x{\mathbf{x}}_0+S_u\Delta \mathbf{U}$$

3. 优化问题：
   $$\begin{gathered} \underset{\Delta \mathbf{U}}{\min }\frac{1}{2}\Delta {\mathbf{U}}^{T}H\Delta \mathbf{U}+g^T\Delta \mathbf{U} \\ \text{s.t.}{A}_I\Delta \mathbf{U}\le U_{\text{max}}-U_{k-1},\Delta U_{\text{min}}\le \Delta \mathbf{U}\le \Delta U_{\text{max}} \end{gathered}$$

4. 控制输入更新：
   $$u_k=\Delta u_0+u_{k-1}+u_r$$

代码流程总结

1. 状态增广：将状态误差和控制误差组合为增广状态。
2. 模型线性化：在参考输入附近构建状态转移矩阵 A 和控制矩阵 B。
3. 预测矩阵生成：计算预测时域内的状态和控制影响矩阵。
4. 优化求解：通过二次规划计算最优控制增量。
5. 约束处理：确保控制输入在物理限制范围内。

通过MPC框架，在每个控制周期内求解最优控制输入，实现机器人的动态路径跟踪与约束满足。

17. 算法流程总结

1. 路径预处理：反转并插值路径。
2. 主循环：
   • 终止条件：到达目标点。
   • 前瞻点计算：动态确定路径上的跟踪点。
   • 控制决策：
     • 若角度误差大，优先旋转。
     • 否则，使用MPC计算最优控制输入。
   • 状态更新：通过运动学模型更新机器人位姿。
3. 输出结果：记录机器人位姿序列pose。

关键参数对性能的影响

• Q 和 R：
  • Q增大：更注重状态跟踪精度。
  • R增大：更注重控制输入平滑性。
• p 和 m：
  • p增大：预测更长远，但计算量增加。
  • m增大：优化更多控制输入，响应更灵活。
• 前瞻距离：
  • 增大：稳定性提高，但响应速度降低。
  • 减小：响应更快，但可能震荡。