머리

이 기사는 "제어 공학 Advent Calendar 2017"의 12/21 기사로 작성되었습니다.
내용에 대해 이해하고 있다고는 말하기 어렵기 때문에, 실수에 대해서는 자꾸자꾸 코멘트해 주시면 다행입니다.

본 기사에서는 Madgwick 필터의 논문을 읽었으므로, 그것에 대해 일부를 일본어로 정리한 것입니다. 원 논문 "An efficient orientation filter for inertial and inertial/magnetic sensor arrays"에서는 자이로 센서와 가속도 센서를 사용한 경우와 거기에 지자기 센서를 더한 경우에 대해서도 논의하고 있습니다만, 이번에는 자이로 센서와 가속도 센서 전용의 경우에 대해 썼습니다. 지자기를 도입한 경우에도 기본 원리는 그다지 변하지 않는 것 같습니다.
쿼터니온 기반 필터이지만, 쿼터니온에 대해서는 별도 문헌을 참조하십시오. 이 기사는 원논문을 읽기 전에 대략적인 개요를 잡는 데 도움이 되었으면 합니다. 또, 실제의 코드도 원논문에 있기 때문에, 그쪽을 보신 것이 빠를지도 모릅니다.

참고 문헌에 대해서는 원논문뿐이므로 참조 번호 등은 생략합니다. 아래의 식번호는 모두 원논문의 물건을 유용하고 있기 때문에, 식번호가 흩어져 있는 부분이 있습니다.

Madgwick 필터란?

자세한 것은, 원논문을 읽는 것이 압도적으로 빠르기 때문에, 자세하게 설명합니다.
Madgwick씨가 제창된 것으로, 칼만 필터에 비해, 고속으로 동정도 이상의 정밀도를 실현한 것 같은 필터입니다. 칼만 필터가 모델의 구조를 모르는 경우에 고정밀도를 실현하는 것이 어렵지만, Madgwick 필터는 같은 정도의 정밀도를 실현하면서 고속으로 처리가 가능한 필터입니다. 한편, 조금 조사한 범위라면, 수렴이 느린 것 같다든가···?
무엇보다 가장 큰 이점은 그 처리 속도입니다. 마이크로컴퓨터(RX631@100MHz)로 실행시에 실측으로 $200\mu$정도로 처리를 실현하고 있습니다.

3. 필터 도출

3.0 쿼터니언의 기본 표현

쿼터니언의 표기로서,

^A_B\hat{q}=[q_1,q_2,q_3,q_4]=[cos\frac{\theta}{2},-r_x sin\frac{\theta}{2}, -r_y sin\frac{\theta}{2},-r_z sin\frac{\theta}{2}]\cdots(1)

이것은 프레임 A에 대한 프레임 B의 방향을 나타냅니다.
또한 공액 쿼터니언

^A_B\hat{q}^*=^B_A\hat{q}=[q_1,-q_2,-q_3,-q_4]\cdots(2)

하고 있습니다.

다음으로, 2개의 쿼터니언의 곱에 대해서는, $\otimes$를 사용해 표기합니다. $^A_B\hat{q}$와 $^B_C\hat{q}$의 곱은

^A_C\hat{q} = ^B_C\hat{q} \otimes  ^A_B\hat{q}\cdots(3)

이것은 프레임 A에서 프레임 C로의 방향을 나타냅니다.

