Gaze Point Tracking Based on a Robotic Body–Head–Eye Coordination Method

Abstract

When the magnitude of a gaze is too large, human beings change the orientation of their head or body to assist their eyes in tracking targets because saccade alone is insufficient to keep a target at the center region of the retina. To make a robot gaze at targets rapidly and stably (as a human does), it is necessary to design a body–head–eye coordinated motion control strategy. A robot system equipped with eyes and a head is designed in this paper. Gaze point tracking problems are divided into two sub-problems: in situ gaze point tracking and approaching gaze point tracking. In the in situ gaze tracking state, the desired positions of the eye, head and body are calculated on the basis of minimizing resource consumption and maximizing stability. In the approaching gaze point tracking state, the robot is expected to approach the object at a zero angle. In the process of tracking, the three-dimensional (3D) coordinates of the object are obtained by the bionic eye and then converted to the head coordinate system and the mobile robot coordinate system. The desired positions of the head, eyes and body are obtained according to the object’s 3D coordinates. Then, using sophisticated motor control methods, the head, eyes and body are controlled to the desired position. This method avoids the complex process of adjusting control parameters and does not require the design of complex control algorithms. Based on this strategy, in situ gaze point tracking and approaching gaze point tracking experiments are performed by the robot. The experimental results show that body–head–eye coordination gaze point tracking based on the 3D coordinates of an object is feasible. This paper provides a new method that differs from the traditional two-dimensional image-based method for robotic body–head–eye gaze point tracking.

1. Introduction

When the magnitude of a gaze is too large, human beings change the orientation of their head or body to assist their eyes in tracking targets because saccade alone is insufficient to keep a target at the center region of the retina. Studies on body–head–eye coordination gaze point tracking are still rare because the body–head–eye coordination mechanism of humans is prohibitively complex. Multiple researchers have investigated the eye–head coordination mechanism, binocular coordination mechanism and bionic eye movement control. In addition, researchers have validated the eye–head coordination models on eye–head systems. This work is significant for the development of intelligent robots for human–robot interaction. However, most of these methods are based on the principle of neurology, and their further developments and applications may be limited by people’s understanding of human processes. However, binocular coordination based on the 3D coordinates of an object is simple and practical, as verified by our previous paper [1].

When the fixation point transfers greatly, the head and eyes should move in coordination to accurately shift the gaze to the target. Multiple studies have built models of eye–head coordination based on the physiological characteristics of humans. For example, Kardamakis A A et al. [2] researched eye–head movement and gaze shifting. The best balance between eye movement speed and the duration time was sought, and the optimal control method was used to minimize the loss of motion. Freedman E G et al. [3] studied the physiological mechanism of coordinated eye–head movement. However, they did not establish an engineering model. Nakashima et al. [4] proposed a method for gaze prediction that combines information on the head direction with a saliency map. In another study [5], the authors presented a robotic head for social robots to attend to scene saliency with bio-inspired saccadic behaviors. The scene saliency was determined by measuring low-level static scene information, motion, and prior object knowledge. Law et al. [6] described a biologically constrained architecture for developmental learning of eye–head gaze control on an iCub robot. They also identified stages in the development of infant gaze control and proposed a framework of artificial constraints to shape the learning of the robot in a similar manner. Other studies have investigated the mechanisms of eye–head movement for robots and achieved satisfactory performance [7,8].

Some application studies based on coordinated eye–head movement have been carried out in addition to the mechanism research. For example, Kuang et al. [9] developed a method for egocentric distance estimation based on the parallax that emerges during compensatory head–eye movements. This method was tested in a robotic platform equipped with an anthropomorphic neck and two binocular pan–tilt units. Reference [10]’s model is capable of reaching static targets posed at a starting distance of 1.2 m in approximately 250 control steps. Hülse et al. [11] introduced a computational framework that integrates robotic active vision and reaching. Essential elements of this framework are sensorimotor mappings that link three different computational domains relating to visual data, gaze control and reaching.

Some researchers have applied the combined movement of the eyes, head and body in mobile robots. In one study [12], large reorientations of the line of sight, involving combined rotations of the eyes, head, trunk and lower extremities, were executed either as fast single-step or as slow multiple-step gaze transfers. Daye et al. [13] proposed a novel approach for the control of linked systems with feedback loops for each part. The proximal parts had separate goals. In addition, an efficient and robust human tracker for a humanoid robot was implemented and experimentally evaluated in another study [14].

On the one hand, human eyes can obtain three-dimensional (3D) information from objects. This 3D information is useful for humans to make decisions. Human can shift their gaze stably and approach a target using the 3D information of the object. When the human gaze shifts to a moving target, the eyes first rotate to the target, and then the head and even the body rotate if the target leaves the sight of the eyes [15]. Therefore, the eyes, head and body move in coordination to shift the gaze to the target with minimal energy expenditure. On the other hand, when a human approaches a target, the eyes, head and body rotate to face the target and the body moves toward the target. The two movements are typically executed with the eyes, head and body acting in conjunction. A robot that can execute these two functions will be more intelligent. Such a robot would need to exploit the smooth pursuit of eyes [16], coordinated eye–head movement [17], target detection and the combined movement of the eyes, head and robot body to carry out these two functions. Studies have achieved many positive results in these aspects.

Mobile robots can track and locate objects according to 3D information. Some special cameras such as deep cameras and 3D lasers have been applied to obtain the 3D information of the environment and target. In one study [18], a nonholonomic under-actuated robot with bounded control was described that travels within a 3D region. A single sensor provided the value of an unknown scalar field at the current location of the robot. Nefti-Meziani S et al. [19] presented the implementation of a stereo-vision system integrated in a humanoid robot. The low cost of the vision system is one of the main aims, avoiding expensive investment in hardware when used in robotics for 3D perception. Namavari A et al. [20] presented an automatic system for the gauging and digitalization of 3D indoor environments. The configuration consisted of an autonomous mobile robot, a reliable 3D laser rangefinder and three elaborated software modules.

The main forms of motion of bionic eyes include saccade [1], smooth pursuit, vergence [21], vestibule–ocular reflex (VOR) [22] and optokinetic reflex (OKR) [23]. Saccade and smooth pursuit are the two most important functions of the human eye. Saccade is used to move eyes voluntarily from one point to another by rapid jumping, while smooth pursuit can be applied to track moving targets. In addition, binocular coordination and eye–head coordination are of high importance to realize object tracking and gaze control.

It is of great significance for robots to be able change their fixation point quickly. In control models, the saccade control system should be implemented using a position servo controller to change and keep the target at the center region of the retina with minimum time consumption. Researchers have been studying the implementation of saccade on robots over the last twenty years. For example, in 1997, Bruske et al. [24] incorporated saccadic control into a binocular vision system by using the feedback error learning (FEL) strategy. In 2013, Wang et al. [25] designed an active vision system that can imitate saccade and other eye movements. The saccadic movements were implemented with an open-loop controller, which ensures faster saccadic eye movements than a closed-loop controller can accommodate. In 2015, Antonelli et al. [26] achieved saccadic movements on a robot head by using a model called recurrent architecture (RA). In this model, the cerebellum is regarded as an adaptive element used to learn an internal model, while the brainstem is regarded as a fixed-inverse model. The experimental results on the robot showed that this model is more accurate and less sensitive to the choice of the inverse model relative to the FEL model.

The smooth pursuit system acts as a velocity servo controller to rotate eyes at the same angular rate as the target while keeping them oriented toward the desired position or in the desired region. In Robinson’s model of smooth pursuit [27], the input is the velocity of the target’s image across the retina. The velocity deviation is taken as the major stimulus to pursue and is transformed into an eye velocity command. Based on Robinson’s model, Brown [28] added a smooth predictor to accommodate time delays. Deno et al. [29] applied a dynamic neural network, which unified two apparently disparate models of smooth pursuit and dynamic element organization to the smooth pursuit system. The dynamic neural network can compensate for delays from the sensory input to the motor response. Lunghi et al. [30] introduced a neural adaptive predictor that was previously trained to accomplish smooth pursuit. This model can explain a human’s ability to compensate for the 130 ms physiological delay when they follow external targets with their eyes. Lee et al. [31] applied a bilateral OCS model on a robot head and established rudimentary prediction mechanisms for both slow and fast phases. Avni et al. [32] presented a framework for visual scanning and target tracking with a set of independent pan–tilt cameras based on model predictive control (MPC). In another study [33], the authors implemented smooth pursuit eye movement with prediction and learning in addition to solving the problem of time delays in the visual pathways. In addition, some saccade and smooth pursuit models have been validated on bionic eye systems [34,35,36,37]. Santini F et al. [34] showed that the oculomotor strategies by which humans scan visual scenes produce parallaxes that provide an accurate estimation of distance. Other studies have realized the coordinated control of eye and arm movements through configuration and training [35]. Song Y et al. [36] proposed a binocular control model, which was derived from a neural pathway, for smooth pursuit. In their smooth pursuit experiments, the maximum retinal error was less than 2.2°, which is sufficient to keep a target in the field of view accurately. An autonomous mobile manipulation system was developed in the form of a modified image-based visual servo (IBVS) controller in a study [37].

The above-mentioned work is significant for the development of intelligent robots. However, there are some shortcomings. First, most of the existing methods are based on the principle of neurology, and further developments and applications may be limited by people’s understanding aimed at humans. Second, only two-dimensional (2D) image information is applied when gaze shifts to targets are implemented, while 3D information is ignored. Third, the studies of smooth pursuit [16], eye–head coordination [17], gaze shift and approach are independent and have not been integrated. Fourth, bionic eyes are different from human eyes; for example, some of them are two eyes that are fixed without movement or move with only 1 DOF, whereas some of them use special cameras or a single camera. Fifth, the movements of bionic eyes and heads are performed separately, without coordination.

To overcome the shortcomings mentioned above to a certain extent, a novel control method that implements the gaze shift and approach of a robot according to 3D coordinates is proposed in this paper. A robot system equipped with bionic eyes, a head and a mobile robot is designed to help nurses deliver medicine in hospitals. In this system, both the pan and each eye have 2 DOF (namely, tilt and pan [38]), and the mobile robot can rotate and move forward over the ground. When the robot gaze shifts to the target, the 3D coordinates of the target are acquired by the bionic eyes and transferred to the eye coordination system, head coordination system and robot coordination system. The desired position of the eye, head and robot are calculated based on the 3D information of the target. Then, the eye, head and mobile robot are driven to the desired positions. When the robot approaches the target, the eye, head and mobile robot first rotate to the target and then move to the target. This method allows the robot to achieve the above-mentioned functions with minimal resource consumption and can separate the control of the eye, head and mobile robot, which can improve the interactions between robots, human beings and the environment.

The rest of the paper is organized as follows. In Section 2, the robot system platform is introduced, and the control system is presented. In Section 3, the desired position is discussed and calculated. Robot pose control is described in Section 4. The experimental results are given and discussed in Section 5; finally, conclusions are drawn in Section 6.

2. Platform and Control System

To study the gaze point tracking of the robot, this paper designs a robot experiment platform including the eye–head subsystem and the mobile robot subsystem.

2.1. Robot Platform

The physical object of the robot is shown in Figure 1. With the mobile robot as a carrier, a head with two degrees of freedom is fixed on the mobile robot, and the horizontal and vertical rotations of the head are controlled by M_hu and M_hd, respectively. The bionic eye system is fixed to the head. The mobile robot is driven by two wheels, each of which is individually controlled by a servo motor. The angle and displacement of the robot platform can be determined by controlling the distance and speed of each wheel’s movement. The output shaft of each stepper motor of the head and eye is equipped with a rotary encoder to detect the position of the motor. Using the frequency multiplication technique, the resolution of the rotary encoder is 0.036°. The purpose of using a rotary encoder is to prevent the effects of lost motor motion on the 3D coordinate calculations. The movement of each motor is limited by a limit switch. The initial positioning of the eye system is based on the visual positioning plate [39].

The robot system includes two eyes and one mobile robot. To simulate the eyes and the head, six DOFs are designed in this system. The left eye’s pan and tilt are controlled by motors M_lu and M_ld, respectively. The right eye’s pan and tilt are controlled by motors M_ru and M_rd, respectively. The head’s pan and tilt are controlled by motors M_hu and M_hd, respectively. The mobile robot has two driving wheels and can perform rotation and forward movement. When the mobile robot needs to rotate, two wheels are set to turn the same amount in different directions. When the mobile robot needs to go forward, two wheels are set to turn the same amount in the same direction.

A diagram of the robot system’s organization is shown in Figure 2. The host computer and the mobile robot motion controller, the head motion controller and the eye motion controller all communicate through the serial ports. For satisfactory communication quality and stability, the baud rate of serial communication is 9600 bps. The camera communicates with the host computer via a GigE Gigabit Network. The camera’s native resolution is 1600 × 1200 pixels. To increase the calculation speed, the system uses an image downsampled to 400 × 300 pixels.

2.2. Control System

Figure 3 shows the control block diagram of the gaze point tracking of the mobile robot. First, based on binocular stereo-vision perception, the binocular pose and the left and right images are used to calculate the 3D coordinates of the target [40], and the coordinates of the target in the eye coordinate system are converted to the head and mobile robot coordinate system. Then, the desired poses of the eyes, head and mobile robot are calculated according to the 3D coordinates of the target. Finally, according to the desired pose, the motor is controlled to move to the desired position, and the change in the position of the motor is converted into changes in the eyes, head and mobile robot.

The tracking and approaching motion control problem based on the target 3D coordinates [1] is equivalent to solving the index J minimization problem of Equation (1), where f_i is the current state vector of the joint pose of the eye, head and mobile robot and f_q is the desired state vector:

$$J=‖{\mathit{f}}_i-{\mathit{f}}_{\mathrm{q}}‖$$

(1)

where J is the indicator function.

