고정 관성 좌표계(Inertial Reference Frame, I)에서 바라본 센서 좌표계(Sensor Frame, S)의 자세는 쿼터니언의 형태로 다음식(1)과 같이 표현 가능하다.

여기서, q₀는 쿼터니언의 스칼라 부분을, e = [q₁ q₂ q₃]^T는 벡터 부분을 의미한다. 본 논문은 구면 관절(Spherical Joint)로 연결된 두 링크 사이의 관절각 추정을 주제로 한다. 이때, 공통 관성 좌표계에서 관찰된 두 링크(즉, i와 j 링크)의 자세를 각각 q_i와 q_j라고 하면, 두 링크의 상대적인 자세, 즉 관절각은 다음 식(2)와 같다.

여기서 ⊗는 쿼터니언의 곱셈 연산자(Product Operator)이며, 위첨자 *는 해당 쿼터니언의 켤레(Conjugate)이다.

3 × 1 벡터 [x₂ x₂ x₃]^T를 쿼터니언의 형태로 x = [0 x₁ x₂ x₃]^T와 같이 나타낼 때, x의 관찰 좌표계를 센서 좌표계 S에서 관성 좌표계 I로 변환하는 식은 다음 식(3)과 같이 표현 가능하다.

강체의 회전운동은 강체 각속도 ω =[ω_x ω_y ω_z]^T를 이용한 다음 식(4)의 쿼터니언 미분 방정식으로 표현할 수 있다.

여기서

식(5)에서 [ω×]는 벡터 ω의 외적 행렬(Product Matrix)로 다음 식(6)과 같이 정의된다.

식(4)를 통해, 자세 쿼터니언의 이산시간(Discrete-Time)에 대한 스트랩다운(Strapdown) 적분식은 다음 식(7)과 같이 표현할 수 있다.

여기서 q_S,t는 이산시간 t에서의 자세 쿼터니언을, Δt는 샘플링 간격 시간을 의미한다.

식(7)의 구현에 있어 각속도 ω는 자이로스코프 신호를 사용하였다. 이때 자이로스코프 신호는 각속도에 더하여 잡음 오차 등을 포함하고 있으므로, 식(7)을 통해 구한 q_S,t에는 적분에 따른 표류오차가 발생한다. 이러한 표류오차를 포함한 자세 쿼터니언을 예측(Predicted) 쿼터니언 q_S,t로 정의한다.

본 논문은 관절각 추정에서 두 링크 간에 상대적인 표류오차를 감소시키기 위해 구면 관절의 기구학적 구속 조건을 이용한다. 구면 관절로 연결된 두 링크에 있어, i 링크로부터 그리고 j링크로부터 계산된 관절 중심에서의 가속도 f_i와 f_j는 각각 식(8)과 식(9)와 같다.⁶

여기서, 예를 들어 a_i는 센서 부착 위치에서의 가속도이며, ω˙i는 자이로스코프 신호값의 수치 미분을 통해 구한 각가속도이다. 또한, p_i는 센서에서 관절 중심까지의 위치 벡터로서 Ref. 31에 소개된 캘리브레이션 과정을 통해 미리 결정된다.³¹ 이때, f_i와 f_j는 모두 자신의 좌표계에서 관찰되어진 채 구해지며 즉,fii와 fij이다. 관절 중심의 가속도 f_i와 f_j는 관성 좌표계에서 관찰된다면 동일하므로, 식(3)을 적용하여 다음 식(10)과 같은 구속 조건식을 도출할 수 있다.

식(10)의 구현에 있어, 식(8)과 식(9)의 외부가속도 ^Sa는 가속도계 신호를 사용한다. 가속도계 신호는 외부가속도와 중력가속도를 모두 포함하고 있지만, 양변에 동일하게 적용된 채 관성 좌표계에서 관찰되면 동일한 중력이 양변에 더해진 셈이 되므로 등식이 성립한다.

본 논문은 q_i와 q_j 각각이 아닌, 관절각, 즉 상대 자세 qij==qi*⊗qj를 추정하는 것이다. 따라서, q_i는 예측값인 qi-로 확정시킨 후, 이에 맞는 q_j를 구함으로써 관절각을 추정한다. 구속 조건 식(10)을 만족하는 q_j를 구하기 위하여, 우선 예측 자세에 의해 좌표 변환된 관절 중심 가속도를 다음 식(11)과 같이 설정한다.

