使用PSINS工具箱实现ros/c++组合导航的快速调参

辉——书生意气

已于 2023-10-17 16:59:45 修改

阅读量511

点赞数 1

文章标签： c++ 开发语言

于 2023-10-12 08:36:24 首次发布

本文链接：https://blog.csdn.net/qq_45391544/article/details/133782561

版权

调参是个很枯燥过程，一点点在线调试，到田里调试，效率很低。本系统由于matlab部分代码和c++代码完全一一对应，可以通过rosbag包形式来录制跑动过程中一次数据，那里就可以在matab中不断调试参数，调整好的后参数可以给小车直接使用：
使用示例：比如我想使用186模型进行ekf组合导航，把录制好的rosbag包放在…/mytest/EKF_186_SinsGps.m中，修改rosbag路径，运行，可以看到我在matlab中蓝色为rtk轨迹，红色为小车实时导航跑的数据，但参数没有调好，绿色为仿真数据。通过我调参，可以显著看到绿色线追踪效果提升显著
在这里插入图片描述
稍微调参
提供代码如下（需要放在PSINS工具箱里使用，我用的版本是PSINS230123)
EKF_186_SinsGps.m

clc;clear;close all;
glvs;
psinstypedef(186);
imufz=100;
%获取rosbag中的imu和gps数据
path='E:\psins230123\mytest\handleGPS\DATA\ESKF_2023-10-11-05-26-56.bag';
bag= rosbag([path]);
% 查找所有的主题名
bag.AvailableTopics;
% 选择你需要的主题
%%%%%%%%%%%%%%%%%%%%%%%%%rosbag中RTK数据%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
selectedTopic = '/double_data';%记录了carstatus中的各种信息
% 从选定的主题中提取消息
msgStructs = readMessages(select(bag,'Topic',selectedTopic));
rtk_lat= zeros(1,numel(msgStructs));
rtk_lon = zeros(1,numel(msgStructs));
rtk_high=zeros(1,numel(msgStructs));
rtk_yaw=zeros(1,numel(msgStructs));
rtk_ve=zeros(1,numel(msgStructs));
rtk_vn=zeros(1,numel(msgStructs));
rtk_vu=zeros(1,numel(msgStructs));
rtk_fix=zeros(1,numel(msgStructs));
% 从每条消息中提取磁力计三轴数据
for i = 1:numel(msgStructs)
    rtk_lat(i) =msgStructs{i}.Latitude;
    rtk_lon(i) =msgStructs{i}.Longitude;
    rtk_high(i)=msgStructs{i}.Altitude;
    rtk_yaw(i)=msgStructs{i}.HeadingAngle;
    rtk_ve(i)=msgStructs{i}.VelE;
    rtk_vn(i)=msgStructs{i}.VelN;
    rtk_vu(i)=msgStructs{i}.VelU;
    rtk_fix(i)=msgStructs{i}.Fix;
end
figure(1);
plot(rtk_fix);title('卫星定位状态');
rtk_lat=rtk_lat';rtk_lon=rtk_lon';rtk_high=rtk_high';
rtk_ve=rtk_ve';rtk_vn=rtk_vn';rtk_vu=rtk_vu';
for i=1:length(rtk_yaw)
    if(rtk_yaw(i)>=0&&rtk_yaw(i)<90)
        rtk_yaw(i)=(-rtk_yaw(i)-90)*glv.deg;
    else
         rtk_yaw(i)=(270-rtk_yaw(i))*glv.deg;
    end
end
rtk_yaw=rtk_yaw';
[startX,startY]=BLH2XY(rtk_lat(1),rtk_lon(1));
localX=[];localY=[];
for i=1:length(rtk_lon)
    [localx(i),localy(i)]=BL2localXY(rtk_lat(i),rtk_lon(i),startX,startY);
    localX=[localX,localx(i)];
    localY=[localY,localy(i)];
end
figure(2);
plot(localX,localY,'LineWidth',2);
hold on;

