Caso práctico: SLAM 2D con RPi 5, RPLIDAR A1 y TB6612FNG

Objetivo y caso de uso

Qué construirás: Un robot que utiliza SLAM 2D para mapear y navegar en interiores con Raspberry Pi 5 y RPLIDAR A1.

Para qué sirve

• Navegación autónoma en entornos interiores utilizando mapeo en tiempo real.
• Generación de mapas 2D precisos para la planificación de rutas en robots móviles.
• Integración de datos de sensores para mejorar la toma de decisiones en robótica.
• Demostración de capacidades de SLAM en proyectos educativos y de investigación.

Resultado esperado

• Mapas generados con precisión de hasta 2 cm en entornos interiores.
• Latencias de procesamiento de datos de sensor de menos de 100 ms.
• Velocidad de navegación del robot de hasta 0.5 m/s con control de motores eficiente.
• Capacidad de manejar hasta 10 paquetes de datos por segundo desde el RPLIDAR A1.

Público objetivo: Desarrolladores y estudiantes avanzados de robótica; Nivel: Avanzado

Arquitectura/flujo: Raspberry Pi 5 + RPLIDAR A1 + TB6612FNG + Python para control y procesamiento de datos.

Nivel: Avanzado

Prerrequisitos

• Sistema operativo:
• Raspberry Pi OS Bookworm 64-bit (Debian 12), kernel 6.6.x o superior. Verificación rápida:
• cat /etc/os-release debe mostrar VERSION_CODENAME=bookworm
• uname -r debe mostrar 6.6.* (o superior)
• Modelo exacto del hardware:
• Raspberry Pi 5
• Slamtec RPLIDAR A1 (con adaptador USB oficial de Slamtec)
• Driver de motores TB6612FNG (para dos motores DC en configuración diferencial)
• Toolchain y versiones exactas utilizadas en este caso:
• Python 3.11.2 (Bookworm) — ver: python3 --version
• pip 24.2
• setuptools 70.0.0
• wheel 0.43.0
• gcc 12.2.0 — ver: gcc --version
• cmake 3.25.1 — ver: cmake --version (opcional, solo si compilas librerías nativas)
• Daemon pigpio (apt) v79 — ver: pigpiod -v y pigs -v
• Librerías Python (instaladas en venv):
• rplidar 0.9.3
• breezyslam 0.0.8
• numpy 1.26.4
• pigpio 1.78
• pillow 10.4.0
• gpiozero 2.0 (para utilidades GPIO; el control principal será con pigpio)
• Conocimientos previos:
• Linux básico, consola, edición de archivos.
• Conceptos de PWM y control H-bridge (TB6612FNG).
• Fundamentos de SLAM 2D y navegación diferencial.

Materiales

• Raspberry Pi 5 (4GB u 8GB; ambas funcionan para este caso).
• Slamtec RPLIDAR A1 con su base y adaptador USB oficial de Slamtec.
• TB6612FNG motor driver (placa con terminales VM, VCC, STBY, AIN1/AIN2, BIN1/BIN2, PWMA/PWMB).
• Dos motores DC de 6–12 V (una base robótica diferencial típica).
• Batería para motores (ej. 7.4 V LiPo 2S) y cableado correspondiente.
• Fuente 5V/3A (o superior) para Raspberry Pi 5 mediante USB-C.
• Cables Dupont (hembra-hembra y hembra-macho según corresponda).
• Cable USB-A a Micro-USB (o USB-C si tu base usa adaptador distinto) para conectar el RPLIDAR A1 al puerto USB 3.0 de la Raspberry Pi 5.
• Estructura/chasis para montaje (plataforma móvil con soporte para lidar).

Nota: Usa siempre GND común entre la Raspberry Pi 5 y el TB6612FNG/baterías de motor.

Preparación y conexión

Habilitar interfaces y servicios

• Actualizar el sistema:
• sudo apt update
• sudo apt full-upgrade -y

• sudo reboot

• Habilitar opciones y preparar permisos:

