2.2.2. MPC Constraints

The constraints on the system state and the control inputs have the following form:

$$\begin{bmatrix} -p\_{B,x}^{\max} \\ -p\_{A,x}^{\max} \\ -p\_{S,z}^{\max} \\ -p\_{B,x}^{\max} \\ -\hat{p}\_{A,x}^{\max} \\ -\hat{p}\_{S,z}^{\max} \end{bmatrix} \leq \varkappa(k) \leq \begin{bmatrix} p\_{B,x}^{\max} \\ p\_{A,x}^{\max} \\ p\_{S,z}^{\max} \\ \hat{p}\_{B,x}^{\max} \\ \hat{p}\_{A,x}^{\max} \\ \hat{p}\_{S,z}^{\max} \end{bmatrix} \tag{11}$$

$$\begin{bmatrix} -\dot{\boldsymbol{p}}\_{\boldsymbol{B},\boldsymbol{x}}^{\text{max}}\\ -\dot{\boldsymbol{p}}\_{\boldsymbol{A},\boldsymbol{x}}^{\text{max}}\\ -\dot{\boldsymbol{p}}\_{\boldsymbol{S},\boldsymbol{x}}^{\text{max}} \end{bmatrix} \leq \boldsymbol{\mu}(\boldsymbol{k}) \leq \begin{bmatrix} \dot{\boldsymbol{p}}\_{\boldsymbol{B},\boldsymbol{x}}^{\text{max}}\\ \dot{\boldsymbol{p}}\_{\boldsymbol{A},\boldsymbol{x}}^{\text{max}}\\ \dot{\boldsymbol{p}}\_{\boldsymbol{S},\boldsymbol{x}}^{\text{max}} \end{bmatrix} \tag{12}$$

The motion of the mobile base is left unconstrained (*p*max *<sup>B</sup>*,*<sup>x</sup>* = ∞). The maximum velocity and acceleration of the mobile base, *p*˙ max *<sup>B</sup>*,*<sup>x</sup>* and *<sup>p</sup>*¨max *<sup>B</sup>*,*<sup>x</sup>* , respectively, are known and directly enforced by these constraints. The maximum values of the robot arm velocities and accelerations in the task space (*p*˙ max *<sup>A</sup>*,*<sup>x</sup>* , *p*˙ max *<sup>S</sup>*,*<sup>z</sup>* , *<sup>p</sup>*¨max *<sup>A</sup>*,*<sup>x</sup>* and *<sup>p</sup>*¨max *<sup>S</sup>*,*<sup>z</sup>* ) must be determined experimentally, as they depend on the current arm configuration and the velocity and acceleration limits of each joint, which are not considered by the MPC algorithm. Instead, the joint space constraints are enforced at the level of the task space control algorithm.

#### *2.3. Manipulator Task Space Control*

Joint velocities *q*˙ are selected through task space control and are used to control the robot arm (*q* represents joint positions). As mentioned earlier, the MPC algorithm only considers the *x* and *z* coordinates of the spray frame position. In the planning phase (phase of the MPC algorithm), the position of the spray frame in the *y* direction (*pS*,*y*) is assumed to be constant, and its roll, pitch and yaw angles are not considered (*φT*, *θ<sup>T</sup>* and *ψT*, respectively, as shown in Figure 6).

**Figure 6.** The orientation of the spray frame depends on the joint configuration of the robot arm. Roll, pitch and yaw angles are referred to as *φT*, *θ<sup>T</sup>* and *ψT*, respectively.

To achieve the assumption of constant *pS*,*y*, the desired task space velocity in the *y* direction, *p*˙*S*,*y*, is computed with a proportional controller

$$
\dot{p}\_{S,y} = K\_{P,y}(p\_{S,y}^d - p\_{S,y}) \tag{13}
$$

where *p<sup>d</sup> <sup>S</sup>*,*<sup>y</sup>* is a desired value of *pS*,*<sup>y</sup>* and *KP*,*<sup>y</sup>* is the controller gain.

The desired linear spray frame velocity can be achieved with different angular velocities. To achieve the largest possible linear spray frame velocities, only the roll angle is controlled directly, while the pitch and yaw angles are not considered. The desired roll angle velocity is calculated with a proportional controller

$$
\dot{\Phi}\_T = -K\_{P,\phi} \phi\_T \tag{14}
$$

where *KP*,*<sup>φ</sup>* is the controller gain and the desired roll angle is zero.

The desired task space velocity is four-dimensional and considers *x*, *y*, *z* position coordinates and the roll angle of the *LS* frame,

$$\mathbf{v}\_d^{4D} = \begin{bmatrix} \dot{\boldsymbol{p}}\_{A,x}^{\mathcal{M}} \\ \dot{\boldsymbol{p}}\_{S,y}^{\mathcal{M}} \\ \dot{\boldsymbol{p}}\_{S,z}^{\mathcal{M}} \\ \dot{\boldsymbol{\Phi}}\_T \end{bmatrix} \tag{15}$$

