Plant-Inspired Soft Growing Robots: A Control Approach Using Nonlinear Model Predictive Techniques

Abstract

Soft growing robots, which mimic the biological growth of plants, have demonstrated excellent performance in navigating tight and distant environments due to their flexibility and extendable lengths of several tens of meters. However, controlling the position of the tip of these robots can be challenging due to the lack of precise methods for measuring the robots’ Cartesian position in their working environments. Moreover, classical control techniques are not suitable for these robots because they involve the irreversible addition of materials, which introduces process constraints. In this paper, we propose two optimization-based approaches, combining Moving Horizon Estimation (MHE) with Nonlinear Model Predictive Control (NMPC), to achieve superior performance in point stabilization, trajectory tracking, and obstacle avoidance for these robots. MHE is used to estimate the entire state of the robot, including its unknown Cartesian position, based on available configuration measurements. The proposed NMPC approach considers process constraints by relying on the estimated state to ensure optimal performance. We perform numerical simulations using the nonlinear kinematic model of a vine-like robot, one of the newly introduced plant-inspired growing robots, and achieve satisfactory results in terms of reduced computation times and tracking error.

1. Introduction

Harmless robotics exploration of confined spaces, such as the application of Minimally Invasive Surgeries (MIS) in human bodies [1], or inspection of archaeological sites [2], is challenging while considering the existing rigid robot designs. Thus, designing new materials and locomotion systems for robots is crucial to perform in such challenging environments. Inspired by biological systems such as elephant trunks, octopus tentacles, and snakes, designs of soft continuum robots with continuous bending backbones have alleviated the non-destructive navigation in congested environments [3,4]. Nonetheless, the small lengths of continuum robots are limiting their applicability in the exploration of distant spaces [5].

Taking inspiration from plants, a novel type of mobility called growth mobility was recently introduced. Namely, growing robots can mimic the biological growth of plants by gradually extending their lengths, volumes, or knowledge: [6]. Due to their soft materials or flexible joints, growing robots can extend their lengths to reach distant spaces while showing compliance when working in confined environments. Earlier research has shown few attempts for designing long flexible robots that can work in tight spaces. For instance, a multiple-degrees of freedom growing robot called “Active Hose” is proposed by Tsukagoshi et al. [7] for search and rescue applications. The proposed robot design was based on stacking two degrees of freedom units in series to enable the robot to extend its length and achieve the required flexibility. Isaki et al. [8] have come up with a flexible thin and extended cable with an attached camera to explore narrow spaces. Moreover, a pneumatically driven expendable soft arm known as “Slime Scope” [9] was developed for rescue applications in rubble environments. Nevertheless, the locomotion of these developed growing robots requires the movement of the entire robot’s body, which in turn causes significant friction between the robot and the environment.

Recently, two designs of novel plant-inspired growing robots were proposed, each having the capability of extending their body lengths with the addition of materials at their distant tips. In particular, in [10,11], a plant root-like growing robot is proposed, and a three-dimensional (3D) printer-like head is attached to its tip to deposit the circular body layers of the robot incrementally. The steering capability is achieved efficiently by varying the speed of material deposition along the robot circumference. However, the robot’s growth speed of the robot is limited based on the environment. On the other hand, a vine-like growing robot has been developed by Hawkes et al. [12] based on a novel mechanism called "tip-eversion” [13]. It is a pneumatically-driven robot made of thin-walled polyethylene tubing that can evert from inside to outside, enabling the robot to extend for tens of meters into slippery and sticky environments. The steering of vine robots was achieved by either utilizing the environment’s obstacles to guide the robot’s movements [4] or by placing three or four Series Pneumatic Artificial Muscles (SPAM) which are inflated along the circumference of the robot to establish the required steering moment at the robot’s tip [14]. Thus, the lengthening capacity, the ratio of the length to the diameter, and the compliance body enable vine-like robots to navigate distant cluttered environments as demonstrated in [15], where three key applications could obtain the benefits of vine-growing robots, namely, showing the deploying and reconfiguring structures, navigating constrained environments, and applying forces on the environment.

