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

@@ -1,561 +0,0 @@
-### 一.前提知识
-
-##### 数据格式
-
-- 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,31 +25,30 @@ 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);//在接收到IMU数据时调用，处理IMU数据
+        void imuCB(const pibot_msgs::RawImu& 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_;//imu订阅者    
+        ros::Subscriber imu_sub_;
-        ros::Subscriber odom_imu_sub_;//odom订阅者
+        ros::Subscriber odom_imu_sub_;
-        Imu_odom_pose_data imu_odom_;//用于存储IMU和里程计数据的结构体实例。
+        Imu_odom_pose_data imu_odom_;
-        Uwb_data uwb_data_;//用于存储UWB数据的结构体实例。
+        Uwb_data uwb_data_;
-        ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime;//用于记录IMU、UWB和里程计数据接收时间的ROS时间对象。
+        ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime;
         Mat imu_odom_StartPos;
         Mat imu_odomPos;
         Mat uwbPos;
-        Mat syncPos;//同步位置，通过卡尔曼滤波得到的最优位置估计，结合IMU和UWB数据
+        Mat syncPos;
         Mat Rotate;
         Mat uwbStartPos;
-        //自定义矩阵类 Mat 的实例，用于存储和计算位置、旋转等信息。
+        double qx, qy, qz, qw;
-        double qx, qy, qz, qw;//表示旋转
+        ros::Time odom_tmp_ ;
-        ros::Time odom_tmp_ ;//存储临时的里程计时间。
         bool write_data_ = false;
         cv::Mat img1;
-        std::queue<std::pair<Imu_odom_pose_data,Uwb_data>> data_queue;//队列，用于存储IMU和UWB数据对。
+        std::queue<std::pair<Imu_odom_pose_data,Uwb_data>> data_queue;
-        std::shared_ptr<uwb_slam::Uwb> uwb_;//指向 Uwb 类实例的智能指针，用于处理UWB数据。
+        std::shared_ptr<uwb_slam::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,7 +116,6 @@ Mat operator /(Mat a,double k)
 {
 	return (1/k)*a;
 }
-//矩阵乘法
 Mat operator *(Mat a,Mat b)
 {
 	Mat c(a.m,b.n,-1);
@@ -182,7 +181,7 @@ Mat operator ~(Mat a)
 }
 
 
-//求解AX = B
+
 Mat operator /(Mat a,Mat b)
 {
 	//高斯消元法
@@ -230,7 +229,7 @@ Mat operator /(Mat a,Mat b)
 	return a;
 }
 
-//求解XA = B
+
 Mat operator %(Mat a,Mat b)
 {
 	//高斯消元法
@@ -349,7 +348,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,32 +52,27 @@ 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;
 
@@ -88,17 +83,16 @@ 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;//存储UDP套接字的地址信息
+        sockaddr_in udpAddress;
         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);//系统调用函数从指定UDP接收数据
+        ssize_t bytesRead = recvfrom(UdpSocket, buffer, sizeof(buffer), 0, nullptr, nullptr);
         if (bytesRead == -1) {
             std::cerr << "Error receiving data." << std::endl;
             return;

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

@@ -21,10 +21,9 @@ 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]);
@@ -88,7 +87,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;

Code/MowingRobot/pibot_ros/runtime/RK3588/Linux/librknn_api/aarch64/librknn_api.so

@@ -1 +0,0 @@
-librknnrt.so

Code/MowingRobot/pibot_ros/runtime/RK3588/Linux/librknn_api/aarch64/librknn_api.so

@@ -0,0 +1 @@
+librknnrt.so

Code/MowingRobot/pibot_ros/third_party/libuvc/.gitattributes

@@ -1,4 +0,0 @@
-# make sure that .gitignore, .travis.yml,... are not part of a
-# source-package generated via 'git archive'
-.git*      	export-ignore
-/.*		export-ignore

Code/MowingRobot/pibot_ros/third_party/libuvc/.gitignore

@@ -1,2 +0,0 @@
-# Build directory (recommended location)
-build/

Code/MowingRobot/pibot_ros/third_party/libuvc/.travis.yml

@@ -1,16 +0,0 @@
-sudo: false
-language: cpp
-compiler:
- - gcc
- - clang
-addons:
- apt:
-  packages:
-   - libusb-1.0-0-dev
-   - libjpeg-dev
-before_script:
- - mkdir build
- - cd build
- - cmake ..
-script:
- - make

Code/MowingRobot/pibot_ros/third_party/so/libopencv_highgui.so

@@ -1 +0,0 @@
-/usr/local/lib/libopencv_highgui.so

Code/MowingRobot/pibot_ros/third_party/so/libopencv_highgui.so

@@ -0,0 +1 @@
+/usr/local/lib/libopencv_highgui.so