坐标变换
- 手机初始状态accelerometer 与 magnetometer 读数为
(假定初始状态为水平放置,如上图所示)
G1=⎡⎣⎢ax1ay1az1⎤⎦⎥=⎡⎣⎢00g⎤⎦⎥(1)(1)G1=[ax1ay1az1]=[00g]
M1=⎡⎣⎢mx1my1mz1⎤⎦⎥=⎡⎣⎢cos(δ)0sin(δ)⎤⎦⎥(2)(2)M1=[mx1my1mz1]=[cos(δ)0sin(δ)] - 依上图坐标系,手机绕x,y,zx,y,z轴旋转的变换矩阵为
Cx(θ)=⎡⎣⎢1000cos(θ)sin(θ)0−sin(θ)cos(θ)⎤⎦⎥(3)(3)Cx(θ)=[1000cos(θ)−sin(θ)0sin(θ)cos(θ)]
Cy(ϕ)=⎡⎣⎢cos(ϕ)0−sin(ϕ)010sin(ϕ)0cos(ϕ)⎤⎦⎥(4)(4)Cy(ϕ)=[cos(ϕ)0sin(ϕ)010−sin(ϕ)0cos(ϕ)]
Cz(ψ)=⎡⎣⎢cos(ψ)sin(ψ)0−sin(ψ)cos(ψ)0001⎤⎦⎥(5)(5)Cz(ψ)=[cos(ψ)−sin(ψ)0sin(ψ)cos(ψ)0001]
姿态角计算
- 下列计算均假设acclerometer读数基本稳定,即手机近似静止,可对加速度数据做低通滤波处理.
- 旋转过后accelerometer与magnetometer读数为(基于机体坐标系,旋转次序 yaw->pitch ->roll)
G2=⎡⎣⎢ax2ay2az2⎤⎦⎥=Cy(ϕ)Cx(θ)Cz(ψ)G1=Cy(ϕ)Cx(θ)Cz(ψ)⎡⎣⎢00g⎤⎦⎥(6)(6)G2=[ax2ay2az2]=Cy(ϕ)Cx(θ)Cz(ψ)G1=Cy(ϕ)Cx(θ)Cz(ψ)[00g]
M2=⎡⎣⎢mx2my2mz2⎤⎦⎥=Cy(ϕ)Cx(θ)Cz(ψ)M1=Cy(ϕ)Cx(θ)Cz(ψ)⎡⎣⎢cos(δ)0sin(δ)⎤⎦⎥(7)(7)M2=[mx2my2mz2]=Cy(ϕ)Cx(θ)Cz(ψ)M1=Cy(ϕ)Cx(θ)Cz(ψ)[cos(δ)0sin(δ)] 计算翻滚角ϕϕ
由公式(6)可得,
G2=⎡⎣⎢ax2ay2az2⎤⎦⎥=Cy(ϕ)Cx(θ)Cy(ψ)G1=Cy(ϕ)Cx(θ)Cy(ψ)⎡⎣⎢00g⎤⎦⎥=⎡⎣⎢cos(ϕ)0−sin(ϕ)010sin(ϕ)0cos(ϕ)⎤⎦⎥⎡⎣⎢1000cos(θ)sin(θ)0−sin(θ)cos(θ)⎤⎦⎥⎡⎣⎢cos(ψ)sin(ψ)0−sin(ψ)cos(ψ)0001⎤⎦⎥⎡⎣⎢00g⎤⎦⎥=⎡⎣⎢cos(ϕ)cos(ψ)+sin(ϕ)sin(θ)sin(ψ)cos(θ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(θ)cos(ψ)sin(ϕ)sin(ψ)+cos(ϕ)sin(θ)cos(ψ)sin(ϕ)cos(θ)−sin(θ)cos(ϕ)cos(θ)⎤⎦⎥⎡⎣⎢00g⎤⎦⎥=⎡⎣⎢g⋅sin(ϕ)cos(θ)−g⋅sin(θ)g⋅cos(ϕ)cos(θ)⎤⎦⎥(365)(366)(367)(368)(8)(8)(365)G2=[ax2ay2az2]=Cy(ϕ)Cx(θ)Cy(ψ)G1=Cy(ϕ)Cx(θ)Cy(ψ)[00g](366)=[cos(ϕ)0sin(ϕ)010−sin(ϕ)0cos(ϕ)][1000cos(θ)−sin(θ)0sin(θ)cos(θ)][cos(ψ)−sin(ψ)0sin(ψ)cos(ψ)0001][00g](367)=[cos(ϕ)cos(ψ)+sin(ϕ)sin(θ)sin(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)sin(ϕ)cos(θ)cos(θ)sin(ψ)cos(θ)cos(ψ)−sin(θ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)sin(ϕ)sin(ψ)+cos(ϕ)sin(θ)cos(ψ)cos(ϕ)cos(θ)][00g](368)=[g⋅sin(ϕ)cos(θ)−g⋅sin(θ)g⋅cos(ϕ)cos(θ)]
由公式(8)可得,
ax2az2=tan(ϕ)ax2az2=tan(ϕ)
则
ϕ=tan(ax2az2)(9)(9)ϕ=tan(ax2az2)计算俯仰角θθ
- 方法一:由公式(8)可得,
ay2=−g⋅sin(θ)ay2=−g⋅sin(θ)
则
θ=−arcsin(ay2g)θ=−arcsin(ay2g),此时g=sqrt(a2x2+a2y2+a2z2)g=sqrt(ax22+ay22+az22). - 方法二:已求得ϕϕ条件下,同由公式(8)得,
ay2ax2sin(ϕ)+az2cos(ϕ)=−tan(θ)ay2ax2sin(ϕ)+az2cos(ϕ)=−tan(θ)
则
θ=−arctan(ay2ax2sin(ϕ)+az2cos(ϕ))(10)(10)θ=−arctan(ay2ax2sin(ϕ)+az2cos(ϕ)) - 方法一与方法二求得结果一致,本文采用方法一(android getOrientation()方法用的方法二)
- 方法一:由公式(8)可得,
- 计算航向角ψψ
由公式(7)可得,
M2=⎡⎣⎢mx2my2mz2⎤⎦⎥=Cy(ϕ)Cx(θ)Cz(ψ)M1=Cy(ϕ)Cx(θ)Cz(ψ)⎡⎣⎢cos(δ)0sin(δ)⎤⎦⎥=⎡⎣⎢cos(ϕ)cos(ψ)+sin(ϕ)sin(θ)sin(ψ)cos(θ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(θ)cos(ψ)sin(ϕ)sin(ψ)+cos(ϕ)sin(θ)cos(ψ)sin(ϕ)cos(θ)−sin(θ)cos(ϕ)cos(θ)⎤⎦⎥⎡⎣⎢cos(δ)0sin(δ)⎤⎦⎥=⎡⎣⎢cos(ϕ)cos(ψ)cos(δ)+sin(ϕ)sin(θ)sin(ψ)cos(δ)+sin(ϕ)cos(θ)sin(δ)cos(θ)sin(ψ)cos(δ)−sin(θ)sin(δ)cos(ϕ)sin(θ)sin(ψ)cos(δ)−sin(ϕ)cos(ψ)cos(δ)+cos(ϕ)cos(θ)sin(δ)⎤⎦⎥(369)(370)(371)(11)(11)(369)M2=[mx2my2mz2]=Cy(ϕ)Cx(θ)Cz(ψ)M1=Cy(ϕ)Cx(θ)Cz(ψ)[cos(δ)0sin(δ)](370)=[cos(ϕ)cos(ψ)+sin(ϕ)sin(θ)sin(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)sin(ϕ)cos(θ)cos(θ)sin(ψ)cos(θ)cos(ψ)−sin(θ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)sin(ϕ)sin(ψ)+cos(ϕ)sin(θ)cos(ψ)cos(ϕ)cos(θ)][cos(δ)0sin(δ)](371)=[cos(ϕ)cos(ψ)cos(δ)+sin(ϕ)sin(θ)sin(ψ)cos(δ)+sin(ϕ)cos(θ)sin(δ)cos(θ)sin(ψ)cos(δ)−sin(θ)sin(δ)cos(ϕ)sin(θ)sin(ψ)cos(δ)−sin(ϕ)cos(ψ)cos(δ)+cos(ϕ)cos(θ)sin(δ)]
已知θθ及ϕϕ条件下,由上式可得:
mx2sin(ϕ)+mz2cos(ϕ)=sin(θ)sin(ψ)cos(δ)+cos(θ)sin(δ)mx2sin(ϕ)+mz2cos(ϕ)=sin(θ)sin(ψ)cos(δ)+cos(θ)sin(δ)
[mx2sin(ϕ)+mz2cos(ϕ)]sin(θ)+my2cos(θ)= sin(ψ)cos(δ)[mx2sin(ϕ)+mz2cos(ϕ)]sin(θ)+my2cos(θ)= sin(ψ)cos(δ)
mx2cos(ϕ)−mz2sin(ϕ)=cos(ψ)cos(δ)mx2cos(ϕ)−mz2sin(ϕ)=cos(ψ)cos(δ)
则:
ψ=arctan(mx2sin(ϕ)sin(θ)+mz2cos(ϕ)sin(θ)+my2cos(θ)mx2cos(ϕ)−mz2sin(ϕ))ψ=arctan(mx2sin(ϕ)sin(θ)+mz2cos(ϕ)sin(θ)+my2cos(θ)mx2cos(ϕ)−mz2sin(ϕ))
Android 应用
- 依上述公式求取航向角为与x轴夹角,通过android api getOrientation()方法获取方位角为与y轴夹角.需做角度转换
float yaw = (float) Math.toDegrees(ECompass.getYaw(accVals,magVals));
//getOrientation获取方位角为与y轴夹角,本文计算方法求取方位角为与x轴夹角,角度需转换
yaw -= 90;
if(yaw<-180)
yaw += 360;
public class ECompass {
private static float yaw;
private static float pitch;
private static float roll;
//accVal,magVal为android phone 获取传感器原始数据
public static float getYaw(float[] accVals, float[] magVals) {
roll = (float)Math.atan2(accVals[0],accVals[2]);
pitch = -(float)Math.atan(accVals[1]/(accVals[0]*Math.sin(roll)+accVals[2]*Math.cos(roll)));
yaw = (float)Math.atan2(magVals[0]*Math.sin(roll)*Math.sin(pitch)+magVals[2]*Math.cos(roll)*Math.sin(pitch)+magVals[1]*Math.cos(pitch),
magVals[0]*Math.cos(roll)-magVals[2]*Math.sin(roll));
return yaw;
}
}