Occlusion-Free Image-Based Visual Servoing using Probabilistic Control Barrier Certificates

Consider a moving camera which is fixed on the robot end-effector with the eye-in-hand configuration and the camera is viewing objects in the workspace. $\mathcal{F}_{c}$ is a right-handed orthogonal coordinate frame whose origin is at the principle point of the camera and $Z$ axis is collinear with the optical axis of the camera. Assuming $\mathbf{q}_{i}=[x_{i},y_{i}]^{\rm T}\in\mathbb{R}^{2}$ is the pixel coordinate of a static point in the image plane, then we can define its normalized image plane coordinate as $\bar{\mathbf{q}}_{i}\in\mathbb{R}^{2}$ according to the camera projection model (Corke and Khatib, 2011).

where $f,p_{x},p_{y}\in\mathbb{R}$ are camera intrinsic parameters and $[X_{i},Y_{i},Z_{i}]^{\rm T}$ is the 3D coordinate of the point in $\mathcal{F}_{c}$.

The image vision feature can be constructed from the normalized image plane coordinates such that $\mathbf{p}_{i}=\bar{\mathbf{q}}_{i}$. Assume there are $m$ feature points extracted from the image and the state vectors of current and target vision feature points are denoted as $\mathbf{s}(t)=\left[\mathbf{p}_{1}(t);\cdots;\mathbf{p}_{m}(t)\right]\in\mathbb{R}^{2m}$ at time $t$ and $\mathbf{s}^{*}=\left[\mathbf{p}_{1}^{*};\cdots;\mathbf{p}_{m}^{*}\right]\in\mathbb{R}^{2m}$ respectively, then the IBVS task aims to regulate the positional image feature points error vector $\mathbf{e}(t)=\mathbf{s}(t)-\mathbf{s}^{*}$ to zero through robot movements, which thus drives the robot to the desired position.

According to (Chaumette and Hutchinson, 2006), the dynamics of the $m$ feature points can be expressed as:

where $\mathbf{L}_{s}=\left[\mathbf{L}_{s_{1}}\cdots\mathbf{L}_{s_{m}}\right]^{\rm T}\in\mathbb{R}^{2m\times d}$ is the image interaction matrix and can be computed refer to (Chaumette and Hutchinson, 2006), $\mathbf{V}_{c}\in\mathbb{R}^{d}$ is a vector of the robot motion controller to express the camera translation and rotation velocity in the workspace and $Z=[Z_{1}\cdots Z_{m}]^{\rm T}\in\mathbb{R}^{m}$ denotes the depths of $m$ feature points. In this paper, we assume the depth information $Z$ of the feature points has been acquired. Therefore in IBVS, (2) represents the system dynamics of the $m$ feature points with $\mathbf{s}$ as the system state and $\mathbf{V}_{c}$ as the system controller.

Since $\mathbf{s}^{*}$ is predefined and time independent, we have:

According to (Chaumette and Hutchinson, 2006), the unconstrained gradient-based IBVS controller driving $\mathbf{e}\to 0$ can be defined as:

where $\alpha\in\mathbb{R}$ is a constant as predefined control gain, and ${{\mathbf{L}}_{s}}^{+}\in\mathbb{R}^{d\times 2m}$ is the pseudo-inverse of $\mathbf{L}_{s}$, which is given by ${{\mathbf{L}}_{s}}^{+}={(\mathbf{L}^{\rm T}_{s}\mathbf{L}_{s})}^{-1}{\mathbf{L}_{s}}^{\rm T}$. To ensure the local asymptotic stability, we should have $\mathbf{L}^{\rm T}_{s}\mathbf{L}_{s}>0$, i.e. at least three feature points should be available in the FoV.

To optimize the IBVS controller over a finite time horizon, the MPC policy is used to construct a $N$ step time horizon planner rendering a sequence of candidate control actions. Consider the problem of IBVS that seeks to regulate the current feature points to the target feature points through robot movements. According to the error dynamics (3), the discrete-time control system can be described by:

