【机器人】无人车-运动规划-参考线Frenet框架下横纵分离关系推导

机器人/无人车-运动规划–车辆运动状态在参考线上的纵横分离推导

背景简介

• 车辆（ego）位于笛卡尔坐标系中，运动状态$(x_e,y_e,v_e,a_e,{\theta }_e,k_e)$。
• 参考线（referenceline）位于笛卡尔坐标系中，利用Frenet框架对其进行描述，坐标点$(s_r,x_r,y_r,{\theta }_r,k_r,k_r^{\prime })$
• 求解车辆运动状态在参考系的Frenet框架下的横纵分离后的运动参数$(l,l^{\prime },l^{\prime \prime },s,s^{\prime },s^{\prime \prime })$

初始条件

车辆（Ego）运动状态$(x_e,y_e,v_e,a_e,{\theta }_e,k_e)$

笛卡尔坐标系中的运动状态

• $x_e$：笛卡尔坐标系中车身的位置，x值
• $y_e$：笛卡尔坐标系中车身的位置，y值
• $v_e$：笛卡尔坐标系中车身的速度，沿车身方向，非向量
• $a_e$：笛卡尔坐标系中车身的加速度，沿车身方向，非向心加速度，非向量
• ${\theta }_e$：笛卡尔坐标系中的车身朝向，非前轮朝向，非方向盘转角
• $k_e$：笛卡尔坐标系中的车身运动曲率

参考线（referenceline)上的坐标点$(s_r,x_r,y_r,{\theta }_r,k_r,k_r^{\prime })$

Frenet框架下的参考点，也是当前车辆的投影点

• $s_r$：参考点坐标，在FF坐标系下的参考线上的坐标点s值，里程/距离
• $x_r$：在笛卡尔坐标系中的x值
• $y_r$：在笛卡尔坐标系中的y值
• ${\theta }_r$：在笛卡尔坐标系中的角度值，朝向
• $k_r$：参考点处的曲率值（笛卡尔坐标系中）
• $k_r’$：参考点处的曲率的变化率（对s求导）（笛卡尔坐标系中）

注意：$l$ 有正负之分

• 在参考线坐标系上，$l$有正负之分
• 沿着参考线前进，位于参考线左侧的点为正$l>0$，右侧的点为负$l<0$。

注意：$k_r$ 有正负之分

• 在参考线坐标系上，$k_r$有正负之分，
• 沿着参考线前进，逆时针旋转为正$k_r>0$，顺时针旋转为负$k_r<0$。

关系推导

将车辆运动状态转化到参考线的Frenet框架上，按横纵$l-s$进行分解，并求解出0阶、1阶、2阶的导数$(l,l^{\prime },l^{\prime \prime },s,s^{\prime },s^{\prime \prime })$

前提假设

• 车辆Ego沿现有车身方向做微量匀速运动（不是匀加速），当运动量过大时，误差会明显增大，此时计算失去意义。
• 微量运动情况下，运动的弧长由线段长度代替，如$\stackrel{⌢}{D}\approx \Delta D、\stackrel{⌢}{E}\approx \Delta E$
• 弧长无方向性，永远为非负值，即$\Delta D>=0、\Delta E>=0$永远成立，取值范围为$[0,\infty )$
• 半径无方向性，永远为非负值，即$R_e>=0、R_r>=0$永远成立，取值范围为$[0,\infty )$
• 曲率具有方向性，逆时针旋转为正，顺时针旋转为负，即$k_e、k_r$的取值范围为$(-\infty ,\infty )$

运动关系示意图

横向求解，对 $s$ 求导

0阶：$l$

大小求解

$$|l|=\sqrt{(x_e-x_r{)}^2+(y_e-y_r{)}^2}$$

正负判断

• 沿着参考线左侧为正，右侧为负，可以用向量外积求解，可参考此文

$$\begin{array}{rl}{V}_e& =((x_e-x_r),(y_e-y_r),0)\\ V_s& =(cos({\theta }_r),sin({\theta }_r),0)\\ V_e\times V_s& =\left|\begin{array}{ccc}\vec{x}& \vec{y}& \vec{z}\\ (x_e-x_r)& (y_e-y_r)& 0\\ cos({\theta }_r)& sin({\theta }_r)& 0\end{array}\right|\end{array}$$

• 取 z ⃗ \vec{z}