鐳射雷達學習筆記（四）定位
原文：http://blog.csdn.net/renshengrumenglibing?viewmode=contents
機器人定位的目的是為了知道“自己在什麼地方”，目前，機器人定位的方法可以分為非自主定位與自
主定位兩大類。所謂非自主定位是在定位的過程中機器人需要藉助機器人本身以外的裝置如：全球定位
系統(GPS)、全域性視覺系統等進行定位；自主定位是機器人僅依靠機器人本身攜帶的感測器進行定位。
由於在室內環境中，不能使用GPS，而安裝其它的輔助定位系統比較麻煩。因此機器人一般採用自主
定位的方法。
按照初始位姿是否已知，可把機器人自主定位分為初始位姿已知的位姿跟蹤(Pose tracking)和初始位
姿未知的全域性定位(Global localization)。
位姿跟蹤是在已知機器人的初始位姿的條件下，在機器人的運動過程中通過將觀測到的特徵與地圖中
的特徵進行匹配，求取它們之間的差別，進而更新機器人的位姿的機器人定位方法。位姿跟蹤通常採用擴
展卡爾曼濾波器(Extended Kalman Filter，EKF)來實現。該方法採用高斯分佈來近似地表示機器人位姿
的後驗概率分佈，其計算過程主要包括三步：首先是根據機器人的運模型預測機器人的位姿，然後將觀測
資訊與地圖進行匹配，最後根據預測後的機器人位姿以及匹配的特徵計算機器人應該觀測到的資訊，並利
用應該觀測到的資訊與實際觀測到的資訊之間的差距來更新機器人的位姿。
全域性定位是在機器人的初始位姿不確定的條件下，利用區域性的、不完全的觀測資訊估計機器人的當前
位姿。能否解決最典型而又最富挑戰性的“綁架恢復”問題在一定程度上反應了機器人全域性定位方法的魯棒
性與可靠性。
一、移動機器人 SLAM 技術
可靠的定位效能是自主移動系統的關鍵要素。傳統的定位方法是基於里程計估計的，存在不可避免的
定位誤差。自從移動機器人誕生以來，對定位問題的研究就和地圖建立問題密切關聯，已知環境地圖的定
位問題和已知定位的地圖建立問題已經被廣泛研究，提出了多種有效的解決途徑。當地圖和機器人的位置
都事先未知時，問題就變得更加複雜，出現了許多獨有的新特徵。在這種情況下，要求機器人在一個完全
未知的環境中從一個未知的位置出發，在遞增地建立環境的導航地圖同時，利用已建立的地圖來同步重新整理
自身的位置。該問題被稱作同步定位和構圖，簡稱 SLAM。在 SLAM 問題中，機器人位置和地圖兩者的估 www.it165.net
算是高度相關的，任何一方都無法獨立獲取，這樣形成了一種相輔相生、不斷迭代的過程，因此有些學者
將其比作“雞與蛋”問題。
近年來，移動機器人 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)]XrTx
= *+
[Yw] [sin(alpha)cos(alpha)]YrTy
由兩條直線解出格子座標系下的交點，代入上式可以解出Tx Ty，上式中都是矩陣計算，由於沒有word那麼
強大，各位只能勉強看了。
此時實際上解釋存在兩個的，解出之後需要進行驗證，記下線段的端點，看知否在交點的同一側，如果在同
一側，那麼結果就是對的，否則就要再轉180度。
為了提高進度，我們可以對資料進行一次中值濾波，抑制噪聲同時儘量保留資料的邊沿。
01.//中值濾波 只能對初始的連續資料濾波
02.//濾波基本不丟棄資料，兩端會各自扔掉幾個資料
03.void
OpenRadar::MedFilter(vector<int>& RadarRho, vector<double>& RadarTheta){
04.vector<int>rho;
05.vector<double>theta;
06.int
halfWindowSize = 2;
07.int
*neighbor = new
int[2*halfWindowSize+1];
08.int
temp;
09.for
(int
i = halfWindowSize; i< (int)RadarRho.size() - halfWindowSize;i++)
10.{
11.for
(int
j = -halfWindowSize;j <= halfWindowSize;j++)
12.{
13.neighbor[j + halfWindowSize] = RadarRho.at(i + j);
14.}
15.//排序
16.for
(int
m = 0; m < 2*halfWindowSize +1;m++)
17.{
18.for
(int
n = m + 1;n < 2*halfWindowSize +
1;n++)
19.{
20.if
(neighbor[m]> neighbor[n])
21.{
22.temp = neighbor[m];
23.neighbor[m] = neighbor[n];
24.neighbor[n] = temp;
25.}
26.}
27.}
28.rho.push_back(neighbor[halfWindowSize]);
29.theta.push_back(RadarTheta.at(i));
30.}
31.
32.RadarRho.clear();
33.RadarTheta.clear();
34.
35.for
(int
i = 0; i < (int)(rho.size());i++)
36.{
37.RadarRho.push_back(rho.at(i));
38.RadarTheta.push_back(theta.at(i));
39.}
40.}
其他處理跟之前相同，此時可以根據已知的兩條直線進行位姿解算。
01.//已知四條直線如何計算變換引數
02.void
Coordinate::CalCoorTransPara(CoorTransPara &transPara,
03.LinePara W1,
04.LinePara W2,
05.LinePara R1,
06.LinePara R2)
07.{
08.double
theta = ( W1.Rho - R1.Rho + W2.Rho - R2.Rho )/2;
09.//double theta = ( W1.Rho - R1.Rho);
10.//求解出Xw Yw Xr Yr
11.double
Xw = (double)(W1.b - W2.b)/(W2.a - W1.a);
12.double
Yw = W1.a*Xw + W1.b;
13.
14.double
Xr = (double)(R1.b - R2.b)/(R2.a - R1.a);
15.double
Yr = R1.a*Xr + R1.b;
16.
17.
18.int
Tx = (int)(Xw - cos(theta)*Xr + sin(theta)*Yr);
19.int
Ty = (int)(Yw - sin(theta)*Xr - cos(theta)*Yr);
20.//交點判定，場地上的幾條直線都是有角點的
21.iPoint crossPoint;//交點
22.iPoint vectorW1,vectorR1;//向量
23.//iPoint vectorR2,vectorW2;
24.if
(W1.startPoint.x == W2.startPoint.x && W1.startPoint.y == W2.startPoint.y)
25.{
26.crossPoint = ipoint(W1.startPoint.x,W1.startPoint.y);
27.vectorW1 = ipoint(W1.endPoint.x - W1.startPoint.x, W1.endPoint.y - W1.startPoint.y);
28.//vectorW2 = ipoint(W2.endPoint.x - W2.startPoint.x, W2.endPoint.y - W2.startPoint.y);
29.}else
if (W1.endPoint.x == W2.startPoint.x && W1.endPoint.y == W2.startPoint.y)
30.{
31.crossPoint = ipoint(W1.endPoint.x,W1.endPoint.y);
32.vectorW1 = ipoint(W1.startPoint.x - W1.endPoint.x, W1.startPoint.y - W1.endPoint.y);
33.//vectorW2 = ipoint(W2.endPoint.x - W2.startPoint.x, W2.endPoint.y - W2.startPoint.y);
34.}else
if (W1.startPoint.x == W2.endPoint.x && W1.startPoint.y == W2.endPoint.y)
35.{
36.crossPoint = ipoint(W1.startPoint.x,W1.startPoint.y);
37.vectorW1 = ipoint(W1.endPoint.x - W1.startPoint.x, W1.endPoint.y - W1.startPoint.y);
38.//vectorW2 = ipoint(W2.startPoint.x - W2.endPoint.x, W2.startPoint.y - W2.endPoint.y);
39.}else
if (W1.endPoint.x == W2.endPoint.x && W1.endPoint.y == W2.endPoint.y)
40.{
41.crossPoint = ipoint(W1.endPoint.x,W1.endPoint.y);
42.vectorW1 = ipoint(W1.startPoint.x - W1.endPoint.x, W1.startPoint.y - W1.endPoint.y);
43.//vectorW2 = ipoint(W2.startPoint.x - W2.endPoint.x, W2.startPoint.y - W2.endPoint.y);
44.}
45.//將鐳射雷達下的兩個點旋轉到W系下
46.transPara.theta = theta;
47.transPara.Tx = Tx;
48.transPara.Ty = Ty;
49.iPoint R1ToW;
50.//iPoint R2ToW;
51.TransformCoord(transPara,R1.startPoint,R1ToW);
52.//TransformCoord(transPara,R2.startPoint,R2ToW);
53.vectorR1.x = R1ToW.x - crossPoint.x;
54.vectorR1.y = R1ToW.y - crossPoint.y;
55.//判斷是否在同一側？
56.if
(vectorW1.x * vectorR1.x + vectorW1.y*vectorR1.y < 0)
57.{
58.//旋轉角度差了180度，需要調轉180度
59.transPara.theta = theta + PI;
60.transPara.Tx = (int)(Xw - cos(transPara.theta)*Xr + sin(transPara.theta)*Yr);
61.transPara.Ty = (int)(Yw - sin(transPara.theta)*Xr - cos(transPara.theta)*Yr);
62.}else{
63.
64.}
65.//資料測試
66./* TransformCoord(transPara,R1.startPoint,R1ToW);
67.cout<<"R1ToW.x "<<R1ToW.x<<" R1ToW.y "<<R1ToW.y<<endl;
68.TransformCoord(transPara,R1.endPoint,R1ToW);
69.cout<<"R1ToW.x "<<R1ToW.x<<" R1ToW.y "<<R1ToW.y<<endl;
70.
71.TransformCoord(transPara,R2.startPoint,R2ToW);
72.cout<<"R2ToW.x "<<R2ToW.x<<" R2ToW.y "<<R2ToW.y<<endl;
73.
74.TransformCoord(transPara,R2.endPoint,R2ToW);
75.cout<<"R2ToW.x "<<R2ToW.x<<" R2ToW.y "<<R2ToW.y<<endl;*/
76.//進行一次驗證，看看交點進行座標變換之後是否接近匹配的點
77./* iPoint R = ipoint(Xr,Yr);
78.iPoint R2W;
79.TransformCoord(transPara,R,R2W);
80.cout<<"R2W.x "<<R2W.x<<" R2W.y "<<R2W.y<<endl;*/
81.
82.}
//完整的Coordinate.h
01.#pragma once
02.#include
"WeightedFit.h"
03.#include <iostream>
04.using namespace std;
05.
06.//場地中的關鍵點
07.static
iPoint FieldPointA = ipoint(9192,0);
08.static
iPoint FieldPointB = ipoint(0,9192);
09.static
iPoint FieldPointC = ipoint(-9192,0);
10.static
iPoint FieldPointD = ipoint(0,-9192);
11.//場地中的直線變數
12.static
LinePara FieldLine1 = linePara(-1.0,9192.3881554,FieldPointA,FieldPointB);
13.static
LinePara FieldLine2 = linePara(1.0,9192.3881554,FieldPointB,FieldPointC);
14.static
LinePara FieldLine3 = linePara(-1.0,-9192.3881554,FieldPointC,FieldPointD);
15.static
LinePara FieldLine4 = linePara(1.0,-9192.3881554,FieldPointD,FieldPointA);
16.static
LinePara FieldLine5 = linePara(100000.0,0.0,FieldPointB,FieldPointD);
17.
18.//場地中的圓
19.static
CirclePara FieldCircle1 = circlePara(-3000,1301,400,350);
20.static
CirclePara FieldCircle2 = circlePara(-1951,880,400,350);
21.static
CirclePara FieldCircle3 = circlePara(-651,815,400,350);
22.static
CirclePara FieldCircle4 = circlePara(-495,2416,400,350);
23.static
CirclePara FieldCircle5 = circlePara(-3347,-997,400,350);
24.static
CirclePara FieldCircle6 = circlePara(-2400,-2848,400,350);
25.static
CirclePara FieldCircle7 = circlePara(-1499,-2499,400,350);
26.
27.static
CirclePara FieldCircle8 = circlePara(3000,1301,400,350);
28.static
CirclePara FieldCircle9 = circlePara(1951,880,400,350);
29.static
CirclePara FieldCircle10 = circlePara(651,815,400,350);
30.static
CirclePara FieldCircle11= circlePara(495,2416,400,350);
31.static
CirclePara FieldCircle12 = circlePara(3347,-997,400,350);
32.static
CirclePara FieldCircle13 = circlePara(2400,-2848,400,350);
33.static
CirclePara FieldCircle14 = circlePara(1499,-2499,400,350);
34.
35.//座標系類，進行座標系相關的計算
36.typedef struct{
37.int
Tx;
38.int
Ty;
39.double
theta;//旋轉角
40.}CoorTransPara;
//座標變換引數
41.
42.class
Coordinate
43.{
44.public:
45.
46.Coordinate(void);
47.~Coordinate(void);
48.//已知四條直線如何計算變換引數
49.void
CalCoorTransPara(CoorTransPara &transPara,
50.LinePara W1,
51.LinePara W2,
52.LinePara R1,
53.LinePara R2);
54.void
CoortransTest();
55.void
CalRadarCoord();
56.
57.CoorTransPara RadarCoordTransPara;//全域性座標系和雷達座標系之間的轉換引數
58.void
printRadarCoordtransPara(CoorTransPara coordtrans);
59.void
TransformCoord(CoorTransPara transPara,iPoint R,iPoint& W);
60.
61.};
//完整的Coordiate.cpp
001.#include "Coordinate.h"
002.
003.
004.Coordinate::Coordinate(void)
005.{
006.}
007.
008.
009.Coordinate::~Coordinate(void)
010.{
011.}
012.
013.//已知四條直線如何計算變換引數
014.void
Coordinate::CalCoorTransPara(CoorTransPara &transPara,
015.LinePara W1,
016.LinePara W2,
017.LinePara R1,
018.LinePara R2)
019.{
020.double
theta = ( W1.Rho - R1.Rho + W2.Rho - R2.Rho )/2;
021.//double theta = ( W1.Rho - R1.Rho);
022.//求解出Xw Yw Xr Yr
023.double
Xw = (double)(W1.b - W2.b)/(W2.a - W1.a);
024.double
Yw = W1.a*Xw + W1.b;
025.
026.double
Xr = (double)(R1.b - R2.b)/(R2.a - R1.a);
027.double
Yr = R1.a*Xr + R1.b;
028.
029.
030.int
Tx = (int)(Xw -
cos(theta)*Xr +
sin(theta)*Yr);
031.int
Ty = (int)(Yw -
sin(theta)*Xr -
cos(theta)*Yr);
032.//交點判定，場地上的幾條直線都是有角點的
033.iPoint crossPoint;//交點
034.iPoint vectorW1,vectorR1;//向量
035.//iPoint vectorR2,vectorW2;
036.if
(W1.startPoint.x == W2.startPoint.x && W1.startPoint.y == W2.startPoint.y)
037.{
038.crossPoint = ipoint(W1.startPoint.x,W1.startPoint.y);
039.vectorW1 = ipoint(W1.endPoint.x - W1.startPoint.x, W1.endPoint.y - W1.startPoint.y);
040.//vectorW2 = ipoint(W2.endPoint.x - W2.startPoint.x, W2.endPoint.y - W2.startPoint.y);
041.}else
if (W1.endPoint.x == W2.startPoint.x && W1.endPoint.y == W2.startPoint.y)
042.{
043.crossPoint = ipoint(W1.endPoint.x,W1.endPoint.y);
044.vectorW1 = ipoint(W1.startPoint.x - W1.endPoint.x, W1.startPoint.y - W1.endPoint.y);
045.//vectorW2 = ipoint(W2.endPoint.x - W2.startPoint.x, W2.endPoint.y - W2.startPoint.y);
046.}else
if (W1.startPoint.x == W2.endPoint.x && W1.startPoint.y == W2.endPoint.y)
047.{
048.crossPoint = ipoint(W1.startPoint.x,W1.startPoint.y);
049.vectorW1 = ipoint(W1.endPoint.x - W1.startPoint.x, W1.endPoint.y - W1.startPoint.y);
050.//vectorW2 = ipoint(W2.startPoint.x - W2.endPoint.x, W2.startPoint.y - W2.endPoint.y);
051.}else
if (W1.endPoint.x == W2.endPoint.x && W1.endPoint.y == W2.endPoint.y)
052.{
053.crossPoint = ipoint(W1.endPoint.x,W1.endPoint.y);
054.vectorW1 = ipoint(W1.startPoint.x - W1.endPoint.x, W1.startPoint.y - W1.endPoint.y);
055.//vectorW2 = ipoint(W2.startPoint.x - W2.endPoint.x, W2.startPoint.y - W2.endPoint.y);