电子罗盘的理论分析与实际应用（AK09915）

传感器
Word count: 5.2k   |   Reading time≈ 22 min

前言：继上篇6轴IMU之后，本文将介绍电子罗盘的原理（数学模型）和以AK09915为目标的应用实例。

磁基础知识

磁的分类

  • 硬磁：也就是永磁性材料。因为这种材料的矫顽力高/磁滞回线面基大，所以在被磁化后将很难退磁。
  • 软磁：普通的纯铁就是很好的例子，可以被磁铁吸住（纯铁被磁化），但是把磁铁拿掉后，很快将不表现出磁性（退磁）。这种材料的矫顽力小/磁滞回线面基小，所以被磁化后将快速退磁；这种材料一般使用的目的就是增加磁导率，比如做成电感的磁芯可以加大电感量（$L=\mu\cdot S_{磁芯} \cdot N^2/l_{磁路}$）而又不带来大损耗。
    硬磁与软磁
  • 地磁：地球产生的磁场，可以理解地球为永磁体，这里我们并不关注地球的磁性从何而来，只关心地磁场的模型（如下图）。NOTE：在地表测量地磁的磁感应强度范围约是0.4~0.6高斯，地磁场的大磁极轴与地球自转轴有一个约11.5°夹角。
    地磁场

磁物理量

首先是常见的磁感应强度$B$，磁场强度$H$，表征磁介质磁性能力的磁导率$\mu=B/H$。磁感应强度$B$的单位有国际标准单位“特斯拉”(T)，也有常听到的“高斯”(G)，这两个量的关系为：$1T=10^4G$。

可以认为磁场强度是与激励源直接挂钩的量，比如电流$I$与磁强$H$的关系为：

$$
H = \frac {NI}{l_{磁路}}
$$

采用霍尔效应测量磁场（转换为霍尔电压$U$）时，属于直接测量磁感应强度$B$物理量：

$$
U=\frac {I}{neh} B = KB
$$

显然，霍尔器件理论上是具有良好线性测量特性，测量灵敏度为$K$。

地磁解算偏航角

在开始前，我们先声名一下，坐标系设计为如下图所示的“北-东-地”坐标系，这是因为我们要测量的磁感应强度$\vec B$显然是一个从南往北的向量。

北-东-地 坐标系

在校准好磁力计后，可以合理的假设我们测量得到的$\vec B$是较干净的地磁向量，则如下图所示分解为水平分向量$\vec B_{north}$和$\vec B_{down}$，此时我们的机头朝向显然为这里的X轴方向即$\vec x$，所以要得到偏航角可以用向量点乘的方法求解：

$$
\alpha = \arcsin(\frac{\vec B_{north} \cdot \vec x}{\vert \vec B_{north} \vert \cdot \vert \vec x \vert})
$$

偏航解算

所以接下来才是本文的重点——校准磁力计。

电子罗盘校准

首先，建立带干扰的误差模型，上面提到过磁介材料有永磁材料、软磁材料，其中永磁材料相当于给测量结果加上一个偏置$H_1$，软磁材料相当于改变了测量结果与实际量的灵敏度（$S_m$）；除此之外，还有机械安装时的误差矩阵$S_c$（相当于旋转矩阵），制造时的刻度系数（灵敏度）误差矩阵$S_i$，轴间非正交误差矩阵$N$，零点偏移矩阵（$H_0$）。因此将测量结果$\hat B$与实际磁场$H$关系描述为：

$$
\hat B = S_m S_c N S_i H+H_0+H_1
$$

方便进行数学处理，我们可以合理地假定上式中的各个影响量是静态的，因此可以将上式各项合并简化为：

$$
\hat B = K^{-1}H+P
$$

所以我们的目标就是获得上式中的矩阵$K\in \mathbb R^{3\times3}$和偏置向量$P\in \mathbb R^3$，就可以校准测量向量$\hat B$为真实地磁向量$H$：

$$
H = K(\hat B-P)
$$

