note update

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/upbot_location阅读笔记.md

@@ -0,0 +1,561 @@
+### 一.前提知识
+
+##### 数据格式
+
+- IMU：线性加速度、角速度、时间戳。
+- uwb：位置、时间戳。
+- LightHouse：位置、姿态四元数、时间戳。
+- 融合数据：位置、姿态四元数、平面速度、角速度。
+
+##### Lighthouse
+
+在机器人定位系统中，"lighthouse" 通常指的是一种基于光学的定位技术，它广泛应用于虚拟现实 (VR) 系统中，例如HTC Vive的定位系统。这种技术被称为“Lighthouse Tracking System”。
+
+**Lighthouse Tracking System 的工作原理**
+
+1. **基站发射信号**：
+   - 系统包括多个基站（通常是两个），它们被放置在房间的不同位置。
+   - 基站会发射红外激光和同步闪光信号。
+   
+2. **激光扫描**：
+   - 基站中的激光会沿水平和垂直方向扫描整个房间。
+   - 基站发射的激光束以已知的频率和角度扫描。
+   
+3. **传感器接收信号**：
+   - 被追踪的物体（例如VR头显或控制器）上安装了多个红外传感器。
+   - 当激光束扫过传感器时，传感器会记录激光束到达的时间。
+   
+4. **计算位置和方向**：
+   
+   - 激光扫描一个完整的周期需要已知的时间（例如，假设为 T）。 
+   
+     ![image-20240607153755424](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_location\Note\Images\lighthouse原理1.png)
+   
+   - 角度到位置的转换
+   
+     ![image-20240607153823884](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_location\Note\Images\lighthouse原理2.png)
+   
+     ![image-20240607153911054](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_location\Note\Images\lighthouse原理3.png)
+     
+     ![image-20240613204029743](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_location\Note\Images\lighthouse原理4.png)
+##### 卡尔曼滤波
+
+卡尔曼滤波是一种线性二次估计算法，能够在存在噪声和不确定性的情况下，通过对状态变量的动态估计，得到系统的最优估计。
+
+**卡尔曼滤波的基本步骤**
+
+卡尔曼滤波器的操作过程可以分为两个阶段：预测阶段和更新阶段。
+
+![image-20240613203928033](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_location\Note\Images\kalman滤波_预测.png)
+
+![image-20240613203954828](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_location\Note\Images\kalman滤波_更新.png)
+
+
+
+
+### 二.主要源文件
+
+- type.h
+
+  **主要功能**：存储定位过程中的各种数据格式。
+
+  **示例**：
+
+  - IMU融合里程计数据结构体：
+
+    ```cpp
+    struct Imu_odom_pose_data
+    {
+        Imu_data imu_data_;
+        double pose_[3];//位置
+        double quat_[4];//姿态四元数
+        double vxy_;//平面速度
+        double angle_v_;//角速度
+        Imu_odom_pose_data(){};
+        Imu_odom_pose_data( Imu_data imu_data,double x,double y,double z, double qw, double qx, double qy, double qz,double vxy, double angle_v):imu_data_(imu_data),pose_{x,y,z},quat_{qw,qx,qy,qz},vxy_(vxy),angle_v_(angle_v){};
+    };
+    ```
+  
+  - IMU数据结构体：
+  
+    ```cpp
+    struct Imu_data
+    {
+        ros::Time imu_t_;
+        double a_[3];//线性加速度
+        double w_[3];//角速度
+        Imu_data(){};
+        Imu_data(double ax,double ay,double az, double wx, double wy, double wz)
+        :a_{ax,ay,az},w_{wz,wy,wz}{};
+    };
+    ```
+
+- uwb.cpp
+
+  **关联头文件**：uwb.h
+
+  **头文件重要参数**：UWB数据对象、互斥锁、基站信息。
+
+  **主要功能**：初始化一些用于定位的锚点（Anchor）的位置信息；初始化用于定位计算的矩阵，通过串口读取UWB设备的数据，对读取到的距离数据进行校正，去除高度差的影响，利用校正后的距离数据和预设的锚点位置，计算目标点的二维平面坐标。
+
+  **UWB定位算法原理：**
+
+  UWB（超宽带）定位算法主要通过测量目标点到多个已知位置的锚点的距离，利用几何方法来计算目标点的位置：
+
+  ![image-20240606145817153](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_location\Note\Images\uwb定位原理1.png)
+
+  ![image-20240606145837783](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_location\Note\Images\uwb定位原理2.png)
+
+  ![image-20240606145941659](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_location\Note\Images\uwb定位原理3.png)
+
+  **4.矩阵表示：**
+  $$
+  A=\begin{pmatrix}
+    2(x_2 - x_1)& 2(y_2 - y_1) \\
+    2(x_3 - x_1)& 2(y_3 - y_1)
+  \end{pmatrix}
+  \\
+  B=\begin{pmatrix}
+   d_2^2 - d_1^2 + x_1^2 - x_2^2 + y_1^2 - y_2^2\\
+  d_3^2 - d_1^2 + x_1^2 - x_3^2 + y_1^2 - y_3^2 
+  \end{pmatrix}
+  \\
+  A\begin{pmatrix}
+   x\\
+  y 
+  \end{pmatrix}=B
+  $$
+  
+
+  **主要函数**：
+
+  ##### 1. `Uwb::Uwb()`
+
+  - **作用**: UWB类的构造函数，用于初始化一些变量和矩阵。
+
+  - **步骤：**
+
+    根据UWB定位算法中目标点的距离差方程来初始化系数矩阵A，以便后续计算目标点坐标。
+
+    ```cpp
+    for(int i=0; i<2; i++)
+            {
+                A.mat[i][0] = -2*(this->AnchorPos[0][0]-this->AnchorPos[i+1][0]);
+                A.mat[i][1] = -2*(this->AnchorPos[0][1]-this->AnchorPos[i+1][1]);
+            
+            }
+    ```
+
+  ##### 2. `Run()`
+
+  ```c++
+  void Uwb::Run()
+  ```
+
+  - **作用**: 通过一个无限循环不断调用`Serread()`函数来读取串口数据。
+
+  ##### 3. `Serread()`
+
+  ```c++
+  void Uwb::Serread()
+  ```
+
+  - **作用**: 用于从UWB设备读取数据，并进行距离校正和位置计算。
+
+  - **函数过程：**
+
+  - 这个函数的主要工作包括：
+
+    - 初始化串口通信参数并读取数据。
+
+      ```cpp
+      boost::asio::io_service io;
+              boost::asio::serial_port serial(io, "/dev/uwb");
+      //设置串口路径
+              serial.set_option(boost::asio::serial_port_base::baud_rate(115200));// 设置波特率
+              serial.set_option(boost::asio::serial_port_base::character_size(8));// 设置数据位
+              serial.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));// 设置校验位
+              serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));// 设置停止位
+      
+              uint8_t tmpdata[9];
+              size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, 9));// 读取串口数据
+      ```
+
+    - 解析和校正读取到的距离数据。
+
+      ```cpp
+      this->uwb_data_.uwb_t_ = ros::Time::now();
+              
+              // for (int i=0;i< bytesRead;i++)
+              // {
+              //     std::cout << std::hex<<static_cast<int>(tmpdata[i]) << " ";
+              // }
+              // std::cout << std::endl;
+              memcpy(&this->d[0], &tmpdata[3], sizeof(uint16_t));
+              memcpy(&this->d[1], &tmpdata[5], sizeof(uint16_t));
+              memcpy(&this->d[2], &tmpdata[7], sizeof(uint16_t));
+              // std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl;
+              if(abs(d[0]) > 2000 || abs(d[1]) > 2000 || abs(d[2]) > 2000) {
+                  return;
+              }
+              for(int i=0; i<3; i++)
+              {
+                  this->d[i] = sqrt(this->d[i] * this->d[i] - (AnchorPos[i][2] - CARHEIGHT) * (AnchorPos[i][2] - CARHEIGHT));//对距离进行校正，去除高度差影响，即目标点到锚点的水平距离(不考虑Z后续计算目标点坐标只需求X,Y)
+              }
+      //通过多项式校正原始测量距离的非线性误差。
+              d[0] = ((((4.9083e-07 * d[0]) - 4.6166e-04) * d[0]) + 1.0789) * d[0] + 5.4539;
+              d[1] = ((((-4.1679e-07 * d[1]) + 5.0999e-04) * d[1]) + 0.7930) * d[1] + 29.8296;
+              d[2] = ((((2.3514e-07 * d[2]) - 1.8277e-04) * d[2]) + 0.9935) * d[2] + 9.8852;
+              std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl;
+      ```
+
+    - 利用校正后的距离数据和线性方程组求解目标点的二维坐标。
+
+      ```cpp
+      for(int i=0; i<2; i++)
+              {
+                  b.mat[i][0] = (this->d[0]*this->d[0]-this->d[i+1]*this->d[i+1])\
+                              -(this->AnchorPos[0][0]*this->AnchorPos[0][0]-this->AnchorPos[i+1][0]*this->AnchorPos[i+1][0])\
+                              -(this->AnchorPos[0][1]*this->AnchorPos[0][1]-this->AnchorPos[i+1][1]*this->AnchorPos[i+1][1]);
+              }
+              Mat AT=~A;
+              uwbPos=(AT*A)%AT*b;
+              this->uwb_data_.x_ = uwbPos.mat[0][0];
+              this->uwb_data_.y_ = uwbPos.mat[1][0];
+      ```
+
+- Mat.cpp
+
+  **关联头文件：**Mat.h
+
+  **主要功能**：实现了一个矩阵类 `Mat`，以及各种矩阵操作函数。这些函数包括矩阵的初始化、基本运算（加减乘除）、矩阵变换（转置）、高斯消元法求解线性方程组、矩阵的行列操作、以及一些向量操作。
+
+  **主要函数：** 
+
+  - 矩阵求解（高斯消元法）
+  
+  ```cpp
+  Mat operator /(Mat a, Mat b) {
+      int x, xb;
+      for (x = 0; x < b.n; x++) {
+          double s = 0;
+          int p = x;
+          double sxb;
+          for (xb = x; xb < b.n; xb++) {
+              sxb = fabs(b.mat[x][xb]);
+              if (sxb > s) {
+                  p = xb;
+                  s = sxb;
+              }
+          }
+          if (x != p) {
+              a.ColExchange(x, p);
+              b.ColExchange(x, p);
+          }
+          double k = 1 / b.mat[x][x];
+          a.ColMul(x, k);
+          b.ColMul(x, k);
+          for (xb = 0; xb < b.n; xb++) {
+              if (xb != x) {
+                  k = (-b.mat[x][xb]);
+                  a.ColAdd(xb, x, k);
+                  b.ColAdd(xb, x, k);
+              }
+          }
+      }
+      return a;
+  }
+  
+  Mat operator %(Mat a, Mat b) {
+      int x, xb;
+      for (x = 0; x < a.m; x++) {
+          double s = 0;
+          int p = x;
+          double sxb;
+          for (xb = x; xb < a.m; xb++) {
+              sxb = fabs(a.mat[xb][x]);
+              if (sxb > s) {
+                  p = xb;
+                  s = sxb;
+              }
+          }
+          if (x != p) {
+              a.RowExchange(x, p);
+              b.RowExchange(x, p);
+          }
+          double k = 1 / a.mat[x][x];
+          a.RowMul(x, k);
+          b.RowMul(x, k);
+          for (xb = 0; xb < a.m; xb++) {
+              if (xb != x) {
+                  k = (-a.mat[xb][x]);
+                  a.RowAdd(xb, x, k);
+                  b.RowAdd(xb, x, k);
+              }
+          }
+      }
+      return b;
+  }
+  ```
+  
+  ​	重载了 `/` 和 `%` 运算符，用于求解矩阵方程 `AX = B` 和 `XA = B`，使用高斯消元法。
+  
+  - 矩阵行列操作
+  
+  ```cpp
+  void Mat::RowExchange(int a, int b) {
+      double s[MAT_MAX];
+      int ncpy = n * sizeof(double);
+      memcpy(s, mat[a], ncpy);
+      memcpy(mat[a], mat[b], ncpy);
+      memcpy(mat[b], s, ncpy);
+  }
+  
+  void Mat::RowMul(int a, double k) {
+      int y;
+      for (y = 0; y < n; y++) {
+          mat[a][y] = mat[a][y] * k;
+      }
+  }
+  
+  void Mat::RowAdd(int a, int b, double k) {
+      int y;
+      for (y = 0; y < n; y++) {
+          mat[a][y] = mat[a][y] + mat[b][y] * k;
+      }
+  }
+  
+  void Mat::ColExchange(int a, int b) {
+      double s;
+      int x;
+      for (x = 0; x < m; x++) {
+          s = mat[x][a];
+          mat[x][a] = mat[x][b];
+          mat[x][b] = s;
+      }
+  }
+  
+  void Mat::ColMul(int a, double k) {
+      int x;
+      for (x = 0; x < m; x++) {
+          mat[x][a] = mat[x][a] * k;
+      }
+  }
+  
+  void Mat::ColAdd(int a, int b, double k) {
+      int x;
+      for (x = 0; x < m; x++) {
+          mat[x][a] = mat[x][a] + mat[x][b] * k;
+      }
+  }
+  ```
+  
+  ​		实现了矩阵的行交换、行乘标量、行加倍数、列交换、列乘标量、列加倍数等操作。
+  
+- read_sensor_data.cpp
+
+  **关联头文件：**read_sensor_data.h
+
+  **头文件重要参数：**里程计消息以及 IMU 消息指针、 IMU 数据订阅器和里程计数据订阅器。
+
+  **主要功能**：读取IMU和里程计传感器的数据。
+
+  **主要函数：** 
+
+  **IMU回调函数**
+
+  ```cpp
+  void Read_sensor_data::imu_call_back(const ImuConstPtr& imu) {
+      Imu_data d1 = Imu_data(imu->linear_acceleration.x, imu->linear_acceleration.y, imu->linear_acceleration.z,
+                             imu->angular_velocity.x, imu->angular_velocity.y, imu->angular_velocity.z);
+  }
+  ```
+
+  这是一个IMU数据的回调函数，当接收到IMU传感器的数据时调用。
+  
+  - `const ImuConstPtr& imu`：IMU数据的常量指针引用。
+  - 函数内部将IMU数据提取并传递给`Imu_data`对象`d1`。
+  
+  **里程计回调函数**
+  
+  ```cpp
+  void Read_sensor_data::odom_call_back(const OdomConstPtr& odom) {
+      Odom_data d1 = Odom_data(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z,
+                             odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, odom->pose.pose.orientation.y, odom->pose.pose.orientation.z,
+                               odom->header.stamp, odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.angular.z);
+  }
+  ```
+  
+  这是一个里程计数据的回调函数，当接收到里程计传感器的数据时调用。
+  
+  - `const OdomConstPtr& odom`：里程计数据的常量指针引用。
+  
+  - 函数内部将里程计数据提取并传递给`Odom_data`对象`d1`。
+  
+  **运行函数**
+  
+  ```cpp
+  void Read_sensor_data::Run(int argc, char* argv[]) {
+      ros::init(argc, argv, "imu_odom");
+      // 创建一个节点句柄
+      ros::NodeHandle nh;
+      //imu_sub_ = nh.subscribe<std_msgs::String>("imu/data_raw", 1000, this->imu_call_back);
+      //odom_sub_ = nh.subscribe("odom", 1000, odom_call_back);
+      ros::spin();
+  }
+  ```
+
+  这是类的主函数，负责初始化ROS节点并订阅传感器话题。
+
+  - `int argc, char* argv[]`：传递给ROS初始化函数的参数。
+  - `ros::init(argc, argv, "imu_odom")`：初始化ROS节点，节点名称为`imu_odom`。
+  - `ros::NodeHandle nh`：创建一个节点句柄，用于与ROS系统交互。
+  - `imu_sub_`和`odom_sub_`：分别订阅IMU和里程计数据的话题（被注释掉的代码行）。
+  - `ros::spin()`：进入ROS循环，等待回调函数的调用。
+  
+- lighthouse.cpp
+
+  **关联头文件：**lighthouse.h
+
+  **头文件重要参数：**lighthouse数据对象、读写锁。
+
+  **主要功能**：通过UDP接收位置信息并处理这些数据（真值数据）。
+
+  **主要函数：** 
+
+  （1）构造函数：`Lighthouse::Lighthouse()`
+
+  - **功能**：初始化`Lighthouse`对象，创建UDP套接字，并将其绑定到指定端口。
+  - **步骤：**
+    1. 输出启动信息。
+    2. 创建UDP套接字。如果创建失败，则输出错误信息并返回。
+    3. 设置套接字地址结构`udpAddress`，指定协议族为`AF_INET`，地址为`INADDR_ANY`，端口为`PORT`。
+    4. 将套接字绑定到地址。如果绑定失败，输出错误信息并关闭套接字。
+  
+  （2）运行函数：`void Lighthouse::Run()`
+  
+  - **功能**：启动一个无限循环，在该循环中调用`UDPRead`函数以读取UDP数据。
+  
+  - **步骤：**
+  1. 进入无限循环，调用`UDPRead`函数。
+  
+  （3）读取UDP数据函数：`void Lighthouse::UDPRead()`
+  
+  - **功能**：从UDP套接字接收数据并解析这些数据以提取位置信息和方向信息。
+  
+  - **步骤：**
+    1. 定义一个缓冲区`buffer`以存储接收到的数据。
+    
+    2. 使用`recvfrom`函数从UDP套接字接收数据。如果接收失败，输出错误信息并返回。
+    
+    3. 将接收到的数据转换为字符串格式。
+    
+    4. 解析数据字符串，提取位置信息（x, y, z）和方向信息（qx, qy, qz）。固定`qw`为1.0。
+    
+    5. 使用互斥锁`mMutexLighthouse`保护对共享数据的访问，更新`data`成员变量。
+    
+    6. 打印接收到的消息（注释掉的部分）。
+  
+- align.cpp
+
+  **关联头文件：**align.h
+
+  **头文件重要参数：**用于接收IMU、里程计数据的话题订阅器、用于融合多传感器数据的中间矩阵、存储最终融合结果的数据对象。
+
+  **主要功能**：实现基于IMU（惯性测量单元）和UWB（超宽带）数据的融合，用于机器人位姿（位置和方向）的估计和同步。
+
+  **主要函数：** 
+
+  1. `Align::Run()`
+
+  该函数是主处理循环，负责初始化订阅者，读取数据，进行数据处理和融合，并将结果保存到文件中。
+
+  - **初始化数据接收时间**
+
+    ```c++
+    imuDataRxTime = ros::Time::now();
+    uwbDataRxTime = ros::Time::now();
+    ```
+
+  - **订阅IMU和里程计数据**
+
+    ```c++
+    imu_sub_ = nh_.subscribe("raw_imu", 10, &Align::imuCB, this);
+    odom_imu_sub_ = nh_.subscribe("odom_imu_ekf", 10, &Align::odom_imuCB, this);
+    ```
+
+  - **打开文件保存数据**
+
+    ```c++
+    std::ofstream outfile("/home/firefly/pibot_ros/ros_ws/src/upbot_location/data.txt", std::ofstream::out);
+    ```
+
+  - **初始化卡尔曼滤波所需的矩阵**
+
+    ```cpp
+    Mat prevPos(2, 1, 0);
+    Mat deltaPos(2, 1, 0);
+    Mat predPos(2, 1, 0);
+    Mat R(2, 2, 1);
+    Mat Q(2, 2 ,1);
+    Q = Q * 0.00001;
+    Mat P(2, 2, 0);
+    Mat F(2, 2, 1);
+    Mat Kg(2, 2, 0);
+    Mat Z(2, 2, 0);
+    ```
+
+  - **设置初始位置**
+
+    ```cpp
+    uwbStartPos.mat[0][0] = 9;
+    uwbStartPos.mat[1][0] = 0;
+    ```
+
+  - **进入主循环，处理数据** 在主循环中，不断检查IMU和UWB数据是否更新，进行数据融合，并通过卡尔曼滤波器计算机器人的同步位置。
+
+    ```cpp
+    while(1) {
+        if(imuDataRxTime != imu_odom_.imu_data_.imu_t_) {
+            // 处理IMU数据
+        }
+        if (uwbDataRxTime != uwb_->uwb_data_.uwb_t_) {
+            // 处理UWB数据
+        }
+    }
+    ```
+
+   2. `Align::imuCB(const pibot_msgs::RawImu& imu)`
+
+  该函数是IMU数据的回调函数，用于接收和处理IMU数据。
+
+  - 更新IMU数据
+
+    ```cpp
+    imu_odom_.imu_data_.imu_t_ = imu.header.stamp;
+    imu_odom_.imu_data_.a_[0] = imu.raw_linear_acceleration.x;
+    imu_odom_.imu_data_.a_[1] = imu.raw_linear_acceleration.y;
+    imu_odom_.imu_data_.a_[2] = imu.raw_linear_acceleration.z;
+    imu_odom_.imu_data_.w_[0] = imu.raw_angular_velocity.x;
+    imu_odom_.imu_data_.w_[1] = imu.raw_angular_velocity.y;
+    imu_odom_.imu_data_.w_[2] = imu.raw_angular_velocity.z;
+    ```
+
+   3. `Align::odom_imuCB(const nav_msgs::Odometry& odom)`
+
+  该函数是IMU里程计数据的回调函数，用于接收和处理融合后的IMU里程计数据。
+
+  - 更新里程计数据
+
+    ```cpp
+    cppodom_tmp_ = odom.header.stamp;
+    imu_odom_.pose_[0] = odom.pose.pose.position.x;
+    imu_odom_.pose_[1] = odom.pose.pose.position.y;
+    imu_odom_.pose_[2] = odom.pose.pose.position.z;
+    imu_odom_.quat_[0] = odom.pose.pose.orientation.x;
+    imu_odom_.quat_[1] = odom.pose.pose.orientation.y;
+    imu_odom_.quat_[2] = odom.pose.pose.orientation.z;
+    imu_odom_.quat_[3] = odom.pose.pose.orientation.w;
+    imu_odom_.vxy_ = odom.twist.twist.linear.x;
+    imu_odom_.angle_v_ = odom.twist.twist.angular.z;
+    ```

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h

