程序员经验分享-hwhale

【code review】2、关于高度的估计过程

2023-05-16

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）解读加速度更新步骤

  1. 首先将加速度进行单位的转换；
  2. 将转换后的加速度数据减去估计器中加速度的偏置值， 这个偏置值是怎么来的，比如Z轴方向的偏置值是气压计的微分得到的；
  3. 进一步处理观测到的加速度数据，就是将其转换到ENU坐标系；
  4. 在水平状态校准重力方向的加速度零偏，这一步没看明白；但是实际值跑一次；
    其中一个重要的作用是计算重力产生的零偏；
  5. 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）解读速度预估和位置预估的步骤

  1. 首先就是用预估器中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方向的速度和位置信息了吗，继续往下面看；

  1. //使用气压计高度误差修正预估的位移和速度（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变量，使其参与到控制中去；

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

【code review】2、关于高度的估计过程 的相关文章

随机推荐

  • 什么是任务栈？

    在一个裸机系统中 xff0c 如果有全局变量 xff0c 有子函数调用 xff0c 有中断发生 那么系统在运行的时候 xff0c 全局变量放在哪里 xff0c 子函数调用时 xff0c 局部变量放在哪里 xff0c 中断发生时 xff0c

  • Ubuntu vscode 配置c/c++环境 ---- 静态代码检查

    我曾一度因为vscode中c语言的静态代码检查问题而困扰 xff0c 想想还是太懒了 xff0c 不愿意折腾 xff0c 今天搞一下 首先在ubuntu中装vscode xff0c xff0c xff0c 然后装clang span cla

  • ubuntu22.04 运行qq音乐闪退

    修改 usr share applications中的qqmusic desktop xff0c 在Exec后加上 no sandbox 如下图所示 xff1a

  • PIXHawk用QGC刷Firmware

    1 准备版本 2 开始刷 确定后会出现弹窗 xff0c 再弹窗的文件名处粘贴

  • Xfce4快捷键个性化配置（个人向）

    将xfce4部分常用快捷键设置为Gnome下的快捷键 窗口操作 xff08 在 窗口管理器 gt 键盘 里 xff09 切换同一应用的窗口 xff1a Alt 43 96 最大化窗口 xff1a Super 43 上隐藏窗口 xff1a S

  • 树莓派安装Dronekit连接PIX读取基础数据

    本文主要参考于苍穹四轴DIY的公众号文章 xff1a http span class token operator span span class token operator span span class token operator

  • PX4调试起飞

    一 下载固件 在px4的git上找到相应对于硬件型号的固件 xff1b 二 烧录固件 在qgc上 xff0c 点击高级上的自定义 xff0c 即可烧录相应固件 xff1b 三 选择机架 校准传感器 设置遥控器 选择机架类型之后 xff0c

  • PX4编译——搭建你的第一个应用（Hello Shy）

    一 编译环境 1 ubuntu20 04 2 px4 V2 二 编写测试程序 下载好px4code之后 xff0c 在src examples px4 simple app里面有cmakelist txt和px4 sample app cp

  • PX4开发说明

    本栏文档主要参考PX4的用户指导 xff1b 记录在px4开发过程中的心得体会和备忘 xff1b PX4 User Guide https docs px4 io master en dev setup dev env html

  • prometheus学习

    记录一下在阿木实验室 学习开源项目prometheus的过程

  • Error: No valid host was found.

    使用openstack创建虚拟机经常会遇到以下的这个错误 Error No valid host was found There are not enough hosts available 从字面意思就可以看出是无法找到可用的host的资

  • debian sid 安装 sopcast

    刚刚装了sopcast 由于是编译的 xff0c 所以记录一下以便以后删除干净 http sopcast com download linux html 上有详细说明 1 xff09 下载 sp auth tgz xff0c 把sp sc

  • 2.1 mavros发布位置指令控制px4

    1 说明 写一个节点给px4发送位置控制指令 xff0c 比如我想让飞机飞到10 xff0c 10 xff0c 10这个坐标 xff1b 2 发布和订阅的mavros主题 发布的主题 xff1a mavros setpoint positi

  • 2.2 mavros发布姿指令控制PX4

    说明 使用遥控飞行 px4在stablize模式下 xff0c 我们使用遥控器去控制px4飞行 xff1b 在飞行过程中 xff0c 通常我们用4个通道就可以控制飞机飞行 xff1b 其中roll pitch yaw打杆的量就是我们期望无人

  • 关于PX4上PID调参

    使用PX4 log view 工具 地址 setp response for roll rate 找到setp response for roll rate这个图片 从图片中可以看到 xff0c roll方向的角速度响应时间不够快 xff1

  • 【record】1、Prometheus-V2 初体验

    一 环境搭建 平时习惯使用虚拟机 xff0c 刚好阿木的公众号里面有送镜像 xff0c 于是在V1的时候就用这个镜像在run了 xff0c 这次V2出来 xff0c 直接pull就可以开始起飞了 xff1b xff08 感觉用虚拟机加镜像是

  • 【record】2、使用非官方遥控器适配prometheus的驱动修改

    0 前言 xff1a prometheus V2推荐使用阿木的遥控器 但是家里遥控器实在太多了 xff0c 所以就尝试修改一下prometheus里关于joystick的驱动 xff0c 使其适配prometheus的控制 xff1b 本篇

  • 【recode】3、地面站使用步骤与体验

    一 前言 从Prometheus的V1到V2 xff0c 无人机的状态显示是在终端中 xff0c 在一堆字符中寻找想要关注的信息 xff0c 确实硬核 xff1b 而今 xff0c 随着社会与科技的发展 xff0c Prometheus也开

  • 【recode】4、二维码自主降落与重复测试code修改

    0 前言 使用二维码辅助无人机降落 xff0c 模拟飞机先飞到二维码上空一定的高度 xff0c 然后切换到command control模式 xff1b 飞机会自动识别二维码的位置然后调整自身的X和Y位置信息 xff0c 同时控制高度进行下

  • 【code review】2、关于高度的估计过程

    0 前言 在定高模式中 xff0c 飞控需要有当前高度的信息 xff0c 也就是z的position信息 xff0c 进行Z轴的位置环控制 xff1b 那么这个Z轴的位置信息是怎么来的呢 xff1f 本文为在解读wukong FPV源码中Z

热门标签