Reference Path Estimation for Lateral Vehicle Control

车辆横向控制的参考路径估计

Abstract

Autonomous driving cars have been a hot topic in the media in recent years, with more and more tech companies and universities presenting projects with fully automated vehicles. Most of these vehicles rely on highly sophisticated and expensive sensors that are not yet feasible for commercial vehicles. On the other hand automated systems that are implemented in commercially available vehicles are largely limited to active safety scenarios where the system only assists the driver in dangerous situations such as collision avoidance or lane support.
自动驾驶汽车近年来一直是媒体的热门话题，越来越多的科技公司和大学展示了完全自动化车辆的项目。这些车辆大多依赖于高度复杂且昂贵的传感器，这些传感器对于商业车辆来说目前还不可实现。另一方面，在商业上可用的车辆中实施的自动化系统主要限于主动安全场景，其中系统仅在危险情况下协助驾驶员，例如碰撞避免或车道支持。
The goal of this thesis project is to use sensors on commercially available vehicles for automating lateral control in selected scenarios. The evaluated scenarios are limited to roads where the sensors can detect lane markings or a preceding vehicle or both. The approach was to generate two different reference paths, one from lane markings and one from the preceding vehicle information. The lane marking path is generated from filtering measurements from the lane detection cameras using a nonlinear Kalman filter. The preceding vehicle path is generated by fusing the radar and camera measurements of the preceding vehicle. In order to develop and tune the filters a detailed analysis was done on the sensor measurements collected for this project.
这篇论文项目的目标是使用商业车辆上的传感器来在选定场景中自动化横向控制。评估的场景仅限于传感器能够检测到车道标记或前车或两者的道路。方法是生成两条不同的参考路径，一条来自车道标记，一条来自前车信息。车道标记路径是通过使用非线性卡尔曼滤波器过滤车道检测摄像头的测量数据生成的。前车路径是通过融合前车的雷达和摄像头测量数据生成的。为了开发和调整滤波器，对该项目收集的传感器测量数据进行了详细分析。
The implemented filters improve the current system in several ways. When the lane marking are lost for short period of time the prediction from the last measurement update can provide a reference while driving up to 40 meters. The path generated from the estimates of the preceding vehicle describe the trajectory which the vehicle has driven. This way a more accurate reference signal can be generated than using only the current position of the preceding vehicle, especially in turns and at long distances. By having two references paths the lateral control is more robust and an algorithm that takes the covariances of the estimation for both paths into account guarantees a smooth transition between them.
实现的滤波器在多个方面对现有系统进行了改进。当车道标记短暂丢失时，最后测量更新的预测可以提供参考，允许车辆在长达40米的范围内行驶。根据前车估计生成的路径描述了车辆行驶的轨迹。这样，可以生成比仅使用前车当前位置更准确的参考信号，尤其是在转弯和长距离行驶时。拥有两条参考路径使得横向控制更加稳健，并且一个考虑到两种路径估计协方差的算法可以保证它们之间的平滑过渡。

4.4 Generating a reference with preceding vehicle

When a vehicle is driving in front of the Ego vehicle its position can be detected by the object detection sensors (radar and camera). These positions can be used as a reference for the lateral control of Ego vehicle.
当一辆车在自车（Ego vehicle）前方行驶时，其位置可以通过物体检测传感器（雷达和摄像头）被探测到。这些位置可以作为自车横向控制的参考。

4.4.1 Generating reference path

A reference path can be constructed from the filtered position of the preceding vehicle. The path contains N number of points in the Ego vehicles coordinate system. The points are placed equidistantly in the x-axis of the Vcs with a fixed longitudinal distance deltaPrec between them.
可以从前车经过滤波的位置构建一条参考路径。该路径包含N个点，这些点位于自车坐标系统中。这些点在 $V_{cs}$ 的 $x$ 轴上等距放置，它们之间有一个固定的纵向距离 $delt{a}_{Prec}$。

Initial path generated

