SLAM从入门到精通(dwa速度规划算法)

2023-10-18 08:36

本文主要是介绍SLAM从入门到精通(dwa速度规划算法),希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

【 声明:版权所有,欢迎转载,请勿用于商业用途。 联系信箱:feixiaoxing @163.com】

        要说搜路算法,这个大家都比较好理解。毕竟从一个地点走到另外一个地点,这个都是直觉上可以感受到的事情。但是这条道路上机器人应该怎么走,以什么样的速度、什么样的角速度走,这里面有很大的学问。一方面,机器人本身的机械特性决定了它的速度、角速度这些参数都有一定范围约束的;另外一方面,不同的速度、角速度走出来的轨迹可能是不一样的,特别是拐弯的时候。这个时候,什么样的轨迹最适合我们机器人,就需要设计出一套标准来甄别了。比如,是越快越好,还是越安全越好,还是说离目标越近越好。

        对于客户来说,速度、角速度肯定是越快越好。但是机械的特性决定了很多时候它快不了,比如转弯的时候,甚至是连续转弯的时候,速度快了反而不安全。正因为有了这些需求,所以才会有了dwa算法设计出来帮助我们来解决这些问题。

1、了解机器人的参数

        每一款机器人都有自己独特的参数,比如最小速度、最大速度;最小角速度、最大角速度;最小线加速度、最大线加速度等等。这些数据都需要做很好的了解。不仅如此,我们还需要知道机器人的最小转弯半径。如果可以原地旋转,这固然很好。但是大多数机器人不一定可以做到这一点。

2、了解机器人的运动学模型

        之前我们说过差速轮的运动学模型,假设速度分别为v和w,那么后面小车的轨迹应该是这样的,

x += v * cos(theta) * dt
y += v * cos(theta) * dt
theta += w * dt

        当然这里描述的只是差速轮的运动学模型,其他机器人的运动学模型也可以通过类似的方法进行计算。

3、速度采样、加速度采样

        以速度为例,机器人本身有一个最小速度,还有一个最大速度。此外,它还有一个最小加速度、最大加速度。所以对于任意时刻的速度v,依据加速度的范围可以得到一个数值[v_min, v_max],但是这个范围不能超过[vmin,vmax]机器人本身要求的范围。所以最终机器人的速度区间应该是在[max(v_min, vmin), min(v_max, vmax)]这个范围之内。加速度也是一样的道理。

4、轨迹评价标准

        本身dwa提供了三个评价标准,分别是目标、速度以及和障碍物的最小距离。当然,这三个标准是不一定适用于所有项目,我们完全可以自己来设计评价标准。

5、测试代码

        dwa算测的测试代码是用python实现的,参考一本ros书上的内容,在此表示感谢。此python代码用python3执行,依赖于库matplotlib,直接输入python3 dwa.py即可。代码内容如下,

