from collections import deque
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import rcParams
rcParams['font.sans-serif'] = ['SimHei', 'Microsoft YaHei', 'WenQuanYi Micro Hei', 'sans-serif']
rcParams['axes.unicode_minus'] = False
class VelocityController:
def __init__(self, dt=0.004):
"""初始化速度控制器
25US之下
参数:
dt: 控制周期(秒), 默认为4ms
"""
# 控制参数
self.dt = 0.004 # 控制周期(4ms)
# 速度环
self.vel_kp = 18.5 # 速度环比例系数
self.vel_ip = 1 # 速度环积分系数
self.vel_dp = 0.20 # 速度环微分系数
self.vel_kf = 1.2 # 前馈补偿(基于目标变化率)
self.prev_target = 0 # 前馈输入
self.vel_integral = 0 # 累计误差
self.vel_prev_error = 0 #
# 阻尼
self.init_vel = 0 # 用于计算阈值 阈值=|实际速度 - 目标速度| / |初始速度 - 目标速度|
self.overshoot_threshold = 0.49 # 过冲检测阈值 (5%) (数值越小,代表越接近目标,最大是1)
self.adaptive_factor = 0.45 # 自适应调节因子 在这个比例进行衰减,直到衰减系数到0
self.adaptive = False # 自适应 开关
# 加速类型
self.accel_type = "T" # "PID" # or "T"
# 衰减极致
self.vel_min_kp = 6 # 最小的 比例系数,如果衰减到0 将代表没有误差,
self.vel_max_dp = 1 # 最大的 微分系数, 太大滞后严重
# 积分直接 = 0 (过大的比例已经会引起超调,再加入积分将更严重)
self.pid_param = {"p": self.vel_kp,
"i": self.vel_ip,
"d": self.vel_dp,
"f": self.vel_kf,
"adaptive_factor": self.adaptive_factor}
# self.vel_kp = 1.2 # 速度环比例系数
self.max_accel = 10000000 # 最大加速度(mm/s²)
self.max_jerk = 200000000 # 最大加加速度 (m/s³)
self.stop_decel = 7000000.0 # 最大减速度(mm/s²) 受限于最大加速度
# 停止(追踪速度到0)
self.__stopSignal = False # 停止信号
self.stop_start_vel = 0.0
self.stop_phase = "NORMAL" # 减速阶段: NORMAL, DECEL, CRAWL
self.crawl_threshold = 0 # 进入低速爬行模式的速度阈值(m/s)
self.position_tolerance = 0.1 # 位置死区 在速度环中无用
self.velocity_tolerance = 0.1 # 速度死区
self.stopModeMaxVel = 0
self.stopMaxVel_factor = 0.5
self.safety_distance = 2000 # 安全停止距离
# 系统状态
self.actual_pos = 0 # 实际位置(mm)
self.actual_vel = 0.0 # 实际速度(mm/s)
self.target_vel = 0.0 # 目标速度(mm/s)
self.actual_acc = 0.0 # 实际加速度
# self.lock = threading.Lock()
数据记录
self.time_log = np.array([], dtype=np.float32)
self.target_pos_log = np.array([], dtype=np.float32)
self.actual_pos_log = np.array([], dtype=np.float32)
self.target_vel_log = np.array([], dtype=np.float32)
self.actual_vel_log = np.array([], dtype=np.float32)
self.actual_overshoot_log = np.array([], dtype=np.float32)
self.actual_overshoot2_log = np.array([], dtype=np.float32)
@property
def stopSignal(self):
return self.__stopSignal
@stopSignal.setter
def stopSignal(self, value):
if (not self.__stopSignal) and value:
self.target_vel = 0 # 将目标速度设为0
self.reset_pid()
self.stop_phase = "NORMAL"
self.stop_start_vel = abs(self.actual_vel)
# 计算需要的减速度 (v² = u² + 2as, 设s=0.5m为安全距离)
if self.stop_start_vel > 0:
required_decel = int((self.stop_start_vel ** 2) / (2 * self.safety_distance))
self.stop_decel = min(self.max_accel, required_decel)
self.stopModeMaxVel = abs(self.actual_vel) # 衰减最大速度
else:
self.stop_decel = self.max_accel
self.__stopSignal = value
def update_target(self, new_vel=None, **kwargs):
"""动态更新目标位置和速度
参数:
new_vel: 新的目标速度(mm/s), 可选
"""
# 目标发生变化
if self.target_vel != new_vel:
# 记录初始速度
self.init_vel = self.actual_vel
# 重置pid
self.reset_pid()
if new_vel == 0:
self.stopSignal = True
else:
self.stopSignal = False
self.target_vel = new_vel
def set_pid(self, p, i, d, f, adaptive_factor):
"""
速度环 pid 参数
:param p: 速度环比例系数
:param i: 速度环积分系数
:param d: 速度环微分系数
:param f: 前馈补偿(基于目标变化率)
:param adaptive_factor: 自适应调节因子
:return:
"""
self.vel_kp = p # 速度环比例系数
self.vel_ip = i # 速度环积分系数
self.vel_dp = d # 速度环微分系数
self.vel_kf = f # 前馈补偿(基于目标变化率)
self.adaptive_factor = adaptive_factor # 自适应调节因子
self.pid_param = {"p": p, "i": i, "d": d, "f": f, "adaptive_factor": adaptive_factor}
def reset_pid(self):
"""
由于自动增加阻尼,会修改参数,每次更新速度会重置参数。
:return:
"""
self.vel_kp = self.pid_param["p"] # 速度环比例系数
self.vel_ip = self.pid_param["i"] # 速度环积分系数
self.vel_dp = self.pid_param["d"] # 速度环微分系数
self.vel_kf = self.pid_param["f"] # 前馈补偿(基于目标变化率)
self.adaptive_factor = self.pid_param["adaptive_factor"] # 自适应调节因子
def detect_overshoot(self, current_vel, target_vel):
"""检测速度过冲并调整参数"""
if abs(target_vel) > 0.1 or self.adaptive_factor > 0: # 忽略零速度附近
overshoot_ratio = abs(current_vel - target_vel) / abs(target_vel - self.init_vel)
if overshoot_ratio < self.overshoot_threshold:
# self.overshoot_count += 1
# 自适应调整增益
if self.adaptive_factor < 0.01:
self.adaptive_factor = 0
else:
self.adaptive_factor *= 0.9 # 减小10%
# 临时增加阻尼
self.vel_dp *= (1 + self.adaptive_factor) # 增加微分增益(Kd)以抑制超调
self.vel_kp *= (1 - self.adaptive_factor) # 减小比例增益(Kp)降低响应速度
self.vel_dp = min(self.vel_dp, self.vel_max_dp)
self.vel_kp = max(self.vel_kp, self.vel_min_kp)
self.vel_ip = 0 # 积分项直到0
return True
return False
def run_cycle(self, current_time=None):
"""运行一个控制周期"""
# 硬件中的控制算法(模拟行为)
if self.__stopSignal:
# 停止模式 - T型减速
current_vel = abs(self.actual_vel)
# self.actual_vel -= np.sign(self.actual_vel) * self.stop_decel * self.dt
# self.actual_vel = np.clip(self.actual_vel, -self.max_accel, self.max_accel)
# 阶段1: 正常减速 (匀减速)
if self.stop_phase == "NORMAL":
# 应用固定减速度
self.actual_vel -= np.sign(self.actual_vel) * self.stop_decel * self.dt
# 检查是否进入低速爬行阶段
if current_vel <= self.crawl_threshold:
self.stop_phase = "CRAWL"
# 阶段2: 低速爬行 (线性减速到零)
elif self.stop_phase == "CRAWL":
# 计算线性衰减因子
decay_factor = current_vel / self.crawl_threshold
# 应用线性减速
# direction = 1 if self.actual_vel > 0 else -1
self.actual_vel -= np.sign(self.actual_vel) * self.stop_decel * decay_factor * self.dt
# 振荡衰减
if abs(self.actual_vel) > 0.1:
last_maxvel = self.stopModeMaxVel
self.stopModeMaxVel = min(abs(self.actual_vel), self.stopModeMaxVel)
if last_maxvel == self.stopModeMaxVel: # 等幅振荡
# 等幅振荡 等比例衰减
self.stopModeMaxVel *= self.stopMaxVel_factor # 0.5
self.actual_vel = np.clip(self.actual_vel, -self.stopModeMaxVel, self.stopModeMaxVel)
if abs(self.actual_vel) <= self.velocity_tolerance:
self.actual_acc = (self.actual_vel - current_vel) / self.dt
self.actual_vel = 0.0
self.stop_phase = "NORMAL"
arange_dec = int(self.actual_vel * self.dt)
self.actual_pos += arange_dec
return abs(self.actual_vel) <= self.velocity_tolerance, arange_dec, self.actual_vel
elif self.accel_type == "PID":
vel_error = self.target_vel - self.actual_vel
# 检测过冲并自适应调整
if self.adaptive:
self.detect_overshoot(self.actual_vel, self.target_vel)
self.vel_integral += vel_error * self.dt
derivative = (vel_error - self.vel_prev_error) / self.dt
# 控制输出
output = (self.vel_kp * vel_error +
self.vel_ip * self.vel_integral +
self.vel_dp * derivative)
self.vel_prev_error = vel_error
# 前馈通道, 应变目标速度的突变
feedforward = self.vel_kf * (self.target_vel - self.prev_target) / self.dt
output += feedforward
# 模拟加速度生成(带加加速度限制)
# 加速度限制
desired_accel = np.clip(output, -self.max_accel, self.max_accel)
# 加速度变化率限制(确保加速度连续)
jerk = (desired_accel - self.actual_acc) / self.dt
jerk = np.clip(jerk, -self.max_jerk, self.max_jerk)
self.actual_acc += jerk * self.dt
# 更新速度和位置
self.actual_vel += self.actual_acc * self.dt
self.prev_target = self.target_vel
# 更新 位置
arange_dec = int(self.actual_vel * self.dt)
self.actual_pos += arange_dec
return False, arange_dec, self.actual_vel
elif self.accel_type == "T":
# T型加速控制策略
vel_error = self.target_vel - self.actual_vel
# 确定加速度方向和大小
if abs(vel_error) > 0:
# 确定加速度方向(加速或减速)
accel_direction = 1 if vel_error > 0 else -1
# 计算所需加速度(使用最大加速度/减速度)
required_accel = abs(vel_error) / self.dt
target_accel = min(required_accel,
self.max_accel # if accel_direction > 0 else self.max_decel
) * accel_direction
# 更新加速度(带平滑过渡)
if abs(self.actual_acc) < abs(target_accel):
self.actual_acc += accel_direction * self.max_jerk * self.dt
self.actual_acc = np.clip(self.actual_acc,
-self.max_accel,
self.max_accel)
else:
self.actual_acc = target_accel
# 更新速度
self.actual_vel += self.actual_acc * self.dt
# 防止超调
if (vel_error > 0 and self.actual_vel > self.target_vel) or \
(vel_error < 0 and self.actual_vel < self.target_vel):
self.actual_vel = self.target_vel
self.actual_acc = 0.0
else:
self.actual_acc = 0
self.jerk = 0
# 更新 位置
arange_dec = int(self.actual_vel * self.dt)
self.actual_pos += arange_dec
return False, arange_dec, self.actual_vel
else:
# 更新 位置
# arange_dec = self.actual_vel * self.dt
# self.actual_pos += arange_dec
return True, 0, 0
# return False, arange_dec, self.actual_vel
# 死区
# if abs(self.target_vel - self.actual_vel) < self.velocity_tolerance:
# return True, arange_dec, self.actual_vel
# else:
# return False, arange_dec, self.actual_vel
# return arange_dec, self.actual_vel
# # 记录数据
# self.time_log = np.append(self.time_log, current_time)
# self.target_pos_log = np.append(self.target_pos_log, self.actual_pos) # self.target_pos)
# self.actual_pos_log = np.append(self.actual_pos_log, self.actual_acc)
# self.target_vel_log = np.append(self.target_vel_log, self.target_vel) # self.max_vel)
# # self.target_vel_log = np.append(self.target_vel_log, self.dec)
# self.actual_vel_log = np.append(self.actual_vel_log, self.actual_vel)
#
# yield arange_dec
# # return arange_dec
#
# def visualize(self):
# return
# """可视化位置和速度曲线"""
# plt.figure(figsize=(12, 8))
#
# # 位置曲线
# plt.subplot(3, 1, 1)
# plt.plot(self.time_log, self.target_pos_log, 'r-', label='实际位置')
# # plt.plot(self.time_log, self.actual_pos_log, 'b-', label='实际位置')
# plt.xlabel('时间 (s)')
# plt.ylabel('位置 (mm)')
# plt.title('位置跟踪曲线')
# plt.legend()
# plt.grid(True)
#
# plt.subplot(3, 1, 2)
# # plt.plot(self.time_log, self.target_vel_log, 'r-', label='目标速度')
# plt.plot(self.time_log, self.actual_pos_log, 'g-', label='实际加速度')
# plt.xlabel('时间 (s)')
# plt.ylabel('加速度 (mm/s2)')
# plt.title('加速度曲线')
# plt.legend()
# plt.grid(True)
#
#
# # 速度曲线
# plt.subplot(3, 1, 3)
# plt.plot(self.time_log, self.target_vel_log, 'r-', label='目标速度')
# plt.plot(self.time_log, self.actual_vel_log, 'g-', label='实际速度')
# plt.xlabel('时间 (s)')
# plt.ylabel('速度 (mm/s)')
# plt.title('速度跟踪曲线')
# plt.legend()
# plt.grid(True)
#
# plt.tight_layout()
# plt.show() 将图像显示