當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

倾斜补偿的电子罗盘(1)：地磁场，磁传感器，倾斜补偿

發布時間：2023/12/31 编程问答 31 豆豆

生活随笔收集整理的這篇文章主要介紹了倾斜补偿的电子罗盘(1)：地磁场，磁传感器，倾斜补偿小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

傾斜補償的電子羅盤(1)：地磁場，磁傳感器，傾斜補償

地磁場和磁傳感器

地磁場可以用于獲取方位信息。以北半球為例，地磁場方向不是與地面水平，而是與水平方向有一定的傾角（指向地面），稱為磁傾角（Inclination)。同時，地磁場的方向也與地理的北方不同，兩者的夾角稱為磁偏角（Declination)。地磁場的磁感應強度大約在30~70uT，與所在地有關。

例如，根據下面這個網站，上海的磁偏角約為-6°，磁傾角約為47°，磁感應強度49.1uT（水平方向33uT，垂直方向36uT）。

World Magnetic Model Calculator (bgs.ac.uk)

（在后面的介紹中忽略磁傾角，假設磁北就是地理北。一般可以通過當地經緯度查詢到磁偏角并加以換算）

地磁傳感器，一般有XY兩軸或者XYZ三軸，大部分基于霍爾效應或是磁阻效應，用于手機、無人機等，實現電子羅盤功能。例如，兩軸磁傳感器可以通過測量XY兩個方向上的磁感應強度，通過簡單的計算獲得設備的朝向。假設磁傳感器的XY平面保持水平，X軸讀數hx，Y軸讀數hy，則設備的X軸方向與磁北的夾角為 $θ=atan2(hyhx)\theta=atan2(\frac{h_y}{h_x})$ ，θ范圍是(-180°,180°]。

hxhyθ

+	0	0
+	+	(0,90°)
0	+	90°
-	+	(90°,180°)
-	0	180°
-	-	(-180°,-90°)
0	-	-90°
+	-	(-90°,0)

2D和3D的旋轉

某個位置地磁場的方向是相對恒定的，而磁傳感器的姿態卻是變化的，因此磁傳感器每個方向的讀數與姿態有關。如下圖，對于MPU-6500，磁傳感器的姿態可以使用繞著XYZ三個軸的旋轉來表示，以逆時針旋轉（從x軸的箭頭看箭尾）為正。

例如，繞著Z軸旋轉，Z軸讀數不變，僅改變X軸和Y軸的讀數。

原讀數：
$v0=[hx,hy]T=[Hcosθ,Hsinθ]Tv0=[h_x,h_y]^T=[Hcos\theta, Hsin\theta]^T$
根據示意圖，XY逆時針旋轉 $δ\delta$ 后的讀數：
$v′=[hx′,hy′]T=[Hcos(θ?δ),Hsin(θ?δ)]T=[Hcosθcosδ+Hsinθsinδ,Hsinθcosδ?Hcosθsinδ]v'=[h_x',h_y']^T=[Hcos(\theta - \delta),Hsin(\theta - \delta)]^T=[Hcos\theta cos\delta+Hsin\theta sin\delta, Hsin\theta cos\delta - Hcos\theta sin\delta]$
這種旋轉可以用矩陣表示：
$\left[ \begin{matrix} cos\delta & sin\delta \\ -sin\delta & cos\delta \end{matrix} \right] \left[ \begin{matrix} Hcos\theta \\ Hsin\theta \end{matrix} \right] = R_{z2d}(\delta)v_0$

