[Control] Mobile robot Control

 

0. Mobile robot

먼저 위키에서 설명하는 이동로봇이라 함은 다음과 같다.

 

이동로봇(Mobile Robot)은 지상에 고정된 로봇이 아닌 움직이는 로봇이다.
바퀴형 이동로봇(wheel-type mobile robot)과 보행형 이동로봇(legged mobile robot 또는 locomotive robot)이 있다.

이동 로봇 - 위키백과, 우리 모두의 백과사전

 

이번 글에서는 이동로봇의 kinematic model의 비선형 제어기 설계에 대해 다뤄본다.

---

 

1. Mobile robot kinematic model

[Fig. 1] kinematic model of the Mobile Robot

$$ \begin{cases} \dot{x} = v\cos\theta\\ \dot{y} = v\sin\theta\\ \dot{\theta} = \omega \end{cases} $$

 

이동로봇의 kinematic model은 위와 같다.

식에서 볼 수 있듯이 제어 입력은 $v$, $\omega$, 각각 선속도, 각속도이다.

하지만 제어 대상은 $\dot{x}$, $\dot{y}$, $\dot{\theta}$로 Underactuated system이다.

 

1-1. Desired model of mobile robot

이동로봇의 reference model은 다음과 같다.

$$ \begin{cases} \dot{x}_{d} = v_{d}\cos\theta_{d}\\ \dot{y}_{d} = v_{d}\sin\theta_{d}\\ \dot{\theta}_{d} = \omega _{d}\end{cases} $$

---

 

2. Define error

Lyapunov function을 설정하기 전 오차를 정의한다.

오차는 제어 목적을 달성하기 위한 오차를 정의한다.

$x \rightarrow x_{d}$, $y \rightarrow y_{d}$, $\theta \rightarrow \theta_{d}$로 보내기 위해 오차를 정의하게 되면 다음과 같다.

2-1.처럼 정의된 오차는 잘못된 오차임을 먼저 밝힌다.

---

 

2-1. Define of incorrect error

$$\begin{bmatrix} e_{1}\\ e_{2}\\ e_{3} \end{bmatrix} = \begin{bmatrix} x - x_{d}\\ y - y_{d}\\ \theta - \theta_{d} \end{bmatrix}$$

이렇게 정의될 경우 Underactuated system에서 제어는 어렵다.

(아래 논문 참조)

A locomotion control method for autonomous vehicles

 

따라서 논문에서 설명한 대로 오차를 다시 정의한다.

---

 

2-2. Define of correct error

 

논문의 식 (4)와 같이 오차를 정의하면 다음과 같다.

$$ \begin{bmatrix} e_{1}\\ e_{2}\\ e_{3}\\ \end{bmatrix} = \begin{bmatrix} \cos\theta & \sin\theta & 0\\ -\sin\theta & \cos\theta & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x - x_{d}\\ y - y_{d}\\ \theta - \theta_{d} \end{bmatrix}$$

 

이를 계산해 보면 아래와 같다.

$$ \begin{align} e_{1} &= \cos\theta \left( x - x_{d} \right) + \sin\theta \left( y - y_{d} \right)\\ e_{2} &= -\sin\theta \left( x - x_{d} \right) + \cos\theta \left( y - y_{d} \right)\\ e_{3} &= \theta - \theta_{d} \end{align} $$

 

각 오차를 시간에 대해 미분하면 다음과 같다.

