모바일 매니퓰레이터의 NMPC 기반 장애물 회피 및 전신 모션 플래닝

1. 서 론

모바일 매니퓰레이터가 갖는 이동성과 높은 자유도는 베이스가 고정된 통상의 매니퓰레이터에 비해 더 넓은 작업공간을 가지기 때문에 보다 다양한 분야에서 사용되고 있다^[1]. 일반적으로 제어 문제의 단순화를 위해 모바일 베이스와 매니퓰레이터의 동작을 분리하여 작업을 수행하는 방식을 채택한다^[2]. 이러한 방식은 모바일 베이스의 위치 추정 문제만 해결되면 기존 베이스가 고정된 매니퓰레이터 방식과 유사하게 기구학 및 모션 플래닝 문제를 해결할 수 있다는 장점이 있다. 그러나 장애물이 있는 환경에서 작업 혹은 이동과 동시에 작업을 수행하는 경우, 기존의 분리된 기구학과 모션 플래닝 방식은 효율적이지 않다. 다른 한편으로 모바일 베이스와 매니퓰레이터의 자유도를 합한 높은 자유도 시스템에 대하여 전신 제어(whole-body control)의 개념을 적용하고 상응하는 기구학 및 모션 플래닝을 수행하여 모바일 매니퓰레이션의 성능과 효율성을 개선시킬 수 있다^[3,4]. 그러나 높은 자유도로 인하여 역기구학(inverse kinematics)의 해가 무수히 많아지게 되며 제어의 복잡도가 상승하는 단점이 있다^[5].

비선형 모델 예측 제어(Nonlinear Model Predictive Control, NMPC)는 주어진 시스템 모델을 기반으로 이동 구간(receding horizon) 만큼의 비용 함수(cost function)와 구속 조건(constraint)을 고려하여 최적 제어 입력과 상태를 예측하는 제어 기법이다^[6]. NMPC의 이러한 특징은 로봇 제어에서도 다양하게 적용이 가능하다. 더욱이 역진자 모델 형태의 불안정한 로봇 시스템의 경우, 동역학 모델링과 구속 조건을 통해 안정상태를 유지하도록 제어할 수 있다^[7]. 또한 모바일 베이스가 장애물을 만나게 될 때 회피를 위한 구속 조건으로 인해 수정된 경로로 우회하며 경로를 생성할 수 있다^[8].

본 연구에서는 모바일 매니퓰레이터를 여유자유도 직렬 체인 매니퓰레이터(redundant serial chain manipulator)로 모델링하고 NMPC기반 전신 모션 플래닝 방법을 제안한다. 충돌 회피를 위한 볼록 영역(convex hull)을 모바일 매니퓰레이터와 장애물에 적용하여 최소 거리를 계산하고, 이를 회피를 위한 구속조건으로 사용한다. Point-to-Point (PTP) 궤적 생성을 위해 매니퓰레이터 끝단 위치 오차, 끝단 회전 오차, 각 관절들의 안정적인 댐핑 제공을 위한 각속도 및 각가속도를 최소화하는 비용 함수를 선정한다. 또한 관절 회전각, 각속도, 각가속도 등의 기구학적 한계를 구속조건으로 선정하고 비선형 모델 예측 제어의 해를 찾기 위한 최적화 문제로 구성한다.

2장에서는 모바일 매니퓰레이터의 기구학 모델링과 장애물 및 자가 충돌 회피를 위해 강체 사이의 거리 계산 방법에 대해 설명하고, 3장에서는 NMPC 구성에 필요한 비용 함수, 구속 조건 그리고 Solver에 대해 자세히 기술한다. 4장에서는 시뮬레이션을 통해 제안한 NMPC 기반 모션 플래닝의 성능을 검증한다.

2. 모바일 매니퓰레이터 모델링

본 연구에서는 평평한 지면의 실내 공간 환경만을 고려하며, 모바일 매니퓰레이터 모델은 [Fig. 1]의 좌측에 위치한 Neobotix사의 MMO-500 모델을 사용하였다. 해당 모델의 모바일 베이스는 4개의 메카넘 휠(mecanum wheel)로 구성되어 전방향(omni-directional) 이동이 가능하며, 작업을 위한 매니퓰레이터는 6-DoFs UR10이 적용되어 있다. MMO-500과 UR10의 기구학, 동역학 변수들은 공개되어 있는 자료를 기반으로 시뮬레이터를 구현하였다.

[Fig. 1]

