精华内容
下载资源
问答
  • 最短路径算法Dijkstra源代码,测试可以正常使用
  • vb.net编写的dijkstra最短路径算法源代码,可以直接用,干货啊。
  • A*算法做的吃金子小游戏,是我的研一作业,与君共享
  • matlab遗传算法路径优化代码实例,可供大家学习。同时带有注释,大家可根据自己需要进行学习。
  • 游戏路径算法,源代码

    2010-03-31 19:27:54
    找到了一个A*的路径算法,挺智能。 * G = 从起点A,沿着产生的路径,移动到网格上指定方格的移动耗费。 * H = 从网格上那个方格移动到终点B的预估移动耗费。这经常被称为启发式的,可能会让你有点迷惑。这样叫的...
  • 为了评职称,写了篇论文[最短路径Floyd算法具体演示],并且发表在自己的博客中[http://blog.csdn.net/vinglemar/article/details/43414439],多少年过去了,仍然有网友写信索取源代码,为此,我把它分享一下.
  • 蚁群算法 万能通用代码 最短路径 matlab万能代码
  • 蚁群算法论文及源代码,有很多文献和相应的源代码,希望能够帮助在学习蚁群算法的朋友
  • 蚁群算法最短路径万能matlab源代码.rar
  • 蚁群算法寻找最优路径matlab源代码及注释

    万次阅读 多人点赞 2018-05-02 15:27:34
    网上已经有很多matlab实现的蚁群算法源代码,也有详细的注释,但是有些注释有误,在这里更正,如有错误,希望各位批评指正。以下是解放军信息工程大学一个老师编的matlab程序,请尊重原作者劳动,引用时请注明出处...

    网上已经有很多matlab实现的蚁群算法的源代码,也有详细的注释,但是有些注释有误,在这里更正,如有错误,希望各位批评指正。

    以下是解放军信息工程大学一个老师编的matlab程序,请尊重原作者劳动,引用时请注明出处。注释有一部分来源于网络,对于明显错误的地方我已经更正


    function [R_best,L_best,L_ave,Shortest_Route,Shortest_Length]=ACATSP(C,NC_max,m,Alpha,Beta,Rho,Q)
    %% 主要符号说明
    %% C n个城市的坐标,n×2的矩阵
    %% NC_max 最大迭代次数
    %% m 蚂蚁个数
    %% Alpha 表征信息素重要程度的参数,信息重要程度的参数如果不是固定的,而是动态变化的会怎么样
    %% Beta 表征启发式因子重要程度的参数,同理启发因子也可能会动态变化
    %% Rho 信息素蒸发系数
    %% Q 信息素增加强度系数
    %% R_best 各代最佳路线
    %% L_best 各代最佳路线的长度


    %%第一步:变量初始化
    n=size(C,1);%n表示问题的规模(城市个数)
    D=zeros(n,n);%D表示完全图的赋权邻接矩阵,D(i,j)表示城市i与城市j之间的距离
    %生成n个城市的邻接矩阵
    for i=1:n
        for j=1:n
            if i~=j
                D(i,j)=((C(i,1)-C(j,1))^2+(C(i,2)-C(j,2))^2)^0.5;%欧氏距离
            else
                D(i,j)=eps;      %i=j时不计算,应该为0,但后面的启发因子要取倒数,用eps(浮点相对精度)表示
            end
            D(j,i)=D(i,j);   %对称矩阵
        end
    end


    Eta=1./D;          %Eta为启发因子,这里设为距离的倒数
    Tau=ones(n,n);     %Tau为信息素矩阵
    Tabu=zeros(m,n);   %记录每一次迭代中每一只蚂蚁的旅游路径
    NC=1;               %迭代计数器,记录迭代次数
    R_best=zeros(NC_max,n);       %各代最佳路线,记录每一次迭代的最佳路径
    L_best=inf.*ones(NC_max,1);   %各代最佳路线的长度
    L_ave=zeros(NC_max,1);        %各代路线的平均长度
    %迭代
    %%
    %学习本算法最弄明白以下问题,思考以下问题,把这几个问题弄明白了,蚁群算法你就可以灵活运用

    %1:各城市的邻接矩阵,其值为个城市之间的欧氏距离,对称矩阵(现实应用中也可以是不对称的)

    %2:启发因子矩阵,其值为各城市距离邻接矩阵的倒数,在整个算法流程中,启发因子矩阵是如何发挥作用的,迭代过程中可不可以不断发展变化?,还是一层不变??
    %3:信息素矩阵,信息素为每一个蚂蚁在履行过程中留下的信息,所以一开始初始化时所有的值相同,信息素在迭代过程中如何起作用,如何发展变化
    %4:在某些特殊且复杂的任务中,可能存在一只蚂蚁需要反复访问某一个节点的情况(例如某一个城市为众多城市的中转站,某些城市之间不能直达)
    %5:有的任务不需要返回出发点,只需要访问所有的城市
    %6:有些任务是不需要遍历所有的城市,只需要遍历给定的城市,(从一个城市到达另一个城市,中途必须经过哪些城市,有的城市可选(有的城市必须经过,有的城市不是必须经过的))
    %%
    while NC<=NC_max        %停止条件之一:达到最大迭代次数,停止
    %%第二步:将m只蚂蚁放到n个城市上,为每一只蚂蚁随机指定一个开始的城市
        Randpos=[];   %随即存取
        for i=1:(ceil(m/n))%m只蚂蚁除以n个城市,向上取整
            Randpos=[Randpos,randperm(n)];%一个列向量,每一次生成一个1:n的乱序整数序列,重复ceil(m/n)次
        end
        Tabu(:,1)=(Randpos(1,1:m))';%随机指定每一只蚂蚁一开始所在的城市
        %%第三步:m只蚂蚁按概率函数选择下一座城市,完成各自的周游
        %概率函数如何给定???
        for j=2:n     %遍历还没有旅行过的城市
            for i=1:m %遍历所有的蚂蚁
                visited=Tabu(i,1:(j-1)); %记录已访问的城市,避免重复访问
                J=zeros(1,(n-j+1));       %初始化待访问的城市列表
                P=J;                      %待访问城市的选择概率分布
                Jc=1;%记录每一只蚂蚁还没有访问过的城市个数
                for k=1:n%此处遍历将每一只蚂蚁还没有访问过的城市查找出来,编程中可以使用队列优化此处
                    if length(find(visited==k))==0   %满足条件表示第k个城市没有被第i只蚂蚁访问过
                    J(Jc)=k;                         %将第k个城市添加到待访问城市列表中
                    Jc=Jc+1;                         %待访问的城市个数自加1
                    end
                end
                %下面计算待选城市的概率分布
                for k=1:length(J)%遍历每一只蚂蚁还没有访问过的城市
                    %Alpha信息素指数,启发因子指数
                    %访问概率为信息素的Alpha次方与启发因子的Beta次方的乘积(启发式因子及遇到障碍物改变方向的可能性)
                    P(k)=(Tau(visited(end),J(k))^Alpha)*(Eta(visited(end),J(k))^Beta);%visited(end)表示上一次访问过的城市,J(k)表示还没有访问过的城市,Tau(visited(end),J(k))表示两个城市之间的信息素
                end
                P=P/(sum(P));
                %按概率原则选取下一个城市
                Pcum=cumsum(P);     %cumsum,按列增量求和,即从第一个元素到当前元素的累加和
                Select=find(Pcum>=rand); %若计算的概率大于原来的就选择这条路线,rand每次返回一个0到1之间的随机数
                to_visit=J(Select(1));%随机选择下一个要访问的城市
                Tabu(i,j)=to_visit;%第i只蚂蚁,第j次访问的城市为to_visit,为什么这样选出来的下一个城市是合理的???????????????
            end
        end
        if NC>=2%如果不是第一次迭代,
            Tabu(1,:)=R_best(NC-1,:);
        end
        %%第四步:记录本次迭代最佳路线
        L=zeros(m,1);     %开始距离为0,m*1的列向量,L存储每一只蚂蚁本次迭代周游所有城市的路径长度
        for i=1:m
            R=Tabu(i,:);%R为第i只蚂蚁的旅游路线
            for j=1:(n-1)
                L(i)=L(i)+D(R(j),R(j+1));    %原距离加上第j个城市到第j+1个城市的距离
            end
            L(i)=L(i)+D(R(1),R(n));      %一轮下来后走过的距离,从最后一个点回到原点的距离
        end
        L_best(NC)=min(L);           %最佳距离取最小,第NC次迭代的最短路径长度
        pos=find(L==L_best(NC));
        R_best(NC,:)=Tabu(pos(1),:); %此轮迭代后的最佳路线
        L_ave(NC)=mean(L);           %此轮迭代后的平均距离
        NC=NC+1                      %迭代继续
        %%第五步:更新信息素
        Delta_Tau=zeros(n,n);%开始时信息素为n*n的0矩阵,本次遍历对信息素矩阵的增量
        for i=1:m%遍历每一只蚂蚁
            for j=1:(n-1)
                %Tabu第i行为第i只蚂蚁在本次便利所有城市时访问城市的顺序列表,L(i)为第i只蚂蚁本次旅行路程的总长度,Q为一个正数,路程越短,对信息素的增量越大(Q/L(i))
                Delta_Tau(Tabu(i,j),Tabu(i,j+1))=Delta_Tau(Tabu(i,j),Tabu(i,j+1))+Q/L(i);          
                %此次循环在路径(i,j)上的信息素增量
            end
            Delta_Tau(Tabu(i,n),Tabu(i,1))=Delta_Tau(Tabu(i,n),Tabu(i,1))+Q/L(i);%从最后一个城市返回第一个城市(有些任务是不需要返回原点,只需要遍历所有城市,要求路径最短)
            %此次循环在整个路径上的信息素增量
        end
        %Rho为信息素蒸发系数
        Tau=(1-Rho).*Tau+Delta_Tau; %考虑信息素挥发,更新后的信息素
        %%第六步:禁忌表清零
        Tabu=zeros(m,n);             %%直到最大迭代次数
    end

    %%第七步:输出结果
    Pos=find(L_best==min(L_best)); %找到最佳路径(非0为真)
    Shortest_Route=R_best(Pos(1),:) %最大迭代次数后最佳路径
    Shortest_Length=L_best(Pos(1)) %最大迭代次数后最短距离
    subplot(1,2,1)                  %绘制第一个子图形
    DrawRoute(C,Shortest_Route)     %画路线图的子函数
    subplot(1,2,2)                  %绘制第二个子图形
    plot(L_best)
    hold on                         %保持图形
    plot(L_ave,'r')
    title('平均距离和最短距离')     %标题
    function DrawRoute(C,R)
    %%=========================================================================
    %% DrawRoute.m
    %% 画路线图的子函数
    %%-------------------------------------------------------------------------
    %% C Coordinate 节点坐标,由一个N×2的矩阵存储
    %% R Route 路线
    %%=========================================================================
    N=length(R);
    scatter(C(:,1),C(:,2));
    hold on
    plot([C(R(1),1),C(R(N),1)],[C(R(1),2),C(R(N),2)],'g')
    hold on
    for ii=2:N
    plot([C(R(ii-1),1),C(R(ii),1)],[C(R(ii-1),2),C(R(ii),2)],'g')
    hold on
    end
    title('旅行商问题优化结果 ')
    展开全文
  • java算法分析与设计之单源最短路径(Dijkstra算法)源代码 算法作为计算机专业学生的必修课,同时也是软件开发过程中必备的编程思想,对学习研究计算机专业意义重大;正因为这门课程难,所以除了相关方面的书籍,网络...
  • 最短路径算法的交通管理系统源代码 c++ 最短路径算法的交通管理系统源代码 c++
  • 单源最短路径算法(MapReduce)源代码

    热门讨论 2012-04-18 21:17:39
    单源最短路径算法(MapReduce)源代码,对与hadoop的初学者来说是很好的入门教程
  • 本文用贪婪算法和最小路径算法解决TSP问题,包含源代码,并且已经调试过了,可以使用
  • 算法实验源代码

    2016-12-17 19:51:22
    算法实验源代码,贪心,动态规划,回溯,分治实现0-1背包、多机调度、最长上升子序列、最短路径等问题
  • 各障碍物顶点连线的中点为路径点,相互连接各路径点,将机器人移动的起点和终点限制在各路径点上,利用最短路径算法来求网络图的最短路径,找到从起点P1到终点Pn的最短路径。上述算法使用了连接线中点的条件,因此...
  • 基于蚁群算法实现路径优化的matlab源代码
  • 两点之间的最短路径(Floyd算法源代码 调试通过的
  • 最短路径算法源程序

    2013-12-10 15:10:18
    最短路径算法源程序
  • 个人感觉确实很不错A算法详解(个人认为最详细,最通俗易懂的一个版本)https://blog.csdn.net/hitwhylz/article/details/23089415A*算法(附c源码)https://www.cnblogs.com/mingbujian/p/4915546.html...

    前段时间用stm32F407制作开发智能仓库小车使用到的这个算法

    借鉴于以下两个网站,个人感觉确实很不错

    A星算法详解(个人认为最详细,最通俗易懂的一个版本)

    https://blog.csdn.net/hitwhylz/article/details/23089415


    A*算法(附c源码)
    https://www.cnblogs.com/mingbujian/p/4915546.html
    展开全文
  • 弗洛伊德算法 源代码

    2009-08-25 16:41:33
    弗洛伊德算法 源代码 求出一对顶点之间的嘴短路径
  • 附送Kruskal最小生成树算法,都是本人的劳动成果,包含输入输出的完整控制台程序,希望大家下完顶一下:)
  • 关于多目标车辆路径优化问题的源代码,C语言,包含文档描述。
  • 最短路径源代码 我觉得还可以 实现最短路径算法
  • 1、蚁群算法的优化计算-旅行商问题(TSP)优化-MATLAB源代码; 2、基于蚁群算法的二维路径规划算法-MATLAB源代码; 3、基于蚁群算法的三维路径规划算法-MATLAB源代码
  • matlab求解遗传算法源代码,包含十余种,例如选址分配,路径
  • 蜂群算法matlab源代码

    2012-11-08 16:35:39
    蜂群算法matlab源代码,解决优化问题
  • 同伦算法matlab源代码

    2017-04-29 15:38:26
    求解非线性方程组的同伦算法matlab源代码
  • 本次博文主要介绍apollo 5.0版本内... 路径规划其实已经发展很多年,从早期的机器人到现在的无人驾驶,主要的方法包括 采样法,图搜索法,数值优化法等,具体可以查阅相关文献阅读。本篇文章主要讲述apollo轨迹规划...

         本次博文主要介绍apollo 5.0版本内使用的轨迹规划算法----public road,该算法的核心思想是PV解耦,即Path-Velocity的解耦,其主要包含两个过程:1.路径规划,2.速度规划。

          路径规划其实已经发展很多年,从早期的机器人到现在的无人驾驶,主要的方法包括 采样法,图搜索法,数值优化法等,具体可以查阅相关文献阅读。本篇文章主要讲述apollo轨迹规划模块里面的路径规划,有时间再更新之后的速度规划。

        与之前EM规划和Lattice规划不同,当前5.0版本使用的路径规划,更加的灵活方便,原因主要是采用了数值优化的思想,通过边界约束等,保证了密集障碍物场景的灵活性。 而之前的lattice等算法由于采样的局限,导致在复杂环境下可能存在无解的情况。言归正传,apollo在内部的路径规划里主要包括以下几个步骤。 (由于场景的差异性,task与stage也有所不同,因此本文只讲述lane follow scenario).

    Status LaneFollowStage::PlanOnReferenceLine(
        const TrajectoryPoint& planning_start_point, Frame* frame,
        ReferenceLineInfo* reference_line_info) {
      if (!reference_line_info->IsChangeLanePath()) {
        reference_line_info->AddCost(kStraightForwardLineCost);
      }

      我们通过 lane_follow_stage.cc文件中 Status LaneFollowStage::PlanOnReferenceLine 函数可以看到具体工作细节

      首先,会根据

     for (auto* optimizer : task_list_) {
        const double start_timestamp = Clock::NowInSeconds();
        ret = optimizer->Execute(frame, reference_line_info);
        if (!ret.ok()) {
          AERROR << "Failed to run tasks[" << optimizer->Name()
                 << "], Error message: " << ret.error_message();
          break;
        }
      }

    当前先验信息判断是否当前参考线是可换道的车道,如果不是那么增加cost。 随后,开始了task的process过程,不同的stage有不同的task,具体可通过 conf/scenario文件夹下的pb.txt,例如:

    scenario_type: LANE_FOLLOW
    stage_type: LANE_FOLLOW_DEFAULT_STAGE
    stage_config: {
      stage_type: LANE_FOLLOW_DEFAULT_STAGE
      enabled: true
      task_type: LANE_CHANGE_DECIDER
      task_type: PATH_LANE_BORROW_DECIDER
      task_type: PATH_BOUNDS_DECIDER
      task_type: PIECEWISE_JERK_PATH_OPTIMIZER
      task_type: PATH_ASSESSMENT_DECIDER
      task_type: PATH_DECIDER
      task_type: RULE_BASED_STOP_DECIDER
      task_type: SPEED_BOUNDS_PRIORI_DECIDER
      task_type: DP_ST_SPEED_OPTIMIZER
      task_type: SPEED_DECIDER
      task_type: SPEED_BOUNDS_FINAL_DECIDER
      task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
      task_type: DECIDER_RSS

    上述task中,根据名称可以看出,path都是与路径相关,从rule_based之后则是与速度规划相关。

    按照任务顺序:

    1.lane change decider: 该任务主要是用来处理refer_line_info,内部有个状态机,根据换道成功时间与换道失败时间以及当前位置与目标位置来切换状态,以此来处理refer_line_info的changelane信息。

    2.lane borrow:该任务相对复杂,主要涉及了一些rules,判断是否可以进行借道超车,rules包括:距离信号交叉口的距离,与静态障碍物的距离,是否是单行道,是否所在车道左右车道线是虚线等规则,以此来判断是否可以借道超车;

    3.path bound decider:根据上一任务的结果,来生成相应的道路边界信息,包括三部分:原车道(左右横向位移0.5m),左超车道,右超车道(具体横向位移根据HDMap及周围障碍物在sl坐标系下的位置所决定);path bound的结果将会作为下一步结果的边界约束,具体原理可查看apollo公开课。

    根据上述边界可以构建optimization method。

    由此开始了第四个任务:piecewise jerk path optimazation。原理可请查看 该篇博文公开课路径规划原理

    本篇文章主要讲述内部的代码推理,主要涉及osqp优化求解问题。

        bool res_opt =
            OptimizePath(init_frenet_state.second, end_state,
                         path_boundary.delta_s(), path_boundary.boundary(),
                         ddl_bounds, w, &opt_l, &opt_dl, &opt_ddl, max_iter);

    上述是优化问题的函数接口,

        
    bool PiecewiseJerkPathOptimizer::OptimizePath(
    std::vector<double>* dx, std::vector<double>* ddx, const int max_iter) {
    //初始化状态,将问题转化为 qp问题
      PiecewiseJerkPathProblem piecewise_jerk_problem(lat_boundaries.size(),
                                                      delta_s, init_state);
    
      piecewise_jerk_problem.set_end_state_ref({1000.0, 0.0, 0.0}, end_state);
      if (end_state[0] != 0) {
        std::vector<double> x_ref(lat_boundaries.size(), end_state[0]);
        piecewise_jerk_problem.set_x_ref(10.0, x_ref);
      }
    // 初始化设置各项权重
      piecewise_jerk_problem.set_weight_x(w[0]);
      piecewise_jerk_problem.set_weight_dx(w[1]);
      piecewise_jerk_problem.set_weight_ddx(w[2]);
      piecewise_jerk_problem.set_weight_dddx(w[3]);
    
      piecewise_jerk_problem.set_scale_factor({1.0, 10.0, 100.0});
    
      auto start_time = std::chrono::system_clock::now();
    //初始化各项变量的边界
      piecewise_jerk_problem.set_x_bounds(lat_boundaries);
      piecewise_jerk_problem.set_dx_bounds(-FLAGS_lateral_derivative_bound_default,
                                           FLAGS_lateral_derivative_bound_default);
      piecewise_jerk_problem.set_ddx_bounds(ddl_bounds);
      piecewise_jerk_problem.set_dddx_bound(FLAGS_lateral_jerk_bound);
      bool success = piecewise_jerk_problem.Optimize(max_iter);
    

    上面是qp问题求解的入口,调用osqp标准求解库,

      //首先进行osqp配置
      OSQPSettings* settings = SolverDefaultSettings();
      settings->max_iter = max_iter;
    
      OSQPWorkspace* osqp_work = osqp_setup(data, settings);
    
      osqp_solve(osqp_work);
    
    
    
    PiecewiseJerkProblem::FormulateProblem()//该函数是重点,主要包含了目标函数与响应约束的建立,以及目标函数的offset补偿。
    
    // x(i)^2 * (w_x + w_x_ref)
      for (int i = 0; i < n - 1; ++i) {
        columns[i].emplace_back(
            i, (weight_x_ + weight_x_ref_) / (scale_factor_[0] * scale_factor_[0]));
        ++value_index;
      } 
      // x(n-1)^2 * (w_x + w_x_ref + w_end_x)
      columns[n - 1].emplace_back(
          n - 1, (weight_x_ + weight_x_ref_ + weight_end_state_[0]) /
                     (scale_factor_[0] * scale_factor_[0]));
      ++value_index;
    
      // x(i)'^2 * w_dx
      for (int i = 0; i < n - 1; ++i) {
        columns[n + i].emplace_back(
            n + i, weight_dx_ / (scale_factor_[1] * scale_factor_[1]));
        ++value_index;
      }
      // x(n-1)'^2 * (w_dx + w_end_dx)
      columns[2 * n - 1].emplace_back(2 * n - 1,
                                      (weight_dx_ + weight_end_state_[1]) /
                                          (scale_factor_[1] * scale_factor_[1]));
      ++value_index;
    
      auto delta_s_square = delta_s_ * delta_s_;
      // x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
      columns[2 * n].emplace_back(2 * n,
                                  (weight_ddx_ + weight_dddx_ / delta_s_square) /
                                      (scale_factor_[2] * scale_factor_[2]));
      ++value_index;
      for (int i = 1; i < n - 1; ++i) {
        columns[2 * n + i].emplace_back(
            2 * n + i, (weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square) /
                           (scale_factor_[2] * scale_factor_[2]));
        ++value_index;
      }
      columns[3 * n - 1].emplace_back(
          3 * n - 1,
          (weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) /
              (scale_factor_[2] * scale_factor_[2]));
      ++value_index;
    
      // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
      for (int i = 0; i < n - 1; ++i) {
        columns[2 * n + i].emplace_back(2 * n + i + 1,
                                        (-2.0 * weight_dddx_ / delta_s_square) /
                                            (scale_factor_[2] * scale_factor_[2]));
        ++value_index;
      }
    
    上述设置了目标函数,主要包括:
    l^2+l'^2+l''^2,以及l'''^2,其中l'''通过l''前后两帧之差与delta_s之比替代。

    osqp内部的核函数矩阵形式是csc格式,需要具体了解该形式的同学,可以百度搜索csc矩阵或通过osqp官方demo学习。

    当设计完目标函数矩阵后,开始设置相应的约束,

    
    void PiecewiseJerkProblem::CalculateAffineConstraint(
        std::vector<c_float>* A_data, std::vector<c_int>* A_indices,
        std::vector<c_int>* A_indptr, std::vector<c_float>* lower_bounds,
        std::vector<c_float>* upper_bounds) {
      // 3N params bounds on x, x', x''
      // 3(N-1) constraints on x, x', x''
      // 3 constraints on x_init_
    
    //说的很详细,主要包含 变量边界约束,三个运动学公式约束以及初始状态约束,边界约束主要是横向最大位移、最大速度、最大加速度等,运动学公式主要是 x(i->i+1)''' = (x(i+1)'' - x(i)'') / delta_s等等,可参考我给的博文

    首先是变量约束,通过赋值变量上下边界,完成约束设置。

      // set x, x', x'' bounds
      for (int i = 0; i < num_of_variables; ++i) {
        if (i < n) {
          variables[i].emplace_back(constraint_index, 1.0);
          lower_bounds->at(constraint_index) =
              x_bounds_[i].first * scale_factor_[0];
          upper_bounds->at(constraint_index) =
              x_bounds_[i].second * scale_factor_[0];
        } else if (i < 2 * n) {
          variables[i].emplace_back(constraint_index, 1.0);
    
          lower_bounds->at(constraint_index) =
              dx_bounds_[i - n].first * scale_factor_[1];
          upper_bounds->at(constraint_index) =
              dx_bounds_[i - n].second * scale_factor_[1];
        } else {
          variables[i].emplace_back(constraint_index, 1.0);
          lower_bounds->at(constraint_index) =
              ddx_bounds_[i - 2 * n].first * scale_factor_[2];
          upper_bounds->at(constraint_index) =
              ddx_bounds_[i - 2 * n].second * scale_factor_[2];
        }
        ++constraint_index;
      }
    

    随后是运动学约束,可能这里各位同学会搞混,按照代码的约束,由于csc就是这样的写法,各位同学可以画一个矩阵,这些约束都是按照constrain index作为序列标号,variables[i]在这里只是调用了第i个变量,后面的-1.0代表该变量的相关系数。以这个for循环为例, 约束形式应该是: 0*variable[0]+0*variable[1]+....-variables[2*n+i]+variables[2*n+i+1] = 0(上下界都是0,因此等于0)正好与之前运动学约束对应。其他的同理。

      // x(i->i+1)''' = (x(i+1)'' - x(i)'') / delta_s 以constrain index作为序列
      for (int i = 0; i + 1 < n; ++i) 
      {
        variables[2 * n + i].emplace_back(constraint_index, -1.0);
        variables[2 * n + i + 1].emplace_back(constraint_index, 1.0);
        lower_bounds->at(constraint_index) =
            dddx_bound_.first * delta_s_ * scale_factor_[2];
        upper_bounds->at(constraint_index) =
            dddx_bound_.second * delta_s_ * scale_factor_[2];
        ++constraint_index;
      }
    
      // x(i+1)' - x(i)' - 0.5 * delta_s * x(i)'' - 0.5 * delta_s * x(i+1)'' = 0
      for (int i = 0; i + 1 < n; ++i) {
        variables[n + i].emplace_back(constraint_index, -1.0 * scale_factor_[2]);
        variables[n + i + 1].emplace_back(constraint_index, 1.0 * scale_factor_[2]);
        variables[2 * n + i].emplace_back(constraint_index,
                                          -0.5 * delta_s_ * scale_factor_[1]);
        variables[2 * n + i + 1].emplace_back(constraint_index,
                                              -0.5 * delta_s_ * scale_factor_[1]);
        lower_bounds->at(constraint_index) = 0.0;
        upper_bounds->at(constraint_index) = 0.0;
        ++constraint_index;
      }
    
      // x(i+1) - x(i) - delta_s * x(i)'
      // - 1/3 * delta_s^2 * x(i)'' - 1/6 * delta_s^2 * x(i+1)''
      auto delta_s_sq_ = delta_s_ * delta_s_;
      for (int i = 0; i + 1 < n; ++i) {
        variables[i].emplace_back(constraint_index,
                                  -1.0 * scale_factor_[1] * scale_factor_[2]);
        variables[i + 1].emplace_back(constraint_index,
                                      1.0 * scale_factor_[1] * scale_factor_[2]);
        variables[n + i].emplace_back(
            constraint_index, -delta_s_ * scale_factor_[0] * scale_factor_[2]);
        variables[2 * n + i].emplace_back(
            constraint_index,
            -delta_s_sq_ / 3.0 * scale_factor_[0] * scale_factor_[1]);
        variables[2 * n + i + 1].emplace_back(
            constraint_index,
            -delta_s_sq_ / 6.0 * scale_factor_[0] * scale_factor_[1]);
    
        lower_bounds->at(constraint_index) = 0.0;
        upper_bounds->at(constraint_index) = 0.0;
        ++constraint_index;
      }

    最后是初始状态约束,即最终轨迹的初始状态要与当前车辆状态保持一致。

      // constrain on x_init
      variables[0].emplace_back(constraint_index, 1.0);
      lower_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0];
      upper_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0];
      ++constraint_index;
    
      variables[n].emplace_back(constraint_index, 1.0);
      lower_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1];
      upper_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1];
      ++constraint_index;
    
      variables[2 * n].emplace_back(constraint_index, 1.0);
      lower_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2];
      upper_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2];
      ++constraint_index;
    

    最后,我们将进行offset 补偿,这里主要指的是 最后的参考线要考虑referline这一因素,即初始解。保证尽可能不要有太大偏差,这样有可能给车辆带来不稳定因素,这里主要是给目标函数进行补偿,目标函数的 ref一项。

    其实目标函数在横向位移上有两项: l^2+(l-ref)^2,因此可以看到为什么在目标函数里,l^2的系数乘以2,在这里将第二项进行了拆解,于是有了offset。 即 -2ref*i,这个就对应了。各位细品。至于为什么不考虑ref^2,因为它是个非负实数,并不包含任何变量,因此不影响梯度下降,从而不影响整个函数的求解。因此在此处省略。

    void PiecewiseJerkPathProblem::CalculateOffset(std::vector<c_float>* q) {
      CHECK_NOTNULL(q);
      const int n = static_cast<int>(num_of_knots_);
      const int kNumParam = 3 * n;
      q->resize(kNumParam, 0.0);
    
      if (has_x_ref_) {
        for (int i = 0; i < n; ++i) {
          q->at(i) += -2.0 * weight_x_ref_ * x_ref_[i] / scale_factor_[0];
        }
      }

    最终,pieccewise jerk完成了求解,后面的To jerk函数在这里就不过多介绍了,主要原理就是利用积分,最终得到整条横向位移(沿着s轴),然后通过frenet转化,从而得到笛卡尔坐标系下的具体路径。 由此完成了轨迹规划中的路径规划。

    关于路径规划部分的代码已单独上传到git,链接如下:path_planning

    展开全文
  • java原始路径寻路算法 关于寻路算法的HappyCoders.eu文章系列的源代码: 英文文章 第1部分: 第2部分: 第3部分: 第4部分: 第5部分: 德国文章 方式1: 等级2: 方式3: 方块4: 标题5:

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 109,130
精华内容 43,652
关键字:

a算法路径搜索源代码