【自动驾驶】运动规划丨轨迹规划丨 Dijk算法有向带权图

运动规划描述

运动规划子系统负责生成车辆的局部运动轨迹，使自动驾驶汽车能从当前状态到达行为规划子系统定义的下一个局部目标状态，是决定车辆行驶质量的直接因素。运动规划的是传感器范围内的局部驾驶路径，满足汽车的运动学和动力学限制，保证乘客舒适性与安全性，以及避免与环境中的静止和移动障碍物碰撞。

运动规划组成

运动规划主要包括轨迹规划、速度规划。其中，轨迹规划主要生成一系列可行驶轨迹，然后经过代价分析和路径筛选得到最优轨迹；速度规划主要对最优轨迹进行碰撞分析及动力学校验（根据道路曲率及车辆动力学参数限制车速），以及对速度和加速度进行平滑处理。经过运动规划处理，可保证自动驾驶的安全性、舒适性、时效性。

通过简单的计算流程，说明轨迹规划算法的步骤：

1.采样点：在设定车道上进行横纵向坐标点采样；

2.轨迹生成：通过连接采样点生成候选轨迹；

两条候选轨迹示意

3.权重计算：计算每条候选轨迹的权重矩阵；

W =

   Inf     7     9   Inf   Inf   Inf
   Inf   Inf    10    15   Inf   Inf
   Inf   Inf   Inf     6   Inf    11
   Inf   Inf   Inf   Inf     6   Inf
   Inf   Inf   Inf   Inf   Inf     9
   Inf   Inf   Inf   Inf   Inf   Inf

4.有向带权图：生成候选轨迹对应的有向带权图；

W=full(R)          %将稀疏矩阵转换为满阵
 IDS={'1','2','3','4','5','6'};%显示坐标点对应的序号
 bg=biograph(W,IDS);%将满矩阵WW输入
 set(bg.nodes,'shape','circle','color',[1,1,1],'lineColor',[0,0,0]);
 set(bg,'layoutType','radial');
 bg.showWeights='on';
 set(bg.nodes,'textColor',[0,0,0],'lineWidth',2,'fontsize',9);
 set(bg,'arrowSize',12,'edgeFontSize',9);
 get(bg.nodes,'position')
 dolayout(bg)
 %采样坐标点输入
 bg.nodes(1).position=[50,20];
 bg.nodes(2).position=[100,40];
 bg.nodes(3).position=[150,20];
 bg.nodes(4).position=[200,60];
 bg.nodes(5).position=[250,60];
 bg.nodes(6).position=[300,40];
 
dolayout(bg,'pathsonly',true);
h=view(bg)%生成有向图，并显示各路径的权值，并赋给h

5.显示最优轨迹：调用Dijk算法进行搜索，生成起点1到终点6，权重最小的轨迹；

distance = 20；path = 1 3 6