割草机器人多维度安全防护检测算法Python

割草机器人多维度安全防护检测算法Python

◦ 包含：碰撞检测、离地检测、翻倒检测、人体触碰急停、高温检测

◦ 逻辑：触发任意安全条件，立即停刀+停机
功能说明

实现碰撞、离地、翻倒、人体触碰、高温5重安全检测，任一异常触发：刀盘停机+整机动力停机，适配算法逻辑分层设计，模块化易移植单片机/嵌入式。
import time

# ====================== 安全阈值配置(硬件标定参数) ======================
SAFE_TEMP_MAX = 75.0        # 电机/主控高温阈值℃
ACC_GROUND_DOWN = 0.3       # 离地判定：Z轴加速度小于该值判定离地
ROLL_MAX_ANGLE = 45.0       # 翻滚倾角阈值(度)，大于则翻倒
COLLIDE_FORCE_MIN = 80      # 碰撞压力传感器阈值
TOUCH_SIGNAL_LEVEL = 1      # 人体触碰传感器电平标识

# 机器人状态全局标识
is_knife_running = True     # 刀盘运转标记
is_robot_running = True     # 整车行走标记

# ====================== 传感器数据模拟采集函数(对接真实ADC/IMU) ======================
def get_sensor_data():
   """模拟实时读取：碰撞压力、离地Z加速度、翻滚倾角、触碰信号、温度"""
   import random
   data = {
       "collision_force": random.randint(20, 120),    # 碰撞传感器压力
       "z_acc": round(random.uniform(0.1, 0.9),2),     # IMU-Z轴加速度(g)
       "roll_angle": round(random.uniform(0,60),1),   # 横滚倾角
       "touch_signal": random.randint(0,1),           # 0无触碰 1人体触碰
       "chip_temp": round(random.uniform(40,85),1)    # 主控+刀盘电机温度
   }
   return data

# ====================== 安全停机执行函数 ======================
def emergency_stop():
   global is_knife_running, is_robot_running
   if is_knife_running:
       is_knife_running = False
       print("【执行急停】刀盘电机断电停转")
   if is_robot_running:
       is_robot_running = False
       print("【执行急停】行走驱动整机停机")

# ====================== 分项安全检测算法 ======================
def check_all_safety(sensor):
   err_list = []
   # 1.碰撞检测：压力超阈值
   if sensor["collision_force"] >= COLLIDE_FORCE_MIN:
       err_list.append("碰撞触发")
   # 2.离地检测：Z轴加速度过小，轮子悬空
   if sensor["z_acc"] <= ACC_GROUND_DOWN:
       err_list.append("机身离地")
   # 3.翻倒检测：横滚角超安全倾角
   if sensor["roll_angle"] >= ROLL_MAX_ANGLE:
       err_list.append("机身翻倒")
   # 4.人体触碰检测：触碰传感器高电平
   if sensor["touch_signal"] == TOUCH_SIGNAL_LEVEL:
       err_list.append("人体触碰")
   # 5.高温检测：温度超标
   if sensor["chip_temp"] >= SAFE_TEMP_MAX:
       err_list.append("硬件高温")

   # 任意故障触发立即急停
   if len(err_list) > 0:
       print(f"\n====安全异常：{err_list}====")
       emergency_stop()
       return False
   print("全部安全，正常运行")
   return True

# ====================== 主循环(机器人实时巡检，100ms周期) ======================
if __name__ == "__main__":
   print("割草机器人多重安全防护系统启动")
   try:
       while True:
           sen_data = get_sensor_data()
           print(f"\n实时传感：{sen_data}")
           check_all_safety(sen_data)
           time.sleep(0.1) # 100ms巡检周期，适配嵌入式采样速率
   except KeyboardInterrupt:
       print("\n系统正常退出")
算法架构&移植说明

1.五大检测逻辑

1. 碰撞检测：防撞环压力传感器采样，压力＞阈值判定撞障碍物

2. 离地检测：IMU三轴加速度Z轴采集，离地后重力分量骤降

3. 翻倒检测：陀螺仪解算横滚角，倾角＞45°判定侧翻

4. 人体触碰：机身周圈电容式触碰传感器，高电平即人体接触

5. 高温检测：NTC热敏电阻采集电机/主控温度，超温停机

2.嵌入式C语言改造要点（适配STM32/ESP32主控）

1. 替换get_sensor_data()为ADC读取、I2C读取MPU6050陀螺仪硬件接口

2. 主循环替换为while(1)，采用定时器100ms定时中断巡检安全

3. emergency_stop()直接操作GPIO引脚关断刀盘MOS管、行走驱动芯片

3.算法优化思路（软件滤波防误触）

• 连续3次采样异常才触发停机，消除传感器抖动误判

• 温度采用一阶RC滤波算法平滑采样值

• 倾角采用互补滤波优化IMU数据，颠簸路面防误翻倒

补充C语言嵌入式版本代码

一、STM32嵌入式C版本｜割草机器人5重安全防护算法（可直接烧录，适配MPU6050+NTC+电容触碰+防撞开关）

设计要点

1. 5项安全：碰撞/离地/翻倒/人体触碰/高温，任一满足立刻关刀盘+行走电机

2. 软件防抖：连续N帧异常才急停，抑制传感器杂波误触发

3. 100ms定时周期安全巡检，标准嵌入式架构
#include "stm32f10x.h"
#include

/*********************参数配置区【硬件标定修改此处】********************/
#define SAFE_TEMP_MAX       75.0f    //最高安全温度℃
#define Z_ACC_THRESHOLD     0.3f     //离地Z轴加速度阈值(g)
#define ROLL_ANGLE_MAX     45.0f     //翻倒倾角阈值°
#define COLLIDE_THRESHOLD   80U      //碰撞压力阈值
#define TOUCH_VALID        1U        //触碰有效电平
#define FILTER_CNT          3U       //连续3次异常才停机，防抖
/**********************************************************************/

