Python을 이용한 위치에서 속도를 구하는 여러가지 방법에 대한 예제

엔코더를 데리고 여러가지 작업을 하다보면 엔코더를 이용해서 각도를 구하는 거야 당연한 이야기이지만, 그걸 이용해서 또 속도를 구하게 됩니다. 물론 아날로그적 세계에서야 미분을 하면 속도가 나오지만... 디지털의 세상에서는 그렇지 못하죠. 차분을 수행해야 합니다. 그런데.. 이 차분이 직접 수행해서 속도를 구해보면 살짝 실망하는 경우가 아~주 많습니다. 그건 위치를 측정하는 샘플 시간이 짧거나 ...엔코더의 분해능이 충분하지 않다면 아주아주아주 엄~~~청난 노이즈를 만나게 됩니다.^^ 이번에는 그런 노이즈를 경험하고도 어떻게 속도를 잘~ 구하는지 확인해보도록 하죠~^^

데이터 준비하기

exVelocityCalcRawData.zip

일단 위의 예제 파일을 받아서 사용하시면 이 글을 따라하실 수 있습니다.

import pandas as pd
import numpy as np
import matplotlib.pyplot as plt
from pandas import DataFrame
%matplotlib inline

sampleTime = 0.0001

rawData = pd.read_csv('exVelocityCalcRawData.csv')

rawData = rawData.drop(rawData.columns[0], axis = 1)

rawData

네.. 공유해 드린 파일에는 연습용 위치가 내장되어 있으며.. 이 데이터를 획득한 샘플주기는 100usec입니다.

plt.figure(figsize=(15,8))
plt.plot(rawData.time, rawData.Position)
plt.title('Position Raw Data', size=15)
plt.xlabel('time (s)', size=15)
plt.ylabel('rad', size=15)
plt.axis([4, 25, 0, 250])
plt.grid(True)

깨끗해 보이는 위치 그래프

네 위 raw data를 단순히 확인해보면 정~말 깨끗한 위치 결과라는 것을 알 수 있습니다. 엄청 깨끗하네요.... 그러나 이 신호를 차분해서 보면 이야기가 달라집니다.

단순 차분(미분)으로 속도 구해보기

velocityUsingyDiff = np.zeros(len(rawData.time))

for i in np.arange(1,len(rawData.time)):
    velocityUsingyDiff[i] = (rawData.Position[i] - rawData.Position[i-1])/sampleTime
    
rawData['velocityUsingyDiff'] = velocityUsingyDiff

plt.figure(figsize=(15,8))
plt.plot(rawData.time, rawData.velocityUsingyDiff, rawData.time, rawData.Position, 'c')
plt.xlabel('time (s)', size=15)
plt.ylabel('rad or rad/sec', size=15)
plt.title('Calculating Velocity using simple differencial', size=15)
plt.legend(['velocityDiff', 'Position'])
plt.axis([4, 25, -270, 240])
plt.grid(True)

위 코드를 보시면 for문 안에 있는 한 줄 코드가 차분을 수행하는 코드인데요. 당연하지만... 차분은 (현재값 - 이전값) / 시간간격입니다. 이렇게 해서 결과를 보면

단순한 차분으로 구한 속도는 구하기는 쉽지만.. 그 결과에 노이즈가 아주 심하다.

놀랍죠?.. 어떻게 저렇게 지저분할 수가 있을지... 그러나 저는 이전에 MATLAB에서 칼만필터로 속도를 구하는 예제를 다룬 글[바로가기]에서 보여드린 것 처럼 아주 작은 노이즈가 큰 차분 노이즈를 가질 수 있습니다. 아무튼.. 저 차분된 속도는 어떻게 사용하기가 좀 어렵겠습니다. 너~무 슬프게 생겼으니까요...

1차 저역통과필터 적용해보기

velocityLPF = np.zeros(len(rawData.time))
tau = 0.01

for i in np.arange(1,len(rawData.time)):
    velocityLPF[i] = (tau*velocityLPF[i-1] + sampleTime*velocityUsingyDiff[i]) / (tau + sampleTime)
    
rawData['velocityLPF'] = velocityLPF

