1. Introduction
Indoor mobile agents are widely used in various industries, such as logistics environments [
1], military applications [
2], automated manufacturing [
3], commerce [
4], etc. One of the most crucial functions of the indoor mobile agent is to accomplish user-specified tasks. Therefore, highly accurate position and orientation information about the mobile agent is a basis for subsequent precise motion control and path planning in the navigation process.
In the past decades, many methods have been proposed for indoor navigation, which can be roughly divided into two categories: relative positioning and absolute positioning [
5]. Relative positioning methods estimate the current state based on measuring the distance and orientation of the robot relative to the initial state. Dead reckoning is a classical relative positioning technology method. It uses an inertial measurement unit (IMU) or odometer to realize navigation, and it does not need to transmit or receive any external information. The dead reckoning method can work independently and continuously to provide positioning services for mobile agents. However, the drift errors and noises of the IMU significantly affect the accuracy of the navigation over a large period of time [
6], which makes this method only meet the state estimation requirements for a short time, and it cannot realize long-time navigation tasks in a complex environment. Although many improvements have been made to reduce the drift errors and noises, it cannot be applied alone for a long period without any correction strategy. Meanwhile, the dead reckoning method needs to know the initial position and orientation, which is difficult to achieve in actual scenes. Therefore, the estimation of positioning and orientation tends to depend more on absolute positioning methods.
Absolute positioning has attracted much attention for a long time and has made significant progress. It requires multiple reference points to determine the location of the moving agent. A typical representative of outdoor absolute positioning technology is the global navigation satellite system (GNSS). The outdoor positioning technology has been developed rapidly since the GNSS was put into service. We can observe its convenience when driving a car or using location sharing employing the GNSS module embedded in a smartphone. However, compared with the satellite channel of the GNSS outdoors, indoor positioning faces the terrestrial channel problems, which make it more complicated. In urban areas or inside buildings, walls and roofs can block the electronic waves from satellites, which makes GNSS only a practical solution for positioning in the indoor environment. Since GNSS does not work normally inside buildings, various studies have focused on indoor absolute positioning, and indoor absolute positioning has entered a new period of great changes and developments [
7].
Indoor absolute positioning is a hot topic. It has been intensively researched for decades due to its importance and difficulty [
8]. During the past few decades, many methods have been proposed to realize indoor positioning, such as Wi-Fi [
9], Ultra-Wide Band (UWB) [
10], ultrasound-assisted [
11], Bluetooth [
12], etc. Moreover, some of these methods such as the UWB can achieve cm-level positioning accuracy. However, these positioning techniques often suffer interference from Non-Line-of-Sight signal propagation and multipath fading effects. Meanwhile, they cannot provide three-dimensional (3D) position and orientation simultaneously, which are significant defects in the modern navigation applications of indoor mobile agents.
The photoelectric measurement technology has developed rapidly and has been used in a variety of indoor environments, and many feasible methods have been proposed. Laser radar mainly uses an infrared laser beam to scan the surrounding environment with a radial field of view. It is widely used in indoor mobile agent navigation because of its flexibility and real-time measurement capability. However, in some cases, such as in the assembly and docking procedures of automobile and large aircraft components, it is difficult for the laser radar to estimate the position and orientation [
13,
14]. In [
15], a laser-based photoelectric scanning method is used to measure the 3D coordinate of anchors, and then estimate the position and orientation by using a coordinate transformation algorithm. Nevertheless, this method needs to use a sophisticated rotating laser scanning instrument, which is costly and not convenient for large-scale use in the case of multiple indoor mobile agents.
The vision sensor based on charge-coupled device (CCD) or CMOS (Complementary Metal Oxide Semiconductor) adopts a non-contact measurement method, which can estimate the motion parameters of indoor moving objects without disturbing the system. In [
16], a method using the area CCD is presented to estimate the position and orientation. However, in such a system, the 2D and 3D situations should be considered, respectively, and then the number of different anchors will be decided. In [
17], a robot equipped with a camera can be controlled to observe a rectangle object constraint, and then obtain the position and pose. Nevertheless, this system requires that the optical axis of the camera always point to the target center point, which makes the method complex and clumsy. In [
18], a CMOS camera based on Wiimote is used to capture infrared LEDs (Light Emitting Diodes) for indoor mobile robot tracking, then the position and orientation of Wiimote can be estimated by using coordinate transformation, but this method is only suitable for 2D scenarios. The ToF camera is also can be used for indoor positioning [
19,
20], but it cannot work alone for indoor positioning, and needs the assistance of other equipment. Meanwhile, it also cannot estimate the position and orientation simultaneously.
Currently, there are some off-the-shelf commercial devices to measure the position and orientation information of the indoor mobile target using computer vision, such as the VICON [
21]. These systems seem to be the right solution for the navigation of the indoor mobile agent, but such devices are not suitable for changing indoor backgrounds as they fail to work in dark or untextured areas. Meanwhile, these systems are expensive and cannot be easily extended on a large scale. A broader measure space means more requirement of camera measurement equipment, which leads to a higher cost overhead.
The existing visual measurement methods of indoor position and orientation mainly use area CCD or CMOS, and seldom use linear CCD to realize the position and orientation measurement of indoor mobile agents. In [
22], the linear CCD is used to measure the position and orientation of moving objects, but this method needs to use multiple sets of linear CCD measuring equipment. Meanwhile, the position and attitude can only be estimated in a small range of indoor spaces. [
23] proposed a 3D motion tracking system by using multiple linear optical sensor arrays, supplemented by an IMU, to achieves better performance in position and orientation measurement. However, this method requires the usage of the linear CCD and the IMU to estimate position and attitude, respectively. Meanwhile, it can also only achieve position and orientation measurements in a small range of indoor spaces.
Currently, there is still a lack of a cost-effective, economical, and satisfactory solution to realize indoor positioning and attitude estimation concurrently. The key to achieving high accuracy indoor position and orientation estimation in an affordable and wide range system is to use the appropriate sensor(s). Motivated by the specific requirement of the indoor position and orientation problem in the navigation application, we design a novel position orientation sensor (POS). This POS can create a 3D coordinate measurement system based on the intersection of four planes by using four pairs of linear CCDs and the cylindrical lenses. We name this created 3D coordinate system of the POS as the position orientation sensor coordinate system (POSCS). Compared with the traditional indoor sensor, which can estimate the distance or angle between the anchors and the mobile target, our method can estimate the 3D coordinate of the anchor in the field of vision (FOV) of the POS. Then according to at least three anchors in the indoor coordinate system (ICS) and their estimated coordinate value in the POSCS, we can use the Rodrigues coordinate transformation algorithm to calculate the rotation vector and translation vector of the POS. The translation vector contains the position of POS in the ICS, and the rotation vector contains the orientation of POS in the ICS. Finally, we can estimate the position and orientation of the indoor mobile agent simultaneously.
The rest of the paper is organized as follows: In
Section 2, the system components and the working principle of POS are introduced, and then the FOV of the designed POS is simulated. In
Section 3, the Rodrigues coordinate transformation algorithm is presented to estimate position and orientation. In
Section 4, the simulation is performed to demonstrate the feasibility and high accuracy of the proposed method. In
Section 5, a brief overview and advantages of the proposed method are discussed. Finally, the main conclusions of the whole work and future improvements are summarized in
Section 6.
3. Position and Orientation Measurement Algorithm
In our indoor positioning and orientation measurement system, there are two different coordinate systems. One of them is the ICS, and its origin of the coordinate system is set at the corner of the room; while the other is the POSCS and its origin of the coordinate system is set at the POS, as shown in
Figure 2. For every POS, it can create a POSCS on its body and then estimates the 3D coordinate of the anchor in its FOV.
3.1. Mathematical Model
Suppose three anchors A
1, A
2, A
3 are fixed on the ceiling, and their coordinates in ICS are A
1(X
1, Y
1, Z
1), A
2(X
2, Y
2, Z
2) and A
3(X
3, Y
3, Z
3). After acquiring the coordinates of three anchors by using POS, and their coordinates in POSCS are C
1(x
c1, y
c1, z
c1), C
2(x
c2, y
c2, z
c2) and C
3(x
c3, y
c3, z
c3). According to the 3D coordinate value of three anchors in POSCS and ICS, three coordinate transformation equations are depicted to realize the coordinate transformation from POSCS to ICS, as shown in Equations (1)–(3):
In (1), (2), (3), we have established the relationship between two coordinate systems for three anchors, where the R is the 3*3 rotation vector, T= [x0, y0, z0]T is the translation vector. In the coordinate transformation equation, the translation vector T contains the origin position of POSCS in ICS, and rotation vector R contains the rotation angle of the POSCS in ICS, which are exactly what we desired.
Suppose the x, y, and z-axis rotation angle of the POS in ICS are α, β, and γ, respectively. Meanwhile, we set the α as the pitch angle, the β as the roll angle, the γ as the yaw angle, and we define the clockwise rotation angle to be positive and the counterclockwise rotation angle to be negative. Then according to the rotation angle, we can get the rotation matrix that surrounds the x, y, z-axis, as shown in Equations (4), (5), and (6):
We define the rotation vector R = R
y(β)* R
x(α)* R
z(γ), and the result of R is shown in (7), where α = arcsin (R[2, 3]), β = −arctan (R[1, 3]/R[3, 3]), γ = −arctan (R[2, 1]/R[2, 2]):
The key to solving the coordinate transformation in (1), (2), and (3) is to determine the rotation vector R and the translation vector T. If we can acquire the R and T by solving the (1), (2), and (3), that means we can estimate the position and orientation of POS in ICS. So how to solve the rotation vector R and the translation vector T are the key point.
3.2. Rodrigues Coordinate Transformation Algorithm
In order to solve the R and T, three common points are required at least [
27]. The 3D coordinate transformation is one of the most frequently encountered operations in geodesy, mapping, photogrammetry, computer vision, geographical informational science, etc. [
28]. In this paper, we will elaborate on the Rodrigues coordinate transformation algorithm to solve the R and T.
In (8), R is the rotation vector, T is the translation vector. The parameters to be estimated are R and T. According to the mathematical transformation model, R is computed first, followed by the T:
Let the anti-symmetric matrix S is equal to (9), in which a, b, c are independent parameters.
The R is composed of anti-symmetric matrix S and 3*3 unit matrix I, as shown in (10).
From (8), each anchor can list three equations, the equation of the second common point subtract the corresponding equation of the first common point, which can eliminate the translation vector T, then get the Equation (11).
The Equation (10) into (11), then get (12).
We substitute (9) into (12), unfold the equation, then extract the a, b, c expression vector form as shown in formula (13), in which X
12 = X
2 − X
1, Y
12 = Y
2 − Y
1, Z
12 = Z
2 − Z
1; x
12 = x
2 − x
1, y
12 = y
2 − y
1, z
12 = z
2 − z
1:
Obviously, in (13), the left coefficient matrix is a singular matrix and only having two independent equations, which cannot solve a, b, c parameters with two anchors. With the first and the third common points, we can get a similar equation, as shown in (14).
Then we combine (13) and (14), and thus get (15):
Solving Equation (15) we can calculate the three parameters a, b, c. According to the value of a, b, and c, we can calculate the value of the rotation vector R by using (10). Then by substituting R into (8), we can calculate the value of the translation vector T. After the above procedure, we can get the R and T, which contain the orientation angle and position of POS in ICS.
3.3. Multiple Common Points in Rodrigues Coordinate Transformation Algorithm
If the POS detects more than three common points within its field of view at a given position, the problem of estimating the rotation vector and the translation vector can be converted to the least square problem, and the error Equation (16) is listed according to Equation (15).
In error Equation (16), X
3×1= [a, b, c]
T, the matrix of A
3(n−1)×3 and L
3(n−1)×1 are shown in Equation (17) and Equation (18).
According to the principle of least squares, the optimal solution of Equation (16) can be obtained, as shown in Equation (19):
According to Equation (19), three independent parameters in the Rodrigues coordinate conversion algorithm can be obtained in the case of multiple common points. Based on a, b, and c, the rotation vector R can be obtained by Equations (9) and (10).
When the measurement error of the common point is considered, the error Equation (20) is obtained according to Equation (8):
For brevity, suppose a
e, b
e, and T
e are as shown in Equation (21):
Then Equation (20) can be simplified to Equation (22):
Derived from the matrix of least squares, the error matrix E can be minimized under the conditions of (23), where ||.||
2 represents the second-order norm:
Equation (23) is unfolded, as shown in Equation (24):
The partial derivative of Equation (24) with respect to the translation vector T
e is obtained and makes its partial derivative equal to zero. When the error matrix E reaches the minimum, the optimal solution of the translation vector T
e is shown in Equation (25):
According to Equation (25), we can calculate the center of gravity of multiple common points in two coordinate systems, respectively. Then, the optimal translation vector Te can be calculated by using rotation matrix R and the gravity of multiple common points.
4. Performance Evaluation
We evaluated the proposed method by using simulation. The simulation scenario is performed in an indoor system mode, and
Figure 5 illustrates the scene of an indoor positioning and orientation measurement system.
As shown in
Figure 5, the dimension of the system model is 5000 mm × 5000 mm × 3000 mm. Thirty-six infrared LEDs are fixed on the ceiling as anchors. Meanwhile, their coordinates in ICS are shown in
Table 2.
The POS moves under these anchors from P1 to P10 position, as shown in
Figure 5. We set the P1 at [500 mm, 500 mm, 1000 mm] in ICS, and the x, y, z-axis rotation angle of POS in ICS are α = 0°, β = 0°, γ = −25°, so the translation vector T = [500 mm, 500 mm, 1000 mm]
T. According to the coordinate of intersection points on linear CCD1, CCD2, CCD3, and CCD4, the anchors of A
1, A
2, and A
7 are located in the FOV of the POS.
According to the parameters of POS in
Table 1, we can get the coordinate of intersection points of A
1 on CCD1 is m
1 = [78.79 mm, 0 mm, 0 mm]
T in POSCS, A
1 on CCD2 is m
2 = [0 mm, 77.35 mm, 0 mm]
T in POSCS, A
1 on CCD3 is m
3 = [−74.27 mm, 0 mm, 0 mm]
T in POSCS, A
1 on CCD4 is m
4 = [0 mm, −75.70 mm, 0 mm]
T in POSCS. Similarly, the coordinate of intersection points of A
2 and A
7 on CCD1, CCD2, CCD3, CCD4 are n
1, n
2, n
3, n
4 and p
1, p
2, p
3, p
4 in POSCS, as shown in
Table 3.
If we compare the coordinate values of these calculated projection point m
1, m
2, m
3, m
4, n
1, n
2, n
3, n
4, p
1, p
2, p
3, p
4 with the simulation parameters in
Table 1, only p
4 is not within the measurable range of the CCD4 in POSCS. However, three projection points on four linear CCDs are enough to reconstruct the 3D coordinates of this anchor in POSCS, and the additional projection point can be used as redundancy. Although p
4 is not located in the measurable range of the CCD4 in POSCS, it does not affect the POS to estimate the coordinate of A
7 in POSCS. This means that even if one of the ODIUs malfunctions, it will not affect the normal operation of the entire system. Thus, we will use m
1, m
2, m
3, n
1, n
2, n
3, p
1, p
2, p
3 to estimate the coordinate of A
1, A
2, and A
7 in POSCS.
Based on the coordinate of m
1, m
2, and m
3, we can estimate the coordinate of A
1 in POSCS is a
1= [−110.75 mm, −40.31 mm, 2499.99 mm]
T. Similarly, according to n
1, n
2, n
3, and p
1, p
2, p
3 we can estimate the coordinate of A
2 and A
7 in POSCS are a
2= [644.50 mm, −392.49 mm, 2499.99 mm]
T and a
3= [241.43 mm, 714.95 mm, 2499.99 mm]
T, as shown in
Table 4.
According to the 3D coordinates of the three anchors A
1, A
2, A
7 in ICS and POSCS, as shown in
Table 2 and
Table 4, we can use the Rodrigues coordinate transformation algorithm in
Section 3 to estimate the position and orientation of the POS at the P1 position.
Similarly, according to the set value from P2 to P10 position, following the steps at the P1 position, we can acquire all measurable anchors, and the results are shown in
Table 5.
According to
Table 5, we can estimate the coordinate of the measurable anchors in POSCS, and the position and orientation from P1 to P10 are listed in
Table 6.
The maximum measurement error and average measurement error of x, y, and z-axis are calculated based on
Table 6, and then the average error is used to calculate the standard deviation, as shown in
Table 7.
Similarly, the maximum measurement error, average measurement error, and standard deviation of pitch, roll, and yaw angle are shown in
Table 8.
According to the evaluation results of measurement error in
Table 7 and
Table 8, the method proposed in this paper is compared with [
15], as shown in
Table 9. The measurement error in our method is smaller than the off-the-shelf methods, which indicates the feasibility and high accuracy of the indoor position and orientation measurement method proposed in this paper.
6. Conclusions
In this paper, a high-precision three-dimensional indoor position and orientation measurement method is proposed, which can be used for indoor navigation. We design a new-style indoor position orientation sensor by using four pairs of linear CCDs and cylindrical lenses, and analyze the field of vision of the position orientation sensor. The proposed indoor POS can estimate the 3D coordinate of the infrared LED anchors, after acquiring the coordinate value of at least three anchors sequentially in the POSCS, the Rodrigues coordinate transformation algorithm is used to estimate the position and orientation of the POS simultaneously. The position and orientation measurement method is performed via simulation. Thirty-six infrared LEDs are employed in simulation model space with dimensions of 5000 mm × 5000 mm × 3000 mm. The maximum value of position error and orientation error during simulation are 0.06 mm and 0.01°, respectively. The simulation result indicates the feasibility and high accuracy of the proposed method. The POS proposed in this paper is robust and can resist certain system failures, which is an alternative for indoor navigation.
There are two fundamental steps for the whole system to work properly. In this paper, we mainly elaborate on the POS operating principle and demonstrate the feasibility and high accuracy of the designed POS. In order to achieve a wide-range measurement of the indoor positioning and orientation, many anchor nodes will be fixed on the ceiling. However, the designed POS can only measure one anchor at a time in its FOV, so in the second phase, we will study how to design the wireless high-speed synchronous exposure between the infrared LED array and the POS. Meanwhile, we will conduct experiments to verify the proposed methods. Additionally, the architecture of the proposed POS is symmetrical and redundant, and we will study how to realize indoor position and orientation measurements without synchronization.
Due to the fast scanning, high-speed data processing, and precise coordinate measurement characteristics of the linear CCD, this system will be the appropriate candidate for the indoor position and orientation estimation, especially for the fasting moving object such as the flying agent indoors.