PLC 경로충돌방지 알고리즘 2편: 비대칭 마진 최적화와 Follow 추종 제어

이전 시리즈 1편: 이동 영역(Band)으로 충돌 예측

 

 

1편에서는 밴드(Band) 개념으로 충돌을 예측하는 기본 알고리즘을 구현했습니다.

이번 편은 그 알고리즘을 실제 설비에 올릴 때 부딪히는 두 가지 현실 문제를 다룹니다.

"마진을 하나만 쓰면 비대칭 로봇에서 손해 아닌가?"
"충돌이 예측되면 무조건 멈춰야 하나? 따라갈 수는 없나?"

앞 질문은 마진을 앞/뒤로 분리해서, 뒤 질문은 Follow(추종) 제어로 풉니다.

그리고 1편의 방향 분기 방식에 숨어 있던 함정 하나를 MIN/MAX 통합식으로 정리합니다.

1. 1편 복습: Band는 결국 MIN/MAX 한 쌍

1편에서 Band는 현재 위치와 목표 위치에 마진을 더한 구간이었습니다.

이동 방향과 관계없이, 두 위치 중 작은 쪽에서 마진을 빼고 큰 쪽에 마진을 더하면 그게 Band입니다.

 

Band_MIN = MIN(현재, 목표) − Margin
Band_MAX = MAX(현재, 목표) + Margin

 

여기서 중요한 포인트가 있습니다.

이 식에는 "전진/후진" 같은 방향 판단이 들어가지 않습니다.

MIN/MAX가 현재와 목표 중 어느 쪽이 큰지 알아서 가려주기 때문입니다.

이 성질이 2편 전체의 토대가 됩니다.

2. 단일 Margin의 한계: 로봇은 대칭이 아니다

단일 Margin이 정확하려면 로봇 몸체가 진행 방향 앞뒤로 대칭이어야 합니다.

현실의 헤드는 대부분 비대칭입니다.

 

 

예를 들어 그리퍼가 앞쪽으로 30mm 튀어나온 헤드라면, 실제로 필요한 여유는 앞뒤가 다릅니다.

 

뒤쪽 여유(Back Margin)  = 20mm
앞쪽 여유(Front Margin) = 50mm   ← 그리퍼 크기만큼 더 필요

 

이때 마진을 하나만 쓰면, 안전을 위해 큰 값(50)을 잡을 수밖에 없습니다.

뒤쪽은 20이면 충분한데 50을 잡으니 Band가 실제보다 30mm 넓어집니다.

Band가 넓어지면 실제로는 안전한데도 충돌로 판정되어 기동이 막히는 경우가 생깁니다.

헤드 여러 개가 좁은 구간에서 동시에 일할수록 이 손해가 커집니다.

3. 해결: Back/Front Margin 분리 (MIN/MAX 통합식)

앞쪽 여유와 뒤쪽 여유를 따로 적용합니다.

여기서 1편의 방향 분기 방식 대신 MIN/MAX 통합식을 권장합니다.

 

Band_MIN = MIN(현재, 목표) − Back Margin
Band_MAX = MAX(현재, 목표) + Front Margin

 

뒤쪽(작은 위치 쪽)에는 Back Margin, 앞쪽(큰 위치 쪽)에는 Front Margin이 붙습니다.

MIN/MAX가 방향을 알아서 가려주므로, 전진이든 후진이든 같은 두 줄로 끝납니다.

수치 예시 (BM=20, FM=50):

방향	현재	목표	tmpMin	tmpMax	Band_MIN	Band_MAX
+ 전진	0	120	0	120	0−20 = −20	120+50 = 170
− 후진	120	40	40	120	40−20 = 20	120+50 = 170
0 제자리	60	60	60	60	60−20 = 40	60+50 = 110

⚠️ 주의

왜 방향 분기가 아니라 통합식인가 — 제자리(편차 0) 함정
마진을 "전진이면 이렇게, 후진이면 저렇게" 방향 비트로 분기하면, 현재=목표인 제자리·정지 상태에서 전진/후진 비트가 모두 OFF가 됩니다.
그 순간 Band 계산식이 실행되지 않아 직전 스캔의 Band 값이 그대로 남고(stale), 충돌 판정이 옛 값으로 오작동할 수 있습니다.
MIN/MAX 통합식은 방향 비트에 의존하지 않으므로 제자리에서도 매 스캔 Band가 정상 갱신됩니다.

💡 TIP

BM = FM 이면 1편 단일 Margin과 결과가 완전히 동일합니다.
대칭 로봇이라면 두 마진에 같은 값을 넣으면 됩니다. 즉 이 방식은 1편의 상위호환입니다.

