机器人/无人车-运动规划–车辆运动状态在参考线上的纵横分离推导
文章目录
背景简介
- 车辆(ego)位于笛卡尔坐标系中,运动状态 ( x e , y e , v e , a e , θ e , k e ) (x_e,y_e,v_e,a_e,\theta_e,k_e) (xe,ye,ve,ae,θe,ke)。
- 参考线(referenceline)位于笛卡尔坐标系中,利用Frenet框架对其进行描述,坐标点 ( s r , x r , y r , θ r , k r , k r ′ ) (s_r, x_r, y_r, \theta_r,k_r,k_r') (sr,xr,yr,θr,kr,kr′)
- 求解车辆运动状态在参考系的Frenet框架下的横纵分离后的运动参数 ( l , l ′ , l ′ ′ , s , s ′ , s ′ ′ ) (l,l',l'',s,s',s'') (l,l′,l′′,s,s′,s′′)
初始条件
车辆(Ego)运动状态 ( x e , y e , v e , a e , θ e , k e ) (x_e,y_e,v_e,a_e,\theta_e,k_e) (xe,ye,ve,ae,θe,ke)
笛卡尔坐标系中的运动状态
- x e x_e xe:笛卡尔坐标系中车身的位置,x值
- y e y_e ye:笛卡尔坐标系中车身的位置,y值
- v e v_e ve:笛卡尔坐标系中车身的速度,沿车身方向,非向量
- a e a_e ae:笛卡尔坐标系中车身的加速度,沿车身方向,非向心加速度,非向量
- θ e \theta_e θe:笛卡尔坐标系中的车身朝向,非前轮朝向,非方向盘转角
- k e k_e ke:笛卡尔坐标系中的车身运动曲率
参考线(referenceline)上的坐标点 ( s r , x r , y r , θ r , k r , k r ′ ) (s_r, x_r, y_r, \theta_r,k_r,k_r') (sr,xr,yr,θr,kr,kr′)
Frenet框架下的参考点,也是当前车辆的投影点
- s r s_r sr:参考点坐标,在FF坐标系下的参考线上的坐标点s值,里程/距离
- x r x_r xr:在笛卡尔坐标系中的x值
- y r y_r yr:在笛卡尔坐标系中的y值
- θ r \theta_r θr:在笛卡尔坐标系中的角度值,朝向
- k r k_r kr:参考点处的曲率值(笛卡尔坐标系中)
- k r ’ k_r’ kr’:参考点处的曲率的变化率(对s求导)(笛卡尔坐标系中)
注意: l l l 有正负之分
- 在参考线坐标系上, l l l有正负之分
- 沿着参考线前进,位于参考线左侧的点为正 l > 0 l>0 l>0,右侧的点为负 l < 0 l<0 l<0。
注意: k r k_r kr 有正负之分
- 在参考线坐标系上, k r k_r kr有正负之分,
- 沿着参考线前进,逆时针旋转为正 k r > 0 k_r>0 kr>0,顺时针旋转为负 k r < 0 k_r<0 kr<0。
关系推导
将车辆运动状态转化到参考线的Frenet框架上,按横纵 l − s l-s l−s进行分解,并求解出0阶、1阶、2阶的导数 ( l , l ′ , l ′ ′ , s , s ′ , s ′ ′ ) (l,l',l'',s,s',s'') (l,l′,l′′,s,s′,s′′)
前提假设
- 车辆Ego沿现有车身方向做微量匀速运动(不是匀加速),当运动量过大时,误差会明显增大,此时计算失去意义。
- 微量运动情况下,运动的弧长由线段长度代替,如 D ⌢ ≈ Δ D 、 E ⌢ ≈ Δ E \overset{\frown}{D}\approx\Delta D、\overset{\frown}{E}\approx\Delta E D⌢≈ΔD、E⌢≈ΔE
- 弧长无方向性,永远为非负值,即 Δ D > = 0 、 Δ E > = 0 \Delta D>=0、\Delta E>=0 ΔD>=0、ΔE>=0永远成立,取值范围为 [ 0 , ∞ ) [0,\infty) [0,∞)
- 半径无方向性,永远为非负值,即 R e > = 0 、 R r > = 0 R_e>=0、R_r>=0 Re>=0、Rr>=0永远成立,取值范围为 [ 0 , ∞ ) [0,\infty) [0,∞)
- 曲率具有方向性,逆时针旋转为正,顺时针旋转为负,即 k e 、 k r k_e、k_r ke、kr的取值范围为 ( − ∞ , ∞ ) (-\infty,\infty) (−∞,∞)
运动关系示意图
横向求解,对 s s s 求导
0阶: l l l
大小求解
∣ l ∣ = ( x e − x r ) 2 + ( y e − y r ) 2 |l| = \sqrt{(x_e-x_r)^2+(y_e-y_r)^2} ∣l∣=(xe−xr)2+(ye−yr)2
正负判断
- 沿着参考线左侧为正,右侧为负,可以用向量外积求解,可参考此文
V e =