增加摄像头打开异常情况下的解决方案

合并瑞瑞提交

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

@@ -0,0 +1,628 @@
+### 一.目录结构
+
+include：包含头文件
+
+model：存放用于目标检测的权重文件
+
+src：源文件
+
+### 二.主要源文件
+
+#### 2.1 前提知识
+
+**1.相机成像**
+
+**四个坐标系：**
+
+图像处理、立体视觉等等方向常常涉及到四个坐标系:世界坐标系、相机坐标系、图像坐标系、像素坐标系。
+
+- 世界坐标系：代表物体在三维世界里的真实坐标，坐标用(Xw, Yw, Zw)表示，其中的w可以认为是world的缩写
+
+- 相机坐标系：代表以相机光学中心为原点的坐标，Z轴与光轴重合，坐标用(Xc, Yc, Zc)表示，其中的c可以认为是camera的缩写
+
+- 图像坐标系：代表相机拍摄时，在成像平面上使用的坐标系，成像平面和相机光轴的交点为原点，坐标用(X, Y)表示
+
+- 像素坐标系：在相机成像平面上的图像，通常情况下我们不能直接使用，我们定义了一套新的坐标系来表示在电子设备上被显示的图像，图像的左上角是原点，坐标系用(u, v)表示
+
+**<font color = "red">坐标系之间的转换：</font>**
+
+- 世界坐标系转相机坐标系
+
+  ![image-20240528142049084](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240528142049084.png)
+
+  这里的transform函数代表具体的转换过程，很好理解，从世界坐标系转成相机坐标系时，需要进行两种操作：旋转和平移：![image-20240528142259790](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240528142259790.png)
+
+  还可以继续用齐次坐标来简化，即把三维坐标增加一个维度，变成四维，第四维的值设为1：
+
+  ![image-20240528142325700](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_vision\Note\Images\世界转相机.png)
+  
+  上面的第二个矩阵就是我们常说的**外参矩阵**。
+
+- 相机坐标系转图像坐标系
+
+  有了相机坐标系的坐标，再根据相机成像的原理，即可推断出对应的成像平面的图像坐标：
+
+  ![                                                                ](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_vision\Note\Images\成像原理.png)
+
+  根据相似三角形的性质，很容易得出下面关系：
+
+  ![image-20240528142626714](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240528142626714.png)
+
+  写成矩阵乘的形式如下：![image-20240528142644836](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_vision\Note\Images\相机转图像.png)
+
+- 图像坐标转像素坐标
+  
+  像素坐标系和图像坐标系都在成像平面上，只是各自的原点和度量单位不一样。图像坐标系的原点为相机光轴与成像平面的交点，通常情况下是成像平面的中点或者叫principal point。图像坐标系的单位是mm，属于物理单位，而像素坐标系的单位是pixel，我们平常描述个像素点都是几行几列。所以这二者之间的转换如下:其中dx和dy表示每一列和每一行分别代表多少mm，即1pixel=dx mm。
+  
+  ![image-20240528143447600](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_vision\Note\Images\图像转像素.png)
+  
+  ![image-20240528143532858](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240528143532858.png)
+  
+  上式中就包含了我们常说的**相机的内参矩阵（intrinsics）**，单独提出来如下所示：
+  
+  ![image-20240528143035233](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240528143035233.png)
+
+![image-20240528172144383](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_vision\Note\Images\四大坐标系.png)
+
+![image-20240528172159999](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_vision\Note\Images\整体转换.png)
+
+**3.双目测距原理及步骤**
+
+（1）去畸变以及立体校正：消除相机镜头的畸变效应，使得图像中的物体保持准确的几何形状，提高图像处理的精度和准确性；校正立体摄像机的图像，使得左右摄像机的图像在几何上保持一致，以便后续的立体视觉处理。校正过程包括旋转和平移操作，以及重投影矩阵Q的计算，以确保左右摄像机的图像具有相同的视图和几何结构。
+
+（2）立体匹配：找出两个相机中相互匹配的点以便后续计算视差以及深度。
+
+（3）识别得到左右目中，待测物体的中心像素点坐标。
+
+（4）计算距离（深度信息）
+
+**4.测距过程中使用到的矩阵**
+
+测距过程使用到的矩阵除了上面1中提到的相机内外参两个矩阵，还包括有：
+
+- 畸变系数矩阵：用于相机标定过程中的去畸变。
+
+- 重投影矩阵Q：用于将视差图转换为3D点云。举例如下：
+  $$
+  \left(
+  \begin{matrix}
+  1 & 0 & 0 & -c_x \\ 0 & 1 & 0 & -c_y \\ 0 & 0 & 0 & f \\ 0 & 0 & -1/T_x & (c_x - c^{'}_x)/T_x 
+  \end{matrix}
+  \right)
+  $$
+  其中cx与cy代表相机主点（相机主光轴与像平面的交点即是主点）的图像坐标；f为重投影后的焦距；Tx相反数等于基线距离；$(c_x - c^{'}_x) $ 代表了左右摄像头图像中心的差距。
+
+- Z矩阵：从 3D 点云中提取的深度图，表示每个像素到相机的距离。
+
+#### 2.2 主要内容
+
+- detection.cc
+
+  **关联头文件**：dection.h
+
+  **主要功能**：对输入的视频帧进行目标检测。
+
+  **主要函数**：
+
+  - ``` Dection()``` ：整个构造函数的作用是在创建`Detection`对象时加载模型，并初始化相关的属性和参数，以备后续的使用；
+  - `detect_result_group_t Detection::outputParse(cv::Mat netInputImg)` ：接收一个图像作为输入，对其进行预处理，推理和后处理，最终的到检测结果保存到detect_result_group_t结构体中并返回。  
+  
+- <font color = "red">**ranging.cc**</font>
+
+  **关联头文件**：ranging.h
+
+  **头文件重要参数**：目标检测模型、相机内外参及畸变系数、图像校正矩阵、重投影矩阵、深度图等。
+
+  **主要功能**：利用两个摄像头拍摄的左右图像，通过图像校正、模板匹配和三维重建等技术来检测目标并计算其距离、宽度、高度和角度信息。系统的主要功能包括图像校正、像素坐标转相机坐标、模板匹配、水平估计以及从双目图像中提取测距信息。
+
+  **主要函数**：
+
+  ##### 1. `Ranging::rectifyImage`
+
+  ```cpp
+  void Ranging::rectifyImage(Mat &oriImgL, Mat &oriImgR)
+  ```
+
+  - **作用**: 对输入的左图像 `oriImgL` 和右图像 `oriImgR` 进行重映射校正，去除图像畸变。
+
+  - **参数:**
+    
+    - `oriImgL`: 左图像的引用。
+    
+    - `oriImgR`: 右图像的引用。
+    
+  - **步骤：**
+
+    使用OpenCV的remap函数对左右图像去畸变：`remap` 函数是 OpenCV 中用于图像重映射的函数，它可以根据给定的映射关系对图像进行变换：
+
+    ```cpp
+     remap(oriImgL, oriImgL, mapX1, mapX2, cv::INTER_LINEAR);//左照片重映射
+     remap(oriImgR, oriImgR, mapY1, mapY2, cv::INTER_LINEAR);//右照片重映射
+    ```
+
+    - `oriImgL`：这是原始的左侧图像，需要进行畸变校正和旋转校正。
+    - `mapX1` 和 `mapX2`：这两个参数是 `initUndistortRectifyMap` 函数计算得到的映射数组，用于指定图像的重映射关系。`mapX` 存储了水平方向的映射关系，`mapY` 则存储了垂直方向的映射关系。
+    - `cv::INTER_LINEAR`：这是插值方法的选项之一。在这里使用的是双线性插值方法，它会根据相邻像素的值进行加权平均，以便在重映射过程中获得更平滑的结果。
+  ##### 2. `Ranging::pic2cam`
+
+  ```c++
+  std::vector<float> Ranging::pic2cam(int u, int v)
+  ```
+
+  - **作用**: 将像素坐标 `(u, v)` 转换为相机坐标 `(x, y)`。
+  - **参数:**
+    - `u`: 像素坐标的横坐标。
+    - `v`: 像素坐标的纵坐标。
+  - **返回值**: 相机坐标 `(x, y)` 的向量。
+  - **依据公式：**相机坐标与像素坐标之间的转换：(u-u0)*f/fx；(v-v0)*f/fy
+
+  ##### 3. `Ranging::muban`
+
+  ```c++
+  std::vector<int> Ranging::muban(Mat &left_image, Mat &right_image, const int *coordinates)
+  ```
+
+  - **作用**: 使用模板匹配的方法根据右图像的指定区域在左图像中找到匹配的位置。
+
+  - **参数:**
+    
+    - `left_image`: 左图像的引用。
+    - `right_image`: 右图像的引用。
+    - `coordinates`: 感兴趣区域的坐标数组 `[x1, y1, x2, y2]`。
+    
+  - **返回值**: 匹配点的坐标向量 `[maxLoc.x, maxLoc.y]`。
+
+  - **重要库函数：**muban函数核心部分使用了 OpenCV 中的 `matchTemplate` 函数，用于在目标图像中寻找模板图像的匹配位置，并计算匹配程度。
+
+    ```cpp
+    matchTemplate(target, tpl, result, TM_CCOEFF_NORMED);
+    ```
+
+    - `target`：这是目标图像，即需要在其中寻找模板匹配的图像。
+    - `tpl`：这是模板图像，即用于匹配的参考图像。
+    - `result`：这是输出的匹配结果图像，用于存储匹配程度。它的大小通常会比目标图像小，因为匹配操作会在目标图像上滑动模板，并在每个位置计算匹配程度，因此结果图像会比目标图像的尺寸小。
+    - `TM_CCOEFF_NORMED`：这是匹配方法的选择参数。在这里使用的是归一化的相关系数匹配方法。该方法会在目标图像上滑动模板，并计算每个位置的相关系数（匹配程度），最终将相关系数归一化到 [0, 1] 的范围内。归一化的相关系数越接近于 1，表示匹配程度越高。
+
+    `matchTemplate` 函数的作用是在目标图像中寻找模板图像的匹配位置，并计算匹配程度。在这个例子中，使用的是归一化的相关系数匹配方法，可以有效地检测出目标图像中与模板图像相似的区域，并提供匹配程度的评估。
+
+  ##### 4. `Ranging::horizon_estimate`
+
+  ```c++
+  void Ranging::horizon_estimate(Mat& img, Mat& bboxs, int k)
+  ```
+
+  - **作用**: （单目系统）计算目标框中的每个点的距离和角度，并存储在 `Z` 矩阵中。
+
+  - **参数:**
+    - `img`: 输入图像的引用。
+    
+    - `bboxs`: 包含目标框坐标的矩阵。
+    
+    - `k`: 目标框的索引。
+    
+  - **重要函数：**要进行深度估计，重要的一步需要使用到OpenCV的canny（）函数进行边缘检测：
+
+    canny()边缘检测过程：
+    1).Canny边缘检测只算子是John F. Canny于 1986 年开发出来的一个多级边缘检测算法。2.)Canny 的目标是找到一个最优的边缘检测算法，最优边缘检测的含义是:好的检测-算法能够尽可能多地标识出图像中的实际边缘。好的定位-标识出的边缘要尽可能与实际图像中的实际边缘尽可能接近。最小响应- 图像中的边缘只能标识一次，并且可能存在的图像噪声不应标识为边缘。
+
+    3).算法步骤:①灰度转换-cvtColor②高斯模糊-GaussianBlur。以平滑图像，滤除噪声。③计算梯度-Sobel/Scharr
+    ④非最大信号抑制，以消除边缘检测带来的杂散响应，
+    高低阈值输出二值图像，检测来确定真实的和潜在的边缘，最后，通过抑制孤立的弱边缘最终完成边缘检测。
+
+    ```cpp
+    cvtColor(tpl, grayImage, COLOR_BGR2GRAY); 
+    GaussianBlur(grayImage,grayImage,Size(5,5),0);
+    Canny(grayImage, edge, 120, 180, 3);  //提取边缘，获取与地面接触点
+    ```
+
+    - `grayImage`：这是输入的灰度图像，即需要进行边缘检测的图像。
+    - `edge`：这是输出的边缘图像，用于存储检测到的边缘信息。
+    - `120` 和 `180`：这两个参数分别是 Canny 边缘检测算法中的低阈值和高阈值。在检测边缘时，像素的梯度值高于高阈值被认为是强边缘，低于低阈值但高于低阈值的像素被认为是弱边缘，低于低阈值的像素被认为是非边缘。
+    - `3`：这是 Sobel 算子的大小，用于计算图像的梯度。在这里，大小为 3 表示使用 3x3 的 Sobel 算子进行梯度计算。
+
+  ##### 5. `Ranging::getInfo`
+
+  ```c++
+  void Ranging::getInfo(Mat &imgL, Mat &imgR, Mat &detBoxes, Mat &info)
+  ```
+
+  - **作用**: （双目系统）从右图像的所有目标框中提取测距信息，包括目标中心距离、物体宽度、高度、与相机光轴之间的水平夹角。
+
+  - **参数:**
+    
+    - `imgL`: 左图像的引用。
+    
+    - `imgR`: 右图像的引用。
+    
+    - `detBoxes`: 包含目标框坐标的矩阵。
+    
+    - `info`: 存储测距信息的矩阵。
+    
+   - **函数过程：**
+
+     针对右图像中每个目标框的边界进行模板匹配得到水平视差-->定义视差矩阵并根据重投影矩阵q得到每个像素点的深度即立体重建过程-->得到每个像素点距离相机的距离-->根据相似三角形等原理计算物体与相机光轴之间的水平夹角、物体宽度、高度。
+
+  - **重要函数：**
+
+    ```cpp
+    reprojectImageTo3D(disp_matrix, threedImage, q, false);
+    ```
+
+    `reprojectImageTo3D` 函数利用视差图和 Q 矩阵，通过对每个像素点进行上述的齐次坐标变换，最终将二维的视差图转化为三维的坐标点云。
+
+    对于一个在图像中的像素点 (𝑢,𝑣)和其视差值 𝑑，该点在相机坐标系下的三维坐标 (𝑋,𝑌,𝑍)可以通过以下过程计算：
+
+    1. **基本关系**：在立体相机系统中，两个相机之间有一个基线距离 𝐵。两个相机的投影平面上的点之间的视差 𝑑 与深度 𝑍 的关系为：
+
+       ![image-20240603151211728](D:\robotkernal-uestc\Code\MowingRobot\pibot_ros\ros_ws\src\upbot_vision\Note\Images\深度计算公式.png)
+
+       其中，𝑓*f* 是相机的焦距，𝐵*B* 是两个相机的基线距离。
+
+    2. **像素坐标到相机坐标**： 假设图像的中心点坐标是 (𝑐𝑥,𝑐𝑦)，然后像素点 (𝑢,𝑣)在图像坐标系中表示，相应的相机坐标 (𝑋,𝑌,𝑍) 可以通过以下公式得到：
+
+       ![image-20240603151302765](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240603151302765.png)
+
+       ### 三维坐标计算
+
+       利用视差图和 Q 矩阵，可以得到像素点的三维坐标。
+
+       ```cpp
+       X = (u * q11 + v * q12 + d * q13 + q14) / (u * q41 + v * q42 + d * q43 + q44)
+       Y = (u * q21 + v * q22 + d * q23 + q24) / (u * q41 + v * q42 + d * q43 + q44)
+       Z = (u * q31 + v * q32 + d * q33 + q34) / (u * q41 + v * q42 + d * q43 + q44)	
+       ```
+
+       ### 公式推导过程
+
+       - 对于 X 坐标：
+
+         ![image-20240603151559322](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240603151559322.png)
+
+       - 对于 Y 坐标：
+
+         ![image-20240603151609412](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240603151609412.png)
+
+       - 对于 Z 坐标：
+
+         ![image-20240603151617966](C:\Users\dd\AppData\Roaming\Typora\typora-user-images\image-20240603151617966.png)
+
+  ##### 6. `Ranging::get_range`
+
+  ```c++
+  std::vector<Mat> Ranging::get_range()
+  ```
+
+  - **作用**: 获取双目相机的图像帧，对图像进行处理并返回测距信息。
+  - **返回值**: 包含右图像、目标框和测距信息的向量。
+
+  ##### 7. `Ranging::Ranging`
+
+  ```c++
+  Ranging::Ranging(int index, int imgw, int imgh)
+  ```
+
+  - **作用**: 类的构造函数，初始化各类矩阵和相机参数。
+
+  - **参数:**
+    
+    - `index`: 摄像头的索引。
+    
+    - `imgw`: 图像宽度。
+    
+    - `imgh`: 图像高度。    
+    
+   - **重要库函数：**在初始化相机参数的过程中需要使用到OenCV的以下库函数：
+
+     `initUndistortRectifyMap` 函数的作用是根据相机的内部参数和畸变系数，以及校正变换的参数，计算出一个映射关系。这个映射关系可以将畸变的图像映射为经过校正后的图像，以消除镜头畸变和校正图像的旋转，使得校正后的图像中的物体保持准确的几何形状。
+
+     ```c++
+     initUndistortRectifyMap(cam_matrix_left.t(), distortion_l, r1, p1, imgSize, CV_32F, mapX1, mapX2);
+     ```
+
+     - `cam_matrix_left.t()`：这是左摄像机的相机矩阵的转置。相机矩阵是一个 3x3 的矩阵，它包含了相机的内部参数，如焦距、主点位置等信息。`.t()` 是转置操作符，用于将相机矩阵进行转置操作。
+
+     - `distortion_l`：这是左摄像机的畸变系数，它包含了径向畸变和切向畸变的参数，用于描述相机镜头的非理想特性。
+
+     - `r1` 和 `p1`：这两个参数是矫正变换的输出矩阵。`r1` 是旋转矩阵，用于将图像校正为水平方向；`p1` 是投影矩阵，用于将校正后的图像映射到一个新的视图中。
+
+     - `imgSize`：这是原始图像的大小，用于指定待校正图像的尺寸。
+
+     - `CV_32F`：这是映射数据的数据类型，指定为 `CV_32F` 表示映射数据将使用 32 位浮点数进行存储。
+
+     - `mapX1` 和 `mapX2`：这两个参数是输出的映射数组，它们用来存储 `initUndistortRectifyMap` 计算出的水平方向的映射关系。
+
+     `stereoRectify` 函数，用于校正立体摄像机的图像，以便在后续的立体视觉处理中获得准确的深度信息。
+     
+     ```cpp
+     stereoRectify(cam_matrix_left.t(), distortion_l, cam_matrix_right.t(), distortion_r,imgSize, rotate.t(), trans, r1, r2, p1, p2, q);
+     ```
+     
+     - `cam_matrix_left.t()` 和 `cam_matrix_right.t()`：分别是左侧和右侧摄像机的相机矩阵的转置。相机矩阵包含了相机的内部参数，如焦距、主点位置等信息。
+     - `distortion_l` 和 `distortion_r`：分别是左侧和右侧摄像机的畸变系数，用于描述相机镜头的非理想特性。
+     - `imgSize`：原始图像的大小，用于指定待校正图像的尺寸。
+     - `rotate`：旋转矩阵，用于将左右摄像机的图像进行校正，使得它们的光学轴平行。
+     - `trans`：平移矩阵，用于将左右摄像机的图像进行校正，使得它们的光学中心在同一水平线上。
+     - `r1` 和 `r2`：分别是左侧和右侧摄像机的旋转矩阵，用于将左右摄像机的图像进行校正，使得它们的光学轴平行。
+     - `p1` 和 `p2`：分别是左侧和右侧摄像机的投影矩阵，用于将左右摄像机的校正后的图像映射到一个新的视图中。
+     - `q`：深度映射矩阵，用于将图像中的像素点映射到三维空间中，并计算出每个像素点的深度信息。
+     
+     `stereoRectify` 函数的作用是校正立体摄像机的图像，使得左右摄像机的图像在几何上保持一致，以便后续的立体视觉处理。校正过程包括旋转和平移操作，以及投影矩阵的计算，以确保左右摄像机的图像具有相同的视图和几何结构。最终的深度映射矩阵 `q` 可以用于将图像中的像素点映射到三维空间中，并计算出每个像素点的深度信息，用于后续的立体视觉分析和深度估计。
+
+- main.cc
+
+  **主要功能**：通过摄像头获取图像数据，并使用自定义的测距算法对图像中的障碍物进行距离测量。测量结果以自定义的ROS消息类型发布，用于机器人导航或其他需要环境感知的应用。
+
+  **主要函数：** 
+
+  1. **宏定义**
+
+     ```c++
+     #define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000) // 角度转化为弧度
+     ```
+
+     - 将角度值转换为弧度值，便于后续计算。
+
+  2. **全局变量**
+
+     ```c++
+     Ranging r(40, 640, 480);
+     std::queue<std::vector<cv::Mat>> frameInfo;
+     std::mutex g_mutex;
+     ```
+
+     - `Ranging r(40, 640, 480)`：初始化测距对象，假设摄像头索引为40，分辨率为640x480。
+     - `std::queue<std::vector<cv::Mat>> frameInfo`：存储图像帧信息的队列。
+     - `std::mutex g_mutex`：用于同步访问队列的互斥锁。
+
+  3. **`void *ranging(void *args)`**
+
+     ```c++
+     void *ranging(void *args) {
+         ros::Publisher dis_pub_;
+         ros::NodeHandle nh;
+         dis_pub_ = nh.advertise<pibot_msgs::dis_info_array>("ceju_info",10);
+     
+         while (true) {
+             std::vector<cv::Mat> result = r.get_range();
+             cv::Mat info = result[2];
+             pibot_msgs::dis_info_array dis_array;
+     
+             if (!info.empty()) {
+                 for (int i = 0; i < info.rows; i++) {
+                     if (info.at<float>(i,0) > 0 && info.at<float>(i,0) < 200) {
+                         pibot_msgs::dis_info data;
+                         data.distance = info.at<float>(i,0);
+                         data.width = info.at<float>(i,1);
+                         data.height = info.at<float>(i,2);
+                         data.angle = info.at<float>(i,3);
+                         dis_array.dis.push_back(data);
+                         std::cout << data.distance << std::endl;
+                     }
+                 }
+                 dis_pub_.publish(dis_array);
+             }
+     
+             g_mutex.lock();
+             while (!frameInfo.empty()) frameInfo.pop();
+             frameInfo.push(result);
+             g_mutex.unlock();
+         }
+     }
+     ```
+
+     - 该函数是一个独立的线程函数，持续从摄像头获取图像数据，并进行障碍物测距。
+     - 使用`r.get_range()`获取测距信息，并将结果存入`pibot_msgs::dis_info_array`中，通过ROS话题发布。
+     - 使用互斥锁同步访问全局队列`frameInfo`，确保线程安全。
+
+  4. **`int main(int argc, char \**argv)`**
+
+     ```c++
+     int main(int argc, char **argv) {
+         ros::init(argc, argv, "stereo");
+         ros::NodeHandle nh;
+         pthread_t tids[1];
+         int ret = pthread_create(&tids[0], NULL, ranging, NULL);
+         pthread_join(tids[0], NULL);
+     
+         return 0;
+     }
+     ```
+
+     - 初始化ROS节点`ros::init(argc, argv, "stereo")`。
+     - 创建并执行测距线程`pthread_create(&tids[0], NULL, ranging, NULL)`。
+     - 使用`pthread_join(tids[0], NULL)`等待测距线程结束，确保主程序在测距线程结束前不会退出。
+
+### 三.代码优化
+
+#### 1.运行速度慢
+
+####  可能存在的时间瓶颈
+
+- **模板匹配** (`muban`)
+
+  ```cpp
+  std::vector<int> disp_pixel = muban(imgGrayL, imgGrayR, coordinates); //模板匹配
+  ```
+  
+  - **原因**: 模板匹配通常是一个计算密集型操作，特别是当模板和搜索区域较大时，计算时间会显著增加。
+  
+  - **优化建议**: 可以尝试缩小模板区域，从减小匹配区域，加快匹配速度。
+  
+- **重投影到3D** (`reprojectImageTo3D`)
+  
+  ```cpp
+  Mat disp_matrix = Mat(imgGrayL.rows, imgGrayL.cols, CV_32F, Scalar(disp_pixel_x));
+  Mat threed_pixel_xyz, threedImage;  
+  reprojectImageTo3D(disp_matrix, threedImage, q, false); //2d->3d
+  ```
+  
+  - **原因**: `reprojectImageTo3D`将2D图像重新投影到3D空间，这个过程涉及大量的矩阵运算，可能会比较耗时；而且代码中是针对图像中所有像素点在每个循环中都计算三维矩阵，由于图像分辨率的原因会导致矩阵很大，从而影响计算速度。
+  - **优化建议**:可以针对图像中的目标区域计算三维矩阵。
+  
+- **计算欧式距离** (`forEach` 操作)
+  
+  ```cpp
+  threed_pixel_xyz.forEach<float>([](float &value, const int *position) { value = sqrt(value); });
+  ```
+  
+  - **原因**: 遍历每个像素并计算平方根操作对于大图像来说是非常耗时的。
+  - **优化建议**: 可以针对图像中的目标区域计算深度信息。
+  
+- **繁琐的矩阵和向量操作**
+  
+  ```cpp
+  std::vector<float> ltPoint = pic2cam(x1, y1);
+  std::vector<float> rbPoint = pic2cam(x2, y2);
+  float w1 = sqrt(pow((threedImage.at<cv::Vec3f>(y2, x1)[0] - threedImage.at<cv::Vec3f>(y2, x2)[0]), 2) +
+                  pow((threedImage.at<cv::Vec3f>(y2, x1)[1] - threedImage.at<cv::Vec3f>(y2, x2)[1]), 2) +
+                  pow((threedImage.at<cv::Vec3f>(y2, x1)[2] - threedImage.at<cv::Vec3f>(y2, x2)[2]), 2));
+  ```
+  
+  - **原因**: 计算距离和角度涉及到多个矩阵和向量操作，这些操作在大图像和多目标的情况下会非常耗时。
+  
+  - **优化建议**: 尽量减少冗余计算，优化数学运算部分，必要时使用并行计算或SIMD指令集加速。
+  
+- **Image Rectification (图像校正)**
+  
+    图像校正涉及重映射操作，可能是时间消耗较大的部分：
+  
+    ```cpp
+    rectifyImage(lframe, rframe);
+    ```
+    
+    **优化建议**:
+  
+    - 确保校正的映射矩阵 (`mapX1`, `mapX2`, `mapY1`, `mapY2`) 只计算一次，不在每帧都重新计算。
+  - 如果可以，尝试对输入图像进行裁剪，只对感兴趣的部分进行校正。
+
+#### 2.优化空间（代码规范性）
+
+##### 2.1 线程管理
+
+- **减少锁的使用**：使用标准库提供的线程安全队列来替代手动加锁。
+
+- **代码结构优化**：将线程函数独立成类成员函数，增加代码的可读性和可维护性。
+
+- **合理使用多线程**：如果某些部分不需要频繁执行，可以考虑使用条件变量来减少无效的循环。
+
+  ```cpp
+  #include <iostream>
+  #include <ros/ros.h>
+  #include <opencv2/opencv.hpp>
+  #include <thread>
+  #include <mutex>
+  #include <queue>
+  #include <condition_variable>
+  #include "upbot_vision/ranging.h"
+  #include "pibot_msgs/dis_info.h"
+  #include "pibot_msgs/dis_info_array.h"
+  
+  #define ANGLE_TO_RADIAN(angle) ((angle) * 3.14159 / 180.0) //角度转化为弧度
+  
+  class RangingSystem {
+  public:
+      RangingSystem()
+          : r(40, 640, 480), nh(), dis_pub_(nh.advertise<pibot_msgs::dis_info_array>("ceju_info", 10)) {
+          startThreads();
+      }
+  
+      ~RangingSystem() {
+          running = false;
+          cv.notify_all();
+          if (ranging_thread.joinable()) ranging_thread.join();
+          if (process_thread.joinable()) process_thread.join();
+      }
+  
+  private:
+      Ranging r;
+      ros::NodeHandle nh;
+      ros::Publisher dis_pub_;
+      std::queue<std::vector<cv::Mat>> frameQueue;
+      std::mutex mtx;
+      std::condition_variable cv;
+      std::thread ranging_thread;
+      std::thread process_thread;
+      bool running = true;
+  
+      void startThreads() {
+          ranging_thread = std::thread(&RangingSystem::rangingTask, this);
+          process_thread = std::thread(&RangingSystem::processTask, this);
+      }
+      //负责从 Ranging 对象中获取数据，并将其推入一个线程安全的队列。
+      void rangingTask() {
+          while (running) {
+              std::vector<cv::Mat> result = r.get_range();
+              {
+                  std::lock_guard<std::mutex> lock(mtx);
+                  if (frameQueue.size() > 0) frameQueue.pop(); // 保持队列最新的帧
+                  frameQueue.push(result);
+              }
+              cv.notify_one();
+          }
+      }
+      //负责处理队列中的数据并发布。
+      void processTask() {
+          while (running) {
+              std::vector<cv::Mat> result;
+              {
+                  std::unique_lock<std::mutex> lock(mtx);
+                  cv.wait(lock, [this] { return !frameQueue.empty() || !running; });
+                  if (!running) break;
+                  result = frameQueue.front();
+                  frameQueue.pop();
+              }
+  
+              cv::Mat info = result[2];
+              pibot_msgs::dis_info_array dis_array;
+              if (!info.empty()) {
+                  for (int i = 0; i < info.rows; ++i) {
+                      if (info.at<float>(i, 0) > 0 && info.at<float>(i, 0) < 200) {
+                          pibot_msgs::dis_info data;
+                          data.distance = info.at<float>(i, 0);
+                          data.width = info.at<float>(i, 1);
+                          data.height = info.at<float>(i, 2);
+                          data.angle = info.at<float>(i, 3);
+                          dis_array.dis.push_back(data);
+                          std::cout << data.distance << std::endl;
+                      }
+                  }
+                  dis_pub_.publish(dis_array);
+              }
+          }
+      }
+  };
+  
+  int main(int argc, char **argv) {
+      ros::init(argc, argv, "stereo");
+      RangingSystem rangingSystem;
+      ros::spin();
+      return 0;
+  }
+  
+  ```
+
+  - **线程安全队列**：使用 `std::condition_variable` 和 `std::unique_lock` 来保证队列的线程安全，避免手动使用互斥锁和条件变量时可能出现的错误。
+  
+  - **条件变量**：用条件变量来管理线程的等待和通知，避免忙等，提高效率。
+  
+  - **类封装**：将相关函数封装到一个类中，增强代码的可读性和可维护性。
+  
+  - **线程管理**：在析构函数中停止线程并等待其结束，确保资源的正确释放。
+  
+##### 2.2 注释
+
+​    对于代码中的部分变量解释不够清晰，影响代码的可读性。
+
+#### 3.通过双目视觉计算深度的核心原理优化深度计算算法
+
+**1.省去模板匹配**
+
+利用rk3588的两个npu分别进行检测，取得识别得到的目标中心的像素坐标，进行计算视差，再而通过重投影矩阵然后计算深度。
+
+**2. 缩小模板匹配的搜索区域** 
+
+尝试缩小待匹配模板的搜索区域，从而减小计算压力，提高运行速度。
+
+**3.引入跟踪算法**
+
+加入跟踪算法，在检测后面加上跟踪框，针对部分置信度较高的框的，可以省去模板匹配，直接使用上一帧目标框的视差值。

Docs/Linux开发板遇到问题和解决方案/USBHID权限问题.md

@@ -0,0 +1,28 @@
+
+
+#### USBHID非root用户在linux下无权限问题
+
+解决：需要设置udev规则来实现。
+
+![image-20240417114720051](C:\Users\李瑞瑞\AppData\Roaming\Typora\typora-user-images\image-20240417114720051.png)
+
+通常情况下，udev规则文件的命名规则是按照数字和规则名称的顺序来加载的。数字越小的规则文件会先被加载，而数字越大的规则文件会后被加载。因此，可以选择一个数字较大的数字作为规则文件的名称，以确保在其他规则文件之后加载。
+
+1. 创建规则文件
+
+   ```bash
+   sudo gedit /etc/udev/rules.d/99-usb-serial.rules
+   ```
+
+2. 写入规则
+
+```bash
+SUBSYSTEM=="usb" ATTRS{idVendor}=="YOUR_VENDOR_ID", ATTRS{idProduct}=="YOUR_PRODUCT_ID", MODE:="0777"
+```
+
+3. 重新加载udev规则，以使更改生效
+
+   ```bash
+   sudo udevadm control --reload-rules
+   sudo udevadm trigger
+   ```

Docs/Linux开发板遇到问题和解决方案/Ubuutu下USB串口冲突问题.md

@@ -0,0 +1,203 @@
+# Ubuutu下USB串口冲突问题
+
+### 一、为何要设置USB设备别名
+
+​		随着我们开发的机器人具备的功能越来越复杂，那么需要外接的USB设备会逐渐多起来，例如：外接两个arduino板，外接两个USB摄像头，外接一个激光雷达，外接一个USB接口的IMU模块等等，那么如此多的USB设备在[linux](https://so.csdn.net/so/search?q=linux&spm=1001.2101.3001.7020)中的挂载点就会多起来，而且慢慢的也会变得较为混乱，导致我们无法分清楚哪一个设备挂载点对应哪一个设备，而且即使我们在本次开机中分清楚了，那么在下次linux系统开机后，USB设备的挂载点也会随着系统挂载设备顺序的不同而导致设备挂载点发生变化。如下图所示，当外接了很多USB设备时会导致无法分清楚挂载点与哪一个设备对应：
+
+![Screenshot from 2018-01-08 11:58:27.png](https://www.corvin.cn/wp-content/uploads/image/20180108/1515384368800854.png)
+
+​		从上图无法得知设备挂载点/dev/ttyACM0和/dev/ttyACM1哪一个挂载点对应哪一个arduino设备，ttyUSB0和ttyUSB1对应哪一个设备也不确定，因此，我们就需要一种方法来保证每次开机后唯一的设备挂载点对应一个确定的设备，这样我们的程序就可以正确操控相应的USB设备了。
+
+------
+
+### 二、什么是udev?
+
+​		udev 是 Linux 内核的设备管理器。总的来说，它取代了 devfs 和 hotplug，负责管理 /dev 中的设备节点。同时，udev 也处理所有用户空间发生的硬件添加、删除事件，以及某些特定设备所需的固件加载。与传统的顺序加载相比，udev 通过并行加载内核模块提供了潜在的性能优势。异步加载模块的方式也有一个天生的缺点：无法保证每次加载模块的顺序，如果机器具有多个块设备，那么它们的设备节点可能随机变化。
+
+udev 规则以管理员身份编写并保存在 /etc/udev/rules.d/ 目录，其文件名必须以 .rules 结尾，各种软件包提供的规则文件位于 /lib/udev/rules.d/。如果 /usr/lib 和 /etc 这两个目录中有同名文件，则 /etc 中的文件优先，关于udev更为详细的介绍，大家可以参考以下链接：[udev介绍](https://www.corvin.cn/go?url=https://wiki.archlinux.org/index.php/Udev_(简体中文))
+
+------
+
+### 三、编写udev规则
+
+​		在简单了解了什么是udev后，我们就可以来编写属于自己USB设备的udev规则了，这样我们就可以为自己的USB设备挂载点来重命名，由于完整的介绍编写udev规则太过于复杂，我们这里来通过大家经常用到的rplidar_ros里面的rplidar的udev规则来做举例说明，如果已经下载过rplidar_ros代码的就不用重新下载，如果没有下载的可以通过以下命令来下载：
+
+git clone [https://github.com/robopeak/rplidar_ros.git](https://www.corvin.cn/go?url=https://github.com/robopeak/rplidar_ros.git)
+
+当下载完该代码后，我们查看目录结构可以发现一个scripts的目录，里面就放着rplidar相关的udev规则，具体操作如下图所示：
+
+![Screenshot from 2018-01-08 13:33:14.png](https://www.corvin.cn/wp-content/uploads/image/20180108/1515389960676415.png)
+
+​		我们先来看rplidar.rules的内容，然后来分析其编写规则：
+
+![Screenshot from 2018-01-08 13:44:43.png](https://www.corvin.cn/wp-content/uploads/image/20180108/1515391254592044.png)
+
+​		在规则文件里，除了以“#”开头的行（注释），所有的非空行都被视为一条规则，但是一条规则不能扩展到多行。规则都是由多个 键值对（key-value pairs）组成，并由逗号隔开，键值对可以分为 条件匹配键值对( 以下简称“匹配键 ”) 和 赋值键值对( 以下简称“赋值键 ”)，一条规则可以有多条匹配键和多条赋值键。匹配键是匹配一个设备属性的所有条件，当一个设备的属性匹配了该规则里所有的匹配键，就认为这条规则生效，然后按照赋值键的内容，执行该规则的赋值。
+
+​		在rplidar.rules中的KERNEL，ATTRS{idVendor}和ATTRS{idProduct}是匹配键，MODE和SYMLINK是赋值键，这条规则的意思是：如果有一个设备的内核设备名称是ttyUSB*(*代表任意数字)，VID是10c4，PID是ea60的话，那么就在/dev目录下增加一个符号链接设备并命名为rplidar，同时为其赋予0777的权限。
+
+​		通过这条简单的规则，大家应该对 udev 规则有直观的了解。但可能会产生疑惑，为什么 KERNEL，ATTRS{idVendor}和ATTRS{idProduct} 是匹配键，而 MODE和SYMLINK是赋值键呢？这由中间的操作符 (operator) 决定。
+
+仅当操作符是“==”或者“!=”时，其为匹配键；若为其他操作符时，都是赋值键，下面是 udev 规则的所有操作符：
+
+“==”：比较键、值，若等于，则该条件满足；
+
+“!=”： 比较键、值，若不等于，则该条件满足；
+
+“=”： 对一个键赋值；
+
+“+=”：为一个表示多个条目的键赋值。
+
+“:=”：对一个键赋值，并拒绝之后所有对该键的改动。目的是防止后面的规则文件对该键赋值。
+
+------
+
+### 四、使udev规则生效
+
+当我们编写好属于自己的udev规则后，接下来就需要将其复制到指定位置，然后使其生效了，这里我们可以参考这两个脚本如何编写的，首先看create_udev_rules.sh：
+
+```bash
+#!/bin/bash
+echo "remap the device serial port(ttyUSBX) to  rplidar"
+echo "rplidar usb connection as /dev/rplidar , check it using the command : ls -l /dev|grep ttyUSB"
+echo "start copy rplidar.rules to  /etc/udev/rules.d/"
+echo "`rospack find rplidar_ros`/scripts/rplidar.rules"
+sudo cp `rospack find rplidar_ros`/scripts/rplidar.rules  /etc/udev/rules.d
+echo " "
+echo "Restarting udev"
+echo ""
+sudo service udev reload
+sudo service udev restart
+echo "finish "
+```
+
+![Screenshot from 2018-01-08 14:24:40.png](https://www.corvin.cn/wp-content/uploads/image/20180108/1515393212202843.png)
+
+接下来看删除规则的脚本内容，主要就是将复制到rules.d目录下的rplidar.rules规则文件删除，然后重启udev服务：
+
+```html
+#!/bin/bash
+echo "delete remap the device serial port(ttyUSBX) to  rplidar"
+echo "sudo rm   /etc/udev/rules.d/rplidar.rules"
+sudo rm   /etc/udev/rules.d/rplidar.rules
+echo " "
+echo "Restarting udev"
+echo ""
+sudo service udev reload
+sudo service udev restart
+echo "finish  delete"
+```
+
+然后我们先来执行create_udev_rules.sh脚本使规则生效，由于该脚本默认情况下需要将rplidar_ros代码放在ROS的工作空间源码目录下，使用catkin_make编译后，然后source devel/setup.bash，然后才能执行该脚本，我在这里将脚本简单修改，这样就没必要在ROS工作空间的源码目录下执行了，我修改后的脚本如下：
+
+```html
+#!/bin/bash
+echo "remap the device serial port(ttyUSBX) to  rplidar"
+echo "rplidar usb connection as /dev/rplidar , check it using the command : ls -l /dev|grep ttyUSB"
+echo "start copy rplidar.rules to  /etc/udev/rules.d/"
+sudo cp ./rplidar.rules  /etc/udev/rules.d
+echo " "
+echo "Restarting udev"
+echo ""
+sudo service udev reload
+sudo service udev restart
+echo "finish "
+```
+
+![Screenshot from 2018-01-08 14:43:11.png](https://www.corvin.cn/wp-content/uploads/image/20180108/1515394129748256.png)
+
+当我们不想再使用rplidar的udev规则时就可以使用delete_udev_rules.sh脚本将规则文件删除，这样就不会再有相应的rplidar映射了：
+
+![Screenshot from 2018-01-08 14:49:45.png](https://www.corvin.cn/wp-content/uploads/image/20180108/1515396077956148.png)
+
+如果不想在每次创建udev规则和删除规则时，重新的插拔USB设备，那么可以将脚本文件做如下修改就可以了，首先来看create_udev_rules.sh如何修改：
+
+```html
+#!/bin/bash
+echo "remap the device serial port(ttyUSBX) to  rplidar"
+echo "rplidar usb connection as /dev/rplidar , check it using the command : ls -l /dev|grep ttyUSB"
+echo "start copy rplidar.rules to  /etc/udev/rules.d/"
+sudo cp ./rplidar.rules  /etc/udev/rules.d
+echo " "
+echo "Restarting udev"
+echo ""
+sudo udevadm control --reload-rules
+sudo service udev restart
+sudo udevadm trigger
+echo "finish "
+```
+
+![Screenshot from 2018-01-08 15:27:50.png](https://www.corvin.cn/wp-content/uploads/image/20180108/1515396713678630.png)
+
+同理可得，修改后的delete_udev_rules.sh脚本内容如下：
+
+```html
+#!/bin/bash
+echo "delete remap the device serial port(ttyUSBX) to  rplidar"
+echo "sudo rm   /etc/udev/rules.d/rplidar.rules"
+sudo rm   /etc/udev/rules.d/rplidar.rules
+echo " "
+echo "Restarting udev"
+echo ""
+sudo udevadm control --reload-rules
+sudo service udev restart
+sudo udevadm trigger
+echo "finish  delete"
+```
+
+![Screenshot from 2018-01-08 15:34:50.png](https://www.corvin.cn/wp-content/uploads/image/20180108/1515397075342437.png)
+
+------
+
+### 五、当PID/VID相同时如何编写udev规则
+
+​		当主控板上外接的USB设备越來越多时，难免遇到两个或多个USB设备的VID，PID相同，这样的话再通过上述那个简单的rules文件就不行了，我们就需要增加新的匹配键来做区分不同的USB设备：
+
+![Screenshot from 2018-01-08 15:48:10.png](https://www.corvin.cn/wp-content/uploads/image/20180108/1515398203891106.png)
+
+​		此时我们就需要在rplidar.rules文件中新增KERNELS匹配键，这样才能与IMU模块的usb转接板区分开，获取KERNELS的方式如下，我们先把其他USB设备拔掉，只留下RPlidar A2设备，然后执行以下命令：
+
+```html
+udevadm info --attribute-walk --name=/dev/ttyUSB1 | grep KERNELS
+```
+
+![Screenshot from 2018-01-08 16:33:14.png](https://www.corvin.cn/wp-content/uploads/image/20180108/1515400642339329.png)
+
+​		然后就可以根据该KERNELS属性来修改rplidar.rules内容了，这样我们就可以使用该udev规则文件来创建映射了，新rplidar.rules内容如下：
+
+```bash
+# set the udev rule , make the device_port be fixed by rplidar
+#
+KERNELS=="1-2.1", KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="rplidar"
+```
+
+​		同理可得，我们可以创建IMU模块的imu.rules文件，内容如下：
+
+```html
+# set the udev rule , make the device_port be fixed by rplidar
+#
+KERNELS=="1-2.4", KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="imu"
+```
+
+​		同时我们还可以创建和imu模块规则文件配套的脚本文件，具体操作如下图所示：
+
+![Screenshot from 2018-01-08 16:44:12.png](https://www.corvin.cn/wp-content/uploads/image/20180108/1515403278950616.png)
+
+​		那执行两个生效的脚本来查看效果：
+
+![Screenshot from 2018-01-08 17:22:43.png](https://www.corvin.cn/wp-content/uploads/image/20180108/1515403521598936.png)
+
+------
+
+### 六、 注意事项
+
+​		使用udev规则来创建设备挂载点新的映射时，需要注意不能随意更换USB设备插的USB口了，因为每次更换USB口，那么相应的KERNELS就换了，切记！我们只要不更换USB设备插的USB口，那么无论开机时设备挂载顺序如何，即使rplidar a2的挂载点这次开机是ttyUSB0，下次开机变成ttyUSB1也不要紧，因为/dev/rplidar总能正确的创建相应的映射到rplidar的挂载点上。
+
+​		在执行上面的各种命令时需要在ubuntu的终端里执行，不可以在windows下使用类似xShell的终端模拟软件来运行。
+
+备忘：
+
+```shell
+ACTION=="add",KERNELS=="1-1.1:1.3",SUBSYSTEMS=="usb",MODE:="0777",SYMLINK+="USB-4G"
+```
+

Docs/Linux开发板遇到问题和解决方案/摄像头打开异常问题.md

@@ -0,0 +1,13 @@
+## `摄像头打开异常问题`
+
+​		部分情况下，`OpenCV`在打开摄像头的时候会出现`v412`的错误，错误内容类似如下：
+
+![](./Image/V412-Error.png)
+
+​		解决方案:
+
+```c++
+/* 在OpenCV中加入以下特性：*/
+cv::VideoCapture cap(0, cv::CAP_V4L2);
+```
+

Docs/UWB定位原理/UWB解算原理.md

@@ -0,0 +1,108 @@
+# UWB 定位原理
+
+#### 1.三维坐标转二维坐标
+
+​		在割草机器人项目中，割草机器人目前只考虑二维平面的定位。但是UWB测量的距离是三维距离，所以我们根据机器人的高度`carH`和`UWB`标签的高度`UwbH`计算出水平距离`dxy`。
+
+```cpp
+// dxy^2 = di^2 - (UwbH = carH)^2 
+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));
+}
+```
+
+<img src="./Image/UWB高度修正.png" alt="" style="zoom:50%;" />
+
+#### 2.多项式拟合
+
+​		UWB的定位是存在波动的，所以会根据UWB计算距离的规律对计算的距离进行多项式拟合，可以起到滤波提高精度作用。下面的计算实际是收集不同实测距离下，UWB的实际输出距离，利用3次多项式拟合得到的结果。
+
+​		下面的计算跟标签的位置以及高度无关，主要跟UWB的硬件设备的特性有关。
+
+```cpp
+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;
+```
+
+#### 3.位置求解
+
+​		UWB位置求解采用如下图示：
+
+<img src="./Image/UWB位置示意图.png" alt="" style="zoom:50%;" />
+
+​		UWB的定位可以用下面公式描述, 其中$(x,y)$是割草机器人上面的UWB的位置，另外三个坐标点是3个UWB标签的位置，可以有如下的公式。
+$$
+d_1^2 = (x_1 - x)^2 + (y_1 - y)^2   \space\space\space\space\space (1)\\
+d_2^2 = (x_2 - x)^2 + (y_2 - y)^2   \space\space\space\space\space(2)\\
+d_3^2 = (x_3 - x)^2 + (y_3 - y)^2   \space\space\space\space\space(3)\\
+$$
+​		$(2)-(1)$以及$(3)-(2)$消去二次项，可得：
+$$
+d_1^2 - d_2^2 = \left[ -2(x_1 - x_2)x + x_1^2 - x_2^2 \right] + \left[ -2(y_1 - y_2)y + y_1^2 - y_2^2 \right] \\
+
+d_1^2 - d_3^2 = \left[ -2(x_1 - x_3)x + x_1^2 - x_3^2 \right] + \left[ -2(y_1 - y_3)y + y_1^2 - y_3^2 \right]
+$$
+​		整理为矩阵形式：
+$$
+-2 \begin{bmatrix}
+x_1 - x_2 & y_1 - y_2 \\
+x_1 - x_3 & y_1 - y_3
+\end{bmatrix}
+\begin{bmatrix}
+x \\
+y
+\end{bmatrix}
+=
+\begin{bmatrix}
+(d_1^2 - d_2^2) - (x_1^2 - x_3^2) - (y_1^2 - y_3^2) \\
+(d_1^2 - d_3^2) - (x_1^2 - x_3^2) - (y_1^2 - y_3^2)
+\end{bmatrix}
+$$
+​		整理可得：
+$$
+\begin{align*}
+A &= -2\cdot \begin{bmatrix}
+x_1 - x_2 & y_1 - y_2 \\
+x_1 - x_3 & y_1 - y_3
+\end{bmatrix}\\
+b &= \begin{bmatrix}
+(d_1^2 - d_2^2) - (x_1^2 - x_2^2) - (y_1^2 - y_2^2) \\
+(d_1^2 - d_3^2) - (x_1^2 - x_3^2) - (y_1^2 - y_3^2)
+\end{bmatrix}\\
+X &= \begin{bmatrix}
+x\\
+y
+\end{bmatrix}
+\end{align*}
+$$
+​		矩阵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]);
+}
+```
+
+​		矩阵b对应的代码：
+
+```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]);
+}
+```
+
+​		那么，上述矩阵可以通过$X=(A^T\cdot A)^{-1}A^T*b$ 求解UWB的位置。
+
+```cpp
+Mat AT=~A;
+uwbPos=(AT*A)%AT*b;
+this->uwb_data_.x_ = uwbPos.mat[0][0];
+this->uwb_data_.y_ = uwbPos.mat[1][0];
+```
+