工作之余写点阻抗模式控制算法
这里是工作之余写的一些小算法，实现一些功能。不过这里仅仅是个算法，只能用来了解一下。不能直接拿过来用，
因为这里要涉及到硬件平台的问题，这里的所有算法都是要生成可执行文件在硬件平台上运行的。
这里版权所有，不能用于非法用途，否则责任自负。转载的话，请注明转载出处！！！
/// <summary> /// 表示阻抗控制的控制算法。 /// </summary> public static class AlgorithmImpedance { #region 1.成员 /// <summary> /// 计算结果（控制量）。 /// </summary> private static ControlResult result; /// <summary> /// 运行开始时间 /// </summary> private static DateTime startTime; /// <summary> /// 开始治疗时间 /// </summary> public static DateTime startCureTime; /// <summary> /// 首次运行标志。 /// </summary> private static bool isFirstRun; /// <summary> /// 冷却时间。 /// </summary> private static double coolingTime; /// <summary> /// 每次增加功率的步长。 /// </summary> private static double powerStep; /// <summary> /// 每次增加功率的时间步长。 /// </summary> private static double timeStep; /// <summary> /// 冷却标志，每次治疗前均需冷却一段时间。 /// </summary> private static bool isCooling; /// <summary> /// 阻抗模式下检测阻抗斜率时间的开始值 /// </summary> private static DateTime dtStart; /// <summary> /// 阻抗的斜率 /// </summary> public static Slope slope; /// <summary> /// 阻抗差值的一阶导，阻抗差值的斜率 /// </summary> public static Slope K1Slope; /// <summary> /// 斜率的一阶导，阻抗差值的两阶导 /// </summary> public static Slope K2Slope; //台阶法。 private static Step step = new Step(); /// <summary> /// 通过计算得到的实时电压值 /// </summary> private static double VoltageValue; /// <summary> /// 电压转换为DA后，调整功率匹配的DA值步长 /// </summary> private static double DAStep; /// <summary> /// 认为匹配功率值的绝对值范围 /// </summary> private static double PowerAbs; /// <summary> /// 功率下降下限 /// </summary> private static double MinPower; /// <summary> /// 稳定阻抗时初始功率 /// </summary> private static double P0; /// <summary> /// 当前输出的功率 /// </summary> public static double P; /// <summary> /// PID 系数1 /// </summary> private static double S1; /// <summary> /// PID 系数2 /// </summary> private static double S2; #region 1. 阻抗法治疗参数设置属性 #region 功率阶梯上升参数设置 /// <summary> /// 阶梯功率起始值 /// </summary> private static double PMin; /// <summary> /// 阶梯功率终点值 /// </summary> public static double PMax; /// <summary> ///阶梯功率上升时间 /// </summary> private static double TRise; #endregion #region 负载阻抗曲线斜率检测参数设置 /// <summary> /// 采样时间间隔 /// </summary> private static double Tsample; /// <summary> /// 采样队列长度 /// </summary> private static int PNum; //斜率阈值 private static double Kthreshod; #endregion #region 负载阻抗超出额定范围参数设置 /// <summary> /// 目标阻抗 /// </summary> private static double RMax; /// <summary> /// 允许超出目标阻抗的范围 /// </summary> private static double MaxOverRating; /// <summary> /// 超出最大阻抗的处理方法 /// </summary> private static bool isOverLoadDealing = true; #endregion #region 射频消融最后时段功率快速提升参数设置 /// <summary> /// 冲刺时间长度（单位：S） /// </summary> private static double EndTime; /// <summary> /// 冲刺的目标功率值 /// </summary> private static double EndPullPower; #endregion #region 负载阻抗斜率平缓控制参数设置 //阈值斜率1 private static double K1; //阈值斜率2 private static double K2; //阈值斜率3 private static double K3; //阈值斜率4 private static double K4; private static double K5; //阈值斜率6 private static double K6; /// <summary> /// DA最低值 /// </summary> public static double DAValue; /// <summary> /// 开始治疗到结束治疗前实际治疗时间 /// </summary> private static double timeTreatment; #endregion #endregion #endregion #region 2.属性 /// <summary> /// 获取当前算法计算的结果（控制量）。 /// </summary> public static ControlResult Result { private set { result = value; } get { return result; } } /// <summary> /// 计算一次阻抗斜率（PNum个点）的时间 /// </summary> private static double RunTime { get { return (DateTime.Now - dtStart).TotalSeconds; } } /// <summary> /// 温度采样值。 /// </summary> private static double AcqTemperature { get { return ControlAlgorithm.AcqTemperature; } } /// <summary> /// 阻抗采集值。 /// </summary> private static double AcqImpedance { get { return ControlAlgorithm.AcqImpedance; } } /// <summary> /// 功率采集值。 /// </summary> private static double AcqPower { get { return ControlAlgorithm.AcqPower; } } /// <summary> /// 阻抗模式下各个参数的调试信息 /// </summary> private static string DebugInf { set { ControlAlgorithm.DebugInf = value; } get { return ControlAlgorithm.DebugInf; } } #endregion #region 3.函数 /// <summary> /// 初始化。 /// </summary> public static void Initialize() { result = new ControlResult(); isFirstRun = true; startTime = new DateTime(); startCureTime = new DateTime(); isCooling = true; isOverLoadDealing = true; slope = new Slope(PNum); K1Slope = new Slope(100); K2Slope = new Slope(100); GetRegistry(); } /// <summary> /// 读取注册表中阻抗模式下的参数值 /// </summary> public static void GetRegistry() { PMin = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "P_Min", "8")); PMax = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "P_Max", "35")); TRise = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "T_Rise", "600")); Tsample = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "T_sample", "1")); PNum = Convert.ToInt32(RegistryFunctions.GetRegistry("HGCF3000", "P_Num", "20")); Kthreshod = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "K_threshod", "0.577")); RMax = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "RMax", "200")); MaxOverRating = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "Max_OverRating", "20")); P0 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "P0", "30")); S1 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "S1", "0")); S2 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "S2", "0")); EndTime = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "End_Time", "180")); EndPullPower = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "End_PullPower", "40")); DAStep = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "DAStep", "0.05")); PowerAbs = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "Abs", "2")); MinPower = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "MinPower", "15")); K1 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "K1", "0")); K2 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "K2", "0.577")); K3 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "K3", "0.788")); K4 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "K4", "1.0")); K5 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "K5", "1.366")); K6 = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "K6", "1.732")); DAValue = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "DAValue", "0.875")); coolingTime = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "Cooling_Time", "5")); powerStep = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "Power_Step", "0.5")); timeStep = Convert.ToDouble(RegistryFunctions.GetRegistry("HGCF3000", "Time_Step", "5")); } /// <summary> /// 根据当前的运行状况（采样值）进行计算。 /// </summary> public static void Control(object sender, EventArgs e) { //是否开始运行 if (isFirstRun) { isFirstRun = false; startTime = DateTime.Now; dtStart = DateTime.Now; startCureTime = DateTime.Now; Result.PowerSwitchRFSource = false; Result.OutputPower = PMin; } //每隔RunTime计算一下阻抗的斜率（20个点） slope.Add(new double[] { RunTime, AcqImpedance }); K1Slope.Add(new double[] { RunTime, RMax-AcqImpedance }); K2Slope.Add(new double[] { RunTime, K1Slope.KValue }); //当前已运行多长时间 double time = (DateTime.Now - startTime).TotalSeconds; //当处于冷却阶段时 if (isCooling) { //超过冷却时间后，重置计时，并关闭冷却标志、打开射频源。 if (time >= coolingTime) { startTime = DateTime.Now; isCooling = false; Result.PowerSwitchRFSource = true;//开射频 } } //并非处于冷却阶段时 else { //每隔Tsample2秒钟计算一下阻抗的斜率（20个点） if (RunTime >= Tsample) { //如果当前时间大于时间步长，开始加功率步长 if (time >= timeStep) { startTime = DateTime.Now; ControlImpedance(); } timeTreatment = (DateTime.Now - startCureTime).TotalSeconds; PowerSet(); Power(); } } Debug(); VoltageValue = Math.Pow(AcqImpedance * Result.OutputPower, 0.5); } /// <summary> /// PID算法功率输出 /// </summary> public static void PID() { //P0 = 30; if (K1Slope.KValue >= 10 || K1Slope.KValue <= -10) { //todo } else if (K2Slope.KValue >= 10 || K2Slope.KValue <= -10) { //todo } else { P = P0 + S1 * K1Slope.KValue + S2 * K2Slope.KValue; } if (P >= 50) { P = 50; } else if (P<=20) { P = 20; } Result.OutputPower = P; Power(); } /// <summary> /// 台阶控制阻抗 /// 吴新强 2013年10月12日15:48:45 /// </summary> public static void ControlImpedance() { if (Math.Abs(RMax - AcqImpedance)<=MaxOverRating) { PID(); } else if (RMax - MaxOverRating > AcqImpedance) { if (Result.OutputPower <= PMax - powerStep) { Result.OutputPower += powerStep; } else { Result.OutputPower = PMax; } //斜率小于K1时（默认为0）2倍的功率步长加功率 if (slope.KValue < -10) { Result.OutputPower +=5* powerStep; } else if (slope.KValue < -5) { Result.OutputPower += 4 * powerStep; } else if (slope.KValue < -1.0) { Result.OutputPower += 3 * powerStep; } else if (slope.KValue < K1) { Result.OutputPower += 2 * powerStep; } else if (slope.KValue < K2)//斜率小于K2时（默认为0.268、0.577）1倍的功率步长加功率 { Result.OutputPower += powerStep; } else if (slope.KValue < K3)//斜率小于K3时（默认为0.423）1倍的功率步长降功率 { //Result.OutputPower -= 4; Result.OutputPower -= powerStep; if (Result.OutputPower <= MinPower) { Result.OutputPower = MinPower; } } else if (slope.KValue < K4)//斜率小于K3时（默认为0.577）1倍的功率步长降功率 { //Result.OutputPower -= 4; Result.OutputPower -= 2 * powerStep; if (Result.OutputPower <= MinPower) { Result.OutputPower = MinPower; } } else if (slope.Value < K5) // //斜率大于K3时（默认为0.683）2倍的功率步长降功率 { Result.OutputPower -= 3 * powerStep; if (Result.OutputPower <= MinPower) { Result.OutputPower = MinPower; } } //斜率小于K3时（默认0.788）1倍的功率步长降功率 else if (slope.KValue < K6) { Result.OutputPower -= 4 * powerStep; if (Result.OutputPower <= MinPower) { Result.OutputPower = MinPower; } } else { Result.OutputPower -= 5 * powerStep; if (Result.OutputPower <= MinPower) { Result.OutputPower = MinPower; } } } else if (AcqImpedance >RMax + MaxOverRating) { //斜率小于K1时（默认为0）2倍的功率步长加功率 if (slope.KValue < -10) { Result.OutputPower +=4* powerStep; } else if (slope.KValue < -5) { Result.OutputPower += 3* powerStep; } else if (slope.KValue < -1.0) { Result.OutputPower += 2 * powerStep; } else if (slope.KValue < K1) { Result.OutputPower += powerStep; } else if (slope.KValue < 0.1) { } //斜率小于K2时（默认为0.12、0.577）1倍的功率步长加功率 else if (slope.KValue < K2) { Result.OutputPower -= powerStep; } //斜率小于K3时（默认为0.268）1倍的功率步长降功率 else if (slope.KValue < K3) { //Result.OutputPower -= 4; Result.OutputPower -=powerStep; if (Result.OutputPower <= MinPower) { Result.OutputPower = MinPower; } } //斜率小于K3时（默认为0.577）1倍的功率步长降功率 else if (slope.KValue < K4) { //Result.OutputPower -= 4; Result.OutputPower -=powerStep; if (Result.OutputPower <= MinPower) { Result.OutputPower = MinPower; } } /* * else if (slope.Value < K5) // //斜率大于K3时（默认为0.683）2倍的功率步长降功率 { Result.OutputPower -= 2 * powerStep; if (Result.OutputPower <= MinPower) { Result.OutputPower = MinPower; } } */ //斜率小于K3时（默认0.788）1倍的功率步长降功率 else if (slope.KValue < K6) { Result.OutputPower -= 2 * powerStep; if (Result.OutputPower <= MinPower) { Result.OutputPower = MinPower; } } else { Result.OutputPower -= 3 * powerStep; if (Result.OutputPower <= MinPower) { Result.OutputPower = MinPower; } } if (Result.OutputPower >= MinPower + 2 * powerStep) { Result.OutputPower -= 2 * powerStep; } else { Result.OutputPower = MinPower; } } else { //PID(); } Power(); } /// <summary> /// 功率匹配(使采集的功率（AcqPower）达到计算输出的功率值（Result.OutputPower）) /// 吴新强 2013年10月12日15:49:55 /// </summary> public static void Power() { if (Math.Abs(Result.OutputPower - AcqPower) >= PowerAbs) { if (Result.OutputPower > AcqPower) { DAValue += DAStep; if (DAValue >= 2.5) { DAValue = 2.5; } } else { DAValue -= DAStep; if (DAValue <= 0.5) { DAValue = 0.5; } } } else { //todo } } /// <summary> /// 功率函数（把U^2=P*R,计算得到的电压值转化为DA值作为射频源电压输出） /// </summary> public static void PowerSet() { double daValue = (double)0; daValue = Math.Pow(Result.OutputPower * AcqImpedance, 0.5); Result.OutputVoltageRFSource = ControlAlgorithm.ConvertRFVolt2DAValue(daValue); //设置DA值的最低值 if (Result.OutputVoltageRFSource <= DAValue) Result.OutputVoltageRFSource = DAValue; } /// <summary> /// 最后阶段功率冲刺 /// </summary> public static void PowerSprint() { if (AcqImpedance < MaxOverRating && AcqPower < PMax) { Result.OutputPower *= 0.5; } else if (Result.OutputPower >= PMax) { Result.OutputPower = PMax; } } /// <summary> /// 台阶功率控制阻抗斜率 /// </summary> public static void AdjustPower() { if (AcqImpedance < RMax && AcqPower < PMax) //if (AcqImpedance < 300 && AcqPower < 40) { //斜率小于K1时（默认为0）2倍的功率步长加功率 if (slope.KValue < -10) { Result.OutputPower *= 2; } else if (slope.KValue < -5) { Result.OutputPower += 10 * powerStep; } else if (slope.KValue < -1.0) { Result.OutputPower += 6 * powerStep; } else if (slope.KValue < K1) { Result.OutputPower += 4 * powerStep; } //斜率小于K2时（默认为0.268、0.577）1倍的功率步长加功率 else if (slope.KValue < K2) { Result.OutputPower += powerStep; } //斜率小于K3时（默认为0.423）1倍的功率步长降功率 else if (slope.KValue < K3) { //Result.OutputPower -= 4; Result.OutputPower -= powerStep; if (Result.OutputPower <= 10) { Result.OutputPower = 10; } } //斜率小于K3时（默认为0.577）1倍的功率步长降功率 else if (slope.KValue < K4) { //Result.OutputPower -= 4; Result.OutputPower -=2* powerStep; if (Result.OutputPower <= 10) { Result.OutputPower = 10; } } else if (slope.Value < K5) // //斜率大于K3时（默认为0.683）2倍的功率步长降功率 { Result.OutputPower -= 3 * powerStep; if (Result.OutputPower <= 10) { Result.OutputPower = 10; } } //斜率小于K3时（默认0.788）1倍的功率步长降功率 else if (slope.KValue < K6) { Result.OutputPower -= 4 * powerStep; if (Result.OutputPower <= 10) { Result.OutputPower = 10; } } else { Result.OutputPower -= 5 * powerStep; if (Result.OutputPower <= 10) { Result.OutputPower = 10; } } } else if (AcqImpedance >= RMax + MaxOverRating) { if (slope.Value >= K3) { Result.OutputPower -=5*powerStep ; if (Result.OutputPower <= 10) { Result.OutputPower = 10; } } else if (slope.Value >= K2) { Result.OutputPower -=3*powerStep; if (Result.OutputPower <= 10) { Result.OutputPower = 10; } } else { //todo } } else if (Result.OutputPower >= PMax) { Result.OutputPower = PMax; } Power(); } /// <summary> /// 界面显示参数调试信息 /// </summary> public static void Debug() { double setTime = ControlAlgorithm.CureTime * 60; DebugInf += "<AlgorithmImpedance>治疗总时间:" + setTime.ToString() + " S\r\n"; DebugInf += "<AlgorithmImpedance>已经治疗时间:" + timeTreatment.ToString("0.00") + " S\r\n"; DebugInf += "<AlgorithmImpedance>输出设置电压值:" + VoltageValue.ToString("0.000") + " V\r\n"; DebugInf += "<AlgorithmImpedance>输出电压值:" + Result.OutputVoltageRFSource.ToString("0.00") + " V\r\n"; DebugInf += "<AlgorithmImpedance>输出功率值:" + Result.OutputPower.ToString("0.0") + " W\r\n"; DebugInf += "<AlgorithmImpedance>PID功率值:" + P.ToString("0.0") + " W\r\n"; DebugInf += "<AlgorithmImpedance>功率最大值:" + PMax.ToString("0.00") + " W\r\n"; DebugInf += "<AlgorithmImpedance>功率最小值:" + PMin.ToString("0.0") + " W\r\n"; DebugInf += "<AlgorithmImpedance>功率下限值:" + MinPower.ToString("0.0") + " W\r\n"; DebugInf += "<AlgorithmImpedance>目标阻抗:" + RMax.ToString("0.0") + " Ω\r\n"; DebugInf += "<AlgorithmImpedance>采集阻抗:" + AcqImpedance.ToString("0.00") + " Ω\r\n"; DebugInf += "<AlgorithmImpedance>斜率值:" + slope.KValue.ToString("0.000") + "\r\n"; DebugInf += "<AlgorithmImpedance>斜率的一阶导:" + K1Slope.KValue.ToString("0.000") + "\r\n"; DebugInf += "<AlgorithmImpedance>斜率的二阶导:" + K2Slope.KValue.ToString("0.000") + "\r\n"; DebugInf += "<AlgorithmImpedance>S1:" + S1.ToString("0.000") + "\r\n"; DebugInf += "<AlgorithmImpedance>S2:" + S2.ToString("0.000") + "\r\n"; DebugInf += "<AlgorithmImpedance>时间步长:" + timeStep.ToString("0.0") + "S\r\n"; DebugInf += "<AlgorithmImpedance>功率步长:" + powerStep.ToString("0.0") + "W\r\n"; } #endregion }