NMPC-based visual path following control with variable perception horizon

Abstract

For greater autonomy of visual control-based solutions, especially applied to mobile robots, it is necessary to consider the existence of unevenness in the navigation surface, an intrinsic characteristic of several real applications. In general, depth information is essential for navigating three-dimensional environments and for the consistent parameter calibration of the visual models. This work proposes a new solution, including depth information in the visual path-following (VPF) problem, which allows the variation of the perception horizon at runtime while forcing the coupling between optical and geometric quantities. A new NMPC (nonlinear model predictive control) framework considering the addition of a new input to an original solution for the constrained VPF-NMPC allows the maintenance of low computational complexity. Experimental results in an outdoor environment with a medium-sized commercial robot demonstrate the correctness of the proposal.

1. Introduction

Growing computational power associated with high-performance embedded instrumentation has led to autonomous vehicles being susceptible to applications in the most varied contexts. Whether for safety or efficiency reasons, it is necessary to increase the autonomy levels in several applications, such as intelligent transport systems [1, 2], search and rescue [3, 4], or for navigation in industrial environments compatible with the precepts of Industry 4.0 [5, 6], to name a few.

Especially for these applications, the wide availability of low-cost visual sensors has enabled the development of controllers capable of generating control actions directly from the image plane, such as the classical image-based visual servoing-based solutions. For this class of controllers, recent solutions range from classical probabilistic methods [7] to deep reinforcement learning [8] approaches.

Among the strategies for robot control based on computer vision, there are relevant techniques for visual path-following (VPF), capable of regulating a robot along an arbitrary visual path acquired at runtime. In this case, there are significant advances for swimming robots [9], humanoids [10], unmanned aerial vehicles [11], or even surgical robots [12], with the citations limited to the last 3 years only.

The constrained nature of the camera’s field of view, in addition to the peculiar characteristics of luminosity and frame rate, encourages the application of optimal controllers, with NMPC (nonlinear model predictive control) being an ideal strategy, due to its ability to apply directly to nonlinear, multivariable, and constrained models, in addition to its good inherent robustness characteristics [13].

Despite the easy adaptation to the requirements of different application scenarios, this class of controllers has been the subject of constant investigations regarding computational cost analysis [14], and new formulations focused on efficiency [15, 16].

Although VPF methods produce good results for planar paths, as proposed by refs. [17] and [18], even associated with NMPC as in refs. [19], and [20], in several real situations, such as autonomous navigation on highways or factory floors of mezzanine format, unevenness in the navigation surface deserves special attention, as the reference paths are non-planar. In this case, path-following solutions must consider the problem’s three-dimensionality to keep the visual system’s calibration parameters coherent. This reality justifies the recent effort in evaluating strategies for lane line detection, such as the systematic review carried out in ref. [21].

An immediate solution to this problem is the use of in-depth information. However, in order not to increase the computational complexity of the proposals, it is necessary to objectively define what information is relevant since the complete treatment of a point cloud for the estimation of a three-dimensional path, as would be the case of applying formal techniques for semantic segmentation of the path [22, 23], increases the dimension of the problem, making it impossible to use optimal and interactive controllers such as the NMPC.

Implementing this class of controllers in the solution of path-following problems has particular computational load requirements due to the prediction and constrained nonlinear optimization resolution stages, which, when added to the demands of sophisticated stages of computer vision, substantially compromise the available processing bandwidth. In this context, alternatives arise based on analytical learning [24] through disturbance modeling by Gaussian processes, the human-like concept of visual memory [25], deep reinforcement learning [26], among others that directly address the problem of path following mobile robots with greater computational efficiency since applications do not always allows high embedded computational power.

This article proposes a new solution for navigation along non-planar paths by including RGB-D sensors, which provide depth information at specific points for generating control actions directly from the image plane. Starting from the original model proposed by ref. [27] and improved by ref. [28], a degree of freedom is used to define the visual horizon, and for the maintenance of low computational complexity indices, the new scheme adds a new input to the NMPC algorithm, forcing the coupling between optical and geometrical quantities. The main advantage of this proposal is due to its ability to explicitly handle constraints, being able to control the pose of the robot along a visual reference path, through an optimal perception horizon, even on irregular and uneven navigation surfaces, among other imperfections.

To the best of the authors’ knowledge, it is the first work that deals with the problem of optimal control under constraints for following visual paths in uneven outdoor terrain with a view to low computational cost through an effective load balance between the computer vision and control stages. It is an effective and generic solution that can be applied individually in structured environments with physical visual paths or in conjunction with a superior perception layer capable of providing virtual visual paths.

Implementation results through the ROS framework, using the robot Clearpath Husky UGV (unmanned ground vehicle) and the RGB-D sensor Microsoft Kinetic in an external navigation environment, demonstrate that the proposed method produces satisfactory results for navigation on non-planar surfaces.

