Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

Estimación de la pose de un vehículo submarino autónomo mediante sensores inerciales y registros de velocidad Doppler

Este ejemplo muestra cómo fusionar datos de un GPS, un registro de velocidad Doppler (DVL) y sensores de unidades de medición inercial (IMU) para estimar la pose de un vehículo submarino autónomo (AUV) que se muestra en esta imagen.

Fondo

Un sensor DVL es capaz de medir la velocidad de un vehículo submarino cuando está cerca del fondo del mar. Por el contrario, el sensor GPS puede medir la posición y la velocidad del vehículo mientras está cerca de la superficie mediante satélites visibles. El sensor IMU ayuda a estos dos sensores a estimar la pose.

Mientras el AUV está buceando, hay un período en el que no se pueden usar ni GPS ni DVL porque no se puede alcanzar los satélites GPS y el fondo del mar no está dentro de una distancia escaneable, lo que requiere que la IMU sea la única que estime la pose. Incluso cuando se utiliza una IMU de alta calidad, como un giroscopio de fibra óptica o de láser anular, la estimación de la pose sigue acumulando errores cuando se utiliza la IMU por sí sola.

La falta de un segundo sensor de orientación, como un magnetómetro, da como resultado una estimación de guiñada inferior para el AUV en relación con las estimaciones de cabeceo y balanceo. Esto se debe a que la IMU utiliza tanto su giroscopio como su acelerómetro para las estimaciones de cabeceo y balanceo, pero utiliza sólo el giroscopio para las estimaciones de guiñada.

Este ejemplo le muestra cómo combinar los sensores IMU, GPS y DVL en un filtro de estimación de pose para mitigar los errores de IMU. Además, muestra cómo reducir el error suavizando toda la estimación del filtro.

Configuración

Establezca la semilla del generador de números aleatorios (RNG) para garantizar resultados repetibles. Luego cree una instancia del objeto auxiliar de ejemplo ExampleHelperAUVTrajectoryPlotter, que le permitirá visualizar los resultados.

rng(1)
plotter = ExampleHelperAUVTrajectoryPlotter;

Crear trayectoria de inmersión y superficie

Para comprender la relación entre los sensores del AUV y la deriva, se necesita una trayectoria simple para el AUV que actúe como ground-truth para la pose del AUV. Siguiendo la trayectoria, el AUV debería viajar a la superficie, sumergirse cerca del fondo del mar, viajar a lo largo del fondo del mar y, finalmente, resurgir.

Trayectoria de diseño

Debido a que las velocidades de los sensores determinan la trayectoria, debe configurar las velocidades a las que se actualizan los sensores IMU, GPS y DVL para las simulaciones de sensores.

imuRate = 200; % 200 Hz update rate for IMU sensor simulation.
gpsRate = 1;   % 1 Hz update rate for GPS sensor simulation.
dvlRate = 5;   % 5 Hz update rate for DVL sensor simulation.

Especifique las tasas de actualización del sensor para la función auxiliar exampleHelperDiveAndSurfaceTrajectory para crear la trayectoria del AUV. Esta trayectoria es ground-truth para comparar con los datos del sensor simulado. La función también devuelve las profundidades a las que los sensores GPS y DVL tienen mediciones confiables.

[imuTraj,gpsTraj,dvlTraj,gpsDepth,dvlDepth] = exampleHelperDiveAndSurfaceTrajectory(imuRate,gpsRate,dvlRate);

Tenga en cuenta que la profundidad máxima de inmersión para el AUV es de 10 metros para este ejemplo. Esta pequeña profundidad ilustra el efecto de los sensores que entran y salen del uso mientras se mantiene la trayectoria lo suficientemente pequeña como para permitir un ajuste de filtro más rápido.

Almacene la trayectoria ground-truth en un objeto timetable para usarlo en comparación y ajuste de filtro.

groundTruth = timetable(imuTraj.Position, ...
                        imuTraj.Orientation, ...
                        VariableNames=["Position","Orientation"], ...
                        SampleRate=imuRate);

Visualizar trayectoria

Utilice la función de objeto visualizeTrajectory del objeto auxiliar para trazar la trayectoria de ground-truth con las regiones de operación de cada sensor.

plotter.visualizeTrajectory(groundTruth,gpsDepth,dvlDepth);

Generar datos de sensores y estimar la pose

Utilice la función auxiliar exampleHelperAUVSensorData para generar datos de sensores para los sensores IMU, GPS y DVL montados en el AUV.

sensorData = exampleHelperGenerateAUVSensorData(imuTraj,imuRate,gpsTraj,gpsRate,dvlTraj,dvlRate,gpsDepth,dvlDepth);

Para estimar la pose del AUV, debe fusionar los datos del sensor mediante un filtro. En este caso, utilice un filtro Kalman ampliado con algo de ruido de medición simulado.

Cargue un objeto insEKF ajustado filt y el ruido de medición del sensor tmn. Para obtener más información sobre cómo ajustar el filtro, consulte la sección Tune Pose Filter .

load("auvPoseEstimationTunedFilter.mat","filt","tmn");

Utilice la función de objeto estimateStates para obtener una estimación de filtro de las poses y una estimación más suave de las poses.

[poseEst,smoothEst] = estimateStates(filt,sensorData,tmn);

Mostrar resultados

Analice la salida del filtro y suavice usando estos:

1. Error RMS de posición de la estimación del filtro, con regiones operativas del sensor.

2. Posición de ground-truth, en comparación con el filtro y estimaciones más suaves.

