智能驾驶中自车轨迹的计算

           目前，带有辅助驾驶功能的车辆生成自车轨迹的方式通常为：通过车辆的EPS(Electronic‑Power‑Steering，电动助力转向系统)传感器输出的横摆角速度计算出与横摆角速度对应的轨迹曲率，进而得到车辆轨迹。但是EPS传感器容易受到外界干扰，因此，根据EPS传感器输出的横摆角速度计算得到的车辆轨迹存在波动较大、噪声较大以及跳帧的现象。车辆的控制终端根据EPS传感器输出的横摆角速度得到的车辆轨迹与目标车辆轨迹生成控制信号以对车辆的方向盘进行控制时，容易导致方向盘存在较大波动和抖动的现象。

          因此在智能驾驶控制算法中，通常通过车辆前轮转角得到对应的横摆角速度，再利用EPS输出的横摆角速度与前轮转角推导出的横摆角速度得到横摆角速度补偿量，并将其补偿到前轮转角算出的横摆角速度上，得到的横摆角速度比直接由车辆传感器输出的横摆角速度更加稳定、平滑，且无噪声和毛刺，还不失真，提高了对车辆进行控制的稳定性。

1.通过方向盘转角推导横摆角速度

由二自由度模型运动微分方程（推导过程可参考线性二自由度汽车模型的微分方程-CSDN博客）：

       可知，车辆在作稳态转向运动时，其质心侧偏角和横摆角速率不变，因此，将稳态条件dB/dt = 0和dWr/dt = 0分别带入上述公式，可以得到前轮转角与汽车横摆角速率之间的关系：

      其中，L为轴距，a为质心到前轴的距离，b为质心到后轴的距离，为车辆前轴的侧偏刚度，为车辆后轴的侧偏刚度，K稳定性因数，是表征车辆稳态影响的一个重要参数。其中，K是通过特征车速Vch得到的，K与特征车速之间存在如下等式关系

，所以上式可表达为：

2.计算自车轨迹C0

已知前轮转角Delta = SteerAngle/Ration，其中，Ration为传动比，设方向盘转角增益为Gain,则

所以通过方向盘转角计算出的横摆角速度为SteerAng_YawRate:

                                                SteerAng_YawRate = SteerAngle * Gain

已知车辆本身的横摆角速度为：Vehicle_YawRate，则其与方向盘转角计算出的横摆角速度之差YawRateError为：

                                     YawRateError = SteerAng_YawRate - Vehicle_YawRate

由此可推算出需要补偿的方向盘角度AngleOffset为：

                                    AngleOffset  = YawRateError  / Gain

所以可得补偿后的横摆角速度YawRate为：

                                  YawRate = ( SteerAngle - AngleOffset) * Gain

已知Wr = V / R，可得自车曲率C0为：