ray
/
RobotKernal-UESTC
forked from logzhan/RobotKernal-UESTC
1
0
Fork 2
Code Pull Requests Activity

更新文档以及修复Mat.cpp和Mat.h的乱码问题

UESTCsecurity 2024-04-17 21:11:09 +08:00
parent b79985e8ad
commit 99c8b58333
7 changed files with 379 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/Mat.h
View File

@ -3,13 +3,13 @@
* Current Version : V1.0 * Current Version : V1.0
* Author : logzhan * Author : logzhan
* Date of Issued : 2022.09.14 * Date of Issued : 2022.09.14
* Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> * Comments : 导航的矩阵计算
********************************************************************************/ ********************************************************************************/
/* Header File Including -----------------------------------------------------*/ /* Header File Including -----------------------------------------------------*/
#ifndef _H_MAT_ #ifndef _H_MAT_
#define _H_MAT_ #define _H_MAT_
#define MAT_MAX 15 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> #define MAT_MAX 15 //决定了能处理的最大矩阵
#include <string.h> #include <string.h>
@ -20,60 +20,55 @@ class Mat
{ {
public: public:
Mat(); Mat();
Mat(int setm,int setn,int kind);//kind=1<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>kind=0<><30><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD> Mat(int setm,int setn,int kind);//kind=1单位阵，kind=0零矩阵,其它不初始化内容。
void Init(int setm,int setn,int kind);//kind=1<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>kind=0<><30><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD> void Init(int setm,int setn,int kind);//kind=1单位阵，kind=0零矩阵,其它不初始化内容。
void Zero(void); void Zero(void);
//<EFBFBD><EFBFBD>Щ<EFBFBD>ؼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊprivate<EFBFBD>ġ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>˷<EFBFBD><EFBFBD>㣬<EFBFBD><EFBFBD>Ҳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>public //这些关键数本应该作为private的。但是为了方便，我也做成了public
int m;//<EFBFBD><EFBFBD><EFBFBD><EFBFBD> int m;//行数
int n;//<EFBFBD><EFBFBD><EFBFBD><EFBFBD> int n;//列数
double mat[MAT_MAX][MAT_MAX];//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> double mat[MAT_MAX][MAT_MAX];//矩阵数据内容
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD> //特殊的矩阵
Mat SubMat(int a,int b,int lm,int ln);//<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> Mat SubMat(int a,int b,int lm,int ln);//获取矩阵一部分
void FillSubMat(int a,int b,Mat s);//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӿ<EFBFBD><EFBFBD><EFBFBD> void FillSubMat(int a,int b,Mat s);//填充子矩阵
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ר<EFBFBD><EFBFBD> //向量专用
double absvec();//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǹ<EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD>صľ<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD> double absvec();//这个是向量的长度。不是个别元素的绝对值。
double Sqrt();//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD>ƽ<EFBFBD><EFBFBD> double Sqrt();//向量长度的平方
friend Mat operator ^(Mat a,Mat b);//<EFBFBD><EFBFBD><EFBFBD> friend Mat operator ^(Mat a,Mat b);//叉乘
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD> //运算
friend Mat operator *(double k,Mat a); friend Mat operator *(double k,Mat a);
friend Mat operator *(Mat a,double k); friend Mat operator *(Mat a,double k);
friend Mat operator /(Mat a,double k); friend Mat operator /(Mat a,double k);
friend Mat operator *(Mat a,Mat b); friend Mat operator *(Mat a,Mat b);
friend Mat operator +(Mat a,Mat b); friend Mat operator +(Mat a,Mat b);
friend Mat operator -(Mat a,Mat b); friend Mat operator -(Mat a,Mat b);
friend Mat operator ~(Mat a);//ת<EFBFBD><EFBFBD> friend Mat operator ~(Mat a);//转置
friend Mat operator /(Mat a,Mat b);//a*inv(b) friend Mat operator /(Mat a,Mat b);//a*inv(b)
friend Mat operator %(Mat a,Mat b);//inv(a)*b friend Mat operator %(Mat a,Mat b);//inv(a)*b
//MAT inv();//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> //MAT inv();//逆矩阵
private: private:
// Ϊ<EFBFBD><EFBFBD><EFBFBD>ø<EFBFBD>˹<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һЩ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> // 为了用高斯消元法，做的一些函数
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> // 交换两行
void RowExchange(int a, int b); void RowExchange(int a, int b);
// ijһ<EFBFBD>г<EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD> // 某一行乘以系数
void RowMul(int a,double k); void RowMul(int a,double k);
// <EFBFBD><EFBFBD>ijһ<EFBFBD>мӼ<EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>еı<EFBFBD><EFBFBD><EFBFBD> // 对某一行加减另一行的倍数
void RowAdd(int a,int b, double k); void RowAdd(int a,int b, double k);
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> // 交换两列
void ColExchange(int a, int b); void ColExchange(int a, int b);
// ijһ<EFBFBD>г<EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD> // 某一列乘以系数
void ColMul(int a,double k); void ColMul(int a,double k);
// <EFBFBD><EFBFBD>ijһ<EFBFBD>мӼ<EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>еı<EFBFBD><EFBFBD><EFBFBD> // 对某一列加减另一列的倍数
void ColAdd(int a,int b,double k); void ColAdd(int a,int b,double k);
}; };
#endif #endif

30
Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/Mat.cpp
View File

