【滤波跟踪】基于多源信息融合算法实现多旋翼无人机组合导航系统含Matlab源码

 1 简介

ITS(智能交通系统)是一个开放的复杂巨系统,是新兴的交叉学科研究领域。ITS是将先进的信息处理技术、数据通信技术、电子控制技术以及计算机处理技术等有效的综合运用于整个运输管理体系,从而建立起大范围内、全方位发挥作用的实时、准确、高效的管理运输系统。而车辆导航系统时ITS应用研究的一个主要方面,是集定位系统、地理信息系统、数据库查询系统、数字蜂窝通讯技术等于一体的综合系统。近年来,如何使GPS定位技术与INS惯性导航系统更好的结合起来已经成为车辆导航系统研究中的一个重要内容。 本文首先介绍了GPS定位的基本原理。然后重点研究的内容是通过信息融合,使得在定位卫星信号接收差的场合工作更可靠。在GPS/INS组合导航系统中利用卡尔曼滤波去估计系统的各种状态,并用状态的估计值去校正系统。卡尔曼滤波算法的基本思想是:采用信号与噪声的状态空间模型,利用前一时刻的估计值和现在时刻的观测值来更新对状态变量的估计,求现在时刻的估计值。它是一种递推估计算法,通过处理一系列带有误差的实际测量数据而得到的物理参数的最佳估算。最大的特点是能够剔除随机干扰噪声,从而获取逼近真实情况的有用信息。​

2 部分代码

function [ theta, omega ] = caculate_theta( ang_motion, crt )
% 名称：Caculate theta(angule) form the seted angule motion
% 功能：给定预设角运动形式，计算current时刻的角度
% 注意：目前还没有添加规范化姿态角的功能，因此尽量不要把摇摆幅值设的过大
%
% Inputs:
%       ang_motion: 预设角运动，每一行对应着一个形如：
%                   theta = A*sin( w*t + phi) + k的运动，载体角运动由若干个
%                   正弦运动叠加而成
%       crt: 当前时间
% Output:
%       theta: 跟预设角运动所对应的姿态 (rad)
%       omega: 与预设角运动所对应的当前时刻角速度 (rad/s)

% Author: Kun Gan, Tongji University
% Email : ciaotigre@126.com
% Date  : 2020/12/1
%%
% i表示 ang_motion 中的第i行
i = 1;

% 如果ang_motion中存在第i行
while i <= size(ang_motion, 1)
    
    if i == 1
        theta = 0;  % rad
        omega = 0;  % rad/s
    end
    
    % ang_motion.p = [A1(幅值), w1(角速度), phi1(初始相位), k1(中心值)
    %                 A2(幅值), w2(角速度), phi2(初始相位), k2(中心值)
    %                                    ...                         ]
    % 注意:为了编程的简单和简洁，ang_motion中的变量单位为rad或rad/s
    A = ang_motion(i, 1);
    w = ang_motion(i, 2);
    phi = ang_motion(i, 3);
    k = ang_motion(i, 4);
    
    % 姿态角
    theta = A*sin( w*crt + phi) + k + theta;
    
    % wnbnx    nx分别是(n n1), (n1 n2), (n1 b)
    omega = w*A*cos(w*crt + phi) + omega;
    
    % i+1, 准备检查ang_motion是否存在下一行
    i = i+1;
    
end

end
 

function [ att_err_normalized ] = atterrnorml( att_err )
% 名称: Attitude Error Normalization (version 1.0)
% Function: 姿态误差标准化 (目前仅修正航向角误差)
%
% Inputs:
%       att_err: 未标准化的姿态角误差
% Outputs:
%       att_err_normalized: 标准化过后的姿态角误差

% 分别提取三个姿态角误差
pitch_err = att_err(1);
roll_err = att_err(2);
yaw_err = att_err(3);

% pitch 角度范围为: [-pi/2, pi/2]
% roll 角度范围为: (-pi, pi]
% yaw 角度范围为: [0, 2*pi)

% 直接计算出的航向角误差∈(-2*pi, 2*pi),但我们希望航向角误差∈(-pi, pi]
% 如果航向角误差绝对值大于pi，就令其标准化
if norm(yaw_err) > pi
    % 如果航向角误差为正
    if sign(yaw_err) == 1
        yaw_err = yaw_err - 2*pi;
    else
        % 航向角误差大于pi，且符号为负
        yaw_err = yaw_err + 2*pi;
    end
end

att_err_normalized = [pitch_err, roll_err, yaw_err]';

end

3 仿真结果

4 参考文献

[1]张欣. 多旋翼无人机的姿态与导航信息融合算法研究[D]. 中国科学院大学.

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。