Thus far, however, there has been little discussion about the control of growing robots’ movements in spatial environments. Feedback control is particularly crucial in vine robots because their nonlinear dynamics are coupled with the absence of effective methods to deploy sensors throughout their long bodies. In the literature, few studies have aimed to control the growth of robots, whether in the joint or task space. As an example, a stimulus-based feedback controller has been employed in [10] to control the locomotion of a root-like growing robot. This was based on the tactile measurements perceived from the sensors embedded in the robot’s tip. In [16], minimizing the energy spent during the penetration of soil environments was achieved by controlling the tip of a plant-inspired root robot through an optimal control technique. To guarantee the performance of the robot when it comes to trajectory following, research in [17] applied a joint-space Proportional-Derivative (PD) controller with gravity compensation to the obtained dynamics model of vine robots to guarantee the robot’s performance. A visual servoing Jacobian-based controller is proposed in [18] to maneuver a vine robot into the environment until reaching the desired target defined in image space. However, two key challenges were encountered in the mentioned control schemes, namely, the lack of incorporating the process constraints inhibited by the growing robot, and dealing with the nonlinearity of the coupled dynamics of the robot. Handling the process constraint is particularly critical since the extension of growing robots is managed by an irreversible addition of materials to the robot’s tip, and once the robot has grown to a certain length, it can not be retracted back to smaller values. Moreover, such conventional controllers assume complete knowledge of the robot state; however, in reality, measuring all the states could be cumbersome in lengthy growing robots, or it could not be advisable from an economic perspective.

In this paper, a Nonlinear Model Predictive Control (NMPC) combined with Moving Horizon Estimation (MHE) is proposed for controlling the growth of vine-like growing robots in terms of point stabilization, trajectory tracking, and obstacle avoidance. The contribution of this paper is twofold: (1) a nonlinear MPC control scheme is proposed to control the movement of the vine growing robot while considering the process constraints utilizing an adaptive nonlinear kinematic model of the vine robot, and (2) a precise online estimation of the vine robot’s state, including its Cartesian coordinates, is introduced based on the MHE scheme considering that no such practical methods exist to deploy sensors to measure the spatial location of the vine robot’s tip. Instead of applying the well-known Extended Kalman Filters (EKF) as in [19], an optimization-based nonlinear MHE framework has proposed an online state estimation technique in this research.

In this paper, a Nonlinear Model Predictive Control (NMPC) combined with Moving Horizon Estimation (MHE) is proposed for controlling the growth of vine-like growing robots in terms of point stabilization, trajectory tracking, and obstacle avoidance. The contribution of this paper is twofold: (1) a nonlinear MPC control scheme is proposed to control the movement of the vine-growing robot while considering the process constraints utilizing an adaptive nonlinear kinematic model of the vine robot, and (2) a precise online estimation of the vine robot’s state, including its Cartesian coordinates, is introduced based on the MHE scheme considering that no such practical methods exist to deploy sensors to measure the spatial location of the vine robot’s tip. Instead of applying the well-known Extended Kalman Filters (EKF) as in [19], an optimization-based nonlinear MHE framework has proposed an online state estimation technique in this research.

Section 2 describes the model of the vine robot utilized in this study. The moving horizon state estimation is explained in detail in Section 3 while Section 4 describes the nonlinear model predictive control problem. The numerical simulations are presented in Section 5 and the results and their discussion. Finally, the conclusions from this research are summarized in Section 6.

2. Model of Vine Robots

The “vine growing robot” introduced in [15] is considered in this research. Vine robots are capable of elongating their tips as far as several tens of meters based on the eversion mechanism introduced in [18]. The robot’s core body is constructed from a thin-walled polyethylene tube that is initially flipped inside out as depicted in Figure 1. To achieve tip extension, air pressure is applied to the robot’s chamber, and hence the robot’s tip is pushed forward away from its base. This inside-out eversion mechanism makes vine robots perfect for the exploration of sticky environments. The steering movement in a vine robot is accomplished by applying air pressure to one or two of the serial Pneumatic Actuator Muscles (sPAM) mounted around the robot’s circumference [15]. To facilitate shape sensing of the robot as will be discussed, an Inertial Measurement Unit (IMU) and a shaft encoder are attached to the robot’s tip and the material rod at the base, respectively.

Kinematics of Vine Robots

To obtain the vine-robot’s forward kinematic model, the constant-curvature assumption is commonly used [20]. The vine-like robot is considered a one-section extensible continuum-like robot where two degrees of freedom are responsible for the robot’s curvature and bending, while another is for lengthening the robot. The pose ${\mathbf{T}}_r^b$ of the robot’s distant tip to its base is determined in terms of the robot’s configurations $\mathit{q}\in {\mathrm{I}\mathrm{R}}^3$; this includes the robot’s length s, the curvature angle $\theta$ and the angle of curvature plan $\varphi$ as illustrated in Figure 2. Thus, ${\mathbf{T}}_r^b$ is obtained as follows:

