SLAM學習筆記（四）定位

原創博客：http://blog.csdn.net/renshengrumenglibing?viewmode=contents
機器人定位的目的是爲了知道“自己在什麼地方”，目前，機器人定位的方法可以分爲非自主定位與自
主定位兩大類。所謂非自主定位是在定位的過程中機器人需要藉助機器人本身以外的裝置如：全球定位
系統(GPS)、全局視覺系統等進行定位；自主定位是機器人僅依靠機器人本身攜帶的傳感器進行定位。
由於在室內環境中，不能使用GPS，而安裝其它的輔助定位系統比較麻煩。因此機器人一般採用自主
定位的方法。
按照初始位姿是否已知，可把機器人自主定位分爲初始位姿已知的位姿跟蹤(Pose tracking)和初始位
姿未知的全局定位(Global localization)。

位姿跟蹤是在已知機器人的初始位姿的條件下，在機器人的運動過程中通過將觀測到的特徵與地圖中
的特徵進行匹配，求取它們之間的差別，進而更新機器人的位姿的機器人定位方法。位姿跟蹤通常採用擴
展卡爾曼濾波器(Extended Kalman Filter，EKF)來實現。該方法採用高斯分佈來近似地表示機器人位姿
的後驗概率分佈，其計算過程主要包括三步：首先是根據機器人的運模型預測機器人的位姿，然後將觀測
信息與地圖進行匹配，最後根據預測後的機器人位姿以及匹配的特徵計算機器人應該觀測到的信息，並利
用應該觀測到的信息與實際觀測到的信息之間的差距來更新機器人的位姿。

全局定位是在機器人的初始位姿不確定的條件下，利用局部的、不完全的觀測信息估計機器人的當前
位姿。能否解決最典型而又最富挑戰性的“綁架恢復”問題在一定程度上反應了機器人全局定位方法的魯棒
性與可靠性。

一、移動機器人 SLAM 技術
可靠的定位性能是自主移動系統的關鍵要素。傳統的定位方法是基於里程計估計的，存在不可避免的
定位誤差。自從移動機器人誕生以來，對定位問題的研究就和地圖創建問題密切關聯，已知環境地圖的定
位問題和已知定位的地圖創建問題已經被廣泛研究，提出了多種有效的解決途徑。當地圖和機器人的位置
都事先未知時，問題就變得更加複雜，出現了許多獨有的新特徵。在這種情況下，要求機器人在一個完全
未知的環境中從一個未知的位置出發，在遞增地建立環境的導航地圖同時，利用已建立的地圖來同步刷新
自身的位置。該問題被稱作同步定位和構圖，簡稱 SLAM。在 SLAM 問題中，機器人位置和地圖兩者的估
算是高度相關的，任何一方都無法獨立獲取，這樣形成了一種相輔相生、不斷迭代的過程，因此有些學者
將其比作“雞與蛋”問題。
近年來，移動機器人 SLAM 技術獲得顯著進步，被認爲是解決環境未知和傳感器信息不確定條件下的
移動機器人自主導航的最有效的技術之一。SLAM 基本思想是利用已創建地圖修正基於運動模型的機器人
位姿估計誤差；同時根據可靠的機器人位姿，創建出精度更高的地圖。

關於傳感器的不確定，以最常見的里程計爲例，其典型的誤差積累如圖所示，其中，左圖是獨立
利用里程計定位、獨立利用激光傳感器感知環境所創建的地圖，由於沒有進行里程計誤差補償，幾次創
建的地圖差異很大，與實際環境也不符；右圖是採用 SLAM 創建的地圖，基於 SLAM 可以利用已創建的
地圖修正里程計的誤差，這樣機器人的位姿誤差就不會隨着機器人的運動距離的增大而無限制增長，因
此可以創建精度更高的地圖，也同時解決了未知環境中的機器人定位問題。

