小车常用路径规划算法

### 常见的小型车辆路径规划算法

对于小型车辆而言，常见的路径规划算法主要包括DWA算法、A*算法以及快速探索随机树（RRT）算法。

#### 动态窗口方法(DWA)

动态窗口方法是一种实时避障的运动控制算法。该算法不仅考虑了机器人的动力学模型还兼顾了速度和加速度约束条件下的最优轨迹选取问题。此法通过预测未来一段时间内的多个候选动作序列，并从中挑选出最能满足当前环境需求的动作执行[^1]。

def dwa_control(x, config, goal, ob):
    # 计算线性和角速度范围
    vs = [config.min_speed, config.max_speed,
          config.min_yawrate, config.max_yawrate]
    
    # 预测轨迹并评估其代价函数值
    u, trajectory = calc_final_input(x, u,vs, config,goal,ob)
    
    return u,trajectory

#### A*搜索算法

A*算法属于启发式图搜索过程的一种形式，广泛应用于二维栅格地图上的全局路径寻找任务当中。这种技术利用估计距离加上已知路程总和最小化原则指导搜索方向直至找到通往目的地的最佳路线为止。

import heapq

def a_star_search(graph,start,goal):
    frontier= PriorityQueue()
    frontier.put(start,0)
    came_from={}
    cost_so_far={start:0}
    
    while not frontier.empty():
        current=frontier.get()

        if current==goal:
            break
        
        for next in graph.neighbors(current):
            new_cost=cost_so_far[current]+graph.cost(current,next)
            
            if next not in cost_so_far or new_cost<cost_so_far[next]:
                cost_so_far[next]=new_cost
                priority=new_cost+heuristic(goal,next)
                frontier.put(next,priority)
                came_from[next]=current
                
    return came_from,cost_so_far

#### 快速探索随机树(RRT)

快速探索随机树算法是一种基于概率的方法论框架下解决复杂环境中移动机器人自主导航难题的有效途径之一。尤其擅长处理高维空间内存在大量不确定因素的情况；它借助蒙特卡洛模拟原理构建一棵连通初始状态至目标区域之间所有可达点集构成的一棵无环有向图——即所谓的“树”，并通过反复尝试新增节点的方式逐步逼近理想解方案[^2]。

function path=RRT(q_start,q_goal,map,max_iter)
% 初始化参数设置
q_tree=q_start;
parent=zeros(size(map));

for i=1:max_iter
    q_rand=rand_config();
    [~,idx]=min(dist(q_tree-q_rand));
    q_near=q_tree(idx,:);
    q_new=extend_towards(q_near,q_rand);
    
    % 如果新加入的位置合法则更新树结构
    if is_valid_position(q_new,map)==true && ...
       dist(q_new-q_goal)<epsilon
           parent(length(q_tree)+1)=idx;
           q_tree=[q_tree;q_new];
           
           reconstruct_path(parent,q_tree,length(q_tree))
           return;
    end
    
end
path=[];
end