• Interfaces (opcional pero recomendado):
• sudo raspi-config
• Interface Options:
  • SSH: Enable (si gestionarás remotamente)
  • Serial Port: Disable login shell, y puedes dejar el puerto serial deshabilitado porque el LIDAR irá por USB.
  • I2C/SPI: No necesarios en este proyecto (puedes dejarlos deshabilitados).
• Finish y reboot si lo pide.
• Arrancar el daemon de pigpio:
• sudo systemctl enable –now pigpiod
• Añadir el usuario al grupo dialout para acceso a /dev/ttyUSB0 (LIDAR):
• sudo usermod -aG dialout $USER

• Cierra sesión y vuelve a entrar (o newgrp dialout).

• Comprobar LIDAR y pigpio:

• Conectar el RPLIDAR A1 al puerto USB 3.0 (azul) de la Raspberry Pi 5.
• Verificar dispositivo:
• ls -l /dev/ttyUSB*
• dmesg | tail
• Verificar pigpio:
• pigs -v (debe mostrar versión 79)
• pigpiod -v (debe coincidir)

Cableado entre Raspberry Pi 5 y TB6612FNG

• Conexiones de potencia:
• VM (TB6612FNG): al positivo de la batería de motores (p. ej., 7.4 V LiPo).
• VCC (TB6612FNG): a 3.3 V de la Raspberry Pi (pin físico 1 o 17).

• GND (TB6612FNG): a GND de la Raspberry Pi (p. ej., pin físico 6). GND de la batería y de la Pi deben estar comunes.

• Conexiones de control (asignación propuesta coherente con PWM hardware):

• Motores:
• A01/A02 a las dos terminales del motor izquierdo (Motor A).
• B01/B02 a las dos terminales del motor derecho (Motor B).

• Si ves que un motor gira al revés, intercambia A01/A02 (o BIN1/BIN2 para el B, etc.) o invierte la lógica en software.

• LIDAR:

• Conecta la base/adaptador USB oficial del RPLIDAR A1 a un puerto USB 3.0 (azul). No es necesario cablear nada al GPIO para el LIDAR.

Código completo (Python 3.11)

A continuación se presenta un único script Python que:
– Controla los motores a través del TB6612FNG usando pigpio a 20 kHz de PWM.
– Lee el RPLIDAR A1 por /dev/ttyUSB0.
– Ejecuta SLAM 2D (breezyslam) construyendo un mapa en tiempo real.
– Implementa dos modos: exploración (explore) y navegación a objetivo (goto).
– Guarda periódicamente la imagen del mapa en PNG como validación.

Guarda este archivo como slam_nav.py en tu proyecto.

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# Proyecto: lidar-2d-slam-navigation
# Hardware: Raspberry Pi 5 + Slamtec RPLIDAR A1 + TB6612FNG motor driver
# Toolchain:
#   - Raspberry Pi OS Bookworm 64-bit (Debian 12), kernel 6.6+
#   - Python 3.11.2, pip 24.2, setuptools 70.0.0, wheel 0.43.0
#   - pigpio daemon v79 (apt), pigpio Python 1.78
#   - rplidar 0.9.3, breezyslam 0.0.8, numpy 1.26.4, pillow 10.4.0
#
import argparse
import math
import os
import signal
import sys
import time

import numpy as np
from PIL import Image

import pigpio
from rplidar import RPLidar
from breezyslam.slam import RMHC_SLAM
from breezyslam.sensors import Laser

# ----------------------------
# Parámetros de hardware
# ----------------------------
# Raspberry Pi 5 -> TB6612FNG
GPIO_PWMA = 12   # Motor A PWM (PWM0) pin físico 32
GPIO_AIN1 = 23
GPIO_AIN2 = 24
GPIO_PWMB = 13   # Motor B PWM (PWM1) pin físico 33
GPIO_BIN1 = 27
GPIO_BIN2 = 22
GPIO_STBY = 5

