机械臂轨迹规划代码实现：直线、圆弧、平滑加减速运动

机械臂轨迹规划代码实现直线、圆弧、平滑加减速运动轨迹规划就像是给机械臂写导航路线——路线不好机械臂不是抖就是冲写好了才能丝滑如舞。一、轨迹规划机械臂运动的灵魂机械臂控制系统里轨迹规划是真正的核心环节。它决定了机械臂从A点到B点怎么走、走多快、什么时候加速减速。规划得好运动平滑精准规划得差机械臂抖得像帕金森精度全丢甚至损坏设备。简单来说轨迹规划解决两个问题路径规划走哪条路几何路径速度规划走多快速度曲线两者结合才能让机械臂既不磕磕绊绊又不拖泥带水。二、关节空间 vs 笛卡尔空间2.1 关节空间轨迹规划关节空间规划就是给每个关节独立规划一条角度随时间变化的曲线。比如6个关节就规划6条独立的 θ(t) 曲线。优点计算简单不需要逆运动学不存在奇异点问题。缺点末端执行器在笛卡尔空间的轨迹不可控。你规划了关节角度但末端走出来的路径可能是一条诡异的曲线完全不是你想要的直线或圆弧。适用于点到点运动PTP不关心中间路径形状的场景。2.2 笛卡尔空间轨迹规划笛卡尔空间规划直接规划末端的路径——先定好末端要走直线还是圆弧再通过逆运动学实时反算出各关节角度。优点末端轨迹可控能精确走直线、圆弧等指定路径。缺点每一步都要做逆运动学计算计算量大可能遇到奇异点导致逆解失败或关节速度突变。适用于焊接、涂胶、3D打印等需要精确末端轨迹的场景。三、直线插补LIN直线插补是最基础的笛卡尔空间插补方式在两点之间做线性插值。数学表示很简单参数化描述P(t) P_start t * (P_end - P_start), t ∈ [0, 1]其中 t 是归一化时间参数由速度规划曲线决定。每一步算出末端位置再调用逆运动学得到关节角度。实际应用中两点之间的距离和速度规划曲线配合保证末端匀速或加减速运动。四、圆弧插补CIRC圆弧插补需要三个点确定一段圆弧起点、中间点、终点。4.1 圆心和半径计算三点确定圆弧的步骤由三个点求圆心三点所在平面确定后做两条线段的中垂线交点即圆心圆心到任意一点的距离即半径 R计算起点到终点对应的圆弧角度 Δθ参数化P(t) Center R * [cos(θ_start t·Δθ), sin(θ_start t·Δθ)]// 三点定圆弧 - 简化2D示例typedefstruct{doublex,y;}Point2D;Point2Dcalc_arc_center(Point2D p1,Point2D p2,Point2D p3){doubleaxp2.x-p1.x,ayp2.y-p1.y;doublebxp3.x-p1.x,byp3.y-p1.y;doubled2.0*(ax*by-ay*bx);if(fabs(d)1e-6){/* 三点共线无圆弧 */}doubleux(by*(ax*axay*ay)-ay*(bx*bxby*by))/d;doubleuy(ax*(bx*bxby*by)-bx*(ax*axay*ay))/d;Point2D center{p1.xux,p1.yuy};returncenter;}五、速度规划梯形 vs S曲线5.1 梯形速度规划梯形速度曲线分三段加速段 → 匀速段 → 减速段。速度随时间的曲线呈梯形形状。速度曲线 ╱‾‾‾‾‾╲ ╱ ╲ ╱ ╲ ╱ ╲ 0 t_total 加速 匀速 减速优点计算简单参数直观最大速度、加速度、总时间。缺点加速度在拐点处不连续产生机械冲击导致振动和噪音。5.2 梯形速度规划C代码typedefstruct{doubleq0;// 起始位置doubleq1;// 目标位置doublev_max;// 最大速度doublea_max;// 最大加速度doublet_acc;// 加速时间doublet_dec;// 减速时间doublet_const;// 匀速时间doublet_total;// 总时间}TrapezoidalPlan;// 规划梯形速度曲线inttrapezoidal_plan(TrapezoidalPlan*plan){doubledqplan-q1-plan-q0;doublesign(dq0)?1.0:-1.0;dqfabs(dq);// 计算加速时间和减速时间对称plan-t_accplan-v_max/plan-a_max;plan-t_decplan-t_acc;// 加减速阶段走过的距离doubled_acc0.5*plan-a_max*plan-t_acc*plan-t_acc;doubled_decd_acc;// 对称减速// 匀速阶段距离doubled_constdq-d_acc-d_dec;if(d_const0){// 无法达到最大速度三角形规划plan-t_accsqrt(dq/plan-a_max);plan-t_decplan-t_acc;plan-t_const0;plan-v_maxplan-a_max*plan-t_acc;}else{plan-t_constd_const/plan-v_max;}plan-t_totalplan-t_accplan-t_constplan-t_dec;return0;}// 根据时间获取当前位置doubletrapezoidal_eval(TrapezoidalPlan*plan,doublet){doubleq0plan-q0,sign(plan-q1plan-q0)?1.0:-1.0;doubleaplan-a_max,vplan-v_max;doubletaplan-t_acc,tcplan-t_const,tdplan-t_dec;if(tta){// 加速段: q q0 0.5*a*t^2returnq0sign*0.5*a*t*t;}elseif(ttatc){// 匀速段doubled_acc0.5*a*ta*ta;returnq0sign*(d_accv*(t-ta));}elseif(ttatctd){// 减速段doubled_acc0.5*a*ta*ta;doubled_constv*tc;doublet_remt-ta-tc;returnq0sign*(d_accd_constv*t_rem-0.5*a*t_rem*t_rem);}returnplan-q1;// 超出总时间返回目标}5.3 S曲线速度规划S曲线在梯形的基础上用多项式曲线平滑加速度的过渡让加速度从0逐渐增加到最大值再逐渐降到0。加速度曲线 ╱‾‾╲ ╱ ╲ ╱ ╲ ╱ ╲ 0 t_total 加 加匀 减减七段式S曲线加加速段 → 匀加速段 → 减加速段 → 匀速段 → 加减速段 → 匀减速段 → 减减速段。优点加速度连续变化无冲击运动更平滑机械振动小。缺点计算复杂参数多需要多段求解。实际工程中6轴机械臂几乎都用S曲线或更高级的速度规划。梯形仅用于简单场景或调试阶段。六、直线插补逆运动学调用完整流程直线插补算出末端位置 → 逆运动学算出关节角度 → 发送到伺服。#defineINTERP_PERIOD_MS10// 插补周期10ms// 直线插补 逆解调用循环voidlinear_interp_exec(Point3D start,Point3D end,doubleduration_s){TrapezoidalPlan plan;plan.q00.0;plan.q11.0;// 归一化参数 0→1plan.v_max1.0/duration_s;plan.a_maxplan.v_max*2.0;// 加速时间设为总时间一半trapezoidal_plan(plan);inttotal_steps(int)(plan.t_total*1000/INTERP_PERIOD_MS);for(inti0;itotal_steps;i){doubleti*INTERP_PERIOD_MS/1000.0;doublestrapezoidal_eval(plan,t);// 归一化位置 s∈[0,1]// 末端位置插值Point3D pose;pose.xstart.xs*(end.x-start.x);pose.ystart.ys*(end.y-start.y);pose.zstart.zs*(end.z-start.z);// 逆运动学求解关节角度JointAngles anglesinverse_kinematics(pose);// 发送到伺服驱动servo_send_command(angles);// 等待插补周期usleep(INTERP_PERIOD_MS*1000);}}七、插补周期选择插补周期是轨迹规划的核心时间参数决定每多久算一步。周期优点缺点5ms精度高轨迹更平滑计算压力大嵌入式可能跟不上10ms工业常用平衡精度与计算量一般场景够用20ms计算压力小低速运动还行高速运动轨迹粗糙工业机器人通常选10ms或8ms。嵌入式平台上10ms 是比较稳妥的选择——在RK3588等ARM平台上10ms内完成一次逆解插补计算绰绰有余。八、常见问题与处理8.1 奇异点处理奇异点是逆运动学中关节速度理论上趋于无穷大的位形。典型情况关节轴线重合导致末端微小运动需要关节极大速度。处理方式规划时避开已知奇异点区域好奇异点附近设置速度衰减因子使用阻尼最小二乘逆解Damped Least Squares避免关节速度爆炸8.2 关节限位检查每一步插补算出的关节角度都要检查是否超出物理限位。超出限位意味着机械臂到了物理极限继续运动会损坏设备。// 关节限位检查for(inti0;i6;i){if(angles[i]joint_limit_min[i]||angles[i]joint_limit_max[i]){EMERGENCY_STOP();log_error(关节%d超出限位: %.2f %.2f,i,angles[i],joint_limit_max[i]);returnERROR_LIMIT_EXCEEDED;}}轨迹规划完成后应做整条轨迹的预检——离线计算所有插补点检查每个关节角度是否在限位内有问题就提前修正不要等执行时才报错。轨迹规划是机械臂从知道要去哪到知道怎么去的关键桥梁。直线圆弧插补决定了路径形状梯形和S曲线速度规划决定了运动的节奏。代码层面不复杂但参数调试和异常处理才是工程的真功夫。把这些基础搞清楚后面接入视觉引导、力控融合才有靠谱的地基。