*2.2. MPC Algorithm*

The MPC algorithm selects trajectories for the mobile base and the robot arm task space that follow a two-dimensional reference, while minimizing an objective function described in this section. The optimization is performed over a prediction horizon *N*, with fixed discretization time steps *T*. Although the velocities of the mobile base and the robot arm are used for low-level control (as seen in Figure 3), MPC operates in the acceleration space to achieve the desired behavior. The system state and control inputs in the *k*-th step, *x*(*k*) and *u*(*k*), respectively, are

$$\mathbf{x}(k) = \begin{bmatrix} p\_{B,x}(k) \\ p\_{A,x}(k) \\ p\_{S,z}(k) \\ \dot{p}\_{B,x}(k) \\ \dot{p}\_{A,x}(k) \\ \dot{p}\_{S,z}(k) \end{bmatrix} \in \mathbb{R}^6 \quad \mathfrak{u}(k) = \begin{bmatrix} \dot{p}\_{B,x}(k) \\ \dot{p}\_{A,x}(k) \\ \dot{p}\_{S,z}(k) \end{bmatrix} \in \mathbb{R}^3 \tag{2}$$

System state consists of positions *pB*,*x*(*k*), *pA*,*x*(*k*) and *pS*,*z*(*k*), seen in Figure 2, as well as the corresponding velocities. System inputs are the accelerations of the spray frame *p*¨*B*,*x*, *p*¨*A*,*<sup>x</sup>* and *p*¨*S*,*z*. The output of the system is a two-dimensional position of the spray frame *LS*, relative to the global frame *LG*. The output of the system in the *k*-th step *y*(*k*) is

$$y(k) = \begin{bmatrix} p\_{\mathcal{B}, \mathbf{x}}(k) + p\_{A, \mathbf{x}}(k) \\ p\_{S, z}(k) \end{bmatrix} \in \mathbb{R}^2 \tag{3}$$

As already mentioned, the *z* component of the spray frame position *pS*,*<sup>z</sup>* is controlled solely by the robot arm, and the *x* component is controlled by both the robot arm and the mobile base (Equation (1)).

The system is therefore linear and its discretization results in the following system dynamics:

$$\mathbf{x}(k+1) = \begin{bmatrix} 1 & 0 & 0 & T & 0 & 0 \\ 0 & 1 & 0 & 0 & T & 0 \\ 0 & 0 & 1 & 0 & 0 & T \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \mathbf{x}(k) + \begin{bmatrix} \frac{T^2}{2} & 0 & 0 \\ 0 & \frac{T^2}{2} & 0 \\ 0 & 0 & \frac{T^2}{2} \\ T & 0 & 0 \\ 0 & T & 0 \\ 0 & 0 & T \end{bmatrix} \mathbf{u}(k) \tag{4}$$
 
$$\mathbf{A} \tag{5}$$

$$\mathbf{y}(k) = \begin{bmatrix} 1 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \end{bmatrix} \mathbf{x}(k) \tag{5}$$

$$\mathbf{C}$$

MPC optimizes over a system input vector *U* containing *N* control inputs,

$$\mathcal{U} = \begin{bmatrix} \mathfrak{u}(0) \\ \mathfrak{u}(1) \\ \vdots \\ \mathfrak{u}(N-1) \end{bmatrix} \in \mathbb{R}^{3N} \tag{6}$$

The system state and output vectors *X* and *Y* are, respectively,

$$\mathbf{X} = \begin{bmatrix} \mathbf{x}(0) \\ \mathbf{x}(1) \\ \vdots \\ \mathbf{x}(N-1) \end{bmatrix} \in \mathbb{R}^{6N}, \ \mathbf{Y} = \begin{bmatrix} \mathbf{y}(0) \\ \mathbf{y}(1) \\ \vdots \\ \mathbf{y}(N-1) \end{bmatrix} \in \mathbb{R}^{2N} \tag{7}$$

The MPC optimization problem is defined as the following QP problem:

$$\begin{aligned} \min\_{\mathbf{U}} \quad & \left\| \mathbf{W}\_{\mathbf{U}} \| \mathbf{Y} - \mathbf{Y}\_{d} \right\|^{2} + \left\| \left\| \mathbf{W}\_{u} \mathbf{U} \right\| \right\|^{2} + \left\| \left\| \mathbf{W}\_{x} \mathbf{X} \right\| \right\|^{2} \\ \text{s.t.} \quad & \mathbf{x}(k+1) = \mathbf{A} \mathbf{x}(k) + \mathbf{B} \mathbf{u}(k) \\ & \mathbf{y}(k) = \mathbf{C} \mathbf{x}(k) \\ & \mathbf{x}(0) = \mathbf{x}\_{0} \\ & \underline{\mathbf{x}} \le \mathbf{x}(k) \le \overline{\mathbf{x}} \\ & \underline{\mathbf{u}} \le \mathbf{u}(k) \le \overline{\mathbf{u}} \end{aligned} \tag{8}$$

