MPC-test.zip

这个MPC模块主要是基于matlab/simulink中的lane keeping assist system搭建的，具体详见博客https://blog.csdn.net/caokaifa/article/details/92378387

function [closestPoint distance] = closestPointOnLine（pt1 pt2 refPt）
    %This function is for internal use only. It may be removed in the future.

    %closestPointOnLine Find a point closest to refPt （i.e. projection point） on line
    % segment between pt1 and pt2. If the projected point is outside line
    % segment then the closest vertex is returned. This function is used in
    % robotics.algs.internal.PurePursuitbase class.

    
    %   Copyright 2016 The MathWorks Inc.

    %#codegen

    % Don‘t do any computation if points are equal
    if isequal（pt1 pt2）
        closestPoint = pt1;
        distance = norm（refPt-closestPoint）;
        return;
    end

    % Vector from pt1 to pt2
    v12 = pt2 - pt1;
    % Vector from refPt to pt2
    vr2 = pt2 - refPt;

    % Projection of the vr2 on v12 normalized by norm（v12）
    alpha = v12*vr2‘/（v12*v12‘）;

    % Find the closet point by interpolation
    if alpha > 1
        closestPoint = pt1;
    elseif alpha < 0
        closestPoint = pt2;
    else
        closestPoint = alpha.*pt1 + （1-alpha）.*pt2;
    end

    distance = norm（refPt-closestPoint）;

end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2019-06-16 10:26  MPC-test\
     文件       29657  2019-04-16 17:45  MPC-test\HelperPathAnalyzer.m
     文件       38293  2019-06-16 10:26  MPC-test\LateralControlMPC.slx
     文件        4773  2019-06-16 10:25  MPC-test\LateralControlMPC.slxc
     文件       29664  2019-06-16 10:17  MPC-test\MpcRefMdl.slx
     文件        4604  2019-06-16 10:25  MPC-test\MpcRefMdl.slxc
     文件      151190  2019-06-11 23:48  MPC-test\automobile.png
     文件        1131  2019-05-21 14:07  MPC-test\closestPointOnLine.m
     文件        1237  2019-04-26 11:33  MPC-test\createCostmap.m
     文件        1610  2019-04-16 17:52  MPC-test\helperGoalChecker.m
     文件        5959  2017-12-06 23:59  MPC-test\helperPlotVehicle.m
     文件        2035  2019-04-16 18:39  MPC-test\helperSLCreateUtilityBus.m
     文件        1186  2018-12-29 03:13  MPC-test\helperSLCreateUtilityStruct.m
     文件         293  2018-01-13 00:49  MPC-test\helperSLMatchStructFields.m
     文件        2474  2018-12-29 03:13  MPC-test\helperSLVisualizePath.m
     文件        4170  2017-11-04 02:48  MPC-test\helperVehiclePolyshape.m
     目录           0  2019-06-16 10:25  MPC-test\slprj\
     目录           0  2019-06-16 10:25  MPC-test\slprj\sim\
     目录           0  2019-06-16 10:25  MPC-test\slprj\sim\varcache\
     目录           0  2019-06-16 10:25  MPC-test\slprj\sim\varcache\LateralControlMPC\
     文件         392  2019-06-16 10:25  MPC-test\slprj\sim\varcache\LateralControlMPC\checksumOfCache.mat
     目录           0  2019-06-16 10:25  MPC-test\slprj\sim\varcache\LateralControlMPC\tmwinternal\
     文件         312  2019-06-16 10:25  MPC-test\slprj\sim\varcache\LateralControlMPC\tmwinternal\simulink_cache.xml
     文件        3536  2019-06-16 10:25  MPC-test\slprj\sim\varcache\LateralControlMPC\varInfo.mat
     目录           0  2019-06-16 10:25  MPC-test\slprj\sim\varcache\MpcRefMdl\
     文件         392  2019-06-16 10:25  MPC-test\slprj\sim\varcache\MpcRefMdl\checksumOfCache.mat
     目录           0  2019-06-16 10:25  MPC-test\slprj\sim\varcache\MpcRefMdl\tmwinternal\
     文件         312  2019-06-16 10:25  MPC-test\slprj\sim\varcache\MpcRefMdl\tmwinternal\simulink_cache.xml
     文件        1040  2019-06-16 10:25  MPC-test\slprj\sim\varcache\MpcRefMdl\varInfo.mat