Embed Size (px)
Citation preview
Ââåäåíèå
SLAM (Simultaneous Localization And Mapping) � çàäà÷à ïîñòðîåíèÿ
êàðòû ïðîñòðàíñòâà, ñòðóêòóðà êîòîðîãî çàðàíåå íåèçâåñòíà, è
îäíîâðåìåííîãî îïðåäåëåíèÿ ïîçèöèè ðîáîòà â íåì.
 íàñòîÿùåå âðåìÿ:
I çàäà÷à ëîêàëèçàöèè ðåøàåòñÿ ñ ïðèåìëåìîé òî÷íîñòüþ ïî
äàííûì ñåíñîðîâ è èìåþùåéñÿ äîñòîâåðíîé êàðòå;
I çàäà÷à ïîñòðîåíèÿ êàðòû ðåøàåòñÿ ïðè èçâåñòíîé òî÷íîé
òðàåêòîðèè ðîáîòà è äàííûì ñåíñîðîâ;
I îáùåãî ïîäõîäà, ðåøàþùåãî ýòè çàäà÷è îäíîâðåìåííî ñ çàäàííîé
òî÷íîñòüþ è âíå çàâèñèìîñòè îò îñîáåííîñòåé îêðóæåíèÿ, íà
äàííûé ìîìåíò íå ñóùåñòâóåò.
1 / 20
Ïðèìåíåíèå SLAM-ìåòîäîâ
Ïðèìåð êàðò, ïîëó÷åííûõ ñ èñïîëüçîâàíèåì òîëüêî �ñûðûõ� äàííûõ
ñåíñîðîâ (a), è ïðè ïðèìåíåíèè îäíîãî èç SLAM-ìåòîäîâ (b):
(a) �Ñûðûå� äàííûå (b) SLAM
2 / 20
Îáùèé ïîäõîä ê ðåøåíèþ çàäà÷è SLAM
Âåðîÿòíîñòíûé ìåòîä, îñíîâàííûé íà òåîðåìå Áàéåñà:
1. Îöåíêà ïîçèöèè ðîáîòà: öèêë ïðåäñêàçàíèå (äàííûå îäîìåòðèè)
� êîððåêöèÿ (âíåøíèå íàáëþäåíèÿ).
2. Îáíîâëåíèå êàðòû, èñïîëüçóÿ âíåøíèå íàáëþäåíèÿ è
ñêîððåêòèðîâàííóþ ïîçèöèþ.
3 / 20
Êëàññû SLAM-àëãîðèòìîâ
Ìåòîäû ðåøåíèÿ çàäà÷è SLAM ìîæíî îáúåäèíèòü â êëàññû:
I ïî òèïó âõîäíûõ äàííûõ (èñïîëüçóåìûõ ðîáîòîì ñåíñîðîâ):I ëàçåðíûé ñêàíåð è îäîìåòðèÿ;I êàìåðà è îäîìåòðèÿ (âèçóàëüíàÿ îäîìåòðèÿ);I ñòåðåîêàìåðà èëè RGB-D êàìåðà;I ñåíñîð ìàãíèòíîãî ïîëÿ;
I ïî òèïó âûõîäíîé êàðòû:I ñåòêà çàíÿòîñòè (occupancy grid);I ãðàô (pose graph);I êîìáèíèðîâàííàÿ.
4 / 20
Îáîáùåííàÿ ñõåìà SLAM-àëãîðèòìîâ
sensor1
sensor2
trackings
1data
fMap
pose est.1
+
pose est.2
sensor
fusion
s2data
poseest
5 / 20
2D-SLAM: òåðìèíîëîãèÿ
Ëàçåðíûé ñêàí (laser scan) � íàáîð èçìåðåíèé ëàçåðíîãî ñêàíåðà
(òî÷åê), êàæäîå èç êîòîðûõ îïèñûâàåòñÿ ðàññòîÿíèåì äî ïðåïÿòñòâèÿ
è óãëîì (íàïðàâëåíèåì ëó÷à ëàçåðà).
Îäîìåòðèÿ (odometry) � äàííûå äàò÷èêîâ ïåðåìåùåíèÿ,
èñïîëüçóåìûå äëÿ îöåíêè èçìåíåíèÿ ïîçèöèè ðîáîòà ñ òå÷åíèåì
âðåìåíè.
Ñåòêà çàíÿòîñòè (occupancy grid) � ïðåäñòàâëåíèå êàðòû
îêðóæàþùåãî ïðîñòðàíñòâà â âèäå ðàâíîìåðíîé ñåòêè, êàæäàÿ ÿ÷åéêà
êîòîðîé õðàíèò âåðîÿòíîñòü íàëè÷èÿ ïðåïÿòñòâèÿ â ýòîì ìåñòå
ïðîñòðàíñòâà.
6 / 20
2D-SLAM: òåðìèíîëîãèÿ (ïðîäîëæåíèå)
Ñêàí-ìàò÷åð (scan matcher) � àëãîðèòì ñîïîñòàâëåíèÿ äâóõ
ëàçåðíûõ ñêàíîâ (ëèáî ëàçåðíîãî ñêàíà è êàðòû), âû÷èñëÿþùèé
òðàíñôîðìàöèþ (ïåðåìåùåíèå è ïîâîðîò) ìåæäó ýòèìè ñêàíàìè.
Ôèëüòð ÷àñòèö (particle �lter) � ñïîñîá îöåíêè ïàðàìåòðîâ ñèñòåìû
(À), êîòîðûå íåëüçÿ èçìåðèòü íàïðÿìóþ, èñïîëüçóÿ èçìåðåíèÿ äðóãèõ
ïàðàìåòðîâ (Á), ñâÿçàííûõ ñ ïåðâûìè. Ôèëüòð ñîçäàåò ìíîæåñòâî
ãèïîòåç (÷àñòèö) î òåêóùåì çíà÷åíèè ïàðàìåòðîâ À, ïîñëå ÷åãî
èòåðàòèâíî îòñåèâàåò íåäîñòîâåðíûå ãèïîòåçû, îñíîâûâàÿñü íà
èçìåðåíèÿõ ïàðàìåòðîâ Á.
7 / 20
Ñõåìà SLAM, èñïîëüçóþùåãî ëàçåðíûé ñêàíåð
IMU trackingodometry pose est.
1
+
pose est.2
sensor
fusion
scanlaser
scanner
poserefgrid map
scan
matcher
+
Ïðèìåðû: tinySLAM, GMapping
8 / 20
TinySLAM
TinySLAM1 � îäèí èç ñàìûõ ïðîñòûõ äëÿ ðåàëèçàöèè è ïîíèìàíèÿ
SLAM-ìåòîäîâ.
Îñîáåííîñòè:
I âõîäíûå äàííûå � îäîìåòðèÿ è ëàçåðíûé ñêàí;
I ñòðóêòóðà îêðóæàþùåãî ïðîñòðàíñòâà ïðåäñòàâëÿåòñÿ â âèäå
ñåòêè çàíÿòîñòè;
I ïîëîæåíèå ðîáîòà çàäàåòñÿ 2D-êîîðäèíàòàìè è íàïðàâëåíèåì;
I ïîääåðæèâàåò ðîâíî îäíó ãèïîòåçó î ñîñòîÿíèè ìèðà (êàðòà +
ïîçèöèÿ ðîáîòà).
1B. Steux, O. E. Hamzaoui. �tinySLAM: A SLAM algorithm in less than 200 linesC-language program�, 2010.
9 / 20
Àëãîðèòì TinySLAM
Tracking
current position
odom
pose estimate
odometry
Scan matcher (Monte-Carlo)
initial pose
initial pose
try1
try2
tryn
sample pose
scan
match scanrefined pose
Map update
(best match)
10 / 20
GMapping
GMapping2 � îäèí èç íàèáîëåå èçâåñòíûõ 2D SLAM ìåòîäîâ.
Îñîáåííîñòè:
I îñíîâàí íà ôèëüòðå ÷àñòèö Ðàî-Áëýêâåëëà (RBPF);
I ïîääåðæèâàåò îäíîâðåìåííî íåñêîëüêî ãèïîòåç î ñîñòîÿíèè ìèðà;
I ïðåäñòàâëåíèå îêðóæàþùåãî ïðîñòðàíñòâà � ñåòêà çàíÿòîñòè,
âõîäíûå äàííûå � ëàçåðíûé ñêàí è îäîìåòðèÿ.
2G. Grisetti, C. Stachniss, W. Burgard. �Improving grid-based SLAM withRao-Blackwellized particle �lters by adaptive proposals and selective resampling�, 2005.
11 / 20
GMapping: äåòàëè
I êàæäàÿ ÷àñòèöà õðàíèò îòäåëüíóþ êîïèþ ñîñòîÿíèÿ ìèðà (êàðòà è
ïîëîæåíèå ðîáîòà), ÷àñòèöà ñ íàèáîëüøèì âåñîì ñ÷èòàåòñÿ
àêòóàëüíûì ñîñòîÿíèåì ìèðà;
I ñêàí-ìàò÷åð ÿâëÿåòñÿ âàðèàöèåé ãðàäèåíòíîãî ñïóñêà: íà êàæäîé
èòåðàöèè òåñòèðóþòñÿ íåñêîëüêî äèñêðåòíûõ íàïðàâëåíèé
ñìåùåíèÿ ðîáîòà, ïîñëå ÷åãî íàïðàâëåíèå ñ íàèëó÷øèì
çíà÷åíèåì ôóíêöèè êîððåëÿöèè (matching score) ñòàíîâèòñÿ
èñõîäíîé ïîçèöèåé äëÿ ñëåäóþùåé èòåðàöèè;
I matching score âñåãî ñêàíà âû÷èñëÿåòñÿ êàê ñóììà î÷êîâ êàæäîé
òî÷êè ñêàíà: ÷åì áëèæå òî÷êà ñêàíà â ïðåäïîëàãàåìîé ïîçèöèè
ðîáîòà ê ïðåïÿòñâèþ íà êàðòå, òåì áîëüøå î÷êîâ îíà ïîëó÷àåò;
I ïîñëå îáðàáîòêè ñêàíà âåñ êàæäîé ÷àñòèöû ïåðåñ÷èòûâàåòñÿ
ïðîïîðöèîíàëüíî ìàêñèìàëüíîìó çíà÷åíèþ matching score,
ïîëó÷åííîìó â ýòîé ÷àñòèöå.
12 / 20
Ðàçðàáîòêà íîâûõ SLAM-ìåòîäîâ
Ïðîáëåìà
I Îáû÷íî íîâûé àëãîðèòì âíîñèò èçìåíåíèÿ òîëüêî â îäèí èëè
íåñêîëüêî ìîäóëåé ñóùåñòâóþùåãî SLAM-ìåòîäà.
I Ðåàëèçàöèÿ íîâîãî SLAM-ìåòîäà �ñ íóëÿ� óñëîæíÿåò åãî
äàëüíåéøóþ ìîäèôèêàöèþ.
Ðåøåíèå
I Áèáëèîòåêà, ïðåäîñòàâëÿþùàÿ ìîäóëè äëÿ áûñòðîãî
ïðîòîòèïèðîâàíèÿ àëãîðèòìà SLAM è íàáîð ÷àñòî èñïîëüçóåìûõ
ìåòîäîâ, îáúåêòîâ è êëàññîâ.
13 / 20
Ïðåèìóùåñòâà ôðåéìâîðêà
I óñêîðÿåò ðàçðàáîòêó íîâûõ àëãîðèòìîâ çà ñ÷åò èñïîëüçîâàíèÿ
îáùèõ êîìïîíåíòîâ;
I ïðåäîñòàâëÿåò âîçìîæíîñòü îáúåäèíÿòü ÷àñòè ñóùåñòâóþùèõ
SLAM-ìåòîäîâ äëÿ ñîçäàíèÿ íîâîãî áîëåå ýôôåêòèâíîãîàëãîðèòìà;
I èíòåãðàöèÿ ñ ROS ïîçâîëÿåò òåñòèðîâàòü àëãîèòì íà
ïîïóëÿðíûõ äàòàñåòàõ è ðàçëè÷íûõ ïëàòôîðìàõ;
I ïîçâîëÿåò âûÿâèòü äåòàëè è ðàçëè÷íûå ad-hoc îñîáåííîñòè, íå
óïîìÿíóòûå â ñòàòüå, îïèñûâàþùåé êîíêðåòíûé àëãîðèòì.
14 / 20
Ñóùåñòâóþùèå ðåøåíèÿ
 íàñòîÿùåå âðåìÿ ñóùåñòâóåò íåñêîëüêî íàáîðîâ áèáëèîòåê è