plt.figure(figsize=(15,8))
plt.plot(rawData.time, rawData.velocityUsingyDiff, rawData.time, rawData.velocityLPF, 'r')
plt.xlabel('time (s)', size=15)
plt.ylabel('rad/sec', size=15)
plt.title('Calculating Velocity using 1st Low Pass Filter', size=15)
plt.legend(['velocityDiff', 'velocityLPF'])
plt.axis([4, 25, -270, 240])
plt.grid(True)

위 코드는 방금 차분한 신호에 1차 저역통과필터를 적용한 것입니다. for 문안에 한 줄이 저역통과필터인데요. IPython에서 저역통과필터 구하기라는 글[바로가기]에서 한 번 언급을 했었습니다.

노이즈가 심한 속도 결과에 저역통과필터를 적용해서 다듬어 볼 수 있다.

그 결과는 위 그림의 빨간색에 나타나 있네요. 훨씬 보기 쉽네요.^^ 

속도 계산 샘플 속도를 늦추기

이제 차분을 통해 속도를 계산하는 것과 또... 차분된 속도에 저역통과필터를 적용해서 다듬는 것 말고... 속도 계산을 하는 주기를 위치를 측정하는 주기보다 느리게 세팅해서 계산해 보는 방법이 있습니다. 

velocityDiffLT = np.zeros(len(rawData.time))
termLength = 50

for i in np.arange(1,len(rawData.time)):
    if (i%termLength == 0):
        velocityDiffLT[i] = (rawData.Position[i] - rawData.Position[i-termLength])/(sampleTime*termLength)
    else:
        velocityDiffLT[i] = velocityDiffLT[i-1]

rawData['velocityDiffLT'] = velocityDiffLT

plt.figure(figsize=(15,8))
plt.plot(rawData.time, rawData.velocityUsingyDiff, 
         rawData.time, rawData.velocityLPF, 'c', rawData.time, rawData.velocityDiffLT, 'r', )
plt.xlabel('time (s)', size=15)
plt.ylabel('rad/sec', size=15)
plt.title('Velocity : Diff, LPF, LongTerm', size=15)
plt.legend(['velocityDiff', 'velocityLPF', 'velocityDiffLT'])
plt.axis([4, 25, -270, 240])
plt.grid(True)

그 구현은 위 코드의 for문 안에서와 같습니다. 위 코드는 100usec의 위치 측정 주기보다 50배 느리게 속도를 계산하도록 하고 있습니다.

또 다른 방법으로 위치 측정 주기보다 느린 주기로 속도를 측정해보는 방법도 있다.

그 결과는 위 그림에 나오는데요. 얼핏 보면 비슷해보이나요?^^

그러나.. 단순히 속도 측정 주기를 떨어뜨리는 것으로는 보다 부드러워지기는 쉽지 않다.

확대해서보면 비슷한듯 다르죠^^ 아무튼 뭐가 좋다 나쁘다 이야기를 하고 싶은 글은 아니고... 그저 위치를 이용해서 속도를 구하는 이야기를 하고 있기때문에... 또하나의 방법도 소개를 합니다.~^^

Moving Average 방법으로 속도 구하기

velocityDiffLTMovingAvr = np.zeros(len(rawData.time))
termLength = 100

for i in np.arange(termLength - 1, len(rawData.time)):
    velocityDiffLTMovingAvr[i] = np.mean(rawData.velocityUsingyDiff[i-(termLength-1):i])

rawData['velocityDiffLTMovingAvr'] = velocityDiffLTMovingAvr

plt.figure(figsize=(15,8))
plt.plot(rawData.time, rawData.velocityUsingyDiff, 
         rawData.time, rawData.velocityLPF, 'c', rawData.time, rawData.velocityDiffLT, 'r', 
         rawData.time, rawData.velocityDiffLTMovingAvr, 'k')
plt.xlabel('time (s)', size=15)
plt.ylabel('rad/sec', size=15)
plt.title('Velocity : Diff, LPF, LongTerm, LongTermMovingAvr', size=15)
plt.legend(['velocityDiff', 'velocityLPF', 'velocityDiffLT', 'velocityDiffLTMovingAvr'])
plt.axis([4, 25, -270, 240])
plt.grid(True)

