Energy Aware Camera Location Search Algorithm for Increasing Precision of Observation in Automated Manufacturing

1. Introduction

In automated industry, automatic alignment plays a critical role in many different applications, such as micromanipulation, autonomous welding, and industrial assembly [1,2,3]. Visual servoing is the technique commonly used in the tasks of alignment as it can guide robotic manipulators to their desired poses (positions and orientations) [4,5,6]. Visual servoing is the method of controlling a robot’s motion using real-time feedback from visual sensors that continuously extract image features [7].

According to the relative positions between cameras and the tool manipulators, visual servoing can be generally divided into two categories: eye-in-hand (EIH) configuration and eye-to-hand (ETH) configuration [8,9]. In EIH, a camera is mounted directly on a robot manipulator, in which case the motion of the robot induces motion of the camera, while in ETH, the camera is fixed in the workspace and observes the motion of the robot from a stationary configuration. Both configurations have their own merits and drawbacks regarding field of view limit and precision that oppose each other. EIH has a partial but precise view of the scene whereas ETH has a global but less precise view of it [10]. For complex tasks in automated manufacturing, one configuration of visual servoing is not adequate as those tasks require the camera to provide global views and to be maneuverable enough to explore the scene [10]. To take advantage of both stationary and robot-mounted sensors, two configurations can be employed in a cooperative scheme, which can be seen in some of the work [11,12,13].

However, we haven’t seen enough discussion on the importance of cameras’ location in the ETH or ETH/EIH cooperative configurations from any work published. To observe a single point in the workspace, there are an infinite number of poses that a fixed camera can be placed in the space. Among all possible poses of the camera that keep the same target within the field of view limit, the image noise at different locations may play a great role in the precision of observations. Many environmental conditions (such as illumination, temperature, etc.) affect the noise level of a single image [14]. In a manufacturing environment, the quality of estimations from a camera may vary significantly from one observation location to another. For instance, the precision of an estimation improves greatly if the camera moves from a location where it is placed within the shade of machines to a location that has better illumination. Also, places near machines may be surrounded by strong electrical signals that also introduce extra noise into the camera sensors.

It is intuitive to consider applying filters to reduce image noise, and therefore, to increase the precision of observations. The Gaussian white noise can be dealt with using spatial filters, e.g., the Gaussian filter, the Mean filter, and the Wiener filter [15]. In addition, wavelet methods [16,17] have the benefit of keeping more useful details but at the expense of computational complexity. However, filters that operate in the wavelet domain still inevitably filter out (or blur) some important high-frequency useful information of the original image, even though they succeed in preserving more edges, compared with spatial filters. Especially at locations where the signal-to-noise ratio (SNR) in an image is low, no matter what filters are applied, it is difficult to safeguard the edges of the noise-free image as well as reduce the noise to a desirable level. Thus, developing an algorithm that searches for locations of the camera where SNR is high is beneficial.

We can approach the denoising problem with multiple noisy images taken from the same perspective. This method is called signal averaging (or image averaging in the application of image processing) [18]. Assume images taken from the same perspective but at different times have the same noise levels. Then averaging multiple images at the same perspective will reduce the unwanted noise as well as retain all original image details. We can also assume the robot’s end effector holds the camera rigidly so that any shaking and drift are negligible when shooting pictures. Moreover, the number of images required for averaging is proportional to the ratio of an image’s original noise level to the reduced level that is desirable for observations. Therefore, the number of images required is a measurement of noise level in a single image. Furthermore, in the denoising process, the precise estimations require that the original image details are retained as much as possible. Based on previous statements, we decided to choose image averaging over all other denoising techniques in this work.

In this paper, we propose an algorithm that searches for the camera’s workspace to find an optimal location (if its orientation is fixed) of the camera so that a single image taken at this location has the smallest noise level among images taken at all locations in the space. With limited energy for moving the camera, this algorithm also ensures the camera ends up at a suboptimal (if the optimal pose is unreachable) location among the locations already searched.

2. Methods

2.1. Camera Location Search Process and Algorithm Flowchart

The proposed algorithm is aimed to search and move the camera to the location where the least number of pictures are needed to reduce the noise level of the averaged image to a degree ${\sigma }_{noise\_reduced}$ . A flowchart describing the algorithm is shown in Figure 1.

A camera, mounted on a 6-joint robot manipulator, moves freely in 6 degrees of freedom (6 DoFs) in space. Assuming the camera is fixed in orientation towards the face of the object of interest, only the position (3 DoFs) of the camera can vary from the movement of the robot arm. The camera can only move to the locations within the maximum reach of the robot manipulator and the object can only be detectable within the view angle of the camera. Those constraints create a camera operational space (Figure 2a), which is then gridded with a certain resolution of distance. All the intersections of grids come up with a set of nodes ($S_{total}$ ), which are all possible candidates of locations that the camera may move and search utilizing the algorithm. Each node is indexed and denoted as ${Node}_{index}.$

represents Euclidean distance (green line).

The algorithm iteratively commands the camera to take a picture at one location, generate the next target location, and move to that location until it finds the optimal location that requires the least number of pictures. In one iteration, the camera takes a photo at the current location $P^C$ and after image processing, it produces a noisy image with an intensity matrix $I_a.$ A previously developed algorithm [19] estimates the noise level ${\sigma }_a$ across the image. Then, at the current location of the camera, the number of pictures $N^C$ needed to reduce the noise level to ${\sigma }_{noise\_reduced}$ is calculated by the equation: $N^C={({\displaystyle \frac{{\sigma }_a}{{\sigma }_{noise\_reduced}}})}^2$ . Based on this information ($N^C$ at $P^C$ ), the algorithm then generates the next target location of the camera, $P^{Next}$ . If the next generated target position $P^{Next}$ is the same as the current position $P^C$ , $P^C$ is the optimal location and the camera stays and observes the object at this location. Otherwise, the robot joint controller moves the robot joint angles to the targets $\overline{q}$ (calculated from forward kinematics [20] (Appendix Equation (B1)–(B5))) so that the camera will move to the next target position, $P^{Next}$ .

2.2. Core Alogorithm

The core of this algorithm is to generate the target locations to be explored in the camera’s operational space. The details of this algorithm are shown in the area circled with dashed lines in Figure 1. In this section, we will put efforts into the development of the picture number estimation function, the explorable nodes set, the stochastic modified evaluation function, the next location selection process, and the estimated energy cost function.

2.2.1. Picture Number Estimation Function

In each iteration of the algorithm, the camera moves to a new node with location $P^C$ , and the algorithm evaluates the number of pictures $N^C$ needed at this location. The node $\left({Node}_{index}\right)$ is then set to be explored and saved in a set $S_{explored}.$ Each node that has been explored in iterations is one-on-one paired with the values: location, and picture numbers of that node. All pairs that are expressed as ${Node}_{index}^{explored}$ : ($P_{index}^{explored},N_{index}^{explored}$ ) are saved in a hash map $M_{explored}$ . Then $M_{explored}$ is used to develop an estimation function that can estimate the number of pictures needed for the rest unexplored nodes. The estimated picture number calculated for all nodes in space is denoted as $S_{\tilde{N}}$ . Expressions of any element in $S_{\tilde{N}}$ are:

For any node index $x$ in the Total Node Set: $S_{total}=\left\{{Node}_x\right|x\in \left(\mathrm{1,2},\dots m\right)\}$ , where $m$ is the total number of nodes.