% %%%%%%%%%%%%%%%%%%%%%%%%%获取机器实时导航录制的包数据%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 提取nav组合导航信息
selectedTopicNav = '/navResult';
msgStructsNav= readMessages(select(bag, 'Topic',selectedTopicNav));
nav_lat = zeros(1, numel(msgStructsNav));
nav_lon=zeros(1, numel(msgStructsNav));
for i = 1:numel(msgStructsNav)
     nav_lat(i) = msgStructsNav{i}.Lat;
     nav_lon(i)=msgStructsNav{i}.Lon;
end
nav_lat=nav_lat';nav_lon=nav_lon';
%进行绘图
[startX,startY]=BLH2XY(rtk_lat(1),rtk_lon(1));
localX2=[];localY2=[];
for i=1:length(nav_lon)
    [localx(i),localy(i)]=BL2localXY(nav_lat(i),nav_lon(i),startX,startY);
    localX2=[localX2,localx(i)];
    localY2=[localY2,localy(i)];
end
plot(localX2,localY2,'LineWidth',2);
hold on;
% 提取imu6轴信息
selectedTopicImu = '/imu/data';
msgStructsImu = readMessages(select(bag, 'Topic',selectedTopicImu));

accX = zeros(1, numel(msgStructsImu));
accY = zeros(1, numel(msgStructsImu));
accZ= zeros(1, numel(msgStructsImu));
gyroX= zeros(1, numel(msgStructsImu));
gyroY= zeros(1, numel(msgStructsImu));
gyroZ= zeros(1, numel(msgStructsImu));

for i = 1:numel(msgStructsImu)
    accX(i) = msgStructsImu{i}.LinearAcceleration.X;
    accY(i) = msgStructsImu{i}.LinearAcceleration.Y;
    accZ(i)=msgStructsImu{i}.LinearAcceleration.Z;
    gyroX(i)=msgStructsImu{i}.AngularVelocity.X;
    gyroY(i)=msgStructsImu{i}.AngularVelocity.Y;
    gyroZ(i)=msgStructsImu{i}.AngularVelocity.Z;
end

[nn, ts, nts] = nnts(1, 1/imufz);
imuerr = imuerrset(10, 15, 0.6, 60);%根据mti300参数
davp0 = avperrset([0.1;0.1;0.1], [0.1;0.1;0.1], [0.02;0.02;0.03]);
imu=[gyroX',gyroY',gyroZ',accX',accY',accZ']*0.01;
% 获取imu的行数
numRows = size(imu, 1);
% 创建时间序列
timeSeries = linspace(0.01, numRows*0.01, numRows)';
% 在imu的最后一列添加时间
imu = [imu, timeSeries];
lever = [0.57; 0.10; 0.35];
gps=[rtk_ve,rtk_vn,rtk_vu,rtk_lat,rtk_lon,rtk_high];%速度 位置
numRows = size(gps, 1);
timeSeries = linspace(0.05, numRows*0.05, numRows)';
gps=[gps,timeSeries];
avp0=[0,0,rtk_yaw(1),0,0,0,rtk_lat(1),rtk_lon(1),rtk_high(1)];
ins = insinit(avp0, ts, davp0);%惯导初始化

