A Novel Position and Orientation Sensor for Indoor Navigation Based on Linear CCDs

The position and orientation of a mobile agent, such as robot or drone, etc., should be estimated in a timely way during operation in the structured indoor environment, so as to ensure the security and efficiency of task execution. Concerning the problem that the position and orientation are often estimated separately by different kinds of sensors in the off-the-shelf methods, we design a novel position orientation sensor (POS). The POS consists of four pairs of linear charge-coupled devices (CCDs) and cylindrical lenses, which can estimate the 3D coordinate of the anchor in the POS’s field of view. After detecting at least three anchors in its field of vision sequentially, the Rodrigues coordinate transformation algorithm is utilized to estimate the position and orientation of POS simultaneously. Meanwhile, the position and orientation are estimated at the receiver side. Hence there is no privacy concern associated with this system. The architecture of the proposed POS is symmetrical and redundant, even if one of the linear CCDs or cylindrical lens malfunctions, the whole system could still work normally. The proposed method is cost-effective and easily extends to a wide range. The numerical simulation demonstrates the feasibility and high accuracy of the proposed method, and it outperforms the off-the-shelf methods.


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 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.

System Component and Working Principle
The linear sensor which we use consists of two major components, a linear CCD and a cylindrical lens. The cylindrical lens is used to form the projection mapping from 3D space to the 1D image. According to the basic principles of optics, the rays from an object point on one side of the cylindrical lens forms a linear image on the other side [24].
As shown in Figure 1, a linear CCD and a cylindrical lens constitute a one-dimensional imaging unit (ODIU); the light from the light emitted diode (LED) marker passing through the cylindrical lens will focus to a line which is coplanar to the marker and optical axis of the lens [25]. To detect the change of plane due to the movement of an LED marker, a linear CCD is used, and it is located at the focal plane of the cylindrical lens. This linear CCD is placed perpendicular to the optic axis of the cylindrical lens to detect the change of the projection line when the LED marker moves. The center of the linear CCD is aligned with the center of the optic axis, and their distance is the focal length of the cylindrical lens. The LED marker is projected to form a narrow spot on the linear CCD, and the center of the spot is taken as the image position of the marker. If the spot position on the linear CCD and the optical axis of the cylindrical lens are known, then the supporting plane with them can be calculated, and the LED marker is situated on the same plane.
As we know, a plane can be defined according to a line and a point that is not on that line. In order to determine the 3-D coordinates of a LED marker in the coordinate system, we need at least three ODIUs to register the LED marker's 1D image position coordinates, and then reconstruct the 3D spatial coordinates of the anchor [26]. However, with the system composed of only three ODIUs, although it can provide the 3D coordinate information reconstruction of one marker, nevertheless, this system is vulnerable if any ODIU breaks. To make full use of the layout of the existing symmetrical structure on the mobile agent, such as a quadrotor drone who has four arms, we will use four ODIUs to determine the 3D coordinates of an anchor. Meanwhile, it is a redundant system by using four ODIUs, even if an ODIU malfunction, the whole system can still work normally.
As shown in Figure 2, four ODIUs are arranged, and the 3D coordinates of the anchor can be reconstructed by the intersection of four planes. When one LED marker (also the anchor) is located in the FOV of the POS, the anchor forms four projection lines crossing the four linear CCDs by passing through the respective cylindrical lens, and the projection lines intersect with the four linear CCD on the focal plane in the respective ODIU. If the position of the projection line in every ODIU is determined, then the plane which contains the anchor is also determined. Thus, the 3D coordinate of the anchor in POSCS can be uniquely resolved by these planes, which is determined by these ODIUs. Meanwhile, we set the intersection of the four linear CCD's extension lines as the origin of the POSCS; the photoelectric detection area of every linear CCD is on the XY plane of POSCS. The linear CCD1 and CCD2 are on the positive half-axis of the X-axis and Y-axis, while the CCD3 and CCD4 are on the negative half-axis of the X-axis and Y-axis. According to the created POSCS based The LED marker is projected to form a narrow spot on the linear CCD, and the center of the spot is taken as the image position of the marker. If the spot position on the linear CCD and the optical axis of the cylindrical lens are known, then the supporting plane with them can be calculated, and the LED marker is situated on the same plane.
As we know, a plane can be defined according to a line and a point that is not on that line. In order to determine the 3-D coordinates of a LED marker in the coordinate system, we need at least three ODIUs to register the LED marker's 1D image position coordinates, and then reconstruct the 3D spatial coordinates of the anchor [26]. However, with the system composed of only three ODIUs, although it can provide the 3D coordinate information reconstruction of one marker, nevertheless, this system is vulnerable if any ODIU breaks. To make full use of the layout of the existing symmetrical structure on the mobile agent, such as a quadrotor drone who has four arms, we will use four ODIUs to determine the 3D coordinates of an anchor. Meanwhile, it is a redundant system by using four ODIUs, even if an ODIU malfunction, the whole system can still work normally.
As shown in Figure 2, four ODIUs are arranged, and the 3D coordinates of the anchor can be reconstructed by the intersection of four planes. When one LED marker (also the anchor) is located in the FOV of the POS, the anchor forms four projection lines crossing the four linear CCDs by passing through the respective cylindrical lens, and the projection lines intersect with the four linear CCD on the focal plane in the respective ODIU. If the position of the projection line in every ODIU is determined, then the plane which contains the anchor is also determined. Thus, the 3D coordinate of the anchor in POSCS can be uniquely resolved by these planes, which is determined by these ODIUs. The LED marker is projected to form a narrow spot on the linear CCD, and the center of the spot is taken as the image position of the marker. If the spot position on the linear CCD and the optical axis of the cylindrical lens are known, then the supporting plane with them can be calculated, and the LED marker is situated on the same plane.
As we know, a plane can be defined according to a line and a point that is not on that line. In order to determine the 3-D coordinates of a LED marker in the coordinate system, we need at least three ODIUs to register the LED marker's 1D image position coordinates, and then reconstruct the 3D spatial coordinates of the anchor [26]. However, with the system composed of only three ODIUs, although it can provide the 3D coordinate information reconstruction of one marker, nevertheless, this system is vulnerable if any ODIU breaks. To make full use of the layout of the existing symmetrical structure on the mobile agent, such as a quadrotor drone who has four arms, we will use four ODIUs to determine the 3D coordinates of an anchor. Meanwhile, it is a redundant system by using four ODIUs, even if an ODIU malfunction, the whole system can still work normally.
As shown in Figure 2, four ODIUs are arranged, and the 3D coordinates of the anchor can be reconstructed by the intersection of four planes. When one LED marker (also the anchor) is located in the FOV of the POS, the anchor forms four projection lines crossing the four linear CCDs by passing through the respective cylindrical lens, and the projection lines intersect with the four linear CCD on the focal plane in the respective ODIU. If the position of the projection line in every ODIU is determined, then the plane which contains the anchor is also determined. Thus, the 3D coordinate of the anchor in POSCS can be uniquely resolved by these planes, which is determined by these ODIUs. Meanwhile, we set the intersection of the four linear CCD's extension lines as the origin of the POSCS; the photoelectric detection area of every linear CCD is on the XY plane of POSCS. The linear CCD1 and CCD2 are on the positive half-axis of the X-axis and Y-axis, while the CCD3 and CCD4 are on the negative half-axis of the X-axis and Y-axis. According to the created POSCS based CCD1 and CCD2 are on the positive half-axis of the X-axis and Y-axis, while the CCD3 and CCD4 are on the negative half-axis of the X-axis and Y-axis. According to the created POSCS based on the four ODIUs, once the anchor is located in the FOV of POS, we can estimate the 3D coordinate of the anchor in POSCS.