$${\mathbf{T}}_r^b=\left[\begin{array}{cc}{C}_{\varphi }^2(C_{\theta }-1)+1& S_{\varphi }{C}_{\varphi }(C_{\theta }-1)\\ S_{\varphi }{C}_{\varphi }(C_{\theta }-1)& C_{\varphi }^2(1-C_{\theta })+C_{\theta }\\ C_{\varphi }{S}_{\theta }& S_{\varphi }{S}_{\theta }\\ 0& 0\end{array}\right.\left.\begin{array}{cc}-C_{\varphi }{S}_{\theta }& \left(s{C}_{\varphi }(C_{\theta }-1)\right)/\theta \\ -S_{\varphi }{S}_{\theta }& \left(s{S}_{\varphi }(C_{\theta }-1)\right)/\theta \\ C_{\theta }& s{S}_{\theta }/\theta \\ 0& 1\end{array}\right]$$

(1)

where $S_{.}$ and $C_{.}$ represent the trigonometric sine and cosine of an angle. The robot’s tip position $\mathit{p}={[x,y,z]}^T\in {\mathbb{R}}^3$ in Cartesian space can be stated from Equation (1) as

$$\begin{array}{cc}\hfill x& =\left(s{C}_{\varphi }(C_{\theta }-1)\right)/\theta ,\hfill \\ \hfill y& =\left(s{S}_{\varphi }(C_{\theta }-1)\right)/\theta ,\hfill \\ \hfill z& =s{S}_{\theta }/\theta \hfill \end{array}$$

(2)

The sPAMs lengths $\mathit{l}=[s,l_1,l_2,l_3]$ are the actual actuation space of the vine-robot. However, using the shape parameters generalizes the control problem to accommodate any continuum-like robot with the constant-curvature model mentioned earlier.

The velocity $\dot{\mathit{p}}\in {\mathbf{R}}^3$ of the vine robot’s tip relies on the time derivatives of the robot’s configuration $\dot{\mathit{q}}$ as follows:

$$\dot{\mathit{p}}=\mathit{J}\left(\mathit{q}\right)\dot{\mathit{q}}$$

(3)

where $\mathit{J}\left(\mathit{q}\right)\in {\mathbf{R}}^{3\times 3}$, representing the Jacobian matrix, is computed analytically as follows:

$${\mathit{J}}_q\left(\mathit{q}\right)=\frac{\partial \mathit{p}}{\partial \mathit{q}}=\frac{\partial \mathit{p}}{\partial (s,\kappa ,\varphi )}$$

(4)

$\mathit{p}$ represents the Cartesian position of the robot’s tip mentioned in Equation (2).

3. Moving Horizon Estimation (MHE)

Having complete knowledge about the state components is essential to obtain a well-performing closed-loop control scheme for a particular dynamical system. However, in reality, measuring all the system state components could be dis-encouraged from an economic perspective. The number of system components that could be practically measured is usually fewer than the number of state components required for describing the dynamics of that system [21]. Notably, in vine-like growing robots which lengthen up to tens of meters, measurement of the robot’s tip Cartesian position with adequate precision could be challenging in unstructured environments. Therefore, in this section, we discussed how the optimization-based MHE is utilized to estimate the entire state components of the nonlinear model of vine robots from the few available measurements. The estimation in MHE is performed over a fixed-sized sliding time window while complying with the system dynamics and system state and input constraints. More details on constrained MHE could be found in [22].

MHE Optimization Problem

The MHE problem is formulated as follows: At time $t_k$, $N_E$ system measurements represented as ${\tilde{\mathit{y}}}_{k-N_E+1},\dots ,{\tilde{\mathit{y}}}_k\in {\mathbb{R}}^{n_y}$ corresponding to the past instants $t_{k-N_E+1},\dots ,t_k$ along with the nominal system inputs ${\tilde{\mathit{u}}}_{k-N_E+1},\dots ,{\tilde{\mathit{u}}}_{k-1}\in {\mathbb{R}}^{n_u}$ corresponding to the past instants $t_{k-N_E+1},\dots ,t_{k-1}$ have to be available to estimate the full system state ${\tilde{\mathit{x}}}_i\in {\mathbb{R}}^{n_x}$ for all $i=0,1,\dots ,k$. Thus, the MHE recursively solves the following nonlinear constrained optimization problem along the $N_E$ estimation horizon:

