Method for determining position and attitude of aircraft in space station cabin based on UWB gyroscope visual combination

Abstract

The invention provides a UWB (Ultra Wideband) gyroscope vision combination-based method for determining the pose of an aircraft in a space station cabin, which comprises the following steps of: detecting an ArUco code on a cabin wall through monocular vision to obtain a relative pose, calculating an absolute position by combining a trilateration method of a UWB system, and resolving a short-time pose by using a gyroscope. And data of the three sensors are fused by adopting Kalman filtering, so that the limitation of a single sensor is overcome. The method has high visual precision, UWB (Ultra Wideband) anti-shielding capability and gyro short-time stability, can realize that the positioning precision is superior to 1.5 cm and the attitude determination precision is superior to 1.0 degree in a complex cabin environment, and has relatively strong robustness. The method is suitable for high-precision navigation and positioning of an aircraft in a space station cabin, and provides reliable support for complex space tasks.

Description

Technical Field

[0001] The present invention belongs to the technical field of high-precision pose measurement in the space station, and particularly relates to a method for determining the pose of an aircraft in the space station cabin based on the combination of UWB gyro and vision. Background Technique

[0002] In the International Space Station and other space environments, autonomous aircraft have become an important research direction. For example, autonomous aircraft such as Astrobee, Bumble developed by NASA, and the early Aerocam have all demonstrated the ability to perform tasks in complex space environments. However, in the space station cabin, due to complex environmental conditions, autonomous pose measurement of aircraft faces many challenges.

[0003] Traditional aircraft navigation and positioning methods based on a single vision system rely on image information captured by cameras. When the camera field of view of the aircraft is blocked, the lighting conditions in the cabin are poor, or there are floating objects interfering, the positioning accuracy will decrease significantly. At the same time, the vision system is also difficult to handle the cumulative errors in long-term tasks.

[0004] To solve these problems, researchers have proposed pose measurement methods that combine multiple sensors, among which the fusion scheme of UWB, gyro, and vision systems has shown significant potential. Ultra-wideband (UWB) positioning has strong anti-multipath and anti-interference capabilities, the gyroscope can provide stable short-term attitude information, and the vision system can provide high-precision pose measurement results in a short time. By fusing the data of these sensors, the accuracy and robustness of positioning and attitude determination can be effectively improved. Summary of the Invention

[0005] Object of the Invention: The technical problem to be solved by the present invention is to provide a method for determining the pose of an aircraft in the space station cabin based on the combination of UWB gyro and vision, mainly for high-precision positioning and attitude measurement of autonomous aircraft in the space station cabin. This method compensates for the limitations of a single sensor by fusing the measurement data of three sensors, ensuring that the aircraft can still obtain reliable pose information in complex environments.

[0006] The core idea of the present invention is to use a monocular vision system to detect the pre-arranged ArUco codes on the space station cabin wall, and calculate the relative pose of the aircraft with respect to the space station. Then, combine the UWB system to solve the absolute position of the aircraft in the space station, and at the same time measure the angle change of the aircraft through the gyro to solve the short-term attitude of the aircraft. Finally, fuse the data of these three sensors through the Kalman filter algorithm to achieve high-precision pose determination.

[0007] The method of the present invention specifically includes the following steps:

[0008] Step 1. System initialization: Install more than two ArUco code targets at known positions on the bulkhead of the space station, set up UWB base stations with known relative positions between them; install a monocular camera, an embedded processor, a UWB tag, and a gyroscope on the aircraft, and complete equipment calibration; among them, the monocular camera, the embedded processor, and the ArUco code target form a vision system; the UWB tag, the embedded processor, and the UWB base station form a UWB system; the gyroscope and the embedded processor form a gyro system;

[0009] Step 2. Execute the working process of the vision system;

[0010] Step 3. Execute the working process of the UWB system;

[0011] Step 4. Execute the working process of the gyro system;

[0012] Step 5. Perform multi-sensor data fusion to obtain the position and attitude information of the aircraft in the space station cabin.

[0013] In Step 1, the equipment calibration includes: determining the camera internal parameter matrix, the ArUco code, and the installation positions of the UWB base stations.