3. Subtramas de ground-truth y la estimación más fluida.

Cree un gráfico para mostrar el error RMS de posición cuando haya varios sensores disponibles.

plotter.plotPositionRMS(groundTruth,poseEst,gpsDepth,dvlDepth);

Tenga en cuenta que el error aumenta cuando solo la IMU está disponible para la estimación de posición.

Trace la estimación del filtro y ground-truth juntas para comparar los resultados. Como se ve en el gráfico anterior, la estimación varía cuando solo está disponible la IMU. El filtro corrige la deriva cuando las mediciones DVL están disponibles.

plotter.plotFilterEstimate(groundTruth,poseEst,gpsDepth,dvlDepth);

Compare la estimación más sencilla con ground-truth. El modo más suave elimina la deriva en los estados de posición cuando solo está disponible la IMU.

plotter.plotSmootherEstimate(groundTruth,smoothEst,gpsDepth,dvlDepth);

Muestre la estimación de posición más suave en toda la trayectoria y compárela con la trayectoria real del AUV.

plotter.animateEstimate(groundTruth,smoothEst,gpsDepth,dvlDepth);

información adicional

El ejemplo mostró cómo las diferentes regiones de operación de cada sensor en un AUV afectan la precisión de la estimación de la pose. Estas secciones muestran el modelo de simulación del sensor DVL utilizado en este ejemplo y cómo se ha ajustado el objeto insEKF .

Modelo de simulación DVL

Este ejemplo modela las velocidades del haz DVL usando esta ecuación:

$\underset{}{\overset{‾}{y_{v_i}}}=[v_{t/p}^p(1+S{F}_{DVL})+{\omega }_{t.p}^p\times l_{D_i}]\times b_i+n_{v_i}+b_{DVLi},\text{where:}\underset{}{\overset{‾}{y_{v_i}}}\text{=}\text{DVL}\text{beam}\text{velocity}{v}_{t/p}^p\text{=}\text{Velocity}\text{of}\text{the}\text{platform}S{F}_{DVL}\text{=}\text{Scale}\text{factor}\text{error}{\omega }_{t/p}^p\text{=}\text{Angular}\text{velocity}\text{of}\text{the}\text{platform}{l}_{D_i}\text{=}\text{Position}\text{vector}\text{from}\text{origin}\text{of}\text{DVL}\text{sensor}\text{to}\text{each}\text{transducer,}\text{i}\text{=}\text{1,}\text{2,}\text{3,}\text{4}{b}_i\text{=}\text{Direction}\text{of}\text{each}\text{beam}\text{=}\left[\begin{array}{c}\cos \underset{}{\overset{\sim }{{\psi }_i}}\sin \underset{}{\overset{\sim }{\theta }}\\ \sin \underset{}{\overset{\sim }{{\psi }_i}}\sin \underset{}{\overset{\sim }{\theta }}\\ \cos \underset{}{\overset{\sim }{\theta }}\end{array}\right]\text{,}\text{i}\text{=}\text{1,}\text{2,}\text{3,}\text{4}\underset{}{\overset{\sim }{{\psi }_i}}\text{=}\text{Yaw}\text{angle}\text{=}(i-1)90°+45°\text{,}\text{i}\text{=}\text{1,}\text{2,}\text{3,}\text{4}\underset{}{\overset{\sim }{\theta }}\text{=}\text{Pitch}\text{angle}\text{=}20°n_{v_i}\text{=}\text{Measurement}\text{noise}{b}_{DVL,i}\text{=}\text{Bias}\text{of}\text{each}\text{beam,}\text{i}\text{=}\text{1,}\text{2,}\text{3,}\text{4}$

La simulación que se muestra en el ejemplo realiza un cálculo de mínimos cuadrados para obtener la medición de velocidad del vehículo basada en DVL. El DVL genera valores NaN cuando el vehículo está por encima del dvlDepth.

Ajuste el filtro de pose

El ejemplo ajusta el objeto insEKF usando la función auxiliar exampleHelperTuneAUVPoseFilter . Para volver a ajustar el filtro, establezca el parámetro tuneFilter en true.

La clase ExampleHelperINSDVL es un complemento de sensor simple que permite al insEKF fusionar datos DVL.

classdef ExampleHelperINSDVL < positioning.INSSensorModel
%ExampleHelperINSDVL Template for sensor model using insEKF
%
%   This function is for use with AUVPoseEstimationIMUGPSDVLExample only.
%   It to be removed in the future. 
%
%   See also insEKF, positioning.INSSensorModel.

%   Copyright 2022 The MathWorks, Inc. 

    methods
       
        function z = measurement(~,filt)
            z = stateparts(filt,'Velocity');
        end

        function dzds = measurementJacobian(~,filt)
 
            N = numel(filt.State); % Number of states

            dzds = zeros(3,N,"like",filt.State);
            idx = stateinfo(filt,'Velocity');

            dzds(1,idx) = [1 0 0];
            dzds(2,idx) = [0 1 0];
            dzds(3,idx) = [0 0 1];
        end
     
    end
end

tuneFilter = false;
if tuneFilter
    [filt,tmn] = exampleHelperTuneAUVPoseFilter(sensorData,groundTruth); %#ok<UNRCH>
    save("auvPoseEstimationTunedFilter.mat","filt","tmn");
end

Consulte también

insEKF | imuSensor | gpsSensor

Temas relacionados

• Combinar datos del sensor inercial utilizando insEKF-Marco de fusión flexible basado en