其中曲率的k大小往往由系统的输入限制条件决定。在此基础上，考虑一条由车辆运动产生的连续轨迹（Path）。我们称沿着轨迹的方向的位移为S方向。轨迹相对于车辆姿态的系统关系由下列偏微分方程式给出：

　　我们的轨迹规划（Trajectory Planning）算法非常依赖于地图对于道路的定义。这里我们定义道路由其道路中心线（Center Line）所定义，且定义道路的采样函数为：

　　其中s代表道路的中心线切向方向的位移（也称为纵向位移s）。于此对应的是道路的中心线垂直方向位移l，也称之为横向位移。如果考虑一个车辆的姿态点p点在道路上(s,l)坐标，那么其实际的姿态和(s,l)的关系

　　满足：

　　其中曲率Kr定义为在道路转弯的内侧曲率加大（随纵向位移l加大），外侧曲率则减小。我们使用右手坐标系，所以如图6所示在靠近原点处朝x轴的正方向，纵向位移l朝着y轴正方向加大。假设对于某条道路Lane(k)，其纵向宽度lk保持不变。那么该条道路变可以表示成为一个随着中心线横向位移s的点集{p(s,lk):s∈R+}。我们称这样的一个坐标系统为坐标系统。

　　图6 XY平面下的SL坐标系统及其网格划分

　　在上述的车辆模型和道路模型下，我们讨论轨迹规划所产生的轨迹曲线。首先我们定义车辆的轨迹（Trajectory）为一个从[0,1]区间到车辆姿态向量集合C={x? }的连续映射：ρ:[0,1]→C 。其中，车辆的初始姿态向量为x? =(x,y,θ,k)。每条轨迹终点处如图7所示，轨迹1的终点姿态为ρ1(1)=qend1轨迹2的终点姿态向量为ρ2(1)=qend2，初始姿态为ρ1(0)=ρ2(0)=qinit。轨迹优化的目标便是在所有可能的轨迹曲线中，筛选出满足边界条件的轨迹曲线，再寻找一条/若干条平滑且Cost函数最低的曲线。其中轨迹的候选曲线我们用类似在路由寻径（Routing）模块中介绍的“撒点”的采样方式来生成。参考图7，在某条Lane的SL坐标系下，我们按照均匀切分的S和L方向的方格内，在固定S和L间隔下，考虑每个(si,lj)区域的中心点（如图7所示，又称为轨迹点Trajectory Point）。一条候选的轨迹（Trajectory）便可以看做是沿着Lane的中心线纵向位移s方向连接不同Trajectory Point的平滑曲线。在图7所示的道路SL分割和采样下，可能的Trajectory Point有16个（4个s位置，4个l位置），从车辆的初始位置出发，我们只考虑在s方向单调增大的可能，不考虑城市综合道路行驶中的倒车情况，那么总的候选曲线的总条数为44=256条。轨迹优化便是要在这256条候选的曲线中找出Cost最优的轨迹。

　　图7 SL坐标系下道路的分割采样以及可能的轨迹