Omnidirectional angle constraint based dynamic six-degree-of-freedom measurement for spacecraft rendezvous and docking simulation

In this paper we present a novel omnidirectional angle constraint based method for dynamic 6-DOF (six-degree-of-freedom) measurement. A photoelectric scanning measurement network is employed whose photoelectric receivers are fixed on the measured target. They are in a loop distribution and receive signals from rotating transmitters. Each receiver indicates an angle constraint direction. Therefore, omnidirectional angle constraints can be constructed in each rotation cycle. By solving the constrained optimization problem, 6-DOF information can be obtained, which is independent of traditional rigid coordinate system transformation. For the dynamic error caused by the measurement principle, we present an interpolation method for error reduction. Accuracy testing is performed in an 8  ×  8 m measurement area with four transmitters. The experimental results show that the dynamic orientation RMSEs (root-mean-square errors) are reduced from 0.077° to 0.044°, 0.040° to 0.030° and 0.032° to 0.015° in the X, Y, and Z axes, respectively. The dynamic position RMSE is reduced from 0.65 mm to 0.24 mm. This method is applied during the final approach phase in the rendezvous and docking simulation. Experiments under different conditions are performed in a 40  ×  30 m area, and the method is verified to be effective.


Introduction
In recent years, autonomous spacecraft rendezvous and docking technology has been a popular research topic because of its essentiality in spacecraft control. The measurement accuracy, volume and mass of the measurement unit are crucial concerns in this field [1]. Our work is focused on all aspects, attempting to both improve the measurement acc uracy and miniaturize the sensor unit.
Several methods have been proposed such as INS (iner tial navigation system), photogrammetry, LIDAR, and laser tracker. However, on account of the integral operation, the measurement errors of INS accumulate over time, and it is prone to drift. Some compensation methods have been pro posed to reduce the measurement error, but they are not ideal in practice because of the compensation uniqueness [2][3][4][5]. As a result, INS is rarely used alone.
Photogrammetry provides alternative solutions for 6DOF (sixdegreeoffreedom) measurement based on a single camera or multiplecameras. Hui et al introduced a 6DOF estimation algorithm for cooperative space targets based on monocular vision [6]. You et al developed a binocular vision system for spacecraft positioning. The maximum standard deviation was less than 2 mm in 4 m [7]. Feng et al proposed a stereovisionbased relative orientation estimation method for the rendezvous and docking of noncooperative satel lites. The position error was 6.4 mm and attitude angle error exceeds 0.34° [8]. Dahlin demonstrated a vision navigation method for orbital rendezvous and docking. The angle error was within 0.1° but the position error exceeded 25 mm [9]. Photogrammetry method has better accuracy and stability than INS. Nevertheless, it is easily affected by light interference and object occlusion. Meanwhile, photogrammetry method is based on image processing, which is often timeconsuming and results in poor dynamic measurement capability.
The emergence of LIDAR has led to a new generation of visionbased rendezvous and docking measurement sys tems. Ruel et al demonstrated an active TriDAR (triangula tion + LIDAR) 3D sensor and efficient modelbased tracking algorithm to provide 6DOF information about the spacecraft. The position error was 28.9 mm and the attitude angle error was 1.3° [10]. Jasiobedzki et al developed an autonomous satellite rendezvous and docking measurement method using LIDAR and modelbased vision. The maximum translation and rotation errors were 4.4 mm and 0.25° respectively [11]. LIDAR has the longest measurement range, but it is usually hard to delivery high absolute accuracy.
6DOF probe cooperated with laser tracker is another method that has been developed commercially, such as Tmac [12]. It is noted for its high accuracy and fast dynamic response. It is often used as benchmark. In large space, mul tiple stations are needed because for measurement visibility. Nonetheless, laser trackers are extremely expensive and have certain requirements on their working environment. The measurement efficiency is relatively low.
Combined measurement methods are also widely used [13][14][15][16][17]. Ideally, the position error is meterlevel and the orientation error exceeds 0.3° outdoors. Qu et al designed a realtime measurement system with vision/INS for close range semiphysical rendezvous and docking simulation. The standard deviation in the relative position was 6.4 mm and the standard deviations in the relative attitude angles were within 0.0315° [18]. The combined measurement methods give full play to the advantages of each subsystem and avoids the dis advantages, but the complicated calibration and alignment processes prevent its prevalence.
Distributed photoelectric scanning measurement tech nology has been further studied and developed rapidly in recent years. It has been widely used in digital manufacturing and assembly. wMPS (workshop measurement and posi tioning system) and iGPS (indoor global positioning system) are examples consisting of transmitters with different rota tion speeds, photoelectric receivers, a signal processor, and a terminal computer. With the advantages of high accuracy, fast response, multitasking and strong extensibility, they are well received. The spherical receiver of wMPS (figure 1(a)) is 38.1 mm in diameter and interchangeable with the laser tracker sphericallymounted retroreflector (SMR). The machining accuracy is 0.02 mm and the alignment accuracy of the photosurface is better than 0.01 mm. The receiving sen sors of iGPS have two types: the singlephotosurface sensor ( figure 1(b)) and the vector bar ( figure 1(c)).
Unlike singlestation measurement systems such as the total station and laser tracker, the accuracy of a distributed measurement network is determined by the transmitter dis tribution rather than the measurement distance. The accuracy increases if more signals are received. Station transforma tion and error accumulation are avoided. Generally, wMPS and iGPS output the 3D coordinates of the static measured receiver if it obtains signals from more than two transmitters. As for a dynamic receiver, the measurement error is extremely obvious and nonnegligible.
Based on coordinate calculations and rigid body trans formations, iGPS can be used for 6DOF measurement. However, this method may fail to work in restricted space because of laser plane intersection failure. We propose a novel dynamic 6DOF measurement method for spacecraft rendezvous and docking simulation based on omnidirec tional angle constraint. Photoelectric receivers are fixed to the measured spacecraft and they are in a loop distribution. Each receiver indicates an angle constraint direction. With the spacecraft moving in the measurement space, the photo electric receivers obtain signals from the rotating transmitter lasers. Omnidirectional angle constraint equations can then be established. If each receiver obtains the signal from at least one transmitter, the 6DOF of the spacecraft can be calculated iteratively with the optim ization algorithm. Independent of coordinate calculation and rigid body transformation, omni directional angle constraint method has stronger applicability than iGPS. The calibration of measurement network can be finished within 20 min and the accuracy is traced to length benchmark in industrial site. For the dynamic error caused by receiver movement, an interpolation method is introduced to improve the dynamic acc uracy. Experiments have verified its effectiveness.
This paper is organized as follows: section 2 introduces the wMPS measurement model and network construction. Section 3 presents the 6DOF measurement principle and mathematical solution. Section 4 describes the interpolation method to synchronize transmitters and reduce the dynamic measurement error. In section 5, two experiments are con ducted to test the proposed method. Finally, we present the conclusions and prospects for future improvement.

