近期在进行规划算法的学习混合A星Hybrid A star是一种在无人车领域应用较多的算法本文提供matlab的混合A星代码实现能够帮助理解图搜索算法中地图、openlist、closelist等概念了解节点如何拓展如何实现路径回溯。 该m文件只使用了matlab基础函数完成算法实现希望对大家剖析算法原理能够有所帮助。当前代码的路径搜索耗时较长主要在于理解算法原理节点的拓展处采用运动学积分启发式函数只启用了欧式距离有需要者可以引入RS曲线进行解析拓展和启发。 另外此处碰撞检测只进行了矩形四点进行检测如有需要可进行拓展为四边检测。以学习为目的进行分享如有问题希望海涵指出。%Hybrid Astar_自己实现 %% 清空当前的绘图 clear; clc;close all; %% 车辆运动学参数 carParamStrcutstruct(W,0,T_L,0,L,0,D,0,max_delta,0); carParamcarParamStrcut; carParam.W1.0;% 车宽 carParam.T_L1.8;% 车总长 carParam.L1.0; %轴距 carParam.D0.4; %后轴到车尾距离 carParam.max_deltadeg2rad(30); %前轮最大转向角度 %% 定义节点数据格式 起始初始的状态应该还包括方向盘转角因为需要引入方向盘转向惩罚 可以再加入离散筒的索引值加速查找 nodeStruct struct(x,0,y,0,theta,0,steer,0,direction,0,g,0,h,0,f,0,parent,[]); %% 定义规划设置参数 HybridAstarParam.X_res0.5; HybridAstarParam.Y_res0.5; HybridAstarParam.yaw_resdeg2rad(15); %离散筒分辨率 HybridAstarParam.primitiveLen1.0; %每次拓展节点的距离 HybridAstarParam.inte_segs20; %每次拓展节点的距离分为多少段积分 HybridAstarParam.MaxSteerdeg2rad(30) ; %最大允许的方向盘角度 HybridAstarParam.nsteer5 ; %每次拓展的可选方向盘角度数量 %惩罚项 HybridAstarParam.reversePenalty2.0 ; %倒车惩罚 HybridAstarParam.steerPenalty0.2 ; %方向盘转角惩罚 HybridAstarParam.steerChangePenalty0.3 ; %方向盘转角变化惩罚 HybridAstarParam.switchPenalty1.0; %前进、后退档位切换惩罚 HybridAstarParam.obsdistencePenalty1.0; %距离最近障碍物距离惩罚 %其他惩罚。。。 % 终点结束搜索判断阈值 HybridAstarParam.d_threshold0.2; %10cm HybridAstarParam.yaw_thresholddeg2rad(5); %5deg HybridAstarParam.max_it40000; %% 定义地图参数 map_ParamStructstruct(X_max,0,Y_max,0,X_min,0,Y_min,0,X_res,0,Y_res,0); %栅格地图最大最小值 分辨率 map_Parammap_ParamStruct; map_Param.X_max20; map_Param.Y_max20; map_Param.X_min0; map_Param.Y_min0; map_Param.X_res0.5; map_Param.Y_res0.5; %% 定义栅格地图定义起点、终点、障碍物 %栅格地图范围和距离 X_map_minmap_Param.X_min; X_map_maxmap_Param.X_max; X_map_ticmap_Param.X_res; %X方向上的栅格地图间距 X_mapX_map_min:X_map_tic:X_map_max; Y_map_minmap_Param.Y_min; Y_map_maxmap_Param.Y_max; Y_map_ticmap_Param.Y_res; %X方向上的栅格地图间距 Y_mapY_map_min:Y_map_tic:Y_map_max; X_map_sizesize(X_map,2); Y_map_sizesize(Y_map,2); %X Y方向总共多少个栅格 mapzeros(X_map_size,Y_map_size); % 添加障碍物信息 %地图边界 x_obs_indxstate2ind(0:X_map_tic:20,X_map_min,X_map_tic); y_obsstate2ind(0,Y_map_min,Y_map_tic); map(x_obs_indx,y_obs)1; x_obs_indxstate2ind(0:X_map_tic:20,X_map_min,X_map_tic); y_obsstate2ind(20,Y_map_min,Y_map_tic); map(x_obs_indx,y_obs)1; x_obs_indxstate2ind(0,X_map_min,X_map_tic); y_obsstate2ind(0:Y_map_tic:20,Y_map_min,Y_map_tic); map(x_obs_indx,y_obs)1; x_obs_indxstate2ind(20,X_map_min,X_map_tic); y_obsstate2ind(0:Y_map_tic:20,Y_map_min,Y_map_tic); map(x_obs_indx,y_obs)1; %障碍物 x_obs_indxstate2ind(8:X_map_tic:16,X_map_min,X_map_tic); y_obsstate2ind(4,Y_map_min,Y_map_tic); map(x_obs_indx,y_obs)1; x_obs_indxstate2ind(8:X_map_tic:12,X_map_min,X_map_tic); y_obsstate2ind(8,Y_map_min,Y_map_tic); map(x_obs_indx,y_obs)1; x_obs_indxstate2ind(16:X_map_tic:20,X_map_min,X_map_tic); y_obsstate2ind(8,Y_map_min,Y_map_tic); map(x_obs_indx,y_obs)1; x_obs_indxstate2ind(4,X_map_min,X_map_tic); y_obsstate2ind(4:Y_map_tic:16,Y_map_min,Y_map_tic); map(x_obs_indx,y_obs)1; x_obs_indxstate2ind(7:X_map_tic:20,X_map_min,X_map_tic); y_obsstate2ind(16,Y_map_min,Y_map_tic); map(x_obs_indx,y_obs)1; %起点信息 终点信息 start[2,2,deg2rad(0)];%x,y,theta(deg) 这个可以任意取 不需要刚好在某个栅格刻度 goal[18,14,deg2rad(120)]; %整个算法全部采用弧度制 %地图可视化 figure(Color, w); hold on; axis equal; imagesc(X_map, Y_map, 1 - map); colormap(gray); %生成障碍物地图的两行代码 xlim([X_map_min-0.5*X_map_tic,X_map_max0.5*X_map_tic]); ylim([Y_map_min-0.5*Y_map_tic,Y_map_max0.5*Y_map_tic]); xlabel(X); ylabel(Y); title(Hybrid A*); plot(start(1), start(2), bo, MarkerSize, 8, LineWidth, 2); plot(goal(1), goal(2), ro, MarkerSize, 8, LineWidth, 2); drawcar(start,carParam,b); drawcar(goal,carParam,r); %% 调用混合A*主函数 [path,nodes,isFind]hybridAStarPlanning(start,goal,map,carParam,HybridAstarParam,map_Param); %% 绘制查找到的轨迹 if(isFind) plot(path(:,1), path(:,2), g-, LineWidth, 1.5); for i1:size(nodes,1) %基于节点绘制车体 drawcar([nodes(i).x,nodes(i).y,nodes(i).theta],carParam,g) ; end end %% 混合A*规划主函数 function [path,nodes,isFind]hybridAStarPlanning(startstate,goalstate,map,carParam,planningParam,mapParam) %输入 初始状态[x,y,theta],终点状态[x,y,theta],栅格障碍地图车辆参数规划参数 %输出 path 记录整条轨迹点即除了节点外还包括节点之间的连接点 nodes:由节点组成的轨迹 %初始化 startNode openList closeList 探索方向 startNode.xstartstate(1); startNode.ystartstate(2); startNode.thetanormalizeAngle(startstate(3)); startNode.bin_xstate2ind(startNode.x,mapParam.X_min,planningParam.X_res); startNode.bin_ystate2ind(startNode.y,mapParam.Y_min,planningParam.Y_res); startNode.bin_thetastate2ind(startNode.theta,-pi,planningParam.yaw_res); startNode.parent[]; startNode.traj[]; %通过怎样的轨迹抵达此节点 startNode.steerdeg2rad(0); %初始方向盘转角 startNode.direction0; %初始行驶方向前进 startNode.g0; startNode.hcal_h([startNode.x,startNode.y,startNode.theta],goalstate); startNode.fstartNode.gstartNode.h; openList[startNode,[]]; %还未探索过的节点 将会从其中提取代价最小的节点进行下一步拓展 %并且当拓展的节点已经存在于openList中时比较其代价如果新的拓展更小则需要更新openList中的该节点 closeList[]; %已经探索过是否为终点的节点 新拓展到的节点将会对比是否处于closeList中如果处于则直接过滤掉 path[]; isFindfalse; %查找成功标记 FinalNodestartNode;% 记录终点节点用于回溯 %单次探索的可选方向 %方向盘转角的可选方向 共nsteer档方向 steer_dirslinspace(-planningParam.MaxSteer,planningParam.MaxSteer,planningParam.nsteer); % 档位的可选方向 前进或后退 moving_dirs[0,1]; dirszeros(2*planningParam.nsteer,2); dirs(1:planningParam.nsteer,1)steer_dirs; dirs(1:planningParam.nsteer,2)moving_dirs(1)*ones(planningParam.nsteer,1); dirs(planningParam.nsteer1:planningParam.nsteer*2,1)steer_dirs; dirs(planningParam.nsteer1:planningParam.nsteer*2,2)moving_dirs(2)*ones(planningParam.nsteer,1); dirs_numsize(dirs,1); isPassFinalfalse; %轨迹是否经过终点的标志 cycle_cnt0; while(~isempty(openList)) cycle_cntcycle_cnt1 if(cycle_cntplanningParam.max_it) break; end %1 从openList中取出最小代价的节点并从openList中剔除该节点并将该节点移入closeList 判断该节点是否为终点,是则结束循环 [cur_node,openList]get_minf_from_openList(openList); closeList[cur_node,closeList]; if(isFinalPoint([cur_node.x,cur_node.y,cur_node.theta],goalstate,planningParam.d_threshold,planningParam.yaw_threshold)) isFindtrue; FinalNodecur_node; break; %提前结束循环 end %2 基于RK4积分 运动学给定的方向盘转角前进后退方向进行节点拓展 next_nodecur_node; %此处要表达的是next_node是一个结构体 只是暂时还没有结构体的定义故用current复制赋值 for i1:dirs_num steerdirs(i,1); directiondirs(i,2); %提取本次拓展的行进方向和方向盘角度,并基于此进行积分拓展 [traj_points, q_end] ack_rk4_integrate([cur_node.x,cur_node.y,cur_node.theta], steer,... carParam.L, planningParam.primitiveLen, planningParam.inte_segs,direction); next_node.xq_end(1); next_node.yq_end(2); next_node.thetaq_end(3); next_node.trajtraj_points; next_node.parentcur_node; %记录父节点 next_node.steersteer; next_node.directiondirection; %判断路径是否发生碰撞或是否在许可地图范围外 if(~traj_collision_check(next_node.traj, map,mapParam,carParam)) continue; end %判断轨迹是否已经达到终点 如果轨迹已经过终点 则需要截断轨迹并修改节点为轨迹的截断终点 [isPassFinal,tructraj]isPassFianlTraj(traj_points,goalstate,planningParam.d_threshold,planningParam.yaw_threshold); next_node.xtructraj(end,1); next_node.ytructraj(end,2); next_node.thetatructraj(end,3); next_node.trajtructraj; %计算节点参数 如代价 离散筒索引等 next_nodecal_node_param(next_node,cur_node,planningParam,goalstate,mapParam); % if(isPassFinal) 感觉此处可以加入提前终止的判断 if(isPassFinal) FinalNodenext_node; break; end % 判断新拓展的节点是否处于closeList,如果是 则过滤continue if(isIncloseList(closeList,next_node)) % disp(过滤节点处于closeList); continue; end % 处理是否处于openList中 % 如果不在openList则直接加入openList,如果在openList但是代价更大则过滤掉如果代价更小则替换掉openList中的节点 openListprocessOpenList(next_node,openList); end %方向拓展循环结束 if(isPassFinal) isFindtrue; break; end end% while循环结束 if(isFind) disp(路径搜索成功); %回溯轨迹 [path,nodes]processPath(FinalNode); else disp(路径搜索失败无可行路径); end end %% 查找openList中代价最小的节点 并且从openList中剔除掉该节点 function [min_node,newOpenList]get_minf_from_openList(openList) newOpenListopenList; numsize(openList,2); min_idx-1; min_finf; for i1:num if(openList(i).fmin_f) min_idxi; min_fopenList(i).f; end end min_nodeopenList(min_idx); newOpenList(min_idx)[]; end %% 计算节点参数 function NodewitchParamcal_node_param(next_node,parentNode,planningParam,goalState,mapParam) %函数的目标更新拓展节点的代价 x_bin,y_bin,theta_bin NodewitchParamnext_node; NodewitchParam.gcal_g(parentNode,next_node,planningParam); NodewitchParam.hcal_h([next_node.x,next_node.y,next_node.theta],goalState); NodewitchParam.fNodewitchParam.gNodewitchParam.h; NodewitchParam.bin_xstate2ind(NodewitchParam.x,mapParam.X_min,planningParam.X_res); NodewitchParam.bin_ystate2ind(NodewitchParam.y,mapParam.Y_min,planningParam.Y_res); NodewitchParam.bin_thetastate2ind(NodewitchParam.theta,-pi,planningParam.yaw_res); end %% 处理新探索的节点是否处于openList中不处于则直接加入处于则对比代价新节点代价更小则更新openList中的节点 function newOpenListprocessOpenList(Node,openList) newOpenListopenList; g_old0; isInfalse; indx-1; for i1:size(openList,2) if(Node.bin_xopenList(i).bin_xNode.bin_yopenList(i).bin_yNode.bin_thetaopenList(i).bin_theta) isIntrue; if(isIn) g_old openList(i).g; indxi; disp(已经出现在openList中); end break; end end if(~isIn) newOpenList[Node,newOpenList]; return; else if(Node.gg_old) return;%不对原来的openList做修改 else newOpenList(indx)Node; %更新openList return; end end end %% 处理探索的节点是否处于closeList中处于则直接过滤 function isInisIncloseList(closeList,Node) isInfalse; for i1:size(closeList,2) if(Node.bin_xcloseList(i).bin_xNode.bin_ycloseList(i).bin_yNode.bin_thetacloseList(i).bin_theta) isIntrue; break; end end return; end %% 回溯出轨迹和节点 function [path,Nodes]processPath(FinalNode) pathFinalNode.traj; NodesFinalNode; cur_nodeFinalNode; while (~isempty(cur_node.parent)) path[cur_node.parent.traj;path]; Nodes[cur_node.parent;Nodes]; cur_nodecur_node.parent; end end %% 基于当前状态计算出可以拓展的下个节点 阿克曼车辆 RK4 积分模块 比一阶欧拉积分具有更好的精度 %输入为当前的状态x,y,theta轴距单次探索的路程长度单次探索的路程长度总共分为多少段进行积分本次探索的方向盘角度 %返回值这一段探索所经过的轨迹点探索到的新节点轨迹的终点 function [traj_points, q_end] ack_rk4_integrate(q0, delta, L, total_s, seg_num,direct) %运动学关系 %dot(x)v*cos(theta),dot(y)v*sin(theta),dot(theta)v*tan(delta)/L %需要替换为对弧长的微分 ds/dtv %则dx/dscos(theta),dy/dssin(theta),d(theta)/dstan(delta)/L % q0 输入行向量 [x0,y0,theta0] ds total_s / seg_num; % 统一转为列向量运算 q q0(:); %这一步实现了行向量转列向量 traj_points zeros(seg_num 1, 3); traj_points(1, :) q0; % 微分函数输出列向量 dqds (state) [cos(state(3)); sin(state(3)); tan(delta) / L]; for i 1 : seg_num k1 dqds(q); k2 dqds(q ds/2 * k1); k3 dqds(q ds/2 * k2); k4 dqds(q ds * k3); if(direct1) k1 -dqds(q); k2 -dqds(q ds/2 * k1); k3 -dqds(q ds/2 * k2); k4 -dqds(q ds * k3); end q q ds/6 * (k1 2*k2 2*k3 k4); % 角度归一化 [-pi, pi] q(3) normalizeAngle(q(3)); % q是列向量转成行写入轨迹表 traj_points(i1, :) q.; end q_end q.; % 输出行向量方便外部使用 end %% 碰撞检测模块 轨迹碰撞检测函数 只检测积分终点坐标是否碰撞是错误的 圆弧轨迹中间位置很容易穿过障碍物造成规划路径穿墙失效 % 取RK4 输出整条轨迹所有采样点逐个校验每一个坐标点是否落在障碍物栅格内 两种检测方式 1只做质点检测 2车体轮廓碰撞检测 function is_safe traj_collision_check(traj_pts, grid_map, map_param,carParam) %输入轨迹点每一行代表一个轨迹点x,y,theta is_safe false; [point_num, ~] size(traj_pts); for i 1 : point_num [car_x,car_y]cal_car_recPoints(traj_pts(i,:),carParam); for j1:size(car_x,2) if ~singleCollision_check(car_x(j),car_y(j),grid_map, map_param) is_safefalse; return; end end end is_safetrue; end %% 考虑车辆外轮廓的碰撞检测 %% 单点碰撞或越界检测 function issafe singleCollision_check(pointX,pointY,grid_map,map_param) %输入x,y坐标 栅格障碍索引地图 栅格地图参数 xpointX; ypointY; x_binstate2ind(pointX,map_param.X_min,map_param.X_res); y_binstate2ind(pointY,map_param.Y_min,map_param.Y_res); X_sizesize(grid_map,1); Y_sizesize(grid_map,2); if(x_bin1||y_bin1||x_binX_size||y_binY_size||grid_map(x_bin,y_bin)0.5) issafefalse; else issafetrue; end return; end %% 基于后车轴中心坐标、朝向车辆尺寸参数计算边框四点矩阵 function [car_x,car_y]cal_car_recPoints(state,carParam) center_xstate(1); center_ystate(2); thetanormalizeAngle(state(3)); WcarParam.W; T_LcarParam.T_L; DcarParam.D; D_headT_L-D; %旋转矩阵 R[cos(theta),-sin(theta); sin(theta),cos(theta)]; %局部坐标系下四个顶点的坐标 local_pts[D_head,W/2; -D,W/2; -D,-W/2; D_head,-W/2]; %旋转加平移 % 旋转 平移 world_pts R * local_pts; car_x world_pts(1,:) center_x; car_y world_pts(2,:) center_y; end %% 计算累积代价函数 function gcal_g(parentNode,curNode,planningParam) gparentNode.g; delta_splanningParam.primitiveLen; w_steerplanningParam.steerPenalty; w_steerchangeplanningParam.steerChangePenalty; w_reverseplanningParam.reversePenalty; w_switchplanningParam.switchPenalty; w_obsplanningParam.obsdistencePenalty; steer_LparentNode.steer; steercurNode.steer; ggdelta_s; ggw_steer*abs(steer)/planningParam.MaxSteer; ggw_steerchange*abs(steer-steer_L)/(planningParam.MaxSteer*2); if (curNode.direction0.5) ggw_reverse; end if (curNode.direction~parentNode.direction) ggw_switch; end %暂时不考虑距离参数的惩罚 end %% 计算代价h的函数 启发式代价 function hcal_h(curState,goalState) cur_xcurState(1); cur_ycurState(2); cur_thetacurState(3); goal_xgoalState(1); goal_ygoalState(2); goal_thetagoalState(3); % habs(goal_x-cur_x)abs(goal_y-cur_y); %曼哈顿距离 hsqrt((goal_x-cur_x)^2(goal_y-cur_y)^2); %欧式距离 % h0; %退化为dijistra算法 end %% 将坐标转换为索引号 处于离散筒的第几段 或处于地图的第个索引 function indstate2ind(state,min_state,res) indfloor((state-min_state)/res)1;%matlab索引从0开始 end %% 绘制车体 function drawcar(state,carParam,color) center_xstate(1); center_ystate(2); thetanormalizeAngle(state(3)); WcarParam.W; T_LcarParam.T_L; DcarParam.D; D_headT_L-D; %旋转矩阵 R[cos(theta),-sin(theta); sin(theta),cos(theta)]; %局部坐标系下四个顶点的坐标 local_pts[D_head,W/2; -D,W/2; -D,-W/2; D_head,-W/2]; %旋转加平移 % 旋转 平移 world_pts R * local_pts; x world_pts(1,:) center_x; y world_pts(2,:) center_y; x[x,x(1)]; y[y,y(1)]; %收尾闭合 plot(x,y,color,LineWidth,2); end %% 角度归一化到[-PI,PI) 考虑离散桶函数复用 此处取左边界更合适 function normaledAnglenormalizeAngle(angle) amod(anglepi,2*pi); if(a0) aa2*pi; end normaledAnglea-pi; return; end %% 判断当前点是否抵达终点 function isFinalisFinalPoint(state_cur,state_final,d_threshold,theta_threshold) distancesqrt((state_cur(2)-state_final(2))^2(state_cur(1)-state_final(1))^2); delta_angleabs(normalizeAngle(state_cur(3)-state_final(3))); if(distanced_threshold delta_angletheta_threshold) isFinaltrue; else isFinalfalse; end return; end %% 判断整条轨迹是否过终点 function [isPassFinal,traj]isPassFianlTraj(trajs,goalstate,d_threshold,theta_threshold) isPassFinalfalse; final_idx-1; for i1:size(trajs,1) if(isFinalPoint(trajs(i,:),goalstate,d_threshold,theta_threshold)) isPassFinaltrue; final_idxi; break; end end if(isPassFinal) trajtrajs(1:final_idx,:); else trajtrajs; end return; end