SLAM中，系統的狀態由機器人的位姿和地圖信息(特徵的位置信息)組成。假設機器人在t時刻觀測到
特徵m1，如圖2所示。根據觀測信息只能獲得特徵m1在機器人座標系R中的座標。機器人需要估計機器
人自己本身在世界座標系W中的位姿，然後通過座標變換才能計算特徵的世界座標。可見在地圖創建的過
程中，必須計算機器人的位姿，也就是進行機器人的定位。然而，根據里程計獲得的機器人位置信息很不
準確，顯然錯誤的位置信息將會導致地圖的不準確。
//SLAM示意圖

在初始時刻，激光雷達創建的地圖中並沒有任何的特徵。當機器人觀測到某特徵m時，可以根據
機器人的位姿，以及特徵在機器人座標系下的位姿，計算出特徵在世界座標系下的位姿，此時將特徵
加入到地圖中（更新地圖）；當機器人位姿改變，再次觀測到特徵m，可以根據特徵在世界座標系下
的位姿和特徵在機器人座標系下的位姿，解算出當前機器人的位姿（機器人定位）。
當機器人繼續運動時，它將觀測到更多的特徵，根據同樣的方法。機器人會把它們加入到地圖中，
並且根據觀測到的信息更新機器人的位姿以及它們的世界座標。簡單的說，SLAM利用觀測到的特徵計
算它們的世界座標以實現地圖創建，同時更新機器人的位姿以實現機器人的定位。
SLAM方法有很多種，主要包括基於擴展卡爾曼濾波的SLAM技術，基於傳統粒子濾波的SLAM技術，
快速SLAM技術，基於掃描匹配的SLAM技術等等。
1.1基於掃描匹配的 SLAM 技術
基於掃描匹配的 SLAM是基於最近鄰掃描匹配來估計兩次掃描間機器人的平移和旋轉的算法。掃描
匹配算法主要源自迭代最近點(Iterative Closest Point, ICP)算法及其改進算法。該算法通過迭代細調由
機器人里程計給出的初始位姿，限定了搜索空間。然而，該算法假定機器人的初始位姿和機器人的真實
位姿之間的偏差足夠小，以便於達到全局最優匹配。
1.2 範例：
已知兩條直線在激光雷達座標系下和全局座標系下的直線方程，求激光雷達在全局座標下
的位置（x,y）和姿態theta
實際上已知兩條直線求解是多解的，當theta是真實解，那麼theta+pi同樣是方程組的解，此時
可以引入新的約束，激光雷達實際上看到的兩條直線，只能是直線交叉點一側的部分，那麼求解之
後可以進行驗證，進而排除一個解，此時解唯一。
假設直線L1 全局下的座標方程分別爲y = a1*x + b，在雷達座標系的方程y = a2*x+b2;
傾斜角分別爲thetaW和thetaR，那麼由轉角alpha+ thetaR = thetaW => alpha = thetaW - thetaR;
[Xw] [cos(alpha)-sin(alpha)]Xr
Tx= [Yw] * [sin(alpha) cos(alpha)]Yr + Ty
由兩條直線解出格子座標系下的交點，代入上式可以解出Tx Ty，上式中都是矩陣計算，由於沒有word那麼
強大，各位只能勉強看了。
此時實際上解釋存在兩個的，解出之後需要進行驗證，記下線段的端點，看知否在交點的同一側，如果在同
一側，那麼結果就是對的，否則就要再轉180度。
直線的擬合參照上一篇筆記，http://blog.csdn.net/renshengrumenglibing/article/details/8604245
爲了提高進度，我們可以對數據進行一次中值濾波，抑制噪聲同時儘量保留數據的邊沿。

 //中值濾波 只能對初始的連續數據濾波
 //濾波基本不丟棄數據，兩端會各自扔掉幾個數據
 void OpenRadar::MedFilter(vector<int>& RadarRho, vector<double>& RadarTheta){
     vector<int>rho;
     vector<double>theta;
     int halfWindowSize = 2;
     int *neighbor = new int[2*halfWindowSize+1];
     int temp;
     for (int i = halfWindowSize; i< (int)RadarRho.size() - halfWindowSize;i++)
      {
          for (int j = -halfWindowSize;j <= halfWindowSize;j++)
          {
              neighbor[j + halfWindowSize] = RadarRho.at(i + j); 
          }
          //排序
          for (int m = 0; m < 2*halfWindowSize +1;m++)
          {
              for (int n = m + 1;n < 2*halfWindowSize + 1;n++)
              {
                  if (neighbor[m]> neighbor[n])
                  {
                      temp = neighbor[m];
                      neighbor[m] = neighbor[n];
                      neighbor[n] = temp;
                  }
              }
          }
          rho.push_back(neighbor[halfWindowSize]);
          theta.push_back(RadarTheta.at(i));
      }

     RadarRho.clear();
     RadarTheta.clear();

     for (int i = 0; i < (int)(rho.size());i++)
     {
         RadarRho.push_back(rho.at(i));
         RadarTheta.push_back(theta.at(i));
     }
 }

