四元数及姿态解算 Mahony算法

Posted 2020-10-18 大G霸

tags:

篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了四元数及姿态解算 Mahony算法相关的知识，希望对你有一定的参考价值。

AHRS(attitude and heading reference system)称为航姿参考系统。

首先，我们明确一下四元数的知识。

四元数（quaternion）是由我们的威廉·哈密顿提出的。哈密顿就是那个‘哈密顿最小作用原理’的提出者。

四元数可视为复数的扩展。在复数中，定义了 $i^2=-1$ ，而四元数中则定义了 $i^2=j^2=k^2=ijk=-1$ 。也就是说四元数是一个具有三个虚部的虚数。把这些虚部理解为一个复数空间的话，四元数就是一个四位空间。这个是纯数学问题，有兴趣可以了解群论，本人也不是很理解所以也不多讲啦。

但是四元数本身最多的就是使用单位四元数来表示三维空间的旋转（rotation）及定向（orientation）。

为了方便，我们下面使用q = ((x, y, z)，w) = (v, w)，其中v是向量，w是实数，这样的式子来表示一个四元数。下述内容采自知乎。

首先，定义一个你需要做的旋转。旋转轴为向量 $v=(vx,vy,vz)$ ，旋转角度为 $\\theta$ （右手法则的旋转）。如下图所示：
此图中 $v=(\\frac{1}{\\sqrt{14} } ,\\frac{2}{\\sqrt{14} } ,\\frac{3}{\\sqrt{14} })$ , $\\theta =\\frac{\\pi }{3}$

那么与此相对应的四元数（下三行式子都是一个意思，只是不同的表达形式）
$q=(cos(\\frac{\\theta }{2} ),sin(\\frac{\\theta }{2} )*vx,sin(\\frac{\\theta }{2} )*vy,sin(\\frac{\\theta }{2} )*vz)$
$q=(cos(\\frac{\\pi }{6} ),sin(\\frac{\\pi }{6} )*\\frac{1}{\\sqrt{14} } ,sin(\\frac{\\pi }{6} )*\\frac{2}{\\sqrt{14} },sin(\\frac{\\pi }{6} )*\\frac{3}{\\sqrt{14} })$
$q=cos(\\frac{\\pi }{6} )+sin(\\frac{\\pi }{6} )*\\frac{1}{\\sqrt{14} }i +sin(\\frac{\\pi }{6} )*\\frac{2}{\\sqrt{14} }j+sin(\\frac{\\pi }{6} )*\\frac{3}{\\sqrt{14} }k$

这时它的共轭（下三行式子都是一个意思，只是不同的表达形式），
$q^{-1} =(cos(\\frac{\\theta }{2} ),-sin(\\frac{\\theta }{2} )*vx,-sin(\\frac{\\theta }{2} )*vy,-sin(\\frac{\\theta }{2} )*vz)$
$q^{-1} =(cos(\\frac{\\pi }{6} ),-sin(\\frac{\\pi }{6} )*\\frac{1}{\\sqrt{14} } ,-sin(\\frac{\\pi }{6} )*\\frac{2}{\\sqrt{14} },-sin(\\frac{\\pi }{6} )*\\frac{3}{\\sqrt{14} })$
$q^{-1} =cos(\\frac{\\pi }{6} )-sin(\\frac{\\pi }{6} )*\\frac{1}{\\sqrt{14} }i -sin(\\frac{\\pi }{6} )*\\frac{2}{\\sqrt{14} }j-sin(\\frac{\\pi }{6} )*\\frac{3}{\\sqrt{14} }k$

