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 941d989..8cb47b9 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 @@ -38,8 +38,8 @@ namespace uwb_slam{ Mat Kg(2, 2, 0); Mat Z(2, 2, 0); - uwbStartPos.mat[0][0]=9; - uwbStartPos.mat[1][0]=0; + uwbStartPos.mat[0][0]=9;//表示UWB设备相对于imu在x方向上有9cm的距离，后期如UWB摆放在imu正上方即删除 + uwbStartPos.mat[1][0]=0;//表示UWB设备相对于imu在y方向上有0cm的距离 Mat H = F; Mat I = F; bool imuStartFlag = 1; @@ -65,7 +65,8 @@ namespace uwb_slam{ std::cout<<"roll:"<uwb_data_.uwb_t_) { std::cout << "uwb received" << std::endl; - + //这一步是为了把上述提到的UWB设备与imu设备不在同一坐标轴上设计的坐标轴对齐操作 Rotate.mat[0][0] = cos(roll); Rotate.mat[0][1] = -sin(roll); Rotate.mat[1][0] = sin(roll); @@ -86,7 +87,8 @@ namespace uwb_slam{ uwbPos.mat[1][0] = uwb_->uwb_data_.y_; uwbPos = uwbPos - Rotate * uwbStartPos; - + //后期如UWB摆放在imu正上方即删除 + //卡尔曼更新过程 predPos = F * syncPos + detPos; Z = H * uwbPos; P = F * P * (~F) + Q; @@ -95,7 +97,7 @@ namespace uwb_slam{ P = (I - Kg * H) * P; uwbDataRxTime = uwb_->uwb_data_.uwb_t_; } else { - syncPos = syncPos + detPos; + syncPos = syncPos + detPos;//如果UWB没有更新信息，则使用imu对齐位置进行更新 } imuDataRxTime = imu_odom_.imu_data_.imu_t_; odomDataRxTime = odom_tmp_; diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp index 2a17887..6d41146 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp @@ -45,15 +45,7 @@ namespace uwb_slam{ while(1){ this->Serread(); - // std::cout<<"s"<x<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; } + // 修正车子和标签的高度差 + // d[i]是三维距离， 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)); } + // 多项式拟合，用于提高精度，注释之后不影响 + // 在不同的距离采集数据，然后拟合出一条曲线 + // 这个地方的参数纯粹和设备特性有关，和标签位置无关 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; @@ -98,6 +96,7 @@ namespace uwb_slam{ -(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]; diff --git a/Docs/UWB解算原理.md b/Docs/UWB解算原理.md new file mode 100644 index 0000000..ae176dd --- /dev/null +++ b/Docs/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)); +} +``` + + + +#### 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]; +``` + diff --git a/Docs/image/UWB位置示意图.png b/Docs/image/UWB位置示意图.png new file mode 100644 index 0000000..19a3be3 Binary files /dev/null and b/Docs/image/UWB位置示意图.png differ diff --git a/Docs/image/UWB高度修正.png b/Docs/image/UWB高度修正.png new file mode 100644 index 0000000..5c8659b Binary files /dev/null and b/Docs/image/UWB高度修正.png differ diff --git a/Docs/image/uwb安装说明.png b/Docs/image/uwb安装说明.png new file mode 100644 index 0000000..699bb7e Binary files /dev/null and b/Docs/image/uwb安装说明.png differ diff --git a/Docs/割草机融合定位：.md b/Docs/割草机融合定位：.md new file mode 100644 index 0000000..6cd8fc1 --- /dev/null +++ b/Docs/割草机融合定位：.md @@ -0,0 +1,9 @@ +割草机融合定位： +地图坐标系与小车坐标系重合，小车起始点为坐标系原点，使用三个标签分别是A，B，C。A放置在充电桩处。车上放置基站M。通过基站到三个标签的距离获得小车的位置信息。如图所示： + +![img](file:///C:\Users\ray\AppData\Local\Temp\ksohtml2328\wps2.png) + +注:为了方便，人为设定UWB坐标系与地图坐标系和小车坐标系均重合。 +小车半径：a +标签高度：h +BC标签距离d \ No newline at end of file