4. PLC 래더 구현: DMIN / DMAX

통합식은 미쓰비시 DMIN(블록 최소)·DMAX(블록 최대) 명령으로 두 위치(현재·목표)의 최소/최대를 한 번에 뽑아 구현합니다.

 

 

| SM400 |--[ DMIN  D49000  D49008  K2 ]   ; tmpMin = MIN(글로벌현재 D49000, 글로벌목표 D49002)
| SM400 |--[ DMAX  D49000  D49012  K2 ]   ; tmpMax = MAX(글로벌현재, 글로벌목표)
| SM400 |--[ D-    D49008  ZR49004  D49004 ]  ; Band_MIN = tmpMin − Back Margin
| SM400 |--[ D+    D49012  ZR49006  D49006 ]  ; Band_MAX = tmpMax + Front Margin

 

DMIN D49000 ... K2는 D49000(글로벌현재)·D49002(글로벌목표) 두 더블워드 블록에서 최소를 찾아 tmpMin에 저장합니다.

방향 분기 없이 4줄로 끝나고, 1편의 중간 레지스터 4개(현재±M, 목표±M)도 사라집니다.

⚠️ 주의

DMIN / DMAX는 결과가 4 word를 점유합니다 — 임시워드 간격 주의
더블워드 블록 검색 명령(DMIN/DMAX)의 결과는 d,d+1(최소/최대값, 32비트), d+2(값의 위치), d+3(값의 개수)로 4 word를 차지합니다.
즉 tmpMin을 D49008에 두면 D49008~D49011을, tmpMax를 D49012에 두면 D49012~D49015를 점유합니다.
이 점유 범위를 모르고 임시워드를 2 word 간격으로 붙여 두면, 위치/개수 word가 옆 디바이스(예: 방향 편차 버퍼)를 덮어써 엉뚱한 판정이 나옵니다.
임시워드는 반드시 4 word 간격을 띄우세요.

디바이스:

디바이스	역할	점유
D49000 / D49002	글로벌 현재 / 목표	각 2 word
D49004 / D49006	Band_MIN / Band_MAX	각 2 word
D49008	tmpMin (DMIN 결과)	4 word (49008~49011)
D49012	tmpMax (DMAX 결과)	4 word (49012~49015)
ZR49004 / ZR49006	Back Margin / Front Margin	각 2 word

5. 다축 디바이스 설계: 축당 영역을 미리 벌려둔다

헤드가 2개일 때는 주소를 대충 잡아도 됩니다.

하지만 16축처럼 늘어나면, 축마다 같은 구조의 워크 영역이 반복되므로 축당 고정 간격으로 영역을 설계해 두는 편이 관리가 쉽습니다.

이 글의 예시는 충돌방지 워크를 D49000 / ZR49000을 선두로, 축당 +20 word 간격으로 배치했습니다.

항목	Ax#1	Ax#2	…	Ax#16
글로벌 현재 / 목표	D49000 / D49002	D49020 / D49022	…	D49300 / D49302
Band_MIN / MAX	D49004 / D49006	D49024 / D49026	…	D49304 / D49306
tmpMin / tmpMax	D49008 / D49012	D49028 / D49032	…	D49308 / D49312
방향 / Follow	ZR49000 / ZR49001	ZR49020 / ZR49021	…	—
오프셋 / BM / FM	ZR49002 / ZR49004 / ZR49006	ZR49022 / ZR49024 / ZR49026	…	—

💡 TIP

축당 간격을 정할 때는 4 word를 점유하는 tmpMin/tmpMax까지 합쳐 한 축이 실제로 쓰는 word 수를 세고, 거기에 여유를 더해 잡으세요.
이 예시는 한 축이 18 word를 쓰므로 +20 간격에 2 word 여유를 남겼습니다.

6. 글로벌 좌표 오프셋 자동 캘리브레이션: 티칭으로 해결

다축을 하나의 좌표계로 묶으려면, 각 헤드의 로컬 좌표(서보 엔코더 값)를 글로벌 좌표로 환산하는 오프셋이 필요합니다.

1편에서는 이 오프셋을 사람이 측정해 직접 넣었습니다.

하지만 헤드가 16개로 늘면 일일이 측정해 넣는 것도 일이고, 기구 재조정 때마다 다시 맞춰야 합니다.

이걸 두 헤드가 같은 물리점을 티칭하게 해서 자동으로 산출할 수 있습니다.

 

6-1. 원리: 같은 점은 글로벌 좌표가 같아야 한다

