gps/ins/wheel count sensor acquisition and fusion
TRANSCRIPT
GPS/INS/Wheel CountGPS/INS/Wheel Count
Sensor Acquisition and FusionSensor Acquisition and Fusion
What do GPS, INS, and Wheel What do GPS, INS, and Wheel Counts do?Counts do?
Report and record the position of the Report and record the position of the robotrobot• Position relative to a starting position for Position relative to a starting position for
INS and wheel count.INS and wheel count.• GPS gives location in Latitude and GPS gives location in Latitude and
LongitudeLongitude
Why do we need three methods to Why do we need three methods to determine position?determine position?
All three systems use different All three systems use different methods to calculate location.methods to calculate location.
By looking at all three, a better By looking at all three, a better approximation of position can be approximation of position can be established.established.
RedundancyRedundancy The weaknesses of each system can The weaknesses of each system can
be offset by the other systems.be offset by the other systems.
Sensor Function OverviewSensor Function Overview
GPS: Triangulates position by GPS: Triangulates position by sending signals to a satellite array. sending signals to a satellite array. Time to receive signals from different Time to receive signals from different satellites allows calculation of satellites allows calculation of position.position.• Strengths: gives latittude/longitude Strengths: gives latittude/longitude
readings anywhere in the world. No readings anywhere in the world. No drifting of measurement.drifting of measurement.
• Weaknesses: inaccuracies, outages Weaknesses: inaccuracies, outages
Sensor Function OverviewSensor Function Overview
INS (Inertial Navigation System) uses a set INS (Inertial Navigation System) uses a set of rate gyroscopes and accelerometers of rate gyroscopes and accelerometers mounted on each of the three axis. mounted on each of the three axis. • Strengths: fast refresh, accurate values, fairly Strengths: fast refresh, accurate values, fairly
reliable with no outages.reliable with no outages.• Weaknesses: drifts over timeWeaknesses: drifts over time
Wheel Count measures the number of Wheel Count measures the number of rotations of each wheel.rotations of each wheel.• Strengths: Works consistently, simple systemStrengths: Works consistently, simple system• Weaknesses: No way to measure wheel slip. Weaknesses: No way to measure wheel slip.
Modeling System Clock to GPS TimeModeling System Clock to GPS Time
GPS has relatively large GPS has relatively large update timeupdate time
Unacceptable for Speed Unacceptable for Speed based GPS estimatesbased GPS estimates
dt(GPS) = loop_time * adt(GPS) = loop_time * a
A = A = 30.179407176330.1794071763
Modeling Dead Reckoning Angle to Modeling Dead Reckoning Angle to Gyroscope Angle Gyroscope Angle
Both initialize to –pi/2Both initialize to –pi/2
Differences are related Differences are related to wheel slipto wheel slip
Simple degrees to Simple degrees to radians conversionradians conversion
Implementation Implementation requires infrequent requires infrequent linkinglinking
Wheel speed to GPS speedWheel speed to GPS speed Sporadic GPS speedSporadic GPS speed
Conversion based on Conversion based on experimental dataexperimental data
GPS speed = GPS speed = wheel_speed*bwheel_speed*b
b =.628734687354b =.628734687354
(X, Y) Coordinates to (X, Y) Coordinates to GPS Longitude and LatitudeGPS Longitude and Latitude
latlen = m1 + (m2.*cos(2.*gps_lat))+ latlen = m1 + (m2.*cos(2.*gps_lat))+ (m3.*cos(4.*gps_lat)) + (m3.*cos(4.*gps_lat)) + (m4.*cos(6.*gps_lat))(m4.*cos(6.*gps_lat))
longlen = (p1 * cos(gps_lat)) + longlen = (p1 * cos(gps_lat)) + (p2.*cos(3.*gps_lat)) + (p2.*cos(3.*gps_lat)) + (p3.*cos(5.*gps_lat))(p3.*cos(5.*gps_lat))
Two sets of (x,y) coordinatesTwo sets of (x,y) coordinates• (x,y) and (x+dx/dt,y+dy/dt)(x,y) and (x+dx/dt,y+dy/dt)
Difference between points as a vectorDifference between points as a vector
Rotate Vector to match GPS angleRotate Vector to match GPS angle• New_angle = original angle-New_angle = original angle-
GPS_angle_offsetGPS_angle_offset
Convert from meters to degreesConvert from meters to degrees
Add to current GPS latitude and Add to current GPS latitude and longitude to get new valuelongitude to get new value
Inertial Navigation System Inertial Navigation System Implementation (ins_conv.c)Implementation (ins_conv.c)
get_gps_est(double est_long, double est_lat, double get_gps_est(double est_long, double est_lat, double est_speed, est_speed, double est_angle, double est_loop_time, int double est_angle, double est_loop_time, int sys_num, double x, double y,double link_offset_angle, sys_num, double x, double y,double link_offset_angle, rstatus r)rstatus r)
• System I: GPS onlySystem I: GPS only• System II: Dead Reckoning to GPSSystem II: Dead Reckoning to GPS • System III: Wheel Speed and System III: Wheel Speed and
Gyroscope to GPSGyroscope to GPS• System IV: GPS Speed and System IV: GPS Speed and
Gyroscope to GPSGyroscope to GPS link_gyro_angle_to_gpslink_gyro_angle_to_gps
Sensor FusionSensor Fusion
By combining all sensor inputs and By combining all sensor inputs and finding “average”, weaknesses of finding “average”, weaknesses of each system can be factored out.each system can be factored out.
Kalman filter is the most common Kalman filter is the most common method to integrate this sensor data.method to integrate this sensor data.
Kalman FilterKalman Filter
A digital filter A digital filter which uses a recursive which uses a recursive process to determine a process to determine a least-squares fit to data least-squares fit to data obtained over time. obtained over time. MIMO, filter for noisy MIMO, filter for noisy sensor data.sensor data.
Works on a Works on a theory of states with theory of states with uncertainties added in uncertainties added in at each recursion. The at each recursion. The uncertainties and uncertainties and weights are updated weights are updated and improved during and improved during each cycle.each cycle.
Source: http://www.cs.unc.edu/~welch/kalman/Levy1997/fig6.htmThe Kalman Filter: Navigation's Integration Workhorse by Larry J Levy
Where would we go from here?Where would we go from here?
Add full INS system to robot.Add full INS system to robot. Implement real-time Kalman filter Implement real-time Kalman filter
including full INS and GPS data.including full INS and GPS data.