文章目录
代码地址: https://github.com/AtsushiSakai/PythonRobotics
涉及两篇论文:
- Optimal rough terrain trajectory generation for wheeled mobile robots
这篇是通过优化控制参数的方式，找满足两点边界约束的最优路径问题。 - State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments
这篇涉及state space采样，不考虑环境的均匀采样，或考虑global cost的稀疏采样，以及考虑路径环境约束或切换道路的采样。采样后利用第一篇最优路径方法生成两点间路径。
大家都知道state lattice基本思想是
- sample in control space。在控制空间采样，通过状态转移方程向前积分。缺点是没有任务导向。
s ˙ = A ⋅ s + B ⋅ u s ( t ) = e A t s 0 + [ ∫ 0 t e A ( t − σ ) B d σ ] u \begin{aligned} \dot{s} &= A \cdot s + B \cdot u \\ s(t) &= e^{At}s_0 + \big[\int_0^t e^{A(t-\sigma)}Bd\sigma\big]u \end{aligned} s˙s(t)=A⋅s+B⋅u=eAts0+[∫0teA(t−σ)Bdσ]u - sampling in state space。在状态空间采样，通过解OBVP问题，求出u(t)。缺点是解题难。
对比图
代码中为在状态空间采样
1.原理
看下求解路径的理论知识，再对应代码。大部分来自两篇论文
1.1 Pontryain’s minimum principle
不是论文中的内容，但是个典型的OBVP问题求解方式。
- 二阶系统参考:用庞特里亚金极小值原理求解二阶系统的最优控制问题
- 三阶类似无人机系统，其中 s = ( p , v , a ) , u = j j e r k , s ˙ = f s ( s , u ) = ( v , a , j ) s=(p,v,a),u=j_{jerk},\dot{s}=f_s(s,u)=(v,a,j) s=(p,v,a),u=jjerk,s˙=fs(s,u)=(v,a,j)，哈密顿函数为 H ( s , u , λ ) = 1 T j 2 + λ T f s ( s , u ) H(s,u,\lambda) = \frac{1}{T}j^2 + \lambda^Tf_s(s,u) H(s,u,λ)=T1j2+λTfs(s,u)，解法相同
1.2 Numerical Optimization
来自第一篇论文。论文中涉及二阶模型，例如车。
1.2.1论文中基础信息
- 边界state定义为(起点和终点)，包括位置
(
x
,
y
,
ψ
)
(x,y,\psi)
(x,y,ψ)，速度
(
v
)
(v)
(v)，角速度
(
ω
)
(\omega)
(ω)（或曲率
k
k
k，不知道为啥叫curvature，看代码似乎和角速度是一个东西）
X 0 = [ x 0 , y 0 , ψ 0 , v 0 , ω 0 ] T X f = [ x f , y f , ψ f , v f , ω f ] T \begin{aligned} X_0 &= [x_0,y_0,\psi_0,v_0,\omega_0]^T\\ X_f &= [x_f,y_f,\psi_f,v_f,\omega_f]^T\\ \end{aligned} X0Xf=[x0,y0,ψ0,v0,ω0]T=[xf,yf,ψf,vf,ωf]T - 状态微分方程和状态限制(state constraints C)(指规划的轨迹在
t
=
t
f
t=t_f
t=tf时，状态
X
(
t
f
)
X(t_f)
X(tf)需要等于目标状态
X
f
X_f
Xf)
x ˙ = f ( x , u , t ) C ( x , t ) = 0 = X f − X ( t f ) \begin{aligned} \dot{x} &= f(x,u,t) \\ C(x,t) &=0 = X_f - X(t_f) \end{aligned} x˙C(x,t)=f(x,u,t)=0=Xf−X(tf) - 把控制量(controls)参数化，其中 p p p是参数向量， ξ \xi ξ的独立变量
u = f ( p , ξ ) u = f(p,\xi) u=f(p,ξ)
-
对于独立变量 ξ \xi ξ，一般是 ( t , s ) (t,s) (t,s)，时间和弧长
-
对于参数 p p p，针对速度模型，角速度(或曲率)模型区分
- 对于线性速度，模型可以是固定速度、线性速度、T型曲线等，可以允许数值线性化的模型。 论文中选取T型曲线。参数列表为
p = [ v 0 , a 0 , v t r a v e r s e , a f , v f , Δ t ] T p =[v_0,a_0,v_{traverse},a_f,v_f,\Delta t]^T p=[v0,a0,vtraverse,af,vf,Δt]T
初始速度 v 0 v_0 v0，初始加速度 a 0 a_0 a0,终止速度 v f v_f vf，终止加速度 a f a_f af，中间速度 v t r a v e r s e v_{traverse} vtraverse，时间间隔 Δ t \Delta t Δt(pythonRobotics中这里固定了速度，猜想如果用T-curve实现， a f , v f a_f,v_f af,vf可以通过采样或固定生成， Δ t \Delta t Δt可以通过采样终止位置，计算起始到终止位置的弧长，再通过速度计算) - 对于角速度，模型可以用样条曲线，一种参数直接是多次项的系数，即
p = [ a , b , c , d , Δ t ] T ω ( p , t ) = a + b t + c t 2 + d t 3 + ⋯ \begin{aligned} p &= [a,b,c,d,\Delta t]^T \\ \omega(p,t)&= a+bt+ct^2+dt^3+\cdots \end{aligned} pω(p,t)=[a,b,c,d,Δt]T=a+bt+ct2+dt3+⋯
另一种参数是曲线上几个点，通过拟合曲线获得曲线方程
p = [ ω 0 , ω 1 , ω 2 , ⋯ , Δ t ] T ( 指 参 数 ) ω 0 = ω ( t 0 ) , ω 1 = ω ( t 0 + Δ t / n ) ( 代 表 参 数 的 物 理 意 义 ) a ^ = f 1 ( p ) , b ^ = f 2 ( p ) , c ^ = f 3 ( p ) , ⋯ ( 代 表 实 际 曲 线 系 数 是 由 参 数 拟 合 得 到 ) ω ( p , t ) = a ^ + b ^ t + c ^ t 2 + d ^ t 3 + ⋯ ( 实 际 的 曲 线 方 程 ) \begin{aligned} p &= [\omega_0,\omega_1,\omega_2,\cdots,\Delta t]^T \quad(指参数)\\ \omega_0 &= \omega(t_0),\omega_1=\omega(t_0+\Delta t/n) \quad(代表参数的物理意义)\\ \hat{a} &= f_1(p),\hat{b} = f_2(p), \hat{c} = f_3(p),\cdots \quad(代表实际曲线系数是由参数拟合得到)\\ \omega(p,t)&= \hat{a}+\hat{b}t+\hat{c}t^2+\hat{d}t^3+\cdots \quad(实际的曲线方程)\\ \end{aligned} pω0a^ω(p,t)=[ω0,ω1,ω2,⋯,Δt]T(指参数)=ω(t0),ω1=ω(t0+Δt/n)(代表参数的物理意义)=f1(p),b^=f2(p),c^=f3(p),⋯(代表实际曲线系数是由参数拟合得到)=a^+b^t+c^t2+d^t3+⋯(实际的曲线方程)
pythonRobotics中的代码使用了第二种参数表示方式，具体是 [ s , k m , k f ] [s,k_m,k_f] [s,km,kf]，代表起点到采样的终点状态的弧长， Δ t / 2 \Delta t/2 Δt/2时的曲率(或称角速度)， Δ t \Delta t Δt时的曲率。其实计算二次曲线实际参数时还需要初始点的曲率 k 0 k_0 k0，但这一般是已知量，所以不放在参数中。并且代码中拟合的是二次曲线，大概因为第二篇论文中提到二次项就足够了。
- 对于线性速度，模型可以是固定速度、线性速度、T型曲线等，可以允许数值线性化的模型。 论文中选取T型曲线。参数列表为
1.2.2 Constrained Trajectory Generation
对应论文中的2.3.1节。这里利用数值优化，每步迭代优化控制参数
p
p
p，使得在控制量
u
(
p
,
t
)
u(p,t)
u(p,t)的控制下，从初始状态
X
0
X_0
X0逐渐向前积分变化到
X
(
t
f
)
X(t_f)
X(tf)时，逐渐缩小
X
f
−
X
(
t
f
)
X_f - X(t_f)
Xf−X(tf)的差距，使得满足约束条件
C
(
x
)
C(x)
C(x)。
优化方法为牛顿迭代，求取残差
Δ
x
f
(
p
)
\Delta x_f(p)
Δxf(p)相对参数
p
p
p的Jacobian矩阵，找到一个参数
p
p
p的迭代变化量
Δ
p
\Delta p
Δp，使得残差在下次计算时能够缩小。
[
∂
Δ
X
f
(
p
)
∂
p
]
Δ
p
=
−
Δ
X
f
(
p
)
=
X
f
−
X
(
t
f
)
Δ
p
=
−
[
∂
Δ
X
f
(
p
)
∂
p
]
−
1
Δ
X
f
(
p
)
\begin{aligned} \big[ \frac{\partial \Delta X_f(p)}{\partial p}\big] \Delta p = -\Delta X_f(p) = X_f - X(t_f) \\ \Delta p = - \big[ \frac{\partial \Delta X_f(p)}{\partial p}\big]^{-1}\Delta X_f(p) \end{aligned}
[∂p∂ΔXf(p)]Δp=−ΔXf(p)=Xf−X(tf)Δp=−[∂p∂ΔXf(p)]−1ΔXf(p)
其中Jacobian没有办法直接计算，可以通过下式计算
∂
Δ
i
,
j
X
f
(
p
)
∂
p
k
=
Δ
X
i
,
j
(
p
k
+
e
,
p
)
−
Δ
X
i
,
j
(
p
)
e
∂
Δ
i
,
j
X
f
(
p
)
∂
p
k
=
Δ
X
i
,
j
(
p
k
+
e
,
p
)
−
Δ
X
i
,
j
(
p
k
−
e
,
p
)
2
e
\begin{aligned} \frac{\partial \Delta_{i,j} X_f(p)}{\partial p_k} &= \frac{\Delta X_{i,j}(p_k +e,p) - \Delta X_{i,j}(p) }{e} \\ \frac{\partial \Delta_{i,j} X_f(p)}{\partial p_k} &= \frac{\Delta X_{i,j}(p_k +e,p) - \Delta X_{i,j}(p_k -e,p)}{2e} \end{aligned}
∂pk∂Δi,jXf(p)∂pk∂Δi,jXf(p)=eΔXi,j(pk+e,p)−ΔXi,j(p)=2eΔXi,j(pk+e,p)−ΔXi,j(pk−e,p)
其中
- i , j i,j i,j代表状态量 X X X矩阵的 i 行 j 列 i行j列 i行j列,
- k k k代表参数 p p p的第 k k k个参数。
这里表达式的意思式，当有参数 p p p，想求解残差 Δ X f = X ( t f ) − X f \Delta X_f =X(t_f) -X_f ΔXf=X(tf)−Xf的雅克比，可以通过近似求解：
- 固定当前 p p p上第 k k k项以外的参数，给第 k k k项参数增加或减少一个小小的扰动 e e e，变为 p k + e p_k+e pk+e或 p k − e p_k-e pk−e
- 使用扰动过的参数 ( p k ± e , p ) (p_k\pm e,p) (pk±e,p)，从起始状态 X 0 X_0 X0向前积分，得到新参数下的轨迹终态 X p k ± e ( t f ) X_{p_k \pm e}(t_f) Xpk±e(tf)
- 求解新参数下的残差 Δ X ( p k ± e , p ) \Delta X(p_k \pm e,p) ΔX(pk±e,p)
- 利用上面的公式近似求解第 k k k个参数的偏导
- 每个参数循环扰动，最终得到对整个参数 p p p的Jacobian
pythonRobotics代码中就使用这种迭代方法
1.2.3 Constrained Optimization Trajectory Generation
对应论文的2.3.2节。使用最优控制解出唯一解。其中参数
p
p
p必须满足约束，并最小化目标函数(utility functional)
J
(
p
)
J(p)
J(p)（过程中想要优化的量）。哈密顿方程
H
H
H(利用拉格朗日乘子
λ
\lambda
λ)定义为
H
(
p
,
λ
)
=
J
(
p
)
+
λ
T
Δ
X
f
(
p
)
=
J
(
p
)
+
λ
T
(
x
f
−
X
(
t
f
)
)
J
(
p
)
=
∫
t
0
t
f
Y
(
x
,
p
,
t
)
d
t
\begin{aligned} H(p,\lambda) &= J(p) + \lambda^T\Delta X_f(p) = J(p) + \lambda^T(x_f - X(t_f)) \\ J(p) &= \int_{t_0}^{t_f} Y(x,p,t) dt \end{aligned}
H(p,λ)J(p)=J(p)+λTΔXf(p)=J(p)+λT(xf−X(tf))=∫t0tfY(x,p,t)dt
论文中有提到的
Y
Y
Y有
J
(
u
)
=
∫
t
0
t
f
Y
(
x
,
u
,
t
)
=
∫
t
0
t
f
1
+
α
c
o
s
t
(
x
,
y
)
d
t
J
(
u
)
=
∫
t
0
t
f
Y
(
x
,
u
,
t
)
=
∫
t
0
t
f
1
+
α
(
ϕ
2
+
θ
2
)
d
t
\begin{aligned} J(u) &= \int_{t_0}^{t_f} Y(x,u,t) = \int_{t_0}^{t_f}1 + \alpha cost(x,y) dt \\ J(u) &= \int_{t_0}^{t_f} Y(x,u,t) = \int_{t_0}^{t_f} 1+\alpha (\phi^2 + \theta^2) dt \end{aligned}
J(u)J(u)=∫t0tfY(x,u,t)=∫t0tf1+αcost(x,y)dt=∫t0tfY(x,u,t)=∫t0tf1+α(ϕ2+θ2)dt
第一个是对时间和能量消耗的tradeoff，第二个是为了减少在斜坡上的时间，对roll和pitch做惩罚。以适应不同地形。
- 最优条件下的一阶微分方程：
∂ H ( p , λ ) ∂ p = ∂ J ( p ) ∂ p + λ T ∂ Δ X f ( p ) ∂ p = 0 T ∂ H ( p , λ ) ∂ λ = Δ X f ( p ) = 0 \begin{aligned} \frac{\partial H(p,\lambda)}{ \partial p} &= \frac{\partial J(p)}{\partial p} + \lambda ^T\frac{\partial \Delta X_f(p)}{\partial p} =0^T \\ \frac{\partial H(p,\lambda)}{ \partial \lambda} &= \Delta X_f(p) =0 \end{aligned} ∂p∂H(p,λ)∂λ∂H(p,λ)=∂p∂J(p)+λT∂p∂ΔXf(p)=0T=ΔXf(p)=0 - 同样经过迭代求取参数
p
p
p和
λ
\lambda
λ的最优解。
[ ∂ 2 H ( p , λ ) ∂ p 2 Δ X f ( p ) T ∂ p Δ X f ( p ) T ∂ p 0 ] [ Δ p Δ λ ] = − [ ∂ H ( p , λ ) ∂ p Δ X f ( p ) ] \begin{bmatrix} \frac{\partial^2H(p,\lambda)}{\partial p^2} & \frac{\Delta X_f(p)^T}{\partial p} \\ \frac{\Delta X_f(p)^T}{\partial p} & 0 \end{bmatrix}\begin{bmatrix}\Delta p \\ \Delta \lambda\end{bmatrix} = - \begin{bmatrix} \frac{\partial H(p,\lambda)}{\partial p} \\\Delta X_f(p)\end{bmatrix} [∂p2∂2H(p,λ)∂pΔXf(p)T∂pΔXf(p)T0][ΔpΔλ]=−[∂p∂H(p,λ)ΔXf(p)]
(不知道其中的 ∂ 2 H ( p , λ ) ∂ p 2 Δ p + Δ X f ( p ) T ∂ p Δ λ = − ∂ H ( p , λ ) ∂ p \frac{\partial^2H(p,\lambda)}{\partial p^2} \Delta p + \frac{\Delta X_f(p)^T}{\partial p} \Delta \lambda = -\frac{\partial H(p,\lambda)}{\partial p} ∂p2∂2H(p,λ)Δp+∂pΔXf(p)TΔλ=−∂p∂H(p,λ)来源是啥，大胆猜测下，由于 ∂ H ( p , λ ) ∂ p \frac{\partial H(p,\lambda)}{\partial p} ∂p∂H(p,λ)需要为0，就和残差的牛顿法同理，正好 ∂ 2 H ( p , λ ) ∂ p ∂ λ = Δ X f ( p ) T ∂ p \frac{\partial^2H(p,\lambda)}{\partial p\partial \lambda}=\frac{\Delta X_f(p)^T}{\partial p} ∂p∂λ∂2H(p,λ)=∂pΔXf(p)T
∂ 2 H ( p , λ ) ∂ p 2 Δ p + ∂ 2 H ( p , λ ) ∂ p ∂ λ Δ λ = − ∂ H ( p , λ ) ∂ p \frac{\partial^2H(p,\lambda)}{\partial p^2} \Delta p + \frac{\partial^2H(p,\lambda)}{\partial p\partial \lambda} \Delta \lambda = -\frac{\partial H(p,\lambda)}{\partial p} ∂p2∂2H(p,λ)Δp+∂p∂λ∂2H(p,λ)Δλ=−∂p∂H(p,λ)) - 其中哈密顿方程
H
H
H对于参数
p
p
p的Hessian矩阵，可以通过下式求解
∂ 2 H i , j ( p ) ∂ p k ∂ p k = H i , j ( p k + e , p l + e , p ) − H i , j ( p k + e , p ) − H i , j ( p l + e , p ) + H i , j ( p ) e 2 \frac{\partial^2H_{i,j}(p)}{\partial p_k\partial p_k} = \frac{H_{i,j}(p_k+e,p_l+e,p)-H_{i,j}(p_k+e,p)-H_{i,j}(p_l+e,p)+H_{i,j}(p)}{e^2} ∂pk∂pk∂2Hi,j(p)=e2Hi,j(pk+e,pl+e,p)−Hi,j(pk+e,p)−Hi,j(pl+e,p)+Hi,j(p)
1.2.4向前积分
X ( t + Δ t ) = X ( t ) + X ˙ ( X , u , t ) Δ t X(t+\Delta t) = X(t) + \dot{X}(X,u,t)\Delta t X(t+Δt)=X(t)+X˙(X,u,t)Δt
1.2.5 lookup table
当状态空间是低维并且光滑，lookup table可以用来对最优参数进行猜测。由于一般使用初始和最终位置(x,y)、角度
ψ
\psi
ψ、和初始和最终的曲率
k
0
,
k
f
k_0,k_f
k0,kf作为边界条件，因此lookup table使用这5维参数作为查询条件，查询最优参数的初始猜测。
代码中把(0,0)坐标作为初始状态，采样不同的终点位置、终点航向、初始曲率，利用牛顿迭代计算最优参数，存储在表中。在规划时，从表中查找最接近终点状态和初始曲率的那个参数，作为用于迭代的最初参数，以便参数快速收敛。
1.3.状态空间采样
来自第二篇论文。
1.3.1局部规划的算法框架为
- 算法输入：
- p s s p_{ss} pss参与状态空间采样的参数向量
- X I X_I XI 初始状态
- f ( x , u ( p , x ) ) f(x,u(p,x)) f(x,u(p,x)) 运动微分方程
- 算法输出:
- u N ( p N , x ) u_N(p_N,x) uN(pN,x)，对于不同的终止状态 X N X_N XN，生成控制量 u N u_N uN，及对应的最优参数 p N p_N pN
- c N c_N cN，不同终止状态的cost
- 算法内容:
- 根据采样参数 p s s p_{ss} pss及初始状态 X I X_I XI ，生成采样集合 X N X_N XN
- 循环采样集合
X
N
X_N
XN中的每个状态
X
F
X_F
XF
- 按照初始状态 X I X_I XI ，最终状态 X F X_F XF，及微分方程 f ( x , u ( p , x ) ) f(x,u(p,x)) f(x,u(p,x))，生成最优控制量 u i ( p i , x ) u_i(p_i,x) ui(pi,x)
- 如果控制量有效，计算轨迹的cost c i c_i ci
- 返回所有控制量集合 c N c_N cN和cost集合 c N c_N cN
1.3.2 均匀采样边界状态伪代码
- 算法输入
-
p
s
s
p_{ss}
pss 参与状态空间采样的参数，
p
s
s
=
[
n
p
,
n
h
,
d
,
α
m
i
n
,
α
m
a
x
,
ψ
m
i
n
,
ψ
m
a
x
]
T
p_{ss} = [n_p,n_h,d,\alpha_{min},\alpha_{max},\psi_{min},\psi_{max}]^T
pss=[np,nh,d,αmin,αmax,ψmin,ψmax]T
- n p n_p np，位置(position)采样的个数
- n h n_h nh，航向(heading)采样的个数
- d d d，初始点到终点的直线距离
- α m i n , α m a x \alpha_{min},\alpha_{max} αmin,αmax, 位置采样的角度范围
- ψ m i n , ψ m a x \psi_{min},\psi_{max} ψmin,ψmax, 航向角偏移量的角度范围
- X I X_I XI初始状态
-
p
s
s
p_{ss}
pss 参与状态空间采样的参数，
p
s
s
=
[
n
p
,
n
h
,
d
,
α
m
i
n
,
α
m
a
x
,
ψ
m
i
n
,
ψ
m
a
x
]
T
p_{ss} = [n_p,n_h,d,\alpha_{min},\alpha_{max},\psi_{min},\psi_{max}]^T
pss=[np,nh,d,αmin,αmax,ψmin,ψmax]T
- 算法输出
- X N X_N XN采样的边界状态集合
- 算法内容
- 按照位置采样的角度范围均匀采样角度，生成位置坐标 ( x , y ) (x,y) (x,y)
- 按照航向角角度范围均匀采样，对起点到终点的方向进行偏移
下图表示不同的初始角速度，相同的均匀采样生成的终止边界状态。
1.3.3 有global guidance的边界状态采样
主要思想是全局规划器会有一个cost判断，走到cost高的地方，再向目标点出发，会有更低的概率走出一条效率高且feasible的路，因此在采样阶段，在cost低的地方稠密的采样，在cost高的地方更稀疏的采样。
- 算法输入
-
p
s
s
p_{ss}
pss 参与状态空间采样的参数，
p
s
s
=
[
n
s
,
n
p
,
n
h
,
d
,
α
m
i
n
,
α
m
a
x
,
ψ
m
i
n
,
ψ
m
a
x
]
T
p_{ss} = [n_s,n_p,n_h,d,\alpha_{min},\alpha_{max},\psi_{min},\psi_{max}]^T
pss=[ns,np,nh,d,αmin,αmax,ψmin,ψmax]T
- n s n_s ns，global guide navigation function 采样数目
- X I X_I XI初始状态
-
p
s
s
p_{ss}
pss 参与状态空间采样的参数，
p
s
s
=
[
n
s
,
n
p
,
n
h
,
d
,
α
m
i
n
,
α
m
a
x
,
ψ
m
i
n
,
ψ
m
a
x
]
T
p_{ss} = [n_s,n_p,n_h,d,\alpha_{min},\alpha_{max},\psi_{min},\psi_{max}]^T
pss=[ns,np,nh,d,αmin,αmax,ψmin,ψmax]T
- 算法输出
- X N X_N XN采样的边界状态集合
- 算法内容
- 均匀采样navigation function，按照 n s n_s ns把均匀采样角度计算坐标用global guide 计算cost (如点到目标点的距离 x , y , ψ x,y, \psi x,y,ψ，距离越远cost越大)
- 通过计算最大值和采样值的差距除以cost总和的方式创建一个分布
- 对这个分布积分 ∫ α m i n α c n a v \int_{\alpha_{min}}^\alpha cnav ∫αminαcnav，最这个积分进行均匀采样，生成对终止位置角度 α \alpha α的非均匀采样
- 生成位置
- 均匀采样航向角进行偏移
1.3.4 环境约束的边界采样
考虑到道路的形状，在沿道路向前一定距离、垂直于道路中心线的横切线上采样，或者使用躲避障碍物和切换道路的策略。
- 算法输入
-
p
s
s
p_{ss}
pss 参与状态空间采样的参数，
p
s
s
=
[
l
c
e
n
t
e
r
,
l
h
e
a
d
i
n
g
,
l
w
i
d
t
h
,
v
w
i
d
t
h
,
d
,
n
p
,
n
l
]
T
p_{ss} = [l_{center},l_{heading},l_{width},v_{width},d,n_p,n_l]^T
pss=[lcenter,lheading,lwidth,vwidth,d,np,nl]T
- l c e n t e r l_{center} lcenter，道路中心线的集合
- l c e n t e r l_{center} lcenter, 道路的航向
- l w i d t h l_{width} lwidth，道路的宽
- v w i d t h v_{width} vwidth，车宽
- d d d，沿道路向前的弧长
- n p n_p np，每条路线上横向偏移的采样数目
- n l n_l nl，路线数目
- X I X_I XI初始状态
-
p
s
s
p_{ss}
pss 参与状态空间采样的参数，
p
s
s
=
[
l
c
e
n
t
e
r
,
l
h
e
a
d
i
n
g
,
l
w
i
d
t
h
,
v
w
i
d
t
h
,
d
,
n
p
,
n
l
]
T
p_{ss} = [l_{center},l_{heading},l_{width},v_{width},d,n_p,n_l]^T
pss=[lcenter,lheading,lwidth,vwidth,d,np,nl]T
- 算法输出
- X N X_N XN采样的边界状态集合
- 算法内容
- 对于每条路，沿道路中心线向前移动 d d d距离时道路的中心坐标 x c e n t e r x_{center} xcenter
- 按照每条路的采样数目，计算采样点距路线中心的偏移量
- 计算路线的航向
- 根据路线中心偏移量计算边界状态坐标
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