$${\tilde{N}}_x=\left\{\begin{array}{c}{\sum }_{i\in S_{index-exp}}{\displaystyle \frac{(\overline{ED}\left(P_i^{exp},P_x\right)^(-K_{est}))}{{\sum }_{j\in S_{index-exp}}((\overline{ED}\left(P_j^{exp},P_x\right)^(-K_{est}))}}{N}_i^{explored}\\ N_x^{explored}, if {Node}_x \in S_{explored} \end{array}\right., if {Node}_x\notin S_{explored}$$

(1)

$$\mathrm{Then}\text{:} S_{\tilde{N}}=\left\{{\tilde{N}}_x\right|x\in \left(\mathrm{1,2},\dots m\right)\}$$

(2)

where $K_{est}$ is a positive constant parameter

$$S_{index-exp}=\left\{index\right|{Node}_{index}\in S_{explored}\}$$

(3)

$$\begin{gathered} \overline{ED}\left(P_a,P_b\right)=\sqrt{{(P_{ax}-P_{bx})}^2+{(P_{ay}-P_{by})}^2+{(P_{az}-P_{bz})}^2} \\ \mathrm{with} P_a,P_b \mathrm{are} 3\mathrm{D} \mathrm{locations} \mathrm{of} {Node}_a and {Node}_b: \\ {P}_a=\left(P_{ax},P_{ay},P_{az}\right) and P_b=(P_{bx},P_{by},P_{bz}) \end{gathered}$$

(4)

Equation (1) is a weighted function concerning each explored node. If one explored node ${Node}_i^{explored}$ is closer to the node ${Node}_x^{explored}$ , its picture number $N_i^{explored}$ has a larger weight (effect) on the estimation of ${\tilde{N}}_x$ . Thus, a minus sign of $K_{est}$ is utilized to indicate the negative correlation between the Euclidean distance and weight.

Equation (1) shows that at least some initial nodes are required to be explored before their values can be used to estimate the picture numbers in other nodes. For the estimation function to work properly by covering all nodes in the camera operational space, four initial nodes are selected in the following steps:

• Find the first two nodes on the boundary of operational space so that the Euclidean distance between these two nodes is the largest.
• Select other two nodes whose connection line is perpendicular to the previous connection line and the Euclidean distance between these two nodes is also the largest among all possible node pairs.
• Move the camera to those four nodes in space with proper orientation. Take a single image at each location and estimate the number of pictures required at those locations. And save all four nodes in $S_{explored}$ and their values in $M_{explored}.$

Figure 2b shows the selection of four initial nodes in a camera operational space and shows an example of the use of the estimation function to estimate the picture number in one node.

2.2.2. Explorable Node Selection

A stage in the algorithm, Explorable Node Selection, generates a set of nodes ${ S}_{explorable},$ which contains all the camera’s next explorable locations from the current location $P^C$ with $\mathrm{a}\mathrm{v}\mathrm{a}\mathrm{i}\mathrm{l}\mathrm{a}\mathrm{b}\mathrm{l}\mathrm{e}$ current energy for moving $E_{bound}.$ The explorable set is found in the following steps (Figure 3):

• With current energy bound $E_{bound}$ , find all nodes in the space that can be reached from the current node $P^c.$ In other words, all feasible nodes are in the set:

  $$S_{feasible}=\left\{{Node}_{index}\right|E(P^c,P_{index})\le E_{bound}\}$$

  (5)

  where $P$ is the position of a node and $E(P_a,P_b)$ is the estimated energy cost from $P_a to P_b$ .

When the current energy bound $E_{bound}$ is greater than a predefined energy threshold $E_T$ ($E_{bound}>E_T$ ), then the algorithm is safe to explore all nodes in the feasible range $S_{feasible}$ in the next iteration loop. In other words:

$$S_{explorable}=S_{feasible} , if{ E}_{bound}>E_T$$

(6)

Note: $E(P_a,P_b)$ is generated from a dynamic model of a robot arm control system. The details of this function are developed in the following section of this paper.

2.

When $E_{bound}\le E_T$ , step 2 and step 3 are applied in finding the explorable set. In the feasible set, look at all explored nodes and find the one that has the smallest number of pictures. In other words:

$$S_{feas\&exp}=\left\{{Node}_{index}\right|{Node}_{index}\in S_{feasible}\cap S_{explored}\}$$

(7)

$$\mathrm{e}\mathrm{x}\mathrm{p}\_min=\left\{index\right|N_{\mathrm{e}\mathrm{x}\mathrm{p}\_min}=\min \left(N_{index}|{Node}_{index}{\in S}_{feas\&exp}\right)\}$$

(8)

where $S_{explored}$ is the set of all explored nodes.

$S_{feas\&exp}$ is the intersection set between $S_{feasible}$ and $S_{explored}$ .

3.

The estimation function may not give accurate results for some unexplored nodes. Therefore, it is possible that the algorithm may make the camera end up in a node that has a large number of pictures in some iteration loops. Because of that, our algorithm needs to make sure at worst it has enough energy to go back to the best (minimum number of pictures) node that has been explored when the available energy is low ($E_{bound}\le E_T$ ). This further reduces the feasible set $S_{feasible}.$ The explorable set $S_{explorable}$ can be written as:

$$S_{explorable}=\left\{{Node}_{index}|E_1+E_2\le E_{bound}\right\},\hspace{1em}\hspace{1em}\hspace{1em}if{ E}_{bound}\le E_T$$

(9)

$$E_1=E(P^c,P_{index}), E_2=E(P_{index},P_{\mathrm{e}\mathrm{x}\mathrm{p}\_min})$$

(10)

Figure 3 illustrates the selection of explorable nodes set.

2.2.3. Stochastic Modified Evaluation

For all nodes in $S_{explorable},$ we set up a hash map $M_{explorable}$ . For each node, its location and number of required pictures for that node are given as: ${Node}_{index}^{explorable}$ : ($P_{index}^{explorable},{\tilde{N}}_{index}^{explorable}$ ). The number of pictures required for nodes in $S_{explorable}$ are calculated from the estimation function in Equation (1).

The algorithm tends to select nodes located around the explored node that has the minimum number of pictures because, from Equation (1), those nodes tend to have the smallest estimated number of pictures. However, this process does not guarantee reaching the global minimum, therefore, minimizing the number of pictures. To sufficiently explore unknown nodes as well as exploit information from already explored nodes, a stochastic process is introduced to modify the picture number estimation function given in Equation (1).

Assume when the locations of a camera in space deviate with a smaller amount from each other, the difference of real picture number values at those locations is also smaller. Thus, among all unexplored nodes, the estimated picture number of an unexplored node, which is closer to the explored node, is more deterministic. To emphasize this feature, we make the estimated picture number follow a normal distribution with a mean ${\mu }_{\tilde{N}}$ being equal to the value from Equation (1) and a standard deviation ${\sigma }_{\tilde{N}}$ . As an unexplored node is further away from explored nodes, the larger value of ${\sigma }_{\tilde{N}}$ should be assumed, which means more indeterministic estimation (illustrated in Figure 4). The following equations summarize the above discussion:

For any nodes${Node}_{index=x}^{explorable}$ in $S_{explorable},$ its picture number estimation should follow the following normal distributions:

$$P(Z_x)=\mathcal{N}({{\mu }_{\tilde{N}}}_x,{{\sigma }_{\tilde{N}}}_x)$$

(11)

$${{\mu }_{\tilde{N}}}_x={\tilde{N}}_x^{explorable}$$

(12)

$${{\sigma }_{\tilde{N}}}_x=K_{sd}\ast {\overline{ED}}_{min}$$

(13)

where $P(Z_x)$ is the probability of a random variable $Z$ at ${Node}_x^{explorable}$ . ${\tilde{N}}_x^{explorable}$ is the estimated value from Equation (1) of ${Node}_x^{explorable}.$  $K_{sd}$ is a constant parameter, and ${\overline{ED}}_{min}$ is the smallest Euclidean distance among distances between that node and explored nodes.

As discussed above, the algorithm ensures that the camera at any iteration, always has enough energy to move back to the previous best node $N_{\mathrm{e}\mathrm{x}\mathrm{p}\_min}$ . Therefore, a new distribution can be generated, which sets values bigger than the minimum ($N_{\mathrm{e}\mathrm{x}\mathrm{p}\_min}$ ) in the original distribution to be the minimum value. Then Equations (11) to (13) are modified with a new random variable $Z_x^{new}$ and its expectations as follows:

$$Z_x^{new}=\left\{\begin{array}{c}{Z}_x, if Z_x<N_{\mathrm{e}\mathrm{x}\mathrm{p}\_min} \\ N_{\mathrm{e}\mathrm{x}\mathrm{p}\_min}, if Z_x \ge N_{\mathrm{e}\mathrm{x}\mathrm{p}\_min}\end{array}\right.$$

(14)

$$P\left(Z_x^{new}\right)=P(Z_x)=\mathcal{N}({{\mu }_{\tilde{N}}}_x,{{\sigma }_{\tilde{N}}}_x)$$

(15)

$${\widehat{N}}_x^{explorable}=\sum [Z_x^{new}\ast P\left(Z_x^{new}\right)]$$

(16)

where ${\widehat{N}}_x^{explorable}$ is the stochastic modified estimated value for the node ${Node}_x^{explorable}$ .

Then a new hash map ${\widehat{M}}_{explorable}$ is set up with pairs as ${Node}_{index}^{explorable}$ : ($P_{index}^{explorable},{\widehat{N}}_x^{explorable}$ ). The next target node to be explored is the one that has the smallest modified estimated value in the map:

$$inde{x}_{Next}=\left\{index\right|{\widehat{N}}_{inde{x}_{Next}}^{explorable}=\min ({\widehat{N}}_{index}^{explorable})\}$$

(17)

$$P^{Next}=P_{inde{x}_{Next}}^{explorable}$$

(18)

> ${\sigma }_2$ ) to differentiate possibility distributions. This approach distinguishes nodes that have the same estimations from equation (1) to have different likelihood to be explored in the next iteration.

2.2.4. Estimated Energy Cost Function

As discussed above, an estimated energy cost function $E(P_a,P_b)$ calculates the estimated energy cost from $P_a to P_b$ . This function is used to find feasible node set $S_{feasible}$ and explorable node set $S_{explorable}$ . In this section, the equations for the cost function are derived from the robot positioning-controlled system’s response. Also, trade-offs between energy cost and settling time of moving are discussed.

The energy of moving the camera between two locations from $P_{initial}$ to $P_{final}$ , results from the energy cost of DC motors in 6 DOFs robot arm’s each joint rotating from joint angles $q_{initial}$ to $q_{final}$ . Therefore, $E(P_{initial},P_{final})$ can be described as the sum of the time integration of rotational power in each joint. That is:

$$E(P_{initial},P_{final})=\sum _{i=1}^6{\int }_{t_0^i}^{t_f^i}{V}^i·I^i·dt$$

(19)

where $V^i$ is the voltage and $I^i$ is the current in the DC motor’s circuit of the $i^{th}$ joint. $t_0^i$ and $t_f^i$ are initial time and final time of the $i^{th}$ joint moving from its initial angle $q_{initial}^i$ to its final angle $q_{final}^i$ .

Without detailed derivation, the dynamic model of a 6dofs revolutionary robotic manipulator and DC motors can be expressed as [20]:

$${\displaystyle \frac{1}{r_k}}{J_m}_k{\ddot{q}}_k+\sum _{j=1}^6{d}_{j,k}{\ddot{q}}_j+\sum _{i,j}^6{c}_{i,j,k}\left(q\right){\dot{q}}_i{\dot{q}}_j+{{\displaystyle \frac{1}{r_k}}\mathrm{B}{\dot{q}}_k+\mathsf{\Phi }}_k\left(q\right)={\displaystyle \frac{K_m}{r_{k}R}}{V}^i$$

(20)

$$J_m=J_a+J_g, {B=B}_m+{\displaystyle \frac{K_b{K}_m}{R}}$$

(21)

$$c_{i,j,k}={\displaystyle \frac{1}{2}}({\displaystyle \frac{\delta d_{k,j}}{\delta q_i}}+{\displaystyle \frac{\delta d_{k,i}}{\delta q_j}}-{\displaystyle \frac{\delta d_{i,j}}{\delta q_k}}), {\mathsf{\Phi }}_k={\displaystyle \frac{\delta V}{\delta q_k}}$$

(22)

$$i,j,k\in (1, 2, 3, 4, 5, 6)$$

(23)

where Equations (20) and (21) express the $k^{th}$ joint dynamics equation. $q_i/q_j/q_k$ is the joint revolute variable. $J_{a}and{J}_g$ represent the moment of inertia of the motor, and the gear of the model. $r_k$ is gear ratio at $k^{th}$ joint, and $B_m$ is the damping effect of the gear. $d_{i,j}$ represents the entry of inertial matrix of the robot manipulator at $i^{th}$ row and $j^{th}$ column. $c_{i,j,k}$ is Christoffel symbols and for a fixed $k$ , with $c_{i,j,k}=c_{j,i,k}$ . And ${\mathsf{\Phi }}_k$ is the derivative of potential energy V with respective to $k^{th}$ joint variance. $K_m$ is the torque constant in $N-m/amp,$  $K_b$ is the back emf constant, and R is Armature Resistance.

Take $u^i$ as the actuator input to the dynamic system (20) measured at $i^{th}$ joint from a designed controller. Therefore, $u^i$ equals to the right-side of Equation (20). That is:

$$V^i={\displaystyle \frac{rR}{K_m}}{u}^i$$

(24)

In addition, without derivation, the expression of current in the Laplace domain [20] is:

$$\left(Ls+R\right)I^i\left(s\right)=V^i\left(s\right)-{\displaystyle \frac{K_b}{r}}s{q}^i(s)$$

(25)

$$\mathrm{Then}\text{:} I^i\left(s\right)={\displaystyle \frac{1}{\left(Ls+R\right)}}{V}^i\left(s\right)-{\displaystyle \frac{K_b}{r}}{\displaystyle \frac{s}{\left(Ls+R\right)}}{q}^i\left(s\right)$$

(26)

$$\mathrm{Take} \mathrm{the} \mathrm{inverse} \mathrm{Laplace}\text{:} I^i\left(t\right)={\displaystyle \frac{1}{L}}{e}^{-{\displaystyle \frac{R}{L}}t}\ast V^i\left(t\right)-{\displaystyle \frac{K_b}{r}}({\displaystyle \frac{1}{L}}-{\displaystyle \frac{R}{L^2}}{e}^{-{\displaystyle \frac{R}{L}}t})\ast q^i\left(t\right)$$

(27)

where L is armature inductance$, K_b$ is back emf constant, and $\ast$ is the convolutional multiplication. Therefore, the instant current in the time domain $I^i\left(t\right)$ is a function of the instant voltage $V^i\left(t\right)$ and the instant angle $\mathrm{q}\left(t\right).$

Various controller designs, such as PID controller [21], and Youla controller [22], of six DoFs revolute robotic manipulators have been well developed in many papers. In this paper, we use a previous Youla controller design [22] with feedback linearization (Figure 5) as the position controller of the robot manipulator that holds the camera.

From Figure 5, the actuator input $u^i$ can be derived from a virtual input $v^i$ so that:

$$u^i=M(q^i\left(t\right))v^i+H(q^i\left(t\right),\dot{q^i}(t))$$

(28)

where M, and H are nonlinear functions of $q^i\left(t\right)$ , and $\dot{q^i}(t)$ , the first derivative of $q^i\left(t\right).$ Without showing the controller development in this paper, the 1 DoF controller transfer function $G_c\left(s\right)$ and 1 DoF nominal plant $G_p\left(s\right)$ with $v^i$ as the virtual actuator input in the feedback linearization are expressed as follows:

$$G_c(s)={\displaystyle \frac{3{{\tau }_{in}}^{2}s+1}{{{\tau }_{in}}^{3}s+3{{\tau }_{in}}^2}}$$

(29)

$$G_p(s)={\displaystyle \frac{1}{s^2}}$$

(30)

where ${\tau }_{in}$ is a constant parameter in the controller design.

Then the following transfer functions can be calculated as:

$${\displaystyle \frac{v^i(s)}{{q^i}_{final}(s)}}={\displaystyle \frac{G_c}{1+G_c{G}_p}}$$

(31)

$${\displaystyle \frac{q^i(s)}{{q^i}_{final}(s)}}={\displaystyle \frac{G_c{G}_p}{1+G_c{G}_p}}$$

(32)

$${\displaystyle \frac{\dot{q^i}(s)}{{q^i}_{final}(s)}}={\displaystyle \frac{{sG}_c{G}_p}{1+G_c{G}_p}}$$

(33)

By taking inverse Laplace transform, and ${q^i}_{final}$ as a step input, the following equations in the time domain are:

$$v^i\left(t\right)={(q^i}_{final}-{q^i}_{intial})({\displaystyle \frac{1}{{{\tau }_{in}}^4}}{t}^2{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}}+{\displaystyle \frac{-5}{{{\tau }_{in}}^3}}t{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}}+{\displaystyle \frac{3}{{{\tau }_{in}}^2}}{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}})$$