接下来我们讨论的是椭圆校准法，之所以叫这个是因为测量的数据点云呈现空间椭圆分布并且最后我们拟合的目标函数也是空间椭圆函数。因此，首先要说说为什么要用到椭圆这个形式，观察上式，$\hat B=[b_1,b_2,b_3]^T$的数据展示时相当于$[x,y,z]^T$坐标（实际看起来确实像椭圆），向量$P$就像是这堆数据点的中心（椭圆心），K就类似于这堆数据的空间范围（椭圆半径）。

椭球拟合

考虑到地磁场在某一个地点的磁向量是固定的，不管磁力计再怎么旋转，测量得到的结果$\hat B$应该是表征出同样的地磁向量$H$。因为旋转不改变向量的模长，所以有：

$$
\vert H \vert^2 \equiv | H_{\alpha,\beta,\theta}\vert^2 =
\big( K(\hat B - P) \big)^{T}\cdot K(\hat B-P)
\\
=(\hat B^T-P^T)K^TK(\hat B-P)
\\
= \hat B^T K^TK \hat B - \hat B^T K^TK P - P^T K^TK \hat B+ P^T K^TKP
$$

因为上式中，最后的形式里每一项都有$K^TK$，所以我们把这项用一个新的矩阵$A=K^TK\in R^{3\times 3}$替代，向量长因为是恒定的，我们用常量$s=\vert H \vert ^2$替代：

$$
s=\hat B^T A \hat B - 2\hat B^T A P+ P^T AP,
\\
A = \begin{bmatrix}
a_{11},a_{12},a_{13} \\
a_{12},a_{22},a_{23} \\
a_{13},a_{23},a_{33}
\end{bmatrix}
$$

注意：上式中的$- 2\hat B^T A P$本来是$-\hat B^T A P-P^TA\hat B$，但因为$A$是二次型矩阵所以两个结果是一样的就合并了。

把测量值用空间坐标表示$\hat B = [x,y,z]^T$，带入上式拆开后（有兴趣你可以试一试）：

$$
ax^2+by^2+cz^2+dxy+exz+fyz+gx+hy+iz+l=s
\\
a=a_{11},b=a_{22},c=a_{33},
d=a_{12}+a_{21},\cdots,
l=P^TAP
$$

得到拟合形式后，我们就可以用最小二乘法等数学工具将采样数据点拿去推参数了。但是上式中的实数项要合并一下还有参数项要拆出一个系数2方便后面的关系转换：

$$
ax^2+by^2+cz^2+d2xy+e2xz+f2yz+g2x+h2y+i2z=1
$$

将上式中要拟合的系数和样本数据分别用向量$V\in \mathbb R^{9\times 1}$和非方矩阵$D\in\mathbb R^{N\times 9}$来表示：

$$
DV=1
\\
D=\begin{bmatrix}
x_1^2 & y_1^2 & z_1^2 & 2x_1y_1 & 2x_1z_1 & 2y_1z_1 & 2x_1 & 2y_1 & 2z_1
\\
&&&& \vdots
\\
x_N^2 & y_N^2 & z_N^2 & 2x_Ny_N & 2x_Nz_N & 2y_Nz_N & 2x_N & 2y_N & 2z_N
\end{bmatrix}
\\
V=\begin{bmatrix}
a & b & c & d & e & f & g & h & i
\end{bmatrix}^T
$$

接下来使用非方矩阵的伪逆来求解$V$的最小二乘拟合（见文末）：

$$
D^TDV=D^T\cdot 1
\\
V=(D^TD)^{-1}D^T \cdot 1
$$

上面的“椭球拟合”图片，就是笔者使用AK09915的实际采样数据，用“非方矩阵伪逆法”拟合的参数效果。Matlab画图相关接口：scatter3、fimplicit3

那么拟合得到的系数，如何对应到校准方程中的矩阵系数呢？首先是偏移量向量：

$$
O=A_{3\times3}^{-1} V_{ghi}
\\
A_{3\times3}=\begin{bmatrix}
a & d & e \\
d & b & f \\
e & f & c
\end{bmatrix} ,
V_{ghi}=\begin{bmatrix}
g \\ h \\ i
\end{bmatrix}
$$

接着求椭圆旋转矩阵，先求一个中间辅助矩阵：

