frenet坐标系与笛卡尔坐标系转换公式

车辆笛卡尔坐标系状态：$q_1=(x,y,{\theta }_x,{\kappa }_x,v,a)$，即车辆位置x,y，航向角，曲率，速度，加速度
车辆frenet坐标系状态：$q_2=(s_r,{\dot{s}}_r,{\ddot{s}}_r,l,l^{\prime },l^{\prime \prime })$，其中${\dot{s}}_r=\frac{d{s}_r}{dt}$,${\ddot{s}}_r=\frac{d^2{s}_r}{d{t}^2}$,$l^{\prime }=\frac{dl}{d{s}_r}$,$l^{\prime }=\frac{ddl}{d{s}_r^2}$，即沿参考线上的里程s，其相对时间的一二阶导数，横向偏移l，其相对s的一二阶导数
指引线参考点坐标系状态：$q_3=(s_r,x_r,y_r,{\theta }_r,{\kappa }_r,d{\kappa }_r)$，即参考点的累计长度s，位置xy，航向角，曲率，曲率对s导数。

frenet坐标系转笛卡尔坐标系

1、已知frenet坐标系状态：$q_2=(s_r,{\dot{s}}_r,{\ddot{s}}_r,l,l^{\prime },l^{\prime \prime })$和reference line，根据指引线插值得到s_r对应参考点状态