A Novel Feedforward Youla Parameterization Method for Avoiding Local Minima in Stereo Image-Based Visual Servoing Control

Abstract

In robot navigation and manipulation, accurately determining the camera’s pose relative to the environment is crucial for effective task execution. In this paper, we systematically prove that this problem corresponds to the Perspective-3-Point (P3P) formulation, where exactly three known 3D points and their corresponding 2D image projections are used to estimate the pose of a stereo camera. In image-based visual servoing (IBVS) control, the system becomes overdetermined, as the six degrees of freedom (DoF) of the stereo camera must align with nine observed 2D features in the scene. When more constraints are imposed than available DoFs, global stability cannot be guaranteed, as the camera may become trapped in a local minimum far from the desired configuration during servoing. To address this issue, we propose a novel control strategy for accurately positioning a calibrated stereo camera. Our approach integrates a feedforward controller with a Youla parameterization-based feedback controller, ensuring robust servoing performance. Through simulations, we demonstrate that our method effectively avoids local minima and enables the camera to reach the desired pose accurately and efficiently.

1. Introduction

Determining the accurate pose of the camera is a fundamental problem in robot manipulations, as it provides the spatial transformation needed to map 3D world points to 2D image coordinates. The task involving camera pose estimation is essential for various applications, such as augmented reality [1], 3D reconstruction [2], SLAM [3], and autonomous navigation [4]. This becomes especially critical when robots operate in unstructured, fast-changing, and dynamic environments, performing tasks such as human–robot interaction, accident recognition and avoidance, and eye-in-hand visual servoing. In such scenarios, accurate camera pose estimation ensures that visual data are readily available for effective robotic control [5].

A classic approach to estimating the pose of a calibrated camera is solving the Perspective-n-Point (PnP) problem [6], which establishes a mathematical relationship between a set of 3D points in the world and their corresponding 2D projections in an image. To uniquely determine the pose of a monocular camera in space, there exists the Perspective-4-Point (P4P) problem, where exactly four known 3D points and their corresponding 2D image projections are used. Bujnak et al. [7] generalize four solutions for the P3P problem while giving a single unique solution for the P4P problem in a fully calibrated camera scenario. To increase accuracy, modern PnP approaches consider more than three 2D–3D correspondences. Among PnP solutions, the Efficient PnP method (EPnP) finds the optimal estimation of pose from a linear system that expresses each reference point as a weighted sum of four virtual control points [8]. Another advanced approach, Sparse Quadratic PnP (SQPnP), formulates the problem as a sparse quadratic optimization, achieving enhanced accuracy by minimizing a sparse cost function [9].

In recent years, many other methods have been developed to show improved accuracy over PnP-based methods. For instance, Alkhatib et al. [10] utilize Structure from Motion (SfM) to estimate a camera’s pose by extracting and matching key features across various images taken from different viewpoints to establish correspondences. Moreover, Wang et al. [11] introduce visual odometry into the camera’s pose estimations based on the movement between consecutive frames. In addition, recent advancements in deep learning have led to the development of models, such as Convolutional Neural Networks (CNNs), specifically tailored for camera pose estimation [12,13,14]. However, these advanced methods often come with significant computational costs, requiring multiple images from different perspectives for accurate estimation. In contrast, PnP-based approaches offer a balance between accuracy and efficiency, as they can estimate camera pose from a single image, making them highly suitable for real-time applications such as navigation and scene understanding.

In image-based visual servoing (IBVS) [15], the primary goal is to control a robot’s motion using visual feedback. Accurate real-time camera pose estimation is crucial for making informed control decisions, particularly in eye-in-hand (EIH) configurations [16,17], where a camera is mounted directly on a robot manipulator. In this setup, robot motion directly induces camera motion, making precise pose estimation essential. Due to its computational efficiency, PnP-based approaches remain widely applied in real-world IBVS tasks [18,19,20]. The PnP process begins by establishing correspondences between 3D feature points and their 2D projections in the camera image. The PnP algorithm then computes the camera pose from these correspondences, translating the geometric relationship into a format that the IBVS controller can use. By detecting spatial discrepancies between the current and desired camera poses, the robot can adjust its movements accordingly.

However, PnP-based IBVS presents challenges for visual control in robotics. One key issue is that IBVS often results in an overdetermined system, where the number of visual features exceeds the number of joint variables available for adjustment. For example, at least four 2D–3D correspondences are needed for a unique pose solution [6], but a camera’s full six-degree-of-freedom (6-DOF) pose means that a 6-DOF robot may need to align itself with eight or more observed features. In traditional IBVS [15], the interaction matrix (or image Jacobian) defines the relationship between feature changes and joint velocities. When the system is overdetermined, this matrix contains more constraints than joint variables, leading to redundant information. Research [15] suggests that this redundancy may cause the camera to converge to local minima, failing to reach the desired pose. Although local asymptotic stability is always ensured in IBVS, global asymptotic stability cannot be guaranteed when the system is overdetermined.

In recent research on visual servoing, various methods have been developed to address the limitations of traditional Image-Based Visual Servoing (IBVS), particularly the issue of local minima due to system overdetermination. Gans et al. [21] proposed a hybrid switched-system control system, which alternates between IBVS and Position-Based Visual Servoing (PBVS) modes based on Lyapunov functions. This method offers formal local stability and effectively prevents loss of visual features or divergence from the goal. However, it may suffer from discontinuities near switching surfaces and is limited by local convergence guarantees. Chaumette et al. [22] developed a 2½ D visual servoing method, which integrates both image and pose features into a block-triangular Jacobian structure, allowing for smoother and more predictable camera trajectories while avoiding singularities. Yet, it is sensitive to depth estimation errors and requires careful selection of hybrid features. Roque et al. [23] introduced an MPC-based IBVS framework for quadrotors, which leverages model predictive control to handle under-actuation while tracking IBVS commands. It provides formal stability guarantees and supports constraint handling in real time, but its reliance on linearization around hover and the absence of direct coupling with visual dynamics limit its generality and precision in highly nonlinear settings.

While these methods achieve significant improvements in most scenarios, they also introduce computational challenges compared to traditional IBVS. The switched control method requires different control strategies tailored to specific dynamics, increasing the complexity of the overall control architecture [24]. The 2-1/2-D visual servoing method demands real-time processing of both visual and positional data, which can impose significant computational loads and limit performance in dynamic environments [25]. MPC approaches introduce additional computational overhead by requiring complex optimization at every time step, making real-time implementation costly [26].

In this paper, we focus on the PnP framework for determining and controlling the pose of a stereo camera within an image-based visual servoing (IBVS) architecture. In traditional IBVS, depth information between objects and the image plane is crucial for developing the interaction matrix. However, with a monocular camera, depth can only be estimated or approximated using various algorithms [15], and inaccurate depth estimation may lead to system instability. In contrast, a stereo camera system can directly measure depth through disparity between two image planes, enhancing system stability.

A key novelty of this paper is providing systematic proof that stereo camera pose determination in IBVS can be formulated as a Perspective-3-Point (P3P) problem, which, to the best of our knowledge, has not been explored in previous research. Since three corresponding points, totaling nine coordinates, are used to control the six DoF camera poses, the IBVS control system for a stereo camera is overdetermined, leading to the potential issue of local minima during control maneuvers. While existing approaches can effectively address local minima, they often introduce excessive computational overhead, making them impractical for high-speed real-world applications.

