2. Problem formalization

Figure 1 presents the necessary elements for geometrically modeling the problem of VPF, as initially proposed by ref. [Reference Ribeiro and Conceição27]. In this case, for a prespecified linear velocity profile $v$ , the states (identical to the present case outputs) consist of features extracted from a computer vision system at each iteration.

Figure 1. Modeling the visual path following problem.

Initially, having as a premise that the robot and the visual system are always in front and longitudinal to the reference path, it defines a Serret–Frenet $\{SF\}$ system at a point $P_{r}$ , representing the movement of a virtual robot that navigates at a constant linear velocity $v$ . Such a point is defined through a distance $H$ that defines a visual horizon lineFootnote ¹ in front of the robot. It intersects the path at $P_{r}$ perpendicularly.

The geometric states shown in Fig. 1(a) are the lateral displacement $Z$ and the error angle $\theta _{r}$ between the longitudinal line to the robot and the tangent line to the path at $P_{r}$ . The visual quantities corresponding to these states are presented in Fig. 1(d), where $\Delta _{x_{i}}$ is the visual correspondent for Z and $\Delta _{\alpha }$ is the visual correspondent for $\theta _{r}$ . Such correspondences are defined through individual calibration constants, as detailed further in this section. The $H$ parameter has a component in the camera’s field of view directly related to $\Delta _{y_{i}}$ and is geometrically calculated as follows:

(1) \begin{equation} H=\frac{l}{2}+d_{1}+d_{2}; \end{equation}

(2) \begin{equation} d_{1}=h_{c}\tan \!\left(\theta _{\text{cam}}-\frac{\theta ^{v}_{\text{fov}}}{2}\right); \end{equation}

(3) \begin{equation} d_{2}=k_{h}\Delta y_{i}, \end{equation}

where

• • $l$ : robot length;

• • $h_{c}$ : camera height;

• • $d_{1}$ : distance between the camera reference frame and the bottom point of the vertical field of view (out of the image plane);

• • $d_{2}$ : distance between the bottom coordinate of the vertical field of view and $P_{r}$ in the image plane (inside the image plane);

• • $\theta _{\text{cam}}$ : camera focal axis angle;

• • $\theta ^{v}_{\text{fov}}$ : vertical field of view angle;

• • $k_{h}$ : visual horizon calibration constant;

• • $y_{i}$ : pixels in the vertical direction in the image plane.

The original model (Model 1 hereafter) for VPF is given as follows:

(4) \begin{align} u_{e}=\omega - \omega _{r},\end{align}

(5) \begin{align} \dot{\textbf{x}}_{e}=\begin{bmatrix} \dot{Z} \\[5pt] \dot{\theta }_{r}\end{bmatrix}=\begin{bmatrix} \omega H + (\omega Z + v)\tan (\theta _{r})\\[5pt] u_{e}\end{bmatrix}. \end{align}

where

• • $u_e$ : control input;

• • $\omega$ : robot angular velocity;

• • $\omega _{r}$ : virtual vehicle angular velocity;

• • $\mathbf{{x}}_{e}$ : two-dimensional state vector;

• • $s$ : Path length;

• • $c(s)$ : Path Curvature at $s$ , given by $\frac{\omega _{r}}{\dot{s}}$ ;

Note that the underactuated nature of the system justifies the search for alternative models and controllers to guarantee performance metrics compatible with the most varied types of applications.

Despite good performance in controlled environments, the original proposal suffers from several practical problems. To solve issues with visual path discontinuity and curvature calculation, low ambient luminosity, among other imperfections of typical navigation scenarios of real applications, [Reference Franco, Ribeiro and Conceição28] proposed the interpolation of the visual path through a second-degree equation of the type $x_{p} = a_{p}{y_{p}}^2 + b_{p}{y_{p}} + c_{p}$ , where $x_{p}$ and $y_{p}$ are coordinates of pixels in the image plane, providing to calculate the curvature as follows:

(6) \begin{equation} c = \frac{{2a_{p}}}{{(1+(2a_{p}y_{p}+b_{p})^2)^{\frac{3}{2}}}}. \end{equation}