èíñòðóìåíòîâ, òàê èëè èíà÷å ñâÿçàííûõ ñ ðàçðàáîòêîé
SLAM-àëãîðèòìîâ:
I Manifold Toolkit (MTK) è áèáëèîòåêà g2o óïðîùàþò
ìèíèìèçàöèþ íåëèíåéíûõ ôóíêöèé îøèáêè, ÷òî ÿâëÿåòñÿ âàæíîé
÷àñòüþ ãðàôîâûõ SLAM-ìåòîäîâ.
I Mobile Robot Programming Toolkit (MRPT) ïðåäîñòàâëÿåòíàáîð áèáëèîòåê, ïîëåçíûõ äëÿ ðåøåíèÿ ïðàêòè÷åñêèõ çàäà÷ â
ðàçëè÷íûõ îáëàñòÿõ ðîáîòîòåõíèêè. Îí ñîäåðæèò íåñêîëüêî
SLAM-ìåòîäîâ â âèäå íàñòðàèâàåìûõ, íî ìîíîëèòíûõ êëàññîâ.
15 / 20
Àðõèòåêòóðà ôðåéìâîðêà
Input / Output
ROS Adapter
Visual Debugger
Physical Sensors
Core
Filtering
Particle Filter
Scan Matchers
Olson (BF)
Hough
Monte-Carlo
Mapping
Grid Map
SLAM Algorithms
GMapping
ORB-SLAM
LSD-SLAM
tinySLAM
FastSLAM
Motion Model
Interoception-based
Visual Odometry
ORB Features
Kalman
Filters
Gradient
DescentGraph
Map
Graph
Optimization
Loop
Closure
Cell
Models
GraphSLAM
(scan based)
Correlation
Functions
Semi-dense Depth
Map
16 / 20
Ïðèìåð èñïîëüçîâàíèÿ: ðåàëèçàöèÿ GMapping
RvizGridViewer
+ void on_pose_update(…)
+ void on_map_update(…)
TopicWithTransform<Msg>
+ void subscribe(…)
- void transformed_msg_cb(…)
TopicObserver<Msg>
+ void handle_transformed_msg(…)
LaserScanObserver
+ void handle_transformed_msg(…)
ROS
WorldObserver<Map>
+ void on_pose_update(…)
+ void on_map_update(…)
World<Observation, Map>
+ void handle_observation(…)
+ void update_robot_pose(…)
+ World<Observation, Map> &world()
GridMap
+ void update_cell(…)
+ Point2D world_to_cell(double, double)
+ GridCell* cell()
+ Rectangle world_cell_bounds(Point2D)
LaserScanGridWorld
+ void handle_observation(…)
GridCell
+ double value()
+void set_value(Occupancy, double)
RobotPose
+ x
+ theta
+ y
SlamFascade<Input>
+ void handle_sensor_data(Input)
GmappingFascade
+ void handle_sensor_data(...)
GmappingWorld
+ void handle_observation(…)
+ void sample(…)
Particle
+void sample(…)
Core
ParticleFilter
+ bool try_resample()
+ vector<Particle*> particles()
+ void normalize_weights()
GmappingParticleFilter
+ void handle_observation(…)
+ void update_robot_pose(…)
+ World<T> &world()
Gmapping
GmappingBaseCell
+ double value()
+ void set_value(Occupancy, double)
GridScanMatcher
+ double process_scan(…)
+ void subscribe(…)
+ void reset_state()
+ void unsubscribe(…)
DescentScanMatcher
+ double process_scan(…)
+ void subscribe(…)
+ void reset_state()
+ void unsubscribe(…)
17 / 20
 ðàçðàáîòêå
