我的博客

一、基础配置声明：本人水平有限，博客可能存在部分错误的地方，请广大读者谅解并向本人反馈错误。一、基础配置测量范围-200g-200g，分辨率为12位，Vs​VDDIO​范围为1.6V-3.5V1.1 引脚配置和功能说明引脚功能比较简单，主要是几个电源引脚(Vs​VDDIO​GND)、SPI接口和两个中断接口。1.2 三种工作模式ADXL372有三种工作模式:①连续测量模式；②低功耗冲击检测的瞬时接通模式；

   声明：本人水平有限，博客可能存在部分错误的地方，请广大读者谅解并向本人反馈错误。   论文全称：《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU》一、 介绍1.1 摘要  选择用四元数表示角度位置数据，并使用扩展卡尔曼滤波作为传感器融合算法。为此，设计了一种新的两级滤波器:第一级利用加速度计数据，第二级利用磁力计数据进行角位置校正。这允许灵活性，更少的计算

最近需要使用AEKF对姿态进行结算，所以又对四元数进了深入的学习，本篇博客仅对四元数进行推导，后续会对基于四元数的AEKF算法进行实现。在秦永元老师的《惯性导航（第二版）》9.2.2节（P248-P253）中对四元数与坐标变换矩阵的关系进行了推导，大家可以参考学习。在第一节中，我们规定了旋转顺序为：Z–>X–>Y，那么最后四元数的旋转矩阵为（参考博客：《是n系到b系的坐标变换矩阵，而在求解时，是将b系的姿态变换到n系上求解，所以最后要。在此假设旋转的顺序为：Z–>X–>Y，那么最后的坐标变换矩阵为（

由于磁力计在钻具径向或切向(ｘ和y方向)测出的信号较好，不受振动和冲击的影响，频率随转速同步变化，且磁力计在ＭＷＤ 系统中固有，无需增加额外的参考信号测量传感器和电路，为此本文选用敏感轴与x轴(或ｙ轴)平行的磁力计的输出信号作为参考信号， 并对幅度进行归一化，现以ｘ轴磁力计为例，可表示为。对于确定性信号，在不同时刻的取值具有较强的相关性；为不含噪声的y轴重力加速度信号，固有频率与转速相同，为包括电路产生的随机噪声、钻具旋转产生的切向加速度干扰、钻具受到横向振动及冲击干扰等在内的各种干扰噪声信号。

在这里，取每个𝑘𝜏0上陀螺仪输出样本的累积和，然后将得到的每个和乘以样本周期𝜏0来得到θ的二进制操作值。一旦我们知道了θ的输出角度值、样本周期和𝑚的值，我们就可以用这个方程来计算Allan方差。时域信号Ω(𝑡)的Allan方差分析是计算其根Allan方差或Allan偏差作为不同平均次数的函数，然后分析Allan偏差曲线的特征区域和对数曲线，以识别不同的噪声模式。最后，对2.1节或2.2节的结果取平方根，就可以得到某一特定值的根Allan方差或Allan偏差的值。1.计算每个陀螺输出样本对应的θ。

下面是电路的原理图和PCB，因为已经购买了MPU6050和QMC5883L模块，所以直接使用这些模块来做，不再设计外围电路(不是懒，主要是这个电路板做测试使用，后续做课题看不会用这中比较low的MEMS传感器)。本周主要工作是在项目上，抽空做了一个跟本课题相关的电路板，之前用开发板来做测试，MPU6050和QMC5883L都是用杜邦线连接的，导致接线很乱，也不美观，当然也不符合“捷联”的定义。，本博客后续的资源链接中也会包含ICM20602的代码。有需要的可以联系我，或者去我的资源中进行下载。

   声明：因为接触本课题时间不长，对于四元数解法一直没太懂什么意思，本篇博客就对这几天的学习进行总结，肯定会有错误，希望读者能够帮忙指正。本篇博客主要参考秦永元老师《惯性导航》第九章第二小节以及几篇论文。一、 四元数1.1 四元数定义   四元数就是由四个元构成的数：Q(q0,q1,q2,q3)=q0+q1i+q2j+q3kQ(q_0,q_1,q_2,q_3)=q_0+q_1\bold i+q_2\bold j+q_3\bold kQ(q0​,q1​,q2​,q3​)=q0​+q1​i+q2​j+

   论文全称：《Continuous Wellbore Surveying While Drilling Utilizing MEMS GyroscopesBased on Kalman Filtering 》摘要：   目前在钻井过程中计算井筒的方法是基于在所需位置的固定测量。这是通过测量当前和先前测量站之间钻孔的倾角和方位角来完成的。利用基于钻孔形状假设的数学模型，可以推导出井眼坐标。目前的方法忽略了两个测量站之间的实际轨迹。   勘探和生产公司需要具有成本效益的钻井作业。因此，对捕捉固定测

