本发明公开了一种无人车横向控制用的偏差精准估计方法，通过横向偏差精准估计模块和航向偏差精准估计模块实现。该方法根据期望轨迹与当前车辆位置的相对关系，按照第一点、最后一点、途径点三种不同的类别判断生成最优估计点，通过最优估计点来计算横向偏差估计和航向偏差估计；将计算得到的横向偏差估计与航向偏差估计作为反馈控制量带入轨迹跟踪控制率，从而实现无人驾驶车辆的精准跟踪控制。本发明可以有效估计车辆控制的横向偏差和航向偏差，很好的克服了期望轨迹上相邻点之间横向位置和航向变化过大的问题，在掉头或急转弯等大曲率的任务中提升尤为显著，从而有效提高车辆控制的跟踪精度。
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不是期望轨迹上的第一点且点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的估计值；即：;
其中，|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所述的一种无人车横向控制用的偏差精准估计方法，其特征在于，将所述横向偏差估计与航向偏差估计作为反馈控制量带入轨迹跟踪控制率，从而实现无人驾驶车辆的精准跟踪控制。
一种无人车横向控制用的偏差精准估计方法
技术领域
[0001] 本发明属于无人车运动控制领域，具体涉及一种无人车横向控制用的偏差精准估计方法。
背景技术
[0002] 精准轨迹跟踪技术是实现无人驾驶重要组成部分之一。目前轨迹跟踪技术主要包括基于预瞄点和最近点两种跟踪方法，针对掉头或急转弯等大曲率路况的路况，应用预瞄点的轨迹跟踪技术会出现提前开始转弯与提前结束转弯的现象，最终导致跟踪精度差或无法通行等；应用最近点跟踪方法实现对大曲率路况精准跟踪，其最重要的反馈输入在于偏差，现有算法以通过当前GPS位置点在期望轨迹上遍历最近点，最近点与当前点的横向距离为横向偏差，计算出的横向偏差准确度不足；以最近点与当前点的航向差为航向偏差，在大曲率路况中，期望轨迹上各点之间的航向变化较大，因此用该方法计算得到的航向偏差准确度差。因此横向偏差与航向偏差估计的精准性是直接影响无人驾驶车辆跟踪效果。
发明内容
[0003] 本发明为克服现有技术中的不足之处，提供了一种无人车横向控制用的偏差精准估计方法。通过精准估计无人车运行过程中的横向偏差与航向偏差，再利用该偏差实现车辆的反馈控制，最终实现无人驾驶车辆的精准控制。
[0004] 为达到上述发明目的，本发明采用的技术方案为：
[0005] 一种无人车横向控制用的偏差精准估计方法，通过横向偏差精准估计模块和航向偏差精准估计模块实现对偏差的精准估计；
[0006] 所述横向偏差精准估计模块的输入为期望轨迹，输出为横向偏差估计，所述期望轨迹包含车辆期望行驶经过的点的经度、纬度及航向信息，通过比较所述期望轨迹中距离车辆当前位置的原点O最小的点C与车辆当前位置的原点O之间的空间关系，按照第一点、最后一点、途径点三种不同的类别判断生成最优估计点， 获得最优估计点与车辆当前位置的原点O之间的位置偏差，计算得到横向偏差估计lateral_error；
[0007] 所述航向偏差精准估计模块的输入为期望轨迹，输出为航向偏差估计，通过计算获得最优估计点处的航向估计，与车辆当前航向 相比较，得到航向偏差估计；进一步地，所述横向偏差估计的计算方法如下：
[0008] 步骤1：将期望轨迹上的各点转换为车辆坐标系下的期望路径点，所述车辆坐标系是以当前车辆经纬度为原点O(xo,yo)、当前车辆航向为X轴的右手坐标系；
[0009] 步骤2：遍历所述期望轨迹，选出所述期望轨迹中距离车辆当前位置的原点O(xo,yo)最小的点C(xc,yc) ；
[0010] 步骤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后的一点；即：
[0011]
[0012] 步骤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的估计值；即：
[0013]
[0014]
[0015] 同理，
[0016]
[0017]
[0018] 步骤5：比较步骤3或步骤4中获得的min_0和min_1的大小，取其中较小的值为横向偏差估计lateral_error的绝对值：
[0019] |lateral_error| = min( min_0, min_1);
[0020] 步骤6：判断点C在当前车辆坐标系下的yc的正负，若yc<0则横向偏差估计为负，若yc>0则横向偏差估计为正。
[0021] 所述航向偏差估计的计算方法如下：
[0022] 步骤1：若最小横向偏差估计点为期望轨迹上的点，即为点C，点C的航向为 ，当前航向为 ，根据下述公式得出航向偏差估计：
[0023]
[0024] 步骤2：若最小横向偏差估计点不是期望轨迹上的点，根据横向偏差精准估计模块得到最优估计点N，若点N为点C与点F之间的点，通过下式计算得到点C与点N之间的距离|CN|、点C与点F之间的距离|CF|：
[0025]
[0026] 根据车辆运行的物理空间连续性和平滑性，将点F与点C之间的轨迹看成一段圆弧，所述圆弧的圆心为点S，计算SN延长线与所述圆弧的交点M的航向 ，作为点N的航向估计 ，通过下式得出航向偏差估计：
[0027]
[0028] 式中， 为点N的航向估计， 为点M的航向， 为点F的航向， 为点C的航向， 为当前车辆航向， 为航向偏差估计。
[0029] 若点N为点C和点B之间的点，通过下式计算得到点B与点N之间的距离|BN|、点C与点B之间的距离|CB|：
[0030]
[0031] 其中，|CO|为点C与点O之间的距离，|ON|为点O到点N之间的距离；
[0032] 根据车辆运行的物理空间连续性和平滑性，将点B与点C之间的轨迹看成一段圆弧，所述圆弧的圆心为点S，计算SN延长线与所述圆弧的交点M的航向 ，作为点N的航向估计 ，通过下式得出航向偏差估计：
[0033]
[0034] 式中， 为点N的航向估计， 为点M的航向， 为点B的航向， 为点C的航向， 为当前车辆航向， 为航向偏差估计。
[0035] 进一步的，将所述横向偏差估计与航向偏差估计作为反馈控制量带入轨迹跟踪控制率，从而实现无人驾驶车辆的精准跟踪控制。
[0036] 有益效果：
[0037] 通过本发明可以精准估计当前车辆相对于期望轨迹的横向偏差与航向偏差，减小系统误差输入，在掉头或急转弯等大曲率的任务中提升尤为显著；通过该方案可以有效估计车辆控制的横向偏差和航向偏差，很好的克服了期望轨迹上相邻点之间横向位置和航向变化过大的问题，从而有效提高车辆控制的跟踪精度。本发明提出的估计方法计算简单，估计精度高，便于使用。
附图说明
[0038] 图1为本发明的无人车横向控制用的偏差精准估计方法的横向偏差精准估计的分析图；
[0039] 图2为本发明的无人车横向控制用的偏差精准估计方法的横向偏差精准估计的流程图；
[0040] 图3为本发明的无人车横向控制用的偏差精准估计方法的航向偏差精准估计的第一种分析图。
[0041] 图4为本发明的无人车横向控制用的偏差精准估计方法的航向偏差精准估计的第二种分析图。
具体实施方式
[0042] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
[0043] 本实施例中，如图1，图2，图3所示，本发明的一种无人车横向控制用的横向偏差精准估计方法,按如下步骤进行：
[0044] 步骤一、期望轨迹获取：
[0045] 预先应用GPS/IMU组合导航系统采集一段期望轨迹，该期望轨迹包括车辆实时的经度、纬度与航向信息。在实际的应用过程中，期望轨迹也可以通过路径规划的方式进行生成。
[0046] 步骤二、通过GPS/IMU组合导航系统获取当前时刻车辆的GPS位置和航向信息，并将预先采集的期望轨迹的点转换为以当前时刻位姿为原点的车辆坐标系下的期望路径点：
[0047]
[0048] 式中，R是地球半径，PI表示圆周率，lat是期望路点的纬度，lng是期望路点的经度，lat0是当前时刻车辆的纬度，lng0是当前时刻车辆的经度，X，Y是期望轨迹点在当前车辆坐标平面的垂直投影，在az是当前时刻车辆的航向，x，y是期望路点在车辆坐标系下的坐标。
[0049] 步骤三、遍历期望轨迹中距离车辆当前位置O(xo,yo)最小的点C(xc,yc) ，计算期望路点与当前车辆之间距离dis：
[0050]
[0051] 式中，x，y是期望路点在车辆坐标系下的坐标，dis是期望路点与当前车辆之间距离。
[0052] 步骤四、 判断点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后的一点；
[0053]
[0054] 步骤五、若点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的估计值；即：
[0055]
[0056]
[0057] 同理，
[0058]
[0059]
[0060] 步骤六、比较min_0和min_1的大小，取其中较小的值作为横向偏差估计的绝对值：
[0061] |lateral_error| = min( min_0, min_1);
[0062] 步骤七、判断点C在当前车辆坐标系下，其yc的正负，若yc<0则横向偏差估计为负，若yc>0则横向偏差估计为正。
[0063] 步骤八、根据步骤五和步骤六获得的最小横向偏差估计及最小横向偏差估计点，若最小横向偏差估计点为期望轨迹上的点，即为点C，点C的航向为 ，当前航向为 ，根据下述公式可以得出航向偏差估计：
[0064]
[0065] 步骤九： 若最小横向偏差估计点不是期望轨迹上的点，根据横向偏差精准估计模块得到最优估计点N，若点N为点C与点F之间的点，如图3所示，通过下式计算得到点C与点N之间的距离|CN|、点C与点F之间的距离|CF|：
[0066]
[0067] 其中，|CO|为点C与点O之间的距离，|ON|为点O到点N之间的距离。
[0068] 根据车辆运行的物理空间连续性和平滑性，将点F与点C之间的轨迹看成一段圆弧，所述圆弧的圆心为点S，计算SN延长线与所述圆弧的交点M的航向 ，作为点N的航向估计 ，通过下式得出航向偏差估计：
[0069]
[0070] 式中， 为点N的航向估计， 为点M的航向， 为点F的航向， 为点C的航向， 为当前车辆航向， 为航向偏差估计。
[0071] 若点N为点C和点B之间的点，如图4所示，通过下式计算得到点B与点N之间的距离|BN|、点C与点B之间的距离|CB|：
[0072]
[0073] 根据车辆运行的物理空间连续性和平滑性，将点B与点C之间的轨迹看成一段圆弧，所述圆弧的圆心为点S，计算SN延长线与所述圆弧的交点M的航向 ，作为点N的航向估计 ，通过下式得出航向偏差估计：
[0074]
[0075] 式中， 为点N的航向估计， 为点M的航向， 为点B的航向， 为点C的航向， 为当前车辆航向， 为航向偏差估计。
[0076] 步骤十：应用下式的轨迹跟踪控制器，根据上述得到的横向偏差估计与航向偏差估计求解得到车辆的期望转向曲率：
[0077] q0=tan(－1*(heading_error*PI/180)+arctan(k*lateral_error/(speed))/L[0078] 式中，heading_error为航向偏差估计，lateral_error为横向偏差估计，k为轨迹跟踪实现需要调节的比例参数，speed为无人车当前速度，L为无人车的轴距，q0为期望转向曲率。
[0079] 步骤十一：根据车辆特性求解期望转向角度。以某型雪佛兰科帕奇车辆为例，根据该车方向盘转向大小与车辆转向曲率之间的关系得到期望转向角度：
[0080] turn=－4857*q0*q0*q0+117.4*q0*q0+2753*q0+3266
[0081] 式中q0为车辆期望转向曲率，turn为车辆期望转向角度。
[0082] 本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