@@ -25,30 +25,31 @@ namespace uwb_slam{
             imu_odom_StartPos.Init(2, 1, 0);
             Rotate.Init(2,2,0);
             uwbStartPos.Init(2,1,0);
-        };
+        };//初始化多个mat
-        void Run();
+        void Run();//主运行函数 
-        void imuCB(const pibot_msgs::RawImu& imu);
+        void imuCB(const pibot_msgs::RawImu& imu);//在接收到IMU数据时调用，处理IMU数据
-        void odom_imuCB(const nav_msgs::Odometry& odom);
+        void odom_imuCB(const nav_msgs::Odometry& odom);//在接收到里程计数据时调用，处理里程计数据
 
     public:
         ros::NodeHandle nh_;
-        ros::Subscriber imu_sub_;
+        ros::Subscriber imu_sub_;//imu订阅者    
-        ros::Subscriber odom_imu_sub_;
+        ros::Subscriber odom_imu_sub_;//odom订阅者
-        Imu_odom_pose_data imu_odom_;
+        Imu_odom_pose_data imu_odom_;//用于存储IMU和里程计数据的结构体实例。
-        Uwb_data uwb_data_;
+        Uwb_data uwb_data_;//用于存储UWB数据的结构体实例。
-        ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime;
+        ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime;//用于记录IMU、UWB和里程计数据接收时间的ROS时间对象。
         Mat imu_odom_StartPos;
         Mat imu_odomPos;
         Mat uwbPos;