(34)

$$q^i\left(t\right)={(q^i}_{final}-{q^i}_{intial})({\displaystyle \frac{1}{{{\tau }_{in}}^2}}{t}^2{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}}-{\displaystyle \frac{1}{{\tau }_{in}}}t{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}}-e^{-{\displaystyle \frac{t}{{\tau }_{in}}}}+1)+{q^i}_{intial}$$

(35)

$$\dot{q^i}(t)={(q^i}_{final}-{q^i}_{intial})({\displaystyle \frac{3}{{{\tau }_{in}}^2}}t{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}}-{\displaystyle \frac{1}{{{\tau }_{in}}^3}}{t}^2{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}})$$

(36)

With expressions of $v^i\left(t\right), q^i\left(t\right),$ and $\dot{q^i}(t)$ , Equations (24) and (28) shows that:

$$V^i(t)=F({q^i}_{intial}, {q^i}_{final},t)$$

(37)

And combine Equations (27) and (37):

$$I^i(t)=G({q^i}_{intial}, {q^i}_{final},t)$$

(38)

where F, and G are nonlinear functions of ${q^i}_{initial}, {q^i}_{final},\mathrm{a}\mathrm{n}\mathrm{d} t$ .

It has been shown so far that the estimated energy cost $E(P_{initial},P_{final})$ from location $P_{initial}$ to $P_{final}$ is a function of $q_{initial}$ and $q_{final}$ .