*p*˙ *<sup>M</sup> <sup>A</sup>*,*<sup>x</sup>* and *<sup>p</sup>*˙ *<sup>M</sup> <sup>S</sup>*,*<sup>z</sup>* are provided by MPC, and *<sup>p</sup>*˙*S*,*<sup>y</sup>* and *<sup>φ</sup>*˙*<sup>T</sup>* are provided by proportional controllers (13) and (14), respectively. The four-dimensional task space velocity depends on the joint velocities as follows:

$$
v^{4D} = \mathcal{I}^{4D} \dot{q} \tag{16}$$

where *J*4*<sup>D</sup>* is the task space Jacobian.

In general, there are multiple solutions for *q*˙ that achieve the desired task space velocities. For this reason, an additional criterion by which the joint commands are selected is introduced. The desired joint velocities *<sup>q</sup>*˙ *<sup>d</sup>* that drive the robot arm to a desired pose *<sup>q</sup><sup>d</sup>* are computed by another proportional controller,

$$
\dot{\boldsymbol{q}}\_d = \boldsymbol{K}\_{\mathcal{P}, \boldsymbol{q}} (\boldsymbol{q}\_d - \boldsymbol{q}) \tag{17}
$$

where *KP*,*<sup>q</sup>* is the controller gain.

Finally, the joint velocity commands are selected by solving the following QP problem:

$$\begin{aligned} \min\_{\dot{\boldsymbol{q}}} & \quad \left\| \boldsymbol{J}^{4D} \dot{\boldsymbol{q}} - \boldsymbol{\boldsymbol{v}}\_{d}^{4D} \right\|^{2} + \boldsymbol{w}\_{\mathcal{P}} \left\| \dot{\boldsymbol{q}}\_{d} - \dot{\boldsymbol{q}} \right\|^{2} \\ \text{s.t.} & \quad \dot{\underline{q}} \le \dot{\underline{q}} \le \overline{\dot{q}} \end{aligned} \tag{18}$$

where *q*˙ and *q*˙ are the lower and upper bounds of the joint velocities, respectively, and *wp* is the arm pose weight.

### **3. Results**

Two experiments were conducted to evaluate the presented approach: a spraying demonstration in a vineyard and an external validation in an indoor environment equipped with Optitrack cameras. Both experiments were performed with the following optimization weights: *Wy* = 800, *wp*¨*B*,*<sup>x</sup>* = 80, *wp*¨*A*,*<sup>x</sup>* = 8, *wp*¨*A*,*<sup>z</sup>* = 8, *wpA*,*<sup>x</sup>* = 1. Parameter values were determined experimentally, as described in Section 2.2.1. Values of proportional controller gains used in the experiments were *KP*,*<sup>y</sup>* = 0.9, *KP*,*<sup>φ</sup>* = 0.1 and *KP*,*<sup>q</sup>* = 1.5. These values were also determined experimentally. The velocity of the reference lawnmower trajectory used in all experiments was 0.3 m/s. A prediction horizon of 40 steps was used, with a sampling period of *T* = 0.1 s. The optimization problems for the MPC and task space control were both solved using the BPMPD interior point solver for convex QP problems [36]. The average computation times of the MPC and task space control optimization problems were 3.998 ms and 0.201 ms, respectively, for 600 trials. This allows the presented approach to be used in a real-time scenario, even with larger prediction horizons. All experiments were performed on an Intel Core i7-10710U CPU @ 1.60 GHz.

#### *3.1. Equipment*

The control equipment used in the experiments is shown in Figure 7. All the software was run on an Intel NUC 10 PC with a Ubuntu 20.04 operating system. The Robot Operating System (ROS) was used as the middleware for controlling the robot arm, the mobile vehicle and the spraying agent pump.

**Figure 7.** Communication diagram of the equipment used in the vineyard spraying experiments.

The Kinova Gen3 7 DoF robot arm uses an ethernet connection to communicate with ROS drivers running on the PC. As can be seen in Figure 3, low-level control of the robot arm is achieved via joint velocity commands *q*˙ . Encoder measurements from the robot arm are used as process feedback. As feedback for the MPC algorithm, the robot arm components of the spray frame position *pA*,*<sup>x</sup>* and *pA*,*<sup>z</sup>* are calculated at every step of the control loop, using forward kinematics and joint position measurements *q*. For the task space control algorithm, joint position data from the encoder are also used as feedback, to calculate the task space Jacobian *J*4*D*.

Mobile vehicle flipper tracks are actuated by Maxon EC 45 brushless motors, which are controlled using EPOS 70/10 brushless motor drives. EPOS drives communicate with the PC via a CAN bus. ROS drivers for the EPOS drives are used to control the velocities and gather encoder data of each motor. The mobile base component of the spray frame position *pB*,*<sup>x</sup>* is calculated using encoder data odometry and is used as feedback for the MPC algorithm.

Finally, the spraying agent pump is controlled with an Arduino Nano microcontroller board and is either turned on or off based on the current position of the spray frame with respect to the row description.
