割草机器人多维度安全防护检测算法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驱动函数，检测逻辑完全不用改动
10个月宝宝每天需要喝多少奶粉？