(全闭环)FUNC_FullCloseLoop

以下是对深圳市汇川技术有限公司全闭环控制模块（FUNC_MultiBlockPos.c）的逐行分析，结合工业伺服系统全闭环控制原理和代码实现逻辑：

一、核心数据结构与全局变量

STR_FULLCLOSE_LOOP STR_FullCloseLoop;  // 全闭环控制状态机
static int64 InnerEncoderFeedbackSum = 0;  // 内部编码器位置累积值（电机侧）
static int64 ExEncoderFeedbackSum = 0;    // 外部编码器位置累积值（机械侧）
static int32 FC_ExPulseFeedback = 0;      // 外部编码器实时脉冲反馈
static int64 DeltaSum = 0;                // 内外位置偏差累积值

功能：存储全闭环控制的实时状态（反馈模式、编码器参数、位置累积值等）。
关键技术：使用int64保存位置累积值，避免长时运行溢出。

二、核心函数分析

1. 全闭环初始化 (FullCloseLoopInit)

void FullCloseLoopInit(Uint32 EncRev) {STR_FullCloseLoop.FeedbackMode = FunCodeUnion.code.FC_FeedbackMode;  // 反馈模式（1:全闭环, 2:半闭环）STR_FullCloseLoop.ExCoderDir   = FunCodeUnion.code.FC_ExCoderDir;     // 外部编码器方向// 计算外部编码器脉冲到内部位置的转换系数（Q7定点数）int64 ExCoder = A_SHIFT16_PLUS_B(FunCodeUnion.code.FC_ExCoderPulse_H, FunCodeUnion.code.FC_ExCoderPulse_L);STR_FullCloseLoop.ExP2InerPosCoffQ7 = (EncRev << 7) / ExCoder;  // Q7格式：内部脉冲/外部脉冲// 计算内部位置到外部编码器脉冲的转换系数（Q20定点数）STR_FullCloseLoop.ExInnerCoderParaQ20 = (ExCoder << 20) / EncRev;  // Q20格式：外部脉冲/内部脉冲
}

关键参数：
ExP2InerPosCoffQ7：外部脉冲→内部位置的转换系数（高精度定点数）。
ExInnerCoderParaQ20：内部位置→外部脉冲的转换系数（用于偏差计算）。

2. 反馈模式切换 (FullCloseLoopModeSet)

void FullCloseLoopModeSet(int32* pPosFdb, Uint32* pExPosCoefQ7, int32 ExPosFdbAbsValue, Uint8 ExPosFeedbackFlag) {// 计算外部编码器增量脉冲（带方向处理）STR_FullCloseLoop.ExPosFdb = ExPosFdbAbsValue - ExPosFdbAbsValueLast;if (STR_FullCloseLoop.ExCoderDir) STR_FullCloseLoop.ExPosFdb = -STR_FullCloseLoop.ExPosFdb;  // 方向校正// 根据模式选择位置反馈源if ((STR_FullCloseLoop.FeedbackMode == 1) || (ExPosFeedbackFlag == 1)) {  // 全闭环模式*pExPosCoefQ7 = STR_FullCloseLoop.ExP2InerPosCoffQ7;  // 启用外部编码器增益// 双反馈融合：将内部位置转换为外部编码器坐标系*pPosFdb = (STR_FullCloseLoop.ExInnerCoderParaQ20 * STR_FullCloseLoop.InPosFdb) >> 20;} else {  // 半闭环模式*pPosFdb = STR_FullCloseLoop.InPosFdb;  // 直接使用电机编码器反馈*pExPosCoefQ7 = 128;                   // 外部增益置为默认值（等效1.0）}
}

模式逻辑：
全闭环：使用外部编码器（光栅尺）反馈作为位置基准，内部编码器用于辅助补偿。
半闭环：仅依赖电机编码器反馈，外部编码器不参与控制。

3. 偏差检测与报警 (FullCloseLoopErrorCheck)

void FullCloseLoopErrorCheck(Uint8 ExPosFeedbackFlag) {// 累积内外编码器位置（用于长期偏差计算）InnerEncoderFeedbackSum += STR_FullCloseLoop.InPosFdb;      // 电机侧位置累积ExEncoderFeedbackSum    += STR_FullCloseLoop.ExPosFdb;       // 机械侧位置累积// 计算多圈累积偏差（外部编码器坐标系）DeltaSum = ABS(ExEncoderFeedbackSum - ((InnerEncoderFeedbackSum * STR_FullCloseLoop.ExInnerCoderParaQ20) >> 20));// 偏差超限报警（仅全闭环模式生效）if (DeltaSum > (int64)STR_FullCloseLoop.MixCtrlMaxPulse) {PostErrMsg(FULLCLOSEERR);  // 触发全闭环错误InnerEncoderFeedbackSum = ExEncoderFeedbackSum = 0;      // 重置累积值}
}

