局部路径规划算法代码——贝塞尔曲线法

由于贝塞尔曲线是一个流行的局部路径规划算法，下面是一些示例代码以进行贝塞尔曲线路径规划。

C++代码示例：

#include <iostream> 
#include <cmath> 

using namespace std; 

struct Point {
    double x, y;
};

Point getNextBezierPoint(double t, Point p0, Point p1, Point p2) {
    double x = pow(1 - t, 2) * p0.x + 2 * (1 - t) * t * p1.x + pow(t, 2) * p2.x;
    double y = pow(1 - t, 2) * p0.y + 2 * (1 - t) * t * p1.y + pow(t, 2) * p2.y;
    return { x, y };
}

int main() {
    Point p0 = { 0, 0 };
    Point p1 = { 3, 3 };
    Point p2 = { 6, 0 };

    for (double i = 0; i <= 1; i += 0.1) {
        Point nextPoint = getNextBezierPoint(i, p0, p1, p2);
        cout << "x: " << nextPoint.x << ", y: " << nextPoint.y << endl;
    }

    return 0;
}

Python代码示例：

import math

class Point:
    def __init__(self, x, y):
        self.x = x
        self.y = y

def get_next_bezier_point(t, p0, p1, p2):
    x = math.pow(1 - t, 2) * p0.x + 2 * (1 - t) * t * p1.x + math.pow(t, 2) * p2.x
    y = math.pow(1 - t, 2) * p0.y + 2 * (1 - t) * t * p1.y + math.pow(t, 2) * p2.y
    return Point(x, y)

p0 = Point(0, 0) 
p1 = Point(3, 3) 
p2 = Point(6, 0)

for i in range(11):
    t = i / 10
    nextPoint = get_next_bezier_point(t, p0, p1, p2)
    print("x:", nextPoint.x, ", y:", nextPoint.y)

这些示例代码演示了如何使用贝塞尔曲线算法找到给定控制点的路径上的坐标。可以使用这些坐标来跟踪车辆或任何机器人的运动。