Real-time position and orientation measurement with occlusion handling for distributed optical large-scale metrology systems

Abstract. Occlusion is a major problem for real-time position and orientation measurement with distributed optical large-scale metrology systems. This paper presents two novel methods with occlusion handling to address this issue, which should be used in combination for practical applications. These two approaches are based on the constraints established by three control points and six control points, respectively, and then the position and orientation can be calculated through iterative optimization algorithms. In this paper, all the work is carried out by using the workspace measuring and positioning system as a verification platform. The experimental results show that the orientation accuracies of the three-point method and the six-point method are kept within 0.1 and 0.04 deg, respectively, and the position accuracy exceeds 0.15 and 0.08 mm.


Introduction
2][3][4] Recently, optical measurement technology, as an advanced means of measurement, is widely applied in modern large-scale industry with its portability, noncontact, high precision, and large measurement range.Considering the need for accurate, flexible, and efficient solutions of real-time large-scale position and orientation measurement, several distributed large-scale metrology systems 5,6 based on optical techniques are currently available, ranging from digital photogrammetry, 7,8 the indoor global positioning system (iGPS), 9,10 to the workspace measuring and positioning system (wMPS). 11The operating theories of these distributed optical systems for position and orientation measurement are essentially the same: having a series of measuring stations working cooperatively, the optical information is collected to determine the coordinates of a set of control points attached to the moving object; subsequently, the position and orientation of the object can be calculated. 12t is intuitively clear that the basic problem to be solved is coordinate measurement of a spatial point in the workspace.With regard to the distributed metrology systems mentioned previously, the coordinate of a spatial point is obtained by ensuring that the optical information from two or more measuring stations is acquired.However, this requirement cannot be satisfied in most cases because of the occlusion caused by many factors, including the complex structure of the moving object, the obstacles and physical obstructions in the working volume, and the limited field of view of the optical devices.
When occlusion occurs in practical applications, it is often the case that we can get information from only one measuring station for each point and sometimes even none for some points.Consequently, measurement cannot be accomplished.
In order to overcome this problem, a lot of work has been conducted and several alternative solutions have been proposed.A straightforward approach is to improve the connectivity of the system through using a large number of transmitting and receiving devices, but it is not a preferred selection according to the cost efficiency.4][15] Although these optimal methods have the potential to solve the problem, they all suffer because (1) the preprocessing feature of them makes measurement complex and time-consuming, (2) when the trajectory of the moving object is changed, these methods may not be available for their poor flexibility, and (3) most of the algorithms are studied and developed on the basis of computer simulations without practical test in applications.
This paper presents two real-time position and orientation measurement methods with occlusion handling for distributed optical large-scale metrology systems, which should be used in combination for practical applications.All the work is carried out by using the wMPS as a verification platform in this paper.With the two proposed methods, three control points and six control points are used to establish the constraints respectively.Then, the position and orientation measurement can be accomplished conveniently and accurately even if occlusion occurs.In addition, since all the processes are on-line, the proposed approaches do not need complex preprocessing procedure and also present good adaptability for trajectory adjustment.
This paper is organized as follows.In Sec. 2, the wMPS, as the verification platform, is described and its operating features are outlined.Section 3 describes the two proposed methods for real-time position and orientation measurement with occlusion handling, including the mathematical model and the establishment of the constraints.In Sec. 4, experiment is performed to verify the feasibility and accuracy of the proposed method.At last, discussion and conclusion are given.