$$\begin{array}{cc}\hfill \underset{\mathit{x}(.),\mathit{u}(.)}{min}& \sum _{k=i-N_E}^i\left|\right|{\tilde{\mathit{y}}}_k-h({\mathit{x}}_k,{\mathit{u}}_k){\left|\right|}_{\mathbf{V}}^2\hfill \\ \hfill & +\sum _{k=i-N_E}^{i-1}\left|\right|{\tilde{\mathit{u}}}_k-{\mathit{u}}_k{\left|\right|}_{\mathbf{W}}^2\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \\ \hfill & \begin{array}{c}\dot{\mathit{x}}(k+1)=f(\mathit{x}\left(k\right),\mathit{u}\left(k\right))\hfill \\ {\mathit{x}}_{min}⩽\mathit{x}\left(k\right)⩽{\mathit{x}}_{max},\hfill \\ {\mathit{u}}_{min}⩽\mathit{u}\left(k\right)⩽{\mathit{u}}_{max}\hfill \end{array}\hfill \end{array}$$

(5)

where $h(.):{\mathbb{R}}^{n_x+n_u}\to {\mathbb{R}}^{n_y}$ is the measurement model that characterizes the sensor transfer function with respect to the system state $\mathit{x}$ and input $\mathit{u}$ while $f(.):{\mathbb{R}}^{n_x+n_u}\to {\mathbb{R}}^{n_x}$ indicates the system dynamics. As noted, in (5), the control inputs are included as well in the optimization problem to consider the deviation that could exist between the nominal controls applied and the controls received by the robot due to actuator noise and/or inaccuracy [23].

The matrices $\mathbf{V}⪰\mathit{0}$ and $\mathbf{W}⪰\mathit{0}$ are semi-positive weighting matrices assumed to be constant over the estimation horizon and are inversely proportional to the co-variances of the noise added to the sensor and the control inputs as follows:

$$\begin{array}{cc}\hfill \mathbf{V}& ={\left[\mathrm{diag}({\sigma }_s,{\sigma }_{\theta },{\sigma }_{\varphi })\right]}^{-1},\hfill \\ \hfill \mathbf{W}& ={\left[\mathrm{diag}({\sigma }_{\dot{s}},{\sigma }_{\dot{\theta }},{\sigma }_{\dot{\varphi }})\right]}^{-1}\hfill \end{array}$$

(6)

In vine robots, we assumed that the joint–space values, i.e., the robot length $s_k$, the angle of curvature ${\theta }_k$, and the plane angle ${\varphi }_k$, are measured at each k sample. As discussed in [15], the robot length is indirectly sensed using an incremental encoder placed on the shaft of the motor that holds the robot material. In addition, according to what has been proposed in [24], the robot angle of curvature $\theta$ and the plane angle $\varphi$ could be anticipated from the tip quaternion that is measured using an Inertial Measurement Unit (IMU) place on the robot’s tip. Thus, to find the measurement model $h(.)$ in (5), we assume that the robot’s output ${\mathit{y}}_k={[s_k,{\theta }_k,{\varphi }_k]}^T$ is measured at a sampled time with a T interval. Hence, with the help of the closed-form Inverse kinematic (IK) solution for one-section continuum robots proposed in [25], the measurement model is mainly depending on the tip Cartesian position $(x,y,z)$ and could be formulated as:

$${\left[\begin{array}{c}s\\ \theta \\ \varphi \end{array}\right]}_k={\left[\begin{array}{c}{cos}^{-1}\left(1-\frac{2(x^2+y^2)}{r^2}\right)\left(\frac{r^2}{2\sqrt{x^2+y^2}}\right)\\ {cos}^{-1}\left(1-\frac{2(x^2+y^2)}{r^2}\right)\\ {tan}^{-1}\left(\frac{y}{x}\right)\end{array}\right]}_k$$

(7)

where $r=\sqrt{x^2+y^2+z^2}$ while the subscript k means the measurements and state components at sample k. It is worth mentioning that increasing the estimation horizon $N_H$ will increase the state estimation accuracy. However, the computational time will increase on the other hand. Thus, choosing the value of $N_H$ is a trade-off between speed and accuracy.

4. Nonlinear Model Predictive Growth Control

This segment discusses the NMPC-based growth control method that has been proposed to control the growth of vine robots in a closed-loop manner. The NMPC intends to take into account the irreversible growing constraint and the feedback restrictions shown by vine robots to achieve the control objectives: collision avoidance, point stability, and trajectory following in task space.