import numpy as np
import matplotlib.pyplot as plt
import mathclass Info():def __init__(self):self.v_min = -0.5self.v_max = 3.0self.w_max = 50.0 * math.pi / 180.0self.w_min = -50.0 * math.pi / 180.0self.vacc_max = 0.5self.wacc_max = 30.0 * math.pi / 180.0self.v_reso = 0.01self.w_reso = 0.1 * math.pi / 180.0self.radius = 1.0self.dt = 0.1self.predict_time = 4.0self.goal_factor = 1.0self.vel_factor = 1.0self.traj_factor = 1.0def motion_model(x,u,dt):x[0] += u[0] * dt * math.cos(x[2])x[1] += u[0] * dt * math.sin(x[2])x[2] += u[1] * dtx[3] = u[0]x[4] = u[1]return xdef vw_generate(x,info):Vinfo = [info.v_min, info.v_max,info.w_min, info.w_max]Vmove = [x[3] - info.vacc_max * info.dt,x[3] + info.vacc_max * info.dt,x[4] - info.wacc_max * info.dt,x[4] + info.wacc_max * info.dt]vw = [max(Vinfo[0], Vmove[0]), min(Vinfo[1], Vmove[1]),max(Vinfo[2], Vmove[2]), min(Vinfo[3], Vmove[3])]return vwdef traj_calculate(x,u,info):ctraj = np.array(x)xnew = np.array(x)time = 0while time <= info.predict_time:x_new = motion_model(xnew,u,info.dt)ctraj = np.vstack((ctraj, xnew))time += info.dtreturn ctrajdef dwa_core(x,u,goal,info, obstacles):vw = vw_generate(x,info)best_ctraj = np.array(x)min_score = 10000.0for v in np.arange(vw[0], vw[1], info.v_reso):for w in np.arange(vw[2], vw[3], info.w_reso):ctraj = traj_calculate(x, [v,w], info)goal_score = info.goal_factor * goal_evaluate(ctraj, goal)vel_score = info.vel_factor * velocity_evaluate(ctraj, info)traj_score = info.traj_factor * traj_evaluate(ctraj, obstacles,info)ctraj_score = goal_score + vel_score + traj_scoreif min_score >= ctraj_score:min_score = ctraj_scoreu = np.array([v,w])best_ctraj = ctrajreturn u,best_ctrajdef goal_evaluate(traj, goal):goal_score = math.sqrt((traj[-1,0]-goal[0])**2 + (traj[-1,1]-goal[1])**2)return goal_scoredef velocity_evaluate(traj, info):vel_score = info.v_max - traj[-1,3]return vel_scoredef traj_evaluate(traj, obstacles, info):min_dis = float("Inf")for i in range(len(traj)):for ii in range(len(obstacles)):current_dist = math.sqrt((traj[i,0] - obstacles[ii,0])**2 + (traj[i,1] - obstacles[ii,1])**2)if current_dist <= info.radius:return float("Inf")if min_dis >= current_dist:min_dis = current_distreturn 1/min_disdef obstacles_generate():obstacles = np.array([[0,10],[2,10],[4,10],[6,10],[3,5],[4,5],[5,5],[6,5],[7,5],[8,5],[10,7],[10,9],[10,11],[10,13]])return obstaclesdef local_traj_display(x,goal,current_traj, obstacles):plt.cla()plt.plot(goal[0], goal[1], 'or', markersize=10)plt.plot([0,14],[0,0],'-k',linewidth=7)plt.plot([0,14],[14,14],'-k',linewidth=7)plt.plot([0,0],[0,14],'-k',linewidth=7)plt.plot([14,14],[0,14],'-k',linewidth=7)plt.plot([0,6],[10,10],'-y',linewidth=10)plt.plot([3,8],[5,5],'-y',linewidth=10)plt.plot([10,10],[7,13],'-y',linewidth=10)plt.plot(obstacles[:,0], obstacles[:,1],'*b',linewidth=8)plt.plot(x[0], x[1], 'ob', markersize=10)plt.arrow(x[0], x[1], math.cos(x[2]), math.sin(x[2]), width=0.02, fc='red')plt.plot(current_traj[:,0], current_traj[:,1], '-g', linewidth=2)plt.grid(True)plt.pause(0.001)def main():x = np.array([2,2,45*math.pi/180,0,0])u = np.array([0,0])goal = np.array([8,8])info = Info()obstacles = obstacles_generate()global_traj = np.array(x)plt.figure('DWA Algorithm')for i in range(2000):u,current_traj = dwa_core(x,u,goal,info,obstacles)x = motion_model(x,u,info.dt)global_traj = np.vstack((global_traj, x))local_traj_display(x, goal, current_traj,obstacles)if math.sqrt((x[0]-goal[0])**2 + (x[1]-goal[1])**2 <= info.radius):print("Goal Arrived")breakplt.plot(global_traj[:,0], global_traj[:,1], '-r')plt.show()    if __name__ == "__main__":main()

6、执行效果

        代码本身是一个仿真过程,大家可以下载下来在ubuntu环境下测试验证一下。最终实现的效果如下所示,

这篇关于SLAM从入门到精通(dwa速度规划算法)的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/231448

相关文章

Spring Boot + MyBatis Plus 高效开发实战从入门到进阶优化(推荐)