The kinematic model of mobile manipulator: PPR (mobile base) + 6R (manipulator); it is equivalent to 9-DoFs serial chain robot, where P stands for the prismatic joint and R for the rotational

2.2 장애물 및 자가 충돌 회피

모바일 매니퓰레이터의 장애물 및 자가 충돌(self-collision) 회피를 위해선, 강체(rigid body) 사이의 충돌 영역의 정의가 필요하다. 본 연구에서는 충돌 감지를 위한 영역으로 [Fig. 2]와 같이 점 혹은 선분으로부터 일정한 거리 r만큼의 영역을 포함하는 Swept Sphere Volume (SSV)기반의 볼록 영역(convex hull)을 적용하였다. [Fig. 2]에서 제시되었 듯이 Point Swept Sphere (PSS)와 Line Swept Sphere (LSS) 두가지 타입을 적용하였다^[10,11]. SSV 중심 간의 최단 거리 d는 두 선분(line segment)간의 거리를 구하는 알고리즘^[12]을 적용하였으며, SSV b₁과 b₂간의 거리 m은 다음 식 (1)과 같은 표현이 가능하다.

$\[ dist\left(b_1,b_2\right)=d-\left(r_1+r_2\right)=m \]$

(1)

[Fig. 2]

Convex hull using swept sphere volume (SSV): (a) distance between LSS and PSS, (b) distance between two LSSs, where PSS stands for point swept sphere, LSS for line swept sphere, d for the shortest distance between the center of SSVs, r for the radius of swept sphere, and m for the minimal distance between two volumes

만약 m값이 음수를 갖게 되는 경우 두 영역은 충돌이 발생한 것으로 간주한다. 이러한 충돌 영역을 [Fig. 3]에서 제시되었 듯이 모바일 매니퓰레이터 끝단(end-effector)과 모바일 베이스는 PSS를 적용하고 두 개의 팔 링크에는 LSS를 적용하였다. PSS와 LSS의 위치들은 각 관절의 위치를 기준으로 설정하였고, 정기구학(forward kinematics)을 통해 위치를 얻을 수 있다.

[Fig. 3]

The convex hull of mobile manipulator for obstacle and self-collision avoidance: two PSSs on mobile base and end-effector, and two LSSs on upper and lower arm links are attached, where r is the radius, a is the length of the link, and green lines are considered for the self-collision avoidance

[Fig. 3]의 녹색 선은 모바일 매니퓰레이터의 자가 충돌을 피하기 위해 고려한 것으로, 모바일 베이스 중심과 끝단 그리고 매니퓰레이터 팔 링크들 사이의 최소 거리를 NMPC의 구속 조건으로 추가하였다. 본 연구에서는 장애물의 충돌 영역 또한 SSV로 정의하였으며, 회피를 위한 거리도 마찬가지로 앞서 기술한 개념을 통해 구속조건에 포함한다.

3. NMPC 최적 제어

2장에서 정의된 모바일 매니퓰레이터 모델링을 기반으로 끝단의 Point-to-Point (PTP) 궤적 생성을 위해 NMPC를 적용한다.

3.2 NMPC 최적 제어

$\[ \begin{array}{c}\mathbf{m}\mathbf{i}\mathbf{n}\\ \mathit{u}\end{array}{l}_T\left(x_T\right)+{\sum }_{k=0}^{T-1}{l}_k\left(x_k,u_k\right) \]$

(5a)

$\[ \mathbf{S}\mathbf{u}\mathbf{b}\mathbf{j}\mathbf{e}\mathbf{c}\mathbf{t}\mathbf{ }\mathbf{t}\mathbf{o}\mathbf{ }{x}_0=x_{init}, \]$

(5b)

$\[ x{̇}_k=f_k\left(x_k,u_k\right)=Ax+Bu \]$

(5c)

$\[ dist\left({bod}_{1,k}\left(x_k\right),{bod}_{i,k}\left(x_k\right)\right)\ge 0,i\in N_{\left[2,n_{bod}\right]} \]$

(5d)

$\[ \begin{array}{c}dist\left({Obs}_{l,k},{bod}_{j,k}\left(x_k\right)\right)\ge 0,l\in N_{\left[1,n_{obs}\right]},\hfill \\ j\in N_{\left[1,n_{bod}\right]}\hfill \end{array} \]$

(5e)

여기서 비용함수 식 (5a)는 다음과 같이 두 부분으로 구성된다: 종단 상태(terminal state)를 위한 비용 Mayer term l_T(x_T), 각 단계의 비용을 나타내는 Lagrange term l_k(x_k, u_k)은 다음과 같이 표현된다.