To address this challenge, we propose a feedforward–feedback control architecture. The feedback component follows a cascaded control loop based on the traditional IBVS framework [15], where the inner loop handles robot joint rotation, and the outer loop generates joint angle targets based on visual data. One key improvement in this work is the incorporation of both kinematics and dynamics during the model development stage. Enhancing model fidelity in the control design improves pose estimation precision and enhances system stability, particularly for high-speed tasks. Both control loops are designed using Youla parameterization [27], a robust control technique that enhances resistance to external disturbances. The feedforward controller takes target joint configurations, which are associated with the desired camera pose as inputs, ensuring a fast system response while avoiding local minima traps. Simulation results presented in this paper demonstrate that the proposed control system effectively moves the stereo camera to its desired pose accurately and efficiently, making it well-suited for high-speed robotic applications.

2. System Configuration

An eye-in-hand robotic system was developed to precisely control the pose of a stereo camera system, as illustrated in Figure 1. The robotic manipulator is equipped with six revolute joints, allowing unrestricted movement of the camera across six degrees of freedom (DoFs)—three for positioning and three for orientation. Assume a set of fiducial markers is placed in the workspace, with their coordinates fixed and predefined in an inertial frame. Utilizing the Hough transform [28] in computer vision, these markers can be detected and localized by identifying their centers in images captured by the stereo camera system. The control system within the robotic manipulator aligns the camera to its desired pose by matching the detected 2D features in the current frame with target 2D features. Throughout this process, it is assumed that all fiducial markers remain within the camera’s field of view. As depicted in Figure 1, multiple Cartesian coordinate systems are illustrated. The base frame {O} serves as an inertial reference fixed to the bottom of the robot manipulator, while the camera frame {C} is a body-fixed frame attached to the robot’s end-effector.

3. Proof of P3P for the Stereo Camera System

Given its intrinsic parameters and a set of n correspondences between its 3D points and 2D projections to determine the camera’s pose is known as the perspective-n-point (PnP) problem. This well-known work [7] has proved that at least four correspondences are required to uniquely determine the pose of a monocular camera, a situation referred to as the P4P problem.

To illustrate, consider the P3P case for a monocular camera (Figure 2). Let points A, B, and C exist in space, with $O_1$, $O_2$ and $O_3$ representing different perspective centers. The angles ∠AOB, ∠AOC and ∠BOC remain the same across all three perspectives. Given a fixed focal length, the image coordinates of points A, B, and C will be identical when observed from these three perspectives. In other words, it is impossible to uniquely identify the camera’s pose based solely on the image coordinates of three points.

The PnP problem with a stereo camera has not been thoroughly addressed in prior research. A stereo camera can detect three image coordinates of a 3D point in space. This paper proposes that a complete solution to the PnP problem for a stereo camera can be framed as a P3P problem. Below is the complete proof of this proposition.

Proof. 

For a stereo system, if all intrinsic parameters are fixed and given, we can readily compute the 3D coordinates of an object point given the image coordinates of that point. This provides a unique mapping from the image coordinates of a point to its corresponding 3D coordinates in a Cartesian frame. The orientation and position of the camera system uniquely define the origin and axis orientations of this Cartesian coordinate system in space. Consequently, PnP problem can be framed as follows: given n points with their 3D coordinates measured in an unknown Cartesian coordinate system in space, what is the minimum number n required to accurately determine the position and orientation of the 3D Cartesian coordinate frame established in that space?

(1)

P1P problem with the stereo camera:

If we know the coordinates of a single point in space, defined by a 3D Cartesian coordinate system, an infinite number of corresponding coordinate systems can be established. Any such coordinate system can have its origin placed on the surface of a sphere centered at this point, with a radius $R=\sqrt{{X^C}^2+{Y^C}^2+{Z^C}^2}$, where ${[X^C,Y^C,Z^C]}^T$ are the coordinates measured by the Cartesian system (see Figure 3).

(2)

P2P problem with the stereo camera:

When the coordinates of two points in space are known, an infinite number of corresponding coordinate systems can be established. Any valid coordinate system can have its origin positioned on a circle centered at point O with a radius $R$ as illustrated in Figure 4. This circle is constrained by the triangle formed by points $O_C$, $P_1$, and $P_2$, where the sides of the triangle are defined by the lengths $R_1, R_2$ and $D$. Specifically, $R_1=\sqrt{{X_1^C}^2+{Y_1^C}^2+{Z_1^C}^2}, R_2=\sqrt{{X_2^C}^2+{Y_2^C}^2+{Z_2^C}^2},$ $D=\sqrt{{(X}_1^C-{X_2^C)}^2+(Y_1^C-{Y_2^C)}^2+(Z_1^C-{Z_2^C)}^2}.$

The radius of the circle R corresponds to the height of the base D of the triangle. The center of the circle O is located at the intersection of the height and the base.

(3)

P3P problem with the stereo camera:

When three points in space are known, and the lines connecting these points are not collinear, we can uniquely establish one coordinate system. As illustrated in the Figure 5 below, three non-collinear points define a plane in space, which has a uniquely defined normal unit vector $\stackrel{⃑}{n}$. Given the coordinates of the three points, we can calculate vectors as follows: the vector $\overrightarrow{P_1{P}_2}$ = ($X_2^C-X_1^C,Y_2^C-Y_1^C,Z_2^C-Z_1^C$), and the vector $\overrightarrow{P_1{P}_3}$ = ($X_3^C-X_1^C,Y_3^C-Y_1^C,Z_3^C-Z_1^C$). The unit vector $\stackrel{⃑}{n}$ which is perpendicular to the plane formed by these three points, can be expressed as:

$$\stackrel{⃑}{n}={\displaystyle \frac{\overrightarrow{P_1{P}_2}\times \overrightarrow{P_1{P}_3}}{\left|\overrightarrow{P_1{P}_2}\times \overrightarrow{P_1{P}_3}\right|}}$$

(1)

Here, × denotes the cross product.

The angles between $\stackrel{⃑}{n}$ and XYZ axes of the coordinate system can be expressed as follows:

$$\cos \left({\theta }_x\right)=\stackrel{⃑}{n}·\overrightarrow{i}$$

(2)

$$\cos \left({\theta }_y\right)=\stackrel{⃑}{n}·\overrightarrow{j}$$

(3)

$$\cos \left({\theta }_z\right)=\stackrel{⃑}{n}·\overrightarrow{k}$$

(4)

where ${\theta }_x, {\theta }_y$ and ${\theta }_z$ are the angles between $\stackrel{⃑}{n}$ and the unit vectors in the X, Y, and Z directions, denoted as $\overrightarrow{i}$, $\overrightarrow{j},$ and $\overrightarrow{k}$, respectively. Therefore, with the direction $\stackrel{⃑}{n}$ fixed in space, the orientations of each axis of the coordinate system can be computed uniquely.

According to the P1P problem, the origin of the coordinate system must lie on the surface of a sphere centered at $P_1$ with radius $=\sqrt{{X_1^C}^2+{Y_1^C}^2+{Z_1^C}^2}$ as depicted in Figure 3. Each coordinate system established with a different origin point on the surface of this sphere results in a unique configuration of the axis orientations. Therefore, as the orientations of the axes are defined in space, the position of the frame (or the position of the origin) is also uniquely defined.

In conclusion, the P3P problem is sufficient to solve the PnP problem for a stereo camera system.

Proof concluded □

This proposition indicates that to uniquely determine the full six DoFs of the stereo camera, at least three points (or nine 2D features) are required to match in the image-based visual servoing control.

4. Model Development

4.1. Stereo Camera Model

Depth between the objects to the camera plane is either approximated or estimated in the IBVS for generating the interaction matrix [15]. Using a stereo camera system in IBVS eliminates the inaccuracies associated with monocular depth estimation, as it directly measures depth by leveraging the disparity between the left and right images.