위 코드의 방법은 아주 예전에 다룬적이 있는 Moving Average라는 이동 평균입니다.[바로가기] 일정 구간을 평균을 취해서 현재의 속도로 보는 것으로 역시 단순 차분한 속도값을 기본으로 하는 것입니다.

일정구간의 차분된 속도값을 평균을 취하는 방법도 있다.

이렇게 보면, 저역통과필터나... 느린 속도 계산 주기... 이동 평균... 모두 비슷해보이네요&^^

생각보다 큰 차이는 없다.

네.. 특정구간을 확대해서 봤는데 역시 양상이 비슷합니다.^^ 이제 단순 차분한 속도 그래프는 빼고 보도록 하죠~^^

차분된 신호를 다듬는 많은 방법이 있겠지만... 저역통과필터와 이동 평균을 많이 사용한다.

이렇습니다. 사실 비슷합니다. 뭐 비슷하게 보일려고 설정한 것도 있구요^^ 아무튼 이러 저런 방법을 보여주는 것이니까요^^

칼만 필터를 이용하여 속도 구하기

import numpy.linalg as lin

velocityKalman = np.zeros(len(rawData.time))

dt = sampleTime
A = np.array([[1, dt], [0, 1]])
H = np.array([[1,0]])
Q = np.array([[1,0],[0,3000]])
R = 0.1

x = np.array([[0],[20]])
P = 3*np.eye(2)

for i in np.arange(0,len(rawData.time)):        
    xp = np.dot(A, x)
    Pp = np.dot(np.dot(A, P), A.T) + Q
    
    K = np.dot(np.dot(Pp, H.T), lin.inv(np.dot(np.dot(H, Pp), H.T) + R))
    
    x = xp + K * (rawData.Position[i] - np.dot(H, xp))
    P = Pp - np.dot(np.dot(K, H), Pp)

    velocityKalman[i] = x[1]
    
rawData['velocityKalman'] = velocityKalman

최근 저는 칼만필터를 이용한 속도 구하기라는 예제를 MATLAB으로 다룬 김성필님의 책 내용을 예제로 다룬 적이 있는데요.[바로가기] 그 MATLAB코드를 Python으로 바꾼것입니다. 이렇게 칼만필터로 구한 속도를 보죠

plt.figure(figsize=(15,8))
plt.plot(rawData.time, rawData.velocityDiffLTMovingAvr, 'b',
         rawData.time, rawData.velocityLPF, 'c', 
         rawData.time, rawData.velocityKalman, 'r', )
plt.xlabel('time (s)', size=15)
plt.ylabel('rad/sec', size=15)
plt.title('Velocity : MovingAvr, LPF, Kalman', size=15)
plt.legend(['velocityDiffLTMovingAvr', 'velocityLPF', 'velocityKalman'])
plt.axis([6, 13, -130, 100])
plt.grid(True)

그래프로 확인해보면...

또 하나의 방법으로 칼만필터를 사용하는 방법이 있다.

아.. 차분한 식은 이제 빼고 보죠~~^^ 눈이 아프니까요. 셋 다 비슷하게 세팅했습니다. 다 속도 성분이 잘 나오네요^^

칼만필터도 비슷한 성능을 보여주고 있다.

마찬가지 입니다. 이번에는 긴 주기로 설정한 속도 계산 결과도 살짝 빼고 본겁니다.

특정 구간만 확대해본 다양하게 구해진 속도

위치와 함께 본 계산된 속도들(^^)

아.. 위치를 살짝 평행이동해서 위치값과 속도가 맞는건지 한 번 보죠... 위치 그래프에서는 보이지 않던 약간의 떨림들이 단순 차분된 속도에서는 노이즈로 보이지 않던 결과가 쉽게 관찰되네요~^^

rawData.to_csv('calculatingVelocitieS.csv')

rawData

아~ 마지막으로는 이 모든 결과를 csv 파일로 저장하는 것입니다. 그러면 다른 엑셀과 같은 스프레드시트에서 쉽게 열람해볼 수 있겠죠^^

최종 저장된 데이터

아무튼.. 그 결과를 담은 파일도 올려둘께요^^

calculatingVelocitieS.zip

오늘은 좀 글이 기네요~~~ 언제가 그렇듯 아~~주 기초적인 글입니다만.. ㅎㅎ