식(10)의 구속 조건을 만족시키기 위해 두 가속도 f~i와 f~j의 방향을 일치시키는 보정 쿼터니언 δ는 다음 식(12)와 같다.⁶

여기서 보정 쿼터니언 δ의 회전각 β와 축 u는 각각 식(13)과 식(14)와 같다.

식(12)의 보정 쿼터니언을 이용하여, 표류오차가 감소된 j링크의 자세 쿼터니언 q^j은 다음 식(15)와 같이 표현된다.⁶

여기서 q^j는 구속 조건식을 이용하여 두 링크로부터 각각 구한 관절 중심 가속도벡터를 일치시키는 쿼터니언을 추정한 것이지, 각 링크에 포함된 표류오차를 감소시키는 것은 아니라는 점을 주의해야 한다. 앞서 언급한 바와 같이, 제안 방법은 구속 조건을 만족시킴으로써 두 링크의 상대적인 표류오차를 감소시키는 것이다.

식(10)과 식(15)를 통해 구속 조건을 만족하도록 보정 과정을 거치지만, 보정된 쿼터니언 q^j역시 다음과 같은 부정확의 요인을 지니고 있다. 식(8)과 식(9)를 통해 구해진 fii와 fjj 는 1) a와 ω에 관련된 가속도계와 자이로스코프 신호의 정확성, 2) ω˙와 관련된 수치 미분에 의한 신호 대 잡음비(Signal to Noise Ratio) 감소, 3) p와 관련된 캘리브레이션의 정확성 등에 민감하다. 그리고 무엇보다 관절 중심의 가속도가 크지 않은 경우, f~i와 f~j 의 방향을 일치시키는 보정 방법은 큰 오차를 발생시킬 수 있다.

제안 방법은 이러한 부정확성의 문제를 보완하기 위해서, 상보 필터를 사용하여 예측된 자세와 표류오차를 감소시킨 자세를 조합함으로써 최적의 자세를 추정한다. 상보 필터를 통해 보정된 j 링크의 자세 쿼터니언 qj,t+는 다음 식(16)과 같다.

여기서, μ는 가중 계수이다.

두 링크의 최종적 자세 qi-와 qj+ 가 추정된 후에 식(2)를 이용하여 두 링크의 상대적인 자세 qij+를 다음 식(17)과 같이 구할 수 있다.

제안하는 표류오차 감소 알고리즘의 성능을 검증하기 위해 6축 IMU와 3축 지자기센서를 포함하는 9축 IMMU (Inertial and Magnetic Measurement Unit)인 MTw (Xsens Technologies B.V.)가 사용되었다. 뚜렷한 표류오차의 감소 효과를 확인하기 위해 자이로스코프 신호는 편향(Bias)오차를 포함한 상태에서 사용되었다. 성능 검증에 필요한 참조값(Truth Reference)은 OptiTrack Flex13(Natrual Point) 광학식 모션 캡쳐 시스템을 통해 얻었으며 MTw와 Flex 13의 샘플링 주기는 100 Hz로 설정하였다. 관절각 추정을 위해 구면 관절로 연결된 2-링크 시스템으로 두 개의 모노포드(FX-3460A, Horusbennu)가 사용되었으며, 각 링크에 고정된 삼각자에 MTw와 광학식 모션 캡쳐를 위한 세 개의 마커를 부착하였다(Fig. 1 참조). i와 j 링크에 부착된 두 개의 센서에서 구면 관절의 중심을 향하는 위치 벡터는 각각 ⁱp_i = [51.8 1.7 -2.2]^T cm, ^jp_j = [53.9 1.5 -2.4]^T cm였다. 식(12)에서의 μ는 적합한 추정 결과를 보이는 0.1로 설정하였다.

Fig. 1 
Experimental setup

