Recovery real-time positioning method based on inertial navigation-vision method

Abstract

The invention provides a recovery real-time positioning method based on an inertial navigation-vision method, and the method comprises the steps: setting an on-rocket vision sensing unit, enabling the on-rocket vision sensing unit to obtain a cooperative target image, fusing the measurement results of an inertial navigation measurement system and a vision pose measurement system, carrying out the mutual correction of measurement errors, and outputting a fusion result. The inertial navigation-vision fusion technology is adopted, the problem that inertial navigation measurement errors are accumulated along with time is solved through the vision pose measurement technology, and meanwhile the inertial navigation system is used for making up for the defect in the aspect of vision navigation stability. A recovery site cooperative target is measured through a visual detector on the rocket, pose information of the rocket relative to the recovery site is determined, and inertial navigation measurement error correction is completed by using the result; and meanwhile, high-precision attitude information provided by the inertial navigation system is used for correcting a measurement error caused by jittering of the visual attitude measurement system, so that the precision and robustness of rocket attitude measurement are improved.

Description

Technical Field

[0001] The present invention relates to the field of physical technology, and in particular to a real-time recovery positioning method based on an inertial navigation-vision method. Background Art

[0002] Rocket recovery technology is rapidly developing, offering significant advantages in launch efficiency and cost, and has garnered widespread attention both domestically and internationally. During the rocket's return phase, rocket positioning is crucial to the success of the recovery. Acquiring high-precision, real-time positioning information is a key technical challenge facing rocket recovery.

[0003] Currently, the commonly used navigation method is inertial navigation. Inertial navigation information is calculated by integrating acceleration over time. The inertial navigation system starts working from the moment the rocket is launched and works for a long time until the rocket is recovered. This integration process will cause errors to accumulate over time, making the cumulative error larger, resulting in poor long-term accuracy, greatly increasing the difficulty of recovery, and making it difficult to provide high-precision positioning information.

[0004] Therefore, in order to improve the positioning accuracy and reliability during the rocket recovery process, other navigation and positioning methods need to be introduced to improve the rocket recovery accuracy. Summary of the Invention

[0005] The present invention aims to solve the problem of poor positioning accuracy during rocket recovery. It provides a real-time positioning method for recovery based on an inertial navigation-vision method. The method adopts inertial navigation-vision fusion technology and utilizes visual pose measurement technology to solve the problem of inertial navigation measurement errors accumulating over time. At the same time, the inertial navigation system is used to compensate for the lack of visual navigation stability. The onboard visual detector measures the recovery site's cooperative target to determine the rocket's pose information relative to the recovery site, and uses this result to correct the inertial navigation measurement error. At the same time, the high-precision pose information provided by the inertial navigation system is used to correct the measurement error caused by jitter in the visual pose measurement system, thereby improving the accuracy and robustness of the rocket's pose measurement.

[0006] The present invention provides a real-time positioning method for recovery based on an inertial navigation-vision method, comprising the following steps:

[0007] S1. Install an inertial navigation unit and a data processing module inside the rocket, and a visual sensing unit on the outer wall. The visual sensing unit includes at least two sets of downward-facing visual sensors. The ground recovery platform is equipped with a pre-calibrated ground visual cooperation target. The ground visual cooperation target includes a ring and a cross marker with a corner point feature located at the center of the ring.

[0008] S2. During the rocket recovery process, the inertial navigation unit calculates the rocket's attitude and obtains inertial navigation attitude data;

[0009] Determine whether the rocket position reaches the working height of the visual sensing unit. If so, the real-time positioning of the rocket recovery enters the inertial navigation-visual combined navigation stage and enters step S3. If not, the real-time positioning of the rocket recovery is still in the inertial navigation positioning stage, and the data processing module directly outputs the measurement data of the inertial navigation unit as the current position information of the rocket to the control system;

[0010] S3. The visual sensor captures and acquires an image of the ground visual cooperation target and transmits it to the data processing module. The data processing module performs image acquisition, image target recognition and segmentation, image feature extraction, and pose calculation on the ground visual cooperation target to obtain visual pose measurement data and feeds it back to the inertial navigation unit. The inertial navigation unit outputs corrected pose data to the control system based on the visual pose measurement data and the inertial navigation pose data.

[0011] The image features include the circular ring edge features and corner point features of the ground visual cooperation target; the data processing module obtains visual pose measurement data based on the circular ring edge from the circular ring features, and obtains visual pose measurement data based on the corner points from the corner point features. The data processing module combines the visual pose measurement data based on the circular ring edge and the visual pose measurement data based on the corner points according to the position and angle of each visual sensor to obtain visual pose measurement data. The inertial navigation pose data and the visual pose measurement data both include the rocket pitch angle and yaw angle.

