PythonRobotics 四旋翼三维轨迹跟踪仿真详解五次多项式轨迹生成与 PD 控制实现【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics本文基于开源仓库 PythonRobotics 的AerialNavigation模块逐行剖析一个四旋翼quadrotor三维轨迹跟踪仿真的完整实现包括如何用五次多项式把路点序列拟合成平滑轨迹、如何用 PD 控制器把期望加速度映射为推力与三轴力矩、以及刚体运动学与三维可视化的落地方式。读完本文你将能独立读懂并复跑这套仿真理解轨迹规划—姿态解耦控制—状态积分的完整闭环并掌握调整控制器增益以改变跟踪性能的具体方法。一、模块概览三个文件构成的完整闭环本模块位于仓库 AerialNavigation/drone_3d_trajectory_following/ 目录由三个源文件组成各自职责清晰文件职责drone_3d_trajectory_following.py仿真主入口包含路点定义、主循环quad_sim、五次多项式求值函数与旋转矩阵TrajectoryGenerator.py五次多项式轨迹生成器用边界条件求解多项式系数Quadrotor.py四旋翼的三维可视化类位姿变换、轨迹绘制对应的文档页为 drone_3d_trajectory_following_main.rst该文档将其定位为 a 3d trajectory following simulation for a quadrotor并通过.. autofunction::指令直接挂载了主仿真函数AerialNavigation.drone_3d_trajectory_following.drone_3d_trajectory_following.quad_sim——也就是说文档正文的技术核心就是这个quad_sim函数本文即围绕它展开。二、核心流程从路点到控制指令整个仿真以main()为入口drone_3d_trajectory_following.py流程如下定义路点序列在高度z 5的平面上定义四个路点[[-5, -5, 5], [5, -5, 5], [5, 5, 5], [-5, 5, 5]]构成一个边长为 10 的正方形飞行走廊逐段生成轨迹对每一对相邻路点含最后一段回到起点的闭环构造一个TrajectoryGenerator并调用solve()求解五次多项式系数分别得到x_c、y_c、z_c三组系数进入仿真主循环把三组系数交给quad_sim(x_c, y_c, z_c)quad_sim 定义在离散时间步上计算期望状态、控制量并积分机体状态。值得注意的是quad_sim内层的while t T循环每完成一段就复位t 0并用i (i 1) % 4轮换段索引外层循环执行n_run 8次——也就是沿正方形闭环连续飞行两圈用于验证控制器对重复轨迹的持续跟踪能力。三、轨迹生成五次多项式与边界条件3.1 为什么要用五次多项式要让四旋翼平滑地飞过路点仅约束位置是不够的——若只约束位置一次/三次多项式轨迹在路点处会出现速度或加速度跳变控制器将被迫输出突变指令。五次多项式拥有 6 个待定系数恰好可以同时约束位置、速度、加速度这 3 个量的起点与终点共 6 个边界条件从而保证轨迹本身在路点处二阶连续。3.2 边界条件矩阵的构造在 TrajectoryGenerator.py 的solve()中系数向量c [c0, c1, c2, c3, c4, c5]通过解线性方程组A · c b求得。矩阵A的六行分别对应六条边界条件位置 t0: [0, 0, 0, 0, 0, 1] 位置 tT: [T^5, T^4, T^3, T^2, T, 1] 速度 t0: [0, 0, 0, 0, 1, 0] 速度 tT: [5T^4, 4T^3, 3T^2, 2T, 1, 0] 加速度 t0: [0, 0, 0, 2, 0, 0] 加速度 tT: [20T^3, 12T^2, 6T, 2, 0, 0]右侧向量b_x、b_y、b_z分别填入 x/y/z 三个方向的起始与目标位置、速度、加速度。TrajectoryGenerator构造函数第 10 行中速度与加速度默认均为零向量即默认要求路点处速度与加速度为零如果你希望轨迹在路点处保持连续速度如匀速过弯可以传入非零的start_vel/des_vel。最终通过np.linalg.solve(A, b)一次性解出三个方向的系数x_c、y_c、z_c存入对象属性供仿真循环使用。3.3 轨迹求值公式在 drone_3d_trajectory_following.py 中三个求值函数给出任意时刻t的期望状态calculate_position(c, t)c0·t⁵ c1·t⁴ c2·t³ c3·t² c4·t c5calculate_velocity(c, t)5·c0·t⁴ 4·c1·t³ 3·c2·t² 2·c3·t c4calculate_acceleration(c, t)20·c0·t³ 12·c1·t² 6·c2·t 2·c3正是多项式系数 时间的这套模式把离线规划结果变成了仿真循环里实时可查询的期望轨迹。四、控制器设计推力与三轴力矩的 PD 控制这是quad_sim的核心。四旋翼是一个欠驱动系统——只有 4 个执行器总推力 三轴力矩却要控制 6 个自由度3 个位置 3 个姿态。源码采用经典的位置环 → 姿态环解耦策略先由位置误差和期望加速度反解出期望姿态再用姿态误差生成力矩。4.1 总推力z 轴高度控制thrust m * (g des_z_acc Kp_z * (des_z_pos - z_pos) Kd_z * (des_z_vel - z_vel))推力由三部分组成重力补偿m·g、期望加速度前馈m·des_z_acc以及 z 方向位置/速度的 PD 反馈。由于四旋翼的总推力方向默认对准机体 z 轴z 轴控制天然与姿态解耦这也是为什么仿真中只在推力里用到des_z_pos/des_z_vel而 x/y 方向则通过倾斜机体roll/pitch来间接产生水平加速度。4.2 横滚/俯仰力矩水平位置控制roll_torque Kp_roll * (((des_x_acc * sin(des_yaw) - des_y_acc * cos(des_yaw)) / g) - roll) pitch_torque Kp_pitch * (((des_x_acc * cos(des_yaw) - des_y_acc * sin(des_yaw)) / g) - pitch) yaw_torque Kp_yaw * (des_yaw - yaw)期望加速度des_x_acc、des_y_acc除以重力加速度g后得到所需倾角的小角度近似再通过des_yaw做坐标旋转把机体系需要的加速度映射为世界系下的期望 roll/pitch 角最后与当前姿态角做误差比例控制。当前仿真中des_yaw 0第 58 行即保持机头朝向固定此时旋转项退化为简单的投影关系若要跟踪带偏航变化的轨迹可将des_yaw改为由轨迹生成的期望偏航序列。4.3 控制器增益速查表quad_sim文件头部第 14-33 行集中定义了全部仿真参数与 PD 增益参数值含义g9.81重力加速度m/s²m0.2机体质量kgIxx, Iyy, Izz1, 1, 1三轴转动惯量单位化便于演示T5每段轨迹的持续时间sKp_x, Kp_y, Kp_z1, 1, 1位置环比例增益Kd_x, Kd_y10, 10x/y 方向速度阻尼增益Kd_z1z 方向速度阻尼增益Kp_roll, Kp_pitch, Kp_yaw25, 25, 25姿态环比例增益从增益配置可以观察到设计意图姿态环增益25远高于位置环1体现内环快、外环慢的经典分层控制思想Kd_x/Kd_y 10高于Kd_z 1因为 x/y 位置依赖姿态倾斜间接控制需要更强的阻尼来抑制振荡。五、刚体动力学与状态积分5.1 旋转矩阵与世界系加速度机体坐标系下的推力是[0, 0, thrust]默认沿机体 z 轴需要变换到世界系。rotation_matrix(roll, pitch, yaw)第 170-189 行按 ZYX 顺序构造 3×3 旋转矩阵R rotation_matrix(roll, pitch, yaw) acc (np.matmul(R, np.array([0, 0, thrust.item()]).T) - np.array([0, 0, m * g]).T) / m即世界系加速度 旋转后的推力 − 重力矢量/ 质量。5.2 姿态与位置的欧拉积分力矩通过转动惯量转为角加速度再按时间步积分出角速度与姿态角roll_vel roll_torque * dt / Ixx pitch_vel pitch_torque * dt / Iyy yaw_vel yaw_torque * dt / Izz roll roll_vel * dt pitch pitch_vel * dt yaw yaw_vel * dt位置侧则对世界系加速度依次积分得到速度与位置x_vel x_acc * dt x_pos x_vel * dt整个仿真采用前向欧拉积分时间步dt 0.1s第 60 行。每步积分完成后调用q.update_pose(...)更新可视化。从源码结构看这套离散积分模型牺牲了部分数值精度以换取简洁清晰更适合教学演示若用于更高保真度的研究可替换为 RK4 等更高阶积分器。六、三维可视化Quadrotor 类Quadrotor.py 负责把抽象的状态向量还原为可见的飞行器模型机体模型用 4 个顶点p1~p4两条相互垂直的臂长度由size参数控制仿真中取 1表示四旋翼的十字结构姿态变换transformation_matrix()第 48-60 行把 roll/pitch/yaw 与位置合成 3×4 齐次变换矩阵将 4 个机体坐标点投影到世界系轨迹绘制update_pose()会持续把位置追加到x_data/y_data/z_dataplot()中以蓝色点线画出飞行历史轨迹b:臂以红色实线绘制交互控制使用matplotlib的 3D 投影坐标范围固定为 x/y ∈ [−5, 5]、z ∈ [0, 10]第 83-85 行并按 esc 键可随时退出仿真第 27-28 行。仿真初始状态drone_3d_trajectory_following.py 第 42-56 行为x-5, y-5, z5速度、加速度与姿态角全零——恰好落在第一条轨迹的起点路点上因此仿真开始即进入稳定的轨迹跟踪阶段。七、运行与测试验证7.1 运行仿真从仓库根目录直接执行主脚本即可看到三维动画需已安装numpy、matplotlib依赖清单见 requirements/requirements.txtpython AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py文件头部show_animation True控制是否开启动画窗口第 12 行仿真结束后控制台打印Done第 122 行动画窗口保留便于观察最终轨迹。7.2 测试用例仓库为每个模块都配套了 pytest 测试。本模块的测试位于 tests/test_drone_3d_trajectory_following.pyimport conftest from AerialNavigation.drone_3d_trajectory_following \ import drone_3d_trajectory_following as m def test1(): m.show_animation False m.main() if __name__ __main__: conftest.run_this_test(__file__)测试的关键技巧是把show_animation置为False从而在无图形界面的 CI 环境中完整跑通轨迹生成 → 控制循环 → 状态积分的数值链路Quadrotor.plot()也被标记为# pragma: no cover不参与覆盖率统计。可通过 pytest 直接运行python -m pytest tests/test_drone_3d_trajectory_following.py或按仓库 runtests.sh 的组织方式批量执行全部测试。八、可调点与扩展方向基于以上源码分析以下是几个可以直接动手的调试/扩展方向均为从当前代码结构推断出的可行路径调整跟踪性能修改Kp_roll/Kp_pitch姿态环与Kd_x/Kd_y阻尼可观察跟踪误差与振荡的此消彼长Kd_z 1时 z 轴响应较软可尝试提高以加快高度收敛更换飞行轨迹修改main()中的waypoints列表即可让四旋翼沿任意三维路点序列飞行例如加入 z 方向高度变化形成立体轨迹开启偏航跟踪当前des_yaw 0固定机头若将des_yaw替换为随时间变化的期望偏航序列即可验证姿态耦合下的跟踪表现升级积分器把前向欧拉积分替换为 RK4 等更高阶方法可研究dt 0.1下数值误差对跟踪精度的影响修改分段时长调整T当前 5 s会同时改变轨迹的加加速度需求与单段飞行速度可用于压力测试控制器带宽。这套五次多项式轨迹生成 分层 PD 控制 三维可视化的仿真框架是理解四旋翼飞控中轨迹规划与姿态控制协同工作的最小可运行范例也是后续深入学习 MPC、几何控制等更先进方法的良好起点。相关模块入口可继续阅读 aerial_navigation_main.rst 以及同目录下的 rocket_powered_landing.py火箭动力着陆对照学习。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考
PythonRobotics 四旋翼三维轨迹跟踪仿真详解五次多项式轨迹生成与 PD 控制实现【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics本文基于开源仓库 PythonRobotics 的AerialNavigation模块逐行剖析一个四旋翼quadrotor三维轨迹跟踪仿真的完整实现包括如何用五次多项式把路点序列拟合成平滑轨迹、如何用 PD 控制器把期望加速度映射为推力与三轴力矩、以及刚体运动学与三维可视化的落地方式。读完本文你将能独立读懂并复跑这套仿真理解轨迹规划—姿态解耦控制—状态积分的完整闭环并掌握调整控制器增益以改变跟踪性能的具体方法。一、模块概览三个文件构成的完整闭环本模块位于仓库 AerialNavigation/drone_3d_trajectory_following/ 目录由三个源文件组成各自职责清晰文件职责drone_3d_trajectory_following.py仿真主入口包含路点定义、主循环quad_sim、五次多项式求值函数与旋转矩阵TrajectoryGenerator.py五次多项式轨迹生成器用边界条件求解多项式系数Quadrotor.py四旋翼的三维可视化类位姿变换、轨迹绘制对应的文档页为 drone_3d_trajectory_following_main.rst该文档将其定位为 a 3d trajectory following simulation for a quadrotor并通过.. autofunction::指令直接挂载了主仿真函数AerialNavigation.drone_3d_trajectory_following.drone_3d_trajectory_following.quad_sim——也就是说文档正文的技术核心就是这个quad_sim函数本文即围绕它展开。二、核心流程从路点到控制指令整个仿真以main()为入口drone_3d_trajectory_following.py流程如下定义路点序列在高度z 5的平面上定义四个路点[[-5, -5, 5], [5, -5, 5], [5, 5, 5], [-5, 5, 5]]构成一个边长为 10 的正方形飞行走廊逐段生成轨迹对每一对相邻路点含最后一段回到起点的闭环构造一个TrajectoryGenerator并调用solve()求解五次多项式系数分别得到x_c、y_c、z_c三组系数进入仿真主循环把三组系数交给quad_sim(x_c, y_c, z_c)quad_sim 定义在离散时间步上计算期望状态、控制量并积分机体状态。值得注意的是quad_sim内层的while t T循环每完成一段就复位t 0并用i (i 1) % 4轮换段索引外层循环执行n_run 8次——也就是沿正方形闭环连续飞行两圈用于验证控制器对重复轨迹的持续跟踪能力。三、轨迹生成五次多项式与边界条件3.1 为什么要用五次多项式要让四旋翼平滑地飞过路点仅约束位置是不够的——若只约束位置一次/三次多项式轨迹在路点处会出现速度或加速度跳变控制器将被迫输出突变指令。五次多项式拥有 6 个待定系数恰好可以同时约束位置、速度、加速度这 3 个量的起点与终点共 6 个边界条件从而保证轨迹本身在路点处二阶连续。3.2 边界条件矩阵的构造在 TrajectoryGenerator.py 的solve()中系数向量c [c0, c1, c2, c3, c4, c5]通过解线性方程组A · c b求得。矩阵A的六行分别对应六条边界条件位置 t0: [0, 0, 0, 0, 0, 1] 位置 tT: [T^5, T^4, T^3, T^2, T, 1] 速度 t0: [0, 0, 0, 0, 1, 0] 速度 tT: [5T^4, 4T^3, 3T^2, 2T, 1, 0] 加速度 t0: [0, 0, 0, 2, 0, 0] 加速度 tT: [20T^3, 12T^2, 6T, 2, 0, 0]右侧向量b_x、b_y、b_z分别填入 x/y/z 三个方向的起始与目标位置、速度、加速度。TrajectoryGenerator构造函数第 10 行中速度与加速度默认均为零向量即默认要求路点处速度与加速度为零如果你希望轨迹在路点处保持连续速度如匀速过弯可以传入非零的start_vel/des_vel。最终通过np.linalg.solve(A, b)一次性解出三个方向的系数x_c、y_c、z_c存入对象属性供仿真循环使用。3.3 轨迹求值公式在 drone_3d_trajectory_following.py 中三个求值函数给出任意时刻t的期望状态calculate_position(c, t)c0·t⁵ c1·t⁴ c2·t³ c3·t² c4·t c5calculate_velocity(c, t)5·c0·t⁴ 4·c1·t³ 3·c2·t² 2·c3·t c4calculate_acceleration(c, t)20·c0·t³ 12·c1·t² 6·c2·t 2·c3正是多项式系数 时间的这套模式把离线规划结果变成了仿真循环里实时可查询的期望轨迹。四、控制器设计推力与三轴力矩的 PD 控制这是quad_sim的核心。四旋翼是一个欠驱动系统——只有 4 个执行器总推力 三轴力矩却要控制 6 个自由度3 个位置 3 个姿态。源码采用经典的位置环 → 姿态环解耦策略先由位置误差和期望加速度反解出期望姿态再用姿态误差生成力矩。4.1 总推力z 轴高度控制thrust m * (g des_z_acc Kp_z * (des_z_pos - z_pos) Kd_z * (des_z_vel - z_vel))推力由三部分组成重力补偿m·g、期望加速度前馈m·des_z_acc以及 z 方向位置/速度的 PD 反馈。由于四旋翼的总推力方向默认对准机体 z 轴z 轴控制天然与姿态解耦这也是为什么仿真中只在推力里用到des_z_pos/des_z_vel而 x/y 方向则通过倾斜机体roll/pitch来间接产生水平加速度。4.2 横滚/俯仰力矩水平位置控制roll_torque Kp_roll * (((des_x_acc * sin(des_yaw) - des_y_acc * cos(des_yaw)) / g) - roll) pitch_torque Kp_pitch * (((des_x_acc * cos(des_yaw) - des_y_acc * sin(des_yaw)) / g) - pitch) yaw_torque Kp_yaw * (des_yaw - yaw)期望加速度des_x_acc、des_y_acc除以重力加速度g后得到所需倾角的小角度近似再通过des_yaw做坐标旋转把机体系需要的加速度映射为世界系下的期望 roll/pitch 角最后与当前姿态角做误差比例控制。当前仿真中des_yaw 0第 58 行即保持机头朝向固定此时旋转项退化为简单的投影关系若要跟踪带偏航变化的轨迹可将des_yaw改为由轨迹生成的期望偏航序列。4.3 控制器增益速查表quad_sim文件头部第 14-33 行集中定义了全部仿真参数与 PD 增益参数值含义g9.81重力加速度m/s²m0.2机体质量kgIxx, Iyy, Izz1, 1, 1三轴转动惯量单位化便于演示T5每段轨迹的持续时间sKp_x, Kp_y, Kp_z1, 1, 1位置环比例增益Kd_x, Kd_y10, 10x/y 方向速度阻尼增益Kd_z1z 方向速度阻尼增益Kp_roll, Kp_pitch, Kp_yaw25, 25, 25姿态环比例增益从增益配置可以观察到设计意图姿态环增益25远高于位置环1体现内环快、外环慢的经典分层控制思想Kd_x/Kd_y 10高于Kd_z 1因为 x/y 位置依赖姿态倾斜间接控制需要更强的阻尼来抑制振荡。五、刚体动力学与状态积分5.1 旋转矩阵与世界系加速度机体坐标系下的推力是[0, 0, thrust]默认沿机体 z 轴需要变换到世界系。rotation_matrix(roll, pitch, yaw)第 170-189 行按 ZYX 顺序构造 3×3 旋转矩阵R rotation_matrix(roll, pitch, yaw) acc (np.matmul(R, np.array([0, 0, thrust.item()]).T) - np.array([0, 0, m * g]).T) / m即世界系加速度 旋转后的推力 − 重力矢量/ 质量。5.2 姿态与位置的欧拉积分力矩通过转动惯量转为角加速度再按时间步积分出角速度与姿态角roll_vel roll_torque * dt / Ixx pitch_vel pitch_torque * dt / Iyy yaw_vel yaw_torque * dt / Izz roll roll_vel * dt pitch pitch_vel * dt yaw yaw_vel * dt位置侧则对世界系加速度依次积分得到速度与位置x_vel x_acc * dt x_pos x_vel * dt整个仿真采用前向欧拉积分时间步dt 0.1s第 60 行。每步积分完成后调用q.update_pose(...)更新可视化。从源码结构看这套离散积分模型牺牲了部分数值精度以换取简洁清晰更适合教学演示若用于更高保真度的研究可替换为 RK4 等更高阶积分器。六、三维可视化Quadrotor 类Quadrotor.py 负责把抽象的状态向量还原为可见的飞行器模型机体模型用 4 个顶点p1~p4两条相互垂直的臂长度由size参数控制仿真中取 1表示四旋翼的十字结构姿态变换transformation_matrix()第 48-60 行把 roll/pitch/yaw 与位置合成 3×4 齐次变换矩阵将 4 个机体坐标点投影到世界系轨迹绘制update_pose()会持续把位置追加到x_data/y_data/z_dataplot()中以蓝色点线画出飞行历史轨迹b:臂以红色实线绘制交互控制使用matplotlib的 3D 投影坐标范围固定为 x/y ∈ [−5, 5]、z ∈ [0, 10]第 83-85 行并按 esc 键可随时退出仿真第 27-28 行。仿真初始状态drone_3d_trajectory_following.py 第 42-56 行为x-5, y-5, z5速度、加速度与姿态角全零——恰好落在第一条轨迹的起点路点上因此仿真开始即进入稳定的轨迹跟踪阶段。七、运行与测试验证7.1 运行仿真从仓库根目录直接执行主脚本即可看到三维动画需已安装numpy、matplotlib依赖清单见 requirements/requirements.txtpython AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py文件头部show_animation True控制是否开启动画窗口第 12 行仿真结束后控制台打印Done第 122 行动画窗口保留便于观察最终轨迹。7.2 测试用例仓库为每个模块都配套了 pytest 测试。本模块的测试位于 tests/test_drone_3d_trajectory_following.pyimport conftest from AerialNavigation.drone_3d_trajectory_following \ import drone_3d_trajectory_following as m def test1(): m.show_animation False m.main() if __name__ __main__: conftest.run_this_test(__file__)测试的关键技巧是把show_animation置为False从而在无图形界面的 CI 环境中完整跑通轨迹生成 → 控制循环 → 状态积分的数值链路Quadrotor.plot()也被标记为# pragma: no cover不参与覆盖率统计。可通过 pytest 直接运行python -m pytest tests/test_drone_3d_trajectory_following.py或按仓库 runtests.sh 的组织方式批量执行全部测试。八、可调点与扩展方向基于以上源码分析以下是几个可以直接动手的调试/扩展方向均为从当前代码结构推断出的可行路径调整跟踪性能修改Kp_roll/Kp_pitch姿态环与Kd_x/Kd_y阻尼可观察跟踪误差与振荡的此消彼长Kd_z 1时 z 轴响应较软可尝试提高以加快高度收敛更换飞行轨迹修改main()中的waypoints列表即可让四旋翼沿任意三维路点序列飞行例如加入 z 方向高度变化形成立体轨迹开启偏航跟踪当前des_yaw 0固定机头若将des_yaw替换为随时间变化的期望偏航序列即可验证姿态耦合下的跟踪表现升级积分器把前向欧拉积分替换为 RK4 等更高阶方法可研究dt 0.1下数值误差对跟踪精度的影响修改分段时长调整T当前 5 s会同时改变轨迹的加加速度需求与单段飞行速度可用于压力测试控制器带宽。这套五次多项式轨迹生成 分层 PD 控制 三维可视化的仿真框架是理解四旋翼飞控中轨迹规划与姿态控制协同工作的最小可运行范例也是后续深入学习 MPC、几何控制等更先进方法的良好起点。相关模块入口可继续阅读 aerial_navigation_main.rst 以及同目录下的 rocket_powered_landing.py火箭动力着陆对照学习。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考
