Dove si trova il TurtleBot 4?

Un localizzatore ICP in ROS 2, misurato contro la ground truth OptiTrack.

Guarda le misure Leggi il codice

  • OptiTrack
  • Stima ICP
  • Mappa del laboratorio

1. Conoscere il robot

Un TurtleBot 4 Standard, con il LiDAR montato a 90° rispetto alla marcia.

Base
iRobot Create 3
Computer di bordo
Raspberry Pi 4 Model B
LiDAR
RPLIDAR A1, 720 raggi per giro
Camera
OAK-D Pro
Il TurtleBot 4 acceso sul pavimento del laboratorio, con tre marker riflettenti fissati sopra la camera
Tre marker riflettenti sopra la camera: così OptiTrack vede il robot.

2. Misurare il laser

Dove il fascio incontra i pilastrini del telaio la lettura manca, e ai bordi oscilla.

  • Distanza media, in metri
  • Deviazione standard
Robot fermo in un recinto quadrato, 137 letture per raggio. I settori in arancio vengono esclusi dai nodi reattivi.

3. Agganciare la mappa

Il nodo icp_map allinea ogni scansione a una mappa di 569 punti.

Ricerca iniziale
griglia da 1 m, 8 orientamenti
Corrispondenze
entro 0,5 m
Iterazioni
15 in ricerca, 50 in tracking
Raggi usati
oltre 0,3 m dal sensore
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>::Ptr est(
    new pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>);
icp.setTransformationEstimation(est);

icp.setInputSource(current_cloud);
icp.setInputTarget(map_cloud_);
icp.setMaxCorrespondenceDistance(0.5);
icp.setMaximumIterations(50);
icp_map.cpp
Il RIMLab dall’alto: il TurtleBot al centro del pavimento, con gli assi x e y del sistema di riferimento disegnati sopra

4. Chiedere la verità a OptiTrack

Dodici telecamere a infrarossi seguono i marker. A robot fermo l’ICP sbaglia di circa 4 cm e 1°.

5. Confrontare tre percorsi

Sul cerchio l’errore medio dell’ICP è di 2,39 cm.

Cerchio di raggio 50 cm, due giri
ConfrontoErrore medioσ
OptiTrack sull’ideale0,30 cm0,32 cm
ICP sull’ideale1,34 cm1,35 cm
ICP su OptiTrack2,39 cm1,74 cm
  • Ideale
  • OptiTrack
  • ICP
Quadrato da 1 m, comandato a tempo
ConfrontoErrore medioσ
OptiTrack sull’ideale3,87 cm3,56 cm
ICP sull’ideale5,71 cm4,79 cm
ICP su OptiTrack3,42 cm3,18 cm
Chiusura, OptiTrack6,78 cm
Chiusura, ICP8,80 cm
Quadrato da 1 m, chiuso sull’odometria
ConfrontoErrore medioσ
OptiTrack sull’ideale5,19 cm3,98 cm
ICP sull’ideale7,87 cm5,34 cm
ICP su OptiTrack6,01 cm4,05 cm
Chiusura, OptiTrack10,30 cm
Chiusura, ICP10,62 cm

Otto nodi ROS 2 in C++

Apri una riga per leggere il passaggio che conta.

  • icp_map

    Localizza il robot sulla mappa con ICP planare262 righe
    La ricerca globale che aggancia la prima posa
    double step = 1.0;
double test_angles[8] = {0.0, M_PI_4, M_PI_2, 3*M_PI_4, M_PI, -3*M_PI_4, -M_PI_2, -M_PI_4};

