3、Python与C++基础速通：面向轨迹规划的Python与C++核心语法回顾

说实话，很多刚入行的朋友问我：「做轨迹规划，到底用Python还是C++？」

我的回答一直是：两个都得会。Python用来快速验证算法、画曲线、做原型；C++用来落地到真机、跑实时控制、对接底层硬件。我自己做项目时，经常是Python里调通一条轨迹，再翻译成C++塞进控制器里。

这一章，咱们就直奔主题。不讲那些用不上的语法糖，只聊你在轨迹规划里真正会碰到的核心语法。

3.1 Python：快速原型验证的利器

我个人习惯用Python做算法验证。为什么？因为写起来快，改起来也快。你想想看，调一条五次多项式轨迹，Python里改个参数，回车就能看到曲线变化。C++还得编译半天。

3.1.1 列表与数组：轨迹点的容器

轨迹规划里最基础的数据结构就是「点」和「路径」。Python的列表天然适合干这个。

# 存储一条二维轨迹点
trajectory = [
    [0.0, 0.0],
    [0.5, 0.3],
    [1.0, 0.8],
    [1.5, 1.2],
    [2.0, 1.5]
]

# 提取所有x坐标
x_vals = [p[0] for p in trajectory]
# 提取所有y坐标
y_vals = [p[1] for p in trajectory]

print(f"轨迹点数: {len(trajectory)}")
print(f"x范围: {min(x_vals):.2f} ~ {max(x_vals):.2f}")

3.1.2 NumPy：矩阵运算的加速器

做轨迹规划，绕不开坐标变换。旋转矩阵、平移向量、齐次坐标……这些用纯Python列表算起来太慢了。NumPy就是干这个的。

import numpy as np

# 定义一个旋转矩阵（绕Z轴转30度）
theta = np.radians(30)
R = np.array([
    [np.cos(theta), -np.sin(theta), 0],
    [np.sin(theta),  np.cos(theta), 0],
    [0,              0,             1]
])

# 定义一个平移向量
t = np.array([1.0, 2.0, 0.0])

# 齐次变换矩阵
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t

# 对一个点做变换
point = np.array([0.5, 0.3, 0.0, 1.0])
transformed = T @ point
print(f"变换后: {transformed[:3]}")

3.1.3 Matplotlib：把轨迹画出来

光算不行，得看得见。我每次调轨迹参数，第一件事就是画曲线。肉眼一看就知道平滑不平滑、有没有超调。

import matplotlib.pyplot as plt

# 生成一条S形速度曲线
t = np.linspace(0, 1, 100)
v = 3 * t**2 - 2 * t**3  # 归一化S曲线

plt.figure(figsize=(8, 4))
plt.plot(t, v, 'b-', linewidth=2, label='速度曲线')
plt.plot(t, np.gradient(v, t), 'r--', label='加速度曲线')
plt.xlabel('时间 (s)')
plt.ylabel('速度 / 加速度')
plt.title('S形速度规划')
plt.grid(True, alpha=0.3)
plt.legend()
plt.show()

3.2 C++：真机落地的硬核选择

Python验证完了，就该C++上场了。说实话，C++的语法比Python啰嗦不少，但换来的是性能和可控性。我在工业机器人项目里，所有实时轨迹生成都是用C++写的。

3.2.1 std::vector：动态数组管理轨迹点

C++里最常用的轨迹点容器就是 std::vector。它和Python的列表很像，但需要指定类型。

#include <vector>
#include <array>
#include <iostream>

// 定义一个二维点类型
using Point2D = std::array<double, 2>;

int main() {
    // 存储轨迹点
    std::vector<Point2D> trajectory;
    
    // 添加点
    trajectory.push_back({0.0, 0.0});
    trajectory.push_back({0.5, 0.3});
    trajectory.push_back({1.0, 0.8});
    
    // 遍历所有点
    for (const auto& p : trajectory) {
        std::cout << "x: " << p[0] << ", y: " << p[1] << std::endl;
    }
    
    // 获取轨迹长度
    std::cout << "轨迹点数: " << trajectory.size() << std::endl;
    
    return 0;
}

3.2.2 Eigen：C++里的NumPy