\begin{align} \dot{e}_{1} &= -\dot{\theta}\sin\theta \left( x - x_{d} \right) + \cos\theta \left( \dot{x} - \dot{x}_{d} \right) +  \dot{\theta}\cos\theta \left( y - y_{d} \right) + \sin\theta \left( \dot{y} - \dot{y}_{d} \right)\\ &= -\dot{\theta} \sin\theta \left( x - x_{d} \right) + \cos\theta \left( v\cos\theta - v_{d}\cos\theta_{d} \right) + \dot{\theta}\cos\theta\left( y - y_{d} \right)  + \sin\theta \left( v\sin\theta - v_{d}\sin\theta_{d} \right)\\ &= \dot{\theta}\{ -\sin\theta \left( x - x_{d} \right) + \cos\theta \left( y - y_{d} \right) \} + v\left( \sin^{2}\theta + \cos^{2}\theta \right) - v_{d}\left(\cos\theta\cos\theta_{d} + \sin\theta\sin\theta_{d} \right)\\ &= \omega e_{2} + v - v_{d}\cos\left( \theta - \theta_{d} \right)\\ &= \omega e_{2} + v - v_{d}\cos e_{3}\\\\ \dot{e}_{2} &= -\dot{\theta}\cos\theta \left( x - x_{d} \right) - \sin\theta \left( \dot{x} - \dot{x}_{d} \right) -  \dot{\theta}\sin\theta \left( y - y_{d} \right) + \cos\theta \left( \dot{y} - \dot{y}_{d} \right)\\ &= -\dot{\theta} \cos\theta \left( x - x_{d} \right) - \sin\theta \left( v\cos\theta - v_{d}\cos\theta_{d} \right) - \dot{\theta}\sin\theta\left( y - y_{d} \right)  + \cos\theta \left( v\sin\theta - v_{d}\sin\theta_{d} \right)\\ &= \dot{\theta}\{ -\cos\theta \left( x - x_{d} \right) - \sin\theta \left( y - y_{d} \right) \} - v\sin\theta\cos\theta + \sin\theta\cos\theta + v_{d}\left(\sin\theta\cos\theta_{d} - \cos\theta\sin\theta_{d} \right)\\ &= - \omega e_{1} + v_{d}\sin\left( \theta - \theta_{d} \right)\\ &= - \omega e_{1} + v_{d}\sin e_{3}\\\\ \dot{e}_{3} &= \dot{\theta} - \dot{\theta}_{d}\\ &= \omega - \omega_{d} \end{align}

 

이때 $\dot{x}$, $\dot{y}$, $\dot{\theta}$는 이동로봇의 kinematic model, $\dot{x}_{d}$, $\dot{y}_{d}$, $\dot{\theta}_{d}$는 이동로봇의 reference model을 대입하면 된다.

또한 삼각함수 덧셈 공식을 활용하여 간단하게 나타낼 수 있다.

---

 

3. Lyapunov function

정의된 오차를 기반으로 Lyapunov function을 정의한다.

$$ \begin{align} V &= \frac{1}{2} \left( e_{1}^{2} + e_{2}^{2} \right) + \frac{1 - \cos e_{3}}{k_{2}} \end{align} $$

where, $k_{2}$ is positive control gain.

 

이를 시간에 대해 미분하면 다음과 같다.

$$ \begin{align} \dot{V} &= e_{1}\dot{e}_{1} + e_{2}\dot{e}_{2} + \frac{sin e_{3}}{k_{2}}\dot{e}_{3}\\ &= e_{1}\left( \omega e_{2} + v - v_{d}\cos e_{3} \right) + e_{2} \left( -\omega e_{1} + v_{d}\sin e_{3} \right) + \frac{\sin e_{3}}{k_{2}}\left( \omega - \omega_{d} \right) \end{align} $$

 

여기서 negative definite 함을 만족시키기 위한 제어 입력은 다음과 같다.

$$ \begin{align} v &= v_{d}\cos e_{3} - k_{1}e_{1}\\ \omega &= \omega_{d} - k_{3}\sin e_{3} - v_{d}k_{2}e_{2} \end{align} $$

where, $k_{1}$, $k_{2}$, $k_{3}$ is positive control gain.

 

변하지 않는 중요한 사실 중 하나는 제어 입력은 모두 알고 있는 값으로 구성되어야 한다.

위의 설계된 제어기는 모두 알고 있는 값이므로 문제없다.

 

이를 다시 time derivative of Lyapunov function에 대입하여 정리하면 아래와 같다.

$$ \begin{align} \dot{V} &= e_{1}\left( \omega e_{2} + v - v_{d}\cos e_{3} \right) + e_{2} \left( -\omega e_{1} + v_{d}\sin e_{3} \right) + \frac{\sin e_{3}}{k_{2}}\left( \omega - \omega_{d} \right)\\ &= e_{1}\left( \omega e_{2} + v_{d}\cos e_{3} - k_{1}e_{1} - v_{d}\cos e_{3} \right) + e_{2} \left( -\omega e_{1} + v_{d}\sin e_{3} \right) + \frac{\sin e_{3}}{k_{2}}\left( \omega_{d} - k_{3}\sin e_{3} - v_{d}k_{2}e_{2} - \omega_{d} \right)\\ &= \omega e_{1}e_{2} + e_{1}v_{d}\cos e_{3} - k_{1}e_{1}^{2} - e_{1}v_{d}\cos e_{3} - \omega e_{1}e_{2} + e_{2}v_{d}\sin e_{3} - \frac{k_{3}}{k_{2}}\sin^{2}e_{3} - e_{2}v_{d}\sin e_{3}\\ &= -k_{1}e_{1}^{2} - \frac{k_{3}}{k_{2}}\sin^{2}e_{3} \end{align} $$

---

 

4. MATLAB simulation

MATLAB simulation결과는 다음과 같다.

