diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/kalman滤波_更新.png b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/kalman滤波_更新.png new file mode 100644 index 0000000..ba159ca Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/kalman滤波_更新.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/kalman滤波_预测.png b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/kalman滤波_预测.png new file mode 100644 index 0000000..a7d5475 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/kalman滤波_预测.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理1.png b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理1.png new file mode 100644 index 0000000..cf1716f Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理1.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理2.png b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理2.png new file mode 100644 index 0000000..969c61a Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理2.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理3.png b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理3.png new file mode 100644 index 0000000..5d8f707 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理3.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理4.png b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理4.png new file mode 100644 index 0000000..b3124de Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/lighthouse原理4.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/uwb定位原理1.png b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/uwb定位原理1.png new file mode 100644 index 0000000..e07139d Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/uwb定位原理1.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/uwb定位原理2.png b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/uwb定位原理2.png new file mode 100644 index 0000000..a760b7f Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/uwb定位原理2.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/uwb定位原理3.png b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/uwb定位原理3.png new file mode 100644 index 0000000..78efb26 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/Images/uwb定位原理3.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/upbot_location阅读笔记.md b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/Note/upbot_location阅读笔记.md new file mode 100644 index 0000000..f2cdd05 --- /dev/null +++ b/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<(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("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; + ``` diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h index 601f30b..45b3743 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h +++ b/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); - }; - void Run(); - void imuCB(const pibot_msgs::RawImu& imu); - void odom_imuCB(const nav_msgs::Odometry& odom); + };//初始化多个mat + void Run();//主运行函数 + void imuCB(const pibot_msgs::RawImu& imu);//在接收到IMU数据时调用，处理IMU数据 + void odom_imuCB(const nav_msgs::Odometry& odom);//在接收到里程计数据时调用，处理里程计数据 public: ros::NodeHandle nh_; - ros::Subscriber imu_sub_; - ros::Subscriber odom_imu_sub_; - Imu_odom_pose_data imu_odom_; - Uwb_data uwb_data_; - ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime; + ros::Subscriber imu_sub_;//imu订阅者 + ros::Subscriber odom_imu_sub_;//odom订阅者 + Imu_odom_pose_data imu_odom_;//用于存储IMU和里程计数据的结构体实例。 + Uwb_data uwb_data_;//用于存储UWB数据的结构体实例。 + 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; - ros::Time odom_tmp_ ; + //自定义矩阵类 Mat 的实例，用于存储和计算位置、旋转等信息。 + double qx, qy, qz, qw;//表示旋转 + ros::Time odom_tmp_ ;//存储临时的里程计时间。 bool write_data_ = false; cv::Mat img1; - std::queue> data_queue; - std::shared_ptr uwb_; + std::queue> data_queue;//队列，用于存储IMU和UWB数据对。 + std::shared_ptr uwb_;//指向 Uwb 类实例的智能指针，用于处理UWB数据。 std::shared_ptr lighthouse_; }; }; diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/type.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/type.h index 93b8358..9c248f7 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/type.h +++ b/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 w_[3]; + 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}{}; @@ -19,10 +19,10 @@ struct Imu_data struct Imu_odom_pose_data { Imu_data imu_data_; - double pose_[3]; - double quat_[4]; - double vxy_; - double angle_v_; + 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){}; }; @@ -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 qw_,qx_,qy_,qz_; + float x_,y_,z_;//位置 + 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): diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/Mat.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/Mat.cpp index 8bbf4da..fcc58cc 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/Mat.cpp +++ b/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]; diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp index 9a96d95..cd4eb81 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp +++ b/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"<uwb_data_.uwb_t_) { // std::cout << "uwb received" << std::endl; @@ -80,19 +85,20 @@ namespace uwb_slam{ // Rotate.mat[0][1] = -sin(yaw); // Rotate.mat[1][0] = sin(yaw); // Rotate.mat[1][1] = cos(yaw); - + 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; } diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/lighthouse.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/lighthouse.cpp index 562b494..ebfa2dc 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/lighthouse.cpp +++ b/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; diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp index d81a85d..143dd78 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp +++ b/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:" <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;