비홀로노믹 모바일 매니퓰레이터의 영공간 투영에 기반한 충돌 회피

3.1 기존 방식의 반발력

물체 간의 반발력을 생성하기 위해서는 대상 물체 간에 최소 거리를 갖는 두 점의 좌표가 필요하다. 예를 들어, 문 열기 작업을 할 때 두 점을 P₁과 P₂라고 하면, P₁과 P₂는 각각 모바일 플랫폼과 문에서 서로와 가장 가까운 두 점이다. 기존 방식의 반발력을 생성하기 위해 두 점의 좌표를 구하는 방법은 연구마다 상이하지만, GJK 알고리즘을 사용하면 연산량이 적어 다른 방법에 비해 실시간 제어가 용이하다는 장점이 있다. 따라서 본 연구에서는 두 점의 좌표를 구하기 위해서 GJK 알고리즘^[8]을 활용한다. 이 알고리즘은 3D 모델 정보를 기반으로 두 물체의 좌표 정표를 받아오고, 민코스키 차를 이용하여 두 물체 간의 가장 가까운 점의 좌표를 구한다.

먼저 두 점의 최소 제곱 놈을 통해 물체와 모바일 플랫폼 간의 거리를 구할 수 있다. 기존 방식의 충돌 회피 방법^[2]에서는 두 점 간의 거리를 기반으로 반발력의 크기를 다음과 같이 정의한다.

여기서 d는 충돌 대상 간의 최소 거리, d_th는 충돌 위험이 있다고 판단하는 임계 거리, f_r는 충돌 대상 간의 반발력의 크기이다. 또한 k_r는 반발력 크기를 결정하는 이득인데 실험적으로 적절한 값을 설정하고, 본 연구의 실험에서는 값을 1으로 정하였다.

기존 방식의 충돌 회피 방법에서는 가장 가까운 두 점의 벡터 차를 통해 반발력의 방향을 구할 수 있다. 여기서 매니퓰레이터 링크 말단에 반발력이 적용되므로 기준 좌표계로 [Fig. 1]의 매니퓰레이터 좌표계 {A}로 사용한다. 또한, 모바일 플랫폼이 z축으로는 움직일 수 없으므로, x축과 y축에 대한 충돌 회피만을 고려한다. 그러므로 반발력 벡터는 다음과 같이 주어진다.

여기서 F_r은 충돌 대상 간의 반발력 벡터, f_rx와 f_ry는 각각 {A}에 기준한 x축및 y축 방향의 반발력이다.

3.2 비홀로노믹 제약 조건을 고려한 반발력

앞서 기존 방식의 충돌 회피를 위한 반발력에 대해서 설명하였다. 그러나 기존 방식의 반발력을 사용하면 두 가지 문제점이 발생한다.

먼저, [Fig. 2(a)]와 같이 최댓값이 정해져 있어 충돌을 회피하기에 반발력이 부족한 경우에 충돌 회피에 실패할 수 있다. 이를 방지하기 위해 반발력의 크기를 다음과 같이 정의한다^[9].

[Fig. 2]

Repulsive force by (a) the conventional method[2] and (b) the modified method[9]

위 식을 반발력으로 사용하면 [Fig. 2(b)]와 같이 거리가 줄어듦에 따라 반발력이 급격히 증가하고, 극단적으로 0이 되면 반발력이 무한대로 가기 때문에 반발력이 부족하여 충돌 회피에 실패하는 경우가 없게 된다.

