一种无人车横向控制用的偏差精准估计方法与流程

技术特征：
1.一种无人车横向控制用的偏差精准估计方法，其特征在于，通过横向偏差精准估计模块和航向偏差精准估计模块实现对偏差的精准估计；所述横向偏差精准估计模块的输入为期望轨迹，输出为横向偏差估计，所述期望轨迹包含车辆期望行驶经过的点的经度、纬度及航向信息，通过比较所述期望轨迹中距离车辆当前位置的原点o最小的点c与车辆当前位置的原点o之间的空间关系，按照第一点、最后一点、途径点三种不同的类别判断生成最优估计点，获得最优估计点与车辆当前位置的原点o之间的位置偏差，计算得到横向偏差估计lateral_error；所述航向偏差精准估计模块的输入为期望轨迹，输出为航向偏差估计，通过计算获得最优估计点处的航向估计，与车辆当前航向相比较，得到航向偏差估计；所述横向偏差估计的计算方法如下：步骤1：将期望轨迹上的各点转换为车辆坐标系下的期望路径点，所述车辆坐标系是以当前车辆经纬度为车辆当前位置的原点o(xo,yo)、当前车辆航向为x轴的右手坐标系；步骤2：遍历所述期望轨迹，选出所述期望轨迹中距离车辆当前位置的原点o(xo,yo)最小的点c(xc,yc) ；步骤3：判断点c是否是所述期望轨迹上的第一点，若是，则取点f等于点c，即（xf,yf）=（xc,yc），min_0等于点f与点o之间的距离值|fo|；若不是，则取点f为期望轨迹上点c的前一点；判断点c是否是期望轨迹上的最后一点，若是，则取点b等于点c，即（xb,yb）=（xc,yc），min_1等于点b与点o之间的距离|bo|，若不是，则b点为期望轨迹上点c后的一点；即：其中，|co|为点c与点o之间的距离，|fo|为点f到点o之间的距离；步骤4：若点c不是期望轨迹上的第一点或最后一点，判断∠bco与∠fco的余弦值是否大于0；若cos∠fco大于0，则∠fco为锐角，表明点f与点c之间的连线fc上存在比点c与点o之间的距离|co|更近的点n，其值|on|等于点o到fc的距离，将该值作为min_0的估计值；若cos∠fco小于0，则∠fco为钝角，说明点f与点c之间的连线fc上不存在比点c与点o之间的距离|co|更近的点，将|co|作为min_0的估计值；即：n_0的估计值；即：同理，
其中，|fc|为点f与点c之间的距离，|bc|为点b到点c之间的距离；步骤5：比较步骤3或步骤4中获得的min_0和min_1的大小，取其中较小的值为横向偏差估计lateral_error的绝对值：|lateral_error| = min( min_0, min_1);步骤6：判断点c在当前车辆坐标系下的yc的正负，若yc<0则横向偏差估计为负，若yc>0则横向偏差估计为正。2.根据权利要求1所述的一种无人车横向控制用的偏差精准估计方法，其特征在于，所述航向偏差估计的计算方法如下：步骤1：若最小横向偏差估计点为期望轨迹上的点，即为点c，点c的航向为，当前航向为，根据下述公式得出航向偏差估计：步骤2：若最小横向偏差估计点不是期望轨迹上的点，根据横向偏差精准估计模块得到最优估计点，记为点n，若点n为点c与点f之间的点，通过下式计算得到点c与点n之间的距离|cn|、点c与点f之间的距离|cf|：其中，|co|为点c与点o之间的距离，|on|为点o到点n之间的距离；根据车辆运行的物理空间连续性和平滑性，将点f与点c之间的轨迹看成一段圆弧，所述圆弧的圆心为点s，计算sn延长线与所述圆弧的交点m的航向，作为点n的航向估计，通过下式得出航向偏差估计：式中，为点n的航向估计，为点m的航向，为点f的航向，为点c的航向，为当前车辆航向，为航向偏差估计；
若点n为点c和点b之间的点，通过下式计算得到点b与点n之间的距离|bn|、点c与点b之间的距离|cb|：根据车辆运行的物理空间连续性和平滑性，将点b与点c之间的轨迹看成一段圆弧，所述圆弧的圆心为点s，计算sn延长线与所述圆弧的交点m的航向，作为点n的航向估计，通过下式得出航向偏差估计：式中，为点n的航向估计，为点m的航向，为点b的航向，为点c的航向，为当前车辆航向，为航向偏差估计。3.根据权利要求2所述的一种无人车横向控制用的偏差精准估计方法，其特征在于，将所述横向偏差估计与航向偏差估计作为反馈控制量带入轨迹跟踪控制率，从而实现无人驾驶车辆的精准跟踪控制。

技术总结
本发明公开了一种无人车横向控制用的偏差精准估计方法，通过横向偏差精准估计模块和航向偏差精准估计模块实现。该方法根据期望轨迹与当前车辆位置的相对关系，按照第一点、最后一点、途径点三种不同的类别判断生成最优估计点，通过最优估计点来计算横向偏差估计和航向偏差估计；将计算得到的横向偏差估计与航向偏差估计作为反馈控制量带入轨迹跟踪控制率，从而实现无人驾驶车辆的精准跟踪控制。本发明可以有效估计车辆控制的横向偏差和航向偏差，很好的克服了期望轨迹上相邻点之间横向位置和航向变化过大的问题，在掉头或急转弯等大曲率的任务中提升尤为显著，从而有效提高车辆控制的跟踪精度。制的跟踪精度。制的跟踪精度。


技术研发人员：孙超 王智灵
受保护的技术使用者：安徽中科星驰自动驾驶技术有限责任公司
技术研发日：2022.11.28
技术公布日：2022/12/30