Figure 4a shows the definition of each coordinate system of the robot. The coordinate system of the eye is O_eX_eY_eZ_e, which coincides with the left motion module’s base coordinate system at the initial position. The head coordinate system is O_hX_hY_hZ_h, and the coordinates P_h (x_h, y_h, z_h) of the point P in the head coordinate system can be calculated using the coordinates P_e (x_e, y_e, z_e) in the eye coordinate system. The definitions of d_x and d_y are shown in Figure 4b. The robot coordinate system O_wX_wY_wZ_w coincides with the head coordinate system of the initial position. In the bionic eye system, the axis of rotation of the robot approximately coincides with Y_w.

Figure 4b,c show the definition of each system parameter. ^lθ_p and ^lθ_t are the pan and tilt of the left eye, respectively. ^rθ_p and ^rθ_t are the pan and tilt of the right eye, respectively. ^hθ_p and ^hθ_t are the pan and tilt of the head, respectively. The angle of the robot that rotates around the Y_w axis is ^wθ_p. The robot can not only rotate around Y_w but can also shift in the X_wO_wZ_w plane. When the robot moves, the robot coordinate system at time i is the base coordinate system, and the position of the robot at time i + 1 relative to the base coordinate system is ^wP_m (^wx_m, ^wz_m). When the robot performs gaze point tracking or approaches the target, the 3D coordinates of the target are first calculated at time i, and then the desired posture f_q of each part of the robot at time i + 1 is calculated according to the 3D coordinates of the target. When the current pose f_i of the robot system is equal to the desired pose, the robot maintains the current pose; when not equal, the system controls the various parts of the robot to move to the desired pose. The current pose vector of the robot system is f_i = (^wx_mi, ^wz_mi, ^wθ_pi, ^hθ_pi, ^hθ_ti, ^lθ_pi, ^lθ_ti, ^rθ_pi, ^rθ_ti), and the desired pose is f_q = (^wx_mq, ^wz_mq, ^wθ_pq, ^hθ_pq, ^hθ_tq, ^lθ_pq, ^lθ_tq, ^rθ_pq, ^rθ_tq). When performing in situ gaze point tracking, the robot performs only pure rotation and does not move forward. When the robot approaches the target, it first turns to the target and then moves straight toward the target. Therefore, the definition of f_q in the two tasks is different. Let ^gf_q be the desired pose when the gaze point is tracked and ^af_q be the desired pose of the robot when approaching the target.

After analyzing the control system, we found that the most important step in solving this control problem is to determine the desired pose.

3. Desired Pose Calculation

When performing in situ gaze point tracking, the robot performs only pure rotation and does not move forward. When the robot approaches the target, it first turns to the target and then moves straight toward the target. Therefore, the calculation of the desired pose can be divided into two sub-problems: (1) desired pose calculation for in situ gaze point tracking and (2) desired pose calculation for approaching gaze point tracking.

The optimal observation position is used for the accurate acquisition of 3D coordinates. The 3D coordinate accuracy is related to the baseline, time difference and image distortion. In the bionic eye platform, the baseline is changed with the changes in the cameras’ positions because the optical center is not coincident with the center of rotation. The 3D coordinate error of the target is smaller when the baseline of the two cameras is longer. Therefore, it is necessary to keep the baseline unchanged. On the other hand, there is a time difference caused by unstick synchronization between image acquisition and camera position acquisition. In addition, it is necessary to keep the target in the center areas of the two camera images to obtain accurate 3D coordinates of the target.

3.1. Optimal Observation Position of Eyes

In the desired pose of the robot, the most important aspect is the expected pose of the bionic eye [40]. Following the definition of this parameter, the calculation of the desired pose of the robot system is greatly simplified; thus, we present an engineering definition here of the desired pose of the bionic eye.

As shown in Figure 5, ^lm_i (^lu_i, ^lv_i) and ^r(^ru_i, ^rv_i) are the image coordinates of point ^eP in the camera at time i. ^lm_o and ^rm_o are the image centers of the left and right cameras, respectively. ^lP is the vertical point of ^eP along the line ^lO_c^lZ_c, and ^rP is the vertical point of ^eP along the line ^rO_c^rZ_c. ^l∆m is the distance between ^lm and ^lm_o. ^r∆m is the distance between ^rm and ^rm_o. D_b is the baseline length. The pan angles of the left and right cameras in the optimal observation position are ^lθ_p and ^rθ_p, respectively. The tilt angles of the left and right cameras in the optimal observation position are ^lθ_t and ^rθ_t, respectively. P_ob (^lθ_p, ^lθ_t, ^rθ_p, ^rθ_t) is the optimal observation position.

When the two eyeballs of the bionic eye move relative to each other, the 3D coordinates of the target obtained by the bionic eye produces a large error. To characterize this error, we give a detailed analysis of its origins in Appendix A. Through analysis, we obtain the following conclusions to reduce the measurement error of the bionic eye:

(1) Make the length of D_b long enough, and maintain as much length as possible during the movement;

(2) Try to observe the target closer to the target so that the depth error is as small as possible;

(3) During the movement of the bionic eye, control the two cameras so that they move at the same angular velocity;

(4) Try to keep the target symmetrical, and make ^lΔm and ^rΔm as equal as possible in the left and right camera images.

Based on these four methods, the motion strategy of the motor is designed, and the measurement accuracy of the target’s 3D information can be effectively improved.

According to the conclusion, we can define a definition of the optimal observed pose of the bionic eye to reduce the measurement error.

The optimal observation position needed to meet the conditions is listed in Equation (2). When the target is very close to the eyes, the target’s optimal observation position cannot be obtained because the image position of the target can be kept at the image center region. It is challenging to obtain the optimal solution of the observation position based on Equation (12). However, a suboptimal solution can be obtained by using a simplified calculation method. First, ^lθ_t and ^rθ_t are calculated in the case that ^lθ_t and ^rθ_t are equal to zero; then, ^lθ_t and ^rθ_t are calculated while ^lθ_t and ^rθ_t are kept equal to the calculated value. Trial-and-error methods can be used to obtain the optimal solution when the suboptimal solution is obtained.

$$\left\{\begin{array}{l}{}^{\mathrm{l}}{\theta }_{\mathrm{pq}}={}^{\mathrm{r}}{\theta }_{\mathrm{pq}}={\theta }_{\mathrm{p}}\\ {}^{\mathrm{l}}{\theta }_{\mathrm{tq}}={}^{\mathrm{r}}{\theta }_{\mathrm{tq}}={\theta }_{\mathrm{t}}\\ {}^{\mathrm{l}}\mathsf{\Delta }\mathit{m}={-}^{\mathrm{r}}\mathsf{\Delta }\mathit{m}\end{array}\right.$$

(2)

where

$${}^{\mathrm{l}}\mathsf{\Delta }\mathit{m}=\left(\begin{array}{l}{}^{\mathrm{l}}\mathsf{\Delta }u\\ {}^{\mathrm{l}}\mathsf{\Delta }v\end{array}\right)=\left(\begin{array}{c}{}^{\mathrm{l}}{u}_i-{}^{\mathrm{l}}{u}_0\\ {}^{\mathrm{l}}{v}_i-{}^{\mathrm{l}}{v}_0\end{array}\right)$$

(3)

$${}^{\mathrm{r}}\mathsf{\Delta }\mathit{m}=\left(\begin{array}{l}{}^{\mathrm{r}}\mathsf{\Delta }u\\ {}^{\mathrm{r}}\mathsf{\Delta }v\end{array}\right)=\left(\begin{array}{c}{}^{\mathrm{r}}{u}_i-{}^{\mathrm{r}}{u}_0\\ {}^{\mathrm{r}}{v}_i-{}^{\mathrm{r}}{v}_0\end{array}\right)$$

(4)

3.2. Desired Pose Calculation for In Situ Gaze Point Tracking

When the range of target motion is large and the desired posture of the eyeball exceeds its reachable posture, the head and mobile robot move to keep the target in the center region of the image. In robotic systems, eye movements tend to consume the least amount of resources and do not have much impact on the stability of the head and mobile robot during exercise. Head rotation consumes more resources than the eyeball but consumes fewer resources than trunk rotation. At the same time, the rotation of the head affects the stability of the eyeball but does not have much impact on the stability of the trunk. Mobile robot rotation consumes the most resources and has a large impact on the stability of the head and eyeball. When tracking the target, one needs only to keep the target in the center region of the binocular image. Therefore, when performing gaze point tracking, the movement mechanism of the head, eyes and mobile robot are designed with the principle of minimal resource consumption and maximum system stability. When the eyeball can perceive the 3D coordinates of the target in the reachable and optimal viewing posture, only the eye is rotated; otherwise, the head is rotated. The head also has an attainable range of poses. When the desired pose exceeds this range, the mobile robot needs to be turned so that the bionic eye always perceives the 3D coordinates of the target in the optimal viewing position. Let ^hγ_p and ^hγ_t be the angles between the head and the gaze point in the X_hO_hZ_h and Y_hO_hZ_h planes, respectively. The range of binocular rotation in the horizontal direction is [−^eθ_pmax, ^eθ_pmax], and the range of binocular rotation in the vertical direction is [−^eθ_tmax, ^eθ_tmax]. The range of head rotation in the horizontal direction is [−^hθ_pmax, ^hθ_pmax], and the range of head rotation in the vertical direction is [−^hθ_tmax, ^hθ_tmax]. For the convenience of calculation, the angles between the head and the fixation point in the horizontal direction and the vertical direction are designated as [−^hγ_pmax, ^hγ_pmax] and [−^hγ_tmax, ^hγ_tmax], respectively. When the angle between the head and the target exceeds a set threshold, the head needs to be rotated to the ${}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }$ and ${}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }$ positions in the horizontal and vertical directions, respectively. When ${}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }$ exceeds the angle that the head can attain, the angle at which the mobile robot needs to be compensated is ^wθ_p. In the in situ gaze point tracking task, the cart does not need to translate in the X_wO_wZ_w plane, so x_w = 0, and z_w = 0. Furthermore, according to the definition of the optimal observation pose of the bionic eye, the conditions that ^gf_q should satisfy are

$${}^{\mathrm{g}}{\mathit{f}}_{\mathrm{q}}=\left(\begin{array}{l}{}^{\mathrm{w}}{x}_{\mathrm{mq}}=0\\ {}^{\mathrm{w}}{z}_{\mathrm{mq}}=0\\ {}^w{\theta }_{\mathrm{pq}}=\{\theta \left|\right. |\theta |\le 2\mathsf{\pi },{}^{\mathrm{h}}{\theta }_{\mathrm{pq}}+\theta ={}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }\}\\ {}^{\mathrm{h}}{\theta }_{\mathrm{pq}}=\{\theta \left|\right. |\theta |\le {}^{\mathrm{h}}{\theta }_{\mathrm{pmax}},{|}^{\mathrm{h}}{\gamma }_{\mathrm{p}}|{\le }^{\mathrm{h}}{\gamma }_{\mathrm{pmax}}\}\\ {}^{\mathrm{h}}{\theta }_{\mathrm{tq}}=\{\theta \left|\right. |\theta |\le {}^{\mathrm{h}}{\theta }_{\mathrm{tmax}},{|}^{\mathrm{h}}{\gamma }_{\mathrm{t}}|{\le }^{\mathrm{h}}{\gamma }_{\mathrm{tmax}}\}\\ {}^{\mathrm{l}}{\theta }_{\mathrm{pq}}={}^{\mathrm{r}}{\theta }_{\mathrm{pq}}=\{\theta \left|\right. |\theta |\le {}^{\mathrm{e}}{\theta }_{\mathrm{pmax}},{}^{\mathrm{l}}{\mathsf{\Delta }m}_{\mathrm{l}}=-\mathsf{\Delta }{m}_{\mathrm{r}}\}\\ {}^{\mathrm{l}}{\theta }_{\mathrm{tq}}={}^{\mathrm{r}}{\theta }_{\mathrm{tq}}=\{\theta \left|\right. |\theta |\le {}^{\mathrm{e}}{\theta }_{\mathrm{tmax}},\mathsf{\Delta }{m}_{\mathrm{l}}=-\mathsf{\Delta }{m}_{\mathrm{r}}\}\end{array}\right)$$

(5)

The desired pose needs to be calculated based on the 3D coordinates of the target. Therefore, to obtain the desired pose, it is necessary to acquire the 3D coordinates of the target according to the current pose of the robot.

3.2.1. Three-Dimensional Coordinate Calculation

The mechanical structure and coordinate settings of the system are shown in Figure 6a. The principle of binocular stereoscopic 3D perception is shown in Figure 6b. E is the eye coordinate system, E_l is the left motion module’s end coordinate system, E_r is the right motion module’s end coordinate system, B_l is the left motion module’s base coordinate system, B_r is the right motion module’s base coordinate system, C_l is the left camera coordinate system and C_r is the right camera coordinate system. In the initial position, E_l coincides with B_l, and E_r overlaps with B_r. When the binocular system moves, the base coordinate system does not change. ^lT represents the transformation matrix of the eye coordinate system E to the left motion module’s base coordinate system B_l, ^rT represents the transformation matrix of E to B_r, ^lT_e represents the transformation matrix of B_l to E_l, ^rT_e represents the transformation matrix of B_r to E_r and ^lT_m represents the leftward motion. The module end coordinate system corresponds to the transformation matrix of the left camera coordinate system, and ^rT_m represents the transformation matrix of the right motion module’s end coordinate system to the right camera coordinate system. ^lT_r represents the transformation matrix of the right camera coordinate system to the left camera coordinate system at the initial position.