실험은 다양한 관절각이 나타나도록 Fig. 1에서와 같이 양쪽에서 각 링크의 말단을 손으로 잡고 다양한 방향으로 회전시키며 진행되었다. 제안 방법은 상대 표류오차를 감소시키기 위해 기구학적 구속 조건을 사용하므로, Tests 1-3은 동적 조건에 따른 추정 성능의 검증을 위해 각각 다른 가속도에서 진행되었다(Table 1 참조). 검증 실험에서의 최대 평균 가속도는 3.88 m/s²로, 유명 육상선수인 우사인 볼트의 경우 최대 가속도가 3.09 m/s²인 점을 고려하였을 때 대부분의 역동적 스포츠 동작에 대해서도 적용 가능한 것으로 판단된다.³⁰ Test 4는 지자기센서를 사용하는 알고리즘과 비교하기 위하여 실험 중에 주기적인 자기교란을 주었으며 평균 외부가속도 2 m/s²로 진행되었다. Test 5는 동적 조건이 변화할 때 제안 알고리즘의 추정 성능을 확인하기 위해 3분의 동적 조건과 1분의 정적 조건으로 실험을 진행하였으며 동적 조건에서 중간의 1분은 평균 외부 가속도가 0.62 m/s²인 저속 조건을, 나머지 2분은 2.93 m/s²인 고속 조건을 유지하였다. Tests 1-4는 180초 동안 진행되었고, Test 5는 240초 동안 진행되었다.

제안 방법의 성능은 기존의 세 가지 방법들과 추정 오차를 비교함으로써 검증되었다. Method 1은 식(7)의 스트랩다운 적분으로 예측된 두 링크의 자세를 이용하여 식(2)를 통해 관절각을 구하는 방법이다. Method 2는 함수 최적화 방법 중 하나인 경사 하강법(Gradient Descent)을 이용하여 자세를 추정한 후에 상보 필터를 적용하여 예측된 자세와 추정된 자세를 조합하는 IMMU 기반의 자세 추정 방법이다.²⁰ Method 3은 제안 알고리즘의 기반이 되는 Ref. 6의 표류오차 감소법으로 관절 구속 조건을 이용하여 두 링크 사이의 상대 표류오차를 보정하는 비실시간 알고리즘이다. 표류오차 감소를 위한 후처리 과정으로 두 링크의 자세 추정 과정에서의 고정 좌표계의 수직 방향 성분에 대한 보정과 보정 쿼터니언의 평균화 및 보간 등의 과정이 포함된다. Method 2의 파라미터 β는 해당 논문에서 사용된 0.033으로 설정되었다.²⁰

추정 결과의 성능 검증을 위해, 관절각을 의미하는 식(17)의 상대적인 자세 쿼터니언 qij+로부터 Z-Y-X 오일러각들인 롤(Roll), 피치(Pitch), 요(Yaw)를 추출하였다. 이들의 참조값과의 오차를 RMSE (Root Mean Squared Error)로 분석하였는데, Table 2는 Tests 1-5의 결과이다. 한편, Fig. 2는 Test 2에 대한 각 비교 방법의 추정 결과이다.

Fig. 2 
Fig. 2Res ults of Test 1: (a) Estimation errors of Method 1(Blue) and the proposed method (Red) with respect to the reference (Black dashed) and (b) Estimation errors of Method 2(Green), Method 3(Blue) and the proposed method (Red)

저속 조건의 Test 1에서 IMMU 기반의 Method 2가 가장 우수한 추정 성능을 보였으며 기구학적 구속 조건을 사용하는 Method 3과 제안 방법은 저하된 추정 성능을 보였다. 특히 요 성분의 RMSE는 각각 6.03과 9.19^o로 큰 오차를 나타냈다. 반면, 고속 조건의 Tests 2와 3에서 Method 3과 제안 방법은 모두 현저한 추정 성능의 향상을 보였으며 Method 2의 경우에 저하되었다.

보정 과정이 없는 Method 1은 고속 조건의 Test 2에서 평균 RMSE가 19.95^o로 큰 오차가 나타나는 반면에 표류오차 감소 상보 필터가 적용된 제안 방법은 1.20^o로 우수한 성능을 보였다. Fig. 2(a)에서 볼 수 있듯이 Method 1은 시간의 경과에 따라 오차가 증가하지만 제안 방법은 높은 정확도로 추정이 이루어졌다. IMMU 기반의 Method 2와 제안 방법을 비교하였을 때 Method 2는 저속 조건의 Test 1에서 평균 RMSE가 2.53^o로 제안 방법에 비해 1.78^o 우수한 성능을 보였으나 고속 조건의 Tests 2와 3에서는 제안 방법의 추정 성능이 우세하였다. Method 2의 경우, 고속의 시험 조건이 요 추정 성능을 저하시키는 것으로 판단된다.