where *Y<sup>d</sup>* is the desired system output and *Wy* is the reference tracking weight. *W<sup>u</sup>* and *W<sup>x</sup>* are control input and system state weight matrices, respectively.

The reference tracking part of the criterion function (*Wy <sup>Y</sup>* <sup>−</sup> *<sup>Y</sup><sup>d</sup>* <sup>2</sup> ) is used to ensure that the system output follows the reference trajectory for the spray frame. The second part ( *<sup>W</sup>u<sup>U</sup>* <sup>2</sup> ) minimizes the system inputs: the acceleration of the vehicle along the row and the task space acceleration of the manipulator. The control input weight matrix is

$$\mathbf{W}\_{\boldsymbol{u}} = \begin{bmatrix} w\_{\boldsymbol{u}} \\ w\_{\boldsymbol{u}} \\ \vdots \\ w\_{\boldsymbol{u}} \end{bmatrix}^{\mathrm{T}}, \boldsymbol{w}\_{\boldsymbol{u}} = \begin{bmatrix} w\_{\boldsymbol{\mathcal{P}}\_{\mathcal{B},\boldsymbol{x}}} & 0 & 0 \\ 0 & w\_{\boldsymbol{\mathcal{P}}\_{\mathcal{A},\boldsymbol{x}}} & 0 \\ 0 & 0 & w\_{\boldsymbol{\mathcal{P}}\_{\mathcal{S},\boldsymbol{x}}} \end{bmatrix} \tag{9}$$

where the scalars *wp*¨*B*,*<sup>x</sup>* , *wp*¨*A*,*<sup>x</sup>* and *wp*¨*A*,*<sup>z</sup>* represent weights with respect to the acceleration of the mobile base along the row, and the acceleration of the robot arm in the *x* and *z* directions, respectively.

The third and final part of the criterion function ( *<sup>W</sup>x<sup>X</sup>* <sup>2</sup> ) minimizes a function of the system state vector. In particular, it minimizes the displacement of the robot arm in the *x* direction, with a weight matrix of the following form:

$$\mathbf{W}\_{\mathbf{x}} = \begin{bmatrix} w\_{\mathbf{x}} \\ w\_{\mathbf{x}} \\ \vdots \\ w\_{\mathbf{x}} \end{bmatrix}^{\mathrm{T}}, \; w\_{\mathbf{x}} = \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & w\_{pA,\mathbf{x}} & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix} \tag{10}$$

#### 2.2.1. MPC Parameter Tuning

The main challenge in tuning the parameters of the criterion function (8) is the balance between robot arm and mobile base motion. The *x* component of the reference trajectory can be tracked by either robot arm or mobile base. The idea behind minimizing arm displacement and mobile base acceleration is to achieve the effect where the mobile base is responsible for slower, global changes in the reference trajectory and the arm is responsible for faster, local changes. Parameters are tuned by trial and error to achieve this effect. Extreme examples of mobile base and robot arm trajectories are shown in Figure 5, with the corresponding optimization weights presented in Table 1.

**Figure 5.** Solutions to the MPC optimization problem in the *x* direction using different criterion function parameters. Parameters are given in Table 1. Spray frame trajectory *pS*,*<sup>x</sup>* is a sum of *pA*,*<sup>x</sup>* and *pB*,*x*.

**Table 1.** MPC criterion function parameters resulting in trajectories shown in Figure 5.


The behavior of the trajectories selected by the MPC algorithm depends on the ratios between the different optimization weights rather than their exact value. Figure 5 shows solutions to the MPC problem with different ratios between the optimization weights, where the system assumes an initial state with zero velocity. Increasing the weight corresponding to the vehicle acceleration *wp*¨*B*,*<sup>x</sup>* too much leads to small accelerations of the mobile base and thus to a large arm displacement (Figure 5a). Lowering the value of *wpA*,*<sup>x</sup>* and *wp*¨*A*,*<sup>x</sup>* has a similar effect. Lowering *wp*¨*B*,*<sup>x</sup>* or increasing *wpA*,*<sup>x</sup>* and *wp*¨*A*,*<sup>x</sup>* too much (Figure 5c) leads to the effect where the mobile base follows the reference with large accelerations, while the robot arm displacement remains close to zero. To achieve the desired behavior (Figure 5b), parameters are tuned following a general tuning rule: a larger weight is used for minimization of the vehicle acceleration than for that of the robot arm. This, in combination with the minimization of the arm displacement, leads to the already discussed effect.

For a set of reference trajectories generated as shown in Section 2.1, it was concluded that different values can be used as weights for the MPC criterion function and still result in satisfactory overall system behavior. The values for the sampling period, horizon and weights of the criterion function used in the experiments can be found in Section 3.