The stereo camera model is illustrated in Figure 6. A stereo camera consists of two lenses separated by a fixed baseline b. Each lens has a focal length $F$ (measured in mm) which is the distance from the image plane to the focal point. Assuming the camera is calibrated, the intrinsic parameters: b, $F$ is accurately estimated. A scene point $I$ is measured in the 3D coordinate frame {C} centered at the middle of the baseline with its coordinates as ${[X^C, Y^C,Z^C]}^T$. The stereo camera model maps the 3D coordinates of this point to its 2D coordinates projected on the left and right image plane as ${[u_l,v]}^T$ and ${[u_r,v]}^T$, respectively. The full camera projection map, incorporating both intrinsic and extrinsic parameters, is given by:

$$s·p_{image}=K·\left[R\right|T]{·P}_C$$

(5)

where $p_{image}$ are the image coordinates of the point and $P_C$ are the 3D coordinates measured in the camera frame {C}. $s$ is the scale factor that ensures correct projection between 2D and 3D features. $K$ is the intrinsic matrix with a size of 3 × 3, and the mathematical expression is presented as:

$$K=\left[\begin{array}{ccc} F& k& u_0\\ 0& F& v_0\\ 0& 0& 1\end{array}\right]$$

(6)

where $k$ is the skew factor, which represents the angle between the image axes (u and v axis). $u_0$ and $v_0$ are coordinates offsets in image planes.

In Equation (5), R is the rotational matrix from camera frame {C} to each image coordinate frame, and T is the translation matrix from camera frame {C} to each camera lens center. Since there is no rotation between the camera frame {C} and image frames but only a translation along the $X_C$ axis occurs, the transformation matrices for the left and right image planes are expressed as:

$${\left[R\right|T]}_{Left}=\left[\begin{array}{ccc}\begin{array}{ccc} 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}& |& \begin{array}{c}-b/2\\ 0\\ 0\end{array}\end{array}\right]$$

(7)

$${\left[R\right|T]}_{Right}=\left[\begin{array}{ccc}\begin{array}{ccc} 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}& |& \begin{array}{c}b/2\\ 0\\ 0\end{array}\end{array}\right]$$

(8)

Assume the $u$ and $v$ axis are perfectly perpendicular (take k = 0), and there are no offsets in the image coordinates (take $u_0=v_0=0$) for both lens. Also, set factor $s=Z_I^C$ accounts for perspective depth scaling. The projection equations for the left and right image planes can be rewritten in homogeneous coordinates as:

$$Z_I^C·\left[\begin{array}{c}{u}_l\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc} F& 0& 0\\ 0& F& 0\\ 0& 0& 1\end{array}\right]·\left[\begin{array}{ccc}\begin{array}{ccc} 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}& |& \begin{array}{c}-b/2\\ 0\\ 0\end{array}\end{array}\right]·\left[\begin{array}{c}\begin{array}{c}{X}_I^C\\ Y_I^C\end{array}\\ \begin{array}{c}{Z}_I^C\\ 1\end{array}\end{array}\right]$$

(9)

$$Z_I^C·\left[\begin{array}{c}{u}_r\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc} F& 0& 0\\ 0& F& 0\\ 0& 0& 1\end{array}\right]·\left[\begin{array}{ccc}\begin{array}{ccc} 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}& |& \begin{array}{c}b/2\\ 0\\ 0\end{array}\end{array}\right]·\left[\begin{array}{c}\begin{array}{c}{X}_I^C\\ Y_I^C\end{array}\\ \begin{array}{c}{Z}_I^C\\ 1\end{array}\end{array}\right]$$

(10)

Equations (9) and (10) establish the mathematical relationship between the 3D coordinates of a point in the camera frame {C} and its 2D projections on the left and right image planes. The pixel value along the $v$-axis remains the same for both images. As a result, a scene point’s 3D coordinates can be mapped to a set of three image coordinates in the stereo camera system, expressed as:

$$\mathrm{Stereo}\text{-}\mathrm{camera} \mathrm{mapping} M : P^C={[X^C, Y^C,Z^C]}^T\to p_{image}={[u_l,u_r,v]}^T$$

(11)

The mapping function $M$ is nonlinear and depends on the stereo camera parameters $P_{a\_camera}$, specifically b, and $F$.

4.2. Robot Manipulator Forward Kinematic Model

A widely used method for defining and generating reference frames in robotic applications is the Denavit–Hartenberg (D-H) convention [29]. In this approach, each robotic link is associated with a Cartesian coordinate frame ${ O}_i{X}_i{Y}_i{Z}_i$. According to the D-H convention, the homogeneous transformation matrix $A_i^{i-1}$, which represents the transformation from frame $i-1$ to frame $i$, can be decomposed into a sequence of four fundamental transformations:

$$\begin{array}{l}{A}_i^{i-1}={Rot}_{z,q_i}{Trans}_{z,d_i}{Trans}_{x,a_i}{Rot}_{x,{\alpha }_i}\\ \mathrm{E}\mathrm{x}\mathrm{p}\mathrm{a}\mathrm{n}\mathrm{d}\mathrm{i}\mathrm{n}\mathrm{g} \mathrm{t}\mathrm{h}\mathrm{e} \mathrm{t}\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{s}\mathrm{f}\mathrm{o}\mathrm{r}\mathrm{m}\mathrm{a}\mathrm{t}\mathrm{i}\mathrm{o}\mathrm{n} \mathrm{i}\mathrm{n}\mathrm{t}\mathrm{o} \mathrm{i}\mathrm{t}\mathrm{s} \mathrm{m}\mathrm{a}\mathrm{t}\mathrm{r}\mathrm{i}\mathrm{x} \mathrm{f}\mathrm{o}\mathrm{r}\mathrm{m}:\\ A_i^{i-1}=\left[\begin{array}{cccc}{c}_{q_i}& -s_{q_i}& 0& 0\\ s_{q_i}& c_{q_i}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_i\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& a_i\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c_{{\alpha }_i}& {-s}_{{\alpha }_i}& 0\\ 0& s_{{\alpha }_i}& c_{{\alpha }_i}& 0\\ 0& 0& 0& 1\end{array}\right]\end{array}$$

(12)

$$=\left[\begin{array}{cccc}{c}_{q_i}& -s_{q_i}{c}_{{\alpha }_i}& s_{q_i}{s}_{{\alpha }_i}& a_i{c}_{q_i}\\ s_{q_i}& c_{q_i}{c}_{{\alpha }_i}& -c_{q_i}{s}_{{\alpha }_i}& a_i{s}_{q_i}\\ 0& s_{{\alpha }_i}& c_{{\alpha }_i}& d_i\\ 0& 0& 0& 1\end{array}\right]$$

(13)

$$\mathrm{N}\mathrm{o}\mathrm{t}\mathrm{e}:c_{{\theta }_i}\equiv \cos \left(q_i\right), c_{{\alpha }_i}\equiv \cos \left({\alpha }_i\right), s_{{\theta }_i}\equiv \sin \left(q_i\right), s_{{\alpha }_i}\equiv \mathrm{s}\mathrm{i}\mathrm{n}\left({\alpha }_i\right)$$

(14)

The parameters $q_i$, $a_i$, ${\alpha }_i$ and $d_i$ define the link and joint characteristics of the robot. Here, $a_i$ is the link length, $q_i$ is the joint rotational angle, ${\alpha }_i$ is the twist angle, and $d_i$ is the offset between consecutive links. The values for these parameters are determined following the procedure outlined in [29].