$$
B_{4\times4}=T A_{4\times 4}T^T
\\
T = \begin{bmatrix}
1 &0 &0 &0 \\
0 &1 &0 &0 \\
0 &0 &1 &0 \\
Ox & Oy & Oz & 1
\end{bmatrix},
A_{4\times 4}=\begin{bmatrix}
a & d & e & g \\
d & b & f & h \\
e & f & c & i \\
g & h & i & -1
\end{bmatrix}
$$

然后把这个辅助矩阵$B_{4\times4}$进行分块来求解另一个辅助矩阵：

$$
B_{4\times4} = \begin{bmatrix}
B_{3\times3-core} & N_{3\times 1}
\\
M_{1\times3} & b_{44}
\end{bmatrix}
\\
B_{3\times3}=\frac{1}{-b_{44}} B_{3\times3-core}
$$

对$B_{3\times3}$求解出3个特征值$\lambda_i$和3个特征向量$v_i$。则可以用特征值构造轴增益（椭圆半径）：

$$
G=[\frac{1}{\sqrt{\lambda_1}},\frac{1}{\sqrt{\lambda_2}},\frac{1}{\sqrt{\lambda_3}}]^T
$$

用特征向量构造旋转矩阵：

$$
R_{3\times3}=\begin{bmatrix}
v_1 & v_2 & v_3
\end{bmatrix}
$$

最终，我们就可以对读取到的传感器采样数据$\hat B$进行校准了：

$$
\hat B_{withoutBias} = \hat B + O
\\
\hat B_{invRot} = R^{-1} \hat B_{withoutBias}
\\
\hat B_{pred} = \hat B_{invRot} / G
$$

上面三步依次为：去偏移量、反旋转、去增益。

电子罗盘使用实战

这一节主要有两个部分，分别是：

  1. AK09915底层驱动程序：分享了从驱动一颗电子罗盘芯片到获得采样数据的全流程代码；
  2. 椭球矫正算法：接着在该部分中，分享了对采样数据进行分析得出干扰补偿数据的算法实现，并给出了矫正效果对比图；
  3. 矫正电子罗盘并求解航向角：最后这个部分，将矫正算法用C实现部署到了实际嵌入式硬件上，并求解最终目标——航向角。

AK09915底层驱动程序

AK09915寄存器表

上图是这个电子罗盘的寄存器表，可以看出这个器件可以
配置的寄存器起始就三个：CNTL1、CNTL2、CNTL3。配置完成后，读取HXL~HZH的采样数据寄存器就可以了。初始化代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
/**
  * @brief  设置工作模式
  * @note   无
  * @param  targetMode: 要进入的工作模式
  * @retval None
  */
void AK09915_setOpMode(AK09915_operationMode_t targetMode)
{
	/*保护其他位（FIFO，SDR）不被修改*/
	uint8_t preStatus;
	AK09915_IIC_ReadRegs(MAGREG_CONTROL2, &preStatus, 1);
	
	preStatus &= 0xE0;  // 1110 0000，清除MODE{4,0}位，变成PowerDown模式
	
	// 进入Power Down 模式
	AK09915_IIC_WriteRegs(MAGREG_CONTROL2, preStatus);
	
	// 等待1ms
	HAL_Delay(1);
	
	// 进入目标模式
	if(targetMode != AK09915_operationMode_PowerDown)
	{
		preStatus |= targetMode;
		AK09915_IIC_WriteRegs(MAGREG_CONTROL2, preStatus);
	}
	
	return;
}

/**
  * @brief  设备初始化
  * @note   使用前需初始化好IIC接口
  * @param  None
  * @retval None
  */
void AK09915_initDev(void)
{
	/***
	 * 配置 CNTL1 和 CNTL2 寄存器
	 * 使能噪声抑制滤波器，关闭FIFO，低噪声模式，200Hz采样率
	****/
	uint8_t ctrlRegs[2] = { AK09915_NSF_EN, 
		                      AK09915_FIFO_DIS | AK09915_SDR_LowNoise | AK09915_operationMode_ContinueMode1_10Hz
	                      };
	
	// 设备复位
	AK09915_resetDev_HardMethod();
	
	// 进入空闲模式
	AK09915_setOpMode(AK09915_operationMode_PowerDown);
	
	// 配置参数
	AK09915_IIC_WriteRegs(MAGREG_CONTROL1,ctrlRegs[0]);
	AK09915_IIC_WriteRegs(MAGREG_CONTROL2,ctrlRegs[1]);
												
	// 读出参数，校验一下
//	HAL_Delay(1);
//	AK09915_IIC_ReadRegs(MAGREG_CONTROL1, ctrlRegs,2);
}

