주행 중인 도로의 경사도 구하기 1/t.b.d.

gradient_dynamic

• 주행 중 노면의 경사도를 구한다.

# import

import sys
from pathlib import Path
import numpy as np
import pandas as pd
import plotly.express as px
from PIL import Image   # plotly 그래프로 출력하면 노트북 크기가 너무 커서 GitHub에 표시가 안됨. 그래서 이미지로 저장해서 표시하기 위해 사용함함

# 블로그에 올리기 위해 plotly renderer의 설정이 필요하다. 

import plotly.io as pio
pio.renderers.default = "notebook_connected" 

# constant

# 프로젝트 디렉토리 경로
k_dir_project = Path('.').absolute()

# 데이터 파일 저장 경로
k_dir_data = Path(r'C:\data\tosun\projects\20240725_venue\Logging\Bus')
if k_dir_data.exists():
    print(f'{k_dir_data = }')
else:
    print('No data directory found')
    sys.exit(1)

# data 디렉토리에 feather 파일이 많이 있을 경우, 전체 feather 파일을 출력하여 확인한다.
feathers = list(k_dir_data.glob('*.feather'))
print('feathers = ')
for i, feather in enumerate(feathers, start=1):
    print(f'{i}: {feather.name}')

# 1개 feather 파일을 대상으로 데이터를 처리한다.
i_feather = 0
feather = feathers[i_feather]

df = pd.read_feather(feather)

k_dir_data = WindowsPath('C:/data/tosun/projects/20240725_venue/Logging/Bus')
feathers = 
1: 20240725_venue2024_07_26_09_17_29.feather

vehicle speed를 구한다.

• vs = 네바퀴 속도 평균

df['vs'] = df[['ws_fl', 'ws_fr', 'ws_rl', 'ws_rr']].mean(axis=1)
fig_vs = px.line(df, x='ts', y='vs', title='차속', width=800)
fig_vs.write_image('vs.png')
display(Image.open('vs.png'))

• 그래프 확대
  

  

• 차속이 매끄럽지 않다.
• 매끄럽게 되도록 필터링을 한다.
• 전에 claude.ai가 작성한 칼만 필터 코드를 이용한다.

# 칼만 필터 구현
class KalmanFilter:
    def __init__(self, initial_state, initial_estimate_error, measurement_noise, process_noise):
        self.state = initial_state
        self.estimate_error = initial_estimate_error
        self.measurement_noise = measurement_noise
        self.process_noise = process_noise

    def update(self, measurement):
        # 예측
        prediction = self.state
        prediction_error = self.estimate_error + self.process_noise

        # 업데이트
        kalman_gain = prediction_error / (prediction_error + self.measurement_noise)
        self.state = prediction + kalman_gain * (measurement - prediction)
        self.estimate_error = (1 - kalman_gain) * prediction_error

        return self.state

kf_vs = KalmanFilter(initial_state=0, initial_estimate_error=0.1, measurement_noise=3, process_noise=0.003)
df['vs_filt'] = np.array([kf_vs.update(x) for x in df['vs'].to_numpy()])
fig_vs_filt = px.line(df, x='ts', y=['vs', 'vs_filt'], title='vs: raw vs filtered', width=800)
fig_vs_filt.update_yaxes(title='차속')

fig_vs_filt.write_image('vs_filt.png')
display(Image.open('vs_filt.png'))

• 그래프 확대
  

  

• 위 필터링 정도가 적당해 보인다.
• kf_vs = KalmanFilter(initial_state=0, initial_estimate_error=0.1, measurement_noise=3, process_noise=0.003)

df.tail(5)

	ts	ws_fl	ws_fr	ws_rl	ws_rr	sas	yaw_rate	lat_accel	cyl_pres	long_accel	brake_state	vs	vs_filt
38511	383.89	0.0	0.0	0.0	0.0	-14.6	0.90	0.15	15.5	-0.50	3	0.0	0.003120
38512	383.90	0.0	0.0	0.0	0.0	-14.6	0.92	0.11	15.5	-0.50	3	0.0	0.003023
38513	383.91	0.0	0.0	0.0	0.0	-14.6	0.92	0.10	15.6	-0.53	3	0.0	0.002929
38514	383.92	0.0	0.0	0.0	0.0	-14.6	0.86	0.12	15.5	-0.57	3	0.0	0.002838
38515	383.93	0.0	0.0	0.0	0.0	-14.6	0.84	0.15	15.5	-0.57	3	0.0	0.002749