安全机制：当机械传动误差（如皮带打滑、联轴器断裂）导致内外位置偏差超过阈值（MixCtrlMaxPulse）时，立即触发错误并停机。
工业意义：防止机械损坏和加工事故

4. 双反馈位置融合 (DoublePosFeedbackCal)

int32 DoublePosFeedbackCal(int32 PosRef, int32* PosAmplifErr) {// 计算外部编码器的随动误差STR_FullCloseLoop.ExPosAmplifErr = PosRef - STR_FullCloseLoop.ExPosFdb;// 低通滤波融合内外误差（抑制高频噪声）STR_FullCloseLoop.DoubleFeedLowPassOutput = LowPassFilter(STR_FullCloseLoop.ExPosAmplifErr - *PosAmplifErr);  // 滤波实现略// 动态清零机制：当融合误差接近外部误差时，重置内部误差累积if (FusedError == STR_FullCloseLoop.ExPosAmplifErr) {*PosAmplifErr = 0;  // 清零内部误差ResetLowPassFilter(); // 重置滤波器状态}return FusedError;
}

控制策略：将外部编码器的高精度位置反馈与电机编码器的快速响应结合，通过低通滤波抑制机械振动噪声。
性能优势：提升系统抗干扰能力，适应高速高精度场景（如数控机床）

5. 调试接口 (FullCloseParaDisp)

void FullCloseParaDisp(void) {// 将内部变量映射到上位机可读寄存器FunCodeUnion.code.FC_InnerPulseFeedback_L = InnerEncoderFeedbackSum & 0xFFFF;FunCodeUnion.code.FC_InnerPulseFeedback_H = InnerEncoderFeedbackSum >> 16;FunCodeUnion.code.FC_ExPulseFeedback_L    = FC_ExPulseFeedback & 0xFFFF;FunCodeUnion.code.FC_ExPulseFeedback_H    = FC_ExPulseFeedback >> 16;
}

功能：通过Modbus/TCP协议向上位机暴露关键变量（如FC_ExPulseFeedback），支持实时监控与调试

三、关键技术亮点

1. 定点数运算优化
   使用 Q格式定点数（如ExP2InerPosCoffQ7）替代浮点数，提升实时性。
   例：ExInnerCoderParaQ20 = (ExCoder << 20) / EncRev实现高精度比例系数计算。
2. 全闭环安全防护
   多圈累积偏差检测：防止瞬时干扰误触发，仅当持续偏差超限时报错。
   动态模式切换：支持运行时切换全闭环/半闭环模式，适应不同工况。
3. 低通滤波设计
   自定义整数低通滤波器（DoublePosFeedbackCal），资源占用极低且无浮点依赖。
4. 调试友好性
   关键变量映射到Modbus寄存器（如FC_ExPulseFeedback），可通过上位机实时监控

四、潜在改进方向

1. 振动抑制算法
   注释掉的BateVibrationFunc函数展示了抑振控制框架，可结合模型预测控制（MPC）增强动态性能
2. 滤波器参数自整定
   当前低通滤波时间常数（FC_ExInErrFilterTime）需手动设定，可增加自适应逻辑。
3. 偏差检测优化
   引入移动窗口平均替代多圈累积检测，提升响应速度。

五、调试建议

1. 关键监控变量：

DeltaSum              // 内外位置偏差
STR_FullCloseLoop.ExPosAmplifErr  // 外部编码器随动误差
FULLCLOSEERR          // 全闭环错误标志

2. 测试用例：
   机械打滑模拟：人为偏移外部编码器位置，验证FULLCLOSEERR触发逻辑。
   模式切换测试：运行时切换全闭环/半闭环模式，观察位置反馈平滑性

六、FUNC_FullCloseLoop.h