The origin ^lO_c of C_l lies at the optical center of the left camera, the ^lZ_c axis points in the direction of the object parallel to the optical axis of the camera, the ^lX_c axis points horizontally to the right along the image plane and the ^lY_c axis points vertically downward along the image plane. The origin ^rO_c of C_r lies at the optical center of the right camera, ^rZ_c is aligned with the direction of the object parallel to the optical axis of the camera, ^rX_c points horizontally to the right along the image plane and ^rY_c points vertically downward along the image plane. E_l’s origin ^lO_e is set at the intersection of the two rotation axes of the left motion module, ^lZ_e is perpendicular to the two rotation axes and points to the front of the platform, ^lX_e coincides with the vertical rotation axis and ^lY_e coincides with the horizontal rotation axis. Similarly, the origin ^rO_e of the coordinate system E_r is set at the intersection of the two rotation axes of the right motion module, ^rZ_e is perpendicular to the two rotation axes and points toward the front of the platform, ^rX_e coincides with the vertical rotation axis and ^rY_e coincides with the horizontal rotation axis.

The left motion module’s base coordinates system B_l coincides with the eye coordinate system E; thus, ^lT consists of an identity matrix. To calculate the 3D coordinates of the feature points in real time from the camera pose, it is necessary to calculate ^rT. At the initial position of the system, the external parameters ^lT_r of the left and right cameras are calibrated offline, as are the hand–eye parameters of the left–right motion module to the camera coordinate system.

When the system is in its initial configuration, the coordinates of point P in the eye coordinate system are P_e (x_e, y_e, z_e). Its coordinates in B_l are ^lP_e (^lx_e, ^ly_e, ^lz_e), and its coordinates ^lP_c (^lx_c, ^ly_c, ^lz_c) in C_l are

$${}^{\mathrm{l}}{\mathit{P}}_{\mathrm{c}}={}^{\mathrm{l}}{\mathit{T}}_{\mathrm{m}}^{-1}{\mathit{P}}_{\mathrm{e}}$$

(6)

The coordinates ^rP_e (^rx_e, ^ry_e, ^rz_e) of point P in B_r are

$${}^{\mathrm{r}}{\mathit{P}}_{\mathrm{e}}={}^{\mathrm{r}}\mathit{T}{\mathit{P}}_{\mathrm{e}}$$

(7)

The coordinates ^rP_c (^rx_c, ^ry_c, ^rz_c) of point P in C_r are

$${}^{\mathrm{r}}{\mathit{P}}_{\mathrm{c}}={}^{\mathrm{r}}{\mathit{T}}_{\mathrm{m}}^{-1}{}^{\mathrm{r}}\mathit{T}{\mathit{P}}_{\mathrm{e}}$$

(8)

The point in C_r is transformed into C_l:

$${}^{\mathrm{l}}{\mathit{P}}_{\mathrm{c}}={}^{\mathrm{l}}{\mathit{T}}_{\mathrm{r}}{}^{\mathrm{r}}{\mathit{T}}_{\mathrm{m}}^{-1}{}^{\mathrm{r}}\mathit{T}{\mathit{P}}_{\mathrm{e}}$$

(9)

Based on the Equations (6) and (9), ^rT is available:

$${}^{\mathrm{r}}\mathit{T}={}^{\mathrm{r}}\mathit{T}{}_{\mathrm{m}}{}^{\mathrm{l}}{\mathit{T}}_{\mathrm{r}}^{-1}{}^{\mathrm{l}}{\mathit{T}}_{\mathrm{m}}^{-1}$$

(10)

During the movement of the system, when the left motion module rotates by ^lθ_p and ^lθ_t in the horizontal and vertical directions, respectively, the transformation relationship between B_l and E_l is

$${}^{\mathrm{l}}{\mathit{T}}_{\mathrm{e}}=\left(\begin{array}{cc}\mathrm{Rot}(Y,{}^{\mathrm{l}}{\theta }_{\mathrm{p}})\mathrm{Rot}(X,{}^{\mathrm{l}}{\theta }_{\mathrm{t}})& 0\\ 0& 1\end{array}\right)$$

(11)

The coordinates of point P in C_l are

$${}^{\mathrm{l}}{\mathit{P}}_{\mathrm{e}}={}^{\mathrm{l}}{\mathit{T}}_{\mathrm{m}}^{-1}{}^{\mathrm{l}}{\mathit{T}}_{\mathrm{e}}{\mathit{P}}_{\mathrm{w}}={}^{\mathrm{l}}{\mathit{T}}_{\mathrm{d}}{\mathit{P}}_{\mathrm{e}}$$

(12)

Assume that

$${}^{\mathrm{l}}{\mathit{T}}_{\mathrm{d}}=\left(\begin{array}{cccc}{}^{\mathrm{l}}{n}_x& {}^{\mathrm{l}}{o}_x& {}^{\mathrm{l}}{a}_x& {}^{\mathrm{l}}{p}_x\\ {}^{\mathrm{l}}{n}_y& {}^{\mathrm{l}}{o}_y& {}^{\mathrm{l}}{a}_y& {}^{\mathrm{l}}{p}_y\\ {}^{\mathrm{l}}{n}_z& {}^{\mathrm{l}}{o}_z& {}^{\mathrm{l}}{a}_z& {}^{\mathrm{l}}{p}_z\\ 0& 0& 0& 1\end{array}\right)$$

(13)

The point ^lP_1c (^lx_1c, ^ly_1c) at which line P^lO_c intersects ^lZ_c = 1 is

$$\left(\begin{array}{c}{}^{\mathrm{l}}{x}_{1\mathrm{c}}\\ {}^{\mathrm{l}}{y}_{1\mathrm{c}}\end{array}\right)=\left(\begin{array}{c}\frac{{}^{\mathrm{l}}{n}_x{x}_{\mathrm{e}}+{}^{\mathrm{l}}{o}_x{y}_{\mathrm{e}}+{}^{\mathrm{l}}{a}_x{z}_{\mathrm{e}}+{}^{\mathrm{l}}{p}_x}{{}^{\mathrm{l}}{n}_z{x}_{\mathrm{e}}+{}^{\mathrm{l}}{o}_z{y}_{\mathrm{e}}+{}^{\mathrm{l}}{a}_z{z}_{\mathrm{e}}+{}^{\mathrm{l}}{p}_z}\\ \frac{{}^{\mathrm{l}}{n}_y{x}_{\mathrm{e}}+{}^{\mathrm{l}}{o}_y{y}_{\mathrm{e}}+{}^{\mathrm{l}}{a}_y{z}_{\mathrm{e}}+{}^{\mathrm{l}}{p}_y}{{}^{\mathrm{l}}{n}_z{x}_{\mathrm{e}}+{}^{\mathrm{l}}{o}_z{y}_{\mathrm{e}}+{}^{\mathrm{l}}{a}_z{z}_{\mathrm{e}}+{}^{\mathrm{l}}{p}_z}\end{array}\right)$$

(14)

The image coordinates of ^lP_1c in the left camera are m_l (u_l, v_l), (^lx_1c, ^ly_1c) and (u_l, v_l) and can be converted by the parameters of the camera. According to the camera’s internal parameter model, the following can be obtained:

$$\left(\begin{array}{c}{}^{\mathrm{l}}{x}_{1\mathrm{c}}\\ {}^{\mathrm{l}}{y}_{1\mathrm{c}}\\ 1\end{array}\right)={}^{\mathrm{l}}{\mathit{M}}_{\mathrm{in}}^{-1}\left(\begin{array}{c}{u}_{\mathrm{l}}\\ v_{\mathrm{l}}\\ 1\end{array}\right)$$

(15)

where ^lM_in is the internal parameter matrix of the left camera. The value of (^lx_1c, ^ly_1c) can be obtained by the image coordinates of ^lP_1c, and the parameters of the left camera can be obtained by substituting (15) into (14):

$$\left\{\begin{array}{l}({}^{\mathrm{l}}{n}_x-{}^{\mathrm{l}}{x}_{1\mathrm{c}}{}^{\mathrm{l}}{n}_z)x_{\mathrm{e}}+({}^{\mathrm{l}}{o}_x-{}^{\mathrm{l}}{x}_{1\mathrm{c}}{}^{\mathrm{l}}{o}_z)y_{\mathrm{e}}+({}^{\mathrm{l}}{a}_x-{}^{\mathrm{l}}{x}_{1\mathrm{c}}{}^{\mathrm{l}}{a}_z)z_{\mathrm{e}}+{}^{\mathrm{l}}{p}_x-{}^{\mathrm{l}}{x}_{1\mathrm{c}}{}^{\mathrm{l}}{p}_z=0\\ ({}^{\mathrm{l}}{n}_y-{}^{\mathrm{l}}{y}_{1\mathrm{c}}{}^{\mathrm{l}}{n}_z)x_{\mathrm{e}}+({}^{\mathrm{l}}{o}_y-{}^{\mathrm{l}}{y}_{1\mathrm{c}}{}^{\mathrm{l}}{o}_z)y_{\mathrm{e}}+({}^{\mathrm{l}}{a}_y-{}^{\mathrm{l}}{y}_{1\mathrm{c}}{}^{\mathrm{l}}{a}_z)z_{\mathrm{e}}+{}^{\mathrm{l}}{p}_y-{}^{\mathrm{l}}{y}_{1\mathrm{c}}{}^{\mathrm{l}}{p}_z=0\end{array}\right.$$

(16)

During the motion of the system, when the right motion module rotates through ^rθ_p and ^rθ_t in the horizontal and vertical directions, respectively, the transformation relationship between B_r and E_r is

$${}^{\mathrm{r}}\mathit{T}{}_{\mathrm{e}}=\left(\begin{array}{cc}\mathrm{Rot}(Y,{}^{\mathrm{r}}{\theta }_{\mathrm{p}})\mathrm{Rot}(X,{}^{\mathrm{r}}{\theta }_{\mathrm{t}})& 0\\ 0& 1\end{array}\right)$$

(17)

The coordinates of point P in C_r are

$${}^{\mathrm{r}}{\mathit{P}}_{\mathrm{e}}={}^{\mathrm{r}}{\mathit{T}}_{\mathrm{m}}^{-1}{}^{\mathrm{r}}\mathit{T}{}_{\mathrm{e}}{}^{\mathrm{r}}\mathit{T}{\mathit{P}}_{\mathrm{e}}={}^{\mathrm{r}}\mathit{T}{}_{\mathrm{d}}{\mathit{P}}_{\mathrm{e}}$$

(18)

Assume that

$${}^{\mathrm{r}}\mathit{T}{}_{\mathrm{d}}=\left(\begin{array}{cccc}{}^{\mathrm{r}}{n}_x& {}^{\mathrm{r}}{o}_x& {}^{\mathrm{r}}{a}_x& {}^{\mathrm{r}}{p}_x\\ {}^{\mathrm{r}}{n}_y& {}^{\mathrm{r}}{o}_y& {}^{\mathrm{r}}{a}_y& {}^{\mathrm{r}}{p}_y\\ {}^{\mathrm{r}}{n}_z& {}^{\mathrm{r}}{o}_z& {}^{\mathrm{r}}{a}_z& {}^{\mathrm{r}}{p}_z\\ 0& 0& 0& 1\end{array}\right)$$

(19)

The point ^lP_1c (^rx_1c, ^ry_1c) at which line P^rO_c intersects ^rZ_c = 1 is

$$\left(\begin{array}{c}{}^{\mathrm{r}}{x}_{1\mathrm{c}}\\ {}^{\mathrm{r}}{y}_{1\mathrm{c}}\end{array}\right)=\left(\begin{array}{c}\frac{{}^{\mathrm{r}}{n}_x{x}_{\mathrm{e}}+{}^{\mathrm{r}}{o}_x{y}_{\mathrm{e}}+{}^{\mathrm{r}}{a}_x{z}_{\mathrm{e}}+{}^{\mathrm{r}}{p}_x}{{}^{\mathrm{r}}{n}_z{x}_{\mathrm{e}}+{}^{\mathrm{r}}{o}_z{y}_{\mathrm{e}}+{}^{\mathrm{r}}{a}_z{z}_{\mathrm{e}}+{}^{\mathrm{r}}{p}_z}\\ \frac{{}^{\mathrm{r}}{n}_y{x}_{\mathrm{e}}+{}^{\mathrm{r}}{o}_y{y}_{\mathrm{e}}+{}^{\mathrm{r}}{a}_y{z}_{\mathrm{e}}+{}^{\mathrm{r}}{p}_y}{{}^{\mathrm{r}}{n}_z{x}_{\mathrm{e}}+{}^{\mathrm{r}}{o}_z{y}_{\mathrm{e}}+{}^{\mathrm{r}}{a}_z{z}_{\mathrm{e}}+{}^{\mathrm{r}}{p}_z}\end{array}\right)$$

(20)

The image coordinates of ^rP_1c in the camera, namely, m_r (u_r, v_r), (^rx_1c, ^ry_1c) and (u_r, v_r), can be converted using the parameters of the camera. According to the camera’s internal parameter model, the following can be obtained:

$$\left(\begin{array}{c}{}^{\mathrm{r}}{x}_{1\mathrm{c}}\\ {}^{\mathrm{r}}{y}_{1\mathrm{c}}\\ 1\end{array}\right)={}^{\mathrm{l}}{\mathit{M}}_{\mathrm{in}}^{-1}\left(\begin{array}{c}{u}_{\mathrm{r}}\\ v_{\mathrm{r}}\\ 1\end{array}\right)$$

(21)

where ^rM_in is the inner parameter matrix of the right camera. The value of (^rx_1c, ^ry_1c) can be obtained by the image coordinates of ^rP_1c and the parameters in the camera, and the following can be obtained by substituting (21) into (20):

$$\left\{\begin{array}{l}({}^{\mathrm{r}}{n}_x-{}^{\mathrm{r}}{x}_{1\mathrm{c}}{}^{\mathrm{r}}{n}_z)x_{\mathrm{e}}+({}^{\mathrm{r}}{o}_x-{}^{\mathrm{r}}{x}_{1\mathrm{c}}{}^{\mathrm{r}}{o}_z)y_{\mathrm{e}}+({}^{\mathrm{r}}{a}_x-{}^{\mathrm{r}}{x}_{1\mathrm{c}}{}^{\mathrm{r}}{a}_z)z_{\mathrm{e}}+{}^{\mathrm{r}}{p}_x-{}^{\mathrm{r}}{x}_{1\mathrm{c}}{}^{\mathrm{r}}{p}_z=0\\ ({}^{\mathrm{r}}{n}_y-{}^{\mathrm{r}}{y}_{1\mathrm{c}}{}^{\mathrm{r}}{n}_z)x_{\mathrm{e}}+({}^{\mathrm{r}}{o}_y-{}^{\mathrm{r}}{y}_{1\mathrm{c}}{}^{\mathrm{r}}{o}_z)y_{\mathrm{e}}+({}^{\mathrm{r}}{a}_y-{}^{\mathrm{r}}{y}_{1\mathrm{c}}{}^{\mathrm{r}}{a}_z)z_{\mathrm{e}}+{}^{\mathrm{r}}{p}_y-{}^{\mathrm{r}}{y}_{1\mathrm{c}}{}^{\mathrm{r}}{p}_z=0\end{array}\right.$$

(22)

Four equations can be obtained from Equations (16) and (22) for x_e, y_e and z_e, and the 3D coordinates of point P_e can be calculated by the least squares method.

The 3D coordinates P_h (x_h, y_h, z_h) in the head coordinate system can be obtained by Equation (23). d_x and d_y are illustrated in Figure 4.

$$\left(\begin{array}{c}{x}_{\mathrm{h}}\\ y_{\mathrm{h}}\\ z_{\mathrm{h}}\end{array}\right)=\left(\begin{array}{c}{x}_{\mathrm{e}}-d_{\mathrm{x}}\\ y_{\mathrm{e}}-d_{\mathrm{y}}\\ z_{\mathrm{e}}\end{array}\right)$$

(23)

Let the angles at which the current moment of the head rotate relative to the initial position be ^hθ_pi and ^hθ_ti; the coordinates of the target in the robot coordinate system are

$$\left(\begin{array}{c}\begin{array}{c}{}^{\mathrm{w}}{x}_{\mathrm{m}}\\ {}^{\mathrm{w}}{y}_{\mathrm{m}}\\ {}^{\mathrm{w}}{z}_{\mathrm{m}}\end{array}\\ 1\end{array}\right)={\left(\begin{array}{cc}\mathrm{Rot}(X{,}^{\mathrm{h}}{\theta }_{\mathrm{t}i}\left)\mathrm{Rot}\right(Y{,}^{\mathrm{h}}{\theta }_{\mathrm{p}i})& 0\\ 0& 1\end{array}\right)}^{-1}\left(\begin{array}{c}\begin{array}{c}{x}_{\mathrm{h}}\\ y_{\mathrm{h}}\\ z_{\mathrm{h}}\end{array}\\ 1\end{array}\right)$$

(24)

According to the 3D coordinates of the target in the head coordinate system, the angle between the target and Z_h in the horizontal direction and the vertical direction can be obtained as follows:

$${}^{\mathrm{h}}{\gamma }_{\mathrm{p}}=\arctan (\frac{x_{\mathrm{h}}}{z_{\mathrm{h}}})$$

(25)

$${}^{\mathrm{h}}{\gamma }_{\mathrm{t}}=\arctan (\frac{y_{\mathrm{h}}}{z_{\mathrm{h}}})$$

(26)

When ^hγ_p and ^hγ_t exceed a set threshold, the head needs to rotate. To leave a certain margin for the rotation of the eyeball and for the convenience of calculation, the angles required for the head to rotate in the horizontal direction and the vertical direction are calculated by the principle shown in Figure 7a,b, respectively. Figure 7a shows the calculation principle of the horizontal direction angle when the target’s x coordinates of the head coordinate system is greater than zero. After the head is rotated to ${}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }$, the target point is on the ^lZ_e axis of the left motion module end coordinate system, and the left motion module reaches the maximum rotatable threshold ^eθ_pmax. Figure 7b shows the calculation principle of the vertical direction when the target’s y coordinates of the head coordinate system are greater than d_y. After the head is rotated to ${}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }$, the target point is on the Z_e axis of the eye coordinate system, and the eye reaches the maximum threshold ^eθ_tmax that can be rotated.