然后把Z軸補上，變成一個3x3的矩陣（因為Z軸讀數不變，所以只有3,3的元素是1，其他都是0）：
$Rz(ψ)=[cosψsinψ0?sinψcosψ0001]R_z(\psi)= \left[ \begin{matrix} cos\psi & sin\psi & 0\\ -sin\psi & cos\psi & 0 \\ 0 & 0 & 1 \end{matrix} \right]$
同理，圍繞X軸和Y軸旋轉，對應的矩陣為：
$Rx(?)=[1000cos?sin?0?sin?cos?],Ry(θ)=[cosθ0?sinθ010sinθ0cosθ]R_x(\phi)= \left[ \begin{matrix} 1 & 0 & 0 \\ 0 & cos\phi & sin\phi\\ 0 & -sin\phi & cos\phi \end{matrix} \right], \; R_y(\theta)= \left[ \begin{matrix} cos\theta & 0 & -sin\theta \\ 0 & 1 & 0 \\ sin\theta & 0 & cos\theta \end{matrix} \right]$

注意 $Ry(θ)R_y(\theta)$ 的 $sinθsin\theta$ 符號與其他兩個矩陣不同，這與坐標軸設置有關，這里對應于上圖MPU6500。

對于三軸磁傳感器，定義一個初始位置：XY平面平行于水平面，同時X軸與地磁場方向重合。此時讀數為： $h_0=[HcosI,0,HsinI]^T$ 。I是磁傾角。

如果只有Z軸的旋轉，則讀數變為：
$hrz=Rz(ψ)h0=hrz=Rz(ψ)h0=[cosψsinψ0?sinψcosψ0001][HcosI0HsinI]=[HcosIcosψ?HcosIsinψHsinI]h_{rz}=R_z(\psi)h_0= h_{rz}=R_z(\psi)h_0= \left[ \begin{matrix} cos\psi & sin\psi & 0\\ -sin\psi & cos\psi & 0 \\ 0 & 0 & 1 \end{matrix} \right] \left[ \begin{matrix} HcosI\\ 0 \\ HsinI \end{matrix} \right] = \left[ \begin{matrix} HcosIcos\psi \\ -HcosIsin\psi \\ HsinI \end{matrix} \right]$
此時可以直接用 $ψ=atan(?hyhx)\psi = atan(\frac{-h_y}{h_x})$ 獲得方位角。

如果XYZ軸都有旋轉，則讀數變為：
$hrxyz=Rx(?)Ry(θ)Rz(ψ)h0=Rx(?)Ry(θ)hrzh_{rxyz}=R_x(\phi)R_y(\theta)R_z(\psi)h_0=R_x(\phi)R_y(\theta)h_{rz}$
此時需要進行傾斜補償，即基于讀數 $h_{rxyz}$ ，還原為僅有Z軸旋轉的讀數 $h_{rz}$ ，然后同樣可以獲得方位角。

傾斜補償

原理

進行傾斜補償，需要對X軸和Y軸反向轉動，按之前的記號，X軸轉動 $(??)(-\phi)$ ，Y軸轉動 $(?θ)(-\theta)$ ：
$hrz=[hx0,hy0,hz0]=[hx,hy,hz]=Ry(?θ)Rx(??)hrxyz=[cosθ0sinθ010?sinθ0cosθ][1000cos??sin?0sin?cos?]hrxyz=[cosθsinθsin?sinθcos?0cos??sin??sinθcosθsin?cosθcos?][hxhyhz]h_{rz} =[h_{x0},h_{y0},h_{z0}]= [h_x,h_y,h_z]=R_y(-\theta)R_x(-\phi)h_{rxyz} \\ = \left[ \begin{matrix} cos\theta & 0 & sin\theta \\ 0 & 1 & 0 \\ -sin\theta & 0 & cos\theta \end{matrix} \right] \left[ \begin{matrix} 1 & 0 & 0 \\ 0 & cos\phi & -sin\phi\\ 0 & sin\phi & cos\phi \end{matrix} \right] h_{rxyz} \\ = \left[ \begin{matrix} cos\theta & sin\theta sin\phi & sin\theta cos\phi \\ 0 & cos\phi & -sin\phi\\ -sin\theta & cos\theta sin\phi & cos\theta cos\phi \end{matrix} \right] \left[ \begin{matrix} h_x \\ h_y \\ h_z \end{matrix} \right]$
其中， $h_x,h_y,h_z]^T$ 是實際讀數。