-        Mat syncPos;
+        Mat syncPos;//同步位置，通过卡尔曼滤波得到的最优位置估计，结合IMU和UWB数据
         Mat Rotate;
         Mat uwbStartPos;
-        double qx, qy, qz, qw;
+        //自定义矩阵类 Mat 的实例，用于存储和计算位置、旋转等信息。
-        ros::Time odom_tmp_ ;
+        double qx, qy, qz, qw;//表示旋转
+        ros::Time odom_tmp_ ;//存储临时的里程计时间。
         bool write_data_ = false;
         cv::Mat img1;
-        std::queue<std::pair<Imu_odom_pose_data,Uwb_data>> data_queue;
+        std::queue<std::pair<Imu_odom_pose_data,Uwb_data>> data_queue;//队列，用于存储IMU和UWB数据对。
-        std::shared_ptr<uwb_slam::Uwb> uwb_;
+        std::shared_ptr<uwb_slam::Uwb> uwb_;//指向 Uwb 类实例的智能指针，用于处理UWB数据。
         std::shared_ptr<uwb_slam::Lighthouse> lighthouse_;
     };
 };

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/type.h

@@ -7,8 +7,8 @@ namespace uwb_slam{
 struct Imu_data
 {
     ros::Time imu_t_;
-    double a_[3];
+    double a_[3];//线性加速度
-    double w_[3];
+    double w_[3];//角速度
     Imu_data(){};
     Imu_data(double ax,double ay,double az, double wx, double wy, double wz)
     :a_{ax,ay,az},w_{wz,wy,wz}{};
@@ -19,10 +19,10 @@ struct Imu_data
 struct Imu_odom_pose_data
 {
     Imu_data imu_data_;
-    double pose_[3];
+    double pose_[3];//位置
-    double quat_[4];
+    double quat_[4];//姿态四元数
-    double vxy_;
+    double vxy_;//平面速度
-    double angle_v_;
+    double angle_v_;//角速度
     Imu_odom_pose_data(){};
     Imu_odom_pose_data( Imu_data imu_data,double x,double y,double z, double qw, double qx, double qy, double qz,double vxy, double angle_v):imu_data_(imu_data),pose_{x,y,z},quat_{qw,qx,qy,qz},vxy_(vxy),angle_v_(angle_v){};
 };
@@ -45,7 +45,7 @@ struct Imu_odom_pose_data
 
 struct  Uwb_data
 {
-    float x_,y_;
+    float x_,y_;//位置信息
     ros::Time uwb_t_;
     Uwb_data(){};
     Uwb_data(float x,float y,float t):x_(x),y_(y),uwb_t_(t){};
@@ -53,8 +53,8 @@ struct  Uwb_data
 
 struct LightHouseData
 {
-    float x_,y_,z_;
+    float x_,y_,z_;//位置
-    float qw_,qx_,qy_,qz_;
+    float qw_,qx_,qy_,qz_;//四元数
     ros::Time lighthouse_t_;
     LightHouseData(){};
     LightHouseData(float x,float y,float z, float qw, float qx, float qy, float qz, float t):

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/Mat.cpp

@@ -47,7 +47,7 @@ void Mat::Init(int setm,int setn,int kind)
 	{
 		memset(mat,0,MAT_MAX*MAT_MAX*sizeof(double));
 	}
-
+    //单位矩阵
 	if(kind==1)
 	{
 		int x;
@@ -116,6 +116,7 @@ Mat operator /(Mat a,double k)
 {
 	return (1/k)*a;
 }
+//矩阵乘法
 Mat operator *(Mat a,Mat b)
 {
 	Mat c(a.m,b.n,-1);
@@ -181,7 +182,7 @@ Mat operator ~(Mat a)
 }
 
 
-
+//求解AX = B
 Mat operator /(Mat a,Mat b)
 {
 	//高斯消元法
@@ -229,7 +230,7 @@ Mat operator /(Mat a,Mat b)
 	return a;
 }
 
-
+//求解XA = B
 Mat operator %(Mat a,Mat b)
 {
 	//高斯消元法
@@ -348,7 +349,7 @@ double Mat::absvec()
 {
 	return sqrt(Sqrt());
 }
-
+//向量叉积
 Mat operator ^(Mat a, Mat b)
 {
 	double ax=a.mat[0][0];

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp

@@ -26,16 +26,16 @@ namespace uwb_slam{
             std::cout<<"file can not open"<<std::endl;
         }
         
-        Mat prevPos(2, 1, 0);
+        Mat prevPos(2, 1, 0);//之前的矩阵
-        Mat deltaPos(2, 1, 0);
+        Mat deltaPos(2, 1, 0);//当前位置与之前位置的差值
-        Mat predPos(2, 1, 0);
+        Mat predPos(2, 1, 0);//预测的位置
-        Mat R(2, 2, 1);
+        Mat R(2, 2, 1);//测量噪声协方差矩阵
-        Mat Q(2, 2 ,1);
+        Mat Q(2, 2 ,1);//过程噪声协方差矩阵
-        Q = Q * 0.00001;
+        Q = Q * 0.00001;//
-        Mat P(2, 2, 0);
+        Mat P(2, 2, 0);//估计误差协方差矩阵
-        Mat F(2, 2, 1);
+        Mat F(2, 2, 1);//状态转移矩阵
-        Mat Kg(2, 2, 0);
+        Mat Kg(2, 2, 0);//卡尔曼增益矩阵
-        Mat Z(2, 2, 0);
+        Mat Z(2, 2, 0);//测量值矩阵
 
         uwbStartPos.mat[0][0]=9;
         uwbStartPos.mat[1][0]=0;
@@ -52,27 +52,32 @@ namespace uwb_slam{
             // } else if (key3 == 'e') {
             //     printFlag = 0;
             // }
+            //检查IMU数据是否更新
             if(imuDataRxTime!=imu_odom_.imu_data_.imu_t_){
                 // std::cout << "imu received" << std::endl;
+                //更新IMU位置
                 imu_odomPos.mat[0][0] = imu_odom_.pose_[0] * 100;
                 imu_odomPos.mat[1][0] = imu_odom_.pose_[1] * 100;
                 qx = imu_odom_.quat_[0];
                 qy = imu_odom_.quat_[1];
                 qz = imu_odom_.quat_[2];
                 qw = imu_odom_.quat_[3];
-                yaw = -atan2(2*(qw*qz+qx*qy),1-2*(qy*qy+qz*qz));
+                yaw = -atan2(2*(qw*qz+qx*qy),1-2*(qy*qy+qz*qz));//机器人的航向角
 
                 //std::cout<<"yaw:"<<yaw<<std::endl;
                 if (imuStartFlag == 1 && imu_odomPos.mat[0][0] != 0 && imu_odomPos.mat[1][0] != 0) {
                     imuStartyaw = yaw-PI/2;
                     imu_odom_StartPos = imu_odomPos;
                     imuStartFlag = 0;
-                }
+                }//记录初始航向角和初始位置
                 //std::cout << imu_odom_StartPos.mat[0][0] << " " << imu_odom_StartPos.mat[1][0] << std::endl;
                 yaw = yaw - imuStartyaw;
                 imu_odomPos = imu_odomPos - imu_odom_StartPos;
+                //调整yaw角以考虑初始偏移，并更新相对于初始位置的相对位置
                 deltaPos = imu_odomPos - prevPos;
                 prevPos = imu_odomPos;
+                //计算位置变化，更新prevPOS
+                //检查UWB数据是否更新
                 if (uwbDataRxTime != uwb_->uwb_data_.uwb_t_) {
                     // std::cout << "uwb received" << std::endl;
 
@@ -83,16 +88,17 @@ namespace uwb_slam{
                   
                     uwbPos.mat[0][0] = uwb_->uwb_data_.x_;
                     uwbPos.mat[1][0] = uwb_->uwb_data_.y_;
-
+                    //更新uwb初始位置
                     // uwbPos = uwbPos - Rotate * uwbStartPos;
 
-                    predPos = F * syncPos + deltaPos;
+                    predPos = F * syncPos + deltaPos;//预测下一个位置
                     Z = H * uwbPos;
                     P = F * P * (~F) + Q;
                     Kg = P * (~H) / (H * P * (~H) + R);
                     syncPos = predPos + Kg * (Z - H * predPos);
                     P = (I - Kg * H) * P;
                     uwbDataRxTime = uwb_->uwb_data_.uwb_t_;
+                    //如果uwb更新，使用卡尔曼滤波算法进行状态预测和更新
                 } else {
                     syncPos = syncPos + deltaPos;
                 }

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/lighthouse.cpp

@@ -16,7 +16,7 @@ namespace uwb_slam{
         std::cout << "Creating UDP socket sucess. "  << std::endl;
 
         // 设置套接字地址结构
-        sockaddr_in udpAddress;
+        sockaddr_in udpAddress;//存储UDP套接字的地址信息
         std::memset(&udpAddress, 0, sizeof(udpAddress));
         udpAddress.sin_family = AF_INET;
         udpAddress.sin_addr.s_addr = htonl(INADDR_ANY);
@@ -42,7 +42,7 @@ namespace uwb_slam{
 
     void Lighthouse::UDPRead(){
         char buffer[1024];
-        ssize_t bytesRead = recvfrom(UdpSocket, buffer, sizeof(buffer), 0, nullptr, nullptr);
+        ssize_t bytesRead = recvfrom(UdpSocket, buffer, sizeof(buffer), 0, nullptr, nullptr);//系统调用函数从指定UDP接收数据
         if (bytesRead == -1) {
             std::cerr << "Error receiving data." << std::endl;
             return;

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp

@@ -40,7 +40,7 @@ namespace uwb_slam
         int syncPosPointY = posData[2].y / PIXEL_SCALE + ( fmod(posData[2].y ,PIXEL_SCALE) != 0) + realHeight / 2;
         // std::cout << "syncPos:" <<posData[2].x << " " << posData[2].y;
         // std::cout << " uwbPos:" <<posData[1].x << " " << posData[1].x;
-        // std::cout << " imuPos:" <<posData[0].x << " " << posData[0].y << std::endl;
+        // std::cout << " imuPos:" <<posData[0].x << " " << p osData[0].y << std::endl;
 
         // std::cout << "syncPos:" <<syncPosPointX << " " << syncPosPointY;
         // std::cout << " uwbPos:" <<uwbPosPointX << " " << uwbPosPointY;

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp

@@ -21,9 +21,10 @@ namespace uwb_slam{
             std::cout<<std::endl;
             
         }
-        A.Init(2,2,0);
+        A.Init(2,2,0);//线性方程组中的系数矩阵
         b.Init(2,1,0);
         uwbPos.Init(2,1,0);
+        //根据UWB定位算法中目标点的距离差方程
         for(int i=0; i<2; i++)
         {
             A.mat[i][0] = -2*(this->AnchorPos[0][0]-this->AnchorPos[i+1][0]);
@@ -87,7 +88,7 @@ namespace uwb_slam{
         }
         for(int i=0; i<3; i++)
         {
-            this->d[i] = sqrt(this->d[i] * this->d[i] - (AnchorPos[i][2] - CARHEIGHT) * (AnchorPos[i][2] - CARHEIGHT));
+            this->d[i] = sqrt(this->d[i] * this->d[i] - (AnchorPos[i][2] - CARHEIGHT) * (AnchorPos[i][2] - CARHEIGHT));//对距离进行校正，去除高度差影响，即目标点到锚点的水平距离
         }
         d[0] = ((((4.9083e-07 * d[0]) - 4.6166e-04) * d[0]) + 1.0789) * d[0] + 5.4539;
         d[1] = ((((-4.1679e-07 * d[1]) + 5.0999e-04) * d[1]) + 0.7930) * d[1] + 29.8296;