The existence of a well-defined mathematical object for calculating the states and the curvature parameter allows us to follow paths with more complex curvature profiles. It makes it possible to propose new techniques considering the temporal variation of other parameters, such as the visual perception horizon proposed in the present work.

With this new way of estimating the visual path, we have an analytical method to obtain the current system states ( $Z$ and $\theta _{r}$ ), given as follows:

(7) \begin{align} Z=k_{z}\!\left(\frac{a_{p}}{k_{h}^{2}}d_{2}^{2}+\frac{b_{p}}{k_{h}}d_{2}+c_{p}-x_{0}\right ); \end{align}

(8) \begin{align} \theta _{r}=k_{\theta }\,\,\text{atan}\!\left (\frac{k_{h}Z}{k_{z} d_{2}}\right ). \end{align}

where

• • $d_{2}=H-\frac{l}{2}-d_{1}$ the component of the visual perception horizon in the image plane, as defined in (1);

• • $x_{0}$ : horizontal coordinate of the vertex of the second-order function on the image plane

• • $k_{z}$ : lateral displacement calibration constant;

• • $k_{\theta }$ : angular error calibration constant.

As $y_{p}$ is related to $H$ through $d_{2}$ , there is a formal representation for the path, increasing the representation of the static VPF model. However, it still has limited applicability in some practical situations, as highlighted below:

• • Parameter calibration: As can be seen in (7) and (8), both states depend on the calibration constant $k_{h}$ , initially obtained by the relation between the number of pixels on the axis $y_{p}$ of the image plane, for a single visual horizon value. Thus, it is necessary to define three calibration constants, which increases the uncertainties in the measurements of the states.

• • Constant horizon: Assuming a constant visual horizon invalidates the use of Model 1 on uneven terrain, as it will generate inaccurate measurements of the states due to the slope of the terrain. Moreover, this model imposes limitations on model-based controllers since it employs the prediction horizon concept without establishing a physical correspondence with the visual horizon and misuses the receding horizon concept.

• • Model representativeness: Taking the time derivatives of (7) and (8), a substantial kinematic inconsistency with (5) is observed (for $H=\text{cte} \rightarrow \dot{H}=\dot{Z}=\dot{\theta }_{r}=0$ ), restricting the potential gains from the application of model-based techniques. This fact forces the visual quantities to be used only as an initial guess for the states, having no connection with the kinematic model obtained from the geometric relations.

For successfully implementing the Model 1 approach, whose path profile in the image plane is illustrated in Fig. 2(a), it is necessary to position the camera in front of and very close to the path. Thus, it is possible to reduce the effects of distortions in the images and obtain unique calibration constants. This constraint makes the path appear practically straight in all frames so that the accuracy of the measured curvatures is not critical. Interpolation using a second-order function, as shown in Fig. 2(b), can compensate for imperfections along the path and extend the fixed horizon to higher values. Nevertheless, obtaining the calibration parameters remains a challenging task.

Figure 2. Typical challenges of real situations.

Additionally, Fig. 2 still illustrates two practical situations that justify the previous highlights. In Fig. 2(c), the robot moves on an uneven surface, typical of natural outdoor environments, in such a way that parameter calibration is practically impossible without some preliminary information on the nature of the irregularities. In Fig 2(d), one can see that the robot cannot navigate on non-planar surfaces, given that the model used is incapable of considering variations in the horizon.

Finally, one of the most critical problems is the lack of correspondence between the kinematics of pixels in the image plane and the state variables in the real world since with $H$ constant makes consistency only in a motionless case, since in this case (4) and (5) would become zero, which violates the assumption of a constant velocity profile $v$ .

In order to not further increase the computational complexity of the prior schemes, we solve this issue by proposing a new model, contemplating variations in the visual horizon through depth information, and exploring the good inherent robustness characteristics of NMPC controllers, as detailed in the next section.

3. NMPC-based visual path following control with variable perception horizon