wMPS Technology and Operating Features
The wMPS is a laser-based measurement device for largescale metrology applications, which is developed by Tianjin University, China.As shown in Fig. 1, a typical setup of the wMPS is composed of transmitters, receivers, signal processors, a scale bar, and a terminal computer.
The transmitter consists of a rotating head and a stationary base.With two line laser modules fixed on the rotating head and several pulsed lasers mounted around the stationary base, the transmitter generates three optical signals: two fan-shaped planar laser beams rotating with the head and an omnidirectional laser strobe emitted by the pulsed lasers synchronously when the head rotates to a predefined position of every cycle.The receiver captures the three signals and then converts them into electrical signals through a photoelectrical sensor.The signal processor distinguishes between the electrical signals obtained from different transmitters and then extracts the information of the laser planes from them.Subsequently, using wireless Ethernet, the information is sent to the terminal computer to calculate the coordinates of the receiver.
It is noteworthy that the locations of the transmitters should be determined before the start of the measurement, which is a part of the setup procedure.This is achieved by a calibration algorithm known as bundle adjustment. 16,17ith this algorithm, the positions and orientations of the transmitters with respect to the global coordinate frame can be calculated by using a calibrated scale bar.Once the system setup is completed, the measurement can be performed.The transmitters distributed around the working volume rotate at different speeds to allow the signals from them to be differentiated.When the laser planes emitted from at least two transmitters intersect at a receiver, the equations of the planes are exactly known from the information captured by it.Then the coordinates of the receiver can be obtained by leastsquares solution of these plane equations.
Compared with the iGPS, another instrument designed for large-scale metrology, the wMPS uses the same operating mode, that is, distributed optical measurement based on rotary-laser scanning.But the measurement principles and the mathematical models of them are essentially different.The iGPS is based on multiple angle measurements, while the wMPS discussed in this paper is based on multiplane intersection.Nevertheless, they are both good selections for large-scale orientation and position measurement with some powerful features, especially the multitasking capability.
In order to determine the position and orientation, the conventional method with the wMPS is measuring the global coordinates of three or more receivers attached to the moving object simultaneously, which is based on the location principle of the wMPS.Then, the position and orientation of the object can be calculated directly through a mathematical method such as quaternion algorithm. 18,19However, if occlusion occurs during measurement, it is often the case that the receivers cannot be located because they can capture signals from only one or even none of the transmitters.Consequently, the conventional method is not feasible.Therefore, we present two methods with occlusion handling to address this problem.The details will be described in the section below.

Position and Orientation Measurement with
Occlusion Handling

Measurement Schematic and Mathematical Model
The measurement schematic of the proposed methods is the same as the conventional method, which is shown in Fig. 2.
NðN ≥ 2Þ transmitters are distributed around the working volume to construct a measurement network, and MðM ≥ 3Þ receivers are integrated with the moving object to create a coordinate frame that moves with it.The coordinates of these receivers in the object coordinate frame are precalibrated.
According to the measurement schematic described previously, the mathematical model constructed with the   3.For simplicity, the transmitter can be treated as two planes rotating around a public axis in anticlockwise direction and an omnidirectional pointolite emitting laser pulses with a fixed frequency at the origin point.The receiver can also be simplified as a mass point P i at the optical center of its photoelectrical sensor.
The local coordinate frame of the transmitter is defined as follows: the rotation shaft of the two laser planes is defined as axis z tn .The origin o tn is the intersection of laser plane 1 and axis z tn .The axis x tn is in laser plane 1 at the initial time (the time when the pulsed lasers emit the omnidirectional laser strobe) and perpendicular to axis z tn .The axis y tn is determined according to the right-hand rule.
The equations of the two laser planes in o tn − x tn y tn z tn at the initial time can be represented with three characteristics:

and
Δd n , which are precalibrated as intrinsic parameters of the transmitter. 20n n1 and n n2 are the normal vectors of the two laser planes at the initial time, respectively.Δd n is the deviation between the two laser planes along the axis z tn .At the initial time shown in Fig. 3(a), the receiver captures the omnidirectional laser strobe and starts the timer.As shown in Fig. 3(b), when laser plane 1 passes through the receiver point P i , the time t n1i is recorded.Assuming that the angular velocity of the transmitter is ω, the corresponding normal vector of it can be given by In a similar way, for laser plane 2 Therefore, when the two laser planes pass through the receiver successively, corresponding equations of them in o tn − x tn y tn z tn can be expressed as where As mentioned previously, the positions and orientations of the transmitters with respect to the global coordinate frame are calibrated by bundle adjustment.Consequently, the equations of the laser planes in the global coordinate frame can be obtained as In Eq. ( 4), the coefficient can be calculated through Eq. ( 5).
where R g tn and T g tn are rotation matrix and translation vector, which relate the local coordinate frame of the n'th transmitter to the global coordinate frame.