When there is no vehicle visible to the sensors no path is generated. As soon as a vehicle appears in the field of view of the Ego vehicle a path between the Ego vehicle and the preceding vehicle is generated depending if the lane markings path is available or not. The path is a series of N points in the vehicle coordinate system. If the lane markings are available the points are placed parallel to the lane markings with an offset. The offset is the lateral difference between the position of the preceding vehicle to the lane markings. The lane marking path consists of N-1 points in the vehicle coordinate system, therefore the Nth point in the preceding vehicle path is the position of the preceding vehicle. This method of generating the initial path can be seen in figure 4.6 A. If no lane marking path is available the initial points are placed by linear interpolating six points between the Ego vehicle and the preceding vehicle (figure 4.6 B).
当传感器没有检测到任何车辆时，不会生成路径。一旦自车的视野中出现车辆，就会根据是否可用车道标记路径生成自车与前车之间的路径。路径是车辆坐标系统中的一系列N个点。如果车道标记可用，这些点会与车道标记平行放置，并有一个偏移量。偏移量是前车位置与车道标记之间的横向差异。车道标记路径由车辆坐标系统中的N-1个点组成，因此前车路径中的第N个点是前车的位置。生成初始路径的这种方法可以在图4.6 A 中看到。如果没有可用的车道标记路径，初始点是通过在自车和前车之间线性插值六个点来放置的（见图4.6 B）。

图4.6：当车辆进入视野时从前车生成的初始路径。A）展示了如果车道标记路径可用，如何通过偏移车道标记路径并添加前车位置作为第N个点来生成路径中的N个点；B）展示了当没有车道标记路径可用时，如何通过自车位置和前车位置之间的线性插值来生成这些点。

Movement of the path

The points of the path represent the trajectory of the preceding vehicle in Ego vehicle coordinate system. Since the coordinate system moves with the Ego vehicle the points need to move within the coordinate system according to the distance traveled (dx) and change in orientation (dϕ) of the Ego vehicle. The distance and the orientation is calculated using the yaw rate (ϕ˙), the velocity measurement(v) and the step length(dt):
路径上的点代表了前车在自车坐标系统中的轨迹。由于坐标系随自车移动，因此这些点需要根据自车行驶的距离 $(dx)$和方向变化 $(d\varphi )$在坐标系内移动。距离和方向的变化是利用偏航率$(\dot{\varphi })$ 、速度测量 $(v)$和步长 $(dt)$来计算的：

Update points of the path

When the distance between the point in path which is closest to the preceding vehicle and the current filtered point on the preceding vehicle is equal to deltaPrec, oldest point (furthest away from the preceding vehicle) is discarded and current filtered point added to the path. The update is illustrated in figure 4.7.
当路径上离前车最近的点与前车上当前经过滤波的点之间的距离等于 $deltaPrec$ 时，最旧的点（离前车最远的点）会被丢弃，并将当前经过滤波的点添加到路径中。更新过程如图4.7 所示。

图4.7：更新前车路径。每当路径上最新点与前车之间的距离大于deltaPrec时，就移除离前车最远的点，并将前车的位置作为新点添加进来。

deltaPrec的意思应该就是不要把所有的点都记录下来，按照一定最小间隔来采样路径点

Uncertainties

The movement of the points can be considered as pure prediction since there are no measurements of the points in the path to update with. The prediction step is the same as for the lane marking sampled points and therefore the same prediction model can be used. The covariance is based on the noise of the yaw rate and the velocity. Since there is no update step, the uncertainties increase over time, which results in very uncertain points on the path especially if the preceding vehicle is far ahead. By filtering the states of the vehicle such as vehicle and heading a more accurate movement of the vehicle between time instances can be achieved. This was not in the scope of this thesis but discussed in future work chapter 8.
路径上点的移动可以看作是纯粹的预测，因为路径上的点没有测量数据可供更新。预测步骤与车道标记采样点的步骤相同，因此可以使用相同的预测模型。协方差基于偏航率和速度的噪声。由于没有更新步骤，不确定性会随着时间的推移而增加，这会导致路径上的点特别在前车距离较远时变得非常不确定。通过对车辆状态，如车辆位置和航向进行滤波，可以在时间间隔之间实现更准确的车辆运动。这并不在本论文的研究范围内，但在第8章的未来工作讨论中有所提及。

4.5 Preceding Vehicle Filter

