基于车道线的直线辅助横向定位

已知信息

1.当前位置$P$P距离左右车道线距离$d_1,d_2$d1​,d2​
2.惯导推测出的错误位置$P'$P′
3.左右车道线的直线方程$y_1=K_1x+B_1,y_2=K_2x+B_1$y1​=K1​x+B1​,y2​=K2​x+B1​

已知信息获取方式

1.通过视觉检查车道线，根据相机标定数据解算出$d_1,d_2$d1​,d2​
2.惯导输出
3.在惯导测量准确的地方，采集车道线的一个点，计算得到直线方程

算法目的

通过车辆距离车道线左右两侧的距离，把惯导推算出的错误位置，拉回到车道之间，减小车体坐标系中横向误差，然后投射到全局坐标中

算法流程

如图所示，红色线为两条车道线，圆形点为当前实际位置$P$P，$d1$d1为$P$P与左车道线距离，$d2$d2为$P$P与右车道线距离。矩形点为惯导输出的错误位置，利用车道线的距离可以确定行驶路线(蓝色线条)直线方程，在这条直线上选取和错误位置最近一点把定位点拉回到车道行驶路线中，缩小车体坐标系中横向误差，根据在行驶直线方程上得到的位置，来取代惯导错误的定位。