$q_{initial}$ and $q_{final}$ are derived from the inverse kinematics process [20] (Shown in Appendix Equations (B6)–(B13)), that is:

$$q_{initial}=inverkinematics(P_{initial})$$

(39)

$$q_{final}=inverkinematics(P_{final})$$

(40)

$$\mathrm{And} q_{initial}=\left[{q^i}_{initial}|i\in \left(\mathrm{1,2},\mathrm{3,4},\mathrm{5,6}\right)\right], q_{final}=\left[{q^i}_{final}|i\in \left(\mathrm{1,2},\mathrm{3,4},\mathrm{5,6}\right)\right]$$

(41)

Development of Equations (34) to (36) assumes that the target angle of each joint ${q^i}_{final}$ is a step input. A more realistic assumption is to set ${q^i}_{final}$ as a delayed input.

$${q^i}_{final}\left(t\right)=(1-e^{-{\displaystyle \frac{t}{{\tau }_{delay}}}}){q^i}_{final}$$

(42)

where ${\tau }_{delay}$ is a time constant that measures the time delay of the target in the real positioning control. ${\tau }_{delay}\ge 0$ , and when ${\tau }_{delay}=0$ , it indicates no delay exists in the input.

The Laplace form of (42) is:

$${q^i}_{final}\left(s\right)=({\displaystyle \frac{1}{s}}-{\displaystyle \frac{1}{s+\frac{1}{{\tau }_{delay}}}}){q^i}_{final}$$

(43)

With new expression of ${q^i}_{final}\left(s\right)$ , Equation (34) to (36) can be developed as:

$$v^i\left(t\right)={(q^i}_{final}-{q^i}_{intial})({\displaystyle \frac{A_v}{{{\tau }_{in}}^3}}{t}^2{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}}+B_v{\displaystyle \frac{B_v}{{{\tau }_{in}}^2}}t{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}}+{\displaystyle \frac{C_v}{{\tau }_{in}}}{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}}+{\displaystyle \frac{D_v}{{\tau }_{delay}}}{e}^{-{\displaystyle \frac{t}{{\tau }_{delay}}}})$$

(44)

$$\begin{gathered} A_v={\displaystyle \frac{1}{{\tau }_{in}}}-{\displaystyle \frac{-{\tau }_{delay}}{{\tau }_{in}({\tau }_{in}-{\tau }_{delay})}}\hspace{1em}\hspace{1em}\hspace{1em}{B}_v={\displaystyle \frac{-5}{{\tau }_{in}}}-{\displaystyle \frac{7{\tau }_{in}{\tau }_{delay}-5{{\tau }_{delay}}^2}{{\tau }_{in}{({\tau }_{in}-{\tau }_{delay})}^2}} \\ {C}_v={\displaystyle \frac{3}{{\tau }_{in}}}-{\displaystyle \frac{{\tau }_{delay}(-8{{\tau }_{in}}^2+9{\tau }_{in}{\tau }_{delay}-3{{\tau }_{delay}}^2)}{{{\tau }_{in}({\tau }_{in}-{\tau }_{delay})}^3}}\hspace{1em}\hspace{1em}\hspace{1em}{D}_v=-{\displaystyle \frac{3{\tau }_{in}{\tau }_{delay}-{{\tau }_{delay}}^2}{{({\tau }_{in}-{\tau }_{delay})}^3}} \end{gathered}$$

