您的位置: 首页  >  文章  >  四元数及姿态解算 Mahony算法

四元数及姿态解算 Mahony算法

分类: 文章 • 2024-09-29 20:14:22

https://www.cnblogs.com/TIANHUAHUA/p/8031606.html

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

 

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

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

四元数可视为复数的扩展。在复数中，定义了四元数及姿态解算 Mahony算法，而四元数中则定义了四元数及姿态解算 Mahony算法。也就是说四元数是一个具有三个虚部的虚数。把这些虚部理解为一个复数空间的话，四元数就是一个四位空间。这个是纯数学问题，有兴趣可以了解群论，本人也不是很理解所以也不多讲啦。

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

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

首先，定义一个你需要做的旋转。旋转轴为向量四元数及姿态解算 Mahony算法，旋转角度为四元数及姿态解算 Mahony算法（右手法则的旋转）。如下图所示：
此图中四元数及姿态解算 Mahony算法,四元数及姿态解算 Mahony算法

四元数及姿态解算 Mahony算法

那么与此相对应的四元数（下三行式子都是一个意思，只是不同的表达形式）
四元数及姿态解算 Mahony算法
四元数及姿态解算 Mahony算法
四元数及姿态解算 Mahony算法

这时它的共轭（下三行式子都是一个意思，只是不同的表达形式），
四元数及姿态解算 Mahony算法
四元数及姿态解算 Mahony算法
四元数及姿态解算 Mahony算法

如果你想算一个点四元数及姿态解算 Mahony算法在这个旋转下新的坐标四元数及姿态解算 Mahony算法,需要进行如下操作，
1.定义纯四元数
四元数及姿态解算 Mahony算法
2.进行四元数运算
四元数及姿态解算 Mahony算法
3.产生的四元数及姿态解算 Mahony算法一定是纯四元数，也就是说它的第一项为0，有如下形式：
四元数及姿态解算 Mahony算法
4.四元数及姿态解算 Mahony算法中的后三项四元数及姿态解算 Mahony算法就是四元数及姿态解算 Mahony算法：
四元数及姿态解算 Mahony算法
这样，就完成了一次四元数旋转运算。

同理，如果你有一个四元数：
四元数及姿态解算 Mahony算法
那么，它对应一个以向量四元数及姿态解算 Mahony算法为轴旋转四元数及姿态解算 Mahony算法角度的旋转操作（右手法则的旋转）。

 

 

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

 其他的数学性质：

 四元数四元数及姿态解算 Mahony算法是描述了B相对于A的旋转，其中的rx,ry,rz组成的向量也是处于A中的向量。

如果要一个共轭四元数，也就是A对B的四元数，即 四元数及姿态解算 Mahony算法。我们有：

四元数及姿态解算 Mahony算法

注意，这里的当四元数表示由B相对于A坐标的四元数为四元数及姿态解算 Mahony算法时，他的共轭四元数则表示A相对于B坐标的四元数。

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

 

 四元数的乘法， 

 四元数及姿态解算 Mahony算法

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

 

 四元数对欧拉角转换，

四元数及姿态解算 Mahony算法

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

 

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

数学操作部分：

如果那些角速度被分配到向量四元数及姿态解算 Mahony算法的时候，即

四元数及姿态解算 Mahony算法

的时候。

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

 四元数及姿态解算 Mahony算法

四元数及姿态解算 Mahony算法为从E坐标到S坐标的预测值。

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

四元数及姿态解算 Mahony算法

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

 

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

 

四元数及姿态解算 Mahony算法

开始，基于一个初始为[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源码。

四元数及姿态解算 Mahony算法 View Code

 

　　

 

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

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

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

代码如下：

四元数及姿态解算 Mahony算法

%% 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　

四元数及姿态解算 Mahony算法

 而最后的SixDOFanimation函数如下：

四元数及姿态解算 Mahony算法 View Code

其中可以通过更改PI部分参数，重力补偿值和滤波频率，来适应自己的信号。

如果想要了解更多细节获得源程序，可以查看原官方资料网站：

http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

 

参考资料：

四元数参考

 X-IO开源

矩阵乘法本质 

Pixhawk

相关推荐