一、 论文内容一、 论文内容1.1 摘要为准确测量旋转钻井时的钻具姿态，提出了一种新的信号处理方法。测斜仪旋转时，垂直于其旋转轴方向加速度计的输出信号中重力加速度信号分量具有周期性特征，以及非周期性离心加速度分量频率低于重力加速度信号分量频率的特征，用高通或带通滤波将非周期性离心加速度分量分离；对于常规滤波方法无法滤除的通频带内振动信号，增加一个对置加速度计，采取不同隔振措施补偿振动信号，滤除通频带内的振动信号。

一、 ICM20602介绍本篇博客对ICM20602芯片进行学习，目的是后续设计一个电路板，采集ICM20602的数据，通过这些数据对各种姿态解算的方法进行仿真学习。一、 ICM20602介绍加重突出1.1 初识芯片3轴陀螺仪：可编程全刻度范围(FSR)为±250 dps，±500 dps，±1000 dps和±2000 dps（dps:degrees per second，度/秒）。3轴加速度计：可编程全刻度范围(FSR)为±2g，±4g，±8g和±16g。

一、 论文简要说明一、 论文简要说明提出了一种在磁场域中对捷联式磁强计进行标定的算法。传统的摆经方法需要计算一系列的航向修正参数，因此受限于双轴系统，而该算法直接估计磁力计输出误差。因此，该算法可用于标定全三轴磁强计。校准算法使用迭代的批量最小二乘估计器，该估计器使用两步非线性估计器初始化。通过仿真验证了算法的收敛特性，并利用磁强计三联仪采集的实验数据进一步验证了算法的有效性。结果表明，校正后的残差很小，导致系统航向误差在1 ~ 2度之间。本文的主题是估计测量误差并从磁力计测量值中去除它们。

一、 前言的其中一篇参考文献，本博客对该篇论文进行学习记录本专栏博客参考《STM32Cube高效开发教程(基础篇)》，有意向的读者可以购买正版书籍辅助学习，本书籍由王维波老师、鄢志丹老师、王钊老师倾力打造，书籍内容干货满满。一、 前言我们提出了一个非线性两步估计算法的扩展，最初是为了对固态挂载磁强计进行校准而开发的。我们将该算法扩展到包括两轴和三轴传感器的传感器组中的非正交性。非正交性可能是由于制造问题、安装几何和在磁强计的情况下，软铁偏置误差引起的。

一、前言完整的论文名：《Attitude Determination with Magnetometers and Accelerometers to Use in Satellite Simulator》注意：该篇论文中有很多地方未介绍，主要是直接用了另外两篇文献的方法，所以后续会对另外两篇文件做介绍一、前言众所周知，磁力计和加速度计需要校准，以便使这些测量能够达到其最终精度。

该论文针对三轴加速度计、磁通门和速率陀螺随钻测量系统，建立了基于四元数井眼轨迹参数测量模型，并依据状态方程和量测方程，应用2个扩卡尔曼滤波器、1个无迹卡尔曼滤波器和磁干扰校正系统对加速度计、磁通门信号进行滤波、校正，形成了基于数据融合的近钻头井眼轨迹参数动态测量方法。

垂直导向钻井工具在近钻头振动和工具旋转的钻井工作状态下，工具姿态参数的动态测量精度不高。为此，通过理论分析和数值仿真，提出了转速补偿的算法以消除工具旋转对测量的影响;采用最小均方算法( Least Mean Square—LMS) 自适应滤波算法，可以有效滤除近钻头振动对测量的影响。数值仿真表明，经过转速补偿和LMS 自适应滤波后的井斜角测量误差可小于0． 1°，工具面角测量误差小于6°，有效地提高了垂直导向钻井工具的动态测量精度。

