企业官网建设流程全解析

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

轨迹规划就像是给机械臂写"导航路线"——路线不好，机械臂不是抖就是冲，写好了才能丝滑如舞。

一、轨迹规划：机械臂运动的灵魂

机械臂控制系统里，轨迹规划是真正的核心环节。它决定了机械臂从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 圆心和半径计算

三点确定圆弧的步骤：

1. 由三个点求圆心：三点所在平面确定后，做两条线段的中垂线，交点即圆心
2. 圆心到任意一点的距离即半径 R
3. 计算起点到终点对应的圆弧角度 Δθ
4. 参数化：P(t) = Center + R * [cos(θ_start + t·Δθ), sin(θ_start + t·Δθ)]

// 三点定圆弧 - 简化2D示例typedefstruct{doublex,y;}Point2D;Point2Dcalc_arc_center(Point2D p1,Point2D p2,Point2D p3){doubleax=p2.x-p1.x,ay=p2.y-p1.y;doublebx=p3.x-p1.x,by=p3.y-p1.y;doubled=2.0*(ax*by-ay*bx);if(fabs(d)<1e-6){/* 三点共线，无圆弧 */}doubleux=(by*(ax*ax+ay*ay)-ay*(bx*bx+by*by))/d;doubleuy=(ax*(bx*bx+by*by)-bx*(ax*ax+ay*ay))/d;Point2D center={p1.x+ux,p1.y+uy};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){doubledq=plan->q1-plan->q0;doublesign=(dq>=0)?1.0:-1.0;dq=fabs(dq);// 计算加速时间和减速时间（对称）plan->t_acc=plan->v_max/plan->a_max;plan->t_dec=plan->t_acc;// 加减速阶段走过的距离doubled_acc=0.5*plan->a_max*plan->t_acc*plan->t_acc;doubled_dec=d_acc;// 对称减速// 匀速阶段距离doubled_const=dq-d_acc-d_dec;if(d_const<0){// 无法达到最大速度，三角形规划plan->t_acc=sqrt(dq/plan->a_max);plan->t_dec=plan->t_acc;plan->t_const=0;plan->v_max=plan->a_max*plan->t_acc;}else{plan->t_const=d_const/plan->v_max;}plan->t_total=plan->t_acc+plan->t_const+plan->t_dec;return0;}// 根据时间获取当前位置doubletrapezoidal_eval(TrapezoidalPlan*plan,doublet){doubleq0=plan->q0,sign=(plan->q1>=plan->q0)?1.0:-1.0;doublea=plan->a_max,v=plan->v_max;doubleta=plan->t_acc,tc=plan->t_const,td=plan->t_dec;if(t<=ta){// 加速段: q = q0 + 0.5*a*t^2returnq0+sign*0.5*a*t*t;}elseif(t<=ta+tc){// 匀速段doubled_acc=0.5*a*ta*ta;returnq0+sign*(d_acc+v*(t-ta));}elseif(t<=ta+tc+td){// 减速段doubled_acc=0.5*a*ta*ta;doubled_const=v*tc;doublet_rem=t-ta-tc;returnq0+sign*(d_acc+d_const+v*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.q0=0.0;plan.q1=1.0;// 归一化参数 0→1plan.v_max=1.0/duration_s;plan.a_max=plan.v_max*2.0;// 加速时间设为总时间一半trapezoidal_plan(&plan);inttotal_steps=(int)(plan.t_total*1000/INTERP_PERIOD_MS);for(inti=0;i<=total_steps;i++){doublet=i*INTERP_PERIOD_MS/1000.0;doubles=trapezoidal_eval(&plan,t);// 归一化位置 s∈[0,1]// 末端位置插值Point3D pose;pose.x=start.x+s*(end.x-start.x);pose.y=start.y+s*(end.y-start.y);pose.z=start.z+s*(end.z-start.z);// 逆运动学求解关节角度JointAngles angles=inverse_kinematics(pose);// 发送到伺服驱动servo_send_command(angles);// 等待插补周期usleep(INTERP_PERIOD_MS*1000);}}

七、插补周期选择

插补周期是轨迹规划的核心时间参数，决定每多久算一步。

工业机器人通常选10ms或8ms。嵌入式平台上，10ms 是比较稳妥的选择——在RK3588等ARM平台上，10ms内完成一次逆解+插补计算绰绰有余。

八、常见问题与处理

8.1 奇异点处理

奇异点是逆运动学中关节速度理论上趋于无穷大的位形。典型情况：关节轴线重合，导致末端微小运动需要关节极大速度。

处理方式：

• 规划时避开已知奇异点区域
• 好奇异点附近设置速度衰减因子
• 使用阻尼最小二乘逆解（Damped Least Squares）避免关节速度爆炸

8.2 关节限位检查

每一步插补算出的关节角度，都要检查是否超出物理限位。超出限位意味着机械臂到了物理极限，继续运动会损坏设备。

// 关节限位检查for(inti=0;i<6;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曲线速度规划决定了运动的节奏。代码层面不复杂，但参数调试和异常处理才是工程的真功夫。把这些基础搞清楚，后面接入视觉引导、力控融合才有靠谱的地基。