$$q^i\left(t\right)={(q^i}_{final}-{q^i}_{intial})({\displaystyle \frac{A_Q}{{{\tau }_{in}}^3}}{t}^2{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}}+{\displaystyle \frac{B_Q}{{{\tau }_{in}}^2}}t{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}}+{\displaystyle \frac{C_Q}{{\tau }_{in}}}{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}}+{\displaystyle \frac{D_Q}{{\tau }_{delay}}}{e}^{-{\displaystyle \frac{t}{{\tau }_{delay}}}}+1)+{q^i}_{intial}$$

(45)

$$\begin{gathered} A_Q={\tau }_{in}+{\displaystyle \frac{{\tau }_{in}{\tau }_{delay}}{({\tau }_{in}-{\tau }_{delay})}}\hspace{1em}\hspace{1em}\hspace{1em}{B}_Q=-{\tau }_{in}-{\displaystyle \frac{3{{\tau }_{in}}^2{\tau }_{delay}-{\tau }_{in}{{\tau }_{delay}}^2}{{({\tau }_{in}-{\tau }_{delay})}^2}} \\ {C}_Q=-{\tau }_{in}-{\displaystyle \frac{-3{{\tau }_{in}}^2{{\tau }_{delay}}^2+{{{\tau }_{in}\tau }_{delay}}^3}{{({\tau }_{in}-{\tau }_{delay})}^3}}\hspace{1em}\hspace{1em}\hspace{1em}{D}_Q=-{\displaystyle \frac{3{\tau }_{in}{{\tau }_{delay}}^3-{{\tau }_{delay}}^4}{{({\tau }_{in}-{\tau }_{delay})}^3}} \end{gathered}$$

$$\dot{q^i}(t)={(q^i}_{final}-{q^i}_{intial})({\displaystyle \frac{A_{\dot{Q}}}{{{\tau }_{in}}^4}}{t}^2{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}}+{\displaystyle \frac{B_{\dot{Q}}}{{{\tau }_{in}}^3}}t{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}}+{\displaystyle \frac{C_{\dot{Q}}}{{{\tau }_{in}}^2}}{e}^{-{\displaystyle \frac{t}{{\tau }_{in}}}}+{\displaystyle \frac{D_{\dot{Q}}}{{{\tau }_{delay}}^2}}{e}^{-{\displaystyle \frac{t}{{\tau }_{delay}}}})$$

(46)

$$\begin{gathered} A_{\dot{Q}}= -A_Q\hspace{1em}\hspace{1em}\hspace{1em}{B}_{\dot{Q}}=2A_Q-B_Q \\ {C}_{\dot{Q}}=B_Q-C_Q\hspace{1em}\hspace{1em}\hspace{1em}{D}_{\dot{Q}}=-D_Q \end{gathered}$$

A simulation scenario is set up to calculate how estimated energy cost changes with varying ${\tau }_{delay}.$ Set $P_{initial}=[-0.30, 0.05, 1.20]$ , and $P_{final}=[-0.45, 0.45, 1.20]$ and use the equations above. Figure 6 shows the response of one angle $q^1\left(t\right)$ with varying ${\tau }_{delay}$ and Table 1 presents the estimated energy cost: $E(P_{initial},P_{final})$ and settling time: $t_s$ with varying ${\tau }_{delay}.$

Table 1 clearly shows a tradeoff between the estimated energy cost and settling time; reducing the energy cost of moving the camera inevitably increases the response time. This finding matches the results in Figure 6.

If the delay of input is given or can be measured, the estimated energy cost can be calculated through the process in this section. However, if the value is unknown, the delay constant must be chosen and decided based on the following criteria:

• Select small ${\tau }_{delay}$ for a conservative algorithm that searches a small area but ensures it ends up with the minimum that has been explored.
• Select large ${\tau }_{delay}$ for an aggressive algorithm that searches a large area but risks not ending up at the minimum that has been explored.

In this section, the estimated energy cost function is well developed. The parameter values of motors and gears used in the simulations are summarized in Appendix Table A4.

3. Results and Discussion

The developed algorithm has been simulated on an application in automated manufacturing (Figure 7). This is a multi-robot system composed of a visual system and a tool manipulation system. In the visual system, a camera is mounted on an elbow robot arm while a tool is held by the end-effector of the robot manipulator arm. The goal of the visual system is to provide a precise estimation of the tool pose so that the tool manipulator can control the pose with guidance from the visual system. The first step of the manufacturing process is to move the camera in the space and search for the best location so that the number of images required for averaging is minimized to reduce the image noises within an acceptable limit. The model of both robot manipulators is ABB IRB 4600 [23] and the model of the stereo camera we choose in this project is Zed 2 [24].

In this section, we first define the camera’s operational space in this application and then provide the simulation results of a made-up scenario.

3.1. Camera Operational Space

In the above discussion, a camera operational space is gridded with nodes and each node is a potential candidate of the camera’s optimal position. The camera operational space is defined by three geometric constraints below:

1. The camera can only be allowed to locations where fiducial markers that are attached to the tool are recognizable on the image frame. Therefore, the fiducial markers are in the angle of view of the camera, and the distance between the markers and the camera center is within a threshold.

2. The camera can only be allowed to locations within the reach of the robot arm.

3. The camera can only be allowed in locations where the visual system and the tool manipulation system do not physically interfere with each other.

From the specifications of the stereo camera and the robot arm (Table A1 and Table A3), the geometry and dimensions of the camera operational space are analyzed in the following part of the subsections.

3.1.1. Reachable and Dexterous Workspace of Two-Hybrid Systems

In the spherical wrist model of the robot arm, three rotational axes represent the pitch, roll, and yaw of the end-effector independently. As shown in Figure 8, those three axes intersect at point $H$ . Point $P$ is the location of the end-effector and it’s the place where the camera or tool is mounted. For the elbow manipulator mounted with the stereo camera, the camera is placed so that its optical center line coincides with axis C. The location of camera optical center C is determined and a good estimation of it can be found in this paper [25]. For now, assume the optical center is in the middle of $\overline{PJ}$ (set $\overline{PC}=17 mm$ ). For the elbow manipulator mounted with the tool, the screwdriver is attached at the point $P$ . Point J indicates the far end of the object (tool or camera) attached to the end effector. Point H is kept stationary no matter which axis rotates. Therefore, the task of positioning and rotation is decoupled in the spherical wrist robot model.

The idea of reachable and dexterous workspaces of an ideal elbow manipulator has been introduced in the paper [26]. An ideal elbow manipulator is a manipulator whose angles of rotation are free to move in the whole operational range [0, 2$\pi ].$ However, a realistic elbow manipulator is limited to moving its joints within certain ranges of angle.

The workspace of an ideal elbow manipulator (Figure 9) is a sphere centered at the joint 1 of the manipulator, denoted as point $o$ . The reachable workspace concerning the center o of a manipulator is the aggregate of all possible locations of the point $J$ attached with the end-effector and is denoted as $W_o(J)$ . The dexterous workspace to the center o of a manipulator is the aggregate of all possible locations that point $J$ can reach all possible orientations of the end-effector and is denoted as $W_o^d(J)$ . The reachable workspace of $H$ is denoted as $W_o(H)$ .

It is clearly shown in the figure:

$$W_o^d\left(J\right) \subseteq W_o(H) \subseteq W_o(J).$$

(47)

For an ideal elbow manipulator, the radiuses of workspaces are expressed as below:

$$\mathrm{Radius} \mathrm{of} W_o\left(H\right)=L_2+\sqrt{{L_3}^2+{L_4}^2}$$

