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-km
double 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)));
}