## 4 Control Design
### Linear Time-Varying Model Approximation  
At first, the second order differential equation of motion (4) is transformed into a first order system yielding  
\[\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x},\mathbf{u})=\left[ \begin{array}{c}\dot{\mathbf{q}}\\ \mathbf{M}^{-1}(\mathbf{q})\left(\mathbf{h}(\mathbf{q},\dot{\mathbf{q}})-\mathbf{k}(\mathbf{q},\dot{\mathbf{q}} )\right)\end{array} \right]+\left[ \begin{array}{c}\mathbf{0}\\ \mathbf{M}^{-1}(\mathbf{q})\mathbf{D}\end{array} \right]\mathbf{u}, \tag{14}\]  
where the state vector \(\mathbf{x}=\left[\mathbf{q}^{T},\dot{\mathbf{q}}^{T}\right]^{T}\in\mathbb{R}^{n}\) consists of the generalized coordinates \(\mathbf{q}\) and velocities \(\dot{\mathbf{q}}\) with \(n=2\,n_{\text{DOF}}\). The function \(\mathbf{f}:\mathbb{R}^{n}\times\mathbb{R}^{m}\to\mathbb{R}^{n}\) describes the system dynamics as a function of the states \(\mathbf{x}\) and inputs \(\mathbf{u}\in\mathbb{R}^{m}\). With regard to safety requirements of the motor currents or mechanical limitations like limited space or velocities, system (14) is subject to state and input constraints stated as  
\[\mathbf{x}(t)\in\mathcal{X},\qquad\mathbf{u}(t)\in\mathcal{U}, \tag{15}\]  
Figure 6: Workflow for trajectory planning using Inkscape, Matlab, and Neweul-M\({}^{2}\). where \(\mathcal{X}\subseteq\mathbb{R}^{n}\) and \(\mathcal{U}\subseteq\mathbb{R}^{m}\) are polytopes. In case of simple limitations, the sets \(\mathcal{X}\) and \(\mathcal{U}\) are given by box constraints of the form  
\[\mathcal{X}=\{\mathbf{x}\in\mathbb{R}^{n}\mid\mathbf{x}_{\min}\leq\mathbf{x}\leq\mathbf{x}_{ \max}\}\quad\text{and}\quad\mathcal{U}=\{\mathbf{u}\in\mathbb{R}^{m}\mid\mathbf{u}_{ \min}\leq\mathbf{u}\leq\mathbf{u}_{\max}\}. \tag{16}\]  
Assuming that the actual state and input of the system is in a neighborhood of the desired trajectory and feed-forward controller, the nonlinear system (14) can be linearized with respect to the desired trajectory \(\mathbf{x}_{\text{des}}(t)=\left[\mathbf{q}_{\text{des}}(t)^{\text{T}},\dot{\mathbf{q}}_{ \text{des}}(t)^{\text{T}}\right]^{\text{T}}\) and the feed-forward controller \(\mathbf{u}_{\text{ff}}(t)\) from Equation (12) resulting in the LTV system  
\[\Delta\dot{\mathbf{x}}(t)=\mathbf{A}(t)\Delta\mathbf{x}(t)+\mathbf{B}(t)\Delta\mathbf{u}(t), \tag{17}\]  
where the time-varying system matrices \(\mathbf{A}(t)\in\mathbb{R}^{n\times n}\), \(\mathbf{B}(t)\in\mathbb{R}^{n\times m}\) and the deviations \(\Delta\mathbf{x}(t)\), \(\Delta\mathbf{u}(t)\) are given as  
\[\mathbf{A}(t)=\frac{\partial\mathbf{f}}{\partial\mathbf{x}}\bigg{|}_{\mathbf{x}_{\text{des}} (t),\;\mathbf{u}_{\text{ff}}(t)}\,\qquad\mathbf{B}(t)=\frac{\partial\mathbf{f}}{\partial\mathbf{u}} \bigg{|}_{\mathbf{x}_{\text{des}}(t),\;\mathbf{u}_{\text{ff}}(t)}\, \tag{18}\]  
\[\Delta\mathbf{x}(t)=\mathbf{x}(t)-\mathbf{x}_{\text{des}}(t)\,\qquad\Delta\mathbf{u}(t)=\mathbf{ u}(t)-\mathbf{u}_{\text{ff}}(t). \tag{19}\]  
The continuous LTV system (17) describes the dynamics of the deviations of the nonlinear system (14) from the desired trajectory \(\mathbf{x}_{\text{des}}(t)\) and feed-forward controller \(\mathbf{u}_{\text{ff}}(t)\). Note that in the presence of the equations of motion in symbolical form, the previous calculations can be done easily by the Matlab Symbolic Math Toolbox. Implementing a controller on a digital control unit leads indispensably to discretization. Consequently, the desired trajectory and the feed-forward controller have to be sampled and the continuous LTV system (17) has to be discretized subject to the sampling interval \(\Delta T\) of the real-time process control system. Therefore, consider the time discretization \(t_{k}=k\Delta T\) with sampling instance \(k=0,1,2,\ldots,t_{\text{f}}/\Delta T\). For notational simplification, let \(\Delta\mathbf{x}(k)=\Delta\mathbf{x}(t_{k})\), \(\Delta\mathbf{u}(k)=\Delta\mathbf{u}(t_{k})\), and so on.