where $\mathbf{I}\in\mathbb{R}^{2m\times 2m}$ is an identity matrix and $\mathbf{e}(t)\in\mathbb{R}^{2m}$ represents the state of the image feature error of $m$ feature points at time step $t$. The system state is $\mathbf{s}(t)$ with $\mathbf{V}_{c}(t)\in\mathcal{U}\subset\mathbb{R}^{d}$ as the control input and $g$ is locally Lipschitz.

Therefore, the finite-time optimal control problem can be solved at time step $t$ using the following policy $\pi$:

where $N$ is the prediction time horizon, and $\mathbf{Q}\in\mathbb{R}^{2m\times 2m}$, $\mathbf{R}\in\mathbb{R}^{d\times d}$ and $\mathbf{F}\in\mathbb{R}^{2m\times 2m}$ are weighting matrices which represent a trade-off between the small magnitude of the control input (with larger value of $\mathbf{R}$) and fast response (with larger value of $\mathbf{Q}$ and $\mathbf{F}$). $\mathbf{e}(t+k|t)$ denotes the error vector at time step $t+k$ predicted at time step $t$, which is obtained from the current image feature error $\mathbf{e}(t)$ and the control input of $\mathbf{V}_{c}({t:t+N-1})$. Finally, the optimized control sequence can be obtained as $\mathbf{V}_{c}^{t:t+N-1}$.

Without loss of generality, Consider an obstacle $O$ which is moving in the workspace and may occlude the feature points in the camera view. We model the obstacle as a rigid sphere with the radius $R$ in the workspace. Similar to the feature points, the normalized image plane coordinates of the obstacle center and radius of the obstacle can be defined as $\mathbf{s}_{\mathrm{o}}(t)=[\frac{X_{\mathrm{o}}}{Z_{\mathrm{o}}(t)},\frac{Y_{\mathrm{o}}}{Z_{\mathrm{o}}(t)}]^{\rm T}\in\mathbb{R}^{2}$ and $R_{n}(t)=\frac{R}{Z_{\mathrm{o}}(t)}$ respectively. To simplify the notation, we will use $\mathbf{s}_{o}$ and $R_{n}$ to denote the real-time state of the obstacle center and obstacle radius in the normalized image plane.

In this way, for any pair-wise occlusion avoidance between the feature point $i$ and the obstacle $O$ in the normalized image plane, the occlusion-free condition and state set can be defined as follows:

Then for all feature points, the desired set for occlusion-free states is thus defined as:

Control Barrier Functions (CBF) (Ames et al., 2019) have been widely applied to generate control constraints that render a set forward invariant, i.e. if the system state starts inside a set, it will never leave this set under the satisfying controller. The main idea of CBF is summarized as the following Lemma.

Based on the occlusion avoidance condition  (10)-(11) and the Lemma 1, if the feature points are not occluded by the obstacle initially, then the admissible control space for the robot to keep the feature points free from occlusion can be represented by the following control constraints over $\mathbf{V}_{c}$:

where $\mathcal{B}(\mathbf{s},\mathbf{s}_{\mathrm{o}})$ defines the Control Barrier Certificates (CBC) for the feature point-obstacle occlusion avoidance, and $\gamma$ is a user-defined parameter in the particular choice of $\kappa(h(\mathbf{x}))=\gamma(h(\mathbf{x}))$ as in (Luo et al., 2020b). It will render the occlusion-free set $\mathcal{H}^{c}$ forward invariant, i.e. as long as we can guarantee the control input $\mathbf{V}_{c}$ lies in the set $\mathcal{B}(\mathbf{s},\mathbf{s}_{\mathrm{o}})$, the feature points will not be occluded by the obstacle at all times.

Although (14) indicates an explicit condition for occlusion avoidance with the perfect knowledge of the actual position $\mathbf{s}$, the presence of measurement uncertainty on $\mathbf{s}$ makes it challenging to enforce (14), or even impossible when the measurement noise is unbounded, e.g. Gaussian noise. In this paper, we consider the realistic situation where the pixel coordinates of the extracted feature points acquired by the camera have Gaussian distribution noise that are formulated as follows:

where ${\mathbf{w}_{i},\mathbf{w}_{\mathrm{o}}}\in\mathbb{R}^{2}$ are the Gaussian measurement noises and can be considered as independent random variables with zero mean and $\Sigma_{i},\Sigma_{\mathrm{o}}$ as the variances. With that, for the rest of the paper we assume only the noisy positions $\hat{\mathbf{s}}_{i}$ and $\hat{\mathbf{s}}_{\mathrm{o}}$ of extracted feature points are available when the uncertainty is considered.

Then the occlusion avoidance condition in (10)-(11) can be considered in a chance-constrained setting. Formally, given the user-defined satisfying probability threshold of the occlusion avoidance as $\sigma\in(0,1)$, we have:

where Pr(.) indicates the probability of an event. Note that when the $\sigma$ is approaching 1, it will lead to a more conservative controller to maintain the probabilistic occlusion-free set. In Section 3.2, we will discuss how to transfer the chance constraints on the feature points states into a deterministic admissible control space $\mathcal{S}^{\sigma}(\mathbf{\hat{s}},\mathbf{\hat{s}}_{\mathrm{o}})\subset\mathcal{U}$ w.r.t. $\mathbf{V}_{c}$, so that the occlusion-free performance could be guaranteed with satisfying probability as implied in (16).

To achieve the occlusion-free IBVS, we first use the MPC as a planner to generate the unconstrained control sequence $\mathbf{V}_{c}^{t:t+N-1}$ at each time step $t$. To address the occlusion problem brought by the obstacle, we discuss two situations to define the optimized problem.

Without Noise: First, we assume that the camera can get the accurate pixel coordinates from the image, i.e. we can extract the feature points precisely. In this way, we assume the feature points are not occluded by the obstacle at the initial location. Then we can formally define this occlusion-free constraint with the following step-wise Quadratic-Program (QP) at each time step $t$:

where $\mathbf{V}_{c}^{mpc}\in\mathbb{R}^{d}$ is the first step of the control sequence $\mathbf{V}_{c}^{t:t+N-1}$ generated from MPC at time $t$, and ${V}_{\mathrm{max}}$ is the maximum velocity. Hence the resulting $\mathbf{V}_{c}^{*}\in\mathbb{R}^{d}$ is the step-wise optimized controller to be executed at each $t$ that renders the occlusion-free set $\mathcal{H}^{c}$ in (12) forward invariant.

With Noise: In presence of camera measurement noise, then only the inaccurate vision feature information of the normalized image plane $\hat{\mathbf{s}}_{i}$ and $\hat{\mathbf{s}}_{\mathrm{o}}$ can be obtained for IBVS task. In this case, the chance-constrained occlusion-free problem can be formally defined as:

First, we consider the condition where the camera can acquire the accurate pixel coordinate from the world space when there is no measurement noise. Given the occlusion free condition in (10)- (11) and the form of CBC in (14), we now formally define the CBC for the occlusion avoidance condition as follows:

We demonstrate that our proposed control barrier function $h^{c}_{i,\mathrm{o}}(\mathbf{s}_{i},\mathbf{s}_{\mathrm{o}})$ in (10) is a valid control barrier function. As summarized in (Capelli and Sabattini, 2020), from Lemma 1 the condition for a function $h(\mathbf{x})$ to be a valid CBF should satisfy the following three conditions: (a) $h(\mathbf{x})$ is continuously differentiable, (b) the first-order time derivative of $h(\mathbf{x})$ depends explicitly on the control input $\mathbf{u}$ (i.e. $h(\mathbf{x})$ is of relative degree one), and (c) it is possible to find an extended class-$\mathcal{K}$ function $\kappa(\cdot)$ such that $\sup_{\mathbf{u}\in\mathcal{U}}\{\dot{h}(\mathbf{x},\mathbf{u})+\kappa(h(\mathbf{x}))\}\geq 0$ for all $\mathbf{x}$.

