【matlab数学建模项目】matlab实现捷联解算四元数法INS和GPS位置组合导航研究——GPS导航

阿里matlab建模师

1000人浏览 · 2025-03-14 12:48:55

阿里matlab建模师 · 2025-03-14 12:48:55 发布

MATLAB实现捷联解算四元数法INS和GPS位置组合导航研究

1、项目下载：

本项目完整讲解和全套实现源码见下资源，有需要的朋友可以点击进行下载

说明	文档（点击下载）
全套源码+学术论文	matlab实现捷联解算四元数法INS和GPS位置组合导航研究-导航系统-四元数法-卡尔曼滤波-GPS-INS-matlab

更多阿里matlab精品数学建模项目可点击下方文字链接直达查看：

300个matlab精品数学建模项目合集（算法+源码+论文）

2、项目介绍：

摘要

随着导航技术的不断发展，高精度导航系统的需求日益增长。捷联式惯性导航系统（INS）与全球定位系统（GPS）的组合导航技术，通过融合两者的优势，有效克服了单一系统的局限性，提高了定位精度和可靠性。本文深入研究了捷联解算四元数法INS与GPS位置组合导航的原理、流程，并通过实验对比了两者在位置和速度误差上的差异，同时展示了组合导航系统的轨迹对比结果。此外，本文还提供了相应的MATLAB源代码及运行步骤，以便读者进行复现和进一步研究。

关键词
捷联式惯性导航系统；全球定位系统；四元数法；卡尔曼滤波；组合导航；误差分析

一、引言

导航技术在军事、民用等领域具有广泛的应用。传统的导航方式，如惯性导航和卫星导航，各自存在局限性。惯性导航系统（INS）虽然能够提供连续的角速率和加速度测量，但长时间运行会产生累积误差；而全球定位系统（GPS）虽然能提供绝对的地理位置信息，但存在接收信号差、卫星遮挡等问题。因此，将INS与GPS进行组合导航，通过算法融合两者的数据，成为一种提高定位精度和可靠性的有效手段。

二、捷联解算四元数法INS和GPS位置组合导航

1.原理

（1）INS原理
捷联式惯性导航系统（INS）基于陀螺仪和加速度计的数据，通过积分和滤波算法计算出航向、姿态和速度等运动参数。陀螺仪用于测量载体的角速度，加速度计用于测量载体的线加速度。然而，由于传感器存在漂移和噪声，长时间运行会产生累积误差，导致定位精度下降。

为了减小累积误差，可以采用四元数法进行姿态解算。四元数法是一种描述旋转的数学工具，相比传统的欧拉角法，四元数法具有避免万向锁、计算效率高等优点。通过四元数法，可以实时更新载体的姿态矩阵，进而计算出载体的速度和位置。

（2）GPS原理
全球定位系统（GPS）是一种基于卫星的导航系统，能够提供绝对的地理位置信息。GPS接收器通过接收来自多个卫星的信号，解算出载体的位置和时间信息。虽然GPS具有定位精度高、更新速度快等优点，但也存在接收信号差、卫星遮挡等问题，导致在某些环境下定位精度下降。

2.流程

a.初始化
在组合导航系统的初始化阶段，利用GPS的初始位置信息对INS进行校准。通过对比GPS提供的位置信息与INS预测的位置信息，可以计算出INS的初始误差，并对INS的内部状态进行修正。同时，记录下当前的时间同步点，以便后续进行时间同步。

b.数据融合
在运行过程中，组合导航系统不断接收GPS位置和时间更新，并与INS的内部状态（如角速度和位置估计）进行融合。常用的融合算法包括卡尔曼滤波（Kalman Filter）或其他更高级的滤波方法。卡尔曼滤波是一种递推估计算法，通过预测和更新两个步骤，对系统状态进行最优估计。在组合导航系统中，卡尔曼滤波可以实时地融合INS和GPS的数据，提高定位精度。

c.错误处理
对于GPS的短暂失锁或INS的短期故障，组合导航系统需要采取相应的错误处理措施。一种常用的方法是使用历史数据进行平滑过渡或临时保持上一时刻的状态。这样可以避免由于单一系统故障导致的定位精度下降，提高系统的可靠性。

d.位置更新
在每次接收到GPS位置和时间更新后，组合导航系统需要实时更新并修正INS的预测位置。通过对比GPS提供的位置信息与INS预测的位置信息，可以计算出INS的误差，并对INS的内部状态进行修正。这样可以使INS的预测位置尽可能接近GPS提供的位置，提高定位精度。

3.位置和速度误差比较及轨迹对比

（1）位置和速度误差比较
通过分析INS和GPS的输出，可以发现两者在位置和速度上的差异。通常，GPS在静态区域误差较小，因为静态环境下GPS信号接收稳定，定位精度高。而INS在动态且环境变化大的地方误差较大，因为动态环境下传感器漂移和噪声对INS的影响更加显著。

