目标定位基于matlab扩展卡尔曼滤波器多机器人定位含Matlab源码 2327期

Posted 2023-01-18 海神之光

⛄一、简介

1 机器人运动模型分析
1.2 坐标系
一个是全局坐标系(Xw,Yw), 另一个是机器人的局部坐标系(Xr,Yr). 如下图所示:

图1 机器人坐标系
室内环境用二维平面表示在全局坐标系XwOwYw中,机器人坐标系表示为XrOrYr. 机器人的位姿用三维状态矢量(x,y,θ)表示. 其中, 机器人在全局坐标系中的坐标(x,y)表示机器人的位置; θ 表示机器人的运动方向, 它是以正前方为起点的逆时针方向来定义的,如图1 所示, 取值范围为-π ~π .

1.2 里程计模型
里程计模型可以分为直线模型和圆弧模型. 由于圆弧模型考虑机器人在运动过程中航向角的变化, 用圆弧近似机器人移动的实际轨迹. 虽然圆弧模型的计算复杂度要高于直线模型, 但能更好地逼近实际轨迹.如图2 所示, 机器人从点B运动到点B’的过程:

图2 机器人运动位姿
假设移动机器人在第k个采样时刻(在B点)的状态为X (k ) =[ x( k ) , y (k ) ,θ (k )]T. 同理, 在第k +1个采样时刻( 在B’ 点) 时, 机器人的系统状态为X(k+1)=[x(k+1),y(k+1),θ(k+1)]T. 结合图2 中几何关系,系统的状态方程为:

其中, U(k) =(∇D(k),∇θ(k))为里程计的输入模型.ΔD(k)和 Δθ(k)分别是机器人在第k个采样周期内的位置增量和方向增量. W(k)是动态的系统高斯白噪声,其统计特性为:

由公式可知, 里程计模型是一个非线性模型, 由误差传递近似公式可知, 机器人在X(k+1)位姿下的误差协方差矩阵更新方程为:

其中, P(k/ k)为本次机器人位姿误差协方差矩阵,F(x(k),u(k))为里程计模型方程,∇Fx和∇Fu分别为F对机器人位姿x(k)和输入向量u(k +1)的雅克比矩阵.

1.3 视觉探测模型
Li表示视觉传感器探测到的第i个路标的位置.系统的状态由机器人所在位置和在此观测到的路标组成, 表示为x( k ) =[xvT(k ),L1T,…,LNT]T, N表示路标的个数.

如图3 所示, 根据几何关系可以得到如下的系统观测方程

其中,zi(k)为双目视觉传感器对于第i个路标的观测输出,νi是动态的观测高斯白噪声, 并具有如下的统计特性:

图3 SLAM场景示意图

2 EKF算法分析
离散的非线性系统为

其中, X(k)是n维系统状态向量, z(k)是m维观测向量.F是nn阶的系统状态转移矩阵, 它反映了系统从k-1个采样时刻的状态到第k个采样时刻的状态转换. 假设k-1 时刻到k时刻的物理时差为Δt时, F的形式可表示为

H是mn阶观测矩阵, 它表示了从状态X(k)到观测值z(k)的转换, 其形式可表示为

ω(k)为系统噪声, ν(k)是观测噪声. 他们的统计特性可以表示为

系统(4)相应的扩展卡尔曼滤波算法是:

性质1 EKF算法最优的充要条件是其滤波增益矩阵满足

性质2 EKF算法最优的充要条件是其新息序列是零均值白噪声.

⛄二、部分源代码

clear all;
close all;
clc;
%##########################################################################

% Number of robots in a team
numRobots=5;

% Discrete counter for the length of simulation (larger number, longer simulation)
simulationLength=50;

% Number of times the simulation is repeated under the same initial…
% and control conditions (for statistical analysis).
numRuns=1;

%##########################################################################

% create robots with random initial ground truths…
% and specified sensor errors.

for i=1:numRobots

% robot = createRobot( initialgroundTruth, initialPoseError, encoderError, sensorError, distanceBetweenWheels)
rng(‘shuffle’);
robots(i)= createRobot([5randn,5randn,normalizeAngle(5*randn)],[0.15,0.15,0.15],[0.02,0.02],[0.1,0.1],0.5);

end

% create a copy of initialized robots for repeated simulations under…
% the same initial starting conditions an sensor noise
robots_copy=robots;

%##########################################################################

% randomly choose moving and stationary robots…
% at each step of the simulation

for i=1:simulationLength

%   randomly choose up to (numRobots-1) robots to move
movingRobotsi=randsample(numRobots,randsample(numRobots-1,1));

%   generate control inputs for each of the moving robots
for j=1:length(movingRobotsi)
    uti,j=abs(0.1*randn(10,2));
end

%   the rest of the robots (at least one) remain stationary
stationaryRobotsi=setdiff(1:length(robots),movingRobotsi);

end

%##########################################################################

% simulate numRuns times
for i=1:numRuns
disp(['Run ',num2str(i), ’ of ', num2str(numRuns)]);

%   use a fresh copy of robots and subject them to the same motion...
%   sequences and control inputs
robots=robots_copy;

%   for each step within a run
for j=1:simulationLength
    
    %   select a robot to move
    
    for k=1:length(movingRobotsj)
        
        movingRobot=movingRobotsj(k);
        
        %   localize the selected moving w.r.t first stationary robot.
        
        stationaryRobot=stationaryRobotsj(1);
        
        robots(movingRobot)=localizeRobot(robots(movingRobot),robots(stationaryRobot), utj,k);
                    
        %   localize the selected moving robot w.r.t the rest of stationary robots
        
        for l=2:length(stationaryRobotsj)
            
            stationaryRobot=stationaryRobotsj(l);
            
            robots(movingRobot)=localizeRobot(robots(movingRobot),robots(stationaryRobot),[0,0]);
            
        end
                    
    end
    
    
end
%   save the copies of robots for statistical analysis (evaluation of ANEES)
robotsNRuns(i,:)=robots;

end

%##########################################################################

% compute averages.
for i=1:length(robots)

robotsAverage(i)=computeAverages(robotsNRuns(:,i));

end

⛄三、运行结果

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]孟祥萍,张本法,苑全德.自适应扩展卡尔曼滤波器在移动机器人定位中的应用[J].计算机系统应用. 2015,24(12)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除