• vs_filt가 완전히 0 kph가 되지 않는다. 바람직하지 않다. 내 용도에는 문제 없으리라 판단하여 진행한다.

a_long을 구한다.

• vs를 미분하여 a_long을 구한다.

k_kph_to_mps = 1000 / 3600
k_n_period = 50  # peroid = 10msec. k_n_period가 100이면, 1s 이다. 
df['d_vs'] = df['vs_filt'].diff(periods=k_n_period).mul(k_kph_to_mps).fillna(0)
df['d_ts'] = df['ts'].diff(periods=k_n_period).bfill()
df['a_long'] = df['d_vs'].div(df['d_ts']) # 미분

# long_accel 그래프를 그려보니 너무 노이지하다. 
# 제동 혹은 조향과 관계가 있는지 보기 위해 그래프에 brake_state와 sas를 추가한다.
# brake_state가 category 형이라 타입이 다른 vs, vs_filt와 한 그래프에 출력을 할 수 없단다. 
df['brake_state'] = df['brake_state'].astype(int) 
fig_a_long = px.line(df, x='ts', y=['long_accel', 'a_long', 'brake_state'], 
                     title='종가속도: long_accel(센서) vs a_long(vs_filt에서 계산)', width=800)
fig_a_long.update_yaxes(title='종가속도/ brake_state')
fig_a_long.write_image('a_long.png')

fig_sas = px.line(df, x='ts', y='sas', title='조향각', width=800)
fig_sas.write_image('sas.png')

display(Image.open('vs_filt.png'))
display(Image.open('a_long.png'))
display(Image.open('sas.png'))

• long_accel이 노이지한 것과 제동은 관계가 없어 보이고, 조향과 어느 정도 관계가 있어 보인다.
• sas의 절대값이 작은 범위(= 직진)일 경우에만 경사도 계산을 해야겠다. 우선 현재 방식대로 끝까지 해본다.
• vs가 0 kph인 구간에서 a_long은 0 m/sec^2이다. 맞다.
• 이 구간에서 a_long과 long_accel 사이의 차이는 노면 경사의 영향이다.
• long_accel이 너무 노이지하다. 칼만 필터를 적용한다.

long_accel 필터하기

kf_long_accel = KalmanFilter(initial_state=0, initial_estimate_error=0.1, measurement_noise=5, process_noise=0.001)
df['long_accel_filt'] = np.array([kf_long_accel.update(x) for x in df['long_accel'].to_numpy()])
fig_long_accel_filt = px.line(df, x='ts', y=['long_accel', 'long_accel_filt'], 
                              title='종가속도(측정): raw vs filtered', width=800)
fig_long_accel_filt.update_yaxes(title='종가속도')
fig_long_accel_filt.write_image('long_accel_filt.png')
display(Image.open('long_accel_filt.png'))

• 위 그림 정도의 필터링 정도가 적당해 보인다.
  • 1초 정도 지연이 있다.
• kf_long_accel = KalmanFilter(initial_state=0, initial_estimate_error=0.1, measurement_noise=5, process_noise=0.001)

경사도 구하기

• 경사도와 long_accel, a_long 사이의 관계는 아래 그림과 같다.
  

  

k_a_gravity = 9.81
df['gradient'] = np.tan((df['long_accel_filt'] - df['a_long']).div(k_a_gravity)) * 100

fig_long_accel_vs_a_long = px.line(df, x='ts', y=['long_accel_filt', 'a_long'], 
                                   title='종가속도: long_accel_filt(필터된 센서값) vs a_long(차속 미분값)', 
                                   width=800)
fig_long_accel_vs_a_long.update_yaxes(title='종가속도')
fig_long_accel_vs_a_long.write_image('long_accel_vs_a_long.png')

fig_gradient = px.line(df, x='ts', y=['gradient'], title='경사도 [%]', width=800)
fig_gradient.update_yaxes(title='경사도')
fig_gradient.write_image('gradient.png')