上面的代码，把AK09915配置成了DRDY引脚输出有效数据，因此MCU可以使用外部GPIO中断来读数据：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
/**
  * @brief  中断解析回调函数
  * @note   在MCU中断引脚对应的EXTI函数中调用
  *         开启测量后自动生成中断
  * @param  None
  * @retval None
  */
void AK09915_callback_interrupt(void)
{
	uint8_t statusCache;
	
	// - 读状态寄存器1
	AK09915_IIC_ReadRegs(MAGREG_STATUS1, &statusCache, 1);
	
	// - 检验是否有数据，NOTE：这里优化去掉，因为是中断模式
//	if (statusCache & MAGREGMASK_CTRL1_DRDY)
//	{
//		/*有数据，读数据*/
//	}
	
	// - 检验是否有数据遗漏
	if(statusCache & MAGREGMASK_CTRL1_DOR)
	{
		/*处理数据遗漏问题*/
	}
	
	// - 读取数据
	AK09915_IIC_ContinueReadRegs((uint8_t *)&ak09915_rawRegDataCache, sizeof(ak09915_rawRegDataCache));
	
	// - 读状态寄存器2
	AK09915_IIC_ContinueReadRegs(&statusCache, 1);	// 一个Dummy
	AK09915_IIC_ContinueReadRegs(&statusCache, 1);
	
	// - 判断数据是否溢出（如果总磁强大于4912uT，芯片判为溢出）
	if(statusCache & MAGREGMASK_CTRL2_HOFL)
	{
		/*处理磁强爆表问题*/
	}
	
	// - 判断数据有效与否（仅用于FIFO模式，标志FIFO缓冲区过读）
	#ifdef _USER_USE_AK09915_FIFO_
	if(statusCache & MAGREGMASK_CTRL2_INV)
	{
		/*处理FIFO读到无效数据*/
	}
	#endif
}

最后给出上面用到的底层通信接口，读者可以根据需要优化成DMA和操作系统信号量的形式：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
/******************************************************************
**  通信封装
*******************************************************************/

/**
  * @brief  向指定寄存器写入数据
  * @note   一次只写一个数据！
  * @param  regAddr: 目标寄存器起始地址
  * @param  Data:   写入的数据
  * @retval None
  */
void AK09915_IIC_WriteRegs(uint8_t regAddr, uint8_t Data)
{
	uint8_t ak09915_transCache[2];
	
	ak09915_transCache[0] = regAddr;	// 寄存器地址
	ak09915_transCache[1] = Data;
	HAL_I2C_Master_Transmit(&AK09915DRV_IIC_HANDLER,	/*HAL IIC Handler*/
		                         AK09915DRV_IIC_ADDR,   /*AK09915的IIC地址*/
	                           ak09915_transCache, 2, /*传输的数据和长度*/
	                           0xff);   /*超时阈值*/
}

/**
  * @brief  从指定寄存器读取数据
  * @note   支持连续读一串数据，地址自增
  * @param  regAddr: 目标寄存器起始地址
  * @param  pData:   读取数据存放位置
  * @param  len  :   读数据个数
  * @retval None
  */
void AK09915_IIC_ReadRegs(uint8_t regAddr, uint8_t * pData, uint8_t len)
{
	uint8_t ak09915_transCache;
	
	ak09915_transCache = regAddr;	// 寄存器地址
	HAL_I2C_Master_Transmit(&AK09915DRV_IIC_HANDLER,	/*HAL IIC Handler*/
		                         AK09915DRV_IIC_ADDR,   /*AK09915的IIC地址*/
	                           &ak09915_transCache, 1, /*传输的数据和长度*/
	                           0xff);   /*超时阈值*/
	
	/*- 收数据 -*/
	HAL_I2C_Master_Receive(&AK09915DRV_IIC_HANDLER,	/*HAL IIC Handler*/
		                         AK09915DRV_IIC_ADDR,   /*AK09915的IIC地址*/
	                           pData, len, /*传输的数据和长度*/
	                           0xff);   /*超时阈值*/
}