Measurement model and network construction
Each transmitter consists of a rotating head and a static base. Two linear laser modules are fixed in the rotating head and emit planar laser beams (scanning signals) with an angle of 90° between them. In the base, several synchronization laser modules are distributed circumferentially and emit pulses (synchronization signals). The floor transmitter has a typical configuration, located on a tripod (figure 2(a)). The scanning angle is 0-360° horizontally and −22.5° to +22.5° vertically. The floor transmitter is similar to the iGPS transmitter. In this paper, the marble platform used for the rendezvous and docking simulation is about 40 × 30 m. We designed the roof transmitter (figure 2(b)) considering the application environ ment and light occlusion caused by other equipment, which is unsupported for iGPS.
The roof transmitter emits laser beams downward. The coefficients for each plane a ki b ki c ki d ki (i: laser plane index and k: transmitter index) are calibrated after the transmitter is assembled. The transmitter coordinate system, also called the local coordinate system (LCS), is defined as follows: the axis of rotation is Zaxis. The intersection point of plane 1 and the Zaxis is defined as the origin O. The Xaxis is in plane 1 at the initial time and vertical with respect to Zaxis. The direc tion of Yaxis is determined according to the righthand rule. The photoelectric receiver can be seen as a mass point. When the transmitter works, the head rotates counterclockwise at the speed of 25-50 rev s −1 (revolutions/second). Each time plane 1 rotates across the initial position, the receiver obtains the synchronization signals and records the initial time t k . The time when the receiver obtains the scanning signals (plane 1 and plane 2) is recorded as t k1 and t k2 . Then the rotation angle is described by: ω k represents the rotational angular speed of transmitter k. The parameters of planes 1 and 2 in the LCS are changed:  For an observed point (x l , y l , z l ) in the LCS, the equations of planes 1 and 2 can be listed as follows: Generally, the coordinates in the global coordinate system (GCS) are more meaningful than those in the LCS. Therefore, the orientation and position relationship from the GCS to the LCS is calibrated, described by R gl and T gl are the rotation and translation matrices from the GCS to the LCS respectively. If a receiver obtains signals from more than two transmitters, the coordinates (x g , y g , z g ) can be determined. The coordinate accuracy is positively correlated with the number of transmitters scanning over the receiver. According to the model in figure 3, the roof transmitter has a cone projection area of 53°, in which 15° is the blind area. The rotational speed stability of the roof transmitter is within ±1 rev min −1 and the angle measurement accuracy is better than 2 arc seconds. The operating distance of the roof transmitter is 5-25 m.   NO. To evaluate the distribution of accuracy in 40 × 30 m area, a MonteCarlo simulation of the accuracy is made. The layout of the ten transmitters in the GCS and the simulation results are listed in table 1 and figure 4: The effective measurement zone in this layout almost covers the entire 40 × 30 m area, and the simulation errors in most of the region are better than 0.15 mm. The maximum error is less than 0.25 mm. The layout satisfies the measure ment requirements.