여기서 실제로 $\otimes$는 {\bf{a}} \otimes {\bf{b}} =[a_1, a_2, a_3, a_4]\otimes [b_1,b_2,b_3,b_4]\\ =\begin{bmatrix} a_1 b_1 - a_2 b_2 - a_3 b_3 -a_4 b_4\\\ a_1 b_2 - a_2 b_1 + a_2 b_1 + a_3 b_4 - a_4 b_3\\\ a_1 b_3 - a_2 b_4 + a_3 b_1 + a_4 b_2 \\\ a_1 b_4 +a_2 b_3 -a_3 b_2 +a_4 b_1] \end{bmatrix} \cdots(4) 됩니다. 다음으로, 3차원 벡터의 쿼터니온에 의한 회전은, ^B{\bf{v}} = ^A_B\hat{q} \otimes ^A{\bf{v}} \otimes ^A_B\hat{q}^* \cdots(5) 로 표시됩니다. 여기서 오른쪽에서는 3 차원 벡터의 선두 요소에 0이 삽입되어 계산됩니다. $^A{\bf{v}} = [a,b,c]$ 때, $[0,a,b,c]$ 로 확장되어 오른쪽이 계산됩니다. 원논문이라면 쿼터니언에서 회전 행렬로의 변환이 조금 설명되어 있지만 생략합니다. 3.1 각속도로부터의 자세 계산 우선, 센서 프레임에서의 각속도를, ^S\omega=[0 ,\omega_x,\omega_y,\omega_z] [rad/s]\cdots(10) 합니다. 또한 접지 프레임에서 센서 프레임으로의 쿼터니언에 의한 회전의 시간 미분은 ^S_E\dot{q}=\frac{1}{2}^S_E\hat{q}\otimes^S\omega\cdots(11) 로 계산됩니다. 다음으로 이산 시간으로 생각하면 시각 t에서의 자세 쿼터니언을 $^S_Eq_{\omega,t}$, t-1에서의 추정치를 $^S_E\hat{q}_{est,t-1}$로 한다. , ^S_E\dot{q}_{\omega,t}=\frac{1}{2}^S_E\hat{q}_{est,t-1}\otimes^S\omega_t\cdots(12) ^S_Eq_{\omega,t}=^S_E\hat{q}_{\omega,t-1} + ^S_E\dot{q}_{\omega,t}\Delta t\cdots(13) 됩니다. 3.2 계측 벡터로부터의 자세 표현 가속도 센서가 측정하는 값은 센서 프레임의 중력 가속도와 센서 프레임의 모션에 의해 발생하는 가속도의 복합이다. 그러나 여기에서는 중력 가속도 만 측정됩니다. (지자기도 비슷한 느낌) 그런 다음 접지 프레임에서 참값을 알고 있다면 센서 프레임에서 측정 할 값을 자세 쿼터니언에서 계산할 수 있어야하며 두 벡터의 차이를 구할 수도 있습니다. 그리고 그 차이가 가장 작아지는 자세 쿼터니언 $^S_E\hat{q}$가 가장 적절한 자세일 것이다. min(^S_E\hat{q}\in R^4) {\bf{f}}(^S_E\hat{q},^E\hat{d},^S\hat{s})\cdots( 14)\\\ {\bf{f}}(^S_E\hat{q},^E\hat{d},^S\hat{s}) = ^S_E\hat{q}^*\otimes ^E\hat{d } \otimes ^S_E\hat{q} - ^S\hat{s}\cdots(15)\\\ ^S_E\hat{q}=[q_1,q_2,q_3,q_4]\cdots(16)\\\ ^E\hat{d}=[0, d_x,d_y,d_z]\cdots(17)\\\ ^S\hat{s}=[0,s_x,s_y,s_z]\cdots(18) 이 최적화 문제에 대한 알고리즘으로 가장 가파른 하강 방법을 사용합니다. 이 알고리즘은 다음 네 가지 방정식으로 설명됩니다. ^S_E{q}_{k+1}=^S_E\hat{q}_k -\mu\frac{\nabla{\bf{f}}(^S_E\hat{q}_k,^E\hat{ d},^S\hat{s})}{||\nabla{\bf{f}}(^S_E\hat{q}_k,^E\hat{d},^S\hat{s}) ||},k=0,1,2\dots n \cdots(19)\\\ \nabla{\bf{f}}(^S_E\hat{q}_k,^E\hat{d},^S\hat{s})={\bf{J}}^T(^S_E\hat {q}_k,^E\hat{d}) {\bf{f}}(^S_E\hat{q}_k,^E\hat{d},^S\hat{s}) \cdots(20 )\\\ {\bf{f}}(^S_E\hat{q}_k,^E\hat{d},^S\hat{s})= \begin{bmatrix} 2d_x(\frac{1}{2}-q_3^2-q_4^2)+2d_y(q_1q_4+q_2q_3)+2d_z(q_2q_4-q_1q_3)-s_x\\\ 2d_x(q_2q_3-q_1q_4)+2d_y(\frac{1}{2}-q_2^2-q_4^2)+2d_z(q_1q_2+q_3q_4)-s_y\\\ 2d_x(q_1q_3+q_2q_4)+2d_y(q_3q_4-q_1q_2)+2d_z(\frac{1}{2}-q_2^2-q_3^2)-a_z \end{bmatrix} \cdots(21)\\\ {\bf{J}}^T(^S_E\hat{q}_k,^E\hat{d})= \begin{bmatrix} 2d_yq_4-2d_zq_3 & 2d_yq_3+2d_zq_4 & -4d_xq_32d_yq_2-2d_zq_1 & -4d_xq_4+2d_yq_1+2d_zq_2\\\ -2d_xq_4+2d_zq_2 & 2d_xq_3-4d_yq_2+2d_zq_1 & 2d_xq_2+2d_zq_4 & -2d_xq_1-4d_yq_4+2d_zq_3\\\ 2d_xq_3-2d_yq_2 & 2d_xq_4-2d_yq_1-4d_xq_2 & 2d_xq_1+2d_yq_4-4d_zq_3 & 2d_xq_2+2d_yq_3 \end{bmatrix} \cdots(22) 이 설명은 원논문에는 있지만 생략합니다. 게다가, 가속도 센서를 이용하는 경우를 생각하면, 계측값을 정규화한 것으로 하고, ^E\hat{g}=[0, 0, 0, 1]\cdots(23)\\\ ^S\hat{a}=[0,a_x, a_y ,a_z]\cdots(24) 로 표현된다. 이것을 앞의 방정식에 대입하면, {\bf{f}}_g(^S_E\hat{q},^S\hat{a})= \begin{bmatrix} 2(q_2q_4-q_1q_3)-a_x\\\ 2(q_1q_2+q_3q_4)-a_y\\\ 2(\frac{1}{2}-q_2^2 -q_3^2)-a_z \end{bmatrix} \cdots(25)\\\ {\bf{J}}_g(^S_E\hat{q})= \begin{bmatrix} -2q_3 & 2q_4 & -2q_1 & 2q_2 \\\ 2q_2 & 2q_1 & 2q_4 & 2q_3 \\\ 0 & -4q_2 & -4q_3 &0 \end{bmatrix} \cdots(26) 따라서 최종 업데이트 공식은 ^S_Eq_{\nabla,t}=^S_Eq_{est,t-1}-\mu_t \frac{\nabla{\bf{f}}}{||\nabla{\bf{f}}||}\ cdots(33) 지자기의 경우에도 유사한 논의로 공식을 얻을 수 있습니다. 원논문에서는 이것들을 복합한 식을 최종적으로 구하고 있지만 이번은 생략했다. 여기서 식에서 $ \ mu_t $의 최적 값은 $ ^ S_Eq_ {\ Delta, t} $의 수렴률로 정의 될 수있다. 이것은 불필요하게 큰 스텝 크기로 인한 오버 슈트를 피하기 위해 물리적 자세 변화 속도에 의해 제한됩니다. 따라서, $\Delta t$를 샘플링 시간으로, $^S_E\dot{q}_{\omega, t}$를 자이로 센서에 의한 계측치, $\alpha$를 가속도 센서의 노이즈에 의해 결정되는 값으로, \mu_t = \alpha || ^S_E\dot{q}_{\omega ,t}|| \Delta t , \alpha > 1\cdots(35) 로 계산할 수 있습니다. 3.3 필터 퓨전 알고리즘 이 절에서는 필터 퓨전 알고리즘에 대해 설명합니다. 지자기를 사용하지 않는 경우의 설명은 비교적 적었으므로, 읽기 쉽다고 생각합니다. 최종 추정치 ${^S_E\hat{q_{est,t}}}$는 각속도에서 계산된 $^S_E\hat{q_{\omega,t}}$와 3.2까지 설명했다. 수식에서 계산 된 $ ^ S_E \ hat {q_ {\ nabla, t}} $의 융합으로 계산됩니다. 각 자세 쿼터니언에 대한 가중치의 변수로서 $\gamma _t$를 찍으면, ^S_E{q}_{est,t} = {\gamma_t} ^S_E{q}_{\nabla,t} + (1-\gamma_t)^S_E{q}_{\omega,t} , 0\ leq \gamma_t \leq 1\cdots(36) 로 계산합니다. 여기서 적절한 가중치는 자이로 센서에 의한 자세의 발산의 가중치와 가속도 센서에 의한 자세의 수렴의 가중치가 같음으로써 정의할 수 있다. 가속도 센서에 의한 자세 $^S_E{q_{\nabla,t}}$의 수렴율은 $\frac{\mu_t}{\Delta t}$이며, $\beta$를 자이로 센서의 계측 오차에 의한 쿼터니온으로 미분의 크기로 표현되는 $^S_E{q}_{\omega,t}$를 발산율로, (1-\gamma_t)\beta = \gamma_t \frac{\mu_t}{\Delta t}\cdots(37) 따라서, \gamma_t=\frac{\beta}{\frac{\mu_t}{\Delta t}+\beta}\cdots(38) 된다. 여기서 $\alpha$에 상한이 없고 $\mu_t$에 대해 매우 크다고 가정하면 식 (33)의 $^S_E\hat{q}_{est,t-1} $는 무시할 수 있다. ^S_E{q}_{\nabla,t}\approx -\mu_t \frac{\nabla {\bf{f}}}{||{\bf{f}}||}\cdots(39) 로 근사 할 수 있습니다. 마찬가지로 식 (38)의 분모의 $ \ beta $도 무시할 수 있으므로, \gamma_t \approx \frac{\beta \Delta t}{\mu_t}\cdots(40) 식 (40)을 $ \ gamma_t \ approx 0 $와 근사 할 수 있습니다. 상기 식 (13), (39), (40)을 (36)에 대입함으로써 식 (41)을 얻을 수있다. ^S_Eq_{est,t}=\frac{\beta \Delta t}{\mu_t}(-\mu_t \frac{\nabla {\bf{f}}}{||\nabla {\bf{f}} ||})+(1-0)(^S_E\hat{q}_{est,t-1}+^S_E\dot{q}_{\omega,t}\Delta t) \cdots(41) 따라서, 식 (41)로부터 최종적으로 필터의 갱신 식은 (42), (43), (44)로 주어진다. ^S_Eq_{est,t} = ^S_E\hat{q}_{est,t-1}+^S_E\dot{q}_{est,t}\Delta t \cdots(42)\\\ ^S_E\dot{q}_{est,t} = ^S_E\dot{q}_{\omega,t}-\beta ^S_E\dot{\hat{q}}_{\epsilon,t} \ cdots(43)\\\ ^S_E\dot{\hat{q}}_{\epsilon,t}= \frac{\nabla {\bf{f}}}{||{\bf{f}}||} \cdots(44) 이상이 필터 퓨전 부분이다. 그 후 원논문에서는 지자기에 대한 고찰이 진행되고 있지만 생략합니다. 여기까지가, 논문을 읽은 범위에서의 자신 나름의 이해의 하나의 단락입니다. Matlab의 Madgwick Filter 검증 그래서, 지금까지의 알고리즘을 실제의 센서로 취득한 값에 적응해 효과를 보고 싶습니다. C 언어의 코드는 논문의 끝에 있었으므로, 이번은 MATLAB에서 실현해 보겠습니다. 센서는 MPU9250을 이용해, 측정주기 1kHz, 각종 필터 기능은 오프로 설정했습니다. 측정 시간은 25sec로 적당히 손으로 움직였을 때의 데이터입니다. 먼저 자이로 센서와 가속도 센서의 원시 데이터를 각각 아래에 그림으로 나타냅니다. 여기 IMU 원시 데이터
그림 1 가속도 센서의 원시 값