/**
  * @brief  从上一次读取后继续读取
  * @note   小心越界循环读取！
  * @param  pData:   读取数据存放位置
  * @param  len  :   读数据个数
  * @retval None
  */
void AK09915_IIC_ContinueReadRegs(uint8_t * pData, uint8_t len)
{
	/*- 收数据 -*/
	HAL_I2C_Master_Receive(&AK09915DRV_IIC_HANDLER,	/*HAL IIC Handler*/
		                         AK09915DRV_IIC_ADDR,   /*AK09915的IIC地址*/
	                           pData, len, /*传输的数据和长度*/
	                           0xff);   /*超时阈值*/
}

/******************************************************************
**  基础功能
*******************************************************************/

/**
  * @brief  硬件复位AK09915设备
  * @note   提前配置RSTN引脚
  * @param  None
  * @retval None
  */
void AK09915_resetDev_HardMethod(void)
{
	// 拉低RSTN引脚
	HAL_GPIO_WritePin(MAG_RSTN_GPIO_Port, MAG_RSTN_Pin,
	                  GPIO_PIN_RESET);
	
	// 等待1ms
	HAL_Delay(1);
	
	// 拉高RSTN引脚
	HAL_GPIO_WritePin(MAG_RSTN_GPIO_Port, MAG_RSTN_Pin,
	                  GPIO_PIN_SET);
	
}

/**
  * @brief  读取设备号通信测试
  * @note   检验 WHOIAM 寄存器数据
  * @param  None
  * @retval 0正确；1通信错误；2数值错误
  */
uint32_t AK09915_communicateTest(void)
{
	uint8_t ak09915_testCache[2];
	
	AK09915_resetDev_HardMethod();	// 复位并使能设备
	
	
	// 发地址
	ak09915_testCache[0] = MAGREG_WHOIAM1_COMPANYID;
	if(HAL_I2C_Master_Transmit(&AK09915DRV_IIC_HANDLER,
		                         AK09915DRV_IIC_ADDR,
	                           ak09915_testCache, 1,
	                           0xff) \
	     != HAL_OK)
	{
		return 1;	// 通信错误
	}
	
	// 读取WHOAMI1和WHOAMI2数据
	if (HAL_I2C_Master_Receive(&AK09915DRV_IIC_HANDLER,
		                         AK09915DRV_IIC_ADDR,
	                           ak09915_testCache, 2,
	                           0xff) \
	     != HAL_OK)
	{
		return 1;	// 通信错误
	}
	
	// 检验正确性
	if(ak09915_testCache[0] == MAGREGVAL_WHOIAM1_COMPANYID \
		 && \
	   ak09915_testCache[1] == MAGREGVAL_WHOIAM2_DEVICEID)
	{
		return 0;	// 正确
	}
	else
		return 2;	// 数值错误
}

/**
  * @brief  获取地磁数据
  * @note   原始16位有符号数据
  * @param  pRes : 存放读取数据的位置
  * @retval None
  */
void AK09915_getRawData(AK09915_rawRegData_t * pRes)
{
	AK09915_IIC_ReadRegs(MAGREG_X_L, (uint8_t *)pRes, sizeof(AK09915_rawRegData_t));
}

必要类型与变量的声名：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
#ifndef __AK09915_DEFS_H__
#define __AK09915_DEFS_H__

#include "stdint.h"

/******************************************************************
**  数据格式
*******************************************************************/

typedef struct
{
	union{
		uint8_t L_H_bytes[2];
		int16_t getSigned;
	} HX;
	
	union{
		uint8_t L_H_bytes[2];
		int16_t getSigned;
	} HY;
	
	union{
		uint8_t L_H_bytes[2];
		int16_t getSigned;
	} HZ;
	
} AK09915_rawRegData_t;