Hence, consider our proposed candidate CBF $h^{c}_{i,\mathrm{o}}(\mathbf{s}_{i},\mathbf{s}_{\mathrm{o}})$ in (10), and according to the differential function in (25), it is straightforward that the first order derivative of (10) in the form of (25) depends explicitly on the control input $\mathbf{V}_{c}$. Thus, the function ${h}^{c}_{i,\mathrm{o}}(\mathbf{s}_{i},\mathbf{s}_{\mathrm{o}})$ in (10) is (a) continuously differential and (b) is of relative degree one.

For condition (c) $\sup_{\mathbf{V}_{c}\in\mathcal{U}}[\dot{h}^{c}_{i,\mathrm{o}}(\mathbf{s},\mathbf{s}_{\mathrm{o}},\mathbf{V}_{c})+\gamma(h^{c}_{i,\mathrm{o}}(\mathbf{s},\mathbf{s}_{\mathrm{o}}))]\geq 0$, we need to prove that the following inequality has at least one solution.

Given the form of $\dot{h}^{c}_{i,\mathrm{o}}(\mathbf{s},\mathbf{s}_{\mathrm{o}},\mathbf{V}_{c})$ in (25), then (26) can be re-written as:

where $\mathbf{M}=2((\mathbf{s}-[\mathbf{s}_{\mathrm{o}};\mathbf{s}_{\mathrm{o}};\mathbf{s}_{\mathrm{o}};\mathbf{s}_{\mathrm{o}}])^{\rm T}(\mathbf{L}_{s}-[\mathbf{L}_{\mathrm{o}};\mathbf{L}_{\mathrm{o}};\mathbf{L}_{\mathrm{o}};\mathbf{L}_{\mathrm{o}}])-R_{n}\mathbf{L}_{\mathrm{or}})\in\mathbb{R}^{1\times 6}$, and $n=\gamma(R_{n}^{2}-{\left\|\mathbf{s}-[\mathbf{s}_{\mathrm{o}};\mathbf{s}_{\mathrm{o}};\mathbf{s}_{\mathrm{o}};\mathbf{s}_{\mathrm{o}}]\right\|}^{2})\in\mathbb{R}$. Given a specific state of $\mathbf{s}$ and $\mathbf{s}_{o}$, $n$ will be a constant. It is also straightforward that the six terms of $\mathbf{M}$ can not be zero at the same time. Therefore, if we consider an unbounded control input $\mathbf{V}_{c}\in\mathcal{U}\in\mathbb{R}^{6}$, we can always find a solution such that $\mathbf{M}\mathbf{V}_{c}\geq n$.

In the case that $\mathbf{V}_{c}\in\mathcal{U}\in\mathbb{R}^{6}$ is bounded, several existing approaches could be employed to enforce the feasibility of the condition in (27). For example, in (Lyu et al., 2021), we provided an optimization solution for parameter $\gamma$ over time to guarantee that the admissible control set is always not empty when a feasible solution does exist. Besides, the authors in (Xiao et al., 2022) provided a novel method to find sufficient conditions, which are captured by a single constraint and enforced by an additional CBF, to guarantee the feasibility of the original CBF control constraint. Readers are referred to (Xiao et al., 2022) for further details. With that, we conclude the proof. $\square$

In presence of uncertainty, similar to (Luo et al., 2020a), we have the sufficient condition of (16) as

Following (Luo et al., 2020a), we define our PrCBC for the chance-constrained occlusion free condition as follows:

With that, we have $\mathbf{A}^{\sigma}_{i,\mathrm{o}}=\frac{\Delta\mathbf{L}^{\rm T}\Delta\mathbf{L}}{\gamma^{2}}$, $\mathbf{b}_{i,\mathrm{o}}=\frac{-2\Delta\mathbf{s}^{\rm T}(\Delta\mathbf{L}-4R_{n}\mathbf{L}_{r})}{\gamma}$ and $c_{i,\mathrm{o}}=2R_{n}^{2}+4e^{2}-||\Delta\mathbf{s}||^{2}_{2}$. Due to the size limitation of the physical workspace in the visual servoing scenario, the quadratic control constraint may not be feasible, e.g. when the moving obstacle is very large, it is impossible to avoid occlusion. In this extreme case, one alternative solution is to hold the robot static and to wait until the obstacle no longer occlude the feature points.

