A positioning and attitude determination algorithm based on flickering light signals in dynamic scenes

Abstract

A positioning and attitude determination algorithm based on flickering light signals in dynamic scenes. The steps are as follows: 1. Image information and inertial navigation data acquisition; 2. Establishment of coordinate system; 3. Star point extraction; 4. Image registration; 5. Light source identification; 6. Posture solution. Through the above process, a positioning and attitude determination algorithm based on flickering light signals in dynamic scenes is proposed. The center of mass position of the light source is obtained by star point extraction, and the three-dimensional position of the light source is obtained by light source identification, so as to perform posture solution. The algorithm makes full use of the flickering characteristics of inertial navigation data and light sources, can avoid the problem of communication with the light source, ensure the robustness of the algorithm, and achieve high-precision positioning and attitude determination under the condition of user posture changes.

Description

Technical Field

[0001] The invention provides a dynamic positioning algorithm based on a flashing light signal, which relates to a method for positioning and determining an attitude under the condition of a user's own motion by using star point extraction and image registration, and belongs to the technical field of navigation positioning and determining an attitude. Background Art

[0002] The most widely used navigation and positioning system at present is the Global Navigation Satellite System (GNSS), which can provide high-precision positioning and timing around the clock, and has played a significant role in many civil and military fields. However, due to its weak radio signal energy, the signal is easily interfered in closed environments such as underground and indoors, resulting in poor positioning results.

[0003] The navigation methods that do not rely on GNSS mainly include inertial navigation system (INS), celestial navigation system, laser navigation system, vision navigation, etc. Among them, vision navigation plays an important role in indoor navigation, autonomous driving and other fields due to its low cost and high precision. Visual navigation is mainly composed of a camera and a data processing unit. By shooting feature points, the relative posture between multiple images is restored, and the three-dimensional posture of the camera is restored using the two-dimensional projection information of the feature points. On the one hand, visual navigation can not only give the user's location information, but also the user's posture, which is of great significance for aircraft, missiles and other carriers; on the other hand, visual navigation uses the camera to shoot continuous pictures, and the posture update rate is fast. In addition, the combination of visual navigation and inertial navigation is also an important application direction. The addition of inertial navigation solves the problem that pure visual navigation cannot handle fast rotating scenes, and reduces the dependence on scene texture features and lighting conditions. Visual navigation can also compensate and correct the drift of the gyroscope to ensure the accuracy of position and posture. Therefore, the combined navigation system consisting of visual navigation and inertial navigation has gradually been put into use.

[0004] Selecting feature points is an important step in visual navigation. Generally, fixed feature information of the scene can be selected as feature points, or artificial optical beacons can be arranged in advance. Optical navigation is an important part of the field of visual navigation, and has great application prospects in underground rescue, indoor navigation and other fields. Existing research on positioning using optical signals is mostly in the field of indoor positioning, and most of them are only applicable to quasi-static scenes or low-speed scenes. The optical signal generated by the optical beacon is constant. During the positioning process, it is necessary to communicate with the light source or measure the intensity of the optical signal to estimate the distance, which increases the complexity of the positioning system and makes it difficult to improve the positioning accuracy. How to achieve positioning and attitude determination using only optical signals under the condition of user's own movement and ensure system reliability is the key and difficulty of expanding the application scope of optical navigation. Summary of the invention

[0005] (I) Purpose of the invention

[0006] The present invention uses the characteristic that artificially modulated flickering light signals can be easily identified in the scene, pre-sets the flickering code of the artificial light source, and proposes a positioning and attitude determination algorithm based on flickering light signals in dynamic scenes based on the projection imaging model of the monocular camera and the theory of inertial navigation and attitude kinematics. The algorithm makes full use of the flickering characteristics of inertial navigation data and light sources, can avoid the problem of communication with the light source, and achieves high-precision positioning and attitude determination under the condition of user posture changes.

[0007] (II) Technical solution