두 헤드가 같은 물리점 P를 각자 티칭하면, 각자의 로컬 좌표 L1*·L2*가 위치 테이블에 남습니다.

같은 점이므로 글로벌 좌표는 서로 같아야 합니다.

 

Off1 + s1·L1*  =  Off2 + s2·L2*
   s = 설치 방향 부호 ( +1 : Posi / −1 : Nega )

 

1번 헤드를 글로벌 기준으로 고정(Off1 = 0)하면, 2번 헤드의 오프셋이 바로 나옵니다.

 

Off2 = s1·L1* − s2·L2*

 

즉 두 티칭값의 차이가 곧 오프셋입니다.

티칭값은 운전 중 변하지 않으므로, 이 식을 매 스캔 돌려도 결과가 흔들리지 않습니다.

6-2. 설치 방향 4조합

두 헤드의 설치 방향(Posi/Nega) 조합에 따라 부호가 달라집니다.

1번(s1)	2번(s2)	Off2
Posi	Posi	L1* − L2*
Posi	Nega	L1* + L2*
Nega	Posi	−(L1* + L2*)
Nega	Nega	L2* − L1*

6-3. PLC 구현

공유 티칭점은 양 헤드 위치 테이블의 같은 번호(예: No.02)를 약속해서 씁니다.

 

 

; 1번 헤드 = 글로벌 기준 (Off1 = 0)
| 기준설정 |--[ DMOV  K0  ZR49002 ]                         ; Ax#1 오프셋 = 0

; 2번 헤드 = 4조합으로 자동 산출 (s1=ZR49000.0, s2=ZR49020.0)
| ZR49000.0/ |--| ZR49020.0/ |--[ D-  ZR40002  ZR40102  ZR49022 ]   ; Posi·Posi
| ZR49000.0/ |--| ZR49020.0  |--[ D+  ZR40002  ZR40102  ZR49022 ]   ; Posi·Nega
| ZR49000.0  |--| ZR49020.0/ |--[ D+  ZR40002  ZR40102  ZR49022 ]
                              --[ DNEG ZR49022 ]                    ; Nega·Posi → −(L1+L2)
| ZR49000.0  |--| ZR49020.0  |--[ D-  ZR40102  ZR40002  ZR49022 ]   ; Nega·Nega

 

ZR40002·ZR40102는 1·2번 헤드가 공유 약속한 티칭점(No.02)의 로컬 좌표, ZR49022는 2번 헤드의 글로벌 오프셋입니다.

💡 TIP

3축 이상은 체인으로 확장합니다.
인접한 두 헤드가 각각 공유 티칭점을 가지면, Off3 = Off2 + (2·3번 공유점 차이) 식으로 옆 헤드 기준 상대 오프셋을 이어 붙입니다.
충돌 판정은 인접 헤드끼리만 비교하므로, 멀리 떨어진 헤드까지 한 점을 공유할 필요가 없습니다.

7. 마진 값은 얼마로: MinMinVal 계산

마진을 너무 작게 잡으면 실제로 충돌하고, 너무 크게 잡으면 기동이 과하게 차단됩니다.

기준은 헤드 간 최소 간격입니다.

모든 헤드가 정지해 있을 때 인접 헤드 사이 간격을 구하고, 그중 가장 좁은 값(MinMinVal)의 절반을 마진 상한으로 봅니다.

 

 

MinMinVal      = MIN(간격_A-B, 간격_B-C, 간격_C-D)
Margin 권장 상한 = floor(MinMinVal / 2)
최솟값 제약     = Margin ≥ 10   (물리적 충돌 방지 하한)

 

이 값은 시운전 초기 1회 또는 헤드 구성이 바뀔 때 산출해서 ZR49004(BM)·ZR49006(FM)에 반영합니다.

운전 중 매 스캔 재계산은 스캔타임 부담이 있으니 별도 트리거로 관리하는 것을 권장합니다.

8. Follow 추종 제어: 멈추지 말고 따라간다

충돌이 예측되면(NG) 무조건 정지하는 대신, 상대 Band 경계 직전까지만 부분 이동시키는 것이 Follow 제어입니다.

앞 헤드가 비켜주는 만큼 따라 들어가므로, 좁은 구간에서 작업 효율이 올라갑니다.

 

 

핵심은 밀어내는 방향의 마진을 쓴다는 점입니다.

 

[+ 전진: 내 앞쪽(Front)이 상대 뒤쪽에 닿음]
새 목표 = 상대 Band_MIN − Front Margin − 여유

[− 후진: 내 뒤쪽(Back)이 상대 앞쪽에 닿음]
새 목표 = 상대 Band_MAX + Back Margin + 여유

 