[0012] S4. As the subsequent rocket continues to lower its altitude, the rocket continues to lower its altitude toward the ground recovery platform. The visual sensing unit maintains position solution and outputs visual measurement data until the rocket recovery is completed. A real-time positioning method for recovery based on the inertial navigation-vision method is completed.

[0013] The present invention provides a real-time recovery positioning method based on inertial navigation-vision method. As a preferred embodiment, in step S3, a target recognition and segmentation method based on deep learning is used for image target recognition and segmentation, the loss function uses the TaskAlignedAssigner strategy, and the network output is only the classification branch and the regression branch, and iterative training is performed;

[0014] Image object recognition and segmentation also undergoes post-processing, including scale restoration, non-maximum suppression, and instance segmentation based on recognition location.

[0015] The present invention describes a real-time positioning method for recovery based on inertial navigation-vision method, as a preferred method, TAL=s α +u β ;

[0016] Where s is the predicted score corresponding to the labeled category, u is the intersection-over-union ratio of the predicted box and the true box, and the multiplication of s and u can measure the degree of alignment. α and β are weight parameters, and TAL is the weighted decision metric.

[0017] The loss function of the classification branch is the BCE loss function, and the CIOU loss and DFL loss functions are used in the regression branch.

[0018] The real-time positioning method for recycling based on an inertial navigation-vision method described in the present invention, as a preferred embodiment, comprises the following steps of extracting the edge of a circular ring:

[0019] SA, perform edge detection on the ground vision cooperation target through the edge detection operator to obtain the points on the edge map and store them in the array G;

[0020] SB, traverse each point on the two-dimensional plane, calculate the distance to the points in array G, and obtain the maximum distance between each point and the points in array G. The point with the smallest maximum distance among all points is the center of the ellipse (p, q) of the ground visual cooperation target. The maximum distance is the length of the major semi-axis of the ellipse a:

[0021] SC, substitute the value of each point in the array G and p, q, and a into the ellipse equation E to obtain the length b of the minor axis of the ellipse and the angle θ between the line connecting the point on the ellipse and the origin and the positive direction of the x-axis. Statistically analyze the parameters b and θ in the two-dimensional parameter space. The set of parameters whose peak value exceeds the threshold is the detected ring edge.

[0022] The real-time positioning method for recycling based on the inertial navigation-vision method described in the present invention is preferably configured such that, in step SA, edge detection is performed using the Canny operator;

[0023] In step SB, the ellipse equation E is:

[0024]

[0025] Among them, (x, y) is the coordinate of the edge point of the ground vision cooperation target ring.

[0026] The present invention describes a real-time recovery positioning method based on an inertial navigation-vision method. As a preferred method, the method for extracting corner features is as follows: a local window is moved on a ground vision cooperative target image, and when the window moves in both the horizontal and vertical directions, the pixel value change E(x, y) inside the window is calculated; a corner response function is calculated for each window; and then a threshold processing is performed on the corner response function. If the corner response function is greater than the threshold, the window corresponds to a corner feature.

[0027] The present invention provides a real-time recovery positioning method based on an inertial navigation-vision method. As a preferred embodiment, the data processing module uses a ring positioning method based on multiple frames of images to obtain visual posture measurement data based on the ring edge from the ring features; the visual sensor includes a camera;

[0028] The ring positioning method based on multi-frame images is as follows: the data processing module reads the ground vision cooperation target images of continuous frames taken by each camera and calculates the pose solution set of each camera's continuous frame images respectively, and selects the solution set with the smallest pose difference from the solution sets of different cameras as the visual pose measurement data based on the ring edge; records the pose parameters of the current ground vision cooperation target image, continues to read and solve the pose parameters of the next frame of the ground vision cooperation target image, and selects the solution with the smallest difference with the pose parameters of the current frame as the correct pose solution for the next frame of the image during the solution; repeats the above steps to obtain the pose parameters of the continuous images;

[0029] The method for solving the pose parameters is as follows: the extracted ring edge coordinates (x, y, z) are transformed into the standard coordinate space with the rotation axis z' to obtain the coordinate point (x', y', z'), and the center of the circle (x'0, y'0, z'0) in the standard coordinate system and the cutting plane normal vector (n') in the standard coordinate system are obtained by solving the radius R and direction of the ring edge. x ,n' y ,n' z ), and then transform back to the camera coordinate system to obtain the center of the circle (x0, y0, z0) and the cutting plane normal vector (n x ,n y ,n z ), according to the geometric relationship, the pitch angle φ and yaw angle θ are obtained;

[0030]

[0031] Among them, [x0,y0,z0] T =P[x'0,y'0,z'0] T ,[n x ,n y ,n z ] T =P[n' x ,n' y ,n' z ] T , P is the transformation matrix;

[0032] The center position of the circle in standard space is:

[0033]

[0034] The normal vector of the cutting plane in standard space is:

[0035]

[0036] λ1, λ2, λ3 are coefficients, λ1x' 2 +λ2y' 2 +λ3z' 2=0.