// iteriamo su tutta la superficie della stanza
for (double x = min_pt.x; x <= max_pt.x; x += step) {
    for (double y = min_pt.y; y <= max_pt.y; y += step) {
        for (int i = 0; i < 8; ++i) {
            double cos_y = std::cos(test_angles[i]);
            double sin_y = std::sin(test_angles[i]);

            Eigen::Matrix4f guess = Eigen::Matrix4f::Identity();
            guess(0, 0) = cos_y;
            guess(0, 1) = -sin_y;
            guess(1, 0) = sin_y;
            guess(1, 1) = cos_y;
            guess(0, 3) = x;
            guess(1, 3) = y;

            pcl::PointCloud<pcl::PointXYZ> temp_final;
            icp.setMaximumIterations(15);
            icp.align(temp_final, guess);

            if (icp.hasConverged()) {
                double score = icp.getFitnessScore();
                if (score < best_score) {
                    best_score = score;
                    best_guess = guess;
                }
            }
        }
    }
}

  • laser_stat

    Media e deviazione standard di ogni raggio79 righe
    Le somme accumulate a ogni scansione
    for (int i = 0; i < num_raggi; i++) {
    float distanza = msg->ranges[i];

    if (std::isfinite(distanza)) {
        somme_[i] += distanza;
        somme_quadrati_[i] += (distanza * distanza);
        conteggi_[i]++;
    }
}

  • virtual_bumper

    Ferma il robot a 0,35 m dall’ostacolo96 righe
    Il settore frontale e la soglia di arresto
        if (a_robot > -0.5 && a_robot < 0.5) {
        if (r < min_distance) {
            min_distance = r;
        }
    }
}

if (min_distance < threshold_) {
    cmd.linear.x = 0.0;
    cmd.angular.z = 0.0;
} else {
    cmd.linear.x = speed_;
    cmd.angular.z = 0.0;
}

  • obstacle_avoider

    Gira verso il lato più libero105 righe
    La scelta tra sinistra e destra
    if (min_front < threshold_) {
    cmd.linear.x = 0.0;
    if (min_left > min_right) {
        cmd.angular.z = turnrate_;
    } else {
        cmd.angular.z = -turnrate_;
    }
} else {
    cmd.linear.x = speed_;
    cmd.angular.z = 0.0;
}

  • quadrato_nodo

    Quadrato da 1 m a tempo, senza feedback88 righe
    La curva di 90° contata in cicli
    // FASE 1: GIRA DI 90 GRADI
else if (fase_ == 1) {
  messaggio.angular.z = 0.785;
  contatore_++;

  // CURVA: 2 secondi (20 cicli)
  if (contatore_ >= 20) {
    contatore_ = 0;
    lati_completati_++;

    if (lati_completati_ >= 4) {
      fase_ = 2; // Ha fatto 4 lati completi
    } else {
      fase_ = 0;
      RCLCPP_INFO(this->get_logger(), "==> PARTENZA LATO %d", lati_completati_ + 1);
    }
  }
}

  • quadrato_odom_nodo

    Lo stesso quadrato, chiuso sull’odometria95 righe
    Il lato da 1 m misurato su /odom
    // FASE 0: VAI DRITTO FINO A 1 METRO
if (fase_ == 0) {
    double distance = std::sqrt(std::pow(current_x - start_x_, 2) + std::pow(current_y - start_y_, 2));

    if (distance < 1.0) {
        cmd.linear.x = 0.15;
        cmd.angular.z = 0.0;
    } else {
        cmd.linear.x = 0.0;
        fase_ = 1;
        init_pose_captured_ = false;
        RCLCPP_INFO(this->get_logger(), "inizio curva %d ", lati_completati_ + 1);
    }
}

  • cerchio_nodo

    Cerchio di raggio 50 cm a velocità costante45 righe
    Due velocità costanti bastano
    void cerchio_robot()
{
  geometry_msgs::msg::Twist messaggio;
  messaggio.linear.x = 0.2;
  messaggio.angular.z = 0.4;
  publisher_->publish(messaggio);
}

  • slalom_nodo

    Curve alternate ogni due secondi69 righe
    Destra e sinistra, quattro cicli per parte
    // fase 0 destra
if (fase_==0){
messaggio.linear.x = 0.45;
messaggio.angular.z = 1.2;
cont_=cont_+1;
  if (cont_>=4){
      fase_=1;
      cont_=0;
  }
}