Tải bản đầy đủ (.pdf) (24 trang)

Field and Service Robotics - Corke P. and Sukkarieh S.(Eds) Part 10 ppsx

Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (2.53 MB, 24 trang )

MotionAnalysis of aParallel MobileRobot
ShragaS
ho
val
1
and
Moshe
Sho
ham
2
1
Department of
IndustrialEng
inee
ring&M
anage
ment,
AcademicCollege of Judeaand Samaria, Ariel,Israel

2
Faculty of MechanicalEngineering,
Technion,Haifa, Israel,

Abstract. This paper presents akinematicand forceanalysis of amobile robot built on the
principle of parallel mechanisms.The robot consists of an upper plateconnected to 3 legs,
eachequipped withanasynchronous driving unit.Akinematicmodel for the robot
providesdatafor accurateposition estimate,even in rough and slippery terrains where
conventionalodometryfails.The paper presents ananalysis of the forces actingon the
robot under various surfaceconditions and robot configurations.This analysis provides
usefuldatatodetermine whether a specificmotioncanbecompleted given the limitations
on stability, the geometryand friction of the surface,and the required motion direction.


The paper presents simulation results that are verified byexperiments using our prototype
model.
1Introductio
n
Parallelmechanisms consist of anupperplatform that ismaneuvered by several
(3-6)legs connected toalower stationary platform. The maneuverability, rigidity
and accuracy arefunctions of the number of legs and the type of joints between
the plates and the legs.The basicconceptualmechanics is knownas the Stewart
platfo
rm[Stewart,19
65]even
tho
ugh
earlier
versions
arek
nown,and sin
ce the
n
many
mani
pulators
wered
evel
ope
dbased
on
t
hi
s

me
chani
sm[Hunt19
83,T
sai
and Tahmasebi1983 and others].
Ben Horin and Shoham[1997]have suggested using mobilejoints between
the legs and the stationary platform, turning the mechanismintoa semi-mobile
robot.The mechanismconsists of the followingcomponents: three links of fixed
length,havinga sphericaljoint on one endand a revolutejoint on the other end,
three actuators whichmoveplenary on a stationary platformand anoutput
platformhaving sixdegrees-of-freedom (DOF). Tofurther increasemobility,Ben
Horin and Shoham[1999,2000] suggest amoreflexible design. This mechanism,
showninFig. 1,is based on 3 inflatable legs,an upper platformand 3
asynchronous driving units for the three legs.The upper joint of eachleg is a
revol
ut
ejoi
nt, w
hi
le t
he
lo
wer
jo
in
t, whi
chcon
ne
cts the

leg
t
o t
he
driving
uni
t,is
a sphericaljoint.This configuration offers six DOF for the upper plate where the
control parameters are the positions (X,Y,Z)ofthe three driving units.The
mechanismis designed for applications that requirealightweight and easy
S. Yuta et al. (Eds.): Field and Service Robotics, STAR 24, pp. 323–331, 2006.
© Springer-Verlag Berlin Heidelberg 2006
324S.Shovaland
M.Shoham
depl
oyable
robot.Given
t
he
r
eq
ui
red
tr
aje
ctory
fo
r
the
en

d
ef
fecter
(6
parame
ters)
the inversekinematics model cangenerate the required pathofeachdriving unit,
subject toits non-holonomicconstraints.
Fig.1. The inflatable mobile robot [Ben Horin and Shoham, 2000]
In this paper wepresent akinematicand forceanalysis for the parallel
mo
bile
robot.The
ki
nem
atica
nalys
is, s
howni
n s
ectio
n 2
,en
able
s a
ni
mp
roved
position
estimateba

sed
on
addi
tion
ale
ncode
rs attache
d to t
he
upp
er
pl
atform.
Section 3 details the forceanalysis for the robot,and section4 shows simulation
results for motions in various configurations and con straints.Section5 verifies
the simulationresults withexperiments conducted withour prototype platform
and section 6 providesconcluding remarks.
2KinematicA
nalysis
Figure 2 is a schematicdescriptionof the parallel mobile mechanism. The upper
pl
ateis con
ne
cted
toe
acho
f t
he
three leg
s