To compute the transformation from the end-effector frame $O_6{X}_6{Y}_6{Z}_6$ (denoted as {E}) to the base frame $O_0{X}_0{Y}_0{Z}_0$ (denoted as {O}), we multiply the individual transformations along the kinematic chain:

$$T_6^0=A_1^0{A}_2^1{A}_3^2{A}_4^3{A}_5^4{A}_6^5$$

(15)

Furthermore, the transformation matrix from the base frame {O} to the end-effector frame {E} can be derived by taking the inverse of $T_6^0$:

$$T_0^6=({T_6^0)}^{-1}$$

(16)

If a point $P^O$ is defined in the base frame, its coordinates in the end-effector frame $P^E$ can be found using:

$$P^E=T_0^6 P^O$$

(17)

Equation (17) describes a coordinates transformation process from the base frame to the end-effector frame.

4.3. Eye-to-Hand Transformation Model

Assuming the camera remains fixed relative to the robot’s end-effector, we introduce a constant transformation matrix $T_E^C$ that maps points from the end-effector frame {E} to the camera frame {C}. The process of finding the matrix $T_E^C$ is called Eye-to-Hand Calibration.

In general, the goal is to solve for $T_E^C$, such that:

$$A_i^j{·T}_E^C=T_E^C· B_i^j$$

(18)

Here, $A_i^j$ is transformation of the end-effector between robot poses $i$ to pose $j$, and $B_i^j$ represents the transformation of a fixed object (e.g., a marker) as observed in the camera frame during the same motion.

These transformations are computed as:

$$A_i^j={{(T}_0^{6,i})}^{-1}{·T}_0^{6,j}$$

(19)

$$B_i^j={{(T}_M^{C,i})}^{-1}{·T}_M^{C,j}$$

(20)

where $T_0^{6,i}$ and $T_0^{6,j}$ denote the poses of the end-effector at robot configurations $i$ and $j$, measured in the base frame, as defined in Equation (17). $T_M^{C,i}$ and $T_M^{C,j}$ denote the transformations of a marker M observed in the camera frame at the same respective robot poses. Further details are provided in Appendix C.

Various methods have been proposed to accurately estimate $T_E^C$ using multiple $(A_i^j, B_i^j)$ pairs. While calibration techniques are not the primary focus of this paper, several recent approaches are summarized for reference:

Kim et al. [30] presented a technique for eye-on-hand configurations, deriving a closed-form solution for Equation (18) using two robot movements. Evangelista et al. [31] introduced a unified iterative method based on reprojection error minimization, applicable to both eye-on-base and eye-in-hand setups. This approach does not require explicit camera pose estimation and demonstrates improved accuracy compared to traditional methods. Strobl and Hirzinger [32] proposed an optimal calibration method for eye-in-hand systems, utilizing a novel metric on SE(3) and an error model for nonlinear optimization. This method adapts to system precision characteristics and performs well in Equation (18) formulation.

4.4. Robot-to-Camera Kinematic Model

For a stereo camera system, this camera frame is located at the center of the stereo baseline, as shown in Figure 6. The coordinates of a point in space, measured in the base frame, can then be expressed in the camera frame as:

$$P^C={T_E^{C}T}_0^6 P^O$$

(21)

Equation (21) describes how a given 3D point in the base frame $P^O$ = [$X^O, Y^O,Z^O$] is mapped to the camera frame $P^{ C}$ = [$X^C, Y^C,Z^C$] using transformation:

$$\mathrm{Robot} \mathrm{manipulator} \mathrm{mapping} \mathcal{H}: P^O={[X^O, Y^O,Z^O]}^T\to P^C={[X^C, Y^C,Z^C]}^T$$

(22)

The mapping function $\mathcal{H}$ is nonlinear and depends on the current joint angle of robot $q=\left[q_i\right|i ϵ 1,2,3,4,5,6]$, robot geometric parameter $P_{a\_robot}$ = [$a_i$, ${\alpha }_i$, $d_i$ |i ϵ 1,2,3,4,5,6], and constant transformation matrix $T_E^C.$

4.5. Eye-in-Hand Kinematic Model

By combining the stereo camera mapping $M$ from Equation (11) and the robot manipulator mapping $\mathcal{H}$ from Equation (22), we define a nonlinear transformation $\mathcal{F}$, which maps any point measured in the base frame {O} to its image coordinates as captured by the stereo camera. This transformation is expressed as:

$$\mathrm{Eye}\text{-}\mathrm{in}\text{-}\mathrm{hand} \mathrm{mapping} \mathcal{F} : P^O={[X^O, Y^O,Z^O]}^T\to p_{image}={[u_l,u_r,v]}^T$$

(23)

The Mapping $\mathcal{F}$ is nonlinear and depends on variables current joint angles: $q$ and parameters $P_a$, which includes stereo camera parameter $P_{a\_camera}$, robot geometric parameter $P_{a\_robot},$ and transformation matrix $T_E^C$. In other words:

$$\mathrm{F}\mathrm{o}\mathrm{r} \mathrm{a}\mathrm{n}\mathrm{y} \mathrm{t}\mathrm{i}\mathrm{m}\mathrm{e} t\ge 0, p_{image}=\mathcal{F}(q\left(t\right), Pa, P^O)$$

(24)

$$Pa=[P_{a_{camera}},P_{a_{robot}},T_E^C]$$

(25)

4.6. Robot-to-Camera Inverse Kinematic Model

Inverse kinematics determines the joint angles required to achieve a given camera pose relative to the inertial frame. The camera pose in the inertial frame can be expressed as a 4 × 4 matrix:

$${Pose}^O=\left[\begin{array}{cc}\begin{array}{cc}{n}_x& s_x\\ n_y& s_y\end{array}& \begin{array}{cc}{a}_x& d_x\\ a_y& d_y\end{array}\\ \begin{array}{cc}{n}_z& s_z\\ 0& 0\end{array}& \begin{array}{cc}{a}_z& d_z\\ 0& 1\end{array}\end{array}\right]$$

(26)

Here, the vectors ${[n_x,n_y,n_z]}^T$, ${[s_x,s_y,s_z]}^T$ and ${[a_x,a_y,a_z]}^T$ represent the camera’s directional vectors for Yaw, Pitch, and Roll, respectively, in the base frame {C}. Additionally, the vector ${[d_x,d_y,d_z]}^T$ denotes the absolute position of the camera center in the base frame {C}.

The camera pose in the camera frame {C} is straightforward as it can be expressed as another 4 × 4 matrix:

$${Pose}^C=\left[\begin{array}{cc}\begin{array}{cc}1& 0\\ 0& 1\end{array}& \begin{array}{cc}0& 0\\ 0& 0\end{array}\\ \begin{array}{cc}0& 0\\ 0& 0\end{array}& \begin{array}{cc}1& 0\\ 0& 1\end{array}\end{array}\right]$$

(27)

The nonlinear inverse kinematics problem involves solving for the joint angles $q$ that satisfy the equation:

$${Pose}^C={T_E^C·T}_0^6\left(q\right)·{Pose}^O$$

(28)

where $T_E^C$ is the transformation from the end-effector frame to the camera frame, and $T_0^6\left(q\right)$ represents the transformation from the base frame to the end-effector frame, which is a function of the joint angles $q$.

The formulas for computing each joint angle are derived from the geometric parameters of the robot. The results of the inverse kinematics calculations for the ABB IRB 4600 elbow manipulator [33], used for simulations in this paper, are summarized in Appendix A.

4.7. Robot Dynamic Model

Dynamic models are included in the inner joint control loop, which will be discussed in Section 6. Without derivation, the dynamic model of a serial of 6-link rigid, non-redundant, fully actuated robot manipulator can be written as [34]:

$$\left(D\left(q\right)+J\right)\ddot{q}+(C\left(q,\dot{q}\right)+{\displaystyle \frac{B}{r}})\dot{q}+g\left(q\right)=u$$

(29)

where $q ϵ {\mathbb{R}}^{6\times 1}$ is the vector of joint positions, and $u ϵ {\mathbb{R}}^{6\times 1}$ is the vector of electrical power input from DC motors inside joints, $D\left(q\right)$ $ϵ {\mathbb{R}}^{6\times 6}$ is the symmetric positive defined matrix, $C\left(q,\dot{q}\right) ϵ {\mathbb{R}}^{6\times 6}$ is the vector of centripetal and Coriolis effects, $g\left(q\right) ϵ {\mathbb{R}}^{6\times 1}$ is the vector of gravitational torques, $Jϵ {\mathbb{R}}^{6\times 6}$ is a diagonal matrix expressing the sum of actuator and gear inertias, $B$ $ϵ {\mathbb{R}}^{6\times 1}$ is the damping factor, $r$ $ϵ {\mathbb{R}}^{6\times 1}$ is the gear ratio.

5. Control Policy Diagram

Figure 7 illustrates the overall control system architecture, designed to guide the robot manipulator so that the camera reaches its desired pose, $\overline{{pose}^O}ϵ {\mathbb{R}}^{4\times 4}$, in the world space. This architecture follows a similar structure to that used in prior work on tool trajectory control for robotic manipulators [35]. To facilitate visual servoing, three fiducial markers are placed within the camera’s field of view, with their coordinates in the inertial frame pre-determined and represented as $P^Oϵ {\mathbb{R}}^{9\times 1}$. Using the robot’s inverse kinematics and the eye-in-hand kinematic model, the expected image coordinates of these fiducial markers, when viewed from the desired camera pose, are computed as $\overline{p_{image}}ϵ {\mathbb{R}}^{9\times 1}$. These computed image coordinates serve as reference targets in the feedback control loop.

The IBVS framework is implemented within a cascaded feedback loop. In the outer control loop, the camera controller processes the visual feedback error, $e_p$, which represents the difference between the image coordinates of the fiducial points at the current and desired camera poses. Based on this error, the outer loop generates reference joint angles, $q_{feedback}ϵ {\mathbb{R}}^{6\times 1}$, to correct the robot’s configuration.

The inner control loop, shown in Figure 8, incorporates the robot’s dynamic model to regulate the joint angles, ensuring they align with the commanded reference angles, $q_{ref}ϵ{\mathbb{R}}^{6\times 1}$. However, due to the limitations of low-fidelity and inexpensive joint encoders, as well as inherent dynamic errors such as joint compliance, high-frequency noises and low-frequency model disturbances are introduced into the system. All sources of errors from the joint control loop are collectively modeled as an input disturbance, $d_{q_T}$ $ϵ{\mathbb{R}}^{6\times 1}$, which affects the outer control loop.

The feedforward control loop operates as an open-loop system, quickly bringing the camera as close as possible to its target pose, despite the presence of input disturbances. The feedforward controller outputs a reference joint angle command, ${ q}_{feedforward} ϵ{\mathbb{R}}^{6\times 1}$, which is sent to the inner loop to facilitate rapid convergence to the desired configuration.

6. Controller Designs

6.1. Inner Joint Angle Control Loop

As illustrated in Figure 8, the primary objective of the inner joint controller is to stabilize the manipulator dynamics, represented by the nonlinear Equation (29). This dynamic model can be simplified to the following form:

$$M\left(q\right)\ddot{q}+h\left(q,\dot{q}\right)=u$$

(30)

where:

$$M\left(q\right)=D\left(q\right)+J$$

(31)

$$h\left(q,\dot{q}\right)=(C\left(q,\dot{q}\right)+{\displaystyle \frac{B}{r}})\dot{q}+\mathrm{g}\left(q\right)$$

(32)

To facilitate control design, the input u is redefined as:

$$u=M\left(q\right)v+h\left(q,\dot{q}\right)$$

(33)

Here $v$ $ϵ{\mathbb{R}}^{6\times 1}$ is a virtual input. Substituting Equation (33) into Equation (30), and noting that $M\left(q\right)ϵ {\mathbb{R}}^{6\times 6}$ is invertible, the system dynamics simplify to:

$$\ddot{q}=v$$

(34)

This transformation technique is known as feedback linearization, resulting in a system of six decoupled double integrators, as described in Equation (34). The entire feedback linearization framework is illustrated in Figure 9. In this control architecture, the actual joint angles $q$ $ϵ{\mathbb{R}}^{6\times 1}$ are regulated to follow the desired joint angles $q_R ϵ{\mathbb{R}}^{6\times 1}$.

A nonlinear interface block computes the actual control input $u$ $ϵ {\mathbb{R}}^{6\times 1} \mathrm{f}\mathrm{r}\mathrm{o}\mathrm{m} \mathrm{t}\mathrm{h}\mathrm{e} \mathrm{v}\mathrm{i}\mathrm{r}\mathrm{t}\mathrm{u}\mathrm{a}\mathrm{l} \mathrm{c}\mathrm{o}\mathrm{n}\mathrm{t}\mathrm{r}\mathrm{o}\mathrm{l} \mathrm{i}\mathrm{n}\mathrm{p}\mathrm{u}\mathrm{t} v ϵ{\mathbb{R}}^{6\times 1}$ using Equation (33). This block also calculates the matrices $M\left(q\right)$ $ϵ {\mathbb{R}}^{6\times 6},$ and $h\left(q,\dot{q}\right)$ ${ ϵ\mathbb{R}}^{6\times 1}$ based on the current joint angles $q$ and their velocities $\dot{q}.$ The linear joint controller is designed using Youla parameterization technique [27] to control the nominally linear system in Equation (34).

Since the six equations in (34) are decoupled and identical in structure, a Single-Input Single-Output (SISO) controller can be first designed and then extended to a Multiple-Input Multiple-Output (MIMO) controller by duplication. The SISO version is defined as:

$$v_{SISO}={\ddot{q}}_{SISO}$$

(35)

Let the SISO controller be denoted $G_{C_{MIMO}}^{Inner}$, then the MIMO controller is:

$$G_{C_{MIMO}}^{Inner}=G_{C_{SISO}}^{Inner}·I_{6\times 6}$$

(36)

where $I_{6\times 6}$ is a $6\times 6$ identity matrix. The transfer function of the SISO plant, derived from Equation (34), is:

$$G_{p_{SISO}}^{Inner}={\displaystyle \frac{1}{s^2}}$$

(37)

This system has two Bounded Input Bounded Output (BIBO) unstable poles at the origin. To ensure the internal stability of the closed-loop system, the transfer function $T_{SISO}$, must satisfy the interpolation conditions [36]:

$$T_{SISO}^{inner} \left(s=0\right)=1$$

(38)

$${\displaystyle \frac{d{T}_{SISO}^{inner}}{ds}}{|}_{s=0}=0$$

(39)

Using the relationship:

$$T_{SISO}^{inner}=Y_{SISO}^{inner}{G}_{p_{SISO}}^{Inner}$$

(40)

the Youla parameter $Y_{SISO}^{inner}$ can be derived from a properly chosen $T_{SISO}^{inner}$ that satisfies Equations (38) and (39). Without delving into the full design details, a candidate closed-loop transfer function that meets these conditions is:

$$T_{SISO}^{inner}={\displaystyle \frac{(3{\tau }_{in}s+1)}{{({\tau }_{in}s+1)}^3}}$$

(41)

