智能割草机器人刀片启停&转速控制算法 Python

刀片启停&转速控制算法 Python

功能说明

1. 工况联锁停刀：返航回站、坐标越界、机身倾翻、整机急停四类触发立即安全停机

2. 刀片软启动/软停机：斜坡缓升缓降PWM占空比，无冲击启停

3. 负载过载闭环保护：母线电流超限→降速→超时停机三级保护

4. PID恒转速闭环控制：目标转速+编码器实测转速闭环稳速
适配割草机直流无刷刀片驱动，可直接移植C单片机
import time

# ====================== 1.全局配置参数 ======================
# 转速PID参数
KP = 0.35
KI = 0.012
KD = 0.005
MAX_PWM = 100.0    # PWM最大输出百分比 0~100
MIN_PWM = 8.0      # 刀片最低有效驱动PWM
# 软启停配置
SOFT_RAMP_STEP = 0.8    # 每毫秒PWM变化步长
SOFT_DELAY_MS = 1
# 过载保护配置
OVERLOAD_CURR = 12.5    # 过载阈值电流A
SEVERE_OVERLOAD = 16.0  # 严重过载阈值
OVERLOAD_TIMEOUT_S = 1.2# 过载持续超时停机
# 工况安全标志位
SAFE_FLAG = {
   "return_home": False,    # 返航回站
   "pos_outbound": False,   # 坐标越界
   "roll_over": False,      # 机身倾翻
   "emergency_stop": False, # 整机急停
}

class BladeCtrl:
   def __init__(self, target_rpm:float):
       self.tar_rpm = target_rpm
       self.real_rpm = 0.0
       self.pwm_out = 0.0
       # PID缓存
       self.err_last = 0.0
       self.err_sum = 0.0
       # 软启停标记
       self.running_flag = False
       self.soft_stop_flag = False
       # 过载计时
       self.overload_start_t = 0.0
       self.curr_load = 0.0 # 实时负载电流(A)

   # ------------------- PID恒转速计算 -------------------
   def pid_calc(self):
       err = self.tar_rpm - self.real_rpm
       self.err_sum += err
       diff = err - self.err_last
       pid_out = KP*err + KI*self.err_sum + KD*diff
       self.err_last = err
       # PWM限幅
       self.pwm_out += pid_out
       self.pwm_out = max(MIN_PWM, min(MAX_PWM, self.pwm_out))
       return self.pwm_out

   # ------------------- 安全联锁判断：任意故障触发立即软停 -------------------
   def safety_interlock_check(self)->bool:
       """True=需要停机"""
       for v in SAFE_FLAG.values():
           if v:
               return True
       return False

   # ------------------- 过载三级保护逻辑 -------------------
   def overload_protect(self):
       now = time.time()
       if self.curr_load >= OVERLOAD_CURR:
           if self.overload_start_t == 0:
               self.overload_start_t = now
           dur = now - self.overload_start_t
           # 一级过载：临时降速15%
           if self.curr_load < SEVERE_OVERLOAD:
               self.pwm_out = self.pwm_out * 0.85
           # 二级严重过载：大幅降速35%
           else:
               self.pwm_out = self.pwm_out * 0.65
           # 三级超时过载：触发软停机
           if dur >= OVERLOAD_TIMEOUT_S:
               self.running_flag = False
               self.soft_stop_flag = True
       else:
           self.overload_start_t = 0 # 负载恢复清零计时

   # ------------------- 软启动：PWM斜坡缓慢上升 -------------------
   def soft_start(self):
       if self.pwm_out < self.pid_calc():
           self.pwm_out += SOFT_RAMP_STEP
           time.sleep(SOFT_DELAY_MS/1000)
       self.pwm_out = min(self.pid_calc(), MAX_PWM)

   # ------------------- 软停止：PWM斜坡缓慢下降到0 -------------------
   def soft_stop(self):
       if self.pwm_out > 0:
           self.pwm_out -= SOFT_RAMP_STEP
           if self.pwm_out < 0:
               self.pwm_out = 0
           time.sleep(SOFT_DELAY_MS/1000)
       else:
           self.soft_stop_flag = False

   # ------------------- 主控周期调度（1ms周期轮询） -------------------
   def ctrl_loop(self, real_rpm_in:float, load_curr_in:float):
       # 刷新实时采样
       self.real_rpm = real_rpm_in
       self.curr_load = load_curr_in

       # 1.安全联锁优先
       if self.safety_interlock_check():
           self.running_flag = False
           self.soft_stop_flag = True

       # 2.过载保护
       if self.running_flag:
           self.overload_protect()

       # 3.运行：软启动+PID稳速
       if self.running_flag and not self.soft_stop_flag:
           self.soft_start()

       # 4.停机：软停止
       if self.soft_stop_flag:
           self.soft_stop()

       return round(self.pwm_out,2)

   # 外部调用：启动刀片
   def blade_start(self):
       self.running_flag = True
       self.soft_stop_flag = False
       self.err_sum = 0
       self.err_last = 0

   # 外部调用：手动停止刀片
   def blade_stop(self):
       self.running_flag = False
       self.soft_stop_flag = True

# ====================== 业务调用示例 ======================
if __name__ == "__main__":
   # 实例：目标转速3200RPM刀片
   blade = BladeCtrl(target_rpm=3200)

   # 示例1：正常启动运行
   blade.blade_start()
   for _ in range(30):
       pwm = blade.ctrl_loop(real_rpm_in=2900, load_curr_in=7.2)
       print(f"正常运行 PWM:{pwm}% 实测转速:{blade.real_rpm}")

   # 示例2：模拟返航回站触发联锁停刀
   SAFE_FLAG["return_home"] = True
   for _ in range(30):
       pwm = blade.ctrl_loop(real_rpm_in=3100, load_curr_in=7.5)
       print(f"返航联锁停机 PWM:{pwm}%")

   # 示例3：模拟过载工况
   SAFE_FLAG["return_home"] = False
   blade.blade_start()
   for _ in range(50):
       pwm = blade.ctrl_loop(real_rpm_in=3300, load_curr_in=13.8)
       print(f"过载保护 PWM:{pwm}% 负载电流:{blade.curr_load}A")
关键逻辑拆解（对应需求）

1.工况联动停刀

SAFE_FLAG四个标志位由上层导航模块赋值：

• return_home=True：导航回站任务触发→软停刀（你之前返航停刀子逻辑）

• pos_outbound=True：GPS/坐标越界

• roll_over=True：倾角传感器>安全阈值倾翻

• emergency_stop=True：物理急停按键/整机故障

2.软起/软停

通过固定步长阶梯增减PWM，避免瞬间满占空比冲击电机、刀片抖动。

3.三级过载保护

1. 轻度过载(12.5~16A)：输出PWM降85%降载

2. 重度过载(＞16A)：输出PWM降65%强制降速

3. 过载持续＞1.2s：自动进入软停机流程

4.PID恒转速闭环

编码器回传实时转速，动态修正PWM，负载波动时自动补压稳转速。

移植C语言改造要点

1. 把time.sleep替换为单片机硬件定时器1ms中断

2. 浮点可改为定点Q格式节约MCU资源

3. SAFE_FLAG由导航/IMU/限位传感器寄存器赋值