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 orientación mediante mediciones de orientación derivadas del GPS

Este ejemplo utiliza:

Este ejemplo muestra cómo definir y utilizar un modelo de sensor personalizado para el objeto insEKF junto con los modelos de sensor integrados. Utilizando un sensor de ángulo de guiñada personalizado, un acelerómetro y un giroscopio, este ejemplo utiliza el objeto insEKF para determinar la orientación de un vehículo. Utiliza la velocidad de un receptor GPS para calcular la orientación del vehículo. Siguiendo un enfoque similar al que se muestra en este ejemplo, puede desarrollar modelos de sensores personalizados para sus propias aplicaciones de fusión de sensores.

Este ejemplo requiere la Sensor Fusion and Tracking Toolbox o la Navigation Toolbox. Este ejemplo también utiliza opcionalmente MATLAB Coder para acelerar el ajuste del filtro.

Descripción del problema

Está intentando estimar la orientación de un vehículo mientras está en movimiento. Los únicos sensores disponibles son un acelerómetro, un giroscopio y un receptor GPS que genera una estimación de velocidad. No se puede utilizar un magnetómetro porque hay una gran cantidad de interferencia magnética en el vehículo.

Acercarse

Hay varias herramientas disponibles en la toolbox para determinar la orientación, incluidas

ecompass
imufilter
ahrsfilter
complementaryFilter

Sin embargo, estos filtros requieren alguna combinación de un acelerómetro, un giroscopio y/o un magnetómetro. Si necesita determinar la orientación absoluta (en relación con el norte verdadero) y no tiene un magnetómetro disponible, ninguno de estos filtros es ideal. Los objetos imufilter y complementarioFilter fusionan datos de acelerómetro y giroscopio, pero solo proporcionan una orientación relativa. Además, si bien estos filtros pueden estimar y compensar la polarización del giroscopio, no hay ningún sensor adicional para ayudar a rastrear la polarización del giroscopio para el ángulo de guiñada, lo que puede producir una estimación subóptima. Normalmente, se utiliza un magnetómetro para lograr esto. Sin embargo, como se mencionó, los magnetómetros no se pueden utilizar en situaciones con perturbaciones magnéticas grandes y muy variables. (Tenga en cuenta que el objeto ahrsfilter puede soportar perturbaciones magnéticas leves durante un corto período de tiempo).

En este ejemplo, para obtener la orientación absoluta, utiliza la estimación de velocidad del GPS para determinar el ángulo de guiñada. Esta estimación del ángulo de guiñada puede tener el mismo propósito que un magnetómetro sin problemas de perturbaciones magnéticas. Construirá un sensor personalizado en el marco insEKF para modelar este sensor sin procesar basado en GPS.

Trayectoria

Primero, crea una trayectoria ground-truth y simula el sensor usando las funciones exampleHelperMakeTrajectory y exampleHelperMakeIMUGPSData adjuntas a este ejemplo.

groundTruth = exampleHelperMakeTrajectory;
originalSensorData = exampleHelperMakeIMUGPSData(groundTruth);

Reajustados como un calendario, los datos del sensor original incluyen el acelerómetro, el giroscopio y los datos de velocidad del GPS. Transforme los datos de velocidad del GPS en una estimación del ángulo de guiñada.

velyaw = atan2(originalSensorData.GPSVelocity(:,2), originalSensorData.GPSVelocity(:,1));

Los resultados anteriores suponen restricciones no holonómicas. Es decir, suponen que el vehículo apunta en la dirección del movimiento. Esta suposición es generalmente cierta para algunos vehículos, como un automóvil, pero no para otros vehículos, como un cuadricóptero.

Crear datos del sensor de ángulo de guiñada sintético

Es difícil trabajar con el ángulo de guiñada porque el ángulo de guiñada se ajusta a $π$ y $- π$ . El ángulo que salta en el límite de envoltura podría causar divergencia del filtro de Kalman extendido. Para evitar este problema, convierta el ángulo de guiñada en un cuaternión.

velyaw(:,2) = 0; % set pitch and roll to 0 
velyaw(:,3) = 0;
qyaw = quaternion(velyaw, 'euler', 'ZYX', 'frame');

Una convención común es forzar que el cuaternión tenga una parte real positiva.

isNegQuat = parts(qyaw) < 0; % Find quaternions with a non-negative real part 
qyaw(isNegQuat) = -qyaw(isNegQuat); % Invert the negative quaternions.

Tenga en cuenta que cuando el objeto insEKF rastrea un estado Orientation de 4 elementos como en este ejemplo, asume que el estado Orientation es un cuaternión y obliga a que el cuaternión se normalice y tener una parte real positiva. Puede desactivar esta regla llamando al estado de otra manera, como "Actitud".

Ahora puede crear el cronograma de los datos del sensor que se fusionarán.

sensorData = removevars(originalSensorData, 'GPSVelocity');
sensorData = addvars(sensorData,compact(qyaw),'NewVariableNames','YawSensor');

Cree un sensor `insEKF` para fusionar el ángulo de guiñada