Spacecraft coordinate system construction
To measure the 6DOF information, it is necessary to construct a coordinate system on the measured spacecraft. On the top of the spacecraft, six receivers are fixed as reference points. The coordinates of the reference points are measured by the laser tracker at four different positions as depicted in figure 5. The distance and angular errors are optimized according to the constraints provided by the laser tracker. The coordinates of  (3.3) For receiver m (x m y m z m ) in the spacecraft coordinate system, the distance between the reference point and the laser plane can be defined as: In the equation above, the unknowns are rotation matrix R and translation matrix T which indicate the transformation from the spacecraft LCS to the GCS. θ, γ and ψ are Euler angles.
the reference points (x m , y m , z m ) in the spacecraft coordinate system can be obtained [19].

Algorithm and optimization
According to (2.4), the transformation from the LCS to the GCS can be expressed as: Furthermore, due to the orthogonality of the rotation matrix, (3.5) satisfies the equations below: The 6DOF measurement problem can be formulated as an optimization based on the penalty function method. The objective function can be described by: Only if each receiver obtains the signal of at least one trans mitter can, the optimization problem be resolved by the Levenberg-Marquardt algorithm [20]. The attitude angle can be calculated through R, and the position information is in T . In the measurement process, receivers may get the signals from transmitters in the entire space. Generally, the accuracy of the optimization result is positively correlated with the number of signals each receiver obtains.

Initial iteration value calculation
For the Levenberg-Marquardt algorithm, it is necessary to provide a proper initial iteration value to make the optim ization result convergent and reduce the iteration times. d mki can be expanded and constitute a system of equations such that AX = D · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · a gkθ1 x m a gkθ1 y m a gkθ1 z In the transmitter model, Xaxis is located in plane 1, so d mk1 should be zero. Additionally, with the assembly technique, plane 2 can be adjusted to the position that approximately passes through the original O. d mk2 can also be seen to be zero. Thus, the initial value Xcan be produced by solving the homo geneous linear equation system: (3.12) By the singular value decomposition on A, matrices U and V can be obtained. According to the least squares principle, the singular vector in matrix Vcorresponding to the minimum sin gular value in matrix S is the least squares solution of (3.12).
The initial value of the iteration is now produced. Substituting it into the object function, the 6DOF measure ment can be solved.

Dynamic error and reduction
In 6DOF measurement, the dynamic error is mainly caused by the measurement principle. The raw data is the time when the receiver obtains the synchronization signals and scanning signals from different transmitters. During the movement of the spacecraft, a receiver gets signals from more than one transmitter. However, each transmitter emits synchronization signals at different times, and the scanning signals also reach the receiver in a certain order. Thus, the dynamic receiver is in different positions during a measurement process, which may cause absolute errors. Take two transmitters for example. Figures 6(a) and (b) show the measurement process for static and dynamic receivers.
Essentially, the dynamic error is caused by the time differ ences between the synchronization signals and the scanning signals. It is affected by the velocity and the rotational speed of the transmitters. In the first case, experiments about the relationship of the velocity and dynamic error have already been done. They have a linear relationship, which agrees with the Matlab simulation results. The error will get larger with increasing the velocity. Secondly, the dynamic error is nega tively related with the rotating speed. However, the transmitter shaft will suffer severe abrasion and the lifetime of comp onents will decrease with high rotational speeds. Taking into account these factors, it is set as 25-50 rev s −1 . To reduce the measurement error, an effective way is to build a timed shaft and set a series of time nodes according to the measurement frequency, as is shown in figure 7. Calculating the rotation angles of each transmitter at the time nodes, transmitter syn chronization can be achieved.
It has been verified by experiments that the scanning times t k1 and t k2 will be continuous and smooth if the trajectory of the receiver is continuous. Based on this conclusion, Lagrange's interpolation is applied to calculate the scanning angle at any time. The interpolation polynomial is written as: L n (t) is used to replace the actual polynomial. The interpola tion error is described by: Taking equidistant interpolation for instance, With increasing the interpolation interval h, the interpolation error R n (t) will also increase. To avoid the Runge's phenom enon, the error decreases with the increasing of n, but the calcul ation will become more complicated [21]. Weighing these factors, we choose n to be 1 or 2: n = 1, the movement of receiver is seen as uniform linear motion.
n = 2, the movement is seen as uniformly variable linear motion.
(4.7) In this way, we can obtain the value of the polynomial at any moment and the signals from all transmitters are aligned to the time node. The synchronization error of the dynamic 6DOF measurement can be reduced.