其他處理跟之前相同，此時可以根據已知的兩條直線進行位姿解算。

//已知四條直線如何計算變換參數
void Coordinate::CalCoorTransPara(CoorTransPara &transPara,
                                  LinePara W1,
                                  LinePara W2, 
                                  LinePara R1, 
                                  LinePara R2)
{
    double theta = ( W1.Rho - R1.Rho + W2.Rho - R2.Rho )/2;
    //double theta = ( W1.Rho - R1.Rho);
    //求解出Xw Yw Xr Yr
    double Xw = (double)(W1.b - W2.b)/(W2.a - W1.a);
    double Yw = W1.a*Xw + W1.b;

    double Xr = (double)(R1.b - R2.b)/(R2.a - R1.a);
    double Yr = R1.a*Xr + R1.b;


    int Tx = (int)(Xw - cos(theta)*Xr + sin(theta)*Yr);
    int Ty = (int)(Yw - sin(theta)*Xr - cos(theta)*Yr);
    //交點判定，場地上的幾條直線都是有角點的
    iPoint crossPoint;//交點
    iPoint vectorW1,vectorR1;//向量
    //iPoint vectorR2,vectorW2;
    if (W1.startPoint.x == W2.startPoint.x && W1.startPoint.y == W2.startPoint.y)
    {
        crossPoint = ipoint(W1.startPoint.x,W1.startPoint.y);
        vectorW1 = ipoint(W1.endPoint.x - W1.startPoint.x, W1.endPoint.y - W1.startPoint.y);
        //vectorW2 = ipoint(W2.endPoint.x - W2.startPoint.x, W2.endPoint.y - W2.startPoint.y);
    }else if (W1.endPoint.x == W2.startPoint.x && W1.endPoint.y == W2.startPoint.y)
    {
        crossPoint = ipoint(W1.endPoint.x,W1.endPoint.y);
        vectorW1 = ipoint(W1.startPoint.x - W1.endPoint.x, W1.startPoint.y - W1.endPoint.y);
        //vectorW2 = ipoint(W2.endPoint.x - W2.startPoint.x, W2.endPoint.y - W2.startPoint.y);
    }else if (W1.startPoint.x == W2.endPoint.x && W1.startPoint.y == W2.endPoint.y)
    {
         crossPoint = ipoint(W1.startPoint.x,W1.startPoint.y);
         vectorW1 = ipoint(W1.endPoint.x - W1.startPoint.x, W1.endPoint.y - W1.startPoint.y);
         //vectorW2 = ipoint(W2.startPoint.x - W2.endPoint.x, W2.startPoint.y - W2.endPoint.y);
    }else if (W1.endPoint.x == W2.endPoint.x && W1.endPoint.y == W2.endPoint.y)
    {
         crossPoint = ipoint(W1.endPoint.x,W1.endPoint.y);
         vectorW1 = ipoint(W1.startPoint.x - W1.endPoint.x, W1.startPoint.y - W1.endPoint.y);
         //vectorW2 = ipoint(W2.startPoint.x - W2.endPoint.x, W2.startPoint.y - W2.endPoint.y);
    }
    //將激光雷達下的兩個點旋轉到W系下
    transPara.theta = theta;
    transPara.Tx = Tx;
    transPara.Ty = Ty;
    iPoint R1ToW;
    //iPoint R2ToW;
    TransformCoord(transPara,R1.startPoint,R1ToW);
    //TransformCoord(transPara,R2.startPoint,R2ToW);
    vectorR1.x = R1ToW.x - crossPoint.x;
    vectorR1.y = R1ToW.y - crossPoint.y;
    //判斷是否在同一側？
    if (vectorW1.x * vectorR1.x + vectorW1.y*vectorR1.y < 0)
    {
        //旋轉角度差了180度，需要調轉180度
        transPara.theta = theta + PI;
        transPara.Tx = (int)(Xw - cos(transPara.theta)*Xr + sin(transPara.theta)*Yr);
        transPara.Ty = (int)(Yw - sin(transPara.theta)*Xr - cos(transPara.theta)*Yr);
    }else{

    }
    //數據測試
    /* TransformCoord(transPara,R1.startPoint,R1ToW);
     cout<<"R1ToW.x "<<R1ToW.x<<"  R1ToW.y "<<R1ToW.y<<endl;
     TransformCoord(transPara,R1.endPoint,R1ToW);
     cout<<"R1ToW.x "<<R1ToW.x<<"  R1ToW.y "<<R1ToW.y<<endl;

     TransformCoord(transPara,R2.startPoint,R2ToW);
     cout<<"R2ToW.x "<<R2ToW.x<<"  R2ToW.y "<<R2ToW.y<<endl;

     TransformCoord(transPara,R2.endPoint,R2ToW);
     cout<<"R2ToW.x "<<R2ToW.x<<"  R2ToW.y "<<R2ToW.y<<endl;*/
    //進行一次驗證，看看交點進行座標變換之後是否接近匹配的點
   /* iPoint R = ipoint(Xr,Yr);
    iPoint R2W;
    TransformCoord(transPara,R,R2W);
    cout<<"R2W.x "<<R2W.x<<"  R2W.y "<<R2W.y<<endl;*/

}