Determine the Position and Orientation with
Three-Point Distance-Plane Constraint Assuming that occlusion occurs, three noncollinear ones of the receivers attached to the moving object can capture the optical signals from one transmitter respectively.Then the position and orientation of the object can be calculated through the algorithm based on the three-point distanceplane constraint described below.We define the corresponding coordinate pairs of the three receivers as fP oi ; P gi g; ði ¼ 1; 2; 3Þ.P oi ¼ ð x oi y oi z oi Þ T is the coordinates related to the object coordinate frame, which is precalibrated before measurement.P gi ¼ ð x gi y gi z gi Þ T is the unknown coordinates related to the global coordinate frame.Assuming that the i'th receiver captures the signals from the n'th transmitter, the three-point distance-plane constraint can be established by using the plane equations from Eq. ( 4) and the distances between the three receivers, which can be expressed as where n ∈ ð1; 2; • • • ; NÞ; i; j ∈ ð1; 2; 3Þ; i ≠ j.From Eq. ( 6), we can make an optimal objective function.
The coordinates of the three receivers can be obtained by solving Eq. ( 7), and then the position and orientation of the object can be calculated by using a mathematical method known as quaternion algorithm. 18,19For this nonlinear objective function, Eq. ( 7) should be solved by an iterative optimization method such as Levenberg-Marquardt algorithm. 21,22The problem of initial value selection then arises.In order to overcome this problem, we use the coordinates of the three points measured at the moment before occlusion occurs as the initial value for the first measurement, and then during occlusion the earlier result obtained by the proposed method can be used as the initial value for the next measurement.

Determine the Position and Orientation with
Six-Point Multiplane Constraint The method described above has presented a convenient solution based on the three-point distance-plane constraint.However, it does not work for all cases.If occlusion exists in the whole working volume constantly, the initial value of the iterative process cannot be determined, and the threepoint method will be unavailable.Therefore, we propose another method based on the six-point multiplane constraint to address this issue.The requirement of this method is that six of the receivers attached to the moving object can capture the optical signals from one transmitter respectively.Similar to the three-point method, we define the corresponding coordinate pairs of the six receivers as fP oi ; P gi g; ði ¼ 1; 2 • • • 6Þ.Then the position and orientation of the moving object can be given by where is the rotation matrix of the object coordinate frame with respect to the global coordinate frame, and T g o ¼ ð t x t y t z Þ T is the translation vector.
Assuming that the i'th receiver captures the signals from the n'th transmitter, the six-point multiplane constraint can be established by substituting Eq. ( 8) into Eq.( 4).

> > <
> > : where n ∈ ð1; 2; The R g o and T g o to be determined include 12 unknown parameters.From Eq. ( 9), each receiver gives two equations.Therefore, the R g o and T g o can be determined by the solution of the linear equations provided by the six receivers.However, in practical applications, R g o determined with this method does not in general satisfy the orthogonal constraint condition.Thus, we consider Eq. ( 9) and the orthogonal constraint of R g o .

> > > > > > <
> > > > > > : To make an optimal objective function, where M is the penalty factor.Minimizing Eq. ( 11) is a nonlinear minimization problem, which can be solved by using an iterative optimization method like Levenberg-Marquardt algorithm. 21,22The initial value for the iterative method can be calculated through the linear solution of Eq. ( 9).

Experiment and Results
According to recent study, 11 the wMPS can present high accuracy for coordinate measurement in a large volume.As an off-the-shelf device, it has been successfully used in industrial applications for real-time target detection with the conventional method. 23Therefore, a comparison experiment is conducted with the wMPS between the conventional method and the proposed methods.

Setup of the Verification Platform
The experimental setup used to validate the proposed methods is shown in Fig. 4. As illustrated in Fig. 4, a target object is used in the experiment, whose position and orientation are measured for comparison.Six receivers are fixed on the object to provide a coordinate frame for it, and the coordinates of the receivers in this coordinate frame were experimentally identified using a precision coordinate measurement machine.P o1 ¼ ð 0.0000 0.0000 0.0000 Þ T mm;

