路径规划

如果你能成功的把目标检测的物体坐标以及车道线的坐标定位到一个坐标系中，那么接下来的事情就可以交给路径规划了。你可以在网上找到很多关于路径规划的算法，在这里只推荐给大家一个资源：

本节示例也受到此资源的启发，我们会把精力聚焦到动态窗口算法上，总的来说此算法的主要思想是：

定义小车的当前状态[坐标x, 坐标y, 转角, 速度, 角速度]

实现预测下一个时间段的方法：motion(x,u)

在给定范围内遍历速度与角速度的预测轨迹

根据障碍，距离，速度的成本评估方法选择出最优的预测轨迹

遍历所有预测轨迹选择最优解

相关论文可参考此处：

实践：

代码下载：

本例测试代码为：DWATest.py, 代码里有详细讲解，主要流程为

while True:

plt.plot(goal[0], goal[1], xb)# 目标点

plt.plot(ob[:, 0], ob[:, 1], ok)# 障碍物点

best_u, best_trajectory=ppClass.searchBestTrajectory(robotState, ob, goal)# 遍历所有轨迹得出成本最低路径

plt.plot(best_trajectory[:, 0], best_trajectory[:, 1], -g)# 绿线指示预测最好轨迹

robotState = ppClass.motion(robotState, best_u)# 更行状态

trajectoryHistory = np.vstack((trajectoryHistory, robotState))# 存储轨迹状态历史

plt.plot(robotState[0], robotState[1], xr)# 机器当前位置

ppClass.plot_arrow(robotState[0], robotState[1], robotState[2], 3 * robotState[3])# 绘制箭头

ppClass.plot_robot(robotState[0], robotState[1], robotState[2])# 绘制机器

在DWAConfig.py文件中,你会看到配置动态窗口的参数，主要如下：

# 设置路径规划参数

__C.PATHPLAN.max_accel = 2.0# 最大加速度 [m/ss]

__C.PATHPLAN.min_accel = -2.0# 最小加速度（允许倒车） [m/ss]

__C.PATHPLAN.yawRange = 0.5 # 偏置范围

__C.PATHPLAN.max_speed = 2.0 # 最大速度

__C.PATHPLAN.predictTime=6.0 # 预测时间

__C.PATHPLAN.dt=0.1 # 时间微分段

__C.PATHPLAN.robotType=RobotType.rectangle #机器形状

__C.PATHPLAN.robotLength=1.2 # 机器长

__C.PATHPLAN.robotWidth=0.5 # 机器宽

__C.PATHPLAN.robotRadius=1.0 # 机器半径

__C.PATHPLAN.velocityRate=0.5 # 速度分辨率

__C.PATHPLAN.yawRate=math.pi / 180.0 # 偏置(转角)分辨率

__C.PATHPLAN.maxYawRate=5 * __C.PATHPLAN.yawRate # 最大转角范围

__C.PATHPLAN.obCostWeight=6.0 # 障碍成本权重

__C.PATHPLAN.distCostWeight=1.0 # 距离成本权重

__C.PATHPLAN.speedCostWeight=2.0 # 速度成本权重

最终效果为：

DWA效果1

DWA效果2

DWA最终轨迹

（路径规划）宝，都看到这里了你确定不收藏一下？？