Hybrid Classical/RL Local Planner for Ground Robot Navigation文章目录创新A. (路点生成)B. (净空检测/障碍物检测)C. Filtering (滤波)实验设置根据周围环境在规划器之间进行切换利用这两种方法的优势【AI/传统】创新1. 用了 **极坐标代价地图**普通的地图是XY坐标的格子极坐标是“距离-角度”图。对于机器人避障来说极坐标更直观比如前方3米处有个障碍这有助于Sim-to-Real仿真到真机的迁移 2. 启发式逻辑切换就是写几条硬规则比如前方X米内有障碍物就切RL没有就切DWA既简单又好用不需要再训练一个网络一些实现探索速度空间并基于配置空间中的前向模拟对候选速度进行评分严格来说这使它们成为规划器而其他实现则解决将状态映射到动作的约束优化问题严格来说这使它们成为控制器DWA 规划器生成一组允许的速度这些是在给定当前速度和机器人动力学约束即加速度限制的情况下可以达到的速度。对于每个允许的速度DWA 执行前向模拟以计算机器人使用该速度时产生的轨迹。最后对每个模拟轨迹进行评分并选择成本最低的轨迹。目标函数反映了向目标的进展、与障碍物的间隙、遵循全局计划到路点的距离和旋转核心优化函数C o s t α ⋅ Goal_Dist β ⋅ Obstacle_Dist γ ⋅ Path_Align Cost \alpha \cdot \text{Goal\_Dist} \beta \cdot \text{Obstacle\_Dist} \gamma \cdot \text{Path\_Align}Costα⋅Goal_Distβ⋅Obstacle_Distγ⋅Path_AlignDWA 的做法是假设我在接下来的 0.1秒保持速度( v , ω ) (v, \omega)(v,ω)不变我会走到哪它会模拟出一条圆弧。模拟几十条这样的圆弧然后给每一条打分。离障碍物远的加分离目标近的加分。缺点如果路径中有障碍物成本函数中的障碍物距离分量将开始占主导地位指向远离全局路径的圆弧将具有更低的成本从而使机器人偏离全局计划或目标。随着机器人转向离开成本函数中的计划距离和目标距离分量将达到平衡机器人将受引力回到计划上。接下来可能发生三种情况机器人可能已经取得了足够的前向进展使得下一个路点位于障碍物后面在这种情况下局部规划器将使机器人返回到由全局计划确定的路径机器人可能转回障碍物向其移动并再次转向离开障碍物但这次由于靠近障碍物而处于更困难的境地机器狗在人面前左右横跳就是不过去全局规划器可能被触发并生成一组新的路点引导机器人绕过障碍物。理想的局部规划器应该总是产生第一种情况… 第二种情况通常会导致活锁live-lock表现为机器人接近障碍物并犹豫不决地振荡而不取得进展。在某些情况下由于传感器限制可能会发生碰撞。即在我们的实验中我们看到碰撞是因为我们使用的 LiDAR 传感器具有最小距离范围。一旦机器人太靠近障碍物反射未被注册机器人就会冲向障碍物。我们认为这些缺点是 DWA 使用的单圆弧运动规划的直接后果成功的避障需要三个连续的圆弧如图 1 中的绿色所示。第一个圆弧将机器人推离障碍物第二个圆弧在成功绕过障碍物后将其送回正轨第三个圆弧重新调整方向以指向计划。DWA 规划器根本不探索超出这一个速度矢量的空间… 所有测试都指向缺乏对正在评分的圆弧之后的后续圆弧的可见性。A. (路点生成)为了生成路点通向目标的全局路径由 Dijkstra 算法生成被降采样,并在局部代价地图上选择固定数量的路点B. (净空检测/障碍物检测)查找在没有任何动态障碍物的情况下的路径是否可以无碰撞地通过一条有宽度的“虚拟走廊”模拟机器狗的宽度,如果这个“虚拟走廊”里哪怕有一个像素是障碍物红色部分系统就判定Block受阻如果是Clear- 用DWA跑得快如果是Block- 用SACPlanner避得开。C. Filtering (滤波)传感器数据中的噪声可能导致净空检测器在两个规划器之间快速反复跳变flip-flopping如果仅使用最新的净空状态进行规划器选择的话。为了稳定只有当我们确信路径上存在障碍物时切换才应该发生。处理这种情况下的噪声的典型方法是检查基于直到当前时间 $t$ 的过去 $n$ 次观测 $O_{t-n:t}$路径被阻塞 $b$ 的似然度 $\mathcal{L}(b|O_{t-n:t})$。[**公式解释** $\mathcal{L}(b|O_{t-n:t})$这只是个数学写法。意思是根据过去 $n$ 次的观察判断现在是不是真的堵了。] 如果障碍物的似然度高于用户定义的阈值 $\tau$我们认为路径被阻塞。我们将此策略实现为一个滤波器跟踪检测器给出的最近 $n3$ 次路径净空状态。如果所有状态都指示路径受阻我们使用 SACPlanner实际上使用了 $\tau1$。否则使用 DWA 这个方案在图 2 的“滤波Filtering”步骤右下角的框中进行了可视化。这种设计有助于在传感器强烈指示路径上存在障碍物时使用 SACPlanner并带来高效的导航因为相对更平滑和更快速的方法 DWA 在大多数时间被使用只有在必要时才发生切换。公式详细讲解这个公式由5个部分累加而成R ( s , a ) ( d o l d − d n e w ) ⋅ C 1 ⏟ 1. 距离进步奖励 ( ∣ θ o l d ∣ − ∣ θ n e w ∣ ) ⋅ C 2 ⏟ 2. 角度对齐奖励 − R m a x ⋅ I c o l l i s i o n ⏟ 3. 碰撞惩罚 R m a x ⋅ I g o a l ⏟ 4. 到达目标奖励 − G ( s ′ ) ⏟ 5. 危险区域惩罚 \begin{aligned} R(s,a) \ \underbrace{(d_{old}-d_{new}) \cdot C_1}_{\text{1. 距离进步奖励}} \\ \ \underbrace{(|\theta_{old}|-|\theta_{new}|) \cdot C_2}_{\text{2. 角度对齐奖励}} \\ - \ \underbrace{R_{max} \cdot \mathbb{I}_{collision}}_{\text{3. 碰撞惩罚}} \\ \ \underbrace{R_{max} \cdot \mathbb{I}_{goal}}_{\text{4. 到达目标奖励}} \\ - \ \underbrace{G(s)}_{\text{5. 危险区域惩罚}} \end{aligned}R(s,a)−−1.距离进步奖励(dold−dnew)⋅C12.角度对齐奖励(∣θold∣−∣θnew∣)⋅C23.碰撞惩罚Rmax⋅Icollision4.到达目标奖励Rmax⋅Igoal5.危险区域惩罚G(s′)距离奖励 (Distance Reward):( d o l d − d n e w ) ⋅ ( 1 if d o l d − d n e w ≥ 0 , else 2 ) (d_{old}-d_{new}) \cdot (\text{1 if } d_{old}-d_{new}\ge0, \text{ else } 2)(dold−dnew)⋅(1 ifdold−dnew≥0,else2)正值表示靠近目标负值表示远离。非对称加权Asymmetric Weightingelse 2是关键。这意味着后退的惩罚是前进奖励的两倍。这迫使 Agent 除非万不得已即将碰撞否则绝不走回头路。这解决了 RL 机器人容易原地打转刷分Reward Hacking的问题 19。角度奖励 (Heading Reward)( ∣ θ o l d ∣ − ∣ θ n e w ∣ ) ⋅ ( 1 if ∣ θ o l d ∣ − ∣ θ n e w ∣ ≥ 0 , else 2 ) (|\theta_{old}|-|\theta_{new}|) \cdot (\text{1 if } |\theta_{old}|-|\theta_{new}|\ge0, \text{ else } 2)(∣θold∣−∣θnew∣)⋅(1 if∣θold∣−∣θnew∣≥0,else2)鼓励机器人车头对准目标。同样采用了非对称加权惩罚偏离目标的旋转。碰撞惩罚 (Collision Penalty):− R m a x ⋅ ( 1 if collision, else 0 ) - R_{max} \cdot (\text{1 if collision, else } 0)−Rmax⋅(1 if collision, else0)到达目标奖励 (Goal Reward): R m a x ⋅ ( 1 if d n e w 0 , else 0 ) R_{max} \cdot (\text{1 if } d_{new}0, \text{ else } 0)Rmax⋅(1 ifdnew0,else0)高斯势场 (Gaussian Potential Field)G ( s ′ ) G(s^{\prime})G(s′)G ( s ′ ) ≈ ∑ o b s exp ( − d i s t ( r o b o t , o b s ) 2 2 σ 2 ) G(s) \approx \sum_{obs} \exp(-\frac{dist(robot, obs)^2}{2\sigma^2})G(s′)≈∑obsexp(−2σ2dist(robot,obs)2)。这实际上引入了一个人工势场Artificial Potential Field。即使没有发生碰撞只要机器人靠近障碍物这个项就会产生负值惩罚。这就是为什么 SACPlanner 在实验中表现得比 DWA 更“胆小”——它不仅怕撞还怕靠得太近。DWA 只有在撞上的一瞬间或极近距离才会有巨大的代价而 RL 通过这个项学会了保持安全距离 21。它不是硬邦邦的一堵墙而是一个斥力场这对于处理动态障碍物非常有帮助因为动态障碍物的位置是不确定的留出安全余量能减少碰撞率实验设置无障碍复杂轨迹路径上的意外静态障碍物路径上的动态障碍物动态障碍物穿过路径反应时间(C1) DWA虽然能走但你可以看到它贴墙很近甚至撞了碰撞图标。(C1) SAC像喝醉了一样红色轨迹扭来扭去虽然没撞但走得太丑了。(C1) Hybrid完美。主要用红色DWA模式跑直道平滑且快。不要只列“成功率”。要列出平均速度Efficiency、路径长度Pathoptimality、平滑度角速度的变化率Jerky。
论文阅读-Hybrid Classical/RL Local Planner for Ground Robot Navigation
Hybrid Classical/RL Local Planner for Ground Robot Navigation文章目录创新A. (路点生成)B. (净空检测/障碍物检测)C. Filtering (滤波)实验设置根据周围环境在规划器之间进行切换利用这两种方法的优势【AI/传统】创新1. 用了 **极坐标代价地图**普通的地图是XY坐标的格子极坐标是“距离-角度”图。对于机器人避障来说极坐标更直观比如前方3米处有个障碍这有助于Sim-to-Real仿真到真机的迁移 2. 启发式逻辑切换就是写几条硬规则比如前方X米内有障碍物就切RL没有就切DWA既简单又好用不需要再训练一个网络一些实现探索速度空间并基于配置空间中的前向模拟对候选速度进行评分严格来说这使它们成为规划器而其他实现则解决将状态映射到动作的约束优化问题严格来说这使它们成为控制器DWA 规划器生成一组允许的速度这些是在给定当前速度和机器人动力学约束即加速度限制的情况下可以达到的速度。对于每个允许的速度DWA 执行前向模拟以计算机器人使用该速度时产生的轨迹。最后对每个模拟轨迹进行评分并选择成本最低的轨迹。目标函数反映了向目标的进展、与障碍物的间隙、遵循全局计划到路点的距离和旋转核心优化函数C o s t α ⋅ Goal_Dist β ⋅ Obstacle_Dist γ ⋅ Path_Align Cost \alpha \cdot \text{Goal\_Dist} \beta \cdot \text{Obstacle\_Dist} \gamma \cdot \text{Path\_Align}Costα⋅Goal_Distβ⋅Obstacle_Distγ⋅Path_AlignDWA 的做法是假设我在接下来的 0.1秒保持速度( v , ω ) (v, \omega)(v,ω)不变我会走到哪它会模拟出一条圆弧。模拟几十条这样的圆弧然后给每一条打分。离障碍物远的加分离目标近的加分。缺点如果路径中有障碍物成本函数中的障碍物距离分量将开始占主导地位指向远离全局路径的圆弧将具有更低的成本从而使机器人偏离全局计划或目标。随着机器人转向离开成本函数中的计划距离和目标距离分量将达到平衡机器人将受引力回到计划上。接下来可能发生三种情况机器人可能已经取得了足够的前向进展使得下一个路点位于障碍物后面在这种情况下局部规划器将使机器人返回到由全局计划确定的路径机器人可能转回障碍物向其移动并再次转向离开障碍物但这次由于靠近障碍物而处于更困难的境地机器狗在人面前左右横跳就是不过去全局规划器可能被触发并生成一组新的路点引导机器人绕过障碍物。理想的局部规划器应该总是产生第一种情况… 第二种情况通常会导致活锁live-lock表现为机器人接近障碍物并犹豫不决地振荡而不取得进展。在某些情况下由于传感器限制可能会发生碰撞。即在我们的实验中我们看到碰撞是因为我们使用的 LiDAR 传感器具有最小距离范围。一旦机器人太靠近障碍物反射未被注册机器人就会冲向障碍物。我们认为这些缺点是 DWA 使用的单圆弧运动规划的直接后果成功的避障需要三个连续的圆弧如图 1 中的绿色所示。第一个圆弧将机器人推离障碍物第二个圆弧在成功绕过障碍物后将其送回正轨第三个圆弧重新调整方向以指向计划。DWA 规划器根本不探索超出这一个速度矢量的空间… 所有测试都指向缺乏对正在评分的圆弧之后的后续圆弧的可见性。A. (路点生成)为了生成路点通向目标的全局路径由 Dijkstra 算法生成被降采样,并在局部代价地图上选择固定数量的路点B. (净空检测/障碍物检测)查找在没有任何动态障碍物的情况下的路径是否可以无碰撞地通过一条有宽度的“虚拟走廊”模拟机器狗的宽度,如果这个“虚拟走廊”里哪怕有一个像素是障碍物红色部分系统就判定Block受阻如果是Clear- 用DWA跑得快如果是Block- 用SACPlanner避得开。C. Filtering (滤波)传感器数据中的噪声可能导致净空检测器在两个规划器之间快速反复跳变flip-flopping如果仅使用最新的净空状态进行规划器选择的话。为了稳定只有当我们确信路径上存在障碍物时切换才应该发生。处理这种情况下的噪声的典型方法是检查基于直到当前时间 $t$ 的过去 $n$ 次观测 $O_{t-n:t}$路径被阻塞 $b$ 的似然度 $\mathcal{L}(b|O_{t-n:t})$。[**公式解释** $\mathcal{L}(b|O_{t-n:t})$这只是个数学写法。意思是根据过去 $n$ 次的观察判断现在是不是真的堵了。] 如果障碍物的似然度高于用户定义的阈值 $\tau$我们认为路径被阻塞。我们将此策略实现为一个滤波器跟踪检测器给出的最近 $n3$ 次路径净空状态。如果所有状态都指示路径受阻我们使用 SACPlanner实际上使用了 $\tau1$。否则使用 DWA 这个方案在图 2 的“滤波Filtering”步骤右下角的框中进行了可视化。这种设计有助于在传感器强烈指示路径上存在障碍物时使用 SACPlanner并带来高效的导航因为相对更平滑和更快速的方法 DWA 在大多数时间被使用只有在必要时才发生切换。公式详细讲解这个公式由5个部分累加而成R ( s , a ) ( d o l d − d n e w ) ⋅ C 1 ⏟ 1. 距离进步奖励 ( ∣ θ o l d ∣ − ∣ θ n e w ∣ ) ⋅ C 2 ⏟ 2. 角度对齐奖励 − R m a x ⋅ I c o l l i s i o n ⏟ 3. 碰撞惩罚 R m a x ⋅ I g o a l ⏟ 4. 到达目标奖励 − G ( s ′ ) ⏟ 5. 危险区域惩罚 \begin{aligned} R(s,a) \ \underbrace{(d_{old}-d_{new}) \cdot C_1}_{\text{1. 距离进步奖励}} \\ \ \underbrace{(|\theta_{old}|-|\theta_{new}|) \cdot C_2}_{\text{2. 角度对齐奖励}} \\ - \ \underbrace{R_{max} \cdot \mathbb{I}_{collision}}_{\text{3. 碰撞惩罚}} \\ \ \underbrace{R_{max} \cdot \mathbb{I}_{goal}}_{\text{4. 到达目标奖励}} \\ - \ \underbrace{G(s)}_{\text{5. 危险区域惩罚}} \end{aligned}R(s,a)−−1.距离进步奖励(dold−dnew)⋅C12.角度对齐奖励(∣θold∣−∣θnew∣)⋅C23.碰撞惩罚Rmax⋅Icollision4.到达目标奖励Rmax⋅Igoal5.危险区域惩罚G(s′)距离奖励 (Distance Reward):( d o l d − d n e w ) ⋅ ( 1 if d o l d − d n e w ≥ 0 , else 2 ) (d_{old}-d_{new}) \cdot (\text{1 if } d_{old}-d_{new}\ge0, \text{ else } 2)(dold−dnew)⋅(1 ifdold−dnew≥0,else2)正值表示靠近目标负值表示远离。非对称加权Asymmetric Weightingelse 2是关键。这意味着后退的惩罚是前进奖励的两倍。这迫使 Agent 除非万不得已即将碰撞否则绝不走回头路。这解决了 RL 机器人容易原地打转刷分Reward Hacking的问题 19。角度奖励 (Heading Reward)( ∣ θ o l d ∣ − ∣ θ n e w ∣ ) ⋅ ( 1 if ∣ θ o l d ∣ − ∣ θ n e w ∣ ≥ 0 , else 2 ) (|\theta_{old}|-|\theta_{new}|) \cdot (\text{1 if } |\theta_{old}|-|\theta_{new}|\ge0, \text{ else } 2)(∣θold∣−∣θnew∣)⋅(1 if∣θold∣−∣θnew∣≥0,else2)鼓励机器人车头对准目标。同样采用了非对称加权惩罚偏离目标的旋转。碰撞惩罚 (Collision Penalty):− R m a x ⋅ ( 1 if collision, else 0 ) - R_{max} \cdot (\text{1 if collision, else } 0)−Rmax⋅(1 if collision, else0)到达目标奖励 (Goal Reward): R m a x ⋅ ( 1 if d n e w 0 , else 0 ) R_{max} \cdot (\text{1 if } d_{new}0, \text{ else } 0)Rmax⋅(1 ifdnew0,else0)高斯势场 (Gaussian Potential Field)G ( s ′ ) G(s^{\prime})G(s′)G ( s ′ ) ≈ ∑ o b s exp ( − d i s t ( r o b o t , o b s ) 2 2 σ 2 ) G(s) \approx \sum_{obs} \exp(-\frac{dist(robot, obs)^2}{2\sigma^2})G(s′)≈∑obsexp(−2σ2dist(robot,obs)2)。这实际上引入了一个人工势场Artificial Potential Field。即使没有发生碰撞只要机器人靠近障碍物这个项就会产生负值惩罚。这就是为什么 SACPlanner 在实验中表现得比 DWA 更“胆小”——它不仅怕撞还怕靠得太近。DWA 只有在撞上的一瞬间或极近距离才会有巨大的代价而 RL 通过这个项学会了保持安全距离 21。它不是硬邦邦的一堵墙而是一个斥力场这对于处理动态障碍物非常有帮助因为动态障碍物的位置是不确定的留出安全余量能减少碰撞率实验设置无障碍复杂轨迹路径上的意外静态障碍物路径上的动态障碍物动态障碍物穿过路径反应时间(C1) DWA虽然能走但你可以看到它贴墙很近甚至撞了碰撞图标。(C1) SAC像喝醉了一样红色轨迹扭来扭去虽然没撞但走得太丑了。(C1) Hybrid完美。主要用红色DWA模式跑直道平滑且快。不要只列“成功率”。要列出平均速度Efficiency、路径长度Pathoptimality、平滑度角速度的变化率Jerky。