PWM_FREQ = 20000  # 20 kHz para evitar zumbidos audibles
PWM_RANGE = 255   # Rango estándar pigpio para duty

# Geometría del robot (ajústala a tu base)
WHEEL_BASE_M = 0.16   # distancia entre ruedas en metros
MAX_ABS_SPEED = 1.0   # escala normalizada [-1..1] para motores

# ----------------------------
# Parámetros de LIDAR y SLAM
# ----------------------------
LIDAR_PORT = '/dev/ttyUSB0'   # Común en RPLIDAR A1 con adaptador USB
LIDAR_SCAN_SIZE = 360         # 1 grado por muestra
LIDAR_MAX_RANGE_MM = 6000     # 6 m
LIDAR_MIN_RANGE_MM = 150      # ignorar ecos muy cercanos no fiables

# Mapa SLAM
MAP_SIZE_PIXELS = 800
MAP_SIZE_METERS = 16.0  # 2 cm/pixel aprox.

# ----------------------------
# Clases auxiliares
# ----------------------------
class RPLidarA1(Laser):
    def __init__(self):
        super().__init__(scan_size=LIDAR_SCAN_SIZE,
                         scan_rate_hz=10,               # A1 ~5-10 Hz
                         detection_margin=15,           # grados ignorados al límite
                         distance_noise=50.0,           # mm
                         min_range=LIDAR_MIN_RANGE_MM,
                         max_range=LIDAR_MAX_RANGE_MM)

class Motor:
    def __init__(self, pi, pwm_pin, in1_pin, in2_pin, name="M"):
        self.pi = pi
        self.pwm_pin = pwm_pin
        self.in1_pin = in1_pin
        self.in2_pin = in2_pin
        self.name = name

        for p in (self.pwm_pin, self.in1_pin, self.in2_pin):
            self.pi.set_mode(p, pigpio.OUTPUT)
        self.pi.set_PWM_frequency(self.pwm_pin, PWM_FREQ)
        self.pi.set_PWM_range(self.pwm_pin, PWM_RANGE)
        self.set_speed(0.0)

    def set_speed(self, speed):
        # speed en [-1.0, 1.0]
        sp = max(-1.0, min(1.0, speed))
        if sp > 0:
            self.pi.write(self.in1_pin, 1)
            self.pi.write(self.in2_pin, 0)
        elif sp < 0:
            self.pi.write(self.in1_pin, 0)
            self.pi.write(self.in2_pin, 1)
        else:
            # freno suave: entradas a 0 (coast) o high-high (brake)
            self.pi.write(self.in1_pin, 0)
            self.pi.write(self.in2_pin, 0)

        duty = int(abs(sp) * PWM_RANGE)
        self.pi.set_PWM_dutycycle(self.pwm_pin, duty)

    def brake(self):
        self.pi.write(self.in1_pin, 1)
        self.pi.write(self.in2_pin, 1)
        self.pi.set_PWM_dutycycle(self.pwm_pin, 0)

class TB6612Robot:
    def __init__(self):
        self.pi = pigpio.pi()
        if not self.pi.connected:
            raise RuntimeError("No se pudo conectar a pigpio. ¿Está el daemon pigpiod en ejecución?")

        # STBY alto para habilitar
        self.pi.set_mode(GPIO_STBY, pigpio.OUTPUT)
        self.pi.write(GPIO_STBY, 1)

        self.motor_left = Motor(self.pi, GPIO_PWMA, GPIO_AIN1, GPIO_AIN2, name="L")
        self.motor_right = Motor(self.pi, GPIO_PWMB, GPIO_BIN1, GPIO_BIN2, name="R")

    def set_twist(self, v_lin, v_ang):
        # v_lin en [-1..1], v_ang en rad/s escalado
        v = max(-1.0, min(1.0, v_lin))
        w = v_ang
        # Mezcla diferencial (normalizada)
        # Asumimos w ya escalado a [-1..1] aprox. tras controlador
        left = v - 0.5 * w
        right = v + 0.5 * w

        # Normalización si se excede
        m = max(1.0, abs(left), abs(right))
        left /= m
        right /= m

        self.motor_left.set_speed(left * MAX_ABS_SPEED)
        self.motor_right.set_speed(right * MAX_ABS_SPEED)

    def stop(self):
        self.motor_left.set_speed(0.0)
        self.motor_right.set_speed(0.0)

    def shutdown(self):
        try:
            self.stop()
            self.motor_left.brake()
            self.motor_right.brake()
            self.pi.write(GPIO_STBY, 0)
        except Exception:
            pass
        self.pi.stop()

