核心内容摘要
深度共鸣与权力的舞步：开启“男男打屁股”的奇妙冒险之旅
六自由度机械臂三套代码-三次/五次/七次多项式样条插值曲线、五次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-
)/dt; end通过节点矢量的控制既能保证C2连续性加速度连续又能局部调整而不影响整条曲线。
实测在绘制圆弧轨迹时比多项式方法节省30%以上的算力特别是在处理机械臂奇异点附近运动时稳得一匹。
六自由度机械臂三套代码-三次/五次/七次多项式样条插值曲线、五次B样条曲线插值-2 轨迹规划圆弧轨迹机器人工具箱带源码注释。
起始点、中间点、终止点可自行修改。
一共三套代码。
机械臂数据可自行安需修改适合学习三套代码跑下来个人推荐从五次多项式入门等摸清自家机械臂的物理特性后再上B样条。
代码中那些看似啰嗦的注释比如//注意单位换算mm转m其实都是前人踩过的坑千万别手贱删了。
下次可以试试在过渡段混用不同插值方法说不定能调教出更风骚的走位。