[0014] Step 2 includes: When the aircraft starts to work, the monocular camera takes pictures of the cabin environment, detects the ArUco code corners in the image, and uses the direct linear transformation method (DLT) to solve for the camera external parameters, where the camera external parameters include the rotation and translation matrices;

[0015] Given the actual size of the ArUco code, obtain the rotation and translation amount of the ArUco code in the camera coordinate system, and combine with the known installation positions and attitudes of the known ArUco code targets in the space station coordinate system to obtain the rotation and translation information of the aircraft relative to the space station:

[0016]

[0017] where, represents the attitude of the aircraft in the space station coordinate system, represents the position of the aircraft in the space station coordinate system, represents the transpose.

[0018] Step 3 includes: The UWB tag on the aircraft sends ultra-wideband signals to the base stations on the bulkhead. After the base stations receive the signals, they calculate the time difference of arrival (TDOA) of the signals, and thus calculate the relative distance between the aircraft and the current base station. The three-axis position of the aircraft is solved through the relative distances between the aircraft and more than four base stations and the installation positions of the base stations themselves in the space station cabin

[0019]

[0020]

[0021] wherein respectively represent the X, Y, and Z coordinates of the aircraft in the space station coordinate system, respectively represent the installation positions (three-axis coordinates) of UWB base stations a, b, c, and d inside the space station cabin, d a , d b , d c , d d respectively represent the distances between the aircraft and UWB base stations a, b, c, and d measured by the UWB system.

[0022] Step 4 includes: the gyroscope measures the angular velocity and acceleration of the aircraft in real time, and uses the inertial navigation principle to calculate the attitude information of the aircraft at the current moment t is the known initial attitude of the aircraft relative to the space station cabin, R B is the angle change matrix calculated by the gyroscope; to reduce the drift error of the gyroscope, the initial calibrated attitude data is used to correct the zero bias of the gyroscope.

[0023] In step 5, the distributed Kalman filtering algorithm is used, and two independent Kalman filters are used to perform distributed processing on the position and attitude calculation of the aircraft. The position fusion process includes: establishing a linear discrete system model for UWB and vision combined positioning, and the formula is:

[0024]

[0025] z P k = Hx P k + R P

[0026] where x P k is the combined measurement vector at the current moment k, is the position calculated by UWB at moment k, is the position calculated by vision at moment k, T represents matrix transpose, vk is the combined measurement speed, obtained by differentiating the position, Δt is the prediction step (time interval), x P k-1 and v P k-1 respectively represent the predicted position and speed at the previous moment, w P k-1 is the process noise, zP k is the position measurement value at time k, H is the measurement matrix, which is the identity matrix in this system, and R P is the measurement noise matrix;

[0027] The Kalman filter steps include:

[0028] The state prediction is carried out using the following formula:

[0029]

[0030] where the state transition matrix is the predicted value, is the predicted value at the previous time; The covariance prediction is carried out using the following formula: P k|k-1 = AP k-1 A T + Q, where P k|k-1 is the predicted covariance and Q is the covariance matrix of the process noise vector w P k ;

[0031] The Kalman gain K is calculated using the following formula k : K k = P k|k-1 H T (HP k|k-1 H T + R P ) -1

[0032] The state update is carried out using the following formula to obtain the position prediction as:

[0033] The covariance update is carried out using the following formula: P k = (I - K k H)P k|k-1 , where P k is the updated covariance and I is the identity matrix;

[0034] The state equation and measurement equation for attitude fusion are:

[0035]

[0036] z R k = Hx R k + R R

[0037] where x R k is the attitude obtained by gyroscope solution and the attitude obtained by visual solution The combined measurement vector, ω k is the combined measured angular velocity, obtained by angle differentiation, z R k is the attitude measurement value at time k, H is the measurement matrix, which is the identity matrix in the present invention, R R is the attitude measurement error matrix. The attitude prediction and update steps of the Kalman filter are the same as those of the position;

[0038] Finally, the output of the position filter and the output of the attitude filter at the same time are combined to obtain the position and attitude information of the aircraft in the space station cabin.