Para fusionar el cuaternión del ángulo de guiñada, personaliza un modelo de sensor y lo utiliza junto con los insAccelerometer y insGyroscope en el objeto insEKF. . Para personalizar el modelo del sensor, crea una clase que hereda de la clase de interfaz positioning.INSSensorModel e implementa el método de medición. Nombra la clase exampleHelperYawSensor

classdef exampleHelperYawSensor < positioning.INSSensorModel
%EXAMPLEHELPERYAWSENSOR Yaw angle as quaternion sensor
% This class is for internal use only. It may be removed in the future.


%   Copyright 2021 The MathWorks, Inc.    

    methods
        function z = measurement(~, filt)
            % Return an estimate of the measurement based on the
            % state vector of the filter saved in filt.State.
            %
            % The measurement is just the quaternion converted from the yaw
            % of the orientation estimate, assuming roll=0 and pitch=0.
            
            q = quaternion(stateparts(filt, "Orientation"));
            eul  = euler(q, 'ZYX', 'frame');
            yawquat = quaternion([eul(1) 0 0], 'euler', 'ZYX', 'frame');
            
            % Enforce a positive quaternion convention
            if parts(yawquat) < 0
                yawquat = -yawquat;
            end

            % Return a compacted quaternion
            z = compact(yawquat);
        end
    end
end

Ahora use un objeto exampleHelperYawSensor junto con un objeto insAccelerometer y un objeto insGyroscope para construir el objeto insEKF .

opts = insOptions(SensorNamesSource='property', SensorNames={'Accelerometer', 'Gyroscope', 'YawSensor'});
filt = insEKF(insAccelerometer, insGyroscope, exampleHelperYawSensor, insMotionOrientation, opts);

Inicialice el filtro utilizando las funciones de objeto stateparts y statecovparts .

stateparts(filt, 'Orientation', sensorData.YawSensor(1,:));
statecovparts(filt,'Accelerometer_Bias', 1e-3);
statecovparts(filt,'Gyroscope_Bias', 1e-3);

Ajuste de filtro

Puede utilizar la función de objeto tune para encontrar los parámetros de ruido óptimos para el objeto insEKF . Puede llamar directamente a la función de objeto tune , o puede usar una función de coste acelerada por MEX con MATLAB Coder.

% Trim ground truth to just contain the Orientation for tuning.
trimmedGroundTruth = timetable(groundTruth.Orientation, ...
    SampleRate=groundTruth.Properties.SampleRate, ...
    VariableNames={'Orientation'});

% Use MATLAB Coder to accelerate tuning by MEXing the cost function.
% To run the MATLAB Coder accelerated path, prior to running the example,
% type:
%   exampleHelperUseAcceleratedTuning(true);
% To avoid using MATLAB Coder, prior to the example, type:
%   exampleHelperUseAcceleratedTuning(false);
% By default, the example will not tune the filter live and will not use
% MATLAB Coder.

% Select the accelerated tuning option.
acceleratedTuning = exampleHelperUseAcceleratedTuning(); 

if acceleratedTuning
    createTunerCostTemplate(filt); % A new cost function in the editor
    % Save and close the file
    doc = matlab.desktop.editor.getActive;
    doc.saveAs(fullfile(pwd, 'tunercost.m'));
    doc.close;
    % Find the first parameter for codegen
    p = tunerCostFcnParam(filt); %#ok<NASGU> 
    % Now generate a mex file
    codegen tunercost.m -args {p, sensorData, trimmedGroundTruth};
    % Use the Custom Cost Function and run the tuner for 20 iterations
    tunerIters = 20;
    cfg = tunerconfig(filt, MaxIterations=tunerIters, ...
        Cost='custom', CustomCostFcn=@tunercost_mex, ...
        StepForward=1.5, ...
        ObjectiveLimit=0.0001, ...
        FunctionTolerance=1e-6, ...
        Display='none');

    mnoise = tunernoise(filt);
    tunedmn = tune(filt, mnoise, sensorData, trimmedGroundTruth, cfg);
else
    % Use optimal parameters obtained separately.
    tunedmn = struct(...
        'AccelerometerNoise', 0.7786515625000000, ...
        'GyroscopeNoise', 167.8674323338237, ...
        'YawSensorNoise', 1.003122320344434); 

    adp = diag([...
          1.265000000000000;
          1.181989791398048;
          0.735171900658607;
          0.765000000000000;
          0.026248409763699;
          0.154586330266264;
          31.823154516336434;
          0.000546245218270;
          5.517012554348883;
          0.809085937500000;
          0.139035477206961;
          41.340145917279159;
          0.592875000000000]);
    filt.AdditiveProcessNoise = adp;
end

Fusionar datos y compararlos con Ground Truth

Fusione por lotes los datos con la función de objeto estimateStates .

est = estimateStates(filt, sensorData, tunedmn)

