Post on 20-Aug-2020
InitializationofaPedestrianNavigationSystemUsingaVehicleNavigationSystem
ArchitThopayDavidBevlyScottMartinHowardChen
2019StanfordPNTSymposium
AGVLAB
PresentationOutline
• Introduction– Motivation– ProblemDefinition– ProposedSolution
• TheoreticalBasis– RigidityDetection– OffsetCalculation
• PerformanceAnalysis– Subsystems– CompleteSystem
• ConclusionsandFutureWork
2
AGVLAB
FirstVisittoStanfordUniversity
3
HooverInstitution AndersonCenter
AGVLAB
UsingaMap
4
Iamhere
AGVLAB
GNSSPositioning
5
AGVLAB
GNSSWeaknesses
• GoodGNSSsignalsnotalwaysavailable.– Blockage– Attenuation
• Pedestriannavigationtechniquescanfillthegap.– IndependentofGNSS
6
AGVLAB
PedestrianNavigationTechniques
PedestrianDeadReckoning• Attitude
– AngularVelocity– Acceleration– MagneticField
• StepLength– Inertial
InertialNavigation• 6-DOFangularandpositional
displacement– Angularvelocity– Acceleration
7
[1]
AGVLAB
InitializationProblem
PedestrianDeadReckoning
𝑥↓𝑖+1 = 𝑥↓𝑖 +(𝑆𝐿𝑐𝑜𝑠(𝜓))𝑦↓𝑖+1 = 𝑦↓𝑖 +(𝑆𝐿𝑠𝑖𝑛(𝜓))
InertialNavigation
𝐶 ↓𝑏↑𝑛 = 𝐶↓𝑏↑𝑛 [𝜔×]𝑣=∫↑▒𝐶↓𝑏↑𝑛 𝑓 ↓𝑏 𝑑𝑡 + 𝑣↓𝑖 𝑝=∫↑▒𝑣𝑑𝑡 + 𝑝↓𝑖
8
InitialAttitude
InitialPositionandVelocity
AGVLAB
CurrentInitializationTechnique
Position PitchandRoll Heading
Standstilloutdoorsforoneminute
GNSS Accelerometer Magnetometer
[2]
9
AGVLAB10
UsetheVehicle’sSensors
• Firstrespondersrideinvehicles.
– Vehicles→Navigationsensors
• AssumevehiclehasagoodPNTsolution.
• Thepedestriannavigationsystemcanbeinitializedusingthevehicle’snavigationsystemasareference.
– Pedestrian→InsideVehicle
AGVLAB11
ProposedSystem
1) DetectwhenpedestrianIMUisrigidwithvehicleIMUduringthedrive.
2)CalculaterotationandtranslationbetweenvehicleandpedestrianIMUs.
3)Transformvehiclepositionandattitudeintopedestrian’sframe.
[𝑅|𝑡]
AGVLAB
TheoreticalBasis
RigidityDetectionandOffsetCalculation
12
AGVLAB13
RigidityDetectionBackground
• DetectrigidperiodsusingIMUmeasurements– Pedestrianmounted– VehicleMounted
• Similartofoot-mountedstepdetectionapproachesusedforpedestriannavigation.[2]
|𝑎|=√�𝑎↓𝑥↑2 + 𝑎↓𝑦↑2 + 𝑎↓𝑧↑2
[3]
AGVLAB14
RigidityDetectionTheory
• Differenceinangularvelocitymagnitudes.
• Twopointsonrigidbodyexperiencesameangularvelocitymagnitude.
• Ifdifferencefallsbelowthreshold,thenIMUscanbeassumedtorigidlyattached.
𝜔↓𝑣𝑒ℎ𝑖𝑐𝑙𝑒↑𝑛𝑜𝑟𝑚 (𝑘)=√�𝜔↓𝑥 (𝑘)↑2 + 𝜔↓𝑦 (𝑘)↑2 + 𝜔↓𝑧 (𝑘)↑2
Ω↓𝑝𝑒𝑑𝑒𝑠𝑡𝑟𝑖𝑎𝑛↑𝑛𝑜𝑟𝑚 (𝑘)=√�Ω↓𝑥 (𝑘)↑2 +
Ω↓𝑦 (𝑘)↑2 + Ω↓𝑧 (𝑘)↑2
𝑔↓𝑑𝑖𝑓𝑓↑𝑛𝑜𝑟𝑚 (𝑘)= 𝜔↓𝑣𝑒ℎ𝑖𝑐𝑙𝑒↑𝑛𝑜𝑟𝑚 (𝑘)− Ω↓𝑝𝑒𝑑𝑒𝑠𝑡𝑟𝑖𝑎𝑛↑𝑛𝑜𝑟𝑚 (𝑘)
𝑑𝑒𝑡𝑒𝑐𝑡↓𝑟𝑖𝑔𝑖𝑑↓𝑘 ={█𝑓𝑎𝑙𝑠𝑒&𝑖f
𝑔↓𝑑𝑖𝑓𝑓↑𝑛𝑜𝑟𝑚 |↓𝑘−𝑠↑𝑘 >𝑡ℎ↓𝑚𝑎𝑥 @𝑡𝑟𝑢𝑒&otherwise
AGVLAB
OffsetCalculationBackground
15
PedestrianIMUVehicleIMU
AngularVelocity
SpecificForce
Vehicle 𝜔 ↓𝑣 𝑓 ↓𝑣
Pedestrian 𝜔 ↓𝑝 𝑓 ↓𝑝
AGVLAB16
AngularOffsetCalculation
• Needtocalculaterotationmatrix𝑹betweenvehicleandpedestrianIMUs.
• Wahba’sproblem– Spacecraftattitudedetermination
[4]
𝜔 ↓𝑣 = 𝑹𝜔 ↓𝑝 𝐽= 1/2 𝑤↓𝑘 ‖𝜔 ↓𝑣 −𝑹𝜔 ↓𝑝 ‖↑2
ReferenceVectors OffsetVectors
ExistingReferenceStarLocations
orMagneticFieldMeasuredStarLocations
orMagneticField
Proposed AngularVelocityfromVehicleIMU
AngularVelocityfromPedestrianIMU
AGVLAB17
Wahba’sProblem
• AttitudeProfileMatrix[4]• PossibleSolution
– SingularValueDecomposition(SVD)
• LessComputationallyIntenseSolutions– QuaternionEstimator
(QUEST)– FastOptimalAttitudeMatrix
(FOAM)– EstimatoroftheOptimal
Quaterion(ESOQ)
𝐵=∑𝑘↑▒𝑤↓𝑘 𝜔 ↓𝑣 𝜔 ↓𝑝↑𝑇
𝐵=𝑈 𝑑𝑖𝑎𝑔[𝑠↓1 𝑠↓2 𝑠↓3 ] 𝑉↑𝑇 𝑹=𝑈 𝑑𝑖𝑎𝑔[11det�(𝑈) det�(𝑉) ] 𝑉↑𝑇
SVD
AGVLAB18
TranslationalOffsetCalculation
• Needtocalculatetranslationvector𝒕betweenvehicleandpedestrianIMUs.
– Assumesknowledgeof𝑹
𝑓 ↓𝑣 = 𝑹𝑓 ↓𝑝 +( 𝜔 ↓𝑣 ×(𝜔 ↓𝑣 × 𝑡 ))+( 𝜔 ↓𝑣 × 𝑡 )
[█𝑓↓𝑥 @𝑓↓𝑦 @𝑓↓𝑧 ]↓𝑣 −𝑹[█𝑓↓𝑥 @𝑓↓𝑦 @𝑓↓𝑧 ]↓𝑝 =[█𝜔↓𝑦↑2 𝜔↓𝑧↑2 &𝜔 ↓𝑧 −𝜔↓𝑥 𝜔↓𝑦 &− 𝜔 ↓𝑦 −𝜔↓𝑥 𝜔↓𝑧 @− 𝜔 ↓𝑧 −𝜔↓𝑥 𝜔↓𝑦 &𝜔↓𝑥↑2 𝜔↓𝑧↑2 &𝜔 ↓𝑥 −𝜔↓𝑦 𝜔↓𝑦 @𝜔 ↓𝑦 −𝜔↓𝑥 𝜔↓𝑧 &− 𝜔 ↓𝑥 −𝜔↓𝑦 𝜔↓𝑧 &𝜔↓𝑥↑2 𝜔↓𝑦↑2 ][█𝒕↓𝒙 @𝒕↓𝒚 @𝒕↓𝒛 ]
𝑦 𝐻 𝑥
𝑦=𝐻𝑥𝑥 = ( 𝐻↑𝑇 𝐻)↑−1 𝐻↑𝑇 𝑦 [5]
AGVLAB
DeterministicObservability
• Calculationofoffsetsdependoninertialmeasurementoutputs.
– Inertialmeasurements→VehicularMotion
• RotationalOffset
– Wahba’sProblem→Twolinearlyindependentreferencevectors[4]
– Linearlyindependentrotationintwoaxes
• TranslationalOffset– Measurementmodelmustbefullrank– Nonzeroangularvelocityand
accelerationinatleasttwoaxes.
• Lanechanging,turning
[█[█𝜔↓𝑦↑2 𝜔↓𝑧↑2 &𝜔 ↓𝑧 −𝜔↓𝑥 𝜔↓𝑦 &− 𝜔 ↓𝑦 −𝜔↓𝑥 𝜔↓𝑧 @− 𝜔 ↓𝑧 −𝜔↓𝑥 𝜔↓𝑦 &𝜔↓𝑥↑2 𝜔↓𝑧↑2 &𝜔 ↓𝑥 −𝜔↓𝑦 𝜔↓𝑦 @𝜔 ↓𝑦 −𝜔↓𝑥 𝜔↓𝑧 &− 𝜔 ↓𝑥 −𝜔↓𝑦 𝜔↓𝑧 &𝜔↓𝑥↑2 𝜔↓𝑦↑2 ]↓1 @⋮@[█𝜔↓𝑦↑2 𝜔↓𝑧↑2 &𝜔 ↓𝑧 −𝜔↓𝑥 𝜔↓𝑦 &− 𝜔 ↓𝑦 −𝜔↓𝑥 𝜔↓𝑧 @− 𝜔 ↓𝑧 −𝜔↓𝑥 𝜔↓𝑦 &𝜔↓𝑥↑2 𝜔↓𝑧↑2 &𝜔 ↓𝑥 −𝜔↓𝑦 𝜔↓𝑦 @𝜔 ↓𝑦 −𝜔↓𝑥 𝜔↓𝑧 &− 𝜔 ↓𝑥 −𝜔↓𝑦 𝜔↓𝑧 &𝜔↓𝑥↑2 𝜔↓𝑦↑2 ]↓𝑘 ]
19
𝜔 ↓𝑣↓1 =[█1@0@0 ]𝑟𝑎𝑑/𝑠, 𝜔 ↓𝑣↓2 =[█0@1@0 ]𝑟𝑎𝑑/𝑠,𝜔 ↓𝑣↓2 =[█0@1@0 ]𝑟𝑎𝑑/𝑠
AGVLAB
DoubleLaneChangeSimulation
• AngularandtranslationaloffsetcalculationtestedusingvehicleIMUdatasimulatedinCarsim.
• TrueAngularOffset: [50 30 −20]↑° • TrueLeverArm:[1.524 0.938 0.8128] 𝑚
20
AGVLAB
SimulationResults
• ReferenceandOffsetIMUdatawasusedtocalculaterotationalandtranslationaloffsets.
• Maneuvercausesestimatestoconvergetotruevalues
21
AGVLAB
PerformanceAnalysis
22
SubsystemsandTotalSystem
AGVLAB
DataCollectionSetup
23
TacticalGradeIMU
MEMSGradeIMUonfoot
MEMSGradeIMUonvehicle
AGVLAB
RigidityDetectionTestandResults
• Footwasmovedatvaryingintervals.
• Buttonpushedatbeginningandendofmovementintervals.
24
RigidPeriod
Movement
ButtonPushes
Still-MovementInterval(sec)
PercentRigid
Detected
PercentMovementDetected
30 97% 99%
20 98% 99%
10 95% 97%
AGVLAB
OffsetCalculationResults
AGVLAB
InitializingPedNavSystem
1) Detectrigidity2) Estimateoffsetsduring
rigidperiods.3) Transformvehiclenav
statesintopedestrianframe
4) Initializepedestriannavigationalgorithm.
5) Pedestrianexitedandwalkedstraightaway.
26
Starting/DropOffPoint
AGVLAB
RunI–MinimalMovement
27
AGVLAB
RunI–MinimalMovement
• InitialpednavcoursecloselymatchesGPScourse.
• Initializedheadingbetterthanassumingpedestrianisperpendiculartovehicleheading
28
𝑥
𝑦𝑥
𝑦
InitializedError PerpendicularError
1° 22°
𝟐𝟐°
AGVLAB
RunII–MoreMovement
29
Imovedmyfoot
AGVLAB
RunII–MoreMovement
• InitialpednavcoursedoesnotmatchGPScourse.– Moreaccurateinitializationin
RunI.
• Movingfoot-mountedIMU15secondsbeforevehicleexitinvalidatesinitialization.– Notenoughmotiontore-establish
angularoffset
30
𝟐𝟔°
AGVLAB
• TwoIMUsrigidlymountedtothevehicle.• Calculateangularoffsetinsideeachdatawindow;calculatedsolutionerror.
• DifferentRigidPeriod/DataWindowSizes:60,50,40,30,20,10,and5seconds
HowOftenisThereExcitation?
31
Error:+ [𝟏𝟎° 𝟑° −𝟐°]
DrivingEnvironment:CityLength:1hourTrueOffset:[𝟗𝟎° 𝟒𝟓° −𝟒𝟓°]
Error:+ [−𝟑° 𝟕° 𝟓°]
AGVLAB
AttitudeEstimationErrorStatistics
• Accuracyandprecisiondecreasewithsmallerrigidperiods/datawindowsizes.
• Shorterrigidperiods→Lesslikelyfortheretobesufficientexcitation
32
AGVLAB
AttitudeEstimationErrorStatistics• Accuracyandprecisiondecreasewithsmallerrigidperiods/datawindow
sizes.• Shorterrigidperiods→Lesslikelyfortheretobesufficientexcitation
33
1°>10° 5°
>20°
AGVLAB
ConclusionandFurtherWork
• AngularandlinearoffsetsbetweenpedestrianandvehicleIMUscanbeaccuratelycalculated.
• Offsetcalculationcanyieldaccurateinitializationinformationforpedestriannavigation.– Rigidperiods– Maneuverbeforevehicleexit
• Furtherwork– MechanizeIMUoutputduringnon-rigidperiods– RigidlymountpedestrianIMUtovehicle
34
AGVLAB
Questions?
ThankYou!
35
AGVLAB
References
[1]P.Groves,G.Pulford,C.AaronLittlefield,D.LJNash,andC.JMather,InertialNavigationVersusPedestrianDeadReckoning:OptimizingtheIntegration,vol.2.2007.[2]N.Strozzi,F.Parisi,andG.Ferrari,“Onsinglesensor-basedinertialnavigation,”2016IEEE13thInternationalConferenceonWearableandImplantableBodySensorNetworks(BSN),2016.[3]I.Skog,P.Handel,J.O.Nilsson,andJ.Rantakokko,“Zero-VelocityDetection—AnAlgorithmEvaluation,”IEEETransactionsonBiomedicalEngineering,vol.57,no.11,pp.2657–2666,2010.[4]Markley,Landis,“30YearsofWahba’sProblem,”presentedattheFlightMechanics,Greenbelt,MD;UnitedStates,1999.[5]M.Crabolu,D.Pani,L.Raffo,M.Conti,P.Crivelli,andA.Cereatti,“Invivoestimationoftheshoulderjointcenterofrotationusingmagneto-inertialsensors:MRI-basedaccuracyandrepeatabilityassessment,”BiomedEngOnline,vol.16,no.1,pp.34–34,Mar.2017.36