[0008] The present invention relates to a positioning and attitude determination algorithm based on flashing light signals in dynamic scenes. The algorithm requires a positioning demonstration platform as hardware support, which is composed of: a camera 1, a computer 2, an inertial navigation element 3, and an artificial light source 4. Figure 1 The computer is a data processor, which is connected to the camera and the inertial navigation element and reads the image information and attitude change data.

[0009] The present invention requires that multiple artificial light sources be arranged and fixed in advance, and the flashing codes and three-dimensional positions of the light sources be pre-stored in a computer; a receiver consisting of a computer, a camera, and an inertial navigation element is fixed on the user, and the clock synchronization between the camera shooting and the flashing of the light source is ensured. When positioning begins, the light source starts working, and the user can freely perform posture movements with a relatively small angular velocity. The camera and the inertial navigation element record the image and angular velocity data, and then the computer calculates the user's posture.

[0010] The implementation steps of the positioning and attitude determination algorithm based on flashing light signals in dynamic scenes described in the present invention are as follows:

[0011] Step 1: Image information and inertial navigation data collection

[0012] The user moves freely, and the camera and inertial navigation record the real-time image information and angular velocity of movement, and transmit them to the computer for storage, which lasts for one flashing cycle of the light source.

[0013] Step 2: Establish a coordinate system

[0014] Establish the ENU coordinate system as the navigation coordinate system; the user coordinate system and the camera coordinate system can be considered unified. Establish the following camera coordinate system: X is the yaw axis, Y is the pitch axis, and Z is the roll axis; u and v are the corresponding two-dimensional pixel coordinate axes. Figure 2 .

[0015] Assuming that the camera rotates from the s coordinate system to the t coordinate system in the order of yaw-pitch-roll, rotating α, β, and γ around the X axis, Y axis, and Z axis respectively, the corresponding attitude rotation matrix is:

[0016]

[0017] For two consecutive frames, the rotation angle can be considered to be extremely small. Ignoring the second-order small quantity, it can be approximately considered that:

[0018]

[0019] Step 3: Star point extraction

[0020] Extract the light points in each stored frame image using the star point extraction algorithm. There are relatively mature methods for star point extraction, such as the block peak point algorithm and the local entropy algorithm, which can be used directly here without further explanation; the two-dimensional pixel coordinates of the light source in each frame image will be obtained through star point extraction.

[0021] Step 4: Image Registration

[0022] Since the user is in motion, it is necessary to first use inertial navigation data to align each frame of the image, that is, to transform it into a quasi-static situation before decoding the light source.

[0023] The input information of inertial navigation is the movement of the camera in the camera coordinate system. Generally speaking, six quantities are input: angular velocity of yaw, pitch, roll, and acceleration in the three directions of xyz. Since the time interval between two consecutive frames taken by the camera is very short, it can be considered that the camera has only changed its posture and is considered to be at a constant speed, and the change in position is negligible.

[0024] Assume the camera's intrinsic parameter matrix is The three-dimensional coordinates of the light source in the navigation coordinate system are Where i represents the light source number; T n is the position of the camera in the navigation coordinate system, R kis the absolute posture of the camera when taking the kth frame of image. According to the projection imaging model of the camera, we have:

[0025]

[0026]

[0027] In the formula Also called depth factor; (u i,k ,v i,k ) is the two-dimensional image coordinate of the i-th light source at the k-th frame, is its three-dimensional coordinate in the camera system.

[0028] Assume the camera is [ω x ,ω y ,ω z ], the angular velocity is based on the camera system as the coordinate reference, and the time interval between two consecutive frames is δt. Then the attitude transformation matrix of the camera from the kth frame to the k+1th frame is:

[0029]

[0030] For the sake of neatness in subsequent expressions, formula (5) is written as:

[0031]

[0032] So at the k+1th frame:

[0033]

[0034]

[0035] Combining equations (4), (7), and (8), we get:

[0036]

[0037] Then Substituting the specific form of K into formula (9), we get:

[0038]

[0039] From formula (4), we can see that Substituting into (10) we finally get:

[0040]

[0041]

[0042]

[0043] The two-dimensional pixel coordinates of the light source and the movement of the camera are both known quantities, so the registration value of each star point in the kth frame to the k+1th frame is obtained, so that the star points in each frame captured can be registered to the position at the same time, allowing decoding to be performed in a quasi-static state.