[0037] The present invention provides a real-time positioning method for recycling based on an inertial navigation-vision method. As a preferred embodiment, the data processing module uses a corner-based pose visual measurement method to obtain corner-based visual pose measurement data from corner features.

[0038] The pose visual measurement method based on corner points is:

[0039] According to the camera type of the visual sensor unit, when the world coordinate point Z w When it is 0, the perspective projection process is simplified to:

[0040]

[0041] Among them, u and v are the coordinates of the corner point in the image coordinate system, λ is the coefficient, r1 is the first column of the corner point rotation matrix, r2 is the first column of the corner point rotation matrix, t is the translation vector, K is the camera intrinsic parameter matrix, X w 、Y w , Z w are the coordinates of the corner points in the world coordinates, is the H matrix;

[0042] The H matrix is obtained based on the characteristic corner points of the cross marker in each frame of the ground vision cooperation target image;

[0043] It is the abbreviation of H matrix.

[0044] The present invention discloses a real-time positioning method for recycling based on an inertial navigation-vision method. As a preferred method, after completing the view pose calculation of all visual sensing units, a nonlinearization is performed with the minimum reprojection error as the objective function. The calculation results of multiple single views are integrated to output the maximum likelihood solution of the pose:

[0045] e(i)=f(P,Q,A i ,R i ,t i );

[0046] Among them, i is the pose parameter to be solved, P and Q are the image coordinate point matrix and the world coordinate point matrix respectively, (A i ,R i ,t i ) are the internal parameters of the camera corresponding to each single view and its external parameters to the coordinate system, and the maximum likelihood solution of the pose is the visual pose measurement data.

[0047] The real-time positioning method for recycling based on the inertial navigation-vision method described in the present invention is preferably configured such that, in step S1, the visual sensing unit includes four groups of visual sensors evenly arranged, a measurement mathematical model of the multiple visual sensors is established, and parameter calibration is performed;

[0048] In step S2, the working height of the visual sensing unit is 1 km;

[0049] In step S3, the visual pose measurement data and the inertial navigation pose data are fused through the Kalman filtering method to obtain the correction result; when measuring the actual altitude of the rocket, the inertial navigation unit provides the altitude change rate, and the visual sensing unit performs absolute altitude correction.

[0050] This invention relates to positioning, navigation, and control technologies and can be used for precise, real-time position and attitude measurement during a rocket's recovery and landing phase. Based on real-time measurements from an onboard inertial navigation system and a visual position and attitude measurement system, it acquires information about the rocket's position and attitude relative to the recovery site. This invention proposes a real-time positioning and navigation method based on inertial navigation and vision. This method fuses inertial navigation information with visual position and attitude measurement results, mutually correcting measurement errors to improve measurement accuracy and reliability.

[0051] The visual attitude navigation system uses visual sensors to perceive the surrounding environment and utilizes computer vision and image processing technologies to provide guidance for the rocket's flight. Visual sensors are passive sensors that utilize natural information such as visible light or infrared. The visual information they acquire is highly resistant to interference and highly reliable in complex environments. Furthermore, the positioning accuracy of the visual attitude navigation system is only affected by the current image frame, and errors do not accumulate over time. The rocket recovery and landing phase involves highly dynamic motion, and visual measurement methods can significantly increase the accuracy of measurement data, complementing the limitations of inertial navigation.

[0052] Compared with the limitations of using the inertial navigation system and the visual pose measurement system independently, the two navigation and positioning methods are combined, the visual pose measurement system is used to correct the measurement error of the inertial navigation system, and the inertial navigation system is used to make up for the shortcomings of the visual pose measurement system under extreme conditions. The two complement each other. This method can resist external interference, has high measurement accuracy, and has better adaptability to various environments and weather.

[0053] The present invention is mainly used in the return landing phase of a rocket recovery mission. Combining inertial navigation and visual posture measurement methods, a high-precision navigation and positioning method for rocket recovery is proposed. This method adopts inertial navigation-vision fusion technology and utilizes visual posture measurement technology to solve the problem of inertial navigation measurement errors accumulating over time, while using the inertial navigation system to make up for the lack of visual navigation stability. The on-rocket visual detector measures the cooperative target of the recovery site to determine the posture information of the rocket relative to the recovery site, and uses the result to complete the correction of the inertial navigation measurement error; at the same time, the high-precision posture information provided by the inertial navigation system is used to correct the measurement error caused by jitter of the visual posture measurement system, thereby improving the accuracy and robustness of the rocket posture measurement.

[0054] The specific steps of recovery positioning are as follows: (1) When the onboard visual measurement equipment cannot capture the image of the ground cooperative target, the inertial navigation system measurement data is directly output to the control system; (2) After the onboard visual measurement equipment captures the ground cooperative target, the position and attitude information of the rocket body relative to the cooperative target is calculated in real time based on the image information, and the visual posture measurement system solution data and the inertial navigation system data are fused using algorithms such as Kalman filtering to correct the measurement error and output the corrected result; (3) The corrected position, attitude and other information are fed back to the inertial navigation system for posture information calculation in the next navigation cycle; (4) When the onboard inertial navigation system has an abnormality, the visual posture measurement system measurement result is directly output.