3.2.2. Horizontal Rotation Angle Calculation

Let the current angle of the head in the horizontal direction be ^hθ_pi. When the head is rotated in the horizontal direction to ${}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }$, the 3D coordinates of the target in the new head coordinate system are

$$\left(\begin{array}{c}\begin{array}{c}{x}_{\mathrm{h}}^{\prime }\\ y_{\mathrm{h}}^{\prime }\\ z_{\mathrm{h}}^{\prime }\end{array}\\ 1\end{array}\right)=\left(\begin{array}{cccc}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})& 0& -\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})& 0\\ 0& 1& 0& 0\\ \sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})& 0& \cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}\begin{array}{c}{x}_{\mathrm{h}}\\ y_{\mathrm{h}}\\ z_{\mathrm{h}}\end{array}\\ 1\end{array}\right)$$

(27)

Therefore,

$$\left(\begin{array}{c}{x}_{\mathrm{h}}^{\prime }\\ y_{\mathrm{h}}^{\prime }\\ z_{\mathrm{h}}^{\prime }\end{array}\right)=\left(\begin{array}{c}{x}_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})-z_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})\\ y_{\mathrm{h}}\\ x_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})+z_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})\end{array}\right)$$

(28)

The coordinates of the target in the new eye coordinate system are

$$\left(\begin{array}{c}{}^{\mathrm{e}}{x}_{\mathrm{h}}^{\prime }\\ {}^{\mathrm{e}}{y}_{\mathrm{h}}^{\prime }\\ {}^{\mathrm{e}}{z}_{\mathrm{h}}^{\prime }\end{array}\right)=\left(\begin{array}{c}{d}_x+x_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})-z_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})\\ y_{\mathrm{h}}+d_y\\ x_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})+z_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})\end{array}\right)$$

(29)

After turning, the left motion module reaches the maximum threshold ^eθ_pmax that can be rotated, so that

$$\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{pmax}})=\frac{{}^{\mathrm{e}}{z}_{\mathrm{h}}^{\prime }}{{}^{\mathrm{e}}{x}_{\mathrm{h}}^{\prime }}=\frac{x_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})+z_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})}{d_x+x_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})-z_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})}$$

(30)

Simplifying Equation (30), we have

$$\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})=\frac{d_x\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})}{x_{\mathrm{h}}+z_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})}+\frac{x_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})-z_{\mathrm{h}}}{x_{\mathrm{h}}+z_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})$$

(31)

Assume that

$$\left\{\begin{array}{l}{k}_1=\frac{x_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})-z_{\mathrm{h}}}{x_{\mathrm{h}}+z_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})}\\ k_2=\frac{d_x\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})}{x_{\mathrm{h}}+z_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})}\end{array}\right.$$

(32)

According to the triangular relationship,

$${[k_1\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})+k_2]}^2+{\cos }^2{(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})=1$$

(33)

The solution of Equation (33) is

$$\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})=\frac{-k_1{k}_2\pm \sqrt{k_1^2-k_2^2+1}}{k_1^2+1}$$

(34)

Therefore,

$${}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{=}^{\mathrm{h}}{\theta }_{\mathrm{p}i}+\arccos (\frac{-k_1{k}_2\pm \sqrt{k_1^2-k_2^2+1}}{k_1^2+1})$$

(35)

Equation (35) has two solutions; therefore, we choose the solution in which the deviation e of Equation (36) is minimized:

$$e=\left|\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})-\frac{x_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})+z_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})}{d_x+x_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})-z_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})}\right|$$

(36)

When the obtained ^h${\theta }_p^{\prime }$ is outside of the range [−^hθ_pmax, ^hθ_pmax], the value of ^hθ_pq is

$${}^{\mathrm{h}}{\theta }_{\mathrm{pq}}=\left\{\begin{array}{lll}{}^{\mathrm{h}}{\theta }_{\mathrm{pmax}}& ,& {}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{\ge }^{\mathrm{h}}{\theta }_{\mathrm{pmax}}\\ {-}^{\mathrm{h}}{\theta }_{\mathrm{pmax}}& ,& {}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }\le {-}^{\mathrm{h}}{\theta }_{\mathrm{pmax}}\\ {}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }& ,& \mathrm{else}\end{array}\right.$$

(37)

Finally, one can obtain the ^wθ_pq value:

$${}^{\mathrm{w}}{\theta }_{\mathrm{pq}}=\left\{\begin{array}{lll}{}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{pmax}}& ,& {}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{>}^{\mathrm{h}}{\theta }_{\mathrm{pmax}}\\ {}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{+}^{\mathrm{h}}{\theta }_{\mathrm{pmax}}& ,& {}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }<{-}^{\mathrm{h}}{\theta }_{\mathrm{p}\max }\\ 0& ,& \mathrm{else}\end{array}\right.$$

(38)

Based on the same principle, when the x coordinate of the target in the head coordinate system is less than 0, the coordinates of the target in the right motion module base coordinate system after the rotation are

$$\left(\begin{array}{c}{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }\\ {}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }\\ {}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }\\ 1\end{array}\right)=\left(\begin{array}{c}{x}_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})-z_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})-d_x\\ y_{\mathrm{h}}+d_y\\ x_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})+z_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})\\ 1\end{array}\right)$$

(39)

After turning, the right motion module reaches −^eθ_pmax, and the following can be obtained:

$$\tan ({-}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})=\frac{{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }}{{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }}=\frac{x_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})+z_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})}{x_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})-z_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})-d_x}$$

(40)

We simplify Equation (40) as follows:

$$\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})=\frac{d_x\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})}{x_{\mathrm{h}}-z_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})}-\frac{x_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})+z_{\mathrm{h}}}{x_{\mathrm{h}}-z_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})$$

(41)

Let

$$\left\{\begin{array}{l}{k}_1^{\prime }=-\frac{x_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})+z_{\mathrm{h}}}{x_{\mathrm{h}}-z_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})}\\ k_2^{\prime }=\frac{d_x\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})}{x_{\mathrm{h}}-z_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})}\end{array}\right.$$

(42)

The same two solutions are available:

$${}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{=}^{\mathrm{h}}{\theta }_{\mathrm{p}i}+\arccos (\frac{-k_1^{\prime }{k}_2^{\prime }\pm \sqrt{{(k_1^{\prime })}^2-{(k_2^{\prime })}^2+1}}{{(k_1^{\prime })}^2+1})$$

(43)

Select the solution in which the deviation e of Equation (44) is minimized:

$$e=\left|-\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{qmax}})-\frac{x_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})+z_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})}{x_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})-z_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{p}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{p}i})-d_x}\right|$$

(44)

Using Equations (37) and (38), ^hθ_pq and ^wθ_pq can be obtained.

3.2.3. Vertical Rotation Angle Calculation

When the target’s y coordinate in the head coordinate system is greater than d_y, the current angle of the head in the vertical direction is ^hθ_ti, and when the head is rotated in the vertical direction to ${}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }$, the target is in the new head coordinate system. The 3D coordinates are

$$\left(\begin{array}{c}\begin{array}{c}{x}_{\mathrm{h}}^{\prime }\\ y_{\mathrm{h}}^{\prime }\\ z_{\mathrm{h}}^{\prime }\end{array}\\ 1\end{array}\right)=\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})& \sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})& 0\\ 0& -\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})& \cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}\begin{array}{c}{x}_{\mathrm{h}}\\ y_{\mathrm{h}}\\ z_{\mathrm{h}}\end{array}\\ 1\end{array}\right)$$

(45)

Therefore,

$$\left(\begin{array}{c}{x}_{\mathrm{h}}^{\prime }\\ y_{\mathrm{h}}^{\prime }\\ z_{\mathrm{h}}^{\prime }\end{array}\right)=\left(\begin{array}{c}{x}_{\mathrm{h}}\\ y_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})+z_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})\\ z_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})-y_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})\end{array}\right)$$

(46)

Using Equation (29), the coordinates of the eye coordinate system after the rotation of the target can be calculated:

$$\left(\begin{array}{c}{}^{\mathrm{e}}{x}_{\mathrm{h}}^{\prime }\\ {}^{\mathrm{e}}{y}_{\mathrm{h}}^{\prime }\\ {}^{\mathrm{e}}{z}_{\mathrm{h}}^{\prime }\end{array}\right)=\left(\begin{array}{c}{d}_x+x_{\mathrm{h}}\\ y_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})+z_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})+d_y\\ z_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})-y_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})\end{array}\right)$$

(47)

After rotation, the left and right motion modules reach the rotatable maximum value ^eθ_tmax in the vertical direction, so that

$$\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})=\frac{{}^{\mathrm{e}}{y}_{\mathrm{h}}^{\prime }}{{}^{\mathrm{e}}{z}_{\mathrm{h}}^{\prime }}=\frac{y_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})+z_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})+d_y}{z_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})-y_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})}$$

(48)

Simplifying Equation (48), we obtain

$$\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})=\frac{z_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})-y_{\mathrm{h}}}{z_{\mathrm{h}}+y_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})-\frac{d_y}{z_{\mathrm{h}}+y_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})}$$

(49)

Let

$$\left\{\begin{array}{l}{k}_1=\frac{z_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})-y_{\mathrm{h}}}{z_{\mathrm{h}}+y_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})}\\ k_2=-\frac{d_y}{z_{\mathrm{h}}+y_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})}\end{array}\right.$$

(50)

Therefore,

$${}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{=}^{\mathrm{h}}{\theta }_{\mathrm{t}i}+\arccos (\frac{-k_1{k}_2\pm \sqrt{k_1^2-k_2^2+1}}{k_1^2+1})$$

(51)

Equation (51) has two solutions; therefore, we choose the solution in which the deviation e of Equation (52) is minimized:

$$e=\left|\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})-\frac{y_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})+z_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})+d_y}{z_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})-y_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})}\right|$$

(52)

Similarly, when the target’s y coordinates in the head coordinate system are less than d_y, we have