$\[ \begin{array}{c}{l}_T\left(x_T\right)={‖P_{goal}-P_T‖}_{Q_T^{pos}}^2+{‖e_T^{ori}‖}_{Q_T^{ori}}^2+{‖\dot{q_T}‖}_{Q_T^{vel}}^2\hfill \\ +{‖q_{offset}-q‖}_{Q_T^{offset}}^2,\hfill \\ l_k\left(x_k,u_k\right)={‖P_{goal}-P_k‖}_{Q_k^{pos}}^2+{‖e_k^{ori}‖}_{Q_k^{ori}}^2+{‖u_k‖}_{R_k}^2\hfill \\ +{‖\dot{q_k}‖}_{Q_k^{vel}}^2\hfill \end{array} \]$

NMPC 비용 함수와 구속 조건은 식 (5a-e)와 같다^[13]. T는 이동구간 크기, P는 끝단의 위치, $$ e_k^{ori}$$는 쿼터니안(quaternion) 기반 끝단 회전 오차(orientation error)를 의미한다. q_offset은 로봇 팔의 관절 기준점으로, NMPC 비용 함수 구성 시 끝단의 오차 최소화만 고려할 경우 매니퓰레이터가 목표점을 향해 모든 링크들이 뻗는 자세를 유지하게 되며, 다음 목적지가 주어질 때, 매니퓰레이터 관절은 위치 한계를 넘지 않기 위하여 뒤틀린 자세를 갖게 된다. 이러한 문제를 해결하기 위해 관절 기준점에 가상의 비틀림 스프링을 제공하는 방법^[14]을 종단 비용(terminal cost) l_T(x_T)에 적용하였다. 비용함수의 각 가중치(weights)는 [Table 1]과 같이 적용하였다. 끝단의 오차를 최소화화는 궤적 생성을 위해 위치와 회전에 대해 높은 가중치를 설정하였다. 모바일 매니퓰레이터가 작업을 수행 중, 더 많은 매니퓰레이터 움직임을 발생시키기 위하여 Lagrange term의 관절 속도 가중치 $$ Q_k^{vel}$$ 선정시, [Table 1]에 제시되었 듯이 모바일 베이스 보다 매니퓰레이터에 상대적으로 낮은 가중치를 설정하였다.

[Table 1]

NMPC-OCP weights & parameters

이동구간의 경우 짧아질수록 예측 범위가 좁아지기 때문에 목표점 도달까지 걸리는 시간이 길어지게 된다.

또한 짧은 샘플링 타임으로 많은 샘플 수를 가지는 이동구간의 경우 NMPC연산량의 증가로 인해 목표로 하는 샘플링 타임보다 연산시간이 길어진다. 제안된 최적 제어 문제는 1초 이동 구간(receding horizon)의 상태를 예측하기 위해 100 ms 샘플링타임과 10 샘플로 설정하였다. 비선형 최적화 solver로는 COIN-OR에서 개발한 Interior-Point(IP) method 기반의 IPOPT-MA57를 적용하였다^[15,16].

4. 시뮬레이션

본 연구에서 제안한 NMPC기반의 모바일 매니퓰레이터의 전신 모션 제어와 장애물 회피 성능을 시뮬레이션을 통해 검증하였다. 검증을 위한 시뮬레이션 구성은 다음과 같다. 시뮬레이션은 Pybullet기반으로 모바일 매니퓰레이터의 정기구학(forward kinematics) 및 자코비안(Jacobian)은 [Fig. 1]을 기반으로 작성된 URDF를 통해Pinocchio interface^[17]을 이용하여 구현하였다. 제안된 NMPC 최적 제어의 결과로 이동구간 샘플 수 만큼 예측된 상태(관절 위치, 속도)와 제어 값(관절 가속도)을 얻을 수 있으며, 시뮬레이션 상 모바일 매니퓰레이터 구동을 위해 예측된 관절 속도 기반 제어를 적용하였다. 모바일 로봇과 장애물의 위치 추정을 위한 localization 알고리즘은 적용하지 않기 때문에 시뮬레이션에서 얻을 수 있는 로봇과 장애물의 위치 정보를 기반으로 진행하였다.

4.1 PTP 궤적 생성