Aiming for simplicity and considering that low computing power is available, we propose using RGB-D cameras to acquire depth information at runtime. With such information, it is possible to obtain the visual horizon directly from the images and efficiently calculate the constant $k_{h}$ through simple trigonometric relations, partially solving the issue relating to estimating visual parameters.

Additionally, it will be possible to modify the camera’s pose so that the path in the image plane becomes more representative, enabling the identification of broader profiles through longer perception horizons. For this purpose, it is necessary to adjust the Model 1, starting from the geometric relations illustrated in Fig. 1 and from the side views illustrated in Figs. 2(c) and 2(d), as follow:

(9) \begin{equation} P_{r}(s(t))=P(t)+H(t)\vec{x_{r}}(\theta (t))-Z(t)\vec{y_{r}}(\theta (t)). \end{equation}

From the time derivative of the previous expression:

(10) \begin{align} & \dot{s_{T}}\vec{T}(s)+\dot{s_{N}}\vec{N}(s)= \nonumber\\[5pt] \nonumber = \dot{x}\vec{x_{r}}(\theta (t))&+\dot{y}\vec{y_{r}}(\theta (t))+\dot{H}\vec{x_{r}}(\theta (t))+H(t)\dot{\theta }\vec{y_{r}}(\theta (t))+ \\[5pt] & +Z(t)\dot{\theta }\vec{x_{r}}(\theta (t))-\dot{Z}\vec{y_{r}}(\theta (t)). \end{align}

Knowing that $\dot{s_{T}}=\dot{s}$ and $\dot{s_{N}}=\dot{y}\vec{y_{r}}=0$ due to non-holonomic constraints, omitting the angular and temporal dependencies:

(11) \begin{equation} \dot{s}\vec{T}(s)=(\dot{x}+\dot{H}+Z\dot{\theta })\vec{x_{r}}+(H\dot{\theta }-\dot{Z})\vec{y_{r}}. \end{equation}

The relationship between the robot coordinate system $\{r\}$ and the Serret–Frenet $\{SF\}$ system is given as follows:

(12) \begin{equation} \begin{bmatrix} \vec{x_{r}}\\[5pt] \vec{y_{r}} \end{bmatrix}=\begin{bmatrix} \cos \theta _{r}\;\;\;\;\; & \sin \theta _{r} \\[5pt] -\sin \theta _{r}\;\;\;\;\; & \cos \theta _{r} \end{bmatrix} \begin{bmatrix} \vec{T}\\[5pt] \vec{N} \end{bmatrix}. \end{equation}

Projecting this expression in the Serret–Frenet system and replacing the kinematic model of the differential drive ( $\dot{x}=v \cos{\theta };\; \dot{y}=v \sin{\theta } ;\; \dot{\theta }=\omega$ ):

(13) \begin{align} \dot{Z}=\omega H + (v + \dot{H} + \omega Z)\tan \theta _{r}; \end{align}

(14) \begin{align} \dot{s}=\frac{v + \dot{H} + \omega Z}{\cos \theta _{r}}. \end{align}

Since $\dot{\theta _{r}}=\omega -\dot{s}c(s)$ , we have:

(15) \begin{align} \dot{Z}=\omega H + (v + \dot{H} + \omega Z)\tan (\theta _{r}); \end{align}

(16) \begin{align} \dot{\theta _{r}}=\omega -c(s)\frac{(v + \dot{H} + \omega Z)}{\cos \theta _{r}}. \end{align}

The current development makes it possible to establish a direct relationship between the measurements of quantities in pixels, measured in the image plane, for the quantities measured in meters, obtained from geometric modeling. This fact is noticeable when observing that the time derivative of (7) and (8) are no longer null (Model 1 case) since $\dot{Z}$ and $\dot{\theta }$ are given as follows:

(17) \begin{align} \dot{Z}=\left (2\frac{a_{p}}{k_{h}^{2}} d_{2} + \frac{b_{p}}{k_{h}}\right )\dot{H}; \end{align}