The remainder of this article is structured as follows: Section 2 briefly formalizes the problem. Section 3 presents the proposed new model and the version of the NMPC algorithm used. Section 4 presents the results, and Section 5 the main conclusions.

2. Problem formalization

Figure 1 presents the necessary elements for geometrically modeling the problem of VPF, as initially proposed by ref. [27]. 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 line¹ 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, [28] 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. [29], 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.²

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 [28]); SQP: Sequential Quadratic Programming – Nonlinear optimization method used (further details in [30]).

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.

4. Results

Initially, the proposed model was validated in a realistic simulation environment, built using the software gazebo, as seen in the video available at https://youtu.be/Ob7pSZ3O7_Q. These simulations show that only the method based on Model 2 can follow the path completely, enabling the assembly of a setup to acquire practical results.

For experimental evaluation, we define a scenario composed of a reference path with an arbitrary curvature profile, in yellow color, drawn along a non-planar navigation surface, as illustrated in Fig. 4(a). The algorithms were developed using the ROS framework and applied to the Clearpath Husky UGV robot, illustrated in Figs. 4(b) and 4(c), equipped with a sensor RGB-D Microsoft kinect with the following pose in relation to the robot’s center of mass: $x_{\text{cam}}$ = 0.4 m; $y_{\text{cam}}$ = 0 m; $z_{\text{cam}}$ = 0.5 m; $\theta _{\text{cam}}$ = $\pi$ /4 rad.

Table I. NMPC tuning parameters.

Table II. NMPC inequality constraints.

Figure 5. Environment for comparison between models.

The embedded computer system where the proposal is implemented has the following specifications: intel^® CORE^® i5 vPro 7th Gen, 8 GB RAM, Ubuntu 16.04 LTS.

For an adequate comparison between the models, variations were made in the reference path to meet the cases of navigation between two different levels and curves in unevenness. Additionally, the evaluation criteria consider a scenario with significant variability of ambient lighting and long paths, aiming to show, in a practical sense, that the proposed technique is robust to errors in the acquisition of visual parameters and invariant to path length.

For the implementation of the NMPC controller, we consider $T_{p} = T_{c} = 3T_{s}$ due to the nonlinearities mentioned above, and $T_{s}$ = 0.2 s, due to the dynamics of the open-loop system. The optimization problem was solved using the general-purpose nonlinear optimizer DONLP2 [30].

In both Model 1 and 2 evaluations, Table I presents the tuning parameters, and Table II shows the constraints on states and inputs.

Figure 6. Models 1 and 2 based methods experimental results.

As it can be seen, the tuning parameter related to variations in the visual horizon has greater weight. The idea is to avoid high variations that would lead the controller to produce a horizon estimate outside the reference path due to imperfections in the visual system used for the test environment. This behavior is also specified via a low value for the constraint corresponding to this parameter.

Figure 7. Environment for Model 1 analysis.

Figure 8. Model 1 analysis.

The following subsections provide the main results obtained.³

4.1. Models comparison

A high curvature loop was added to the path in a high slope area for the present evaluation, as illustrated in Fig. 5. The curved path is approximately 10 m long and connects two uneven environments with a vertical distance of approximately 1.6 m. We consider the start plan as level 0 and an artificial light environment.

The objective here is to directly confront the methods based on Models 1 and 2 for a navigation velocity of 0.3 m/s, producing the results illustrated in Fig. 6. It is observed in Fig. 6(a)⁴ that the method based on Model 1 loses the visual path in the section with high curvature and unevenness, around 25 s. In contrast, the proposed method follows the entire path, regardless of its curvature or unevenness.

Figure 6(b) shows the runtime instantaneous curvature profiles, which are pretty noisy due to the performance of the visual parameters acquisition system for the scenario in question. As they are very similar profiles, we credited the impossibility of varying the perception horizon as the main reason for the poor performance of the method based on Model 1, confirming the existence of practical limits for the inherent robustness of NMPC controllers.

Figure 6(c) shows the measures of visual horizon variation given by $\Delta H = H(k)-H(k-1)$ , measured in centimeters. It is possible to verify values coherent with the surface’s unevenness both with the robot facing the unevenness and, laterally, at the point of most significant curvature.

Figure 6(d) shows the state errors, from which one can verify that the state constraints were satisfied throughout the experiment, even in the most critical stretch, where the errors came close to the maximum value due to the parameters of tuning used. Figure 6(e) shows control actions compatible with the experimental platform, reserve capacity for regulating more significant disturbances, and coherent activity of the proposed controller.

The following subsection investigates why Model 1 does not work in the present scenario more closely.

Table III. Three velocity quantitative comparison.

Figure 9. Environment for Model 2 analysis.

Figure 10. Model 2 analysis.

Figure 11. Three experiments comparison.

4.2. Limitation of Model 1

Calibration parameters were defined to level 0, related to the quota at the beginning of the experiment. Thus, it is natural to expect the method based on Model 1 to work correctly for just this case. To show this, Fig. 7 illustrates the loop closing the path between the starting and ending points of the previous experiment. The idea is to evaluate the behavior of Model 1 when returning to the level at which the parameters were originally calibrated.

