手把手教用matlab做无人驾驶（十八）--Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame

                    Frenet坐标下动态街道场景的最优轨迹生成           

目录          

1.摘要（Abstract）

2.论文主要内容

3.Matlab代码实现与仿真

---

1.摘要

动态高速公路和市内场景的无人驾驶汽车安全处理包括生成合理的交通轨迹。为了解决全自动驾驶系统的实际需求，我们提出了一种与行为层紧密结合的半被动的轨迹生成方法。通过使用在Frenet坐标系下的最优控制策略实现街道的被动避障，该方法实现的长期目标为：速度保持、合并、跟车、停车。最后在一个模拟的典型的高速公路场景验证了该方法。

 

2.论文主要内容

论文通过下图给出了主要的结论：

                                  

这里，横向使用如下公式：

                      

纵向使用速度保持公式：

                        

最后组合：

                           

3.Matlab代码实现与仿真

对上面公式的核心代码：

  for di=obj.MIN_ROAD_WIDTH: obj.D_ROAD_W:obj.MAX_ROAD_WIDTH
                %Lateral motion palnning
                for Ti=obj.MINT:obj.DT:obj.MAXT
                    
                    % Generate quintic polynomial for lateral plan using dynamics
                    % d0; start position offset
                    % dd0: start lateral velocity
                    % ddd0: start lateral acceleration
                    % di: Varoated lateral target lateral position
                    % ddT: lateral target velocity
                    % dddT: Lateral target acceleration
                    ddT=0;
                    dddT=0;
                    latPoly5 = QuinticPoly(d0, dd0, ddd0, di, ddT, dddT, Ti);
                    
                    %create a frenet trajetory consisting of 
                    % s (longitudinal) and d (lateral) dynamics
                    % and initialize the lateral (d) part
                    ft=FrenetTrajectory();
                    ft.t=0.0:obj.DT:Ti;
                    ft.d=latPoly5.X(ft.t);
                    ft.dd=latPoly5.dX(ft.t);
                    ft.ddd=latPoly5.ddX(ft.t);
                    ft.dddd=latPoly5.dddX(ft.t);
%                     hold on
%                     plot(ft.t,ft.d,'*r')
                    
                    % Longitudinal motion planning (velocity keeping)
                    for  tv = (obj.TARGET_SPEED - obj.D_T_S * obj.N_S_SAMPLE): obj.D_T_S: (obj.TARGET_SPEED...
                            + obj.D_T_S * obj.N_S_SAMPLE)
                          targetft=ft;
                          lonPoly4=QuarticPoly(s0, ds0, 0.0, tv, 0.0, Ti);
                          
                          targetft.s=lonPoly4.X(ft.t);
                          targetft.ds=lonPoly4.dX(ft.t);
                          targetft.dds=lonPoly4.ddX(ft.t);
                          targetft.ddds=lonPoly4.dddX(ft.t);
%                            hold on
%                            plot(ft.t,targetft.s,'*g')
                          % Square of lateral jerk
                          Jd = sum(targetft.dddd.^2);
                          % Square of longitudinal jerk
                          Js = sum(targetft.ddds.^2);  
                          
                          % Square of diff from target speed
                          dv = (obj.TARGET_SPEED - targetft.ds(end)).^2;
                          
                          targetft.Jd = obj.KJ * Jd + obj.KT * Ti + obj.KD * targetft.d(end)^2 ;
                          targetft.Js = obj.KJ * Js + obj.KT * Ti + obj.KV * dv;
                          targetft.J=obj.KLAT * targetft.Jd + obj.KLON * targetft.Js;

然后转换到全局坐标系下：

function frenetTrajectories = CalcGlobalTrajectories(obj, frenetTrajectories, referencePath)
            for iTarj = 1: length(frenetTrajectories)
                 ft = frenetTrajectories{iTarj};
                 % calc global positions
                 for i = 1:(length(ft.s))
                     [ix, iy] = referencePath.calc_position(ft.s(i));
                     if isnan(ix)
                         break
                     end
                     iyaw = referencePath.calc_yaw(ft.s(i));
                     di = ft.d(i);
                     fx = ix + di * cos(iyaw + pi /2.0);
                     fy = iy + di * sin(iyaw + pi / 2.0);
                     ft.x(end+1)=fx;
                     ft.y(end+1)=fy;
                     
                 end
%                  plot(ft.x, ft.y, 'color', [1, 1, 1]*0.5)
%                  drawnow;
                 
                 % calc theta and dL (running length)
                 for i = 1: (length(ft.x) - 1) 
                     dx = ft.x(i+1) - ft.x(i);
                     dy = ft.y(i+1) - ft.y(i);
                     ft.theta(end+1) = atan2(dy, dx);
                     ft.dL(end+1) = sqrt(dx^2 + dy^2);
                 end
                 
                 ft.theta(end+1) = ft.theta(end);
                 ft.dL(end+1) = ft.dL(end);
                 
                 % calc curvature
                 for i = 1: (length(ft.theta) - 1)
                     ft.kappa(end+1) = (ft.theta(i+1) - ft.theta(i)) / ft.dL(i) ;
                 end
                 ft.kappa(end+1) = ft.kappa(end);
                 
                 frenetTrajectories{iTarj} = ft;
                 
            end
        end

通过最大加速度限制，最大曲率限制，最大速度限制，障碍物碰撞等筛选出符合的轨迹，最后根据最小代价函数选择最优轨迹：

function okTrajectories = CheckTrajectories(obj, frenetTrajectories, objects)
            okTrajectories = {};
            for i = 1 : (length( frenetTrajectories))
                ft = frenetTrajectories{i};
                if any(ft.ds > obj.MAX_SPEED)  % Max speed check
                    continue
                elseif any(abs(ft.dds) > obj.MAX_ACCEL)     % Max accleration check
                    continue
                elseif any(abs(ft.kappa) > obj.MAX_CURVATURE)   % Max curvature check
                    continue
                elseif (obj.CheckCollision(ft, objects)==1)
                    continue
                end
                okTrajectories{end+1} = ft;
%                 plot(ft.x, ft.y, 'g');
%                 drawnow;
            end
        end

最后的仿真结果如下图：

 

csdn代码下载地址https://download.csdn.net/download/caokaifa/12716221，欢迎交流

github:https://github.com/caokaifa/Matlab-planning

---