ํ์ค ์์ฝ
์ ์ ๊ตด์ฐฉ๊ธฐ์ ์ค์๊ฐ ๊ถค์ ๊ณํ์ ์ํด global planner (trajectory optimization)์ local planner (MPC + disturbance estimation)๋ฅผ ํตํฉํ ํ๋ ์์ํฌ๋ฅผ ์ ์ํ๋ฉฐ, force/torque, power, cylinder displacement, flow rate ๋ฑ ๋ฌผ๋ฆฌ์ ์ ์ฝ์กฐ๊ฑด์ ๋ชจ๋ ๋ง์กฑํ๋ฉด์ 30 Hz ์ด์์ ์ค์๊ฐ ๊ตญ์ ๊ถค์ ์ฌ๊ณํ์ด ๊ฐ๋ฅํจ์ ์๋ฎฌ๋ ์ด์ ์ผ๋ก ๊ฒ์ฆํ์๋ค. ์ ์ญ ๊ถค์ ์ ๊ตด์ฐฉ 1ํ๋น 1๋ฒ ๊ณ์ฐ๋๋ ๊ณ ์ ๊ธฐ์ค๊ฐ์ด๋ค.
1. ์ฐ๊ตฌ ๋ฐฐ๊ฒฝ
1.1 ๋ฌธ์ ์ ์
- ์ ์ ๊ตด์ฐฉ๊ธฐ(hydraulic excavator)์ ๊ตด์ฐฉ ์์ ์๋ํ์๋ ๋ค์ํ operational constraints (๊ตด์ฐฉ๋, ๋ฒํท ์์ธ ๋ฑ)์ physical constraints (force/torque ํ๊ณ, ํํ ์ ๋ ํ๊ณ, ํ์ ํ๊ณ ๋ฑ)๋ฅผ ๋์์ ๋ง์กฑํ๋ ๊ถค์ ๊ณํ์ด ํ์
- ๊ธฐ์กด trajectory optimization ๋ฐฉ๋ฒ์ ์ ์ฒด ๊ถค์ ์ ํ ๋ฒ์ offline์ผ๋ก ์ต์ ํํ๋ฏ๋ก, ์ด์ ์ค ๋ฐ์ํ๋ ์ธ๋(disturbance) โ ํ ์-๋ฒํท ์ํธ์์ฉ, ์ ์ ์ค๋ฆฐ๋ ๋ง์ฐฐ โ ์ ์ทจ์ฝ
- ์ธ๋์ผ๋ก ์ธํด ์ต์ ์ฑ ์ ํ๋ ๋ฌผ๋ก ์ ์ฝ์กฐ๊ฑด ์๋ฐ(constraint violation)๊น์ง ๋ฐ์ ๊ฐ๋ฅ
1.2 ๊ธฐ์กด ์ฐ๊ตฌ ๋ถ๋ฅ
| ๋ถ๋ฅ | ๋ํ ์ฐ๊ตฌ | ํน์ง | ํ๊ณ |
|---|---|---|---|
| Trajectory optimization | [6,7,9] | ๋น์ฉํจ์+์ ์ฝ์กฐ๊ฑด+์์คํ ๋์ญํ ๊ธฐ๋ฐ ์ ์ฒด ๊ถค์ ์ต์ ํ | Offline ๊ณ์ฐ, ์ธ๋ ๋ฏธ๋์, ๋จ์ ์ปค๋ ํ๋ผ๋ฏธํฐํ |
| Receding horizon / MPC | [12,13] | ์ด๋ ์ํ์ ๋ฐฉ์ ๊ถค์ ์ต์ ํ | [12] ๋จ์ผ ์ ๋ ฅ decoupled ๋์ญํ๋ง ๊ณ ๋ ค, [13] ์ฐ์ฐ์๊ฐ ๋ถ์กฑ (~3.3 Hz) |
| Non-optimization ๊ธฐ๋ฐ | [1,14-16] | DMP, reachability map, ๊ณ์ธต์ ์ ์ด | ์ ์ฝ์กฐ๊ฑด ๋ช ์์ ๊ณ ๋ ค ๋ถ๊ฐ, feasibility ๋ณด์ฅ ์ด๋ ค์ |
1.3 ์ฐ๊ตฌ ๊ธฐ์ฌ (Contributions)
- Global + Local planner ํตํฉ: operational constraints์ physical constraints๋ฅผ ๋ชจ๋ ๊ณ ๋ ค
- ์ถ์ ์ฐจ์ global trajectory optimization: ์ ์ด ์ด๋ด ๊ณ์ฐ ๊ฐ๋ฅํ ๋ฌธ์ ์ ์ํ
- ์ค์๊ฐ ์ ์ฝ ๊ถค์ ์์ฑ: ์ธ๋ ์ถ์ ๊ธฐ๋ฐ disturbance-aware ์ค์๊ฐ ๊ถค์ ๊ณํ
2. ์์คํ ๋ฐ ๋ฐฉ๋ฒ๋ก
2.1 ์ ์ฒด ์์คํ ์ํคํ ์ฒ
์ง๋ฉด ํ์
Trajectory Optimization
Disturbance Estimation โ MPC
FB Linearization PID
๊ตด์ฐฉ๊ธฐ ๋์ญํ ยท ์ํ ํผ๋๋ฐฑ
2.2 ๊ตด์ฐฉ๊ธฐ ๊ตฌ์ฑ (Hyundai HX300L)
3. ํต์ฌ ๊ธฐ์
3.1 ์ด๋ํ (Kinematics)
Configuration์ ๊ด์ ๊ฐ๋ ๋๋ ์ค๋ฆฐ๋ ๋ณ์๋ก ํํ:
qฮธ = [ฯU ฮธB ฮธA ฮธK]T โ โ4,โqL = [ฯU LB LA LK]T โ โ4๋ ํํ ์ฌ์ด์ ๋น์ ํ ์ ๋จ์ฌ(bijective) ๋งคํ:
qฮธ = gL(qL),โqL = gฮธ(qฮธ),โqฬฮธ = JL(qL)qฬL,โqฬL = Jฮธ(qฮธ)qฬฮธโ(1)๋ฒํท ํ ์์ธ(bucket tip pose):
E = [Ex(L) Ez(L) ฮธ(L)]T โ โ3์ฌ๊ธฐ์ L = [LB LA LK]T, ฮธ = ฮธB + ฮธA + ฮธK.
3.2 ๋์ญํ (Dynamics)
๊ด์ ๊ฐ๋ ๊ธฐ๋ฐ ๋์ญํ (Euler-Lagrange):
xฬฮธ = [qฬฮธ; qฬฮธ] = [qฬฮธ; Mฮธโ1(โhฮธ + JฮธTu + ฮ)] =: fฮธ(xฮธ, u, ฮ)โ(2)| ๊ธฐํธ | ์๋ฏธ |
|---|---|
| xฮธ = [qฮธT qฬฮธT]T โ โ8 | ์์คํ ์ํ |
| u = [TU FB FA FK]T โ โ4 | ์ ์ด ์ ๋ ฅ (swing torque + ์ ์ ์ค๋ฆฐ๋ force) |
| Mฮธ(qฮธ) โ โ4ร4 | ์ง๋ ํ๋ ฌ |
| hฮธ(qฮธ, qฬฮธ) โ โ4 | Coriolis-์์ฌ๋ ฅ + ์ค๋ ฅ ํฉ์ฑํญ |
| ฮ โ โ4 | ์ด๊ด ์ธ๋ (lumped disturbance) |
Mฮธ์ hฮธ์ ์์๋ณ ์ ๊ฐ, ๋ณ์ ์๊น ๊ตฌ๋ถ(์ธก์ /CAD/๋ฏธ์ง), FL๊ณผ์ ์ฐ๊ฒฐ์ ๋ณ๋ ๋ ธํธ ์ฐธ์กฐ:
02_research/ํ๋๋ก๋ด/๊ตด์ฐฉ๊ธฐ ๋์ญํ M๊ณผ h ์์ ํด๋ถ ยท ๊ตด์ฐฉ๊ธฐ ๋์ญํ M๊ณผ h ์์ ํด๋ถ
์ค๋ฆฐ๋ ๋ณ์ ๊ธฐ๋ฐ ๋์ญํ (local planning์์ ์ฌ์ฉ):
xฬL = [qฬL; qฬL] = [qฬL; MLโ1(โhL + u + ฮL)] =: fL(xL, u, ฮL)โ(3)์ฌ๊ธฐ์ ML(qL) = JLTMฬLJL, hL = JLT(MฬLJฬLqฬL + hฬL), ฮL = JLTฮ.
๋์: ์(2)์ M(ํ์ด ์ผ๋ง๋ ๋ฌด๊ฑฐ์ด๊ฐ)๊ณผ h(์ค๋ ฅยทํ์ ๊ด์ฑ ํ)๋ฅผ ๊ณ์ฐํ๋ ์ฝ๋๊ฐ dynamics.py์ ์๊ณ , ์ค์ ๋ก 3๊ตฐ๋ฐ์ ์ฌ์ฉ ์ค์
๋๋ค:
- ๊ตญ์ ์ ์ด ์ต์ ํ์ โ์ง๊ธ ์์ธ์์ ์ค๋ ฅ์ด ์ด๋งํผ ๋น๊ธด๋คโ ํญ
- โ์ด ์์ง์์ ํ์ด ์ผ๋ง๋ ๋๋๊ฐโ ๊ณ์ฐ (ํ๊ณ ์ด๊ณผ ํ์ ์ฉ)
- ํ ์ ํญ ์ถ์ ๊ธฐ์ โ์ ํญ์ด ์๋ค๋ฉด ์ด๋ ๊ฒ ์์ง์์ ๊ฒโ ์์ธก
์จ๊ธฐ์ง ์๋ ํ๊ณ: ์ด ๋ชจ๋ธ์ ์์น(๋ฌด๊ฒยท๊ด์ฑยทํ ํ๊ณ)๋ ์ค์ธก ๋ณด์ ์ ํ์ง ์์ ์์๊ฐ์ ๋๋ค. ์๋ฎฌ๋ ์ดํฐ๊ฐ ์๋ ค์ฃผ๋ ์ค์ ์ค๋ฆฐ๋ ํ๊ณผ ๋ชจ๋ธ ์์ธก์ ๋น๊ตํ๋ฉด ์๊ด์ด ์ฝํฉ๋๋ค(์๊ด๊ณ์ 0.07~0.43 ์์ค; ์ ์ถ์ ๋ถํธ๊ฐ ๋ฐ๋๋ก ๋์ ์ง๋ ๋ ๋ฐฉํฅ ํด์ ์ค๋ฅ ํ๋ณด๋ก ๊ธฐ๋ก). ๊ทธ๋์ โํ ํ๊ณ ๋ช % ์ฌ์ฉโ ๊ฐ์ ํ์ ์ ํ์ ๋ถ๊ฐ๋ก ๋ชป๋ฐ๊ณ , ํฉ๊ฒฉ ํ์ ์ ๊ตด์ฐฉ ๊ฒฐ๊ณผ(๊น์ดยท๋ถํผ)๋ก ํฉ๋๋ค.
ํ์ ๐ก ๋ถ๋ถ โ ๊ตฌ์กฐ๋ ์ฐ๊ฒฐยท๊ฐ๋, ์์น ๋ณด์ ์ ์ ์ ์๋ฎฌ ๊ณผ์ ์ ํจ๊ป ํ์
๐ ๋ฏธ๋ณด์ ์ด ์ค์ ๋ก ๋ฌด์์ ๋ง๊ฐ๋จ๋ฆฌ๋๊ฐ (2026-08-14 ๋ถ์ ์ถ๊ฐ)
๋ ผ๋ฌธ์ ์ด ๋ฌธ์ ๋ฅผ ์๋ฆฌ์ ์ผ๋ก ๋ง๋์ง ์์์ต๋๋ค. ๋ ผ๋ฌธ ์๋ฎฌ๋ ์ดํฐ์ ๊ฐ์ฒด ํ๋ผ๋ฏธํฐ์ MPC ๋ชจ๋ธ์ด ๊ฐ์ CAD ๊ฐ์ด๋ผ(ACKNOWLEDGMENT์ ํ๋๊ฑด์ค๊ธฐ๊ณ CAD ์ ๊ณต ๋ช ์), M, h ์ค์ฐจ๊ฐ 0์ด๊ณ LuGre ๋ง์ฐฐ + FEE ํ ์๋ง ์ธ๋์ผ๋ก ๋ค์ด๊ฐ๋๋ค. ์ฆ ๋ ผ๋ฌธ์ โ๋ชจ๋ ๋ฌผ๋ฆฌ ์ ์ฝ ์ถฉ์กฑโ์ nominal model = true plant ์กฐ๊ฑด์ ๊ฒฐ๊ณผ์ด๋ฉฐ, ์ค๊ธฐ ์ด์ ์ ๊ฐ์ฅ ๋จผ์ ๊นจ์ง ์ง์ ์ ๋๋ค. ์ฐ๋ฆฌ๊ฐ ์ง๊ธ ๊ทธ ์ง์ ์ ๋จผ์ ๋ง๋ ๊ฒ์ ๋๋ค.
๊ด์ธก๋ ์ฆ์ 3๊ฐ. ๋จ โ ์ ํด์์ ๋ณธ ์ ๋์ ใ์ ์ (2026-08-14)ใ ๋ ๊ฑด์ด ์ฐ์ ํฉ๋๋ค โ 0.07์ ๋ถํธ ๋ฐ์ ์ผ๋ก ์ค๋ช ๋์ง ์์ผ๋ฉฐ, ๋ฌด๊ฒยท๊ด์ฑ ๋ฏธ๋ณด์ ์ ์์ฌํ๊ธฐ ์ ์ ๋น๊ต ์กฐ๊ฑด๋ถํฐ ํ์ธํด์ผ ํฉ๋๋ค.
| ๊ธฐ๋ก๋ ์ฆ์ | ๋ฌด์์ ์๋ฏธํ๋๊ฐ | ๊ธฐ๋ก ์์น |
|---|---|---|
| โ ์๊ด๊ณ์ 0.07~0.43, ์ ์ถ ๋ถํธ ๋ฐ๋ | ๋ ํ์์ ์์ธ์ด ๋ค๋ฆ โ 0.07(๋ฌด์๊ด)์ ๋น๊ต ์กฐ๊ฑด(ฮ ๋๋ฝ) ์์ฌ, ๋ถํธ ๋ฐ์ ์ ๊ธฐํยท๋ถํธ ๊ท์ฝ ์์ฌ. ๋ฏธ๋ณด์ ์์ฒด์ ์ฆ๊ฑฐ๋ก ์ฐ๊ธฐ ์ ์ ์ ๋ ์ ์ ์ ๋จผ์ ๋ณผ ๊ฒ | ๋ณธ ์ |
| โก ฮฬ๊ฐ ํ ํ๊ณ 80%์ ์์ ํฌํ, ํ 4๋ฐฐ ๋จ๋จํด์ ธ๋ ๋ถ๋ณ | ฮฬ๊ฐ ํ ์ ๋ฐ๋ ฅ์ด ์๋๋ผ ๋ชจ๋ธ ์ค์ฐจ๋ฅผ ํก์ํ๋ ์ค | ยง3.4 ์(11) |
| โข ์ ์ฝ ์ผ๋ โํ์ ํ ์ด๊ณผโ ๊ฒฝ๊ณ ~24% ์์กด | ์ ์ฝ์ด ์ค์ ํ๋์จ์ด ํ๊ณ๊ฐ ์๋ ์์ ์ซ์์ ๊ฑธ๋ฆผ | ยง3.4 ์(10a-d) |
๋ฏธ๋ณด์ ์ ์ง์ ์ฆ๊ฑฐ๋ก ๊ฐ์ฅ ๋จ๋จํ ๊ฒ์ โก์ โข์ ๋๋ค. โ ์ ์์ธ์ด ๊ฐ๋ฆฌ๋ฏ๋ก ์ง๋จ 0๋จ๊ณ์์ ๋จผ์ ๋ถํดํด์ผ ํฉ๋๋ค.
์ค์ฐจ ์ ํ ๊ฒฝ๋ก โ โ์กฐ๊ธ ๋ถ์ ํโ ์ด์์ธ ์ด์ :
โ FL ์์ ์คํจ
u = hฬ + Mฬยทv โ ฮฬ โ qฬ = v + (์๋ฅํญ)
MPC ์์ธก ๋ชจ๋ธ x_{k+1} = A_dยทx_k + B_dยทv_k ๊ฐ ๊ฑฐ์ง์ด ๋จ
โก ์ํ ์ค์ผ โ
๊ฐ์ฅ ์ค์
์ธ๋ ์ถ์ ๊ธฐ ์์ฐจ๊ฐ M ๊ณผ h ๋ฅผ ์
โ ๋ชจ๋ธ ์ค์ฐจ๊ฐ ฮฬ ๋ก ํ๋ฌ๋ค์ด๊ฐ (์ํ ์์กด ํธํฅ)
โ FL์ด ๊ทธ ฮฬ ๋ฅผ ๋ค์ ๋นผ์ค
โ ์ค์ฐจ๊ฐ ํ๋ฃจํ๋ฅผ ํ ๋ฐํด ๋๋ฉฐ ์๋ก๋ฅผ ๊ฐ๋ฆผ
โป ์ด๋ ํญ์ด ์ง๋ฐฐํ๋์ง๋ ์ถ์ ๊ธฐ ํํ์ ๋ฐ๋ผ ๋ค๋ฆ โ ์๋ ์ ์ ์ฐธ์กฐ
โข ์ ์ฝ ์ค์ผ โ ๋จ, ๋ฒ์๋ ํ์ ๋จ
(10a) u^l โค u โค u^u โ u๊ฐ ํ๋ฆฌ๋ฉด ๋ฌด์๋ฏธ โ ์ค์ผ
(10b) p = uแตยทqฬ_L โค p^u โ u ๊ธฐ๋ฐ์ด๋ผ ๋์ ์ค์ผ โ ์ค์ผ
(10c) ์ค๋ฆฐ๋ ๋ณ์ โ ๊ธฐํ ๊ธฐ๋ฐ โ
์์
(10d) ์ ๋ ฮฃ|qฬ_L| โ ์ด๋ํ ๊ธฐ๋ฐ โ
์์
์ด๋ํ์ ยง3.1์ 40์นธ ์ค์ธก ํ๋ก ์ด๋ฏธ ๐ข ์ถฉ์ค์ด๋ฏ๋ก, ์ค์ผ ๋ฒ์๋ (10a)(10b) + ฮฬ + FL๋ก ํ์ ๋ฉ๋๋ค. ๋ณด์ ์์ ๋ฒ์๊ฐ ๊ทธ๋งํผ ์ข์์ง๋ค๋ ๋ป์ ๋๋ค.
์ด์ ํ์ โ์ธ๋ ์ถ์ ๊ธฐ๊ฐ p = Mฮธqฬ๋ก M์ ์โ์ด๋ผ๊ณ ์ ์์ผ๋, p = Mqฬ๋ ๋
ผ๋ฌธ ์(11)์ ๋ชจ๋ฉํ
ํํ์ด๊ณ ์ฐ๋ฆฌ disturbance.py๋ ๊ทธ ํํ๋ฅผ ์ฐ์ง ์๋๋ค(โ ยง3.4 ์(11) ใ๊ตฌ์กฐ ๋์กฐใ ์ฐธ์กฐ). ์ฐ๋ฆฌ ์์ฐจ๋ residual = Mqฬ + h โ JTu์ด๋ฏ๋ก ๋ชจ๋ธ ์ค์ฐจ๋ ์ด๋ ๊ฒ ๋ค์ด์จ๋ค:
residual = M qฬ + h โ Jแตu
โ โ
ฮดMยทqฬ ๋งํผ ฮดh ๋งํผ
(๊ฐ์๋์ ๋น๋ก) (ํญ์ ์ ๋)
๊ตด์ฐฉ์ ์ ์ยท์ ๊ฐ์์ด๋ผ qฬ์ด ์๋ค. ๋ฐ๋ผ์ ์ฐ๋ฆฌ ์ถ์ ๊ธฐ์์ ์ง๋ฐฐ์ ์ค์ผ์์ ๊ด์ฑ ฮดM์ด ์๋๋ผ ์ค๋ ฅยท์ง๋ ฮดh๋ค. ์๋ ํด๊ฒฐ ๊ฒฝ๋ก์ ์ฐ์ ์์๊ฐ ์ด์ ๋ฐ๋ผ ๋ฐ๋๋ค(0๋จ๊ณ ์น๊ฒฉ / 2๋จ๊ณ ๊ฐ๋ฑ).
๐ ํด๊ฒฐ ๊ฒฝ๋ก โ ์์๊ฐ ๊ฒฐ๊ณผ๋ฅผ ์ข์ฐํจ
- ๋ง์ฐฐ ์ด์ค ๊ณ์:
disturbance.py์local_mpc.py๊ฐ ๊ฐ์ h๋ฅผ ์ฐธ์กฐํ๋๊ฐ? ๋ค๋ฅด๋ฉด ๋ง์ฐฐ์ด ฮฬ ํก์ + FLdamping*qd๋ก ์ด์ค ๋ณด์๋์ด ์ฆ์ โก(์์ ํฌํ)์ ์์ธ์ด ๋๋ค โ ์ ๋ถ - ํฌํ ์์น: ํด๋ฆฝ์ด ํํฐ ๋ค์ธ๊ฐ ์์ธ๊ฐ? ์์ด๋ฉด ์ก์์ด ์ ๋ฅ๋์ด ์๋ ํธํฅ์ด ์๊ธด๋ค โ ์ ๋ถ
| ๋จ๊ณ | ํ ์ผ | ์ ์ด ์์์ธ๊ฐ | ๋น์ฉ |
|---|---|---|---|
| 0 | ์ ์ ์ค๋ ฅ ํ ์คํธ โ ํ์ ์ฌ๋ฌ ์์ธ๋ก ์ ์ง(qฬโ0, qฬโ0)์์ผ ๋จ๋ ํญ์ด ์ค๋ ฅ h(q)๋ฟ์ธ ์ํ์์ ์ค์ธก ํ๊ณผ ๋์กฐ. ์ด๋ ๋น์ ์ดยท์ ์ง์ด๋ฏ๋ก ฮโ๋ง์ฐฐ๋ฟ์ด๋ผ ์ ๋ ์ ์ ์ด ์ง์ ํ โฮ ๋๋ฝโ ๊ต๋์ด ์์ฒ ์ ๊ฑฐ๋จ | ํ ๋ฒ์ ์คํ์ผ๋ก โ ์ ๋ ์์ธ์ ๋ถํดํ๋ค: ์ฌ๊ธฐ์๋ ์๊ด์ด ๋ฎ์ผ๋ฉด ์ง์ง ์ง๋ยท๊ด์ฑ ์ค์ฐจ, ์๊ด์ ๋์๋ฐ ๋ถํธ๋ง ๋ฐ๋๋ฉด ๊ท์ฝยท์ฅ์ฐฉ๋ฐฉํฅ ๋ฌธ์ . ๋ถํธ ๋ฐ์ ์ ์์น๋ฅผ ์๋ฌด๋ฆฌ ๋ง์ถฐ๋ ์ ๊ณ ์ณ์ง. โ ์ฐ๋ฆฌ ์ถ์ ๊ธฐ ํํ์์๋ ฮดh๊ฐ ์ง๋ฐฐ ์ค์ผ์์ด๋ฏ๋ก ์ด ๋จ๊ณ์ ์ค์๋๊ฐ ์ ๋ ์ ์ ์ ๋ฐ๋ผ ๋ ์ฌ๋ผ๊ฐ | ๋ฐ๋์ |
| 1 | AGX์ ์ ๋ต์ ์ง์ ์ง์ โ RigidBody์ mass/COM/inertia๋ฅผ ๋คํํด dynamics.py์ URDF ๊ฐ๊ณผ ๋งํฌ๋ณ 1:1 ๋์กฐ |
์ค๊ธฐ์ ๋ฌ๋ฆฌ ์๋ฎฌ์ ground truth๋ฅผ ์๊ณ ์์. URDF์ AGX๊ฐ ๋ค๋ฅธ ์์ค์์ ์๋ค๋ฉด ์ฌ๊ธฐ์ ํฐ ๋ถ์ผ์น๊ฐ ๋์ด. ์ด๊ฑธ ๊ฑด๋๋ฐ๊ณ ํ๊ท๋ถํฐ ํ๋ฉด ์๊ฐ ๋ญ๋น | 1~2์ผ |
| 2 | ๊ด์ฑ ํ๋ผ๋ฏธํฐ ์ต์์์น ์๋ณ (1๋จ๊ณ๋ก ์ ๋ง์ ๋๋ง) | ๊ฐ์ฒด ๋์ญํ์ ๊ด์ฑ ํ๋ผ๋ฏธํฐ์ ์ ํ โ ํ์ค ๊ธฐ๋ฒ์ผ๋ก ํด๊ฒฐ. โผ ๋จ ์ฐ๋ฆฌ ์ถ์ ๊ธฐ์์ ฮดM์ qฬ์ ๋น๋กํด ๋ค์ด์ค๋๋ฐ ๊ตด์ฐฉ์ ์ ๊ฐ์์ด๋ผ ๊ธฐ์ฌ๊ฐ ์๋ค โ 0ยท3๋จ๊ณ ๋ค๋ก ํ์์ (์ ๋ ์ ์ ) | 1์ฃผ |
| 3 | ํ ํ๊ณ uu, ul ๊ต์ฒด โ Q22์ ๋ฐฉ๋ฒ 1(AGX actuator effort limit ๋ ธ์ถ) ์ ์ฉ | ฮฬ 80% ํฌํ๋ ํ๊ณ๊ฐ์ด ์ค์ ๋ณด๋ค ์๋ค๋ ๊ฐํ ์ฆ๊ฑฐ. ์ซ์๋ง ๋ฐ๊ฟ๋ ์ฆ์ ๊ฐ์ | ์ |
| 4 | ํ์ด๋ก๋ ์ถ์ ๋ถ๋ฆฌ โ Phase 3(๋น์ ์ด ์ด๋ฐ) ๊ตฌ๊ฐ์ ์ค๋ ฅ ํ ํฌ ์์ฐจ๋ก ๋ฒํท ๋ด ํ ์ง๋ ์ญ์ฐ โ M์ ๋ฐ์ | ํ ์ง๋์ 0โ๋ง์ฌ๋ก ์๋ณ์ด๋ผ CAD๋ก๋ ์คํ๋ผ์ธ ์๋ณ๋ก๋ ๋ชป ์ก์ | ์ค |
| 5 | ์์ฌ ์ค์ฐจ ๋ฐฉ์ด โ ํ์ ํ โค ํ๊ณ์ 90%์ 0.9๋ฅผ ์๋ณ ์์ฐจ ๊ธฐ๋ฐ ๋ฐฑ์คํ uueff = uu โ kฯresid๋ก ๋์ฒด |
์์ ์ซ์๋ฅผ ๊ทผ๊ฑฐ ์๋ ๊ฐ์ผ๋ก ์น๊ฒฉ | ์ |
2๋จ๊ณ ์๋ณ์ ํํ (๊ฐ์ฒด ๋์ญํ์ ๊ด์ฑ ํ๋ผ๋ฏธํฐ ์ ํ์ฑ):
ฯ = Y(q, qฬ, qฬ) ยท ฯ
ฯ = [m, mยทc_x, mยทc_z, I_yy, ...] โ ๋งํฌ๋ณ ๊ด์ฑ ํ๋ผ๋ฏธํฐ
Y = ํ๊ท ํ๋ ฌ (q, qฬ, qฬ ๋ก๋ง ๊ตฌ์ฑ)
ํ๋ฉด 3๋งํฌ(๋ถยท์ยท๋ฒํท)๋ผ ๋งํฌ๋น 10๊ฐ๊ฐ ์๋๋ผ ~4๊ฐ
โ base parameter 12๊ฐ ๋ด์ธ, ๋ฐ์ดํฐ ์๋ฐฑ ์ ์ด๋ฉด ์ถฉ๋ถ
- ๋น ๋ฒํท + ๊ณต์ค(๋น์ ์ด)์์ ์ฌ๊ธฐ ๊ถค์ (excitation trajectory) ์ํ โ ํ ์ ๋ฐ๋ ฅ ๋ฐฐ์
- ๋ฌผ๋ฆฌ ํ๋น์ฑ ์ ์ฝ(m>0, ๊ด์ฑ ์ผ๊ฐ๋ถ๋ฑ์) ๊ฑธ์ด ์ ์ฝ ์ต์์์น
- ๋ง์ฐฐ(Coulomb + viscous)์ ๊ฐ์ ํ๊ท์ ์ด๋ก ์ถ๊ฐํ๋ฉด ๋์ ๋ถ๋ฆฌ ๊ฐ๋ฅ โ
local_mpc.py์ ์ด๋ฏธdamping*qdํญ์ด ์์ด ์์ฐ์ค๋ฝ๊ฒ ํ์ฅ๋จ
3๋จ๊ณ(ํ๊ณ๊ฐ)๋ฅผ ๋จผ์ ์ฌ๋ฆฌ๋ฉด ฮฬ ํฌํ๊ฐ ํ๋ฆฌ๋ฉด์ ๋ชจ๋ธ ์ค์ฐจ๊ฐ ฮฬ๋ก ๋ ๋ง์ด ํ๋ฌ๋ค์ด๊ฐ โ์ข์์ง ๊ฒ์ฒ๋ผโ ๋ณด์ ๋๋ค. 0 โ 1ยท2 โ 3 โ 4 ์์๋ฅผ ์ง์ผ์ผ ๋ค ๋จ๊ณ๊ฐ ์ ๋จ๊ณ ์ค์ฐจ๋ฅผ ํก์ํด ์๋ชป๋ ๊ฐ์ผ๋ก ์๋ ดํ๋ ๊ฒ์ ๋ง์ต๋๋ค.
โ ๋ณด์ ์ฑ๊ณต ํ์ ์งํ
๊ฐ์ฅ ๊ฐ๋ ฅํ ๋จ์ผ ์งํ๋ ์์ ๊ณต๊ฐ(๋น์ ์ด)์์ ฮฬ โ 0์ ๋๋ค. ํ์ ๋ฟ์ง ์์๋๋ฐ ฮฬ โ 0์ด๋ฉด ๊ทธ ๊ฐ์ด ๊ณง ๋ชจ๋ธ ์ค์ฐจ์ ํฌ๊ธฐ์ ๋๋ค.
| ์งํ | ํ์ฌ | ๋ชฉํ |
|---|---|---|
| ์ ์ ์ค๋ ฅ ๊ตฌ๊ฐ ์๊ด๊ณ์ | ๋ฏธ์ธก์ (๊ธฐ์กด 0.07~0.43์ ฮ๊ฐ ์์ธ ๋์ ๊ตฌ๊ฐ ๊ฐ์ด๋ผ ์ง์ ๋น๊ต ๋ถ๊ฐ) | 0.95+ |
| ์ ์ถ ๋ถํธ (์ ์ ๊ตฌ๊ฐ) | ๋ฐ๋ | ์ผ์น |
| ๋น์ ์ด ๊ตฌ๊ฐ ฮฬ | ํ๊ณ์ 80% ํฌํ | ํ๊ณ์ 5% ์ด๋ด |
| ํ ๋ฌผ์ฑ 3์ข ์์ ฮฬ ๋ณํ | ์ ๋ณํจ | ๋ฌผ์ฑ์ ๋น๋กํด ๋ณํจ |
| โํ์ ํ ์ด๊ณผโ ๊ฒฝ๊ณ ์จ | ~24% | 0% ๋๋ ๊ทผ๊ฑฐ ์๋ ์์กด |
| ์ค๋ ฅยท๊ด์ฑ ๋ณด์ on/off ๋จ๋ ํจ๊ณผ | ๋ฏธ์ฆ๋ช | ์ ์๋ฏธํ ์ฐจ์ด ๋ฐ์ |
๋ง์ง๋ง ํญ๋ชฉ์ด ํนํ ์ค์ํฉ๋๋ค. ์ง๊ธ โ์ค๋ ฅยท๊ด์ฑ ๋ณด์ ๋ง ๋๋ฉด ๊ฒฐ๊ณผ๊ฐ ์ ๋ณํ๋คโ(ยง3.4 ์(12)(13)(14) ์ฃผ์)๋ฅผ ๊ตด์ฐฉ์ด ์จํํด์๋ก ํด์ํด ๋์๋๋ฐ, ๋ณด์ ํ์๋ ์ฌ์ ํ ์ ๋ณํ๋ค๋ฉด ๊ทธ ํด์์ด ํ๋ ธ๊ณ ๋ณด์ ์ด ์ฌ์ ํ ๋ถ์ ํํ๋ค๋ ๋ป์ ๋๋ค. ๋ฐ๋๋ก ์ฐจ์ด๊ฐ ๋๊ธฐ ์์ํ๋ฉด ๊ทธ ์์ฒด๊ฐ ๋ณด์ ์ฑ๊ณต์ ์ฆ๊ฑฐ๊ฐ ๋ฉ๋๋ค. ์ฆ ์ด ํญ๋ชฉ์ ยง3.4์ โ๋จ๋ ํจ๊ณผ ๋ฏธ์ฆ๋ช โ ๊ธฐ๋ก๊ณผ ํ ์์ผ๋ก ํ์ ํด์ผ ํฉ๋๋ค.
ยง3.1์ 40์นธ ํ์์ ์์น ๋ฏธ๋ถํ JL์ ๋ถํธ๊ฐ ๋ค์งํ๋ค๋ ์ค๋ช
์ ์ฑ๋ฆฝํ์ง ์๋๋ค. ํ๋ monotone ํ์ธ ํ ์ฐ์ด๋ฏ๋ก ๊ตฌ๊ฐ ๊ธฐ์ธ๊ธฐ์ ๋ถํธ๊ฐ ๋ฐ์ดํฐ์ ์ํด ๊ณ ์ ๋๊ณ , ๋ฏธ๋ถ ๊ณผ์ ์์ ๋ถํธ๊ฐ ๋ฐ๋ ์ฌ์ง๊ฐ ์๋ฆฌ์ ์ผ๋ก ์๋ค. ํ ๊ธฐ๋ฐ์ด๋ผ๋ ์ฌ์ค ์์ฒด๊ฐ ์คํ๋ ค ์ด ํ๋ณด๋ฅผ ๋ฐฐ์ ํ๋ค. ์ค์ ํ๋ณด๋ ์๋ ์ธ ๊ฐ์ง๋ค.
| ํ๋ณด | ์ ์ ์ถ๋ง ๋ค์งํ๋๊ฐ |
|---|---|
| ์ค์ธก ํ์ ๋ถํธ ๊ท์ฝ | ์๋ฎฌ๋ ์ดํฐ๊ฐ ์ฃผ๋ ๊ฐ์ด (ํค๋์ธกโ๋ก๋์ธก)์ธ์ง ๋ก๋ ์ถ๋ ฅ์ธ์ง, ์ธ์ฅ ์์์ธ์ง ์์ถ ์์์ธ์ง |
| ์ค๋ฆฐ๋ ์ฅ์ฐฉ ๋ฐฉํฅ | ์์ ์ ์ฅ ์ ๊ฐ๋๊ฐ ๊ฐ์ํ๋ ๋ฐฐ์น๊ฐ ํํ๋ค โ dLA/dฮธA < 0 (๋ถยท๋ฒํท๊ณผ ๋ฐ๋ ๋ถํธ) |
| ๋ถํธ ํ๋์ฝ๋ฉ | ์ฝ๋๊ฐ abs(dL/dฮธ)๋ฅผ ์ฐ๊ฑฐ๋ ๋ถํธ๋ฅผ ์์๋ก ๋ฐ์๋ ๊ฒฝ์ฐ โ ์ผ์ฝ๋น์ ๋ฌธ์ ๋ ๋ง์ง๋ง ์์น ๋ฏธ๋ถ ํ์ ์๋๋ค |
๋ ธํธ์ ์ด๋ฏธ ์ ์ด๋ โ์ง๋ ๋ ๋ฐฉํฅ ํด์ ์ค๋ฅ ํ๋ณดโ๊ฐ ์ด ์ ์ ๊ฐ๋ฆฌํค๋ ๋ ์ ํํ ํํ์ด๋ค.
๋ถํธ๋ง ๋ค์งํ๋ค๋ฉด ์๊ด์ ํฌ๊ธฐ๊ฐ ํฌ๊ณ ์์(์: โ0.8)๋ก ๋์จ๋ค. 0.07์ ๋ฌด์๊ด์ด๊ณ , ์ด๋ ๋น๊ต ๋์์ด u = MLqฬL + hL โ ฮL์ธ๋ฐ ฮ(๋ง์ฐฐยทํ ์ ์ ํญ)๋ฅผ ๋บ ์ฑ ์ค์ธก ์ค๋ฆฐ๋ ํ๊ณผ ๊ฒฌ์คฌ๊ธฐ ๋๋ฌธ์ผ ๊ฐ๋ฅ์ฑ์ด ํฌ๋ค. ๋
ผ๋ฌธ์ด ๋ชจ๋ฉํ
๊ธฐ๋ฐ ์ธ๋ ์ถ์ ๊ธฐ(์ 11)๋ฅผ ๋ฐ๋ก ๋ ์ด์ ๊ฐ ๊ทธ๊ฒ์ด๋ค. ์ฆ ๋ฌด๊ฒยท๊ด์ฑ ์์น ๋ฏธ๋ณด์ ์ ์์ฌํ๊ธฐ ์ ์ ๋น๊ต ์กฐ๊ฑด ์์ฒด๋ฅผ ๋จผ์ ํ์ธํด์ผ ํ๋ค (ยง6-โข).
3.3 Global Planning
๊ตด์ฐฉ ์์ ์ 3๊ฐ phase๋ก ๊ตฌ์ฑ:
์ด๊ธฐ ์์น โ ์งํ๋ฉด ์ง์
Bernstein ํ๋ผ๋ฏธํฐํ
ํ ์ ๊ตด์ฐฉ
Waypoint ์ต์ ํ
ํ ์ ์ด๋ฐ
Bernstein ํ๋ผ๋ฏธํฐํ
Phase 2: ๊ตด์ฐฉ ๊ถค์ ์ต์ ํ
๋ฒํท ์ธกํ์ด ํ์ ๋ฐ์ด๋ด๋ ๊ฒ์ ๋ง๊ธฐ ์ํด, Phase 2์์๋ ์บ๋น์ ์์ง์ด์ง ์๋๋ก ๊ตฌ์ํ ์ฑ ๊ถค์ ์ ์ต์ ํํ๋ค(ยงIII-A). ์ฆ ์ด ์ ์ ์ต์ ํ๋ ๋ถยท์ยท๋ฒํท์ ํ๋ฉด ์ด๋๋ง ๋ค๋ฃฌ๋ค.
n2 + 1๊ฐ waypoint L0, L1, โฆ, Lnโ๋ก ์ด์ฐํํ๊ณ , ๋น์ฉํจ์๋ฅผ ๋ค์๊ณผ ๊ฐ์ด ์ ์:
JPhase2 = wd ฮฃk=1nโ โLk โ Lkโ1โW2 โ wvVโ(4)| ํญ | ์๋ฏธ |
|---|---|
| wd ฮฃ โLk โ Lkโ1โW2 | ์ ์ ์ค๋ฆฐ๋ ์ด๋ ๊ฑฐ๋ฆฌ ์ต์ํ. ๊ตด์ฐฉ ์ค์๋ ์ธ๋์ด ํฌ๊ณ ์์ธก์ด ์ด๋ ค์ ์ ํต์ ์ธ ํ/ํ ํฌ ์ต์ํ๊ฐ ๋ถ์ ํฉํ๋ฏ๋ก, โํ์ ๋ ๋ฒํท ์ด๋์ ์ ํญํ๋คโ๋ ์ฑ์ง์ ์ด์ฉํด ์ด๋ ๊ฑฐ๋ฆฌ๋ฅผ ๋์ ์ด๋ค (ยงIII-A) |
| wvV | ๊ตด์ฐฉ ์ฒด์ ์ต๋ํ |
์งํ๋ฉด๊ณผ ๋ชฉํ ํ์์ ๋คํญ์์ผ๋ก ๋ชจ๋ธ๋ง:
zsurf = a0surf + a1surfx + ยทยทยท + ansurfsurfxnsurfโ(5)ztarget = a0target + a1targetx + ยทยทยท + antargettargetxntarget๊ตด์ฐฉ ์ฒด์ (swept volume, ์ฌ๋ค๋ฆฌ๊ผด ์ ๋ถ):
Vswept = โซxnโxโ zsurf(x)dx โ [(zโ + 2zโ + ยทยทยท + 2znโโ1 + znโ)/(2nโ)]ยท(xโ โ xnโ)โ(6)V = min{Vswept, Vcap(ฮธ(Lnโ))}
Phase 2 ์ ์ฝ์กฐ๊ฑด ์์ฝ:
| # | ์ ์ฝ์กฐ๊ฑด | ์ค๋ช |
|---|---|---|
| 1 | ์์น ์ ์ฝ | ๋ฒํท ํ์ด ๊ด์ฌ ์์ญ ๋ด, ๋ชฉํ ํ๋ฉด ์, ์งํ๋ฉด ์๋์ ์์น. ์์ยท์ข ๋ฃ ์์ ์๋ ์งํ๋ฉด ์์ ๋์ฌ์ผ ํจ (Phase 1ยท3๊ณผ์ ์ ํฉ ์กฐ๊ฑด) |
| 2 | ๋จ์กฐ ์ฆ๊ฐ ฮธ | ๋ฒํท ํ์ ๊ฐ์ด ๋จ์กฐ ์ฆ๊ฐํด์ผ ํจ (์๋์ง ํจ์จ) |
| 3 | ์์ ์ฌ์ ๊ฐ๋ ฮฑ > 0 | ๋ฒํท ํ ์ธ ๋ถ๋ถ์ด ์ง๋ฉด์ ๋ฐ์ง ์๋๋ก |
| 4 | ๋ฒํท ๋ชธ์ฒด ์์น | ๋ฒํท ์ ์ฒด๊ฐ ํ ๊ฒฝ๋ก ์์ ์์น |
| 5 | ฮธ < ฯ | ํ ์ ํ๋ฆผ ๋ฐฉ์ง (Fig. 4 ์ฐธ์กฐ) |
๋์: global_planner.py::_solve_phase2_with_theta โ ์(4)์ฒ๋ผ โ์ค๋ฆฐ๋ ์ด๋์ ์ ๊ฒ, ํผ๋ด๋ ํ์ ๋ง์ดโ๋ฅผ ์ต์ ํ ์๋ฒ(IPOPT)๋ก ํ๋๋ค.
๊ตฌํ ๋ฐฉ์: ๋ฒํท ๊ฐ๋ ฮธ๋ฅผ ๊ฒฐ์ ๋ณ์๋ก ๋ช ์์ ์ผ๋ก ๋๊ณ (๋ ผ๋ฌธ์ ์ค๋ฆฐ๋ ๋ณ์ LK๋ฅผ ํตํด ์๋ฌต์ ์ผ๋ก ๋ โ ์๋ ์ ์ ์ฐธ์กฐ), โ๋ ์ด ์งํ ๋ฐฉํฅ์ ์์์ผ๋ง ํ์ด ๋ด๊ธด๋คโ๋ ์กฐ๊ฑด์ ์ ์ฝ์ผ๋ก ๊ฒ๋๋ค. ๊ทธ ๊ฒฐ๊ณผ ์ด์ ์๋ค์ด ํ๋ โ๋ฒ๋ฆฐ ์ฑ ๋๋ค๊ฐ ๋ง์ง๋ง์ ์ค๋ฏ๋ฆฌ๊ธฐโ๊ฐ ๊ท์น์ ํ๋์ฝ๋ฉํ์ง ์์๋ ์ต์ ํ ๊ฒฐ๊ณผ๋ก ์ ์ ๋ก ๋์ต๋๋ค:
# ๋ ํ ๊ฐ๋ โ ์งํ๋ฐฉํฅ ๊ฐ๋ = phi. phi๊ฐ ์์์ผ ํ์ "๋ฒ๋ค"
phi = 0.5 * (th[index] + th[index + 1]) - tangent
constraints.append(phi) # ๋ ๋จผ์ ์กฐ๊ฑด: ๋ฑํ์ผ๋ก ํ ๋ฐ๊ธฐ ๊ธ์ง
lbg.append(theta_opts["rake_min"])
- ์(5) ์ง๋ฉด ์์ํ: ์ค์บํ ๋์ด ์ง๋์์ ์ง๋ฉด์ ๋คํญ์์ผ๋ก ์๋ ์์ฝ(
apex_grid_surface_fit.py). ๊ฒ์ฆ: ์งํ์ 8ยฐ ๊ฒฝ์ฌ๋ฅผ ์ฌ์ด ๋์ผ๋ 7.32ยฐ๋ก ์ฝ์ด๋. ๋ ผ๋ฌธ์ โ์ง๋ฉด ๋ชจ์์ ๋ฏธ๋ฆฌ ์๋ค๊ณ ๊ฐ์ โ ํ๊ณ๋ฅผ ์ค์ธก ์ค์บ์ผ๋ก ์ผ๋ถ ํด์ - ์(6) ํผ๋ธ ๋ถํผ: ๋ ผ๋ฌธ๊ณผ ๊ฐ์ ์ฌ๋ค๋ฆฌ๊ผด ์ ๋ถ. ๋จ โ๋ฒํท์ ๋ด๊ธธ ์ ์๋ ์ต๋๋โ Vcap(ฮธ) ํญ์ ๋ฏธ๊ตฌํ โ ์๋ฎฌ๋ ์ดํฐ ์ค์ธก ๋ถํผ๋ก ์ฌํ ํ์ธ(๐ก). ์ด ํญ์ด ๋ ผ๋ฌธ์์ ฮธ๊ฐ ๋ชฉ์ ํจ์๋ก ๋ค์ด๊ฐ๋ ์ ์ผํ ํต๋ก์ด๋ฏ๋ก, ๋ฏธ๊ตฌํ์ ๋จ์ ๋๋ฝ์ด ์๋๋ผ ๊ตด์ฐฉ ์ ๋ต์ ์ํฅ์ ์ฃผ๋ ์ถ์์
- ํ์ ๐ข ์ถฉ์ค (Vcap ํญ ๋ฏธ๊ตฌํ)
์ด์ ํ์๋ โ๋ ผ๋ฌธ์ ๊ฒฝ๋ก์ ๋ง ์ต์ ํํ๊ณ ๋ฒํท ๊ฐ๋๋ ๋ฐ๋ผ์ค๋ ๊ฐ์ธ๋ฐ, ์ฐ๋ฆฌ๋ ๋ฒํท ๊ฐ๋ ์์ฒด๋ฅผ ์ต์ ํ ๋ณ์๋ก ์น๊ฒฉํ๋คโ๊ณ ์ ํ ์์์ผ๋ ์ฌ์ค์ด ์๋๋ค. ๋ ผ๋ฌธ๋ ๋ฒํท ๊ฐ๋๋ฅผ ์ต์ ํํ๋ค. ๊ทผ๊ฑฐ ์ :
- ๊ฒฐ์ ๋ณ์๊ฐ ์ด๋ฏธ 3์ฐจ์์ด๋ค. ๋ ผ๋ฌธ ยงII์์ Phase 2 waypoint๋ Lk = [LB LA LK]T โ โ3๋ก, ๋ฒํท ์ค๋ฆฐ๋ LK๊ฐ ํฌํจ๋๋ค. ์ด๋ ํ ์์ธ E = [Ex Ez ฮธ]T์ ์ ๋จ์ฌ(bijective) ๋์์ด๋ผ ์ฌ์ ์์ ๋(null space)๊ฐ ์๋ค. ์ฆ Lk๋ฅผ ๊ณ ๋ฅด๋ ๊ฒ์ด ๊ณง (ํ ์์น + ๋ฒํท ๊ฐ๋)๋ฅผ ๋์์ ๊ณ ๋ฅด๋ ๊ฒ์ด๋ค. ์ ๋ง ์์น๋ง ์ต์ ํํ๋ค๋ฉด ๊ฒฐ์ ๋ณ์๋ 2์ฐจ์์ด์ด์ผ ํ๋ค.
- ฮธ๊ฐ ๋ชฉ์ ํจ์์ ์ง์ ๋ค์ด๊ฐ๋ค. V = min{Vswept, Vcap(ฮธ(Lnโ))}์ด๊ณ ๋ฒํท ์ฉ๋ Vcap์ ์ต์ข ๋ฒํท ๊ฐ๋ ฮธnโ์ ํจ์(Fig. 4)๋ค. ๋ฐ๋ผ์ โwvV๋ฅผ ์ต์ํํ๋ฉด ์๋ฒ๊ฐ ๋ง์ง๋ง waypoint์์ ๋ฒํท์ ๋ซ๋ ๋ฐฉํฅ์ผ๋ก ๋ฏผ๋ค. ์ฐ๋ฆฌ๊ฐ โ์ฐ๋ฆฌ ํ์ฅ ๋๋ถ์ ์ ์ ๋ก ๋์จ๋คโ๊ณ ์ ์๋ โ๋ง์ง๋ง์ ์ค๋ฏ๋ฆฌ๊ธฐโ๋ ์ด๋ฏธ ๋ ผ๋ฌธ ๋ชฉ์ ํจ์์์ ๋์จ๋ค.
- Phase 2 ์ ์ฝ 5๊ฐ ์ค 4๊ฐ๊ฐ ๋ฒํท ๊ฐ๋ ์ ์ฝ์ด๋ค. #2 ฮธ ๋จ์กฐ ์ฆ๊ฐ, #3 ฮฑ > 0, #4 ๋ชธ์ฒด โฅ ํ ๊ถค์ (์ผ๊ฐํ ์ถฉ๋๋ฐ์ค), #5 ฮธ < ฯ.
ํนํ ์ฐ๋ฆฌ ๊ณ ์ ๊ธฐ์ฌ๋ก ์ ์ด ๋์๋ phi >= rake_min์ ๋
ผ๋ฌธ ์ ์ฝ #3(ฮฑ > 0) ๊ทธ ์์ฒด๋ค. ๋
ผ๋ฌธ Fig. 5 ์บก์
์ ฮฑ๋ฅผ โ๋ฒํท ํ ์๋ ๋ฐฉํฅ vE = (ฤx, ฤz) ๊ธฐ์ค์ผ๋ก ์ธก์ โํ๋ค๊ณ ๋ช
์ํ๋ค โ ์ฐ๋ฆฌ phi = ๋ ํ ๊ฐ๋ โ tangent์ ๊ฐ์ ๋ฌผ๋ฆฌ๋์ด๊ณ , ์ฐจ์ด๋ ํํ์ด 0์ด๋ rake_min์ด๋๋ ๋ง์ง๋ฟ์ด๋ค.
๋ํ ์ด์ ์์ ์ โํฌํ ํจ์จ์ ๋ชฉ์ ํจ์์ ๋ฃ์๋คโ๊ณ ํ์ผ๋, ๋ฐ๋ก ์ ์ฝ๋๋ constraints / lbg, ์ฆ ์ ์ฝ์กฐ๊ฑด์ด๋ค. ์๊ธฐ ์ฆ๊ฑฐ๊ฐ ์๊ธฐ ์ค๋ช
๊ณผ ์ด๊ธ๋ ์์๋ค.
์์ธ ์ถ์ : ํจ์๋ช
_solve_phase2_with_theta๊ฐ ์์ฌํ๋ฏ APEX ์์ฒด ์ด๊ธฐ ๋ฒ์ ์ด ์์น๋ง ํ๋ค๊ฐ ฮธ๋ฅผ ๋์ค์ ์ถ๊ฐํ ๊ฒ์ผ๋ก ๋ณด์ธ๋ค. โ์ฐ๋ฆฌ ์ด์ ๋ฒ์ ๋๋น ํ์ฅโ์ด โ๋
ผ๋ฌธ ๋๋น ํ์ฅโ์ผ๋ก ์๋ชป ์ฎ๊ฒจ ์ ํ ๊ฒ์ผ๋ก ํ๋จ๋๋ค.
์ค์ ์ฐจ์ด๋ก ๋จ๋ ๊ฒ (๋ชจ๋ โ์ถฉ์คํ ์ฌํ + ์ค๋ฌด ์กฐ์ โ ์์ค์ด๋ฉฐ ํ์ฅ์ด๋ผ ๋ถ๋ฅผ ๋งํ ๊ฒ์ ์์):
- ํ๋ผ๋ฏธํฐํ: ๋ ผ๋ฌธ์ ์ค๋ฆฐ๋ ๊ณต๊ฐ LK, ์ฐ๋ฆฌ๋ ๊ด์ ๊ฐ๋ ฮธ + ์ค์ธก 40์นธ ๋ณด๊ฐํ โ ์ ๋จ์ฌ ๋งคํ์ด๋ผ ์ํ์ ์ผ๋ก ๋ฑ๊ฐ, ์์น ์กฐ๊ฑด์ยท๊ตฌํ ํธ์ ์ฐจ์ด
- ๋ง์ง: ฮฑ > 0 โ
phi >= rake_min - ์๋ฒ: ALGLIB โ IPOPT
- ํ๊ท: Vcap(ฮธ) ํญ ๋ฏธ๊ตฌํ (์ ์(6) ํญ๋ชฉ)
Phases 1 & 3: Bernstein ๋คํญ์ ํ๋ผ๋ฏธํฐํ
Bernstein ๋คํญ์:
p(s) = ฮฃk=0n ฮฒk C(n,k)sk(1โs)nโk,โs โ [0,1]โ(7)Bernstein ๊ธฐ์ ๋ฅผ ์ฐ๋ ์ด์ (ยงIII-B): ๊ธฐ์ ์ convex hull ์ฑ์ง ๋๋ถ์ ์ ์ด์ ฮฒk์๋ง ๋ฐ์ค ์ ์ฝ์ ๊ฑธ์ด๋ ๊ถค์ ์ ์ฒด๊ฐ qL ๋ฒ์๋ฅผ ์งํค๊ณ , qฬL์ ์ ํ ๋ถ๋ฑ์ ์ ์ฝ๋ ์ฝ๊ฒ ๋ฐ์๋๋ค. ์๊ฐ ๋ฏธ๋ถ ๊ณ์ฐ๋ ๊ฐ๋จํด ์ฐ์ฐ์ด ๋นจ๋ผ์ง๋ค. ์๋ ์(8)์ ์ ์ฝ์ด ์ ์ด์ ๋ง์ผ๋ก ํํ๋๋ ๊ทผ๊ฑฐ๊ฐ ๋ฐ๋ก ์ด ์ฑ์ง์ด๋ค.
Phase ๊ฐ ์ฐ์์ฑ (ยงIII): ์ ์ด์ ์ฐ์์ฑ์ ๋ณด์ฅํ๊ธฐ ์ํด, ์ธ์ ํ phase ์ฌ์ด์์ configuration๊ณผ ๊ทธ ๋ฏธ๋ถ์ด ์ผ์นํด์ผ ํ๋ค.
Phases 1, 3์ ์ต์ ํ ๋ฌธ์ :
minฮฒk, T โซ0T ยฝuTWuu dtโ(8)s.t.โqLl โค ฮฒk โค qLu,โqฬLl โค n(ฮฒk+1โฮฒk)/T โค qฬLuPhase 3 ์ถ๊ฐ ์ ์ฝ (ํ ์ ํ๋ฆผ ๋ฐฉ์ง):
ฮธ0 โค ฮธ(ฮฒk) โค ฯโ(9)๋์: ์(7)์ Bernstein ๊ณก์ (์ ์ด์ ๋ช ๊ฐ๋ก ๋ถ๋๋ฌ์ด ๊ณก์ ์ ๋ง๋๋ ๋ฐฉ๋ฒ)์ _bernstein ํจ์์ ๊ทธ๋๋ก ์์ต๋๋ค:
def _bernstein(control_points, samples):
for i in range(degree + 1):
coeff = comb(degree, i) * (t**i) * ((1.0 - t)**(degree - i))
curve += coeff[:, None] * points[i]
๋จ์ํํ ๋ถ๋ถ (๐ก): ๋ ผ๋ฌธ์ ์(8)๋ก โํ์ด ๊ฐ์ฅ ์ ๊ฒ ๋๋ ์๊ฐ ๋ฐฐ๋ถโ๊น์ง ์ต์ ํํ์ง๋ง, ์ฐ๋ฆฌ๋ ๊ณก์ ๋ชจ์์ ๊ณ ์ ํ๊ณ ์๋ ์ํ(๊ตฌ๊ฐ๋ณ ํ ์๋ 0.5/0.25/0.5 m/s)์ ๋ง์ถฐ ์๊ฐ๋ง ์ฌ๋ฐฐ๋ถํฉ๋๋ค.
์(9) ํ๋ฆผ ๋ฐฉ์ง (๐ข, ๋ฐฉ์๋ง ๋ค๋ฆ): ๋ ผ๋ฌธ์ ๋ถ๋ฑ์ ์ ์ฝ, ์ฐ๋ฆฌ๋ ๋ค์ด์ฌ๋ฆฌ๋ ๋์ ์ค๋ฏ๋ฆผ ๊ฐ๋๋ฅผ ์ง์ ์ ์ง์ํค๊ณ + ์ด๋ฐ ๋์ด๋ฅผ ๊ฐ์ ์กฐ๊ฑด์ผ๋ก ๊ฒ๋๋ค.
์ด์ ์(์ฌ์ฉ์) ๊ด์ฐฐ๋ก ๊ณ ์น ๊ฒ 2๊ฑด: โ ๋ด๋ ค๊ฐ ๋ ๋ฒํท์ ํ์ง ๋ฒ๋ฆฐ ์์ธ๋ก(์ค๋ฏ๋ฆฐ ์ฑ ๋ด๋ ค๊ฐ๋ฉด ๋ฑํ์ด ๋จผ์ ๋ ์ ๋ฟ๋ ๋น์ ์ ๋์) โก ์์ ๋ ๊ฐ๋ 1.30rad๋ฅผ ๋ช ๋ นํด๋ ์ค์ ๋ก๋ 8%๋ฐ์ ์ ์ด๋ฆฌ๋ ๊ฒฐํจ(์ 3.1์ ๋น๋น๋ก ๊ด๊ณ ํ) โ ์ค์ธก ๊ฐ๋๋ฒ์์ 92%๋ฅผ ์ง์ ๋ชฉํ๋ก ๋ฐ๊พธ๊ณ 2์ด๊ฐ ์ ์ง.
๐ ์๊ฐ ๋ฐฐ๋ถ ๋จ์ํ ์ฌ๊ฒํ (2026-08-14) โ โ๋ฌดํดํจโ์ด ์์ง ์ฆ๋ช ๋์ง ์์๋ค
์(8)์ minฮฒk,T๋ก T๊ฐ ๊ฒฐ์ ๋ณ์๋ค. ์๋ฌธ๋ ๋ชป๋ฐ๋๋ค โ โwhere ฮฒk is the k-th control point ... and T is the time cost for complete traversalโ. T๊ฐ ๋น์ฉ์ ์ด๋ป๊ฒ ๋ค์ด๊ฐ๋์ง ํ๋ฉด ๊ตฌ์กฐ๊ฐ ๋๋ฌ๋๋ค.
s = t/T ๋ก ๋๋ฉด ๊ณก์ ๋ชจ์ ฮฒ ๋ T ์ ๋ฌด๊ดํ๊ณ
qฬ_L = (1/T)ยทqโฒ(s) qฬ_L = (1/Tยฒ)ยทqโณ(s)
u = M_L qฬ_L + h_L = (1/Tยฒ)ยทA(s) + G(s)
โ๊ด์ฑยท์ฝ๋ฆฌ์ฌ๋ฆฌโ โ์ค๋ ฅโ
(T์ ๋ฐ๋น๋ก) (T์ ๋ฌด๊ด โ ํต์ฌ)
J(T) = โซโแต ยฝuแตW_u u dt = a/Tยณ + b/T + cยทT
a=ยฝโซAแตW_uA ds, b=โซAแตW_uG ds, c=ยฝโซGแตW_uG ds
dJ/dT = 0 โ cยทTโด โ bยทTยฒ โ 3a = 0
T* = โ( [ b + โ(bยฒ + 12ac) ] / (2c) )
๋ถ์ ๋ค๊ณ ์๋ ํ ๊ฐ๋งํ ์์ด๋ ํ์ด ๋ ๋ค(G๋ T์ ์ ์ค์ด๋ ๋ค). ๊ทธ๋์ ๋นจ๋ฆฌ ๊ฐ๋ฉด ๊ด์ฑ์ด a/Tยณ๋ก ํญ์ฆํ๊ณ , ๋๋ฆฌ๊ฒ ๊ฐ๋ฉด ์ค๋ ฅ์ cยทT๋งํผ ๋ ์ค๋ ์ง๋ถํ๋ค. ๋
ผ๋ฌธ์ T ์ต์ ํ๋ ์ด ๊ท ํ์ ์ ์ฐพ๋ ๊ฒ์ด๊ณ , ์ฐ๋ฆฌ๊ฐ ๋ฒ๋ฆฐ ๊ฒ์ด ๊ทธ ๊ท ํ์ด๋ค.
๋ค๋ง ๋ฌดํดํ ๊ฐ๋ฅ์ฑ์ด ์ค์ฌํ๋ค: ๊ด์ฑ์ด ์ค๋ ฅ์ ๋นํด ์์ ์ ์ ์์ญ(aโ0)์ด๋ฉด T*๊ฐ ์๋ ์ํ ์ฝ๋๋ก ๋ถ์ผ๋ฏ๋ก, ๋
ผ๋ฌธ์ ์ต์ ํ๊ธฐ๋ ์ฐ๋ฆฌ์ ๊ฐ์ ๋ต(= ํ์ฉ ๋ฒ์ ๋ด ์ต๋ ์๋)์ ๋ธ๋ค. E85์ 0.25~0.5 m/s๋ ๊ทธ ์์ญ์ผ ๊ฐ์ฐ์ฑ์ด ์๋ค. ์ถ์ธก์ผ๋ก ๋จ๊ธฐ์ง ๋ง๊ณ ๊ณ์ฐ์ผ๋ก ํ์ ํ ๊ฒ โ ยง6.2
์ถ๊ฐ๋ก ํ์ธ๋ ๊ฒฉ์ฐจ 3๊ฑด (๋น์ฉ ์ต์ ์ฑ๊ณผ ๋ณ๊ฐ๋ก ์กด์ฌ):
| ๊ฒฉ์ฐจ | ๋ด์ฉ | ์ฐ๊ฒฐ๋ ๊ธฐ์กด ๊ธฐ๋ก |
|---|---|---|
| ์ ์ฝ ๊ณต๊ฐ ๋ถ์ผ์น | ๋
ผ๋ฌธ์ ์๋ ์ํ์ qฬ_L(์ค๋ฆฐ๋ ๊ณต๊ฐ)์ ๊ฑธ์ง๋ง ์ฐ๋ฆฌ๋ ํ ์๋(Cartesian)์ ๊ฑด๋ค. ์ผ์ฝ๋น์ ํ์ฐ๋น๊ฐ ์์ธ๋ง๋ค ๋ฌ๋ผ, ์ด๋ค ์์ธ์์๋ ์ค๋ฆฐ๋ ์๋ยท์ ๋ ์ํ์ ์กฐ์ฉํ ๋๊ณ ์ด๋ค ์์ธ์์๋ ๊ณผ๋ณด์์ ์ด๋ค |
ยง3.4 (10d) pump_flow = ฮฃ abs(qฬ_L), โํ์ ํ ์ด๊ณผ ๊ฒฝ๊ณ ~24% ์์กดโ |
| phase ๊ฒฝ๊ณ ์๋ ๋ถ์ฐ์ | 0.5/0.25/0.5 ๊ตฌ๊ฐ๋ณ ์์ ์๋๋ ๊ฒฝ๊ณ์์ ์๋๊ฐ ๊ณ๋จ์ผ๋ก ์ ํํ๋ค โ ๊ฐ์๋ ์ํ์ค. ์๋ฌธ์ โThe boundary values ... are chosen such that the transition between any two consecutive phases is smoothโ๋ก ์ด๋ฅผ ๋ช ์์ ์ผ๋ก ๋ง๋๋ค(๋ณธ ์ ์ โPhase ๊ฐ ์ฐ์์ฑโ ํญ๋ชฉ) | ยง5 ํ๊ณ โค ๋์ปน๊ฑฐ๋ฆผ(์ ํฌ) โ โฃ MPC horizon๊ณผ ๊ฒฝ์ ๊ฐ์ค |
| ์ ์ญ ๊ณํ์ ํ ์ธ์ ์์ค | ์(8)์ ๋ชฉ์ ํจ์๋ Wu ๊ฐ์ค ํ ๋น์ฉ์ด๋ฏ๋ก ๋ ผ๋ฌธ์ ์ ์ญ ๊ณํ๊ธฐ๋ ์ด๋ ์ก์ถ์์ดํฐ๊ฐ ํ๊ณ์ ๊ฐ๊น์ด์ง ์๋ค. ํ ์๋ ์ํ์ ์์ ๊ธฐํ๋ผ ๋ชจ๋ฅธ๋ค โ ํ ์ฑ ์ 100%๊ฐ ๊ตญ์ MPC๋ก ์ด์ | ยง3.4 24% ๊ฒฝ๊ณ , ยง3.2 ฮฬ ํ๊ณ 80% ์์ ํฌํ |
ํ์ : ๐ก ์ ์ง. ๋จ ์ฌ์ ๋ฅผ โ๋จ์ํํ์โ์์ โ๋จ์ํ์ ํ๋น์ฑ์ด ๋ฏธ๊ฒ์ฆโ์ผ๋ก ๊ต์ฒดํ๋ค. ยง6.2์์ ๊ฒฐ๋ก ์ด ๋๋ฉด ๐ข ์น๊ฒฉ ๋๋ ์์ ์์ ์ผ๋ก ํ์ .
3.4 Local Planning (MPC)
์๋ ์ธ ์ฝ์์(์ 10a-d / ์ 11 / ์ 12~14)์ด ํญ๋ชฉ๋ณ ์์ธ๋ผ๋ฉด, ์ด ์ ์ local planning ์ ์ฒด๋ฅผ ํ ์ฅ์ผ๋ก ๋ณธ ๊ฒ์ ๋๋ค. โ๋ฌด์์ด ๋ค๋ฅธ๊ฐโ์ โ๊ทธ ๋จ์ํ๊ฐ ์ค์ ๋ก ๋ฌธ์ ์ธ๊ฐโ๋ฅผ ๋ถ๋ฆฌํด ํ์ ํ์ต๋๋ค.
์ฐจ์ด 7์ถ
| # | ์ถ | ๋ ผ๋ฌธ | APEX | ์ํ๋ |
|---|---|---|---|---|
| โ | ์์ธก ์งํ N | 50์คํ (1000ms) ์ ๋ ฅ ์ด ์ ์ฒด | 1์คํ โ ํ์ฌ ๊ฐ์๋ 1๊ฐ | ๐ด |
| โก | ์ ์ด ์ฃผ๊ธฐ | 20ms (์ค์ธก 33~46Hz) | 50ms (20Hz), ์ฐ์ฐ p95 7ms | ๐ก |
| โข | ์ธ๋ ์ถ์ ๊ตฌ์กฐ | ๋ชจ๋ฉํ ๊ธฐ๋ฐ ์ ๋ถํ | ๊ฐ์๋ ์์ฐจ + IIR | ๐ด |
| โฃ | ์ก์ถ์์ดํฐ ์ธํฐํ์ด์ค | ํ ์ธ๊ฐ | ์๋ ๋ช ๋ น (ํโ์๋ ํ์ฐ) | ๐ก |
| โค | ํ์ ์ ์ฝ (10b) | 4์ถ ์ดํฉ ํ๋ | ์ถ๋ณ ๊ฐ๋ณ | ๐ด |
| โฅ | ์ ๋ ์ ์ฝ (10d) | ํํ 2๋ ๋ถ๋ฆฌ | 1๋ฉ์ด๋ฆฌ ํฉ์ฐ | ๐ข |
| โฆ | ์๋ฒ | DDP + Augmented Lagrangian | IPOPT | ๐ข |
์ ํจ ์๊ฒฌ ๊ฑฐ๋ฆฌ๋ก ํ์ฐํ๋ฉด ๊ฒฉ์ฐจ๊ฐ ๋ ์ ๋ช ํฉ๋๋ค.
๋
ผ๋ฌธ: 1000 ms ์์ ๋ด๋ค๋ด
APEX: 50 ms ์์ ๋ด๋ค๋ด โ 20๋ฐฐ ์ฐจ์ด
โค ํ์ ์ ์ฝ โ ํ ์กฐ๊ฑด์์ ์ด๋ฏธ ๊ฒฐํจ (์ต์ฐ์ )
๋
ผ๋ฌธ: T_Uยทฯฬ + F_BยทLฬ_B + F_AยทLฬ_A + F_KยทLฬ_K โค p^u (ํฉ ํ๋)
APEX: |F_i ยท qฬ_i| โค p_i^u ๊ฐ ์ถ๋ง๋ค (๊ฐ๋ณ 4๊ฐ)
(10b)๋ โ๋ชจ๋ ์ก์ถ์์ดํฐ๊ฐ ํ๋์ ์์งยทํํ๋ฅผ ๊ณต์ ํ๋คโ๋ ์ ์ ๊ตด์ฐฉ๊ธฐ์ ํต์ฌ ๋ฌผ๋ฆฌ๋ฅผ ํํํ๋ ์ ์ฝ์ด๋ค. ์ถ๋ณ๋ก ์ชผ๊ฐ๋ฉด ๊ทธ ๋ฌผ๋ฆฌ๊ฐ ์ฌ๋ผ์ง๋ค.
| ์ํฉ | ๊ฒฐ๊ณผ |
|---|---|
| 4์ถ์ด ๋์์ ๊ฐ์ ์ํ์ 90% ์ฌ์ฉ | ์ถ๋ณ๋ก๋ ์ ๋ถ ํต๊ณผ, ์ดํฉ์ 3.6๋ฐฐ ์ด๊ณผ โ ์ค๊ธฐ๋ผ๋ฉด ์์ง ์คํจยทํํ ์๋ ฅ ๊ฐํ |
| ๋ถ๋ง ํฌ๊ฒ ์ฐ๊ณ ๋๋จธ์ง ์ ์ง | ๋ ผ๋ฌธ์ ์ด ์์ฐ์ ๋ชฐ์์ค ์ ์์ผ๋ APEX๋ ์ถ๋ณ ์ํ์ ๋งํ โ ๋ถํ์ํ๊ฒ ๋ณด์์ |
์ฆ ๊ฐ์ฅ ์ํํ ๋ ๋์จํ๊ณ , ์ฌ์ ์์ ๋ ๋นก๋นกํ ์ ํํ ๋ฐ๋ ๋ฐฉํฅ์ ์ค๋ฅ๋ค. ์ ์ฝ์ ํ ์ค์ ํฉ์ผ๋ก ๋ฐ๊พธ๋ฉด ํด๊ฒฐ๋๋ฏ๋ก ๋น์ฉ ๋๋น ํจ๊ณผ๊ฐ ๊ฐ์ฅ ํฌ๋ค.
โ N=1 โ ์๋ฐํ๋ MPC๊ฐ ์๋
N=1์ด๋ฉด receding horizon์ ์์ธก ์ฑ๋ถ์ด ์ฌ๋ผ์ง๊ณ , ์ค์ง์ ์ผ๋ก ์ ์ฝ์ด ๋ถ์ 1์คํ QP = ์ ์ฝ ์๋ PD ์ ์ด๊ธฐ๊ฐ ๋๋ค. ๋ ผ๋ฌธ ๊ธฐ์ฌ โข์์ โ๊ถค์ ์์ฑโ์ด ๋น ์ง๊ณ โ์๊ฐ ์ ์ฝ ๋ง์กฑโ๋ง ๋จ๋๋ค.
(a) ์ ์ฝ ๊ทผ์์ โ MPC์ ์กด์ฌ ์ด์ ๋ โ์ง๊ธ์ ๊ด์ฐฎ์ง๋ง ๋ช ์คํ ๋ค์ ๊ฑธ๋ฆฐ๋คโ๋ฅผ ๋ฏธ๋ฆฌ ๋ณด๊ณ ์ง๊ธ๋ถํฐ ๊ฐ์ํ๋ ๊ฒ์ธ๋ฐ, N=1์ โ์ง๊ธ ๋๋?โ๋ง ๋ณธ๋ค.
์ค๋ฆฐ๋ ๋ณ์ (10c) ์์:
N=50 โ ์คํธ๋กํฌ ๋ ์ ๊ทผ์ ๋ฏธ๋ฆฌ ๋ณด๊ณ ์์ํ ๊ฐ์
N=1 โ ๋์ ๋ฟ๋ ์๊ฐ๊น์ง ์ ์ โ ๊ธ์ ์ง / ํ๋์คํฑ ์ถฉ๋
๋ ๊ทผ๋ณธ์ ์ผ๋ก recursive feasibility๊ฐ ์๋ฆฌ์ ์ผ๋ก ๋ณด์ฅ๋์ง ์๋๋ค โ ์ง๊ธ ์คํ ์ ํ๋ ค๋ ๊ทธ ํด๊ฐ ๋ค์ ์คํ ์ infeasible๋ก ๋ชฐ ์ ์๋ค. ์๋ ์(12)(13)(14) ์ฝ์์์ ๊ธฐ๋ก๋ โ์ ์ด ์ค 14~20% ์คํ ์์ ๋ฐฑ์ ์ ์ด ์ ํโ์ด ์ด ์ฆ์์ผ ๊ฐ๋ฅ์ฑ์ด ๋๋ค(๋ฐฑ์ ์ ํ = ์ต์ ํ ์คํจ = infeasible ๋ฐ์).
(b) ๋์ปน๊ฑฐ๋ฆผ โ โํ๋ณดโ๊ฐ ์๋๋ผ ๊ฑฐ์ ํ์ โ ๊ทผ๊ฑฐ๋ ๋ชฉ์ ํจ์์ ์๋ค.
ํ์ฌ ๋ชฉ์ ํจ์: 120*(์์น์ค์ฐจยฒ) + 18*(์๋์ค์ฐจยฒ)
โ ์ ํฌยท์
๋ ฅ ๋ณํ๋ฅผ ์ต์ ํ๋ ํญ์ด ์์ ์์
๋
ผ๋ฌธ ๋ชฉ์ ํจ์: ฮฃ_k ( ||x_k โ x_r,k||ยฒ_Qฬ + ||v_k||ยฒ_Rฬ )
โ ์
๋ ฅ ์ด ์ ์ฒด๋ฅผ ํ๋๋ก ๋ฌถ์ด ์ต์ ํ โ ์์ฐ์ค๋ฝ๊ฒ ๋งค๋๋ฌ์
N=1์ ๋งค ์คํ ๋ ๋ฆฝ์ ์ผ๋ก v๋ฅผ ๊ณ ๋ฅด๋ฏ๋ก vk์ vk+1 ์ฌ์ด ์ฐ์์ฑ ์ ์ฝ์ด ์๋ค. ๊ฐ์๋๊ฐ ๊ณ๋จ ํจ์ โ ์ ํฌ๊ฐ ์๋ฆฌ์ ์ผ๋ก ๋ฌดํ๋.
(c) ์ญ์ค์ ์ผ๋ก ์ ๋ฆฌํ ์ 1๊ฐ โ ๋ ผ๋ฌธ์ ์ ์ผํ ๋ช ์์ ๊ฐ์ ฮฬk+1 = ฮฬk๋ N=50์ด๋ฉด 1์ด๊ฐ ํ ์ ๋ฐ๋ ฅ์ด ์ผ์ ํ๋ค๋ ๊ฐํ ๊ฐ์ ์ด๋ค. N=1์ด๋ฉด 50ms๋ง ํ์ํ๋ฏ๋ก ์คํ๋ ค ๋ ์์ ํ๋ค. ๋ค์ง์ผ๋ฉด N์ ๋๋ฆด ๋ ์ด ๊ฐ์ ์ ๋ถ๋ด๋ ๊ฐ์ด ์ปค์ง๋ฏ๋ก, N ํ๋์ ์ธ๋ ์์ธก ๋ชจ๋ธ ๊ฐ์ ์ ํจ๊ป ๊ฒํ ํด์ผ ํ๋ค.
โก ์ ์ด ์ฃผ๊ธฐ โ ์ฌ๊ธฐ์ ๊ธฐํ๊ฐ ์์
๋
ผ๋ฌธ: ์ฃผ๊ธฐ 20ms์ธ๋ฐ ์ฐ์ฐ์ด 21.7~30.2ms โ ์ฃผ๊ธฐ๋ฅผ ๋ชป ์ง์ผ 33~46Hz๋ก ๋ฐ๋ฆผ (๋น ๋ฏ)
APEX: ์ฃผ๊ธฐ 50ms์ธ๋ฐ ์ฐ์ฐ์ด p95 7ms โ ์์ฐ์ 14%๋ง ์ฌ์ฉ (์ฌ์ 86%)
์ฐ์ฐ ์์ฐ์ด 86% ๋จ์ ์ฑ๋ก N=1์ ์ฐ๊ณ ์๋ค. N์ ๋๋ฆด ์ฌ๋ ฅ์ด ์ด๋ฏธ ํ๋ณด๋์ด ์๋ค๋ ๋ป์ด๋ฉฐ, โ ์ ๊ทผ์์ ๋ฌธ์ ๋ฅผ ํธ๋ ๋ฐ ์ถ๊ฐ ํ๋์จ์ด๊ฐ ํ์ ์๋ค. ํฌ์ ๊ตฌ์กฐ ์ ์ง ์ N=10 ์ ๋๋ 50ms ์์ฐ ๋ด ์์ โ ์ ํํ ์์น๋ ์ค์ธก ํ์.
โข ์ธ๋ ์ถ์ ๊ตฌ์กฐ โ ๋ ผ๋ฌธ์ด ์๋์ ์ผ๋ก ํผํ ํจ์
์๋ณธ ๋ ธํธ Q23์ โ๋ชจ๋ฉํ ๊ด์ธก๊ธฐ๊ฐ ํํผํ 4๊ฐ์ง ํจ์ โ๊ณผ ๋์กฐํ๋ฉด ๋ช ํํ๋ค.
| ๋ ผ๋ฌธ ๋ชจ๋ฉํ ๊ด์ธก๊ธฐ | APEX ๊ฐ์๋ ์์ฐจ ๋ฐฉ์ |
|---|---|
| qฬ ๋ถํ์ (์ด๋๋ ์ ์๋ก ํก์) | M @ (์ค์ธก ๊ฐ์๋ โ predicted) โ qฬ ํ์ โ ๏ธ |
| ์ ๋ถ๋ง ์ฌ์ฉ โ ๋ ธ์ด์ฆ ํํ | ๊ฐ์๋ ์ฌ์ฉ โ ๋ ธ์ด์ฆ ์ฆํญ โ ๏ธ |
| ฮฬ = LPF(ฮ), ์๋ ดยท๋ฌด๋ฐ์ด์ด์ค ๋ณด์ฅ | 0.8/0.2 IIR โ ํด์์ ๋ณด์ฅ ์์ |
๋ ผ๋ฌธ์ด ๊ตณ์ด ์ด๋๋ ํ์์ ํํ ์ด์ ๊ฐ ์ ํํ qฬ๋ฅผ ํผํ๊ธฐ ์ํด์๋ค. ๋ค๋ง ์ค๋์์ ํ์ธ๋์๊ณ (๋๋ฉด 7.2cm ์์์ง), ๋ ธํธ๊ฐ ์ด๋ฏธ โpaper-inspired โ paper-equivalentโ๋ก ์ ์งํ๊ฒ ๋ผ๋ฒจ๋งํ๊ณ ์์ด ์ฒ๋ฆฌ ์์ฒด๋ ์ณ๋ค.
โฃ ์๋ ๋ช ๋ น โ ํ์ฐ์ ํ๋น, ๊ทธ๋ฌ๋ ์ ์ฝ์ด ๋ฌผ๋ฆฌ์ ์ผ๋ก ๊ฐ์ ๋์ง ์์
๋ชฉํ์๋ = ํ์ฌ์๋ + (v โ Mโปยนยทฮฬ)ยทdt๋ ๊ฐ์๋๋ฅผ ์ ๋ถํด ์๋๋ฅผ ๋ง๋๋ ๋
ผ๋ฆฌ์ ์ผ๋ก ํ๋นํ ํ์ฐ์ด๋ค. ๋ฌธ์ ๋ ๋ค๋ฅธ ๋ฐ ์๋ค.
ํ์ ๋ช ๋ นํ์ง ์์ผ๋ฏ๋ก, MPC๊ฐ โ์ด ๊ฐ์๋๋ฉด ํ์ด ํ๊ณ ์โ์ด๋ผ ํ๋จํด๋ ์ค์ ๋ชจํฐ๋ ์๋ ์ถ์ข ์ ํ์ํ ๋งํผ ํ ํฌ๋ฅผ ์ด๋ค.
โํ์ ํ ์ด๊ณผ ๊ฒฝ๊ณ ~24% ์์กดโ๊ณผ ์ง๊ฒฐ๋๋ค. ๋จ ยง2.1์์ ์ง์ ํ๋ฏ ๋ ผ๋ฌธ๋ ๋ฐธ๋ธ ๋์ญํ์ ์๋ตํ์ผ๋ฏ๋ก, โ๋ ผ๋ฌธ๋ณด๋ค ๋ชปํ๋คโ๊ธฐ๋ณด๋ค ๊ฐ์ ๋ฌธ์ ๋ฅผ ๋ค๋ฅธ ์ธต์์์ ๋ง๋ ๊ฒ์ด๋ผ๋ ๊ธฐ์กด ํด์์ ๊ณต์ ํ๋ค.
โฅโฆ ์ ๋ ํฉ์ฐ ยท ์๋ฒ โ ํ ๋จ๊ณ ๋ฌดํด
- ์ ๋: ์ค๊ธฐ๋ ๋ถ/์์ด ๋ค๋ฅธ ํํ์ ๋ฌผ๋ ค ์์ด ํ ํํ์ ๋ชฐ๋ฆฌ๋ฉด flow starvation์ด ๋๋ค. ํฉ์ฐ ์ ์ฝ์ ๋ชป ์ก์ง๋ง AGX์๋ ์ค์ ์ ์ ํ๋ก๊ฐ ์์ด ์ง๊ธ์ ๋ฌดํด. ์ค๊ธฐ ์ด์ ์ ํ์ ๋ถ๋ฆฌ ํญ๋ชฉ.
- ์๋ฒ: DDP๋ N์ด ํด ๋ O(N) ์ค์ผ์ผ๋ง ๋๋ฌธ์ ๊ณ ๋ฅธ ๊ฒ์ด๋ผ N=1์์๋ DDP๋ฅผ ์ธ ์ด์ ์์ฒด๊ฐ ์๋ค. IPOPT๊ฐ ์ ์ . ๋จ N์ ํฌ๊ฒ ๋๋ฆฌ๋ฉด ์ฌ๊ฒํ ํ์(acados, OSQP+SQP ๋ฑ).
์ข ํฉ ํ์
| ์์ | ํญ๋ชฉ | ํ์ | ์์ ๋์ด๋ |
|---|---|---|---|
| 1 | ํ์ ์ ์ฝ ์ถ๋ณ ๋ถ๋ฆฌ | ๐ด ๋ฌผ๋ฆฌ ์๋ฏธ ์์ค, ํ ์กฐ๊ฑด์์ ์ด๋ฏธ ๊ฒฐํจ | ๋ฎ์ (์ ์ฝ ํ ์ค) |
| 2 | N=1 recursive feasibility ์์ | ๐ด ๋ฐฑ์ ์ ํยท๋์ปน๊ฑฐ๋ฆผ์ ์ ๋ ฅ ์์ธ | ์ค (์ฐ์ฐ ์ฌ์ ์์) |
| 3 | ์ธ๋ ์ถ์ qฬ ์์กด | ๐ด ๋ณด์ฅ ์์ค, ๋จ ์ค๋์์ ํ์ธ๋จ | ์ค |
| 4 | ํ ์ ์ฝ ๋ฏธ๊ฐ์ | ๐ก ์๋ ์ธํฐํ์ด์ค์ ๊ตฌ์กฐ์ ๊ท๊ฒฐ | ๋์ (์ ์ ์๋ฎฌ ํ์) |
| 5 | ์ ๋ ํฉ์ฐ | ๐ข ์๋ฎฌ ๋ฌดํด, ์ค๊ธฐ ์ ๋ถ๋ฆฌ | ๋ฎ์ |
| 6 | ์๋ฒ | ๐ข ๋ฌดํด | โ |
| โ | ์ธ๋ ๋ถ๋ณ ๊ฐ์ ๋ถ๋ด | โ ์คํ๋ ค ์ ๋ฆฌ (1/20) | โ |
๊ฒฐ๋ก : 6๊ฐ ์ค 3๊ฐ๊ฐ ์ค์ ์ํ์ด๊ณ , ๊ทธ์ค โ ๋ฒ(ํ์ ์ ์ฝ)์ ์ง๊ธ ์กฐ๊ฑด์์๋ ์ด๋ฏธ ๊ฒฐํจ์ด๋ค. ๋๋จธ์ง 2๊ฐ(N=1, ์ธ๋ ์ถ์ )๋ ์ด๋ฏธ ๊ด์ธก๋ ์ฆ์(๋ฐฑ์ ์ ํ 14~20%, ๋์ปน๊ฑฐ๋ฆผ, ฮฬ ํฌํ)๊ณผ ์ธ๊ณผ๊ฐ ์ฐ๊ฒฐ๋๋ค.
ํ์ธ ์คํ 3๊ฐ
โ ํ์ ์ ์ฝ ๊ฒฐํจ ํ์
4์ถ ๋์ ๊ธ๊ฐ์ ๊ถค์ ์์ ์ถ๋ณ ํ์์ "ํฉ"์ ๋ก๊น
โ ๋
ผ๋ฌธ์ ์ดํฉ ์ํ ๋๋น ๋ช %? 100% ์ด๊ณผ ์ ๊ฒฐํจ ํ์
โก N=1์ด ๋ฐฑ์
์ ํ์ ์์ธ์ธ์ง ํ์
N = 1 โ 5 โ 10 ์ผ๋ก ๋๋ฆฌ๋ฉฐ ๋ฐฑ์
์ ํ์จ ์ธก์
โ ์ ํ์จ์ด ๋จ์ด์ง๋ฉด N=1์ด ์์ธ์์ด ํ์ ๋จ
โข ๋์ปน๊ฑฐ๋ฆผ ์ธ๊ณผ ํ์
๋ช
๋ น ๊ฐ์๋์ ์๊ฐ ์ฐจ๋ถ(์ ํฌ) ํ์คํ ๊ทธ๋จ
โ N ์ฆ๊ฐ์ ๋ฐ๋ผ ๊ฐ์ํ๋์ง ํ์ธ
โก๋ ์ฐ์ฐ ์์ฐ์ด 86% ๋จ์ ์์ด ์ถ๊ฐ ๋น์ฉ ์์ด ๋ฐ๋ก ์๋ ๊ฐ๋ฅํ๋ค.
๋ฌผ๋ฆฌ์ ์ ์ฝ์กฐ๊ฑด
ul โค u โค uuโ(10a)p = uTqฬL โค puโ(10b)Ll โค L โค Luโ(10c)fi = Ai(sgn(qฬL))TqฬLabs โค fiu,โi = 1,2โ(10d)| ์ ์ฝ | ๋ฌผ๋ฆฌ์ ์๋ฏธ |
|---|---|
| (10a) Force/torque limit | ๊ฐ ์ก์ถ์์ดํฐ์ ํ/ํ ํฌ ์ํํ |
| (10b) Power limit | ์ ์ ํํ์ ์ด ํ์ ์ํ (๋ชจ๋ ์ก์ถ์์ดํฐ๊ฐ ๋์ผ ํ์์ ๊ณต์ ) |
| (10c) Cylinder displacement | ์ค๋ฆฐ๋ ๋ณ์์ ๋ฌผ๋ฆฌ์ ์ํํ |
| (10d) Pump flow rate | 2๊ฐ ์ ์ ํํ์ ์ ๋ ์ ํ |
๋์: ๊ตญ์ ์ ์ด๊ฐ ๋งค๋ฒ ํธ๋ ์ต์ ํ ๋ฌธ์ ์ ๋
ผ๋ฌธ์ 4๊ฐ์ง ํ๊ณ๊ฐ ๊ทธ๋๋ก ๋ค์ด ์์ต๋๋ค(local_mpc.py):
approx_u = inertia*v + damping*qd + h0 # (10a) ์ด ์์ง์์ ํ์ํ ํ โค ํ๊ณ์ 90%
power = approx_u * qd # (10b) ํ์(ํร์๋) โค ํ๊ณ
q_L = neutral + scale*q # (10c) ์ค๋ฆฐ๋๊ฐ ๋ฌผ๋ฆฌ์ ์ด๋ ๋ฒ์ ์
pump_flow = ฮฃ|qฬ_L| # (10d) ์ ๋(์ค๋ฆฐ๋ ์๋ ํฉ) โค ํํ ์ฉ๋
๋ ผ๋ฌธ๊ณผ ๋ค๋ฅธ ์ : (10b) ํ์๋ฅผ ๋ ผ๋ฌธ์ ์ ์ฒด ํฉ์ผ๋ก ํ ๋ฒ์, ์ฐ๋ฆฌ๋ ์ถ๋ง๋ค ๋ฐ๋ก ์ ํ. (10d) ์ ๋๋ ๋ ผ๋ฌธ์ ํํ 2๋์ ๋๋ ๋ฐฐ๋ถ, ์ฐ๋ฆฌ๋ ํ ๋ฉ์ด๋ฆฌ๋ก ํฉ์ฐ(๐ก ๋จ์ํ).
ํ์ ์ ์ ์ง์ฑ: ํ๊ณ ์ด๊ณผ ์ฌ๋ถ๋ ์๋ผ๋ด๊ธฐ ์ ์ ์๋ ํ ๊ฐ์ผ๋ก ํ์ ํฉ๋๋ค. ๋จผ์ ์๋ผ๋๊ณ โํ๊ณ ์ ๋์๋คโ๋ผ๊ณ ํ๋ฉด ๋์์์ด๋ฏ๋ก.
โ์ด ์กฐ๊ฑด๋ค์ด ์ง์ง ์ผํ๋๊ฐโ ๊ฒ์ฆ: ์ ์ฝ์ ๋๊ณ ๋จ์ ์ถ์ข ๋ง ์์ผ๋ณด๋ฉด 858๋ฒ ์ค 645~657๋ฒ ํ๊ณ ์ด๊ณผ ๋ฐ์ โ ์ฆ ์ ์ฝ์ด 76% ๊ตฌ๊ฐ์์ ์ค์ ๋ก ๊ฐ์ ํ๊ณ ์์์ต๋๋ค. ๋จ, ์ ์ฝ์ ์ผ๋ โํ์ ํ์ด ํ๊ณ๋ฅผ ๋๋๋คโ๋ ๊ฒฝ๊ณ ํ์๋ ~24% ๋จ๋๋ฐ, ์ด๋ ์ 3.2์ ๋ฏธ๋ณด์ ํ ํ๊ณ ์์น ํ์ ๋๋ค(๊ธฐ๋ก๋ ํ๊ณ).
์ธ๋ ์ถ์ (Disturbance Estimation)
๋ชจ๋ฉํ ๊ธฐ๋ฐ ์ธ๋ ์ถ์ (momentum-based disturbance estimation):
ฮฬ = KE[p โ p(0) โ โซ0t(JฮธTu + CฮธTqฬฮธ โ Gฮธ + ฮฬ)dฯ],โp = Mฮธqฬฮธโ(11)Mฬฮธ = Cฮธ + CฮธT ์ฑ์ง์ ํ์ฉํ๋ฉด, ์ค์ ์ธ๋ ฮ์ ์ ์ญํต๊ณผ ํํฐ(low-pass filter)๋ก ์๋:
ฮฬฬ = KE(ฮ โ ฮฬ)์ค๋ฆฐ๋ ๋ณ์ ๊ธฐ๋ฐ ์ถ์ ๊ฐ: ฮฬL = JLTฮฬ = [ฮฬU ฮฬB ฮฬA ฮฬK]T
๋์: disturbance.py โ โ๋ชจ๋ธ๋๋ก๋ฉด ์ด๋ ๊ฒ ์์ง์์ด์ผ ํ๋๋ฐ ์ค์ ๋ ์ด๋งํผ ๋ชป ์์ง์๋ค โ ๊ทธ ์ฐจ์ด๊ฐ ํ์ ์ ํญโ์ด๋ผ๋ ๋
ผ๋ฌธ ์(11)์ ์์ด๋์ด๋ฅผ, ์ก์์ ๊ฑธ๋ฌ๋ด๋ ํํฐ์ ํจ๊ป ๊ตฌํ:
predicted = ๋ชจ๋ธ์ด ์์ธกํ ๊ฐ์๋(์ ํญ 0 ๊ฐ์ )
residual_torque = M @ (์ค์ธก ๊ฐ์๋ - predicted) # = ํ ์ ํญ ์ถ์
estimate = 0.8*์ง์ _์ถ์ + 0.2*residual_torque # ์ ์ญํต๊ณผ(์ก์ ์ ๊ฑฐ)
- ํจ๊ณผ ์ ์ฆ (ํต์ฌ ์ฑ๊ณผ): ์ด ์ถ์ ๊ธฐ๋ฅผ ๋๋ฉด ๊ฐ์ ๊ถค์ ์ธ๋ฐ 7.2cm ์๊ฒ ํ๊ณ ํ์ 45% ๋ ๋ด๊น๋๋ค(3ํ ๋ฐ๋ณต ์ค์๊ฐ, ์๋ก ๋ค๋ฅธ 3๊ฐ์ง ์ธก์ ์ด ์ผ์น). ์ฆ ํ์ ๋ซ๊ณ ๋ค์ด๊ฐ๋ ๋ฅ๋ ฅ์ด ์ด ๋ถํ์์ ๋์ต๋๋ค.
- ์ฃผ์ํ ์ (์ ์ง ๊ธฐ๋ก): โ ์ฐ๋ฆฌ ๋ชจํฐ๋ ์๋ ๋ช ๋ นํ์ด๋ผ, ์ถ์ ๊ฐ์๋ ํ ์ ํญ ์ธ์ โ๋ชจํฐ ๋ฐ์ ์ง์ฐโ๋ ์์ ๋๋ค. โก ์ถ์ ๊ฐ์ด ์์ ์ํ(ํ ํ๊ณ์ 80%)์ ๊ฑฐ์ ํญ์ ๋ถ์ด ์์ด, ํ์ด 4๋ฐฐ ๋จ๋จํด์ ธ๋ ์ถ์ ๊ฐ ์ซ์๋ ์ ๋ณํฉ๋๋ค โ ๋ฏธ๋ณด์ ํ ์์น ํ์ด๋ฉฐ ๊ฐ์ ํญ๋ชฉ(๐ก).
๋จผ์ ๋ฌดํ์ ๋ฐ๋์์ ๋ ๋ฐฉ์์ ์์ ํ ๋์ผํ๋ค. ์ฐ๋ฆฌ ์์ฐจ๋ฅผ ์ ๊ฐํ๋ฉด ๋ ผ๋ฌธ ์ธ๋๊ณผ ์ ํํ ์ผ์นํ๋ค:
๋
ผ๋ฌธ ๋์ญํ (2): M qฬ = โh + Jแตu + ฮ โน ฮ = M qฬ + h โ Jแตu
์ฐ๋ฆฌ: qฬ_pred = Mโปยน(โh + Jแตu) (ฮ=0 ๊ฐ์ )
residual = M(qฬ_meas โ qฬ_pred) = M qฬ_meas + h โ Jแตu = ฮ โ
์(11)๋ ๋ฏธ๋ถํ๋ฉด ฮฬฬ = KE(ฮ โ ฮฬ)๋ก ฮ์ 1์ฐจ ์ ์ญํต๊ณผ์ด๊ณ , ์ฐ๋ฆฌ 0.8ยท์ง์ + 0.2ยท์์ฐจ๋ 1์ฐจ ์ ์ญํต๊ณผ๋ค. ์ฐจ์ด๋ ์ ๋ถ โ์ด๋ป๊ฒ ์คํํ๋๋โ์ ์๋ค.
| ๋ ผ๋ฌธ ์(11) | ์ฐ๋ฆฌ disturbance.py | |
|---|---|---|
| ๊ณ์ด | ๋ชจ๋ฉํ ๊ธฐ๋ฐ ๊ด์ธก๊ธฐ | ์ญ๋์ญํ ์์ฐจ + IIR |
| qฬ ํ์ | ๋ถํ์ (p = Mqฬ, ์ ๋ถํ โ ๋ฏธ๋ถ ์์) | ํ์ (์ค์ธก ๊ฐ์๋) |
| ํํฐ | KE (์ฐ์์๊ฐ ๊ฒ์ธ, ๋ฃจํ๋ ์ดํธ ๋ฌด๊ด) | ฮฑ=0.8 (์คํ ๋น ๊ณ์, ๋ฃจํ๋ ์ดํธ ์์กด) |
| ๊ตฌ์กฐ | ฮฬ๊ฐ ์ ๋ถ ์์ ์๋ ํ๋ฃจํ | ๊ฐ๋ฃจํ ์์ฐจ์ ํํ |
| ํฌํ | ์์ | ํ ํ๊ณ 80%๋ก ํด๋ฆฝ |
โ qฬ ์๊ตฌ โ ๋ชจ๋ฉํ ๊ด์ธก๊ธฐ๋ฅผ ์ด ์ด์ ๋ฅผ ๋๋๋ฆฐ ๊ฒ. ๋ ผ๋ฌธ์ด p = Mqฬ + ์ ๋ถ ํํ๋ฅผ ํํ ์ ์ผํ ์ด์ ๊ฐ qฬ ํํผ๋ค(์ ์ ๊ฐ์ ๋ฏธ๋ถ์ด ํ ๋ฒ๋ ์๋ค). ๊ตด์ฐฉ๊ธฐ์๋ ๊ด์ ๊ฐ๊ฐ์๋ ์ผ์๊ฐ ์์ด ์ค๋ฆฐ๋ ๋ณ์๋ฅผ 2ํ ๋ฏธ๋ถํด์ผ ํ๊ณ , ์ก์ ์ฆํญ์ 1/ฮtยฒ์ ๋น๋กํ๋ค(ฮt=20ms โ 2500sโปยฒ). ์ฌ๊ธฐ์ ์ ํค๊ธ M์ ๊ณฑํ๋ฉด ์์ ์์ฐจ๊ฐ ๋งค์ฐ ์๋๋ฌ์์ง๋ค. IMU๋ฅผ ๋ฌ์๋ ์์ด๋ก๋ ฯ๋ฅผ ์ค ๋ฟ ฯฬ๋ฅผ ์ฃผ์ง ์์ ๋ฏธ๋ถ์ ์ฌ์ ํ ํ์ํ๋ค.
โ ๋จ, ์ง๊ธ์ ๋์ฒด๋ก ๋ฌดํดํ๋ค. AGX๊ฐ ๊ฐ์๋๋ฅผ ๊นจ๋ํ๊ฒ ์ง์ ์ฃผ๊ธฐ ๋๋ฌธ์ด๋ค. ์ฆ ์ด๊ฑด โ์ง๊ธ ์ฑ๋ฅ์ ๋ง๊ฐ๋จ๋ฆฌ๋ ๊ฒฐํจโ์ด ์๋๋ผ ์ค๊ธฐ ์ด์์์ ํ์คํ ๋งํ๋ sim-only ์ง๋ฆ๊ธธ์ด๋ฉฐ, ์ฐ์ ์์๋ ์ค๊ธฐ ์ด์ ๊ณํ์ ์ฌ๋ฆฌ๋ ๊ฒ์ด๋ค.
โก ฮฑ=0.8์ ์๊ฐ์์๊ฐ ์๋๋ผ ์คํ ๋น ๊ณ์ โ ๋์ญํญ์ด ๋ฃจํ๋ ์ดํธ ๋ฐ๋ผ ์กฐ์ฉํ ์์ง์ธ๋ค.
ฯ = โฮt / ln(0.8) = 4.48ยทฮt
50 Hz (20 ms) โ ฯ = 90 ms โ f_c โ 1.8 Hz (K_E โ 11 sโปยน)
100 Hz (10 ms) โ ฯ = 45 ms โ f_c โ 3.6 Hz (K_E โ 22 sโปยน)
20 Hz (50 ms) โ ฯ = 224 ms โ f_c โ 0.7 Hz (K_E โ 4.5 sโปยน)
ยง3.5์ โ์ ์ด ์ค 14~20% ์คํ
์์ ๋ฐฑ์
์ ์ด๋ก ์ ํโ์ด ๊ธฐ๋ก๋ผ ์๋๋ฐ, ์ฃผ๊ธฐ๊ฐ ํ๋ค๋ฆฌ๋ฉด ์ถ์ ๊ธฐ ๋์ญํญ๋ ๊ฐ์ด ํ๋ค๋ฆฐ๋ค. ์์ ์ ํ ์ค: ฮฑ = 1 โ exp(โK_Eยทฮt)๋ฅผ ๋งค ์คํ
์ค์ธก ฮt๋ก ๊ณ์ฐ.
โข ํฌํ๋ ๋ ผ๋ฌธ์ ์๋ค. ์(11)์ ฮฬ์๋ ์ํ์ด ์๋ค. ์ฐ๋ฆฌ ํด๋ฆฝ์ ๊ด์ธก๊ธฐ ์ ํ์ฑ์ ๊นจ๊ณ , ์ โ์ฃผ์ํ ์ โกโ์ ํ์ด 4๋ฐฐ ๋จ๋จํด์ ธ๋ ๊ฐ ๋ถ๋ณ์ด ๊ทธ ์ง์ ๊ท๊ฒฐ์ด๋ค โ ๊ทธ ๊ตฌ๊ฐ์์ ์ถ์ ๊ธฐ๋ ์ผ์๋ก์ ์ฃฝ๊ณ ์์ ์ ๋ฐฉ๋ณด์์ผ๋ก ๋์ํ๋ค.
โฃ ํ์ธ ํ์ (์ฝ๋ ๋ฏธํ์ธ, ๋ณผํธ์ disturbance.py ์์)
- ๋ง์ฐฐ ์ด์ค ๊ณ์: ์ ์์ฌ์ฝ๋์ โ์ ํญ 0 ๊ฐ์ โ์ด ฮ=0์ ๋ปํ๋ฉด ์ ์์ด๋, damping ํญ๊น์ง ๋บ h๋ฅผ ๋ปํ๋ฉด ๋ง์ฐฐ์ด ฮฬ์ ํก์๋๋ ๋์์ FL์
damping*qd๋ก ๋ ๋ณด์๋์ด ์ด์ค ๊ณ์๋๋ค. โdisturbance.py์local_mpc.py๊ฐ ๊ฐ์ h๋ฅผ ์ฐธ์กฐํ๋์ง ํ์ธํ๋ฉด ์ฆ์ ํ์ . ์ฌ์ค์ด๋ฉด ์ โ์ฃผ์ํ ์ โกโ(์์ ํฌํ)์ ์์ธ ์ค ํ๋. - ํฌํ ์์น: ํด๋ฆฝ์ด ํํฐ ๋ค๋ฉด ๋จ์ ์ ๋จ์ด์ง๋ง, ํํฐ ์(์์ ์์ฐจ)์ด๋ฉด ์ก์์ด ์ ๋ฅ(rectify)๋์ด ์๋ ํธํฅ์ด ์๊ธด๋ค. qฬ ๊ธฐ๋ฐ์ด๋ผ ์์ ์์ฐจ๊ฐ ํนํ ์๋๋ฌ์ ์ํ์ด ํฌ๋ค.
โค ์ ๋ ฅ u์ ๊ด์ธก ๊ฐ๋ฅ์ฑ โ ๋ ผ๋ฌธ์ u(ํ/ํ ํฌ)๋ฅผ ๋ช ๋ นํ๊ณ ์ ํํ ์๋ ์ ์ ๋ค. ์ฐ๋ฆฌ๋ ์๋ ๋ช ๋ นํ์ด๋ผ u๋ฅผ ์ง์ ์์ง ๋ชปํ๊ณ ์ถ์ ์ผ๋ก ์ฑ์์ผ ํ๋ฉฐ, ๊ทธ ์ค์ฐจ๊ฐ ๊ทธ๋๋ก ฮฬ ํธํฅ์ด ๋๋ค. ์ โ์ฃผ์ํ ์ โ โ์ ๋ชจํฐ ๋ฐ์ ์ง์ฐ์ ๊ตฌํ ๋ ธ์ด์ฆ๊ฐ ์๋๋ผ ๋ ผ๋ฌธ ๋๋น ๊ตฌ์กฐ์ ์ฐจ์ด๋ก ๋ด์ผ ํ๋ค.
MPC ๋ฌธ์ ์ ์ํ
์๋ ๋น์ ํ MPC:
minuk โxNโxr,NโP2 + ฮฃk=0Nโ1(โxkโxr,kโQ2 + โukโR2)โ(12)s.t.โxk+1 = fd(xk, uk, ฮฬk),โck,i(xk,uk) โฅ 0disturbance-aware MPC ๋ฌธํ [22โ24]์ ํต์์ ๊ฐ์ ์ ๋ฐ๋ผ, ์งง์ ์์ธก ๊ตฌ๊ฐ์์๋ ์ธ๋์ด ํฌ๊ฒ ๋ณํ์ง ์๋๋ค๊ณ ๋ณธ๋ค:
ฮฬk+1 = ฮฬk๋ ผ๋ฌธ์ด โassumeโ์ด๋ผ๊ณ ๋ช ์ํ ์ ์ผํ ๋ชจ๋ธ๋ง ๊ฐ์ ์ด๋ฉฐ, ์(11)์ ์ธ๋ ์ถ์ ๊ฐ์ ์์ธก ๊ตฌ๊ฐ ์ ์ฒด์ ์์๋ก ๋ฐ์ด ๋ฃ๋ ๊ทผ๊ฑฐ๋ค.
Feedback Linearization ๊ธฐ๋ฐ ๋จ์ํ
๊ฐ์ ์ ๋ ฅ v โ โ4์ ํผ๋๋ฐฑ ์ ํํ ์ ์ด ๋ฒ์น ๋์ :
u = FL(x,v) := hL + MLv โ ฮฬL์ด๋ฅผ ํตํด ๋น์ ํ ๋์ญํ์ ์ ํ ๋์ญํ์ผ๋ก ๋ณํ:
xฬ = Acx + Bcv,โAc = [04ร4 I4; 04ร4 04ร4],โBc = [04ร4; I4]โ(13)์ต์ข MPC ๋ฌธ์ (์ ํํ๋ ๋์ญํ ๊ธฐ๋ฐ):
minvk โxNโxr,NโPฬ2 + ฮฃk=0Nโ1(โxkโxr,kโQฬ2 + โvkโRฬ2)โ(14)s.t.โxk+1 = Adxk + Bdvk,โck,i(xk, FL(xk,vk)) โฅ 0fL(xL,u,ฮL)
u = hL + MLv โ ฮฬL
xฬ = Acx + Bcv
๋ฌผ๋ฆฌ ์ ์ฝ ์ ์ง ยท DDP + AL
FL(feedback linearization, ํผ๋๋ฐฑ ์ ํํ) = โํ ๋ฌด๊ฒยท์ค๋ ฅยทํ ์ ํญ์ ๋ฏธ๋ฆฌ ๊ณ์ฐํด ์์ํ๋ ํ์ ๋ํด์ฃผ๋ฉด, ๋ณต์กํ ๊ธฐ๊ณ๊ฐ ๋จ์ํ ์์์ฒ๋ผ ์์ง์ธ๋คโ๋ ์ ์ด ๊ธฐ๋ฒ. ์ด๋ ๊ณ์ฐ๋๋ ํ์ ์ด ๋ฌธ์์์ โ๋ณด์ ํ(FL ํ ํฌ)โ์ด๋ผ ๋ถ๋ฆ ๋๋ค.
์(12)โ(14) ์ต์ ํ: ๋ ผ๋ฌธ์ ์์ผ๋ก 1์ด๊ฐ์ ์ ๋ ฅ ์ด ์ ์ฒด๋ฅผ ๊ณ ๊ธ ์๋ฒ(DDP)๋ก ํ๊ณ , ์ฐ๋ฆฌ๋ โ์ง๊ธ ์ด ์๊ฐ์ ๊ฐ์๋ ํ๋โ๋ง ์ ํ๋ ๋จ์ํ ๋ฒ์ ์ ๋ฒ์ฉ ์๋ฒ(IPOPT)๋ก ํ๋๋ค(๐ก):
# ๋ชฉํ์์ ๊ฑฐ๋ฆฌ(์์นยท์๋)๋ฅผ ์ค์ด๋ ๊ฐ์๋ v๋ฅผ, ์ 4๊ฐ์ง ํ๊ณ ์์์ ์ ํ
objective += 120*(์์น์ค์ฐจยฒ) + 18*(์๋์ค์ฐจยฒ)
์(14) ๋ณด์ ํ: ๋
ผ๋ฌธ ์์ u = h + Mยทv โ ฮฬ ๊ทธ๋๋ก ๊ตฌํ(+๋ง์ฐฐ ํญ):
raw_u = M@๊ฐ์๋ + ์ค๋ ฅํญ + ๋ง์ฐฐ*์๋ - ํ์ ํญ์ถ์
์ฐ๋ฆฌ ํ๋ํธ ์ ์ (์ค์ํ ์ฐจ์ด): ์ด ๋ณด์ ํ์ ์๋ ๋ชจํฐ์ ์ง์ ๋ฃ์ ์ ์์ผ๋ฏ๋ก, ์๋ ๋ช
๋ น์ ๊ทธ๋งํผ ๋ ์ธ๊ฒ ๋ฏธ๋ ๋ฐฉ์์ผ๋ก ๊ฐ์ ํจ๊ณผ๋ฅผ ๋
๋๋ค: ๋ชฉํ์๋ = ํ์ฌ์๋ + (v โ Mโปยนยทํ์ ํญ์ถ์ )ยทdt.
๊บผ๋ณด๊ธฐ ๊ฒ์ฆ์ ์ ์งํ ๊ฒฐ๊ณผ: ํ ์ ํญ ๋ถ๋ถ์ ๋๋ฉด ์ฑ๋ฅ์ด ํฌ๊ฒ ๋๋น ์ง์ง๋ง(์ ์(11) ์ฃผ์), ์ค๋ ฅยท๊ด์ฑ ๋ณด์ ๋ถ๋ถ๋ง ๋๋ฉด ๊ฒฐ๊ณผ๊ฐ ์ ๋ณํ์ต๋๋ค(์ฐ๋ฆฌ ๊ตด์ฐฉ์ด ํ ํ๊ณ์ ์ ๊ฑธ๋ฆด ๋งํผ ์จํํด์ ์ด ๋ณด์ ์ด ๊ฐ์ ํ ๊ธฐํ๊ฐ ์์). โ์ด ๋ถํ์ ๋จ๋ ํจ๊ณผ๋ ์ด ์กฐ๊ฑด์์ ์ฆ๋ช ์ ๋จโ์ผ๋ก ๊ธฐ๋ก.
๊ณ์ฐ ์๋: ์ฐ๋ฆฌ ๋จ์ํ ๋์ p95 7ms(100๋ฒ ์ค 95๋ฒ์ด 7ms ์ด๋ด). ๋ ผ๋ฌธ์ ํ๊ท 21.7~30.2ms โ ์ต์ ๊ฐ์ ๊ฐ๊น์ด p95์ ํ๊ท ์ ๊ฒฌ์ค ๊ฒ์ด๋ผ ์ฐ๋ฆฌ ์ชฝ์ ๋ถ๋ฆฌํ(๋ณด์์ ) ๋น๊ต์ ๋๋ค. ๋จ ๊ฐ๋ 23~25ms ํ๋ ์ด๋ด ๋ถํฌ.
โ๊ฐ์๋ ํ๋๋ง ์ ํด ์ ์งโํ๋ ์ฐ๋ฆฌ ๋จ์ํ๋ ๊ฐ์๋๊ฐ ๊ณ๋จ์์ผ๋ก ๋ฐ๋์ด ๋ชธ์ฒด๊ฐ ๊ฟ๋ ์ด๋ ๊ตฌ์กฐ์ ์์ธ ํ๋ณด 1์์์ ๋๋ค. ๋ ผ๋ฌธ์ฒ๋ผ ์ ๋ ฅ ์ด์ ๋ถ๋๋ฝ๊ฒ ์ต์ ํํ๋ฉด ์ํ ์ฌ์ง. ์ ์ด ์ค 14~20% ์คํ ์์ ๋ฐฑ์ ์ ์ด๋ก ์ ํ๋๋ฉฐ ๋ช ๋ น์ด ์ด์ง ํ๋ ๊ฒ๋ ์์ธ ํ๋ณด. โ ๋ค์ ๊ฐ์ ์์ ์ผ๋ก ๋ฑ๋ก๋จ.
4. ์คํ ๊ฒฐ๊ณผ
4.1 ์๋ฎฌ๋ ์ด์ ํ๊ฒฝ
| ํญ๋ชฉ | ์ค์ |
|---|---|
| ํ๋ซํผ | Hyundai HX300L ๋ชจ๋ธ ํ๋ผ๋ฏธํฐ |
| ๊ตฌํ | ROS + C++ (๊ฐ ๋ชจ๋ ๊ฐ๋ณ ๋ ธ๋) |
| ์ธ๋ ๋ชจ๋ธ | Modified LuGre friction (์ค๋ฆฐ๋ ๋ง์ฐฐ) + Modified FEE (ํ ์-๋ฒํท ์ํธ์์ฉ) |
| ์ ์ญ ์ต์ ํ | ALGLIB (๋น์ ํ ์ต์ ํ ์๋ฒ) |
| MPC ์๋ฒ | DDP + Augmented Lagrangian [30] |
| MPC ํ๋ผ๋ฏธํฐ | ์๊ฐ ์ํ์ 1000ms, ์ ์ด ์ด์ฐํ/์ ๋ถ ์ฃผ๊ธฐ 20ms |
| Bernstein ์ ์ด์ | Phase 1, 3: 9๊ฐ / Phase 2: 20๊ฐ via point |
| ํ๋์จ์ด | Intel Core i7-9750 CPU @ 2.6GHz |
4.2 Scenario 1 โ Shallow Excavation (์์ ๊ตด์ฐฉ)
์ฃผ์ ๊ฒฐ๊ณผ:
| ์งํ | ๊ฐ |
|---|---|
| Global trajectory ๊ณ์ฐ ์๊ฐ | 2.1์ด |
| Local planning ํ๊ท ๊ณ์ฐ ์๊ฐ | 21.7ms (์ฝ 46Hz) |
| ๋ฌผ๋ฆฌ์ ์ ์ฝ์กฐ๊ฑด ๋ง์กฑ | ๋ชจ๋ ์ ์ฝ์กฐ๊ฑด ์ถฉ์กฑ |
| ์ธ๋ ์ถ์ | ๋ฒํท ์ค๋ฆฐ๋์์ ๋ง์ฐฐ ๋ชจ๋ธ ๋ช ํํ ๊ด์ธก, ๋ถ/์์์๋ ํ ์ ์ํธ์์ฉ ์ธ๋์ด ๋ ํผ. ์ถ์ ๊ฐ(๊ฒ์ )์ด ๋ชจ๋ ์ถ์์ ๋ชจ๋ธ ์ธ๋(๋นจ๊ฐ)์ ์ ์ถ์ข โ ์ธ๋ ์ถ์ ๊ธฐ ์ ํจ์ฑ์ ์ง์ ๊ทผ๊ฑฐ |
- ๋ชฉํ ํ์(target shape)์ ๊ดํตํ์ง ์๋๋ก ๋์ ์์ญ์์ ๊ถค์ ์์ฑ
- Tip-grading-like motion ๋ฐ์
- Global planning์ด ์๋์ ์ผ๋ก ๋ฌผ๋ฆฌ์ ํ๊ณ๋ฅผ ์ด๊ณผํ๋๋ผ๋, local planning์์ ์ ์ฝ์กฐ๊ฑด ๋ง์กฑ
4.3 Scenario 2 โ Deep Excavation (๊น์ ๊ตด์ฐฉ)
์ฃผ์ ๊ฒฐ๊ณผ:
| ์งํ | ๊ฐ |
|---|---|
| Global trajectory ๊ณ์ฐ ์๊ฐ | 1.6์ด |
| Local planning ํ๊ท ๊ณ์ฐ ์๊ฐ | 30.2ms (์ฝ 33Hz) |
| ๋ฌผ๋ฆฌ์ ์ ์ฝ์กฐ๊ฑด ๋ง์กฑ | ๋ชจ๋ ์ ์ฝ์กฐ๊ฑด ์ถฉ์กฑ |
| ์ธ๋ ํฌ๊ธฐ | Scenario 1๋ณด๋ค ํฐ ์ธ๋ (๋ฒํท์ด ํ ์์ ๋ ๊น์ด ์นจํฌ) |
- ๋ชฉํ ํ์์ด ๊น์ผ๋ฉด Phase 2์์ ์ผ๊ฐํ ํํ์ ๊ถค์ ์์ฑํ์ฌ ๊ตด์ฐฉ ์ฒด์ ์ต๋ํ
- ๋ฐ๋ณต ๊ตด์ฐฉ์ ํตํด ๊น์ ๋ชฉํ ํ์์ ์ ์ง์ ์ผ๋ก ๋๋ฌํ๋ ์ ๋ต(target-shape-aware excavation-volume-maximizing strategy)
- ๋ ํฐ ์ธ๋์๋ ๋ถ๊ตฌํ๊ณ global reference ์ ์ถ์ข ๋ฐ ๋ชจ๋ ๋ฌผ๋ฆฌ์ ์ ์ฝ์กฐ๊ฑด ์ถฉ์กฑ
4.4 ๋ ์๋๋ฆฌ์ค ๋น๊ต
| ํญ๋ชฉ | Shallow Excavation | Deep Excavation |
|---|---|---|
| ๊ถค์ ํํ | ๋์ ์์ญ tip-grading | ์ผ๊ฐํ ์ฒด์ ์ต๋ํ |
| ๋ชฉํ ๋๋ฌ | 1ํ ๊ตด์ฐฉ์ผ๋ก ๋ชฉํ ํ์ ์ถ์ข (์๋ฌธ์ pass ์๋ฅผ ๋ช ์ํ์ง ์์) | ๋ฐ๋ณต ๊ตด์ฐฉ์ผ๋ก ์ ์ง ๋๋ฌ (์๋ฌธ ๋ช ์) |
| Global ๊ณ์ฐ | 2.1s | 1.6s |
| Local ๊ณ์ฐ | 21.7ms (46Hz) | 30.2ms (33Hz) |
| ์ธ๋ ํฌ๊ธฐ | ์๋์ ์ | ์๋์ ๋ |
| ์ ์ฝ ๋ง์กฑ | O | O |
| ํญ๋ชฉ | ๋ ผ๋ฌธ (30t, ์์ฒด ์๋ฎฌ) | APEX (8.5t, AGX ์๋ฎฌ) |
|---|---|---|
| ์ ์ญ ๊ณํ ๊ณ์ฐ | 1.6~2.1์ด | ~0.4์ด + ์ง๋ฉด ์์ฝ (pass๋ง๋ค) |
| ๊ตญ์ ์ ์ด ๊ณ์ฐ | ํ๊ท 21.7~30.2ms | 95%๊ฐ 7ms ์ด๋ด (๊ฐ๋ 23~25ms) |
| ํ๊ณ ์กฐ๊ฑด ๋ง์กฑ | ์ ๋ถ ์ถฉ์กฑ | ๋ช ๋ น ํฌํ 0, ์ฌ์ ๋ฑ๋ก ๊ธฐ์ค ๋ด (๋จ โํ์ ํ ๊ฒฝ๊ณ โ ~24%๋ ๋ฏธ๋ณด์ ์์น ํ) |
| ๊น์ ๊ตด์ฐฉ ์ ๋ต | ์ฌ๋ฌ pass ๋ฐ๋ณต ์ ์ง ๋๋ฌ | ๋์ผ ์ ๋ต: 3-pass๋ก โ0.27โโ0.30โโ0.44m |
| ์ง๋ฉด ์ ๋ณด | ๋ฏธ๋ฆฌ ์๋ค๊ณ ๊ฐ์ | LiDAR ์ค์ธก ์ค์บ (๊ฒฝ์ฌ 8ยฐโ7.32ยฐ ์ฝ์) |
| ์กฐ๊ฑด ๋ค์์ฑ | ์์/๊น์ 2๊ฐ์ง | 5๊ฐ์ง (๋จ๋จํ ํ/๋ชจ๋/๊ฒฝ์ฌ/๋๋/์กฐํฉ) + ๋ผ์ด๋ธ ์ฌํ |
๊ตด์ฐฉ๋์ ๊ธฐ์กด ์๋๊ตด์ฐฉ(scripted) ๋๋น 84% โ ํ๊ณ ์กฐ๊ฑด์ ์งํค๋ ๋๊ฐ๋ก ๊ธฐ๋ก.
5. ํ๊ณ ๋ฐ ํฅํ ์ฐ๊ตฌ
๋ ผ๋ฌธ์ด ๋ช ์ํ ๊ฐ์ ยทํ๊ณ (์๋ฌธ์ ๊ทผ๊ฑฐ๊ฐ ์๋ ๊ฒ):
- ์๋ฎฌ๋ ์ด์ ๋ง ๊ฒ์ฆ: ์ค์ ๊ตด์ฐฉ๊ธฐ ์คํ ๋ฏธ์ํ. ๊ฒฐ๋ก ์ด ๋ฐํ ์ ์ผํ ํฅํ ๊ณผ์ โ โFuture work may include validation of the proposed algorithm in actual excavatorsโ (ยงVI)
- ๋จ์ผ ๊ตด์ฐฉ ์์ (single digging task): ๊ฒฐ๋ก ์ด ์ค์ค๋ก ๋ฒ์๋ฅผ ์ด๋ ๊ฒ ํ์ (ยงVI). ๋จ, ๋ฐ๋ณต ๊ตด์ฐฉ์ Scenario 2์ ๋ช ์์ ์ ๋ต์ด๊ณ ํธ๋ญ ์ ์ฌ๋ Phase 3์ ๋ชฉํ ์ง์ ์ผ๋ก ์ด๋ฏธ ํฌํจ๋์ด ์๋ค โ ๋น ์ง ๊ฒ์ ๊ทธ ์์ ํ์คํฌ ํ๋๋์ด๋ค
- ํ๊ฒฝ ์ฌ์ ์ง์ ๊ฐ์ : ์ง๋ฉด ํ์(ground shape)์ ์ฌ์ ์ ์๋ค๊ณ ๊ฐ์ . ์๋ฌธ์ ๋ผ์ด๋คยท์นด๋ฉ๋ผ ๋ฑ ์จ๋ณด๋ ์ผ์๋ ํ ํ์คํ ์ด์ ์ผ๋ก ์ทจ๋ ๊ฐ๋ฅํ๋ค๊ณ ์ธ๊ธ (ยงV-A)
- ์ธ๋ ๋ถ๋ณ ๊ฐ์ : ์งง์ ์์ธก ๊ตฌ๊ฐ์์ ์ธ๋์ด ํฌ๊ฒ ๋ณํ์ง ์๋๋ค๊ณ ๊ฐ์ โ ฮฬk+1 = ฮฬk. MPC ์ ์ํ์ ํต์ฌ ์ ์ ์ด๋ฉฐ ๋ ผ๋ฌธ์ด โassumeโ์ด๋ผ ์ ์ ์ ์ผํ ๋ชจ๋ธ๋ง ๊ฐ์ (ยงIV-C)
- Phase 2 ์ค์ ๊ณ ์ : ๋ฒํท ์ธกํ(side plate)์ด ํ์ ๋ฏธ๋ ๊ฒ์ ๋ง๊ธฐ ์ํด ์บ๋น์ ๊ณ ์ ํ ์ฑ ๊ตด์ฐฉ ๊ถค์ ์ ์ต์ ํ (ยงIII-A)
- ์ค์๊ฐ์ฑ์ ์กฐ๊ฑด๋ถ: ์ ์ญ ๊ณํ์ด ๊ถค์ ์คํ ์๊ฐ ์์ ๋๋์ผ ํตํฉ planner๊ฐ ์ค์๊ฐ์ผ๋ก ์ฑ๋ฆฝ (ยงI-B)
- ์ ๋จ์ฌ ๋งคํ์ ์ ํจ ๋ฒ์: ์(1)์ ์ผ๋์ผ ๋์์ ๋ฌผ๋ฆฌ์ ์ผ๋ก ๊ฐ๋ฅํ ์์ญ ์์์๋ง ์ฑ๋ฆฝํ๋ฉฐ, ๊ทธ ์์ญ์ ๊ตญ์ ๊ณํ์ ์ค๋ฆฐ๋ ๋ณ์ ์ ์ฝ์ผ๋ก ๋ฐ์ (์๋ฌธ ๊ฐ์ฃผ 1)
์์ฝ์ ํ๋จ (์๋ฌธ์ ์๋ ์ถ๊ฐ ํ๊ฐ โ ์ฐธ๊ณ ์ฉ):
- ์ง๋ฉด ํ์์ ๋ถํ์ค์ฑ ๋ฏธ๊ณ ๋ ค โ ์๋ฌธ์ ์ผ์๋ก ์ทจ๋ ๊ฐ๋ฅํ๋ค๊ณ ๋ง ํ๊ณ ๋ถํ์ค์ฑ์ ๋ ผํ์ง ์๋๋ค
- ํ ์ ์กฐ๊ฑด ๋ค์์ฑ ๋ฏธ๋ฐ์ โ ๋ค๋ง ์๋ฌธ์ ๋ง์ฐฐยทํ ์ ๋ชจ๋ธ์ด planner์๊ฒ๋ ์๋ ค์ง์ง ์๋๋ค๊ณ ๋ช ์ํ๋ค (ยงV-A). ์ฆ planner๊ฐ ํ ์ ๋ชจ๋ธ์ ์์กดํ์ง ์๋ ๊ฒ์ ํ๊ณ๊ฐ ์๋๋ผ ์ค๊ณ ์๋์ด๊ณ , ๋ค์์ฑ ๋ฌธ์ ๋ ์๋ฎฌ๋ ์ด์ ํ๊ฒฝ ์ชฝ ํ๊ณ๋ก ๋ด์ผ ํ๋ค
| ๋ ผ๋ฌธ์ ํ๊ณ | ์ฐ๋ฆฌ ์ํ |
|---|---|
| ์๋ฎฌ๋ ์ด์ ๋ง ๊ฒ์ฆ | ๊ฐ์ (์ค์ฐจ ์ด์์ ๋ณ๋ ๋ก๋๋งต) |
| ์ง๋ฉด ๋ชจ์์ ๋ฏธ๋ฆฌ ์๋ค๊ณ ๊ฐ์ | ํด์ โ LiDAR ์ค์บโ์๋ ์์ํ (๊ฒฝ์ฌยท๋๋ ์ค์ธก ๊ฒ์ฆ) |
| ํ ๋ชจ๋ธ ๋จ์ํ | ๋ถ๋ถ ํด์ โ ์ ์ ๋ฌผ๋ฆฌ(AGX) + ๋ฌผ์ฑ 3์ข ๋ฐ๊ฟ ๊ฒ์ฆ (๋จ 3๊ฐ ์์น๋ง ๋ณ๊ฒฝ) |
| ํ ๋ฒ์ ๊ตด์ฐฉ๋ง ๋ค๋ฃธ | ๋ถ๋ถ ํด์ โ 3-pass ์ฐ์ + ํ์ ๋คํ (๊ทธ ์์ ์์ ๊ณํ์ ๋ฏธํฌํจ) |
์ฐ๋ฆฌ์๊ฒ๋ง ์๋ ํ๊ณ (๋ ผ๋ฌธ์๋ ์๋ ๊ฒ): โ ๋ฌด๊ฒยท๊ด์ฑยทํ ํ๊ณ ์์น ๋ฏธ๋ณด์ โก ํ์ผ๋ก ๋ฏธ๋ ๋ชจํฐ ์์(์๋ ๋ชจํฐ โ ๋ณด์ ํ์ ์๋๋ก ํ์ฐ ์ ์ฉ) โข ์ค๋ ฅยท๊ด์ฑ ๋ณด์ ๋ถํ์ ๋จ๋ ํจ๊ณผ ๋ฏธ์ฆ๋ช โฃ ๊ตญ์ MPC์ horizon ๋จ์ํ โ ๋ ผ๋ฌธ์ 1์ด ์ ๋ ฅ ์ด ์ ์ฒด๋ฅผ DDP๋ก, ์ฐ๋ฆฌ๋ โ์ง๊ธ ์๊ฐ์ ๊ฐ์๋ ํ๋โ๋ง IPOPT๋ก (ยง3.4) โค ๋์ปน๊ฑฐ๋ฆผ(์ ํฌ) โ ๋ค์ ๊ฐ์ ์์ (โฃ๊ฐ ์์ธ ํ๋ณด 1์์).
์ข ์ โฃ๋ โpass ๋จ์ ์ฌ๊ณํโ์ด์์ผ๋ ์ด๋ ํ๊ณ๊ฐ ์๋๋ฏ๋ก ๊ต์ฒดํ๋ค. ๋ ผ๋ฌธ๋ ์ ์ญ ๊ณํ์ ๊ตด์ฐฉ 1ํ๋น 1๋ฒ์ด๊ณ (๊ณ์ฐ 1.6~2.1์ด, 20ms ๋ฃจํ์ ๋ค์ด๊ฐ ์ ์์), ์ ์ญ ๊ถค์ ์ MPC์ ๊ณ ์ ๊ธฐ์ค๊ฐ์ด๋ฉฐ, Scenario 2๋ โ๋ฐ๋ณต ๊ตด์ฐฉ์ผ๋ก ์ ์ง ๋๋ฌโํ๋ค โ ์ ํ์ โ๋์ผ ์ ๋ตโ ํ์ ๊ณผ ๊ฐ์ ๋ด์ฉ์ด๋ค. ์คํ๋ ค ์ฐ๋ฆฌ๋ pass ์ฌ์ด LiDAR ์ฌ์ค์บ์ผ๋ก ๋ ผ๋ฌธ์ โ์ง๋ฉด ์ฌ์ ์ง์โ ๊ฐ์ ์ ์ผ๋ถ ํด์ํ ์ชฝ์ด๋ค. ์ค์ ๊ฒฉ์ฐจ๋ ๊ตญ์ MPC์ horizon์ด๋ฏ๋ก โฃ๋ฅผ ๊ทธ๊ฒ์ผ๋ก ๋์ฒด.
6. ํ์ ๊ฒ์ฆ ๊ณผ์ (2026-08-14)
์๋ ํญ๋ชฉ์ ์ฝ๋ฉ ์์ด์ ํธ์๊ฒ ๊ทธ๋๋ก ๋๊ธฐ๊ธฐ ์ํ ์์ ๋ช ์ธ๋ค. ๊ฐ ํญ๋ชฉ์ โ ๋ฌด์์ ํ์ธ/๊ตฌํํ๋๊ฐ โ ํ์ ๊ธฐ์ค(์ซ์) โ ํ์ง ๋ง ๊ฒ์ ๊ฐ์ถ๊ณ ์๋ค. ํ์ ๊ธฐ์ค ์์ด โ๊ฐ์ ํด์คโ๋ก ๋๊ธฐ๋ฉด ๊ฒ์ฆ ๋ถ๊ฐ๋ฅํ ๊ฒฐ๊ณผ๊ฐ ๋์์จ๋ค.
6.1 ๊ฐ๋โ์ค๋ฆฐ๋ ๋ณํ๊ณผ ์ผ์ฝ๋น์
ยง3.1์ ๐ข๊ณผ ยง3.2์ ๐ก ์ฌ์ด์ ์์ง ๋ถ๋ฆฌ๋์ง ์์ ํญ๋ชฉ์ด ํ๋ ์๋ค โ 40์นธ ํ์์ ํ์๋๋ JLยทJฬL์ด ยง3.2์ ๋ฏธ๋ณด์ ํ์ ์ ์ผ๋ง๋ ๊ธฐ์ฌํ๋๊ฐ. ๋น์ฉ์ด ๋ฎ์ ์์๋ก ์ ๋ฆฌํ๋ค.
| # | ํ ์ผ | ์ ์ด ์์์ธ๊ฐ | ๋น์ฉ |
|---|---|---|---|
| โ | dynamics.py๊ฐ JL์ ์ด๋์ ์ป๋์ง ํ์ธ โ ยง3.1์ 40์นธ ํ์์ ์์น ๋ฏธ๋ถํ๋์ง, URDF ๊ธฐํ์์ ์ง์ ๋ฝ๋์ง |
์ด๊ฒ ์ ํด์ง์ง ์์ผ๋ฉด ๋๋จธ์ง ๋ ผ์๊ฐ ๊ณต์ค์ ๋ฌ๋ค. URDF์์ ์ง์ ๋ฝ๋๋ค๋ฉด ํ๋ ยง3.2์ ๋ฌด๊ดํ๋ค | 5๋ถ |
| โก | ์ถ๋ณ dL/dฮธ ๋ถํธ๋ฅผ ๊ทธ๋ฅ ์ถ๋ ฅ โ ๋ถยท์ยท๋ฒํท ๊ฐ๊ฐ ์์ ๋ฐฉํฅ๊ณผ ๋์กฐ |
ํ๊ฐ ์ด๋ฏธ ์์ผ๋ฏ๋ก 3์ค์ด๋ฉด ๋๋ค. โ์ ์ถ ๋ถํธ ๋ฐ๋โ์ ์ธ ํ๋ณด ์ค ๋(์ฅ์ฐฉ ๋ฐฉํฅยทํ๋์ฝ๋ฉ)์ ์ฆ์ ๊ฐ๋ฅธ๋ค | 3์ค |
| โข | ๋น๊ต ์กฐ๊ฑด ์ฌ์ค์ โ ์ค์ ์ (์ ์) ๊ตฌ๊ฐ๋ง ๊ณจ๋ผ Mqฬ + h vs ์ค์ธก ์ค๋ฆฐ๋ ํ ์๊ด์ ๋ค์ ๊ณ์ฐ |
๊ด์ฑยท๋ง์ฐฐยทํ ์ ์ ํญ์ด ์ต์์ธ ๊ตฌ๊ฐ์ด๋ผ ์ค๋ ฅํญ์ด ์ง๋ฐฐํ๋ค. ์ฌ๊ธฐ์๋ ์๊ด์ด ๋ฎ์ผ๋ฉด ฮ ๋๋ฝ ํ์ด ์๋๋ผ ์ง์ง ๋ชจ๋ธ ๋ฌธ์ ๋ก ํ์ ๋๋ค | ๋ฐ๋์ |
| โฃ | 40์ ์ L(ฮธ) = โ(aยฒ + bยฒ โ 2abยทcos(ฮธ+ฯ)) 3ํ๋ผ๋ฏธํฐ ์ต์์์น ํผํ
|
์ ๋ฐ์ดํฐ ์์ด ๊ธฐ์กด ํ๋ง์ผ๋ก ๋๋ค. ๋ถยท์์ ์ค๋ฆฐ๋๊ฐ ๋ ํ ์ฌ์ด ์ผ๊ฐํ์ ํ ๋ณ์ด๋ผ ์์ฐจ๊ฐ ๊ฑฐ์ 0์ผ๋ก ๋จ์ด์ง ๊ฒ์ด๊ณ , ๊ทธ๋ฌ๋ฉด ํ ์ขํ ์์ด ์ ํจ ๊ธฐํ๋ฅผ ๋ณต์ํด JยทJฬ์ ํด์์ ์ผ๋ก ์ป๋๋ค | ๋ฐ๋์ |
โฃ์ ๋จ์: ๋ฒํท์ 4์ ๋งํฌ(์ฌ๋ค๋ฆฌ๊ผด)๋ผ ์ผ๊ฐํ 3ํ๋ผ๋ฏธํฐ ํํ๋ก ๋ง์ง ์๋๋ค. ๋ฒํท๋ง ํ๋ฅผ ์ ์งํ๋, โ ์์ โํ์์ JฬL์ ๋ฝ๋๋คโ๋ก ํ์ธ๋๋ฉด np.interp โ CubicSpline(Cยฒ ์ฐ์)์ผ๋ก ๊ต์ฒดํด Jฬ์ ๊ณ๋จยท์คํ์ดํฌ๋ฅผ ์์ค๋ค.
์๋ฌธ ยงII.A๋ ๋งคํ์ โThe nonlinear, bijective mapping between qฮธ and qL can be derived by solving closed-chain kinematicsโ ํ ๋ฌธ์ฅ์ผ๋ก๋ง ์ ์ธํ๊ณ , ์(1)์ gL, gฮธ, JL, Jฮธ๋ฅผ ์ ๋๋ ํจ์๋ก ๋ ๋ค where J_L, J_ฮธ are corresponding Jacobian matrices ํ ์ค๋ก ๋์ด๊ฐ๋ค โ ๊ตฌ์ฒด์ ์ธ ์์ ์ฃ์ง ์์๋ค. ๋งํฌ ์น์ยทํ ์ขํ๊ฐ HX300L์ ์ค์ CAD ํ์์ด๋ผ ๊ณต๊ฐํ์ง ์์ ๊ฒ์ผ๋ก ๋ณด์ธ๋ค.
๋ฐ๋ผ์ โ๋ ผ๋ฌธ์ด ์ฐ๋ฆฌ์ ๋ค๋ฅด๊ฒ ์ฒ๋ฆฌํ๋คโ๊ธฐ๋ณด๋ค, ๋ ผ๋ฌธ์ ํด์ํด ์์ ๋น์ ํ์ฑ์ ๋ฃ์ด๋๊ณ ๋ณด์ฌ์ฃผ์ง ์์ ๊ฒ์ด๊ณ ์ฐ๋ฆฌ๋ ๊ทธ ๋น์ ํ์ฑ์ ์ค์ธก ํ๋ก ์ง์ ๋ด์ ๊ฒ์ด๋ค. ๋ฒํท์ ๋น๋น๋ก์ฑ(๋ฒ๋ฆฌ๋ ์ชฝ๊ณผ ์ค๋ฏ๋ฆฌ๋ ์ชฝ ๊ธฐ์ธ๊ธฐ 2๋ฐฐ ์ฐจ์ด)๋ 4์ ๋งํฌ ํ์ ์กฐ๊ฑด์ ๋น์ ํ์ฑ์ด๋ผ ํด์ํด ์์ ์ด๋ฏธ ๋ค์ด ์๋ค. ๋ค๋ง 4์ ๋งํฌ ์์นํด์์ ๋ซํ ํด๊ฐ ์กด์ฌํ๋ฏ๋ก(๋ฐ๊ฐ ํ์ ํธ ์นํ / ์ฝ์ฌ์ธ ๋ฒ์น 2ํ), โํด์์ ์ผ๋ก ์ ํ๋ฆฐ๋คโ๋ ๊ฒ์ ์ฌ์ค์ด ์๋๋ค.
์ค์ง์ ์ฐจ์ด๋ Jฬ๊น์ง ํ์ํ๋ค๋ ์ (์ 3์ hL)์ ์๋ค. ํด์ํด๋ ๋ฏธ๋ถํด์ ๊ทธ๋๋ก ์ป์ง๋ง, 40์นธ ์ด์ฐ ํ์์๋ 2์ฐจ ๋ฏธ๋ถ์ ํด๋นํ๋ ์์ ๋ฝ์์ผ ํ๋ค.
๊ธฐ์ข ๋ ๋ค๋ฅด๋ค โ ๋ ผ๋ฌธ์ HX300L(~30t๊ธ), APEX๋ E85(~8t๊ธ). ํ๊ฐ โ์ค์ธก์ด๋ผ ์ค์ ์ ์ ์์ ์ผ์นโํ๋ค๋ ํํ์ ์ ํํ๋ AGX ์๋ฎฌ๋ ์ดํฐ์ E85 URDF ๊ธฐํ์ ์ผ์นํ๋ค๋ ๋ป์ด๋ฉฐ, ์ค๊ธฐ ์ผ์น์๋ ๋ณ๊ฐ๋ค.
6.2 Phase 1ยท3 ์๊ฐ ๋ฐฐ๋ถ(T) ๋จ์ํ์ ํ๋น์ฑ ๊ฒ์ฆ
๋ฐฐ๊ฒฝ๊ณผ ์ ๋๋ ยง3.3 ์(7)(8)(9) ์ฝ์์์ ใ๐ ์๊ฐ ๋ฐฐ๋ถ ๋จ์ํ ์ฌ๊ฒํ ใ ์ฐธ์กฐ. ์์ง๋ โ์ฐ๋ฆฌ ๋ฐฉ์์ด ํ๋ ธ๋คโ๊ฐ ์๋๋ผ โ๋ง๋์ง ์์ง ์ ์ฌ๋ดค๋คโ์ด๋ฉฐ, ์ฌ๋ ๋ฐ ๋๋ ๊ณ์ฐ์ด ์ค์นผ๋ผ 1๊ฐ์ง๋ฆฌ ๋ซํ ํด๋ผ๋ ๊ฒ์ด๋ค.
โช ์ฐฉ์ ์ ๋ฐ๋์ ํ์ ํ ๊ฒ (์ฝ๋ ํ์ธ, 5๋ถ)
์ด๊ฒ ์ ํด์ง์ง ์์ผ๋ฉด ์๋ โค~โฆ ์ ๋ถ๊ฐ ์๋ชป๋ ์ ์ ์์ ์ ๋ค.
| ํ์ธ ํญ๋ชฉ | ์ ์ค์ํ๊ฐ |
|---|---|
โ์๊ฐ๋ง ์ฌ๋ฐฐ๋ถโ์ ์ ํํ ์๋ฏธ โ (A) T = ฮฃ(๊ตฌ๊ฐ๊ธธ์ด / ๊ตฌ๊ฐ์๋)๋ก T๊ฐ ์๋ ์ํ์ ๊ฒฐ๊ณผ์ธ๊ฐ, (B) ์ ์ฒด T๋ ์ธ๋ถ ๊ณ ์ ์ด๊ณ ๊ตฌ๊ฐ ๋ฐฐ๋ถ๋ง ์กฐ์ ํ๋๊ฐ |
(A)๋ฉด โค์ T_cap ์ ์๊ฐ ๊ทธ๋๋ก ์ฑ๋ฆฝํ๋ค. (B)๋ฉด T๊ฐ ๋ฌผ๋ฆฌ์ ๋ฌด๊ดํ ์์๋ผ๋ ๋ป์ด๋ผ ๊ฒฉ์ฐจ๊ฐ ๋ ํฌ๊ณ , โค์ ๋น๊ต ๋์ ์์ฒด๋ฅผ ๋ค์ ์ธ์์ผ ํ๋ค |
| 0.5 / 0.25 / 0.5์ ์ ์ฒด โ phase 1/2/3์ ๊ฐ๊ฐ ๋์ํ๋๊ฐ, ์๋๋ฉด phase 1(๋๋ 3) ๋ด๋ถ์ ๊ตฌ๊ฐ๋ณ ๊ฐ์ธ๊ฐ | phase ๋์์ด๋ฉด ์๋ ๋ถ์ฐ์์ด ๋ ผ๋ฌธ์ด ๋งค๋๋ฝ๊ฒ ํ๋ผ๊ณ ๋ช ์ํ ๋ฐ๋ก ๊ทธ ๋ ์ง์ ์์ ๋ฐ์ํ๋ค (โฆ์ ํ์ ์ด ๋ฌ๋ผ์ง) |
| ์๋ ์ํ 0.5/0.25m/s์ ์ถ์ฒ | ์์ ๊ธฐ์ค์ธ์ง, ์ก์ถ์์ดํฐ ํ๊ณ ํ์ฐ์ธ์ง, ๊ฒฝํ๊ฐ์ธ์ง. โค์์ T*๊ฐ ๋ ๋๋ฆฌ๊ฒ ๋์์ ๋ ์ํ์ ๋ฐ๊ฟ๋ ๋๋์ง๊ฐ ์ฌ๊ธฐ์ ๊ฐ๋ฆฐ๋ค |
โค T* ๊ณ์ฐ ํ ํ์ฌ T์ ๋์กฐ โ ์ด ์ ์ ํต์ฌ ์์
๊ณก์ ๋ชจ์ ฮฒ๋ฅผ ๊ณ ์ ํ ์ฑ๋ก๋ T๊ฐ ์ค์นผ๋ผ ํ๋์ด๊ณ ๋ซํ ํด๊ฐ ์๋ค. ์ ์๋ฒยท์ฌ์ต์ ํ ๋ถํ์.
1) ๊ณ ์ ๋ Bernstein ๊ณก์ ์ ์์์ ๊ธฐ์ค Tโ ๋ก ์ํ๋ง (s โ [0,1], ๊ท ๋ฑ N์ )
2) ์ญ๋์ญํ 2ํ ํ๊ฐ๋ก A, G ๋ถ๋ฆฌ:
G(s) = u |_{qฬ=0, qฬ=0} โ ์ค๋ ฅ๋ง (์ ์ ์ ์ง๋ ฅ)
A(s) = Tโยฒ ยท ( u|_{Tโ} โ G(s) ) โ ๋๋จธ์ง = ๊ด์ฑยท์ฝ๋ฆฌ์ฌ๋ฆฌ
3) ์ฌ๋ค๋ฆฌ๊ผด ์ ๋ถ 3๊ฐ:
a = ยฝโซโยน AแตW_u A ds
b = โซโยน AแตW_u G ds
c = ยฝโซโยน GแตW_u G ds
4) T* = โ( [ b + โ(bยฒ + 12ac) ] / (2c) )
5) T_cap = (ํ์ฌ ๋ฐฉ์์ด ์ฐ์ถํ๋ ์ค์ ์์์๊ฐ) โ โช-(A)/(B)์ ๋ฐ๋ผ ์ ์
6) ๋น์จ r = T* / T_cap ์ถ๋ ฅ
ํ์ ๊ธฐ์ค (์ด ํ๋๋ก ๊ฒฐ๋ก ์ ์ ์ ๊ฒ):
| r = T*/Tcap | ์๋ฏธ | ์กฐ์น |
|---|---|---|
| 0.9 ~ 1.1 | ์๋ ์ ์ฝ์ด ์ค์ ๋ก binding โ ์ฐ๋ฆฌ ์ ํ์ด ๋ ผ๋ฌธ ํด์ ์ฌ์ค์ ์ผ์น | ยง3.3 ํ์ ๐ก โ ๐ข ์น๊ฒฉ, ๊ทผ๊ฑฐ ์์น์ ํจ๊ป ์ข ๊ฒฐ. ์ฝ๋ ๋ณ๊ฒฝ ์์ |
| r > 1.1 | ๋ถํ์ํ๊ฒ ๋นจ๋ฆฌ ๋ฌ๋ ค ๊ด์ฑ๋ ฅ์ ๋ญ๋น ์ค | T๋ฅผ T*๋ก ๋๋ฆฐ๋ค. ํ ์ฌ์ ๊ฐ ์๊ธฐ๋ฏ๋ก ยง3.4 โ24% ๊ฒฝ๊ณ โ์ ยง3.2 ฮฬ ํฌํ๊ฐ ํจ๊ป ์ค์ด๋๋์ง ํ์ธ |
| r < 0.9 | ๋ ๋นจ๋ฆฌ ๊ฐ ์ ์๋๋ฐ ๋ชป ๊ฐ๋ ์ค | ์ฌ์ดํด ํ์ ์ํด. โช์ โ์๋ ์ํ ์ถ์ฒโ๋ฅผ ๊ทผ๊ฑฐ๋ก ์ํ ์ฌ๊ฒํ |
ํ์ง ๋ง ๊ฒ:
- ฮฒ(๊ณก์ ๋ชจ์)๊น์ง ๊ฐ์ด ์ต์ ํํ์ง ๋ง ๊ฒ. ์(8)์ ์์ ํ
minฮฒ,T๋ฅผ ์ฌํํ๋ ค ๋ค๋ฉด ๋ฒ์๊ฐ ํญ๋ฐํ๊ณ ยง3.3์ ๊ฒ์ฆ๋ ํ์(์ด์ ์ ๊ด์ฐฐ๋ก ๊ณ ์น 2๊ฑด ํฌํจ)์ด ๊นจ์ง๋ค. ์ด๋ฒ ์์ ์ ฮฒ ๊ณ ์ , T๋ง์ด๋ค. W_u๋ฅผ ์์๋ก ์ ํ์ง ๋ง ๊ฒ. ํ์ฌ ์ฝ๋์ ํ ๊ฐ์ค์ด ์๋ค๋ฉด ๋จ์ํ๋ ฌ๋ก ๋๊ณ ๊ทธ ์ฌ์ค์ ๊ฒฐ๊ณผ์ ๋ช ์ํ๋ค. ์์ ๊ฐ์ค์ T*๋ฅผ ์ํ๋ ๊ฐ์ผ๋ก ๋ง๋ค ์ ์์ด ํ์ ์ด ๋ฌด์๋ฏธํด์ง๋ค.- โช์ด ๋ฏธํ์ ์ธ ์ฑ๋ก ์งํํ์ง ๋ง ๊ฒ.
โฅ ์ ์ฝ ๊ณต๊ฐ ์ ํฉ โ ํ ์๋ ์ํ์ด ์ค๋ฆฐ๋ ์๋ ์ํ์ ๋๋ ๊ตฌ๊ฐ ์ฐพ๊ธฐ
ํ์ฌ ์ํ์ Cartesian ํ ์๋์ธ๋ฐ, (10d) ์ ๋ ์ ์ฝ๊ณผ ๋ ผ๋ฌธ ์(8)์ ์๋ ์ ์ฝ์ ์ค๋ฆฐ๋ ์๋ qฬL์ด๋ค.
์ ์ฒด ๊ถค์ ์ ํ์ผ๋ฉฐ ๊ฐ ์์ ์์
qฬ_L = J_ฮธ(q_ฮธ) ยท qฬ_ฮธ (๋๋ ํ/ํผํ
์์ ์ป์ dL/dฮธ ์ ์ฉ)
๋ฅผ ๊ณ์ฐํ๊ณ ์๋๋ฅผ ์ถ๋ ฅ:
- max |qฬ_L| ์ ์ถ๋ณ๋ก โ ์ค๋ฆฐ๋ ์๋ ์ํ ๋๋น ๋ช %์ธ๊ฐ
- ฮฃ abs(qฬ_L) ์ ์๊ณ์ด โ (10d) ํํ ์ ๋ ํ๊ณ๋ฅผ ๋๋ ๊ตฌ๊ฐ ๋น์จ
- ์๋ฐ ๊ตฌ๊ฐ์ ์์ธ(ฮธ_B, ฮธ_A, ฮธ_K) ๊ธฐ๋ก โ ์ด๋ค ์์ธ์์ ํฐ์ง๋๊ฐ
ํ์ : ์๋ฐ ๊ตฌ๊ฐ์ด 0%๋ฉด ํ ์๋ ์ํ์ด ์ถฉ๋ถํ ๋ณด์์ ์ด๋ผ๋ ๋ป์ด๋ฏ๋ก ์ด ๊ฒฉ์ฐจ๋ ๋ซ๋๋ค. 0%๊ฐ ์๋๋ฉด, ์ ์ญ ๊ธฐ์ค๊ถค์ ์ด ์ค๋ฆฐ๋ ๊ณต๊ฐ์์ ์ด๋ฏธ ์คํ ๋ถ๊ฐ๋ฅํ๋ค๋ ๋ป์ด๊ณ โ ๊ตญ์ MPC๊ฐ ๋งค ์คํ ๋ฐ์ด๋ด๋ฉฐ ๋ฐ๋ผ๊ฐ๋ ์ค โ ยง3.4 โ24% ๊ฒฝ๊ณ โ์ ์์ธ ํ๋ณด๋ก ํ์ (๊ธฐ์กด ํ๋ณด์ธ โ๋ฏธ๋ณด์ ํ ํ๊ณ ์์นโ์ ๋ณ๊ธฐ).
โฆ ์ ํฌ(ยง5 ํ๊ณ โค)์ ์์ธ ํ๋ณ โ ์ ์คํ ๋ถํ์, ๊ธฐ์กด ๋ก๊ทธ ์ฌ๋ถ์
โค ๋์ปน๊ฑฐ๋ฆผ์ ์์ธ์ผ๋ก โฃ MPC horizon ๋จ์ํ๋ฅผ 1์์๋ก ์ ์ด๋์์ผ๋, ๊ฒฝ์ ๊ฐ์ค์ด ์๊ฒผ๋ค: 0.5/0.25/0.5 ๊ตฌ๊ฐ๋ณ ์์ ์๋์ ๊ฒฝ๊ณ ๋ถ์ฐ์.
๊ธฐ์กด ์ฃผํ ๋ก๊ทธ์์ ์ ํฌ(ํ ๊ฐ์๋์ ์๊ฐ ๋ฏธ๋ถ ๋๋ qฬ_L ๋ณํ์จ)๋ฅผ ๋ฝ๊ณ ,
phase ์ ํ ์๊ฐ์ ๋ง์ปค๋ฅผ ์ฐ์ด ๋ถํฌ๋ฅผ ๋ณธ๋ค.
โ ์ ํฌ ์คํ์ดํฌ๊ฐ phase ์ ํ ์์ ์ ๊ตญ์์ ์ผ๋ก ๋ชฐ๋ฆผ
โ โ ์์ธ = ์๋ ์ค์ผ์ค ๋ถ์ฐ์.
โ ์กฐ์น: ๊ฒฝ๊ณ์์ ์๋๊ฐ ์ด์ด์ง๋๋ก ์๊ฐ ๋ฐฐ๋ถ์ Cยน ์ฐ์ ์กฐ๊ฑด ์ถ๊ฐ
โ (๋
ผ๋ฌธ์ "boundary values chosen ... smooth" ์ ํด๋น)
โ
โ ์ ํฌ๊ฐ ์ฃผํ ๋ด๋ด ๊ณ ๋ฅด๊ฒ ๊น๋ฆผ
โ ์์ธ = โฃ MPC horizon. ๊ธฐ์กด 1์์ ์ ์ง
ํ์ ๊ธฐ์ค: phase ์ ํ ยฑ0.2์ด ๊ตฌ๊ฐ์ด ์ ์ฒด ์ ํฌ ์๋์ง์ 50% ์ด์์ ์ฐจ์งํ๋ฉด ์๋ ์ค์ผ์ค์ด ์ฃผ์์ธ.
์์ ์์์ ๋น์ฉ
| ์์ | ํญ๋ชฉ | ๋น์ฉ | ์ ํ ์กฐ๊ฑด |
|---|---|---|---|
| 1 | โช ์ฝ๋ ํ์ธ | 5๋ถ | โ |
| 2 | โฆ ์ ํฌ ํ๋ณ | 30๋ถ (๊ธฐ์กด ๋ก๊ทธ) | โช |
| 3 | โฅ ์ ์ฝ ๊ณต๊ฐ ์ ๊ฒ | ๋ฐ๋์ | โช, ยง6.1-โ (JL ์ถ์ฒ) |
| 4 | โค T* ๊ณ์ฐ | ๋ฐ๋์ | โช |
โฆ์ ๋จผ์ ๋๋ ์ด์ : ์ ๋ฐ์ดํฐ๊ฐ ํ์ ์๊ณ , ๊ฒฐ๊ณผ๊ฐ ยง5 ํ๊ณ โค์ 1์์ ํ๋ณด๋ฅผ ๋ฐ๊ฟ ์ ์์ด ๋ค๋ฅธ ๊ฐ์ ์์ ์ ์ฐ์ ์์ ์์ฒด๋ฅผ ์ฌ๋ฐฐ์นํ๊ธฐ ๋๋ฌธ์ด๋ค.
APEX๋ ํ์ผ๋ก ๋ฏธ๋ ๋ชจํฐ๊ฐ ์๋๋ผ ์๋ ๋ชจํฐ๋ฅผ ์ด๋ค(ยง5 ์ฐ๋ฆฌ๋ง์ ํ๊ณ โก). ๋ฐ๋ผ์ ์(8)์ ํ ๋น์ฉ โซuTWuu dt๋ฅผ ์ค์ด๋ ๊ฒ์ด ๊ณง๋ฐ๋ก ์ ์ด๊ธฐ ์ฑ๋ฅ์ผ๋ก ํ์ฐ๋์ง๋ ์๋๋ค. T* ๋์
์ ์ค์ต์ ์๋์ง ์ ๊ฐ์ด ์๋๋ผ ์ต๋ ํ ์ฌ์ ํ๋ณด์ ์ ์ฝ ์๋ฐ ๊ฐ์์ ์๋ค๊ณ ๋ณด๊ณ ํ์ ํ ๊ฒ.
ํ๋ง๋๋ก ์ ๋ฆฌ
- Global + Local planner ํตํฉ ํ๋ ์์ํฌ๋ก ์ ์ ๊ตด์ฐฉ๊ธฐ์ ์ค์๊ฐ ๊ถค์ ๊ณํ ๊ตฌํ
- Force/torque, power, cylinder displacement, flow rate ๋ฑ 4๊ฐ์ง ๋ฌผ๋ฆฌ์ ์ ์ฝ์กฐ๊ฑด ๋์ ์ถฉ์กฑ ๋ฌ์ฑ
- Feedback linearization์ผ๋ก ๋น์ ํ MPC๋ฅผ ์ ํํํ์ฌ 30Hz ์ด์ ์ค์๊ฐ ์ฐ์ฐ ๋ฌ์ฑ
- ๋ชจ๋ฉํ ๊ธฐ๋ฐ ์ธ๋ ์ถ์ ์ผ๋ก ๋ง์ฐฐ ๋ฐ ํ ์ ์ํธ์์ฉ disturbance-aware planning ๊ตฌํ
- Shallow/deep excavation ๋ ์๋๋ฆฌ์ค์์ ์ ์ฝ ๋ง์กฑ + ์ ์ญ ๊ถค์ ์ถ์ข ๊ฒ์ฆ ์๋ฃ
- ์ค์ ๊ตด์ฐฉ๊ธฐ ์คํ ๋ฏธ์ํ โ ์๋ฎฌ๋ ์ด์ ๋ง ๊ฒ์ฆ
์ ๋ณ ํ์ ํ๋์:
- ๐ข ์ถฉ์ค: ์ด๋ํ(์ค์ธก ํ) ยท ๊ตด์ฐฉ ์ต์ ํ(๋ฒํท ๊ฐ๋ ํฌํจ, Vcap ํญ๋ง ๋ฏธ๊ตฌํ) ยท ํ๋ฆผ ๋ฐฉ์ง ยท ์ง๋ฉด ์๋ ์์ํ
- ๐ก ๋ถ๋ถ/๋จ์ํ: ๋ฌด๊ฒยท๊ด์ฑ ๋ชจ๋ธ(์์น ๋ฏธ๋ณด์ ) ยท ์ง์ /์ธ์ ์๊ฐ๋ฐฐ๋ถ ยท ํ์/์ ๋ ๋ฐฐ๋ถ ๋ฐฉ์ ยท โ๊ฐ์๋ ํ๋โ ๋จ์ํ ยท ๋ณด์ ํ(์๋ ํ์ฐ ์ ์ฉ) ยท ํ ์ ํญ ์ถ์ (์ํ ํฌํ)
- ๐ด ๋ค๋ฆ/์์: ์๋ฒ ์ข ๋ฅ(DDPโIPOPT) ยท ํ์ (์ค์) ๊ณํ ๋ฐฉ์ ยท ํ์ผ๋ก ๋ฏธ๋ ๋ชจํฐ ์ฑ๋
์ฝ๋ ๋ฒ: ์ ๋ง๋ค โ๋
ผ๋ฌธ ๋๋น ์ด ์ ๋๋ฉด ์ถฉ๋ถํ๊ฐโ๋ฅผ ํ์ ํด ์ฃผ์๋ฉด, ๋ฏธํก ํ์ ์ ์ด ๊ทธ๋๋ก ๋ค์ ์์
๋ชฉ๋ก์ด ๋ฉ๋๋ค. ์์น ๊ทผ๊ฑฐยท๊ฒ์ฆ ์ด๋ ฅ์ด ๊ถ๊ธํ ์ ์ mpc_paper_mapping.md์ ๊ฒ์ดํธ ํ์ ์์ธ๊ฐ ์์ต๋๋ค.