-
Notifications
You must be signed in to change notification settings - Fork 3
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
e5c3cd6
commit ac0d46e
Showing
82 changed files
with
1,254 additions
and
728 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,28 @@ | ||
| Model Predictive Controller Project | ||
|
|
||
| Introduction | ||
| The goal of this project is to navigate a track in a Udacity-provided simulator, which communicates telemetry and track waypoint data via websocket, by sending steering and acceleration commands back to the simulator. The solution must be robust to 100ms latency, as one may encounter in real-world application. | ||
|
|
||
| This solution, as the Nanodegree lessons suggest, makes use of the IPOPT and CPPAD libraries to calculate an optimal trajectory and its associated actuation commands in order to minimize error with a third-degree polynomial fit to the given waypoints. The optimization considers only a short duration's worth of waypoints, and produces a trajectory for that duration based upon a model of the vehicle's kinematics and a cost function based mostly on the vehicle's cross-track error (roughly the distance from the track waypoints) and orientation angle error, with other cost factors included to improve performance. | ||
|
|
||
| 模型预测控制（MPC）是一种致力于将更长时间跨度、甚至于无穷时间的最优化控制问题，分解为若干个更短时间跨度，或者有限时间跨度的最优化控制问题，并且在一定程度上仍然追求最优解。模型预测控制由如下三个要素组成： | ||
| 预测模型：预测模型能够在短时间内很好地预测系统状态的变化 | ||
| 在线滚动优化：通过某种最优化算法来优化未来一段短时间的控制输入，使得在这种控制输入下预测模型的输出与参考值的差距最小 | ||
| 反馈校正：到下一个时间点根据新的状态重新进行预测和优化 | ||
|
|
||
| 透过一些Sensor fusion我们可以把Camera/Lidar/Radar的资料转换成道路和物体的座标，配合汽车的物理模型(汽车加速度，加速度，角速度)我们就可以来做MPC了。在每一个时间点t，我们可以用汽车现在的状态来估算下一个时间t1点的状态x, y, psi, v, delta, a | ||
| x, y 是汽车的座标，psi 是汽车面向的方向，v是汽车的瞬时速度而delta是汽车轮胎的角度，a是则是汽车的加速率（油门/煞车)。因次我们可以用以下的公式来推测下一个时间点汽车的状态（公式如下）。其中最特别的就是第三行，Lf 是一个常数，代表汽车前端到汽车重心的长度。意思就是车子不会只依照着方向盘的角度转向，车子的长度和车子移动的速度也会影响单位时间车子改变方向的量 | ||
|
|
||
| 通过使用不同的预测模型和损失函数，可以构造出各种模型预测控制器，但是，总的来说，模型预测控制往往可以分解成如下几步： | ||
| 1. 从 tt 时刻开始，预测未来 aa 步系统的输出信号 | ||
| 2. 基于模型的控制信号以及相应的输出信号，构造损失函数，并且通过调整控制信号最优化损失函数 | ||
| 3. 将控制信号输入到系统 | ||
| 4. 等到下一个时间点，在新的状态重复步骤1 | ||
|
|
||
| x1 = x0 + v0 * cos(psi) * dt; | ||
| y1 = y0 + v0 * sin(psi) * dt; | ||
| psi1 = psi0 + v0 / Lf * delta * dt; // Lf = 2.67 in simulator | ||
| v1 = v0 + a0 * dt; | ||
| 有了这些公式我们就可以预测出未来T (prediction horizon) 的时间内汽车可能的路径。因此把一个follow的问题转化成一个控制最佳化的问题。尝试各种控制的排列组合[delta0, a0, delta1, a1, delta2, a2 …]找出最合适的路径 | ||
|
|
||
| ipopt的C++ library来计算来解最佳路径，因此我们只要定义cost function 然后把限制丢进去ipopt就可以了，超方便！首先cost想要minimize的就是error，error包括偏移路径的量(cte)和汽车的方向的偏移(epsi)。另外为了让汽车稳定要尽量减少使用方向盘跟油门的(actuator)，同时也要避免突然踩油门煞车或是突然转方向盘(actuator change)，最后还要避免电脑出奥步(直接把车开超慢甚至停在路中间就没有error了)所以再加上速度太慢的cost就完成 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,88 @@ | ||
| Path Planning Project | ||
|
|
||
| Introduction | ||
| The goal of this project is to navigate a car around a simulated highway scenario, including traffic and given waypoint, telemetry, and sensor fusion data. The car must not violate a set of motion constraints, namely maximum velocity, maximum acceleration, and maximum jerk, while also avoiding collisions with other vehicles, keeping to within a highway lane (aside from short periods of time while changing lanes), and changing lanes when doing so is necessary to maintain a speed near the posted speed limit. | ||
|
|
||
| This implementation is summarized in the following five steps: | ||
| 1.Construct interpolated waypoints of nearby area | ||
| 2.Determine ego car parameters and construct vehicle object | ||
| 3.Generate predictions from sensor fusion data | ||
| 4.Determine best trajectory | ||
| 5.Produce new path | ||
|
|
||
|
|
||
|
|
||
| Implementation: | ||
|
|
||
| 1. Construct Interpolated Waypoints of Nearby Area | ||
| The track waypoints given in the highway_map.csv file are spaced roughly 30 meters apart, so the first step in the process is to interpolate a set of nearby map waypoints (in the current implementation, five waypoints ahead of and five waypoints behind the ego vehicle are used) and produce a set of much more tightly spaced (0.5 meters apart) waypoints which help to produce more accurate results from the getXY and getFrenet methods and also account for the discontinuity in s values at the end/beginning of the track. | ||
|
|
||
| 2. Determine Ego Car Parameters and Construct Vehicle Object | ||
| The simulator returns instantaneous telemetry data for the ego vehicle, but it also returns the list of points from previously generated path. This is used to project the car's state into the future and a "planning state" is determined based on the difference between points at some prescribed number of points along the previous path. In effect, this can help to generate smoother transitions, handle latency from transmission between the controller and the simulator, and alleviate the trajectory generator of some computation overhead. | ||
|
|
||
| The vehicle state and its associated (self-explanatory) methods are contained in the Vehicle class. These methods include update_available_states (i.e. "keep lane", "lane change left", "lane change right"), get_target_for_state, generate_trajectory_for_target, get_leading_vehicle_data_for_lane, and generate_predictions (for sensor fusion data). | ||
|
|
||
| 3. Generate Predictions from Sensor Fusion Data | ||
| The sensor fusion data received from the simulator in each iteration is parsed and trajectories for each of the other cars on the road are generated. These trajectories match the duration and interval of the ego car's trajectories generated for each available state and are used in conjunction with a set of cost functions to determine a best trajectory for the ego car. A sample of these predicted trajectories (along with the ego car's predicted trajectory) is shown below. | ||
|
|
||
|
|
||
| 4. Determine Best Trajectory | ||
| Using the ego car "planning state", sensor fusion predictions, and Vehicle class methods mentioned above, an optimal trajectory is produced. | ||
|
|
||
| Available states are updated based on the ego car's current position, with some extra assistance from immediate sensor fusion data (I think of this similar to ADAS, helping to, for example, prevent "lane change left" as an available state if there is a car immediately to the left). | ||
| Each available state is given a target Frenet state (position, velocity, and acceleration in both s and d dimensions) based on the current state and the traffic predictions. | ||
| A quintic polynomial, jerk-minimizing (JMT) trajectory is produced for each available state and target (*note: although this trajectory was used for the final path plan in a previous approach, in the current implementation the JMT trajectory is only a rough estimate of the final trajectory based on the target state and using the spline.h library). | ||
| Each trajectory is evaluated according to a set of cost functions, and the trajectory with the lowest cost is selected. In the current implementation, these cost functions include: | ||
| Collision cost: penalizes a trajectory that collides with any predicted traffic trajectories. | ||
| Buffer cost: penalizes a trajectory that comes within a certain distance of another traffic vehicle trajectory. | ||
| In-lane buffer cost: penalizes driving in lanes with relatively nearby traffic. | ||
| Efficiency cost: penalizes trajectories with lower target velocity. | ||
| Not-middle-lane cost: penalizes driving in any lane other than the center in an effort to maximize available state options. | ||
|
|
||
| 5. Produce New Path | ||
| The new path starts with a certain number of points from the previous path, which is received from the simulator at each iteration. From there a spline is generated beginning with the last two points of the previous path that have been kept (or the current position, heading, and velocity if no current path exists), and ending with two points 30 and 60 meters ahead and in the target lane. This produces a smooth x and y trajectory. To prevent excessive acceleration and jerk, the velocity is only allowed increment or decrement by a small amount, and the corresponding next x and y points are calculated along the x and y splines created earlier. | ||
|
|
||
|
|
||
|
|
||
|
|
||
| In this project your goal is to safely navigate around a virtual highway with other traffic that is driving +-10 MPH of the 50 MPH speed limit. You will be provided the car's localization and sensor fusion data, there is also a sparse map list of waypoints around the highway. The car should try to go as close as possible to the 50 MPH speed limit, which means passing slower traffic when possible, note that other cars will try to change lanes too. The car should avoid hitting other cars at all cost as well as driving inside of the marked road lanes at all times, unless going from one lane to another. The car should be able to make one complete loop around the 6946m highway. Since the car is trying to go 50 MPH, it should take a little over 5 minutes to complete 1 loop. Also the car should not experience total acceleration over 10 m/s^2 and jerk that is greater than 50 m/s^3. | ||
|
|
||
| The map of the highway is in data/highway_map.txt | ||
| Each waypoint in the list contains [x,y,s,dx,dy] values. x and y are the waypoint's map coordinate position, the s value is the distance along the road to get to that waypoint in meters, the dx and dy values define the unit normal vector pointing outward of the highway loop. | ||
|
|
||
| The highway's waypoints loop around so the frenet s value, distance along the road, goes from 0 to 6945.554. | ||
|
|
||
| Basic Build Instructions | ||
| Clone this repo. | ||
| Make a build directory: mkdir build && cd build | ||
| Compile: cmake .. && make | ||
| Run it: ./path_planning. | ||
| Here is the data provided from the Simulator to the C++ Program | ||
|
|
||
| Main car's localization Data (No Noise) | ||
| ["x"] The car's x position in map coordinates | ||
|
|
||
| ["y"] The car's y position in map coordinates | ||
|
|
||
| ["s"] The car's s position in frenet coordinates | ||
|
|
||
| ["d"] The car's d position in frenet coordinates | ||
|
|
||
| ["yaw"] The car's yaw angle in the map | ||
|
|
||
| ["speed"] The car's speed in MPH | ||
|
|
||
| Previous path data given to the Planner | ||
| //Note: Return the previous list but with processed points removed, can be a nice tool to show how far along the path has processed since last time. | ||
|
|
||
| ["previous_path_x"] The previous list of x points previously given to the simulator | ||
|
|
||
| ["previous_path_y"] The previous list of y points previously given to the simulator | ||
|
|
||
| Previous path's end s and d values | ||
| ["end_path_s"] The previous list's last point's frenet s value | ||
|
|
||
| ["end_path_d"] The previous list's last point's frenet d value | ||
|
|
||
| Sensor Fusion Data, a list of all other car's attributes on the same side of the road. (No Noise) | ||
| ["sensor_fusion"] A 2d vector of cars and then that car's [car's unique ID, car's x position in map coordinates, car's y position in map coordinates, car's x velocity in m/s, car's y velocity in m/s, car's s position in frenet coordinates, car's d position in frenet coordinates. |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.