[0044] Step 5: Light source identification

[0045] In the quasi-static case, the two-dimensional centroid coordinates of the same star point only change within a certain small range. The communication domain is obtained by taking the union of these ranges, and then judging whether the centroid coordinates after registration are within the selected area, the flicker code of the light source corresponding to the star point is obtained, and finally matched with the light source flicker code stored in the receiver, the three-dimensional coordinates of the light source in the navigation coordinate system can be obtained.

[0046] The similarity of binary sequences can be determined by using the cross-correlation method to obtain the cross-correlation function of two discrete sequences:

[0047] R(n)=∑ m [x(m)y(n+m)] (13)

[0048] Where: x and y represent discrete sequences, and m represents the entire possible integer summation region.

[0049] The flash code is cross-correlated with the stored target star code. By comparing the maximum value of the cross-correlation function, a set of sequences with the highest similarity can be obtained. The star point corresponding to the sequence is the target star code, so that the light source is successfully identified and the corresponding three-dimensional coordinates of the light source are obtained.

[0050] Step 6: Pose calculation

[0051] Using the Newton-Raphson formula to iterate, there are six quantities to be determined, namely the displacement T n =[X n Y n Z n ] T , yaw angle α, pitch angle β, roll angle γ, so at least six equations are required. To ensure that the Newton-Raphson iteration formula converges to the correct value, as many equation constraints as possible should be sought. As far as this algorithm is concerned, the more constraints there are, the less sensitive the equation is to the initial value. Since the light source is constantly flickering, it is very likely that there are few star points in a frame of image, resulting in insufficient constraint equations. To ensure the robustness of the algorithm, it is necessary to use the star point information of multiple frames of images and inertial navigation information to solve the pose.

[0052] Here, three frames of information are used for the solution. Assuming that the camera pose at the kth frame is to be solved, the information from the k-2th frame to the kth frame is used for the solution. Similarly, it can be considered that the camera only changes in its pose. According to formula (1), and for the sake of simplicity, the camera pose at the kth frame is assumed to be:

[0053]

[0054] First, use the image information of the kth frame to list equations. For a certain star point, the three-dimensional coordinates of its corresponding light source are known, and its two-dimensional image coordinates are normalized, that is:

[0055]

[0056] Combining the above formula with (3) and (4) we get:

[0057]

[0058] set up R k , T n Substituting the specific form into (16) and using the third line of the formula to eliminate the depth factors of the first two lines, we can obtain:

[0059]

[0060] Or written as: F k,1 (θ)=0 (17)

[0061] Where Θ = [α, β, γ, X n ,Y n ,Z n ] T is the column vector to be found. Similarly, for other star points, F can be found k,2 (θ)=0,F k,3 (Θ)=0,…

[0062] For the k-1th frame and the k-2th frame, the attitude transfer matrix can be obtained by using the inertial navigation information and formula (5): So we have:

[0063]

[0064] R k-1 , R k-2 Each item in the matrix is ​​still just a function of α, β, and γ, so similar to the process of equations (15) and (16), it can be listed as follows:

[0065] F k-1，1 (θ)=0,F k-1，2 (θ)=0,…(19)

[0066] F k-2，1 (θ)=0,F k-2，2 (θ)=0,…

[0067] Combining equations (17), (19), and (20), we obtain:

[0068]

[0069] Thus, the objective function F(Θ) of the Newton-Raphson iteration is obtained. Let the Jacobian matrix of F(Θ) be J, then the iteration formula is:

[0070] Θ k+1 =Θ k -(J T J) -1 J T F(θ k ) (twenty two)

[0071] Where J represents J(Θ k ). Thus, the user’s position and posture in the navigation coordinate system are obtained.

[0072] In summary, the implementation steps of the present invention are as follows: Figure 3 As shown. Through the above process, a positioning and attitude determination algorithm based on flickering light signals in dynamic scenes is proposed. The centroid position of the light source is obtained by star point extraction, and the three-dimensional position of the light source is obtained by light source identification, so as to perform posture calculation. The algorithm makes full use of the flickering characteristics of inertial navigation data and light sources, can avoid the problem of communication with the light source, ensure the robustness of the algorithm, and achieve high-precision positioning and attitude determination under the condition of user posture changes.

