Embed Size (px)
Citation preview
1
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
Visual Motion Observer with
Target Object Motion Models
Takeshi Hatanaka
FL seminar
April 28, 2011
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
Experimental Result: Visual Motion Observer
x
yz
2
or
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
・ Basis on Rigid Body Motion on SE(3)
・ Passivity-based Visual Motion Observer (VMO) [1]
Outline
[1] M. Fujita, H. Kawai and M. W. Spong, “Passivity-based Dynamic Visual Feedback Control for
Three Dimensional Target Tracking: Stability and L2-gain Performance Analysis,” IEEE TCST, Vol.
15, No. 1, pp. 40-52, 2007. 3
・VMO with Target Object Motion Model
・ Concluding Remarks
・ Convergence of Orientation Estimates
Point 1: Velocity Feedback as Passivation
・ Convergence of Position Estimates
Point 2: Two-stage-proof and Perturbation Theory
・ Class of Velocity Models
Point 3: Developments of Software
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
Body Velocity of Vision Camera
（41） （42）
Body Velocity of Object
（43） （44）
Body Velocity of Object relative to
（45） （46）
Vision Camera
4
World FrameObject Frame
Vision Camera Frame
Pose of Object relative to
（39）Vision Camera
: Position
: Orientation
（40）
Relative Pose and Body Velocity
Fig. 14: Relative Pose and Body Velocity of
Object relative to Vision Camera
in SE(3)
Pose of Vision Camera
Pose of Object
（37）
（38）
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
5
Relative Rigid Body Motion
Object Frame World Frame
Vision Camera Frame
Relative Rigid Body Motion
Body Velocity of
Vision Camera
Body Velocity
of Object
Relative Rigid Body Motion
Relative RigidBody Motion
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
6
Feature Points
Fig. 19：Pinhole Camera Model
: not measurable
World FrameObject Frame World Frame
Unknown
Vision CameraFrame
not measurable
Unknown
Relative RigidBody Motion
Fig. 18: Block Diagram of Relative Rigid Body Motion with Vision Camera
Object’s i-th Feature Point
PointFeature
（63）
i-th Feature Point
Points
Feature
（62）
2
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
（64）
Image Information (m Points)
（65）
7
Perspective Projection
Perspective Projection (Pinhole Camera)
Fig. 19：Pinhole Camera ModelUnknown
not measurable
Vision Camera
measurable
Points
Feature
Projection
Perspective
Unknown
Relative RigidBody Motion
Fig. 18: Block Diagram of Relative Rigid Body Motion with Vision Camera
World FrameObject Frame World Frame
Vision CameraImage Plane
: Focal Length
Frame
PointFeature
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
・ Basis on Rigid Body Motion on SE(3)
・ Passivity-based Visual Motion Observer (VMO) [1]
Outline
[1] M. Fujita, H. Kawai and M. W. Spong, “Passivity-based Dynamic Visual Feedback Control for
Three Dimensional Target Tracking: Stability and L2-gain Performance Analysis,” IEEE TCST, Vol.
15, No. 1, pp. 40-52, 2007. 8
・VMO with Target Object Motion Model
・ Concluding Remarks
・ Convergence of Orientation Estimates
Point 1: Velocity Feedback as Passivation
・ Convergence of Position Estimates
Point 2: Two-stage-proof and Perturbation Theory
・ Class of Velocity Models
Point 3: Developments of Software
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
Relative Rigid Body Motion (RRBM)
Actual Posenot measurable
measurable
Image Information
Luenberger Observer
（64） （65）
RRBMCamera
not measurable measurable
Vision
Unknown
Fig. 20: Block Diagram of Estimation Error Vector 9
（66）：Input for Estimation Error
Relative Rigid Body Motion (RRBM) Model
Estimated Image Information
Estimated Pose
（67） （68）
to ControlInput
estimated
ModelCamera
estimated
RRBMVision
Model
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
Estimation Error
Estimation Error Vector
（69）
(Error between Estimated State
and Actual One)
Estimation Error
(Position)
(Orientation)
（70）
Image Information Error
（71）
(Actual) Image
Information
Estimated Image
Information
Fig. 20: Block Diagram of Estimation Error Vector
estimated
ModelCamera
RRBMCamera
not measurable measurable
estimated
RRBM
Vision
Vision
Model
Unknown
: i-th Image Jacobian（72）
Relation between Image Information Error and Estimation Error Vector
10
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
Estimation Error System
Fig. 20: Block Diagram of Estimation Error Vector
estimated
ModelCamera
RRBMCamera
not measurable measurable
estimated
RRBM
Vision
Vision
Model
Unknown
Estimation Error
（69）
Estimation Error Vector
（70）Image
Jacobian
（73）Image
Information
Estimation error can be calculatedusing image information !!
Pose
Information
System
estimated
ModelCamera
RRBMCamera
not measurable measurable
estimated
RRBM
ErrorEstimation
Vision
Vision
Model
11Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
Passivity of Estimation Error System
Passivity Lemma 1
（76）
where and is a positive scalar.
If the object is static , then
the estimation error system satisfiesPassive
Fig. 21: Block Diagram of Passivity of Estimation Error System
System
estimated
ModelCamera
RRBMCamera
not measurable measurable
estimated
RRBM
ErrorEstimation
Vision
Vision
Model
Passive
Skew-symmetric Matrix
(Sketch of Proof)
（78）
Storage Function
（77）
13
3
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
Fig. 22: Block Diagram of Visual Motion Observer
Control Law for Visual Motion Observer : Passivity Approach
Visual MotionControl Law for
Observer
System
estimated
ModelCamera
RRBMCamera
not measurable measurable
estimated
RRBM
ErrorEstimation
Vision
Vision
Model
Visual Motion Observer
（79）
Theorem 1
If , then the equilibrium point for the
closed-loop system (75) and (79) is asymptotic stable.
Lyapunov Function Candidate
（77）
（80）
System
estimated
ModelCamera
RRBMCamera
not measurable measurable
estimated
RRBM
ErrorEstimation
Vision
Vision
Model
Fig. 23: Block Diagram of Visual Motion Observer
MotionVisual
Observer
Visual Motion Observer
14Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
・ Basis on Rigid Body Motion on SE(3)
・ Passivity-based Visual Motion Observer (VMO) [1]
Outline
[1] M. Fujita, H. Kawai and M. W. Spong, “Passivity-based Dynamic Visual Feedback Control for
Three Dimensional Target Tracking: Stability and L2-gain Performance Analysis,” IEEE TCST, Vol.
15, No. 1, pp. 40-52, 2007. 15
・VMO with Target Object Motion Model
・ Concluding Remarks
・ Convergence of Orientation Estimates
Point 1: Velocity Feedback as Passivation
・ Convergence of Position Estimates
Point 2: Two-stage-proof and Perturbation Theory
・ Class of Velocity Models
Point 3: Developments of Software
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
16
In this talk, we assume that target object has a constant velocity
Relative Rigid Body Motion
Then, the relative rigid body motion is reformulated by
Target Object Motion
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
17
Relative Rigid Body Motion
Actual Estimated
Relative Rigid Body Motion Model
Target Object Motion Model
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
18
Relative Rigid
Body Motion
Relative Rigid
Body Motion
Model
Computation of Estimation Error
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
19
Relative Rigid
Body Motion
Relative Rigid
Body Motion
Model
Controlled Output: Measured Output:
CameraVision
CameraVision
Model
Estimation Error System
Passive with Storage function
Estimation Error System (Augmented)
4
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
20
Relative Rigid
Body Motion
Relative Rigid
Body Motion
Model
Controller Output: Measured Output:
CameraVision
CameraVision
Model
Estimation Error System with Inner Loop
Closing An Inner Loop
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
21
Division into Position and Orientation Parts
Inner Loop
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
22
Orientation evolution is independent of the position evolution
while position evolution depends on orientation evolution
Time Derivative of along with (1):
We first consider the orientation part
Energy Function:
Evolution of Orientation Error
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
23
The system would be passive from
to without the first term
Lemma 1: The total system (1) is passive from to
Estimation Error System with Inner Loop
(1)
Time Derivative of Energy and Lemma 1
Inner Loop
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
24
Relative Rigid
Body Motion
Relative Rigid
Body Motion
Model
CameraVision
CameraVision
Model
Controller Output: Measured Output:
Estimation Error System with Inner Loop
The inner loop is viewed as “Passivation” at least for orientation
Interpretation of Inner Loop (Point 1)
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
25
Lemma 2: The trajectory of along with
with is non-increasing
(Proof)
Relative Rigid
Body Motion
Relative Rigid
Body Motion
Model
CameraVision
CameraVision
Model
Lemma 2
5
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
26
LaSalle Invariance Principle
Suppose that a system has a positively invariant set .
If a function satisfies , then all the state
trajectories converge to the largest invariant set contained in the set
LaSalle Invariance Principle
Relative Rigid
Body Motion
Relative Rigid
Body Motion
Model
CameraVision
CameraVision
Model
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
27
Controller Output: Measured Output:
Estimation Error System with Inner Loop
What is the state equation of the estimation error system?
not a function of
Application of LaSalle Invariance Principle
Relative Rigid
Body Motion
Relative Rigid
Body Motion
Model
CameraVision
CameraVision
Model
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
28
What is the set for the estimation error system?
State:
never gets out of the set
never gets out of the set
Application of LaSalle Invariance Principle
holds for all
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
In the set , we have
29
The velocity estimate is
equal to the actual one
Application of LaSalle Invariance Principle
From LaSalle invariance principle, all the trajectories of
asymptotically converge to the set
Theorem 1: for the constant velocity motion ,
the observer input
leads the estimates to the actual values
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
30
Relative Rigid Body Motion Model
Controller
integrator
“P control” → “PI control”
Internal Model Principle for a class of nonlinear systems
Internal Model Principle
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
31
姿勢推定結果
姿勢＋角速度偏差エネルギー
角速度推定結果
6
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
32
Actual Estimated
Velocity Error System Orientation Error System
Assumption 1.1: the velocity error system is passive from
to with a storage function
Assumption 1.2: the state is bounded → positively invariance
General Motion
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
33
Corollary 1: Suppose that the target object motion satisfies
Assumption 1.1 and 1.2, then the observer input
leads the estimates to the actual
Theorem 1
Conjecture : Under only assumption 1.1, at least the orientation
estimate converges to the actual value.
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
34
姿勢推定結果 角速度推定結果
姿勢＋角速度偏差エネルギー
定常偏差
Constant Acceleration Model
Not satisfies Assumption 1.2
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
・ Basis on Rigid Body Motion on SE(3)
・ Passivity-based Visual Motion Observer (VMO) [1]
Outline
[1] M. Fujita, H. Kawai and M. W. Spong, “Passivity-based Dynamic Visual Feedback Control for
Three Dimensional Target Tracking: Stability and L2-gain Performance Analysis,” IEEE TCST, Vol.
15, No. 1, pp. 40-52, 2007. 35
・VMO with Target Object Motion Model
・ Concluding Remarks
・ Convergence of Orientation Estimates
Point 1: Velocity Feedback as Passivation
・ Convergence of Position Estimates
Point 2: Two-stage-proof and Perturbation Theory
・ Class of Velocity Models
Point 3: Developments of Software
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
36
In all of the research works at Fujita Lab. including pose sync. and
flocking, it is proved that both of the position and orientation errors
monotonically decrease simultaneously in order to use a framework
of Lyapunov, BUT… if robots have constant BODY velocity…
targetestimate
In the presence of the orientation
error, the position error can increase
even in the presence of the position
error feedbacks
In case of orientation, the body
angular velocity is independent
of the orientation error. The
orientation evolution is
independent of the position
→ Orientations get equal
Body? World? Spatial?
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
37
After the convergence of orientations …
The velocities are equal from the
inertial frame and hence the situation
becomes equal to the case without
body velocity
Position Feedbacks
The energy of the position error can temporarily increase and hence
we need a framework other than a simple Lyapunov Theorem
As a result… Case of Flocking
never executed without a common
knowledge on the reference frameOLD NEW
Body? World? Spatial?
7
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
38
Evolution of Position Error
(Constant velocity)
depends on orientation
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
39
Evolution of Position Error
nominal (exponential stable)
(perturbation)
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
40
System with exponentially stable equilibrium and perturbation
(Nonlinear Systems, Sec. 9.3)
: Lyanunov Function for Nominal System
Theory of Perturbed System (Point 2)
(Theorem 1)
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
41
Then, both of the position and linear velocity estimates
converge to actual values
Assumption 2: (the velocity increases
slower than the decay of the square root of the energy function),
Theorem 2
Assumption 3: the target model provides a nominal model
with an exponential stable origin such as constant velocity.
Theorem 2: Suppose that the target object motion satisfies
Assumptions 1.1, 1.2, 2 and 3, then the observer input
achieves all the estimates converge to their actual values
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
42
位置推定結果
併進速度推定結果
位置＋併進速度偏差エネルギー
Constant Velocity
Assumption 2 is satisfied
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
43
位置推定結果
併進速度推定結果
位置＋併進速度偏差エネルギー
Constant Acceleration
定常偏差定常偏差
Assumption 2 is not satisfied
8
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
・ Basis on Rigid Body Motion on SE(3)
・ Passivity-based Visual Motion Observer (VMO) [1]
Outline
[1] M. Fujita, H. Kawai and M. W. Spong, “Passivity-based Dynamic Visual Feedback Control for
Three Dimensional Target Tracking: Stability and L2-gain Performance Analysis,” IEEE TCST, Vol.
15, No. 1, pp. 40-52, 2007. 44
・VMO with Target Object Motion Model
・ Concluding Remarks
・ Convergence of Orientation Estimates
Point 1: Velocity Feedback as Passivation
・ Convergence of Position Estimates
Point 2: Two-stage-proof and Perturbation Theory
・ Class of Velocity Models
Point 3: Developments of Software
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
45
Assumption 1.1: the velocity error system is passive from
to with a storage function
Assumption 1.2: the state is bounded → positively invariance
Assumption 3: the target model provides a nominal model with
an exponential stable origin such as constant velocity.
Assumption 2: (the velocity increases
slower than the decay of the square root of the energy function),
On Assumptions
Target Object Motion Model
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
46
A Variety of Target Motion
: bounded (Assumption 1.2)
(Assumption 1.1)
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
47
A Variety of Target Motion
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
48
satisfies Assumptions 1.1 and 1.2
A Variety of Target Motion
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
49
+
(Perhaps) stable
just checked by MATLAB
2(i-1) 2(n-i)
(perhaps) satisfies Assumptions 2 and 3
(Perhaps) stable
A Variety of Target Motion
9
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
50
Simulation Results
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
51
Simulation Results
have to modify
the velocity energy
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
52
MATLAB function: “fft”
for image data
Approximate !The frequency can be flexibly adjusted
according to the change of measurements
This adaptation can be implemented
automatically
Developments of intelligent vision
systems with adaptation and learning
Future Developments (Point 3)
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
53
Conclusion
In this talk, we have presented a visual motion observer with target
object motion models.
Roughly speaking, if the velocity error system is passive and the
velocity is bounded, then we can correctly estimate both of the
target object pose and body velocities
• The velocity loop is closed as a passivation of the pose error loop
• We have presented a two-stage procedure (position convergence
after orientation convergence) in order to allow temporary
position energy increase caused by the orientation error
Fujita LaboratoryTokyo Institute of Technology
Tokyo Institute of Technology
54
