发现

 DF创客社区 产品资料库 DF创客商城
回到首页 返回首页
回到顶部 回到顶部
返回上一页 返回上一页

基于行空板 M10 扩展板的自动跟随机器人 简单

头像 豆爸 2025.06.20 33 0

一、项目背景

 

自动跟随机器人是一种能够自动跟踪并跟随目标物体的智能机器人。本项目基于 PID 控制算法，利用超声波传感器检测目标物体的距离，并通过电机驱动模块控制机器人的移动，实现对目标的自动跟随功能。该机器人可以应用于物流配送、智能家居、教育科研等多个领域。

微信图片_20250622190459.png

 

二、设计思路

 

PID控制是一种通过按被控对象的实时数据采集的信息与给定值比较产生的误差的比例、积分和微分进行控制的控制系统，是一种技术成熟、应用最为广泛的控制系统，被广泛应用于工业过程控制。

 

20240313_22676.jpg

 

本项目采用 PID 控制算法实现对目标物体的精确跟随。PID 控制器根据当前距离与目标距离之间的误差，计算出合适的控制量，驱动电机调整机器人的位置，使距离误差逐渐减小。为了提高系统的稳定性和可靠性，设计中加入了滤波处理、死区控制和异常处理等机制。

 

系统工作流程如下：

1、超声波传感器实时检测前方障碍物的距离

2、将当前距离与目标距离进行比较，计算误差

3、PID 控制器根据误差计算控制量

4、根据控制量驱动电机，调整机器人的位置

5、循环执行上述步骤，实现连续跟随

 

微信图片_20250622191720.jpg

 

三、硬件介绍

 

1. 硬件清单

材料清单

  • 行空板M10 X1 链接
  • 行空板 M10电机IO扩展板+电池扩展板 X1 链接
  • 超声波测距传感器 X1 链接
  • N20减速电机 X4 链接
  • 42mmx19mm轮子 X4 链接
  • 电机支架 X4 链接

 

2.接线图

 

未标题-1.png

 

3. 硬件介绍

 

（1）行空板M10电机IO扩展板

 

（2）行空板M10电池扩展板

 

 

 

四、流程图

 

exported_image.png

 

五、主要代码片段及说明

 

1. 初始化与参数设置

代码
# PID控制参数
TARGET_DISTANCE = 20  # 目标距离(cm)
KP = 20               # 比例系数
KI = 0.03             # 积分系数
KD = 10               # 微分系数
MIN_DISTANCE = 5      # 最小有效距离(cm)
MAX_DISTANCE = 100    # 最大有效距离(cm)
MIN_SPEED = 150       # 电机最低启动速度
MAX_SPEED = 1023      # 电机最高速度
DEAD_ZONE = 2         # 死区范围(cm)
ACTION_THRESHOLD = 50 # PID动作阈值

# PID控制变量
last_error = 0
integral = 0
last_valid_distance = TARGET_DISTANCE  # 最后有效距离

说明：设置 PID 控制所需的各种参数，包括目标距离、PID 系数、距离范围和电机控制范围等。

 

 

2. 距离检测与滤波

代码
def get_filtered_distance():
    """获取滤波后的距离值，处理异常读数"""
    global last_valid_distance
    try:
        distance = sonar.distance_cm()
        # 检查距离是否在有效范围内
        if MIN_DISTANCE <= distance <= MAX_DISTANCE:
            last_valid_distance = distance
            return distance
        return last_valid_distance  # 返回上次有效值
    except Exception as e:
        print(f"传感器错误: {str(e)}")
        return last_valid_distance  # 返回上次有效值

说明：读取超声波传感器的距离值，并进行有效性检查，确保返回的距离值在合理范围内，提高系统稳定性。

 

 

3. PID 控制算法

代码
# 计算误差
error = TARGET_DISTANCE - distance

# 计算PID各项
P = KP * error
integral += error
# 积分限幅防止积分饱和
integral = constrain(integral, -100, 100)
I = KI * integral
derivative = KD * (error - last_error)
last_error = error

# 计算PID输出
pid_output = constrain(P + I + derivative, -MAX_SPEED, MAX_SPEED)

说明：根据当前距离与目标距离的误差，计算 PID 控制器的三个分量（比例、积分、微分），并将它们加权求和得到最终的控制量。

 

 

4. 电机控制

代码
# 死区处理
motor_state = -1  # -1表示停止
actual_speed = 0
if abs(error) > DEAD_ZONE:  # 死区范围
    # 修正方向逻辑
    if pid_output > ACTION_THRESHOLD:  
        # 当距离小于目标（太近）时：需要后退
        actual_speed = set_motor_speed(0, pid_output)  # 0=后退
        motor_state = 0
    elif pid_output < -ACTION_THRESHOLD:  
        # 当距离大于目标（太远）时：需要前进
        actual_speed = set_motor_speed(1, -pid_output)  # 1=前进
        motor_state = 1
    else:
        shield.m1.run(0, 0)
        shield.m2.run(0, 0)
else:
    shield.m1.run(0, 0)
    shield.m2.run(0, 0)

说明：根据 PID 输出和距离误差，控制电机的运行状态和速度。当误差超过死区范围时，根据误差方向控制机器人前进或后退；当误差在死区范围内时，停止电机运行，避免机器人在目标位置附近震荡。

 

 

六、程序代码

代码
import sys
import time
sys.path.append("/root/mindplus/.lib/thirdExtension/doudad-unihiker_ioextension-thirdex")
from shield import SHIELD
from pinpong.board import Board, Pin
from pinpong.board import SR04_URM10
from pinpong.extension.unihiker import *
from unihiker import GUI

# 初始化开发板
Board().begin()
shield = SHIELD()
u_gui = GUI()