[0073] (III) Advantages

[0074] The advantages of the positioning and attitude determination algorithm based on flashing light signals in dynamic scenes provided by the present invention are:

[0075] ① The algorithm proposed in the present invention does not require additional communication between the light source and the receiver, and positioning can be achieved using only the flashing light signal.

[0076] ② The algorithm proposed in the present invention can achieve positioning when the user is moving. BRIEF DESCRIPTION OF THE DRAWINGS

[0077] Figure 1 It is a schematic diagram of the demonstration platform required by the present invention.

[0078] Figure 2 It is the coordinate system established by the present invention.

[0079] Figure 3 It is a flowchart of the algorithm of the present invention.

[0080] Figure 1 Among them, 1 camera, 2 computer, 3 inertial navigation element, 4 artificial light source. DETAILED DESCRIPTION

[0081] The specific implementation process of the present invention will be further described in detail below in conjunction with the technical solution.

[0082] The present invention relates to a positioning and attitude determination algorithm based on flashing light signals in dynamic scenes. The algorithm requires a positioning demonstration platform as hardware support, which is composed of: a camera 1, a computer 2, an inertial navigation element 3, and an artificial light source 4. Figure 1 The computer is a data processor, which is connected to the camera and the inertial navigation element and reads the image information and attitude change data.

[0083] The present invention requires that multiple artificial light sources be arranged and fixed in advance, and the flashing codes and three-dimensional positions of the light sources be pre-stored in a computer; a receiver consisting of a computer, a camera, and an inertial navigation element is fixed on the user, and the clock synchronization between the camera shooting and the flashing of the light source is ensured. When positioning begins, the light source starts working, and the user can freely perform posture movements with a relatively small angular velocity. The camera and the inertial navigation element record the image and angular velocity data, and then the computer calculates the user's posture.

[0084] The implementation steps of the positioning and attitude determination algorithm based on flashing light signals in dynamic scenes described in the present invention are as follows:

[0085] Step 1: Image information and inertial navigation data collection

[0086] The user moves freely, and the camera and inertial navigation record the real-time image information and angular velocity of movement, and transmit them to the computer for storage, which lasts for one flashing cycle of the light source.

[0087] Step 2: Establish a coordinate system

[0088] Establish the ENU coordinate system as the navigation coordinate system; the user coordinate system and the camera coordinate system can be considered unified. Establish the following camera coordinate system: X is the yaw axis, Y is the pitch axis, and Z is the roll axis; u and v are the corresponding two-dimensional pixel coordinate axes. Figure 2 .

[0089] Assuming that the camera rotates from the s coordinate system to the t coordinate system in the order of yaw-pitch-roll, rotating α, β, and γ around the X axis, Y axis, and Z axis respectively, the corresponding attitude rotation matrix is:

[0090]

[0091] For two consecutive frames, the rotation angle can be considered to be extremely small. Ignoring the second-order small quantity, it can be approximately considered that:

[0092]

[0093] Step 3: Star point extraction

[0094] Extract the light points in each stored frame image using the star point extraction algorithm. There are relatively mature methods for star point extraction, such as the block peak point algorithm and the local entropy algorithm, which can be used directly here without further explanation; the two-dimensional pixel coordinates of the light source in each frame image will be obtained through star point extraction.

[0095] Step 4: Image Registration

[0096] Since the user is in motion, it is necessary to first use inertial navigation data to align each frame of the image, that is, to transform it into a quasi-static situation before decoding the light source.

[0097] The input information of inertial navigation is the movement of the camera in the camera coordinate system. Generally speaking, six quantities are input: angular velocity of yaw, pitch, roll, and acceleration in the three directions of xyz. Since the time interval between two consecutive frames taken by the camera is very short, it can be considered that the camera has only changed its posture and is considered to be at a constant speed, and the change in position is negligible.

