【record】tof定高（二）——tof进行高度融合调试过程

最新推荐文章于 2022-10-22 12:59:57 发布

翁庆年的六便士

最新推荐文章于 2022-10-22 12:59:57 发布

阅读量612

点赞数

分类专栏：【drone】Alientek_wukongFPV 文章标签：计算机视觉

本文链接：https://blog.csdn.net/weixin_43705900/article/details/127378457

版权

【drone】Alientek_wukongFPV 专栏收录该内容

10 篇文章 6 订阅

订阅专栏

1、说明

在正点原子的源码中，高度融合两种数据，分别是z轴方向的加速度计计算预测值，气压计去除零偏之后的值作为测量值；两者做差得到一个纠偏值，将该差值乘上气压计权值，去更新预测值，最后得到最优估计值；此处我将tof的数据替换气压计的数据，最终将估计得到z轴速度和位置打印到匿名上位机中；
其中，重要的是如何调试出tof的修正权值，此处我将融合的数据波形实时打印到匿名上位机上，观察不同的权值下的融合效果，最后试出一个较为理想的值；

2、code

(1)融合估计


//速度预估和位置预估
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轴）
#if USE_TOF_NO_BARO
	const float tofResidual =   posEstimator.tof.alt - posEstimator.est.pos.z;
	posAndVelocityCorrect(Z, dt, tofResidual, W_Z_TOF_P);
#else
	const float baroResidual =   posEstimator.baro.alt - posEstimator.est.pos.z;
	posAndVelocityCorrect(Z, dt, baroResidual, W_Z_TOF_P);
#endif
	
	if (updateAccBias) 
	{
#if USE_TOF_NO_BARO
	accelBiasCorr.z -= tofResidual * sq(W_Z_TOF_P);
#else
	accelBiasCorr.z -= baroResidual * sq(W_Z_BARO_P);	
#endif
		
	}
	
	//修正加速度偏置值
    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;
        }
    }
}

//预估器初始化
static void initializePositionEstimator(void)
{
    posEstimator.imu.gravityCalibrationComplete = false;
	
    for (int axis = 0; axis < 3; axis++) 
	{
        posEstimator.imu.accelBias.axis[axis] = 0;
        posEstimator.est.pos.axis[axis] = 0;
        posEstimator.est.vel.axis[axis] = 0;
    }
}

此处重要的就是这里的tof正值；实际在调试的过程中，需要不断调试这个值的大小；
我在调试的过程中，将融合的高度和速度，以及tof直接输出的数据都打印到上位机的波形显示界面上面；
根据实际融合的高度是否实时跟随tof直出的数据，不断尝试调整这个修正的值，最后得到一个相对理想的值；

（2）相关定义

#define  W_Z_TOF_P  5.1f		//tof修正权重
#define USE_TOF_NO_BARO 1

（3）上位机自定义输出

分别输出：
气压计高度、
tof原始高度、
姿态估计中z轴的高度、
期望的z轴的高度、
姿态估计中z轴的速度、
期望的z轴的速度、
油门调整值、

static void sendaboutalt(float bar_alt,float tof_alt,float state_pos_z,float setpoint_pos_z,float state_vel_z,float setpoint_vel_z,float ThrustAdj)
{
	u8 _cnt=0;
	atkp_t p;
	p.msgID = 0xF1;
	p.data[_cnt++]=BYTE3(bar_alt);
	p.data[_cnt++]=BYTE2(bar_alt);
	p.data[_cnt++]=BYTE1(bar_alt);
	p.data[_cnt++]=BYTE0(bar_alt);

	p.data[_cnt++]=BYTE3(tof_alt);
	p.data[_cnt++]=BYTE2(tof_alt);
	p.data[_cnt++]=BYTE1(tof_alt);
	p.data[_cnt++]=BYTE0(tof_alt);

	p.data[_cnt++]=BYTE3(state_pos_z);
	p.data[_cnt++]=BYTE2(state_pos_z);
	p.data[_cnt++]=BYTE1(state_pos_z);
	p.data[_cnt++]=BYTE0(state_pos_z);
	
	p.data[_cnt++]=BYTE3(setpoint_pos_z);
	p.data[_cnt++]=BYTE2(setpoint_pos_z);
	p.data[_cnt++]=BYTE1(setpoint_pos_z);
	p.data[_cnt++]=BYTE0(setpoint_pos_z);

	p.data[_cnt++]=BYTE3(state_vel_z);
	p.data[_cnt++]=BYTE2(state_vel_z);
	p.data[_cnt++]=BYTE1(state_vel_z);
	p.data[_cnt++]=BYTE0(state_vel_z);
	
	p.data[_cnt++]=BYTE3(setpoint_vel_z);
	p.data[_cnt++]=BYTE2(setpoint_vel_z);
	p.data[_cnt++]=BYTE1(setpoint_vel_z);
	p.data[_cnt++]=BYTE0(setpoint_vel_z);

	p.data[_cnt++]=BYTE3(ThrustAdj);
	p.data[_cnt++]=BYTE2(ThrustAdj);
	p.data[_cnt++]=BYTE1(ThrustAdj);
	p.data[_cnt++]=BYTE0(ThrustAdj);

	p.dataLen = _cnt;
	atkpSendPacket(&p);
}

（4）打印队列添加

	if(!(count_ms % PERIOD_MOTOR))
	{
		sendaboutalt(sensorData.baro.asl*1000,sensorData.tof.asl*1000,state.position.z*1000,setpoint.position.z*1000,state.velocity.z*1000,setpoint.velocity.z*1000,altThrustAdj*1000);
	}