如果你想算一个点 $w=(wx,wy,wz)$ 在这个旋转下新的坐标 $w^{\'}$ ,需要进行如下操作，
1.定义纯四元数
$qw=(0,wx,wy,wz)=0+wx*i+wy*j+wz*k$
2.进行四元数运算
$qw^{\'} =q*qw*q^{-1}$
3.产生的 $qw^{\'}$ 一定是纯四元数，也就是说它的第一项为0，有如下形式：
$qw^{\'} =(0,wx^{\'},wy^{\'},wz^{\'})=0+wx^{\'}*i+wy^{\'}*j+wz^{\'}*k$
4. $qw^{\'}$ 中的后三项 $(wx^{\'},wy^{\'},wz^{\'})$ 就是 $w^{\'}$ ：
$w^{\'} =(wx^{\'},wy^{\'},wz^{\'})$
这样，就完成了一次四元数旋转运算。

同理，如果你有一个四元数：
$q=(q1,q2,q3,q4)=(cos(\\frac{\\theta }{2} ),sin(\\frac{\\theta }{2} )*vx,sin(\\frac{\\theta }{2} )*vy,sin(\\frac{\\theta }{2} )*vz)$
那么，它对应一个以向量 $v=(vx,vy,vz)$ 为轴旋转 $\\theta$ 角度的旋转操作（右手法则的旋转）。

这里基本解释了部分四元数的基本原理，如果想要更深入了解可以去看原帖：https://www.zhihu.com/question/23005815

其他的数学性质：

四元数

是描述了B相对于A的旋转，其中的rx,ry,rz组成的向量也是处于A中的向量。

如果要一个共轭四元数，也就是A对B的四元数，即。我们有：

注意，这里的当四元数表示由B相对于A坐标的四元数为时，他的共轭四元数则表示A相对于B坐标的四元数。

当我们想求相对四元数时，可以使用。详情见笔记11012018A01。

四元数的乘法，

封闭性：易证明，p和p的共轭相乘即可，|p*q|=1。
结合律：这条也很好证明，只要证明(p*q)*r=p*(q*r)。

四元数对欧拉角转换，

更多资料可以参考：https://zhuanlan.zhihu.com/p/28189289

我们下一步来看一下通过陀螺仪输出的角加速度估计姿态问题。

数学操作部分：

如果那些角速度被分配到向量的时候，即

的时候。

表示传感器所在坐标相对于地磁坐标的姿态改变速率的四元数微分可以表示为下边公式：

为从E坐标到S坐标的预测值。

而在时间t，地球坐标系相对于传感器坐标系的四元数可以同过的积分获得。而是通过预测值与时间t时刻形成的角速度的叉乘获得的。

在不在对q进行二次处理的话，姿态预测值就是现在的姿态值。

下面就是一个求解传感器所在坐标系的姿态。

开始，基于一个初始为[1 0 0 0]的四元数得到 q = [1 0 0 0], 根据加速度求得加速度的单位向量，根据q求转换到b系的值v。

而 v = [2*(q(2)*q(4)-q(1)*q(3)) 　　2*(q(1)*q(2)+q(3)*q(4)) 　　q(1)^2 -q(2)^2 -q(3)^2+q(4)^2]

而后计算误差，其中加速度与计算得到的v的向量积可以得到误差值。

计算得到的积分误差为误差的积分，即误差和加上误差乘以采样时间。

经过PI降低陀螺仪的误差。陀螺仪数据为自身数据加上P乘以误差加I乘以积分误差。

而后计算四元数的微分值等于0.5*上一次采样的四元数乘角速度。

最后在对四元数进行积分得到当前四元数。最后再将四元数单位化为单位向量。

下面是这个过程的matlab源码。

classdef MahonyAHRS < handle
%MAYHONYAHRS Madgwick\'s implementation of Mayhony\'s AHRS algorithm
%
%   For more information see:
%   http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
%
%   Date          Author          Notes
%   28/09/2011    SOH Madgwick    Initial release
 
    %% Public properties
    properties (Access = public)
        SamplePeriod = 1/256;
        Quaternion = [1 0 0 0];     % output quaternion describing the Earth relative to the sensor
        Kp = 1;                     % algorithm proportional gain
        Ki = 0;                     % algorithm integral gain
    end
    
    %% Public properties
    properties (Access = private)
        eInt = [0 0 0];             % integral error
    end    
 
    %% Public methods
    methods (Access = public)
        function obj = MahonyAHRS(varargin)
            for i = 1:2:nargin
                if  strcmp(varargin{i}, \'SamplePeriod\'), obj.SamplePeriod = varargin{i+1};
                elseif  strcmp(varargin{i}, \'Quaternion\'), obj.Quaternion = varargin{i+1};
                elseif  strcmp(varargin{i}, \'Kp\'), obj.Kp = varargin{i+1};
                elseif  strcmp(varargin{i}, \'Ki\'), obj.Ki = varargin{i+1};
                else error(\'Invalid argument\');
                end
            end;
        end
        function obj = Update(obj, Gyroscope, Accelerometer, Magnetometer)
            q = obj.Quaternion; % short name local variable for readability
 
            % Normalise accelerometer measurement
            if(norm(Accelerometer) == 0), return; end   % handle NaN
            Accelerometer = Accelerometer / norm(Accelerometer);    % normalise magnitude
 
            % Normalise magnetometer measurement
            if(norm(Magnetometer) == 0), return; end    % handle NaN
            Magnetometer = Magnetometer / norm(Magnetometer);   % normalise magnitude
 
            % Reference direction of Earth\'s magnetic feild
            h = quaternProd(q, quaternProd([0 Magnetometer], quaternConj(q)));
            b = [0 norm([h(2) h(3)]) 0 h(4)];
            
            % Estimated direction of gravity and magnetic field
            v = [2*(q(2)*q(4) - q(1)*q(3))
                 2*(q(1)*q(2) + q(3)*q(4))
                 q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2];
            w = [2*b(2)*(0.5 - q(3)^2 - q(4)^2) + 2*b(4)*(q(2)*q(4) - q(1)*q(3))
                 2*b(2)*(q(2)*q(3) - q(1)*q(4)) + 2*b(4)*(q(1)*q(2) + q(3)*q(4))
                 2*b(2)*(q(1)*q(3) + q(2)*q(4)) + 2*b(4)*(0.5 - q(2)^2 - q(3)^2)]; 
 
            % Error is sum of cross product between estimated direction and measured direction of fields
            e = cross(Accelerometer, v) + cross(Magnetometer, w); 
            if(obj.Ki > 0)
                obj.eInt = obj.eInt + e * obj.SamplePeriod;   
            else
                obj.eInt = [0 0 0];
            end
            
            % Apply feedback terms
            Gyroscope = Gyroscope + obj.Kp * e + obj.Ki * obj.eInt;            
            
            % Compute rate of change of quaternion
            qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]);
 
            % Integrate to yield quaternion
            q = q + qDot * obj.SamplePeriod;
            obj.Quaternion = q / norm(q); % normalise quaternion
        end
        function obj = UpdateIMU(obj, Gyroscope, Accelerometer)
            q = obj.Quaternion; % short name local variable for readability
 
            % Normalise accelerometer measurement
            if(norm(Accelerometer) == 0), return; end   % handle NaN
            Accelerometer = Accelerometer / norm(Accelerometer);    % normalise magnitude
 
            % Estimated direction of gravity and magnetic flux
            v = [2*(q(2)*q(4) - q(1)*q(3))
                 2*(q(1)*q(2) + q(3)*q(4))
                 q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2];
 
            % Error is sum of cross product between estimated direction and measured direction of field
            e = cross(Accelerometer, v); 
            if(obj.Ki > 0)
                obj.eInt = obj.eInt + e * obj.SamplePeriod;   
            else
                obj.eInt = [0 0 0];
            end
            
            % Apply feedback termszai
            Gyroscope = Gyroscope + obj.Kp * e + obj.Ki * obj.eInt;            
            
            % Compute rate of change of quaternion
            qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]);
 
            % Integrate to yield quaternion
            q = q + qDot * obj.SamplePeriod;
            obj.Quaternion = q / norm(q); % normalise quaternion
        end
    end
end

View Code

在获得的姿态上减去带上方向的重力加速度，而后通过重力加速度进行积分，可以获得速度。然而由于积分误差的累计，会导致信号失真，并漂移。

所以通过高通滤波处理速度。

位移可以通过同样的方法获得。

代码如下：

%% Housekeeping
 
addpath(\'ximu_matlab_library\');    % include x-IMU MATLAB library
addpath(\'quaternion_library\');    % include quatenrion library
close all;                         % close all figures
clear;                             % clear all variables
clc;                              % clear the command terminal
 
%% Import data

xIMUdata = xIMUdataClass(\'LoggedData/LoggedData\');

samplePeriod = 1/256;

gyr = [xIMUdata.CalInertialAndMagneticData.Gyroscope.X...
       xIMUdata.CalInertialAndMagneticData.Gyroscope.Y...
       xIMUdata.CalInertialAndMagneticData.Gyroscope.Z];        % gyroscope
acc = [xIMUdata.CalInertialAndMagneticData.Accelerometer.X...
       xIMUdata.CalInertialAndMagneticData.Accelerometer.Y...
       xIMUdata.CalInertialAndMagneticData.Accelerometer.Z];    % accelerometer
  
% Plot
figure(\'NumberTitle\', \'off\', \'Name\', \'Gyroscope\');
hold on;
plot(gyr(:,1), \'r\');
plot(gyr(:,2), \'g\');
plot(gyr(:,3), \'b\');
xlabel(\'sample\');
ylabel(\'dps\');
title(\'Gyroscope\');
legend(\'X\', \'Y\', \'Z\');

figure(\'NumberTitle\', \'off\', \'Name\', \'Accelerometer\');
hold on;
plot(acc(:,1), \'r\');
plot(acc(:,2), \'g\');
plot(acc(:,3), \'b\');
xlabel(\'sample\');
ylabel(\'g\');
title(\'Accelerometer\');
legend(\'X\', \'Y\', \'Z\');

%% Process data through AHRS algorithm (calcualte orientation)
% See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/

R = zeros(3,3,length(gyr));     % rotation matrix describing sensor relative to Earth

ahrs = MahonyAHRS(\'SamplePeriod\', samplePeriod, \'Kp\', 1);

for i = 1:length(gyr)
    ahrs.UpdateIMU(gyr(i,:) * (pi/180), acc(i,:));    % gyroscope units must be radians
    R(:,:,i) = quatern2rotMat(ahrs.Quaternion)\';    % transpose because ahrs provides Earth relative to sensor
end

%% Calculate \'tilt-compensated\' accelerometer

tcAcc = zeros(size(acc));  % accelerometer in Earth frame

for i = 1:length(acc)
    tcAcc(i,:) = R(:,:,i) * acc(i,:)\';
end

% Plot
figure(\'NumberTitle\', \'off\', \'Name\', \'\'\'Tilt-Compensated\'\' accelerometer\');
hold on;
plot(tcAcc(:,1), \'r\');
plot(tcAcc(:,2), \'g\');
plot(tcAcc(:,3), \'b\');
xlabel(\'sample\');
ylabel(\'g\');
title(\'\'\'Tilt-compensated\'\' accelerometer\');
legend(\'X\', \'Y\', \'Z\');

%% Calculate linear acceleration in Earth frame (subtracting gravity)

linAcc = tcAcc - [zeros(length(tcAcc), 1), zeros(length(tcAcc), 1), ones(length(tcAcc), 1)];
linAcc = linAcc * 9.81;     % convert from \'g\' to m/s/s

% Plot
figure(\'NumberTitle\', \'off\', \'Name\', \'Linear Acceleration\');
hold on;
plot(linAcc(:,1), \'r\');
plot(linAcc(:,2), \'g\');
plot(linAcc(:,3), \'b\');
xlabel(\'sample\');
ylabel(\'g\');
title(\'Linear acceleration\');
legend(\'X\', \'Y\', \'Z\');

%% Calculate linear velocity (integrate acceleartion)

linVel = zeros(size(linAcc));

for i = 2:length(linAcc)
    linVel(i,:) = linVel(i-1,:) + linAcc(i,:) * samplePeriod;
end

% Plot
figure(\'NumberTitle\', \'off\', \'Name\', \'Linear Velocity\');
hold on;
plot(linVel(:,1), \'r\');
plot(linVel(:,2), \'g\');
plot(linVel(:,3), \'b\');
xlabel(\'sample\');
ylabel(\'g\');
title(\'Linear velocity\');
legend(\'X\', \'Y\', \'Z\');

%% High-pass filter linear velocity to remove drift

order = 1;
filtCutOff = 0.1;
[b, a] = butter(order, (2*filtCutOff)/(1/samplePeriod), \'high\');
linVelHP = filtfilt(b, a, linVel);

% Plot
figure(\'NumberTitle\', \'off\', \'Name\', \'High-pass filtered Linear Velocity\');
hold on;
plot(linVelHP(:,1), \'r\');
plot(linVelHP(:,2), \'g\');
plot(linVelHP(:,3), \'b\');
xlabel(\'sample\');
ylabel(\'g\');
title(\'High-pass filtered linear velocity\');
legend(\'X\', \'Y\', \'Z\');

%% Calculate linear position (integrate velocity)

linPos = zeros(size(linVelHP));

for i = 2:length(linVelHP)
    linPos(i,:) = linPos(i-1,:) + linVelHP(i,:) * samplePeriod;
end

% Plot
figure(\'NumberTitle\', \'off\', \'Name\', \'Linear Position\');
hold on;
plot(linPos(:,1), \'r\');
plot(linPos(:,2), \'g\');
plot(linPos(:,3), \'b\');
xlabel(\'sample\');
ylabel(\'g\');
title(\'Linear position\');
legend(\'X\', \'Y\', \'Z\');

%% High-pass filter linear position to remove drift

order = 1;
filtCutOff = 0.1;
[b, a] = butter(order, (2*filtCutOff)/(1/samplePeriod), \'high\');
linPosHP = filtfilt(b, a, linPos);

% Plot
figure(\'NumberTitle\', \'off\', \'Name\', \'High-pass filtered Linear Position\');
hold on;
plot(linPosHP(:,1), \'r\');
plot(linPosHP(:,2), \'g\');
plot(linPosHP(:,3), \'b\');
xlabel(\'sample\');
ylabel(\'g\');
title(\'High-pass filtered linear position\');
legend(\'X\', \'Y\', \'Z\');

%% Play animation

SamplePlotFreq = 8;

SixDOFanimation(linPosHP, R, ...
                \'SamplePlotFreq\', SamplePlotFreq, \'Trail\', \'Off\', ...
                \'Position\', [9 39 1280 720], ...
                \'AxisLength\', 0.1, \'ShowArrowHead\', false, ...
                \'Xlabel\', \'X (m)\', \'Ylabel\', \'Y (m)\', \'Zlabel\', \'Z (m)\', \'ShowLegend\', false, \'Title\', \'Unfiltered\',...
                \'CreateAVI\', false, \'AVIfileNameEnum\', false, \'AVIfps\', ((1/samplePeriod) / SamplePlotFreq));            
 
%% End of script

而最后的SixDOFanimation函数如下：

function fig = SixDOFanimation(varargin)

    %% Create local variables

    % Required arguments
    p = varargin{1};                % position of body
    R = varargin{2};                % rotation matrix of body
    [numSamples dummy] = size(p);

    % Default values of optional arguments
    SamplePlotFreq = 1;
    Trail = \'Off\';
    LimitRatio = 1;
    Position = [];
    FullScreen = false;
    View = [30 20];
    AxisLength = 1;
    ShowArrowHead = \'on\';
    Xlabel = \'X\';
    Ylabel = \'Y\';
    Zlabel = \'Z\'卡尔曼（Kalman）滤波--卡尔曼滤波的应用： 四元数卡尔曼滤波（QKF）的C代码实现姿态解算
 四轴之互补滤波与四元数算法最详细分析
 软件姿态解算
 [UVA]Pixhawk之姿态解算篇
 [UVA]Pixhawk之姿态解算篇_源码姿态解算算法分析
 Pixhawk之姿态解算篇_入门篇（DCM Nomalize）