class SLAMNavigator:
    def __init__(self, map_pixels=MAP_SIZE_PIXELS, map_meters=MAP_SIZE_METERS):
        self.sensor = RPLidarA1()
        self.slam = RMHC_SLAM(self.sensor, map_pixels, map_meters)
        self.map_bytes = bytearray(map_pixels * map_pixels)
        self.distances = [0] * LIDAR_SCAN_SIZE
        self.pose = (0, 0, 0)  # x_mm, y_mm, theta_deg

    def update_with_scan(self, scan):
        # scan: lista de tuplas (quality, angle_deg, distance_mm)
        # Convertimos a vector de distancias indexado por ángulo entero
        for i in range(LIDAR_SCAN_SIZE):
            self.distances[i] = 0

        for (_, angle, distance) in scan:
            a = int(angle) % 360
            d = int(distance)
            if LIDAR_MIN_RANGE_MM <= d <= LIDAR_MAX_RANGE_MM:
                self.distances[a] = d

        # SLAM
        self.slam.update(self.distances)
        self.pose = self.slam.getpos()
        self.slam.getmap(self.map_bytes)
        return self.pose

    def save_map_png(self, path="map_latest.png"):
        side = int(math.sqrt(len(self.map_bytes)))
        img = np.frombuffer(self.map_bytes, dtype=np.uint8).copy().reshape((side, side))
        # rotar para que "x derecha, y hacia arriba" sea intuitivo al visualizar
        img = np.flipud(img)
        im = Image.fromarray(img, mode='L')
        im.save(path)

def angle_wrap_pi(a):
    while a > math.pi:
        a -= 2*math.pi
    while a < -math.pi:
        a += 2*math.pi
    return a

def polar_to_cartesian(d_mm, angle_deg):
    a = math.radians(angle_deg)
    return d_mm * math.cos(a), d_mm * math.sin(a)

def compute_front_clearance(distances, span_deg=20):
    # Busca la distancia mínima en el sector frontal [-span, +span]
    vals = []
    for i in range(360 - span_deg, 360):
        if distances[i] > 0:
            vals.append(distances[i])
    for i in range(0, span_deg + 1):
        if distances[i] > 0:
            vals.append(distances[i])
    if not vals:
        return LIDAR_MAX_RANGE_MM
    return min(vals)

def compute_free_direction(distances, sector=60):
    # Escoge dirección (signo) con mayor espacio libre: izquierda vs derecha
    left = []
    right = []
    for i in range(1, sector + 1):
        # derecha: 0..+sector
        if distances[i] > 0:
            right.append(distances[i])
        # izquierda: -sector..-1 => 360-sector..359
        j = (360 - i) % 360
        if distances[j] > 0:
            left.append(distances[j])
    left_min = min(left) if left else LIDAR_MAX_RANGE_MM
    right_min = min(right) if right else LIDAR_MAX_RANGE_MM
    return -1 if left_min > right_min else +1  # -1 girar izquierda, +1 derecha