Experiment
To verify the method for dynamic 6DOF measurement, two experiments were conducted.

Accuracy testing experiment
Experiment 1 was conducted for accuracy testing. The exper imental setup is shown in figure 8.
Six photoelectric receivers were fixed to the automated guided vehicle (AGV). The coordinate system of the AGV was calibrated according to the method stated in section 3.1. Laser tracker Tmac was also fixed to the AGV. Tmac outputs the 6DOF information in the laser tracker system. In this experi ment it was used as the reference because of its high accuracy in dynamic measurement. Before the experiment, the wMPS measurement field GCS was unified with the laser tracker system. The relative orientation and position of the AGV and Tmac was also calibrated with the laser tracker. Thus, the out puts of the wMPS and Tmac could be compared in the laser tracker coordinate system after a coordinate system transfor mation shown in figure 9: The laser tracker and wMPS were triggered by the soft ware, and the measurement frequency was synchronized at 20 Hz. The AGV was moved along a curvilinear path at a speed of 0.2 m s −1 , which was consistent with the maximum velocity of the tracker at the final approach. The experiment was conducted twice: once with synchronization and the other without. The dynamic error is evaluated through rootmean square errors (RMSEs) given by:  The curve with rhombi and curve with asterisks represent the measurement errors without and with synchronization, respectively. After transmitter synchronization, the orienta tion error decreases from 0.077° to 0.044°, 0.040° to 0.030°, 0.032° to 0.015° in each axis. The position error decreases from 0.31 mm to 0.15 mm, 0.42 mm to 0.14 mm, 0.38 mm to 0.12 mm in each axis. The dynamic measurement accuracy is improved and the effectiveness of the proposed method is verified.

Spacecraft rendezvous and docking simulation experiment
Experiment 2 was conducted in the rendezvous and docking simulation laboratory. The experimental area is a 40 × 30 m marble platform. The simplified experiment setup is shown in figure 11.
The tracker and target were placed on the platform. The normal of the platform was parallel to the direction of gravity. The target was static. The tracker has 6DOF and moves toward the target with the aid of an airflotation system. The initial speed of the tracker was 0.2 m s −1 at the starting point, about 40 m away from the target. It moved along a planned trajectory and experienced two decelerations at distances of 25 m and 10 m; the velocity was reduced to 0.1 m s −1 and 0.03 m s −1 , respectively. Six receivers were fixed to the top of the tracker. They were arranged horizontally, and the normal of the photosensitive surface was approximately parallel to that of the platform. The coordinates of each receiver in the tracker coordinate system were calibrated with the Leica AT 901 laser tracker as the method stated in section 3.1. The calibration results are listed in table 3.
The GCS in this experiment was defined as the convention of eastnorthup. The position and orientation of the target in the GCS was known before the experiment. When the tracker moved towards the target, the 6DOF of the target was cal culated continuously according to the method above. In the 40 × 30 m area, each receiver obtained signals from more than two transmitters. The measurement accuracy could be guaranteed this way. Referring to the orientation and position of the target, the tracker adjusted itself in in real time. The docking command was sent when the tracker and target were 0.45 m apart. Measurement results throughout the whole pro cess are shown in figure 12.
It is clear that the 6DOF measurement result curves are continuous and stable. No gross error occurs. The rendezvous and docking process was successfully conducted in different states. The feasibility of this method was verified.

Conclusions
Dynamic 6DOF measurement methods with high accuracy are urgently required in spacecraft rendezvous and docking simulation. Because of the complexity of the problem and on site situations (e.g. light occlusion), most existing methods fail to achieve a highly accurate measurement. In this paper we propose a novel method based on the omnidirectional angle constraint. The characteristics of dynamic error are analyzed and an interpolation method is proposed to reduce the dynamic error. As the experiments above indicate, the acc uracy is improved. In a 40 × 30 m area, the dynamic 6DOF   measurement results meet the requirements and provide a basis for feedback control.
In future research, we will focus on improving the meas urement frequency and value initialization method. The initial value for optimization can be obtained through the INS for algorithm simplification. The drift error of INS in a short time can be controlled. Moreover, the INS output can also be used for data fusion with wMPS. Thus, the 6DOF measurement accuracy may be further improved.