with r
evol
ut
ej
oi
nts.The
three le
gs
are
driven by three asynchronous units thatareconnected to the legswith spherical
joints.Controlledmotionof the three driving units determines the pose(position
and orientation)of the upper plate.
Motion Analys
is
of aParallel Mobile
Robot
3
25
Fig.2. Sche maticdescription of the mobile parallel mechanism
Todetermine the accurateconfiguration of the upper plate,both the absolute
position
of
eachd
rivin
g unit (X
i
,Y
i
,Z
i

)and the directkinematics model are
required
.The
position
of t
he
driving
uni
ts
is de
termi
ne
dby
Odo
me
tr
yu
sing
encode
rs attached t
o t
he
drivin
g w
heels.Ta
hmasebiand
Tsai[19
94
] s
how

that
fo
r
the
abovep
arallel
me
chani
smthe
reare1
6
po
ssible
di
rect ki
ne
matics
sol
ut
ion
s,
which requireextensivecomputationaleffort.Furthermore,determining the
absolutepositionofeachdriving unit is subjected toodometricerrors and cannot
provide a reliable position estimate. Toimprove the accuracy and to simplify the
directkinematics model Shovaland Shoham[2001] useadditionalmeasurements
taken from on boardencoders attached to the upper revolutejoints.The additional
encod
ers
me
asure t

he
rotation
ang
le
betwee
n t
he
upp
er
pl
ateand the
legs.Based
on
the
semeasu
rem
en
ts, t
he
position
s
of the
three
driving u
ni
ts arederived
in
the
U-V-Wcoordinate system (attached to the center of the upper plate).
Ba

sed on
the positionof the drive units as determinedin the U-V-W system, the Euclidean
distances between the drive units l
1
, l
2
and l
3
aregiven byEq. 1:
2
11
2
11
2
11
)()()(
+−+−+−
−+−+−=
iiiiiii
WWVVUUl
(1
)
i –incyclicorder
TheseEuclideandistances canalsobederived according to the position of
the driving units as determinedby the Odometricmodel according toEq. 2.
2
11
2
11
2

11
)()()(
+−+−+−
−+−+−=
iiiiiii
ZZYYXXm
(2)
If the odometric system is accurate, the distances derived in the upper plate
coordinate system ( l
i
)areequal t
o t
he
di
st
ances
derived
in
t
he
w
orld
coo
rdi
nate
system
(
m
i
). If,however, thesedistances aredifferent, the odometriccalculation

is faulty.Anodometricerror in one drive unit affects twodistances according to
Eq. 2.Assuming a single odometricerror at eachinterval time, the accurate
Driving Units
+Spherical
Joints
Revolute
Joi
nts
+
Encode
rs
(X
1
,Y
1
,Z
1
)
(X
3
,Y
3
,Z
3
)
(X
2
,Y
2
,Z

2
)
326 S.Shovaland M.Shoham
position of three driving units is continuously updated.In the unlikely event that
two(or three) driving units are subjected toodometricerrors simultaneously, the
proposed procedurecannot beimplemented and additionalmeasures must be
taken(i.e. re-calibrationof the robot’s position).
3 ForceAnalyses
Fig. 3 shows the contact forces appliedon the robot by the ground.Doand Yang
[1988] suggest a solution based on the Newton-Euler me thod, resultingin 36
linear equations.Other researchers use the principle of virtual work to reduce the
complexity of the solution.Ben Horin [1999] uses the Kane method todetermine
the dynamicequations, solvingit withnumericalprocedures.To simplify the
solution weassume that the robot’s mass is concentrated at the upper plate, while
the legs havenegligible mass.Wealsoassume that internalchanges in the robot
internalconfiguration arequasistatic(dynamicforces associated withchanges of
internalconfiguration arenegligible compared withother).
a
F
1
t
F
1
a
F
2
Y
t
F
2

a
F
3
t
F
3
X
Z
Fig.3. Contact forces applied on the robot
The revolutejoints between the legs and the upper plate sustain only axial
force-F
a
actin
galong t
he
leg,and tangentialf
orce–
F
t
actin
gp
arallel t
o t
he
axis
of the revolutejoint.Reducing the problem to 6 unknownforces simplifies the
solution to the following equations:
x
i
ii

t
i
i
ii
a
ix
maFFF =+=
∑∑∑
==
3
1
3
1
coscoscoscos
γδµθ
(3)
y
i
ii
t
i
i
ii
a
iy
maFFF =+=
∑∑∑
==
3
1

3
1
sincossincos
γδµθ
(4)
∑∑∑
==
=+=
3
1
3
1
sinsin
i
zi
t
i
i
i
a
iz
maFFF
δθ
(5)
••
=
=−+−−+=
∑∑
θδθγδµθ
jrrFFrrFFM

y
cb
ii
t
i
a
iz
cb
i
i
ii
t
iii
a
ix
}))(sinsin()()sincossincos{(
3
1
(6)
Motion Analys
is
of aParallel Mobile
Robot
3
27
••
=
=−++−+−=
∑∑
ξδθγδµθ

jrrFFrrFFM
x
cb
ii
t
i
a
iz
cb
i
i
ii
t
iii
a
iy
}))(sinsin()()coscoscoscos{(
3
1
(7)
••
=
=−++−+−=
∑∑
ωγδµθγδµθ
jrrFFrrFFM
y
cb
iii
t

iii
a
ix
cb
i
i
ii
t
iii
a
iz
}))(coscoscoscos()()sincossincos{(
3
1
(8)
where
a
i
F - the axialforceinleg I ,
t
i
F - the tangentialforceinleg i .
θ
i
- the angle between leg i and the X-Yplane (thereforethe anglebetween the
axialforceand the X-Yplane).
µ
i
- the angle between leg i and the positiveXaxes.
δ

i
- the angle between the tangentialforceofleg i and the X-Y.
γ
i
-the angle between the tangentialforces of leg i and the positiveXaxes.
b
i
r - the position vectorof the bottom of leg i .
c
r
- the position vectorof center of the upper plate.
a
x
,a
y
,a
z
–accelerations along the X,Y and Z
θ
,
ω
,
ξ
-Euler
orien
tatio
nang
le
s
J -Mom

en
t
of
in
ert
iaof
t
he
upp
er
pl
ate
m –Mass of the upper plate
Let us assume afriction coefficient µ
between the driving wheels and the surface.
We transformthe axial-
a
i
F
and tangential-
t
i
F
forces (derived by Eq. 3-8) toa
new coordinate set definedby:
d
u
)
-in t
he

di
rectio
no
f t
he
req
uired
mo
tio
n,
l
u
)
-in
the lateraldirection,and
n
u
)
-normal to the surface. The lateralforce F
l
actingon
the driving unit is given by:
)
(
l
t
il
a
il
uFuFF

))
•+•=
(9)
Toprevent lateral slippage, F
l
is bounded by:
)(
n
t
in
a
il
uFuFF
))
•+•<
µ
(10)
The twoexpressions associated with F
l
provide the first conditionfor
preventinglateral slippage:
µ
<
•+•
•+•
)(
)(
l
t
il

a
i
n
t
in
a
i
uFuF
uFuF
))
))
(11)
Given a specificmoment M
d
generated by the drivingmotorand r - the radius of
the driving wheel, the frictiondrivingforce–F
d
generating the motionof the
driving unit in the required directionis limited by:
)(
n
t
in
a
idd
uFuFrMF
))
•+•<=
µ
(12)