(18) \begin{align} \dot{\theta _{r}}= \frac{{k_{\theta }k_{h}k_{z}(\dot{Z} d_{2} + Z \dot{H})}}{(k_{z} d_{2})^{2} + (k_{H} Z)^{2}}. \end{align}

As can be seen with the proposed model, $\dot{Z}$ and $\dot{\theta }$ are related to $\dot{H}$ through optical quantities and calibration parameters, providing greater representativeness of the model and coherence between the visual and geometric quantities.

For a preliminary analysis of the proposed model nonlinearity, we get the equilibrium points of the system (13) and (14) as follows:

(19) \begin{align} \theta _{r} = \sin ^{-1}({-Hc}); \end{align}

(20) \begin{align} Z=\frac{\omega \sqrt{1-(Hc)^{2}}}{c} - (v + \dot{H}). \end{align}

Thus, there are no equilibrium points for $Hc$ outside the interval [−1,1] and a discontinuity in $c=0$ . Consequently, there is no trivial way to apply analytical linearization techniques directly and use traditional schemes to handle the stability of NMPC controllers. Also, it is possible to note the importance of $H$ for the present model, even if restricted to instrumental aspects, since there is no direct way to control the curvature parameter.

Considering the availability of distance information to calculate the current $H$ , we propose to use $\dot{H}$ as a degree of freedom for changes in the visual horizon by adding a new input to the NMPC algorithm as follows:

(21) \begin{align} {u}_{1} = \frac{\dot{H}}{\cos \theta _{r}}. \end{align}

Another control action, referring to angular velocity errors, is maintained like Model 1 approach, that is:

(22) \begin{align} {u}_{2} = \omega - c(s)\frac{(v + \omega Z)}{\cos \theta _{r}}. \end{align}

The new model (Model 2 hereafter) for the VPF problem, considering variations in the perception horizon and compatible with the nonlinear representations in state space, typically used in the application of predictive controllers, is finally written as follows:

(23) \begin{equation} \mathbf{u}_{e}=\begin{bmatrix}{u}_{1} \\[5pt] {u}_{2}\end{bmatrix} \end{equation}

(24) \begin{equation} \dot{\textbf{x}}_{e}=\begin{bmatrix} \dot{Z} \\[5pt] \dot{\theta _{r}}\end{bmatrix}=\begin{bmatrix} \omega H + \left (\dfrac{\omega -{u}_{2}}{c(s)}\right )\sin \theta _{r} +{u}_{1}\sin \theta _{r}\\[5pt] {u}_{2} - c(s){u}_{1} \end{bmatrix}. \end{equation}

Considering that the outputs are the states themselves, the problem of following visual paths, with variable perception horizon, for differential robots can be summarized as follows:

Find $\dot{H}$ and $\omega$ , such that $u_{1}$ , $u_{2}$ $Z$ and $\theta _{r}$ are feasible.

With this proposal, it is possible to use depth information at specific points of interest without requiring a complete point cloud treatment to estimate a three-dimensional path, thus maintaining low computational cost requirements.

It is worth mentioning that the approach proposed here can be directly applied to structured environments, such as those commonly found for automated guided vehicle navigation on the factory floor or docking stations in general. A prominent case can be found in ref. [Reference Arrais, Veiga, Ribeiro, Oliveira, Fernandes, Conceição, Farias, Oliveira, Novais and Reis29], where controllers for VPF based on NMPC were responsible for essential navigation tasks in a case study of additive manufacturing operations by a mobile manipulator within a practical application.Footnote ²

However, the path does not necessarily need to be physical. It is possible to explicit the generality of the proposal by considering that the physical path can be used only in a training step and then removed or even eliminated by using a virtual path that can be generated at runtime by a layer added on top of the proposals in this article.

3.1. NMPC control scheme

The model represented by (23) and (24) is nonlinear, time-varying, and has constraints on inputs and states (outputs), justifying the use of computationally efficient optimal control strategies. Predictive control-based approaches meet some requirements due to their performance with constrained, time-varying, multivariable problems. Due to the moving horizon principle, such controllers have good inherent robustness characteristics and adapt well to disturbances, nonlinearities, and modeling errors. In order to obtain effective solutions for the regulation of states around the origin $(Z=\theta _{r}=0)$ with low computational complexity requirements, this article deals with the following continuous-time NMPC approach:

(25) \begin{equation} J_{\text{min}}=\min _{\mathbf{u}_{e}}\int _{t}^{t+T_{p}}F(\mathbf{x}_{e}(\tau ),\mathbf{u}_{e}(\tau ))d\tau, \end{equation}

(26) \begin{align} \text{subject to:}\; {\dot{\textbf{x}}_{e}}(\tau ) = f(\mathbf{x}_{e}(\tau ),\mathbf{u}_{e}(\tau )), \end{align}

(27) \begin{align} \mathbf{u}_{e}(\tau ) \in \mathcal{U}, \forall \ \tau \in [t,t+T_{c}], \end{align}

(28) \begin{align} \mathbf{x}_{e}(\tau ) \in \mathcal{X}, \forall \ \tau \in [t,t+T_{p}], \end{align}

with the stage cost $F$ given by:

(29) \begin{eqnarray} F(\mathbf{x}_{e}(\tau ),\mathbf{u}_{e}(\tau )) = \mathbf{x}_{e}^{T}\mathbf{Q}\mathbf{x}_{e}+\mathbf{u}_{e}^{T}\mathbf{R}\mathbf{u}_{e}, \end{eqnarray}

where

$T_{p}$ : Prediction horizon;

$T_{c}$ : Control horizon; With $T_{c} \leq T_{p}$ ;

$\mathcal{U}$ : Set of feasible Inputs;

$\mathcal{X}$ : Set of feasible states;

$\mathbf{Q}$ , $\mathbf{R}$ : Positive definite matrices that weight deviations from required values.

Due to the characteristics of the proposed model and the need to evaluate the proposal in comparison with the original method (based on Model 1), this work does not address techniques to guarantee feasibility or stability. Thus, the robustness characteristics inherent to NMPC controllers are explored by including a direct correspondence between the prediction and perception horizons.

After solving the optimization problem referring to the NMPC algorithm ((26) to (30)), as the final implementation step, the visual reference horizon $H_{\text{ref}}$ , for the definition of $P_{r}(s(t))$ along the visual path, and the physical control effort $\omega _{\text{ref}}$ are obtained using the optimal control inputs, $ u_{1_{\text{opt}}}$ and $u_{2_{\text{opt}}}$ , as follows:

(30) \begin{equation} H_{\text{ref}}=\int _{t=t_{k}}^{t=t_{k}+T_{s}}{{u_{1}}(t)_{\text{opt}}\cos (\theta _{r}(t))} d{u_{1}}(t); \end{equation}

(31) \begin{equation} \omega _{\text{ref}}=\frac{{u_{2}}(t_{k})_{\text{opt}} \cos \theta _{r}(t_{k}) + c(s)v}{\cos \theta _{r}(t_{k})-cZ(t_{k})}, \end{equation}

where $t_{k}$ the actual sampling instant.

The Algorithm 1 provides a pseudocode of the proposed solution to characterize the proposal better. After that, the physical control actions ( $v,\omega _{\text{ref}}$ ) are sent to the internal control loop embedded in the robot (PID for the wheels), and the new reference visual horizon is updated for the calculation of new features. Figure 3 illustrates more details of the elements necessary for the implementation. In this figure, the expressions implemented in each block are highlighted.

Algorithm 1. Pseudocode of the Visual Path Following with variable horizon

Figure 3. Proposed scheme for NMPC-based visual path-following control with variable perception horizon. OLS: Ordinary Least Squares – The method used to fit the second-order curve to the path image (further details in [Reference Franco, Ribeiro and Conceição28]); SQP: Sequential Quadratic Programming – Nonlinear optimization method used (further details in [Reference Spellucci30]).

Figure 4. Experimental environment.

This new visual control method directly from the image plane makes it possible to navigate on irregular and non-planar surfaces, in addition to increasing the levels of robustness concerning imperfections in the visual system, as demonstrated in the experimental results of the next section.