因此，考慮傾斜補償后，方位角變為：
$ψ=atan(?hy0hx0)=atan(hysin??hxcos?hxcosθ+hysinθsin?+hzsinθcos?)\psi = atan\left( \frac{-h_{y0}}{h_{x0}}\right)=atan \left( \frac{ h_y sin\phi- h_xcos\phi}{h_xcos\theta + h_ysin\theta sin\phi + h_z sin\theta cos\phi} \right)$
所以，基于當前 $?\phi$ 和 $θ\theta$ 的信息，就可以進行傾斜補償。

使用加速度傳感器獲得角度信息

根據三軸加速度傳感器的讀數可以獲得所需的角度信息。注意在讀數時，加速度傳感器沒有其他方向上的加速，只受到重力影響。

假設初始狀態下，加速度傳感器的XY軸平行于水平面，Z軸與重力方向相同，則初始狀態的讀數： $a_0=[0,0,g]^T$

可以看出，圍繞Z軸旋轉對讀數沒有影響，即： $Rz(ψ)a0=a0R_z(\psi)a_0=a_0$

在分別圍繞XY兩軸旋轉后，
$arxyz=Rx(?)Ry(θ)Rz(ψ)a0=Rx(?)Ry(θ)a0a_{rxyz}=R_x(\phi)R_y(\theta)R_z(\psi)a_0=R_x(\phi)R_y(\theta)a_{0}$

同樣，在XY兩軸反向旋轉后，理論上，讀數應和初始位置的讀數相同：
$Ry(?θ)Rx(??)arxyz=[cosθsinθsin?sinθcos?0cos??sin??sinθcosθsin?cosθcos?][axayaz]=[00g]R_y(-\theta)R_x(-\phi)a_{rxyz} = \left[ \begin{matrix} cos\theta & sin\theta sin\phi & sin\theta cos\phi \\ 0 & cos\phi & -sin\phi\\ -sin\theta & cos\theta sin\phi & cos\theta cos\phi \end{matrix} \right] \left[ \begin{matrix} a_x\\ a_y \\ a_z \end{matrix} \right] =\left[ \begin{matrix} 0 \\ 0 \\ g \end{matrix} \right]$

因此有：
$axcosθ+aysinθsin?+azsinθcos?=0aycos??azsin?=0a_xcos\theta + a_ysin\theta sin\phi +a_z sin\theta cos\phi =0 \\ a_ycos \phi - a_z sin\phi = 0$
從而計算兩個角度：
$?=atan(ayaz)θ=atan(?axaysin?+azcos?)\phi = atan\left( \frac{a_y}{a_z}\right) \\ \theta = atan \left( \frac{-a_x}{a_ysin\phi + a_zcos\phi}\right)$

總結

地磁場和磁傳感器
電子羅盤基本原理
通過加速度傳感器獲得角度信息，進行傾斜補償

目前是假設這些傳感器的測量值是完全準確的。但實際上會有各種測量誤差，需要進行校準，后面繼續補充。

參考資料

NXP的應用手冊AN4246、AN4247、AN4248，解釋得比較清楚（https://www.nxp.com/docs/en/application-note/AN4248.pdf）
很多三軸（磁、加速度、陀螺儀）、六軸（一般是加速度+陀螺儀，少量加速度+磁）、九軸（磁+加速度+陀螺儀）的傳感器，可以看下應用手冊

例如，ISM303DAC集成了三軸加速度傳感器和三軸磁傳感器，便于實現電子羅盤。

總結

以上是生活随笔為你收集整理的倾斜补偿的电子罗盘(1)：地磁场，磁传感器，倾斜补偿的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。

上一篇：侧信道攻击之模板攻击
下一篇： ubuntu dock栏添加应用启动图标