그림 2 자이로 센서의 원시 값

다음으로 자이로 센서에 의한 각속도만을 적분하여 구한 요·피치·롤 각도와 Madgwick 필터를 이용하여 구한 것을 아래에 그림으로 나타냅니다.

여기에 ypr 데이터

그림 3 자이로 온리


그림 4 Madgwick filter 적용 후

이상과 같이 자이로 센서만으로는 피치, 롤각이 드리프트하고 있는 것에 대해서, Madgwick 필터를 이용하는 것으로 억제할 수 있는 것을 확인할 수 있습니다. 한편, 지자기를 사용하지 않기 때문에 요각은 드리프트가 발생하고 있습니다.

사이고에게

실제로 기사를 써 보면서, 자신의 이해의 얕음이 점점 몸에 물들어 오는 것이었습니다. 상당한 부분이 자신이라도 이해가 어색하기 때문에, 읽어 주신 분에게는 꼭 원논문을 읽어 주시면 좋겠습니다.

아래에 사용한 Matlab 코드를 참고 붙여 넣습니다.

clear all;
close all;

% データ読み込み
load('data_imu_20171220_8.mat');

imu_acc=mpu_data(:,1:3);
imu_gyro=mpu_data(:,4:6);

Ts_imu=1e-3;    %サンプリング間隔
N_imu=length(imu_gyro);
t=0:Ts_imu:(N_imu -1) *Ts_imu;
q = [1 0 0 0];  %初期クォータニオン