전진은 Front Margin, 후진은 Back Margin을 씁니다.

왜 이렇게 짝을 맞춰야 할까요?

부분 이동 후 그 자리에서 다시 충돌 판정을 하기 때문입니다.

이동 후 내 Band는 다시 목표 + Front Margin(앞)·목표 − Back Margin(뒤)으로 계산됩니다.

전진에서 새 목표를 상대 Band_MIN − Front Margin − 여유로 잡으면, 이동 후 내 Band_MAX가 정확히 상대 Band_MIN − 여유가 되어 항상 안전 거리(여유)만큼 떨어집니다.

만약 전진에 Back Margin을 쓰면 이 관계가 깨져서, Front가 Back보다 큰 비대칭 로봇에서는 여유값에 기대는 위태로운 안전이 됩니다.

 

 

| M40079 |--| ZR49001.0 |--                         ; NG AND Follow 모드
  --| M40071(++전진) |--[ D- 상대BandMin(D49024) ZR49006(Front) D40036 ]
                       [ D- K10000  D40036 ]          ; − 여유
  --| M40072(−−후진) |--[ D+ 상대BandMax(D49026) ZR49004(Back)  D40036 ]
                       [ D+ K10000  D40036 ]          ; + 여유
  ----[ D- D40036 ZR49002(offset) D40036 ]            ; 글로벌 → 로컬 역변환
  --[ MEP ]--[ CALL P0 ]                              ; 보정 직후 재판정 (같은 스캔에서 기동까지)

 

보정한 목표로 P0(충돌 판정 루틴)를 즉시 다시 호출하므로, 같은 스캔 안에서 "따라 들어갈 만큼만 이동 → 재판정 → 기동"까지 완결됩니다.

상대가 계속 움직이면 상대 Band도 매 스캔 갱신되므로 추종 목표가 자동으로 따라갑니다.

9. 실제 테스트용 프로그램 (GX Works2 + 프로페이스 HMI)

글에서 설명한 알고리즘(통합식 Band / BM·FM 분리 / 티칭 캘리브 / Follow)을 그대로 담은 테스트용 PLC 래더(GX Works2)와 프로페이스 HMI 화면 파일을 첨부합니다.

 

r05_Back-Front-Margin_260618.gxw

1.47MB

 

r05_Back-Front-Margin_260618.prx

0.44MB

 

실제 서보 없이도 HMI에서 가상으로 위치를 움직여 조그 기동 / 위치결정 기동 / 충돌 검출 / Follow 추종을 확인할 수 있습니다.

테스트 순서:

 

1. 각 축의 글로벌 좌표 오프셋과 설치 방향(Posi/Nega)을 설정합니다. (또는 §6 티칭 자동 산출 사용)
2. Back Margin / Front Margin을 로봇 사이즈에 맞춰 기입합니다. (대칭이면 BM=FM)
3. 조그로 헤드를 움직여 Band가 그려지는지 확인합니다.
4. 두 헤드를 마주보게 위치결정 기동시켜 충돌 검출(SAFE OFF)과 Follow 추종 동작을 확인합니다.

💡 TIP

본 프로그램은 16축 영역 설계(축당 +20)로 작성되어 있어, 헤드를 늘려가며 확장 테스트하기 좋습니다.

10. 정리: 단일 Margin vs BM/FM 분리 + Follow

항목	단일 Margin (1편)	BM/FM 분리 + Follow (2편)
마진 방식	단일	Back / Front 분리
계산 구조	MIN/MAX 통합	MIN/MAX 통합 (동일)
비대칭 로봇	과보수적 Band	정확한 Band
충돌 예측 시	정지	경계까지 추종(Follow)
동시 기동 허용률	낮음	최대화
추천 상황	개념 검증	실 설비 최적화

핵심 재요약

• Band는 방향 판단 없이 MIN/MAX 한 쌍으로 계산한다 (제자리에서도 안전)
• 비대칭 로봇은 Back/Front Margin을 분리하면 Band가 정확해진다
• BM = FM이면 1편 단일 Margin과 완전히 동일하다
• DMIN/DMAX는 결과가 4 word를 점유하므로 임시워드 간격을 벌린다
• Follow는 밀어내는 방향의 마진(전진=Front, 후진=Back)을 써야 재판정이 안전하다

 

다음 편(3편: 데드락과 양보(Yield) 제어)에서는 Follow로도 풀 수 없는 마주보는 교착(데드락) 을 한 축의 양보(park)로 푸는 방법을 다룹니다.

테스트해보시고 문제점이 있다면 댓글로 피드백 주시면 감사드리겠습니다. ^^