未知環境中機器人巡航問題之研究
王銀添 副教授
機器人實驗室
淡江大學機械與機電工程學系
Applied Electronics Technology, NTNU
1/48
目錄
•
•
•
•
•
機器人巡航
感測器輔助機器人執行巡航任務
可能遭遇的問題與解決方案
不確定性(uncertainty)現象
機率式狀態估測方法
– 貝氏規則、Kalman Filter、Particle filter
– 同時定位、建圖、物件追蹤之高維度非線性系統
• 同時定位、建圖、物件追蹤實測範例
• 結論與未來之研究議題
Applied Electronics Technology, NTNU
2/48
機器人巡航
• 在未知的環境中巡航時，機器人想知道
– 自己在哪裡?
– 環境是什樣的長相?
– 是否有移動的障礙物?
Example: Dead reckoning (deduced reckoning)
 x
p   y 
 
Applied Electronics Technology, NTNU
vx 
p  p  v y  t
  
3/48
Calibration of Errors for Robot with Odometry
(Borenstein [1992])
• The unidirectional square path experiment
Applied Electronics Technology, NTNU
4/48
Calibration of Errors for Robot with Odometry
(Borenstein [1992])
• The bi-directional square path experiment
Applied Electronics Technology, NTNU
5/48
感測器輔助巡航
• 在未知的環境中巡航時，機器人必須依賴自身的
移動與(外部、多個)感測器對環境特徵的感測，
執行以下任務：
– 自我定位(self-localization)任務
– 環境地圖建構(mapping)任務
– 移動物體偵測與追蹤(detection and tracking of moving
objects)任務
Applied Electronics Technology, NTNU
6/48
機器人感測(Robot Perception)系統
Sensor Classification
• Proprioceptive sensors
– measure values internally to the system (robot)
– e.g. motor speed, wheel load, heading of the robot, battery status
• Exteroceptive sensors
– information from the robots environment
– distances to objects, intensity of the ambient light, unique features
• Passive sensors
– energy coming for the environment
– e.g. temperature probe, microphones, and CCD or CMOS camera.
• Active sensors
– emit their proper energy and measure the environmental reaction
– better performance, but some influence on environment
– e.g. wheel quadrature encoders, ultrasonic sensors, and laser
rangefinders.
Applied Electronics Technology, NTNU
7/48
[Siegwart and Nourbakhsh 2004]
Sensor Classification (1)
Applied Electronics Technology, NTNU
8/48
Sensor Classification (2)
Applied Electronics Technology, NTNU
9/48
Laser Range Finder (LRF)
 (  x )2  (  x )2  v 
x
x
y
y
r
r  
g (x)     

 y  xy
  
tan1(
)  v



x
x
x


Applied Electronics Technology, NTNU
10/48
Mapping Using LRF
Applied Electronics Technology, NTNU
11/48
Inertial Measurement Unit (IMU)
• A unit has 3-axis gyroscope
(pitch, roll, yaw ) and 3-axis
accelerometer.
• Localization using IMU
• A unit with a tri-axis
accelerometer, tri-axis
magnetometer and a
tri-axis gyro
Applied Electronics Technology, NTNU
12/48
Vision Sensors
image plane
Iy
camera center
Ix
(u0 ,v0)
hC(Ix ,Iy )
fC
zC
(0,0,0)
C
hC(hxC, hC
y , hz )
P
xC
yC
{c}
zc
xc
yc
hic
Image projection model (3D to 2D)

hxC 
u 0  f u C 
I x  
hz 
 

hC
 I y  
y 
v  fv
 0
hzC 

rw
Yi w
z
{w}
beacon
y
x
Applied Electronics Technology, NTNU
13/48
Web Camera Based 3D Scanner
Applied Electronics Technology, NTNU
14/48
Light Detection and Ranging (LiDAR)
Applied Electronics Technology, NTNU
15/48
Airborne LiDAR
Applied Electronics Technology, NTNU
16/48
Microsoft Kinect
– 3D depth image and RGB color image
in 30fps.
– Low-cost. (NT$4,550 tax. included)
– Software development kit provided by
Microsoft.
Applied Electronics Technology, NTNU
17/48
Skanect – Real-time Kinect-based 3D Scanner
manctl.com
Applied Electronics Technology, NTNU
18/48
Mapping Using Kinect
Applied Electronics Technology, NTNU
19/48
可能遭遇的問題與解決方案
• 有幾個問題會造成巡航的任務相當棘手，包括
– 感測器的侷限性
– 移動與量測都具有不確定性質(uncertainty)
– 定位、建圖、追蹤物體 系統變成高維度與非線性
• 本研究針對以上問題進行探討，考慮的議題包括
– 感測器的選用、移動偵測
– 不確定性現象的描述
– 同時求解定位、建圖、追蹤物體等問題
• 並且以機率理論解決機器人在未知環境中巡航問
題。
Applied Electronics Technology, NTNU
20/48
不確定性(Uncertainty)現象
• The structured errors
– The locations of the CW
and CCW clusters.
• The random errors
– The random distribution of
errors in the cluster.
– Uncertainty in motion.
Borenstein [1992]
Applied Electronics Technology, NTNU
21/48
Uncertainty in Robot Motion
Applied Electronics Technology, NTNU
22/48
Uncertainty in Robotics
• 不確定性現象的描述
– 以參數函數描述不確定性，例如高斯常態分佈
N(x;m,s2)
m : mean value
s : deviation
f(x) is the probability
density function (pdf)
– 以非參數函數描述不確定性，例如蒙地卡羅模擬
• 以機率理論求解具不確定性的機器人巡航問題
Applied Electronics Technology, NTNU
23/48
系統的狀態與量測
• State sequence
•
xk  f xk 1, uk 1, wk 1 
x is the state of the system;
u is the input;
f is a nonlinear function of the state;
w is the uncertainty of the state.
Measurement sequence
z k  g xk , vk 
z is the measurement of the system;
g is nonlinear measurement function;
v is the uncertainty of the
measurement.
vx 
 wx 