//完整的Coordinate.h

#pragma once
#include "WeightedFit.h"
#include <iostream>
using namespace std;

//場地中的關鍵點
static iPoint FieldPointA = ipoint(9192,0);
static iPoint FieldPointB = ipoint(0,9192); 
static iPoint FieldPointC = ipoint(-9192,0);
static iPoint FieldPointD = ipoint(0,-9192);
//場地中的直線變量
static LinePara FieldLine1 = linePara(-1.0,9192.3881554,FieldPointA,FieldPointB);
static LinePara FieldLine2 = linePara(1.0,9192.3881554,FieldPointB,FieldPointC);
static LinePara FieldLine3 = linePara(-1.0,-9192.3881554,FieldPointC,FieldPointD);
static LinePara FieldLine4 = linePara(1.0,-9192.3881554,FieldPointD,FieldPointA);
static LinePara FieldLine5 = linePara(100000.0,0.0,FieldPointB,FieldPointD);

//場地中的圓
static CirclePara FieldCircle1 = circlePara(-3000,1301,400,350);
static CirclePara FieldCircle2 = circlePara(-1951,880,400,350);
static CirclePara FieldCircle3 = circlePara(-651,815,400,350);
static CirclePara FieldCircle4 = circlePara(-495,2416,400,350);
static CirclePara FieldCircle5 = circlePara(-3347,-997,400,350);
static CirclePara FieldCircle6 = circlePara(-2400,-2848,400,350);
static CirclePara FieldCircle7 = circlePara(-1499,-2499,400,350);

