参考论文
[1] Shan T , Wang W , Englot B , et al. A Receding Horizon Multi-Objective Planner for Autonomous Surface Vehicles in Urban Waterways[J]. 2020.
前提条件
进行完全局路径规划，得到全局路径G0
参数定义
基本变量的定义，其余变量在之后的内容中定义
| 变量名 | 解释 |
|---|---|
| C | configuration space船的位置状态空间 |
| x | 船的位置状态，x属于C |
| Cobst | 与障碍物有冲突（如重合）的状态集合，包含于C |
| Cfree | 与障碍物不冲突的状态集合，为C去除Cobst的闭集 |
| S | 传感器数据 |
| G0 | 全局参考路径 |
| adj(G0) | 临近路径G0且在C附近区域的路径集合 |
| V | 由状态所组成的顶点集合（用于有向图） |
| E | 由V中状态间的连接边组成的集合（用于有向图） |
| eij | E中连接V中xi和xj的连接边 |
| G(V,E) | 以V为顶点集合，E为连接边集合的有向图 |
| σG | 属于adj(G0)，通过有向图获得G(V,E)获得的路径 |
| σG0 | 从G0中取出来的一段路径 |
| σ | 路径函数σ=σG∪σG0，将[0,1]映射到C中状态，σ(0)为起点，σ(1)为终点 |
| Σ | 所有路径函数的集合 |
| xc | 当前位置状态 |
| xg | 目标位置状态 |
Lexicographic Optimization字典优化
个人理解，仅供参考
变量定义
| 变量名 | 解释 |
|---|---|
| Ck | 第k级代价函数，以路径为自变量，值为该路径的代价值 |
基本思路
设置K个分等级的代价函数，等级高的代价函数优先，仅当等级高的代价函数所得到的代价相同的时候才考虑低级的代价函数。
（1）将k个代价函数全用上（优先级仅为计算优先级）
对路径进行计算。
需保证每个代价函数所得的最优路径只有一条。
（2）仅用必要的代价函数（优先级为计算优先级和评估优先级）
对路径进行计算。
每个代价函数所得的最优路径可以有多条，但最低级的代价函数所得的最优路径只能有一条；如果高优先级的代价函数可以找出最优路径，则不必对低优先级的的代价函数进行计算。
此处第三级代价函数即为最低级代价函数，一般通过使最低级代价函数值非负且单增来保证其最佳结果只有一个。
代价函数
三个基本的代价函数（成本函数），按照优先级先后依次是：风险成本 > 航向成本 > 距离成本
（1）风险成本
与障碍物等向冲突的风险导致的成本
c
1
(
σ
)
=
∫
0
1
R
i
s
k
(
σ
(
s
)
)
d
s
.
\ c1(σ) = \int_0^1 Risk(σ(s))ds\,.
c1(σ)=∫01Risk(σ(s))ds.
R
i
s
k
(
x
)
=
{
R
(
x
)
R
(
x
)
>
T
h
r
i
s
k
0
o
t
h
e
r
w
i
s
e
Risk(x)=\begin{cases} R(x) & R(x)>Thrisk \\ 0 & otherwise \end{cases}
Risk(x)={R(x)0R(x)>Thriskotherwise
c1(σ) 为路径的风险成本，Risk(x)为路径中每个状态自己的风险成本，R(x)为状态自己风险成本的计算式，文中给出的示例R(x)为状态点与距离它最近的障碍物点之间的距离的倒数。
Thrisk 为一临界值，表征一个安全区，例如我们将与距离障碍物距离大于a的地方视为安全区，则仅当其与障碍物距离小于a时视为有效，即其距离倒数R(x)大于1/a的时候，此时Thrisk=1/a.
（这几个值的表现形式为代价地图中障碍物周围的膨胀，由于船体本身有体积，所以距障碍物最近的黑色区域表示碰撞膨胀，不可到达；在该膨胀的基础上再向外膨胀1/a的灰色区域为非安全区，在综合考虑各种成本的前提下可以到达，例如即使风险代价最小的时候也要经过灰色区域则此时允许船体到达灰色区域，这样可以防止膨胀将路径阻塞。）
（2）航向成本
与全局参考路径航向不同所导致的成本，默认走全局参考路径，则航向偏移成本可以代表转向带来的动力损耗等成本
c
2
(
σ
)
=
∫
0
1
H
e
a
d
i
n
g
(
σ
(
s
)
)
d
s
.
\ c2(σ) = \int_0^1 Heading(σ(s))ds\,.
c2(σ)=∫01Heading(σ(s))ds.
H
e
a
d
i
n
g
(
x
)
=
{
H
(
x
)
H
(
x
)
>
T
h
h
e
a
d
0
o
t
h
e
r
w
i
s
e
Heading(x)=\begin{cases} H(x) & H(x)>Thhead \\ 0 & otherwise \end{cases}
Heading(x)={H(x)0H(x)>Thheadotherwise
H(x) 为状态x与G0上与x接近的路径段的航向的差异，论文中没说怎么计算，个人认为取x附近的G0路径段的航向与x本身航向做差即为H(x).
Thhead ：由于风浪等的影响，实际跑到x位置时的H(x)与路径规划时的有偏差，所以对于较小的偏差值可以忽略不计，该值为Thhead.
（通过这个代价，可以使局部规划出来的路径更加贴合全局规划路径）
（3）路程成本
路径长短导致的成本
c
3
(
σ
)
=
∫
0
1
D
i
s
t
a
n
c
e
(
σ
(
s
)
)
d
s
.
\ c3(σ) = \int_0^1 Distance(σ(s))ds\,.
c3(σ)=∫01Distance(σ(s))ds.
Distance(x)：论文中没有给出如何计算该成本，个人认为可以用类似A*算法中计算每个点成本 f = g + h的方法来计算Distance(x).
有向图与无向图
参考博客
在路径的搜索中用到了有向图的方法。其中顶点集合V是在一定范围内，以固定密度划分出许多离散的点；连接边集合E文为每个点指向其周围最近的8个点的有向连接的集合。中未具体说明如何确定路径，个人认为是在确定起点和终点后，经过多次有限制条件的深度优先搜索，得到几条较为满意的备选路径。
Receding Horizon Planner
为了减少计算量，以G0为参考路径，仅在必要时刻（如G0穿过障碍物）去规划adj(G0)中的局部路径。
变量定义
| 变量名 | 解释 |
|---|---|
| dspan | 规划中可选择的状态样本与G0之间的最大距离 |
| droll | roll-in与roll-out之间的总距离 |
| dsensor | 传感器的检测范围，一般小于droll，防止在droll末端出现障碍物使路径不能收敛导致规划失败 |
| xinit | 存在于离散点集V中的距离初始状态xc最近的状态 |
| Xqueue | 待搜索的状态队列 |
| xgoal | 存在于G0上roll-in收敛到的点 |
基本思路
（1）字典搜索
此算法将V中各个本状态的父状态，配置为由径该父状态令本状态代价最小的状态（优先级从高到低进行比较），这样一直配置到终点，从终点开始指向各状态父状态即可指出一条路径。但论文中的算法并未给出到终点结束，个人认为配置过程中出现xi为终点或者xj为终点配置了父状态为xi即可停止运算，可以减少计算量。
（2）Receding horizon planner
个人没怎么理解，没看出啥特别之处
扫一扫
231
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