[0098] Assume the camera's intrinsic parameter matrix is The three-dimensional coordinates of the light source in the navigation coordinate system are Where i represents the light source number; T n is the position of the camera in the navigation coordinate system, R k is the absolute posture of the camera when taking the kth frame of image. According to the projection imaging model of the camera, we have:

[0099]

[0100]

[0101] In the formula Also called depth factor; (u i,k ,v i,k ) is the two-dimensional image coordinate of the i-th light source at the k-th frame, is its three-dimensional coordinate in the camera system.

[0102] Assume the camera is [ω x ,ω y ,ω z ], the angular velocity is based on the camera system as the coordinate reference, and the time interval between two consecutive frames is δt. Then the attitude transformation matrix of the camera from the kth frame to the k+1th frame is:

[0103]

[0104] For the sake of neatness in subsequent expressions, formula (27) is written as:

[0105]

[0106] So at the k+1th frame:

[0107]

[0108]

[0109] Combining equations (26), (29), and (30), we get:

[0110]

[0111] Then Substituting the specific form of K into formula (31), we get:

[0112]

[0113] From formula (26), we can see that Substituting into (32) we finally get:

[0114]

[0115]

[0116] The two-dimensional pixel coordinates of the light source and the movement of the camera are both known quantities, so the registration value of each star point in the kth frame to the k+1th frame is obtained, so that the star points in each frame captured can be registered to the position at the same time, allowing decoding to be performed in a quasi-static state.

[0117] Step 5: Light source identification

[0118] In the quasi-static case, the two-dimensional centroid coordinates of the same star point only change within a certain small range. The communication domain is obtained by taking the union of these ranges, and then judging whether the centroid coordinates after registration are within the selected area, the flicker code of the light source corresponding to the star point is obtained, and finally matched with the light source flicker code stored in the receiver, the three-dimensional coordinates of the light source in the navigation coordinate system can be obtained.

[0119] The similarity of binary sequences can be determined by using the cross-correlation method to obtain the cross-correlation function of two discrete sequences:

[0120] R(n)=∑ m [x(m)y(n+m)] (35)

[0121] Where: x and y represent discrete sequences, and m represents the entire possible integer summation region.

[0122] The flash code is cross-correlated with the stored target star code. By comparing the maximum value of the cross-correlation function, a set of sequences with the highest similarity can be obtained. The star point corresponding to the sequence is the target star code, so that the light source is successfully identified and the corresponding three-dimensional coordinates of the light source are obtained.

[0123] Step 6: Pose calculation

[0124] Using the Newton-Raphson formula to iterate, there are six quantities to be determined, namely the displacement T n =[X n Y n Z n ] T , yaw angle α, pitch angle β, roll angle γ, so at least six equations are required. To ensure that the Newton-Raphson iteration formula converges to the correct value, as many equation constraints as possible should be sought. As far as this algorithm is concerned, the more constraints there are, the less sensitive the equation is to the initial value. Since the light source is constantly flickering, it is very likely that there are few star points in a frame of image, resulting in insufficient constraint equations. To ensure the robustness of the algorithm, it is necessary to use the star point information of multiple frames of images and inertial navigation information to solve the pose.

[0125] Here, three frames of information are used for the solution. Assuming that the camera pose at the kth frame is to be solved, the information from the k-2th frame to the kth frame is used for the solution. Similarly, it can be assumed that the camera only changes in its pose. According to equation (23), and for the sake of simplicity, the camera pose at the kth frame is assumed to be:

[0126]

[0127] First, use the image information of the kth frame to list equations. For a certain star point, the three-dimensional coordinates of its corresponding light source are known, and its two-dimensional image coordinates are normalized, that is:

[0128]

[0129] Combining the above formula with (25) and (26) we get:

[0130]

[0131] set up R k 、T n Substituting the specific form into (38) and using the third line of the formula to eliminate the depth factors of the first two lines, we can obtain:

[0132]

[0133] Or written as: F k，1 (θ)=0 (39)