[0039] The present invention also provides an electronic device, including a processor and a memory. The memory stores program code. When the program code is executed by the processor, the processor executes the steps of the above method.

[0040] The present invention also provides a storage medium, storing a computer program or instruction. When the computer program or instruction runs on a computer, it executes the steps of the above method.

[0041] The present invention has the following beneficial effects:

[0042] 1. The present invention combines the advantages of the vision system, UWB system and gyroscope, and overcomes the limitations of single sensors in terms of occlusion, drift and cumulative error through multi-sensor fusion technology, realizing high-precision determination of the position and attitude of the aircraft.

[0043] 2. The distributed Kalman filter algorithm is used to dynamically fuse multi-sensor data, and the positioning accuracy can be better than 1.5 cm and the attitude determination accuracy can be better than 1.0 degree in a complex cabin environment, significantly improving the robustness and reliability of the system.

[0044] 3. Innovatively, by combining the ArUco code detection method and the UWB positioning method, the positioning problem in the entire space station cabin is solved.

[0045] 4. The present invention can rely on the redundant information of other sensors to ensure the continuous and stable operation of the aircraft when a single sensor fails or the data is abnormal, and is applicable to various complex space mission scenarios, providing reliable support for the high-precision navigation of autonomous aircraft in the space station cabin. Description of the Drawings

[0046] The following further specifically describes the present invention in conjunction with the drawings and specific embodiments, and the above and / or other advantages of the present invention will become clearer.

[0047] Figure 1It is the principle block diagram of the method of the present invention.

[0048] Figure 2 It is the schematic diagram of the UWB trilateration principle of the present invention.

[0049] Figure 3 It is the schematic diagram of the system modeling principle of the present invention.

[0050] Figure 4 It is the schematic diagram of the ground simulation experiment verification principle of the present invention.

[0051] Figure 5 It is the schematic diagram of the simulation hardware of the embodiment of the present invention.

[0052] Figure 6 It is the schematic diagram of the installation of the vision target and the sensor.

[0053] Figure 7a It is the experimental result of the position measurement of the experiment carried out according to the present method.

[0054] Figure 7b It is the experimental result of the attitude measurement of the experiment carried out according to the present method. Detailed implementation manners

[0055] The embodiment of the present invention provides a method for determining the position and attitude of an in-spacecraft vehicle based on the combination of UWB and gyro vision, including the following steps:

[0056] Step 1, system initialization;

[0057] Install more than two ArUco code targets at known positions on the spacecraft cabin wall, set up UWB base stations, and the relative positions between the base stations are known; install a monocular camera, an embedded processor, a UWB tag and a gyroscope on the vehicle, and complete device calibration; among them, the monocular camera, the embedded processor and the ArUco code target form a vision system; the UWB tag, the embedded processor and the UWB base station form a UWB system; the gyroscope and the embedded processor form a gyro system;

[0058] Step 2, the working process of the vision system;

[0059] When the vehicle starts to work, the monocular camera takes pictures of the cabin environment and detects the ArUco codes in the images. Calculate the relative position and attitude of the vehicle with respect to the ArUco codes on the cabin wall through image processing algorithms. Use the DLT (Direct Linear Transformation) method to decompose the homography matrix to obtain the rotation and translation information of the vehicle.

[0060] Step 3, the working process of the UWB system;

[0061] The UWB tag on the aircraft sends an ultra-wideband signal to the base station on the bulkhead. After receiving the signal, the base station calculates the time difference of arrival (TDOA) of the signal, thereby calculating the absolute position of the aircraft in three-dimensional space. Based on the geometric algorithm, the position of the aircraft is determined through the data of multiple base stations.

[0062] Step 4, the working process of the gyro system;

[0063] The gyro measures the angular velocity and acceleration of the aircraft in real time, and uses the inertial navigation principle to calculate the current attitude information of the aircraft. To reduce the drift error of the gyro, the zero bias of the gyro is corrected using the initially calibrated attitude data.

[0064] Step 5, multi-sensor data fusion;

