车辆坐标系下历史轨迹的坐标变换

        在一些智能驾驶的控制算法中，常常需要使用一些目标，车道线的历史轨迹去做曲线拟合或补偿，此时，我们需要对历史轨迹上的点根据车辆的运动特性，进行相应的坐标旋转平移等变换，得到当前时刻下，较为准确的历史坐标。

         如上图所示，坐标系XOY为当前车辆坐标系，F为当前时刻前车相对自车的位置点，坐标为（x，y）。下一时刻，当车辆从O点走至O’点时，短时间可认为车辆走了一段圆弧，此时半径R = V / yawrate ，角度θ = yawrate * t，则车辆移动的位置偏差为：

Δx = R * sinθ

Δy = R – R * cosθ

建立辅助坐标系X’O’Y’，假设X1O’Y1为下一时刻车辆的坐标系，此时前车的历史轨迹点F在下一时刻坐标系的位置为：X1 = O’C，Y1 = O‘D

已知：

AB =（y – Δy ）*cosθ 

AF = x – Δx

AE = AF * sinθ =（x – Δx）*sinθ

O’B = （y – Δy ）*sinθ

BC = FE = AF * cosθ = （ x – Δx ）* cosθ 

所以：

X1 = O’C = O’B + BC = （y – Δy ）*sinθ + （ x – Δx ）* cosθ。

Y1 = O‘D = AB – AE = （y – Δy ）*cosθ  - （x – Δx）*sinθ

所以历史轨迹在新的坐标系下的坐标可总结为：