[0055] The present invention combines an inertial navigation method and an on-arrow visual posture measurement method for rocket recovery and positioning. The visual posture measurement method is added to the rocket landing stage to make up for the defect of inertial navigation measurement errors accumulating over time and meet the requirement of continuously obtaining high-precision positioning information. A lightweight deep learning algorithm is introduced into the on-arrow visual positioning method, and only a small number of samples are needed to achieve accurate feature extraction. In addition, the test and actual recovered data can be used as training samples. As the number of tests and recoveries increases, the accuracy of image feature extraction gradually increases, and the accuracy of posture measurement gradually improves. The rocket is equipped with a visual sensing module, a data processing module, and an inertial navigation unit. The inertial navigation unit and the data processing module are installed inside the rocket, and the visual sensing module is installed on the outer wall of the rocket (viewing angle downward). The ground is equipped with visual cooperation targets. The cooperation targets include circular features and corner features. Rocket recovery navigation and positioning is divided into two stages: when the onboard visual sensing unit cannot capture the cooperative target image, it outputs the inertial navigation system measurement results; after the onboard visual sensing unit obtains the cooperative target image, it fuses the measurement results of the inertial navigation measurement system and the visual posture measurement system, corrects the measurement errors of each other, and outputs the fusion results.

[0056] The present invention has the following advantages:

[0057] (1) The present invention combines the inertial navigation method with the on-rocket visual posture measurement method to continuously obtain high-precision positioning information. Compared with the traditional satellite navigation positioning method, this method does not rely on external signals, has higher accuracy and stronger independence and adaptability, and provides accurate positioning results for guiding the recovery and landing of the rocket.

[0058] (2) The present invention adds a visual posture measurement mode during the rocket landing phase to compensate for the defect of inertial navigation measurement errors accumulating over time and meet the requirement of continuously obtaining high-precision positioning information.

[0059] (3) The present invention introduces deep learning technology into the visual posture measurement system, uses the test and actual recovered data as training samples, and learns through the deep learning network, which can greatly increase the image recognition rate. As the number of recovered samples increases, the measurement accuracy gradually improves.

[0060] (4) The present invention can independently output the measurement results of the inertial navigation system and the visual posture system, and can also fuse the measurement posture data of the two to correct each other's errors. The working modes are diverse and can meet the navigation and positioning requirements of different stages. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 This is a flow chart of a real-time positioning method for recovery based on inertial navigation-vision method;

[0062] Figure 2 The schematic diagram of a real-time positioning method for recovery based on inertial navigation-vision method is shown below;

[0063] Figure 3 This is a working process diagram of a real-time positioning method for recovery based on inertial navigation-vision method;

[0064] Figure 4 This is a schematic diagram of the installation location of a real-time positioning method for recovery based on inertial navigation-vision method. DETAILED DESCRIPTION

[0065] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments.

[0066] Example 1

[0067] like Figures 1 to 4As shown in the figure, a real-time positioning method for recovery based on the inertial navigation-vision method is used. During the recovery phase, the rocket is equipped with a visual sensor unit, a data processing module, and an inertial navigation unit. The inertial navigation unit and the data processing module are installed inside the rocket, and the visual sensor unit is installed on the outer wall of the rocket (with a downward viewing angle). The ground is equipped with a visual cooperative target. Among them, the cooperative target needs to be pre-calibrated to ensure the solution accuracy of the visual pose measurement system. According to the conditions of the recovery site, the cooperative target completes the arrangement of circular features and corner features to improve the recovery positioning accuracy of the rocket. In addition, in severe weather conditions, the cooperative target can use invisible light bands such as near-infrared, and the visual sensing module can be equipped with filters of the corresponding bands to increase the target recognition rate and reduce the interference of the environment on the position calculation.

[0068] Depending on the rocket's descent height, the recovery phase can be divided into the inertial navigation positioning phase and the inertial navigation-vision combined navigation phase.

[0069] The first phase is the inertial navigation positioning phase. During this phase, the rocket is more than 1 kilometer above the ground. The onboard visual sensor unit cannot capture clear images of the ground cooperating target, and the data processing module cannot effectively extract and resolve features from the cooperating target image. At this time, the data processing module directly outputs the inertial navigation system measurement data as the rocket's current position information.

[0070] The second phase is the inertial-visual combined navigation phase. During this phase, the rocket is less than 1 kilometer above the ground, reaching a height at which the onboard visual sensing unit can stably acquire images of ground cooperating targets. The onboard visual sensing unit captures images of ground cooperating targets and transmits them to the onboard data processing module. This module performs image processing, feature extraction, and pose calculations, generating measurement data from the visual pose processing system. As the rocket continues to descend toward the recovery platform, the optical detection module maintains its position calculations and continuously outputs positioning results.

