์ด๋ฆ GitHub ์ ์ค๋ชจ @i3months ๊น๋ฏผ์ @new-borned
STM32F429(Cortex-M4) + FreeRTOS ๊ธฐ๋ฐ์ผ๋ก ์ด์ํ ์ผ์ 3๊ฐ์ ์ ์ธ์ (IR) ์ผ์ 2๊ฐ๋ฅผ ์ตํฉํ์ฌ, ์ฌ์ ์ ์ญ ๊ฒฝ๋ก ์์ด ๋ถ์ชฝ(North) ํ์ถ๊ตฌ๋ก ์๋ ดํ๋ ๋ฏธ๋ก ์์จ์ฃผํ ๋ก๋ด์ ๋๋ค.
์ต์ข ์ํคํ ์ฒ
ํ๋์ฝ๋ฉํ ๋ฏธ๋ก(๊ณ ์ ๋ฒฝ) + EKF ์ผ์ ํจ์ + Greedy Cardinal ๋ด๋น๊ฒ์ด์ + ๊ฒฐ์น ์ง์ ๋์ ์ฅ์ ๋ฌผ ํ๋ฅ ๋ชจ๋ธ๋ง
๋ฏธ๋ก๋ ์ฝ 5 m ร 10 m ์ง์ฌ๊ฐํ ๊ณต๊ฐ์ด๋ฉฐ, ์ถ๋ฐ์ ์์ ์ถ๋ฐํด ๋ถ์ชฝ ํ์ถ๊ตฌ์ ์ถฉ๋ ์์ด ๊ฐ์ฅ ๋น ๋ฅด๊ฒ ๋๋ฌํ๋ ๊ฒ์ด ๋ชฉํ์ ๋๋ค. ์ฅ์ ๋ฌผ์ uBrain ํฌ์ฅ ๋ฐ์ค๋ก ๊ตฌ์ฑ๋ ๊ณ ์ ํ ์ฅ์ ๋ฌผ๊ณผ ์ด๋ํ uBrain 1๋(๋์ ์ฅ์ ๋ฌผ)๋ก ๊ตฌ๋ถ๋ฉ๋๋ค.
๋น์ผ ์ค์ ๊ณต๊ฐ๋ ๋ฏธ๋ก ๋งต. ๋ชจ๋ ๋ฒฝ์ด ์ง๊ต(orthogonal)์ด๋ฉฐ ํ์ถ๊ตฌ๋ ๋ถ์ชฝ์ ์์นํ๋ค.
ํต์ฌ ๊ฐ์ ์ ๋ค์๊ณผ ๊ฐ์ต๋๋ค.
- ํ์ถ๊ตฌ๋ ๋ฐ๋์ ๋ถ์ชฝ์ ์์นํ๋ค.
- ๋ฏธ๋ก๋ 'ใ 'ํ(๊ณ ๋ฆฝ ๋ฃจํ) ๋งต์ด ์๋๋ค.
- ํ์ถ๊ตฌ ๋๋ฌ์ ํ์ํ ๋ฐฉํฅ ์ ํ ํ์๊ฐ ์ ํด์ ธ ์๋ค.
์ด ๊ฐํ ๊ฐ์ ์์์ ์์ ์ฑ์ด ๋ณด์ฅ๋๋ฏ๋ก, ๋ฌธ์ ๋ฅผ "๋๋ฐ์ด์ค์ ๋ฐฉํฅ ์ ๋ณด๋ฅผ ์ ๊ณตํ์ฌ ๋ถ์ชฝ ํ์ถ๊ตฌ๋ก ์์ ์ ์ผ๋ก ์๋ ด์ํค๋ ๊ทธ๋ฆฌ๋ ๋ฌธ์ "๋ก ์ฌ์ ์ํ์ต๋๋ค.
์ด๊ธฐ์๋ Occupancy Grid ๊ธฐ๋ฐ mapping โ ๋ฒฝ ์ถ์ถ โ A*/D* ์ ์ญ ๊ฒฝ๋ก๊ณํ์ ํตํฉํ ํ์คํ ์์จ์ฃผํ ํ์ดํ๋ผ์ธ์ ๊ตฌํยท์ค์ธกํ์ต๋๋ค. ๊ทธ๋ฌ๋ ์ค์ธก ๊ฒฐ๊ณผ,
- ์ฐ๋ฆฌ ํ ๋ก๋ด์ ์ขยท์ฐ ๋ชจํฐ ์ถ๋ ฅ ๋น๋์นญ์ด ํ ํ ๋๋น ์ปธ๊ณ ,
- ๋ณด๋์ IMU ์ผ์๊ฐ ๋ถ์ฌํ์ฌ ๋ถ์ ํํ ์ํ ์ถ์ ์ด ๋ถ๊ฐํผํ์ต๋๋ค.
์ด๋ก ์ธํด ์์ธ ์ถ์ ์ ๋์ ์ค์ฐจ๊ฐ ์ปค์ ธ Occupancy Grid ๋งคํ ์ ์คํ๋ ค ์ฃผํ ์์ ์ฑ์ด ๋จ์ด์ก์ต๋๋ค. ๋์ ๋น์ผ ์ค์ ์ ๋งต์ด ๊ณต๊ฐ๋๋ค๋ ์ ์ ์ฐฉ์ํ์ฌ, 2D ๋งต์ ํ๋์ฝ๋ฉํ๊ณ ๋์ ์ฅ์ ๋ฌผ ๊ตฌ๊ฐ๋ง y์ขํ ์๊ณ์ ์ผ๋ก ํ๋ฅ ๋ชจ๋ธ๋งํ๋ ๋ฐฉํฅ์ผ๋ก ์ค๊ณ๋ฅผ ์ถ์ฝํ์ต๋๋ค.
์ด๋ก ์ ์ต์ ์ฑ๋ณด๋ค, ํ๋์จ์ด ์ ์ฝ ํ์์ ๊ฒฌ๊ณ ์ฑ์ ๋ณด์ฅํ๋ ์๊ณ ๋ฆฌ์ฆ์ด ์๋ฒ ๋๋ ์์คํ ์์ ๋ ์ค์ํ๋ค๊ณ ํ๋จํ์ต๋๋ค.
๋ฏธ๋ก์๋ ๊ณก์ ๊ตฌ์กฐ๊ฐ ์ ํ ์๊ณ ๋ชจ๋ ๊ตฌ์ฑ์ด ์ง๊ต(orthogonal)์ ๋๋ค. ๋ฐ๋ผ์ ๋ก๋ด์ ์ฃผํ์ ์ํ์ข์ฐ + 90ยฐ ์ฝ๋ ํ์ ์ผ๋ก ๋จ์ํํ ์ ์๊ณ , ๋ณต์กํ ๊ฒฝ๋ก ์ถ์ข ๋ก์ง ๋์ FSM์ผ๋ก ํ์ํ์ต๋๋ค.
stateDiagram-v2
[*] --> INIT
INIT --> DRIVE: dF > 0
DRIVE --> EMERGENCY: isEmergency()
EMERGENCY --> DRIVE: rotate done
| ์ํ | ์ญํ |
|---|---|
INIT |
์ ์ ์ธ๊ฐ ํ ์ ํจ ์ธก๊ฑฐ(dF > 0) ๋๊ธฐ |
DRIVE |
์ฝ๋ ํ์ ยท VEER ยท ์ง์ง ๋ฑ ์ ์ ์ฃผํ |
EMERGENCY |
์ ๋ฉด ์ถฉ๋ ์๋ฐ(์ด์ํ/IR) ์ ํํผ ํ์ ํ ๋ณต๊ท |
์ด FSM์ด ์ ๋๋ก ์๋ํ๋ ค๋ฉด ์ง์ง ์๋ ด์ฑ๊ณผ 90ยฐ ํ์ ์ ๋ฐ๋๊ฐ ๋งค์ฐ ์ค์ํฉ๋๋ค. ํ์ ยท์ง์ง์ด ์กฐ๊ธ๋ง ์ด๊ธ๋๋ ์ค์ฐจ๊ฐ ๋์ ๋์ด ๊ฒฝ๋ก๊ฐ ๋ฌด๋์ง๋ฏ๋ก, ํ์ ยท์ง์ง ๋ณด์ ์ ์ง์ค์ ์ผ๋ก ๊ฒํ ํ์ต๋๋ค.
Non-wall-following ๋ฐฉ์์ผ๋ก, ์ธก๋ฒฝ ๊ฑฐ๋ฆฌ์ ๋ณํ๋ ฮd๋ฅผ ๊ธฐ์ค์ผ๋ก ์ฝ๋ ๋ฐ ๊ต์ฐจ๋ก๋ฅผ ์ด์ฐ ๊ฐ์งํ์ฌ ํ์ ยทํต๊ณผํฉ๋๋ค. ์์ฌ๊ฒฐ์ ์ ํญ์ ๋ถ์ชฝ(๋์ฐฉ์ง)์ ์ ํธํ๋๋ก ์ค์ ํ์ต๋๋ค.
- ์งํ ๋ฐฉํฅ์ ์ฐ์ ์์๋ฅผ ๋ถ์ฌํ๋ค: ๋ถ:0, ์ยท๋:1, ๋จ:2.
- 90ยฐ ํ์ (EMERGENCY ํฌํจ) ์ ์ด ์ฐ์ ์์๋ก ํํผ ๋ฐฉํฅ์ ๊ฒฐ์ ํ๋ค (priority-prefer rotation).
- ์ฐ์ ์์๊ฐ ๋์ผํ๋ฉด ์ขยท์ฐ ์ผ์ ๊ฐ์ ๋น๊ตํด ๋ ๋์ ์ชฝ์ผ๋ก ์งํํ๋ค.
- ๊ต์ฐจ๋ก ํ๋ณ ์ ๋ถํฅ ์งํ์ด ๊ฐ๋ฅํ๋ฉด ๋ถ์ชฝ์ ๊ฐ์ ์ ํํ๋ค.
์ด๋ก์จ ๋ถํฅ ๋จ์กฐ ๊ตฌ์กฐ ๊ฐ์ ํ์์ ์ฌ์ ์ ์ญ ๊ฒฝ๋ก ์์ด๋ ์ถ๊ตฌ ๋๋ฌ์ด ๋ณด์ฅ๋ฉ๋๋ค. ์ ๋ ์ขํ๊ฐ ์๋๋ผ ๊ตญ์ ์ธก๋ฒฝ ๊ฑฐ๋ฆฌ์ cardinal heading(์๋ ๋ฐฉํฅ)์ผ๋ก ๋ถ๊ธฐ๋ฅผ ํ๋จํ๋ฏ๋ก, ์์น ์ถ์ ๋์ ์ค์ฐจ(drift)์ ๊ฐํฉ๋๋ค.
์ด๋ํ uBrain 1๋๋ ์์น๊ฐ ๋ณํ๋ฏ๋ก ํ๋์ฝ๋ฉ ๋งต์ผ๋ก ๋ค๋ฃฐ ์ ์๊ณ , IMUยท์ ๋ฐ ์ผ์๊ฐ ์๋ ๋ณด๋์์ ์ด๋์ฒด๋ฅผ ์ ๋ฐ ์ถ์ ํ๋ ๊ฒ์ ์คํ๋ ค ๋ถ์์ ํฉ๋๋ค. ๋ฐ๋ผ์ ์ ๋ฐ ์ถ์ ๋์ ๊ด์ธก-ํ-๊ฒฐ๋จ ํด๋ฆฌ์คํฑ์ ์ฑํํ์ต๋๋ค.
- ๊ฒฐ์น์ ์ง์ ๋ง์ง๋ง ๊ตฌ๊ฐ(์ค์ต ์ฑ์ ๊ธฐ์ค 4๊ตฌ๊ฐ)์ ์ง์ ํ๋ฉด ๋์ ํํผ ๋ชจ๋๋ฅผ ํ์ฑํํ๋ค.
- ์ผ์ ๋น ์ข ์ ์ ์ ์ ํ๋ฅ ์ ์๊ฐ ๊ฐ์ค์ผ๋ก ๋์ ยท๊ฐ์ ์์ผ ์ ์ ๋ฒฝ๊ณผ ์ด๋ ๋ฌผ์ฒด๋ฅผ ๋ถ๋ฆฌํ๋ค.
- ๋ก๋ด์ Nํ ๋ถํฅ ์ฌ์ดํด์ ์ํํ๋ฉฐ ์ด๋ ์ฅ์ ๋ฌผ์ ํต๊ณผ ๋ฆฌ๋ฌยท๊ฐ๊ฒฉ์ ๊ธฐ๋กํ๋ค.
- ์ ๋ฉด์ด ์๊ณ ๊ฑฐ๋ฆฌ ์ด์์ผ๋ก ์ด๋ฆฐ ํต๊ณผ ์ฐฝ(window)์ด ํ์ธ๋๋ฉด ์ํญ ์๋๋ฅผ ์ผ์์ ์ผ๋ก ๋์ฌ(
V_cruise โ V_dash) ์ฅ์ ๋ฌผ์ด ๋๋์์ค๊ธฐ ์ ์ ๋น ๋ฅด๊ฒ ํต๊ณผํ๋ค.
if ( front_clear โฅ D_pass AND cycle_count โฅ N ) โ v โ V_dash
์ค๊ณ ๊ทผ๊ฑฐ: ์ ํํ ์์นยท์๋ ์ถ์ ์ ๊ณ ํ์ง ์ผ์ยทํํฐ๊ฐ ํ์ํ์ง๋ง, "ํต๊ณผ ๊ฐ๋ฅํ ์์ ๋ง ํฌ์ฐฉ"ํ๋ ๋ฌธ์ ๋ก ํ์ํ๋ฉด ์ ํ์ง ์ผ์๋ก๋ ์ถฉ๋ถํฉ๋๋ค. ํ๋ฅ ๊ฐ์ ๋๋ถ์ ํ ๋ฒ์ ์๊ฐ์ ๊ฐ์ง๊ฐ ํต๋ก๋ฅผ ์๊ตฌ ์ฐจ๋จํ์ง ์์ผ๋ฉฐ, Nํ ๊ด์ธก์ ์ฐ์ฐํ ์ค๊ฐ์ง์ ์ํ ๋ฌด๋ชจํ ๋์ง์ ๋ง๋ ์์ ์ฅ์น๋ก ์๋ํฉ๋๋ค.
์ธก๋ฒฝ ๊ฐ๊ตฌ๋ ์ ์ฑ๋ฅ ์ธก๋ฉด ์ด์ํ ์ผ์๋ก ํ์ ํ๋ฏ๋ก, ๋จ๋ฐ ์คํ์ดํฌ๊ฐ ์๋ ์ผ์ ์๊ฐ ์ง์์ ์ผ๋ก ์ด๋ ค ์๋์ง๋ฅผ ํ์ธํ๋๋ก ์ฆ๊ฑฐ๋ฅผ ๋์ ํ๊ณ ์๊ฐ์ด ์ง๋๋ฉด leak ํ๋ ๊ตฌ์กฐ๋ก ๋ง๋ค์์ต๋๋ค.
-
$\alpha \in (0,1)$ : ๋์ ๊ณ์(ํด์๋ก ๊ณผ๊ฑฐ ์ฆ๊ฑฐ ์ค๋ ์ ์ง). ๋ณดํต 0.7 ~ 0.9. -
$\mathbf{1}[\cdot]$ : ์ด๋ฒ ํฑ์ ๊ฐ๊ตฌ ์กฐ๊ฑด ๋ง์กฑ์ด๋ฉด 1, ์๋๋ฉด 0. - ํ์ :
$w_{open}(t) > \theta_{trigger}$ ์ด๋ฉด ๊ฐ๊ตฌ ํ์ .
์ฐ์์ ์ผ๋ก ์ฌ๋ฌ tick์ด ์ด๋ ค์ผ ์๊ณ๋ฅผ ๋์ผ๋ฏ๋ก ์คํ์ด ์ต์ ๋๊ณ , ํ ๋ฒ ํ๋จํ ๊ต์ฐจ๋ก๋ฅผ ์ฆ์ ์ฌํธ๋ฆฌ๊ฑฐํ์ง ์๋ debounce ํจ๊ณผ๋ ๋ถ์์ ์ผ๋ก ์ป์ต๋๋ค.
์ ์ง์๊ฐ ๋ณด์ : ๊ฐ๊ตฌ ํ์ ์ฆ์ ํ์ ํ๋ฉด ๊ต์ฐจ๋ก ์ค์์ด ์๋ ์ง์
๋ถ์์ ๋์ ๋ฒฝ์ ์ถฉ๋ํฉ๋๋ค. ์งํ ์๊ฐ์ ๋จ์ผ ์์๋ก ํ๋์ฝ๋ฉํ๋ฉด ์๋์ ๋ฐ๋ผ ํ์ ์์น๊ฐ ํ๋ค๋ฆฌ๋ฏ๋ก, ์ง์ motor_stop() ์ดํ ๊ฒฝ๊ณผ ์๊ฐ ฮt๋ฅผ ํ์ฌ ์๋์ ๋๋ฆฌ ๋ณ์๋ก ์ผ์ ์งํ ์๊ฐ์ ์ ์ํ์ต๋๋ค.
์ด๋ก์จ ๊ต์ฐจ๋ก ํ๋จ์ ์์ ์ฑ๊ณผ ๋ฐ์์ฑ์ ๋์์ ํ๋ณดํฉ๋๋ค.
๊ฐ์ฅ ๋ง์ ์๊ฐ์ ํฌ์ํ ์์ญ์ ๋๋ค. ์ด์์ ์๊ณ ๋ฆฌ์ฆ์ด ํ์ค ํ๋์จ์ด์์ ๊ทธ๋๋ก ๋์ํ์ง ์์ผ๋ฏ๋ก, ๋ฌผ๋ฆฌ ์ค์ฐจ๋ฅผ ์ถ์ ยท๋ณด์ ํ๋ ์ผ์ด ๋งค์ฐ ์ค์ํ์ต๋๋ค.
| ๋ฌธ์ | ์์ธ | ๋์ |
|---|---|---|
| ๋ฐฐํฐ๋ฆฌ ์์กด์ฑ | ์ถฉ์ ์ํ์ ๋ฐ๋ผ ์ขยท์ฐ ๋ชจํฐ ๊ฑฐ๋์ด ๋ฌ๋ผ์ ธ ์ง์ง์ด ํ๊ณ 90ยฐ ํ์ ์ด ์ด๊ธ๋จ (TICKS_PER_CM, pivot substep์ด ๋ฐฐํฐ๋ฆฌ ์ํ์ ๋ฐ๋ผ ๋ณํจ) |
ํญ์ 100% ์ถฉ์ ์ํ๋ฅผ ๊ฐ์ ํด Calibration, ์ค์ ์์๋ ํ์ถฉ์ ์ ์ง. ํ์ ์ ์ธ์ฝ๋ ํ๋ฃจํ๋ก ๊ตฌํํด ์๊ฐ๋ณด์ |
| ๋ฐํด์ ์ํจ ๋จธ๋ฆฌ์นด๋ฝ | ์ฐ์ธก ๋ฐํด ์ถ์ ๋จธ๋ฆฌ์นด๋ฝ์ด ์์ผ ํ์ ์ ๋ฌผ๋ฆฌ์ ์ผ๋ก ๋ฐฉํด โ ์ขยท์ฐ ์ธ์ฝ๋ ๊ฐ ๋น๋์นญ | ํ๋์จ์ด ์ง์ ์ ๊ฒ ํ ์ ๊ฑฐํ์ฌ ์ข์ฐ ๋์นญ์ฑ ํ๋ณต |
| ์ขยท์ฐ ๋ชจํฐ ๋น๋์นญ | ๋ฐ์ ๋ก๋ด์ ๊ตฌ์กฐ์ ์ถ๋ ฅ ํธ์ฐจ | ์ง์งยท๋๋ฆฌํํธ์ฉ V_TRIM_L, ํ์ ์ฉ ์ข/์ฐ ๋ถ๋ฆฌ substep, ํ๋ฃจํ pivot์ผ๋ก ๋น๋์นญ ์ต์ |
EXTI ์ธ์ฝ๋ ISR์ด quadrature edge๋ง๋ค ์ขยท์ฐ ํ ์นด์ดํฐ๋ฅผ ์ฆ๊ฐํ๋ฉฐ, uint16 wrap์ signed delta ์บ์คํ
์ผ๋ก ๋ณด์ ํด ํ tick์
์ขยท์ฐ ํ ๋ณ์๋ก๋ถํฐ ๋ณธ์ฒด ๋ณ์ ฮs์ ํ์ ฮฮธ๋ฅผ ๊ณ์ฐํฉ๋๋ค.
ํ tick ๋์ ํค๋ฉ์ด ์ ํ ๋ณํํ๋ค๊ณ ๋ณด๊ณ ํ๊ท ํค๋ฉ์ ์ฌ์ฉํ ์ค์ (RK2) ์ ๋ถ์ผ๋ก 1์ฐจ Euler๋ณด๋ค ํ ์ฐจ์ ๋์ ์ ํ๋๋ฅผ ํ๋ณดํฉ๋๋ค.
- Calibration:
TICKS_PER_CM = 50.8(์ง์ง 3 s ร 3ํ, ๋ถ์ฐ < 1 %),WHEEL_BASE_CM = 22.0(์บ๋ฆฌํผ ์ค์ธก ํ ์ ์ฌ๊ฐ ํ ์คํธ๋ก ์ฌ๋ณด์ ). 20 ms ์ฃผ๊ธฐ๋กSensorTask๊ฐ ํธ์ถ.
์ํ๋ฒกํฐ
Predict โ ๋น์ ํ ์ด๋๋ชจ๋ธ์ ์ ํํ๊ณ , ์ํ Jacobian Fยท์ ์ด Jacobian V๋ก ๊ณต๋ถ์ฐ์ propagate. ๋ ธ์ด์ฆ๋ ํ ์ฌ๋ฆฝ์ ๋ณ์ ๋น๋ก๋ก ๋ชจ๋ธ๋ง ํ V๋ก ํฌ์ํ์ฌ (x,y,ฮธ) ์๊ด๊ด๊ณ๋ฅผ ๋ณด์กดํฉ๋๋ค.
ํด์์ RayโSegment ๊ต์ฐจ โ ์ธก์ ๊ฐ์ ์ด์ํ 3๊ฐ์ ๊ฑฐ๋ฆฌ
์ผ์๋ณ ์์ฐจ 1D ๊ฐฑ์ โ R์ด ๋๊ฐ์ด๋ฏ๋ก 3์ผ์๋ฅผ ๋์์ ์ฒ๋ฆฌํ์ง ์๊ณ ์์ฐจ 1D Kalman update๋ก ์ฒ๋ฆฌํ์ฌ ํ๋ ฌ ์ญ์ฐ์ฐ์ ํํผํฉ๋๋ค.
์ด๋
ธ๋ฒ ์ด์
๊ฒ์ดํ
+ Joseph form โ ์๋ชป ์ ์๋ ๋ฒฝ์ผ๋ก ์ธํ EKF ๋ฐ์ฐ์ ๋ง๊ธฐ ์ํด
์ง์งยท90ยฐ ํ์ ๊ฐ์ ์ ์ฑํจ๊ฐ ํค๋ฉ ์ ํ๋์ ๋ฌ๋ ค ์์ผ๋ฏ๋ก, ์ธ์ฝ๋ ์ ๋ถ์ผ๋ก ์ป์ ์ ๋ ํค๋ฉ ฮธ์ ๋ํด ์ธก๋ฒฝ ํ์์ผ๋ก๋ถํฐ ์ฐ์ถํ ํค๋ฉ์ผ๋ก ๋ณด์ ํฉ๋๋ค. ์ธก๋ฉด ์ด์ํ๊ฐ ๋์ผ ๋ฒฝ๊น์ง ์ธก์ ํ ์๋ค ๊ฑฐ๋ฆฌ์ฐจ ฮd์ ๋ ์ธก์ ์ ์ ์ข ๋ฐฉํฅ ๋ฒ ์ด์ค๋ผ์ธ L๋ก๋ถํฐ,
์ฌ์ ๊ณต๊ฐ๋ ํ๋์ฝ๋ฉ ๋งต์์ ํ์ฌ ์ถ์ข
์ค์ธ ๋ฒฝ์ ์ ๋ ๋ฐฉํฅ
๋ ์ผ์์ ์ญํ ์ ๋ถ๋ฆฌํ์ฌ ์ํธ ๋ณด์ํฉ๋๋ค. ์ด์ํ๋ ์ ์ญ ์ธก๊ฑฐ ๋ฐ ํญ๋ฒ, ์ ์ธ์ ์ ๊ทผ์ ๋ฒํผ๋ฅผ ๋ด๋นํฉ๋๋ค.
| ์ผ์ | ๊ตฌํ ๋ฐ ์ญํ |
|---|---|
| ์ด์ํ F/L/R | TIM3 ์
๋ ฅ์บก์ฒ๋ก HC-SR04 ์์ฝ ํญ ์ธก์ โ US_TICKS_PER_CM(58)๋ก cm ํ์ฐ. 7-์ํ ์ฌ๋ผ์ด๋ฉ ์๋์ฐ์ median7(์คํ์ดํฌ ์ ๊ฑฐ)ยทstddev7(์ ๋ขฐ๋) ์ ์ฉ. ๋ฒฝ์ถ์ข
ยท์ฝ๋ rate-jumpยท์ ๋ฉด EMERGENCYยทEKF ์ธก์ ๊ฐฑ์ ์ ์ฃผ ์
๋ ฅ |
| ์ ์ธ์ L/R/๋ฐ๋ฅ | ADC 3์ฑ๋(์ข ADC3ยท์ฐ ADC2ยท๋ฐ๋ฅ ADC1, 12-bit) ํด๋ง. ์ข/์ฐ IR์ ๊ทผ์ '๋ฒํผ'๋ก ์ฌ์ฉ: ๊ฐ > IR_BUMPER_THRESH(2100)์ด๋ฉด 30ยฐ EMERGENCY ํธ๋ฆฌ๊ฑฐ(์ด์ํ์ ๊ทผ๊ฑฐ๋ฆฌ ์ฌ๊ฐ ๋ฐ ์ง์ฐ ๋ณด์) |
| ์ตํฉ ์๋ฆฌ | ์ด์ํ = ์ฅ๊ฑฐ๋ฆฌ ์ธก๊ฑฐ/ํค๋ฉ/์ฝ๋, ์ ์ธ์ = ๊ตญ์ ์ธก์/๊ทผ์ ์์ . ๋จ์ผ ์ผ์ ํ๊ณ(์ด์ํ ์ง์ฐ, IR ๋จ๊ฑฐ๋ฆฌ)๋ฅผ ์ญํ ๋ถ๋ฆฌ๋ก ์์ |
์ ์ ์ธ๊ฐ๋ถํฐ ์์ฃผ๊น์ง ControlTask FSM์ ์ ์ด ํ๋ฆ์ ๋๋ค. ๋งค 20 ms ํฑ๋ง๋ค EMERGENCY โ ์ฝ๋ ํ์ธ โ VEER โ ์ง์ง ์์ผ๋ก ์ฐ์ ์์ ํ๋จ์ ์ํํฉ๋๋ค.
flowchart TD
PWR([์ ์ ON ยท ์์คํ
์ด๊ธฐํ]) --> INIT["INIT ์ํ<br/>์ ํจ ์ธก๊ฑฐ dF > 0 ๋๊ธฐ"]
INIT -->|dF > 0| TICK([์ ์ด ํฑ 20 ms])
TICK --> C1{"isEmergency?<br/>์ ๋ฉด US โค 14cm or IR ๋ฒํผ"}
C1 -->|Yes| EMG["EMERGENCY ํํผ<br/>์ ๋ฉด: 90ยฐ N-์ ํธ ํ์ <br/>IR: 30ยฐ ํ์ + ์ ์ง<br/>heading ๊ฐฑ์ "]
C1 -->|No| C2{"์ฝ๋ ๊ฐ๊ตฌ๋ถ?<br/>์ธก๋ฒฝ rate-jump & N-ํฅ ๊ฐ์ "}
C2 -->|Yes| LEAD["์ ์ง(lead) โ 90ยฐ N-ํฅ ํ์ <br/>heading ๊ฐฑ์ "]
LEAD --> HN{"heading == N?<br/>(๋ถํฅ ์ฝ๋)"}
HN -->|Yes| NTC["north_turn_count++<br/>๋์ โฅ 4 โ ์ํญ ๋ถ์คํธ ๋์น<br/>V_CRUISE โ V_CRUISE_BOOST"]
HN -->|No| DONE
C2 -->|No| C3{"์ธก๋ฒฝ < 5cm?"}
C3 -->|Yes| VEER["VEER: 10ยฐ ํํผ ํ์ + ์ ์ง"]
C3 -->|No| FWD["์ง์ง cruise_duty<br/>๋ถ์คํธ ์ V_CRUISE_BOOST"]
EMG --> DONE([๋์ ์๋ฃ])
NTC --> DONE
VEER --> DONE
FWD --> DONE
DONE -->|๋ฃจํ 20 ms| TICK
FWD -->|๋ถ์ชฝ ํ์ถ๊ตฌ ๋๋ฌ| GOAL([์์ฃผ])
์ฃผ๊ธฐยท์ด๋ฒคํธ ํน์ฑ์ ๋ฐ๋ผ 4๊ฐ ํ์คํฌ๋ก ๋ถ๋ฆฌํ๊ณ , ์ฐ์ ์์๋ฅผ Sensor = IR(3) > Control(2) > Debug(1)๋ก ์ค์ ํฉ๋๋ค. ๊ฐ์ง๊ฐ ๋ชจ๋ ํ๋จ์ ์ ์ ์ด๋ฏ๋ก ์ผ์๋ฅผ ๊ฐ์ฅ ์ฐ์ ํฉ๋๋ค.
| ํ์คํฌ | ์ฐ์ ์์ | ์ฃผ๊ธฐ | ์ญํ |
|---|---|---|---|
SensorTask |
3 (1024) | 20 ms | ์ด์ํ median7/stddev7 โ odom_tick โ ekf_predict โ ekf_update |
IR_Task |
3 (512) | 20 ms | IR ADC 3์ฑ๋ ํด๋ง โ ir_left/right/floor ๊ฐฑ์ |
ControlTask |
2 (1024) | 20 ms | INIT/DRIVE/EMERGENCY FSM ยท ์ฝ๋ ยท VEER ยท ํ์ |
DebugTask |
1 (512) | 100 ms | LED ์ํ ์ธ์ฝ๋ฉ(์ฐ์ ์์ Cascade) |
- ํ์คํฌ ์์ฑ/์ค์ผ์ค๋ง:
xTaskCreate()๋ก 4 ํ์คํฌ ์์ฑ ํvTaskStartScheduler(). ์ฐ์ ์์ ๊ธฐ๋ฐ ์ ์ ์ค์ผ์ค๋ง. - ์ฃผ๊ธฐยท์๋ฐ์
์ ์ด:
osDelay()๋ก ๊ฐ ํ์คํฌ ์ฃผ๊ธฐ(20/100 ms)์ ๋ถํ ์์ ํ ์ง์ฐ(TASK_WARMUP_MS,CTRL_WARMUP_MS) ๋ถ์ฌ. - Lock-Free ๊ณต์ ์ํ: ์ผ์ฑโ์ ์ด ๋ฐ์ดํฐ(
dF/dL/dR๋ฑ)๋ ๋จ์ผ ์์ฐ์-๋จ์ผ ์๋น์ ํจํด. SensorTask๋ง ์ฐ๊ณ ControlTask๋ง ์ฝ์ผ๋ฉฐ, Cortex-M4์์ ์ ๋ ฌ๋ 32-bitint์ ๊ทผ์ด ์์์ ์ด๋ฏ๋ก Lock ์์ด ์์ . - ISR ๊ณ์ธต ๋ถ๋ฆฌ: ์ธ์ฝ๋(EXTI ๋ฐฉํฅ ๋์ฝ๋), ์ด์ํ ์์ฝํญ(TIM3 ์ ๋ ฅ์บก์ฒ, 16-bit wrap ์ฒ๋ฆฌ), IR(ADC ํด๋ง)์ ISRยทํ์คํฌ๋ก ์ ์ ํ ๋ถ๋ด.
์ด๊ธฐ์๋ TeraTerm์ผ๋ก UART ๋ก๊ทธ๋ฅผ ํ์ธํ์ง๋ง, Calibrationยท์ํ ์ฃผํ ์ค ์๋ฆฌ์ผ ์ผ์ด๋ธ์ ๊ณ์ ๊ฝ๊ณ ๋ค๋๋ ๊ฒ์ด ๋งค์ฐ ๋ถํธํ์ต๋๋ค. ์ด์ LED๋ก ๋ฐํ์ ์ํ๋ฅผ ์ธ์ฝ๋ฉํด ๋ฌด์ ์ผ๋ก ๋ด๋ถ ์ํ๋ฅผ ์ฝ๋๋ก ํ๊ณ , ์ฌ๋ฌ ์ํ๊ฐ ๊ฒน์น ๋๋ ์ฐ์ ์์ Cascade๋ก ๊ฐ์ฅ ์ค์ํ ๊ฒ์ ํ์ํฉ๋๋ค.
| ์ฐ์ ์์ | LED ํจํด | ์๋ฏธ |
|---|---|---|
| 1 VEER ์ค๋ฒ๋ ์ด | LED1(์ฐ)/LED4(์ข) | ์ง์ VEER ๋ฐ์ ์ธก ํ์ (์ฝ 2์ด) |
| 2 EMERGENCY | ์ ์ฒด ์ ๋ฉธ / LED2ยทLED3 | ์ ๋ฉด US=์ ์ฒด ์ ๋ฉธ, IR ์ข=LED2ยท์ฐ=LED3 |
| 3 ์๋ ๋ถ์คํธ | ์ ์ฒด ์ ๋ฉธ(์ง์) | cruise_boosted ํ์ ์๊ฐ ํ์ธ |
| 4 ๊ธฐ๋ณธ | LED1โ3 ์ํ 2์ง ยท LED4 ํค๋ฉ | FSM ์ํ + cardinal heading(N=off, S=on, E/W=์ ๋ฉธ) |
| ์์ | ๊ฐ | ์๋ฏธ |
|---|---|---|
US_TICKS_PER_CM |
58 | ์ด์ํ IC ์นด์ดํธ โ cm ํ์ฐ |
PIVOT_SUBSTEPS_90_L/R |
20 / 25 | 90ยฐ ํ์ substep (์ข์ฐ ๋น๋์นญ ๋ณด์ ) |
EMG_FRONT / D_MIN / D_OPEN |
14 / 5 / 150 | ๊ธด๊ธยท๊ทผ์ ยท๋ฌด๋ฒฝ ์๊ณ (cm) |
IR_BUMPER_THRESH |
2100 | IR ๋ฒํผ ํธ๋ฆฌ๊ฑฐ ADC ์๊ณ |
CORNER_RATE_CM |
15 | ์ฝ๋(๊ฐ๊ตฌ๋ถ) ํ์ ๊ธ๋ณ๋ |
NORTH_TURN_THRESHOLD |
4 | ์ํญ ๋ถ์คํธ ๋์น ๋ถํฅ ์ฝ๋ ์ |
V_CRUISE / _BOOST / _TURN |
18k / 20k / 20k | ์ํญยท๋ถ์คํธยทํ์ PWM ๋ํฐ |
์ ์ฌ๊ฐ ์ฃผํ ํ ๋ณต๊ท ์์น
| Run | (x, y, ฮธยฐ) ๊ฒฐ๊ณผ | ํด์ |
|---|---|---|
| 1 | (73, 94, 108) | ๋ฐฐํฐ๋ฆฌ ์ ์ ์ ์์ฌ โ ์ถ๋ ฅ ์ ํ๋ก ํธ์ฐจ ํผ |
| 2 | (34, 64, โ175) | ํ์ถฉ์ ์ํ |
| 3 | (9, 0, 103) | ์์น ํํฉ (wheelbase ์ฌ๋ณด์ ), ฮธ๋ง ์์ฐจ |
๋ฐฐํฐ๋ฆฌ ์ถฉ์ ์ํ๊ฐ ๊ฒฐ๊ณผ๋ฅผ ๋ค๋ฅด๊ฒ ๋ง๋๋ ์ฃผ์์ธ์ ๋๋ค. ๊ฒฝ๊ธฐ ์ง์ ์ถฉ์ ๊ด๋ฆฌ์ wheelbase ์ฌ๋ณด์ ์ผ๋ก ํํฉ ์ค์ฐจ๋ฅผ ์ต์ํํ์ผ๋ฉฐ, ์์ฌ ฮธ ์ค์ฐจ๋ IMU ๋ถ์ฌ์ ๊ธฐ์ธํ๋ฏ๋ก EKF ์ธก์ ๊ฐฑ์ ์ผ๋ก ์ต์ ํฉ๋๋ค.
| ์ด์ | ๋์ |
|---|---|
| ์ข์ฐ ๋ชจํฐ ๋น๋์นญ(ํผ) | ์ข/์ฐ ๋ถ๋ฆฌ substep(20/25) + V_TRIM ๋
ธ๋ธ + ํ๋ฃจํ, ๋ฌผ๋ฆฌ ์ ๊ฒ(๋จธ๋ฆฌ์นด๋ฝ ์ ๊ฑฐ) |
| IMU(์์ด๋ก) ๋ถ์ฌ | cardinal heading ๋ชจ๋ธ + EKF ์ธก์ ๊ฐฑ์ ์ผ๋ก ํค๋ฉ ๋ณด๊ฐ |
| ๊ตฌ๋ถ | ๋งํฌ |
|---|---|
| 1ํ์ฐจ ์ฃผํ (3 ๊ตฌ๊ฐ) | https://youtube.com/shorts/K_pr-sQFMR8 |
| 2ํ์ฐจ ์ฃผํ (2 ๊ตฌ๊ฐ) | https://youtube.com/shorts/Clsc8AX5SYY |
| 3ํ์ฐจ ์ฃผํ (1 ๊ตฌ๊ฐ) | https://youtube.com/shorts/vPwYhzRY5yc |
| ์ฐ์ต ์ฃผํ (์์ฃผ) | https://youtube.com/shorts/dIfozfXyMbs |
| GitHub | https://github.com/CNU-Embedded-Programming/Embedded-Challenge |
Embedded-Challenge/
โโโ Drivers/ # STM32F4xx HAL, CMSIS, BSP
โโโ Middlewares/ # FreeRTOS (CMSIS-OS)
โโโ docs/ # ์ด๋ฏธ์ง (uBrain, ๋งต)
โโโ project/RTOS/
โโโ Inc/ # main.h, HAL conf, IT ํค๋
โโโ Src/ # main.c (์ ์ฒด ๋ก์ง), it/msp/system
โโโ MDK-ARM/ # Keil uVision ํ๋ก์ ํธ (Project.uvprojx) โ ์ฌ์ฉ
โโโ EWARM/ # IAR ํ๋ก์ ํธ
โโโ TrueSTUDIO/ # TrueSTUDIO ํ๋ก์ ํธ
- Keil MDK-ARM uVision์์
project/RTOS/MDK-ARM/Project.uvprojx๋ฅผ ์ฝ๋๋ค. - ํ๋ก์ ํธ๋ฅผ ๋น๋ํ ๋ค STM32F429 ๋ณด๋์ ๋ค์ด๋ก๋/ํ๋์ํฉ๋๋ค.
- ์ ์๊ณผ ๋ชจํฐ ๋๋ผ์ด๋ฒ๋ฅผ ์ฐ๊ฒฐํ๊ณ , ์ฃผํ ํ๊ฒฝ์ ๋ง๊ฒ ์ผ์ ์๊ณ๊ฐยทํ์ substep์ ํ์ธํฉ๋๋ค.
- ํ์ /IR/์ง์ง ์บ๋ฆฌ๋ธ๋ ์ด์
์ด ํ์ํ๋ฉด
main.c์ ๋น๋ ์ค์์น(CALIB_PIVOT,CALIB_IR,CALIB_ODOM)๋ฅผ ๋ณ๊ฒฝ ํ ์ฌ๋น๋ํฉ๋๋ค.