@ -3,7 +3,7 @@
* Current Version : V1.0 * Current Version : V1.0
* Author : logzhan * Author : logzhan
* Date of Issued : 2022.09.14 * Date of Issued : 2022.09.14
* Comments : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> * Comments : 矩阵运算库
********************************************************************************/ ********************************************************************************/
/* Header File Including -----------------------------------------------------*/ /* Header File Including -----------------------------------------------------*/
#include "Mat.h" #include "Mat.h"
@ -28,7 +28,7 @@ int mini(int a,int b)
} }
//<EFBFBD><EFBFBD>Ҫ<EFBFBD>ڳ<EFBFBD>Ա<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><EFBFBD>ù<EFBFBD><EFBFBD>캯<EFBFBD><EFBFBD> //不要在成员函数内调用构造函数
Mat::Mat() Mat::Mat()
{ {
Init(1,1,0); Init(1,1,0);
@ -51,7 +51,7 @@ void Mat::Init(int setm,int setn,int kind)
if(kind==1) if(kind==1)
{ {
int x; int x;
//Cԭ<EFBFBD>е<EFBFBD>max min<69>ᵼ<EFBFBD><E1B5BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>и<EFBFBD><D0B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ķ<EFBFBD><C4B6><EFBFBD><EFBFBD><EFBFBD>Ҫֱ<D2AA>ӷŵ<D3B7>max<61><78><EFBFBD>档 //C原有的max min会导致两次运行自变量。有附带操作的东西不要直接放到max里面。
int xend = mini(this->m, this->n); int xend = mini(this->m, this->n);
for(x=0;x < xend;x++){ for(x=0;x < xend;x++){
mat[x][x] = 1; mat[x][x] = 1;
@ -184,12 +184,12 @@ Mat operator ~(Mat a)
Mat operator /(Mat a,Mat b) Mat operator /(Mat a,Mat b)
{ {
//<EFBFBD><EFBFBD>˹<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD> //高斯消元法
int x,xb; int x,xb;
for(x=0;x<b.n;x++) for(x=0;x<b.n;x++)
{ {
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><EFBFBD><EFBFBD>ѵ<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼԪ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> //首先找到最佳的列。让起始元素最大
double s=0; double s=0;
int p=x; int p=x;
double sxb; double sxb;
@ -202,19 +202,19 @@ Mat operator /(Mat a,Mat b)
s=sxb; s=sxb;
} }
} }
//ͬʱ<EFBFBD>任<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> //同时变换两侧矩阵
if(x!=p) if(x!=p)
{ {
a.ColExchange(x,p); a.ColExchange(x,p);
b.ColExchange(x,p); b.ColExchange(x,p);
} }
//<EFBFBD><EFBFBD>һ<EFBFBD>й<EFBFBD>һ //这一列归一
double k=1/b.mat[x][x];//<EFBFBD><EFBFBD>һ<EFBFBD>䲻ҪǶ<EFBFBD>׵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD>²<EFBFBD>ͬ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> double k=1/b.mat[x][x];//这一句不要嵌套到下面两行中，否则会因为更新不同步导致计算错误。
a.ColMul(x,k); a.ColMul(x,k);
b.ColMul(x,k); b.ColMul(x,k);
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><EFBFBD><EFBFBD> //把其它列归零
for(xb=0;xb<b.n;xb++) for(xb=0;xb<b.n;xb++)
{ {
if(xb!=x) if(xb!=x)
@ -232,11 +232,11 @@ Mat operator /(Mat a,Mat b)
Mat operator %(Mat a,Mat b) Mat operator %(Mat a,Mat b)
{ {
//<EFBFBD><EFBFBD>˹<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD> //高斯消元法
int x,xb; int x,xb;
for(x=0;x<a.m;x++) for(x=0;x<a.m;x++)
{ {
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><EFBFBD><EFBFBD>ѵ<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼԪ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> //首先找到最佳的行。让起始元素最大
double s=0; double s=0;
int p=x; int p=x;
double sxb; double sxb;
@ -249,19 +249,19 @@ Mat operator %(Mat a,Mat b)
s=sxb; s=sxb;
} }
} }
//ͬʱ<EFBFBD>任<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> //同时变换两侧矩阵
if(x!=p) if(x!=p)
{ {
a.RowExchange(x,p); a.RowExchange(x,p);
b.RowExchange(x,p); b.RowExchange(x,p);
} }
//<EFBFBD><EFBFBD>һ<EFBFBD>й<EFBFBD>һ //这一行归一
double k=1/a.mat[x][x];//<EFBFBD><EFBFBD>һ<EFBFBD>䲻ҪǶ<EFBFBD>׵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD>²<EFBFBD>ͬ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> double k=1/a.mat[x][x];//这一句不要嵌套到下面两行中，否则会因为更新不同步导致计算错误。
a.RowMul(x,k); a.RowMul(x,k);
b.RowMul(x,k); b.RowMul(x,k);
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><EFBFBD><EFBFBD> //把其它行归零
for(xb=0;xb<a.m;xb++) for(xb=0;xb<a.m;xb++)
{ {
if(xb!=x) if(xb!=x)

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

@ -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
```

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

@ -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"
```

BIN
Docs/UWB定位原理/Image/UWB位置示意图.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 33 KiB

BIN
Docs/UWB定位原理/Image/UWB高度修正.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 22 KiB

108
Docs/UWB定位原理/UWB解算原理.md Normal file
View File

@ -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];
```