C++ 算法匯總
基于C++的城市道路場景
以下是基于C++的城市道路場景中車輛緊急變道軌跡生成的實現方法和示例代碼。內容涵蓋軌跡規劃算法、數學建模及代碼實現，適用于自動駕駛或駕駛輔助系統開發。
基于多項式曲線的軌跡生成
采用五次多項式（Quintic Polynomial）生成平滑軌跡，滿足起點和終點的位置、速度、加速度約束：
數學模型
橫向位移（$y$）與縱向位移（$x$）的關系：
邊界條件（$t=0$為起點，$t=T$為終點）：
$$ \begin{cases} y(0)=y_0, & y(T)=y_T \ y'(0)=v_{y0}, & y'(T)=v_{yT} \ y''(0)=a_{y0}, & y''(T)=a_{yT} \end{cases} $$
C++代碼片段
#include <Eigen/Dense> // 使用Eigen求解線性方程組struct State {double pos, vel, acc;
};QuinticPolynomial solve_quintic(State start, State end, double T) {Eigen::MatrixXd A(6, 6);Eigen::VectorXd b(6);A << 0, 0, 0, 0, 0, 1,0, 0, 0, 0, 1, 0,0, 0, 0, 2, 0, 0,T*T*T*T*T, T*T*T*T, T*T*T, T*T, T, 1,5*T*T*T*T, 4*T*T*T, 3*T*T, 2*T, 1, 0,20*T*T*T, 12*T*T, 6*T, 2, 0, 0;b << start.pos, start.vel, start.acc, end.pos, end.vel, end.acc;Eigen::VectorXd coeffs = A.colPivHouseholderQr().solve(b);return {coeffs[5], coeffs[4], coeffs[3], coeffs[2], coeffs[1], coeffs[0]};
}
基于Frenet坐標系的軌跡規劃
在Frenet坐標系下分解縱向（$s$）和橫向（$d$）運動，避免直接處理復雜笛卡爾坐標：
步驟
- 路徑離散化：將參考線（如車道中心線）離散為點集
- 生成候選軌跡：在目標車道橫向偏移量$d$處生成多項式軌跡
- 碰撞檢測：檢查軌跡與障礙物的最小距離
代碼示例
vector<Trajectory> generate_candidates(const Road& road, double target_d) {vector<Trajectory> candidates;for (double t = 1.0; t <= 3.0; t += 0.5) { // 不同時間周期QuinticPolynomial lat = solve_quintic({current_d, lateral_vel, lateral_acc},{target_d, 0.0, 0.0}, t);Trajectory traj;for (double s = current_s; s <= current_s + 50; s += 0.5) {double d = lat.eval(s - current_s);auto xy = road.to_cartesian(s, d);traj.points.push_back(xy);}candidates.push_back(traj);}return candidates;
}
動態障礙物避讓邏輯
引入動態障礙物的預測位置進行軌跡評分：
評分函數
$$ \text{score} = w_1 \cdot \text{jerk} + w_2 \cdot \text{time} + w_3 \cdot \text{distance_to_obstacles} $$
實現示例
Trajectory select_best(vector<Trajectory>& candidates, const Obstacles& obs) {Trajectory best;double min_cost = INFINITY;for (const auto& traj : candidates) {double cost = 0.0;cost += 0.1 * calculate_jerk(traj); // 舒適性cost += 0.3 * traj.duration(); // 效率cost += 0.6 * min_distance_to_obs(traj, obs); // 安全性if (cost < min_cost) {min_cost = cost;best = traj;}}return best;
}
完整實現需結合具體場景的HD地圖和感知模塊輸出。以上代碼需配合ROS或Autoware等框架實現完整功能。
停車場自動泊車的多段路徑平滑規劃
以下是基于C++的停車場自動泊車多段路徑平滑規劃實例，結合B樣條曲線、貝塞爾曲線和優化算法實現路徑平滑：
例1：基于三次B樣條的直角轉彎平滑
#include <vector>
#include <Eigen/Dense>
using namespace Eigen;// 三次B樣條基函數計算
double BsplineBasis(int i, int k, double t, const VectorXd& knots) {if (k == 1) return (t >= knots[i] && t < knots[i+1]) ? 1.0 : 0.0;double denom1 = knots[i+k-1] - knots[i];double denom2 = knots[i+k] - knots[i+1];double term1 = (denom1 != 0) ? (t - knots[i])/denom1 * BsplineBasis(i, k-1, t, knots) : 0;double term2 = (denom2 != 0) ? (knots[i+k] - t)/denom2 * BsplineBasis(i+1, k-1, t, knots) : 0;return term1 + term2;
}// 生成平滑路徑
VectorXd generateSmoothPath(const VectorXd& control_points) {VectorXd knots = VectorXd::LinSpaced(control_points.size()+4, 0, 1);VectorXd path(100);for (int i = 0; i < 100; ++i) {double t = i / 99.0;double point = 0;for (int j = 0; j < control_points.size(); ++j) {point += control_points[j] * BsplineBasis(j, 4, t, knots);}path[i] = point;}return path;
}
例2：貝塞爾曲線連接多段直線
struct Point { double x, y; };Point quadraticBezier(Point p0, Point p1, Point p2, double t) {double mt = 1 - t;return {mt*mt*p0.x + 2*mt*t*p1.x + t*t*p2.x,mt*mt*p0.y + 2*mt*t*p1.y + t*t*p2.y};
}vector<Point> smoothParkingPath(vector<Point> waypoints) {vector<Point> path;for (size_t i = 0; i < waypoints.size() - 2; i += 2) {for (int j = 0; j <= 20; ++j) {double t = j / 20.0;path.push_back(quadraticBezier(waypoints[i], waypoints[i+1], waypoints[i+2], t));}}return path;
}
例3：基于梯度下降的路徑優化
#include <cmath>double pathCost(const vector<double>& path) {double cost = 0;for (size_t i = 1; i < path.size(); ++i) {cost += pow(path[i] - path[i-1], 2); // 平滑項cost += pow(path[i] - 0.5, 2); // 偏離懲罰}return cost;
}vector<double> optimizePath(vector<double> init_path) {double learning_rate = 0.01;for (int iter = 0; iter < 1000; ++iter) {vector<double> gradient(init_path.size(), 0);for (size_t i = 1; i < init_path.size()-1; ++i) {gradient[i] = 2*(init_path[i] - init_path[i-1]) - 2*(init_path[i+1] - init_path[i])+ 2*(init_path[i] - 0.5);}for (size_t i = 0; i < init_path.size(); ++i) {init_path[i] -= learning_rate * gradient[i];}}return init_path;
}
例4：基于Dubins路徑的平行泊車
struct DubinsPath {double start_x, start_y, start_theta;double end_x, end_y, end_theta;double curvature;
};vector<Point> generateDubinsPath(DubinsPath params) {vector<Point> path;double step = 0.1;double L = sqrt(pow(params.end_x - params.start_x, 2) + pow(params.end_y - params.start_y, 2));int steps = static_cast<int>(L / step);for (int i = 0; i <= steps; ++i) {double t = i * step;double x = params.start_x + t * cos(params.start_theta);double y = params.start_y + t * sin(params.start_theta);path.push_back({x, y});}return path;
}
例5：基于多項式插值的垂直泊車
vector<Point> polynomialSmoothing(vector<Point> key_points) {vector<Point> smoothed_path;MatrixXd A(key_points.size(), 4);VectorXd bx(key_points.size()), by(key_points.size());for (size_t i = 0; i < key_points.size(); ++i) {double t = static_cast<double>(i) / (key_points.size()-1);A(i, 0) = 1.0; A(i, 1) = t; A(i, 2) = t*t; A(i, 3) = t*t*t;bx[i] = key_points[i].x;by[i] = key_points[i].y;}VectorXd cx = A.colPivHouseholderQr().solve(bx);VectorXd cy = A.colPivHouseholderQr().solve(by);for (int i = 0; i <= 100; ++i) {double t = i / 100.0;double x = cx[0] + cx[1]*t + cx[2]*t*t + cx[3]*t*t*t;double y = cy[0] + cy[1]*t + cy[2]*t*t + cy[3]*t*t*t;smoothed_path.push_back({x, y});}return smoothed_path;
}
高速公路施工區域的動態改道規劃
動態改道規劃模型（C++示例）
#include <iostream>
#include <vector>
#include <queue>
using namespace std;struct RoadSegment {int id;double length;int capacity;int currentFlow;
};void optimizeDiversion(vector<RoadSegment>& segments) {priority_queue<pair<double, int>> pq; // 擁堵系數優先隊列for (auto& seg : segments) {double congestion = (double)seg.currentFlow / seg.capacity;pq.push({congestion, seg.id});}while (!pq.empty()) {auto [congestion, id] = pq.top();pq.pop();if (congestion > 0.7) { // 觸發改道閾值cout << "重定向路段 " << id << " 的車流" << endl;}}
}
實時交通流監控系統
class TrafficMonitor {
private:vector<int> flowRates;const int CRITICAL_FLOW = 1500; // 車輛/小時public:void updateFlow(int sensorId, int flow) {if (flowRates.size() <= sensorId) {flowRates.resize(sensorId+1);}flowRates[sensorId] = flow;}bool checkCongestion() {return any_of(flowRates.begin(), flowRates.end(), [this](int f){ return f > CRITICAL_FLOW; });}
};
多目標優化算法
vector<int> findOptimalPath(const vector<vector<pair<int,int>>>& graph, int start, int end, const vector<int>& roadWorks) {vector<int> dist(graph.size(), INT_MAX);priority_queue<pair<int,int>> pq;dist[start] = 0;pq.push({0, start});while (!pq.empty()) {auto [d, u] = pq.top();pq.pop();if (u == end) break;for (auto [v, w] : graph[u]) {if (find(roadWorks.begin(), roadWorks.end(), v) != roadWorks.end()) {w *= 2; // 施工路段懲罰權重}if (dist[v] > dist[u] + w) {dist[v] = dist[u] + w;pq.push({-dist[v], v});}}}return reconstructPath(start, end, dist);
}
動態路徑規劃技術
基于強化學習的改道策略
class QLearningModel {unordered_map<string, double> qTable;double alpha = 0.1, gamma = 0.6;public:string getState(const TrafficSnapshot& snapshot);void updateQValue(string state, string action, double reward, string nextState) {double oldValue = qTable[state+"_"+action];double maxNext = /* 計算下一狀態最大值 */;qTable[state+"_"+action] = oldValue + alpha*(reward + gamma*maxNext - oldValue);}
};
可變信息標志系統
void updateVMS(vector<VMS>& signs, const vector<Diversion>& routes) {for (auto& sign : signs) {auto nearest = findNearestDiversion(sign.position, routes);sign.displayMessage(nearest.alternativeRoute, nearest.estimatedDelay);}
}
施工區域管理方案
車道關閉協調系統
struct LaneClosure {int segmentId;time_t startTime;time_t endTime;int closedLanes;
};void synchronizeClosures(vector<LaneClosure>& closures) {sort(closures.begin(), closures.end(), [](auto& a, auto& b){ return a.startTime < b.startTime; });for (int i = 1; i < closures.size(); ++i) {if (closures[i].startTime < closures[i-1].endTime && abs(closures[i].segmentId - closures[i-1].segmentId) < 5000) {closures[i].startTime = closures[i-1].endTime + 3600; // 延遲1小時}}
}
應急車輛優先通行
void handleEmergencyVehicle(int segmentId, vector<TrafficLight>& lights) {auto& tl = lights[segmentId];tl.setPriorityPhase();broadcastDiversion(segmentId, EMERGENCY_DETOUR);
}
交通影響評估模型
延誤計算算法
double calculateDelay(const TrafficData& before, const TrafficData& during) {double totalDelay = 0;for (int i = 0; i < before.flow.size(); ++i) {double speedReduction = before.speed[i] - during.speed[i];totalDelay += during.flow[i] * speedReduction / 3600;}return totalDelay;
}
排放量估算
const double EMISSION_FACTOR = 2.3; // kg/veh-kmdouble estimateEmissions(double divertedDistance, int vehicleCount) {return divertedDistance * vehicleCount * EMISSION_FACTOR;
}
協同施工規劃系統
資源分配優化
vector<ConstructionTeam> allocateTeams(const vector<WorkZone>& zones,const vector<ConstructionTeam>& teams) {vector<double> workloads(zones.size());transform(zones.begin(), zones.end(), workloads.begin(), [](auto& z){ return z.estimatedWorkload; });vector<ConstructionTeam> allocation;for (int i = 0; i < teams.size(); ++i) {int zoneIdx = min_element(workloads.begin(), workloads.end()) - workloads.begin();allocation.push_back({teams[i].id, zones[zoneIdx].id});workloads[zoneIdx] += teams[i].productivity;}return allocation;
}
動態限速控制
自適應限速算法
int computeSpeedLimit(int segmentId, int baseSpeed, int visibility, int accidentRisk) {int reduction = 0;if (visibility < 100) reduction += 20;if (accidentRisk > 0.7) reduction += 30;return max(baseSpeed - reduction, 40);
}
改道效益評估
成本-效益分析模型
struct CostBenefit {double travelTimeSaved;double accidentReduction;double implementationCost;double score() const {return (travelTimeSaved * 25 + accidentReduction * 10000) / implementationCost;}
};void evaluateDiversion(const vector<DiversionOption>& options) {vector<CostBenefit> assessments;for (auto& opt : options) {CostBenefit cb;cb.travelTimeSaved = opt.baseTime - opt.diversionTime;assessments.push_back(cb);}sort(assessments.begin(), assessments.end(), [](auto& a, auto& b){ return a.score() > b.score(); });
}
多時段改道策略
分時段流量預測
map<int, vector<double>> predictHourlyFlows(const HistoricalData& data, int dayOfWeek) {map<int, vector<double>> predictions;for (int segId : data.segmentIds) {auto hist = data.getHistoricalFlows(segId, dayOfWeek);predictions[segId] = movingAverage(hist, 4);}return predictions;
}
協同信號控制
信號配時優化
vector<TrafficLight> coordinateSignals(const vector<Intersection>& intersections, const DiversionPlan& plan) {vector<TrafficLight> adjusted;for (auto& inter : intersections) {TrafficLight tl = inter.trafficLight;if (plan.affectedIntersections.count(inter.id)) {tl.increaseGreenTime(plan.mainRoute);tl.decreaseGreenTime(plan.closedRoute);}adjusted.push_back(tl);}return adjusted;
}
駕駛員行為建模
路徑選擇概率
double routeChoiceProbability(double t1, double t2, double beta = 0.5) {return 1 / (1 + exp(beta * (t1 - t2)));
}
