forked from logzhan/RobotKernal-UESTC
3.18
parent
e73ef2a5a8
commit
b0f5f708eb
|
|
@ -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:"<<roll<<std::endl;
|
||||
if (imuStartFlag == 1 && imuPos.mat[0][0] != 0 && imuPos.mat[1][0] != 0) {
|
||||
imuStartRoll = roll-PI/2;
|
||||
imuStartRoll = roll-PI/2;//跟imu摆放的位置有关，减PI / 2是为了让角度变成跟车前进方向的夹角
|
||||
|
||||
imuStartPos = imuPos;
|
||||
imuStartFlag = 0;
|
||||
}
|
||||
|
|
@ -76,7 +77,7 @@ namespace uwb_slam{
|
|||
prevPos = imuPos;
|
||||
if (uwbDataRxTime != uwb_->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_;
|
||||
|
|
|
|||
|
|
@ -45,15 +45,7 @@ namespace uwb_slam{
|
|||
|
||||
while(1){
|
||||
this->Serread();
|
||||
// std::cout<<"s"<<std::endl;
|
||||
// std::cout<<this->x<<std::endl;
|
||||
/*if(t_tmp!=imu_odom_.imu_data_.imu_t_){
|
||||
imu_odom_queue_.push(imu_odom_);
|
||||
t_tmp=imu_odom_.imu_data_.imu_t_;
|
||||
}*/
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -82,13 +74,19 @@ namespace uwb_slam{
|
|||
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;
|
||||
}
|
||||
// 修正车子和标签的高度差
|
||||
// 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];
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
```
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 33 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 22 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 14 KiB |
|
|
@ -0,0 +1,9 @@
|
|||
割草机融合定位：
|
||||
地图坐标系与小车坐标系重合，小车起始点为坐标系原点，使用三个标签分别是A，B，C。A放置在充电桩处。车上放置基站M。通过基站到三个标签的距离获得小车的位置信息。如图所示：
|
||||
|
||||
|
||||
|
||||
注:为了方便，人为设定UWB坐标系与地图坐标系和小车坐标系均重合。
|
||||
小车半径：a
|
||||
标签高度：h
|
||||
BC标签距离d
|
||||
Loading…
Reference in New Issue