机器人路径规划之动态窗口法

作者丨白鸟无言

来源丨古月居

点击进入—>3D视觉工坊学习交流群

动态窗口法概述

DWA是一种基于速度的局部规划器，可计算达到目标所需的机器人的最佳无碰撞速度。

程序实现

DWA算法主要分三步：

• 计算动态窗口

• 计算最优[ v , ω ]

• 更新机器人状态

流程图如下：

以下代码参考：

https://github.com/AtsushiSakai/PythonRobotics

初始化机器人状态、目标位置、障碍物位置

# 初始化机器人状态 [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
# 目标位置 [x(m), y(m)]
goal = np.array([gx, gy])
# 障碍物位置 [x(m), y(m)]
ob = np.array([[-1, -1], ...... , [13.0, 13.0]])

获取动态窗口

这个动态窗口就是机器人在当前状态下能达到的速度v 和转速ω范围，受到自身机械特性以及当前状态的影响。

def calc_dynamic_window(x, config):
    """
    calculation dynamic window based on current state x
    """
 
 
    # Dynamic window from robot specification
    Vs = [config.min_speed, config.max_speed,
          -config.max_yawrate, config.max_yawrate]
 
 
    # Dynamic window from motion model
    Vd = [x[3] - config.max_accel * config.dt,
          x[3] + config.max_accel * config.dt,
          x[4] - config.max_dyawrate * config.dt,
          x[4] + config.max_dyawrate * config.dt]
 
 
    #  [vmin, vmax, yaw_rate min, yaw_rate max]
    dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
          max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]
 
 
    return dw

计算最优[ v , ω ] 

def calc_control_and_trajectory(x, dw, config, goal, ob):
    """
    calculation final input with dynamic window
    """
 
 
    x_init = x[:]
    min_cost = float("inf")
    best_u = [0.0, 0.0]
    best_trajectory = np.array([x])
 
 
    # 计算动态窗口内所有的采样样本的代价函数
    for v in np.arange(dw[0], dw[1], config.v_reso):
        for y in np.arange(dw[2], dw[3], config.yawrate_reso):
 
 
            trajectory = predict_trajectory(x_init, v, y, config)
 
 
            # 计算代价函数
            to_goal_cost = config.to_goal_cost_gain * calc_to_goal_cost(trajectory, goal)
            speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
            ob_cost = config.obstacle_cost_gain * calc_obstacle_cost(trajectory, ob, config)
 
 
            final_cost = to_goal_cost + speed_cost + ob_cost
 
 
            # 寻找具有最小代价的样本以及它的轨迹
            if min_cost >= final_cost:
                min_cost = final_cost
                best_u = [v, y]
                best_trajectory = trajectory
 
 
    return best_u, best_trajectory

更新状态

根据最优u = [ v , ω ] 更新机器人状态

x = motion(x, u, config.dt)

完整代码参见这里

版权声明：本文为CSDN博主「白鸟无言」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。

原文链接：

https://blog.csdn.net/qq_42688495/article/details/104972832

本文仅做学术分享，如有侵权，请联系删文。

点击进入—>3D视觉工坊学习交流群

干货下载与学习

后台回复：巴塞罗那自治大学课件，即可下载国外大学沉淀数年3D Vison精品课件

后台回复：计算机视觉书籍，即可下载3D视觉领域经典书籍pdf

后台回复：3D视觉课程，即可学习3D视觉领域精品课程

3D视觉工坊精品课程官网：3dcver.com

1.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
2.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
3.国内首个面向工业级实战的点云处理课程
4.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
5.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
6.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
7.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

8.从零搭建一套结构光3D重建系统[理论+源码+实践]

9.单目深度估计方法：算法梳理与代码实现

10.自动驾驶中的深度学习模型部署实战

11.相机模型与标定(单目+双目+鱼眼）

12.重磅！四旋翼飞行器：算法与实战

13.ROS2从入门到精通：理论与实战

14.国内首个3D缺陷检测教程：理论、源码与实战

15.基于Open3D的点云处理入门与实战教程

16.透彻理解视觉ORB-SLAM3：理论基础+代码解析+算法改进

17.机械臂抓取从入门到实战

重磅！粉丝学习交流群已成立

交流群主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、ORB-SLAM系列源码交流、深度估计、TOF、求职交流等方向。

扫描以下二维码，添加小助理微信（dddvisiona）,一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿，微信号：dddvisiona

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、源码分享、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答等进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，6000+星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看，3天内无条件退款

高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~