利用多个深度计解算水下机器人姿态的方法原理分析与仿真

2024-02-11 22:33:28

姿态仪作为水下机器人系统中一个重要传感器，它的重要性不言而喻，它是水下机器人实现自稳控制的关键传感器。目前在低成本的水下机器人系统中常常采用低成本的mems姿态仪，它主要是通过融合加速度计、角速度计和磁力计来实现对三维空间的姿态信息测量。它的优点是体积小、低成本，但是当水下机器人进行高速机动时，其自身的加速度变化非常大，导致mems姿态仪的姿态输出不稳定和真值相差较大。为了解决这个问题，本文提出了一种利用多个深度计来测量机器人的横滚角和俯仰角的方法，该方法可以不受机器人机动的影响，从而实现稳定的横滚角和俯仰角测量。

理论原理介绍

下面本文具体的来介绍一下该方法的原理以及仿真分析。首先，我们看图1，编号1、2、3、4的黑点分别代表分布在水下机器人表面四周的深度计，它们在一个平面上，机器人载体的坐标系定义如下，四个深度计构成了一个长方形，长为a，宽为b，它们在机器人坐标系的坐标分别为：(b/2,a/2,0)，(b/2,-a/2,0)，(-b/2,-a/2,0)，(-b/2,a/2,0)。我们定义机器人在大地的横滚角(roll)、俯仰角(pitch)、航向角(heading)分别为，，，则有旋转矩阵R,

图1 机器人载体上4个深度计的分布图

分别将4个深度计在机器人载体中的坐标乘上上述旋转矩阵，即可得到四个水听器在大地坐标系中的坐标值，这里我们只考察它们的Z轴坐标值，即它们的深度值。则有，深度计1的深度值，深度计2的深度值，深度计3的深度值，深度计4的深度值。可以看到深度值与航向角无关，则它们的深度差值，这里我们只考虑深度计1与深度计2的差值、深度计1与深度计3的差值、深度计1与深度计4的差值，则、、。这里，我们采用扩展卡尔曼滤波器(EKF)来进行姿态解算，实际上只需要3个深度计即可求得横滚角和俯仰角，考虑到机器人的形状和冗余的需要，所以采用4个深度计。那么最关键的部分在于观测矩阵的线性化处理，这里将，作为状态量进行估计，则观测矩阵为：

仿真分析

下面给出的是具体的基于EKF滤波器的姿态解算matlab程序

clc;
clear all;
close all;
%%
point_num = 600;
fs = 200;
f1 = 5;
f2 = 10;
f3 = 3;
nTs = (0:point_num-1)/fs;
roll_amp = 10/180*pi;
pitch_amp = 5/180*pi;
yaw_amp = 45/180*pi;
roll = roll_amp*sin(2*pi*f1*nTs);    %横滚角
pitch = pitch_amp*cos(2*pi*f2*nTs);  %俯仰角
yaw = yaw_amp*sin(2*pi*f3*nTs);      %航向角
gyro_roll = 2*pi*f1*roll_amp*cos(2*pi*f1*nTs);    %横滚角速度
gyro_pitch = -2*pi*f2*pitch_amp*sin(2*pi*f2*nTs); %俯仰角速度 
gyro_yaw = 2*pi*f3*yaw_amp*cos(2*pi*f3*nTs);
a = 1.5;    %深度计方阵的长和宽
b = 1.2;

R31 = sin(pitch);
R32 = cos(pitch).*sin(roll);