또 다른 문제점은, 기존 방식의 반발력은 모바일 플랫폼의 비홀로노믹 제약 조건을 극복하지 못한다는 점이 있다. 예를 들어, [Fig. 3(a)]와 같이 로봇 말단이 그리퍼를 통해 물체를 파지하여 시작점 S에서 목표점 G로 점선을 따라 이동시키는 상황을 가정해 보자. 충돌 방지를 위해서는 적절한 반발력을 발생시켜야 하는데, 기존의 방법에서는 모바일 플랫폼과 물체 간에 가장 가까운 점 P₁과 P₂의 연장선 방향으로 반발력의 방향을 설정하고, 식 (4) 또는 (6)에서 구한 크기의 반발력을 발생시켰다. 물체가 모바일 플랫폼과 가까워지면 반발력이 커지고, 이 반발력이 모바일 플랫폼의 순간 속도에 반영되어 플랫폼이 이동함으로써 물체와의 충돌을 회피하게 된다. 그러나 이륜 형태의 차동 구동형 모바일 플랫폼은 [Fig. 3(a)]에서 x축 방향으로만 순간 속도를 가질 수 있다. 그러므로 그림에서 반발력 F_r의 x축 성분은 x축 속도에 반영되지만, y축 성분은 y축 속도에 전혀 반영되지 못한다. 만약 제자리 회전을 한다면 y축 방향의 순간 속도가 발생할 수는 있지만, 플랫폼의 원점은 동일하므로 충돌 대상과 멀어지지는 못한다. 결과적으로, 기존의 방법에서는 y축 방향 반발력이 순간 속도에 제대로 반영되지 못해 충돌 회피에 실패할 가능성이 크다. 여기서 x축 방향 반발력이 적용되어 충돌을 회피할 수도 있지만, 충돌 대상이 모바일 플랫폼의 측면에 있을 경우 x축 방향 반발력의 크기가 매우 작거나 0이므로 대부분의 경우 충돌 회피에 실패한다. 이러한 문제를 해결하기 위해서, 본 연구에서는 반발력의 크기와 방향을 구하는 새로운 알고리즘인 CANS (Collision Avoidance based on Null Space projection)을 다음과 같이 제안한다.

[Fig. 3]

Repulsive force based on CANS (a) for general collision avoidance and (b) self-collision avoidance

CANS에서는 반발력을 구할 때 식 (5)와 같은 기존 방식의 반발력 F_r에 x축 방향으로 가상의 반발력 f_v를 추가한다. 이 경우에 물체가 모바일 플랫폼으로 접근함에 따라 모바일 플랫폼이 x축 방향으로 큰 속도를 가지게 되고, 충돌을 회피하게 된다. 이때 가상의(virtual) 반발력 크기는 P₁과 P₂ 간의 거리가 가까울수록 커져야 충돌의 위험이 있을 때 빠른 속도로 회피할 수 있다. 또한 F_r에서 x축 성분의 비중이 크면, 가상의 반발력이 작아도 모바일 플랫폼의 x축 방향의 순간 속도가 충분히 커서 충돌을 피할 수 있다. 그러나 y축 성분의 비중이 크면, 모바일 플랫폼의 순간 속도에 반발력이 제대로 반영되지 못하므로 가상의 반발력이 커야 한다. 따라서 다음 식과 같이 거리 기반의 반발력 크기와 F_r에서 y축 성분의 비중에 비례하게 가상의 반발력 크기를 정한다. 결과적으로, 가상 반발력의 크기 f_v는 f_ry와 크기가 같다.

가상의 반발력을 위 식과 같이 구하고, 기존의 반발력에 더하면, [Fig. 3(a)]와 같이 반발력의 방향이 변형된 반발력 $$ {\mathit{F}}_r^{\mathit{\text{'}}}$$방향으로 변경된다.

[Fig. 3]의 변형된 반발력을 수식으로 표현하면 다음과 같다.

where

여기서 f'_rx와 f'_ry는 각각 가상의 반발력을 추가한 x축과 y축 방향의 반발력, F'_r은 가상의 반발력을 추가한 반발력 벡터, x_s와 x_g는 각각 [Fig. 4]에서 좌표계 {P}에 기준한 로봇 말단의 x축 시작점(S)와 x축 목표점(G)이다. 가상의 반발력을 더하는 방향은 x_g를 기준으로 정한다. [Fig. 4(a)]와 같이 x_g가 0보다 작으면 a가 b보다 작으므로, 모바일 플랫폼이 x축 양의 방향으로 회피하는 것이 효율적이다. 따라서 가상의 반발력을 x축 음의 방향으로 설정하여 모바일 플랫폼이 x축 양의 방향으로 순간 속도를 가지도록 한다. 반면에, [Fig. 4(b)]와 같이 x_g가 0보다 크면 b가 a보다 작으므로, 모바일 플랫폼이 x축 음의 방향으로 회피하는 것이 효율적이다. 따라서 가상의 반발력을 x축 양의 방향으로 설정하여 모바일 플랫폼이 x축 음의 방향으로 순간 속도를 가지도록 한다.

[Fig. 4]

Direction of virtual repulsive force for various cases

그러나 로봇 말단이 x축 기준으로 플랫폼을 벗어나 있는 경우에는 다음과 같이 처리한다. 우선 [Fig. 4(c)]와 같이 x_s가 0.5L보다 크고 x_g가 0보다 작은 경우, 모바일 플랫폼의 순간 속도는 x축 양의 방향이고 말단의 경로는 x축 음의 방향이므로 충돌의 위험이 있다. 따라서 이 경우에는 가상의 반발력을 x축 양의 방향으로 설정한다. 또한 [Fig. 4(d)]와 같이 x_s가 -0.5L보다 작고 x_g가 0보다 큰 경우, 모바일 플랫폼의 순간 속도는 x축 음의 방향이고 말단의 경로는 x축 양의 방향이므로 충돌의 위험이 있다. 따라서 이 경우에는 가상의 반발력을 x축 음의 방향으로 설정한다. 이때 목표점 G는 플랫폼에 고정된 점이 아니라, 플랫폼에 상관없이 공간상의 점임에 유의한다.

다음으로, 식 (8)에서 구한 반발력을 매니퓰레이터에 적용시키기 위해서 다음과 같이 반발력을 자코비안을 통해 매니퓰레이터의 각 축에 대한 토크로 변환하고, 여기에 변환 인자를 곱하여 반발 속도로 변환한다.

여기서 J_a는 매니퓰레이터 암의 자코비안, τ_r은 반발력에 대응되는 반발 토크 벡터, $$ {\dot{\mathit{q}}}_r$$은 반발력에 대응하는 매니퓰레이터 각 축의 반발 속도, k_t는 반발 토크를 반발 속도로 변환하여 주는 인자이다. 본 연구에서는 식 (6)의 k_r을 통해 반발력의 크기를 조정하므로 k_t는 1을 사용한다. [Fig. 3(a)]와 같이 물체와 모바일 플랫폼의 충돌은 매니퓰레이터 말단과 물체를 하나의 강체로 보고 충돌을 회피하기 때문에 J_a를 사용하여 매니퓰레이터 말단에 반발력을 적용한다. 반발력을 반발 속도로 변환하여 식 (3)의 b에 대입하면, 말단 궤적을 유지하면서 모바일 플랫폼과 물체와의 충돌을 회피하도록 반발력이 적용된다.

한편, [Fig. 3(b)]와 같이 매니퓰레이터와 모바일 플랫폼 사이의 충돌을 회피하도록 적용하면 CANS 알고리즘을 자가 충돌 회피에도 확장하여 적용할 수 있다. 매니퓰레이터와 모바일 플랫폼 사이의 가장 가까운 두 점 P₁과 P₂를 구하고, 반발력의 방향과 크기는 앞서 설명한 방법과 동일하게 구한다. 그러나 반발력을 적용하는 대상을 로봇 말단으로 선정하면 P₁과 P₂가 아닌 P₁과 P₃이 멀어지도록 반발력이 발생된다. 따라서 반발력이 적용되는 대상을 P₂가 포함된 링크의 말단으로 선정해야 P₁과 P₂가 멀어지도록 반발력이 발생된다. 따라서 다음 식과 같이 자코비안 역시 J_a가 아닌 P₂가 포함된 링크까지의 자코비안을 적용한다.

여기서 i는 모바일 플랫폼과 가장 가까운 매니퓰레이터 링크 번호, J_i는 링크 i까지의 자코비안이다. 위 식과 같이 반발력을 반발 속도로 변환하여 식 (3)의 b에 대입하면, 말단 궤적을 유지하면서 모바일 플랫폼과 매니퓰레이터의 자가 충돌을 회피하도록 반발력이 적용된다.

전체 알고리즘에 대한 이해를 돕기 위해 다음과 같이 수치를 포함한 예시를 들어 설명한다. 매니퓰레이터가 [Fig. 3(b)]와 같이 q ={45°, -150°, -110°, -20°, 80°, 0°}인 자세에 있는 순간의 자가 충돌 회피에 대해서 생각해 보자. 먼저 GJK 알고리즘을 통해 거리 d = 0.25 m를 구한다. 다음으로, 식 (5)과 (9)에 의해 f'_rx = 1.61, f'_ry = -1.05를 구한다. f'_rx가 양수이므로 반발력의 방향은 x축 양의 방향이다. 다음으로, 반발력을 반발 속도로 변환하여 영공간에 반영하면 식 (3)의 영공간 속도$$ {\dot{\mathit{q}}}_{\mathrm{n}\mathrm{u}\mathrm{l}\mathrm{l}}$$ = {21°/s, -1.7°/s, 16°/s, -17°/s, 2.5°/s, 16°/s, -0.28 m/s, -5.1°/s}를 구할 수 있다. $$ {\dot{\mathit{q}}}_{\mathrm{n}\mathrm{u}\mathrm{l}\mathrm{l}}$$에서 모바일 플랫폼의 영공간 속도가 –0.28 m/s로 충돌을 회피하기 위해 x축 음의 방향으로 속도 성분을 가지게 된다. 위 예시처럼 실시간으로 반발 속도를 구하여 로봇 말단이 움직일 때 충돌을 회피한다.

4.1 자가 충돌 회피 실험

모바일 매니퓰레이터의 자가 충돌 회피 실험의 조건은 다음과 같다. [Fig. 6]의 영역 ① ~ ⑧ 내부의 무작위 위치에 로봇 말단을 위치시켜 모바일 플랫폼의 중심으로 매니퓰레이터의 말단을 이동하였고, 이 작업을 영역별로 5회 반복하였다. 매니퓰레이터 말단과 모바일 플랫폼의 거리가 너무 가까우면 궤적의 시작부터 매우 큰 반발력을 가지게 되어 로봇의 최대 속도를 초과할 가능성이 있다. 또한 링크 길이가 정해져 있으므로 로봇 말단이 일정 거리 이상 멀어질 수 없다. 따라서 d는 0.3 ~ 1 m의 범위 내에서 정하였다. 실험은 기존 방식과 제안한 CANS 방식의 두 가지로 진행하여 비교하였다.

[Fig. 6]

Experimental condition and results of conventional method[2]

먼저 기존 방식의 장애물 회피 방법^[2]을 적용하면, 매니퓰레이터의 위치에 따라 [Fig. 7(a)]와 같이 자가 충돌이 발생할 수 있다. 특히 [Fig. 6]의 영역 ④와 ⑧에서 성공률이 낮았는데, 비홀로노믹 제약 조건으로 인해 y축으로 반발력을 반영하지 못한 결과로 볼 수 있다. 결과적으로, [Table 1]과 같이 총 40번의 실험에서 30번 성공하였다.

[Fig. 7]

Self-collision avoidance path of (a) conventional method[2] and (b) CANS

[Table 1]

Experiment results of self-collision avoidance

다음으로, 제안된 방식인 CANS의 충돌 회피 알고리즘을 적용하면, 앞서 충돌이 발생하였던 [Fig. 7(a)]의 경우와 달리 [Fig. 7(b)]와 같이 자가 충돌 회피에 성공한다. 결과적으로, [Table 1]과 같이 총 40번의 실험에서 40번 모두 성공하였다.

4.2 문 열기 실험

모바일 매니퓰레이터를 이용한 문 열기 실험을 [Fig. 8]과 같이 다음과 같은 순서로 진행하였다. 먼저 모바일 매니퓰레이터가 문으로 접근한 다음, Apriltag로 문 정보를 입수하고, 문 손잡이를 파지한다. 마지막으로, 통합 제어를 통해 로봇 말단이 원호 궤적을 따라 문을 연다. 본 연구에서의 실험 조건은 다음과 같다. [Fig. 9]의 점선 영역 내부의 무작위 위치에 모바일 플랫폼을 위치시키고, 문 열기를 시도한다. 모바일 플랫폼의 각도에 따라 카메라가 Apriltag를 인식하지 못하는 경우가 발생할 수 있으므로, θ는 –30° ~ 30° 사이로 정하였다. 또한, 문과 모바일 플랫폼의 거리가 너무 가까우면 궤적의 시작부터 매우 큰 반발력을 가지게 되어 로봇의 최대 속도를 초과할 가능성이 있으므로, d는 0.3 m 이상으로 정하였다. 실험은 기존 방식과 제안한 CANS 방식의 두 가지로 진행하여 비교하였다.

[Fig. 8]

Snapshot of door opening experiment at (a) 0 sec and (b) 5 sec and (c) 10 sec and (d) 15 sec

[Fig. 9]

Experimental condition and results of conventional method[2]

먼저 기존 방식의 장애물 회피 방법^[2]을 문 열기에 적용하면, 처음 문에 접근하는 자세에 따라 [Fig. 10(a)]와 같이 궤적 중간에 문과 모바일 플랫폼의 충돌이 발생할 수 있었다. 결과적으로, [Table 2]와 같이 총 10번의 실험에서 6번만 성공하였다. 다음으로, 제안된 CANS 방식의 충돌 회피 알고리즘을 적용하면, [Fig. 10(b)]와 같이 문과 모바일 플랫폼의 충돌을 회피하면서 문 열기에 성공하였다. 결과적으로, [Table 2]와 같이 총 10번의 실험에서 모두 성공하였다.

[Fig. 10]

Door opening path of (a) conventional method[2] and (b) CANS

[Table 2]

Experiment results of door opening