To integrate the advantages of the Model Predictive Control policy for high-level planning and CBC/PrCBC to enforce the occlusion-free condition, we combine the procedure of the MPC and proposed CBC/PrCBC to generate an optimized control sequence. Specifically, after executing the CBC/PrCBC to minimally modify the control $\mathbf{V}_{c}^{mpc}(t)$ to $\mathbf{V}_{c}^{*}(t)$ at time step $t$, the process is repeatedly implemented at the time step $t+1$: using policy $\pi$ in Section 2.2 to generate the control sequence $\mathbf{V}_{c}^{t+1:t+N|t+1}$, obtain the optimized control $\mathbf{V}_{c}^{mpc}(t+1)$, and calculate the occlusion-free control $\mathbf{V}_{c}^{*}(t+1)$ to execute next.

To validate the performance of our experiment, we implement our algorithm under different experiment setups:

CBC without noise: According to the experiment setup shown in Fig. 2(a), the obstacle is moving in the workspace with initial location $[0.43,0.23,0.10]^{\rm T}$. In this experiment, we assume the camera can acquire the accurate coordinates of the feature points and obstacle center. With designed controller CBC, the results are shown in Fig. 2(b) and Fig. 2(c). From Fig. 2(b) and Fig. 2(c), the feature points can avoid occlusion of the obstacle and converge to the pre-defined target locations successfully.

CBC with noise: With the same initial condition of Fig. 2(a), we assume the pixel coordinates of the feature points and obstacle center are acquired by the camera with Gaussian distributed noise $\mathbf{w}_{i},\mathbf{w}_{\mathrm{o}}\sim N(0,10)$. From Fig. 2(d), it can be observed that the feature point $\#3$ has been occluded by the obstacle in the camera view.

PrCBC with noise: With the same initial condition and measurement noise in Fig. 2(a), the confidence level is set to be $\sigma=0.8$. As shown in Fig. 2(e), the feature points can avoid the occlusion of the obstacle in the camera view. As shown in Fig, 2(f), the robot could navigate the camera to the desired location where the four feature points converge to the pre-defined target locations in the camera view.

Hence, the CBC could enforce an occlusion-free IBVS only when accurate feature points information is available. In comparison, the PrCBC is able to guarantee chance-constrained probabilistic occlusion avoidance even in presence of the measurement noise.

Next, we present quantitative results from the simulation experiments. As shown in Fig. 3(a), without noise in the pixel coordinates, the CBC method performs well, always ensuring that the minimum feature points-obstacle distance exceeds the occlusion-free safety distance. However, when noise is added, the CBC method is unable to guarantee this, as shown in Fig. 3(b) (here we continue running the simulation until the obstacle moving out of the camera view). The PrCBC method, on the other hand, consistently maintains the minimum feature points-obstacle distance above the safety distance, even in the presence of observation noise. Fig. 3(d) shows the convergence of the feature points locations in the camera view. It validates that our method can accomplish the IBVS task with occlusion-free performance.

We also performed 50 random trials (5 different initial locations with each one having 10 random trails) under the confidence level of $\sigma=0.9$ to validate the effectiveness of the PrCBC controller in presence of random camera measurement noise. We define $Dis=min(Dis_{i}(t))$ to express the distance between the feature point and the obstacle edge in the image plane with $Dis_{i}(t)$ denoted as:

where $r(t)=\frac{fR}{Z_{\mathrm{o}}(t)}\in\mathbb{R}$ is the obstacle radius in the image plane (pixel scale). Fig. 4 shows the mean $Dis$ with its corresponding variance, which verified that the feature points would not collide with the obstacle using the proposed PrCBC, indicating the occlusion-free performance.