[0071] The specific solution process of the data processing module is as follows:

[0072] The rocket uses four sets of visual sensors to capture characteristic images of the target area. Preliminary work establishes a mathematical model for multi-visual sensor measurement and calibrates relevant parameters to provide a parameter foundation for subsequent calculations. This foundation allows the data processing module to continuously acquire and process images. This includes image target recognition and segmentation techniques, which accurately locate the target area within a large field of view and segment its features. Image feature extraction techniques are then used to extract geometric feature data from the target area, including circular and corner features. Finally, pose calculations are performed based on these extracted features.

[0073] Image target recognition adopts a target recognition and segmentation method based on deep learning to build a single-stage end-to-end target recognition and segmentation network to achieve target recognition and segmentation with high accuracy, high real-time performance and flexibility. The overall network adopts a lightweight design, making it suitable for carrier embedded computing environments.

[0074] (1) The loss function of the target recognition and segmentation network adopts the TaskAlignedAssigner strategy, as shown in formula (1), which performs weighted fusion of the results obtained by classification and regression to determine the positive and negative of the sample.

[0075] TAL=s α +u β (1)

[0076] Where s is the predicted score corresponding to the labeled category, u is the intersection-over-union ratio (IoU) between the predicted and true boxes. Multiplying the two measures the degree of alignment. α and β are weight parameters, and TAL is the weighted decision metric. Furthermore, the network output lacks a confidence branch, with only classification and regression branches. The classification loss uses the BCE loss function, while the regression branch uses the CIOU loss and DFL loss functions.

[0077] Post-processing for the region recognition and segmentation network includes scale restoration, non-maximum suppression, and instance segmentation based on the identified location. To ensure accurate identification and segmentation of falling areas under varying weather and environmental conditions, an iterative training approach is employed. This involves collecting as diverse environmental samples as possible for training, and continuously using the latest collected data as trials and tests progress, thereby optimizing the network for robustness.

[0078] Image feature extraction includes two parts: ring edge extraction and corner point extraction. Regarding ring feature extraction, based on the geometric property that the center of an ellipse is the point with the smallest maximum distance from all points on the plane to the points on the ellipse contour, the center of the circle is found by finding the point with the smallest maximum distance from the points on the ellipse. At the same time, this smallest maximum distance is the length of the major axis of the ellipse. Through this method, the five parameters of the ellipse (the horizontal and vertical coordinates of the ellipse center point, the major and minor axis lengths, and the rotation angle) are reduced to two parameters. Statistics can be performed in a two-dimensional parameter space and converted into the same method of detecting ellipses using the Hough transform to detect lines. The specific implementation steps of the algorithm are as follows:

[0079] 1. Edge Detection: Commonly used edge detection operators include Sobel, LoG, and Canny. This solution uses the Canny operator for edge extraction. The Canny operator is an edge detection operator based on first-order differentials. It filters out noise while effectively preserving edge features, meeting the requirements of subsequent recognition. The points on the edge map are stored in array G.

[0080] ② Determine the center of the ellipse: traverse each point on the two-dimensional plane, calculate the distance to the point in the array G obtained in the previous step, and obtain the maximum distance between each point and the point in the array G. The point with the smallest maximum distance among all points is the center of the ellipse (p, q), and this maximum distance is the length a of the major semi-axis of the ellipse.

[0081] ③Hough transform ellipse detection: Substitute the value of each point in the array G and the three ellipse parameters p, q, and a just obtained into the ellipse equation E:

[0082]

[0083] Among them, (x, y) are the coordinates of the point on the ellipse, b is the length of the minor semi-axis of the ellipse, and θ is the angle between the line connecting the point on the ellipse and the origin and the positive direction of the x-axis. The parameters b and θ are statistically analyzed in the two-dimensional parameter space. The set of parameters whose peak value exceeds a certain threshold is the ellipse, which realizes the detection of ellipses in the image.

[0084] As for corner features, the Harris corner detection algorithm is used to extract sub-pixel image coordinates of feature corners. The core idea is to use a local window to move on the image to determine whether there is a significant change in grayscale. If the grayscale value within the window (on the gradient map) has a significant change, then there is a corner in the area where the window is located. The corner detection algorithm is divided into the following three steps: when the window (local area) moves in both the x (horizontal) and y (vertical) directions at the same time, the pixel value change E(x, y) inside the window is calculated; for each window, a corresponding corner response function is calculated; then the function is thresholded. If the corner response function is greater than the threshold, it means that the window corresponds to a corner feature.

[0085] After completing feature extraction, the data processing module further adopts a ring-based pose measurement algorithm and a corner-based pose measurement algorithm. The two algorithms complement each other and are finally integrated to obtain the pose calculation results of the visual pose measurement system.