[0065] The Kalman filter algorithm is used to fuse the measurement data of the vision system, UWB system, and gyro. First, the pose of the aircraft at the next moment is predicted, and then it is corrected according to the measurement values of the UWB and vision systems. The Kalman filter can effectively reduce the noise in the sensor measurement and significantly improve the positioning accuracy by fusing multiple measurement results.

[0066] In step 1, the equipment calibration includes: determining the camera internal parameter matrix, ArUco code, and the installation position of the UWB base station.

[0067] Step 2 includes: when the aircraft starts to work, the monocular camera takes pictures of the cabin environment, detects the corner points of the ArUco code in the image, and uses the direct linear transformation method (DLT) to solve the external parameters of the camera. The external parameters of the camera include the rotation and translation matrices;

[0068] Given the actual size of the ArUco code, the rotation and translation amount of the ArUco code in the camera coordinate system are obtained. Combined with the known installation position and attitude

[0069]

[0070] where represents the attitude of the aircraft in the space station coordinate system, represents the position of the aircraft in the space station coordinate system, represents the transpose.

[0071] Step 3 includes: The UWB tag on the aircraft sends an ultra-wideband signal to the base station on the bulkhead. After receiving the signal, the base station calculates the time difference of arrival (TDOA) of the signal, thereby calculating the relative distance between the aircraft and the current base station. The three-axis position of the aircraft is solved based on the relative distances between more than four aircraft and the base station and the installation position of the base station itself in the space station cabin.

[0072]

[0073] Among them respectively represent the coordinates of the aircraft in the X, Y, and Z directions in the space station coordinate system. respectively represent the installation positions (three-axis coordinates) of the UWB base stations a, b, c, and d in the space station cabin. a d b d c d d respectively represent the distances between the aircraft and the UWB base stations a, b, c, and d measured by the UWB system.

[0074] Step 4 includes: The gyroscope measures the angular velocity and acceleration of the aircraft in real time, and uses the inertial navigation principle to calculate the attitude information of the aircraft at the current moment t. is the known initial attitude of the aircraft relative to the space station cabin, and R B is the angle change matrix calculated by the gyroscope; to reduce the drift error of the gyroscope, the gyroscope is zero-biased corrected using the initially calibrated attitude data.

[0075] In step 5, the distributed Kalman filter algorithm is used, and two independent Kalman filters are used to perform distributed processing on the position and attitude calculation of the aircraft. The position fusion process includes: establishing a linear discrete system model for UWB and vision combined positioning, and the formula is:

[0076]

[0077] z P k = Hx P k + R P

[0078] Among them, x P k is the combined measurement vector at the current moment k. is the position solved by UWB at the moment k. is the position solved by vision at the moment k. T represents matrix transpose, and v kThe combined measurement speed is obtained by differentiating the position. Δt is the prediction step (time interval), and x P k-1 and v P k-1 represent the predicted position and speed at the previous moment, respectively. w P k-1 is the process noise, and z P k is the position measurement value at time k. H is the measurement matrix, which is the identity matrix in this system. R P is the measurement noise matrix;

[0079] The Kalman filter steps include:

[0080] The state prediction is performed using the following formula:

[0081]

[0082] where the state transition matrix is the predicted value, is the predicted value at the previous moment;

[0083] The covariance prediction is performed using the following formula: P k|k-1 = AP k-1 A T + Q, where P k|k-1 is the predicted covariance, and Q is the covariance matrix of the process noise vector w P k ;

[0084] The Kalman gain K is calculated using the following formula k : K k = P k|k-1 H T (HP k|k-1 H T + R P ) -1

[0085] The state update is performed using the following formula to obtain the position prediction as:

[0086] The covariance update is performed using the following formula: P k = (I - K k H)P k|k-1 , where P k is the updated covariance, and I is the identity matrix;

[0087] The state equation and measurement equation for attitude fusion are:

[0088]

[0089] z R k = Hx R k + R R

