dwa轨迹规划，局部路径规划

傷城~ 2023-01-19 09:41 16阅读 0赞

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 1][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 2][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 3][]

# 更新机器人状态的函数 u  ；  u：卡尔曼滤波中表示控制向量
    def updateState(state,u,dt):
        """
        :param state [x,y,里程计走的角度,当前最新的速度,当前最新的角速度]:
        :param u 控制向量 [当前速度,当前角速度]:
        :param dt: 时间间隔，经过dt时间后 推算出x y 角度的值
        """
        # xt= xt-1 + v*cos（角度）*dt     xt-1 ：上一次的坐标
        #当前的状态， 当前x 坐标 =state[0]：上一次的坐标 +加上 u[0]：速度 *乘以   math.cos(state[2]：之前的角度) *乘以时间间隔dt
        state[0] = state[0] + u[0]*math.cos(state[2]) * dt
        # y 当前y  坐标
        state[1] = state[1] + u[0]*math.sin(state[2]) * dt
        # 角度 = 上一次的角度*里程计走的角度*时间间隔
        state[2] = state[2] + u[1] * dt
        # 存最新的线速度
        state[3] = u[0]
        # 存最新的角速度
        state[4] = u[1]
        return state
    
    # 计算小车轨迹的函数  state:初始状态  u:控制向量 [速度,角速度]; config：配置
    def calc_path(state,u,config):
        """
        根据当前的状态,向未来预测轨迹
        :param state: 小车当前的状态
        :param u:     控制向量   [当前的速度,当前的角速度]
        :param config: dwa算法的配置
        :return: 返回生成的轨迹
        """
        state = np.array(state)
        path = np.array(state)
        time = 0
    	#时间从0开始到config.sim_time结束，时间间隔
        while time <= config.sim_time:
    	    # 更新机器人状态的函数
            state = updateState(state,u,config.dt)
            #所有的轨迹存起来；vstack：垂直的方式堆叠
            path = np.vstack( (path,state) )
            #config.dt 时间间隔
            time += config.dt
    
    
        return path;

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 4][]

# 计算一个动态的速度窗口集合  state：状态； config：配置；【计算速度的取值范围】
    def calc_dynamic_window(state,config):
        # 小车能够达到的最大速度 角速度
        vel_limit = [config.min_vel,config.max_vel,-config.max_rot_vel,config.max_rot_vel]
    
        # 根据小车当前的速度 角速度, 计算在一个采样周期内dt最大的速度 和 角速度
        # 当前速度 = 初始速度 + 加速度* dt
    	# state [x,y,里程计走的角度,当前的速度,当前的角速度]:
        vel_limit_dt = [
            state[3] - config.accel_vel * config.dt,#采样时间dt内最小能够到达的速度； state[3] ：当前的速度；
            state[3] + config.accel_vel * config.dt,#采样时间dt内最大能够到达的速度； state[3] ：当前的速度；
    
            state[4] - config.accel_rot * config.dt,#采样时间dt内最小能够到达的角速度； state[4] ：当前的角速度；
            state[4] + config.accel_rot * config.dt,#采样时间dt内最大能够到达的角速度； state[4] ：当前的角速度；
        ]
    
        # 求两个集合的交集
        vel_limit_window = [
            max(vel_limit[0],vel_limit_dt[0]), min(vel_limit[1],vel_limit_dt[1]),
    
            max(vel_limit[2], vel_limit_dt[2]), min(vel_limit[3], vel_limit_dt[3]),
        ]
    
        return vel_limit_window

所有代码

