Segway control¶

$$ \newcommand{\eg}{{\it e.g.}} \newcommand{\ie}{{\it i.e.}} \newcommand{\argmin}{\operatornamewithlimits{argmin}} \newcommand{\mc}{\mathcal} \newcommand{\mb}{\mathbb} \newcommand{\mf}{\mathbf} \newcommand{\minimize}{{\text{minimize}}} \newcommand{\diag}{{\text{diag}}} \newcommand{\cond}{{\text{cond}}} \newcommand{\rank}{{\text{rank }}} \newcommand{\range}{{\mathcal{R}}} \newcommand{\null}{{\mathcal{N}}} \newcommand{\tr}{{\text{trace}}} \newcommand{\dom}{{\text{dom}}} \newcommand{\dist}{{\text{dist}}} \newcommand{\R}{\mathbf{R}} \newcommand{\SM}{\mathbf{S}} \newcommand{\ball}{\mathcal{B}} \newcommand{\bmat}[1]{\begin{bmatrix}#1\end{bmatrix}} \newcommand{\loss}{\ell} \newcommand{\eloss}{\mc{L}} \newcommand{\abs}[1]{| #1 |} \newcommand{\norm}[1]{\| #1 \|} \newcommand{\tp}{T} $$

Jong-Han Kim (jonghank@inha.ac.kr)

Seungyeop Lee (seungyeoplee@inha.edu)

---

Problem Formulation¶

A Segway is given with the following parameters.

$$ \begin{align*} &r &&:\text{the radius of the wheel}\\ &d &&:\text{the distance from the wheel's center to the center of mass}\\ &\theta &&:\text{The angle of the rod with respect to the y-axis}\\ &\phi &&:\text{The angle of the wheel with respect to the y-axis}\\ &\tau &&:\text{The torque applied to the wheel} \end{align*} $$

---

Segway Dynamic Simulation¶

We have to implement a dynamic simulation of the given Segway before designing the control systems.

---

Segway dynamic model¶

Ignoring slips of a wheel, the position of the center of a wheel and rod can be calculated as bellow.

$$\;\\ p_{\rm w}=\begin{bmatrix}r\phi \\ 0\end{bmatrix}, \quad p_{\rm r}=\begin{bmatrix}r\phi+{d\over2}\sin{\theta} \\ {d\over2}\cos{\theta}\end{bmatrix}\;\\ $$

And the velocities are as follows.

$$\;\\ \dot{p}_{\rm w}=\begin{bmatrix}r\dot{\phi} \\ 0\end{bmatrix}, \quad \dot{p}_{\rm r}=\begin{bmatrix}r\dot{\phi}+{d\over2}\dot{\theta}\cos{\theta}\\ -{d\over2}\dot{\theta}\sin{\theta}\end{bmatrix}\;\\ $$

Now, let's examine the angles $ \phi $ and $ \theta $ of the wheel and the rod. By calculating the energy of the system, including the wheel and the rod, we can use the Euler-Lagrange equation to obtain the model.

The kinetic energy of the wheel and the rod, $ T_{\rm w} $ and $ T_{\rm r} $, consists of both rotational and translational kinetic energy, as follows.

$$\;\\ \begin{align*} T_{\rm w, rot} &= {1\over2}I_{\rm w}\dot{\phi}^2={1\over2}\Big({1\over2}m_{\rm w}r^2\Big)\dot{\phi}^2\\ T_{\rm w, trn} &= {1\over2}m_{\rm w}(r\dot{\phi})^2\\\\ T_{\rm r, rot} &= {1\over2}I_{\rm r}\dot{\theta}^2={1\over2}\Big({1\over12}m_{\rm r}d^2\Big)\dot{\theta}^2\\ T_{\rm r, trn} &= {1\over2}m_{\rm r}\Big( \Big(r\dot{\phi}+{d\over2}\dot{\theta}\sin{\theta}\Big)^2 + \Big({d\over2}\dot{\theta}\sin{\theta}\Big)^2 \Big)\\\\ \end{align*}\;\\ $$

And the total kinetic energy of the wheel and the rod can be calculated as follows.

$$\;\\ T = T_{\rm w,rot} + T_{\rm w,trn} + T_{\rm r,rot} + T_{\rm r,trn}\;\\ $$

The potential energy of the wheel and the rod is as follows. Since the origin is located on the same horizontal line as the wheel axis, the potential energy of the wheel is zero.

$$\;\\ \begin{align*} U_{\rm w} &= 0\\ U_{\rm r} &= m_{\rm r}g{d\over2}\cos{\theta}\\ \end{align*}\;\\ $$

Similarly, the potential energy of the wheel and the rod can be calculated as follows.

$$\;\\ U = U_{\rm w} + U_{\rm r}\;\\ $$

The Lagrangian $ L $ is defined as a difference between the two energies as follows.

$$\;\\ L=T-U\;\\ $$

Using this, we can construct the Euler-Lagrange equations as follows. The non-conservative force, torque $ \tau $, acts on the wheel, while the reaction force $ -\tau $ acts on the rod.

$$\;\\ \begin{align*} {d\over dt}\Big( { \partial L \over \partial \dot{\phi} } \Big) - { \partial L \over \partial {\phi} } &= \tau\\ {d\over dt}\Big( { \partial L \over \partial \dot{\theta} } \Big) - { \partial L \over \partial {\theta} } &= -\tau \end{align*}\;\\ $$

By expanding and simplifying the above Euler-Lagrange equations, we obtain the following dynamics model for the Segway system.

$$\;\\ \begin{align*} { \partial L \over \partial \dot{\phi} } &= ( {3\over2}m_{\rm w}r^2 + m_{\rm r}r^2 ) \dot{\phi} + {1\over2}m_{\rm r}rd\cos{\theta}\dot{\theta}\\ { \partial L \over \partial {\phi} } &= 0\\ { \partial L \over \partial \dot{\theta} } &= {1\over3}m_{\rm r}d^2\dot{\theta}+{1\over2}m_{\rm r}rd\cos{\theta}\dot{\theta}\\ { \partial L \over \partial {\theta} } &= -{1\over2}m_{\rm r}rd\sin{\theta}\dot{\phi}\dot{\theta}+{1\over2}m_{\rm r}dg\sin{\theta}\\\\ \end{align*} $$$$ \Rightarrow \begin{cases} ( {3\over2}m_{\rm w}r^2 + m_{\rm r}r^2 ) \ddot{\phi} + {1\over2}m_{\rm r}rd(-\sin{\theta}\dot{\theta}^2+\cos{\theta}\ddot{\theta})=\tau\\\\ {1\over3}m_{\rm r}d^2\ddot{\theta}+{1\over2}m_{\rm r}rd\cos{\theta}\ddot{\phi}-{1\over2}m_{\rm r}dg\sin{\theta} = -\tau \end{cases}\\\;\\ $$$$ \Leftrightarrow \begin{bmatrix} {1\over2}m_{\rm r }rd\cos{\theta}& {3\over2}m_{\rm w}r^2 + m_{\rm r}r^2\\ {1\over3}m_{\rm r}d^2&{1\over2}m_{\rm r }rd\cos{\theta} \end{bmatrix} \begin{bmatrix} \ddot{\theta}\\ \ddot{\phi} \end{bmatrix}= \begin{bmatrix} \tau + {1\over2}m_{\rm r}rd\sin{\theta}\dot{\theta}^2\\ -\tau + {1\over2}m_{\rm r}dg\sin{\theta} \end{bmatrix} $$

By solving the linear equation above( i.e. $x^*=A^{-1}b$ ), we can obtain the $[\;\ddot\theta \quad \ddot\phi \;]^T$.

Now, let's implement the dynamic model. The state vector $x$ and control input $u$ is defined as follows.

$$ x:= \begin{bmatrix} \theta\\ \phi\\ \dot\theta\\ \dot\phi \end{bmatrix},\quad u=\tau $$

With following model,

$$ \dot x= \begin{bmatrix} \dot\theta\\ \dot\phi\\ \ddot\theta\\ \ddot\phi \end{bmatrix}= \begin{bmatrix} f_1(x,u)\\ f_2(x,u)\\ f_3(x,u)\\ f_4(x,u) \end{bmatrix}= f(x,u) $$

We can calculate the state of the Segway over time by numerically integrating the differential equation.

$$ x(t) = \int^t_{t_0}f(x,u) dt $$

In the given code, we can see that the run_simulation() function numerically integrates the Segway's dynamic model segway_dynamics() through the propagate_state() function.

(Problem 1) Complete the segway_dynamics() function based on the dynamic model shown above. The segway_dynamics() function takes state variables and control inputs as inputs and outputs the state change $ \dot{x} $, effectively functioning as $ \dot x = f(x, u) $.

(Problem 2) Several methods have been introduced for numerical integration, such as the Euler method, the Trapezoidal method, etc. Among them, the Runge-Kutta integration is known as a simple and accurate method. Complete the propagate_state() function based on the given 4-th order Runge-Kutta integration given as follows.

$$ x_{k+1} = x_{k} + {1\over6}(K_1+2K_2+2K_3+K_4)\\ \begin{align*} \text{where, }K_1 &= \Delta t \cdot f(x_{k}, \tau_k)\\ K_2 &= \Delta t \cdot f(x_{k}+K_1/2, \tau_k)\\ K_3 &= \Delta t \cdot f(x_{k}+K_2/2, \tau_k)\\ K_4 &= \Delta t \cdot f(x_{k}+K_3, \tau_k) \end{align*} $$

In the Segway model, the wheel and rod masses are set to 1 kg, the wheel radius is 1 m, and the rod length is 3 m. The system is equipped with a wheel encoder to measure the wheel rotation speed $ \dot{\phi} $ and a gyro sensor to measure the tilt angle $ \theta $ of the rod. These measurements can be used to control the Segway's motion.

You can simulation its kinematics as following codes.

---

Segway Controller Design¶

Pendulum balancing control¶

Let's design a control system to maintain an angle of the Segway's rod. In other words, the control command is the desired rod angle $ \theta_{\rm cmd} $, and the control input is the torque $ \tau $ applied to the wheel.

To design the controller, we need to obtain the relationship between the wheel torque $ \tau $ and the rod angle $ \theta $, i.e., the transfer function $ \frac{\theta(s)}{\tau(s)} $, based on the previously modeled Segway dynamics. Since the given model is nonlinear, we need to linearize it to simplify the analysis.

Assuming that $ \theta $ and $ \dot{\theta} $ are sufficiently small, we can simplify the model as follows.

$$\;\\ \approx \begin{bmatrix} {1\over2}m_{\rm r }rd& {3\over2}m_{\rm w}r^2 + m_{\rm r}r^2\\ {1\over3}m_{\rm r}d^2&{1\over2}m_{\rm r }rd \end{bmatrix} \begin{bmatrix} \ddot{\theta}\\ \ddot{\phi} \end{bmatrix}= \begin{bmatrix} \tau\\ -\tau + {1\over2}m_{\rm r}dg\theta \end{bmatrix}\\ \overset{\rm let}{=} \begin{bmatrix} m_1 & m_2\\ m_3 & m_4 \end{bmatrix} \begin{bmatrix} \ddot{\theta}\\ \ddot{\phi} \end{bmatrix}= \begin{bmatrix} \tau\\ -\tau + b\theta \end{bmatrix}\;\\ $$

By expanding and simplifying the linearized model, we obtain the following expression.

$$\;\\ \begin{align*} \begin{bmatrix} \ddot{\theta}\\ \ddot{\phi} \end{bmatrix} &= \begin{bmatrix} m_1 & m_2\\ m_3 & m_4 \end{bmatrix}^{-1} \begin{bmatrix} \tau\\ -\tau + b\theta \end{bmatrix}\\ &={1\over m_1m_4-m_2m_3} \begin{bmatrix} (m_2+m_4)\tau - m_2b\theta\\ -(m_1+m_3)\tau + m_1b\theta \end{bmatrix} \end{align*}\;\\ $$

From the linearized model, the transfer function $ \frac{\theta(s)}{\tau(s)} $ can be obtained as follows.

$$\;\\ \begin{align*} \ddot{\theta}&={1\over m_1m_4-m_2m_3}\Big((m_2+m_4)\tau - m_2b\theta\Big)\\\\ \Rightarrow &G_{\theta}(s):={\theta(s)\over\tau(s)}={m_2+m_4 \over (m_1m_4-m_2m_3)s^2+m_2b} \end{align*}\;\\ $$

And let’s assume that the actual torque $ \tau $ has a first-order delay relationship with the torque command $ \tau_{\rm cmd} $, given as follows.

$$ G_{\tau}(s) = {\alpha_{\tau} \over s + \alpha_{\tau}} $$

For the plant described above, adding a PD controller to track the angle command $ \theta_{\rm cmd} $, the control system can be configured as follows.

Now, let's analyze the stability of the control system. We can express the system as a product of the gain and the transfer function, as follows.

Where, the gain and transfer function are as follows.

$$ K:= \displaystyle {(m_2 + m_4)\alpha_\tau K_d \over m_1m_4 - m_2m_3} , \quad G(s):={s + K_p/K_d \over s^3 + \alpha_\tau s^2 + (m_2b/(m_1m_4 - m_2m_3)) s + \alpha_\tau m_2 b / (m_1m_4 - m_2m_3)} $$

By plotting the root locus of the transfer function $G(s)$, we can observe the locations of the closed-loop poles as a function of the gain $K$. The root locus will show that the PD controller has the effect of adding a zero at $(-K_p/K_d,0)$ in the root locus. This added zero influences the trajectory of the poles, improving the system's performance. Specifically, this zero helps to adjust the locations of the poles for better stability and responsiveness, ensuring the system reaches the desired response more quickly and accurately.

(Problem 3) With given transfer function in the block diagram, draw the root locus of it.

$$ G(s)={s + K_p/K_d \over s^2 + m_2b/(m_1m_4 - m_2m_3)} $$

( Note. Use ct.rlocus() function. )

In the root locus, the poles marked in red represent the positions of the poles at a specific gain. By adjusting the gain, we can find the appropriate position for the poles. Once the desired pole locations are identified, the corresponding gains can be determined, and the PD controller's $ K_p $ and $ K_d $ values can be set accordingly to achieve the desired system performance.

$$\;\\ \begin{align*} K_d^*&= \displaystyle {m_1m_4 - m_2m_3 \over (m_2 + m_4)\alpha_\tau } \cdot K^*\\\\ K_p^*&= {(K_p / K_d)} \cdot K_d^* \end{align*}\;\\ $$

(Problem 4) Implement the PD controller with the gains determined from the root locus as shown above.

---

Segway speed control¶

Next, let's design the controller for controlling the speed of the Segway. As mentioned earlier, when slip is neglected, the relationship between the wheel speed and the wheel's angular velocity is given by

$$ \dot{p}_{\rm w}=r\dot{\phi} $$

In other words, speed control is essentially the same as controlling the wheel's angular velocity. The linearized dynamics of the wheel's angular velocity had been obtained as follows.

$$\;\\ \begin{align*} \ddot{ϕ} &= {1\over m_1m_4-m_2m_3} (-(m_1+m_3)\tau + m_1b\theta) \end{align*}\;\\ $$

However, upon closer inspection, we can see that $\ddot{\phi}$ depends on both $\tau$ and $\theta$. This type of system, known as a multi-input single-output (MISO) system, is difficult to analyze using methods like root locus, as we have done previously. (if you try with a simple classical controller, you will quickly realize that it is quite challenging.) Therefore, it is necessary to use a more convenient method for analysis.

One of the methods we can apply is Linear quadratic regulator (LQR), which is based on linear model defined with state space. First, let's define a state vector as follows.

$$ x:= \begin{bmatrix} \theta\\ \dot\theta\\ \dot\phi \end{bmatrix}$$

With the $x$, we can represent the linearized model as follows.

$$ \begin{align*} \dot x &= \begin{bmatrix} \dot\theta\\ \ddot\theta\\ \ddot\phi \end{bmatrix}= \underset{A} { \begin{bmatrix} 0 & 1 & 0 \\ -{m_2b\over m_1m_4-m_2m_3}&0&0\\ {m_1b\over m_1m_4-m_2m_3}&0&0 \end{bmatrix} } \underset{x} { \begin{bmatrix} \theta\\ \dot\theta\\ \dot\phi \end{bmatrix} }+ \underset{B} { \begin{bmatrix} 0\\ {m_2+m_4\over m_1m_4-m_2m_3}\\ -{m_1+m_3\over m_1m_4-m_2m_3} \end{bmatrix} } \tau \end{align*} $$

By using state-space representation, we can reduce the dimensionality, resulting in a more compact form of the equations.

LQR (Linear Quadratic Regulator) is an optimal control technique that calculates the control input $ u $ to minimize the following objective function, while satisfying the dynamics $ \dot{x} = Ax + Bu $.

$$ J = \int^\infty_{0} ( x^TQx + u^TRu )dt $$

Matrices $ Q $ and $ R $ are weighting matrices, which are user-defined parameters that determine the degree of state regulation and control effort conservation, respectively.

The optimal control input for this LQR problem is given as follows.

$$\\\;\\ u^* = -K^*x\\\;\\ \begin{align*} \text{where, } K^* &= R^{-1} B^T P^*,\\ P^*&\text{ is a solution of Riccati equation :}\\ &A^T P + P A - P B R^{-1} B^T P + Q = 0 \end{align*}\\\;\\ $$

Now, let’s apply this LQR control to our problem. Our goal is to control the wheel rotational speed, $\dot{\phi}$, so that it matches the command speed $\dot{\phi}_{\rm cmd}$. In other words, we want to regulate the error $\dot{\phi} - \dot{\phi}_{\rm cmd}$ to zero. Therefore, let’s define the error variable as follows.

$$ x_{\rm err}:=x-x_{\rm cmd}= \begin{bmatrix} \theta\\ \dot\theta\\ \dot\phi - \dot\phi_{\rm cmd} \end{bmatrix}\\ \downarrow\\ \dot x_{\rm err} = Ax_{\rm err} + Bu $$

And since we only want to control $\dot{\phi}_{\text{err}}$, we can define the weighting matrix to focus solely on $\dot{\phi}_{\text{err}}$ as follows.

$$\\\;\\ Q= \begin{bmatrix} 0&0&0\\ 0&0&0\\ 0&0&0.5 \end{bmatrix}\\\;\\ $$

Additionally, increasing $R$ results in less control input usage, making the response slower, while decreasing $R$ allows for more liberal use of control input, leading to a higher responsiveness in the controller. let's set to 1 for this example.

$$ R = 1 $$

Once the LQR problem is solved and the optimal gain $ K^* $ is obtained, the optimal control input $ u^* $ can be computed as follows:

$$ u^* = -K^*x_{\rm err} $$

(Problem 5) Complete the linear model $A,B,C,D$ and the LQR gain $Q,R$ in the code below.

( Note. Although $R $ is a scalar, it should be passed as a (1, 1) matrix when input into the function. )

(Problem 6) Implement a velocity controller using these optimal gains.

( Hint. The $ \dot{\theta} $ is not a directly measurable variable. However, we can calculate it by numerically differentiating the measurable variable $ \theta $. )

---

Pendulum balancing control for noisy measurements¶

Now, let's examine the case where noise is mixed into the measurements. Let's assume that the IMU and encoder measurements are each affected by Gaussian noise with variances $\sigma_{\rm IMU}^2=0.3^2\; {\rm rad^2}$ and $\sigma_{\rm enc}^2 =0.3^2 \;{\rm (rad/s)^2}$, respectively.

The results with noise included are as follows.

The most basic filter is the first-order filter, which is defined as follows.

$$ F(s) = { K_f \over \tau_f s + 1} $$

(Problem 7) Implement the first-order filter. And simulate with $K_f=1,\tau_f=0.003$.**

( Note. Refer to the appendix. It is normal for the result with the filter to be similar to or slightly worse than the result without the filter. )

As seen in the results, even though a filter is applied, the delay introduced by the filter actually reduces the stability of the control. In such cases, filters that are more robust to delay while still effectively filtering out noise are recommended. Examples of such filters include second-order filters and Kalman filters.

---

Appendix¶

Eular-Lagrange Equation¶

A well-known method for general dynamical modeling is Newtonian mechanics, which is based on equations of motion such as $ F = ma $. In contrast, Lagrangian mechanics views the same physical phenomena from the perspective of Hamilton's "least action principle".

The principle of least action states that physical phenomena occur in such a way that the action is minimized. By applying this principle mathematically through variation, which involves changes in the function values based on variations in the independent variables rather than time, we can derive equations that describe physical phenomena.

This equation is known as the Euler-Lagrange equation, which, in the case of conservative forces, takes the following form.

$$ \begin{align*} {d\over dt}\Big( { \partial L \over \partial \dot{x}_i } \Big) - { \partial L \over \partial x_i } &= 0 \end{align*} $$

$x$ represents the state variable, and $L$ is the Lagrangian, which is defined as the difference between the kinetic energy $T$ and the potential energy $U$, as follows.

$$ L = T-U $$

From the above equation, a dynamic model can be derived, and it has the advantage of being simpler to model compared to Newtonian mechanics. This is because Lagrangian mechanics often leads to more straightforward formulations, especially in systems with constraints or complex configurations.

---

Z-transform¶

The dynamic models and controllers introduced above are all defined in continuous time. However, since the data in a computer is digital, the continuous time models and controllers must be "discretized" for implementation in digital systems.

Similar to the $s$-domain, the $z$-domain is a complex plane used for analyzing systems, but it differs in that it is specifically designed for discrete signals. While the $s$-domain handles continuous-time systems and is based on differential equations, the $z$-domain is used to work with difference equations, which govern discrete-time systems.

The transformation from the $s$-domain to the $z$-domain can be done using various methods. One commonly used method, due to its simplicity and minimal error introduced by discretization, is the "bilinear transform". The bilinear transform maps the entire s-plane onto the z-plane and is defined as follows:

$$ s = {2\over T}\cdot{1 - z^{-1} \over 1 + z^{-1}} $$

where:

• $ s $ is the complex frequency variable in the $s$-domain,
• $ T $ is the sampling period (the inverse of the sampling rate),
• $ z $ is the complex frequency variable in the $z$-domain.

This transformation preserves the stability of the system and maps the left half of the s-plane (which corresponds to stable systems) to the inside of the unit circle in the z-plane. It is widely used because it approximates the continuous-time behavior while keeping the system stable and minimizing the error between the continuous and discrete representations.

Let's take an example. Suppose the following first-order filter transfer function is given, and we will convert it to the $z$-domain.

$$ G(s) = {X_F(s) \over X(s)} = { \alpha \over s + \alpha } $$

Substituting the previously introduced bilinear transformation relationship between s and z, we get the following.

$$ G(z) = {X_F(z) \over X(z)} = { \alpha z^{-1} + \alpha \over (\alpha - 2/T) z^{-1} + 2/T + \alpha } \overset{\rm let}{=} { a_1 z^{-1} + a_0 \over b_1 z^{-1} + b_0 } $$

From the transfer function in the $z$-domain, we can derive the relationship between the input and output as follows. The $[n]$ represents the current data at the $n$-th time step, and $[n-1]$ represents the data from one step prior, indicating a 1-step delay.

$$ b_1x_f[n-1]+b_0 x_f[n] = a_1x[n-1]+a_0 x[n]\\ \downarrow\\ x_f[n] = {a_1\over b_0}x[n-1]+{a_0\over b_0} x[n] - {b_1\over b_0} x_f[n-1] $$

This would be pretty much easier to implement.

---