Field of Vision of the POS
According to the above introduction of POS, it can only measure the anchor's coordinate when it is located inside the FOV of the POS. The FOV is very important during the layout design of anchors. Therefore, it is necessary to carry out the FOV simulation for the designed POS.
The structure of the designed POS on the XY plane in POSCS is shown in Figure 3. The sensing length of the linear CCD's photo-element is 30 mm. The linear CCD1 and CCD3 are on the positive half-axis and negative half-axis of the X-axis, and they are symmetrically placed relative to the origin point, while the CCD2 and CCD4 are on the positive half-axis and negative half-axis of the Y-axis, and they are also symmetrically placed relative to the origin point. on the four ODIUs, once the anchor is located in the FOV of POS, we can estimate the 3D coordinate of the anchor in POSCS.

Field of Vision of the POS
According to the above introduction of POS, it can only measure the anchor's coordinate when it is located inside the FOV of the POS. The FOV is very important during the layout design of anchors. Therefore, it is necessary to carry out the FOV simulation for the designed POS.
The structure of the designed POS on the XY plane in POSCS is shown in Figure 3. The sensing length of the linear CCD's photo-element is 30 mm. The linear CCD1 and CCD3 are on the positive half-axis and negative half-axis of the X-axis, and they are symmetrically placed relative to the origin point, while the CCD2 and CCD4 are on the positive half-axis and negative half-axis of the Y-axis, and they are also symmetrically placed relative to the origin point. The CCD1_In, CCD2_In, CCD3_In, and CCD4_In are inner margins of every linear CCD, and their distances d to the origin are 60 mm; the CCD1_Out, CCD2_Out, CCD3_Out, and CCD4_Out are outer margins of the linear CCD, and their distances to the origin are 90 mm. The a1 and b1, a2 and b2, a3 and b3, a4 and b4 are two extreme points on the optical axis of cylindrical lens 1, cylindrical lens 2, cylindrical lens 3, and cylindrical lens 4. The focal length of the cylindrical lens is 50 mm. The values of the above parameters are summarized in Table 1. According to Table 1, we divide the photosensitive area of the four linear CCD every 1 mm to choose the projection line, and there will be many spatial intersections. Here we consider the case The CCD1_In, CCD2_In, CCD3_In, and CCD4_In are inner margins of every linear CCD, and their distances d to the origin are 60 mm; the CCD1_Out, CCD2_Out, CCD3_Out, and CCD4_Out are outer margins of the linear CCD, and their distances to the origin are 90 mm. The a1 and b1, a2 and b2, a3 and b3, a4 and b4 are two extreme points on the optical axis of cylindrical lens 1, cylindrical lens 2, cylindrical lens 3, and cylindrical lens 4. The focal length of the cylindrical lens is 50 mm. The values of the above parameters are summarized in Table 1. According to Table 1, we divide the photosensitive area of the four linear CCD every 1 mm to choose the projection line, and there will be many spatial intersections. Here we consider the case that the calculated z value of the spatial intersection below 4000 mm. Figure 4 is the simulated FOV of POS according to the parameters in Table 1.
Sensors 2020, 20, x FOR PEER REVIEW 6 of 15 that the calculated z value of the spatial intersection below 4000 mm. Figure 4 is the simulated FOV of POS according to the parameters in Table 1.
In Figure 4, the measurable area of every layer is a square structure, and the closest measurement distance of the POS is 300 mm, that means the distance from an anchor to POS needs to be at least 300 mm if the POS can detect the anchor, meanwhile, the higher of the height, and the broader measurement scope of the POS.

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.