모바일 매니퓰레이터에 작업 공간상 목표 지점을 정점(set-point)으로 주었다. [Fig. 4(a)], [Fig. 4(b)]에서 확인할 수 있듯이 이동 구간만큼 예측된 위치를 통해 목표지점으로 도달하는 궤적이 생성되는 것을 확인할 수 있다. PTP 궤적 생성 시 관절 기준점 적용에 따른 결과의 차이점을 비교하고자 한다. 3장에서 설명한 것처럼 [Fig. 4(b)]에서처럼 관절 기준점이 없을 때 매니퓰레이터의 링크들이 모두 펼쳐진 자세를 하고 있는 반면, [Fig. 4(a)]은 종단 비용에 기준점을 추가함으로 인해 목표점 도달 후 manipulability가 높아지도록 자세를 수정 기준점에 가까워지도록 취한다. 그로 인해 [Fig. 4(c)]의 manipulability 지수가 15배 더 높게 나타나며, 동일 작업대비 특이점(singularity)으로부터 멀어지게 되는 효과를 볼 수 있다.

[Fig. 4]

Point-to-Point trajectory generation experiment; the comparison of the manipulator postures when the joint offset constraint is considered in the NMPC formulation or not

4.2 장애물 회피

모바일 매니퓰레이터가 목표점에 도달한 상태에서 움직이는 장애물이 접근할 때의 회피 성능을 확인하고자 한다. 움직이는 장애물로는 [Fig. 5(a)]에서 보이는 것처럼원기둥 형태의 볼록 영역을 씌운 사람을 모델로 사용하였다. [Fig. 5(b)]에서 확인할 수 있듯이 장애물이 접근하는 경우 비용함수의 끝단 위치 및 회전 에러를 최소화하며 볼록 영역간 거리 구속조건 고려로 인해, 끝단은 고정된 상태에서 모바일 베이스의 여유자유도로 움직이며 회피하는 것을 확인할 수 있다.

[Fig. 5]

Moving obstacle avoidance scenario: (a) cylindrical convex hull on human model, (b) when human approaches to mobile manipulator, the mobile manipulator can avoid the human while holding the end-effector to the goal

5. 결 론

본 연구에서는 모바일 매니퓰레이터의 NMPC 기반 장 애물 회피 및 전신 모션 플래닝 방법을 제시하였다. 모바일 베이스와 매니퓰레이터를 통합하여 하나의 여유자유도 매니퓰레이터로 모델링함으로써 기구학적 전신 모션 제어가 가능하였다. 끝단의 포즈(pose) 오차를 최소화하는 NMPC 비용 함수와 관절의 물리적 제한 구속 조건의 적용을 통해 PTP 궤적 생성이 가능함을 확인하였으며, 관절 기준점과 장애물과의 거리를 구속 조건에 추가함으로서 manipulability가 증가하는 현상 및 장애물 회피의 성능을 확인할 수 있었다.

본 연구는 Pybullet 시뮬레이션 상에서 로봇과 장애물의 위치를 정확하게 알 수 있었고, 이를 제안된 방법에 적용할 수 있었다. 그러나 실제 로봇 환경에 구현하기 위해서는 주변 환경 및 로봇의 위치를 인지하는 기술을 필요로 한다. 후속 연구로는 모바일 매니퓰레이터에 설치된 센서(Lidar, RGB-D, 근접센서 등)의 데이터들을 통해 미지의 환경이더라도, 장애물 회피 및 작업을 수행^[18]하는 것을 목표로 하고 있다.

Acknowledgments

This research was supported by the Ministry of Trade, Industry, and Energy in Korea, under the Fostering Global Talents for Innovative Growth Program (P0008745) and the Technology Innovation Program (20008908) supervised by the Korea Institute for Advancement of Technology (KIAT), Republic of Korea

References