Given a specific terrain topography and friction, the actual
driving forces applied
on eac
hleg
ca
nbed
etermi
ne
dbothalon
g t
he
lon
gi
tu
di
naland
laterald
irectio
ns.
Furthermore,based on theseforces the accurate dynamic reaction of the robot
canbecalculated.
328S.Shovaland
M.Shoham
4SimulationResults
Wefirst examine the forces applied on the robot when travelingat aconstant
speedonahorizontal surface, withanidenticalinclination angles ( η )between the
legs and the upper plate. The robot is travelingin the positiveYdirection witha
configuration is showninFig.5a.We start the experiment with η =90
o
(legsare

perpendicular to the surfaceand the upper plate), while gradually and
simultaneously reducing the inclinationangles for all legs.The weight of the
upper plateis 100N.As expected, the axialforces -
a
i
F on all three legs are
identical, starting with33.33Nwhen the legs areperpendicular ( η =90
o
),and
increasing as η decreases(Figure5b). Sinceall driving units arein the travel
direction, the tangentialforces -
t
i
F areclose tozeroinall legs,and thereforeare
notshownin the graphs.
0
100
200
300
400
500
600
020406080100
Inclenatio n Angle-n [deg]
Force [N]
(a)(
b)
Fig.5. Axialforces for constant motionoverahorizontal surface.
Next, we transform the axialand tangentialforces to the corresponding
components in the longitudinal(F

d
),lateral(F
l
),and
normal t
o the surface
di
rection
s
(
F
n
). Due to the symmetry, the normalcomponents in all legs remain
con
stant
(33
.3N).
The
friction
coe
fficien
t is
0.7,ge
ne
ratin
gamaxim
alf
riction
fo
rceof23