4.1. Model Description

To account for the vine robots’ irreversible growth constraint, the state $\mathit{x}={\left[\mathit{p}\mathit{q}\right]}^T\in {\mathbb{R}}^6$ was chosen to incorporate both the position of the robot’s $\mathit{p}$ in Cartesian space and its joint variables $\mathit{q}$. Due to the lack of full state information, the proposed MHE is combined to predict the full state of the robot. As a result, the nonlinear model representing vine robot motion kinematics is defined as

$$\dot{\mathit{x}}=\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{z}\\ \dot{s}\\ \dot{\theta }\\ \dot{\varphi }\end{array}\right]=f(\mathit{x},\mathit{u})=\left[\begin{array}{c}\mathit{J}\left(\mathit{q}\right)\\ {\mathit{I}}_3\end{array}\right]\mathit{u}$$

(8)

where $\mathit{J}\left(\mathit{q}\right)\in {\mathbb{R}}^{3\times 3}$ is the Jacobian of the robot in Equation (3), and $\mathit{u}=\dot{\mathit{q}}\in {\mathbb{R}}^3$ is the velocity in the robot’s configuration space reflecting the manipulated variables. $\mathit{x}$ is the vine robot’s state. It is the controlled variable, and it is fully observed with the help of the MHE state estimation that has been mentioned earlier. The vine robots’ irreversible growing constraint is described as an inequality constraint forced on the robot’s growing velocity and length, i.e.,

$$\left[\begin{array}{c}0\\ 0\end{array}\right]\le \left[\begin{array}{c}\dot{s}\left(t\right)\\ s\left(t\right)\end{array}\right]\le \left[\begin{array}{c}{\dot{s}}_{max}\\ s_{max}\end{array}\right],\forall t\ge 0$$

(9)

4.2. Control Objective

The main intention of the proposed MPC-based growth control scheme is to confirm the vine robot’s stabilization performance over a desired reference state ${\mathit{x}}_r={[x_r,y_r,z_r,s_r,{\theta }_r,{\varphi }_r]}^T$ specified in task and joint spaces. The proposed controller has to take into account the irreversible growth of the robot and the actuators’ limit constraints while finding the optimal control actions. The objective function J is selected such that the tracking performance is enhanced, and the control effort over a prediction horizon N is minimized as follows:

$$\begin{array}{cc}\hfill J\left(k\right)=& \sum _{j=1}^N{\mathit{e}}_{(k+j)}^T\mathit{Q}{\mathit{e}}_{(k+j)}\hfill \\ \hfill & +\sum _{j=1}^N\Delta {\mathit{u}}_{(k+j-1)}^T\mathit{R}\Delta {\mathit{u}}_{(k+j-1)}\hfill \end{array}$$

(10)

The tracking error is represented as $\mathit{e}=\mathit{x}-{\mathit{x}}_r$, while $\Delta \mathit{u}$ represents the control increment. The objective function’s weighting matrices, $\mathit{Q}\ge 0$ and $\mathit{R}\ge 0$, are assumed to be fixed over the prediction horizon N.

4.3. Controller Design

The MPC approach introduced regulating the growing process of the vine robots that is explained in Figure 3. The robot’s velocities ($\mathit{u}=\dot{\mathit{q}}$) in the configuration space are chosen as the manipulated variables that are involved to extend or bend the vine robot. The goal is to get the robot state $\mathit{x}\left(t\right)$ to be as close as possible to the reference state ${\mathit{x}}_r$ in the point stabilization scenarios and to the reference trajectory ${\mathit{x}}_r\left(t\right)$ in trajectory tracking for all time instances t. Meanwhile, taking into account the growth and control limit constraints is necessary for the control approach.

As pictured from Equation (8), the vine-like growing robot kinematic model is continuous and nonlinear as it relies on $\mathit{q}$, which is the robot’s configuration. In standard MPC, the prediction model commonly consists of a discrete-time linear model for the process under discussion. However, the prediction model in the proposed NMPC-based growth control is chosen as the discrete variant of the robot’s kinematic model. The Euler discretization technique at each instance k along the predication horizon,

$$\mathit{x}(k+1)=\mathit{x}\left(k\right)+\Delta T\left[\begin{array}{c}\mathit{J}\left(\mathit{q}\right(k\left)\right)\\ {\mathit{I}}_3\end{array}\right]\mathit{u}\left(k\right)$$

(11)

