核心内容摘要
关于“扌噪辶畐”的
基于matlab的全局路径规划算法中的快速扩展随机树RRT路径规划算法及其改进方法RRT Star、RRT_Conncet是一种具有状态约束的非线性系统生成开环轨迹的技术相比于其他算法可以轻松处理障碍物的问题。
最近在折腾机器人路径规划的时候被各种RRT变种算法搞得又爱又恨。
这玩意儿就像玩迷宫游戏既要绕开障碍物又要找最短路线。
今天咱们就扒一扒这个让无数工程师秃头的RRT家族手把手撸几个Matlab代码片段看看门道。
先来段最原始的RRT算法核心代码热热身function new_node extend_rrt(tree, goal, map) rand_point [randi(map.width), randi(map.height)]; nearest_node find_nearest(tree, rand_point); new_point steer(nearest_node.position, rand_point, step_size); if collision_free(nearest_node.position, new_point, map) new_node.position new_point; new_node.parent nearest_node; tree.add(new_node); if norm(new_point - goal) goal_radius path extract_path(new_node); return end end end这段代码里藏着RRT的精髓——随机撒点最近邻连接。
steer函数控制着每次扩展的步长就像盲人摸象一样在空间里试探。
不过原始RRT生成的路径跟喝醉似的歪歪扭扭这时候就该RRT*登场了。
看这段改进的重新布线逻辑% RRT*特有的重选父节点环节 near_nodes find_near_nodes(tree, new_point, radius); min_cost nearest_node.cost norm(new_point - nearest_node.position); for node near_nodes if node.cost norm(new_point - node.position) min_cost if collision_free(node.position, new_point, map) min_cost node.cost norm(new_point - node.position); new_node.parent node; end end end % 反向优化邻居节点 for node near_nodes if new_node.cost norm(node.position - new_point) node.cost if collision_free(new_node.position, node.position, map) node.parent new_node; end end end这里搞了个双重优化先给新节点找更划算的爹再反向检查能不能当别人的爹。
就像在菜市场砍价既要找最便宜的供应商还要看看能不能自己当二道贩子。
这种动态调整让路径成本逐渐收敛到最优代价是计算量直接翻倍。
基于matlab的全局路径规划算法中的快速扩展随机树RRT路径规划算法及其改进方法RRT Star、RRT_Conncet是一种具有状态约束的非线性系统生成开环轨迹的技术相比于其他算法可以轻松处理障碍物的问题。
说到计算效率RRT_Connect这个双向生长狂魔必须拥有姓名% 双树交替扩展 while ~timeout % 从起点树扩展 [tree_a, reached] extend_tree(tree_a, tree_b, map); if reached return combined_path; end % 交换两棵树继续扩展 [tree_b, reached] extend_tree(tree_b, tree_a, map); if reached return combined_path; end end这算法就像两个拆迁队从城市两头同时推进哪边容易挖就先往哪边使劲。
实测在复杂迷宫环境里比单向RRT快至少3倍。
不过要注意两棵树碰头时的衔接判断搞不好会在拐角处出现蜜汁抖动。
最后分享个实战踩坑经验在写碰撞检测时别傻乎乎地用离散点采样。
试过用射线和障碍物多边形求交结果计算量爆炸。
后来改用了bresenham算法做线段穿障检测速度直接起飞function free collision_free(start, goal, map) [line_x, line_y] bresenham(start, goal); for k 1:length(line_x) if map.obstacle(line_x(k), line_y(k)) free false; return end end free true; end不过要注意地图分辨率太高了还是顶不住。
这时候就得祭出多分辨率地图的骚操作——先粗后精先拿低清地图探路找到大致方向再用高清地图微调。
这些算法在无人机避障项目里实测RRT*适合静态环境求最优Connect适合动态环境快速响应。
要是遇到狭长通道记得给随机采样加个偏向目标点的策略不然等着看算法表演鬼打墙吧。
路径规划这事说到底是概率和几何的玄学结合多调参多烧香才是王道。