display(Image.open('long_accel_vs_a_long.png'))
display(Image.open('gradient.png'))

• 경사도를 구했다.
• 노이지한 부분들이 보인다.
• 실제 경사도가 위 그래프처럼 빠르게 변동하지는 않을 것이다.
• 경사도에 필터를 적용한다.

경사도를 필터한다.

kf_gradient = KalmanFilter(initial_state=0, initial_estimate_error=0.1, measurement_noise=100, process_noise=0.01)
df['gradient_filt'] = np.array([kf_gradient.update(x) for x in df['gradient'].to_numpy()])
fig_gradient_filt = px.line(df, x='ts', y=['gradient', 'gradient_filt', 'brake_state'], 
                            title='경사도: raw vs filtered', width=800)
fig_gradient_filt.update_yaxes(title='경사도')
fig_gradient_filt.write_image('gradient_filt.png')
display(Image.open('gradient_filt.png'))

df.tail(5)

	ts	ws_fl	ws_fr	ws_rl	ws_rr	sas	yaw_rate	lat_accel	cyl_pres	long_accel	brake_state	vs	vs_filt	d_vs	d_ts	a_long	long_accel_filt	gradient	gradient_filt
38511	383.89	0.0	0.0	0.0	0.0	-14.6	0.90	0.15	15.5	-0.50	3	0.0	0.003120	-0.003345	0.5	-0.006691	-0.538648	-5.427924	-5.968650
38512	383.90	0.0	0.0	0.0	0.0	-14.6	0.92	0.11	15.5	-0.50	3	0.0	0.003023	-0.003241	0.5	-0.006483	-0.538106	-5.424505	-5.963235
38513	383.91	0.0	0.0	0.0	0.0	-14.6	0.92	0.10	15.6	-0.53	3	0.0	0.002929	-0.003140	0.5	-0.006281	-0.537992	-5.425404	-5.957884
38514	383.92	0.0	0.0	0.0	0.0	-14.6	0.86	0.12	15.5	-0.57	3	0.0	0.002838	-0.003043	0.5	-0.006085	-0.538441	-5.431998	-5.952651
38515	383.93	0.0	0.0	0.0	0.0	-14.6	0.84	0.15	15.5	-0.57	3	0.0	0.002749	-0.002948	0.5	-0.005896	-0.538884	-5.438466	-5.947535

결론

• 센서로 측정한 종가속도와 바퀴 속도들에게 계산한 차속을 미분하여 주행 중 노면의 경사도를 구해보았다.
• 계산한 경사도가 얼마나 정확한지 판정하기 위해서는 실제 경사도 데이터가 필요하다. 현재 나는 이 정보가 없다.
• 경사도가 알려진 도로에서 다양한 가속도로 주행하며 데이터를 측정해서 계산법이 맞는지 검증을 해봐야겠다.
• 경사도를 제동 제어에 고려하여 제동 성능, 제동 안정성, 제동감(comfort, smoothness), 연비 개선을 이룰 수 없을까?

미니프로그램 아이디어

• 주행 중 실시간으로 경사도를 표시하도록 한다.
• 경사로에 차를 정차하여 경사도를 확인한다.
• 경사로를 오르며/내리며 데이터를 측정한다. 가속도를 바꿔가며 반복하여 데이터를 측정한다.
• 아마도 실시간으로 계산의 정확도를 확인할 수 있을 것이다.
• 데이터를 이용하여 경사도 계산법을 개선할 수 있을 것이다.
• 계산법과 데이터를 ai와 함께 검토하면, ai가 개선된 계산법을 만들어줄 수도 있을까?

참고

long_accel 필터

이동 평균

df['long_accel_filt'] = df['lat_accel'].rolling(window=50).mean()
fig_long_accel_ma = px.line(df, x='ts', y=['long_accel', 'long_accel_filt'], 
                            title='종가속도: 센서 기준 vs 이동 평균', width=800)
fig_long_accel_ma.update_yaxes(title='종가속도')
fig_long_accel_ma.write_image('long_accel_ma.png')
display(Image.open('long_accel_ma.png'))

이동 평균 결과

• 신호 지연과 왜곡이 심하다.
• 부적합

 

 

목차 :: hsl's tsmaster 사용기