r0 = vperrset(0.06, 0.001);
kf = kfinit(ins, davp0, imuerr, lever, r0);
len = length(imu); 
[avp, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
% timebar(nn, len, '18-state SINS/GPS simulation.'); 
ki=1;
for k=1:nn:len-nn+1
    k1 = k+nn-1; 
    wvm = imu(k:k1,1:6); 
    t = imu(k1,end);
    ins = insupdate(ins, wvm);
    kf.Phikk_1 = kffk(ins);
    %kf = kfupdate(kf);
    kf=ekf(kf);
%     [kgps, dt] = imugpssyn(k, k1, 'F');%进行时间同步
    dt=0;
    if mod(t,0.05)==0
        if k1 <= size(gps, 1)
            vnGPS = gps(k1,1:3)';
            posGPS = gps(k1,4:6)'; 
            ins = inslever(ins);
            kf.Hk = kfhk(ins);
            zk = [ins.vnL-ins.an*dt-vnGPS; ins.posL-ins.Mpvvn*dt-posGPS];
            kf=ekf(kf,zk,'M');
            kf.xkk_1(3)=kf.xkk_1(3)-rtk_yaw(k1);
            [kf, ins] = kffeedback(kf, ins, 1, 'avp');
            avp(ki,:) = [ins.avp', t];
            xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]';
            ki = ki+1;
        else
            continue;  
        end
    end
end
avp(ki:end,:) = []; 
xkpk(ki:end,:) = [];

%%%%%%%%%%%%%%%%%%%%%%%%%%%matlab组合导航仿真结果%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fusion_lat=avp(:,7);
fusion_lon=avp(:,8);
[startX,startY]=BLH2XY(rtk_lat(1),rtk_lon(1));
localX3=[];localY3=[];
for i=1:length(fusion_lat)
    [localx(i),localy(i)]=BL2localXY(fusion_lat(i),fusion_lon(i),startX,startY);
    localX3=[localX3,localx(i)];
    localY3=[localY3,localy(i)];
end
plot(localX3,localY3,'LineWidth',2);
legend('rtk','机器实时eskf','matlab仿真ekf');
title('组合导航对比');

部分成员函数：

function[y,x]  = BLH2XY(B,L)
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here

%*******  6度带投影正算  ******
%    DB1=[17,33,55.7339];
%   DL1=[119,15,52.1159];
%   n = floor((L + 1.5) / 3);
% L0 = n * 3;
L0 = 114;    %84
%     L0 = 117;   %%%54
%   B=DB1(1)+DB1(2)/60+DB1(3)/3600;%把B，l矩阵度分秒转换成 度的小数形式
B=B/(180/pi);    %角度转弧度
%    L=DL1(1)+DL1(2)/60+DL1(3)/3600;
l=L-L0;
%克拉索夫斯基椭球参数
%     a=6378245.0;              %长半轴54
a= 6378137;   %84
%     b=6356863.0187730473;     %短半轴54
b=6356752.3142;   %84
%     c=6399698.901782711;      %极点处曲率半径
c=6399593.6258;
ro=206264.806247096355;   %角度转弧度常数
E1=(a^2-b^2)/a^2;         %E2=e^2
E2=(a^2-b^2)/b^2;         %E2=(e')^2

t=tan(B);
YeTa2=E2*cos(B)*cos(B);

v=sqrt(1+E2*cos(B)*cos(B));
m0=a*(1-E1);
m2=3/2*E1*m0;
m4=5/4*E1*m2;
m6=7/6*E1*m4;
m8=9/8*E1*m6;
a0=m0+m2/2+3/8*m4+5/16*m6+35/128*m8;
a2=m2/2+m4/2+15/32*m6+7/16*m8;
a4=m4/8+3/16*m6+7/32*m8;
a6=m6/32+m8/16;
a8=m8/128;
X=a0*B-a2*sin(2*B)/2+a4*sin(4*B)/4-a6*sin(6*B)+a8*sin(8*B)/8;

N=c/v;
x=X+N*sin(B)*cos(B)*(l*3600)^2/(2*ro^2)+N*sin(B)*cos(B)^3*(5-t^2+9*YeTa2+4*YeTa2^2)*(l*3600)^4/(24*ro^4)+N*sin(B)*cos(B)^5*(61-58*t^2+t^4)*(l*3600)^6/(720*ro^6);
y=N*cos(B)*l*3600/ro+N*cos(B)^3*(1-t^2+YeTa2)*(l*3600)^3/(6*ro^3)+N*cos(B)^5*(5-18*t^2+t^4+14*YeTa2-58*YeTa2*t^2)*(l*3600)^5/(120*ro^5);
y=y+500000;
end


function [localX,localY] = BL2localXY(B,L,mapStart_x,mapStart_y)
    [localX,localY]=BLH2XY(B,L);
    localX=localX-mapStart_x;
    localY=localY-mapStart_y;

end