화성 로버의 모델링 및 제어

이 예제에서는Simscape Multibody와 Robotics System Toolbox를 사용하여 표본 회수 작업을 수행하는 화성 로버를 모델링합니다. 이 로버는 단단한 지형 표면에서 목표 경로를 따라가다가 목표 지점에 멈춰 매니퓰레이터를 사용하여 표면에서 표본을 집어서 저장합니다. 다음의 주요 기능을 사용하여 이 응용 모델의 다양한 측면을 모델링합니다.

• Grid Surface는 단단한 지형 표면을 모델링합니다. (Simscape™ Multibody™ 라이선스 필요)

• Point Cloud 및 Spatial Contact Force는 로버 바퀴와 단단한 지형 사이의 접촉을 모델링합니다. (Simscape Multibody 라이선스 필요)

• Pure Pursuit (Robotics System Toolbox)는 로버의 경로 추적을 제어합니다. (Robotics System Toolbox 라이선스 필요)

• KinematicsSolver(Simscape Multibody 라이선스 필요) 및 Trapezoidal Velocity Profile Trajectory (Robotics System Toolbox)(Robotics System Toolbox 라이선스 필요)는 로버 팔의 조인트 공간 궤적을 계획하고 제어합니다.

• 조인트 모드 구성(Simscape Multibody 라이선스 필요)은 엔드 이펙터와 표본 간의 상호 작용을 모델링합니다. 모드 구성에 대한 자세한 내용은 Revolute Joint 블록의 모드 구성(Mode Configuration) 섹션을 참조하십시오.

화성 로버 모델

이 예제에서 언급된 서브시스템을 보려면 sm_mars_rover.slx 모델을 참조하십시오.

화성 로버 애니메이션

로버 플랜트 모델링 및 제어

이 서브시스템은 로버, 단단한 지형 표면, 경로 계획, 시스템 제어 측면을 모델링합니다.

로버 서브시스템

이 서브시스템은 섀시, 로커-보기 서스펜션, 바퀴 등 로버의 다양한 컴포넌트를 모델링합니다. 기하 CAD 요소는 File Solid 블록을 사용하여 Simscape Multibody로 가져옵니다.

이 로버의 액추에이터는 6개 바퀴 각각에 장착되어 속도를 제어하는 토크 구동식 회전 조인트 6개와, 4개의 코너 바퀴 상단에 장착되어 조향을 제어하는 토크 구동식 회전 조인트 4개로 구성되어 있습니다. 이 외에도 서스펜션 메커니즘의 세 가지 주요 컴포넌트인 차동 암(differential arm), 로커(rocker), 보기(bogie)를 모델링합니다.

바퀴와 단단한 지형 사이의 접촉은 Point Cloud와 Grid Surface 접촉 쌍을 Spatial Contact Force 블록과 함께 사용하여 모델링합니다. 각 바퀴의 그라우저(grouser) 위의 점들은 Point Cloud 블록을 사용하여 생성합니다.

단단한 지형 표면

화성 표면을 모델링하기 위해 Grid Surface 블록을 사용하여 단단한 지형을 생성합니다. STL 파일에서 Grid Surface를 생성하는 데 필요한 파라미터를 설정하려면 rover_rigid_terrain_params.m 파일을 참조하십시오.

로버 경로 계획 및 제어

이 서브시스템은 로버의 경로 추적 제어 시스템을 모델링합니다. 경로는 로버가 통과해야 하는 X-Y 평면 상의 순서가 지정된 웨이포인트들로 구성됩니다. 이러한 웨이포인트는 하이 레벨 경로 플래너가 제공한 것으로 가정하며, 로버가 장애물 없이 이동할 수 있는 경로를 나타냅니다. 웨이포인트는 roverDesiredPath.mat 파일을 통해 불러올 수 있습니다.

이 서브시스템의 첫째 목표는 목표 경로와 목표 섀시 선형 속도를 추종하기 위해 필요한 조향각과 바퀴 속도를 계산하는 것이고, 둘째 목표는 이 조향각과 바퀴 속도를 달성하기 위해 필요한 액추에이터 토크를 계산하는 것입니다.

경로 추적 제어기를 개발할 때 다음 사항을 고려하십시오.