/******************************************************************
**  寄存器地址表
*******************************************************************/

#define MAGREG_WHOIAM1_COMPANYID  0x00	// AK公司ID
#define MAGREG_WHOIAM2_DEVICEID   0x01	// 设备ID
#define MAGREG_STATUS1            0x10	// 状态：数据准备、数据覆盖
#define MAGREG_X_L                0x11
#define MAGREG_X_H                0x12
#define MAGREG_Y_L                0x13
#define MAGREG_Y_H                0x14
#define MAGREG_Z_L                0x15
#define MAGREG_Z_H                0x16
#define MAGREG_STATUS2            0x18	// 状态：磁量溢出、无效数据
#define MAGREG_CONTROL1           0x30	// 配置：NSF使能、FIFO的阈值
#define MAGREG_CONTROL2           0x31	// 配置：工作模式、功耗模式、FIFO使能
#define MAGREG_CONTROL3           0x32	// 配置：软件复位

/******************************************************************
**  寄存器内容
*******************************************************************/

#define MAGREGVAL_WHOIAM1_COMPANYID  (0x48)
#define MAGREGVAL_WHOIAM2_DEVICEID   (0x10)

/**
  * @reg MAGREG_STATUS1
***/

#define MAGREGMASK_CTRL1_DRDY 0x01	// 数据准备好了
#define MAGREGMASK_CTRL1_DOR  0x02	// 数据覆盖
#define MAGREGMASK_CTRL1_HSM  0x80	// 高速IIC 模式

/**
  * @reg MAGREG_STATUS2
***/

#define MAGREGMASK_CTRL2_INV  0x04	// 无效数据
#define MAGREGMASK_CTRL2_HOFL 0x08	// 数据溢出

/**
  * @reg MAGREG_CONTROL1
***/

typedef enum
{
	AK09915_NSF_EN  = (0x01<<5),
	AK09915_NSF_DIS = (0x00<<5),
} AK09915_NSF_t;

/**
  * @reg MAGREG_CONTROL2
***/

typedef enum
{
	AK09915_operationMode_PowerDown          = 0x0,
	AK09915_operationMode_SingleMeasure      = 0x1,
	AK09915_operationMode_ContinueMode1_10Hz = 0x2,
	AK09915_operationMode_ContinueMode2_20Hz = 0x4,
	AK09915_operationMode_ContinueMode3_50Hz = 0x6,
	AK09915_operationMode_ContinueMode4_100Hz= 0x8,
	AK09915_operationMode_ContinueMode5_200Hz= 0xA,
	AK09915_operationMode_ContinueMode6_1Hz  = 0xC,
	AK09915_operationMode_SelfTest          = 0x10,
} AK09915_operationMode_t;

typedef enum{
	AK09915_SDR_LowPower = (0x0<<6),
	AK09915_SDR_LowNoise = (0x1<<6),
} AK09915_SDR_t;

typedef enum
{
	AK09915_FIFO_EN  = (0x01<<7),
	AK09915_FIFO_DIS = (0x00<<7),
} AK09915_FIFO_t;

/**
  * @reg MAGREG_CONTROL3
***/

typedef enum
{
	AK09915_SRST_EN  = 1,
	AK09915_SRST_DIS = 0,
} AK09915_SRST_t;

#endif

椭球矫正算法

矫正效果

为了方便算法的验证，这里使用Matlab作为主要编程工具，上图就是对原始采样数据矫正前后的效果对比（显然矫正前后椭球的中心补偿到了零点，椭球的轴长比接近1：1也就是圆球）。当然也可以使用Python或者纯C实现（最后部署时还得是C），但是Py和C在矫正效果展示方面（3D绘图）实在太困难了，没有Matlab方便。以下代码即可在Matlab上直接实现原始数据的分析、矫正、数据展示：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
%% 配置工作条件 ----------------------------------------------------------

% 存储电子罗盘采样数据的文件名
compassDataFileName = "ak09915.csv";
% 显示图像的方式：1. 子图形式， 2. 分别显示
SHOW_FIG_PATTERN_METHOD = 1;

%% 读取采样数据 -----------------------------------------------------------