where ${\tau }_{in}$ determines the location of poles and zeros, and thus the system′s bandwidth. Tuning ${\tau }_{in}$ allows the controller to achieve a desired tradeoff between speed and overshoot.

The next step is to derive $G_{C_{SISO}}^{Inner}$ from relationships between the closed-loop transfer function, $T_{SISO}^{inner}$, the sensitivity transfer function, $S_{SISO}^{inner},$ and the Youla transfer function, $Y_{SISO}^{inner},$ in Equations (42)–(44):

$$Y_{SISO}^{inner}=T_{SISO}^{inner}{G_{p_{SISO}}^{Inner}}^{-1}={\displaystyle \frac{s^2(3{{\tau }_{in}}^{2}s+1)}{{({\tau }_{in}s+1)}^3}}$$

(42)

$$S_{SISO}^{inner}=1-T_{SISO}^{inner}={\displaystyle \frac{s^2({{\tau }_{in}}^{3}s+3{{\tau }_{in}}^2)}{{({\tau }_{in}s+1)}^3}}$$

(43)

$$G_{C_{SISO}}^{Inner}=Y_{SISO}^{inner}{S_{SISO}^{inner}}^{-1}={\displaystyle \frac{3{{\tau }_{in}}^{2}s+1}{{{\tau }_{in}}^{3}s+3{{\tau }_{in}}^2}}$$

(44)

Substituting back into Equation (36), the resulting MIMO controller is:

$$G_{C_{MIMO}}^{Inner}={\displaystyle \frac{3{{\tau }_{in}}^{2}s+1}{{{\tau }_{in}}^{3}s+3{{\tau }_{in}}^2}} ·I_{6\times 6}$$

(45)

The final expression for the closed-loop MIMO system is:

$$T_{MIMO}^{inner}={\displaystyle \frac{(3{\tau }_{in}s+1)}{{({\tau }_{in}s+1)}^3}} ·I_{6\times 6}$$

(46)

6.2. Feedforward Control Loop

The feedforward control loop operates in open-loop form and is subject to input disturbances, as depicted in Figure 10. The feedforward controller is designed to approximate the inverse dynamics of the inner-loop joint controller, whose closed-loop transfer function is given in Equation (46). Therefore, the feedforward transfer function is constructed as the inverse of the closed-loop system:

$$T_{forward}={\displaystyle \frac{1}{T_{inner-closed}}}{\displaystyle \frac{1}{({{\tau }_{forward}s+1)}^2}}={\displaystyle \frac{{({\tau }_{in}s+1)}^3}{(3{\tau }_{in}s+1)}}{\displaystyle \frac{1}{({{\tau }_{forward}s+1)}^2}}·I_{6\times 6}$$

(47)

Substituting the closed-loop response from Equation (46), we obtain:

$$T_{forward}={\displaystyle \frac{{({\tau }_{in}s+1)}^3}{(3{\tau }_{in}s+1)}}{\displaystyle \frac{1}{({{\tau }_{forward}s+1)}^2}}·I_{6\times 6}$$

(48)

Here, the additional double poles at s = −1/${\tau }_{forward}$ are introduced to ensure that the feedforward transfer function is proper (i.e., physically realizable). To minimize the impact on bandwidth while ensuring realizability, the double poles are placed significantly faster than the original system dynamics. Specifically, the time constant ${\tau }_{forward}$ is chosen to be one-tenth of the inner-loop time constant:

$${\tau }_{forward}=0.1{\tau }_{in}$$

(49)

This design ensures that the feedforward action closely approximates the inverse of the inner-loop dynamics without introducing high-frequency noise amplification.

6.3. Outer Feedback Control Loop

In the control system block diagram (Figure 7), the closed-loop linear transfer function is derived in Equation (46), while the eye-in-hand kinematic model is defined as a nonlinear mapping in Equations (24) and (25).

When the input to this model is a set of perturbed joint angles $\check{q}\left(t\right),$ the output corresponds to the estimated image coordinates $\widehat{p_{image}.}$ Revising Equation (24) accordingly, we express the model as:

$$\widehat{p_{image}}=\mathcal{F}(\check{q}\left(t\right), Pa, P^O)$$

(50)

Here, $P^Oϵ {\mathbb{R}}^{9\times 1},$ denotes the coordinates of fiducial markers in the inertial frame, and $Pa$ includes fixed parameters such as camera intrinsics, robot kinematics, and the transformation matrix from the end-effector to the camera frame.

To linearize this nonlinear model, we select a nominal joint configuration ${\breve{q}}^0ϵ {\mathbb{R}}^{6\times 1}$, and approximate the function $\mathcal{F}$ using a first-order Taylor expansion, resulting in the following linearized form:

$$\widehat{p_{image}}\approx J\left({\check{q}}^0,Pa,P^O\right)\check{q}\left(t\right)+\mathcal{F}({\check{q}}^0,Pa,P^O)$$

(51)

where $J\left({\check{q}}^0,Pa,P^O\right)ϵ{\mathbb{R}}^{6\times 6}$ is the Jacobian matrix of $\mathcal{F}$ evaluated at ${\check{q}}^0.$ Letting $C_1=J\left({\check{q}}^0,Pa,P^O\right)$, $C_2=\mathcal{F}(\check{q}\left(t\right),Pa,P^O)$, Equation (51) can be rewritten as:

$$\widehat{p_{image}}=C_1\check{q}\left(t\right)+C_2$$

(52)

Defining the offset-corrected output as ${\widehat{p_{image}}}^{\prime }=\widehat{p_{image}}-C_2$, we obtain a purely linear relation that simplifies analysis. The complete linearized feedback loop is illustrated in Figure 11.

The resulting linear plant transfer function is given by:

$${G_p}_{MIMO\_outer}^{linear}={\displaystyle \frac{{\widehat{p_{image}}}^{\prime }}{q_{feedback}}}=C_1{\displaystyle \frac{\left(3{\tau }_{in}s+1\right)}{{\left({\tau }_{in}s+1\right)}^3}}·I_{6\times 6}$$

(53)

Since $C_1$ introduces coupling, the first step toward designing an observer for this multivariable system is to derive its Smith–McMillan form [36]. This can be achieved via Singular Value Decomposition (SVD):

$${G_p}_{MIMO\_outer}^{linear}=U_L{M}_p{U}_R$$

(54)

where $U_Lϵ {\mathbb{R}}^{9\times 9}$ and $U_R$ $ϵ {\mathbb{R}}^{6\times 6}$ are the left and right unimodular matrices, and $M_p$ $ϵ {\mathbb{R}}^{9\times 6}$ is the Smith–McMillan form a diagonal matrix of transfer functions. Each nonzero diagonal element takes the form:

$$M_p\left(i,i\right)=gain\left(i\right)·{\displaystyle \frac{\left(3{\tau }_{in}s+1\right)}{{\left({\tau }_{in}s+1\right)}^3}}, iϵ(1,2,3,4,5,6)$$

(55)

where $gain$ $ϵ {\mathbb{R}}^{6\times 1}$ is a numerical vector.

The Youla controller design for each decoupled entry in $M_p$ is straightforward, as the system is stable (i.e., all poles and zeros lie in the left-half plane). The Youla parameterization allows the shaping of the closed-loop dynamics via a new transfer function $M_Y$, which modifies the system’s poles and zeros. A second-order Butterworth filter is selected for the desired closed-loop dynamics:

$$M_T={\displaystyle \frac{{{\omega }_n}^2}{(s^2+2\mathsf{\zeta }{\omega }_{n}s+{{\omega }_n}^2)}}·[\begin{array}{cc}{I}_{6\times 6}& {Zero}_{3\times 3}\\ {Zero}_{3\times 3}& {Zero}_{3\times 3}\end{array}],$$