(48)

$$\mathrm{Radius} \mathrm{of} W_o(J)=L_2+\sqrt{{L_3}^2+{L_4}^2}+L_t+\overline{PJ}$$

(49)

$${\mathrm{Radius}\mathrm{of} W}_o^d\left(J\right)=L_2+\sqrt{{L_3}^2+{L_4}^2}-L_t-\overline{PJ}$$

(50)

where $L_2,$  $L_3, and L_4$ are links’ length of the manipulator. $L_t$ is the length of the end-effector and $\overline{PJ}$ is the length of the object that mounts on the end-effector.

Because the camera and the tool must be able to rotate in all 3 DoFs when they are at the target position. So, the dexterous workspace is used as the working space of the visual system and the tool manipulation system. Dexterous workspace of the optical center is used for the visual system and Dexterous workspace of the far endpoint of the tool is used for the tool manipulator system.

$$\begin{gathered} \mathrm{Radius} \mathrm{of} W_o^d\left(Visual\right)=r_V=L_2+\sqrt{{L_3}^2+{L_4}^2}-L_t-{\overline{PC}}_{camera} \\ =1.716 m (\mathrm{From} \mathrm{Specification} \mathrm{Appendix} \mathrm{Table} \mathrm{A}1) \end{gathered}$$

(51)

$$\begin{gathered} \mathrm{Radius} \mathrm{of} W_o^d\left(Tool\right)=r_T=L_2+\sqrt{{L_3}^2+{L_4}^2}-L_t-{\overline{PJ}}_{tool} \\ =1.606 m (\mathrm{From} \mathrm{Specification} \mathrm{Appendix} \mathrm{Table} \mathrm{A}1) \end{gathered}$$

(52)

Figure 9 shows the workspace setup for the hybrid system. The dexterous space of robot arm with camera is a sphere with radius of $r_V$ and sphere center is $O_V$ and the dextrous space of the robot arm with tools is a sphere the radius is $r_T$ and center of this sphere is $O_T.$ Two systems intersect with the ground with an angle ${\theta }_1$ and ${\theta }_2$ .

$${\theta }_1=\arcsin \left({\displaystyle \frac{L_1}{r_V}}\right)=16.77°$$

(53)

$${\theta }_2=\arcsin \left({\displaystyle \frac{L_1}{r_T}}\right)=17.95°$$

(54)

To avoid interference of the visual and the tool manipulation systems (as the third geometric constraint of the operational space), the reachable workspaces (the maximum reach) of two systems should have no overlap. The reachable workspaces of two systems are calculated from Equation (49) is:

$$\begin{gathered} \mathrm{Radius} \mathrm{of} W_o\left(Visual\right)=r_{Vr}=L_2+\sqrt{{L_3}^2+{L_4}^2}+L_t+{\overline{PJ}}_{camera} \\ =2.044 m (\mathrm{From} \mathrm{Specification} \mathrm{Appendix} \mathrm{Table} \mathrm{A}1) \end{gathered}$$

(55)

$$\begin{gathered} \mathrm{Radius} \mathrm{of} W_o\left(Tool\right)=r_{Tr}=L_2+\sqrt{{L_3}^2+{L_4}^2}+L_t+{\overline{PJ}}_{tool} \\ =2.138 m (\mathrm{From} \mathrm{Specification} \mathrm{Appendix} \mathrm{Table} \mathrm{A}1) \end{gathered}$$

(56)

Let $L_{VT}$ is the distance between $O_V$ and $O_T$ . The following relationship must be satisfied:

$$L_{VT}\ge r_{Vr}+r_{Tr}=4.182 m$$

(57)

The visual system is able to detect the tool when it gets close to the target pose (Just above the bolt). Also reference points are put near the tool’s target pose. Assume a marker is put at where the bolt is, and this marker represents as an approximated location of all interest points and reference points. Therefore, the marker should be within the dexterous space of the tool manipulation system. Set up a coordinate system with ${\overline{O}}_V$ (the projection of $O_V$ on the ground with $a_1$ offset) as the origin. The projection of $O_T$ on the ground with $a_1$ offset is ${\overline{O}}_T.$ Assume the marker is placed on the line $\overline{{\overline{O}}_V{\overline{O}}_T}$ . The distance m from marker M to ${\overline{O}}_V$ should be:

$$L_M\ge {a_1+L}_{VT}-r_T\cos \left({\theta }_2\right)\ge 2.829 m$$

(58)

3.1.2. Detectable Space for the Stereo Camera

From Appendix Table A3, the angle of view in width and height for both lens in stereo camera is $\alpha$ = $86.05°$ , and $\beta =55.35°$ . The detectable space for each lens can be modeled as the inner area of a cone with angle $\alpha$ and $\beta .$ And the overlapped space is the detectable space for the stereo camera. The model is shown below (Figure 10). Point C is the center of the baseline whose length is denoted as $b$ or center of the camera system. The overlap area is also a cone with angles of vertex $\alpha$ and $\beta$ with the offset $d$ from baseline. Point Q is the vertex of the cone.

$$d={\displaystyle \frac{b/2}{\mathrm{t}\mathrm{a}\mathrm{n}(\raisebox{1ex}{$\alpha $}\!\left/ \!\raisebox{-1ex}{$2$}\right.)}}=0.064m$$

(59)

There is an upper bound of the distance between the object to the camera center; if the object is too far away from the camera center, the dimensions of projected images are too small to be measured. Suppose to have a clear image of the fiducial markers (circle shapes), it is required that the diameter of the projected image should takes at least 5-pixel numbers in the image frame:

$$N_H \ge 5 \mathrm{resolution}$$

(60)

where $N_H$ is the pixel number in the image frame.

Select the Zed camera’s mode so that its resolution is 1920*1082 and the image sensor size is 5.23mm X 2.94mm. Then numbers resolution in unit length is:

$$n=367 \mathrm{resolution}/\mathrm{mm}$$

(61)

Then the range of markers’ diameter on the image frame is:

$$d_H={\displaystyle \frac{N_H }{n}}\ge 0.0136 mm$$

(62)

Also assume in the inertial frame, the diameter of attached fiducial marks is:

$$D_H=12 mm$$

(63)

Then from pinhole model of the camera, the range of distance between markers and camera center Z is:

$$Z={\displaystyle \frac{f_u{D}_H}{d_H}}\le 2.47 m=Z_{max}$$

(64)

where $Z_{max}$ is the maximum depth of camera to detect the markers. This parameter defines the height of the cone in Figure 10. The detectable space (abiding the second geometric constraint) forms an elliptic cone with different angles of vertex.

3.1.3. Camera Operational Space Development

In Figure 11, set the camera lens so that it is always parallel to the face of the marker and the baseline is parallel to${\overline{X}}_V$ axis. In this case, lens is kept facing downward when detecting the marker on the ground. As discussed in the above section, the detectable space of the camera is modeled as a cone. This cone intersects with the horizontal plane and forms an oval. To have the camera detect the marker, the marker must be contained inside the oval.

An inertia coordinate system ${\overline{O}}_V{\overline{X}}_V{\overline{Y}}_V{\overline{Z}}_V$ is set up with its origin seated at the bottom center of the visual system. Make point $C$ as the location of the camera center and its coordinates in the inertial frame are ($X_C^V$ , $Y_C^V$ , $Z_C^V$ ). $Q$ is the vertex of the detectable elliptic cone, and $Q’$ is the projection of $Q$ on the horizontal plane. $M$ is the marker.