$\Delta T$ indicates the sampling time. Using this prediction model, the NMPC is able to predict the robot’s state ${\mathit{x}}_p$ along the prediction horizon while looking into every possible control action $\overline{\mathit{u}}$ as indicated in Figure 3.

5. Results and Discussion

We conducted a series of simulation experiments to assess the performance of the proposed Nonlinear MPC scheme for control of the growing robot. Namely, the performance of the MHE robot’s state estimation, point stabilization, obstacle avoidance, and trajectory tracking performance. In these experiments, MATLAB is used to simulate the robot in different scenarios, where the robot initially has an initial length $s_0$ with curvature equal to zero.

5.1. MHE Performance Evaluation

In the first set of simulation experiments, we evaluated the performance of the proposed MHE towards full state estimation of the vine robot. Measurements of joint-space values ${\mathit{y}}_m$ were involved in the estimation process with estimation horizon $N_e=10$ and sample time $T=0.1$. A sensor noise was added to the measurements with ${\sigma }_s=0.1$ (m), ${\sigma }_{\theta }=10$ deg., and ${\sigma }_{\varphi }=10$ deg. while uncertainties in the control inputs with ${\sigma }_{\dot{s}}=0.1$ (m/s), ${\sigma }_{\theta }=10$ deg./s, and ${\sigma }_{\varphi }=10$ deg./s were considered. Figure 4 shows the joint-space values $s_e,{\theta }_e$ and ${\varphi }_e$ estimated from measurements $s_e,{\theta }_e$ and ${\varphi }_e$. Meanwhile, the robot’s Cartesian position, $x_e$, $y_e$ and $z_e$ are estimated and compared to the ground-truth data generated beforehand. A satisfactory small Sum of Root Mean Square Errors (SRMSE) is achieved of 74.5 mm where

$$\mathrm{SRMSE}={\mathrm{RMSE}}_x+{\mathrm{RMSE}}_y+{\mathrm{RMSE}}_z$$

(12)

and where

$$\mathrm{RMSE}=\sqrt{{\displaystyle \frac{1}{N_s}}\sum _{i=1}^{N_s}{(\psi -{\psi }_e)}^2},\psi =x,y\mathrm{or}z$$

with $N_s$ samples of the estimated ${\psi }_e$ and the ground-truth $\psi$ data.

Table 1. Effect of increasing the estimation horizon $N_e$ on the performance and the average computational time of the MHE.

${\mathit{N}}_{\mathit{e}}$	RMSE (m)	Avg. Computational Time (ms)
6	0.1333	19.3
8	0.0785	19.8
10	0.0745	21.4
15	0.0609	24.6
20	0.0625	29.0
40	0.0633	44.0
100	0.0645	112.2

shows how increasing the estimation horizon values could affect the estimation performance and the mean computation time of the MHE. Particularly, increasing $N_e$ values will increase the mean computation time dramatically. However, surprisingly, there is no significant enhancement achieved in terms of reduced SRMSE. This is due to the initial state estimation at $t_k=N_e$ that mainly depends on the unknown initial state $\mathit{x}\left(0\right)$. It is worth mentioning that increasing the estimation horizon has negative effects when it comes to incorporating the state estimation of MHE with MPC, since the controller has to wait for $N_e$ samples until having a reliable estimation of the unknown state. Otherwise, during the time $t_k<N_e$, the MPC will rely on inaccurate estimation, as will be shown shortly.

5.2. Point Stabilization Results

As previously discussed, one of the purposes of vine-like growing robots is to act as a channel to transport supplies to those in dangerous situations. In this first simulation experiment, based on the state estimation achieved by the proposed MHE framework, the vine robot began at an initial state ${\mathit{x}}_0={[0,0,0.4,0.4,0,0]}^T$, and the suggested NMPC-based growth controller is involved in maintaining the robot’s tip at predefined desired states in the space, i.e., ${\mathit{x}}_d\in {\mathbb{R}}^6$. These goals could describe potential places for the robot to attend within the working environment. The sampling time is decided to be $T_s=0.1s$ with a prediction horizon $N=10$. Diagonal state and input weighting matrices in (10) are taken, where $\mathit{Q}=\mathrm{diag}(1,1,1,0,0,0)$, while $\mathit{R}=\mathrm{diag}(0.5,0.5,0.5)$. The working environment is designed to be constrained between $[-4,4]$ m amongst the three Cartesian coordinates. The extremes of the robot’s shape variables were selected based on the robot’s configuration limits discussed beforehand in the kinematics section.

