工业机器人关节电机编程：实现高精度的位置环与速度环协同控制
关节电机不是“给个目标角度让电机转过去”就完事。机器人臂对轨迹跟踪的要求是：到位准、换向不抖、负载变也不飘。工业做法都是三环级联——电流环（内）→速度环（中）→位置环（外），位置环只决定“去哪”，速度环管“跑多稳”，电流环管“出多大劲”。 协同的核心不在单环增益高，而在外环别拖垮内环、前馈把大部分误差提前消掉。
一、三环分工与周期层级
典型周期配置：
电流环：10~20kHz（50~100μs），FOC算Iq/Id
速度环：1~3kHz，PI输出Iq指令
位置环：500Hz~1kHz，纯P或PD输出速度指令
内环带宽必须高于外环，否则外环一变内环还没跟上，系统直接振荡。 调试顺序固定“先电流→再速度→最后位置”，反过来必震。
二、反馈来源：关节端+电机端
单编码器方案容易栽在减速器回差上：
位置环反馈 → 关节端绝对编码器（末端实际角度）
速度环反馈 → 电机端编码器差分转速，或关节角度微分
高端方案双编码器：位置精度靠关节端，动态阻尼靠电机端
绝对编码器上电免回零，多圈靠磁编SSI/SPI或电池备ABZ计数。
三、位置环：纯P+前馈，别加I
位置环加积分是常见误区——到位后会过冲来回摆。工程做法：
typedef struct {
float kp; // 位置比例
float kff_v; // 速度前馈系数
float v_max; // 输出速度限幅
float pos_fb;
} pos_loop_t;
float pos_loop_step(pos_loop_t*p, float pos_ref, float vel_ref){
float e = pos_ref - p->pos_fb;
float v_cmd = p->kp * e + p->kff_v * vel_ref; // P+前馈
if(v_cmd > p->v_max) v_cmd = p->v_max;
if(v_cmd < -p->v_max) v_cmd = -p->v_max;
return v_cmd; // 交给速度环
}
vel_ref不是自己差分出来的，是轨迹规划器直接给的——多项式/S曲线规划同时出位置、速度、加速度，事后差分必带噪。
四、速度环：PI+抗饱和
速度环输出Iq指令进FOC电流环：
typedef struct {
float kp, ki;
float integ;
float iq_max;
float v_fb;
} vel_loop_t;
float vel_loop_step(vel_loop_t*v, float v_cmd, float v_fb){
float e = v_cmd - v_fb;
v->integ += e;
/* 抗饱和：限幅前先钳积分 */
float iq = v->kp*e + v->ki*v->integ;
if(iq > v->iq_max){
iq = v->iq_max;
v->integ -= e; // 回退本次积分
}else if(iq < -v->iq_max){
iq = -v->iq_max;
v->integ -= e;
}
return iq;
}
抗饱和必须做，否则位置环给个大额速度指令、速度环饱和几秒，积分项堆满，退出饱和时电机猛冲一下。
五、主调度：三层周期解耦
void motor_1ms_task(void){ // 位置环
float pos = joint_abs_pos();
float vref = traj_vel();
float vcmd = pos_loop_step(&pl, traj_pos(), vref);
set_vel_cmd(vcmd);
}
void motor_250us_isr(void){ // 速度环
float vfb = enc_motor_speed();
float iq_ref = vel_loop_step(&vl, get_vel_cmd(), vfb);
set_iq_ref(iq_ref);
}
void foc_current_isr(void){ // 电流环
foc_step(); // Clark/Park -> PI -> SVPWM
}
位置环写映像变量vel_cmd，速度环读；速度环写iq_ref，电流环读。中断层级别解耦，别在位置环里直接算Iq。
六、前馈进阶：加速度/重力补偿
垂直关节要扛重力，纯反馈P会留稳态误差。加速度前馈叠加：
iq_ff = Kt * (J*acc_ref + T_grav(theta) + T_fric(vfb));
iq_cmd = vel_loop_step(...) + iq_ff;
重力项按关节角查表，摩擦项库仑+粘滞。前馈走并联加法、最后一起限幅，别单独限幅前馈。
七、换向回差处理
减速器有背隙时，位置环用关节反馈会在过零点看到误差跳变。做法：
位置环反馈用关节端（保证终点准）
速度环反馈用电机端（保证阻尼稳）
前馈承担主要跟踪，反馈P不用调太高
回差大就换零背隙谐波/RV，别全靠增益硬压
八、安全兜底
软件限位角+硬件限位开关双重
垂直轴断电抱闸，使能前先松闸
位置误差超阈值→触发急停零力矩
编码器失效/CRC错→切安全态
速度指令做加速度限幅，别一步给满
过流FAULT进中断，立刻清Iq输出
九、落坑清单
位置环加I→定位过冲摆动
先调位置环再调电流环→低频振荡
速度反馈靠位置硬差分→噪声放大
前馈单独限幅→补偿被砍歪
抗饱和不做→退出饱和猛冲
单编码器走电机反馈→回差消不掉
vel_ref自己差分→轨迹抖
外环周期等于内环→带宽打架失稳
垂直轴不加重力前馈→稳态下垂
抱闸时序错→上电掉臂
十、落地顺序
FOC电流环先跑稳，Iq阶跃几百微秒跟住
速度环禁位置环，阶跃速度调PI+抗饱和
位置环纯P，Kp从小加，观察超调
轨迹规划器输出pos/vel/acc三段
速度前馈接上，Kp可适当收小
垂直轴加重力/摩擦前馈
关节端位置+电机端速度双反馈
限位/抱闸/FAULT兜底
关节电机高精度定位的本质是“外环下指令、内环快执行、前馈扛大头”。位置环别碰积分，速度环必做抗饱和，前馈从轨迹规划拿而不是差分出来，反馈源按“位置看关节、速度看电机”分开——这四件事做对，协作臂重复定位到±0.05mm级是水到渠成的事。剩下就是机械回差和编码器分辨率，那是硬件底线，软件增益救不回来。
下载文档