《SpringBoot+MyBatisPlus高效开发实战从入门到进阶优化(推荐)》本文将详细介绍SpringBoot+MyBatisPlus的完整开发流程,并深入剖析分页查询、批量操作、动... 目录Spring Boot + MyBATis Plus 高效开发实战:从入门到进阶优化1. MyBatis

SpringBoot实现MD5加盐算法的示例代码

《SpringBoot实现MD5加盐算法的示例代码》加盐算法是一种用于增强密码安全性的技术,本文主要介绍了SpringBoot实现MD5加盐算法的示例代码,文中通过示例代码介绍的非常详细,对大家的学习... 目录一、什么是加盐算法二、如何实现加盐算法2.1 加盐算法代码实现2.2 注册页面中进行密码加盐2.

Java时间轮调度算法的代码实现

《Java时间轮调度算法的代码实现》时间轮是一种高效的定时调度算法,主要用于管理延时任务或周期性任务,它通过一个环形数组(时间轮)和指针来实现,将大量定时任务分摊到固定的时间槽中,极大地降低了时间复杂... 目录1、简述2、时间轮的原理3. 时间轮的实现步骤3.1 定义时间槽3.2 定义时间轮3.3 使用时

Python FastAPI入门安装使用

《PythonFastAPI入门安装使用》FastAPI是一个现代、快速的PythonWeb框架,用于构建API,它基于Python3.6+的类型提示特性,使得代码更加简洁且易于绶护,这篇文章主要介... 目录第一节:FastAPI入门一、FastAPI框架介绍什么是ASGI服务(WSGI)二、FastAP

如何通过Golang的container/list实现LRU缓存算法

《如何通过Golang的container/list实现LRU缓存算法》文章介绍了Go语言中container/list包实现的双向链表,并探讨了如何使用链表实现LRU缓存,LRU缓存通过维护一个双向... 目录力扣:146. LRU 缓存主要结构 List 和 Element常用方法1. 初始化链表2.

golang字符串匹配算法解读

《golang字符串匹配算法解读》文章介绍了字符串匹配算法的原理,特别是Knuth-Morris-Pratt(KMP)算法,该算法通过构建模式串的前缀表来减少匹配时的不必要的字符比较,从而提高效率,在... 目录简介KMP实现代码总结简介字符串匹配算法主要用于在一个较长的文本串中查找一个较短的字符串(称为

通俗易懂的Java常见限流算法具体实现

《通俗易懂的Java常见限流算法具体实现》:本文主要介绍Java常见限流算法具体实现的相关资料,包括漏桶算法、令牌桶算法、Nginx限流和Redis+Lua限流的实现原理和具体步骤,并比较了它们的... 目录一、漏桶算法1.漏桶算法的思想和原理2.具体实现二、令牌桶算法1.令牌桶算法流程:2.具体实现2.1

Python中的随机森林算法与实战

《Python中的随机森林算法与实战》本文详细介绍了随机森林算法,包括其原理、实现步骤、分类和回归案例,并讨论了其优点和缺点,通过面向对象编程实现了一个简单的随机森林模型,并应用于鸢尾花分类和波士顿房... 目录1、随机森林算法概述2、随机森林的原理3、实现步骤4、分类案例:使用随机森林预测鸢尾花品种4.1

Spring Security 从入门到进阶系列教程

Spring Security 入门系列 《保护 Web 应用的安全》 《Spring-Security-入门(一):登录与退出》 《Spring-Security-入门(二):基于数据库验证》 《Spring-Security-入门(三):密码加密》 《Spring-Security-入门(四):自定义-Filter》 《Spring-Security-入门(五):在 Sprin

不懂推荐算法也能设计推荐系统

本文以商业化应用推荐为例,告诉我们不懂推荐算法的产品,也能从产品侧出发, 设计出一款不错的推荐系统。 相信很多新手产品,看到算法二字,多是懵圈的。 什么排序算法、最短路径等都是相对传统的算法(注:传统是指科班出身的产品都会接触过)。但对于推荐算法,多数产品对着网上搜到的资源,都会无从下手。特别当某些推荐算法 和 “AI”扯上关系后,更是加大了理解的难度。 但,不了解推荐算法,就无法做推荐系