def main():
    parser = argparse.ArgumentParser(description="Lidar 2D SLAM Navigation (RPi5 + RPLIDAR A1 + TB6612FNG)")
    parser.add_argument("--mode", choices=["explore", "goto", "test-motors"], default="explore",
                        help="explore (mapeo reactivo), goto (ir a objetivo), test-motors (prueba PWM)")
    parser.add_argument("--goal-x", type=float, default=2.0, help="Objetivo X en metros (solo modo goto)")
    parser.add_argument("--goal-y", type=float, default=1.0, help="Objetivo Y en metros (solo modo goto)")
    parser.add_argument("--map-save-interval", type=float, default=5.0, help="Intervalo para guardar PNG del mapa (s)")
    parser.add_argument("--forward-speed", type=float, default=0.5, help="Velocidad lineal normalizada base")
    parser.add_argument("--turn-gain", type=float, default=1.0, help="Ganancia de giro (rad -> comando)")
    parser.add_argument("--avoid-dist", type=float, default=0.5, help="Distancia mínima frontal (m)")
    args = parser.parse_args()

    robot = TB6612Robot()
    lidar = RPLidar(port=LIDAR_PORT)
    nav = SLAMNavigator()

    run = True

    def sigint_handler(sig, frame):
        nonlocal run
        run = False

    signal.signal(signal.SIGINT, sigint_handler)
    signal.signal(signal.SIGTERM, sigint_handler)

    last_save = 0.0

    try:
        if args.mode == "test-motors":
            print("Probando motores: adelante 1s, atrás 1s, giro en sitio 1s...")
            robot.set_twist(0.6, 0.0)
            time.sleep(1.0)
            robot.set_twist(-0.6, 0.0)
            time.sleep(1.0)
            robot.set_twist(0.0, 1.0)
            time.sleep(1.0)
            robot.stop()
            print("Test completado.")
            return

        print("Iniciando LIDAR...")
        lidar.start_motor()
        time.sleep(0.5)

        print("Leyendo escaneos y ejecutando SLAM...")
        for scan in lidar.iter_scans(max_buf_meas=5000):
            pose_mm = nav.update_with_scan(scan)
            x_mm, y_mm, theta_deg = pose_mm
            x_m, y_m = x_mm / 1000.0, y_mm / 1000.0
            theta_rad = math.radians(theta_deg)

            # Distancia frontal para evitar colisiones
            front_min_mm = compute_front_clearance(nav.distances, span_deg=20)
            front_min_m = front_min_mm / 1000.0

            v_cmd = 0.0
            w_cmd = 0.0

            if args.mode == "explore":
                # Navegación reactiva para mapear: avanza si despejado, si no esquiva
                if front_min_m < args.avoid_dist:
                    sgn = compute_free_direction(nav.distances, sector=60)
                    v_cmd = 0.0
                    w_cmd = 0.9 * sgn
                else:
                    v_cmd = args.forward_speed
                    # Pequeño giro para explorar suavemente
                    w_cmd = 0.1 * math.sin(time.time())

            elif args.mode == "goto":
                # Control hacia el objetivo con evasión de obstáculos
                dx = args.goal_x - x_m
                dy = args.goal_y - y_m
                goal_dist = math.hypot(dx, dy)
                goal_heading = math.atan2(dy, dx)
                heading_error = angle_wrap_pi(goal_heading - theta_rad)

                # Evasión simple
                if front_min_m < args.avoid_dist:
                    sgn = compute_free_direction(nav.distances, sector=60)
                    v_cmd = 0.0
                    w_cmd = 0.9 * sgn
                else:
                    # Proporcional a distancia y error angular
                    v_cmd = max(0.0, min(args.forward_speed, 0.4 + 0.2 * goal_dist))
                    w_cmd = max(-1.0, min(1.0, args.turn_gain * heading_error))

                # Llega al objetivo (umbral 0.15 m)
                if goal_dist < 0.15:
                    v_cmd = 0.0
                    w_cmd = 0.0
                    robot.stop()
                    print("Objetivo alcanzado.")
                    run = False

            robot.set_twist(v_cmd, w_cmd)

            # Guardar mapa periódicamente
            now = time.time()
            if now - last_save > args.map_save_interval:
                nav.save_map_png("map_latest.png")
                last_save = now
                print(f"Pose: ({x_m:.2f} m, {y_m:.2f} m, {theta_deg:.1f}°), front={front_min_m:.2f} m; mapa actualizado.")

            if not run:
                break

    except KeyboardInterrupt:
        pass
    finally:
        try:
            robot.stop()
        except Exception:
            pass
        try:
            lidar.stop()
            lidar.stop_motor()
            lidar.disconnect()
        except Exception:
            pass
        robot.shutdown()
        print("Apagado seguro completado.")