Experiment Procedure and Results
As shown in Fig. 4, the target object was in conjunction with the end-effector of the robot, whose position and orientation changed accordingly when the robot moved.During the experiment, the robot was moved to 10 different locations in the working volume, and the corresponding positions and orientations of the object were measured by the conventional method and the proposed methods for comparison.At each location, an experiment was performed in the same way   by using a removable obstacle.First, without the obstacle, each receiver captured the signals from both transmitters, and the position and orientation of the object were recorded by the conventional method as a reference.After that, with the robot keeping still, we placed the obstacle between the transmitters and the object to simulate occlusion in practical applications.Optical signals from the transmitters to the receivers were partially interrupted by the obstacle, and the position and orientation of the object were then measured by using the three-point method with receivers 1, 3, 5 and the six-point method with all six receivers.The position and orientation of the object are given by the translation vector T g o ¼ ð t x t y t z Þ T and the rotation matrix R g o , which relate the object coordinate frame to the global coordinate frame.In order to compare the results intuitively, R g o is represented by three single rotation angles as usual, which are pitch ω, yaw φ, and roll κ.Then, the deviations between the positions and orientations measured by the two proposed methods and the conventional method are shown in Figs. 5 and 6, respectively.
As we can see from Fig. 5, the deviations of the rotation angles with the three-point method are kept within 0.1 deg, and the position accuracy exceeds 0.15 mm.Also, from Fig. 6, the orientation accuracy and position accuracy of the six-point method exceed 0.04 deg and 0.08 mm, respectively.The experimental results demonstrate that the proposed methods are entirely feasible when occlusion occurs and also exhibit good accuracy.
Another experiment was also designed to compare the measurement results for both methods when the obstacle is removed.Without the obstacle, we controlled the robot to make it move to 10 different locations in the working volume.At each location, the corresponding positions and orientations of the object were measured by the three-point method with receivers 1, 3, 5 and the six-point method with all six receivers, and then compared with the ground truth obtained by the conventional method.The comparison results are shown in Figs.7 and 8.
From Figs. 7 and 8, it is clear that when the obstacle is removed, we come to the same conclusion as the measurement results with the obstacle.Furthermore, it is worthy to note that the accuracy of the six-point method is superior to the three-point method, which is caused by the redundant data provided by more control points.

Discussion
Concerning the two methods proposed in this paper, there are some issues to be discussed as below.
1.The two methods are appropriate to different cases because of their individual characteristics.The threepoint method needs less control points, but it will be unavailable if occlusion exists in the whole working volume constantly because the iterative initial value cannot be determined.The six-point method is not limited by the condition of occlusion, but more control points should be used.Furthermore, from the experimental results, the accuracy of the six-point method is superior to the three-point method due to the redundant data provided by more control points.Therefore, in practical applications, these two methods should be used in combination for an efficient compromise among the number of available control points, the feasibility of the mathematical calculation, and the measurement accuracy.2. With regard to these two methods, it is worthy to note that if redundant data are obtained from more control points, we can reduce uncertainty and achieve a higher measurement precision.3. The proposed methods can also be applied to distributed optical large-scale measurement systems other than wMPS, such as iGPS and stereo vision system, by simply changing the communication model and the constraint equations.

Fig. 1
Fig. 1 System configuration of the workspace measuring and positioning system.

Fig. 2
Fig. 2 Schematic of position and orientation measurement with the proposed methods.

Fig. 3
Fig. 3 Mathematical model with the n'th transmitter and the i'th receiver.(a) The initial position of the two laser planes.(b) The position of the two laser planes at the time when they pass through the receiver point P i successively.

Fig. 4
Fig. 4 Experimental setup to validate the proposed methods.

Fig. 5
Fig. 5 Measurement deviations of the three-point method.(a) The rotation angle deviations.(b) The translation deviations.

Fig. 6
Fig. 6 Measurement deviations of the six-point method.(a) The rotation angle deviations.(b) The translation deviations.

Fig. 7
Fig. 7 Comparison of the orientation measurement of the two methods: (a) the pitch deviations, (b) the yaw deviations, and (c) the roll deviations.

Fig. 8
Fig. 8 Comparison of the position measurement of the two methods: (a) the t x translation deviations, (b) the t y translation deviations, and (c) the t z translation deviations.

6 Conclusions
Two real-time position and orientation measurement methods with occlusion handling have been presented for distributed optical large-scale measurement systems, which should be used in combination for practical applications.These techniques are based on the constraints established by three control points and six control points, respectively, and the measurement principles are expounded in detail.The feasibility and accuracy of the proposed methods are verified by comparing the results of position and orientation measurement with the conventional method.The experiment reveals that the orientation deviations of the three-point method and the six-point method are kept within 0.1 and 0.04 deg, respectively, and the position accuracy exceeds 0.15 and 0.08 mm.It clearly demonstrates that the methods are feasible and also exhibit good accuracy.The proposed approaches address the occlusion problem of the conventional position and orientation measurement method, and expand the actual application of distributed optical largescale measurement systems.