[0086] The core of the pose vision measurement algorithm based on the ring is to solve the three-dimensional coordinates of the center of the spatial circle and the normal vector of the spatial circle supporting plane. Once the normal vector of the supporting plane is determined, the pitch angle and yaw angle can be calculated based on the geometric trigonometric relationship. However, when estimating the pose based on a single circular feature, the problem of ambiguity will be encountered, that is, there may be two different camera poses that can generate the same circular projection. This is because the projection of a circle may come from two different three-dimensional ring positions. In order to solve this problem, a ring positioning algorithm based on multi-frame images is proposed. The pose of the target in each frame is determined by the continuity of the pose states of consecutive frames, eliminating ambiguity and making it possible to determine a unique pose. The specific calculation process is as follows:

[0087] Considering that the spatial circle is projected as an ellipse on the image plane, the matrix form of the ellipse equation in the pixel coordinate system is:

[0088] [x yz]Q[xy z] T =0(3)

[0089]

[0090] Among them, A, B, C, D, and E are the coefficients of the ellipse equation, and f is the focal length of the ellipse. Assuming that the radius of the space circle is r, solving the position and direction of the space circle is equivalent to finding a cutting plane in space so that after the plane cuts the elliptical cone, it intersects with the elliptical cone surface to form a circle with a radius of R. The three-dimensional coordinates of the center of the circle and the normal vector of the cutting plane are what we are looking for. Since the expression of the cone is very complicated when calculated directly in the camera coordinate system space, and it is not easy to describe and calculate when the cutting plane cuts the elliptical cone, it is transformed into the standard coordinate space to calculate the result and then transformed back to the camera coordinate system. The standard elliptical cone with the rotation axis z' obtained by the transformation is expressed as:

[0091] λ1x' 2 +λ2y' 2 +λ3z' 2 =0(5)

