六自由度机械臂三套代码-三次/五次/七次多项式样条插值曲线、五次B样条曲线插值-2 轨迹规划，圆弧轨迹，机器人工具箱，带源码注释。 起始点、中间点、终止点可自行修改。 （一共三套代码。 机械臂数据可自行安需修改，适合学习）
三次多项式：稳如老狗的基础操作
先看这个最老实的三次多项式插值。为啥用三次？因为位移、速度、加速度这三个关键参数刚好能凑出四个方程。代码里这个矩阵运算看着唬人，其实核心就干一件事——解方程组：
% 系数矩阵生成（关键片段） A = [1 t0 t0^2 t0^3; 0 1 2*t0 3*t0^2; 1 t1 t1^2 t1^3; 0 1 2*t1 3*t1^2]; % 解算系数 coeff = A \ [q0; v0; q1; v1];这里t0和t1是时间节点，q是关节角度。实际跑起来会发现，三次曲线在中间点的加速度会突变——就像开车时突然踩刹车，机械臂运行会有轻微抖动。适合对平滑性要求不高的搬运场景。
五次多项式：丝滑进阶版
想要更顺滑？看五次多项式这段。比三次多了两个约束条件，能保证加速度连续：
% 五次多项式参数计算 A = [1 t0 t0^2 t0^3 t0^4 t0^5; 0 1 2*t0 3*t0^2 4*t0^3 5*t0^4; 0 0 2 6*t0 12*t0^2 20*t0^3; 1 t1 t1^2 t1^3 t1^4 t1^5; 0 1 2*t1 3*t1^2 4*t1^3 5*t1^4; 0 0 2 6*t1 12*t1^2 20*t1^3];代价是计算量翻倍，实测关节角度变化曲线像德芙巧克力般丝滑。但注意！当中间点间距过小时可能出现超调，就像新手司机猛打方向盘，这时候得调小加速度限制参数。
七次多项式：控制狂的最爱
七次项就更骚了，八个方程能控制到加加速度（jerk）级别：
# 七次项系数求解 A = np.array([ [1, t, t**2, t**3, t**4, t**5, t**6, t**7], [0, 1, 2*t, 3*t**2, 4*t**3, 5*t**4, 6*t**5, 7*t**6], ... # 八个约束条件 ])实验室里跑数据曲线完美得像PS的，但实际控制时遇到过什么问题？电机扭矩跟不上理论计算！所以代码里的力矩校验模块千万不能删，不然分分钟上演机械臂蹦迪现场。
五次B样条：老司机的选择
最后压轴的五次B样条才是真大佬，看看这个去峰值的操作：
% 去尖峰速度处理 if abs(velocity(i)) > v_max velocity(i) = sign(velocity(i)) * v_max; % 重计算对应加速度 acceleration(i) = (velocity(i)-velocity(i-1))/dt; end通过节点矢量的控制，既能保证C2连续性（加速度连续），又能局部调整而不影响整条曲线。实测在绘制圆弧轨迹时，比多项式方法节省30%以上的算力，特别是在处理机械臂奇异点附近运动时稳得一匹。
六自由度机械臂三套代码-三次/五次/七次多项式样条插值曲线、五次B样条曲线插值-2 轨迹规划，圆弧轨迹，机器人工具箱，带源码注释。 起始点、中间点、终止点可自行修改。 （一共三套代码。 机械臂数据可自行安需修改，适合学习）
三套代码跑下来，个人推荐从五次多项式入门，等摸清自家机械臂的物理特性后再上B样条。代码中那些看似啰嗦的注释（比如//注意单位换算mm转m）其实都是前人踩过的坑，千万别手贱删了。下次可以试试在过渡段混用不同插值方法，说不定能调教出更风骚的走位。
)