从C++代码看机器人控制:一个工业级PVT控制器的深度解析

tauDes = PV_enable[i] * pvt_Kp[i]*(motor_pos_des[i]-motor_pos_cur[i])          + PV_enable[i]*pvt_Kd[i]*(motor_vel_des[i]-motor_vel[i]);tauDes = tau_out_lpf[i].ftOut(tauDes) + motor_tor_des[i];

// 1. 力矩饱和保护if (fabs(tauDes) >= fabs(maxTor[i]))    tauDes = sign(tauDes)*maxTor[i];// 2. 关节限位保护（在构造函数中从JSON加载）maxPos[i] = root_read[motorName[i]]["maxPos"].asDouble();minPos[i] = root_read[motorName[i]]["minPos"].asDouble();// 3. 速度限制maxVel[i] = root_read[motorName[i]]["maxSpeed"].asDouble();// 4. 增量位置限制（在重载的calMotorsPVT中）if (fabs(delta) >= fabs(deltaP_Lim))    delta = deltaP_Lim * sign(delta);

tau_out_lpf.assign(jointNum, lpf_fst());double fc = root_read[motorName[i]]["PVT_LPF_Fc"].asDouble();tau_out_lpf[i].setPara(fc, timeStepIn);tauDes = tau_out_lpf[i].ftOut(tauDes) + motor_tor_des[i];

voidpvt_ctrl::dataBusRead(DataBus &busIn);voidpvt_ctrl::dataBusWrite(DataBus &busIn);

pvt_ctrl::pvt_ctrl(double timeStepIn, const char *jsonPath)

voidpvt_ctrl::calMotorsPVT(double deltaP_Lim){    double delta = motor_pos_des[i] - motor_pos_des_old[i];    if (fabs(delta) >= fabs(deltaP_Lim)) {        delta = deltaP_Lim * sign(delta);    }    double pDes = delta + motor_pos_des_old[i];    // ... 使用pDes而非原始motor_pos_des[i]}

• 案例2：Boston Dynamics Atlas (动态平衡)

// 1. 增加摩擦力前馈补偿double frictionComp = coulombFric[i] * tanh(vel/velThreshold)                      + viscousFric[i] * vel;// 2. 自适应抗饱和（处理积分饱和）if (torqueSaturated && errorSameSign) {    // 暂停积分累积}// 3. 基于模型的前馈tau_ff = M(q_des)·q̈_des + C(q_des, q̇_des)·q̇_des + G(q_des);