.33Nfo
r all
le
gs.Fig
. 6
a s
ho
ws
the
lateraland
lo
ng
itu
di
nalf
orcesfo
r
legs 1and 2.Theseforces areidenticaldue to the symmetry of the twolegs
relative to the drivingdirection. Fig.6b shows the same forces for leg 3.As
shown, the lateralforces on legs1and 2 pass the maximalfrictionforceat η =53
o
.
The longitudinalforceofleg 3passes the frictionforcelimit at η =55
o
.The robot
can therefore travel at aconstant speedonahorizontal surface withafriction
coefficient of 0.7 as long as the inclinationangleof the legs is larger than55
o
.At
that angle,longitudinal slippage occurs at leg 3.Further decreaseof the

inclinationangle to53
o
causes additionallateral slippage in legs 1and 2.
Travel
Direction
3
2
1
X
Y
Motion Analys
is
of aParallel Mobile
Robot
3
29
Le
g 1
an
d
le
g 2
0
5
10
15
20
25
30
020406080100

n [deg]
Force [N ]
Fd
Fl
Fu
Leg 3
-10
0
10
20
30
40
020406080100
n [deg]
Fo
rc
e
[N
]
Fd
Fl
Fu
(a)(b)
Fig.6. Forces on legs 1and 2 (a)and 3 (b)for constant speed.
In the next set of simulations, the robot travels along ahorizontal surfacein
a straight line withconstant accelerationalong the Y+axis (equivalent to
travelingataconstant speedonaninclined surface). Inaddition tofriction
constraints,external stability must alsobeconsidered.Fig. 7 shows the forces on
legs 1and 2 as afunction of the inclination angle η duringa5m/sec
2

(equivalent
to traveling on aninclined surfaceof30
o
). The results indicate that legs1and 2
losecontact at inclinationangles larger than 78
o
(shownas anegativefriction
force). However, the lateralforceis larger than the friction limit for all inclination
angles, resultinginlateral slippage for any internalconfiguration.Inorder to
completea stable motionat a5m/sec
2
the inclinationof the “front”leg (leg3)
must be reduced,as showninFig.7b.This changeadjusts the forcedistribution,
sim
ilar toh
umans
clim
bin
ga s
teep hi
ll.
The ne
w in
ternalcon
fi
guration
en
ab
les
stab

le mo
tio
nas
long
as in
cli
natio
nangl
ef
or
le
g 3
is
in t
he
rang
eo
f68
o
-5
3
o
,and
legs1and 2 arelarger than 78
o
.
Legs 1 and 2
-40
-2
0

0
20
40
60
80
100
020406080100
n [deg]
Force [N]
Fd
Fl
Fu
(a)(b)
Fig.7. Accelerated motion of 5m/sec
2
.
Sim
ilar resu
lts areo
btain
ed
fo
r circularmo
tio
n.
Again
, s
ymmetricin
ternal
con

figuration r
esu
lts eithe
r in tip
over
or
im
me
di
ate s
lip
page
in
one
or
mo
rel
eg
s.
Asymmetricinternalconfiguration enables the robot to safely complete the
required motioneven for sharpcurvatures with relatively high speeds.
Travel
Direction
330 S.Shovaland
M.Shoham
To verify the simulation results wehaveconducted fieldexperiments usingour
inflatable mobile platform showninFig. 1. In theseexperiments wemeasured the
stab
ility li
mits

fo
r
vario
us
internal.
The
r
esu
lts ind
i
cateclo
sem
atchbetween t
he
theoretical simulations and the field experiments.For example, stability limit for
horizontal s
ur
face with symmetricconfiguration
(showni
nFig
s.5
-6)is obtaine
d
for inclinationangle of 60
o
,compared with53
o
determinedin the simulation.
Motion over inclined surfaceof 30
o

is stable wheninclinationangle for leg3 is in
the range of 65
o
-55
o
compared with 68
o
-53
o
determinedin the simulation.
5Field Experiments
6Conclusions
Anew designfor aparallelmobile robot is presented.The robot consists of three
legs,eachdriven by anasynchronous mechanismconnected to the legs witha
sphericaljoint.Eachleg is connected toanupper platform withrevolutejoint and
additionalencoders,measuring the revoluteangle of the upper joints.These
encoders providedata used bythe kinematicmodel forearly detectionand
correctionofpositioning errors generated by odometry.Early detectionand
correction of odometricerrors in eachleg prevent accumulation of significant
errors of the upper plate,and canidentify irregularities on the surface.
A simplified dynamicmodelprovides a solutionfor the forces applied on the
robot.The
mo
del
determi
ne
s whether
a s
pe
cif