(56)

Here ${\omega }_n$ is the natural frequency that determines the bandwidth of the outer loop (ensuring ${1/\omega }_n>{\tau }_{in}$ to maintain proper hierarchy with the inner loop), and $\mathsf{\zeta }$ is the damping ratio for tuning response smoothness. The last three coordinates remain uncontrolled in the feedback loop.

The corresponding decoupled Youla controller is:

$$M_Y\left(i,i\right)={\displaystyle \frac{M_T\left(i,i\right)}{M_p\left(i,i\right)}}={\displaystyle \frac{1}{gain\left(i\right)}}{\displaystyle \frac{{{\omega }_n}^2}{\left(s^2+2\mathsf{\zeta }{\omega }_{n}s+{{\omega }_n}^2\right)}} {\displaystyle \frac{{\left({\tau }_{in}s+1\right)}^3}{\left(3{\tau }_{in}s+1\right)}}, iϵ(1,2,3,4,5,6)$$

(57)

Following Equations (43)–(45), the final coupled Youla, closed loop, sensitivity, and observer transfer function matrices are computed as:

$$Y_{MIMO\_outer}^{linear}ϵ {\mathbb{R}}^{6\times 9}=U_R{M}_Y{U}_L$$

(58)

$${T_y}_{MIMO\_outer}^{linear}ϵ {\mathbb{R}}^{9\times 9}={G_p}_{MIMO\_outer}^{linear}·Y_{MIMO\_outer}^{linear}$$

(59)

$${S_y}_{MIMO\_outer}^{linear}ϵ {\mathbb{R}}^{9\times 9}=1-{T_y}_{MIMO\_outer}^{linear}$$

(60)

$${G_C}_{MIMO\_outer}^{linear}ϵ {\mathbb{R}}^{6\times 9}=Y_{MIMO\_outer}^{linear}·({{S_y}_{MIMO\_outer}^{linear})}^{-1}$$

(61)

This controller is based on a linearization about a fixed point ${\check{q}}^0,$ and is therefore valid only within a neighborhood of that configuration. As the actual joint angles $\check{q}\left(t\right)$ deviate from ${\check{q}}^0,$ the mismatch between the linearized model (Equation (51)) and the true nonlinear model (Equation (50)) grows.

To address this, an adaptive control strategy is proposed, illustrated in Figure 12. This method updates the controller in real time using the current estimated joint configuration. First, joint angles $\tilde{q}$ are estimated from the current image coordinates:

$$\tilde{q}={\mathcal{F}}^{-1}(\widehat{p_{image}}, Pa, P^O)$$

(62)

Here, ${\mathcal{F}}^{-1}$ is the inverse of the nonlinear eye-in-hand model, which includes both coordinate transformation from the image to the end-effector frame and the inverse kinematics of the robot.

Using $\tilde{q}$, the current Jacobian and updated SVD-based decomposition are recalculated each time step. The Smith–McMillan form and corresponding controller ${G_C}_{MIMO\_outer}^{linear}$ are then reconstructed via Equations (58)–(61), yielding an adaptive Youla controller that remains effective across varying joint configurations.

7. Simulations Results

To evaluate the performance of our controller design, we simulated two scenarios in MATLAB R2024b Simulink using a Zed 2 stereo camera system [37] and an ABB IRB 4600 elbow robotic manipulator [33]. All simulations are conducted entirely in Simulink, with hardware-in-the-loop components replaced by their corresponding mathematical models. The specifications for the camera system and robot manipulator are summarized in the tables in the appendix. The camera system performs 2D feature estimation of three virtual points in space, with their coordinates in the inertial frame selected as: ${[-0.5m 0 0]}^T$, ${[0 0 0.5m]}^T$, ${[2m, -2m, 0]}^T$.

Many camera noise removal algorithms have been proposed and shown to be effective in practical applications, such as spatial filters [38], wavelet filters [39], and the image averaging technique [40]. Among these denoising methods, there is always a tradeoff between computational efficiency and performance. For this paper, we assume that the images captured by the camera have been preprocessed using one of these methods, and the noise was almost perfectly attenuated. In other words, the only remaining disturbances in the system are due to unmodeled joint dynamics, such as compliance and flexibility, which are modeled as input disturbances in the controlled system.

Two scenarios were simulated:

• Scenario 1: Without input disturbances.
• Scenario 2: With a 1° step input disturbance added to each joint of the robot arms for the entire simulation time.

In both scenarios, the camera system starts from an initial pose in the inertial frame, denoted as ${Pose}_{initila}^O$, and maneuvers target pose, denoted as ${Pose}_{final}^O$.

Table 1. Camera pose and robot joint angles at initial and final state of simulation scenarios.

	Camera Pose		Robot Joint Angles
Format	$\left[\begin{array}{ccc}{n}_x& s_x& \begin{array}{cc}{a}_x& d_x\end{array}\\ n_y& s_y& \begin{array}{cc}{a}_y& d_y\end{array}\\ n_z& s_z& \begin{array}{cc}{a}_z& d_z\end{array}\end{array}\right]$		$\left[\begin{array}{cc}{q}_1& q_2\\ q_3& q_4\\ q_5& q_6\end{array}\right]$
	Where ${[n_x,n_y,n_z]}^T$, ${[s_x,s_y,s_z]}^T$ and ${[a_x,a_y,a_z]}^T$ are Yaw, Pitch, and Roll, and ${[d_x,d_y,d_z]}^T$ (in meters) is the position, measured in inertial frame {O}.		Where [$q_1,q_2,q_3,q_4,q_5,q_6$] in (degrees) are robot joint angles.
	$\mathit{S}\mathit{c}\mathit{e}\mathit{n}\mathit{a}\mathit{r}\mathit{i}\mathit{o} 1$		$\mathit{S}\mathit{c}\mathit{e}\mathit{n}\mathit{a}\mathit{r}\mathit{i}\mathit{o} 2$
	Camera Pose	Robot Joint Angles	Camera Pose	Robot Joint Angles
Initial State	$\left[\begin{array}{ccc}0& 0& \begin{array}{cc}1& 1.27\end{array}\\ 0& 1& \begin{array}{cc}0& 0\end{array}\\ -1& 0& \begin{array}{cc}0& 1.57\end{array}\end{array}\right]$	$\left[\begin{array}{cc}0°& 0°\\ 0°& 0°\\ 0°& 0°\end{array}\right]$	$\left[\begin{array}{cc}\begin{array}{cc}-0.070& -0.998\\ -0.996& 0.070\end{array}& \begin{array}{cc}0.002& 1.064\\ -0.060& 0.964\end{array}\\ \begin{array}{cc}-0.060& -0.007\\ 0& 0\end{array}& \begin{array}{cc}-0.998& 0.939\\ 0& 1\end{array}\end{array}\right]$	$\left[\begin{array}{cc}42.40°& 21.20°\\ 4.58°& -2.86°\\ 66.46°& -42.40°\end{array}\right]$
Final State	$\left[\begin{array}{ccc}-0.11& 0.14& \begin{array}{cc}0.98& 1.31\end{array}\\ -0.09& 0.98& \begin{array}{cc}-0.15& -0.01\end{array}\\ -0.99& -0.10& \begin{array}{cc}-0.09& 1.49\end{array}\end{array}\right]$	$\left[\begin{array}{cc}0.48°& 2.21°\\ 2.05°& -82.68°\\ 9.46°& 77.47°\end{array}\right]$	$\left[\begin{array}{ccc}0& -1& \begin{array}{cc}0& 1\end{array}\\ -1& 0& \begin{array}{cc}-1& 1\end{array}\\ 0& 0& \begin{array}{cc}0& 1\end{array}\end{array}\right]$	$\left[\begin{array}{cc}45°& 18.59°\\ 4.35°& 0°\\ 67.06°& -45°\end{array}\right]$