$$\tan ({-}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})=\frac{{}^{\mathrm{e}}{y}_{\mathrm{h}}^{\prime }}{{}^{\mathrm{e}}{z}_{\mathrm{h}}^{\prime }}=\frac{y_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})+z_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})+d_y}{z_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})-y_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})}$$

(53)

$$\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})=-\frac{z_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})+y_{\mathrm{h}}}{z_{\mathrm{h}}-y_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})-\frac{d_y}{z_{\mathrm{h}}-y_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})}$$

(54)

$$\left\{\begin{array}{l}{k}_1=-\frac{z_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})+y_{\mathrm{h}}}{z_{\mathrm{h}}-y_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})}\\ k_2=-\frac{d_y}{z_{\mathrm{h}}-y_{\mathrm{h}}\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})}\end{array}\right.$$

(55)

$$e=\left|-\tan {(}^{\mathrm{e}}{\theta }_{\mathrm{tmax}})-\frac{y_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})+z_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})+d_y}{z_{\mathrm{h}}\cos {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})-y_{\mathrm{h}}\sin {(}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{-}^{\mathrm{h}}{\theta }_{\mathrm{t}i})}\right|$$

(56)

When the obtained ${}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }$ is outside of the range [−^hθ_tmax, ^hθ_tmax], the value of ^hθ_tq is

$${}^{\mathrm{h}}{\theta }_{\mathrm{tq}}=\left\{\begin{array}{lll}{}^{\mathrm{h}}{\theta }_{\mathrm{tmax}}& ,& {}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }{\ge }^{\mathrm{h}}{\theta }_{\mathrm{tmax}}\\ {-}^{\mathrm{h}}{\theta }_{\mathrm{tmax}}& ,& {}^{\mathrm{h}}{\theta }_{\mathrm{t}}^{\prime }\le {-}^{\mathrm{h}}{\theta }_{\mathrm{tmax}}\\ {}^{\mathrm{h}}{\theta }_t^{\prime }& ,& \mathrm{else}\end{array}\right.$$

(57)

After obtaining ^hθ_pq, ^hθ_tq and ^wθ_pq, ${\mathbf{P}}_{\mathrm{e}}^{\prime }\left(x_{\mathrm{e}}^{\prime },y_{\mathrm{e}}^{\prime },z_{\mathrm{e}}^{\prime }\right)$ are the coordinates of the target in the eye coordinate system after the mobile robot and the head are rotated:

$$\left(\begin{array}{c}\begin{array}{c}{x}_{\mathrm{e}}^{\prime }\\ y_{\mathrm{e}}^{\prime }\\ z_{\mathrm{e}}^{\prime }\end{array}\\ 1\end{array}\right)=\left(\begin{array}{cc}\mathrm{Rot}(X{,}^{\mathrm{h}}{\theta }_{\mathrm{tq}}\left)\mathrm{Rot}\right(Y{,}^{\mathrm{h}}{\theta }_{\mathrm{pq}})& 0\\ 0& 1\end{array}\right)\left(\begin{array}{cc}\mathrm{Rot}(Y{,}^{\mathrm{w}}{\theta }_{\mathrm{pq}})& 0\\ 0& 1\end{array}\right)\left(\begin{array}{c}\begin{array}{c}{x}_{\mathrm{w}}\\ y_{\mathrm{w}}\\ z_{\mathrm{w}}\end{array}\\ 1\end{array}\right)+\left(\begin{array}{c}{d}_x\\ d_y\\ 0\\ 0\end{array}\right)$$

(58)

The desired observation pose of the eye, characterized by ^lθ_tq, ^lθ_pq, ^rθ_tq and ^rθ_pq, can be obtained using the method described in the following section.

3.2.4. Calculation of the Desired Observation Poses of the Eye

According to Formula (2), ^lθ_tq = ^rθ_tq = θ_t, and ^lθ_pq = ^rθ_pq = θ_p.

The inverse of the hand–eye matrix of the left camera and left motion module end coordinate system is

$${}^{\mathrm{l}}{\mathit{T}}_{\mathrm{m}}^{-1}=\left(\begin{array}{cccc}{}^{\mathrm{l}}{n}_x& {}^{\mathrm{l}}{o}_x& {}^{\mathrm{l}}{a}_x& {}^{\mathrm{l}}{p}_x\\ {}^{\mathrm{l}}{n}_y& {}^{\mathrm{l}}{o}_y& {}^{\mathrm{l}}{a}_y& {}^{\mathrm{l}}{p}_y\\ {}^{\mathrm{l}}{n}_z& {}^{\mathrm{l}}{o}_z& {}^{\mathrm{l}}{a}_z& {}^{\mathrm{l}}{p}_z\\ 0& 0& 0& 1\end{array}\right)$$

(59)

The coordinate ^lP_c (^lx_c, ^ly_c, ^lz_c) of ${\mathbf{P}}_{\mathrm{e}}^{\prime }\left(x_{\mathrm{e}}^{\prime },y_{\mathrm{e}}^{\prime },z_{\mathrm{e}}^{\prime }\right)$ in the left camera coordinate system satisfies the following relationship:

$$\left(\begin{array}{c}{}^{\mathrm{l}}{\mathit{P}}_{\mathrm{c}}\\ 1\end{array}\right)={}^{\mathrm{l}}{\mathit{T}}_{\mathrm{m}}^{-1}\left(\begin{array}{cc}\mathrm{Rot}(X,-{}^{\mathrm{l}}{\theta }_{\mathrm{t}})& 0\\ 0& 1\end{array}\right)\left(\begin{array}{cc}\mathrm{Rot}(Y,-{}^{\mathrm{l}}{\theta }_{\mathrm{p}})& 0\\ 0& 1\end{array}\right)\left(\begin{array}{c}{\mathit{P}}_{\mathrm{e}}^{\prime }\\ 1\end{array}\right)$$

(60)

According to the small hole imaging model, the imaging coordinates of the ${\mathbf{P}}_{\mathrm{e}}^{\prime }\left(x_{\mathrm{e}}^{\prime },y_{\mathrm{e}}^{\prime },z_{\mathrm{e}}^{\prime }\right)$ point in the left camera are

$$\left(\begin{array}{c}{}^{\mathrm{l}}u\\ {}^{\mathrm{l}}v\\ 1\end{array}\right)={}^{\mathrm{l}}{M}_{\mathrm{in}}{}^{\mathrm{l}}{P}_{1\mathrm{c}}=\left(\begin{array}{ccc}{}^{\mathrm{l}}{k}_x& 0& {}^{\mathrm{l}}u{}_0\\ 0& {}^{\mathrm{l}}{k}_y& {}^{\mathrm{l}}v{}_0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{}^{\mathrm{l}}{x}_{\mathrm{c}}/{}^{\mathrm{l}}{z}_{\mathrm{c}}\\ {}^{\mathrm{l}}{y}_{\mathrm{c}}/{}^{\mathrm{l}}{z}_{\mathrm{c}}\\ 1\end{array}\right)=\left(\begin{array}{c}{}^{\mathrm{l}}{k}_x{}^{\mathrm{l}}{x}_{\mathrm{c}}/{}^{\mathrm{l}}{z}_{\mathrm{c}}+{}^{\mathrm{l}}u{}_0\\ {}^{\mathrm{l}}{k}_y{}^{\mathrm{l}}{y}_{\mathrm{c}}/{}^{\mathrm{l}}{z}_{\mathrm{c}}+{}^{\mathrm{l}}v{}_0\\ 1\end{array}\right)$$

(61)

Substituting Equation (61) into Equation (2), we obtain

$$\left(\begin{array}{c}{}^{\mathrm{l}}\mathsf{\Delta }u\\ {}^{\mathrm{l}}\mathsf{\Delta }v\end{array}\right)=\left(\begin{array}{c}{}^{\mathrm{l}}{k}_x{}^{\mathrm{l}}{x}_{\mathrm{c}}/{}^{\mathrm{l}}{z}_{\mathrm{c}}\\ {}^{\mathrm{l}}{k}_y{}^{\mathrm{l}}{y}_{\mathrm{c}}/{}^{\mathrm{l}}{z}_{\mathrm{c}}\\ 1\end{array}\right)$$

(62)

Based on the same principle, the coordinate ^rP_c (^rx_c, ^ry_c, ^rz_c) of ${\mathbf{P}}_{\mathrm{e}}^{\prime }\left(x_{\mathrm{e}}^{\prime },y_{\mathrm{e}}^{\prime },z_{\mathrm{e}}^{\prime }\right)$ in the right camera coordinate system is

$$\left(\begin{array}{c}{}^{\mathrm{r}}{\mathit{P}}_{\mathrm{c}}\\ 1\end{array}\right)={}^{\mathrm{r}}{\mathit{T}}_{\mathrm{m}}^{-1}\left(\begin{array}{cc}\mathrm{Rot}(X,-{}^{\mathrm{r}}{\theta }_{\mathrm{t}})& 0\\ 0& 1\end{array}\right)\left(\begin{array}{cc}\mathrm{Rot}(Y,-{}^{\mathrm{r}}{\theta }_{\mathrm{p}})& 0\\ 0& 1\end{array}\right){}^{\mathrm{r}}\mathit{T}{}_{\mathrm{e}}\left(\begin{array}{c}{\mathit{P}}_{\mathrm{e}}^{\prime }\\ 1\end{array}\right)$$

(63)

The imaging coordinates of point ${\mathbf{P}}_{\mathrm{e}}^{\prime }\left(x_{\mathrm{e}}^{\prime },y_{\mathrm{e}}^{\prime },z_{\mathrm{e}}^{\prime }\right)$ in the right camera are

$$\left(\begin{array}{c}{}^{\mathrm{r}}u\\ {}^{\mathrm{r}}v\\ 1\end{array}\right)={}^{\mathrm{r}}{M}_{\mathrm{in}}{}^{\mathrm{r}}{P}_{1\mathrm{c}}=\left(\begin{array}{ccc}{}^{\mathrm{r}}{k}_x& 0& {}^{\mathrm{r}}u{}_0\\ 0& {}^{\mathrm{r}}{k}_y& {}^{\mathrm{r}}v{}_0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{}^{\mathrm{r}}{x}_{\mathrm{c}}/{}^{\mathrm{r}}{z}_{\mathrm{c}}\\ {}^{\mathrm{r}}{y}_{\mathrm{c}}/{}^{\mathrm{r}}{z}_{\mathrm{c}}\\ 1\end{array}\right)=\left(\begin{array}{c}{}^{\mathrm{r}}{k}_x{}^{\mathrm{r}}{x}_{\mathrm{c}}/{}^{\mathrm{r}}{z}_{\mathrm{c}}+{}^{\mathrm{r}}u{}_0\\ {}^{\mathrm{r}}{k}_y{}^{\mathrm{r}}{y}_{\mathrm{c}}/{}^{\mathrm{r}}{z}_{\mathrm{c}}+{}^{\mathrm{r}}v{}_0\\ 1\end{array}\right)$$

(64)

$$\left(\begin{array}{c}{}^{\mathrm{r}}\mathsf{\Delta }u\\ {}^{\mathrm{r}}\mathsf{\Delta }v\end{array}\right)=\left(\begin{array}{c}{}^{\mathrm{r}}{k}_x{}^{\mathrm{r}}{x}_{\mathrm{c}}/{}^{\mathrm{r}}{z}_{\mathrm{c}}\\ {}^{\mathrm{r}}{k}_y{}^{\mathrm{r}}{y}_{\mathrm{c}}/{}^{\mathrm{r}}{z}_{\mathrm{c}}\\ 1\end{array}\right)$$

(65)

By Equations (2), (62) and (65), two equations related to θ_t and θ_p (see Appendix C for the complete equations) can be obtained. It is challenging to calculate the values of θ_t and θ_p directly from these two equations, however. To obtain a solution, we consider a suboptimal observation pose and use this pose as the initial value; then, we use the trial-and-error method to obtain the optimal observation pose. When θ_t is calculated, let θ_p = 0; the solution of θ_t can then be obtained by Δv_l = −Δv_r. When θ_p is calculated, the solution of θ_p is solved by Δu_l = −Δu_r. The solution P_ob (θ_t, θ_t, θ_p, θ_p) is a suboptimal observed pose. Based on the suboptimal observation pose, the trial-and-error method can be used to obtain the optimal solution with the smallest error. The range of θ_t is [−θ_tmax, θ_tmax]. The range of θ_p is [−θ_pmax, θ_pmax].

According to Equations (60) and (63), let θ_p be equal to 0 to obtain

$$\left(\begin{array}{c}{}^{\mathrm{l}}{\mathbf{P}}_{\mathrm{c}}\\ 1\end{array}\right)={({}^{\mathrm{l}}{\mathit{T}}_{\mathrm{m}})}^{-1}\left(\begin{array}{cc}\mathrm{Rot}(X,-{\theta }_{\mathrm{t}})& 0\\ 0& 1\end{array}\right){}^{\mathrm{l}}{\mathit{T}}_{\mathrm{e}}\left(\begin{array}{c}{P}_{\mathrm{e}}^{\prime }\\ 1\end{array}\right)$$

(66)

The following result is also available:

$$\left(\begin{array}{c}{}^{\mathrm{r}}{\mathbf{P}}_{\mathrm{c}}\\ 1\end{array}\right)={({}^{\mathrm{r}}{\mathit{T}}_{\mathrm{m}})}^{-1}\left(\begin{array}{cc}\mathrm{Rot}(X,-{\theta }_{\mathrm{t}})& 0\\ 0& 1\end{array}\right){}^{\mathrm{r}}{\mathit{T}}_{\mathrm{e}}\left(\begin{array}{c}{P}_{\mathrm{e}}^{\prime }\\ 1\end{array}\right)$$

(67)

The base coordinate system of the left motion module is the world coordinate system. Therefore, ^lT_w is a unit matrix. To simplify the calculation, we have