ic t
askcanbe r
el
iab
ly
pe
rfo
rme
d,
given a specific surface topography and friction. The model canalsodetect
instabilities either by loosing contact with the ground (tipover),or by slippage
(longitudinalor lateral).Anunstable configuration canbeavoidedbychanging
the inclination angles between the legs and the upper plate. This featureallows
the robot tocompletemotions in complex terrains whereconventional robots
cannot maintain stability due toinertialforces, surface topography,or friction
constraints.
References
1. Ben Horin (Dombiak) P.,1999,Analysis and Synthesis of anInflatable Parallel
Robot”, M.Sc.Thesis,Technion, Haifa.
2.Ben Horin (Dombiak) P.,Shoham,M.,and Grossman,G.,“AParallel Six Degrees of-
FreedomInflatable Robot,” ASME 2000 Mechanismand Robotics Conference,
Washington, 2000.
3.Ben Horin R.,“Criteriafor Analysis of Parallel Robots”, D.Sc.Thesis,Technion, Haifa,
1997.
4. DoW.Q.D.,Yang D.C.H.,“InverseDynamics Analysisand simulation of a
PlatformType of Robot”, JournalofRoboticSystems, Vol. 5,No. 3,pp. 209-227,1998.
5. Hunt K.H.,1983,“StructuralKinematics of In-Parallel-Actuated Robot Arm”, ASME
JournalofMechanisms Transmissionsand automation in Design, Vol. 105,pp. 705-
712.
Motion Analys

is
of aParallel Mobile
Robot
3
31
6.ShovalS.and ShohamM.“ARedundant Parallel Mobile Mechanism”, Proceedings of
IEEEInternationalConferencefor Robotics and Automation, May 2001,Seoul,Korea,
pp. 2273-2279.
7.Stewart D.,1965,“Aplatform with six Degrees of Freedom”, Proceedings of Instittute
of Mechanica
lEngineering
,L
on
don England
,
Vol
.180,pp
. 371-
386.
8. TahmsebiF.,TsaiL.W.,1994,“Closed-FormDirect Kinematics Solution of aNew
Parallel Minimanipulator”, Transactionsof the ASME, Vol. 116,pp. 1141-1147.
9. TsaiL.W.,TahmasebiF.,1983,“Synthesis and Analysis of aNew Class of Six
Degree-of-Freedom ParallelMinimanipulators”, JournalofRoboticResearch, Vol. 10,
pp. 561-580.







1

2

2
1



2



 

             




 




   



  










       
 
 





             
 








 









D
l
˙
x
l
+ K
l
x
l
=
l
F
lx

D
1
˙x
1
+ K
1
x
1
=
1

F
1 x

 D       K   
 
l
x
l

1
x
1
        
        
l
F
lx

1
F
1 x

            
     
      
      

      
    
    C

of f

      
    C
of f
   
   
            
              
            
            
   
      C
off
        
               
                
             
          
           
           
             
            
               
C
off
( > 0)            
  − C
off
       


 
             
           
          
         
              
                
             
          
 

V h 
avd i
      
i 










  


       







   







          



      
      
       
       
         
        
  
   








 
 


          


 
 | PQ| 


V
avd

 
     
       
  
            
h
i
         P     
             
               
                
              
         

          
             
             ( x
m
,y
m
) 
              
( x
m
,y
m
)      ( x
c 1
,y
c 1
)        
               
       ( x
c 1
,y
c 1
)       
 ( x
c 2
,y
c 2
)           
   | PQ|    ( x
c 2

,y
c 2
) 
        V
avd
  

V
avd
=
n

i = 1
v ( r
i
) ·
L
l
i

v ( r
i
) =
k
| r
i
|
2
r
i


 l
i
             p
i
 k  
            
   V
avd
       
          
            
             
      
    
         
     V
f
 
   V
avd
  
       
  
   V
avd
 P  Q        
            V
l
   

     P  V
f
         Q 
θ
Vf
       P  Q  V
f
    
        V
f
cos θ
Vf
   
      
θ
Vf− avd
=tan
− 1
V
avd
| V
f
· cos θ
Vf
|

           
     
˜
f

1 x
    θ
Vf− avd
  
˜
f
1 y
     θ
Vf− avd
     
    f         
˜
f
1 x
= f cos(θ
Vf − avd
) 
˜
f
1 y
= f sin(θ
Vf − avd
) 
  
˜
f
1 x

