基于Levenberg-Marquardt算法的加速度计标定方法

基于Levenberg-Marquardt算法的加速度计标定方法

本文方法来自于全权老师的《多旋翼飞行器设计与控制》一书，且仅仅是标定方法中的一种。

误差模型

令

$^{\mathrm{b}} \mathrm{a}_{\mathrm{m}} ^{\prime} \in \mathbb{R}^{3}$bam′​∈R3

表示标定前的比力，令

$^{\mathrm{b}} \mathrm{a}_{\mathrm{m}} \in \mathbb{R}^{3}$bam​∈R3

表示标定后的比力。

两者之间的关系如下式所示：

$^{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​=Ta​Ka​(bam′​+ba′​)

其中：

$\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​​Δψa​1−Δϕa​​−Δθa​Δϕa​1​⎦⎤​

$\mathbf{T}_{\mathrm{a}}\in \mathbb{R}^{3 \times 3}$Ta​∈R3×3表示由安装误差产生的微小倾斜角矩阵。 $T_a$Ta​可由安装坐标系$s$s到机体坐标系$b$b的旋转矩阵近似得到：

$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θ​⎦⎤​

安装误差角一般都很小，则有$sin\theta = \theta ,cos \theta = 1$sinθ=θ,cosθ=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​⎦⎤​

$\mathbf{K}_{\mathrm{a}}\in \mathbb{R}^{3 \times 3}$Ka​∈R3×3表示尺度因子

$\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​=⎣⎡​sax​00​0say​0​00saz​​⎦⎤​

$\mathbf{b}_{\mathrm{a}}^{\prime}\in \mathbb{R}^{3}$ba′​∈R3表示偏移。
$\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′​​⎦⎤​

标定原理

为了标定加速度计，需要估计以下参数：

$\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​​Δϕa​​sax​​say​​saz​​bax′​​bay′​​baz′​​]T

加速度计的误差模型可以写成如下形式：

$\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′​)≜Ta​Ka​(bam′​+ba′​)

我们在对加速度计进行标定时，需要使加速度计以不同角度旋转$M \in \mathbb{Z}_{+}$M∈Z+​次，且在各个固定角度下保持一段时间；记录下在第$k$k个时间间隔内测得的比力为
$\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,根据此原理，我们可以得到如下所示的优化方程。

$\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Θa​min​k=1∑M​(∥∥​ha​(Θa​,bam,k′​)∥∥​−g)2

接着利用Levenberg-Marquardt算法求出$\Theta_{\mathrm{a}}^{*}$Θa∗​即可。

首先来构建标定指标，根据上式，指标表达式可以写成：

$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_{\mathrm{a}}$Va​的输入为9维，输出为N维。

其中$N$N表示整个标定过程中的总采样点数；

令：

$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

表示每一次标定过程中的指标函数；其中

$\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}-\left(J_{m}^{\top} J_{m}+\lambda I\right)^{-1} g_{m}$xm+1​=xm​−(Jm⊤​Jm​+λI)−1gm​

其基本思想是是参数值向指标函数的负梯度方向移动，$\left(J_{m}^{\top}J_{m}+\lambda I\right)^{-1}$(Jm⊤​Jm​+λI)−1为步长，引入单位阵可有效防止矩阵奇异。

其算法流程为：

1.计算当前参数x_{m}下的性能指标函数的雅可比矩阵$J$J;

2.计算当前参数x_{m}下的性能指标函数的梯度$g_{m}$gm​;

3.计算x_{m+1}，参数更新；

4.计算当前参数下的标定指标$\Theta_{\mathrm{a}}^{*}$Θa∗​；并与上一次计算得到的标定指标做对比，若指标变小则减小惩罚因子$\lambda$λ，若指标变大则增加惩罚因子$\lambda$λ；若指标减小的幅度小于预设值，优化完成，退出迭代；

5.重复以上迭代过程。

补充

$V_{\mathrm{a}}$Va​的输入为9维，输出为N维。可知其雅可比矩阵为$N \times 9$N×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​​​⎦⎥⎥⎤​
其中$\Theta_{a,i}$Θa,i​表示$\Theta_a$Θa​中的第$i$i个元素。

第$m$m次迭代过程中$V_{\mathrm{a}}$Va​的梯度可由下式计算：

$g_m=J^T V_{a}(m)$gm​=JTVa​(m)

$V_{\mathrm{a}}(m)$Va​(m)表示第$m$m次迭代中的指标值($N \times 1$N×1维矩阵)，故$g_m$gm​为$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

matlab代码和标定数据下载链接