H12 = b*R31;
H13 = b*R31 + a*R32;
H14 = a*R32;
xigma_H = 0.001;  %深度计精度
H12_o = H12 + xigma_H*randn(1,point_num);   %加入噪声，模拟测量过程
H13_o = H13 + xigma_H*randn(1,point_num);
H14_o = H14 + xigma_H*randn(1,point_num);
BWgro = 47; %47角速度计带宽  Hz
xigam_wz = 0.014*BWgro/180*pi; %角速度精度  rec/s
gyro_roll_o = gyro_roll + xigam_wz*randn(1,point_num);
gyro_pitch_o = gyro_pitch + xigam_wz*randn(1,point_num);
%%
figure;
subplot(3,1,1);
plot(nTs,roll/pi*180,'r');
hold on; grid on;
plot(nTs,pitch/pi*180,'b');
legend('roll','pitch');
title('真实姿态角');
subplot(3,1,2);
plot(nTs,H12,'r');
hold on; grid on;
plot(nTs,H13,'b');
plot(nTs,H14,'c');
legend('H12','H13','H14');
title('深度计的真实深度差');
subplot(3,1,3);
plot(nTs,H12_o,'r');
hold on; grid on;
plot(nTs,H13_o,'b');
plot(nTs,H14_o,'c');
legend('H12测量','H13测量','H14测量');
title('深度计测量的深度差');
%%
H = [H12_o;H13_o;H14_o];
wxy = [gyro_roll_o; gyro_pitch_o];
xigma_wxy = [xigam_wz; xigam_wz];
[atti,P] = EKF_deep_atti(H,wxy,xigma_wxy,xigma_H,1/fs,a,b);
%%
% R13_0 = cos(atti(1,:)).*sin(atti(2,:));
% R23_0 = sin(atti(1,:));
% roll_ekf = asin(R13_0.*sin(yaw) + R23_0.*cos(yaw));
% pitch_ekf = asin(R13_0./(cos(roll_ekf).*cos(yaw)) - tan(roll_ekf).*tan(yaw));
roll_ekf = atti(1,:);
pitch_ekf = atti(2,:);
%%
figure;
subplot(3,1,1);
plot(nTs,roll/pi*180,'r');
hold on; grid on;
plot(nTs,pitch/pi*180,'b');
plot(nTs,roll_ekf/pi*180,'c.');
plot(nTs,pitch_ekf/pi*180,'k.');
legend('roll','pitch','roll ekf','pitch ekf');
title('姿态角');
subplot(3,1,2);
plot(nTs,(roll_ekf - roll)/pi*180,'r');
hold on; grid on;
plot(nTs,(pitch_ekf - pitch)/pi*180,'b');
legend('roll误差','pitch误差');
title('姿态角误差');
subplot(3,1,3);
plot(P(1,:),'r');
hold on; grid on;
plot(P(2,:),'b');
legend('roll方差','pitch方差');

其中EKF滤波器具体实现的函数代码如下：

function [atti,P] = EKF_deep_atti(H,wxy,xigma_wxy,xigma_H,dt,a,b)
% H         测量的深度差  深度计1-深度计2  深度计1-深度计3  深度计1-深度计4
% wxy       测量的角速度  横滚角速度   俯仰角度的
% xigma_wxy 角速度方差
% xigma_H   深度计方差
% dt        采样时间间隔
% a         深度计方阵的长
% b         深度计方阵的宽

%%
point_num = size(H,2);

for k = 1:point_num
    if k == 1
        xigma_alfa = 10/180*pi;
        Pk_1_k_1 = [xigma_alfa^2      0          %初始化状态协方差矩阵
                   0       xigma_alfa^2];
        Qk = [xigma_wxy(1)^2      0
                   0         xigma_wxy(2)^2];    %初始化扰动协方差矩阵
        Rk = [xigma_H^2     0        0           %初始化高度计测量协方差矩阵
                 0      xigma_H^2    0
                 0          0     xigma_H^2];
        Xk_1_k_1 = [asin(H(3,1)/a)                          %初始化状态矢量
                    asin(H(1,1)/b/(sqrt(1-(H(3,1)/a)^2)))];    
    end
    d_alfa = wxy(1,k)*dt;
    d_beta = wxy(2,k)*dt;
    Xk_k_1 = Xk_1_k_1 + [d_alfa; d_beta];     %状态预测
    R31 = sin(Xk_k_1(2));
    R32 = cos(Xk_k_1(2))*sin(Xk_k_1(1));
    Zk = [b*R31; b*R31+a*R32; a*R32];  %测量预测
    Pk_k_1 = Pk_1_k_1 + Qk;     %协方差预测
    vk = H(:,k) - Zk;           %新息
    R31_alfa = 0;
    R31_beta = cos(Xk_k_1(2));
    R32_alfa = cos(Xk_k_1(2))*cos(Xk_k_1(1));
    R32_beta = -sin(Xk_k_1(2))*sin(Xk_k_1(1));
    Hk = [b*R31_alfa             b*R31_beta             %观测矩阵
          b*R31_alfa+a*R32_alfa  b*R31_beta+a*R32_beta
          a*R32_alfa             b*R32_beta];       
    Sk = Hk*Pk_k_1*Hk' + Rk;     %新息协方差矩阵
    Wk = Pk_k_1*Hk'*inv(Sk);     %卡尔曼增益
    Xkk = Xk_k_1 + Wk*vk;        %状态更新
    Pkk = Pk_k_1 - Wk*Sk*Wk';    %协方差矩阵更新
    
    Xk_1_k_1 = Xkk;
    Pk_1_k_1 = Pkk;
    
    atti(:,k) = Xkk;
    P(1,k) = Pkk(1);
    P(2,k) = Pkk(3);
end

程序中设定的条件是：深度计误差1mm，深度计方阵尺寸是1.5m x 1.2m。运行一下该程序得到如下结果：

可以看到通过EKF滤波器解算的结果与真实的值相差最多不超过0.1°。

码农公寓

理论原理介绍

仿真分析

相关文章