为了定量评估组合导航系统的性能，可以计算INS和GPS在位置和速度上的均方根误差（RMSE）。通过对比RMSE值，可以直观地看出组合导航系统在提高定位精度方面的优势。

（2）轨迹对比
利用数学模型和算法，可以生成并对比INS和GPS的追踪轨迹。通过对比两者的轨迹，可以找出异常值并进行后期的数据处理和校正。同时，轨迹对比也可以直观地展示组合导航系统在提高定位精度和可靠性方面的效果。

三、源代码和运行步骤

1.MATLAB源代码（全套源码见下载资源）

以下是实现捷联解算四元数法INS和GPS位置组合导航的MATLAB源代码示例：

% 初始化参数
dt = 0.01; % 采样时间间隔（秒）
N = 1000; % 采样点数

% INS参数
gyro_bias = 0.01; % 陀螺仪漂移（度/秒）
accel_bias = 0.01; % 加速度计漂移（米/秒^2）
gyro_noise = 0.001; % 陀螺仪噪声（度/秒）
accel_noise = 0.001; % 加速度计噪声（米/秒^2）

% GPS参数
gps_error = 1; % GPS定位误差（米）
gps_update_rate = 1; % GPS更新频率（秒）

% 载体真实运动轨迹（示例）
true_position = zeros(N, 3);
true_velocity = zeros(N, 3);
true_attitude = zeros(N, 4); % 四元数表示姿态

% 模拟真实运动（示例）
for i = 1:N
if mod(i, gps_update_rate) == 0
% 更新真实位置（示例：匀速直线运动）
true_position(i, :) = true_position(i-1, :) + true_velocity(i-1, :) * dt;

% 更新真实速度（示例：恒定速度）
true_velocity(i, :) = [1, 0, 0];

% 更新真实姿态（示例：恒定姿态）
true_attitude(i, :) = [1, 0, 0, 0]; % 单位四元数表示无旋转
else
true_position(i, :) = true_position(i-1, :);
true_velocity(i, :) = true_velocity(i-1, :);
true_attitude(i, :) = true_attitude(i-1, :);
end
end

% INS测量值（模拟）
gyro_measurement = zeros(N, 3);
accel_measurement = zeros(N, 3);

% 添加漂移和噪声
for i = 1:N
gyro_measurement(i, :) = true_attitude_to_euler(true_attitude(i, :))' * 57.2957795 + gyro_bias + gyro_noise * randn(1, 3); % 转换为度/秒
accel_measurement(i, :) = true_velocity(i, :) / dt + accel_bias + accel_noise * randn(1, 3);
end

% INS状态估计
ins_position = zeros(N, 3);
ins_velocity = zeros(N, 3);
ins_attitude = zeros(N, 4);

% 初始化INS状态
ins_position(1, :) = true_position(1, :);
ins_velocity(1, :) = true_velocity(1, :);
ins_attitude(1, :) = true_attitude(1, :);

% INS解算（四元数法）
for i = 2:N
% 角速度更新
omega = gyro_measurement(i, :) - gyro_bias;
omega_quat = quaternion_from_euler(omega);

% 姿态更新
ins_attitude(i, :) = quaternion_multiply(ins_attitude(i-1, :), quaternion_exp(omega_quat * dt));
ins_attitude(i, :) = quaternion_normalize(ins_attitude(i, :));

% 加速度更新
accel_body = accel_measurement(i, :) - accel_bias;
accel_world = quaternion_rotate_vector(ins_attitude(i, :), accel_body);

% 速度更新
ins_velocity(i, :) = ins_velocity(i-1, :) + accel_world * dt;

% 位置更新
ins_position(i, :) = ins_position(i-1, :) + ins_velocity(i, :) * dt;
end

% GPS测量值（模拟）
gps_position = true_position + gps_error * randn(N, 3);
gps_time = (0:N-1) * dt;

