0、前言
在定高模式中,飞控需要有当前高度的信息,也就是z的position信息,进行Z轴的位置环控制;那么这个Z轴的位置信息是怎么来的呢?本文为在解读wukong FPV源码中Z轴高度估计的过程的笔记;
1、更新预估器的中气压计数据
(1)估计器数据结构
在估计器中,有三个成员变量,下面是估计器的数据结构:
typedef struct
{
// Data sources
navPositionEstimatorBARO_t baro;
navPosisitonEstimatorIMU_t imu;
// Estimate
posEstimatorEst_t est;
} posEstimator_t;
上面代码块中,源数据是气压数据和imu数据,最终要合成est中的最终估计值,而飞行的state就是由est信息发布而来;
(2)code——气压计数据更新
//更新气压计数据
static void updateBaroTopic(float newBaroAlt)
{
if (baroIsCalibrationComplete())
{
posEstimator.baro.alt = newBaroAlt;
}
else
{
posEstimator.baro.alt = 0.0f;
}
}
上述代码块就是将传感器中气压计的数据赋值进去;
2、更新预估器的加速度数据
(1)估计器中imu的结构体
typedef struct
{
Axis3f accelNEU;
Axis3f accelBias;
bool gravityCalibrationComplete;
} navPosisitonEstimatorIMU_t;
上述code块中,imu的数据只有NEU上的加速度数据和加速度偏置,还有一个重力方向的校验标志位;
(2)code——更新NEU坐标系的加速度
//更新NEU坐标系的加速度
//accBF:为机体坐标系加速度
static void updateIMUTopic(const Axis3f accBF)
{
static float calibratedGravityCMSS = GRAVITY_CMSS;
static u32 gravityCalibrationTimeout = 0;
Axis3f accelCMSS;
//机体坐标系的加速度转为(cm/ss)
accelCMSS.x = accBF.x * GRAVITY_CMSS;
accelCMSS.y = accBF.y * GRAVITY_CMSS;
accelCMSS.z = accBF.z * GRAVITY_CMSS;
//去除加速度偏置
accelCMSS.x -= posEstimator.imu.accelBias.x;
accelCMSS.y -= posEstimator.imu.accelBias.y;
accelCMSS.z -= posEstimator.imu.accelBias.z;
//旋转机体坐标系加速度到NEU坐标系
imuTransformVectorBodyToEarth(&accelCMSS);
//在水平状态校准重力方向的加速度零偏
if (!posEstimator.imu.gravityCalibrationComplete && STATE(SMALL_ANGLE))
{
//慢慢收敛校准重力零偏
const float gravityOffsetError = accelCMSS.z - calibratedGravityCMSS;
calibratedGravityCMSS += gravityOffsetError * 0.0025f;
if (ABS(gravityOffsetError) < 5)//误差要小于5cm/ss
{
if ((getSysTickCnt() - gravityCalibrationTimeout) > 250)
{
posEstimator.imu.gravityCalibrationComplete = true;
}
}
else
{
gravityCalibrationTimeout = getSysTickCnt();
}
}
//NEU坐标系加速度处理
if (posEstimator.imu.gravityCalibrationComplete)
{
accelCMSS.z -= calibratedGravityCMSS;//去除重力
for (int axis = 0; axis < 3; axis++)
{
applyDeadband(accelCMSS.axis[axis], 4);//去除4(cm/ss)死区
posEstimator.imu.accelNEU.axis[axis] += (accelCMSS.axis[axis] - posEstimator.imu.accelNEU.axis[axis]) * 0.3f;//一阶低通
}
}
else
{
posEstimator.imu.accelNEU.x = 0;
posEstimator.imu.accelNEU.y = 0;
posEstimator.imu.accelNEU.z = 0;
上述code块中,输入的是imu的原始加速度数据;
(3)解读加速度更新步骤
- 首先将加速度进行单位的转换;
- 将转换后的加速度数据减去估计器中加速度的偏置值, 这个偏置值是怎么来的,比如Z轴方向的偏置值是气压计的微分得到的;
- 进一步处理观测到的加速度数据,就是将其转换到ENU坐标系;
- 在水平状态校准重力方向的加速度零偏,这一步没看明白;但是实际值跑一次;
其中一个重要的作用是计算重力产生的零偏; - NEU坐标系加速度处理,这个是最重要的处理:
//NEU坐标系加速度处理
if (posEstimator.imu.gravityCalibrationComplete)
{
accelCMSS.z -= calibratedGravityCMSS;//去除重力
for (int axis = 0; axis < 3; axis++)
{
applyDeadband(accelCMSS.axis[axis], 4);//去除4(cm/ss)死区
posEstimator.imu.accelNEU.axis[axis] += (accelCMSS.axis[axis] - posEstimator.imu.accelNEU.axis[axis]) * 0.3f;//一阶低通
}
}
else
{
posEstimator.imu.accelNEU.x = 0;
posEstimator.imu.accelNEU.y = 0;
posEstimator.imu.accelNEU.z = 0;
}
这里将code再贴一次,最终得到了估计器中imu的加速度值;其中Z方向的加速度观测值数据再减去重力的偏置之后;最终得到:
z加速度估计值 = z加速度估计值 + (z加速度观测值 - z加速度的估计值) 0.3*
3、速度预估和位置预估
终于到速度和位置的估计了,这一步就是通过之前的估计得到的imu的加速度数据,得到飞机的当前位置和速度信息;
(1)code——速度预估和位置预估
//速度预估和位置预估
static void updateEstimatedTopic(float dt)
{
//使用加速度预估位移和速度
posAndVelocityPredict(Z, dt, posEstimator.imu.accelNEU.z);
posAndVelocityPredict(Y, dt, posEstimator.imu.accelNEU.y);
posAndVelocityPredict(X, dt, posEstimator.imu.accelNEU.x);
//加速度偏置值
const bool updateAccBias = (W_ACC_BIAS > 0);
Axis3f accelBiasCorr = { { 0, 0, 0} };
//使用气压计高度误差修正预估的位移和速度(Z轴)
const float baroResidual = posEstimator.baro.alt - posEstimator.est.pos.z;
posAndVelocityCorrect(Z, dt, baroResidual, W_Z_BARO_P);
if (updateAccBias)
{
accelBiasCorr.z -= baroResidual * sq(W_Z_BARO_P);
}
//修正加速度偏置值
if (updateAccBias)
{
const float accelBiasCorrMagnitudeSq = sq(accelBiasCorr.x) + sq(accelBiasCorr.y) + sq(accelBiasCorr.z);
if (accelBiasCorrMagnitudeSq < sq(GRAVITY_CMSS * 0.25f))//偏置小于0.25G时可以修正
{
//将加速度偏置值由世界坐标系转换为机体坐标系
imuTransformVectorEarthToBody(&accelBiasCorr);
posEstimator.imu.accelBias.x += accelBiasCorr.x * W_ACC_BIAS * dt;
posEstimator.imu.accelBias.y += accelBiasCorr.y * W_ACC_BIAS * dt;
posEstimator.imu.accelBias.z += accelBiasCorr.z * W_ACC_BIAS * dt;
}
}
}
(2)解读速度预估和位置预估的步骤
- 首先就是用预估器中imu数据预估速度和位置
static void posAndVelocityPredict(int axis, float dt, float acc)
{
posEstimator.est.pos.axis[axis] += posEstimator.est.vel.axis[axis] * dt + acc * dt * dt / 2.0f;
posEstimator.est.vel.axis[axis] += acc * dt;
}
上述code非常容易理解,其实就是
x = x0 + V0 *dt + 1/2 * a t^2
v = at + v0
如此就可以知道X,Y,Z方向的速度和位置信息了吗,继续往下面看;
- //使用气压计高度误差修正预估的位移和速度(Z轴)
此处就是有一个观测到的高度数据,也就是气压计的数据,另一个是估计的数据,也就是预估器中的z轴方向的速度和位置信息;
那么就可以做一下处理了;
//误差修正预估的位移和速度
static void posAndVelocityCorrect(int axis, float dt, float e, float w)
{
float ewdt = e * w * dt;
posEstimator.est.pos.axis[axis] += ewdt;
posEstimator.est.vel.axis[axis] += w * ewdt;
}
其中的e是Z位置观测值与估计的Z位置估计值的差;
总结就是:
Z位置估计值 = Z位置估计值+ ( Z位置观测值- Z位置估计值) 权重*dt*
这个操作就和上面的
z加速度估计值 = z加速度估计值 + (z加速度观测值 - z加速度的估计值) 0.3
非常相似;但是为什么要乘上一个dt呢;
3. //修正加速度偏置值
4、速度和位置的发布
(1)code——速度和位置的发布
//发布预估位置和速度
static void publishEstimatedTopic(state_t *state)
{
static u32 publishTime;
//更新世界坐标系的加速度
state->acc.x = posEstimator.imu.accelNEU.x;
state->acc.y = posEstimator.imu.accelNEU.y;
state->acc.z = posEstimator.imu.accelNEU.z;
//更新估计的位置和速度(10ms->100Hz)
if ((getSysTickCnt() - publishTime) >= 10)
{
state->position.x = posEstimator.est.pos.x;
state->position.y = posEstimator.est.pos.y;
state->position.z = posEstimator.est.pos.z;
state->velocity.x = posEstimator.est.vel.x;
state->velocity.y = posEstimator.est.vel.y;
state->velocity.z = constrainf(posEstimator.est.vel.z, -150.0f, 150.0f);//限制Z轴的速度为150cm/s
publishTime = getSysTickCnt();
}
}
上述code就比较好理解了,就是将预估器中得到的速度和位置信息赋值给无人机的state变量,使其参与到控制中去;