The elliptic cone of the camera’s detectable space always intersects with the horizontal plane. Consider an extreme case when point $C$ is located at the apex of the visual system workspace as shown in Figure 11. Then the $Z_C^V$ coordinate of $C$ at the inertial frame is:

$$Z_C^V\left\{max\right\}=L_1+r_V=2.211 m \le Z_{max}$$

(65)

where $Z_{max}$ defined above is the maximum depth of camera to detect the markers. Therefore, the cone intersects with the horizontal plane when the camera center $C$ is placed at any location of the workspace’s contour.

From expression of Cartesian coordinates in the inertia frame:

$$a_{Q\text{'}}=(Z_C^V-d)tan({\displaystyle \frac{\alpha }{2}}), b_{Q\text{'}}=(Z_C^V-d)tan({\displaystyle \frac{\beta }{2}})$$

(66)

where $d$ is the same offset in Equation (59). $a_{Q\text{'}}$ and $b_{Q\text{'}}$ are major radius and minor radius of the oval. Also coordinates of points $Q’$ and $M$ are $(X_C^V,Y_C^V, 0)$ and $(L_M,0, 0)$ . $Q’$ is the center of the oval. To ensure the marker $M$ is inside the projected oval, it is required that:

$${\displaystyle \frac{{(X_C^V-L_M)}^2}{{a_{Q\text{'}}}^2}}+{\displaystyle \frac{{(Y_C^V)}^2}{{b_{Q\text{'}}}^2}}\le 1$$

(67)

Plug Equation (66) into Equation (67):

$${\displaystyle \frac{{(X_C^V-L_M)}^2}{{tan(\frac{\alpha }{2})}^2}}+{\displaystyle \frac{{(Y_C^V)}^2}{{tan(\frac{\beta }{2})}^2}}\le {(Z_C^V-d)}^2$$

(68)

Inequality (68) is exactly a mathematic expression of all points $C$ ($X_C^V$ , $Y_C^V$ , $Z_C^V$ ) within a cone whose opening is in the positive Z direction with its vertex at ($L_M$ ,$0,d$ ) and the opening parameters as $\tan ({\displaystyle \frac{\alpha }{2}})=0.934 \mathrm{and} \tan ({\displaystyle \frac{\beta }{2}})=0.524.$ Therefore, this inequality defines the second geometric constraint of the camera center. It forms an elliptical cone with its vertex located in Figure 12 at point E, which is the offset point of the marker $M$ from the ground$.$ As $C$ ($X_C^V$ , $Y_C^V$ , $Z_C^V$ ) should also be within the sphere of the workspace (abiding the first geometric constraint), the camera operational space is presented as the overlap between the cone and the sphere (the shaded area in Figure 12).

From Figure 12, the marker should not be too far from the visual system, otherwise there is no overlap region between the cone and the sphere. The largest value of $L_M$ , which is the distance from the marker to the coordinate center, occurs when the cone is tangent to the sphere as shown in Figure 13. To find the limit of $L_M$ , draw a horizontal line across point $O_V$ so that it intersects the surface of the cone at point A and the center line of the cone at point B as shown in Figure 13. Let $\overline{O_{V}A}$ and $\overline{AB}$ are line segments’ length.

Therefore, from the geometry, the upper limit of $L_M$ is:

$$L_M<\overline{O_{V}A}+\overline{AB}+a_1={\displaystyle \frac{r_V}{\cos \left(\frac{\alpha }{2}\right)}}+\left(L_1-d\right)\tan \left({\displaystyle \frac{\alpha }{2}}\right)+a_1$$

(69)

Combining with the lower bound of $L_M$ in Equation (58), the marker M should be placed in the system so that:

$$2.829 m\le L_{VT}-r_T\cos \left({\theta }_2\right)\le L_M<{\displaystyle \frac{r_V}{\cos \left(\frac{\alpha }{2}\right)}}+(L_1-d)\tan \left({\displaystyle \frac{\alpha }{2}}\right)+a_1=2.925 m$$

(70)

3.1.4. Mathematic Expression for Node Coordinates Within the Camera Operational Space

From Equation (70), the closer the marker’s distance $L_M$ to its lower bound, the larger the operational space is. In general, larger operational space is preferred, because the camera has a larger space for searching the optimal location. Set $L_M$ equals to its lower bound:

$$L_M=2.83m$$

(71)

Get the cross-section area at ${\overline{X}}_V-{\overline{Z}}_V$ plane (Figure 14).

The function of semi-circle and the line EFG is:

$$({{\overline{Z}}_V-L_1)}^2+{\left({\overline{X}}_V-a_1\right)}^2 ={r_V}^2$$

(72)

$${\overline{X}}_V=-tan({\displaystyle \frac{\alpha }{2}})( {\overline{Z}}_V-d)+L_M$$

(73)

Combine Equations (72) and (73) and solve:

$$(1+{tan({\displaystyle \frac{\alpha }{2}})}^2){{\overline{Z}}_V}^2- 2({tan({\displaystyle \frac{\alpha }{2}})}^{2}d+tan\left({\displaystyle \frac{\alpha }{2}}\right)(L_M-a_1)+L_1){\overline{Z}}_V+{\tan \left({\displaystyle \frac{\alpha }{2}}\right)}^2{d}^2+2\tan \left({\displaystyle \frac{\alpha }{2}}\right)(L_M-a_1)d+{(L_M-a_1)}^2-{r_V}^2+{L_1}^2 =0$$

(74)

Solve Equation (74) by plugging numbers:

$$1.872 {{\overline{Z}}_V}^2-6.061{\overline{Z}}_V+4.670=0$$

(75)

$$\mathrm{Solve}\text{:} {{\overline{Z}}_V}_{max},{{\overline{Z}}_V}_{min}=1.972 m, 1.265 m$$

(76)

${{\overline{Z}}_V}_{max}$ and ${{\overline{Z}}_V}_{min}$ are the largest and smallest ${\overline{Z}}_V$ coordinates of the camera center $C$ that is within the camera operation space.

Take a value of $Z_C^V$ ( ${\overline{Z}}_V$ coordinates of the camera center $C$ ) in the range (${{\overline{Z}}_V}_{min}$ ,${{\overline{Z}}_V}_{max}$ ) = ($1.265 m$ ,$1.972 m$ ). Set a plane ${\overline{Z}}_V=Z_C^V$ , and that plane intersects the camera operational space and forms a shade area, which is the overlap between the circle and the oval, as shown in Figure 14. The inequality governing points $C$ within sphere at a specific $Z_C^V$ is:

$$({{\overline{X}}_V-a_1)}^2+{{\overline{Y}}_V}^2\le {r_V}^2-({Z_C^V-L_1)}^2$$

(77)

The inequality governing points $C$ within cone at $Z_C^V$ is:

$${\displaystyle \frac{{({\overline{X}}_V-L_M)}^2}{{\tan (\frac{\alpha }{2})}^2}}+{\displaystyle \frac{{({\overline{Y}}_V)}^2}{{\tan (\frac{\beta }{2})}^2}}\le {(Z_C^V-\mathrm{d})}^2$$

(78)

By plotting those inequalities in Figure 15, the shaded area is where $X_C^V$ and $Y_C^V$ (${\overline{X}}_V$ and ${\overline{Y}}_V$ coordinates of the camera center $C$ ) should be located within. The intersection of the two curves occurs when equality holds for (77) and (78), that is:

$$({{\overline{X}}_V-a_1)}^2+{{\overline{Y}}_V}^2={r_V}^2-({Z_C^V-L_1)}^2$$

(79)

$${\displaystyle \frac{{({\overline{X}}_V-L_M)}^2}{{\mathrm{t}\mathrm{an}(\frac{\alpha }{2})}^2}}+{\displaystyle \frac{{({\overline{Y}}_V)}^2}{{\tan (\frac{\beta }{2})}^2}}={(Z_C^V-d)}^2$$

(80)

Solve by plugging numbers:

$$\begin{gathered} ({{\overline{Y}}_V}_{max}, {{\overline{Y}}_V}_{min})=\pm 0.5\ast \\ \sqrt{3.44{\ast Z_C^V}^2-2.0241\ast Z_C^V-29.802+2.451\ast \sqrt{-29.475{\ast Z_C^V}^2+23.707\ast Z_C^V+137.28}} \end{gathered}$$

(81)

${{\overline{Y}}_V}_{max}$ and ${{\overline{Y}}_V}_{min}$ are the largest and the smallest ${\overline{Y}}_V$ coordinates of the camera center $C$ in the camera operation space with specific $Z_C^V$ .

Take $Y_C^V$ in the range (${{\overline{Y}}_V}_{min}$ , ${{\overline{Y}}_V}_{max}$ ), then for specific $X_C^V$ :

$${{\overline{X}}_V}_{max}=\sqrt{{r_V}^2-({Z_C^V-L_1)}^2-{Y_C^V}^2}+a_1$$

(82)

$${{\overline{X}}_V}_{min}=L_M-\sqrt{{{(Z}_C^V-d)}^2{\tan ({\displaystyle \frac{\alpha }{2}}) }^2-{Y_C^V}^2{\displaystyle \frac{{\tan (\frac{\alpha }{2})}^2}{{\tan (\frac{\beta }{2})}^2}}}$$

(83)

${{\overline{X}}_V}_{max}$ and ${{\overline{X}}_V}_{min}$ are the largest and the smallest ${\overline{X}}_V$ coordinates of the camera center $C$ in the camera operation space with specific $Z_C^V$ and $Y_C^V$ .

3.1.5. Numerical Solution of the Ideal Camera Operation Space

With known parameters from the camera and robot arm specifications, all possible location of the camera center $C$ ($X_C^V$ , $Y_C^V$ , $Z_C^V$ ) inside the camera operational space can be derived from the following steps:

1. Calculate ${{\overline{Z}}_V}_{max}$ and ${{\overline{Z}}_V}_{min}$ by Equation (74), and mesh grid $Z_C^V$ in (${{\overline{Z}}_V}_{min}$ ,${{\overline{Z}}_V}_{max}$ ).

2. Take a mesh value $Z_C^V$ and calculate ${{\overline{Y}}_V}_{max}$ and ${{\overline{Y}}_V}_{min}$ by Equations (79) and (80), and mesh grid $Y_C^V$ in (${{\overline{Y}}_V}_{min}$ ,${{\overline{Y}}_V}_{max}$ ).

3. Take a mesh $Y_C^V$ and calculate ${{\overline{X}}_V}_{max}$ and ${{\overline{X}}_V}_{min}$ by Equations (82) and (83), and mesh grid $X_C^V$ in (${{\overline{X}}_V}_{min}$ ,${{\overline{X}}_V}_{max}$ ).

Each above computed $C$ ($X_C^V$ , $Y_C^V$ , $Z_C^V$ ) is inside the camera operation space. And those three steps completely account for all geometric constraints defined above. The number of mesh points (nodes available for searching) depends on the mesh grid size.

3.1.6. The Camera Operational Space from the Realistic Robot Manipulator

The above procedures of finding the camera operational space assumes the use of ideal robotic manipulators, whose joints are freely to move from $[0, 2\pi ]$ . However, for realistic robotic manipulators, their revolutionary movement is limited within certain ranges of angles, and the ranges of motion in every joint of the specific robot manipulator: ABB IRB 4600 are listed in Appendix Table A2.

The geometric shape of the operational space for a realistic robotic manipulator is usually irregular (unlike an ideal manipulator whose operational space is the combination of a sphere and a cone) and the mathematical methods of directly finding the nodes within the space is computationally expensive. A faster way of generating realistic camera operational space is to decrease the ideal operational space by taking out nodes that are out of the operational ranges. Those outlier nodes can be found from the inverse kinematic model of the robotic manipulator. Figure 16 shows the decreased nodes from the ideal camera operational space to the realistic space.

3.2. Simulations of scenarios

A simulation in MATLAB is used to validate the proposed algorithm. In the simulation, the environment is set up with two minimums in the space (only one is the global minimum). $K_{est}$ and $K_{sd}$ are tunable parameters and set as $K_{est}=5, and K_{sd}=50 \left(m^{-1}\right).$ The initial available energy bound is set to be $E_{bound}=$ 12 (Watt Second (ws) or Joule). Figure 17 shows the sequences of nodes being explored in iteration steps by setting different energy threshold $E_T$ .

By trials of different simulation runs, it can be concluded that:

When $E_T$ is large (in this scenario, ${3ws<E}_T\le 12ws),$ the algorithm is conservative, so that it has only searched a small area that excludes the global minimum.

When $E_T$ is in mid-range (in this scenario, ${0.5ws\le E}_T\le 3ws$ ), the algorithm drives the camera to the global minimum.

When $E_T$ is small (in this scenario, ${0ws\le E}_T<0.5ws),$ the algorithm is aggressive. Although it has searched a large amount of areas that includes the global minimum, it doesn’t have enough energy to move the camera back to the best location explored in the previous stages

The two tunable parameters $K_{est}$ and $K_{sd}$ are selected randomly in the simulation. Those parameters are used in the models to estimate the noise spatial distribution in the environment. Future research can focus on development of adaptive algorithms that tune $K_{est}$ and $K_{sd}$ over iterations based on errors between real measurements and estimations.

In addition, the numerical values of $E_T$ are also picked randomly for testing. In real applications, a fixed $E_T$ should be selected before the operation of this algorithm. The selection of $E_T$ is related to the specific application scenario, size of camera’s operational space, and total energy available $E_{bound}$ at beginning. However, the general thumb of the rule is the following: choose small values of $E_T$ when both $E_{bound}$ and operational space is large to encourage exploring over exploiting; otherwise, choose large $E_T.$

4. Conclusion

In this article, we have developed an algorithm that a camera can explore the workspace in a manufacturing environment and search for a location so that images’ noise is minimized within the space that it can reach. This article also provides detailed development of the camera’s operational space for a specific application. Simulation results show how energy threshold ${\mathit{E}}_{\mathit{T}}$ affects the behavior of the algorithm. A moderate threshold is suggested so that the algorithm can search for enough amount of area but enables to drive the camera back to the minimum with limited energy available. The analysis of ${\mathit{E}}_{\mathit{T}}$ ’s value in different applications is a focus of the future work. Future work may also include adaptive algorithms that update ${\mathit{K}}_{\mathit{e}\mathit{s}\mathit{t}}$ and ${\mathit{K}}_{\mathit{s}\mathit{d}}$ online. Also, orientations of the camera are fixed in this paper. In the future, we would like to develop advanced algorithms that provide not only the optimal of positions but also the orientations inside the space.