figure('Name','IMU Acc');
plot(t,imu_acc); legend('acc x','acc y','acc z');
ylabel('加速度[m/s^2]');xlabel('time[s]');
saveas(gcf,'imu_acc.jpg');
figure('Name','IMU Gyro');
plot(t,imu_gyro);legend('gyro x','gyro y','gyro z');
ylabel('角速度[rad/s]');xlabel('time[s]');
saveas(gcf,'imu_gyro.jpg');

% もっとも単純なジャイロセンサの積分(クォータニオン空間)
for i=2:N_imu
    imu_quatlized = [0 imu_gyro(i,:)
            -imu_gyro(i,1) 0 -imu_gyro(i,3) imu_gyro(i,2)
            -imu_gyro(i,2) -imu_gyro(i,3) 0 -imu_gyro(i,1)
            -imu_gyro(i,3) imu_gyro(i,2) imu_gyro(i,1) 0];

    q(i,:)=q(i-1,:) + (1/2).* q(i-1,:) * imu_quatlized .* Ts_imu;
    q(i,:) = q(i,:)./norm(q(i,:));
end

[pitch, roll , yaw] = quat2angle(q,'YXZ');
deg_pitch = rad2deg(pitch);
deg_roll = rad2deg(roll);
deg_yaw = rad2deg(yaw);

