首页 » 学习 » 整合两轮车模型与传感器融合模型

整合两轮车模型与传感器融合模型

上一篇文章中对两轮车的模型进行了验证。核心思想是用模型和输入估计系统的后面的输出,再根据实际采得的数据进行比较以对模型进行验证。这里模型的输出是两轮车的车体角度。实际工作中,该角度是通过加速度计和陀螺仪两个传感器的数据融合得到。整个系统工作结构如下图所示。这里融合模型较准确,以其输出作为标准,与两轮车模型的输出做比较。比较结果标志折两轮车模型的准确性。

两轮车模型
滤波器模型

本文目标是简化模型,直接使用电机加速度估计加速度计和陀螺仪数据,这样在实际使用中减少计算量,提高程序运行效率。

融合模型

传感器数据融合模型

加速度计测量的是加速度,在忽略车体运动的加速度时,车体唯一的加速度是重力加速度。所以在低频区域,加速度计测量的数据可以转换为车体角度。陀螺仪测量的是车体角速度,可以通过积分得到车体角度。积分过程中,噪声会不断累积使结果产生漂移。所以陀螺仪的动态性能较好。这里各取所长,融合加速度计数据和陀螺仪数据估计真实的车体角度。

本文使用卡尔曼滤波器对车体角度进行估计。使用卡尔曼滤波需要给出系统模型、系统输入、输出以及输出和状态的方差。这里使用状态空间模型构建系统,输入输出分别是角速度和角度。设X为车体真实的角度。由机理得A=1 B=Ts C=1

\begin{equation*}
X_{k+1}=A*X_k+u_k\\
Y_{k+1}=C*X_{k+1}
\end{equation*}

模型整合

整合的核心是在离散系统中对输出求导。两轮车模型输出是车体角度可以直接对应加速度计数据,另需要对车体角度求导得车体角速度对应陀螺仪数据。

\begin{align*}
Y’_{k+1}&=\frac{Y_{k+1}-Y_k}{T_s}=\frac{C*X_{k+1}-C*X_k}{T_s}\\
&=\frac{C*(A*X_k+B*u_k)-C*X_k}{T_s}\\
&=\frac{C*A-C}{T_s}X_k+\frac{B}{T_s}u_k
\end{align*}

对应得到新的系数

\begin{matrix}
A=A_{old}&
B=B_{old}\\
C=\begin{bmatrix} C_{old}\\ \frac{C_{old}*A_{old}-C_{old}}{T_S} \end{bmatrix}&
D=\begin{bmatrix}0\\ \frac{B}{T_S} \end{bmatrix}
\end{matrix}

仿真

仿真程序与上一篇得区别主要在于新的模型有两个输出,所以C是个矩阵而不是一个常数,此外新增了D矩阵。

for i=2:length(input)-steps     %时间更新方程
  x(:,i)=ax(:,i-1)+binput(i-1);     
  p=apa'+q;     %模型预测误差统计      
  est_x=x(:,i);     
  for j=0:steps     
    est_output(:,i+1)=cest_x+dinput(i);     
    error_output(:,i)=error_output(:,i)+abs(output(i+1,:)'-est_output(:,i+1));     
    est_x=aest_x+binput(i+j);     
  end     %状态更新方程     
  k=pc'(cpc'+r)^(-1);     
  x(:,i)=x(:,i)+k(output(i,:)'-(cx(:,i)+dinput(i-1)));     
  p=(eye(2)-kc)*p; 
end

完整程序:kalman_2var.m

下图是5步预测的结果。效果没有上一篇文章中好,因为陀螺仪和加速度计都不能作为标准数据做校准使用。化简模型的主要意义在于简化后面运算量,而不在于检验模型正确性。

仿真结果

标签:

发表评论

电子邮件地址不会被公开。 必填项已用*标注