summarizes the initial and final poses for each scenario, along with the corresponding joint configurations. The results are demonstrated in Figure 13 and Figure 14 for scenario 1, and Figure 15 and Figure 16 for scenario 2.

Figure 13 and Figure 15 present the responses of the six joint angles for the two scenarios, respectively. Figure 14 and Figure 16 show the responses of the nine image coordinates over time for each scenario. In each case, the feedback-only controlled system (left plot) is compared to the feedforward-and-feedback controlled system (right plot). These comparisons focus on overshoot, response time, and target tracking performance.

The bandwidth of actuators in industrial robot manipulators typically ranges from 5 Hz to 100 Hz, corresponding approximately to 31 rad/s to 628 rad/s. For the purposes of this simulation, we assume that the inner loop—representing joint-level control—operates at a fixed bandwidth of 100 rad/s across all scenarios. This corresponds to a time constant of:

$${\tau }_{in}=0.01\mathrm{s}$$

(63)

In cascaded control architectures, it is standard practice to design the inner loop to be at least 10 times faster than the outer loop to ensure proper time-scale separation and prevent interference. Accordingly, the outer-loop bandwidth is selected as one-tenth of the inner-loop bandwidth:

$${\omega }_n=10\mathrm{rad}/\mathrm{s}$$

(64)

Additional parameters for the controller are either computed analytically or selected based on design guidelines. In the feedforward controller, the time constant ${\tau }_{forward}$ is chosen to be one-tenth of the inner-loop time constant, as described in Equation (49):

$${\tau }_{forward}=0.1{\tau }_{in}=0.001 \mathrm{s}$$

(65)

To ensure the outer-loop response is overdamped, and thus suppresses overshoot, the damping ratio ζ is selected to be relatively large:

$$\mathsf{\zeta }=10$$

(66)

The response plots indicate that both the feedback-only controller and the combined feedforward-and-feedback controller successfully stabilize the system and reach a steady state within three seconds, even in the presence of small input disturbances (Scenario Two). However, the feedback-only controller fails to guide the camera to its desired pose and falls into local minima, as evident from the third point’s coordinates (${ul}_3, {ur}_3$, $v_3$), which do not match the target at steady state. This issue arises from the overdetermined nature of the stereo-based visual servoing system, where the number of output constraints (9) exceeds the degrees of freedom (DoFs) available for control (6). As a result, the feedback controller can only match six out of nine image coordinates, leaving the rest coordinates unmatched.

In contrast, the feedforward-and-feedback controller avoids local minima and accurately moves the camera to the target pose. This is because the feedforward component directly controls the robot’s joint angles rather than image features. Since the joint angles (6 DoFs) uniquely correspond to the camera’s pose (6 DoFs), the feedforward controller helps the system reach the global minimum by using the desired joint configurations as inputs.

When comparing performance, the system with the feedforward controller exhibits a shorter transient period (less than 2 s) compared to the feedback-only system (less than 3 s). However, the feedforward controller can introduce overshoot, particularly in the presence of disturbances. This occurs because feedforward control provides an immediate control action based on desired setpoints, resulting in significant initial actuator input that causes overshoot. Additionally, a feedforward-only system is less robust against disturbances and model uncertainties. Fine-tuning the camera’s movement under these conditions requires a feedback controller.

Therefore, the combination of feedforward and feedback control ensures fast and accurate camera positioning. The feedforward controller enables rapid convergence toward the desired pose, while the feedback controller improves robustness and corrects errors due to disturbances or uncertainties. Together, they work cooperatively to achieve optimal performance.

8. Conclusions

This work addresses a fundamental challenge in stereo-based image-based visual servoing (IBVS): the presence of local minima arising from the overdetermined nature of aligning the 6-DOF camera poses with a higher number of image feature constraints. Through a formal reformulation of the stereo camera pose estimation problem as a Perspective-3-Point (P3P) case, we clarify the minimum information needed to uniquely resolve the camera pose and highlight the risk of instability when controlling more image features than available degrees of freedom. To overcome this, a cascaded control framework was introduced, combining a feedforward joint-space controller with an adaptive Youla-parameterized feedback loop. The feedforward loop accelerates convergence by directly commanding joint configurations associated with the desired pose, while the feedback loop ensures robustness and fine-tuned tracking under disturbances or modeling uncertainties.

When compared to existing solutions, the proposed method offers a number of compelling advantages. Unlike hybrid switched controllers, which require explicit mode-switching logic and may suffer from discontinuities, our approach provides a unified framework that avoids local minima without controller mode changes. In contrast to 2½ D visual servoing, which blends pose and image features into a hybrid Jacobian, our design maintains the pure IBVS structure while preserving real-time feasibility. Moreover, compared to model predictive control (MPC) approaches that require high-frequency optimization, our strategy achieves comparable performance using linear control theory and avoids excessive computational overhead.

The

Table 2. Comparative analysis of four visual servoing strategies addressing local minima and control robustness.

Aspect	Hybrid Switched-System (Gans and Hutchinson, 2007) [21]	2½ D Visual Servoing (Chaumette and Malis, 2000) [22]	MPC-Based IBVS (Roque et al., 2020) [23]	Proposed Feedforward-Feedback IBVS
Main Idea	Switch between IBVS and PBVS	Combine 2D image + 3D pose feature	Predict joint input from image error via MPC	Combine feedforward joint control with adaptive feedback
Local Minima Handling	Switching avoids image traps	Blended Jacobian avoids singularities	Predictive avoidance via optimization	Feedforward brings pose close to global minimum
Stability Guarantee	Local Lyapunov stability	Local/global under ideal pose estimation	Guaranteed via MPC feasibility	Inner/outer loop stability via Youla design
Computational Load	Moderate (switch logic)	Moderate (hybrid Jacobian computation)	High (real-time optimization)	Moderate (adaptive SVD and closed-form update)
Overshoot Risk	Low, but may switch abruptly	Minimal, smooth trajectories	Well-regulated by MPC	May occur in feedforward if not well-tuned
Real-Time Suitability	Yes	Yes	Yes, with efficient solver	Yes

below summarizes the key differences between the four approaches:

Despite these advantages, several practical considerations must be acknowledged. The open-loop nature of the feedforward controller can introduce overshoot, particularly in systems with low damping or compliance in joints. While this is mitigated by tuning time constants and damping ratios, it underscores the importance of dynamic modeling accuracy. Additionally, the feedback controller’s reliance on linearized approximations restricts global validity, though the proposed adaptive update strategy helps maintain performance over a broad state space. Implementation in real hardware would also require precise stereo calibration and real-time computation of joint-space inverse kinematics, both of which may present engineering challenges.

Overall, this work demonstrates that combining classical control design with geometric insights and modern parameterization techniques can yield a robust, efficient, and scalable solution to a longstanding problem in visual servoing. Future research may explore multi-rate updates, gain-scheduled switching between linearized controllers, or simultaneous $H_{\infty }$ feedforward–feedback design [41] to further enhance robustness and applicability in unstructured environments.

In summary, this paper investigates the overdetermination problem in stereo-based IBVS tasks. While future improvements to the algorithm are possible, the proposed control policy has demonstrated significant potential as an accurate and fast solution for real-world eye-in-hand (EIH) visual servoing tasks.