if __name__ == "__main__":
    main()

Breve explicación de partes clave:
– TB6612Robot/Motor: administración de pines de dirección (IN1/IN2) y PWM por pigpio a 20 kHz. Standby (STBY) se pone en alto para habilitar el driver.
– RPLidarA1 y SLAMNavigator: configuración para 360 muestras por revolución, 6 m de alcance; uso de RMHC_SLAM de BreezySLAM; conversión de escaneo a vector de distancias por ángulo.
– Control reactive y de objetivo: en modo “explore” avanza si no hay obstáculos y explora con leve oscilación; en “goto” combina control al objetivo con evasión frontal simple.
– Map saving: genera map_latest.png periódicamente para validar el SLAM.

Compilación/flash/ejecución

Sigue estos pasos exactamente en la Raspberry Pi 5 con Raspberry Pi OS Bookworm 64-bit:

1) Preparación del entorno y dependencias del sistema:
– sudo apt update
– sudo apt install -y python3.11-venv python3-dev build-essential pkg-config libatlas-base-dev libopenblas-dev pigpio
– sudo systemctl enable –now pigpiod

2) Crear y activar un entorno virtual para aislar dependencias de Python:
– python3 –version
– Debe ser 3.11.x
– python3 -m venv ~/venvs/lidar-slam
– source ~/venvs/lidar-slam/bin/activate
– pip install –upgrade pip==24.2 setuptools==70.0.0 wheel==0.43.0

3) Instalar las librerías Python exactas:
– pip install rplidar==0.9.3 breezyslam==0.0.8 numpy==1.26.4 pigpio==1.78 pillow==10.4.0 gpiozero==2.0

4) Crear el proyecto y copiar el código:
– mkdir -p ~/projects/lidar-2d-slam-navigation
– cd ~/projects/lidar-2d-slam-navigation
– nano slam_nav.py
– Pega el código completo anterior y guarda.

5) Comprobar el LIDAR y permisos:
– ls -l /dev/ttyUSB*
– Debe aparecer /dev/ttyUSB0 (si hay más de un adaptador USB, puede ser ttyUSB1).
– Si no eres parte de dialout (necesario para acceder a /dev/ttyUSB0):
– sudo usermod -aG dialout $USER
– Cierra sesión y vuelve a entrar o ejecuta: newgrp dialout

6) Prueba rápida de motores (sin LIDAR):
– Con la batería de motores conectada (VM), y verificando que las ruedas estén en el aire:
– python3 slam_nav.py –mode test-motors
– Debes ver un avance, retroceso y giro en sitio.

7) Ejecución en modo exploración (mapear y evitar obstáculos):
– python3 slam_nav.py –mode explore –forward-speed 0.5 –avoid-dist 0.5 –map-save-interval 5
– El mapa se guardará cada ~5 s en map_latest.png.

8) Ejecución en modo navegación hacia objetivo (coordenadas en metros desde el origen inicial):
– python3 slam_nav.py –mode goto –goal-x 2.0 –goal-y 1.0 –turn-gain 1.0 –forward-speed 0.5
– El robot intentará alcanzar (2.0 m, 1.0 m) evitando obstáculos.

Opcional: para crear una regla udev que cree un nombre estable /dev/rplidar (en lugar de /dev/ttyUSB0):

# /etc/udev/rules.d/99-rplidar.rules
SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", SYMLINK+="rplidar", GROUP="dialout", MODE="0660"

• sudo udevadm control –reload-rules
• Desconecta y reconecta el RPLIDAR
• Usa LIDAR_PORT = «/dev/rplidar» en el script si aplicas esta regla.

Validación paso a paso

