[Project][Conveyor Tracking] 4. Conveyor Motion Algorithm

순서

I. Conveyor Tracking Motion 방식
II. 가속 Algorithm
III. 등속 Algorithm
IV. 감속 Algorithm

 

---

I. Conveyor Tracking Motion 방식

 

통상적으로 Motion의 순서 방식은 회사마다 두 가지(??) 로 나뉜다.
(Conveyor Tracking의 전제는 직선 운동이므로 Cartesian Linear Motion이다)

1. Tracking 조건이 있는 경우

Tracking 조건이 있는 경우는 아래와 같다. KEBA, Yaskawa 같은 산업용 로봇에서는

1. Object를 따라잡을 Upstream위치
2. Object를 놓아줄 Downstream위치
3. 로봇의 한계를 정하는 min, max position

이러한 조건을 사용자가 세팅하게 된다. 

 

2. Tracking 조건이 없는 경우

Universal Robot(UR) 협동로봇의 Conveyor Tracking의 경우는 자잘한 세팅이 없다.

그냥 엔코더 타입, 엔코더 스펙만 설정하게 되어있다.

 

3. 1과 2의 결정적인 차이

Tracking 조건이 있고 없고의 차이는 아래와 같다.

"Tracking이 자동으로 정지되느냐, 정지되지 않느냐"

1의 경우 Object를 놓으면서 자동으로 컨베이어 동작을 정지하고 돌아가지만
2의 경우 Linear의 한계(Singular)에 도달할 때 까지 컨베이어를 계속 따라간다.

기구적인 한계로 볼 때 2는 기구부에 타격이 있을 수 있다고 생각한다..
기구부에 무리가 가기 전에 제어로 잡아주면 되겠지만...

대신 UR의 경우 사용자가 아주 많은 Interface를 사용하여 Script상으로
직접 다 구현할 수 있다. 오히려 자유도는 높은 것 같다!!

 

나는 1의 경우를 구현했다.
(사실 알고리즘상 감속의 유무만 다르지 똑같다)

 

 

 

---

II. 가속 Algorithm

 

가속 Algorithm이란 로봇이 Object를 대기하며 정지하고 있다가 Object가 진입 시
가속하며 Conveyor 등속까지 속도를 따라잡는 Algorithm이다.

고려해야 할 점은, Object는 계속 등속으로 이동 중이고
로봇은 0부터 가속하기 때문에 발생하는 변위의 차이이고
이를 Overshooting speed로 보정 해 주어야 한다.

 

1. 초안

 

생각한 모델은 간단한 사다리꼴 Profile로 초기에 아래와 같았다.

사다리꼴 profile

아주 간단하다. 면적을 같게 만드는 최고 속도까지 도달했다가
컨베이어 속도로 떨어지면 된다.
(Red Line : Robot Speed)

비록 smooth한 궤적은 아니지만 초안으로 굉장히 쉬웠다.

 

2. 문제발생

 

하지만 테스트 시 Command Jump가 발생한다.
(last Target과 cur Target이 면밀이 주어지지 않고 튀어버린다)

초안의 데이터

 

3. 해결방안

 

위 Command Jump는 가속을 마치는 부분의 알고리즘에 문제가 있다고 생각하여
굉장히 많은 시행착오를 거치며 궤적 알고리즘을 수정 했지만 해결되지가 않았다.

근본적인 원인은 Conveyor의 Noise였다.

알고리즘 궤적을 위해서는 Conveyor 속도를 처음에 정의하고 궤적을 만들게 된다.
그래야 가속시간 및 이동거리를 계산하고 진행할 수 있었기 때문이다.

하지만 Noise로 계속 달라지는 Conveyor의 속도로 인한 누적오차가 최종적으로 Jump를 유발하였다.
(Encoder Count를 1차적으로 Filtering해도 꽤 남아있다)

따라서 처음에 정의된 Conveyor 속도로 궤적은 만들지만
실시간으로 계산된 Conveyor의 속도와의 오차를 보상하였다 내리도록 했다.

Conveyor Speed 오차가 보상된 궤적

따라서 가속 시작 ~ 가속 정지 까지 컨베이어의 속도변화를 잘 추적하여 가속이 문제없이 마무리 되었다.
또한 자연스럽게 등속 (실제로는 Noise가 있지만) Tracking으로 state 전환도 되는 모습이다.

 

4. 추가개선

 

하지만 사다리꼴 Profile의 문제는 가속 시작에 있다.
-> 속도 Profile이 미분 불가능이라 가속도가 무한대가 된다.
-> 하지만 가속의 마무리 부분에서는 Conveyor 속도 보상으로 인해 자연스럽게 Smoothing이 되었다.

따라서 3차삼수 Profile을 사용하도록 하였다.

시작부분만 Smooth하다면 마무리 부분에서는 자연스럽게 Shooth하게 보상이 될 것을 기대했다.

End Time 조건만 새로 지정 해주고 함수를 손으로 풀 수 있어서 쉬웠다.

 

그래프로보면Nnoise때문에 지렁이 같고 이상하긴 한데... ㅋㅋ 실제 로봇 구동 시
사다리꼴일 때 발생했던 문제들이 사라졌다.

 

---

III. 등속 Algorithm

 

가속 Algorith이 끝나고 Conveyor의 속도를 받아 그저 따라가준다. 아주 쉬움.

ConveyorRefTargetIncPose = ConveyorSpeed * SystemCycleTime;

으로 구현한다.

 

---

IV. 감속 Algorithm

 

자동으로 Object를 Release (Tracking end)를 하기 위해
Tracking 속도부터 감속 Algorithm을 넣어준다.

감속 Algorihtm 자체는 감속 시작 때, 정지할때 접하는 Profile을 만들면 된다.

3차 함수 써도 될 것 같은데 어쩌다보니 생각없이 4차 함수로 구현했다...ㅋㅋ

4차함수로 하니 당연히 이중근이 나온다.

감속에서 특히 신경 써야 할 점은 예외 상황 구조이다.

모든 실제 동작 상황이 이상적으로 감속하진 않고
Tracking 진행 중 에러가 발생 했을 때, 로봇 프로그램이 강제 종료 됐을 때 등
여러 예외처리가 필요한데

이를 이전 Conveyor System Flow의 Exceptional Stop으로 구현하여 
예외 상황에서 초기 감속 조건을 다르게 설정하며 구현하였다. (알고리즘은 동일)

 

---

이렇게 각 State에서 다음 System Step에서 이동할
Conveyor 좌표기준 Increasement를 구하면

이 컨베이어 모션은 단순히 한 방향으로 Conveyor만 따라가는 것이고,
기능을 위해서는 추가로 로봇 Motion의 궤적과 같이 합성 해 주어야 한다.

그래야 Tracking을 진행하며 물건을 집기도하고, 작업도하고 할 수 있다.