I äîáàâèòü ïîääåðæêó ãðàôîâûõ SLAM-ìåòîäîâ: áàçîâûå êëàññû è
ìåòîäû îïòèìèçàöèè (ad-hoc èëè g2o);
I ðåàëèçîâàòü äîïîëíèòåëüíûå ìåòîäû ñîïîñòàâëåíèÿ ëàçåðíûõ
ñêàíîâ;
I äîáàâèòü ïîääåðæêó äðóãèõ òèïîâ ñåíñîðîâ (ìîíîêóëÿðíûå èëè
ñòåðåî-êàìåðû);
I ðåàëèçîâàòü äîïîëíèòåëüíûå êîìïîíåíòû è àëãîðèòìû, ÷àñòî
èñïîëüçóåìûå â SLAM (ôèëüòð Êàëìàíà è ò.ï.);
I äîáàâèòü ðåàëèçàöèè SLAM-ìåòîäîâ, îñíîâàííûõ íà îñîáûõ
òî÷êàõ (feature-based) (íàïðèìåð, FastSLAM).
18 / 20
Çàêëþ÷åíèå
I Áèáëèîòåêà ïðåäîñòàâëÿåò áàçîâûå êîìïîíåíòû, íåîáõîäèìûå äëÿ
ðåàëèçàöèè SLAM-ìåòîäîâ, îñíîâàííûõ íà äâóìåðíûõ ëàçåðíûõ
ñêàíàõ.
I Òåêóùàÿ ðåàëèçàöèÿ âûïîëíåíà íà ÿçûêå C++ è ñîâìåñòèìà ñ
ROS.
I Òåêóùàÿ âåðñèÿ âêëþ÷àåò â ñåáÿ ðåàëèçàöèè òàêèõ àëãîðèìîâ,
êàê tinySLAM è GMapping.
I Ðàçðàáîòêà áèáëèîòåêè íàõîäèòñÿ íà íà÷àëüíîì ýòàïå è
ìíîæåñòâî ïëàíèðóåìûõ êîìïîíåíòîâ íà äàííûé ìîìåíò â íåé
îòñóòñòâóåò.
I Òåêóùàÿ âåðñèÿ ôðåéìâîðêà äîñòóïíà ïî ññûëêå:
https://github.com/OSLL/slam-constructor
19 / 20