[0090] where x R k is the attitude obtained from gyroscopic solution and the attitude obtained from vision solution combined measurement vector, ω k is the combined measurement angular velocity, obtained by differentiating the angle, z R k is the attitude measurement value at time k, H is the measurement matrix, which is the identity matrix in the present invention, R R is the attitude measurement error matrix. The attitude prediction and update steps of the Kalman filter are the same as those for position;

[0091] Finally, the output of the position filter and the output of the attitude filter at the same time are combined to obtain the position and attitude information of the aircraft inside the space station module.

[0092] This embodiment has been experimentally verified: a semi-physical simulation experiment is carried out on an experimental platform simulating the environment inside the space station module (as Figure 5 shown).

[0093] An optical three-dimensional motion capture system (hereinafter referred to as motion capture) is used to provide the true pose of the mobile platform in the scene. This system records the actual position and attitude data of the platform in a high-precision manner, providing a reference standard for subsequent algorithm evaluation. A two-axis air-bearing table is used as the mobile platform to ensure that the platform can achieve stable and uniform reciprocating motion during the experiment. The parameters are shown in Table 1.

[0094] Table 1

[0095] Motion capture system Two-axis air-bearing table UWB Position accuracy 1mm Better than 0.1mm 5cm Attitude accuracy 0.1° Better than 0.1° /

[0096] An industrial-grade camera HBVCAM-F2209HD V11 is used, and the DETA10 series IMU is selected. Its parameters are shown in Table 2 below.

[0097] Table 2

[0098]

[0099] On the air-bearing table, 3 types of sensors are fixed, and the positioning centers of the 3 sensors are unified before the experiment, as Figure 6 shown. An ARM processor Raspberry Pi 4B is used as the main control device of the system, responsible for visual processing and collecting the processed visual pose information, IMU data, and UWB positioning data.

[0100] The final experimental results are as follows Figure 7a 、 Figure 7b (In the legend, Motion Capture represents the true position / attitude measured by the motion capture system, and Kalman Filtering represents the position / attitude values calculated by the Kalman filtering method in the present invention), Table 3 (Error statistics of three positioning methods), Table 4 (Error statistics of three attitude determination methods) (Taking the Y-axis as an example for position and the Z-axis as an example for attitude). The Motion Capture data is output by the motion capture system and serves as the true value.)

[0101] Table 3

[0102] Vision UWB Vision / UWB fusion Maximum error (MAX) / m 0.0743 0.1366 0.046 Root mean square error (RMSE) / m 0.0252 0.0415 0.0152

[0103] Table 4

[0104] Vision IMU Vision / IMU fusion Maximum error (MAX) / ° 4.8248 2.2092 1.7076 Root mean square error (RMSE) / ° 1.7629 0.8633 0.6954

[0105] The experimental results show that the positioning accuracy after fusion is better than 1.5 cm (RMSE), and the attitude determination accuracy is better than 1.0 degree (RMSE). When a single sensor fails, the method of the present invention still has strong robustness and can maintain a high positioning accuracy.

[0106] The present invention provides a method for determining the position and attitude of an in-spacecraft in a space station cabin based on the combination of UWB and gyro vision. There are many methods and ways to specifically implement this technical solution. The above description is only the preferred embodiment of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and modifications can be made, and these improvements and modifications should also be regarded as the protection scope of the present invention. Each component not clearly defined in this embodiment can be implemented by existing technologies.

Claims

1. A method for determining the position and posture of an aircraft in a space station cabin based on UWB gyro-vision combination, characterized in that: The following steps are involved: Step 1, system initialization: install more than two ArUco code targets at known positions on the space station wall, set up UWB base stations, and the relative positions between the base stations are known; install a monocular camera, embedded processor, UWB tag and gyroscope on the aircraft, and complete the equipment calibration; among them, the monocular camera, embedded processor and ArUco code target constitute the visual system; UWB tag, embedded processor and UWB base station constitute the UWB system; The gyroscope and embedded processor form the gyroscope system; Step 2: Execute the visual system workflow; Step 3, execute the UWB system workflow; Step 4, execute the gyro system workflow; Step 5: Perform multi-sensor data fusion to obtain the position and attitude information of the spacecraft in the space station cabin.

