已知无人车起点和终点位姿进行轨迹规划c++程序

以下是一个简单的无人车轨迹规划 C 程序，假设已知无人车起点和终点位姿，且车辆速度为固定值：

#include <stdio.h>
#include <math.h>

#define PI 3.14159265358979323846

typedef struct {
    double x;
    double y;
    double theta;
} Pose;

int main() {
    // 定义起点和终点位姿
    Pose start = {0, 0, PI/4}; // 起点：x=0, y=0, theta=45度
    Pose end = {10, 10, PI/4}; // 终点：x=10, y=10, theta=45度
    
    // 定义时间步长和车速
    double dt = 0.1;
    double v = 1.0; // 假设车速为1m/s
    
    // 计算起点到终点的距离和方向
    double dx = end.x - start.x;
    double dy = end.y - start.y;
    double d = sqrt(dx*dx + dy*dy); // 距离
    double direction = atan2(dy, dx); // 方向
    
    // 初始化轨迹规划变量
    double t = 0;
    double x = start.x;
    double y = start.y;
    double theta = start.theta;
    
    // 循环计算轨迹
    while (t < d/v) { // 当时间小于从起点到终点的时间时
        // 计算运动学模型
        double dx = v * cos(theta) * dt;
        double dy = v * sin(theta) * dt;
        double dtheta = 0; // 假设车辆不转弯
        
        // 更新位姿
        x += dx;
        y += dy;
        theta += dtheta;
        
        // 输出当前时刻的位姿
        printf("t=%f, x=%f, y=%f, theta=%f\n", t, x, y, theta);
        
        // 更新时间
        t += dt;
    }
    
    // 输出终点位姿
    printf("t=%f, x=%f, y=%f, theta=%f\n", t, end.x, end.y, end.theta);
    
    return 0;
}

这个程序中，我们首先定义了起点和终点的位姿，然后计算了起点到终点的距离和方向。接着，我们初始化了轨迹规划变量，并在循环中计算运动学模型，更新位姿，输出当前时刻的位姿，最后输出终点位姿。

需要注意的是，这个程序的轨迹规划是一种非常简单的直线行驶模式，实际中需要考虑更加复杂的运动模型和轨迹规划算法。