#encoding:utf-8
    
    import math
    import numpy as np
    import matplotlib.pyplot as plt
    
    """
    1. 在线速度和角速度最大范围内,迭代,找出最优的线速度和角速度
    2. 评价轨迹
        2.1 末端离目标点越近越好
        2.2 末端离障碍物越远越好
        2.3 速度越大越好
    """
    class Config():
        """
        配置对象
        """
        def __init__(self):
            print("初始化")
            # 设置小车最大的线速度
            self.max_vel = 0.3 # m/s
            # 设置小车最小的线速度
            self.min_vel = 0.1 # 不允许倒车
            # 设置小车最大的角速度
            self.max_rot_vel = math.radians(120)
    
            # 加速度  最大加速度
            self.accel_vel = 0.8
            # 角速度的加速度 最大角加速度 弧度/秒
            self.accel_rot = math.radians(120)
    
            # 采样时间 0.1
            self.dt = 0.1
            # 预测3秒内的轨迹
            self.sim_time = 3
    
            # 配置$$ G(v,w) = weight_1*goal + weight_2*(1/dist) + weight_3*velocity$$
            self.goal_weight = 10   #距离目标的权重，目标权重
            self.dist_weight = 10   #距离障碍物的权重
            self.vel_weight = 10    #速度的权重
    
            # 假设机器人是圆形的，配置机器人的尺寸 机器人的半径
            self.robot_radius = 0.1
    
    # 计算一个动态的速度窗口集合  state：状态； config：配置；【计算速度的取值范围】
    def calc_dynamic_window(state,config):
        # 小车能够达到的最大速度 角速度
        vel_limit = [config.min_vel,config.max_vel,-config.max_rot_vel,config.max_rot_vel]
    
        # 根据小车当前的速度 角速度, 计算在一个采样周期内dt最大的速度 和 角速度
        # 当前速度 = 初始速度 + 加速度* dt
    	# state [x,y,里程计走的角度,当前的速度,当前的角速度]:
        vel_limit_dt = [
            state[3] - config.accel_vel * config.dt,#采样时间dt内最小能够到达的速度； state[3] ：当前的速度；
            state[3] + config.accel_vel * config.dt,#采样时间dt内最大能够到达的速度； state[3] ：当前的速度；
    
            state[4] - config.accel_rot * config.dt,#采样时间dt内最小能够到达的角速度； state[4] ：当前的角速度；
            state[4] + config.accel_rot * config.dt,#采样时间dt内最大能够到达的角速度； state[4] ：当前的角速度；
        ]
    
        # 求两个集合的交集
        vel_limit_window = [
            max(vel_limit[0],vel_limit_dt[0]), min(vel_limit[1],vel_limit_dt[1]),
    
            max(vel_limit[2], vel_limit_dt[2]), min(vel_limit[3], vel_limit_dt[3]),
        ]
    
        return vel_limit_window
    
    # 更新机器人状态的函数 u  ；  u：卡尔曼滤波中表示控制向量
    def updateState(state,u,dt):
        """
        :param state [x,y,里程计走的角度,当前的速度,当前的角速度]:
        :param u 控制向量 [速度,角速度]:
        :param dt: 时间间隔，经过dt时间后 推算出x y 角度的值
        """
        # xt= xt-1 + v*cos（角度）*dt     xt-1 ：上一次的坐标
        #当前的状态， 当前x 坐标 =state[0]：上一次的坐标 +加上 u[0]：速度 *乘以   math.cos(state[2]：之前的角度) *乘以时间间隔dt
        state[0] = state[0] + u[0]*math.cos(state[2]) * dt
        # y 当前y  坐标
        state[1] = state[1] + u[0]*math.sin(state[2]) * dt
        # 角度 = 上一次的角度*里程计走的角度*时间间隔
        state[2] = state[2] + u[1] * dt
        # 线速度
        state[3] = u[0]
        # 角速度
        state[4] = u[1]
        return state
    
    # 计算小车轨迹的函数  state:初始状态  u:控制向量 [速度,角速度]; config：配置
    def calc_path(state,u,config):
        """
        根据当前的状态,向未来预测轨迹
        :param state: 小车当前的状态
        :param u:     控制向量   [当前的速度,当前的角速度]
        :param config: dwa算法的配置
        :return: 返回生成的轨迹
        """
        state = np.array(state)
        path = np.array(state)
        time = 0
    	#时间从0开始到config.sim_time结束
        while time <= config.sim_time:
    	    # 更新机器人状态的函数
            state = updateState(state,u,config.dt)
            #所有的轨迹存起来；vstack：垂直的方式堆叠
            path = np.vstack( (path,state) )
            #config.dt 时间间隔
            time += config.dt
    
    
        return path;
    
    
    # 评价轨迹，评价目标点的轨迹
    def calc_goal_cost(path,goal,config):
        """
        计算与目标点的代价
        :param path: 计算得到的轨迹
        :param goal: 目标点
        :param config:
        """
        # 获取轨迹末端的点
        path_x = path[-1,0]
        path_y = path[-1,1]
        # 计算x边与y边的长度 goal目标点
        diff_x = goal[0] - path_x;
        diff_y = goal[1] - path_y
         #计算距离 用勾股定理
        distance = math.sqrt(diff_x**2 + diff_y**2)
    
        # 代价
        cost = config.goal_weight * distance
        return cost
    
    
    # 评价距障碍物的距离
    def calc_obstacle_cost(path,obstacle,config):
        min_dist = 3413413241
        """计算与障碍物的距离"""
        for path_state in path:
    	    #遍历制造障碍物
            for i in range(len(obstacle[:, 0])):
                #print(ob)
                # 与障碍物的距离
                px = path_state[0]
                py = path_state[1]
    
                ox = obstacle[i,0]
                oy = obstacle[i,1]
    
                distance = math.sqrt((px-ox)**2 + (py-oy)**2)
    
                #  校验是否会与机器人发生碰撞
                if distance < config.robot_radius:
                    print("机器人可能会发生碰撞")
                    return float("Inf")
    
                if  distance <= min_dist:
                    min_dist = distance;
    
        return  config.dist_weight*(1/min_dist);
    
    def draw_path(paths,best_path,state,goal,obstacles):
    
        plt.gca();
    
        for path in paths:
            plt.axis("equal")
            plt.grid(True)
            plt.plot(0, 0, "og")
            plt.plot(path[:,0],path[:,1],'r-')
    
        # 将障碍物绘制出来
        plt.plot(obstacles[:,0], obstacles[:,1], "bo")
    
        # 将起始点绘制出来
        plt.plot(goal[0], goal[1], "ro")
    
        # 将最优的路径绘制出来
        plt.plot(best_path[:, 0], best_path[:, 1], 'g*')
        plt.show()
    
    
    if __name__ == '__main__':
    
        config = Config()
        ## 初始状态   state [x,y,里程计走的角度,当前的速度,当前的角速度]  
    	# x：坐标为0 ；y：坐标为0 ；里程计走的角度math.pi/2.0；当前的速度0.2；当前的角速度0
        init_state = [0,0,math.pi/2.0,0.2,0]
        # 控制向量  [速度,角速度]
        u = [0.3,0.0]
    
        # 目标点位置()
        goal = np.array([0,1])
    	# 制造障碍物
        obstacles = np.matrix([[0.0,0.5]])
        # # 计算出来的轨迹 计算小车轨迹的函数
        # paths = calc_path(init_state,u,config)
        # # 评价距障碍物的距离
        # calc_obstacle_cost(paths,None,config)
    
        # 计算窗口 小车能够达到的最大速度 角速度 # 计算一个动态的速度窗口集合  state：状态； config：配置；【计算速度的取值范围】
        vel_window =calc_dynamic_window(init_state,config)
    
        paths = []
        min_cost=213412341234
        best_path = None;
        print(vel_window)
        # 轨迹的推算 v 当前的速度,w 当前的角速度 ；vel_window[0]：最线速度小值 ； vel_window[1]：最线速度大值； 步长：0.03
        for v in np.arange(vel_window[0],vel_window[1],0.03):
    	    #vel_window[2]：最小角速度值 ； vel_window[3]：最角速度大值； 步长：0.03
            for w in np.arange(vel_window[2],vel_window[3],math.pi/180):
                u = [v,w]  #控制向量   [当前的速度,当前的角速度]
    
                # 推算一条轨迹
                curr_path = calc_path(init_state,u,config)
                # 代价  G(v,w) = weight_1*goal + weight_2*(1/dist) + weight_3*velocity$$
                total_cost = calc_goal_cost(curr_path,goal,config) + calc_obstacle_cost(curr_path,obstacles,config) + config.vel_weight*(config.max_vel - curr_path[-1,3])
    
                paths.append(curr_path)
    
                # 找出代价最小的轨迹
                if total_cost < min_cost:
                    min_cost = total_cost;
                    best_path = curr_path;#得到最好的轨迹
    
    
        draw_path(paths,best_path,init_state,goal,obstacles)

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70]: /images/20221021/b7e7845e0e9b465ebe09208872e8811b.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 1]: /images/20221021/144b936119e74211a562c16a4120f448.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 2]: /images/20221021/f75b986247c647ae825b1caf0625cfdb.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 3]: /images/20221021/0fbfff3b3801432babbccc0c1cf777c3.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 4]: /images/20221021/c2bc03e780b74e65be1d115161e5919c.png