未知環境中機器人巡航問題之研究

1/48Applied Electronics Technology, NTNU

未知環境中機器人巡航問題之研究

王銀添副教授機器人實驗室

淡江大學機械與機電工程學系


目錄• 機器人巡航• 感測器輔助機器人執行巡航任務• 可能遭遇的問題與解決方案• 不確定性 (uncertainty) 現象• 機率式狀態估測方法– 貝氏規則、 Kalman Filter 、 Particle filter

– 同時定位、建圖、物件追蹤之高維度非線性系統• 同時定位、建圖、物件追蹤實測範例• 結論與未來之研究議題


機器人巡航• 在未知的環境中巡航時，機器人想知道– 自己在哪裡 ?

– 環境是什樣的長相 ?

– 是否有移動的障礙物 ?

Example: Dead reckoning (deduced reckoning)

tv

v

pp y

x

y

x

p


Calibration of Errors for Robot with Odometry (Borenstein [1992])

• The unidirectional square path experiment


• The bi-directional square path experiment

Calibration of Errors for Robot with Odometry (Borenstein [1992])


感測器輔助巡航

• 在未知的環境中巡航時，機器人必須依賴自身的移動與 ( 外部、多個 ) 感測器對環境特徵的感測，執行以下任務：– 自我定位 (self-localization) 任務– 環境地圖建構 (mapping) 任務– 移動物體偵測與追蹤 (detection and tracking of moving o

bjects) 任務


