基于Levenberg-Marquardt算法的加速度计标定方法
本文方法来自于全权老师的《多旋翼飞行器设计与控制》一书，且仅仅是标定方法中的一种。
误差模型
令
b a m ′ ∈ R 3 ^{\mathrm{b}} \mathrm{a}_{\mathrm{m}} ^{\prime} \in \mathbb{R}^{3} bam′∈R3
表示标定前的比力，令
b a m ∈ R 3 ^{\mathrm{b}} \mathrm{a}_{\mathrm{m}} \in \mathbb{R}^{3} bam∈R3
表示标定后的比力。
两者之间的关系如下式所示：
b a m = T a K a ( b a m ′ + b a ′ ) ^{b} \mathbf{a}_{\mathrm{m}}=\mathbf{T}_{\mathrm{a}} \mathbf{K}_{\mathrm{a}}\left(^{\mathrm{b}} \mathbf{a}_{\mathrm{m}}^{\prime}+\mathbf{b}_{\mathrm{a}}^{\prime}\right) bam=TaKa(bam′+ba′)
其中：
T a = [ 1 Δ ψ a − Δ θ a − Δ ψ a 1 Δ ϕ a Δ θ a − Δ ϕ a 1 ] \mathbf{T}_{\mathrm{a}}=\left[ \begin{array}{ccc}{1} & {\Delta \psi_{\mathrm{a}}} & {-\Delta \theta_{\mathrm{a}}} \\ {-\Delta \psi_{\mathrm{a}}} & {1} & {\Delta \phi_{\mathrm{a}}} \\ {\Delta \theta_{\mathrm{a}}} & {-\Delta \phi_{\mathrm{a}}} & {1}\end{array}\right] Ta=⎣⎡1−ΔψaΔθaΔψa1−Δϕa−ΔθaΔϕa1⎦⎤
T a ∈ R 3 × 3 \mathbf{T}_{\mathrm{a}}\in \mathbb{R}^{3 \times 3} Ta∈R3×3表示由安装误差产生的微小倾斜角矩阵。 T a T_a Ta可由安装坐标系 s s s到机体坐标系 b b b的旋转矩阵近似得到：
T a = R s b = [ cos θ cos ψ cos θ sin ψ − sin θ sin ϕ sin θ cos ψ − cos ϕ sin ψ sin ϕ sin θ sin ψ + cos ϕ cos ψ sin ϕ cos θ cos ϕ sin θ cos ψ + sin ϕ sin ψ cos ϕ sin θ sin ψ − sin ϕ cos ψ cos ϕ cos θ ] T_a=\boldsymbol{R}_{s}^{b}=\left[ \begin{array}{ccc}{\cos \theta \cos \psi} & {\cos \theta \sin \psi} & {-\sin \theta} \\ {\sin \phi \sin \theta \cos \psi-\cos \phi \sin \psi} & {\sin \phi \sin \theta \sin \psi+\cos \phi \cos \psi} & {\sin \phi \cos \theta} \\ {\cos \phi \sin \theta \cos \psi+\sin \phi \sin \psi} & {\cos \phi \sin \theta \sin \psi-\sin \phi \cos \psi} & {\cos \phi \cos \theta}\end{array}\right] Ta=Rsb=⎣⎡cosθcosψsinϕsinθcosψ−cosϕsinψcosϕsinθcosψ+sinϕsinψcosθsinψsinϕsinθsinψ+cosϕcosψcosϕsinθsinψ−sinϕcosψ−sinθsinϕcosθcosϕcosθ⎦⎤
安装误差角一般都很小，则有 s i n θ = θ , c o s θ = 1 sin\theta = \theta ,cos \theta = 1 sinθ=θ,cosθ=1，上式变为：
T a = R s b = [ 1 ψ − θ − ψ 1 ϕ θ − ϕ 1 ] T_a=\boldsymbol{R}_{s}^{b}=\left[ \begin{array}{ccc}{1} & {\psi} & {-\theta} \\ {-\psi} & {1} & {\phi} \\ {\theta} & {-\phi} & {1}\end{array}\right] Ta=Rsb=⎣⎡1−ψθψ1−ϕ−θϕ1⎦⎤
K a ∈ R 3 × 3 \mathbf{K}_{\mathrm{a}}\in \mathbb{R}^{3 \times 3} Ka∈R3×3表示尺度因子
K a = [ s a x 0 0 0 s a y 0 0 0 s a z ] \mathbf{K}_{\mathrm{a}}=\left[ \begin{array}{ccc}{s_{\mathrm{ax}}} & {0} & {0} \\ {0} & {\mathrm{s}_{\mathrm{ay}}} & {0} \\ {0} & {0} & {\mathrm{s}_{\mathrm{az}}}\end{array}\right] Ka=⎣⎡sax000say000saz⎦⎤
b
a
′
∈
R
3
\mathbf{b}_{\mathrm{a}}^{\prime}\in \mathbb{R}^{3}
ba′∈R3表示偏移。
b
a
′
=
[
b
a
x
′
b
a
y
′
b
a
z
′
]
\mathbf{b}_{\mathrm{a}}^{\prime}=\left[ \begin{array}{l}{b_{\mathrm{ax}}^{\prime}} \\ {b_{\mathrm{ay}}^{\prime}} \\ {b_{\mathrm{az}}^{\prime}}\end{array}\right]
ba′=⎣⎡bax′bay′baz′⎦⎤
标定原理
为了标定加速度计，需要估计以下参数：
Θ a ≜ [ Δ ψ a Δ θ a Δ ϕ a s a x s a y s a z b a x ′ b a y ′ b a z ′ ] T \Theta_{\mathrm{a}} \triangleq \left[ \begin{array}{llllllll}{\Delta \psi_{\mathrm{a}}} & {\Delta \theta_{\mathrm{a}}} & {\Delta \phi_{\mathrm{a}}} & {s_{\mathrm{ax}}} & {s_{\mathrm{ay}}} & {s_{\mathrm{az}}} & {b_{\mathrm{ax}}^{\prime}} & {b_{\mathrm{ay}}^{\prime}} & {b_{\mathrm{az}}^{\prime}}\end{array}\right]^{\mathrm{T}} Θa≜[ΔψaΔθaΔϕasaxsaysazbax′bay′baz′]T
加速度计的误差模型可以写成如下形式：
h a ( Θ a , b a m ′ ) ≜ T a K a ( b a m ′ + b a ′ ) \mathbf{h}_{\mathrm{a}}\left(\boldsymbol{\Theta}_{\mathrm{a}}, ^{b}\mathbf{a}_{\mathrm{m}}^{\prime}\right) \triangleq \mathbf{T}_{\mathrm{a}} \mathbf{K}_{\mathrm{a}}\left(^{b}\mathrm{a}_{\mathrm{m}}^{\prime}+\mathbf{b}_{\mathrm{a}}^{\prime}\right) ha(Θa,bam′)≜TaKa(bam′+ba′)
我们在对加速度计进行标定时，需要使加速度计以不同角度旋转
M
∈
Z
+
M \in \mathbb{Z}_{+}
M∈Z+次，且在各个固定角度下保持一段时间；记录下在第
k
k
k个时间间隔内测得的比力为
b
a
m
,
k
′
∈
R
3
(
k
=
1
,
2
,
⋯
 
,
M
)
\mathrm{b}_{\mathrm{a}_{\mathrm{m},k}}^{\prime} \in \mathbb{R}^{3}(k=1,2, \cdots, M)
bam,k′∈R3(k=1,2,⋯,M)
标定的原理在于，不论以何种角度摆放加速度计，其得到的比力的模长应该始终为一定值，即为当地的重力加速度 g g g,根据此原理，我们可以得到如下所示的优化方程。
Θ a ∗ = arg min Θ a ∑ k = 1 M ( ∥ h a ( Θ a , b a m , k ′ ) ∥ − g ) 2 \Theta_{\mathrm{a}}^{*}=\arg \min _{\Theta_{\mathrm{a}}} \sum_{k=1}^{M}\left(\left\|\mathrm{h}_{\mathrm{a}}\left(\Theta_{\mathrm{a}}, ^{b}\mathrm{a}_{\mathrm{m}, k}^{\prime}\right)\right\|-g\right)^{2} Θa∗=argΘamink=1∑M(∥∥ha(Θa,bam,k′)∥∥−g)2
接着利用Levenberg-Marquardt算法求出 Θ a ∗ \Theta_{\mathrm{a}}^{*} Θa∗即可。
首先来构建标定指标，根据上式，指标表达式可以写成：
V
a
=
∑
k
=
1
N
(
∥
h
a
(
Θ
a
,
b
a
k
′
)
∥
−
g
)
2
V_{\mathrm{a}}=\sum_{k=1}^{N}\left(\left\|\mathrm{h}_{\mathrm{a}}\left(\Theta_{\mathrm{a}}, ^{b}\mathrm{a}_{k}^{\prime}\right)\right\|-g\right)^{2}
Va=k=1∑N(∥∥ha(Θa,bak′)∥∥−g)2
可知，
V
a
V_{\mathrm{a}}
Va的输入为9维，输出为N维。
其中 N N N表示整个标定过程中的总采样点数；
令：
V a , k = ( ∥ h a ( Θ a , b a k ′ ) ∥ − g ) 2 V_{a,k}=\left(\left\|\mathrm{h}_{\mathrm{a}}\left(\Theta_{\mathrm{a}}, ^{b}\mathrm{a}_{k}^{\prime}\right)\right\|-g\right)^{2} Va,k=(∥∥ha(Θa,bak′)∥∥−g)2
表示每一次标定过程中的指标函数；其中
∥ h a ( Θ a , b a k ′ ) ∥ = ( b a x , k ) 2 + ( b a y , k ) 2 + ( b a z , k ) 2 \left\|\mathrm{h}_{\mathrm{a}}\left(\Theta_{\mathrm{a}}, ^{b}\mathrm{a}_{k}^{\prime}\right)\right\|=\sqrt{(^{b}\mathrm{a}_{x,k})^2+(^{b}\mathrm{a}_{y,k})^2+(^{b}\mathrm{a}_{z,k})^2} ∥∥ha(Θa,bak′)∥∥=(bax,k)2+(bay,k)2+(baz,k)2
表示标定后的比力模长。
Levenberg-Marquardt算法核心在于每一次迭代过程中的参数更新方程：
x m + 1 = x m − ( J m ⊤ J m + λ I ) − 1 g m x_{m+1}=x_{m}-\left(J_{m}^{\top} J_{m}+\lambda I\right)^{-1} g_{m} xm+1=xm−(Jm⊤Jm+λI)−1gm
其基本思想是是参数值向指标函数的负梯度方向移动， ( J m ⊤ J m + λ I ) − 1 \left(J_{m}^{\top}J_{m}+\lambda I\right)^{-1} (Jm⊤Jm+λI)−1为步长，引入单位阵可有效防止矩阵奇异。
其算法流程为：
1.计算当前参数x_{m}下的性能指标函数的雅可比矩阵 J J J;
2.计算当前参数x_{m}下的性能指标函数的梯度 g m g_{m} gm;
3.计算x_{m+1}，参数更新；
4.计算当前参数下的标定指标 Θ a ∗ \Theta_{\mathrm{a}}^{*} Θa∗；并与上一次计算得到的标定指标做对比，若指标变小则减小惩罚因子 λ \lambda λ，若指标变大则增加惩罚因子 λ \lambda λ；若指标减小的幅度小于预设值，优化完成，退出迭代；
5.重复以上迭代过程。
补充
V
a
V_{\mathrm{a}}
Va的输入为9维，输出为N维。可知其雅可比矩阵为
N
×
9
N \times 9
N×9维，如下所示：
J
=
[
∂
V
a
,
1
∂
Θ
a
,
1
⋯
∂
V
a
,
1
∂
Θ
a
,
9
⋮
⋱
⋮
∂
V
a
,
N
∂
Θ
a
,
1
⋯
∂
V
a
,
N
∂
Θ
a
,
9
]
J=\left[ \begin{array}{ccc}{\frac{\partial V_{a,1}}{\partial \Theta_ {a,1}}} & {\cdots} & {\frac{\partial V_{a,1}}{\partial \Theta_ {a,9}}} \\ {\vdots} & {\ddots} & {\vdots} \\ {\frac{\partial V_{a,N}}{\partial \Theta_ {a,1}}} & {\cdots} & {\frac{\partial V_{a,N}}{\partial \Theta_{a,9}}}\end{array}\right]
J=⎣⎢⎢⎡∂Θa,1∂Va,1⋮∂Θa,1∂Va,N⋯⋱⋯∂Θa,9∂Va,1⋮∂Θa,9∂Va,N⎦⎥⎥⎤
其中
Θ
a
,
i
\Theta_{a,i}
Θa,i表示
Θ
a
\Theta_a
Θa中的第
i
i
i个元素。
第 m m m次迭代过程中 V a V_{\mathrm{a}} Va的梯度可由下式计算：
g m = J T V a ( m ) g_m=J^T V_{a}(m) gm=JTVa(m)
V a ( m ) V_{\mathrm{a}}(m) Va(m)表示第 m m m次迭代中的指标值( N × 1 N \times 1 N×1维矩阵)，故 g m g_m gm为 9 × 1 9 \times 1 9×1维矩阵。
matlab实现
本文使用MPU9250基于STM32嵌入式平台，通过IIC接口以100HZ的速度从串口将传感器的原始数据发送至上位机；再使用matlab对传感器数据进行标定，标定前后的指标对比如下图所示：
其中，黑色曲线为标定前的指标，蓝色为标定后的指标，可见标定后，指标大幅下降，这表明了本文采用的标定算法的有效性。
代码
主函数
%加速度计标定程序
clear;
close all;
load ax
load ay
load az
%迭代次数
N=17;
%传感器数据个数
acqSize=2000;
%分配空间
Acc =zeros(3,acqSize);
sum0=zeros(1,acqSize);
a1=zeros(1,N);
a2=zeros(1,N);
a3=zeros(1,N);
s1=zeros(1,N);
s2=zeros(1,N);
s3=zeros(1,N);
b1=zeros(1,N);
b2=zeros(1,N);
b3=zeros(1,N);
%赋值
for j=1:acqSize
Acc(1,j)=ax(j);
Acc(2,j)=ay(j);
Acc(3,j)=az(j);
end
%由于安装导致的欧拉角误差
a1(1)=0;
a2(1)=0;
a3(1)=0;
%各轴的尺度因子
s1(1)=1;
s2(1)=1;
s3(1)=1;
%各轴的测量偏移
b1(1)=0;
b2(1)=0;
b3(1)=0;
%重力加速度的值
g=9.8;
%迭代终止条件
minvalue=0.5;
%未标定前的传感器性能指标
for n=1:acqSize
sum0(n)=ComputeF(a1(1),a2(1),a3(1),s1(1),s2(1),s3(1),b1(1),b2(1),b3(1),Acc(1,n),Acc(2,n),Acc(3,n),g);
end
%设定一个数组用来存储最优的lamda
Lam=zeros(2,1);
%使用LM算法来求解最小二乘优化问题
%惩罚因子
lamda=1000;
%用于存储使用上一次的X值计算得到的指标
Fk =zeros(acqSize,1);
%用于存储使用本次的X值计算得到的指标
Fk1=zeros(acqSize,1);
for i=2:N
%使用上一次的X值计算得到的指标
value_latest=0;
value_new=0;
for j=1:acqSize
Fk(j)=ComputeF(a1(i-1),a2(i-1),a3(i-1),s1(i-1),s2(i-1),s3(i-1),b1(i-1),b2(i-1),b3(i-1),Acc(1,j),Acc(2,j),Acc(3,j),g);
value_latest=value_latest+Fk(j);
end
%优化X的取值，Levenberg-Marquardt算法
%计算雅可比矩阵
Jac=ComputeJacobi(a1(i-1),a2(i-1),a3(i-1),s1(i-1),s2(i-1),s3(i-1),b1(i-1),b2(i-1),b3(i-1),Acc,g,acqSize);
%状态更新
D=[1;1;1;1;1;1;1;1;1;];
X=[a1(i-1);a2(i-1);a3(i-1);s1(i-1);s2(i-1);s3(i-1);b1(i-1);b2(i-1);b3(i-1);];
X=X-((Jac'*Jac+lamda*diag(D))^-1)*Jac'*Fk;
a1(i)=X(1);
a2(i)=X(2);
a3(i)=X(3);
s1(i)=X(4);
s2(i)=X(5);
s3(i)=X(6);
b1(i)=X(7);
b2(i)=X(8);
b3(i)=X(9);
%判断优化后的指标是否减小
for k=1:acqSize
Fk1(k)=ComputeF(a1(i),a2(i),a3(i),s1(i),s2(i),s3(i),b1(i),b2(i),b3(i),Acc(1,k),Acc(2,k),Acc(3,k),g);
value_new=value_new+Fk1(k);
end
if value_new<=value_latest
lamda=lamda*0.9;
else
lamda=lamda*1.1;
end
end
%绘出标定前后的指标
value_wb=zeros(1,acqSize);
value_yb=zeros(1,acqSize);
for h=1:acqSize
temp1=ComputeF(a1(1),a2(1),a3(1),s1(1),s2(1),s3(1),b1(1),b2(1),b3(1),Acc(1,h),Acc(2,h),Acc(3,h),g);
temp2=ComputeF(a1(N),a2(N),a3(N),s1(N),s2(N),s3(N),b1(N),b2(N),b3(N),Acc(1,h),Acc(2,h),Acc(3,h),g);
value_wb(h)=temp1;
value_yb(h)=temp2;
end
plot(value_wb,'k')
hold on
plot(value_yb,'b')
grid on
T1=norm(a1(N));
T2=norm(a2(N));
T3=norm(a3(N));
S1=norm(s1(N));
S2=norm(s2(N));
S3=norm(s3(N));
T1
T2
T3
S1
S2
S3
计算雅可比矩阵的函数
function J = ComputeJacobi( a1,a2,a3,s1,s2,s3,b1,b2,b3,acc,g,num)
%函数用于计算雅可比矩阵
%赋值
phi =a1;
theta=a2;
psi =a3;
sx=s1;
sy=s2;
sz=s3;
bx=b1;
by=b2;
bz=b3;
amx=acc(1,:);
amy=acc(2,:);
amz=acc(3,:);
J=zeros(num,9);
for k=1:num
Fx=sqrt((sx*(amx(k)+bx)+psi*sy*(amy(k)+by)-theta*sz*(amz(k)+bz))^2 ...
+(-psi*sx*(amx(k)+bx)+sy*(amy(k)+by)+phi*sz*(amz(k)+bz))^2 ...
+(theta*sx*(amx(k)+bx)-phi*sy*(amy(k)+by)+sz*(amz(k)+bz))^2 )...
-g;
J(k,1)=( (-psi*sx*(amx(k)+bx)+sy*(amy(k)+by)+phi*sz*(amz(k)+bz))*sz*(amz(k)+bz) ...
-(theta*sx*(amx(k)+bx)-phi*sy*(amy(k)+by)+sz*(amz(k)+bz))*sy*(amy(k)+by) )*(Fx/(Fx+g));
J(k,2)=(-(sx*(amx(k)+bx)+psi*sy*(amy(k)+by)-theta*sz*(amz(k)+bz))*sz*(amz(k)+bz) ...
+(theta*sx*(amx(k)+bx)-phi*sy*(amy(k)+by)+sz*(amz(k)+bz))*sx*(amx(k)+bx) )*(Fx/(Fx+g));
J(k,3)=( (sx*(amx(k)+bx)+psi*sy*(amy(k)+by)-theta*sz*(amz(k)+bz))*sy*(amy(k)+by) ...
-(-psi*sx*(amx(k)+bx)+sy*(amy(k)+by)+phi*sz*(amz(k)+bz))*sx*(amx(k)+bx) )*(Fx/(Fx+g));
J(k,4)=( (sx*(amx(k)+bx)+psi*sy*(amy(k)+by)-theta*sz*(amz(k)+bz))*(amx(k)+bx) ...
-(-psi*sx*(amx(k)+bx)+sy*(amy(k)+by)+phi*sz*(amz(k)+bz))*psi*(amx(k)+bx) ...
+(theta*sx*(amx(k)+bx)-phi*sy*(amy(k)+by)+sz*(amz(k)+bz))*theta*(amx(k)+bx) )*(Fx/(Fx+g));
J(k,5)=( (sx*(amx(k)+bx)+psi*sy*(amy(k)+by)-theta*sz*(amz(k)+bz))*psi*(amy(k)+by) ...
+(-psi*sx*(amx(k)+bx)+sy*(amy(k)+by)+phi*sz*(amz(k)+bz))*(amy(k)+by) ...
-(theta*sx*(amx(k)+bx)-phi*sy*(amy(k)+by)+sz*(amz(k)+bz))*phi*(amy(k)+by) )*(Fx/(Fx+g));
J(k,6)=(-(sx*(amx(k)+bx)+psi*sy*(amy(k)+by)-theta*sz*(amz(k)+bz))*theta*(amz(k)+bz) ...
+(-psi*sx*(amx(k)+bx)+sy*(amy(k)+by)+phi*sz*(amz(k)+bz))*phi*(amz(k)+bz) ...
+(theta*sx*(amx(k)+bx)-phi*sy*(amy(k)+by)+sz*(amz(k)+bz))*(amz(k)+bz) )*(Fx/(Fx+g));
J(k,7)=( (sx*(amx(k)+bx)+psi*sy*(amy(k)+by)-theta*sz*(amz(k)+bz))*sx ...
-(-psi*sx*(amx(k)+bx)+sy*(amy(k)+by)+phi*sz*(amz(k)+bz))*psi*sx ...
+(theta*sx*(amx(k)+bx)-phi*sy*(amy(k)+by)+sz*(amz(k)+bz))*theta*sx )*(Fx/(Fx+g));
J(k,8)=( (sx*(amx(k)+bx)+psi*sy*(amy(k)+by)-theta*sz*(amz(k)+bz))*psi*sy ...
+(-psi*sx*(amx(k)+bx)+sy*(amy(k)+by)+phi*sz*(amz(k)+bz))*sy ...
-(theta*sx*(amx(k)+bx)-phi*sy*(amy(k)+by)+sz*(amz(k)+bz))*phi*sy )*(Fx/(Fx+g));
J(k,9)=(-(sx*(amx(k)+bx)+psi*sy*(amy(k)+by)-theta*sz*(amz(k)+bz))*theta*sz ...
+(-psi*sx*(amx(k)+bx)+sy*(amy(k)+by)+phi*sz*(amz(k)+bz))*phi*sz ...
-(theta*sx*(amx(k)+bx)-phi*sy*(amy(k)+by)+sz*(amz(k)+bz))*sz )*(Fx/(Fx+g)); %Fx^(-0.5)
end
end
计算指标的函数
function V = ComputeF( a1,a2,a3,s1,s2,s3,b1,b2,b3,accx,accy,accz,g)
%安装误差阵
T=[ 1 a3 -a2;
-a3 1 a1;
a2 -a1 1;];
%尺度因子
K=[s1 0 0;
0 s2 0;
0 0 s3;];
%三轴偏移
B=[b1;b2;b3;];
%加速度计测量值
ACC=[accx;accy;accz;];
%标定后的值
F=T*K*(ACC+B);
%指标
V=(sqrt(F'*F)-g).^2;
end
5363
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
