diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/Mat.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/Mat.h index f8cc745..0586c6c 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/Mat.h +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/Mat.h @@ -3,13 +3,13 @@ * Current Version : V1.0 * Author : logzhan * Date of Issued : 2022.09.14 -* Comments : �����ľ������ +* Comments : 导航的矩阵计算 ********************************************************************************/ /* Header File Including -----------------------------------------------------*/ #ifndef _H_MAT_ #define _H_MAT_ -#define MAT_MAX 15 //�������ܴ����������� +#define MAT_MAX 15 //决定了能处理的最大矩阵 #include @@ -20,60 +20,55 @@ class Mat { public: Mat(); - Mat(int setm,int setn,int kind);//kind=1��λ��kind=0�����,��������ʼ�����ݡ� - void Init(int setm,int setn,int kind);//kind=1��λ��kind=0�����,��������ʼ�����ݡ� + Mat(int setm,int setn,int kind);//kind=1单位阵，kind=0零矩阵,其它不初始化内容。 + void Init(int setm,int setn,int kind);//kind=1单位阵，kind=0零矩阵,其它不初始化内容。 void Zero(void); - //��Щ�ؼ�����Ӧ����Ϊprivate�ġ�����Ϊ�˷��㣬��Ҳ������public - int m;//���� - int n;//���� - double mat[MAT_MAX][MAT_MAX];//������������ + //这些关键数本应该作为private的。但是为了方便，我也做成了public + int m;//行数 + int n;//列数 + double mat[MAT_MAX][MAT_MAX];//矩阵数据内容 - //����ľ��� - Mat SubMat(int a,int b,int lm,int ln);//��ȡ����һ���� - void FillSubMat(int a,int b,Mat s);//����Ӿ��� + //特殊的矩阵 + Mat SubMat(int a,int b,int lm,int ln);//获取矩阵一部分 + void FillSubMat(int a,int b,Mat s);//填充子矩阵 - //����ר�� - double absvec();//����������ij��ȡ����Ǹ���Ԫ�صľ���ֵ�� - double Sqrt();//�������ȵ�ƽ�� - friend Mat operator ^(Mat a,Mat b);//��� + //向量专用 + double absvec();//这个是向量的长度。不是个别元素的绝对值。 + double Sqrt();//向量长度的平方 + friend Mat operator ^(Mat a,Mat b);//叉乘 - //���� + //运算 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,Mat b); friend Mat operator +(Mat a,Mat b); friend Mat operator -(Mat a,Mat b); - friend Mat operator ~(Mat a);//ת�� + friend Mat operator ~(Mat a);//转置 friend Mat operator /(Mat a,Mat b);//a*inv(b) friend Mat operator %(Mat a,Mat b);//inv(a)*b - //MAT inv();//����� + //MAT inv();//逆矩阵 private: - // Ϊ���ø�˹��Ԫ��������һЩ���� - // �������� + // 为了用高斯消元法，做的一些函数 + // 交换两行 void RowExchange(int a, int b); - // ijһ�г���ϵ�� + // 某一行乘以系数 void RowMul(int a,double k); - // ��ijһ�мӼ���һ�еı��� + // 对某一行加减另一行的倍数 void RowAdd(int a,int b, double k); - // �������� + // 交换两列 void ColExchange(int a, int b); - // ijһ�г���ϵ�� + // 某一列乘以系数 void ColMul(int a,double k); - // ��ijһ�мӼ���һ�еı��� + // 对某一列加减另一列的倍数 void ColAdd(int a,int b,double k); }; - - - - - #endif 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 4192cb5..8bbf4da 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 @@ -3,7 +3,7 @@ * Current Version : V1.0 * Author : logzhan * Date of Issued : 2022.09.14 -* Comments : ��������� +* Comments : 矩阵运算库 ********************************************************************************/ /* Header File Including -----------------------------------------------------*/ #include "Mat.h" @@ -28,7 +28,7 @@ int mini(int a,int b) } -//��Ҫ�ڳ�Ա�����ڵ��ù��캯�� +//不要在成员函数内调用构造函数 Mat::Mat() { Init(1,1,0); @@ -51,7 +51,7 @@ void Mat::Init(int setm,int setn,int kind) if(kind==1) { int x; - //Cԭ�е�max min�ᵼ�����������Ա������и��������Ķ�����Ҫֱ�ӷŵ�max���档 + //C原有的max min会导致两次运行自变量。有附带操作的东西不要直接放到max里面。 int xend = mini(this->m, this->n); for(x=0;x < xend;x++){ mat[x][x] = 1; @@ -184,12 +184,12 @@ Mat operator ~(Mat a) Mat operator /(Mat a,Mat b) { - //��˹��Ԫ�� + //高斯消元法 int x,xb; for(x=0;xd[i] = sqrt(this->d[i] * this->d[i] - (AnchorPos[i][2] - CARHEIGHT) * + (AnchorPos[i][2] - CARHEIGHT)); +} +``` + + + +#### 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位置求解采用如下图示： + + + +​ 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]; +``` +