$$\left(\begin{array}{c}{}^{\mathrm{r}}{\mathit{P}}_{\mathrm{e}}\\ 1\end{array}\right)=\left(\begin{array}{c}{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }\\ {}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }\\ {}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }\\ 1\end{array}\right)={}^{\mathrm{r}}{\mathit{T}}_{\mathrm{e}}\left(\begin{array}{c}{P}_{\mathrm{e}}^{\prime }\\ 1\end{array}\right)$$

(68)

According to the calculation principle of Section 3.2.1, we have the following:

$$\left\{\begin{array}{l}\Delta u_{\mathrm{l}}={}^{\mathrm{l}}{f}_x\frac{({}^{\mathrm{l}}{o}_x{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_x{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{t}}+({}^{\mathrm{l}}{o}_x{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_x{y}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{t}}+({}^{\mathrm{l}}{p}_x+{}^{\mathrm{l}}{n}_x{x}_{\mathrm{e}}^{\prime })}{({}^{\mathrm{l}}{o}_z{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_z{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{t}}+({}^{\mathrm{l}}{o}_z{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_z{y}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{t}}+({}^{\mathrm{l}}{p}_z+{}^{\mathrm{l}}{n}_z{x}_{\mathrm{e}}^{\prime })}\\ \Delta v_{\mathrm{l}}={}^{\mathrm{l}}{f}_y\frac{({}^{\mathrm{l}}{o}_y{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_y{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{t}}+({}^{\mathrm{l}}{o}_y{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_y{y}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{t}}+({}^{\mathrm{l}}{p}_y+{}^{\mathrm{l}}{n}_y{x}_{\mathrm{e}}^{\prime })}{({}^{\mathrm{l}}{o}_z{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_z{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{t}}+({}^{\mathrm{l}}{o}_z{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_z{y}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{t}}+({}^{\mathrm{l}}{p}_z+{}^{\mathrm{l}}{n}_z{x}_{\mathrm{e}}^{\prime })}\end{array}\right.$$

(69)

$$\left\{\begin{array}{l}\Delta u_{\mathrm{r}}={}^{\mathrm{r}}{f}_x\frac{({}^{\mathrm{r}}{o}_x{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_x{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{t}}+({}^{\mathrm{r}}{o}_x{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_x{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{t}}+({}^{\mathrm{r}}{p}_x+{}^{\mathrm{r}}{n}_x{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })}{({}^{\mathrm{r}}{o}_z{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_z{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{t}}+({}^{\mathrm{r}}{o}_z{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_z{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{t}}+({}^{\mathrm{r}}{p}_z+{}^{\mathrm{r}}{n}_z{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })}\\ \Delta v_{\mathrm{r}}={}^{\mathrm{r}}{f}_y\frac{({}^{\mathrm{r}}{o}_y{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_y{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{t}}+({}^{\mathrm{r}}{o}_y{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_y{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{t}}+({}^{\mathrm{r}}{p}_y+{}^{\mathrm{r}}{n}_y{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })}{({}^{\mathrm{r}}{o}_z{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_z{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{t}}+({}^{\mathrm{r}}{o}_z{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_z{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{t}}+({}^{\mathrm{r}}{p}_z+{}^{\mathrm{r}}{n}_z{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })}\end{array}\right.$$

(70)

Assume the following:

$$E_{\mathrm{sv}}=\left|\mathsf{\Delta }{v}_{\mathrm{l}}\right|+\left|\mathsf{\Delta }{v}_{\mathrm{r}}\right|$$

(71)

The solution to θ_t that keeps the target at the center of the two cameras needs to satisfy the following conditions:

$$\left\{\begin{array}{l}\mathsf{\Delta }{v}_{\mathrm{l}}+\mathsf{\Delta }{v}_{\mathrm{r}}=0\\ -{\theta }_{\mathrm{tmax}}\le {\theta }_{\mathrm{t}}\le {\theta }_{\mathrm{tmax}}\\ {\theta }_{\mathrm{t}}=\arg \min (E_{\mathrm{sv}})\end{array}\right.$$

(72)

Substituting the second equation of Equations (69) and (70) into Equation (72) and solving the equation, we have

$$k_1{\cos }^2{\theta }_{\mathrm{t}}+k_2{\sin }^2{\theta }_{\mathrm{t}}+k_3\sin {\theta }_{\mathrm{t}}\cos {\theta }_{\mathrm{t}}+k_4\cos {\theta }_{\mathrm{t}}+k_5\sin {\theta }_{\mathrm{t}}+k_6=0$$

(73)

where $k_1,k_2,k_3,k_4,k_5$ are

$$k_1={}^{\mathrm{l}}{f}_y({}^{\mathrm{l}}{o}_y{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_y{z}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{o}_z{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_z{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })+{}^{\mathrm{r}}{f}_y({}^{\mathrm{l}}{o}_z{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_z{z}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{o}_y{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_y{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })$$

(74)

$$k_2={}^{\mathrm{l}}{f}_y({}^{\mathrm{l}}{o}_y{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_y{y}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{o}_z{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_z{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime })+{}^{\mathrm{r}}{f}_y({}^{\mathrm{l}}{o}_z{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_z{y}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{o}_y{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_y{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime })$$

(75)

$$\begin{array}{r}{k}_3={}^{\mathrm{l}}{f}_y({}^{\mathrm{l}}{o}_y{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_y{z}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{o}_z{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_z{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime })+{}^{\mathrm{l}}{f}_y({}^{\mathrm{l}}{o}_y{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_y{y}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{o}_z{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_z{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\\ +{}^{\mathrm{r}}{f}_y({}^{\mathrm{l}}{o}_z{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_z{z}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{o}_y{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_y{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime })+{}^{\mathrm{r}}{f}_y({}^{\mathrm{l}}{o}_z{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_z{y}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{o}_y{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_y{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\end{array}$$

(76)

$$\begin{array}{r}{k}_4={}^{\mathrm{l}}{f}_y({}^{\mathrm{l}}{o}_y{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_y{z}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{p}_z+{}^{\mathrm{r}}{n}_z{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })+{}^{\mathrm{l}}{f}_y({}^{\mathrm{l}}{p}_y+{}^{\mathrm{l}}{n}_y{x}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{o}_z{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_z{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\\ +{}^{\mathrm{r}}{f}_y({}^{\mathrm{l}}{o}_z{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_z{z}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{p}_y+{}^{\mathrm{r}}{n}_y{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })+{}^{\mathrm{r}}{f}_y({}^{\mathrm{l}}{p}_z+{}^{\mathrm{l}}{n}_z{x}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{o}_y{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_y{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\end{array}$$

(77)

$$\begin{array}{r}{k}_5={}^{\mathrm{l}}{f}_y({}^{\mathrm{l}}{o}_y{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_y{y}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{p}_z+{}^{\mathrm{r}}{n}_z{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })+{}^{\mathrm{l}}{f}_y({}^{\mathrm{l}}{p}_y+{}^{\mathrm{l}}{n}_y{x}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{o}_z{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_z{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime })\\ +{}^{\mathrm{r}}{f}_y({}^{\mathrm{l}}{o}_z{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_z{y}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{p}_y+{}^{\mathrm{r}}{n}_y{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })+{}^{\mathrm{r}}{f}_y({}^{\mathrm{l}}{p}_z+{}^{\mathrm{l}}{n}_z{x}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{o}_y{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_y{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime })\end{array}$$

(78)

$$k_6={}^{\mathrm{l}}{f}_y({}^{\mathrm{l}}{p}_y+{}^{\mathrm{l}}{n}_y{x}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{p}_z+{}^{\mathrm{r}}{n}_z{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })+{}^{\mathrm{r}}{f}_y({}^{\mathrm{l}}{p}_z+{}^{\mathrm{l}}{n}_z{x}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{p}_y+{}^{\mathrm{r}}{n}_y{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })$$

(79)

According to the triangle relationship, we have

$${\cos }^2{\theta }_{\mathrm{t}}+{\sin }^2{\theta }_{\mathrm{t}}=1$$

(80)

Replacing cosθ_t in Equation (73) with sinθ_t, we obtain the following:

$$k_1^{\prime }{\sin }^4{\theta }_{\mathrm{t}}+k_2^{\prime }{\sin }^3{\theta }_{\mathrm{t}}+k_3^{\prime }{\sin }^2{\theta }_{\mathrm{t}}+k_4^{\prime }\sin {\theta }_{\mathrm{t}}+k_5^{\prime }=0$$

(81)

where $k_1,k_2,k_3,k_4,k_5$ are

$$k_1^{\prime }={(k_2-k_1)}^2+k_3^2$$

(82)

$$k_2^{\prime }=2(k_2-k_1)k_5+2k_3{k}_4$$

(83)

$$k_3^{\prime }=2(k_2-k_1)k_6+k_5^2+k_4^2-k_3^2$$

(84)

$$k_4^{\prime }=2k_5{k}_6-2k_3{k}_4$$

(85)

$$k_5^{\prime }=k_6^2-k_4^2$$

(86)

Four solutions can be obtained using Equation (81). The optimal solution is a real number, and the most suitable solution can be selected by the condition of Equation (72).

After θ_t is obtained, θ_p can be solved based on the obtained θ_t.

According to Equations (60) and (63), ${\overline{\theta }}_{\mathrm{t}}$ is the solution obtained in Section 3.2.2, so that

$$\left(\begin{array}{c}{}^{\mathrm{l}}{\mathit{P}}_{\mathrm{c}}\\ 1\end{array}\right)={({}^{\mathrm{l}}{\mathit{T}}_{\mathrm{m}})}^{-1}\left(\begin{array}{cc}\mathrm{Rot}(X,-{\overline{\theta }}_{\mathrm{t}})& 0\\ 0& 1\end{array}\right)\left(\begin{array}{cc}\mathrm{Rot}(Y,-{}^{\mathrm{l}}{\theta }_{\mathrm{p}})& 0\\ 0& 1\end{array}\right){}^{\mathrm{l}}{\mathit{T}}_{\mathrm{e}}\left(\begin{array}{c}{P}_{\mathrm{e}}^{\prime }\\ 1\end{array}\right)$$

(87)

The following result is also available:

$$\left(\begin{array}{c}{}^{\mathrm{r}}{\mathit{P}}_{\mathrm{c}}\\ 1\end{array}\right)={({}^{\mathrm{r}}{\mathit{T}}_{\mathrm{m}})}^{-1}\left(\begin{array}{cc}\mathrm{Rot}(X,-{\overline{\theta }}_{\mathrm{t}})& 0\\ 0& 1\end{array}\right)\left(\begin{array}{cc}\mathrm{Rot}(Y,-{}^{\mathrm{l}}{\theta }_{\mathrm{p}})& 0\\ 0& 1\end{array}\right){}^{\mathrm{r}}{\mathit{T}}_{\mathrm{e}}\left(\begin{array}{c}{P}_{\mathrm{e}}^{\prime }\\ 1\end{array}\right)$$

(88)

Since ${\overline{\theta }}_{\mathrm{t}}$ is known, for convenience of calculation, we set

$${}^{\mathrm{l}}{\mathit{T}}_{\mathrm{m}}^{\prime }={({}^{\mathrm{l}}{\mathit{T}}_{\mathrm{m}})}^{-1}\left(\begin{array}{cc}\mathrm{Rot}(X,-{\overline{\theta }}_{\mathrm{t}})& 0\\ 0& 1\end{array}\right)=\left(\begin{array}{cccc}{}^{\mathrm{l}}{n}_x^{\prime }& {}^{\mathrm{l}}{o}_x^{\prime }& {}^{\mathrm{l}}{a}_x^{\prime }& {}^{\mathrm{l}}{p}_x^{\prime }\\ {}^{\mathrm{l}}{n}_y^{\prime }& {}^{\mathrm{l}}{o}_y^{\prime }& {}^{\mathrm{l}}{a}_y^{\prime }& {}^{\mathrm{l}}{p}_y^{\prime }\\ {}^{\mathrm{l}}{n}_z^{\prime }& {}^{\mathrm{l}}{o}_z^{\prime }& {}^{\mathrm{l}}{a}_z^{\prime }& {}^{\mathrm{l}}{p}_z^{\prime }\\ 0& 0& 0& 1\end{array}\right)$$

(89)

$${}^{\mathrm{r}}{\mathit{T}}_{\mathrm{m}}^{\prime }={({}^{\mathrm{r}}{\mathit{T}}_{\mathrm{m}})}^{-1}\left(\begin{array}{cc}\mathrm{Rot}(X,-{\overline{\theta }}_{\mathrm{t}})& 0\\ 0& 1\end{array}\right)=\left(\begin{array}{cccc}{}^{\mathrm{r}}{n}_x^{\prime }& {}^{\mathrm{r}}{o}_x^{\prime }& {}^{\mathrm{r}}{a}_x^{\prime }& {}^{\mathrm{r}}{p}_x^{\prime }\\ {}^{\mathrm{r}}{n}_y^{\prime }& {}^{\mathrm{r}}{o}_y^{\prime }& {}^{\mathrm{r}}{a}_y^{\prime }& {}^{\mathrm{r}}{p}_y^{\prime }\\ {}^{\mathrm{r}}{n}_z^{\prime }& {}^{\mathrm{r}}{o}_z^{\prime }& {}^{\mathrm{r}}{a}_z^{\prime }& {}^{\mathrm{r}}{p}_z^{\prime }\\ 0& 0& 0& 1\end{array}\right)$$

(90)

The following results are obtained:

$$\left\{\begin{array}{l}\Delta u_{\mathrm{l}}={}^{\mathrm{l}}{f}_x\frac{({}^{\mathrm{l}}{n}_x^{\prime }{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_x^{\prime }{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{p}}+({}^{\mathrm{l}}{a}_x^{\prime }{x}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{n}_x^{\prime }{z}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{p}}+({}^{\mathrm{l}}{p}_x^{\prime }+{}^{\mathrm{l}}{o}_x^{\prime }{y}_{\mathrm{e}}^{\prime })}{({}^{\mathrm{l}}{n}_z^{\prime }{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_z^{\prime }{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{p}}+({}^{\mathrm{l}}{a}_z^{\prime }{x}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{n}_z^{\prime }{z}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{p}}+({}^{\mathrm{l}}{p}_z^{\prime }+{}^{\mathrm{l}}{o}_z^{\prime }{y}_{\mathrm{e}}^{\prime })}\\ \Delta v_{\mathrm{l}}={}^{\mathrm{l}}{f}_y\frac{({}^{\mathrm{l}}{n}_y^{\prime }{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_y^{\prime }{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{p}}+({}^{\mathrm{l}}{a}_y^{\prime }{x}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{n}_y^{\prime }{z}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{p}}+({}^{\mathrm{l}}{p}_y^{\prime }+{}^{\mathrm{l}}{o}_y^{\prime }{y}_{\mathrm{e}}^{\prime })}{({}^{\mathrm{l}}{n}_z^{\prime }{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_z^{\prime }{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{p}}+({}^{\mathrm{l}}{a}_z^{\prime }{x}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{n}_z^{\prime }{z}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{p}}+({}^{\mathrm{l}}{p}_z^{\prime }+{}^{\mathrm{l}}{o}_z^{\prime }{y}_{\mathrm{e}}^{\prime })}\end{array}\right.$$

(91)

$$\left\{\begin{array}{l}\Delta u_{\mathrm{r}}={}^{\mathrm{r}}{f}_x\frac{({}^{\mathrm{r}}{n}_x^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_x^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{p}}+({}^{\mathrm{r}}{a}_x^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{n}_x^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{p}}+({}^{\mathrm{r}}{p}_x^{\prime }+{}^{\mathrm{r}}{o}_x^{\prime }{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime })}{({}^{\mathrm{r}}{n}_z^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_z^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{p}}+({}^{\mathrm{r}}{a}_z^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{n}_z^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{p}}+({}^{\mathrm{r}}{p}_z^{\prime }+{}^{\mathrm{r}}{o}_z^{\prime }{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime })}\\ \Delta v_{\mathrm{r}}={}^{\mathrm{r}}{f}_y\frac{({}^{\mathrm{r}}{n}_y^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_y^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{p}}+({}^{\mathrm{r}}{a}_y^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{n}_y^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{p}}+({}^{\mathrm{r}}{p}_y^{\prime }+{}^{\mathrm{r}}{o}_y^{\prime }{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime })}{({}^{\mathrm{r}}{n}_z^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_z^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\cos {\theta }_{\mathrm{p}}+({}^{\mathrm{r}}{a}_z^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{n}_z^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\sin {\theta }_{\mathrm{p}}+({}^{\mathrm{r}}{p}_z^{\prime }+{}^{\mathrm{r}}{o}_z^{\prime }{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime })}\end{array}\right.$$

(92)

Assume that

$$E_{\mathrm{su}}=\left|\mathsf{\Delta }{u}_{\mathrm{l}}\right|+\left|\mathsf{\Delta }{u}_{\mathrm{r}}\right|$$

(93)

The solution to θ_p that keeps the target at the center of the two cameras needs to satisfy the following conditions:

$$\left\{\begin{array}{l}\mathsf{\Delta }{u}_{\mathrm{l}}+\mathsf{\Delta }{u}_{\mathrm{r}}=0\\ -{\theta }_{\mathrm{pmax}}\le {\theta }_{\mathrm{p}}\le {\theta }_{\mathrm{pmax}}\\ {\theta }_{\mathrm{p}}=\arg \min (E_{\mathrm{su}})\end{array}\right.$$

(94)

Substituting the second equation of Equations (91) and (92) into Equation (94) and solving the available equation, we obtain

$$k_1{\cos }^2{\theta }_{\mathrm{p}}+k_2{\sin }^2{\theta }_{\mathrm{p}}+k_3\sin {\theta }_{\mathrm{p}}\cos {\theta }_{\mathrm{p}}+k_4\cos {\theta }_{\mathrm{p}}+k_5\sin {\theta }_{\mathrm{p}}+k_6=0$$

(95)

where

$$k_1={}^{\mathrm{l}}{f}_x({}^{\mathrm{l}}{n}_x^{\prime }{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_x^{\prime }{z}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{n}_z^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_z^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })+{}^{\mathrm{r}}{f}_x({}^{\mathrm{r}}{n}_x^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_x^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })({}^{\mathrm{l}}{n}_z^{\prime }{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_z^{\prime }{z}_{\mathrm{e}}^{\prime })$$

(96)

$$k_2={}^{\mathrm{l}}{f}_x({}^{\mathrm{l}}{n}_x^{\prime }{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_x^{\prime }{x}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{n}_z^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_z^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })+{}^{\mathrm{r}}{f}_x({}^{\mathrm{r}}{n}_x^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_x^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })({}^{\mathrm{l}}{n}_z^{\prime }{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_z^{\prime }{x}_{\mathrm{e}}^{\prime })$$

(97)

$$\begin{array}{r}{k}_3={}^{\mathrm{l}}{f}_x({}^{\mathrm{l}}{n}_x^{\prime }{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_x^{\prime }{z}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{n}_z^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_z^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })+{}^{\mathrm{l}}{f}_x({}^{\mathrm{l}}{n}_x^{\prime }{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_x^{\prime }{x}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{n}_z^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_z^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\\ +{}^{\mathrm{r}}{f}_x({}^{\mathrm{r}}{n}_x^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_x^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })({}^{\mathrm{l}}{n}_z^{\prime }{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_z^{\prime }{z}_{\mathrm{e}}^{\prime })+{}^{\mathrm{r}}{f}_x({}^{\mathrm{r}}{n}_x^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_x^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })({}^{\mathrm{l}}{n}_z^{\prime }{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_z^{\prime }{x}_{\mathrm{e}}^{\prime })\end{array}$$

(98)

$$\begin{array}{r}{k}_4={}^{\mathrm{l}}{f}_x({}^{\mathrm{l}}{n}_x^{\prime }{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_x^{\prime }{z}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{o}_z^{\prime }{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{p}_z^{\prime })+{}^{\mathrm{l}}{f}_x({}^{\mathrm{l}}{o}_x^{\prime }{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{p}_x^{\prime })({}^{\mathrm{r}}{n}_z^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_z^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })\\ +{}^{\mathrm{r}}{f}_x({}^{\mathrm{r}}{o}_x^{\prime }{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{p}_x^{\prime })({}^{\mathrm{l}}{n}_z^{\prime }{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{a}_z^{\prime }{z}_{\mathrm{e}}^{\prime })+{}^{\mathrm{r}}{f}_x({}^{\mathrm{r}}{n}_x^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{a}_x^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime })({}^{\mathrm{l}}{o}_z^{\prime }{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{p}_z^{\prime })\end{array}$$

(99)

$$\begin{array}{r}{k}_5={}^{\mathrm{l}}{f}_x({}^{\mathrm{l}}{n}_x^{\prime }{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_x^{\prime }{x}_{\mathrm{e}}^{\prime })({}^{\mathrm{r}}{o}_z^{\prime }{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{p}_z^{\prime })+{}^{\mathrm{l}}{f}_x({}^{\mathrm{l}}{o}_x^{\prime }{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{p}_x^{\prime })({}^{\mathrm{r}}{n}_z^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_z^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })\\ +{}^{\mathrm{r}}{f}_x({}^{\mathrm{r}}{o}_x^{\prime }{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{p}_x^{\prime })({}^{\mathrm{l}}{n}_z^{\prime }{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{l}}{a}_z^{\prime }{x}_{\mathrm{e}}^{\prime })+{}^{\mathrm{r}}{f}_x({}^{\mathrm{r}}{n}_x^{\prime }{}^{\mathrm{r}}{z}_{\mathrm{e}}^{\prime }-{}^{\mathrm{r}}{a}_x^{\prime }{}^{\mathrm{r}}{x}_{\mathrm{e}}^{\prime })({}^{\mathrm{l}}{o}_z^{\prime }{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{p}_z^{\prime })\end{array}$$

(100)

$$k_6={}^{\mathrm{l}}{f}_x({}^{\mathrm{l}}{o}_x^{\prime }{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{p}_x^{\prime })({}^{\mathrm{r}}{o}_z^{\prime }{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{p}_z^{\prime })+{}^{\mathrm{r}}{f}_x({}^{\mathrm{r}}{o}_x^{\prime }{}^{\mathrm{r}}{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{r}}{p}_x^{\prime })({}^{\mathrm{l}}{o}_z^{\prime }{y}_{\mathrm{e}}^{\prime }+{}^{\mathrm{l}}{p}_z^{\prime })$$

(101)

Replacing cosθ_p in Equation (73) with sinθ_p, we obtain

$$k_1^{\prime }{\sin }^4{\theta }_{\mathrm{p}}+k_2^{\prime }{\sin }^3{\theta }_{\mathrm{p}}+k_3^{\prime }{\sin }^2{\theta }_{\mathrm{p}}+k_4^{\prime }\sin {\theta }_{\mathrm{p}}+k_5^{\prime }=0$$

(102)

where

$$k_1^{\prime }={(k_2-k_1)}^2+{(k_3)}^2$$

(103)

$$k_2^{\prime }=2(k_2-k_1)k_5+2k_3{k}_4$$

(104)

$$k_3^{\prime }=2(k_2-k_1)k_6+{(k_5)}^2+{(k_4)}^2-{(k_3)}^2$$

(105)

$$k_4^{\prime }=2k_5{k}_6-2k_3{k}_4$$

(106)

$$k_5^{\prime }={(k_6)}^2-{(k_4)}^2$$

(107)

Four solutions can be obtained using Equation (102). The optimal solution must be a real number, and the most suitable solution can be selected using the condition of Equation (94). For the case where the four solutions cannot satisfy Equation (94), the position of the target is beyond the position that the bionic eye can reach. In this case, compensation is required through the head or torso. ${\overline{\theta }}_{\mathrm{t}}$ and ${\overline{\theta }}_{\mathrm{p}}$ obtained at this time are suboptimal solutions close to the optimal solution. θ_t and θ_p are the optimal solutions.

Through the above steps, the desired observation pose can be calculated. The calculation steps of ^gf_q can be summarized by the flow chart shown in Figure 8.

3.3. Desired Pose Calculation for Approaching Gaze Point Tracking

The mobile robot approaches the target in two steps: the first step is that the robot and the head rotate in the horizontal direction until the robot and the head are facing the target, and the second step is that the robot moves straight toward the target. The desired position of the approaching motion should satisfy the following conditions: (1) the target should be on the Z axis of the robot and the head coordinate system, (2) the distance between the target and the robot should be less than the set threshold D_T and (3) the eye should be in the optimal observation position. ^af_q can be defined as

$${}^{\mathrm{a}}{f}_{\mathrm{q}}=\left(\begin{array}{l}{}^{\mathrm{w}}{x}_{\mathrm{mq}}=0\\ {}^{\mathrm{w}}{z}_{\mathrm{mq}}=\{z\left|\right. 0<{}^{\mathrm{w}}{z}_{\mathrm{m}}-z\le D_{\mathrm{T}}\}\\ {}^{\mathrm{w}}{\theta }_{\mathrm{pq}}=\{\theta \left|\right. |\theta |\le 2\mathsf{\pi }{,}^{\mathrm{w}}{\gamma }_{\mathrm{p}}=0\}\\ {}^{\mathrm{h}}{\theta }_{\mathrm{pq}}=0\\ {}^{\mathrm{h}}{\theta }_{\mathrm{tq}}=\{\theta \left|\right. |\theta |\le {}^{\mathrm{h}}{\theta }_{\mathrm{tmax}},{|}^{\mathrm{h}}{\gamma }_{\mathrm{t}}|{\le }^{\mathrm{h}}{\gamma }_{\mathrm{tmax}}\}\\ {}^{\mathrm{l}}{\theta }_{\mathrm{pq}}={}^{\mathrm{r}}{\theta }_{\mathrm{pq}}=\{\theta \left|\right. |\theta |\le {}^{\mathrm{e}}{\theta }_{\mathrm{pmax}},\mathsf{\Delta }{m}_{\mathrm{l}}=-\mathsf{\Delta }{m}_{\mathrm{r}}\}\\ {}^{\mathrm{l}}{\theta }_{\mathrm{tq}}={}^{\mathrm{r}}{\theta }_{\mathrm{tq}}=\{\theta \left|\right. |\theta |\le {}^{\mathrm{e}}{\theta }_{\mathrm{tmax}},\mathsf{\Delta }{m}_{\mathrm{l}}=-\mathsf{\Delta }{m}_{\mathrm{r}}\}\end{array}\right)$$

(108)

The desired rotation angle ^wθ_pq of the moving robot is the same as the angle ^bγ_p between the robot and the target and can be obtained by

$${}^{\mathrm{w}}{\theta }_{\mathrm{pq}}={}^{\mathrm{w}}{\gamma }_{\mathrm{p}}=\arctan (\frac{{}^{\mathrm{w}}{z}_{\mathrm{m}}}{{}^{\mathrm{w}}{x}_{\mathrm{m}}})$$

(109)

^hθ_tq can be obtained using the method described in Section 3.2. The optimal observation pose described in Section 3.2.4 can be used to obtain ^lθ_tq, ^lθ_pq, ^rθ_tq and ^rθ_pq.

4. Robot Pose Control

After obtaining the desired pose of the robot system, the control block diagram shown in Figure 9 is used to control the robot to move to the desired pose.

The desired pose is converted to the desired position of the motor. Δθ_lt, Δθ_lp, Δθ_rt, Δθ_rp, Δθ_ht and Δθ_hp are deviations of the desired angle from the current angle of motor M_lu, motor M_ld, motor M_ru, motor M_rd, motor M_hu and motor M_hd, respectively. ^lθ_m and ^rθ_m are the angles at which each wheel of the moving robot needs to be rotated. During the in situ gaze point tracking process, the moving robot performs only the rotation of the original position, and the angle of the robot movement can be calculated according to the desired angle of the robot. When the robot rotates, the two wheels move in opposite directions at the same speed. Let the distance between the two wheels of the moving robot be D_r; when the robot rotates around an angle ^wθ_pq, the distance that each wheel needs to move is

$$S={}^{\mathrm{w}}{\theta }_{\mathrm{pq}}\frac{D_{\mathrm{r}}}{2}$$

(110)

The diameter of each wheel is d_w, and the angle of rotation of each wheel is (where counterclockwise is positive)

$${}^{\mathrm{r}}{\theta }_m=-{}^{\mathrm{l}}{\theta }_m=\frac{2S}{d_{\mathrm{w}}}$$

(111)

In the process of approaching the target, the moving robot follows a straight line, and the angle of rotation of each wheel is

$${}^{\mathrm{r}}{\theta }_m={}^{\mathrm{l}}{\theta }_m=\frac{2{}^{\mathrm{w}}{z}_{\mathrm{mq}}}{d_{\mathrm{w}}}$$

(112)

The movement of the moving robot is achieved by controlling the rotation of each wheel. Each wheel is equipped with a DC brushless motor, and a DSP2000 controller is used to control the movement of the DC brushless motor. Position servo control is implemented in the DSP2000 controller.

In the robot system, the weight of the camera and lens is approximately 80 g, the weight of the camera and the fixed mechanical parts is approximately 50 g and the motor that controls the vertical rotation of the camera (rotating around the horizontal axis of rotation) and the corresponding encoder weighs approximately 250 g. The mechanical parts of the fixed vertical rotating motor and encoder weigh approximately 100 g. The radius of the rotation of the camera in the vertical direction is approximately 1 cm, and the rotation in the horizontal direction (rotation about the vertical axis of rotation) has a radius of approximately 2 cm. Therefore, when the gravitational acceleration is 9.8 m/s², the torque required for the vertical rotating electric machine is approximately 0.013 N·m, and the torque required for the horizontal rotating electric machine is approximately 0.043 N·m. The vertical rotating motor uses a 28BYG5401 stepping motor with a holding torque of 0.1 N·m and a positioning torque of 0.008 N·m. The driver is HSM20403A. The horizontal rotating motor is a 57BYGH301 stepping motor with a holding torque of 1.5 N·m, a positioning torque of 0.07 N·m and drive model HSM20504A. The four stepping motors of the eye have a step angle of 1.8° and are all subdivided by 25, so the actual step angle of each motor is 0.072°, and the minimum pulse width that the driver can receive is 2.5 µs. The stepper motor has a maximum angular velocity of 200°/s.

The head vertical rotary motor uses a 57BYGH401 stepper motor with a holding torque of 2.2 N·m, a positioning torque of 0.098 N·m and drive model HSM20504A. The head horizontal rotary motor is an 86BYG350B three-phase AC stepping motor with a holding torque of 5 N·m, a positioning torque of 0.3 N-m and an HSM30860M driver. The step angle of the head motor after subdivision is also 0.072°. The head vertical motor has a load of approximately 5 kg and a radius of rotation of less than 1 cm. The head horizontal rotary motor has a load of approximately 9.5 kg and a radius of rotation of approximately 5 cm. In the experiment, we found that the maximum horizontal pulse frequency that the head horizontal rotary motor can receive is 0.6 Kpps. Its maximum angular velocity is 43.2°/s.

5. Experiments and Discussion

Using the robot platform introduced in Section 2, experiments on in situ gaze point tracking and approaching gaze point tracking were performed

Each camera has a resolution of 400 × 300 pixels. The directions of rotation are [−45°, 45°]. The range of rotation of the head is [−30°, 30°]. d_x and d_y are 150 mm and 200 mm, respectively. The internal and external parameters, distortion parameters, initial position parameters and left- and right-hand–eye parameters of the dual purpose method are calibrated as follows:

$${}^{\mathrm{l}}{\mathit{M}}_{\mathrm{in}}=\left(\begin{array}{ccc}341.58& 0& 201.6\\ 0& 341.97& 147.62\\ 0& 0& 1\end{array}\right)$$

(113)

$${\mathit{K}}_{\mathrm{l}}=\left(\begin{array}{ccccc}-0.1905& 0.2171& -0.0018& -0.0005& -0.0823\end{array}\right)$$

(114)

$${}^{\mathrm{l}}{\mathit{T}}_m=\left(\begin{array}{cccc}1.0& 0.0078& -0.0022& 58.4172\\ 0.0001& 0.9954& 0.0959& 3.6042\\ 0.0013& -0.0959& 0.9954& 51.9366\\ 0& 0& 0& 1\end{array}\right)$$

(115)

$${}^{\mathrm{r}}{\mathit{M}}_{\mathrm{in}}=\left(\begin{array}{ccc}335.13& 0& 184.32\\ 0& 335.5& 141.26\\ 0& 0& 1\end{array}\right)$$

(116)

$${\mathit{K}}_{\mathrm{r}}=\left(\begin{array}{ccccc}-0.1861& 0.1987& -0.004& -0.0011& -0.0739\end{array}\right)$$

(117)

$${}^{\mathrm{r}}{\mathit{T}}_m=\left(\begin{array}{cccc}0.9999& -0.0086& -0.0125& -45.0147\\ 0.0190& -0.9969& -0.0782& -24.5528\\ 0.0097& -0.0784& 0.9970& 42.9270\\ 0& 0& 0& 1\end{array}\right)$$

(118)

$${}^{\mathrm{l}}{\mathit{T}}_r=\left(\begin{array}{cccc}0.9998& -0.0099& -0.0193& 189.5922\\ 0.0095& 0.9997& -0.0215& -0.0426\\ 0.0195& 0.0213& 0.9996& 8.9671\\ 0& 0& 0& 1\end{array}\right)$$

(119)

The experimental in situ gaze point tracking scene is shown in Figure 10, with a checkerboard target used as the target. For in situ gaze point tracking, the target is held by a person. In the approaching target gaze tracking experiment, the target is fixed in front of the robot.

5.1. In Situ Gaze Point Tracking Experiment

In the in situ gaze experiment, the target moves at a low speed within a certain range, and the robot combines the movement of the eye, the head and the mobile robot so that the binocular vision can always perceive the 3D coordinates of the target at the optimal observation posture. This experiment prompts the robot to find the target and gaze at it. In the gaze point tracking process, binocular stereo vision is used to calculate the 3D coordinates of the target in the eye coordinate system in real time. Through the positional relationship between the eye and the head, the coordinate system of the target in the eye can be converted to the head coordinate system. Similarly, the 3D coordinates of the target in the robot coordinate system can be obtained. Through the 3D coordinates, the desired poses of the eyes, head and mobile robot are calculated according to the method proposed in this paper. Then, the camera is controlled to the desired position by the stepping motor; after reaching the desired position, the image and the motor position information are collected again, and the 3D coordinates of the target are calculated.

In the experiment, the angles between the head and the target, ^hγ_pmax and ^hγ_pmax, are each 30°. The method described in Section 3 is used to calculate the desired pose of each joint of the robot based on the 3D coordinates of the target. In the experiment, the actual coordinate position and desired coordinate position of the target in the binocular image space, the actual position and desired position of the eye and head motor, the angle between the head and the robot and the target, and the target in the robot coordinate system are stored. Figure 11a,b show the u and v coordinates of the target on the left image, respectively, and Figure 11c,d show the u and v coordinates of the target on the right image, respectively. The desired image coordinates are recalculated based on the optimal observation position. Figure 11e–h show the positions of the tilt motor (M_lu) of the left eye, the pan motor (M_ld) of the left eye, the tilt motor (M_ru) of the right eye and the pan motor (M_rd) of the right eye, respectively. Figure 11i shows the positions of the pan motor (M_hd) of the head. Since the target moves in the vertical direction with small amplitude, the motor M_hu does not rotate, and the case is similar to the motion principle of the motor M_hd, so the motor position of the head only provides the result of the motor M_hd. Figure 11j shows the angle deviation and rotation. In this figure, T-h is the angle between the head and target, T-r is the angle between the robot and target, R-r is the angle of the robot rotation from the origin location and T-o is the angle of the target to the origin location. Figure 11k shows the coordinates (^wx, ^wz) of the target in the world coordinate system. Figure 11l shows the coordinates (^ox, ^oz) of the target in the world coordinate system of the origin location.

As shown in Figure 11, the image coordinate of the target is substantially within ±40 pixels in the central region of the left and right images in the x direction. These coordinates are kept within ±10 pixels of the center region of the left and right images in the y direction. Throughout the experiment, the target was rotated approximately 200° around the robot. The robot moved approximately 140°, the head rotated 30° and the target could be kept in the center region of the binocular images. The motor position curve shows that the motor’s operating position can track the desired position very well. The angle variation curve shows that the angle between the target and the head and the robot changes and that the robot turning angles are suitably consistent. The coordinates of the target shown in Figure 11 in the robot coordinate system and the coordinates of the target in the initial position of the world coordinate system are very close to the actual position change in the target’s position.

Through the above analysis, we can determine the following: (1) It is feasible to realize gaze point tracking of a robot based on 3D coordinates. (2) Using the movement of the head, eyes and mobile robot used in this paper, it is possible to achieve gaze point tracking of the target while ensuring minimum resource consumption.

5.2. Approaching Gaze Point Tracking Experiment

The approaching gaze point tracking experimental scene is shown in Figure 12.

The robot approaches the target without obstacles and reaches the area in which the robot can operate on the target. The target can be grasped or carefully observed. In the approaching gaze experiment, a target is fixed at a position 2.2 m from the robot, and when the robot moves to a position where the distance from the target to robot is 0.6 m, the motion is stopped, and the maximum speed of the moving robot is 1 m/s. The experiment realizes the approaching movement to the target in two steps: first, the head, the eye and the moving robot chassis are rotated so that the head and the moving robot are facing the target, and the head observes the target in the optimal observation posture; second, the movement is controlled. The robot moves linearly in the target’s direction. During the movement, the angles of the head and the eye are fine-tuned, and the 3D coordinates of the target are detected in real time until the z coordinate of the target in the robot coordinate system is less than the threshold set to stop the motion.

Figure 13 shows the results of the approaching gaze point tracking experiment. Figure 13a,b show the u and v coordinates of the target on the left image, respectively, and Figure 13c,d show the u and v coordinates of the target on the right image, respectively. The desired image coordinates are recalculated based on the optimal observation position. Figure 13e–h show the positions of the tilt motor (M_lu) of the left eye, the pan motor (M_ld) of the left eye, the tilt motor (M_ru) of the right eye and the pan motor (M_rd) of the right eye, respectively. Figure 13i shows the positions of the pan motor (M_hd) of the head. Figure 13j shows the angle deviation and rotation. T-h is the angle between the head and the target, T-r is the angle between the robot and the target, R-r is the angle of the robot’s rotation from the origin location and T-o is the angle of the target to the origin location. Figure 13k shows the coordinates (^wx, ^wz) of the target in the world coordinate system. Figure 13l shows the robot’s forward distance and the distance between the target and the robot.

The change in the image’s coordinate curve indicates that the coordinates of the target in the left and right images move from the initial position to the central region of the image and stabilize in the center region of the image during the approach process. In the process of turning towards the target in the first step, the target coordinates in the image fluctuate because the head motor rotates a large amount and is accompanied by a certain vibration during the rotation, which can be avoided by using a system with better stability. The variety curve of the motor position in Figure 13 shows that the motion of the motor can track the target well with the desired pose, and the prediction of the 3D coordinates is not used during the tracking process, so this prediction is accompanied by a cycle lag. The changes in angle in Figure 13 show that the robot system achieves the task of steering toward the target in the first few control cycles and then moves toward the target at a stable angle. Figure 13a shows the change in the coordinates of the target in the robot coordinate system. When the robot rotates, fluctuations arise around the measured x coordinate, mainly due to the measurement error caused by the shaking of the system. The experimental results in Figure 13b show that the robot’s movement toward the target is very consistent. During the approach process, the target can be kept within ±50 pixels of the desired position in the horizontal direction of the image while being within ±20 pixels of the desired position in the vertical direction of the image. The eye motor achieves fast tracking of the target in 1.5 s. The angle between the target and the head is reduced from 20° to 0°, and the angle between the target and the robot is reduced from 35° to 0°. The robot then over-turns. At 34°, the target changes by 34° from the initial position.

Through the above analysis, it can be found that by using the combination of the head, the eye and the trunk in the present method, the approach toward the target can be achieved while ensuring that the robot is gazing at the target.

6. Conclusions

This study achieved gaze point tracking based on the 3D coordinates of the target. First, a robot experiment platform was designed. Based on the bionic eye experiment platform, a head with two degrees of freedom was added, using the mobile robot as a carrier.

Based on the characteristics of the robot platform, this paper proposed a method of gaze point tracking. To achieve in situ gaze point tracking, the combination of the eyes, head and trunk is designed based on the principles of minimum resource consumption and maximum system stability. Eye rotation consumes the least amount of resources and has minimal impact on the stability of the overall system during the exercise. The head rotation consumes more resources than the eyeball but fewer than the trunk rotation. At the same time, the rotation of the head affects the stability of the eyeball but only minimally affects the stability of the entire robotic system. The resources consumed by the rotation of the trunk generally predominate, and the rotation of the trunk tends to affect the stability of the head and the eye. Therefore, when the eye can observe the target in the optimal observation posture, only the eye is rotated; otherwise, the head is rotated, and when the angle at which the head needs to move exceeds its threshold, the mobile robot rotates. When approaching gaze point tracking is performed, the robot and head first face the target and then move straight toward the vicinity of the target. Based on the proposed gaze point tracking method, this paper provides an expected pose calculation method for the horizontal rotation angle and the vertical rotation angle.

Based on the experimental robot platform, a series of experiments was performed, and the effectiveness of the gaze point tracking method was verified. In our future works, a practical task of delivering medicine in a hospital and more detailed comparative experiments, as well as discussions with other similar studies, will be implemented.