aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Add wheel encoder odometry

Browse Source 
Uses the existing body frame odometry observation model.
Handles each sensored wheel as a separate sensor.
This commit is contained in:
priseborough 2017-07-27 15:01:48 +10:00 committed by Randy Mackay
parent cbe8f97489
commit c7f6d52065
8 changed files with 142 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -567,6 +567,14 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
AP_GROUPINFO("VIS_VERR_MAX", 52, NavEKF3, _visOdmVelErrMax, 0.9f),
// @Param: WENC_VERR
// @DisplayName: Wheel odometry velocity error
// @Description: This is the 1-STD odometry velocity observation error that will be assumed when wheel encoder data is being fused.
// @Range: 0.01 1.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("WENC_VERR", 53, NavEKF3, _wencOdmVelErr, 0.1f),
AP_GROUPEND
};
@ -1182,6 +1190,23 @@ void NavEKF3::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Ve
}
}
/*
* Write odometry data from a wheel encoder. The axis of rotation is assumed to be parallel to the vehicle body axis
*
* delAng is the measured change in angular position from the previous measurement where a positive rotation is produced by forward motion of the vehicle (rad)
* delTime is the time interval for the measurement of delAng (sec)
* timeStamp_ms is the time when the rotation was last measured (msec)
* posOffset is the XYZ body frame position of the wheel hub (m)
*/
void NavEKF3::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius)
{
if (core) {
for (uint8_t i=0; i<num_cores; i++) {
core[i].writeWheelOdom(delAng, delTime, timeStamp_ms, posOffset, radius);
}
}
}
// return data for debugging body frame odometry fusion
uint32_t NavEKF3::getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar)
{

13
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -212,6 +212,17 @@ public:
*/
void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset);
/*
* Write odometry data from a wheel encoder. The axis of rotation is assumed to be parallel to the vehicle body axis
*
* delAng is the measured change in angular position from the previous measurement where a positive rotation is produced by forward motion of the vehicle (rad)
* delTime is the time interval for the measurement of delAng (sec)
* timeStamp_ms is the time when the rotation was last measured (msec)
* posOffset is the XYZ body frame position of the wheel hub (m)
* radius is the effective rolling radius of the wheel (m)
*/
void writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius);
/*
* Return data for debugging body frame odometry fusion:
*
@ -397,6 +408,8 @@ private:
AP_Int8 _originHgtMode; // Bitmask controlling post alignment correction and reporting of the EKF origin height.
AP_Float _visOdmVelErrMax; // Observation 1-STD velocity error assumed for visual odometry sensor at lowest reported quality (m/s)
AP_Float _visOdmVelErrMin; // Observation 1-STD velocity error assumed for visual odometry sensor at highest reported quality (m/s)
AP_Float _wencOdmVelErr; // Observation 1-STD velocity error assumed for wheel odometry sensor (m/s)
// Tuning parameters
const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration

19
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -214,19 +214,15 @@ void NavEKF3_core::setAidingMode()
PV_AidingMode = AID_RELATIVE;
}
} else if (PV_AidingMode == AID_RELATIVE) {
// Check if the optical flow sensor has timed out
bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
// Check if the fusion has timed out (flow measurements have been rejected for too long)
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
// Check if the body odometry flow sensor has timed out
bool bodyOdmSensorTimeout = ((imuSampleTime_ms - bodyOdmMeasTime_ms) > 5000);
// Check if the fusion has timed out (body odometry measurements have been rejected for too long)
bool bodyOdmFusionTimeout = ((imuSampleTime_ms - prevBodyVelFuseTime_ms) > 5000);
// Enable switch to absolute position mode if GPS or range beacon data is available
// If GPS or range beacons data is not available and flow fusion has timed out, then fall-back to no-aiding
if(readyToUseGPS() || readyToUseRangeBeacon()) {
PV_AidingMode = AID_ABSOLUTE;
} else if ((flowSensorTimeout || flowFusionTimeout) && (bodyOdmSensorTimeout || bodyOdmFusionTimeout)) {
} else if (flowFusionTimeout && bodyOdmFusionTimeout) {
PV_AidingMode = AID_NONE;
}
} else if (PV_AidingMode == AID_ABSOLUTE) {
@ -422,9 +418,16 @@ bool NavEKF3_core::readyToUseOptFlow(void) const
// return true if the filter is ready to start using body frame odometry measurements
bool NavEKF3_core::readyToUseBodyOdm(void) const
{
// We need stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use these measurements
return (imuSampleTime_ms - bodyOdmMeasTime_ms < 200)
&& bodyOdmDataNew.velErr < 1.0f
// Check for fresh visual odometry data that meets the accuracy required for alignment
bool visoDataGood = (imuSampleTime_ms - bodyOdmMeasTime_ms < 200) && (bodyOdmDataNew.velErr < 1.0f);
// Check for fresh wheel encoder data
bool wencDataGood = (imuSampleTime_ms - wheelOdmMeasTime_ms < 200);
// We require stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use odometry measurements
// becasue they are in a body frame of reference
return (visoDataGood || wencDataGood)
&& tiltAlignComplete
&& delAngBiasLearned;
}

27
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -128,6 +128,33 @@ void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, con
}
void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius)
{
// This is a simple hack to get wheel encoder data into the EKF and verify the interface sign conventions and units
// It uses the exisiting body frame velocity fusion.
// TODO implement a dedicated wheel odometry observaton model
// limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised
if (((timeStamp_ms - wheelOdmMeasTime_ms) < frontend->sensorIntervalMin_ms) || (delTime < dtEkfAvg) || !statesInitialised) {
return;
}
wheelOdmDataNew.hub_offset = &posOffset;
wheelOdmDataNew.delAng = delAng;
wheelOdmDataNew.radius = radius;
wheelOdmDataNew.delTime = delTime;
wheelOdmDataNew.time_ms = timeStamp_ms - (uint32_t)(500.0f * delTime);
wheelOdmMeasTime_ms = timeStamp_ms;
// becasue we are currently converting to an equivalent velocity measurement before fusing
// the measurement time is moved back to the middle of the sampling period
wheelOdmDataNew.time_ms -= (uint32_t)(500.0f * delTime);
storedWheelOdm.push(wheelOdmDataNew);
}
// write the raw optical flow measurements
// this needs to be called externally.
void NavEKF3_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset)

2
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -74,7 +74,7 @@ uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velIn
velInnovVar.x = varInnovBodyVel[0];
velInnovVar.y = varInnovBodyVel[1];
velInnovVar.z = varInnovBodyVel[2];
return bodyOdmDataDelayed.time_ms;
return MAX(bodyOdmDataDelayed.time_ms,wheelOdmDataDelayed.time_ms);
}
// return data for debugging range beacon fusion one beacon at a time, incrementing the beacon index after each call

33
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -1564,11 +1564,44 @@ void NavEKF3_core::SelectBodyOdomFusion()
// start performance timer
hal.util->perf_begin(_perf_FuseBodyOdom);
usingWheelSensors = false;
// Fuse data into the main filter
FuseBodyVel();
// stop the performance timer
hal.util->perf_end(_perf_FuseBodyOdom);
} else if (storedWheelOdm.recall(wheelOdmDataDelayed, imuDataDelayed.time_ms)) {
// check if the delta time is too small to calculate a velocity
if (wheelOdmDataNew.delTime > EKF_TARGET_DT) {
// get the forward velocity
float fwdSpd = wheelOdmDataNew.delAng * wheelOdmDataNew.radius * (1.0f / wheelOdmDataNew.delTime);
// get the unit vector from the projection of the X axis onto the horizontal
Vector3f unitVec;
unitVec.x = prevTnb.a.x;
unitVec.y = prevTnb.a.y;
unitVec.z = 0.0f;
unitVec.normalized();
// multiply by forward speed to get velocity vector measured by wheel encoders
Vector3f velNED = unitVec * fwdSpd;
// This is a hack to enable use of the existing body frame velocity fusion method
// TODO write a dedicated observation model for wheel encoders
usingWheelSensors = true;
bodyOdmDataDelayed.vel = prevTnb * velNED;
bodyOdmDataDelayed.body_offset = wheelOdmDataNew.hub_offset;
bodyOdmDataDelayed.velErr = frontend->_wencOdmVelErr;
// Fuse data into the main filter
FuseBodyVel();
}
}
}

7
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -125,6 +125,9 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
if(!storedBodyOdm.init(obs_buffer_length)) {
return false;
}
if(!storedWheelOdm.init(imu_buffer_length)) { // initialise to same length of IMU to allow for multiple wheel sensors
return false;
}
// Note: the use of dual range finders potentially doubles the amount of data to be stored
if(!storedRange.init(MIN(2*obs_buffer_length , imu_buffer_length))) {
return false;
@ -365,7 +368,6 @@ void NavEKF3_core::InitialiseVariables()
// body frame displacement fusion
memset(&bodyOdmDataNew, 0, sizeof(bodyOdmDataNew));
memset(&bodyOdmDataDelayed, 0, sizeof(bodyOdmDataDelayed));
bodyOdmStoreIndex = 0;
lastbodyVelPassTime_ms = 0;
memset(&bodyVelTestRatio, 0, sizeof(bodyVelTestRatio));
memset(&varInnovBodyVel, 0, sizeof(varInnovBodyVel));
@ -374,6 +376,8 @@ void NavEKF3_core::InitialiseVariables()
bodyOdmMeasTime_ms = 0;
bodyVelFusionDelayed = false;
bodyVelFusionActive = false;
usingWheelSensors = false;
wheelOdmMeasTime_ms = 0;
// zero data buffers
storedIMU.reset();
@ -385,6 +389,7 @@ void NavEKF3_core::InitialiseVariables()
storedOutput.reset();
storedRangeBeacon.reset();
storedBodyOdm.reset();
storedWheelOdm.reset();
}
// Initialise the states from accelerometer and magnetometer data (if present)

26
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -223,6 +223,17 @@ public:
*/
void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset);
/*
* Write odometry data from a wheel encoder. The axis of rotation is assumed to be parallel to the vehicle body axis
*
* delAng is the measured change in angular position from the previous measurement where a positive rotation is produced by forward motion of the vehicle (rad)
* delTime is the time interval for the measurement of delAng (sec)
* timeStamp_ms is the time when the rotation was last measured (msec)
* posOffset is the XYZ body frame position of the wheel hub (m)
* radius is the effective rolling radius of the wheel (m)
*/
void writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius);
/*
* Return data for debugging body frame odometry fusion:
*
@ -487,6 +498,14 @@ private:
uint32_t time_ms; // measurement timestamp (msec)
};
struct wheel_odm_elements {
float delAng; // wheel rotation angle measured in body frame - positive is forward movement of vehicle (rad/s)
float radius; // wheel radius (m)
const Vector3f *hub_offset; // pointer to XYZ position of the wheel hub in body frame (m)
float delTime; // time interval that the measurement was accumulated over (sec)
uint32_t time_ms; // measurement timestamp (msec)
};
// update the navigation filter status
void updateFilterStatus(void);
@ -1059,6 +1078,13 @@ private:
bool bodyVelFusionDelayed; // true when body frame velocity fusion has been delayed
bool bodyVelFusionActive; // true when body frame velocity fusion is active
// wheel sensor fusion
uint32_t wheelOdmMeasTime_ms; // time wheel odometry measurements were accepted for input to the data buffer (msec)
bool usingWheelSensors; // true when the body frame velocity fusion method should take onbservation data from the wheel odometry buffer
obs_ring_buffer_t<wheel_odm_elements> storedWheelOdm; // body velocity data buffer
wheel_odm_elements wheelOdmDataNew; // Body frame odometry data at the current time horizon
wheel_odm_elements wheelOdmDataDelayed; // Body frame odometry data at the fusion time horizon
// Range Beacon Sensor Fusion
obs_ring_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer