와이어로 구동하는 적층형 다관절 구조를 지닌 수술 로봇의 구동 속도를 고려한 기구학적 제어기의 게인 최적화

1. 서 론

의료 분야에서 최소침습을 통한 수술 방법은 꾸준히 연구 되고 있는 주제이다. 로봇을 이용한 수술 방법이 하나의 대안 으로 떠올라 다양한 수술 로봇이 개발되고 있다^[1]. 뱀과 유사 한 구조의 로봇은 작은 구멍으로 들어가 높은 자유도를 지닌 움직임을 구현할 수 있어 수술로봇의 구조로 널리 사용되고 있다^[2,3]. 관절을 이루는 요철이 있는 디스크를 여러 개 적층하 고 이를 와이어로 관통시켜 와이어의 변위를 조절하여 전체 로봇 팔의 형상을 공간 상의 곡선의 형태로 제어할 수 있는 구 조이다. 와이어의 변위가 관절들의 회전각과 기하학적으로 구 속되어 있어 로봇 팔 끝단의 위치 및 자세는 와이어의 변위에 대한 기구학 모델로 도출할 수 있다^[4,5]. 외부의 조종 장치로 로 봇을 조종하는 수술자는 자신이 원하는 위치와 자세로 로봇의 끝단을 제어하기 원하기 때문에 위치 제어가 필요하다. 로봇 팔은 크기가 매우 작아 관성이 작고 급격한 움직임을 구현할 상황이 거의 없으며 와이어의 변위에 따라 로봇의 형상이 기 하학적으로 강하게 구속되어 있기 때문에 일반적인 로봇 제어 와 같은 동역학 모델보다는 기구학 모델에 기반하여 제어하는 것이 더 효과적이다.

본 연구에서는 [Fig. 1]과 같은 와이어로 구동하는 단일 경 로 복강경 수술 로봇의 제어 게인 최적화를 다룬다. 하나의 와 이어 구동 로봇 팔은 각각 2자유도의 근위부(proximal segment) 와 말단부(distal segment)의 조인트 구조로 이루어져 있고 전 체 로봇 팔은 전후진 운동과 축방향 회전 운동을 하여 전체 6 자유도 운동을 구현한다. 기구학적 제어 알고리즘으로 폐루 프 역기구학(Closed-loop Inverse Kinematics)을 이용하였고, 구동부의 구동속도 한계를 기구학적 제어기에 제한하기 위해 역계산법(Back-calculation)을 이용하였다^[6].

[Fig. 1]

Wire-driven surgical robot for single port surgery

폐루프 역기구학의 경우 PID 제어기와 같이 게인 조율(gain tuning)이 제어 성능에 큰 영향을 미친다. 역계산법 되먹임을 지닌 6자유도의 PID 제어기는 총 24개의 제어 게인을 지니고 있고, 이들 게인의 조율을 통해 최적의 제어 성능을 얻을 수 있 다. 그러나 최적화해야 할 변수의 개수가 많아 기존의 최적화 알고리즘을 그대로 적용할 경우 발산하여 최적값을 찾지 못하 거나 초기값 근처의 국소적인 최소값(local minimum)에 수렴 하여 유의미한 값을 얻지 못하는 문제가 발생할 수 있다. 설계 된 제어기의 게인 조율에 적합하도록 최적화 알고리즘을 적용 하는 전략이 필요하고, 최적화에 사용될 목적함수와 제한조건 을 적절하게 정의하는 것이 중요하다.

본 연구에서는 6 자유도 PID 제어기의 게인을 먼저 최적화 하고 역계산법의 최적 게인을 추후 찾는 전략으로 다양한 목 적함수와 제한조건, 최적화 알고리즘의 최적화 결과를 몇 가 지 기준 경로에 대해 성능 평가한다.

2. 수술로봇의 기구학 모델

단일경 수술을 위한 수술로봇 팔은 근위부와 말단부로 이 루어져 있고, 각각의 근위부와 말단부는 수직방향으로 굽힘 관절을 지니고 있으며, 전체 기구가 전후진 직선운동과 축방 향 회전운동이 가능하여 [Fig. 2] 및 식 (1)과 같이 6 자유도 정 기구학(forward kinematics) 모델을 수립할 수 있다^[6].

[Fig. 2]

Kinematic scheme of a surgical robot arm

고정 좌표계인 {0}을 기준으로 각 관절에 이동 좌표계를부 착하고 각 이동좌표계 사이의 변환행렬(transformation matrix) 을 순차적으로 곱하여 로봇 끝단의 이동 좌표계 {E}에 대한 변 환행렬 T를 얻는다. 변환행렬로부터 로봇 끝단의 위치 및 자세 를 계산할 수 있다. 아래 첨자는 변환행렬 원소의 위치를 나타 내는 행과 열이다. 사슬 법칙(chain rule)을 통해 각 구동부에 부착된 이동 좌표계에 대한 자코비안(Jacobian) 행렬의 각 열 을 순차적으로 얻을 수 있고, 이로부터 식 (2)와 같은 미분 역기 구학(inverse kinematics)을 도출할 수 있다^[7].

^†는 행렬의 의사 역행렬(pseudo-inverse matrix)이다.

3. 수술로봇의 기구학적 제어기 설계

실시간으로 수술자의 조작 명령을 로봇에 전달하기 위해 기구학 모델로부터 자코비안을 지닌 미분 기구학 모델을 전개 하고 이를 바탕으로 로봇 끝단의 위치 및 자세를 제어하는 PID 제어기를 설계한다. 적층형 다관절의 종속 및 관절각 분배 문 제는 역기구학의 영공간해(null space solution)를 이용하여 구 속할 수 있다. PID 제어기는 실제 시스템으로부터 되먹임을 받는 것이 아니라 각 조인트 각도와 로봇 팔 근위부의 직선 운 동 및 회전운동에 대한 정기구학 모델로부터 되먹임을 받아 역기구학 해를 도출한다. 이런 방식을 폐루프 역기구학이라고 한다^[8,9]. 그러나 기구학 모델에만 기반하여 설계된 제어기이 기 때문에 감쇄와 같은 항이 고려되어 있지 않아 종종 자가 진 동 등의 원인으로 발산하는 제어 결과를 나타내기도 한다. 이 를 극복하기 위하여 [Fig. 3]과 같이 폐루프 역기구학 제어기에 역계산법을 이용한 되먹임을 적분 제어기에 추가하면 제어 입 력에 실제 시스템의 구동기가 지닌 한계만큼 제한을 둘 수 있 어 안정적인 성능을 이끌어 낼 수 있다. 미분 역기구학에 PID 제어와 역계산법을 적용하면 식 (3)과 같은 폐루프 역기구학 식을 도출할 수 있다. 결과적으로, 역계산법을 적용한 폐루프 역기구학의 해를 와이어 구동 모델에 대입하여 와이어를 당기 고 풀어주는 스트로크를 도출하여 로봇 구동기에 개루프 제어 로 입력하는 기구학적 제어기이다. 6 자유도 PID 제어기에 있 는 $$ {\text{K}}_P,{\text{K}}_D,{\text{K}}_I$$의 게인이 총 18개이고, 역계산법에 있는 K_A의 게인이 6개로 총 24개의 게인 인수의 조율이 필요하다.

[Fig. 3]

Block diagram of closed loop inverse kinematics with back-calculation

4. 게인 최적화

본 연구는 MATLAB과 SIMULINK (Mathworks Inc., USA) 를 이용한 시뮬레이션을 통해 진행한다. 와이어로 구동하는 적층형 다관절 구조를 지닌 수술 로봇에 대한 기구학 모델을 바탕으로 제어기를 설계한다. 역계산법 되먹임에 필요한 구동 범위는 로봇의 하드웨어를 설계했을 때 선정한 모터의 사양과 기구적인 제한을 고려하여 선정한다. 설계한 제어기를 시스템 모델에 적용하여 그 제어 결과를 확인할 수 있다.

4.1 최적화 전략

6 자유도 로봇의 제어 게인 조율을 위해 [Fig. 4(a)], [Fig. 5(a)]와 같이 6 자유도 각각의 계단 입력을 인가하고 그 결과로 부터 최적화 알고리즘을 통해 게인의 최적값을 도출한다. 역 계산법으로 구동속도를 제한할 경우 정착속도가 증가하기 때 문에 $$ {\text{K}}_P,{\text{K}}_D,{\text{K}}_I$$의 게인 튜닝과 K_A 게인 튜닝이 서로 상충한 다. 한 번에 총 24개의 게인을 조율할 경우 발산하거나 K_A 가 0 이 되는 무의미한 값으로 수렴하는 현상이 발생하기 때문에 먼저 PID 제어 게인 18개를 최적화하고 이어서 역계산법 게인 6개를 최적화한다^[10]. 18개의 PID 제어 게인을 최적화할 때는 구동속도 제한을 넣지 않아 역계산법을 적용하지 않고 이상적 인 상태에서 게인을 최적화한다. 6 자유도 각각의 계단 입력에 대한 PID 제어 게인을 최적화하면 [Fig. 4(b)], [Fig. 5(b)]와 같 은 결과를 얻을 수 있고, 이는 실제 시스템의 속도를 고려하지 않아 최대한 빠른 속도로 수렴하는 결과를 보인다. 최적의 PID 게인값을 지니는 모델에 역계산법 알고리즘을 추가한 제어기 의 6개 게인을 추가로 최적화하면 [Fig. 4(c)], [Fig. 5(c)]와 같 이 실제 시스템의 구동 속도 한계를 고려하여 완만하게 수렴 하는 결과를 얻을 수 있다. 두 단계의 최적화를 진행할 때 목적 함수는 동일하게 유지하고 제한조건의 범위만 수렴성을 고려 하여 조정한다. MATLAB의 fmincon() 함수를 이용하여 최적 화를 수행하였고, 보다 넓은 범위에서 최적값을 탐색하기 위 해 Active-set 알고리즘을 적용하였다.

[Fig. 4]

Position response: (a) 6 DoF step input, (b) example results of PID gain tuning, and (c) example results of back-calculation gain tuning

[Fig. 5]

Orientation response: (a) 6 DoF step input, (b) example results of PID gain tuning, and (c) example results of back-calculation gain tuning

5. 최적화 결과 비교 평가

성능지수 4가지와 제한조건 2가지로 8가지 조합을 만들어 최적화를 수행하고 결과를 비교하여 최적 게인값을 도출한다. 비교 평가를 위해 시험 궤적을 만든다. 첫번째 시험 궤적은 수 술로봇 팔의 모든 관절을 크기와 위상을 달리 고르게 구동했 을 경우 나타나는 [Fig. 6(a)]와 같은 로봇 팔 끝단의 위치 및 자 세로 표준 궤적(standard path)이라 명명했다. 수술 중 의사의 민 감하고 빠른 움직임을 섬세하게 추종해야 하기 때문에 표준 궤 적의 속도를 2배, 5배로 증가시켜 [Fig. 6(b)], [Fig. 6(c)]와 같은 시험 궤적을 생성한다. 의사가 실제로 햅틱 디바이스를 잡고 봉 합 시술(suturing)을 수행할 때 로봇 팔 끝단의 위치 및 자세에 대한 명령을 저장하여 [Fig. 6(d)], [Fig. 6(e)], [Fig. 6(f)]와 같은 실습 궤적(practical path)을 생성했다. 총 6개의 시험 궤적을 바 탕으로 8가지 최적 게인값 조합의 제어 성능을 시뮬레이션하여 비교 평가한다. 시간에 따라 임의로 변화하는 궤적에 대한 결과 이므로 각 궤적에 대한 제어 오차의 2차 노옴으로 비교했다.

[Fig. 6]

Test trajectories: (a) standard path, (b) standard path x1, (c) standard path x5, (d) practical path #1, (e) practical path #2, and (f) practical path #3

8가지 조합 중 3가지 조합이 대표적으로 좋은 결과를 보였 다. 1번 케이스는 목적함수에 ISE와 정착시간의 합을 적용하 고 제한조건을 다른 자유도의 ISE로 부가한 것이다. 2번 케이 스는 목적함수에 ISE와 정착시간의 합을 적용하고 제한조건 을 다른 자유도의 오차 최대값으로 부가한 것이다. 3번 케이스 는 목적함수에 ITSE를 적용하고 제한조건을 다른 자유도의 오차 최대값으로 부가한 것이다. 각각의 게인값 조합으로 시 험 궤적에 대한 제어 성능을 [Table 1]과 [Fig. 7]로 정리하여 비 교한다. 표준 궤적에서는 2번 케이스가 가장 좋은 제어 성능을 보이지만 전체적으로 보면 1번 케이스가 가장 우수한 결과를 보인다. 특히 표준 궤적을 5배로 빠르게 입력한 시뮬레이션에 서 압도적으로 좋은 결과를 보이고, 빠른 제어 성능을 바탕으 로 임의의 궤적인 실습 궤적에서도 민감한 제어 성능을 보여 줘서 가장 좋은 결과를 보인다. 계단 입력에 대한 제어 결과에 서 시간에 대한 가중치를 고려한 ITSE보다 ISE에 정착시간을 더한 목적함수가 보다 효과적인 최적 게인값을 얻을 수 있었 다는 것이 주목할 점이다. 최적화의 제한조건으로는 계단 입 력을 인가하지 않은 다른 자유도의 안정도를 최대값만이 아닌 전체적인 ISE로 제한하여 수행한 것이 보다 우수한 제어 성능 을 지닌 최적 게인값을 도출했다.

[Table 1]

Comparison of control performance on test trajectories

[Fig. 7]

Comparison of control performance for each case of optimal gain

1번 케이스의 6 자유도 게인값은 [Table 2]와 같다. 6 자유도 운동 중 y, z 방향 직선 운동과 pitch, yaw 방향 회전운동은 와 이어 구동을 통해 제어되고, x 방향 직선 운동과 roll 방향 회 전운동은 로봇 팔 전체를 움직이는 모터로 제어되어 구동 특 성이 다르다. 도출된 최적 제어 게인값도 이에 따라 큰 차이를 보인다. 계단 입력의 크기 조절과 역계산법의 구동 한계 정의 를 통해 기구학적 제어임에도 불구하고 해당 자유도의 제어 게인이 시스템의 동적 특성을 잘 반영하고 있음을 알 수 있다.

[Table 2]

Optimal gain set of case #1

6. 결 론

수술로봇 팔은 작은 크기 때문에 관절각을 측정할 센서를 부착하기 어렵고 관성이 작아 기구학적 제어기로 개루프 제 어가 가능하다. 실시간으로 수술자의 명령을 전달하여 민감 하게 제어하기 위해 폐루프 역기구학을 이용하고 시스템 구 동부의 동적 특성을 반영하게 위해 역계산법을 추가한 제어 기를 설계했다. 설계한 제어기에는 6 자유도 PID 제어와 역계 산법을 위해 총 24개의 게인을 조율해야 한다. 최적 게인값을 도출하기 위해 18개의 PID 게인과 6개의 역계산법 게인을 나 눠 다양한 목적함수와 제한조건으로 최적화를 수행하였다. 총 8가지 전략으로 최적화된 게인값을 6개의 시험 궤적에서 제어 성능을 비교 평가하여 가장 최적의 게인값을 도출한다. 성능지수를 ISE와 정착시간의 합으로 정의하고 6 자유도 계 단 입력에 대한 성능지수의 신호잡음비의 최소화를 목적함 수로 설계하고, 각 계단 입력에 대한 제어 중 나머지 자유도에 대한 오차를 특정 ISE 미만으로 제한하는 제한조건으로 부가 한 최적화 전략이 가장 우수한 제어 성능을 보였다. 본 연구에 서 도출한 최적 제어 게인값은 실제 제작된 시스템의 제어 실 험에 적용된다.

Acknowledgments

This work was supported by a 2-Year Research Grant of Pusan National University.

References

• C. Bergeles and G.-Z. Yang, “From passive tool holders to microsurgeons: safer, smaller, smarter surgical robots,” IEEE Transactions on Biomedical Engineering, vol. 61, no. 5, pp. 1565-1576, May, 2014, [https://doi.org/10.1109/TBME.2013.2293815]
• H.-S. Yoon and B.-J. Yi, “Development of a 4-DOF Continuum Robot Using a Spring Backbone,” Journal of Korea Robotics Society, vol. 3, no. 4, pp. 323-330, Dec., 2008, [Online], http://www.jkros.org/journal/article.php?code=1986.
• F. Jelínek, E. A. Arkenbout, P. W. J. Henselmans, R. Pessers, and P. Breedveld, “Classification of joints used in steerable instruments for minimally invasive surgery-a review of the state of the art,” Journal of Medical Devices, vol. 9, no. 1, 2015, [https://doi.org/10.1115/1.4027035]
• R .J. Webster and B. A. Jones, “Design and kinematic modeling of constant curvature continuum robots: a review,” The International Journal of Robotics Research, vol. 29, no. 13, pp. 1661-1683, 2010, [https://doi.org/10.1177/0278364910368147]
• P. Berthet-Rayne, K. Leibrandt, G. Gras, P. Fraisse, A. Crosnier, and G.-Z. Yang, “Inverse kinematics control methods for redundant snake-like robot teleoperation during minimally invasive surgery,” IEEE Robotics and Automation Letters, vol. 3, no. 3, pp.2501-2508, July, 2018, [https://doi.org/10.1109/LRA.2018.2812907]
• S. Jin, S. K. Lee, J. Lee, and S. Han, “Kinematic Model and Real-Time Path Generator for a Wire-Driven Surgical Robot Arm with Articulated Joint Structure,” Applied Sciences, vol. 9, no. 19, pp. 4114, 2019, [https://doi.org/10.3390/app9194114]
• M. W. Spong, S. Hutchinson, and M. Vidyasagar, “VELOCITY KINEMATICS–THE MANIPULATOR JACOBIAN,” Robot Modeling and Control, 1^st ed., John Wiley & Sons Inc., 2006, ch. 4, pp. 113-130, [Online], https://www.wiley.com/en-us/Robot+Modeling+and+Control-p-9780471649908.
• P. Falco and C. Natale, “On the stability of closed-loop inverse kinematics algorithms for redundant robots,” IEEE Transactions on Robotics, vol. 27, no. 4, pp. 780-784, 2011, [https://doi.org/10.1109/TRO.2011.2135210]
• A. Colomé and C. Torras, “Closed-loop inverse kinematics for redundant robots: comparative assessment and two enhancements,” IEEE/ASME Transactions on Mechatronics, vol. 20, no. 2, pp. 944-955, 2015, [https://doi.org/10.1109/TMECH.2014.2326304]
• J. Bak, J. Kim, T. W. Seo, and S. Jin, “Gain Optimization of a Controller with Decomposition of Thrust Force and Actuation Limit Algorithm for a Tilted Thrusting Underwater Robot,” Journal of the Korean Society for Precision Engineering, vol. 36, no. 11, pp. 1025-1031, 2019, [https://doi.org/10.7736/KSPE.2019.36.11.1025]
• Stanley M. Shinners, “Performance Criteria,” Modern Control System Theory and Design, 2^nd ed., John Wiley & Sons Inc., 1998, ch. 5, sec. 5.6, pp. 290-292, [Online], https://www.wiley.com/en-us/Modern+Control+System+Theory+and+Design%2C+2nd+Edition-p-9780471249061.
• G. S. Peace, “ANALYSIS: SMALLER-THE-BETTER,” Taguchi Methods: A Hands-on Approach, Addison-Wesley, 1993, ch.17, pp. 273-291, [Online], https://books.google.co.kr/books/about/Taguchi_methods.html?id=ZeBTAAAAMAAJ&redir_esc=y.