static CirclePara FieldCircle8 = circlePara(3000,1301,400,350);
static CirclePara FieldCircle9 = circlePara(1951,880,400,350);
static CirclePara FieldCircle10 = circlePara(651,815,400,350);
static CirclePara FieldCircle11= circlePara(495,2416,400,350);
static CirclePara FieldCircle12 = circlePara(3347,-997,400,350);
static CirclePara FieldCircle13 = circlePara(2400,-2848,400,350);
static CirclePara FieldCircle14 = circlePara(1499,-2499,400,350);

//座標系類，進行座標系相關的計算
typedef struct{
    int Tx;
    int Ty;
    double theta;//旋轉角
}CoorTransPara;  //座標變換參數

class Coordinate
{
public:

    Coordinate(void);
    ~Coordinate(void);
    //已知四條直線如何計算變換參數
    void CalCoorTransPara(CoorTransPara &transPara,
                          LinePara W1,
                          LinePara W2, 
                          LinePara R1, 
                          LinePara R2);
    void CoortransTest();
    void CalRadarCoord();

    CoorTransPara RadarCoordTransPara;//全局座標系和雷達座標系之間的轉換參數
    void printRadarCoordtransPara(CoorTransPara coordtrans);
    void TransformCoord(CoorTransPara transPara,iPoint R,iPoint& W);

};

//完整的Coordiate.cpp

#include "Coordinate.h"


Coordinate::Coordinate(void)
{
}


Coordinate::~Coordinate(void)
{
}