• H. Zhang, Y. Jia, Y. Guo, K. Qian, A. Song, and N. Xi, “Online sensor information and redundancy resolution based obstacle avoidance for high dof mobile manipulator teleoperation,” International Journal of Advanced Robotic Systems, vol. 10, no. 5, May, 2013. [https://doi.org/10.5772/56470]
• D. H. Shin, B. S. Hamner, S. Singh, and M. Hwangbo, “Motion planning for a mobile manipulator with imprecise locomotion,” 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453), Las Vegas, NV, USA, 2003. [https://doi.org/10.1109/IROS.2003.1250735.]
• J. Pankert and M. Hutter. “Perceptive model predictive control for continuous mobile manipulation,” IEEE Robotics and Automation Letters, vol. 5, no. 4, pp. 6177-6184, Oct., 2020. [https://doi.org/10.1109/LRA.2020.3010721]
• J. Kindle, F. Furrer, T. Novkovic, J. J. Chung, R. Siegwart, and J. Nieto, “Whole-body control of a mobile manipulator using end-to-end reinforcement learning,” arXiv preprint arXiv:2003.02637, 2020.
• A. H. Khan, S. Li, D. Chen, and L. Liao, “Tracking control of redundant mobile manipulator: An RNN based metaheuristic approach,” Neurocomputing, vol. 400, no. 4, pp. 272-284, Aug., 2020. [https://doi.org/10.1016/j.neucom.2020.02.109]
• E. F. Camacho and C. B. Alba., “Chapter 1 Introduction to Model Predictive Control,” Model Predictive Control, Springer science & business media, 2013. [https://doi.org/10.1007/978-0-85729-398-5]
• M. V. Minniti, F. Farshidian, R. Grandia, and M. Hutter, “WholeBody MPC for a Dynamically Stable Mobile Manipulator,” IEEE Robotics and Automation Letters, vol. 4, no.4, pp. 3687-3694, Jul., 2019. [https://doi.org/10.1109/LRA.2019.2927955]
• Z. Li, J. Deng, R. Lu, Y. Xu, J. Bai, and C. Y. Su, “Trajectory-tracking control of mobile robot systems incorporating neural-dynamic optimized model predictive approach,” IEEE Transactions on Systems, Man, and Cybernetics: Systems, vol. 46, no. 6, pp. 740-749, Aug., 2015. [https://doi.org/10.1109/TSMC.2015.2465352]
• K. M. Lynch, and F. C. Park, “Chapter 13 Wheeled Mobile Robots”, Modern Robotics, Cambridge University Press, 2017, [Online], https://www.amazon.com/Modern-Robotics-Mechanics-Planning-Control/dp/1107156300, .
• M. Tang, Y. J. Kim, and D. Manocha. “C2A: Controlled conservative advancement for continuous collision detection of polygonal models,” 2009 IEEE International Conference on Robotics and Automation, Kobe, Japan, 2009. [https://doi.org/10.1109/ROBOT.2009.5152234]
• M. Krämer, C. Rösmann, F. Hoffmann, and T. Bertram, “Model predictive control of a collaborative manipulator considering dynamic obstacles,” Optimal Control Applications and Methods, vol. 41, no. 4, pp. 1211-1232, Apr., 2020. [https://doi.org/10.1002/oca.2599]
• V. J. Lumelsky, “On fast computation of distance between line segments,” Information Processing Letters, vol. 21 no. 2, pp. 55-61, Aug., 1985. [https://doi.org/10.1016/0020-0190(85)90032-8]
• A. S. Sathya, J. Gillis, G. Pipeleers, and J. Swevers, “Real-time robot arm motion planning and control with nonlinear model predictive control using augmented lagrangian on a first-order solver,” 2020 European Control Conference (ECC), St. Petersburg, Russia, pp. 507-512, 2020. [https://doi.org/10.23919/ECC51009.2020.9143732]
• J.-H. Bae, J.-H. Park, Y. Oh, D. Kim, Y. Choi, and W. Yang, “Task space control considering passive muscle stiffness for redundant robotic arms,” Intelligent Service Robotics, vol. 8, no. 2, pp. 93-104, Jan., 2015. [https://doi.org/10.1007/s11370-015-0165-2]
• J. Fiala and B. Marteau, Nonlinear Optimization: A Comparison of Two Competing Approaches Active-set SQP vs IPM, 2017, [Online], https://www.nag.com/market/nonlinear-optimization-comparison.pdf, .
• A. Wächter and L.T. Biegler. “On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming,” Mathematical Programming, vol. 106, no. 1 pp. 25-57, Apr., 2006. [https://doi.org/10.1007/s10107-004-0559-y]
• J. Carpentier, G. Saurel, G. Buondonno, J. Mirabel, F. Lamiraux, O. Stasse, and N. Mansard, “The Pinocchio C++ library: A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives,” 2019 IEEE/SICE International Symposium on System Integration (SII), Paris, France, 2019. [https://doi.org/10.1109/SII.2019.8700380]
• M. Mittal, D. Hoeller, F. Farshidian, M. Hutter, and A. Garg, “Articulated object interaction in unknown scenes with whole-body mobile manipulation,” arXiv preprint arXiv:2103.10534, 2021.