機器人感測 (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.


Sensor Classification (1)[Siegwart and Nourbakhsh 2004]


Sensor Classification (2)

10/48

Laser Range Finder (LRF)

Applied Electronics Technology, NTNU

vx

x

vxxrg

xx

yy

ryyxx

)(tan

)()((x)

1

22

11/48

Mapping Using LRF


12/48

Inertial Measurement Unit (IMU)


• A unit with a tri-axis accelerometer, tri-axis magnetometer and a tri-axis gyro

• A unit has 3-axis gyroscope (pitch, roll, yaw ) and 3-axis accelerometer.

• Localization using IMU

13/48

Vision Sensors


camera center

image plane

)( yxC I,Ih

),,( Cz

Cy

Cx

C hhhh

Cf

Cy

Cx

Cz

xIyI

P

)( 00 v,u

(0,0,0)

Cz

Cy

v

Cz

Cx

u

y

x

h

hfv

h

hfu

I

I

0

0

Image projection model (3D to 2D)

cz

wiY

wr

cycx

cih

z

y

x

beacon

}{c

}{w

14/48

Web Camera Based 3D Scanner


15/48

Light Detection and Ranging (LiDAR)


16/48

Airborne LiDAR


17/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.


Skanect – Real-time Kinect-based 3D Scanner manctl.com

19/48

Mapping Using Kinect



可能遭遇的問題與解決方案

• 有幾個問題會造成巡航的任務相當棘手，包括– 感測器的侷限性– 移動與量測都具有不確定性質 (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.

不確定性 (Uncertainty) 現象

Borenstein [1992]


Uncertainty in Robot Motion


Uncertainty in Robotics• 不確定性現象的描述– 以參數函數描述不確定性，例如高斯常態分佈

– 以非參數函數描述不確定性，例如蒙地卡羅模擬

• 以機率理論求解具不確定性的機器人巡航問題

N(x;,2): mean value: deviation

f(x) is the probability density function (pdf)


系統的狀態與量測• State sequence

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.

111 , , kkkk wf uxx

w

w

w

tv

v

y

x

y

x

kk 1xx

• Measurement sequence

z is the measurement of the system; g is nonlinear measurement

function; v is the uncertainty of the

measurement.

kkk vg ,xz

y

x

Cz

Cy

v

Cz

Cx

u

y

xk v

v

h

hfv

h

hfu

I

I

0

0

z

Model-basedstate transition

UncertaintyProjection

modelUncertainty

25/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,

• Theorem of total probability: If yi constitute a partition of the sample space, then for x in the same space

• Bayes rule: if x is a quantity that we would like to infer from y,


)(

),((

yP

yxPx|yP )

iy

iyxPxP ),()(

iy

ii yPyxPxP )()|()( dyypyxpxp )()|()(

evidence

prior likelihood

)(

)()|()(

yP

xPxyPyxP

)|(

)|(),|(),|(

zyP

zxPzxyPzyxP

Conditioning Bayes rule on z.

)((),( yPx|yPyxP )

)((),( xPy|xPxyP )

26/48

Probabilistic Generative Laws• The emergence of state xk might be conditioned on all past

states, measurements, and controls,

• 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,

called state transition probability.• If xk is complete, the measurement probability is also generated

by

The state xk is sufficient to predict the measurement zk.Applied Electronics Technology, NTNU

)u,z,x(x ::: kkkk |p 11110

)u,x(x)u,z,x(x ::: kkkkkkk |p|p 111110

)x(z)u,z,x(z ::: kkkkkk |p|p 1110

27/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.



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,

• Thus, the probability density function (pdf) of state is constructed via Bayes’ rule

• 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.

)u,z(z)u,z(x)x(z

)u,z,z(x::

::::

kkk

kkkkkkkkk |p

|p|p|p

111

111111

)|(

)|(),|(),|(

zyP

zxPzxyPzyxP

)u,z(x :: kkk |p 11


State Estimation Using Bayes’ RulePredict 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,

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

where the normalizing constant kkkkkkkkk d|p|p|p x)u,z(x)x(z)u,z(z :::: 111111

11111111111 kkkkkkkkkk d|p|p|p x)u,z(x)u,x(x)u,z(x :::::

)u,z(z)u,z(x)x(z

)u,z(x::

::::

kkk

kkkkkkkk |p

|p|p|p

111

11111

30/48

State Estimation Using Particle Filter


• 引用貝氏規則之遞迴預測與更新系統狀態；• 每個粒子代表一個解，在取樣空間中隨機規劃數量 L 個粒子進行求解

。規劃的粒子數量越多越趨近最佳解。第 l 個粒子的 pdf 表示為

每個粒子 l 遞迴地依據感測訊息更新取樣空間中的狀態之權重值，用以顯示狀態在該數值區段的機率。

][x lk

][lkw

landmarky

x

2d

1dModel-based

Particle 2

Particle 1At time k-1 At time k

1kk |x 1kk |z

positionlandmark , :Given 1-kx

][][::

][ ,x~)u,zx( lk

lkkk

lk w|p 11

l

lk d

w1

][

][][ zz lk|k

lkld 1

][z 11k|k

][z 21k|k

][z 1k

][z 2k

31/48

Procedure of Particle Filter遞迴地預測與更新系統狀態• 所有粒子 l 所存的機器人狀態依據運動模型進行移

動，此為粒子的機器人狀態之預測；• 擷取新的視覺感測訊息，並且透過感測模型 zk重新分配各

粒子之權重值；• 必要時進行重新取樣 (resampling) ；• 正規化 (normalizing) 權重值，以及更新機器人的狀態

。


][x lk

][lkw

][x lk

32/48

Particle Filter for Robot Localization



KF-Based State EstimationKalman filter (KF) estimator• Adapt the concept of recursive

prediction and update estimate process.• Prediction:

(Linear prediction of states and measurements)

• Update:

(Linear update equation for system states)

1111 kkkkk uxx || BA

11 kkkk || xz H

1111 kTkkkkkk QAPAP ||

111 )(

kTkkkk

Tkkkk RHPHHPK ||

11 kkkkkkkk ||| zzxx K

1 kkkkkk || PHKIP

Example: KF-based SLAM

• State vector of robot (camera)

• State vector of static objects

• State vector of moving objects

Measurement models

11

11

111

111

)(

)(

kCk

vkWk

kCk

Ck

vkWk

Wk

Ck

Wk

Ck

Wk

Ck

w

wv

tw

twvr

v

r

x

0

0

Ciz

Ciy

vC

Ciz

Cix

uC

iy

ixik

h

hkfv

h

hkfu

I

Iz

TlnC ] [ 2121 OOOmmmxx

Tikikikik ZYX ][m

Ojk=[ojk sjk]T

kkk KKP find to0Set /

][ Tkkk E eeP


– The camera is presumed to move at constant velocity (CV);

– The acceleration is caused by an impulse noise from the external force.

Visual Sensors for SLAM

t

ta

w

w

k

k

k

vk

Monocular vision Binocular vision

– Velocity noise:

• Camera carried by robot• Free-moving camera


淡江機電系機器人實驗室• 機器人視覺式同時定位、建圖、與移動物體追蹤 (visual

simultaneous localization, mapping, and moving-object tracking)

SLAM

SLAMMOT

36/48

Monocular SLAM


37/48

Monocular SLAM


38/48

Monocular People Detection and Tracking


39/48

Binocular SLAM


40/48

Binocular SLAM


41/48

Binocular SLAM



Differential-drive Mobile Robot [2010]

Binocular Vision

Laser Range Finder

Wheel Encoder

PC-based Controller

Sonar

Mobile Robot

43/48

Visual SLAM of Mobile Robots


44/48



45/48



46/48

結論與未來研究議題• 理論上，同時定位、建圖、與移動物體追蹤的問題已經有

解• 實現技術方面，仍有挑戰性：– 辨識移動物體– 使用移動感測器偵測與追蹤移動物體

• 實際應用時，依需求選擇完整求解或簡化求解– 解答的一致性 (consistency)– 計算複雜性 (computational complexity)

• 與路徑規劃、運動控制器的結合• 新感測器的發展與應用• 新的應用領域


47/48

Visual SLAM of Robot Vacuum Cleaner (Samsung Hauzen RE70V)


48/48

Autonomous Quadrotor Mapping, Localization and Trajectory Following Using LiDAR

University of Pennsylvania


Documents

未知環境中機器人巡航問題之研究