[0092] Where (x', y', z') are the coordinates of the standard elliptical cone with the rotation axis z', and λ1, λ2, and λ3 are coefficients;

[0093] When the elliptical cone in the standard space is found, the center position can be obtained as:

[0094]

[0095] The cutting plane normal vector is:

[0096]

[0097] Combined with the camera's imaging model, the three-dimensional coordinates of the circle center and the normal vector of the cutting plane can be converted to camera coordinates:

[0098] [x0,y0,z0] T =P[x'0,y'0,z'0] T (8)

[0099] [n x ,n y ,n z ] T =P[n' x ,n' y ,n' z ] T(9)

[0100] Where P is the transformation matrix.

[0101] According to the geometric relationship, the pitch angle φ and yaw angle θ can be obtained:

[0102]

[0103] To eliminate ambiguity, the process begins by capturing consecutive frames of scene images. The processor reads the images and calculates a set of pose solutions for each frame. From this set, the solution with the smallest pose difference is selected. The pose parameters of the current image are recorded, and the pose of the next frame is read and solved. The solution with the smallest difference from the pose parameters of the current frame is selected as the correct pose solution for the next frame. Repeating these steps yields the pose parameters for consecutive images.

[0104] The pose visual measurement algorithm based on corner points is mainly calculated based on the point features of the cooperative target. Since the feature points in this solution are placed in a plane, a pose solution algorithm based on the homography matrix is developed. According to the camera type, when the world coordinate point Z w When it is 0, the perspective projection process is simplified to:

[0105]

[0106] Among them, u and v are the coordinates of the corner point in the image coordinate system, λ is the coefficient, r1 is the first column of the corner point rotation matrix, r2 is the first column of the corner point rotation matrix, t is the translation vector, K is the camera intrinsic parameter matrix, X w 、Y w 、Z w are the coordinates of the corner points in the world coordinates, The H matrix can be obtained based on the characteristic corner points of the cross marker in each frame image. When the camera intrinsic parameter matrix is known, r and t can be directly calculated and expressed as:

[0107]

[0108] r3=r1×r2; (14)

[0109] After all camera view poses are calculated, the nonlinearization can be performed with the minimum reprojection error as the objective function, and the calculation results of multiple single views can be combined to output the maximum likelihood solution of the pose.

[0110] e(i)=f(P,Q,A i ,R i ,t i ) (15)

[0111] Among them, i is the pose parameter to be solved, P and Q are the image coordinate point matrix and the world coordinate point matrix respectively, (A i ,R i ,t i ) are the internal parameters of the camera corresponding to each single perspective and its external parameters to the coordinate system.

[0112] Since the visual pose measurement system's measurement results are based only on the current image and are unrelated to previous and subsequent images, they offer strong real-time performance. Visual measurement results provide more position information, while inertial measurements can provide motion status information, such as speed, acceleration, and angular velocity. Fusion of visual pose measurement results with those of the inertial navigation system through the Kalman filter method can effectively improve measurement accuracy and robustness. When measuring the actual altitude of a rocket, the inertial navigation system provides the altitude change rate, while the visual pose system can perform absolute altitude corrections. After simulation verification testing, the measurement error can be reduced to within 0.5%, and the measurement accuracy meets actual requirements. Image reading and feature reading require 25ms, pose solution 5ms, and filtering and outputting the results after calculating the pose parameters takes approximately 3ms. The transmission takes approximately 1ms, resulting in an overall time delay of 40ms, enabling high-speed pose measurement.

[0113] pass Figure 1 By linking the output flow chart with the corresponding links during the rocket recovery and landing phase, we can derive a specific workflow for a high-precision, real-time positioning method for the rocket recovery and landing phase. When the visual pose measurement system's measurement altitude requirements are not met, the inertial navigation system's pose data is independently output. When the visual pose measurement system's measurement altitude requirements are met, the inertial navigation system and visual pose measurement system data are integrated and output.

[0114] The above description is only a preferred specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any technician familiar with the technical field, within the technical scope disclosed by the present invention, who makes equivalent replacements or changes based on the technical solution and inventive concept of the present invention, should be covered by the scope of protection of the present invention.

Claims

1. A real-time positioning method for recovery based on inertial navigation and vision, characterized by: The following steps are involved: S1. Install an inertial navigation unit and a data processing module inside the rocket, and a visual sensing unit on the outer wall. The visual sensing unit includes at least two sets of downward-facing visual sensors. The ground recovery platform is equipped with a pre-calibrated ground visual cooperation target. The ground visual cooperation target includes a ring and a cross marker with a corner point feature located at the center of the ring. S2. During the rocket recovery process, the inertial navigation unit calculates the rocket's posture to obtain inertial navigation posture data; Determine whether the rocket position reaches the working height of the visual sensing unit. If so, the rocket recovery real-time positioning enters the inertial navigation-visual combined navigation stage and enters step S3. If not, the rocket recovery real-time positioning is still in the inertial navigation positioning stage, and the data processing module directly outputs the measurement data of the inertial navigation unit as the current position information of the rocket to the control system; S3, the visual sensor captures and acquires the image of the ground visual cooperation target and transmits it to the data processing module, the data processing module performs image acquisition, image target recognition and segmentation, image feature extraction and posture calculation of the ground visual cooperation target to obtain visual posture measurement data and feeds it back to the inertial navigation unit, and the inertial navigation unit outputs corrected posture data to the control system according to the visual posture measurement data and the inertial navigation posture data; The image features include circular ring edge features and corner point features of the ground visual cooperation target; the data processing module obtains visual pose measurement data based on the circular ring edge from the circular ring features, and obtains visual pose measurement data based on the corner points from the corner point features; the data processing module obtains the visual pose measurement data by combining the visual pose measurement data based on the circular ring edge and the visual pose measurement data based on the corner points according to the position and angle of each visual sensor, and the inertial navigation pose data and the visual pose measurement data both include the rocket pitch angle and yaw angle; S4. As the subsequent rocket continues to decrease in altitude, the rocket continues to lower its altitude toward the ground recovery platform. The visual sensing unit maintains position solution and outputs the visual measurement data until the rocket recovery is completed. A real-time positioning method for recovery based on the inertial navigation-vision method is completed.

2. The real-time positioning method for recovery based on inertial navigation and vision according to claim 1, characterized in that: In step S3, the deep learning-based target recognition and segmentation method is used for image target recognition and segmentation, the TaskAlignedAssigner strategy is used as the loss function, and the network output is only the classification branch and the regression branch, and iterative training is performed; Image object recognition and segmentation also undergoes post-processing, including scale restoration, non-maximum suppression, and instance segmentation based on recognition location.

3. The real-time positioning method for recovery based on inertial navigation and vision according to claim 2, characterized in that: TAL=s α +and β ; Where s is the predicted score corresponding to the labeled category, u is the intersection-over-union ratio of the predicted box and the true box, and the multiplication of s and u can measure the degree of alignment. α and β are weight parameters, and TAL is the weighted decision metric. The loss function of the classification branch is the BCE loss function, and the CIOU loss and DFL loss functions are used in the regression branch.

4. The real-time positioning method for recovery based on inertial navigation and vision according to claim 1, characterized in that: The method for extracting the ring edge includes the following steps: SA, performing edge detection on the ground vision cooperation target by using an edge detection operator to obtain points on the edge map and store them in an array G; SB, traverse each point on the two-dimensional plane, calculate the distance to the points in array G, and obtain the maximum distance between each point and the points in array G. The point with the smallest maximum distance among all points is the center of the ellipse (p, q) of the ground visual cooperation target. The maximum distance is the length of the major semi-axis of the ellipse a: SC, substitute the value of each point in the array G and p, q, and a into the ellipse equation E to obtain the length b of the minor axis of the ellipse and the angle θ between the line connecting the point on the ellipse and the origin and the positive direction of the x-axis. Statistically analyze the parameters b and θ in the two-dimensional parameter space. The set of parameters whose peak value exceeds the threshold is the detected ring edge.

5. The real-time positioning method for recovery based on inertial navigation and vision method according to claim 4 is characterized in that: In step SA, edge detection is performed using the Canny operator; In step SB, the ellipse equation E is: Wherein, (x, y) is the coordinate of the edge point of the ground vision cooperation target ring.

6. The real-time positioning method for recovery based on inertial navigation and vision according to claim 1, characterized in that: The method for extracting corner features is as follows: a local window is moved on the ground visual cooperative target image, and when the window moves in the horizontal and vertical directions at the same time, the pixel value change E(x, y) inside the window is calculated; a corner response function is calculated for each window; and then the corner response function is thresholded. If the corner response function is greater than the threshold, the window corresponds to a corner feature.

7. The real-time positioning method for recovery based on inertial navigation and vision according to claim 1, characterized in that: The data processing module uses a ring positioning method based on multiple frames of images to obtain visual posture measurement data based on the ring edge from the ring features; the visual sensor includes a camera; The ring positioning method based on multiple frames of images is as follows: the data processing module reads the ground visual cooperation target image of each continuous frame taken by the camera and calculates the pose solution set of each camera's continuous frame image respectively, and selects the solution set with the smallest pose difference from the solution sets of different cameras as the visual pose measurement data based on the edge of the ring; records the pose parameters of the current ground visual cooperation target image, continues to read and solve the pose parameters of the next frame of the ground visual cooperation target image, and selects the solution with the smallest difference with the pose parameters of the current frame as the correct pose solution for the next frame of image during the solution; Repeat the above steps to obtain the pose parameters of continuous images; The method for solving the pose parameters is as follows: the extracted ring edge coordinates (x, y, z) are transformed into the standard coordinate space with the rotation axis z' to obtain the coordinate point (x', y', z'), and the center of the circle (x'0, y'0, z'0) in the standard coordinate system and the cutting plane normal vector (n') in the standard coordinate system are obtained by solving the radius R and direction of the ring edge. x ,n' y ,n' z ), and then transform back to the camera coordinate system to obtain the center of the circle (x0, y0, z0) and the cutting plane normal vector (n x ,n y ,n z ), according to the geometric relationship, the pitch angle φ and yaw angle θ are obtained; Among them, [x0,y0,z0] T =P[x'0,y'0,z'0] T ,[n x ,n y ,n z ] T =P[n' x ,n' y ,n' z ] T , P is the transformation matrix; The center position of the circle in standard space is: The normal vector of the cutting plane in standard space is: λ1, λ2, λ3 are coefficients, λ1x' 2 +λ2y' 2 +λ3z' 2 =0.

8. The real-time positioning method for recovery based on inertial navigation and vision according to claim 1, characterized in that: The data processing module obtains corner-based visual pose measurement data from the corner point features using a corner-based pose visual measurement method; The pose visual measurement method based on corner points is: According to the camera type of the visual sensing unit, when the world coordinate point Z w When it is 0, the perspective projection process is simplified to: Among them, u and v are the coordinates of the corner point in the image coordinate system, λ is the coefficient, r1 is the first column of the corner point rotation matrix, r2 is the first column of the corner point rotation matrix, t is the translation vector, K is the camera intrinsic parameter matrix, X w 、Y w 、Z w are the coordinates of the corner points in the world coordinates, is the H matrix; The H matrix is obtained by calculating the characteristic corner points of the cross marker in each frame of the ground vision cooperation target image; 9. The real-time positioning method for recovery based on inertial navigation and vision according to claim 1, characterized in that: After the view pose calculations of all the visual sensing units are completed, nonlinearization is performed with the minimum reprojection error as the objective function, and the calculation results of multiple single views are integrated to output the maximum likelihood solution of the pose: e(i)=f(P,Q,A i ,R i ,t i ); Among them, i is the pose parameter to be solved, P and Q are the image coordinate point matrix and the world coordinate point matrix respectively, (A i ,R i ,t i ) are the internal parameters of the camera corresponding to each single perspective and its external parameters to the coordinate system, and the maximum likelihood solution of the pose is the visual pose measurement data.

10. The real-time positioning method for recovery based on inertial navigation and vision according to claim 1, characterized in that: In step S1, the visual sensing unit includes four groups of visual sensors evenly arranged, a measurement mathematical model of multiple visual sensors is established and parameter calibration is performed; In step S2, the working height of the visual sensing unit is 1 km; In step S3, the visual pose measurement data and the inertial navigation pose data are fused by the Kalman filtering method to obtain the correction result; when measuring the actual altitude of the rocket, the inertial navigation unit provides the altitude change rate, and the visual sensing unit performs absolute altitude correction.

Images