一、 卡尔曼滤波一、 卡尔曼滤波卡尔曼滤波的应用要求系统和底层过程的测量模型都是线性的。xk​Φkk−1​xk−1​Gk−1​wk−1​式中Φkk−1​为状态转移矩阵，xk​为状态向量，Gk−1​为噪声分布矩阵，wk−1​为过程噪声向量，k为测量历元。系统的测量方程由下式给出zk​Hk​xk​ηk​式中zk​为系统输出的测量向量，Hk​为观测或设计，ηk​为测量噪声。

一、 粘滑运动的动态算法一、 粘滑运动的动态算法加重突出在实际钻井过程中，钻柱会出现扭振和粘滑现象（），但并不总是呈现均匀旋转。如下图所示，提取一段地下数据时，转盘转速保持在100 r/min，钻头转速在0-200 r/min之间波动，粘滑现象非常严重。在这种情况下，基于钻柱均匀旋转的理论模型将不可行。"粘滑运动"是一个物理学术语，通常用于描述物体在两个表面之间运动时的特性。

一、 安装误差校准一、 安装误差校准。

该测量系统基于，安装在钻柱内部，随钻柱一起旋转，形成捷联惯性导航系统，安装如下图所示：fbfx​​fy​​fz​​Twbwx​​wy​​wz​​T使用这六轴的输出可以非常有效的求解姿态，应用惯性导航的求解算法，可以实现钻孔时的连续测量。加速度计和陀螺仪可以在坐标系中提供线加速度和角速度。但是，如果要求解钻具的位姿，则需要将该坐标系转换为另一个坐标系。Xe、Ye、Ze三个轴代表地球坐标系。

利用和合理的分配信息，有效地滤除钻具振动对动态姿态测量的影响。针对导向钻井工具动态测量的影响而导致测量不准确的问题，提出一种抗差自适应滤波的动态空间姿态测量方法。通过分析钻具振动对姿态测量的影响，并吸收抗差估计和自适应滤波的优点，利用抗差等价权矩阵自适应的确定量测信息，通过自适应因子调整状态模型信息对状态参数的整体贡献，从而消除钻具振动对动态姿态测量的影响，获得实时性强、精度高的姿态参数，提高钻井效率，降低钻井风险。

为了求解惯性导航的力学方程，姿态矩阵Rbb​可以有姿态微分方程得到。其中，四元数是常用的方法，如下图所示，假设刚体在原点旋转，根据欧拉定理，运动坐标系(b系列)相对于导航坐标系(n系列)的方向，相当于b系绕等效轴旋转一个角度Θ。用四元数Qq1​q2​q3​q4​T来描述b系统相对于n系统的旋转。

当倾角很小时或者趋向于0，cosI=1,sinI=0(此处I为倾角)，于是式(1.1)可以写作式(1.2)和(1.3)。当传感器安装在旋转钻柱的中心时，测量x和y轴信号将呈现正弦波，平均值为零。同样由式(1.5)可以得到式(1.6)(1.7)。因为倾角比较小，所以Gx,Gy也非常小(几乎垂直，重力在X,Y轴上无分量)由式(1.4)可知,此时的sinI不可以近似为0。磁性测量工具安装在非磁性钻铤内，如图1，以避免磁性随钻测量工具测量时受到外部干扰。上式中，T为磁工具面角，G为重力加速度。

当倾角θ较小时，很难计算重力工具的面角，如果θ=0，则测量的Gx和Gy应为零，则不能使用上式。之后，沿着Oy1（由于此时的Y轴与之前的状态不再重合），旋转之后的角度叫做"倾斜角"，或者简称"倾角"；这些轴可以是任意的，但通常选择的是固定坐标系的轴，如X、Y、Z轴。需要注意的是，欧拉旋转的顺序很重要，不同的旋转顺序会导致不同的结果。方位角指的是钻井井眼的方向角度，也就是井眼在水平平面上的方向。我的研究课题是动态测量，不过有关静态测量的知识是可以用在动态测量上面的，可以为动态测量打基础。