x k  x k 1  v y  t   w y 
  
 w 
Model-based Uncertainty
state transition
Applied Electronics Technology, NTNU

u0 
I x  
zk     
I y  
v 
 0

hxC 
fu

hzC   v x 
C   v 
hy   y 
fv
hzC 
Projection
model
Uncertainty
24/48
Basic Probability Theory
• Joint probability： P(X=x and Y=y) = P(x,y)
• Conditional probability: P(x|y) is the probability of x, given y,
P( x, y )  P( x|y) P( y )
P( x , y )
P( x|y) 
P( y )
P( y, x)  P( y|x) P( x)
• Theorem of total probability: If yi constitute a partition of the
sample space, then for x in the same space
P( x)   P( x, yi )
yi
P( x)   P( x | yi )P( yi )
yi
p ( x)   p ( x | y ) p ( y ) dy
• Bayes rule: if x is a quantity that we would like to infer from y,
P( y | x) P( x) likelihood prior
P( x y ) 

P( y )
evidence
Applied Electronics Technology, NTNU
P ( y | x , z ) P( x | z )
P( y | z )
Conditioning Bayes rule on z.
P( x | y, z ) 
25/48
Probabilistic Generative Laws
• The emergence of state xk might be conditioned on all past
states, measurements, and controls,
p(xk | x0:k 1, z1:k 1, u1:k )
• If the state x is complete then xk-1 is a sufficient statistic of all
previous controls and measurements, u1:k-1 and z1:k-1. Only the
control uk matters if we know the state xk-1,
p(xk | x0:k 1, z1:k 1, u1:k )  p(xk | xk 1, uk )
called state transition probability.
• If xk is complete, the measurement probability is also
generated by
p(zk | x0:k , z1:k 1, u1:k )  p(zk | xk )
The state xk is sufficient to predict the measurement zk.
Applied Electronics Technology, NTNU
26/48
Dynamic Bayes Network (DBN)
• The temporal generative model is known as hidden Markov
model (HMM) or dynamic Bayes network (DBN).
– The state at time k is stochastically dependent on the state at time k-1 and
the control uk.
– The measurement zk depends stochastically on the state at time k.
• The dynamic Bayes network that characterizes the evolution of
controls, states, and measurements.
Applied Electronics Technology, NTNU
27/48
State Estimation Using Bayes’ Rule
• From a Bayesian perspective, the state estimation is to
recursively calculate some degree of belief in the state xk at
time k, given the data z1:k and u1:k,
p(xk | z1:k , u1:k )
• Thus, the probability density function (pdf) of state is
P ( y | x , z ) P( x | z )
P( x | y, z ) 
constructed via Bayes’ rule
P( y | z )
p(zk | x k ) p(xk | z1:k 1, u1:k )
p(xk | z k , z1:k 1, u1:k ) 
p(zk | z1:k 1, u1:k )
• The initial pdf p(x0|z0)=p(x0) of the state vector, which is also known
as the prior, is available.
• z0 is the set of no measurements.
• Then, in principle, the pdf p(xk|z1:k,u1:k) may be obtained,
recursively, in two stages: prediction and update.
Applied Electronics Technology, NTNU
28/48
State Estimation Using Bayes’ Rule
Predict and update the state p(xk|z1:k,u1:k) recursively:
• The prediction stage involves using the system model to obtain
the prior pdf of the state at time k,
p(xk | z1:k 1, u1:k )   p(xk | xk 1, u1:k ) p(xk 1| z1:k 1, u1:k 1)d xk 1
Suppose that the required pdf p(xk-1|z1:k-1,u1:k) at time k-1 is
available.
• At time step k, a measurement zk is used to update the prior
(update stage) via Bayes’ rule
p(zk | xk ) p(xk | z1:k 1, u1:k )
p(xk | z1:k , u1:k ) 
p(zk | z1:k 1, u1:k )
where the normalizing constant
p(zk |z1:k 1, u1:k )   p(zk |xk )p(xk |z1:k 1, u1:k )d xk
Applied Electronics Technology, NTNU
29/48
State Estimation Using Particle Filter
• 引用貝氏規則之遞迴預測與更新系統狀態；
• 每個粒子代表一個解，在取樣空間中隨機規劃數量L個粒子進行求解
。規劃的粒子數量越多越趨近最佳解。第l個粒子的pdf表示為