//已知四條直線如何計算變換參數
void Coordinate::CalCoorTransPara(CoorTransPara &transPara,
                                  LinePara W1,
                                  LinePara W2, 
                                  LinePara R1, 
                                  LinePara R2)
{
    double theta = ( W1.Rho - R1.Rho + W2.Rho - R2.Rho )/2;
    //double theta = ( W1.Rho - R1.Rho);
    //求解出Xw Yw Xr Yr
    double Xw = (double)(W1.b - W2.b)/(W2.a - W1.a);
    double Yw = W1.a*Xw + W1.b;

    double Xr = (double)(R1.b - R2.b)/(R2.a - R1.a);
    double Yr = R1.a*Xr + R1.b;


    int Tx = (int)(Xw - cos(theta)*Xr + sin(theta)*Yr);
    int Ty = (int)(Yw - sin(theta)*Xr - cos(theta)*Yr);
    //交點判定，場地上的幾條直線都是有角點的
    iPoint crossPoint;//交點
    iPoint vectorW1,vectorR1;//向量
    //iPoint vectorR2,vectorW2;
    if (W1.startPoint.x == W2.startPoint.x && W1.startPoint.y == W2.startPoint.y)
    {
        crossPoint = ipoint(W1.startPoint.x,W1.startPoint.y);
        vectorW1 = ipoint(W1.endPoint.x - W1.startPoint.x, W1.endPoint.y - W1.startPoint.y);
        //vectorW2 = ipoint(W2.endPoint.x - W2.startPoint.x, W2.endPoint.y - W2.startPoint.y);
    }else if (W1.endPoint.x == W2.startPoint.x && W1.endPoint.y == W2.startPoint.y)
    {
        crossPoint = ipoint(W1.endPoint.x,W1.endPoint.y);
        vectorW1 = ipoint(W1.startPoint.x - W1.endPoint.x, W1.startPoint.y - W1.endPoint.y);
        //vectorW2 = ipoint(W2.endPoint.x - W2.startPoint.x, W2.endPoint.y - W2.startPoint.y);
    }else if (W1.startPoint.x == W2.endPoint.x && W1.startPoint.y == W2.endPoint.y)
    {
         crossPoint = ipoint(W1.startPoint.x,W1.startPoint.y);
         vectorW1 = ipoint(W1.endPoint.x - W1.startPoint.x, W1.endPoint.y - W1.startPoint.y);
         //vectorW2 = ipoint(W2.startPoint.x - W2.endPoint.x, W2.startPoint.y - W2.endPoint.y);
    }else if (W1.endPoint.x == W2.endPoint.x && W1.endPoint.y == W2.endPoint.y)
    {
         crossPoint = ipoint(W1.endPoint.x,W1.endPoint.y);
         vectorW1 = ipoint(W1.startPoint.x - W1.endPoint.x, W1.startPoint.y - W1.endPoint.y);
         //vectorW2 = ipoint(W2.startPoint.x - W2.endPoint.x, W2.startPoint.y - W2.endPoint.y);
    }
    //將激光雷達下的兩個點旋轉到W系下
    transPara.theta = theta;
    transPara.Tx = Tx;
    transPara.Ty = Ty;
    iPoint R1ToW;
    //iPoint R2ToW;
    TransformCoord(transPara,R1.startPoint,R1ToW);
    //TransformCoord(transPara,R2.startPoint,R2ToW);
    vectorR1.x = R1ToW.x - crossPoint.x;
    vectorR1.y = R1ToW.y - crossPoint.y;
    //判斷是否在同一側？
    if (vectorW1.x * vectorR1.x + vectorW1.y*vectorR1.y < 0)
    {
        //旋轉角度差了180度，需要調轉180度
        transPara.theta = theta + PI;
        transPara.Tx = (int)(Xw - cos(transPara.theta)*Xr + sin(transPara.theta)*Yr);
        transPara.Ty = (int)(Yw - sin(transPara.theta)*Xr - cos(transPara.theta)*Yr);
    }else{

    }
    //數據測試
    /* TransformCoord(transPara,R1.startPoint,R1ToW);
     cout<<"R1ToW.x "<<R1ToW.x<<"  R1ToW.y "<<R1ToW.y<<endl;
     TransformCoord(transPara,R1.endPoint,R1ToW);
     cout<<"R1ToW.x "<<R1ToW.x<<"  R1ToW.y "<<R1ToW.y<<endl;

     TransformCoord(transPara,R2.startPoint,R2ToW);
     cout<<"R2ToW.x "<<R2ToW.x<<"  R2ToW.y "<<R2ToW.y<<endl;

     TransformCoord(transPara,R2.endPoint,R2ToW);
     cout<<"R2ToW.x "<<R2ToW.x<<"  R2ToW.y "<<R2ToW.y<<endl;*/
    //進行一次驗證，看看交點進行座標變換之後是否接近匹配的點
   /* iPoint R = ipoint(Xr,Yr);
    iPoint R2W;
    TransformCoord(transPara,R,R2W);
    cout<<"R2W.x "<<R2W.x<<"  R2W.y "<<R2W.y<<endl;*/

}

void Coordinate::CoortransTest(){
    Coordinate coord;
    CoorTransPara coordtrans;
    coord.CalCoorTransPara(coordtrans,FieldLine1,FieldLine5,FieldLine2,FieldLine5);
    cout<<"theta : "<<coordtrans.theta*180/PI<<" Tx: "<<coordtrans.Tx<<"  Ty: "<<coordtrans.Ty<<endl;
}

void Coordinate::printRadarCoordtransPara(CoorTransPara coordtrans){
    cout<<"theta : "<<coordtrans.theta*180/PI<<" Tx: "<<coordtrans.Tx<<"  Ty: "<<coordtrans.Ty<<endl;
}


void Coordinate::TransformCoord(CoorTransPara transPara,iPoint R,iPoint& W){

    W.x = (int)(R.x*cos(transPara.theta) - R.y*sin(transPara.theta) );
    W.y = (int)(R.x*sin(transPara.theta) + R.y*cos(transPara.theta) );
    W.x = W.x + transPara.Tx ;
    W.y = W.y + transPara.Ty ;
}

SLAM學習筆記（四）定位

ROS文件系統介紹(二)

成員函數在內存中的分佈

單繼承類的虛函數表

數據在網絡中的傳輸（一）

編譯ROS程序包（四）

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結