[0134] Where Θ = [α, β, γ, X n , Y n , Z n ] T is the column vector to be found. Similarly, for other star points, F can be found k，2 (θ)=0,Fk，3 (θ)=0,…

[0135] For the k-1th frame and the k-2th frame, the attitude transfer matrix can be obtained by using the inertial navigation information and equation (27): So we have:

[0136]

[0137] R k-1 , R k-2 Each item in the matrix is ​​still just a function of α, β, and γ, so similar to the process of equations (37) and (38), it can be listed as follows:

[0138] F k-1，1 (θ)=0,F k-1，2 (θ)=0,…(41)

[0139] F k-2，1 (0) = 0, F k-2，2 (θ)=0,…(42)

[0140] Combining equations (39), (41), and (42), we obtain:

[0141]

[0142] Thus, the objective function F(Θ) of the Newton-Raphson iteration is obtained. Let the Jacobian matrix of F(Θ) be J, then the iteration formula is:

[0143] Θ k+1 =Θ k -(J T J) -1 J T F(Θ k ) (44)

[0144] Where J represents J(Θ k ). Thus, the user’s position and posture in the navigation coordinate system are obtained.

[0145] In summary, the implementation steps of the present invention are as follows: Figure 3 As shown. Through the above process, a positioning and attitude determination algorithm based on flickering light signals in dynamic scenes is proposed. The centroid position of the light source is obtained by star point extraction, and the three-dimensional position of the light source is obtained by light source identification, so as to perform posture calculation. The algorithm makes full use of the flickering characteristics of inertial navigation data and light sources, can avoid the problem of communication with the light source, ensure the robustness of the algorithm, and achieve high-precision positioning and attitude determination under the condition of user posture changes.

[0146] Related Literature

[0147] Reference [1] uses the method of optical flow tracking feature points to implement a real-time vision-assisted inertial navigation algorithm based on a multi-state constrained Kalman filter.

[0148] Reference [2] uses four coplanar light sources as feature points and uses the LUT algorithm to achieve monocular vision pose estimation.

[0149] References:

[0150] [1]AIMourikis and SIRoumeliotis, "A Multi-State Constraint KalmanFilter for Vision-aided Inertial Navigation," Proceedings 2007IEEEInternational Conference on Robotics and Automation, 2007, pp.3565-3572.

[0151] [2] Xu Weigao. Research on key technologies of posture measurement based on monocular vision[D]. University of Chinese Academy of Sciences (Xi'an Institute of Optics and Precision Mechanics, Chinese Academy of Sciences), 2016.

Claims

1. A positioning and attitude determination method based on flashing light signals in dynamic scenes, characterized by: The inertial navigation data is used to calculate the change value between adjacent frames of the coordinates of each star point, so as to realize the registration of the sequence images, transform the dynamic scene into a quasi-static situation, and then decode the light source; in the quasi-static situation, the two-dimensional centroid coordinates of the same star point only change within a small range. The union of these coordinate change ranges is calculated to obtain the communication domain, and then it is determined whether the registered centroid coordinates are within the coverage of the communication domain to obtain the flicker code of the light source corresponding to the star point. Finally, the cross-correlation calculation is performed with the actual light source flicker code stored in the receiver and the flicker code with the highest cross-correlation is matched to obtain the corresponding three-dimensional coordinates of the light source in the navigation coordinate system.

2. A positioning and attitude determination method based on flashing light signals in dynamic scenes, characterized in that Follow these steps to determine the user's position and posture: Step 1: Use the camera and inertial navigation equipment to collect data on the flickering light source to obtain the light source flickering sequence image and angular velocity and acceleration data in the navigation coordinate system; Step 2: Extract the centroid of the star points in the sequence image to obtain the sequence centroid coordinates; Step 3: Use the angular velocity and acceleration data in the navigation coordinate system to calculate the change value between adjacent frames of the coordinates of each star point, so as to align the sequence images and transform the dynamic scene into a quasi-static situation; Step 4: Use cross-correlation operation to obtain the flicker code of the light source; Step 5: Use the star point information and inertial navigation information of multiple frames of images to perform positioning and attitude determination for the user through Newton-Raphson iteration.

Images