˜
f

1 y
     
D
cast
˙
θ
1
= C
˜
f
1 y

C =

C
off
, 
˜
f
1 x
≥ 0
− C
off
, 
˜
f
1 x
< 0

 D

cast
        C     
              
           
             
          
 
 
     

   


            
         
              
             
             
            
             
            
    
               
           
            
           
             
          
             
             

     
      
    
       
      
   

   
      
        




  








 
Proc. of 1993 IEEE Int. Conf. Robotics and Automation

 
Proc.of 2001 IEEE Int. Conf. Robotics and Automation

 

Journal of the Robots Society of Japan 

  
Proc.of 1997 IEEE/RSJ Int. Conf. Intelligent Robots and Systems

 
Proc. of 1997 IEEE Int. Conf. Intelligent Robots
and Systems
        
        
Proc.of the 1999 IEEE Int.Conf.on Robotics and Automation

  IEEE Transactions
on Robotics and Automation
            
Proc. of 1998 IEEE Int. Conf.
Robotics and Automation
          

Proc. of Distributed Autonomous Robotic Systems



Development of aTerrain Adaptive Stability
Prediction System forMass Articulating Mobile
Robots
 
1
  
2

1
  
   

2
  







 
Abstract.            
               
       
         
            
             
         
           
   
1Introduction
           















            
               























               
            
           
        
1.1 Related Work
       
      
         
          
            






































       
        
         
           
            
        
            
         
        
         

        
    
1.2 Approach
            
            
          
          












































           



















            
           






















             
  tipover stability regime         
















          
         
          
        
    
             
       
    
        
























 
         
           
 


2 Optimal Estimation Framework
    

        

2.1 System Dynamics


x =

v a ω
z
α
z
φ θ

T

v a ω
z
α
z

φ θ 

f ( x ) =

a 0
3 × 1
α
z
0
sin(θ ) cos(φ ) ω

z
cos(θ )
− sin
(
φ ) ω
z

T

         x = x
k

 t
k
              
    ˙x = f ( x )  t
k
 t
k +1
= t
k
+ dt
x
k +1
= x
k
+ f ( x
k
) dt 
2.2 Measurements

             
         
        
         
        
Tr
ansf
ormat
ion
of
inertial
sensor
measur
ement.








 {  } 












{  } 


    {  }     
{  }               






ω
c
a
=

ω
e
a
=

ω
e
c
=0


v
e
a
=0

v
e
c
=0

v
a
c
=0







Fig.1.        
         

v
c
   
     

v

c
=

d

r
c
dt

i
=

v
e
+

v
e
c
+

ω
e
×

r
e
c

   


r
n
c


v
n
c










   nth 
  

t
c
    

t
c
=


t
a
+ ∆

t 
         ∆

t    
             
     


t =

a
a
c
+2

ω
a
×

v
a
c
+

α
a

×

r
a
c
+

ω
a
×

ω
a
×

r
a
c





































  
    {  }

a
a
c

 

α
a
×

r
a
c
  2

ω
a
×

v
a
c
 

ω
a
×

ω
a
×

r
a

c


ω
a


α
a
        
   

Kinematics of the CG. 




B
r
c
=
B
0
T


0
r
c
0

m
0
+
n

i =1


i

j =1
j − 1
j
T ( Θ
j
)


i
r
c
i
m
i


1
m

        

B
r
c

 Θ
j

Measurement model.    z     ω  
  δ    v      t     Φ    Θ 

z =

z
ω
z
δ
z
v
x
z
v
z
z
t
a
z
Φ
z
Θ


T

        x    z) 
       H =
∂ h
∂ x
  
i
B
R = R
z
(0
)
R
y
( θ ) R
x
( φ ) 




















B
i
R =
i
B
R
− 1

h ( x )=

















ω
z
arctan

v
y
− v
δ
c
y
− ω
z
r
δ
c
x
v
x
− v
δ
c
x
+ ω
z
r
δ
c
y


v
x
− v
e
c
x
+ ω
z
r
e
c
x
v
z
− v
e
c
z
a +
B
i
R
i
g − ∆ t
a
arctan


(

a +
B
i
R
i
g − ∆ t
in
c
)
y
(
a +
B
i
R
i
g − ∆ t
inc
)
z

arctan

(
a +
B
i
R
i
g − ∆ t

inc
)
x
(
a +
B
i
R
i
g − ∆ t
inc
)
z



















3A
ssessment
of
Stability
Regime
        
              
            
           
       


×