1) Verificar pigpio y PWM:
– pigs -v
– Debe mostrar versión v79. Si “connection failed”, el daemon no está corriendo.
– Ejecuta test de motores:
– python3 slam_nav.py –mode test-motors
– Observa:
– Ruedas giran hacia adelante 1 s, luego hacia atrás 1 s, luego giro en sitio 1 s.
– Si un motor gira invertido, invierte las conexiones A01/A02 o BIN1/BIN2 (o ajusta el signo en software).

2) Verificar LIDAR por USB:
– ls -l /dev/ttyUSB*
– dmesg | grep ttyUSB
– Si aparece /dev/ttyUSB0, procede.

3) Verificar lectura del LIDAR y SLAM en modo exploración:
– Coloca el robot en el suelo con espacio alrededor.
– python3 slam_nav.py –mode explore
– Observa:
– En consola, cada ~5 s, se imprime la pose (x, y, theta) y se guarda map_latest.png.
– Si hay obstáculos, el robot debería evitar chocar y seguir avanzando.

4) Verificar el mapa:
– Abre el archivo map_latest.png desde la propia Pi:
– xdg-open map_latest.png
– O transfiérelo a tu PC por SCP:
– scp pi@:~/projects/lidar-2d-slam-navigation/map_latest.png .
– Compara el entorno real con el mapa generado (paredes y pasillos deben verse coherentes).

5) Validar navegación hacia objetivo:
– Determina un punto objetivo seguro dentro del rango de tu espacio de pruebas (p. ej., +2 m en x, +1 m en y desde la posición inicial).
– python3 slam_nav.py –mode goto –goal-x 2.0 –goal-y 1.0
– Observa:
– El robot debe orientarse hacia el objetivo, esquivar obstáculos simples y detenerse cerca de la meta (< 0.15 m).
– El mapa debe seguir actualizándose y mejorando la consistencia local.

6) Control de estabilidad:
– Si notas oscilaciones de giro, reduce –turn-gain a 0.7 o 0.5.
– Si el robot se detiene por falsas detecciones, reduce –avoid-dist (p. ej., 0.4).

Troubleshooting

1) El LIDAR no aparece como /dev/ttyUSB0:
– Causa: controlador USB a serie distinto, múltiples dispositivos USB, o cable defectuoso.
– Solución:
– Revisa dmesg | tail justo al conectar.
– Prueba con otro puerto USB 3.0 o un cable diferente.
– Confirma permisos (grupo dialout).
– Usa la regla udev para symlink estable /dev/rplidar.

2) Error “No se pudo conectar a pigpio”:
– Causa: el daemon pigpiod no está en ejecución.
– Solución:
– sudo systemctl enable –now pigpiod
– Verifica con pigs -v.
– Si usas un firewall/selinux (raro en Pi OS), desactívalo para pruebas.

3) Motores no giran o zumban sin moverse:
– Causa: STBY en bajo, VM sin batería, PWM a 0, o cableado IN1/IN2 invertido.
– Solución:
– Comprueba STBY en alto (GPIO 5).
– Verifica que la batería VM esté conectada y con tensión suficiente.
– Ejecuta modo test-motors para aislar problema de SLAM/LIDAR.
– Revisa IN1/IN2: con velocidad positiva IN1=1, IN2=0; con negativa, al revés.

4) El mapa sale borroso o inconsistente:
– Causa: vibraciones, superficies con poca textura o velocidad demasiado alta.
– Solución:
– Reduce la velocidad (–forward-speed 0.3–0.4).
– Asegura físicamente el RPLIDAR A1 para minimizar vibraciones.
– Evita moverse cerca de objetos muy brillantes o translúcidos.

5) “Permission denied” al abrir /dev/ttyUSB0:
– Causa: usuario no pertenece a dialout o el archivo cambió de grupo/permisos.
– Solución:
– sudo usermod -aG dialout $USER; cierra sesión y regresa.
– Verifica ls -l /dev/ttyUSB0 (debe ser grupo dialout).
– Aplica regla udev si es necesario.