$$\begin{array}{cc}\hfill -4& \le \left[\begin{array}{c}x\left(m\right)\\ y\left(m\right)\\ z\left(m\right)\end{array}\right]\le 4,\hfill \\ \hfill \left[\begin{array}{c}0\\ -\pi \\ -\pi \end{array}\right]& \le \left[\begin{array}{c}s\left(m\right)\\ \theta \left(rad\right)\\ \varphi \left(rad\right)\end{array}\right]\le \left[\begin{array}{c}10\\ \pi \\ \pi \end{array}\right]\hfill \end{array}$$

(13)

On the other side, we chose the input inequality constraints that consider the irreversible characteristics of the growing vine-like robot and the actuator boundaries as follows:

$$\left[\begin{array}{c}0\\ -\frac{\pi }{10}\\ -\frac{\pi }{10}\end{array}\right]\le \left[\begin{array}{c}\dot{s}(m/s)\\ \dot{\theta }(rad/s)\\ \dot{\varphi }(rad/s)\end{array}\right]\le \left[\begin{array}{c}0.1\\ \frac{\pi }{10}\\ \frac{\pi }{10}\end{array}\right]$$

(14)

Figure 5 shows the proposed NMPC-based growth controller performance in the point stabilization simulation scenario. Throughout the experiment’s initial 25 s, the controller maintains the vine robot’s tip at $(1,1,1)$ m distance from its base. Satisfying rising time results (20 s) are achieved to attain the objective using the highlighted actuators’ inputs. The z coordinate of the goal is doubled, as illustrated, during the next 25 s, where the length of the robot s is required to be increased. Based on the vine robot’s kinematics, extending the robot’s length to arrive at the new z value would have an effect on the other x and y coordinates. As a result, when reaching this new target, both x and y positions have been marginally affected; this is demonstrated in Figure 5. To address this problem and restore the tip of the robot, the NMPC increased the robot length s while actuating the curvature angle $theta$ in the positive direction. After some time, only the robot length is increased to accommodate for the adjusted x and y locations. Subsequently, following the passing of 50 s of simulation time, the robot is intended to attend a new z target lower than the preceding one. This task necessitates the robot shortening its length. However, because of the irreversible nature of the growing mechanism, the robot is bound and unable to shorten its grown length. As a result, the NMPC reduced the robot curvature in the hope of achieving the current specified goal. While this helped achieve a minimal error in the z coordinate, it substantially impacted the other two coordinates. The NMPC meets the vine-like robot’s state and input saturation limits at all phases.

The NMPC primarily relies on incorrect state computations by solving the forward kinematics in (1) to find the corresponding robot’s Cartesian positions, since the MHE only estimates the robot’s state after passing at least $N_e$ samples. As seen in Figure 5, the approximate states and, as a result, the control behaviors are very noisy during the initial samples of $k<N_e$ relative to the remaining samples.

5.3. Obstacle Avoidance

In the second simulation scenario, the proposed NMPC was assessed against avoiding obstacles in the field. As a result, a static point obstacle with a circular structure is located at ${\mathit{x}}_o={[x_o,y_o,z_o]}^T$ within the robot’s path from a point of reference ${\mathit{x}}_0$ to the target ${\mathit{x}}_g={[1,1,1]}^T$ metres from its base. At this stage, the estimation horizon is set to $N=30$, and the sampling time is set to $T=0.1$ s. A nonlinear inequality restriction has been added to the NMPC’s optimal control problem to escape the obstacle by keeping the Euclidean distance between both the tip of the robot $(x,y,z)$ and the obstacle’s location within a specified secure distance $(r_t+r_o)$ as follows:

$$-\infty \le d_{r,o}+\left(r_t+r_o\right)\le 0$$

(15)

where

$$d_{r,o}=\sqrt{{\left(x-x_o\right)}^2+{\left(y-y_o\right)}^2+{\left(z-z_o\right)}^2}$$

is the span between both the robot’s tip and the obstacle, and the robot’s tip and obstacle radii are $r_t=0.1$ m and $r_o=0.1$ m, respectively. The NMPC has successfully found a secure way for the robot to escape the obstacle using the condition calculated from the MHE, as seen in Figure 6. The subsequent actuation shows that, during navigation, the robot must change its bending angle and curvature to escape the obstacle. It is worth noting that Equation (15) only considers the tip position. This solution to avoiding obstacles cannot promise that the vine robot’s whole body will escape the obstacle. Nevertheless, this could be addressed in future work by splitting the body of the robot into segments whose positions could be predicted using the robot’s shape parameters. After that, another restriction could be applied to ensure that these segments are not in the path of the obstacle.