• 일반적으로 화성 로버는 전진 속도가 낮다고 가정하므로(cm/s 단위의 크기), 운동 동특성은 무시하고 기구학 방정식만을 사용하여 제어 문제를 처리합니다.[1]

• 기구학 공식을 단순화하기 위해, 로버가 평평한 표면에서 움직인다고 가정합니다.[1]

• 로버의 4개 코너 바퀴는 독립적인 조향이 가능하며, 따라서 로버가 Ackerman 조향을 수행할 수 있습니다. 로버는 이 기능을 기반으로 Ackerman 조향을 사용하는 것으로 간주합니다.

• Ackerman 조향 기하학은 등가 회전 반경을 갖는 2차원 기하 자전거 모델을 가정하여 단순화합니다. 이 단순화에서는 각 바퀴 쌍을 가운데에 위치한 단일 바퀴와 로버 중심의 회전 반경에 해당하는 단일 조향각으로 표현합니다.[1][2]

• 앞바퀴와 뒷바퀴는 대칭적으로 조향되는 것으로 간주합니다.

• 바퀴는 미끄러짐 없이 굴러간다고 가정합니다.

위의 고려 사항을 기반으로 6륜 로버를 기하학적 자전거 모델로 동등하게 표현할 수 있습니다.[1]

경로 추적에는 Pure Pursuit (Robotics System Toolbox)이 사용됩니다. 이는 로봇이 현재 위치에서 로봇 앞의 특정한 전방 주시 지점까지 도달하기 위해 필요한 타깃 방향 각도($\alpha$)를 계산하는 기하 알고리즘입니다.[1]

조향각 공식

로버의 4개 코너 바퀴 각각에 대한 조향각은 두 단계를 통해 도출됩니다. 먼저 기하 자전거 모델과 타깃 방향 각도($\alpha$)(Pure Pursuit 제어기에서 제공)를 사용하여 아래와 같이 자전거 조향각($\delta$)과 회전 반경($\mathit{R}$)을 구합니다.[2]

$\delta ={\tan }^{-1}\left(\frac{2\mathit{L}\sin \left(\alpha \right)}{{\mathit{L}}_{\mathit{d}}}\right)$

$\mathit{R}=\frac{\mathit{L}}{\tan \left(\delta \right)}$

여기서 각각은 다음과 같습니다.

$$\begin{array}{l}\alpha :\mathrm{Target}\mathrm{Dir}\mathrm{Angle}\\ \delta :\mathrm{Bicycle}\mathrm{Steering}\mathrm{Angle}\\ \mathit{L}:\mathrm{Bicycle}\mathrm{Length}\\ {\mathit{L}}_{\mathit{d}}:\mathrm{Look}\mathrm{ahead}\mathrm{distance}\mathrm{for}\mathrm{Pure}\mathrm{Pursuit}\\ \mathit{R}:\mathrm{Turn}\mathrm{radius}\mathrm{of}\mathrm{rover}\mathrm{center}\end{array}$$

계산된 자전거 조향각($\delta$)과 로버의 회전 반경($\mathit{R}$)을 바탕으로 Ackerman 조향 기하학을 사용하여 개별 조향각(${\delta }_{\mathrm{LF}},{\delta }_{\mathrm{LB}},{\delta }_{\mathrm{RF}},{\delta }_{\mathrm{LB}}$)을 구합니다.[3]

$${\delta }_{\mathrm{LF}}={\tan }^{-1}\left(\frac{\mathit{L}}{\mathit{R}-\mathit{d}}\right)$$

$${\delta }_{\mathrm{RF}}={\tan }^{-1}\left(\frac{\mathit{L}}{\mathit{R}+\mathit{d}}\right)$$

${\delta }_{\mathrm{LB}}$ = -${\delta }_{\mathrm{LF}}$

${\delta }_{\mathrm{RB}}$ = -${\delta }_{\mathrm{RF}}$

여기서 각각은 다음과 같습니다.

