1 内容介绍
本文研究了基于单目视觉的运动刚体位姿估计问题,提出了基于自适应无迹卡尔曼滤波算法(Adaptive Unscented Kalman Filter,AUKF)的位姿估计方法.考虑到运动刚体位姿估计系统的量测方程为非线性且过程噪声统计特征未知,通过递推噪声估计器在线估计过程噪声的均值和方差阵,解决了位姿估计系统中过程噪声统计特性未知时估计精度下降的问题.实验结果表明,AUKF算法提高了位姿估计的精度,并实现了过程噪声统计特性的在线估计.
2 仿真代码
clear all;clc;
ag=1;
flag =1;
t=0.05*ag;
TxtData1 = importdata('Mvideo1.txt');
armjoints = importdata('ralPointFile.txt');
TxtData2 = importdata('Mvideo2.txt');
m = size(TxtData2,1);
% kx2 = 839.321428295768 ;ky2 = 840.483960297146 ;u02 = 243.868668455832 ;v02 = 216.650954197449 ;
% kx1 = 809.345902119970; ky1 = 803.055062922696;u01 = 380.962537796614;v01 = 234.830825833781;
% % kx1 = 802.336514588841 ;ky1 = 804.376231832541 ;u01 = 331.447470345934 ;v01 = 244.468762099674 ;
% % kx2 = 798.050806080183 ;ky2 = 797.408432851774 ;u02 = 358.151014009806 ;v02 = 232.751596763967 ;
% kx2 = 832.054901757104; ky2 = 828.444768253781;u02 = 332.664199846859;v02 = 211.936118674671;% kx1 = 880.050806080183 ;ky1 = 880.408432851774 ;u01 = 369.151014009806 ;v01 = 212.751596763967 ;
kx1 = 803.345902119970;ky1 = 803.055062922696;u01 = 380.962537796614;v01 = 233.830825833781;
kx2 = 830.054901757104;ky2 = 821.444768253781;u02 = 332.664199846859;v02 = 211.936118674671;
focalIndex = [kx1 ky1 u01 v01;kx2 ky2 u02 v02]';
RelatObjCoor = [-35,-80,0;
35,-80,0;
35,-10,0;
-35,-10,0;
-20,-65,0;
20,-65,0;
20,-25,0;
-20,-25,0];
Init_X2 = [0;0;0;0;0;0;0;0;0;0.00001;0;0;0.000001;0;0;0.000001;0;0];
Init_X1 = [armjoints(1,1);armjoints(1,2);armjoints(1,3);0;0;0;0;0;0;armjoints(1,4)*pi/180;armjoints(1,5)*pi/180;armjoints(1,6)*pi/180;0.000001;0;0;0.000001;0;0];
x1 = Init_X1;
x2 = Init_X2;
P1 = 10*eye(18);P2 = P1;
%Q = diag([0,0,0,0.5,0.5,0.5,0.1,0.1,0.1,0,0,0,0.5,0.5,0.5,0.1,0.1,0.1],0);
% R = 0.06*eye(8);
%R = 10*diag([0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05]);
x_aukf1=Init_X1;
P_aukf1 = 10*eye(18);
qaukf1=zeros(18,1);
%Qaukf1= 0.1*diag([0.00001,0.00001,0.00001,0.2,0.5,0.5,0.1,0.1,0.1, 0.00001,0.00001,0.00001,0.5,0.5,0.5,0.1,0.1,0.1],0);
Qaukf1 = 0.5*diag([0,0,0,0.5,0.5,0.5,0.1,0.1,0.1,0,0,0,0.5,0.5,0.5,0.1,0.1,0.1],0);
%Qaukf1=zeros(18,18);
raukf1=zeros(16,1);
Raukf1= 10*diag([0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05]);
SData_X1 = zeros(fix(m/ag),6);
SData_X2 = zeros(fix(m/ag),6);
aa1=1.5;
aa2=-0.25;
aa3=0.75;
tic;
armjoints(1:m,1) =armjoints(1:m,1)-0.3;
armjoints(1:m,2) =armjoints(1:m,2)-0.1;
armjoints(1:m,3) =armjoints(1:m,3)-0.5;
armjoints(1:m,4) =armjoints(1:m,4)+0.8;
armjoints(1:m,5) =armjoints(1:m,5)-0.8;
armjoints(1:m,6) =armjoints(1:m,6)+0.5;
for i = 1:m/ag
z1 = TxtData1(i,:)';
z2 = TxtData2(i,:)';
real = armjoints(i,:)';
z = [z1,z2];
% [ x1,P1 ] = NonlinerUKF(z1,x1,P1,focalIndex,t,RelatObjCoor,1);
[ x_aukf1,P_aukf1,qaukf1,Qaukf1,raukf1,Raukf1,Q0 ] = NonlinerAUKF(z1,x_aukf1,P_aukf1,focalIndex,t,RelatObjCoor,qaukf1,Qaukf1,raukf1,Raukf1,1,i);
%[ x2,P2 ] = NonlinerUKF(z,x2,P2,focalIndex,t,RelatObjCoor,4);tim2 = toc;
% SData_X2(i,:) = [x1(1),x1(2),x1(3),x1(10)*180/pi,x1(11)*180/pi,x1(12)*180/pi];
SData_X1(i,:) = [x_aukf1(1),x_aukf1(2)+aa2,x_aukf1(3)+aa3,x_aukf1(10)*180/pi,x_aukf1(11)*180/pi,x_aukf1(12)*180/pi];
end
toc
a = 1:m/ag;
save SData3 SData_X1;
%save SData5 SData_X2;
%save TrackTrue armjoints;
subplot(3,2,1);
plot(a,SData_X1(:,1),'r');hold on;
subplot(3,2,2);
plot(a,SData_X1(:,2),'r');hold on;
subplot(3,2,3);
plot(a,SData_X1(:,3),'r');hold on;
subplot(3,2,4);
plot(a,SData_X1(:,4),'r');hold on;
subplot(3,2,5);
plot(a,SData_X1(:,5),'r');hold on;
subplot(3,2,6);
plot(a,SData_X1(:,6),'r');hold on;
subplot(3,2,1);
plot(a,SData_X2(:,1),'b');hold on;
subplot(3,2,2);
plot(a,SData_X2(:,2),'b');hold on;
subplot(3,2,3);
plot(a,SData_X2(:,3),'b');hold on;
subplot(3,2,4);
plot(a,SData_X2(:,4),'b');hold on;
subplot(3,2,5);
plot(a,SData_X2(:,5),'b');hold on;
subplot(3,2,6);
plot(a,SData_X2(:,6),'b');hold on;
%
%
subplot(3,2,1);
plot(a,armjoints(:,1),'k');hold on;
subplot(3,2,2);
plot(a,armjoints(:,2),'k');hold on;
subplot(3,2,3);
plot(a,armjoints(:,3),'k');hold on;
subplot(3,2,4);
plot(a,armjoints(:,4),'k');hold on;
subplot(3,2,5);
plot(a,armjoints(:,5),'k');hold on;
subplot(3,2,6);
plot(a,armjoints(:,6),'k');hold on;
3 运行结果
4 参考文献
[1]张鋆豪, 杨旭升, 冯远静,等. 基于自适应无迹卡尔曼滤波和单目视觉的运动刚体位姿估计[C]// 中国控制会议. 2018.
[2]陈玉寅. 基于卡尔曼滤波器的运动刚体位姿估计方法研究. 浙江工业大学.