Mathematical Model
Suppose three anchors A1, A2, A3 are fixed on the ceiling, and their coordinates in ICS are A1(X1, Y1, Z1), A2(X2, Y2, Z2) and A3(X3, Y3, Z3). After acquiring the coordinates of three anchors by using POS, and their coordinates in POSCS are C1(xc1, yc1, zc1), C2(xc2, yc2, zc2) and C3(xc3, yc3, zc3). 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), (2) and (3): In Figure 4, the measurable area of every layer is a square structure, and the closest measurement distance of the POS is 300 mm, that means the distance from an anchor to POS needs to be at least 300 mm if the POS can detect the anchor, meanwhile, the higher of the height, and the broader measurement scope of the POS.

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.

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): Sensors 2020, 20, 748 7 of 15 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= [x 0 , y 0 , z 0 ] 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] cos β cos γ − sin α sin β sin γ cos β sin γ + sin α sin β cos γ − cos α sin β − cos α sin γ cos α cos γ sin α sin β cos γ + cos β sin α sin γ sin β sin γ − cos β sin α cos γ cos α cos β 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.

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.

of 15
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).
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.

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).
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 T e can be calculated by using rotation matrix R and the gravity of multiple common points.

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. Sensors 2020, 20, x FOR PEER REVIEW 10 of 15

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.  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 A1, A2,  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.

As shown in
According to the parameters of POS in Table 1 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  Table 4. According to the 3D coordinates of the three anchors A 1 , A 2 , A 7 in ICS and POSCS, as shown in Tables 2 and 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 Tables 7 and 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.

Discussion
Indoor navigation is critical for developing intelligent carriers. We design a novel POS by using four pairs of linear CCDs and cylindrical lenses for indoor navigation without the assistance of an IMU or other costly devices. This design has the following advantages: (1) The POS is designed for the indoor position and attitude measurement purposes in a wide range, and achieves sufficient accuracy in the structured indoor environment. (2) Our method does not need to consider the 2D or 3D working situation, respectively. We can estimate the position and orientation in a 3D situation or the position and direction in a 2D situation, as long as a minimum of three anchors is within the field of vision of the designed POS. Meanwhile, after three LED anchors are detected, we can estimate the position and orientation simultaneously, while many indoor positioning methods can only estimate the position. (3) Once the LED's control unit and POS's control unit are simultaneously triggered, and they will have an identical time sequence. Therefore, the number of POS used indoors is not limited, theoretically. (4) The structure of the proposed POS is a redundant system, even if a linear CCD is broken down; it does not affect the normal operation of the entire system, as long as the remaining three linear CCDs could work normally.
Of course, there is still some idealization in the simulation design process. In order to demonstrate the feasibility and effectiveness of the whole system, we ignore the partial distortion and nonlinear effects of the cylindrical lens. However, it depends entirely on the perfection of the cylindrical lens design and the cost, which is beyond the scope of this paper [29].

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.