Indoor positioning methods
By extracting the ORB point features and FLD line features in the image, combining the inertial measurement unit information, predicting and updating the position of the moving carrier, the problem of low indoor positioning accuracy is solved and a higher positioning accuracy is achieved.
Patent Information
- Application Number
- CN202210700292.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-06-20
- Publication Date
- 2024-12-03
- Estimated Expiration
- 2042-06-20
AI Technical Summary
In the prior art, indoor positioning accuracy is poor, especially in environments with insufficient light or weak texture, it is difficult to accurately extract ORB point features, resulting in low positioning accuracy of mobile carriers.
By collecting images at the current and previous moments, ORB point features and FLD line features are extracted respectively, target point feature pairs are determined, target rotation matrix and translation matrix are calculated, and the position information of the inertial measurement unit is combined to predict and update the mobile carrier. pose information.
The positioning accuracy of the mobile carrier in indoor insufficient light or weak texture environments is improved, the accuracy of positioning information is ensured, and the problem of low positioning accuracy in the prior art is solved.
Smart Images

Figure CN114998389B_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of positioning, and in particular, to an indoor positioning method. Background Art
[0002] In recent years, the development of smart homes has made home life more efficient and convenient. Among them, the precise positioning of mobile carriers such as sweeping robots is the basis for achieving efficiency and convenience. For indoor positioning, a single sensor can no longer meet the positioning needs of mobile carriers. For mobile carriers with multi-sensor fusion, its navigation system can coordinate the application of multiple sensor information to improve positioning accuracy. Multi-sensor fusion usually includes lidar and vision fusion, as well as inertial sensor and vision fusion. Among them, compared with lidar and vision fusion, inertial sensor and vision fusion has lower cost and higher accuracy.
[0003] For the fusion of inertial sensors and vision, the fusion methods can be divided into two methods: tight coupling and loose coupling. Tight coupling refers to the direct fusion of visual observation information and inertial measurement unit (IMU) measurement information to construct the motion equation, and then optimize the posture and map points in combination with the back-end nonlinear optimization algorithm. This method can avoid the cumulative error of the intermediate state, but due to the high dimension, the amount of calculation is also large. Therefore, this method is not suitable for embedded terminals. Loose coupling is to treat the two sensors as separate modules, and use the position, posture and other parameters of the camera with the inertial navigation data for fusion calculation. This method is simple to calculate and has high scalability, so it can update the real-time position on the embedded platform. For indoor environments, due to the influence of weak texture information and lighting, it is difficult for the camera to extract point features of the indoor environment. Especially when there are fewer point features extracted from the indoor environment, it is easy to cause problems such as poor positioning accuracy or even positioning failure.
[0004] Therefore, there is an urgent need for a method that can more accurately locate a mobile carrier.
[0005] The above information disclosed in the background technology section is only used to enhance the understanding of the background technology of the technology described in this article. Therefore, the background technology may contain certain information that does not form the prior art known in this country for those skilled in the art. Summary of the invention
[0006] The main purpose of the present application is to provide an indoor positioning method to solve the problem of poor positioning accuracy of mobile carriers in the prior art.
[0007] According to one aspect of an embodiment of the present invention, an indoor positioning method is provided, in which a mobile carrier includes an image acquisition device, and the indoor positioning method includes: respectively acquiring multiple ORB point features and multiple FLD line features of each of a plurality of images, the plurality of images including a first image acquired by the image acquisition device at a current moment and a second image acquired at a moment before the current moment; determining a plurality of target point feature pairs based on at least the ORB point features and the FLD line features corresponding to the first image and the second image, and determining a target rotation matrix and a target translation matrix of the image acquisition device at the current moment based on the plurality of target point feature pairs; and predicting and updating the position and posture information of the mobile carrier at the next moment based on the target rotation matrix, the target translation matrix and the position and posture information of the mobile carrier.
[0008] Optionally, determining a plurality of target point feature pairs at least according to the ORB point features and the FLD line features corresponding to the first image and the second image comprises: determining a plurality of preset feature pairs at least according to each of the ORB point features and each of the FLD line features, wherein the plurality of preset feature pairs comprises a plurality of preset point feature pairs and a plurality of preset line feature pairs, the preset point feature pair comprises two of the ORB point features having a first preset relationship, one of the two ORB point features being in the first image and the other being in the second image, the first preset relationship being that the similarity of the two ORB point features is greater than a first preset threshold, the preset line feature pair comprises two of the FLD line features having a second preset relationship, one of the two FLD line features being in the first image and the other being in the second image, the second preset relationship being that the similarity of the two FLD line features is greater than a second preset threshold; removing dynamic objects from the first image according to the plurality of preset line feature pairs to obtain a plurality of target line feature pairs; determining a plurality of target point feature pairs according to the plurality of preset point feature pairs and the plurality of target line feature pairs.
[0009] Optionally, a plurality of preset feature pairs are determined at least based on each of the ORB point features and each of the FLD line features, including: determining a BRIEF descriptor corresponding to each of the ORB point features, and determining an LBD descriptor corresponding to each of the FLD line features; determining a plurality of preset point feature pairs based on each of the BRIEF descriptors in the first image and each of the BRIEF descriptors in the second image; and determining a plurality of preset line feature pairs based on each of the LBD descriptors in the first image and each of the LBD descriptors in the second image.
[0010] Optionally, a plurality of preset point feature pairs are determined according to each BRIEF descriptor in the first image and each BRIEF descriptor in the second image, including: calculating the Hamming distance between the i-th BRIEF descriptor in the first image and each BRIEF descriptor in the second image to obtain a plurality of first numerical values, wherein i starts from 1 and extends to N, where N is the total number of the BRIEF descriptors in the first image; determining the two ORB point features corresponding to the first numerical values greater than the first preset threshold as a preset point feature pair; and using the RANSAC algorithm to eliminate the mismatched preset point feature pairs to obtain a plurality of the preset point feature pairs.
[0011] Optionally, based on each LBD descriptor in the first image and each LBD descriptor in the second image, a plurality of preset line feature pairs are determined, including: calculating the Hamming distance between the j-th LBD descriptor in the first image and each LBD descriptor in the second image to obtain a plurality of second numerical values, wherein j starts from 1 and extends to K, where K is the total number of LBD descriptors in the first image; determining the two FLD line features corresponding to the second numerical values greater than the second preset threshold as candidate line feature pairs; and using the RANSAC algorithm to eliminate the mismatched candidate line feature pairs to obtain a plurality of preset line feature pairs.
[0012] Optionally, according to the plurality of preset line feature pairs, the dynamic objects in the first image are removed to obtain a plurality of target line feature pairs, including: calculating the spatial distance of each of the preset line feature pairs to obtain a plurality of target spatial distances; and calculating the spatial distance of each of the target spatial distances according to the plurality of preset line feature pairs. Determine multiple third values, where w is the third value, d is the target space distance, v0 is the degree of freedom of t distribution, μ D is the mean of the t distribution, σ D is the variance of the t distribution; among the multiple preset line feature pairs, the preset line feature pairs corresponding to the third value less than the third preset threshold are removed to obtain the multiple target line feature pairs.
[0013] Optionally, multiple target point feature pairs are determined based on multiple preset point feature pairs and multiple target line feature pairs, including: determining multiple alternative point feature pairs based on the endpoints of each target line feature pair; and forming multiple target point feature pairs from multiple preset point feature pairs and multiple alternative point feature pairs.
[0014] Optionally, based on the multiple target point feature pairs, the target rotation matrix and the target translation matrix of the image acquisition device at the current moment are determined, including: using the epipolar geometry algorithm to determine the preset rotation matrix and the preset translation matrix of each target point feature pair; calculating the average value of the multiple preset rotation matrices to obtain the target rotation matrix of the image acquisition device at the current moment, and calculating the average value of the multiple preset translation matrices to obtain the target translation matrix of the image acquisition device at the current moment.
[0015] Optionally, the mobile carrier also includes an inertial measurement unit, and the posture information includes position information, attitude information and speed information. The process of determining the posture information of the mobile carrier includes: determining the target specific force vector in the coordinate system where the mobile carrier is located at least based on the linear acceleration information; determining the target angular rotation velocity in the coordinate system where the mobile carrier is located at least based on the angular acceleration information; using a strapdown inertial navigation algorithm to perform posture solution on the target specific force vector and the target angular rotation velocity to determine the position information, the attitude information and the speed information of the mobile carrier, wherein the linear acceleration information and the angular acceleration information are measurement information of the inertial measurement unit in the navigation coordinate system.
[0016] Optionally, based on the target rotation matrix, the target translation matrix and the posture information of the mobile carrier, the posture information of the mobile carrier at the next moment is predicted and updated, including: using the MSF fusion algorithm to perform information fusion on the target rotation matrix, the target translation matrix and the posture information, and predicting and updating the posture information of the mobile carrier at the next moment.
[0017] In an embodiment of the present invention, in the indoor positioning method, first, a first image at a current moment and a second image at a previous moment are captured by the image acquisition device, and ORB (FAST corner detection and BRIEF descriptor, Oriented FAST and Rotated BRIEF, referred to as ORB) point features and FLD (Linear Discriminant Analysis, Fisher Linear Discriminant, referred to as FLD) line features in the first image and the second image are respectively captured; then, based on the ORB point features and the FLD line features of the first image, and the ORB point features and the FLD line features of the second image, a plurality of target point feature pairs are determined, and based on the plurality of target point feature pairs, a target rotation matrix and a target translation matrix of the image acquisition at the current moment are determined; finally, based on the target rotation matrix, the target translation matrix and the posture information of the mobile carrier, the posture information of the mobile carrier at the next moment is predicted and updated. Compared with the prior art that determines the target rotation matrix and target translation matrix of the image acquisition device at the current moment only through ORB point features or only through FLD line features, this solution determines multiple target point feature pairs based on the ORB point features and FLD line features of the first image, and the ORB point features and FLD line features of the second image, thereby ensuring a large number of determined target point feature pairs, ensuring that the determined target rotation matrix and target translation matrix are relatively accurate, thereby solving the problem that the image acquisition device is difficult to extract ORB point features and extracts fewer ORB point features in an indoor environment with insufficient light or weak texture. This solution predicts and updates the pose information of the mobile carrier at the next moment based on the target rotation matrix, the target translation matrix and the pose information of the mobile carrier, ensuring that the predicted pose information of the mobile carrier at the next moment is relatively accurate, ensuring that the positioning accuracy of the mobile carrier is high, and thus solving the problem of poor positioning accuracy of the mobile carrier in the prior art. BRIEF DESCRIPTION OF THE DRAWINGS
[0018] The drawings constituting part of the present application are used to provide a further understanding of the present application. The exemplary embodiments and descriptions of the present application are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:
[0019] Figure 1 A flow chart of an indoor positioning method according to an embodiment of the present application is shown;
[0020] Figure 2 A schematic diagram of an epipolar geometry algorithm according to an embodiment of the present application is shown;
[0021] Figure 3A schematic diagram of a strapdown inertial navigation algorithm according to an embodiment of the present application is shown;
[0022] Figure 4 A schematic structural diagram of an indoor positioning device according to an embodiment of the present application is shown;
[0023] Figure 5 A flow chart of an indoor positioning method according to a specific embodiment of the present application is shown. DETAILED DESCRIPTION
[0024] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.
[0025] In order to enable those skilled in the art to better understand the solution of the present application, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present application.
[0026] It should be noted that the terms "first", "second", etc. in the specification and claims of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present application described here. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device comprising a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.
[0027] As mentioned in the background technology, the positioning accuracy of the mobile carrier in the prior art is relatively poor. In order to solve the above problem, in a typical implementation of the present application, an indoor positioning method is provided.
[0028] According to an embodiment of the present application, an indoor positioning method is provided.
[0029] Figure 1 is a flow chart of an indoor positioning method according to an embodiment of the present application. The mobile carrier includes an image acquisition device, such as Figure 1 As shown, the indoor positioning method includes the following steps:
[0030] Step S101, respectively collecting a plurality of ORB point features and a plurality of FLD line features of each of the plurality of images, wherein the plurality of images include a first image collected by the image collection device at a current moment and a second image collected at a moment before the current moment;
[0031] Step S102, determining a plurality of target point feature pairs at least according to the ORB point features and the FLD line features corresponding to the first image and the second image, and determining a target rotation matrix and a target translation matrix of the image acquisition device at the current moment according to the plurality of target point feature pairs;
[0032] Step S103, predicting and updating the above-mentioned posture information of the above-mentioned mobile carrier at the next moment according to the above-mentioned target rotation matrix, the above-mentioned target translation matrix and the above-mentioned posture information of the above-mentioned mobile carrier.
[0033] In the above-mentioned indoor positioning method, first, a first image at the current moment and a second image at the previous moment are collected by the above-mentioned image acquisition device, and ORB (FAST corner detection and BRIEF descriptor, Oriented FAST and Rotated BRIEF, referred to as ORB) point features and FLD (Linear Discriminant Analysis, Fisher Linear Discriminant, referred to as FLD) line features in the above-mentioned first image and the above-mentioned second image are respectively collected; then, according to the above-mentioned ORB point features and the above-mentioned FLD line features of the above-mentioned first image, and the above-mentioned ORB point features and the above-mentioned FLD line features of the above-mentioned second image, a plurality of the above-mentioned target point feature pairs are determined, and according to the plurality of the above-mentioned target point feature pairs, a target rotation matrix and a target translation matrix of the above-mentioned image acquisition at the current moment are determined; finally, according to the above-mentioned target rotation matrix, the above-mentioned target translation matrix and the position and posture information of the above-mentioned mobile carrier, the above-mentioned position and posture information of the above-mentioned mobile carrier at the next moment is predicted and updated. Compared with the prior art that determines the target rotation matrix and target translation matrix of the image acquisition device at the current moment only through ORB point features or only through FLD line features, this solution determines multiple target point feature pairs based on the ORB point features and FLD line features of the first image, and the ORB point features and FLD line features of the second image, thereby ensuring a large number of determined target point feature pairs, ensuring that the determined target rotation matrix and target translation matrix are relatively accurate, thereby solving the problem that the image acquisition device is difficult to extract ORB point features and extracts fewer ORB point features in an indoor environment with insufficient light or weak texture. This solution predicts and updates the pose information of the mobile carrier at the next moment based on the target rotation matrix, the target translation matrix and the pose information of the mobile carrier, ensuring that the predicted pose information of the mobile carrier at the next moment is relatively accurate, ensuring that the positioning accuracy of the mobile carrier is high, and thus solving the problem of poor positioning accuracy of the mobile carrier in the prior art.
[0034] Specifically, the above-mentioned mobile carrier can be a mobile robot.
[0035] Specifically, the image acquisition device may be a device having an image capturing function such as a camera or a webcam.
[0036] Specifically, the position and posture information of the mobile carrier can be obtained by an inertial measurement unit, and of course, it can also be obtained by a laser radar or a millimeter wave radar. In a specific embodiment of the present application, the position and posture information of the mobile carrier is obtained by an inertial measurement unit, which can ensure low costs and further ensure high positioning accuracy of the mobile carrier.
[0037] Specifically, before respectively capturing the first image and the second image through the image acquisition device, the above-mentioned indoor positioning method also includes calibrating the image acquisition device through the Zhang Zhengyou chessboard calibration method to obtain the intrinsic parameter matrix and distortion factor of the image acquisition device, and finally inputting a monocular image and using the distortion model to perform distortion correction on the image captured by the image acquisition device.
[0038] In an embodiment of the present application, a plurality of target point feature pairs are determined at least according to the ORB point features and the FLD line features corresponding to the first image and the second image, including: determining a plurality of preset feature pairs at least according to each of the ORB point features and each of the FLD line features, wherein the plurality of preset feature pairs include a plurality of preset point feature pairs and a plurality of preset line feature pairs, the preset point feature pair includes two of the ORB point features having a first preset relationship, one of the two ORB point features is in the first image, and the other is in the second image, and the first preset relationship is that the similarity of the two ORB point features is greater than a first preset threshold, and the preset line feature pair includes two of the FLD line features having a second preset relationship, one of the two FLD line features is in the first image, and the other is in the second image, and the second preset relationship is that the similarity of the two FLD line features is greater than a second preset threshold; according to the plurality of preset line feature pairs, dynamic objects in the first image are removed to obtain a plurality of target line feature pairs; and according to the plurality of preset point feature pairs and the plurality of target line feature pairs, a plurality of target point feature pairs are determined. In actual application, since dynamic objects will affect the positioning accuracy of the mobile carrier, this solution determines multiple preset point feature pairs and multiple preset line feature pairs based on multiple ORB point features and FLD line features in the first image and the second image, and then eliminates the preset line feature pairs corresponding to the dynamic objects in the first image to obtain target line feature pairs, and determines the target point feature pairs based on the multiple preset point feature pairs and the multiple target line feature pairs. This further ensures that a large number of target point feature pairs are extracted, and further solves the problem of poor positioning accuracy of the mobile carrier due to the small number of extracted target point feature pairs in indoor environments with insufficient light or weak texture.
[0039] Specifically, in the above-mentioned embodiment, dynamic objects in the first image are removed according to multiple preset line feature pairs to obtain multiple target line feature pairs, that is, the preset line feature pairs corresponding to the dynamic objects in the first image are eliminated to obtain the target line feature pairs (that is, the line feature pairs corresponding to the stationary objects).
[0040] In order to determine multiple preset feature pairs more simply, in another embodiment of the present application, multiple preset feature pairs are determined at least based on each of the above-mentioned ORB point features and each of the above-mentioned FLD line features, including: determining a BRIEF (binary descriptor, Binary Robust Independent Elementary Features, referred to as BRIEF) descriptor corresponding to each of the above-mentioned ORB point features, and determining an LBD (Line Band Descriptor, referred to as LBD) descriptor corresponding to each of the above-mentioned FLD line features; determining multiple preset point feature pairs based on each of the above-mentioned BRIEF descriptors in the first image and each of the above-mentioned BRIEF descriptors in the second image; determining multiple preset line feature pairs based on each of the above-mentioned LBD descriptors in the first image and each of the above-mentioned LBD descriptors in the second image.
[0041] In another embodiment of the present application, a plurality of the preset point feature pairs are determined according to the BRIEF descriptors in the first image and the BRIEF descriptors in the second image, including: calculating the Hamming distance between the i-th BRIEF descriptor in the first image and the BRIEF descriptors in the second image to obtain a plurality of first numerical values, wherein i starts from 1 and extends to N, where N is the total number of the BRIEF descriptors in the first image; determining the two ORB point features corresponding to the first numerical values greater than the first preset threshold as the preset point feature pairs; and using RANSAC (Random Sample Consensus, RANSAC for short) to eliminate mismatched preset point feature pairs to obtain a plurality of preset point feature pairs. Specifically, this scheme performs feature matching on the ORB point features in the first image and the ORB point features in the second image according to the Hamming distance, which ensures that the predetermined point feature pairs can be determined relatively simply and efficiently, and then the RANSAC algorithm is used to eliminate the mismatched predetermined point feature pairs to obtain multiple preset point feature pairs, which further ensures that the subsequent target point feature pairs are relatively accurate.
[0042] In order to determine multiple preset line feature pairs more simply and accurately, in another embodiment of the present application, multiple preset line feature pairs are determined based on the LBD descriptors in the first image and the LBD descriptors in the second image, including: calculating the Hamming distance between the j-th LBD descriptor in the first image and the LBD descriptors in the second image to obtain multiple second values, where j starts from 1 and extends to K, where K is the total number of LBD descriptors in the first image; determining the two FLD line features corresponding to the second values greater than the second preset threshold as candidate line feature pairs; and using the RANSAC algorithm to eliminate the mismatched candidate line feature pairs to obtain multiple preset line feature pairs.
[0043] In order to obtain multiple target line feature pairs more simply, in one embodiment of the present application, according to the multiple preset line feature pairs, the dynamic objects in the first image are removed to obtain multiple target line feature pairs, including: calculating the spatial distance of each of the preset line feature pairs to obtain multiple target spatial distances; according to each of the target spatial distances and Determine multiple third values, where w is the third value, d is the target space distance, v0 is the degree of freedom of t distribution, μ D is the mean of the above t distribution, σ D is the variance of the above-mentioned t distribution; among the multiple preset line feature pairs, the preset line feature pairs corresponding to the above-mentioned third numerical value which is less than the third preset threshold value are removed to obtain the multiple target line feature pairs, which further reduces the influence of the dynamic object on the positioning accuracy of the mobile carrier, and further ensures that the positioning accuracy of the mobile carrier is high.
[0044] Specifically, in the actual application process, since static objects are stationary and conform to the properties of rigid body transformation, while dynamic objects do not conform to the properties of rigid body transformation, the target space distance and Determine multiple third values, then center the multiple preset line feature pairs, remove the preset line feature pairs corresponding to the third values that are less than the third preset threshold, and obtain multiple target line feature pairs. This ensures that the dynamic objects corresponding to the preset line feature pairs can be eliminated more easily, and further ensures that the target point feature pairs can be obtained more accurately based on the target line feature pairs in the future, further ensuring that the positioning accuracy of the mobile carrier is high.
[0045] Specifically, in actual application, the method is not limited to the above method of eliminating the preset line feature pairs corresponding to the dynamic object to obtain the target line feature pairs. Any other existing feasible method can also be used to eliminate the preset line feature pairs corresponding to the dynamic object.
[0046] In actual application, due to insufficient indoor light or weak texture information, fewer ORB point features are extracted, which in turn leads to poor positioning accuracy of the mobile carrier. In another embodiment of the present application, multiple target point feature pairs are determined based on the multiple preset point feature pairs and the multiple target line feature pairs, including: determining multiple candidate point feature pairs based on the endpoints of each target line feature pair; and forming multiple target point feature pairs from the multiple preset point feature pairs and the multiple candidate point feature pairs. In other words, in this solution, the endpoints corresponding to the target line feature pairs are determined as candidate point feature pairs, and the preset point feature pairs and the candidate point features form the target point feature pairs, which further ensures that a large number of target point feature pairs are obtained, and the target translation matrix and the target rotation matrix of the image acquisition device can be determined more accurately later, further ensuring that the positioning accuracy of the mobile carrier is high.
[0047] In another embodiment of the present application, the target rotation matrix and the target translation matrix of the image acquisition device at the current moment are determined based on the multiple target point feature pairs, including: using the epipolar geometry algorithm to determine the preset rotation matrix and the preset translation matrix of each target point feature pair; calculating the average value of the multiple preset rotation matrices to obtain the target rotation matrix of the image acquisition device at the current moment, and calculating the average value of the multiple preset translation matrices to obtain the target translation matrix of the image acquisition device at the current moment. In this embodiment, in order to more simply obtain the target rotation matrix and the target translation matrix of the image acquisition device, in this embodiment, the target rotation matrix is obtained by calculating the average value of multiple preset rotation matrices, and the target translation matrix is obtained by calculating the average value of multiple preset average matrices, which further ensures that the subsequent position information of the mobile carrier at the next moment is more accurately predicted based on the target rotation matrix, the target translation matrix and the position information of the mobile carrier, and further ensures that the positioning accuracy of the mobile carrier is high.
[0048] Specifically, Figure 2 As shown, the projection points of the image corresponding to P at time k-1 (the moment before the current moment) and time k (the current moment) are p1(x1, y1) and p2(x2, y2), p1 and p2 are one pair of target point feature pairs, O1 and O2 are the optical centers of the camera, the projection of O1P on the image at time k is e2p2, l2 is the epipolar line, e1 and e2 are the poles, s1 and s2 are the depth values of the pixel coordinates of the corresponding image, K is the camera intrinsic parameter matrix, R is the camera rotation matrix, T is the translation matrix, and the solution steps are as follows
[0049] The world coordinate system of aP is: P = [X, Y, Z] T ;
[0050] b. Taking the image at time k-1 as the reference system, the projection equation is: s1p1=KP, s2p2=K(RP+T);
[0051] c. Use normalized coordinates: x1 = K -1 p1,x2=K -1 p2;
[0052] d. The essential matrix E and the basic matrix F can be obtained by formula conversion: E = TR, F = K -T EK -1 ;
[0053] e. Solve the essential matrix E to find the target rotation matrix and target translation matrix of the camera.
[0054] In order to ensure low cost and more accurate determination of the posture information of the mobile carrier, in a specific embodiment of the present application, the above-mentioned mobile carrier also includes an inertial measurement unit, and the process of determining the above-mentioned posture information of the above-mentioned mobile carrier includes: determining the above-mentioned posture information of the above-mentioned mobile carrier according to the measurement information of the inertial measurement unit at the above-mentioned current moment, the above-mentioned measurement information includes linear acceleration information and angular acceleration information in the navigation coordinate system, and the above-mentioned posture information includes position information, attitude information and speed information.
[0055] In one embodiment of the present application, the above-mentioned posture information of the above-mentioned mobile carrier is determined according to the measurement information of the inertial measurement unit at the above-mentioned current moment, including: determining the target specific force vector in the coordinate system where the above-mentioned mobile carrier is located at least according to the above-mentioned linear acceleration information; determining the target angular velocity of rotation in the coordinate system where the above-mentioned mobile carrier is located at least according to the above-mentioned angular acceleration information; using a strapdown inertial navigation algorithm to perform posture solution on the above-mentioned target specific force vector and the above-mentioned target angular velocity of rotation, and determine the above-mentioned posture information of the above-mentioned mobile carrier, thereby ensuring that the determined posture information of the mobile carrier is relatively accurate, and further ensuring that the subsequent predicted posture information of the mobile carrier at the next moment based on the target rotation matrix, the target translation matrix and the posture information is relatively accurate.
[0056] In a specific embodiment of the present application, Figure 3 As shown, the basic principle of the strapdown inertial navigation algorithm is based on Newton's laws of mechanics. Specifically, first, at least according to the linear acceleration information measured by the accelerometer, the preset specific force vector f is determined. b , and make an error on the preset specific force vector to get the target specific force vector Then, through the direction cosine matrix, the target force vector Convert to navigation coordinates, then perform an integral operation on it to get the speed, and then perform an integral operation on the speed to get the position information (i.e. Figure 3Then, the preset rotation angular velocity is determined based on at least the angular acceleration information measured by the gyroscope. And set the preset angular velocity Perform error compensation to obtain the target rotation angular velocity Then, according to the target rotation angular velocity Update the direction cosine matrix, calculate the attitude information and speed information of the mobile carrier (i.e. attitude calculation). Finally, output the above position information, speed information and attitude information for navigation.
[0057] In order to ensure that the target specific force vector can be determined more accurately, and further ensure that the position information of the mobile carrier is subsequently determined more accurately according to the target specific force vector, in another embodiment of the present application, at least according to the above-mentioned linear acceleration information, the target specific force vector in the coordinate system where the above-mentioned mobile carrier is located is determined, including: according to the above-mentioned linear acceleration information and Determine the preset specific force vector in the coordinate system of the mobile carrier, where f b is the above preset specific force vector, is the direction cosine matrix, a n is the above linear acceleration information, g n is the gravity vector in the above navigation coordinate system, b a is the bias error of the accelerometer, n a The inertial measurement unit includes the accelerometer mentioned above, and error compensation is performed on the preset specific force vector to obtain the target specific force vector.
[0058] In another embodiment of the present application, determining the target rotation angular velocity in the coordinate system where the mobile carrier is located at least according to the angular acceleration information includes: determining the target rotation angular velocity in the coordinate system where the mobile carrier is located according to the angular acceleration information and Determine the preset rotational angular velocity in the coordinate system of the mobile carrier, wherein: is the above preset rotation angular velocity, ω b is the above angular acceleration information, b g is the bias error of the gyroscope, n g The inertial measurement unit includes the gyroscope mentioned above; the preset rotation angular velocity is error compensated to obtain the target rotation angular velocity, thereby ensuring that the target rotation angular velocity can be determined more accurately, and further ensuring that the posture information of the mobile carrier can be determined more accurately according to the target rotation angular velocity.
[0059] In order to more accurately predict and update the posture information at the next moment, in another embodiment of the present application, based on the above-mentioned target rotation matrix, the above-mentioned target translation matrix and the posture information of the above-mentioned mobile carrier, the above-mentioned posture information of the above-mentioned mobile carrier at the next moment is predicted and updated, including: using MSF (Multi-Sensor Fusion, MSF for short) fusion algorithm to fuse the above-mentioned target rotation matrix, the above-mentioned target translation matrix and the above-mentioned posture information, and predicting and updating the above-mentioned posture information of the above-mentioned mobile carrier at the next moment.
[0060] Specifically, MSF fusion is a multi-sensor fusion framework based on the extended Kalman filter algorithm, which is mainly divided into two steps: prediction and update. Among them, due to the high frequency of IMU (i.e., inertial measurement unit), it is not used as the update metric. The IMU data is used as the prediction metric, and the IMU posture information is used as the posture prediction of the robot, and the posture value obtained by the image acquisition device is used as the update value of the mobile carrier. Among them, for the prediction step, the state prediction is obtained by IMU measurement, and the composition of the state quantity is
[0061]
[0062] The state quantity includes the pose information (position information) from the world coordinate system w to the IMU coordinate system i Posture information Speed information ), and the bias error of the IMU’s gyroscope and the accelerometer bias error The perturbation of the state is
[0063] δx=[δp,δv,δθ,δb g ,δb a ] T
[0064] According to the EKF (Extended Kalman Filter, EFK for short) principle, the actual motion model of IMU is as follows:
[0065]
[0066]
[0067]
[0068]
[0069] a n is the linear acceleration information of the accelerometer, ω bis the angular acceleration information of the gyroscope. The IMU posture information can be obtained through the above equation. For the update step, the posture data obtained by the image acquisition device is recorded as the observation value z, is the predicted value, is the error, H is the Jacobian matrix obtained by Taylor expansion of the error state function, P is the covariance matrix, and R is the covariance matrix. The specific process is:
[0070] a. Calculate the residual:
[0071] b. Calculate the update matrix S: S = HPH T +R;
[0072] c. Calculate Kalman gain: K = PH T S -1 ;
[0073] d. Calculate the correction value: Correct the value Substituting it into the IMU motion state equation can update the state variable x, thereby predicting the position and posture information of the mobile carrier at the next moment.
[0074] Specifically, in the actual application process, it is not limited to fusing the target rotation matrix, the target translation matrix and the position and posture information by the MSF fusion algorithm, and any feasible fusion method in other prior arts can be used to fuse the target rotation matrix, the target translation matrix and the position and posture information. This is not limited in the present application.
[0075] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown here.
[0076] The embodiment of the present application also provides an indoor positioning device. It should be noted that the indoor positioning device of the embodiment of the present application can be used to execute the indoor positioning method provided by the embodiment of the present application. The indoor positioning device provided by the embodiment of the present application is introduced below.
[0077] Figure 4 is a schematic diagram of the structure of an indoor positioning device according to an embodiment of the present application. The mobile carrier includes an image acquisition device, such as Figure 4 As shown, the indoor positioning device includes:
[0078] The acquisition unit 10 is used to respectively acquire a plurality of ORB point features and a plurality of FLD line features of each of the plurality of images, wherein the plurality of images include a first image acquired by the image acquisition device at a current moment and a second image acquired at a moment before the current moment;
[0079] A first determining unit 20 is used to determine a plurality of target point feature pairs at least according to the ORB point features and the FLD line features corresponding to the first image and the second image, and determine a target rotation matrix and a target translation matrix of the image acquisition device at the current moment according to the plurality of target point feature pairs;
[0080] The prediction unit 30 is used to predict and update the above-mentioned posture information of the above-mentioned mobile carrier at the next moment according to the above-mentioned target rotation matrix, the above-mentioned target translation matrix and the above-mentioned posture information of the above-mentioned mobile carrier.
[0081] In the above-mentioned indoor positioning device, the acquisition unit is used to respectively acquire multiple ORB point features and multiple FLD line features of each of the above-mentioned images, and the multiple images include a first image acquired by the above-mentioned image acquisition device at the current moment and a second image acquired at a moment before the above-mentioned current moment; the first determination unit is used to determine multiple target point feature pairs based on at least the above-mentioned ORB point features and the above-mentioned FLD line features corresponding to the above-mentioned first image and the above-mentioned second image, and determine the target rotation matrix and the target translation matrix of the above-mentioned image acquisition device at the above-mentioned current moment based on the multiple above-mentioned target point feature pairs; the prediction unit is used to predict and update the above-mentioned posture information of the above-mentioned mobile carrier at the next moment based on the above-mentioned target rotation matrix, the above-mentioned target translation matrix and the posture information of the above-mentioned mobile carrier. Compared with the prior art that determines the target rotation matrix and target translation matrix of the image acquisition device at the current moment only through ORB point features or only through FLD line features, this solution determines multiple target point feature pairs based on the ORB point features and FLD line features of the first image, and the ORB point features and FLD line features of the second image, thereby ensuring a large number of determined target point feature pairs, ensuring that the determined target rotation matrix and target translation matrix are relatively accurate, thereby solving the problem that the image acquisition device is difficult to extract ORB point features and extracts fewer ORB point features in an indoor environment with insufficient light or weak texture. This solution predicts and updates the pose information of the mobile carrier at the next moment based on the target rotation matrix, the target translation matrix and the pose information of the mobile carrier, ensuring that the predicted pose information of the mobile carrier at the next moment is relatively accurate, ensuring that the positioning accuracy of the mobile carrier is high, and thus solving the problem of poor positioning accuracy of the mobile carrier in the prior art.
[0082] Specifically, the above-mentioned mobile carrier can be a mobile robot.
[0083] Specifically, the image acquisition device may be a device having an image capturing function such as a camera or a webcam.
[0084] Specifically, the position and posture information of the mobile carrier can be obtained by an inertial measurement unit, and of course, it can also be obtained by a laser radar or a millimeter wave radar. In a specific embodiment of the present application, the position and posture information of the mobile carrier is obtained by an inertial measurement unit, which can ensure low costs and further ensure high positioning accuracy of the mobile carrier.
[0085] Specifically, before respectively capturing the first image and the second image through the image acquisition device, the above-mentioned indoor positioning method also includes calibrating the image acquisition device through the Zhang Zhengyou chessboard calibration method to obtain the intrinsic parameter matrix and distortion factor of the image acquisition device, and finally inputting a monocular image and using the distortion model to perform distortion correction on the image captured by the image acquisition device.
[0086] In one embodiment of the present application, the first determination unit includes a first determination module, a removal module, and a second determination module, wherein the first determination module is used to determine a plurality of preset feature pairs at least according to each of the ORB point features and each of the FLD line features, wherein the plurality of preset feature pairs include a plurality of preset point feature pairs and a plurality of preset line feature pairs, the preset point feature pair includes two of the ORB point features having a first preset relationship, one of the two ORB point features is in the first image, and the other is in the second image, and the first preset relationship is that the similarity of the two ORB point features is greater than a first preset threshold, and the preset line feature pair includes two of the FLD line features having a second preset relationship, one of the two FLD line features is in the first image, and the other is in the second image, and the second preset relationship is that the similarity of the two FLD line features is greater than a second preset threshold; the removal module is used to remove the dynamic object in the first image according to the plurality of preset line feature pairs, so as to obtain a plurality of target line feature pairs; the second determination module is used to determine a plurality of target point feature pairs according to the plurality of preset point feature pairs and the plurality of target line feature pairs. In actual application, since dynamic objects will affect the positioning accuracy of the mobile carrier, this solution determines multiple preset point feature pairs and multiple preset line feature pairs based on multiple ORB point features and FLD line features in the first image and the second image, and then eliminates the preset line feature pairs corresponding to the dynamic objects in the first image to obtain target line feature pairs, and determines the target point feature pairs based on the multiple preset point feature pairs and the multiple target line feature pairs. This further ensures that a large number of target point feature pairs are extracted, and further solves the problem of poor positioning accuracy of the mobile carrier due to the small number of extracted target point feature pairs in indoor environments with insufficient light or weak texture.
[0087] Specifically, in the above-mentioned embodiment, dynamic objects in the first image are removed according to multiple preset line feature pairs to obtain multiple target line feature pairs, that is, the preset line feature pairs corresponding to the dynamic objects in the first image are eliminated to obtain the target line feature pairs (that is, the line feature pairs corresponding to the stationary objects).
[0088] In order to more simply determine multiple preset feature pairs, in another embodiment of the present application, the above-mentioned first determination module includes a first determination submodule, a second determination submodule and a third determination submodule, wherein the above-mentioned first determination submodule is used to determine the BRIEF (binary descriptor, Binary Robust Independent Elementary Features, referred to as BRIEF) descriptor corresponding to each of the above-mentioned ORB point features, and determine the LBD (Line Band Descriptor, referred to as LBD) descriptor corresponding to each of the above-mentioned FLD line features; the above-mentioned second determination submodule is used to determine the multiple preset point feature pairs according to each of the above-mentioned BRIEF descriptors in the above-mentioned first image and each of the above-mentioned BRIEF descriptors in the above-mentioned second image; the above-mentioned third determination submodule is used to determine the multiple preset line feature pairs according to each of the above-mentioned LBD descriptors in the above-mentioned first image and each of the above-mentioned LBD descriptors in the above-mentioned second image.
[0089] In another embodiment of the present application, the second determination submodule includes a first calculation submodule, a fourth determination submodule and a first elimination submodule, wherein the first calculation submodule is used to calculate the Hamming distance between the i-th BRIEF descriptor in the first image and each BRIEF descriptor in the second image to obtain multiple first numerical values, wherein i starts from 1 and extends to N, where N is the total number of BRIEF descriptors in the first image; the fourth determination submodule is used to determine the two ORB point features corresponding to the first numerical values greater than the first preset threshold as predetermined point feature pairs; the first elimination submodule is used to use RANSAC (Random Sample Consensus, RANSAC for short) to eliminate the mismatched predetermined point feature pairs to obtain multiple predetermined point feature pairs. Specifically, this scheme performs feature matching on the ORB point features in the first image and the ORB point features in the second image according to the Hamming distance, which ensures that the predetermined point feature pairs can be determined relatively simply and efficiently, and then the RANSAC algorithm is used to eliminate the mismatched predetermined point feature pairs to obtain multiple preset point feature pairs, which further ensures that the subsequent target point feature pairs are relatively accurate.
[0090] In order to determine multiple preset line feature pairs more simply and accurately, in another embodiment of the present application, the above-mentioned third determination submodule includes a second calculation submodule, a fifth determination submodule and a second elimination submodule, wherein the above-mentioned second calculation submodule is used to calculate the Hamming distance between the j-th LBD descriptor in the above-mentioned first image and each of the above-mentioned LBD descriptors in the above-mentioned second image, and obtain multiple second numerical values, wherein j starts from 1 and extends to K, and K is the total number of the above-mentioned LBD descriptors in the above-mentioned first image; the above-mentioned fifth determination submodule is used to determine the two above-mentioned FLD line features corresponding to the above-mentioned second numerical values greater than the above-mentioned second preset threshold as candidate line feature pairs; the above-mentioned second elimination submodule adopts the RANSAC algorithm to eliminate the above-mentioned candidate line feature pairs that are mismatched, and obtain multiple above-mentioned preset line feature pairs.
[0091] In order to obtain multiple target line feature pairs more simply, in one embodiment of the present application, the removal module includes a third calculation submodule, a sixth determination submodule and a removal submodule, wherein the third calculation submodule is used to calculate the spatial distance of each of the preset line feature pairs to obtain multiple target spatial distances; the sixth determination submodule is used to calculate the spatial distance of each of the preset line feature pairs to obtain multiple target spatial distances; Determine multiple third values, where w is the third value, d is the target space distance, v0 is the degree of freedom of t distribution, μ D is the mean of the above t distribution, σ D is the variance of the above-mentioned t distribution; the above-mentioned removal submodule is used to remove the above-mentioned preset line feature pairs corresponding to the above-mentioned third numerical value which is less than the third preset threshold value among the multiple above-mentioned preset line feature pairs, and obtain multiple above-mentioned target line feature pairs, thereby further reducing the influence of dynamic objects on the positioning accuracy of the mobile carrier, and further ensuring that the positioning accuracy of the mobile carrier is high.
[0092] Specifically, in the actual application process, since static objects are stationary and conform to the properties of rigid body transformation, while dynamic objects do not conform to the properties of rigid body transformation, the target space distance and Determine multiple third values, then center the multiple preset line feature pairs, remove the preset line feature pairs corresponding to the third values that are less than the third preset threshold, and obtain multiple target line feature pairs. This ensures that the dynamic objects corresponding to the preset line feature pairs can be eliminated more easily, and further ensures that the target point feature pairs can be obtained more accurately based on the target line feature pairs in the future, further ensuring that the positioning accuracy of the mobile carrier is high.
[0093] Specifically, in actual application, the method is not limited to the above method of eliminating the preset line feature pairs corresponding to the dynamic object to obtain the target line feature pairs. Any other existing feasible method can also be used to eliminate the preset line feature pairs corresponding to the dynamic object.
[0094] In the actual application process, due to insufficient indoor light or weak texture information, fewer ORB point features are extracted, which in turn leads to poor positioning accuracy of the mobile carrier. In another embodiment of the present application, the second determination module includes a seventh determination submodule and a combination submodule, wherein the seventh determination submodule is used to determine multiple candidate point feature pairs according to the endpoints of each of the target line feature pairs; the combination submodule is used to form multiple target point feature pairs from multiple preset point feature pairs and multiple candidate point feature pairs. That is to say, in this solution, the endpoints corresponding to the target line feature pairs are determined as candidate point feature pairs, and the preset point feature pairs and the candidate point features constitute the target point feature pairs, which further ensures that the number of target point feature pairs obtained is large, and the target translation matrix and target rotation matrix of the image acquisition device can be determined more accurately later, further ensuring that the positioning accuracy of the mobile carrier is high.
[0095] In another embodiment of the present application, the first determination unit further includes a third determination module and a calculation module, wherein the third determination module is used to determine the preset rotation matrix and the preset translation matrix of each of the target point feature pairs using an epipolar geometry algorithm; the calculation module is used to calculate the average value of a plurality of the preset rotation matrices to obtain the target rotation matrix of the image acquisition device at the current moment, and to calculate the average value of a plurality of the preset translation matrices to obtain the target translation matrix of the image acquisition device at the current moment. In this embodiment, in order to more simply obtain the target rotation matrix and target translation matrix of the image acquisition device, in this embodiment, the target rotation matrix is obtained by calculating the average value of a plurality of preset rotation matrices, and the target translation matrix is obtained by calculating the average value of a plurality of preset average matrices, which further ensures that the subsequent position information of the mobile carrier at the next moment is more accurately predicted based on the target rotation matrix, the target translation matrix and the position information of the mobile carrier, and further ensures that the positioning accuracy of the mobile carrier is high.
[0096] Specifically, Figure 2 As shown, the projection points of the image corresponding to P at time k-1 (the moment before the current moment) and time k (the current moment) are p1(x1, y1) and p2(x2, y2), p1 and p2 are one of the target point feature pairs, O1 and O2 are the optical centers of the camera, the projection of O1P on the image at time k is e2p2, l2 is the epipolar line, e1 and e2 are the poles, s1 and s2 are the depth values of the pixel coordinates of the corresponding image, K is the camera intrinsic parameter matrix, R is the rotation matrix of the camera, T is the translation matrix, and the solution steps are as follows:
[0097] The world coordinate system of aP is: P = [X, Y, Z] T ;
[0098] b. Taking the image at time k-1 as the reference system, the projection equation is: s1p1=KP, s2p2=K(RP+T);
[0099] c. Use normalized coordinates: x1 = K -1 p1,x2=K -1 p2;
[0100] d. The essential matrix E and the basic matrix F can be obtained by formula conversion: E = TR, F = K -T EK -1 ;
[0101] e. Solve the essential matrix E to find the target rotation matrix and target translation matrix of the camera.
[0102] In order to ensure low cost and more accurate determination of the posture information of the mobile carrier, in a specific embodiment of the present application, the above-mentioned mobile carrier also includes an inertial measurement unit, and the above-mentioned indoor positioning device also includes a second determination unit, which is used to determine the above-mentioned posture information of the above-mentioned mobile carrier according to the measurement information of the inertial measurement unit at the above-mentioned current moment, the above-mentioned measurement information includes linear acceleration information and angular acceleration information in the navigation coordinate system, and the above-mentioned posture information includes position information, attitude information and speed information.
[0103] In one embodiment of the present application, the second determination unit includes a fourth determination module, a fifth determination module and a sixth determination module, wherein the fourth determination module is used to determine the target specific force vector in the coordinate system where the mobile carrier is located at least based on the linear acceleration information; the fifth determination module is used to determine the target rotation angular velocity in the coordinate system where the mobile carrier is located at least based on the angular acceleration information; the sixth determination module is used to use a strapdown inertial navigation algorithm to perform posture solution on the target specific force vector and the target rotation angular velocity to determine the posture information of the mobile carrier, thereby ensuring that the determined posture information of the mobile carrier is relatively accurate, and further ensuring that the subsequent predicted posture information of the mobile carrier at the next moment based on the target rotation matrix, the target translation matrix and the posture information is relatively accurate.
[0104] In a specific embodiment of the present application, Figure 3 As shown, the basic principle of the strapdown inertial navigation algorithm is based on Newton's laws of mechanics. Specifically, first, at least according to the linear acceleration information measured by the accelerometer, the preset specific force vector f is determined. b , and make an error on the preset specific force vector to get the target specific force vector Then, through the direction cosine matrix, the target force vector Convert to navigation coordinates, then perform an integral operation on it to get the speed, and then perform an integral operation on the speed to get the position information (i.e. Figure 3 Then, the preset rotation angular velocity is determined based on at least the angular acceleration information measured by the gyroscope. And set the preset angular velocity Perform error compensation to obtain the target rotation angular velocity Then, according to the target rotation angular velocity Update the direction cosine matrix, calculate the attitude information and speed information of the mobile carrier (i.e. attitude calculation). Finally, output the above position information, speed information and attitude information for navigation.
[0105] In order to ensure that the target specific force vector can be determined more accurately, and further ensure that the posture information of the mobile carrier is subsequently determined more accurately according to the target specific force vector, in another embodiment of the present application, the fourth determination module includes an eighth determination submodule and a first error compensation submodule, wherein the eighth determination submodule is used to determine the position of the mobile carrier according to the linear acceleration information and Determine the preset specific force vector in the coordinate system of the mobile carrier, where f b is the above preset specific force vector, is the direction cosine matrix, a n is the above linear acceleration information, g n is the gravity vector in the above navigation coordinate system, b a is the bias error of the accelerometer, n a The above-mentioned inertial measurement unit includes the above-mentioned accelerometer, and the above-mentioned first error compensation submodule performs error compensation on the above-mentioned preset specific force vector to obtain the above-mentioned target specific force vector.
[0106] In another embodiment of the present application, the fifth determination module includes a ninth determination submodule and a second error compensation submodule, wherein the ninth determination submodule is used to determine the error according to the angular acceleration information and Determine the preset rotational angular velocity in the coordinate system of the mobile carrier, wherein: is the above preset rotation angular velocity, ω b is the above angular acceleration information, b g is the bias error of the gyroscope, n g The inertial measurement unit includes the gyroscope; the second error compensation submodule is used to perform error compensation on the preset rotation angular velocity to obtain the target rotation angular velocity, thereby ensuring that the target rotation angular velocity can be determined more accurately, and further ensuring that the posture information of the mobile carrier can be determined more accurately based on the target rotation angular velocity.
[0107] In order to more accurately predict and update the posture information at the next moment, in another embodiment of the present application, the above-mentioned prediction unit includes a prediction module, which is used to adopt MSF (Multi-Sensor Fusion, MSF for short) fusion algorithm to perform information fusion on the above-mentioned target rotation matrix, the above-mentioned target translation matrix and the above-mentioned posture information, and predict and update the above-mentioned posture information of the above-mentioned mobile carrier at the next moment.
[0108] Specifically, MSF fusion is a multi-sensor fusion framework based on the extended Kalman filter algorithm, which is mainly divided into two steps: prediction and update. Among them, due to the high frequency of IMU (i.e., inertial measurement unit), it is not used as the update metric. The IMU data is used as the prediction metric, and the IMU posture information is used as the posture prediction of the robot, and the posture value obtained by the image acquisition device is used as the update value of the mobile carrier. Among them, for the prediction step, the state prediction is obtained by IMU measurement, and the composition of the state quantity is
[0109]
[0110] The state quantity includes the pose information (position information) from the world coordinate system w to the IMU coordinate system i Posture information Speed information ), and the bias error of the IMU’s gyroscope and the accelerometer bias error The disturbance of the state quantity is
[0111] δx=[δp,δv,δθ,δb g ,δb a ] T
[0112] According to the EKF (Extended Kalman Filter, EFK for short) principle, the actual motion model of IMU is as follows:
[0113]
[0114]
[0115]
[0116]
[0117] a n is the linear acceleration information of the accelerometer, ω bis the angular acceleration information of the gyroscope. The IMU posture information can be obtained through the above equation. For the update step, the posture data obtained by the image acquisition device is recorded as the observation value z, is the predicted value, is the error, H is the Jacobian matrix obtained by Taylor expansion of the error state function, P is the covariance matrix, and R is the covariance matrix. The specific process is:
[0118] a. Calculate the residual:
[0119] b. Calculate the update matrix S: S = HPH T +R;
[0120] c. Calculate Kalman gain: K = PH T S -1 ;
[0121] d. Calculate the correction value:
[0122] Correct the value Substituting it into the IMU motion state equation can update the state variable x, thereby predicting the position and posture information of the mobile carrier at the next moment.
[0123] Specifically, in the actual application process, it is not limited to fusing the target rotation matrix, the target translation matrix and the position and posture information by the MSF fusion algorithm, and any feasible fusion method in other prior arts can be used to fuse the target rotation matrix, the target translation matrix and the position and posture information. This is not limited in the present application.
[0124] The indoor positioning device includes a processor and a memory. The acquisition unit, the first determination unit and the prediction unit are stored in the memory as program units. The processor executes the program units stored in the memory to realize corresponding functions.
[0125] The processor includes a kernel, which retrieves the corresponding program unit from the memory. One or more kernels can be provided, and the problem of poor positioning accuracy of the mobile carrier in the prior art can be solved by adjusting kernel parameters.
[0126] The memory may include non-permanent memory in a computer-readable medium, random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM, and the memory includes at least one memory chip.
[0127] An embodiment of the present invention provides a computer-readable storage medium on which a program is stored. When the program is executed by a processor, the above-mentioned indoor positioning method is implemented.
[0128] An embodiment of the present invention provides a processor, and the processor is used to run a program, wherein the indoor positioning method is executed when the program is run.
[0129] In a typical embodiment of the present application, a mobile carrier is further provided. The mobile carrier includes an indoor positioning device. The indoor positioning device is used to execute any one of the indoor positioning methods described above.
[0130] The above-mentioned mobile carrier includes the above-mentioned indoor positioning device, and the above-mentioned indoor positioning device can execute any of the above-mentioned indoor positioning methods. In the above-mentioned indoor positioning method, first, the first image at the current moment and the second image at the previous moment are collected by the above-mentioned image acquisition device, and the ORB (FAST corner detection and BRIEF descriptor, Oriented FAST and Rotated BRIEF, referred to as ORB) point features and FLD (Linear Discriminant Analysis, Fisher Linear Discriminant, referred to as FLD) line features in the above-mentioned first image and the above-mentioned second image are respectively collected; then, according to the above-mentioned ORB point features and the above-mentioned FLD line features of the above-mentioned first image, and the above-mentioned ORB point features and the above-mentioned FLD line features of the above-mentioned second image, multiple target point feature pairs are determined, and according to the multiple target point feature pairs, the target rotation matrix and the target translation matrix of the above-mentioned image acquisition at the current moment are determined; finally, according to the above-mentioned target rotation matrix, the above-mentioned target translation matrix and the posture information of the above-mentioned mobile carrier, the above-mentioned posture information of the above-mentioned mobile carrier at the next moment is predicted and updated. Compared with the prior art that determines the target rotation matrix and target translation matrix of the image acquisition device at the current moment only through ORB point features or only through FLD line features, this solution determines multiple target point feature pairs based on the ORB point features and FLD line features of the first image, and the ORB point features and FLD line features of the second image, thereby ensuring a large number of determined target point feature pairs, ensuring that the determined target rotation matrix and target translation matrix are relatively accurate, thereby solving the problem that the image acquisition device is difficult to extract ORB point features and extracts fewer ORB point features in an indoor environment with insufficient light or weak texture. This solution predicts and updates the pose information of the mobile carrier at the next moment based on the target rotation matrix, the target translation matrix and the pose information of the mobile carrier, ensuring that the predicted pose information of the mobile carrier at the next moment is relatively accurate, ensuring that the positioning accuracy of the mobile carrier is high, and thus solving the problem of poor positioning accuracy of the mobile carrier in the prior art.
[0131] An embodiment of the present invention provides a device, the device including a processor, a memory, and a program stored in the memory and executable on the processor, and when the processor executes the program, at least the following steps are implemented:
[0132] Step S101, respectively collecting a plurality of ORB point features and a plurality of FLD line features of each of the plurality of images, wherein the plurality of images include a first image collected by the image collection device at a current moment and a second image collected at a moment before the current moment;
[0133] Step S102, determining a plurality of target point feature pairs at least according to the ORB point features and the FLD line features corresponding to the first image and the second image, and determining a target rotation matrix and a target translation matrix of the image acquisition device at the current moment according to the plurality of target point feature pairs;
[0134] Step S103, predicting and updating the above-mentioned posture information of the above-mentioned mobile carrier at the next moment according to the above-mentioned target rotation matrix, the above-mentioned target translation matrix and the above-mentioned posture information of the above-mentioned mobile carrier.
[0135] The devices in this article can be servers, PCs, PADs, mobile phones, etc.
[0136] The present application also provides a computer program product, which, when executed on a data processing device, is suitable for executing a program for initializing at least the following method steps:
[0137] Step S101, respectively collecting a plurality of ORB point features and a plurality of FLD line features of each of the plurality of images, wherein the plurality of images include a first image collected by the image collection device at a current moment and a second image collected at a moment before the current moment;
[0138] Step S102, determining a plurality of target point feature pairs at least according to the ORB point features and the FLD line features corresponding to the first image and the second image, and determining a target rotation matrix and a target translation matrix of the image acquisition device at the current moment according to the plurality of target point feature pairs;
[0139] Step S103, predicting and updating the above-mentioned posture information of the above-mentioned mobile carrier at the next moment according to the above-mentioned target rotation matrix, the above-mentioned target translation matrix and the above-mentioned posture information of the above-mentioned mobile carrier.
[0140] In order to enable those skilled in the art to more clearly understand the technical solution of the present application, the technical solution and technical effects of the present application will be explained below in conjunction with specific embodiments.
[0141] Example
[0142] like Figure 5As shown, first, the first image at the current moment and the second image at the previous moment are collected by an image acquisition device, and multiple ORB point features and multiple FLD line features of the first image, as well as multiple ORB point features and multiple FLD line features of the second image are collected. Then, according to the BRIEF descriptors corresponding to the multiple ORB point features of the first image and the BRIEF descriptors corresponding to the multiple ORB point features of the second image, the predetermined point feature pairs are determined, and the RANSAC algorithm is used to eliminate the wrongly matched predetermined point feature pairs to obtain multiple predetermined point feature pairs; according to the LBD descriptors corresponding to the multiple FLD line features of the first image and the LBD descriptors corresponding to the multiple FLD line features of the second image, multiple candidate line feature pairs are determined, and the RANSAC algorithm is used to eliminate the wrongly matched candidate line feature pairs to obtain multiple preset line feature pairs. After that, according to the endpoints of the multiple preset line feature pairs, multiple candidate point feature pairs are determined, and then multiple target point feature pairs are formed by the multiple preset point feature pairs and the multiple candidate point feature pairs. Afterwards, the epipolar geometry algorithm is used to calculate the preset rotation matrix and preset translation matrix of each target point feature pair, and the average value of multiple preset rotation matrices is calculated to obtain the target rotation matrix; the average value of multiple preset translation matrices is calculated to obtain the target translation matrix (that is, the above process is to estimate the posture of the image acquisition device). Afterwards, based on the linear acceleration information of the IMU (that is, inertial measurement unit), the target specific force vector in the coordinate system where the mobile carrier is located is determined, and the angular acceleration information of the IMU (that is, inertial measurement unit) is used to determine the target angular velocity of rotation in the coordinate system where the mobile carrier is located. Afterwards, the strapdown inertial navigation algorithm is used to perform posture solution on the target specific force vector and the target angular velocity of rotation (that is, Figure 5 The strapdown inertial navigation posture solution in the above example is used to obtain the posture information of the mobile carrier. Finally, the MSF fusion algorithm is used to fuse the target rotation matrix, target translation matrix and posture information to predict the posture information of the mobile carrier at the next moment.
[0143] In the above embodiments of the present invention, the description of each embodiment has its own emphasis. For parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.
[0144] In the several embodiments provided in this application, it should be understood that the disclosed technical content can be implemented in other ways. Among them, the device embodiments described above are only schematic. For example, the division of the above-mentioned units can be a logical function division. There may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of units or modules, which can be electrical or other forms.
[0145] The units described above as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple units. Some or all of the units may be selected according to actual needs to achieve the purpose of the present embodiment.
[0146] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.
[0147] If the above-mentioned integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions for a computer device (which can be a personal computer, a server or a network device, etc.) to perform all or part of the steps of the above-mentioned methods of each embodiment of the present invention. The aforementioned storage medium includes: U disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), mobile hard disk, magnetic disk or optical disk and other media that can store program codes.
[0148] From the above description, it can be seen that the above embodiments of the present application achieve the following technical effects:
[0149] 1) In the indoor positioning method of the present application, first, a first image at the current moment and a second image at the previous moment are captured by the image acquisition device, and ORB (FAST corner detection and BRIEF descriptor, Oriented FAST and Rotated BRIEF, referred to as ORB) point features and FLD (Linear Discriminant Analysis, Fisher Linear Discriminant, referred to as FLD) line features in the first image and the second image are respectively captured; then, based on the ORB point features and the FLD line features of the first image, and the ORB point features and the FLD line features of the second image, a plurality of target point feature pairs are determined, and based on the plurality of target point feature pairs, a target rotation matrix and a target translation matrix of the image capture at the current moment are determined; finally, based on the target rotation matrix, the target translation matrix and the pose information of the mobile carrier, the pose information of the mobile carrier at the next moment is predicted and updated. Compared with the prior art that determines the target rotation matrix and target translation matrix of the image acquisition device at the current moment only through ORB point features or only through FLD line features, this solution determines multiple target point feature pairs based on the ORB point features and FLD line features of the first image, and the ORB point features and FLD line features of the second image, thereby ensuring a large number of determined target point feature pairs, ensuring that the determined target rotation matrix and target translation matrix are relatively accurate, thereby solving the problem that the image acquisition device is difficult to extract ORB point features and extracts fewer ORB point features in an indoor environment with insufficient light or weak texture. This solution predicts and updates the pose information of the mobile carrier at the next moment based on the target rotation matrix, the target translation matrix and the pose information of the mobile carrier, ensuring that the predicted pose information of the mobile carrier at the next moment is relatively accurate, ensuring that the positioning accuracy of the mobile carrier is high, and thus solving the problem of poor positioning accuracy of the mobile carrier in the prior art.
[0150] 2) In the indoor positioning device of the present application, the acquisition unit is used to respectively acquire multiple ORB point features and multiple FLD line features of each of the multiple images, and the multiple images include a first image acquired by the image acquisition device at a current moment and a second image acquired at a moment before the current moment; the first determination unit is used to determine multiple target point feature pairs based on at least the ORB point features and the FLD line features corresponding to the first image and the second image, and determine the target rotation matrix and the target translation matrix of the image acquisition device at the current moment based on the multiple target point feature pairs; the prediction unit is used to predict and update the posture information of the mobile carrier at the next moment based on the target rotation matrix, the target translation matrix and the posture information of the mobile carrier. Compared with the prior art that determines the target rotation matrix and target translation matrix of the image acquisition device at the current moment only through ORB point features or only through FLD line features, this solution determines multiple target point feature pairs based on the ORB point features and FLD line features of the first image, and the ORB point features and FLD line features of the second image, thereby ensuring a large number of determined target point feature pairs, ensuring that the determined target rotation matrix and target translation matrix are relatively accurate, thereby solving the problem that the image acquisition device is difficult to extract ORB point features and extracts fewer ORB point features in an indoor environment with insufficient light or weak texture. This solution predicts and updates the pose information of the mobile carrier at the next moment based on the target rotation matrix, the target translation matrix and the pose information of the mobile carrier, ensuring that the predicted pose information of the mobile carrier at the next moment is relatively accurate, ensuring that the positioning accuracy of the mobile carrier is high, and thus solving the problem of poor positioning accuracy of the mobile carrier in the prior art.
[0151] 3) The mobile carrier of the present application includes the above-mentioned indoor positioning device, and the above-mentioned indoor positioning device can execute any of the above-mentioned indoor positioning methods. In the above-mentioned indoor positioning method, first, the first image at the current moment and the second image at the previous moment are collected by the above-mentioned image acquisition device, and the ORB (FAST corner detection and BRIEF descriptor, Oriented FAST and RotatedBRIEF, referred to as ORB) point features and FLD (Linear Discriminant Analysis, Fisher Linear Discriminant, referred to as FLD) line features in the above-mentioned first image and the above-mentioned second image are respectively collected; then, according to the above-mentioned ORB point features and the above-mentioned FLD line features of the above-mentioned first image, and the above-mentioned ORB point features and the above-mentioned FLD line features of the above-mentioned second image, a plurality of the above-mentioned target point feature pairs are determined, and according to the plurality of the above-mentioned target point feature pairs, the target rotation matrix and the target translation matrix of the above-mentioned image acquisition at the current moment are determined; finally, according to the above-mentioned target rotation matrix, the above-mentioned target translation matrix and the posture information of the above-mentioned mobile carrier, the above-mentioned posture information of the above-mentioned mobile carrier at the next moment is predicted and updated. Compared with the prior art that determines the target rotation matrix and target translation matrix of the image acquisition device at the current moment only through ORB point features or only through FLD line features, this solution determines multiple target point feature pairs based on the ORB point features and FLD line features of the first image, and the ORB point features and FLD line features of the second image, thereby ensuring a large number of determined target point feature pairs, ensuring that the determined target rotation matrix and target translation matrix are relatively accurate, thereby solving the problem that the image acquisition device is difficult to extract ORB point features and extracts fewer ORB point features in an indoor environment with insufficient light or weak texture. This solution predicts and updates the pose information of the mobile carrier at the next moment based on the target rotation matrix, the target translation matrix and the pose information of the mobile carrier, ensuring that the predicted pose information of the mobile carrier at the next moment is relatively accurate, ensuring that the positioning accuracy of the mobile carrier is high, and thus solving the problem of poor positioning accuracy of the mobile carrier in the prior art.
[0152] The above description is only the preferred embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.
Claims
1. An indoor positioning method, characterized in that: The mobile carrier includes an image acquisition device, and the indoor positioning method includes: respectively collecting a plurality of ORB point features and a plurality of FLD line features of each of the plurality of images, wherein the plurality of images include a first image collected by the image acquisition device at a current moment and a second image collected at a moment before the current moment; Determine a plurality of target point feature pairs at least according to the ORB point features and the FLD line features corresponding to the first image and the second image, and determine a target rotation matrix and a target translation matrix of the image acquisition device at the current moment according to the plurality of target point feature pairs; Predicting and updating the pose information of the mobile carrier at the next moment according to the target rotation matrix, the target translation matrix and the pose information of the mobile carrier; Determining a plurality of target point feature pairs at least according to the ORB point features and the FLD line features corresponding to the first image and the second image, comprises: Determine a plurality of preset feature pairs at least according to each of the ORB point features and each of the FLD line features, wherein the plurality of preset feature pairs include a plurality of preset point feature pairs and a plurality of preset line feature pairs, the preset point feature pair includes two of the ORB point features having a first preset relationship, one of the two ORB point features is in the first image, and the other is in the second image, the first preset relationship being that the similarity of the two ORB point features is greater than a first preset threshold, and the preset line feature pair includes two of the FLD line features having a second preset relationship, one of the two FLD line features is in the first image, and the other is in the second image, the second preset relationship being that the similarity of the two FLD line features is greater than a second preset threshold; According to the plurality of preset line feature pairs, dynamic objects in the first image are removed to obtain a plurality of target line feature pairs; A plurality of target point feature pairs are determined according to the plurality of preset point feature pairs and the plurality of target line feature pairs.
2. The indoor positioning method according to claim 1, characterized in that: Determining a plurality of preset feature pairs at least according to each of the ORB point features and each of the FLD line features, including: Determine the BRIEF descriptor corresponding to each of the ORB point features, and determine the LBD descriptor corresponding to each of the FLD line features; Determine a plurality of the preset point feature pairs according to each of the BRIEF descriptors in the first image and each of the BRIEF descriptors in the second image; A plurality of preset line feature pairs are determined according to each of the LBD descriptors in the first image and each of the LBD descriptors in the second image.
3. The indoor positioning method according to claim 2, characterized in that: Determining a plurality of the preset point feature pairs according to each of the BRIEF descriptors in the first image and each of the BRIEF descriptors in the second image includes: Calculate the Hamming distance between the i-th BRIEF descriptor in the first image and each BRIEF descriptor in the second image to obtain multiple first values, where i starts from 1 and ranges to N, where N is the total number of the BRIEF descriptors in the first image; Determine the two ORB point features corresponding to the first value greater than the first preset threshold as a predetermined point feature pair; The RANSAC algorithm is used to eliminate the mismatched predetermined point feature pairs to obtain multiple predetermined point feature pairs.
4. The indoor positioning method according to claim 2, characterized in that: Determining a plurality of the preset line feature pairs according to each of the LBD descriptors in the first image and each of the LBD descriptors in the second image comprises: Calculate the Hamming distance between the j-th LBD descriptor in the first image and each LBD descriptor in the second image to obtain multiple second values, where j starts from 1 to K, and K is the total number of LBD descriptors in the first image; Determine the two FLD line features corresponding to the second value greater than the second preset threshold as a candidate line feature pair; The RANSAC algorithm is used to eliminate the incorrectly matched candidate line feature pairs and obtain a plurality of the preset line feature pairs.
5. The indoor positioning method according to claim 1, characterized in that: According to the plurality of preset line feature pairs, dynamic objects in the first image are removed to obtain a plurality of target line feature pairs, including: Calculating the spatial distance of each of the preset line feature pairs to obtain multiple target spatial distances; According to the target space distance and Determine multiple third values, where w is the third value, d is the target space distance, v0 is the degree of freedom of t distribution, μ D is the mean of the t distribution, σ D is the variance of the t distribution; Among the plurality of preset line feature pairs, the preset line feature pairs corresponding to the third value that is less than the third preset threshold are removed to obtain the plurality of target line feature pairs.
6. The indoor positioning method according to claim 1, characterized in that: Determining a plurality of target point feature pairs according to the plurality of preset point feature pairs and the plurality of target line feature pairs includes: Determining a plurality of candidate point feature pairs according to the endpoints of each of the target line feature pairs; A plurality of the preset point feature pairs and a plurality of the candidate point feature pairs constitute a plurality of the target point feature pairs.
7. The indoor positioning method according to claim 1, characterized in that: Determining a target rotation matrix and a target translation matrix of the image acquisition device at the current moment according to the plurality of target point feature pairs includes: Using an epipolar geometry algorithm, determining a preset rotation matrix and a preset translation matrix for each of the target point feature pairs; The average value of the plurality of preset rotation matrices is calculated to obtain the target rotation matrix of the image acquisition device at the current moment, and the average value of the plurality of preset translation matrices is calculated to obtain the target translation matrix of the image acquisition device at the current moment.
8. The indoor positioning method according to claim 1, characterized in that: The mobile carrier also includes an inertial measurement unit, The posture information includes position information, posture information and speed information. The process of determining the position information of the mobile carrier includes: Determine a target specific force vector in a coordinate system where the mobile carrier is located based at least on the linear acceleration information; Determine the target rotational angular velocity in the coordinate system where the mobile carrier is located at least according to the angular acceleration information; A strapdown inertial navigation algorithm is used to perform posture solution on the target specific force vector and the target rotational angular velocity to obtain the position information, the attitude information and the velocity information of the mobile carrier, wherein the linear acceleration information and the angular acceleration information are measurement information of the inertial measurement unit in the navigation coordinate system.
9. The indoor positioning method according to any one of claims 1 to 8, characterized in that: Predicting and updating the pose information of the mobile carrier at the next moment according to the target rotation matrix, the target translation matrix and the pose information of the mobile carrier, including: The MSF fusion algorithm is used to fuse the target rotation matrix, the target translation matrix and the position and posture information, and to predict and update the position and posture information of the mobile carrier at the next moment.
Citation Information
Patent Citations
A visible light and SAR image registration method based on linear features and control points
CN102663725A
Self-adaptive monocular VIO initialization method based on point-line characteristics
CN112862768A
Mobile robot pose estimation algorithm based on multi-sensor data fusion
CN113052908A
Method and device for constructing visual point cloud map
WO2022002150A1