est=13500×5 timetable
      Time       Orientation                 AngularVelocity                           Accelerometer_Bias                            Gyroscope_Bias                StateCovariance
    ________    ______________    ______________________________________    _________________________________________    ______________________________________    _______________

    0 sec       1x1 quaternion    0.00029964     0.0003257    0.00033221     3.1017e-08    -3.4836e-24    -1.2416e-09    2.9964e-07     3.257e-07    3.3221e-07    {13x13 double} 
    0.01 sec    1x1 quaternion    0.00058811     0.0010514    0.00077335    -1.7319e-10    -1.9136e-05     1.5274e-06    1.0727e-06    0.00013317    2.6591e-06    {13x13 double} 
    0.02 sec    1x1 quaternion     0.0010153     0.0011293     0.0012812     1.1835e-08     5.0402e-05     4.4661e-06    2.2303e-06     0.0003694    6.6701e-06    {13x13 double} 
    0.03 sec    1x1 quaternion     0.0013953     0.0014522     0.0019019     1.0275e-08     9.5573e-05     2.0662e-06    3.7974e-06    0.00073969    1.2675e-05    {13x13 double} 
    0.04 sec    1x1 quaternion     0.0017109     0.0015025     0.0025803     2.3834e-08     0.00010594     6.3287e-06    5.7708e-06     0.0012283    2.0107e-05    {13x13 double} 
    0.05 sec    1x1 quaternion       0.00205     0.0019622     0.0033511     1.4978e-08     0.00012105     5.2769e-06    8.2924e-06     0.0018652    2.9309e-05    {13x13 double} 
    0.06 sec    1x1 quaternion     0.0020261     0.0023534     0.0041042     8.8652e-09    -6.6667e-05     1.5446e-06    1.1182e-05     0.0025908    3.8877e-05    {13x13 double} 
    0.07 sec    1x1 quaternion     0.0026568     0.0026599     0.0050163     8.3994e-09     0.00012693      4.528e-06    1.4392e-05     0.0034661    5.0998e-05    {13x13 double} 
    0.08 sec    1x1 quaternion     0.0027766     0.0029837     0.0058779      2.101e-09     3.3626e-05     1.1473e-05    1.7889e-05     0.0042033    6.2921e-05    {13x13 double} 
    0.09 sec    1x1 quaternion     0.0032068     0.0030261     0.0068784     1.3912e-08     0.00011826     1.7097e-05    2.1816e-05     0.0051831    7.7173e-05    {13x13 double} 
    0.1 sec     1x1 quaternion     0.0031981     0.0032051      0.007876      1.643e-08    -7.5163e-05     1.2083e-05    2.6806e-05     0.0061659    9.1711e-05    {13x13 double} 
    0.11 sec    1x1 quaternion     0.0036603     0.0032823      0.008879     2.4021e-08     3.2796e-05     1.2375e-05    3.1437e-05       0.00716    0.00010671    {13x13 double} 
    0.12 sec    1x1 quaternion      0.003734     0.0034359     0.0099138     2.7443e-08    -9.4583e-05     8.6765e-06    3.6787e-05     0.0083283     0.0001224    {13x13 double} 
    0.13 sec    1x1 quaternion     0.0039583     0.0036587      0.011055     2.6296e-08    -0.00012251     3.6091e-06    4.2167e-05     0.0095194    0.00014002    {13x13 double} 
    0.14 sec    1x1 quaternion     0.0040944     0.0040022      0.012244     1.7306e-08     -0.0002173      2.531e-06    4.8518e-05       0.01074    0.00015864    {13x13 double} 
    0.15 sec    1x1 quaternion     0.0044477     0.0040462      0.013476     2.4033e-08    -0.00015984     8.1767e-06    5.4321e-05      0.011837    0.00017817    {13x13 double} 
      ⋮

Compare la orientación estimada con la orientación ground-truth. Trazar la distancia del cuaternión entre las orientaciones estimada y verdadera.

% Convert compacted quaternions to regular quaternions
estOrient = quaternion(est.Orientation);

d = rad2deg(dist(estOrient, groundTruth.Orientation));
plot(groundTruth.Time, d);
xlabel('Time (s)')
ylabel('Orientation Error (deg)');
title('Estimated Orientation Error');

Figure contains an axes object. The axes object with title Estimated Orientation Error, xlabel Time (s), ylabel Orientation Error (deg) contains an object of type line.

snapnow;

A partir de los resultados, el error de orientación estimado es bajo en general. Hay un pequeño aumento en el error alrededor de los 100 segundos, lo que probablemente se debe a una ligera inflexión en el ángulo de guiñada ground-truth en ese momento. Pero en general, este es un buen resultado de estimación de orientación para muchas aplicaciones. Puedes visualizar la orientación estimada usando la función poseplot .

p = poseplot(estOrient(1));
for ii=2:numel(estOrient)
    p.Orientation = estOrient(ii);
    drawnow limitrate;
end

Figure contains an axes object. The axes object is empty.

Conclusión

En este ejemplo, aprendió cómo personalizar un sensor y agregarlo al marco insEKF . Los sensores personalizados se pueden integrar con los sensores integrados como insAccelerometer y insGyroscope. También aprendió a utilizar la función de ajuste de objetos para encontrar parámetros de ruido óptimos y mejorar los resultados del filtrado.