% 卡尔曼滤波融合
Q = diag([gyro_noise^2, gyro_noise^2, gyro_noise^2, accel_noise^2, accel_noise^2, accel_noise^2]); % 过程噪声协方差
R = diag([gps_error^2, gps_error^2, gps_error^2]); % 测量噪声协方差
x = [ins_position(1, :); ins_velocity(1, :); quaternion2euler(ins_attitude(1, :))']; % 初始状态
P = eye(9); % 初始误差协方差

kf_position = zeros(N, 3);
kf_velocity = zeros(N, 3);
kf_attitude = zeros(N, 3);

for i = 1:N
if mod(i, gps_update_rate) == 0
% 预测
x_pred = F * x;
P_pred = F * P * F' + Q;

% 更新
z = [gps_position(i, :)'; 0; 0]; % 测量值（位置）
y = z - H * x_pred;
S = H * P_pred * H' + R;
K = P_pred * H' / S;
x = x_pred + K * y;
P = (eye(9) - K * H) * P_pred;

% 提取状态
kf_position(i, :) = x(1:3);
kf_velocity(i, :) = x(4:6);
kf_attitude(i, :) = x(7:9);
else
kf_position(i, :) = kf_position(i-1, :);
kf_velocity(i, :) = kf_velocity(i-1, :);
kf_attitude(i, :) = kf_attitude(i-1, :);
end
end

% 绘图
figure;
subplot(3, 1, 1);
plot(gps_time, true_position(:, 1), 'b', 'LineWidth', 1.5); hold on;
plot(gps_time, ins_position(:, 1), 'r--', 'LineWidth', 1.5);
plot(gps_time, kf_position(:, 1), 'g-.', 'LineWidth', 1.5);
xlabel('时间 (秒)');
ylabel('X 位置 (米)');
legend('真实位置', 'INS 位置', '组合导航位置');
title('X 方向位置对比');
grid on;

subplot(3, 1, 2);
plot(gps_time, true_position(:, 2), 'b', 'LineWidth', 1.5); hold on;
plot(gps_time, ins_position(:, 2), 'r--', 'LineWidth', 1.5);
plot(gps_time, kf_position(:, 2), 'g-.', 'LineWidth', 1.5);
xlabel('时间 (秒)');
ylabel('Y 位置 (米)');
legend('真实位置', 'INS 位置', '组合导航位置');
title('Y 方向位置对比');
grid on;

subplot(3, 1, 3);
plot(gps_time, true_position(:, 3), 'b', 'LineWidth', 1.5); hold on;
plot(gps_time, ins_position(:, 3), 'r--', 'LineWidth', 1.5);
plot(gps_time, kf_position(:, 3), 'g-.', 'LineWidth', 1.5);
xlabel('时间 (秒)');
ylabel('Z 位置 (米)');
legend('真实位置', 'INS 位置', '组合导航位置');
title('Z 方向位置对比');
grid on;

function q = quaternion_from_euler(euler)
% 将欧拉角转换为四元数
cy = cos(euler(1) / 2);
sy = sin(euler(1) / 2);
cp = cos(euler(2) / 2);
sp = sin(euler(2) / 2);
cr = cos(euler(3) / 2);
sr = sin(euler(3) / 2);

q = [cy * cp * cr + sy * sp * sr;
cy * cp * sr - sy * sp * cr;
cy * sp * cr + sy * cp * sr;
sy * cp * cr - cy * sp * sr];
end

function v_rot = quaternion_rotate_vector(q, v)
% 使用四元数旋转向量
q_conj = [q(1), -q(2), -q(3), -q(4)];
q_v = [0, v(1), v(2), v(3)];
q_result = quaternion_multiply(quaternion_multiply(q, q_v), q_conj);
v_rot = [q_result(2), q_result(3), q_result(4)];
end

function q_result = quaternion_multiply(q1, q2)
% 四元数乘法
q_result = [q1(1) * q2(1) - q1(2) * q2(2) - q1(3) * q2(3) - q1(4) * q2(4);
q1(1) * q2(2) + q1(2) * q2(1) + q1(3) * q2(4) - q1(4) * q2(3);
q1(1) * q2(3) - q1(2) * q2(4) + q1(3) * q2(1) + q1(4) * q2(2);
q1(1) * q2(4) + q1(2) * q2(3) - q1(3) * q2(2) + q1(4) * q2(1)];
end

function q = quaternion_exp(q_omega)
% 四元数指数函数
theta = norm(q_omega(2:4));
if theta == 0
q = [1; 0; 0; 0];
else
q_omega_norm = q_omega / theta;
sin_theta = sin(theta);
q = [cos(theta); sin_theta * q_omega_norm(2); sin_theta * q_omega_norm(3); sin_theta * q_omega_norm(4)];
end
end

function q = quaternion_normalize(q)
% 四元数归一化
norm_q = norm(q);
if norm_q == 0
q = [1; 0; 0; 0];
else
q = q / norm_q;
end
end

function euler = quaternion2euler(q)
% 将四元数转换为欧拉角
test = q(1)*q(2) + q(3)*q(4);
if test > 0.499
% 奇异点附近
yaw = 2 * atan2(q(2),q(1));
pitch = atan2(2*(q(1)*q(3) - q(2)*q(4)), 1 - 2*(q(2)^2 + q(3)^2));
roll = 0;
elseif test < -0.499
% 奇异点附近
yaw = -2 * atan2(q(2),q(1));
pitch = atan2(2*(q(1)*q(3) - q(2)*q(4)), 1 - 2*(q(2)^2 + q(3)^2));
roll = 0;
else
% 正常情况
sinr_cosp = 2*(q(1)*q(4) + q(2)*q(3));
cosr_cosp = 1 - 2*(q(2)^2 + q(3)^2);
roll = atan2(sinr_cosp, cosr_cosp);

sinp = 2*(q(1)*q(3) - q(2)*q(4));
pitch = asin(sinp);

siny_cosp = 2*(q(1)*q(2) + q(3)*q(4));
cosy_cosp = 1 - 2*(q(2)^2 + q(4)^2);
yaw = atan2(siny_cosp, cosy_cosp);
end
end