#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include <fstream>
#include <iomanip>
#include <limits>
using namespace std;
const double MIN_RADIUS = 10.0; // 最小转弯半径约束
// 二维点结构
struct Point {
double x, y;
Point(double x = 0, double y = 0) : x(x), y(y) {}
Point operator+(const Point& p) const { return Point(x + p.x, y + p.y); }
Point operator-(const Point& p) const { return Point(x - p.x, y - p.y); }
Point operator*(double s) const { return Point(x * s, y * s); }
Point operator/(double s) const { return Point(x / s, y / s); }
double dot(const Point& p) const { return x * p.x + y * p.y; }
double cross(const Point& p) const { return x * p.y - y * p.x; }
double norm() const { return sqrt(x*x + y*y); }
Point normalized() const { double n = norm(); return n > 0 ? *this / n : *this; }
Point perpendicular() const { return Point(-y, x); } // 垂直向量
friend ostream& operator<<(ostream& os, const Point& p) {
os << "(" << p.x << ", " << p.y << ")";
return os;
}
};
// 圆弧段
struct ArcSegment {
Point center;
double radius;
double start_angle; // 弧度
double end_angle; // 弧度
vector<Point> generatePoints(int num_points = 30) const {
vector<Point> points;
double angle_step = (end_angle - start_angle) / num_points;
for (int i = 0; i <= num_points; i++) {
double angle = start_angle + i * angle_step;
double x = center.x + radius * cos(angle);
double y = center.y + radius * sin(angle);
points.push_back(Point(x, y));
}
return points;
}
};
// 直线段
struct LineSegment {
Point start;
Point end;
vector<Point> generatePoints(int num_points = 30) const {
vector<Point> points;
Point dir = (end - start);
double length = dir.norm();
if (length < 1e-10) return points;
dir = dir / length;
for (int i = 0; i <= num_points; i++) {
double t = static_cast<double>(i) / num_points;
Point p = start + dir * (t * length);
points.push_back(p);
}
return points;
}
};
// 路径规划器
class PathPlanner {
private:
double min_radius;
public:
PathPlanner(double min_radius = MIN_RADIUS) : min_radius(min_radius) {}
// 生成满足转弯半径的几何路径
vector<Point> generateGeometricPath(const Point& start, const Point& end) {
// 设计关键点
Point A = start;
Point B(-2, 4); // 第一个转弯起点
Point C(4, 11); // 第二个转弯起点
Point D = end;
// 计算转弯参数 - 确保半径至少为min_radius
double R = min_radius * 1.5; // 使用1.5倍最小转弯半径
// 第一个转弯:从垂直转向水平
Point center1(B.x + R, B.y);
double angle1_start = M_PI; // 180度
double angle1_end = M_PI_2; // 90度
// 第二个转弯:从水平转向垂直
Point center2(C.x, C.y - R);
double angle2_start = -M_PI_2; // -90度
double angle2_end = 0; // 0度
// 生成路径点
vector<Point> path_points;
// 第一段直线: A to B
LineSegment line1 = {A, B};
vector<Point> line1_points = line1.generatePoints(40);
path_points.insert(path_points.end(), line1_points.begin(), line1_points.end());
// 第一段圆弧: B to arc_end1
ArcSegment arc1 = {center1, R, angle1_start, angle1_end};
Point arc_end1(center1.x, center1.y - R);
vector<Point> arc1_points = arc1.generatePoints(50);
path_points.insert(path_points.end(), arc1_points.begin(), arc1_points.end());
// 第二段直线: arc_end1 to arc_start2
Point arc_start2(center2.x - R, center2.y);
LineSegment line2 = {arc_end1, arc_start2};
vector<Point> line2_points = line2.generatePoints(40);
path_points.insert(path_points.end(), line2_points.begin(), line2_points.end());
// 第二段圆弧: arc_start2 to C
ArcSegment arc2 = {center2, R, angle2_start, angle2_end};
vector<Point> arc2_points = arc2.generatePoints(50);
path_points.insert(path_points.end(), arc2_points.begin(), arc2_points.end());
// 第三段直线: C to D
LineSegment line3 = {C, D};
vector<Point> line3_points = line3.generatePoints(40);
path_points.insert(path_points.end(), line3_points.begin(), line3_points.end());
return path_points;
}
// 从几何路径生成贝塞尔曲线控制点(增加控制点数量)
vector<Point> generateBezierControlPoints(const vector<Point>& path_points) {
vector<Point> control_points;
if (path_points.size() < 4) return control_points;
// 增加控制点数量 - 每10个路径点取一个控制点
int step = max(1, (int)path_points.size() / 20);
for (int i = 0; i < path_points.size(); i += step) {
control_points.push_back(path_points[i]);
}
// 确保包含起点和终点
if (control_points.back() != path_points.back()) {
control_points.push_back(path_points.back());
}
// 在转弯区域添加额外控制点
int turn1_start = path_points.size() * 40 / 100; // 第一个转弯开始位置
int turn1_end = path_points.size() * 50 / 100; // 第一个转弯结束位置
int turn2_start = path_points.size() * 70 / 100; // 第二个转弯开始位置
int turn2_end = path_points.size() * 80 / 100; // 第二个转弯结束位置
// 添加第一个转弯区域的额外控制点
for (int i = turn1_start; i <= turn1_end; i += step/2) {
if (i < path_points.size()) {
control_points.push_back(path_points[i]);
}
}
// 添加第二个转弯区域的额外控制点
for (int i = turn2_start; i <= turn2_end; i += step/2) {
if (i < path_points.size()) {
control_points.push_back(path_points[i]);
}
}
// 排序并去重
sort(control_points.begin(), control_points.end(), [](const Point& a, const Point& b) {
if (a.x != b.x) return a.x < b.x;
return a.y < b.y;
});
auto last = unique(control_points.begin(), control_points.end(), [](const Point& a, const Point& b) {
return abs(a.x - b.x) < 0.1 && abs(a.y - b.y) < 0.1;
});
control_points.erase(last, control_points.end());
// 确保控制点间距足够
for (int i = 1; i < control_points.size() - 1; i++) {
double dist1 = (control_points[i] - control_points[i-1]).norm();
double dist2 = (control_points[i+1] - control_points[i]).norm();
// 如果间距小于转弯半径,调整位置
if (dist1 < min_radius || dist2 < min_radius) {
Point dir = (control_points[i+1] - control_points[i-1]).normalized();
control_points[i] = control_points[i-1] + dir * min_radius * 1.5;
}
}
return control_points;
}
};
// 贝塞尔曲线类
class BezierCurve {
private:
vector<Point> controlPoints;
// 计算二项式系数
double binomialCoefficient(int n, int k) const {
if (k < 0 || k > n) return 0;
if (k == 0 || k == n) return 1;
double result = 1;
for (int i = 1; i <= k; i++) {
result = result * (n - i + 1) / i;
}
return result;
}
// 计算伯恩斯坦基函数
double bernstein(int n, int i, double t) const {
return binomialCoefficient(n, i) * pow(t, i) * pow(1 - t, n - i);
}
public:
BezierCurve(const vector<Point>& points) : controlPoints(points) {
if (points.size() < 2) {
throw invalid_argument("At least 2 control points required");
}
}
// 计算曲线上的点
Point evaluate(double t) const {
if (t < 0 || t > 1) {
t = max(0.0, min(1.0, t));
}
Point result(0, 0);
int n = controlPoints.size() - 1;
for (int i = 0; i <= n; i++) {
double basis = bernstein(n, i, t);
result = result + controlPoints[i] * basis;
}
return result;
}
// 计算一阶导数(速度)
Point derivative(double t) const {
if (t < 0 || t > 1) {
t = max(0.0, min(1.0, t));
}
Point result(0, 0);
int n = controlPoints.size() - 1;
for (int i = 0; i < n; i++) {
double basis = bernstein(n - 1, i, t);
Point diff = controlPoints[i + 1] - controlPoints[i];
result = result + diff * (n * basis);
}
return result;
}
// 计算二阶导数(加速度)
Point secondDerivative(double t) const {
if (t < 0 || t > 1) {
t = max(0.0, min(1.0, t));
}
Point result(0, 0);
int n = controlPoints.size() - 1;
if (n < 2) {
return result;
}
for (int i = 0; i < n - 1; i++) {
double basis = bernstein(n - 2, i, t);
Point diff = (controlPoints[i + 2] - controlPoints[i + 1]) -
(controlPoints[i + 1] - controlPoints[i]);
result = result + diff * (n * (n - 1) * basis);
}
return result;
}
// 计算曲率
double curvature(double t) const {
Point vel = derivative(t);
Point acc = secondDerivative(t);
double speed = vel.norm();
if (speed < 1e-10) {
return numeric_limits<double>::infinity();
}
double crossProd = fabs(vel.cross(acc));
return crossProd / pow(speed, 3);
}
// 计算曲率半径
double curvatureRadius(double t) const {
double k = curvature(t);
if (k < 1e-10 || k > 1e10) {
return numeric_limits<double>::infinity();
}
return 1.0 / k;
}
// 检查曲率约束
bool checkCurvatureConstraint(double minRadius, int samples = 200) const {
for (int i = 0; i <= samples; i++) {
double t = static_cast<double>(i) / samples;
double radius = curvatureRadius(t);
if (radius < minRadius && !isinf(radius)) {
cerr << "Curvature violation at t=" << t
<< ": radius=" << radius << "m < minRadius=" << minRadius << "m" << endl;
return false;
}
}
return true;
}
// 获取曲线上的点集
vector<Point> getCurvePoints(int samples = 200) const {
vector<Point> points;
points.reserve(samples + 1);
for (int i = 0; i <= samples; i++) {
double t = static_cast<double>(i) / samples;
points.push_back(evaluate(t));
}
return points;
}
// 获取最小曲率半径
double getMinCurvatureRadius(int samples = 500) const {
double minRadius = numeric_limits<double>::infinity();
for (int i = 0; i <= samples; i++) {
double t = static_cast<double>(i) / samples;
double radius = curvatureRadius(t);
if (!isinf(radius) {
if (radius < minRadius) {
minRadius = radius;
// 调试信息
// cerr << "New min radius: " << minRadius << " at t=" << t << endl;
}
}
}
return minRadius;
}
};
// 导出点集到CSV文件
void exportToCSV(const string& filename, const vector<Point>& points) {
ofstream file(filename);
if (!file) {
cerr << "Error opening file: " << filename << endl;
return;
}
file << "x,y" << endl;
file << fixed << setprecision(6);
for (const auto& p : points) {
file << p.x << "," << p.y << endl;
}
cout << "Exported " << points.size() << " points to " << filename << endl;
}
// 可视化路径
void visualizePath() {
ofstream script("visualize.py");
if (!script) {
cerr << "Error creating visualization script" << endl;
return;
}
script << R"(
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Circle
# 加载数据
control = np.genfromtxt('control_points.csv', delimiter=',', skip_header=1)
original = np.genfromtxt('original_curve.csv', delimiter=',', skip_header=1)
geometric = np.genfromtxt('geometric_path.csv', delimiter=',', skip_header=1)
constrained_controls = np.genfromtxt('constrained_control_points.csv', delimiter=',', skip_header=1)
constrained = np.genfromtxt('constrained_curve.csv', delimiter=',', skip_header=1)
# 可视化
plt.figure(figsize=(12, 10))
plt.plot(control[:,0], control[:,1], 'ro-', markersize=4, label='Original Control Points')
plt.plot(original[:,0], original[:,1], 'b-', linewidth=1.5, alpha=0.7, label='Original Curve')
plt.plot(geometric[:,0], geometric[:,1], 'm--', linewidth=2, alpha=0.5, label='Geometric Path')
plt.plot(constrained_controls[:,0], constrained_controls[:,1], 'go-', markersize=5, label='Constrained Control Points')
plt.plot(constrained[:,0], constrained[:,1], 'g-', linewidth=2.5, label='Constrained Curve')
# 添加标签和标题
plt.legend(loc='upper left')
plt.grid(True)
plt.axis('equal')
plt.title('Path Planning with Curvature Constraint (Min Radius: 10m)')
plt.xlabel('X')
plt.ylabel('Y')
# 显示曲率约束区域
plt.text(-3, 16, "Min Turning Radius: 10m", fontsize=12, bbox=dict(facecolor='white', alpha=0.8))
# 绘制转弯半径圆
circle1 = Circle((-2+15, 4), 10, color='orange', fill=False, linestyle='--', label='Turning Radius (15m)')
circle2 = Circle((4, 11-15), 10, color='purple', fill=False, linestyle='--')
plt.gca().add_patch(circle1)
plt.gca().add_patch(circle2)
# 添加圆弧中心标记
plt.plot(-2+15, 4, 'yo', markersize=8, label='Turn Center 1')
plt.plot(4, 11-15, 'co', markersize=8, label='Turn Center 2')
# 保存并显示
plt.savefig('path_comparison.png', dpi=300)
plt.show()
)";
cout << "Visualization script saved as visualize.py" << endl;
cout << "Run 'python visualize.py' to generate the plot" << endl;
}
int main() {
// 测试用例控制点
vector<Point> controlPoints = {
{-2, 0}, {-2, 0.5}, {-2, 1}, {-2, 1.5}, {-2, 2}, {-2, 2.5}, {-2, 3}, {-2, 3.5}, {-2, 4},
{4, 11}, {4, 11.5}, {4, 12}, {4, 12.5}, {4, 13}, {4, 13.5}, {4, 14}, {4, 14.5}, {4, 15}
};
try {
// 1. 创建原始贝塞尔曲线
BezierCurve originalCurve(controlPoints);
// 检查原始曲线的曲率约束
bool originalSatisfies = originalCurve.checkCurvatureConstraint(MIN_RADIUS);
double originalMinRadius = originalCurve.getMinCurvatureRadius();
cout << "Original curve min curvature radius: " << originalMinRadius << " m" << endl;
cout << "Original curve satisfies curvature constraint: "
<< (originalSatisfies ? "YES" : "NO") << endl;
// 导出原始曲线和控制点
exportToCSV("control_points.csv", controlPoints);
vector<Point> originalCurvePoints = originalCurve.getCurvePoints(200);
exportToCSV("original_curve.csv", originalCurvePoints);
// 2. 创建几何路径和满足约束的贝塞尔曲线
PathPlanner planner(MIN_RADIUS);
// 生成几何路径(直线+圆弧)
vector<Point> geometricPath = planner.generateGeometricPath(controlPoints[0], controlPoints.back());
exportToCSV("geometric_path.csv", geometricPath);
// 从几何路径生成控制点(增加控制点数量)
vector<Point> constrainedControlPoints = planner.generateBezierControlPoints(geometricPath);
exportToCSV("constrained_control_points.csv", constrainedControlPoints);
// 创建约束曲线
BezierCurve constrainedCurve(constrainedControlPoints);
// 检查优化后的曲线
bool constrainedSatisfies = constrainedCurve.checkCurvatureConstraint(MIN_RADIUS);
double constrainedMinRadius = constrainedCurve.getMinCurvatureRadius();
cout << "\nConstrained curve min curvature radius: " << constrainedMinRadius << " m" << endl;
cout << "Constrained curve satisfies curvature constraint: "
<< (constrainedSatisfies ? "YES" : "NO") << endl;
// 导出优化后的曲线
vector<Point> constrainedCurvePoints = constrainedCurve.getCurvePoints(200);
exportToCSV("constrained_curve.csv", constrainedCurvePoints);
// 3. 生成可视化脚本
visualizePath();
if (constrainedSatisfies) {
cout << "\nSUCCESS: Generated path satisfies minimum turning radius of "
<< MIN_RADIUS << " meters with " << constrainedControlPoints.size()
<< " control points." << endl;
} else {
cerr << "\nWARNING: Curve does not fully satisfy curvature constraints. "
<< "Consider further increasing the number of control points." << endl;
}
} catch (const exception& e) {
cerr << "Error: " << e.what() << endl;
return 1;
}
return 0;
}最小转弯半径为10m,优化一下它的几何路径