$$\begin{array}{l}{\delta }_{\mathrm{LF}},{\delta }_{\mathrm{LB}},{\delta }_{\mathrm{RF}},{\delta }_{\mathrm{LB}}:\mathrm{Steering}\mathrm{angles}\mathrm{of}\mathrm{respective}\mathrm{corner}\mathrm{wheels}\\ \mathit{L}:\mathrm{Bicycle}\mathrm{Length}\left(0.5*\mathrm{Chassis}\mathrm{Length}\right)\\ \mathit{d}:0.5*\mathrm{Chassis}\mathrm{Width}\\ \mathrm{ICR}:\mathrm{Instantenous}\mathrm{Center}\mathrm{of}\mathrm{Rotation}\end{array}$$

바퀴 속도 공식

Ackerman 조향 기하학과 회전 반경($\mathit{R}$)을 바탕으로 아래와 같이 섀시 속도($\mathrm{Vc}$)와 바퀴 속도($\omega$) 간의 관계도 구합니다[3].

$${\omega }_{\mathrm{LF}}=\frac{{\mathit{V}}_{\mathit{C}}}{{\mathit{R}}_{\mathit{w}}}*\frac{\left(\sqrt{{\mathit{L}}^2+{\left(\mathit{R}-\mathit{d}\right)}^2}\right)}{\mathit{R}}$$

$${\omega }_{\mathrm{RF}}=\frac{{\mathit{V}}_{\mathit{C}}}{{\mathit{R}}_{\mathit{w}}}*\frac{\left(\sqrt{{\mathit{L}}^2+{\left(\mathit{R}+\mathit{d}\right)}^2}\right)}{\mathit{R}}$$

${\omega }_{\mathrm{LM}}=\frac{{\mathit{V}}_{\mathit{C}}}{{\mathit{R}}_{\mathit{w}}}*\frac{\left(\mathit{R}-\mathit{d}\right)}{\mathit{R}}$

$${\omega }_{\mathrm{RM}}=\frac{{\mathit{V}}_{\mathit{C}}}{{\mathit{R}}_{\mathit{w}}}*\frac{\left(\mathit{R}+\mathit{d}\right)}{\mathit{R}}$$

${\omega }_{\mathrm{LB}}={\omega }_{\mathrm{LF}}$

${\omega }_{\mathrm{RB}}={\omega }_{\mathrm{RF}}$

여기서 각각은 다음과 같습니다.

$$\begin{array}{l}{{\omega }_{\mathrm{LF}},{\omega }_{\mathrm{LM}},{\omega }_{\mathrm{LB}},{\omega }_{\mathrm{RF}},{\omega }_{\mathrm{RM}},{\omega }_{\mathrm{RB}}}_{}:\mathrm{Angular}\mathrm{speeds}\mathrm{of}\mathrm{respective}\mathrm{wheels}\\ \mathrm{Vc}:\mathrm{Linear}\mathrm{speed}\mathrm{of}\mathrm{the}\mathrm{rover}\mathrm{chassis}\\ {\mathit{R}}_{\mathit{w}}:\mathrm{Radius}\mathrm{of}\mathrm{the}\mathrm{wheel}\end{array}$$

조향각과 바퀴 속도가 목표 경로와 목표 섀시 선형 속도에 맞게 정식화되면, PID 제어기를 사용하여 실제 조향각과 액추에이터(회전 조인트)의 각속도를 이 목표값으로 구동합니다.

로버 시뮬레이션 결과: 경로 1

로버가 울퉁불퉁한 지형에서 최대 0.3m/s 속도로 이동할 때 경로 1의 결과입니다. 모델 내 Rover Sensing 서브시스템에서 더 많은 수치를 확인할 수 있습니다.

로버 시뮬레이션 결과: 경로 2

로버가 울퉁불퉁한 지형에서 최대 0.3m/s 속도로 이동할 때 경로 2의 결과입니다. 모델 내 Rover Sensing 서브시스템에서 더 많은 수치를 확인할 수 있습니다.

매니퓰레이터 플랜트 모델링 및 제어

이 서브시스템은 로버의 로봇 팔과 표면에서 표본을 집어서 수집하는 로봇 팔의 궤적 계획과 제어를 모델링합니다.

로버 팔