The position of the preceding vehicle is measured with the camera and radar. The camera measures longitudinal and lateral distances to the preceding vehicle relative to the Ego vehicle. The radar has the same measurements as the camera but also outputs the relative velocity in longitudinal and lateral direction. These sensor information can be fused together to obtain an estimate of the position of the preceding vehicle relative to Ego vehicle. Based on the measurements of the preceding vehicle the most straight forward choice of a state vector would be [x, y, vx, vy] where x and y is the longitudinal and lateral distances respectively, vx and vy are the corresponding velocities in longitudinal and lateral direction. All of the states are relative to the Ego vehicle.
前车的位置是通过摄像头和雷达进行测量的。摄像头测量与自车相对的前车的纵向和横向距离。雷达具有与摄像头相同的测量功能，并且还输出纵向和横向的相对速度。这些传感器信息可以结合在一起，以获得相对于自车的前车位置的估计。基于对前车的测量，状态向量最直接的选择是包含 $x、y、v_x和v_y$，其中 x 和 y 分别是相对于自车的纵向和横向距离，$v_x和v_y$ 是相应的纵向和横向速度。所有这些状态都是相对于自车来定义的。

4.5.1 Prediction step

When choosing a process model the changes between the measurements samples need to be considered. The measurements are relative to the Ego vehicle and the assumption can be made that the two vehicles travel on the same lane with similar velocity which makes the changes in velocity slow. Therefore a constant velocity (CV) model was chosen. Adding acceleration to the model would not be bring much improvements since the change in acceleration are very small. Since the CV process model is linear the optimal prediction method would be using the Kalman prediction. Since the measurements are not synchronous the step length dt might need to be adaptive, analysis on different step lengths can be seen in section 6.4. The Kalman prediction equations and a discretized CV model can be seen in equation4.11.
在选择过程模型时，需要考虑测量样本之间的变化。由于测量是相对于自车进行的，可以假设两辆车在同一车道上以相似的速度行驶，这使得速度变化较为缓慢。因此，选择了一个恒定速度（CV）模型。在模型中加入加速度不会带来太大的改进，因为加速度的变化非常小。由于CV过程模型是线性的，最优的预测方法就是使用卡尔曼预测。由于测量不是同步进行的，步长 dt 可能需要是自适应的，不同步长的分析可以在第6.4节中找到。卡尔曼预测方程和离散化的CV模型可以在公式4.11中查看。

4.5.2 Update step

In the update step both the radar and the camera measure the x and y states. Different methods can be used to derive the estimate. The optimal method would be to update first using one sensor and then update with the second one. In equation 4.12 the radar and the camera update steps can be seen.
在更新步骤中，雷达和摄像头都测量 x 和 y 状态。可以使用不同的方法来推导出估计值。最优的方法是首先使用一个传感器进行更新，然后使用第二个传感器进行更新。在公式4.12中，可以看到雷达和摄像头的更新步骤。

Since the radar and camera measurements are not synchronous there are few cases to consider dependent on which measurements are available when the filter is executed:
Radar and camera measurements available The update step includes both updates according to equation 4.12.
Only radar measurement available The update step runs update 1 from equation 4.12.
Only camera measurement available The update step runs update 2 from equation 4.12.
No measurements available Update step is skipped.
由于雷达和摄像头的测量不是同步的，在执行滤波器时需要考虑几种情况，这取决于滤波器执行时可用的测量数据：

1. 雷达和摄像头测量数据都可用：更新步骤包括根据公式4.12进行的两次更新。
2. 只有雷达测量数据可用：更新步骤执行公式4.12中的更新1。
3. 只有摄像头测量数据可用：更新步骤执行公式4.12中的更新2。
4. 没有测量数据可用：跳过更新步骤。

4.6 Control Signals