# 设置超声波传感器引脚
sonar = SR04_URM10(Pin(Pin.P9), Pin(Pin.P9))

# 创建GUI元素
title = u_gui.draw_text(text="自动跟随机器人", x=20, y=5, font_size=20, color="#000000")
txt_distance = u_gui.draw_text(text="距离: -- cm", x=10, y=55, font_size=20, color="#0000FF")
txt_pid = u_gui.draw_text(text="PID输出: --", x=10, y=105, font_size=20, color="#FF0000")
txt_motor = u_gui.draw_text(text="电机: 停止", x=10, y=155, font_size=20, color="#008000")
txt_target = u_gui.draw_text(text=f"目标距离: 20 cm", x=10, y=205, font_size=20, color="#000000")

# PID控制参数
TARGET_DISTANCE = 20  # 目标距离(cm)
KP = 20               # 比例系数（降低比例系数减少震荡）
KI = 0.03             # 积分系数（降低积分系数）
KD = 10               # 微分系数
MIN_DISTANCE = 5      # 最小有效距离(cm)
MAX_DISTANCE = 100    # 最大有效距离(cm)
MIN_SPEED = 150       # 电机最低启动速度
MAX_SPEED = 1023       # 电机最高速度
DEAD_ZONE = 2         # 死区范围(cm)
ACTION_THRESHOLD = 50 # PID动作阈值

# PID控制变量
last_error = 0
integral = 0
last_valid_distance = TARGET_DISTANCE  # 最后有效距离

def constrain(value, min_val, max_val):
    """限制数值在指定范围内"""
    return max(min_val, min(value, max_val))

def get_filtered_distance():
    """获取滤波后的距离值，处理异常读数"""
    global last_valid_distance
    try:
        distance = sonar.distance_cm()
        # 检查距离是否在有效范围内
        if MIN_DISTANCE <= distance <= MAX_DISTANCE:
            last_valid_distance = distance
            return distance
        return last_valid_distance  # 返回上次有效值
    except Exception as e:
        print(f"传感器错误: {str(e)}")
        return last_valid_distance  # 返回上次有效值

def set_motor_speed(direction, pid_value):
    """设置电机速度，确保不低于最低启动速度"""
    # 计算实际速度（确保在MIN_SPEED和MAX_SPEED之间）
    actual_speed = int(constrain(abs(pid_value), MIN_SPEED, MAX_SPEED))
    
    # 设置电机
    shield.m1.run(direction, actual_speed)
    shield.m2.run(direction, actual_speed)
    
    return actual_speed

def update_gui(distance, pid_output, motor_state, actual_speed=0):
    """更新GUI显示"""
    txt_distance.config(text=f"距离: {distance:.1f} cm")
    txt_pid.config(text=f"PID输出: {pid_output:.1f}")
    
    if motor_state == 1:  # 前进
        txt_motor.config(text=f"电机: 前进 {actual_speed}", color="#008000")
    elif motor_state == 0:  # 后退
        txt_motor.config(text=f"电机: 后退 {actual_speed}", color="#FF0000")
    else:
        txt_motor.config(text="电机: 停止", color="#0000FF")

try:
    print("PID距离控制启动...")
    print(f"目标距离: {TARGET_DISTANCE}cm")
    print(f"最低电机速度: {MIN_SPEED}")
    
    while True:
        # 获取滤波后的距离
        distance = get_filtered_distance()
        print(f"当前距离: {distance:.1f} cm")
        
        # 计算误差
        error = TARGET_DISTANCE - distance
        
        # 计算PID各项
        P = KP * error
        integral += error
        # 积分限幅防止积分饱和
        integral = constrain(integral, -100, 100)
        I = KI * integral
        derivative = KD * (error - last_error)
        last_error = error
        
        # 计算PID输出 (限制在±512范围内)
        pid_output = constrain(P + I + derivative, -MAX_SPEED, MAX_SPEED)
        
        # 死区处理 (避免在目标距离附近震荡)
        motor_state = -1  # -1表示停止
        actual_speed = 0
        if abs(error) > DEAD_ZONE:  # 死区范围
            # 修正方向逻辑
            if pid_output > ACTION_THRESHOLD:  
                # 当距离小于目标（太近）时：需要后退
                actual_speed = set_motor_speed(0, pid_output)  # 0=后退
                motor_state = 0
                print(f"后退 {actual_speed}")
            elif pid_output < -ACTION_THRESHOLD:  
                # 当距离大于目标（太远）时：需要前进
                actual_speed = set_motor_speed(1, -pid_output)  # 1=前进
                motor_state = 1
                print(f"前进 {actual_speed}")
            else:
                shield.m1.run(0, 0)
                shield.m2.run(0, 0)
                print("停止 (低于动作阈值)")
        else:
            shield.m1.run(0, 0)
            shield.m2.run(0, 0)
            print("目标范围内，停止")
        
        # 更新GUI
        update_gui(distance, pid_output, motor_state, actual_speed)
        
        # 控制循环延迟
        time.sleep(0.15)

except KeyboardInterrupt:
    # 程序中断时停止电机
    shield.m1.run(0, 0)
    shield.m2.run(0, 0)
    print("程序停止")
    txt_motor.config(text="程序已停止", color="#000000")
except Exception as e:
    print(f"发生错误: {str(e)}")
    shield.m1.run(0, 0)
    shield.m2.run(0, 0)
    txt_motor.config(text=f"错误: {str(e)}", color="#FF0000")

七、附件

附件

Makelog作者原创文章，未经授权禁止转载。
#行空板M10#人工智能#物联网#机器人#Python

评论

user-avatar
豆爸
豆爸2019.10.17
icon
创造力4992
icon
项目文章30

大家好，我是豆爸。

icon 他的勋章
    展开更多

    你可能感兴趣的