---

 

4-1 1st Trajectory

[Fig. 2] Trajectory1

[Fig. 3] Error1

[Fig. 4] Control input1

---

 

4-2. 2nd Trajectory

[Fig. 5] Trajectory2

[Fig. 6] Error2

[Fig. 7] Control input2

---

 

4-3. MATLAB code (.m file)

해당 simulation의 코드는 다음과 같다.

직접 m파일을 생성하여 돌려보면 된다.

clear; close all; clc;

%%
t0 = 0;                                                 % Define sampling time
ts = 0.01;
tf = 60;
t = t0:ts:tf;
iter = fix((tf-t0)/ts);

Xd = [0 0 pi/4];                                        % Define state Xd = [xd yd θd]
X = [2 0 pi/4];                                         % Define state X = [x y θ]

%%
% Define gain %
k = [2 2 5];

%%
for i = 1:iter

% define desired value
    if i < 500
        vd(i) = 0.25*(1-cos(pi*t(i)/5));
        wd(i) = 0;
    elseif i < 2500
        vd(i) = 0.5;
        wd(i) = 0;
    elseif i < 4500
        vd(i) = 0.5;
        wd(i) = 0.2*cos(2*pi*t(i)/20);
    else
        vd(i) = 0.5;
        wd(i) = 0;
    end

%      vd(i) = 0.5;
%      wd(i) = 0.1;

    ud(i,:) = [vd(i) wd(i)];                            % Desired velocity

% define error
    e(i,:) = [cos(X(i,3)) sin(X(i,3)) 0 ; -sin(X(i,3)) cos(X(i,3)) 0 ; 0 0 1]*[X(i,1)-Xd(i,1) ; X(i,2)-Xd(i,2) ; X(i,3)-Xd(i,3)];

% control input
    v(i) = vd(i)*cos(e(i,3))-k(1)*e(i,1);
    w(i) = wd(i)-k(3)*sin(e(i,3))-vd(i)*k(2)*e(i,2);
    u(i,:) = [v(i) w(i)];

% update
    Xd(i+1,:) = Xd(i,:) + [ud(i,1)*cos(Xd(i,3)) ud(i,1)*sin(Xd(i,3)) ud(i,2)]*ts;
    X(i+1,:) = X(i,:) + [u(i,1)*cos(X(i,3)) u(i,1)*sin(X(i,3)) u(i,2)]*ts;
end

%%
figure('Name', 'Trajectory', 'color', 'w');             % Plot trajectory
plot(Xd(1:iter,1), Xd(1:iter,2), 'r', 'linewidth', 1.5);
hold on
plot(X(1:iter,1), X(1:iter,2), 'b--', 'linewidth', 1.5);
hold off
legend({'Reference','Mobile robot'}, 'Fontsize', 10, 'Box', 'off','Location', 'southeast');
xlabel('X [m]', 'FontSize', 12);
ylabel('Y [m]', 'FontSize', 12);
title('Trajectory', 'FontSize', 18);

figure('Name', 'Error','color',' w');                   % Plot error
plot(t(1:iter), e(1:iter,1), 'r--', 'linewidth', 1.5)
hold on
plot(t(1:iter), e(1:iter,2), 'b', 'linewidth', 1.5);
hold on
plot(t(1:iter), e(1:iter,3), 'k-.', 'linewidth', 1.5);
hold off
legend({'e_{1}(t)','e_{2}(t)','e_{3}(t)'}, 'Fontsize', 10, 'Box', 'off');
xlabel('Time [s]', 'FontSize', 12);
ylabel('Errors', 'FontSize', 12);
title('Errors', 'FontSize', 18);

figure('Name', 'Control input', 'color',' w');          % Plot control input
plot(t(1:iter), u(1:iter,1), 'r', 'linewidth', 1.5);
hold on
plot(t(1:iter), u(1:iter,2), 'b', 'linewidth', 1.5);
hold off
legend({'v', '\omega'}, 'Fontsize', 10, 'Box', 'off');
xlabel('Time [s]', 'FontSize', 12);
ylabel('Velocity', 'FontSize', 12);
title('Control input', 'FontSize', 18);

---

 

# 마치며

피에로 아저씨 논문 참고

https://onlinelibrary.wiley.com/doi/abs/10.1002/(SICI)1097-4563(199703)14:3%3C149::AID-ROB1%3E3.0.CO;2-R?casa_token=x0byPeSDKaIAAAAA:1URANnzYjI6LlE_TLvhrs5XzTHTcfds2JoNecetcdRjTigoJjs4rAKM--FFo9R7CZk8Iy8-PCyivn-xsNg