이 매니퓰레이터는 섀시 전면에 장착된 6-DOF 로봇 팔로 모델링됩니다. 각 액추에이터는 6개의 토크 구동식 회전 조인트에 해당합니다. 인코더와 같은 센서를 모방하기 위해, 이 서브시스템은 6개의 회전 조인트 각각에 대한 조인트 각도를 출력합니다. 엔드 이펙터와 표본 간의 상호 작용에는 조인트 모드 전환을 활용하는 단순화된 모델을 사용합니다. 엔드 이펙터가 표본에 충분히 가까워지면, 처음에는 연결되어 있지 않던 둘 사이의 6-DOF 조인트가 연결됩니다. 이 6-DOF 조인트는 표본이 엔드 이펙터에 거의 구속되도록 엄격한 위치 제한을 적용합니다. 엔드 이펙터가 표본을 잡은 상태로 표본 저장 위치에 충분히 가까워지면, 엔드 이펙터와 표본 사이의 6-DOF 조인트 연결이 해제되고, 처음에는 연결되어 있지 않던 표본과 저장 위치 사이의 6-DOF 조인트가 연결됩니다. 이 6-DOF 조인트 역시 표본이 섀시의 저장 위치에 거의 구속되도록 엄격한 위치 제한을 적용합니다.

계획 및 제어

로버가 목표 위치에서 멈추면 매니퓰레이터의 계획 및 제어 서브시스템이 활성화됩니다. 로버가 멈추면, Transform Sensor 블록을 사용하여 팔 베이스를 기준으로 한 표본의 위치를 계산합니다(탑재된 카메라를 모방).

엔드 이펙터의 궤적을 계획하기 위해 작업 공간에 6개의 웨이포인트가 정의됩니다. 이 중에서 1, 2, 5, 6에 해당하는 4개 웨이포인트는 로버 섀시의 기하 구조와 저장 장치 위치로부터 도출되어 미리 계산됩니다. 이것은 roverArmTaskSpaceConfig.mat 파일에서 불러올 수 있습니다. 웨이포인트 3과 4는 Transform Sensor 블록에서 구한 표본 위치를 기반으로 계산됩니다. 이 6개의 웨이포인트를 바탕으로, 엔드 이펙터의 궤적 계획이 조인트 공간 궤적 계획 접근법을 사용하여 생성됩니다. 이 플래너는 먼저 모든 6개의 작업 공간 웨이포인트를 역기구학 모듈(KinematicsSolver를 사용하여 구현된 sm_mars_rover_arm_ik.m 참조)을 사용하여 조인트 공간 웨이포인트로 변환합니다. 그런 다음, 이 조인트 공간 웨이포인트는 MATLAB 함수 블록 Task Scheduler에서 사용되어, 위에 표시된 것처럼 팔이 일련의 모드를 따라 움직이게 됩니다. 이 스케줄러는 엔드 이펙터가 허용오차 값 내의 목표 위치에 도달하면 다음 모드로 진행합니다. 스케줄러의 각 작업은 Trapezoidal Velocity Profile Trajectory (Robotics System Toolbox) 블록에 입력으로 제공되어, 조인트 공간 내 각 웨이포인트 간의 매끄러운 궤적을 계산합니다.

조인트 공간 궤적이 생성된 후에는, PID 제어기를 사용하여 액추에이터의 실제 위치가 목표값으로 구동되도록 합니다.

매니퓰레이터 시뮬레이션 결과

결과는 목표 웨이포인트를 통과하는 엔드 이펙터의 궤적과, 액추에이터의 목표 조인트 각도와 실제 조인트 각도의 비교를 보여줍니다. 모델 내 Manipulator Sensing 서브시스템에서 더 많은 수치를 확인할 수 있습니다.

참고 문헌

[1] Filip, Jan, Martin Azkarate, Gianfranco Visentin. "Trajectory control for autonomous planetary rovers." 14th ASTRA(Symposium on Advanced Space Technologies in Robotics and Automation). 2017.

[2] Snider, Jarrod M. "Automatic steering methods for autonomous automobile path tracking." Robotics Institute, Pittsburgh, PA, Tech. Rep. CMU-RITR-09-08 (2009).

[3] X. Wu, L. Yang and M. Xu, "Speed following control for differential steering of 4WID electric vehicle," IECON 2014 - 40th Annual Conference of the IEEE Industrial Electronics Society, 2014, pp. 3054-3059, doi: 10.1109/IECON.2014.7048945.

참고 항목

KinematicsSolver | Grid Surface | Point Cloud