Figure 8 illustrates the results obtained for a navigation velocity of 0.2 m/s, a value even lower than in the previous experiment, aiming to increase the regulation capacity of this method. As expected, the robot gets lost when the curve is on the inclined surface, which it does not, even with degraded performance, at the flat level, where the approach has been calibrated to work.

Figure 8(b) shows correctly regulations within limits established for the constraints, with an abrupt loss occurring when passing through the stretch of the greater curvature. In these results, we perform a manual rotation to repose the robot after losing its visual reference path in the image plane. From Fig. 8(c), one can see that it was not due to the platform’s movement physical limitation since the control actions are far low from the maximum practicable. Thus, we confirm that the model has a significant limitation for this navigation scenario.

On the other hand, we confirmed the proposal’s validity by running this same path for three different velocities, all of which the path was followed thoroughly. Without loss of generality to more straightforward and better direct analyses, we get two quantitative metrics, more specifically, the integral of absolute error (IAE) and the total control variation (TV). The IAE index, calculated by $\int _{0}^{T_{\text{END}}}|e(t)|dt$ , is widely used to compare the performance of different strategies in similar experiments; on the other hand, the TV index, calculated by $\sum _{k=0}^{k_{\text{END}}} | u(k)-u(k-1)|$ , aims to evaluate the effect of noise on control signals. Table III presents the results.

As can be seen, the parameters remained coherent and compatible with the specifications of the experimental platform in all cases.

4.3. Detailed analysis of Model 2

This section provides a detailed evaluation of the proposed method’s performance. We define a long path arranged on an uneven surface, with significant variability of ambient lighting, in stretches with bifurcations and high curvature, as illustrated in Fig. 9. The curved path is approximately 15 m long and connects two uneven environments in approximately 3.8 m. Additionally, an object was positioned in the transition zone between the lowest level and the ascent ramp, subjecting the proposed method to an even more aggressive disturbance.

The results obtained from the complete experiment, at $v=0.2$ m/s, are illustrated in Fig. 10. It is possible to note that the path was followed entirely, in both directions (down and up), even with the various imperfections added (see Fig. 10(b)). Figure 10(b) shows the physical parameters acquired at runtime. As can be seen, even with intentional perturbations that make the equilibrium points undefined, the system can regulate the state errors, as shown in Fig. 10(d).

Figure 10(c) shows the instant visual horizon variation measures. It is possible to notice minor variations due to the lower speed and an abrupt disturbance due to the object positioned along the path. The physical control actions are fully compatible with the experimental platform, as shown in Fig. 10(e). Also, for this experiment, the instantaneous values of the internal control actions are shown in Fig. 10(f), confirming that the inputs’ constraints were fully satisfied.

Regarding computational performance, the instantaneous processing times were acquired throughout the experiment, as illustrated in Fig. 10(g). It is possible to notice the computational efficiency of the proposal since, in just one sample, the processing time $T_{\text{proc}}$ was close to the sampling period $T_{s}$ , highlighting an average of approximately 10% of $T_{s}$ .

Finally, to attest to the repeatability of the proposal, the same experiment was performed three more times, producing the results in Fig. 11. One can note similar and satisfactory performances, especially when considering a robot of about 50 kg weighing navigates a long and uneven path based only on visual information.

Tables IV and V gather some statistical data obtained through these experiments. It is worth highlighting mean values for the error states and the physical control action close to zero, low standard deviations, including for the quantitative metrics IAE and TV.

Table IV. Three experiments state data analysis.

Table V. Three experiments control actions data analysis.

5. Conclusions

This article proposes a new model for the visual perception horizon variation of NMPC-based VPF control. With this new model, navigation problems on uneven and non-planar surfaces are solved, in addition to calibrating visual parameters and ensuring consistency between optical and geometric quantities by including RGB-D sensors, enabling the extension of real applications.

To maintain the requirements of low computational complexity, we include specific and local depth information and define a degree of freedom used for the variation of the visual horizon, even if limited to the physical field of view, so that we detect the optimal point for parameter acquisition at runtime. The inherent characteristics of the NMPC algorithm allow the application of the new proposed model, even without the explicit treatment of feasibility and stability, which could interfere with the overall computational cost of the strategy.

Experimental results in an outdoor navigation environment, using a commercial robot and visual sensor, demonstrated that, with the proposed approach, it is possible to follow visual paths in several adverse situations safely and efficiently, even on non-planar surfaces. These results also show the validity of proposals based on exploring the inherent robustness levels of NMPC controllers, to the detriment of sophisticated solutions, with high theoretical load in analytical terms or computationally costly implementation.

Future work includes investigating strategies for explicit horizon variation depending on a specific application, detailed analysis and treatment of feasibility and stability, and an explicit metric to forward velocity variation.