C++高效实现轨迹规划、自动泊车、RTS游戏、战术迂回包抄、空中轨迹、手术机器人、KD树
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)));
}