The reference paths from the lane marking and preceding vehicle are represented in same way. The control signals can therefore be calculated in same way for both paths. The control signals are lateral error and heading error of the truck relative to the lane at a look ahead distance (see figure 4.8). The look ahead distance is dependent on the trucks velocity and a look ahead time (lookAheadDistance = velocity · lookAheadTime), the look ahead time is set to 1 second. A decision has to be made which path to follow and that is done by using the uncertainty of the paths at the look ahead point.
车道标记和前车的参考路径以相同的方式表示。因此，对于两条路径都可以以相同的方式计算控制信号。控制信号是卡车相对于车道的横向误差和航向误差，在预瞄距离上（见图4.8）。预瞄距离取决于卡车的速度和一个预瞄时间（预瞄距离 = 速度 × 预瞄时间），预瞄时间设定为1秒。需要做出决定，选择跟随哪条路径，这是通过使用路径在预瞄点的不确定性来完成的。
Lane Marking Path Calculate the control signal from the lane marking path. If the uncertainty of the lane marking gets larger than uncertainty of preceding vehicle path change to the state “Preceding Path”.
车道标记路径：基于车道标记路径计算控制信号。如果车道标记的不确定性超过了前车路径的不确定性，就改变到“前车路径”状态。
Preceding Path Calculate the control signal from the preceding vehicle path. If the uncertainty of the preceding vehicle path grows larger than uncertainty of lane marking path, change to the state “Lane Marking Path”.
前车路径：基于前车路径计算控制信号。如果前车路径的不确定性超过了车道标记路径的不确定性，就改变到“车道标记路径”状态。
Another approach could be to fuse the two paths together obtaining a single path that can be used as reference. This approach was not chosen since the lane marking path is in the middle of the lane but the preceding path is varied from the middle. This is because it is difficult to keep a constant lateral position in a lane while driving. This can be seen in figure 6.14. Also the uncertainty of the lane marking is much smaller than the uncertainty of the preceding vehicle path when lane marking measurements are available.
另一种方法是将两条路径合并，形成一个单一的参考路径。由于车道标记路径位于车道的中间，而前车路径则与车道中心有所偏差，因此没有选择这种方法。这是因为在驾驶过程中，很难保持在车道中的恒定横向位置。这一点可以在图6.14中观察到。同时，当车道标记测量可用时，车道标记的不确定性远小于前车路径的不确定性。
In the case of coefficient filter the lateral error is calculated using the approximation in equation 4.2 and the heading error is then its derivative. The equations can be seen in equation 4.13.
在系数滤波器的情况下，横向误差是使用公式4.2中的近似值来计算的，然后航向误差是它的导数。这些公式可以在公式4.13中看到。

6 Results

6.5 Preceding vehicle path

A reference path was constructed from the filtered preceding vehicle position according to section 4.4.1. The coordinate transformation of the points that represent the path in Ego’s coordinate system is based on dead reckoning of the Ego vehicle. This results in increasing uncertainty as the points move away from the preceding vehicle. The path from the preceding vehicle can be seen in figure 6.14. The path from the preceding vehicle is not as smooth as the lane path since the preceding vehicle does not always drive in the middle of the road. It might be of interest to smoothen the path before calculating the control signals but that was not investigated further in this thesis.
根据4.4.1节的描述，从前车滤波位置构建了一条参考路径。代表路径的点在自车坐标系中的坐标转换基于自车的航位推算。这导致随着这些点远离前车，不确定性逐渐增加。前车路径可以在图6.14中看到。由于前车并不总是在道路中间行驶，前车路径没有车道路径那样平滑。在计算控制信号之前，可能有兴趣先平滑路径，但这一点在本论文中没有进一步研究。

图6.14：为横向控制生成的路径。绿色路径是车道标记参考路径，蓝色线是前车路径，蓝色圆圈是前车当前估计位置，红色十字是预瞄点，灰色线条是右侧和左侧的车道标记。

8 Future Work

Initial preceding vehicle path estimation The estimation of the initial preceding vehicle path when no lane marking path is available could be improved. In the proposed solution the path is set by linear interpolation between the Ego vehicle and the preceding vehicle. Since the heading of the Ego vehicle is known a cubic interpolation could be used to get the estimated path to follow the curve of the road.
初始前车路径估计：在没有车道标记路径可用时，对初始前车路径的估计可以进行改进。在提出的解决方案中，路径是通过在自车和前车之间进行线性插值得到的。由于已知自车的航向，可以使用三次插值来使估计出的路径跟随道路的曲线。

Reference paths The reference paths are currently represented with 5 to 6 points which could cause problems for the controller. The paths could be smoothed by drawing a bezier curve through the points before calculating the control signal. This method was looked into but not investigated further because of time constraints on the project.
参考路径目前由5到6个点表示，这可能会给控制器带来问题。在计算控制信号之前，可以通过在点之间绘制贝塞尔曲线来平滑这些路径。这种方法被考虑过，但由于项目时间限制，没有进一步研究。