//传感器结构体
typedef struct{
   uint16_t collide_force;    //碰撞压力
   float    z_acc;            //IMU Z轴加速度 g
   float    roll_angle;       //横滚倾角 °
   uint8_t  touch_level;      //人体触碰 0/1
   float    temp;             //温度℃
}Sensor_t;

//故障计数缓存
typedef struct{
   uint8_t collide_err;
   uint8_t ground_err;
   uint8_t roll_err;
   uint8_t touch_err;
   uint8_t temp_err;
}ErrFilter_t;

ErrFilter_t err_filter = {0};
Sensor_t sensor_data;

/*硬件驱动声明 对接底层IO/ADC/I2C*/
void Read_Sensor(Sensor_t *p_sen);    //读取所有传感器
void Stop_Knife_Motor(void);          //停止刀盘
void Stop_Walk_Motor(void);           //停止行走
void TIM1_Init_100ms(void);           //100ms定时中断

//全局停机标志
uint8_t sys_emerg_stop = 0;

/****************安全检测+滤波算法****************/
uint8_t Safety_Check(Sensor_t *p_sen)
{
   uint8_t is_abnormal = 0;
   //1碰撞检测
   if(p_sen->collide_force >= COLLIDE_THRESHOLD){
       err_filter.collide_err++;
       if(err_filter.collide_err >= FILTER_CNT) is_abnormal = 1;
   }else{
       err_filter.collide_err = 0;
   }
   //2离地检测 Z轴过小悬空
   if(p_sen->z_acc <= Z_ACC_THRESHOLD){
       err_filter.ground_err++;
       if(err_filter.ground_err >= FILTER_CNT) is_abnormal = 1;
   }else{
       err_filter.ground_err = 0;
   }
   //3翻倒检测
   if(p_sen->roll_angle >= ROLL_ANGLE_MAX){
       err_filter.roll_err++;
       if(err_filter.roll_err >= FILTER_CNT) is_abnormal = 1;
   }else{
       err_filter.roll_err = 0;
   }
   //4人体触碰
   if(p_sen->touch_level == TOUCH_VALID){
       err_filter.touch_err++;
       if(err_filter.touch_err >= FILTER_CNT) is_abnormal = 1;
   }else{
       err_filter.touch_err = 0;
   }
   //5高温
   if(p_sen->temp >= SAFE_TEMP_MAX){
       err_filter.temp_err++;
       if(err_filter.temp_err >= FILTER_CNT) is_abnormal = 1;
   }else{
       err_filter.temp_err = 0;
   }

   //任意异常触发全停机
   if(is_abnormal == 1)
   {
       sys_emerg_stop = 1;
       Stop_Knife_Motor();
       Stop_Walk_Motor();
   }
   return is_abnormal;
}

/****************100ms定时中断回调函数 安全轮询入口****************/
void TIM1_IRQHandler(void)
{
   if(TIM_GetITStatus(TIM1,TIM_IT_Update) != RESET)
   {
       if(sys_emerg_stop == 0) //未急停才采集检测
       {
           Read_Sensor(&sensor_data);
           Safety_Check(&sensor_data);
       }
       TIM_ClearITPendingBit(TIM1,TIM_IT_Update);
   }
}

int main(void)
{
   //外设初始化
   TIM1_Init_100ms();
   while(1)
   {
       if(sys_emerg_stop == 0)
       {
           //正常运行逻辑
       }
       else
       {
           //停机保持状态
       }
   }
}

/****************底层硬件空函数，用户根据实际硬件填充IO/ADC/I2C****************/
void Read_Sensor(Sensor_t *p_sen)
{
   //p_sen->collide_force = ADC_Read(通道);
   //p_sen->z_acc = MPU6050_GetZAcc();
   //p_sen->roll_angle = MPU6050_GetRoll();
   //p_sen->touch_level = GPIO_ReadInputPin(TOUCH_PIN);
   //p_sen->temp = NTC_GetTemp(ADC_CH);
}
void Stop_Knife_Motor(void)
{
   //GPIO置低关闭刀盘MOS
}
void Stop_Walk_Motor(void)
{
   //驱动芯片失能，整机停走
}
void TIM1_Init_100ms(void)
{
   //定时器配置，100ms溢出中断
}
二、补充：RC一阶滤波算法（温度/加速度平滑，优化防抖）

加在Read_Sensor内部，抑制传感器跳变：
#define FILTER_ALPHA 0.2f  //滤波系数0~1，越小越平滑
float temp_filter = 0;
float zacc_filter = 0;

//一阶RC滤波函数
float RC_Filter(float new_data,float *old_data,float alpha)
{
   *old_data = alpha*new_data + (1-alpha)*(*old_data);
   return *old_data;
}

//调用示例：
p_sen->temp = RC_Filter(raw_temp,&temp_filter,FILTER_ALPHA);
p_sen->z_acc = RC_Filter(raw_z,&zacc_filter,FILTER_ALPHA);
三、算法使用说明

1. 硬件对应

• 碰撞：机身防撞环+分压压力传感器/微动开关 → ADC采集

• 离地+翻倒：MPU6050/MPU9250 IMU，I2C读取加速度+倾角

• 人体触碰：机身围边电容触摸芯片输出IO电平

• 高温：电机NTC热敏电阻ADC测温

2. 安全逻辑：5路独立计数滤波，单路连续3次异常直接锁死停机，需要人工复位才重启

3. 移植ESP32：只需替换GPIO、ADC、I2C驱动函数，检测逻辑完全不用改动