typedef struct  _STR_FULLCLOSELOOP { // 多段位置功能共用变量Uint16  FeedbackMode;               //lw2012919   编码器反馈模式Uint16  ExCoderDir;                 //lw2012919   外部编码器工作方式//Uint32  ExCoderLine;                //lw2012919   外部编码器线数int64   ExInnerCoderParaQ20;           //lw2012919   电机旋转一圈外部编码器脉冲数Uint32  MixCtrlMaxPulse;            //lw2012919   混合控制偏差最大值Uint16  MixCtrlPulseClr;            //lw2012919   混合控制偏差清除//int16   ExPosFeedbackFlag;        //外部闭环标志int32   ExP2InerPosCoffQ7;       //外环转换至内环速度系数 Q12int32   ExPosAmplifErr;           //外部位置随动偏差int32   ExPosAmplifErrLast;             //外部位置偏差备份int32   InPosFdb;                 //内部位置反馈，用于位置偏差过大判断int32   ExPosFdb;                 //外部位置反馈int32   EncRev;                   //电机1圈编码器反馈脉冲数int32   DoubleFeedLowPassTs;                    //采样时间(us)int32   DoubleFeedLowPassTc;                    //滤波时间(us)int32   DoubleFeedLowPassKa;int32   DoubleFeedLowPassInput;int32   DoubleFeedLowPassOutput;int32   DoubleFeedLowPassOutputLatch;int32   DoubleFeedLowPassRemainder;
}STR_FULLCLOSE_LOOP;	 /* Exported_Variables --------------------------------------------------------*/
/* 可供其它文件调用变量的声明 */
extern STR_FULLCLOSE_LOOP STR_FullCloseLoop;
/* Exported_Functions --------------------------------------------------------*/
/* 可供其它文件调用的函数的声明 */ extern void FullCloseLoopErrorCheck(Uint8 ExPosFeedbackFlag);  //偏差过大保护，打滑保护
//全闭环反馈，转换系数等处理
extern void FullCloseLoopModeSet(int32 * pPosFdb, Uint32 *pExPosCoefQ7, int32 ExPosFdbAbsValue,Uint8 ExPosFeedbackFlag);   
//全闭环初始化，输入电机线数
extern void FullCloseLoopInit(Uint32 EncRev);
//全闭环参数清零
extern void ClearFullLoopVar(void);
//双位置全闭环时位置偏差计算
extern int32 DoublePosFeedbackCal(int32 PosRef,int32 *PosAmplifErr);
//全闭环相关的偏差，位置反馈显示
extern void FullCloseParaDisp(void);extern void FullCloseParaRst(void);

七、全闭环与半闭环系统的核心区别

伺服系统中的全闭环功能是一项能够显著提升设备终端定位精度的先进控制技术。下面这个表格总结了全闭环与半闭环系统的核心区别，帮助你快速抓住要点。

7.1 工作原理与系统构成

全闭环控制系统旨在确保被控对象（如机床工作台）的实际位置与指令期望位置一致。
● 基本工作原理：系统工作时，数控装置或上位控制器发出位置指令。伺服驱动器接收到该指令后，驱动伺服电机旋转。电机通过滚珠丝杠等传动机构，带动工作台移动。关键之处在于，安装在工作台上的光栅尺会实时检测其实际位移，并将此位置信息反馈回驱动器的比较器中。比较器会计算指令位置与实际位置之间的偏差，驱动器则根据这个偏差不断调整输出，驱动电机直至偏差消除为零。这是一个持续的、动态的负反馈调节过程。
● 系统核心组件：一个典型的全闭环系统包含以下关键部件4：
1)伺服电机与驱动器：作为系统的动力源和控制核心。
2)外部位置检测传感器：通常是光栅尺或直接安装在负载上的外部编码器。它们是实现全闭环的“眼睛”，直接监测负载的最终位置。
3)控制器：负责生成指令，并处理反馈信息。

7.2 实现关键与调试要点

实现稳定可靠的全闭环控制，需要注意以下几个环节：
● 正确的方向设置：必须确保外部编码器（光栅尺）的反馈方向与电机的运动方向一致。如果方向设定错误，会导致系统正反馈失控（“飞车”），可能损坏设备。调试时通常先手动推动负载，观察反馈值变化方向是否正确12。
● 精确的参数配置：需要准确设置外部编码器的分辨率等参数，以便系统能正确解读反馈信号。同时，合理设置位置超差保护范围和最大速度限制等参数，是防止误操作引发事故的重要安全措施12。
● 精心的控制器调试：由于全闭环系统直接感知负载端的任何微小振动和变形，其闭环增益（如位置环增益）参数需要比半闭环系统更仔细地调校。若参数设置不当（如增益过高），容易引起系统振荡。现代伺服驱动器通常提供自动整定功能，但手动精细调试往往能获得更优性能。