做轨迹规划，C++里没有NumPy怎么办？用Eigen。这是我最爱的C++线性代数库，语法和NumPy很像，性能还更好。

#include <Eigen/Dense>
#include <iostream>

int main() {
    // 定义旋转矩阵（绕Z轴30度）
    double theta = M_PI / 6.0;
    Eigen::Matrix3d R;
    R << cos(theta), -sin(theta), 0,
          sin(theta),  cos(theta), 0,
          0,           0,          1;
    
    // 定义平移向量
    Eigen::Vector3d t(1.0, 2.0, 0.0);
    
    // 构建齐次变换矩阵
    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
    T.block<3,3>(0,0) = R;
    T.block<3,1>(0,3) = t;
    
    // 变换一个点
    Eigen::Vector4d point(0.5, 0.3, 0.0, 1.0);
    Eigen::Vector4d transformed = T * point;
    
    std::cout << "变换后: " << transformed.head<3>().transpose() << std::endl;
    
    return 0;
}

3.2.3 指针与引用：高效传参的艺术

轨迹规划里经常要传大量数据。如果每次都复制一份，性能就崩了。C++的引用和指针就是解决这个问题的。

#include <vector>

// 不好的写法：传值，复制整个轨迹
void processTrajectoryBad(std::vector<Point2D> traj) {
    // 处理...
}

// 好的写法：传引用，不复制
void processTrajectoryGood(const std::vector<Point2D>& traj) {
    // 处理...
    // 用const保证不修改原数据
}

// 需要修改时：传非常量引用
void addOffset(std::vector<Point2D>& traj, double dx, double dy) {
    for (auto& p : traj) {
        p[0] += dx;
        p[1] += dy;
    }
}

3.3 Python与C++的互转：从原型到产品

实际项目中，我经常这样工作：Python里写好算法原型，验证通过后，再翻译成C++。下面这个表格总结了最常见的对应关系。

功能	Python	C++
动态数组	list	std::vector
矩阵运算	numpy.array	Eigen::Matrix
范围遍历	for x in arr:	for (const auto& x : vec)
列表推导式	[f(x) for x in arr]	std::transform
函数定义	def f(x):	auto f = [](double x){...}
异常处理	try/except	try/catch
文件读写	open()/write()	std::fstream

3.4 实战：一个简单的轨迹插值器

咱们来写一个实际的东西：线性插值器。给定起点和终点，在中间插出N个点。先写Python版本，再写C++版本。

Python版本

def linear_interpolate(start, end, num_points):
    """
    线性插值生成轨迹点
    start: 起点 [x, y]
    end: 终点 [x, y]
    num_points: 插值点数（包含起点终点）
    """
    points = []
    for i in range(num_points):
        t = i / (num_points - 1)  # 归一化参数 0~1
        x = start[0] + t * (end[0] - start[0])
        y = start[1] + t * (end[1] - start[1])
        points.append([x, y])
    return points

# 使用示例
start = [0.0, 0.0]
end = [2.0, 1.5]
traj = linear_interpolate(start, end, 5)
for p in traj:
    print(f"({p[0]:.2f}, {p[1]:.2f})")

C++版本

#include <vector>
#include <array>
#include <iostream>

using Point2D = std::array<double, 2>;

std::vector<Point2D> linearInterpolate(
    const Point2D& start, 
    const Point2D& end, 
    int num_points
) {
    std::vector<Point2D> points;
    points.reserve(num_points);  // 预分配内存，提升性能
    
    for (int i = 0; i < num_points; ++i) {
        double t = static_cast<double>(i) / (num_points - 1);
        double x = start[0] + t * (end[0] - start[0]);
        double y = start[1] + t * (end[1] - start[1]);
        points.push_back({x, y});
    }
    return points;
}

int main() {
    Point2D start = {0.0, 0.0};
    Point2D end = {2.0, 1.5};
    
    auto traj = linearInterpolate(start, end, 5);
    
    for (const auto& p : traj) {
        std::cout << "(" << p[0] << ", " << p[1] << ")" << std::endl;
    }
    
    return 0;
}

好了，这一章的内容就到这里。Python和C++的语法基础，说白了就是工具。工具顺手了，后面的轨迹规划算法才能写得顺。下一章咱们会用到这些知识，开始真正搭建轨迹规划的核心算法。