figure('Name','IMU gyro only');
plot(t,[deg_yaw';deg_pitch';deg_roll']');
ylim([-120,120]);
legend('yaw','pitch','roll');
ylabel('角度[deg]');xlabel('time[s]');
saveas(gcf,'only_gyro.jpg');

% ここからMadgwickフィルタを実装する
% q_mad=q(1,:);
q_mad = q;
beta = sqrt(3/4)*pi*(5.0/180.0);%原論文のまま
for i=2:N_imu
%     加速度センサの正規化
    acc_norm=imu_acc(i,:)./ norm(imu_acc(i,:));
%     J,\nabla{f}の計算
    f_g=[(2*(q_mad(i-1,2)*q_mad(i-1,4)-q_mad(i-1,1)*q_mad(i-1,3)) - acc_norm(1))...
        (2*(q_mad(i-1,1)*q_mad(i-1,2)+q_mad(i-1,3)*q_mad(i-1,4)) -acc_norm(2))...
        (2*(1/2 - q_mad(i-1,2)^2 - q_mad(i-1,3)^2) - acc_norm(3))]';
    J_g=[-2*q_mad(i-1,3) 2*q_mad(i-1,4) -2*q_mad(i-1,1) 2*q_mad(i-1,2)
        2*q_mad(i-1,2) 2*q_mad(i-1,1) 2*q_mad(i-1,4) 2*q_mad(i-1,3)
        0 -4*q_mad(i-1,2) -4*q_mad(i-1,3) 0];
    nabla_f = J_g'*f_g;

%     式(44)の計算
    q_ep=nabla_f ./ norm(nabla_f);

%     式(43)の計算
    imu_quatlized = [0 imu_gyro(i,:)
            -imu_gyro(i,1) 0 -imu_gyro(i,3) imu_gyro(i,2)
            -imu_gyro(i,2) -imu_gyro(i,3) 0 -imu_gyro(i,1)
            -imu_gyro(i,3) imu_gyro(i,2) imu_gyro(i,1) 0];
    q_dot_t_omega = (1/2).* q_mad(i-1,:) * imu_quatlized;
    q_dot_est = q_dot_t_omega - beta * q_ep';

%     式(42)の計算
    q_mad(i,:)=q_mad(i-1,:) + q_dot_est .* Ts_imu;
end

[pitch, roll , yaw] = quat2angle(q_mad,'YXZ');
deg_pitch = rad2deg(pitch);
deg_roll = rad2deg(roll);
deg_yaw = rad2deg(yaw);

% Madgwcik
figure('Name','IMU Madgwick');
plot(t,[deg_yaw';deg_pitch';deg_roll']');
ylim([-120,120]);
legend('yaw','pitch','roll');
ylabel('角度[deg]');xlabel('time[s]');