p( x[kl ]|z1:k , u1:k ) ~ x[kl ] , w[kl ]

每個粒子l遞迴地依據感測訊息更新取樣空間中的狀態 x[kl ] 之權重值 w[lk ]
，用以顯示狀態在該數值區段的機率。
Particle 2
Given : xk -1, landmarkposition
l]
dl  z[kl ]  z[k|k
1
w[kl ] 
1
dl
2]
z[k|k
1
z[k2]
y
d2
landmark
Model-based
z k |k 1
x k |k 1
x
Particle 1
At time k-1
z [k1]
d1
1]
z[k|k
1
At time k
Applied Electronics Technology, NTNU
30/48
Procedure of Particle Filter
遞迴地預測與更新系統狀態
• 所有粒子l所存的機器人狀態 x[kl ] 依據運動模型進行移動，
此為粒子的機器人狀態之預測；
• 擷取新的視覺感測訊息，並且透過感測模型zk重新分配各
粒子之權重值w[lk ]；
• 必要時進行重新取樣(resampling)；
• 正規化(normalizing)權重值，以及更新機器人的狀態x[kl ]。
Applied Electronics Technology, NTNU
31/48
Particle Filter for Robot Localization
Applied Electronics Technology, NTNU
32/48
KF-Based State Estimation
Kalman filter (KF) estimator
• Adapt the concept of recursive
prediction and update estimate process.
• Prediction:
xk |k 1  A xk 1|k 1 B uk 1
z k |k 1  H xk |k 1
(Linear prediction of states and measurements)
• Update:
K k  Pk |k 1H kT ( H k Pk |k 1H kT  Rk )1

Pk|k  I  Kk Hk Pk|k 1
x  [xC m1 m2 mn O1 O2 Ol ]T
• State vector of robot (camera)
 rW   rW  (vW  w

k
k 1
k 1
vk 1 )t
 C  C

C



(


w
)

t

k    k 1
k 1
k 1
xCk   W
v  

vW
 wvk 1
k
k

1

 

C
C
k  

k 1  wk 1
• State vector of static objects
Pk |k 1  Ak Pk 1|k 1 AkT  Qk 1
xk|k  xk|k 1 Kk z k  zk|k 1
Example: KF-based SLAM

mik  [ X ik
Yik
Z ik ]T
• State vector of moving objects
Ojk=[ojk sjk]T
Measurement models

u0 
(Linear update equation for system states)
 I ix  
T
z

Pk  E[ek ek ]
ik    
 I iy  
v0 
Set Pk / K k  0 tofind K k

Applied Electronics Technology, NTNU

C
hix
f C ku

C
hiz 
C
hiy

f C kv
C
hiz

33/48
Visual Sensors for SLAM
• Camera carried by robot
• Free-moving camera
– The camera is presumed to move
at constant velocity (CV);
– The acceleration is caused by an
impulse noise from the external
force.
– Velocity noise:
 wvk   ak t 
 w    t 
 k   k 
Monocular vision
Binocular vision
Applied Electronics Technology, NTNU
34/48
淡江機電系機器人實驗室
• 機器人視覺式同時定位、建圖、與移動物體追蹤(visual
simultaneous localization, mapping, and moving-object tracking)
SLAMMOT
SLAM
Applied Electronics Technology, NTNU
35/48
Monocular SLAM
Applied Electronics Technology, NTNU
36/48
Monocular SLAM
Applied Electronics Technology, NTNU
37/48
Monocular People Detection and Tracking
Applied Electronics Technology, NTNU
38/48
Binocular SLAM
Applied Electronics Technology, NTNU
39/48
Binocular SLAM
Applied Electronics Technology, NTNU
40/48
Binocular SLAM
Applied Electronics Technology, NTNU
41/48
Differential-drive Mobile Robot [2010]
Binocular Vision
PC-based Controller
Laser Range Finder
Sonar
Wheel Encoder
Mobile Robot
Applied Electronics Technology, NTNU
42/48
Visual SLAM of Mobile Robots
Applied Electronics Technology, NTNU
43/48
Visual SLAM of Mobile Robots
Applied Electronics Technology, NTNU
44/48
Visual SLAM of Mobile Robots
Applied Electronics Technology, NTNU
45/48
結論與未來研究議題
• 理論上，同時定位、建圖、與移動物體追蹤的問題已經有
解
• 實現技術方面，仍有挑戰性：
– 辨識移動物體
– 使用移動感測器偵測與追蹤移動物體
• 實際應用時，依需求選擇完整求解或簡化求解
– 解答的一致性(consistency)
– 計算複雜性(computational complexity)
• 與路徑規劃、運動控制器的結合
• 新感測器的發展與應用
• 新的應用領域
Applied Electronics Technology, NTNU
46/48
Visual SLAM of Robot Vacuum
Cleaner (Samsung Hauzen RE70V)
Applied Electronics Technology, NTNU
47/48
Autonomous Quadrotor Mapping, Localization
and Trajectory Following Using LiDAR
University of Pennsylvania
Applied Electronics Technology, NTNU
48/48