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求解线性方程组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$）运动，避免直接处理复杂笛卡尔坐标：

步骤

1. 路径离散化：将参考线（如车道中心线）离散为点集
2. 生成候选轨迹：在目标车道横向偏移量$d$处生成多项式轨迹
3. 碰撞检测：检查轨迹与障碍物的最小距离

代码示例

vector generate_candidates(const Road& road, double target_d) { vector 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& 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 #include 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 smoothParkingPath(vector waypoints) { vector 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 double pathCost(const vector& 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 optimizePath(vector init_path) { double learning_rate = 0.01; for (int iter = 0; iter < 1000; ++iter) { vector 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 generateDubinsPath(DubinsPath params) { vector 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(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 polynomialSmoothing(vector key_points) { vector 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(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 #include #include using namespace std;struct RoadSegment { int id; double length; int capacity; int currentFlow;};void optimizeDiversion(vector& segments) { priority_queue<pair> 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 flowRates; const int CRITICAL_FLOW = 1500; // 车辆/小时 public: void updateFlow(int sensorId, int flow) { if (flowRates.size()  CRITICAL_FLOW; }); }};

多目标优化算法

vector findOptimalPath(const vector<vector<pair>>& graph, int start, int end, const vector& roadWorks) { vector dist(graph.size(), INT_MAX); priority_queue<pair> 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 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& signs, const vector& 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& 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& 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 allocateTeams( const vector& zones, const vector& teams) { vector workloads(zones.size()); transform(zones.begin(), zones.end(), workloads.begin(),  [](auto& z){ return z.estimatedWorkload; }); vector 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  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& options) { vector 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> predictHourlyFlows( const HistoricalData& data, int dayOfWeek) { map<int, vector> predictions; for (int segId : data.segmentIds) { auto hist = data.getHistoricalFlows(segId, dayOfWeek); predictions[segId] = movingAverage(hist, 4); } return predictions;}

协同信号控制

信号配时优化

vector coordinateSignals( const vector& intersections, const DiversionPlan& plan) { vector 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)));}

施工区安