上杭网站建设公司,网站未及时续费,黄骅做网站价格,越南的网站建设【 声明#xff1a;版权所有#xff0c;欢迎转载#xff0c;请勿用于商业用途。 联系信箱#xff1a;feixiaoxing 163.com】 要说搜路算法#xff0c;这个大家都比较好理解。毕竟从一个地点走到另外一个地点#xff0c;这个都是直觉上可以感受到的事情。但是这条道路上机…【 声明版权所有欢迎转载请勿用于商业用途。 联系信箱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]但是这个范围不能超过[vminvmax]机器人本身要求的范围。所以最终机器人的速度区间应该是在[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, markersize10)plt.plot([0,14],[0,0],-k,linewidth7)plt.plot([0,14],[14,14],-k,linewidth7)plt.plot([0,0],[0,14],-k,linewidth7)plt.plot([14,14],[0,14],-k,linewidth7)plt.plot([0,6],[10,10],-y,linewidth10)plt.plot([3,8],[5,5],-y,linewidth10)plt.plot([10,10],[7,13],-y,linewidth10)plt.plot(obstacles[:,0], obstacles[:,1],*b,linewidth8)plt.plot(x[0], x[1], ob, markersize10)plt.arrow(x[0], x[1], math.cos(x[2]), math.sin(x[2]), width0.02, fcred)plt.plot(current_traj[:,0], current_traj[:,1], -g, linewidth2)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环境下测试验证一下。最终实现的效果如下所示