% 自动解析CSV文件，自动识别并去掉表头，返回矩阵类型
elecCompassDataTable = readmatrix(compassDataFileName);

% 提取3个轴向数据向量
xVector = elecCompassDataTable(:,1);
yVector = elecCompassDataTable(:,2);
zVector = elecCompassDataTable(:,3);

%% 求解标准椭球方程的9个参数 -----------------------------------------------

epis9Param = fitEpisode9Param(xVector,yVector,zVector);

%% 显示原始数据与拟合效果 --------------------------------------------------
figure
if SHOW_FIG_PATTERN_METHOD == 1
    subplot(1,2,1)
end

% 计算3D显示的数据范围
intv = [min(xVector) max(xVector) min(yVector) max(yVector) min(zVector) max(zVector)];
intv = 1.2 * intv;  % 比顶端数据范围大20%
% 标准9参数椭球方程
f = @(x,y,z) epis9Param(1)*x.^2  + epis9Param(2)* y.^2 + epis9Param(3)* z.^2 + ...
             epis9Param(4)*2*x*y + epis9Param(5)*2*x*z + epis9Param(6)*2*y*z + ...
             epis9Param(7)*2*x   + epis9Param(8)*2*y   + epis9Param(9)*2*z  -1;
% 绘制椭球面
fimplicit3(f,intv)

% 添加原始采样数据散点
hold on
scatter3(xVector,yVector,zVector)
% 添加标题和轴标
title("电子罗盘采样数据-非方矩阵的伪逆法拟合效果")
xlabel("x")
ylabel("y")
zlabel("z")

%% 求解校准矩阵------------------------------------------------------------

% 中心偏移向量
A3x3 = [ epis9Param(1) epis9Param(4) epis9Param(5) ;
         epis9Param(4) epis9Param(2) epis9Param(6) ;
         epis9Param(5) epis9Param(6) epis9Param(3) ];
Vghi = [ epis9Param(7); epis9Param(8); epis9Param(9) ];
CentorOffVec = inv(A3x3)*Vghi;

% 旋转矩阵
T = [        1             0              0           0
             0             1              0           0
             0             0              1           0
      CentorOffVec(1) CentorOffVec(2) CentorOffVec(3) 1];