Fig. 3에서 보이는 바와 같이 Test 4에서는 자기교란으로 인해 Method 2에 주기적인 요 성분의 표류오차가 발생하였으며 요 성분의 RMSE는 8.32^o로 큰 오차를 보였다. Method 2는 지자기센서를 사용하고 있는데, 자기교란은 센서 주위의 자기장에 영향을 주어 정확한 방향의 자기장을 측정하지 못하게 되므로 자세 추정의 정확성이 저하된다. 반면에 제안 방법은 IMU 기반으로 지자기센서를 사용하지 않기 때문에 자기교란과는 무관한 성능을 보이고 있다. Method 3은 고속 조건의 Tests 2와 3에서 가장 높은 추정 성능을 보였으며 평균 RMSE는 각각 1.09와 1.51^o로 제안 방법에 비해서 0.11과 0.19^o 우수한 성능을 보였다. 반면에 저속 조건의 Test 1에선 2.68^o로 고속 조건에 비해서 저하된 추정 성능을 보였다. Fig. 2(b)에서 확인할 수 있듯이 제안 방법은 비교적 급격한 오차가 발생하며 발생 오차의 폭은 최대 11.67^o로 넓게 나타난 반면에 Method 3은 최대 5.46^o의 비교적 좁은 오차 폭을 나타냈다. 비실시간 알고리즘인 Method 3은 모든 시간에 대해 예측된 자세를 기반으로 표류오차를 보정하는 후처리 과정을 이용하였기 때문에 실시간으로 보정하는 제안 방법에 비해 근소하지만 우수한 추정 성능이 나타나는 것으로 판단된다.

Fig. 3 
Results of Test 4: estimation errors of Method 2(Blue) and the proposed method (Red) with respect to the reference (Black dashed)

Fig. 4는 Test 5에서 실시간 표류오차 감소법에 상보 필터를 적용하지 않은 경우와 적용한 경우의 추정 결과를 보여준다. 동적 조건에 따른 추정 성능을 확인하기 위해 Fig. 4(a)는 관절 중심의 가속도 크기를 나타내고 있다. 저속 및 정적 구간에서 두 경우 모두 시간의 경과에 따른 요 성분의 표류오차가 발생하였다. 상보 필터를 적용하지 않은 경우 고속 구간으로 바뀌면서 표류오차가 감소되는 경향을 보였으나 여전히 큰 오차가 나타났다. 반면에 상보 필터를 적용한 경우, 정적 과정에서 발생한 표류 오차가 동적 조건에서 즉각적으로 감소하였으며 추정 성능의 향상을 통해 상보 필터의 효과를 확인하였다.

Fig. 4 
Results of Test 5: (a) Magnitude of joint center acceleration, and (b) and (c) are estimation results of the drift reduction method without and with the CF, respectively

Ref. 6에서의 보정은 식(8)과 식(9)의 두 링크의 관절 중심 가속도만을 이용하므로 주어진 qi-에 부합하는 유일한 q_j를 결정할 수 없다. 여기서 주의할 점은 식(15)를 통해 q^j구한 는 식(10)을 만족시키는 쿼터니언일 뿐이며 정확한 관절각을 만족하는 쿼터니언이 아니라는 점이다. 즉, 불완전한 보정이 이뤄지게 된다. 하지만 가용한 관측벡터 정보가 한 개밖에 없는 상황에서 불완전할지라도 보정을 거치는 것의 효과를 Ref. 6과 본 논문은 보여주고 있다. 이때 상기해야 할 점은 상대적인 자세의 추정이 반복적인 예측 및 보정 과정을 통해 이뤄진다는 점이다. 즉 예측을 통해 추정하고자 하는 자세는 참 자세의 근처에 위치하며, 보정을 통해 오차의 누적을 지속적으로 제한하고 있다. 보정의 과정 없이 예측만을 의지하여 추정이 이뤄진다면 표류오차의 증가는 불가피하지만, 지속적인 보정이 이뤄지는 경우에 표류를 방지하여 추정 성능이 확보될 수 있는 것이다.

제안 방법은 비실시간 표류오차 감소법인 Method 3에 비해서는 다소의 성능 열세를 보였다. 그 차이는 Tests 2와 3의 추정 결과에 대해서 각각 0.11과 0.19^o의 수준이다. Method 3은 후처리 과정에서 설정되는 다수의 파라미터에 따라서 추정 성능이 달라질 수 있기 때문에 측정 데이터에 적합한 파라미터를 설정해야 하는 어려움이 있다. 반면에 제안 방법은 실시간으로 표류 오차를 감소시킨다는 점에서 보다 많은 적용가능성을 지닌다.