6) SLAM no converge o la posición deriva:
– Causa: trayectorias con pocos rasgos o escaneo bloqueado.
– Solución:
– Asegura un movimiento gradual (no gires demasiado rápido).
– Evita pasillos demasiado largos sin referencias (añade giros).
– Aumenta el tiempo de estacionario inicial para “anclar” el mapa.

7) PWM ruidoso o motores calientes:
– Causa: frecuencia inadecuada o duty excesivo.
– Solución:
– Mantén 20 kHz para salir del rango audible.
– Reduce MAX_ABS_SPEED si tus motores no están refrigerados.

8) El robot gira en sentido opuesto (invertido):
– Causa: polaridad de motores o cableado IN1/IN2/BIN1/BIN2 invertidos.
– Solución:
– Intercambia A01/A02 (motor A) o B01/B02 (motor B) o invierte las líneas INx.
– Alternativamente, multiplica por -1 el comando de ese motor en software.

Mejoras/variantes

• Encoders en ruedas: Añadir odometría para mejorar la estabilidad del SLAM y la navegación hacia objetivos, fusionando odometría con el scan matching del LIDAR.
• Planeación de rutas: Implementar A o D Lite sobre la cuadrícula de ocupación del mapa, generando trayectorias más suaves que el control reactivo.
• Control avanzado: PID para velocidad de ruedas y control de orientación con límites de aceleración/jerk para proteger la mecánica.
• Fusión sensorial: Integrar IMU (giroscopio) para mejorar la estimación de orientación y reducir deriva en giros.
• Persistencia de mapas: Guardar/cargar mapas con timestamps y permitir localización en mapas previamente construidos.
• ROS 2 (opcional): Migrar a ROS 2 Iron/Humble en RPi 5, con nodos para RPLIDAR, SLAM (Hector SLAM o slam_toolbox) y navegación (nav2). Este caso mantiene la consistencia con Python puro por simplicidad de despliegue.
• Seguridad y energía: Añadir supervisión de voltaje de batería y relé de corte de emergencia por GPIO.

Checklist de verificación

• [ ] Sistema: Raspberry Pi OS Bookworm 64-bit instalado y actualizado (kernel 6.6+).
• [ ] Usuario en grupo dialout; /dev/ttyUSB0 visible al conectar el RPLIDAR A1.
• [ ] pigpio daemon habilitado y en ejecución (pigs -v OK).
• [ ] Cableado TB6612FNG coincidente con la tabla (PWMA=GPIO12, PWMB=GPIO13, AIN1=GPIO23, AIN2=GPIO24, BIN1=GPIO27, BIN2=GPIO22, STBY=GPIO5, VCC=3.3V, GND común).
• [ ] Batería de motores conectada a VM; GND común con la Raspberry Pi 5.
• [ ] Entorno virtual creado y activado; pip 24.2; librerías instaladas con versiones exactas (rplidar 0.9.3, breezyslam 0.0.8, numpy 1.26.4, pigpio 1.78, pillow 10.4.0, gpiozero 2.0).
• [ ] Modo test-motors completado con éxito (adelante, atrás, giro).
• [ ] Modo explore: robot avanza y evita obstáculos; map_latest.png se actualiza periódicamente.
• [ ] Modo goto: robot se orienta y navega hacia las coordenadas objetivo evitando obstáculos; se detiene a ~0.15 m del objetivo.
• [ ] Mapa generado refleja el entorno (paredes y pasillos coherentes).
• [ ] Sin errores persistentes en permisos, pigpio o conexión del LIDAR.

Este caso práctico, completamente centrado en el modelo Raspberry Pi 5 + Slamtec RPLIDAR A1 + TB6612FNG motor driver, proporciona una base sólida para Lidar-2D-SLAM-Navigation con Python 3.11 en Raspberry Pi OS Bookworm 64-bit, incluyendo instrucciones precisas de configuración, cableado, código y validación reproducible.