【路径规划】一种越野环境下车辆驾驶风险规避运动规划算法（Matlab代码实现）

  👨‍🎓个人主页：研学社的博客 

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

RRT算法基本原理

在越野环境中的应用调整

结论

📚2 运行结果

 2.1 场景1

 2.2 场景2

2.3 场景3

 2.4 场景4

2.5 场景5 

2.6 场景6

🌈3 Matlab代码、数据、文章讲解

🎉4 参考文献

---

---

💥1 概述

RRT算法生长并维护一棵树，树的每个节点都是工作空间中的一个节点(状态)。算法所探索的区域即为树所占据的区域。算法最初以一棵以起始节点为唯一节点的树开始。在每次迭代时，通过选择一个随机状态并向该状态扩展树来扩展树。扩展是通过将树中距离最近的节点向选定的随机状态扩展一小步来完成的，如图2所示。算法运行，直到某个扩展使树足够靠近目标。

在越野环境下的车辆驾驶中，由于地形复杂多变、路面条件不确定等因素，传统的路径规划算法可能难以满足实时性与安全性的需求。Rapidly-exploring Random Tree (RRT) 算法作为一种概率性的路径规划方法，在这种环境中展现出了较好的适应性。以下是将RRT算法应用于越野车辆风险规避运动规划的简要介绍：

RRT算法基本原理

RRT算法由LaValle在1998年提出，其核心思想是通过随机采样逐步构建一棵从起点到目标点的搜索树。该算法以随机性探索空间，每次迭代都从当前树的最远节点向目标区域随机选择一个方向发射一个射线，找到射线与环境障碍物（在越野场景中即为不可通行区域）的最近点，并以此点为新节点扩展树。这一过程重复进行，直到树的一个节点足够接近或到达目标点，从而形成一条从起始点到目标点的可行路径。

在越野环境中的应用调整

1. 环境建模：首先需要对越野环境进行精确建模，这包括地形的三维地图、地面类型（如泥泞、岩石等）、坡度信息等。这些数据可以通过激光雷达、卫星图像、无人机扫描等手段获取，并转换成适合算法处理的格式。

2. 动态避障：越野环境中障碍物可能包括不稳定的地形、突然出现的障碍（如倒下的树木）、水流等。RRT在每次采样时需要考虑这些动态变化的因素，可能需要结合实时传感器数据（如雷达、摄像头）进行在线避障，确保生成的路径始终保持可通行。

3. 成本函数优化：除了考虑最短路径外，还需要根据越野车辆的性能限制（如最大爬坡角度、牵引力等）和行驶安全性定义合适的成本函数。这可能包括路径的平滑度、能量消耗、预计的车辆稳定性评分等，以指导算法优先选择风险较低的路径。

4. 局部重规划：越野驾驶过程中，路径的可行性可能随着车辆位置的改变而快速变化。因此，RRT算法应具备一定的局部重规划能力，当原计划路径变得不可行时，能够迅速基于当前环境重新计算最优路径。

5. 风险评估与规避策略：引入风险评估机制，对每一步的扩展进行评估，优先考虑低风险的扩展方向。这可能涉及到对潜在滑坡区域、深水区等高风险地带的识别与回避。

结论

RRT算法因其快速探索特性和灵活性，在面对越野环境下复杂的路径规划问题时展现出良好效果。通过上述调整和优化，可以进一步提高车辆在未知、动态环境中的自主导航能力和安全性。然而，实际应用中仍需综合考虑算法效率、实时性与硬件限制，不断调整参数以达到最佳性能。

📚2 运行结果

 2.1 场景1

 2.2 场景2

2.3 场景3

 2.4 场景4

 

2.5 场景5 

 

2.6 场景6

 

 

  

 部分代码：

%**************************************
function T = init_Tree(start,goal)
T.v(1).x = start(1);         % start point
T.v(1).y = start(2); 
T.v(1).theta = start(3); % start orientation

T.v(1).xPrev = start(1);     % parent node for start point
T.v(1).yPrev = start(2);
T.v(1).thetaPrev = start(3);   % parent node orietation

T.v(1).dist=0;          % RRT tree node 1 attribute of distance
T.v(1).angle=0;% RRT tree node 1 attribute of angle change
T.v(1).dist_angle=0; % distance + angle change
T.v(1).Potential_value=0;  % RRT tree node 1 attribute of potential value
T.v(1).Dist_Angle_PFvalue=0;% distance + angle change + potential value

T.v(1).indPrev = 0;     %% RRT tree node 1 attribute of previous node index
T.v(1).path_flag = true;% if the node is within the path
T.v(1).valid = true;  %  if the node is active
T.v(1).Heuristic = norm([goal(1)-start(1) ,goal(2)-start(2)]);% RRT tree node heuristic value, to goal Euclidean distance
T.v(1).toGoal = norm(start(1,1:2) - goal); % estimation distance to goal for start point = cost to come + cost to go
T.v(1).series = zeros(16,3); % trajectory points for node 1
end

function [ChooseNeighbor_Rang, rewiring_range, minimal_rewiring_range, plot_flag, flag_rand_goal, coef_angle, Circle_radius, div_n, diff_margin] = Init_Parameter(Goal)
ChooseNeighbor_Rang = 220; % Neighbor range
rewiring_range = 280; % Rewire range
minimal_rewiring_range = 180; % Minimal rewire range
plot_flag = false; % plot flag
flag_rand_goal = false;     % random sample flag
coef_angle = 40; %  coefficient of angle
Circle_radius =60; % vehicle turing circle
div_n = 15; % segment division
diff_margin = 20; % optimal trajectory cost update margin
end

🌈3 Matlab代码、数据、文章讲解

🎉4 参考文献

部分理论来源于网络，如有侵权请联系删除。

[1]Hongqing Tian, Boqi Li, Heye Huang, Ling Han (2022) A driving risk-aversive motion planning algorithm for vehicles in off-road environment.