A4x4 = [A3x3  Vghi
        Vghi'  -1 ];
B4x4 = T * A4x4 * T' ;
B3x3 = B4x4(1:3,1:3) / (-B4x4(4,4));
[eigVector,eigLambda] = eig(B3x3);   % 分解特征向量和特征值

invRot = inv(eigVector);    % 反旋转矩阵
invGainVector = [eigLambda(1,1); eigLambda(2,2); eigLambda(3,3)];
invGainVector = 1 ./ sqrt(invGainVector); % 各轴增益补偿矩阵

%% 对原本采样的数据进行矫正-------------------------------------------------

xyzRectData = [xVector, yVector, zVector]; % 3轴数据组合起来更快
xyzRectData = xyzRectData + CentorOffVec'; % 球心偏置矫正
xyzRectData = invRot * xyzRectData';
xyzRectData = xyzRectData'; % 旋转矫正
xyzRectData = xyzRectData ./ invGainVector';

%% 显示矫正后的数据---------------------------------------------------------
if SHOW_FIG_PATTERN_METHOD == 1
    subplot(1,2,2);
else
    if SHOW_FIG_PATTERN_METHOD == 2
        figure;
    end
end

xRectVector = xyzRectData(:,1);
yRectVector = xyzRectData(:,2);
zRectVector = xyzRectData(:,3);

% 再按照9参数椭球标准拟合一次
episRect9Param = fitEpisode9Param(xRectVector,yRectVector,zRectVector);
% 计算3D显示的数据范围
intv = [min(xRectVector) max(xRectVector) min(yRectVector) max(yRectVector) min(zRectVector) max(zRectVector)];
intv = 1.2 * intv;  % 比顶端数据范围大20%
% 标准9参数椭球方程
rectF = @(x,y,z) episRect9Param(1)*x.^2  + episRect9Param(2)* y.^2 + episRect9Param(3)* z.^2 + ...
             episRect9Param(4)*2*x*y + episRect9Param(5)*2*x*z + episRect9Param(6)*2*y*z + ...
             episRect9Param(7)*2*x   + episRect9Param(8)*2*y   + episRect9Param(9)*2*z  -1;
% 绘制椭球面
fimplicit3(rectF,intv)

% 添加原始采样数据散点
hold on
scatter3(xRectVector,yRectVector,zRectVector)
% 添加标题和轴标
title("电子罗盘矫正数据")
xlabel("x")
ylabel("y")
zlabel("z")

其中，对椭球表面的采样散点数据进行最小二乘拟合参数的函数fitEpisode9Param实现很简单，就是上面的非方矩阵的伪逆公式的代码形式：

1
2
3
4
5
6
7
function [fitParamVector] = fitEpisode9Param(xVector,yVector,zVector)
    D = [ xVector.*xVector, yVector.*yVector, zVector.*zVector, ...
          2*xVector.*yVector, 2*xVector.*zVector, 2*yVector.*zVector, ...
          2*xVector, 2*yVector, 2*zVector];
    
    fitParamVector = inv(D' * D) * D' * ones(length(xVector),1);
end

矫正电子罗盘并求解航向角（嵌入式）

最后一步了，就是把矫正部署到实际硬件上，并且根据得到的地磁向量直接求解出航向角。由于上面已经多次讨论并且展示了矫正算法，所以我们可以很容易地用C语言实现如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
#include "math.h"

/**
  * @brief  椭球矫正算法
  * @note   输入参数是标准9参数转换后的矩阵形式
  * @param  pSrc : 磁传感数据存放的位置
  * @retval None
  */
void episodeRectify(mag_rawData_t * pSrc)
{
	float EpiCmptCache[3];
	
	// 中心偏移 B_{noBias} = B_{samp}-Offset  注意这里补偿的正负号
	pSrc->compass[0] = pSrc->magX + pSrc->centerOffset[0];
	pSrc->compass[1] = pSrc->magY + pSrc->centerOffset[1];
	pSrc->compass[2] = pSrc->magZ + pSrc->centerOffset[2];
	
	// 反旋转   B_{noRot} = invRot * B_{noBias}
	for (int i =0; i<3; i++)	// 逐行求
	{
		EpiCmptCache[i] = pSrc->invRot[i][0] * pSrc->compass[0] + \
		                  pSrc->invRot[i][1] * pSrc->compass[1] + \
		                  pSrc->invRot[i][2] * pSrc->compass[2];	// 矩阵乘法
	}
	
	// 轴长补偿  注意：这里使用乘法（因为比除法更快），所以加载补偿量时应该是 1/轴长
	for (int i =0; i<3; i++)
	{
		pSrc->compass[i] = EpiCmptCache[i] * pSrc->gain[i];
	}
}

/**
  * @brief  解算航向角
  * @note   若严重非水平，应当传入旋转补偿后的磁向量
  * @param  compass3Axis : x,y,z轴的磁向量数据
  * @retval yaw 偏航角
  */
float getYaw_FromCompass(float compass3Axis[3])
{
	/**                                    Y      */
	/* 水平情况下偏航角求解公式 ： arctan( ------)  */
	/*                                     X      */
	return atan2( compass3Axis[1] , compass3Axis[0] );	// 参见 math.h
}

最后简单点一下上面实现中的一些小技巧。首先，矫正算法中的旋转补偿这里在加载参数的时候就以$R^{-1}$的形式，这样可以节省非常多的计算量；接着，在各轴向补偿上，原本是$/G$，但是在硬件上浮点除法是比乘法要难算的，所以这里我们在加载参数的时候就以$1/G$的形式加载，将除法换成了乘法；最后，求航向角使用的是ARM的数学库，推荐读者在使用不同处理器时进行相应的优化（如果有的话）。

参考链接

Donate
  • Copyright： Copyright is owned by the author. For commercial reprints, please contact the author for authorization. For non-commercial reprints, please indicate the source.
  • Copyrights © 2022-2024 RY.J
  • Visitors: | Views:

请我喝杯咖啡吧~

支付宝
微信