2. The method according to claim 1, characterized in that In step 1, the device calibration includes: determining the camera intrinsic parameter matrix, ArUco code and UWB base station installation location.

3. The method according to claim 2, characterized in that Step 2 includes: when the aircraft starts working, the monocular camera shoots the cabin environment, detects the ArUco code corner points in the image, and uses the direct linear transformation method DLT to solve the camera extrinsic parameters, wherein the camera extrinsic parameters include rotation and translation matrices; Knowing the actual size of the ArUco code, get the rotation of the ArUco code in the camera coordinate system and translation Combined with the known ArUco code target installation position in the space station coordinate system and posture Obtain the rotation and translation information of the spacecraft relative to the space station: in, Indicates the attitude of the aircraft in the space station coordinate system, Indicates the position of the aircraft in the space station coordinate system, Indicates transpose.

4. The method according to claim 3, characterized in that Step 3 includes: the UWB tag on the aircraft sends an ultra-wideband signal to the base station on the cabin wall. After the base station receives the signal, it calculates the signal propagation time difference TDOA, thereby solving the relative distance between the aircraft and the current base station, and solving the three-axis position of the aircraft through the relative distances between more than four aircraft and the base station and the installation position of the base station itself in the space station cabin. in They represent the X, Y, and Z coordinates of the spacecraft in the space station coordinate system. They represent the installation positions of UWB base stations a, b, c, and d in the space station cabin, respectively. a ,d b ,d c ,d d They respectively represent the distances a, b, c, and d from the aircraft to the UWB base station measured by the UWB system.

5. The method according to claim 4, characterized in that Step 4 includes: the gyroscope measures the angular velocity and acceleration of the aircraft in real time, and uses the inertial navigation principle to calculate the attitude information of the aircraft at the current time t is the known initial attitude of the spacecraft relative to the space station cabin, R B The angle change matrix solved by the gyroscope; the gyroscope is corrected for zero bias using the initial calibrated attitude data.

6. The method according to claim 5, characterized in that In step 5, a distributed Kalman filter algorithm is used, and two independent Kalman filters are used to perform distributed processing on the position and attitude solution of the aircraft. The position fusion process includes: establishing a linear discrete system model for UWB and visual combined positioning, and the formula is: z P k =Hx P k +R P where x P k is the combined measurement vector at the current k moment, is the position solved by UWB at time k, is the position of the visual solution at time k, T represents the matrix transpose, v k is the combined measurement speed, Δt is the prediction step length, x P k-1 and v P k-1 Respectively represent the predicted position and speed at the previous moment, w P k-1 is the process noise, z P k is the position measurement value at time k, H is the measurement matrix, R P is the measurement noise matrix; The Kalman filter steps include: The following formula is used for state prediction: The state transfer matrix matrix is the predicted value, is the predicted value of the previous moment; The following formula is used for covariance prediction: P k|k-1 =AP k-1 A T +Q,P k|k-1 is the prediction covariance, Q is the process noise vector w P k The covariance matrix of The Kalman gain K is calculated using the following formula: k :K k =P k|k-1 H T (HP k|k-1 H T +R P ) -1 Use the following formula to update the state and get the position prediction value for: The covariance update is performed using the following formula: k =(IK k H)P k|k-1 , P k is the updated covariance, I is the identity matrix; The state equation and measurement equation of posture fusion are: z R k =Hx R k +R R where x R k It is the posture calculated by the gyroscope and visually solved pose The combined measurement vector, ω k is the combined measurement angular velocity, z R k is the attitude measurement value at time k, H is the measurement matrix, R R is the attitude measurement error matrix; Finally, the position filter output and attitude filter output at the same time are combined to obtain the position and attitude information of the spacecraft in the space station cabin.

7. An electronic device, characterized in that: The method comprises a processor and a memory, wherein the memory stores program codes, and when the program codes are executed by the processor, the processor executes the steps of the method according to any one of claims 1 to 6.

8. A storage medium, characterized in that: A computer program or instruction is stored, and when the computer program or instruction is run on a computer, the steps of the method according to any one of claims 1 to 6 are executed.

Images