5.4. Trajectory Tracking

The performance of the proposed NMPC-based growth controller against trajectory tracking has been evaluated using a spiral reference trajectory. The spiral movement could come in handy in case the robot has to loop around a pillar to get to the tip. The coordinates of the reference state ${\mathit{x}}_{ref}$ have been selected as follows:

$$\begin{array}{cc}\hfill x_{ref}& =2+cos\left(0.25t\right)\hfill \\ \hfill y_{ref}& =2+sin\left(0.25t\right)\hfill \\ \hfill z_{ref}& =2+0.2t\hfill \end{array}$$

(16)

We have chosen $\mathit{x}0={[0,0,2,2,0,0]}^T$ as the initial state of the robot. For an overall simulation time of 20 s, the controller time step is set to $t=0.1$ s and the prediction horizon to $N=20$ s. $\mathit{R}=\mathrm{diag}(0.1,0.1,0.1)$ and $\mathit{Q}=\mathrm{diag}(10,10,1,0,0,0)$ have been chosen as the input and state weighting matrices, respectively. The NMPC output is seen in Figure 7 in terms of the difference between the real and comparison trajectories. The measured Sum Root of the Mean Square Error SRMSE between both the reference and the real robot trajectory is $1.07$ m, which includes the substantial initial error related to the shortage of MHE state estimation. This problem, we believe, could be addressed by properly designed weighting $\mathit{Q}$ and $\mathit{R}$ matrices. With an SRMSE of $1.07$ m, the proposed NMPC achieves adequate tracking efficiency while accepting the imposed limit and the other robot’s locomotion constraints.

5.5. Effect of Sensors’ Noise Levels

To examine the robustness of the suggested NHE-based NMPC growth control across an extensive range of sensor uncertainties, Monte Carlo simulations are used to assess the robustness of tracking performance concerning change in the added sensor noise levels. This strategy would assess the impact of varying the sensor noise levels on the performance of the proposed MHE and NMPC with no obligation on simulating every parameter change separately, which would take up a significant amount of time.

Therefore, 150 values of the variance ${\sigma }_s$, ${\sigma }_{\theta }$ and ${\sigma }_{\varphi }$ are randomly chosen from an admissible uniform distributions $[0.1,0.3]$ m/s, $[2,10]$ rad/s, and $[2,10]$ rad/s. The level of noise superimposed to the sensor readings is described by those uncertainties. The NMPC-based growth control is evaluated by following the trajectory proposed in Section 5.4 by measuring the RMSE at each simulation situation. Figure 8 shows the results of 20 scenarios that have been chosen and ordered according to the RMSE values. We can understand that the RMSE increased significantly in scenarios possessing low noise levels superimposed to the robot’s sensing model. Although the obtained performance characterization depends essentially on the chosen weighting matrices, i.e., $\mathit{Q}$ and $\mathit{R}$, this could account for the fact that, with low levels of sensors noise, the MHE is not able to cover the high variability in the sensor readings in our model. In vine robots, where the robot moves in wide terrains, higher noise levels should be considered to estimate the robot’s state.

6. Conclusions

This article introduces a Moving Horizon Estimation (MHE) coupled with a Nonlinear Model Predictive Control (NMPC) scheme for moving a vine-growing robot’s tip to a spatial target position in the environment. Based on the predicted robot’s state, the suggested MHE-based NMPC growth control preserved the actuation and irreversible growing constraints while effectively operating the robot in a closed-loop. The vine robot’s nonlinear kinematic model has been used as a controlled plant, with a discrete version functioning as the controller’s prediction model. Analyzing the proposed NMPC growth regulation was carried out through various situations, including point stability, trajectory following, and obstacle avoidance, and the findings are promising. Furthermore, the impact of sensor noise levels on the efficiency of the proposed MHE-based NMPC has been investigated using Monte Carlo simulations to assess the robot’s development under different noise levels. Instead of the kinematic model, the dynamics model of vine robots is used to expand our research. In addition, the use of the Inverse Optimal Control (IOC) [26] seems beneficial in interpreting the objective function from the human demonstrations instead of hand engineering. Meanwhile, comparing the proposed NMPC algorithm with the existing non-traditional control scheme, such as [27,28,29] is promising as a future work.