A positioning method of a heterogeneous robot and related device

Abstract

Embodiments of the present application provide a positioning method of a heterogeneous robot and related equipment, which are used to solve the positioning problem of the heterogeneous robot in a dynamic scene. The method of the embodiments of the present application comprises: receiving current frame image information and next frame image information to perform feature extraction, obtaining a plurality of image feature points and feature point confidence; receiving inertial measurement information to obtain prior estimation information of the heterogeneous robot; obtaining estimated pose data according to the current frame image information, the next frame image information and the prior estimation information, performing weighted local pose optimization on the image feature points and the feature point confidence to obtain to-be-optimized pose data; performing posterior estimation on the feature point confidence to obtain to-be-detected weight values; performing large-noise feature point detection on the to-be-detected weight values to obtain target image feature points and target weight values corresponding to the target image feature points; and uploading the to-be-optimized pose data, all the target image feature points and the target weight values to a cloud server.

Description

Technical Field

[0001] The embodiments of the present application relate to the technical field of intelligent devices, and in particular to a positioning method and related devices for heterogeneous robots. Background Art

[0002] Localization is a critical component of robot navigation. Traditional localization algorithms assume a static environment. This means the robot estimates its motion state using fixed objects such as road signs, fences, and buildings as a reference frame. However, in real-world scenarios, robots are often surrounded by numerous dynamic objects, especially in warehouses with multiple heterogeneous robots. These include forklifts handling goods, lift trucks placing shelves, mobile robotic arms picking items, and goods on conveyor belts. If the robot mistakenly uses these dynamic objects as a reference frame, its estimated motion state will be significantly inaccurate, increasing the operational risks of smart warehouses. Summary of the Invention

[0003] The embodiments of the present application provide a positioning method and related equipment for a heterogeneous robot, which are used to solve the positioning problem of heterogeneous robots in dynamic scenarios.

[0004] A first aspect of an embodiment of the present application provides a positioning method for a heterogeneous robot, which is applied to the heterogeneous robot. The heterogeneous robot includes an image acquisition device and an inertial sensor. The method includes:

[0005] receiving current frame image information and next frame image information captured by the image capture device, performing feature extraction on the current frame image information to obtain a plurality of image feature points that match the current frame and the next frame, and feature point credibility corresponding to the image feature points; wherein the current frame image information is used to describe observation constraint information of the current frame of the heterogeneous robot, and the next frame image information is used to describe observation constraint information of the next frame of the heterogeneous robot;

[0006] Receiving inertial measurement information accumulated between a current frame and a next frame sent by the inertial sensor, so as to obtain a priori estimation information of the heterogeneous robot based on the inertial measurement information; wherein the inertial measurement information is used to describe motion constraint information of the heterogeneous robot;

[0007] Predicting the heterogeneous robot based on the current frame image information of the current frame, the next frame image information, and the prior estimation information to obtain estimated pose data of the heterogeneous robot in the next frame, and using the observation constraint information of the current frame, the observation constraint information of the next frame, and the motion constraint information as constraints on the estimated pose data, performing weighted local pose optimization on the image feature points and the credibility of the feature points to obtain pose data to be optimized;

[0008] Performing a posteriori estimation on the feature point credibility of each of the image feature points in the pose data to be optimized, and assigning a weight value to be detected; wherein the weight value to be detected is used to represent the degree of fit of the image feature point to the posteriori estimation;

[0009] Performing large noise feature point detection on the weight values ​​to be detected of all the image feature points to eliminate image feature points that do not meet a preset observation threshold, thereby obtaining target image feature points and target weight values ​​corresponding to the target image feature points;

[0010] The pose data to be optimized, all target image feature points and the target weight value are uploaded to the cloud server, so that the cloud service updates the target weight value, and based on the updated target weight value, all target image feature points are globally spliced ​​according to the pose data to be optimized of the heterogeneous robot to obtain the target pose data and global environment information of the heterogeneous robot in the next frame; wherein, the target pose data is used to describe the pose data with the smallest error.

[0011] Optionally, before receiving the current frame image information and the next frame image information captured by the image capture device, the method further includes:

[0012] Receiving initial position information corresponding to each heterogeneous robot configured by the cloud server;

[0013] In the positioning task of the heterogeneous robot, the start and stop states of the corresponding heterogeneous robot are controlled according to the initial position information, and the step of receiving the current frame image information and the next frame image information captured by the image acquisition device is performed.

[0014] Optionally, obtaining the a priori estimation information of the heterogeneous robot according to the inertial measurement information includes:

[0015] Acquire acceleration error information and rotation error information of the heterogeneous robot between the current frame and the next frame according to the current frame image information, the next frame image information, and the inertial measurement information;

[0016] performing a first integral calculation on the acceleration error information and the rotation information between the current frame and the next frame according to the acceleration information to obtain a velocity change of the heterogeneous robot between the current frame and the next frame;

[0017] performing a second integral calculation on the acceleration error information and the rotation information between the current frame and the next frame according to the acceleration information to obtain a position change of the heterogeneous robot between the current frame and the next frame;

[0018] Determining angular velocity information about different axes in a global coordinate system based on the rotation information, and performing a third integral calculation on the rotation information and the rotation error information between the current frame and the next frame based on the angular velocity information about the different axes to obtain a rotation change of the heterogeneous robot between the current frame and the next frame;

[0019] The speed change, the position change and the rotation change are used as posture change, and the posture change and the estimated posture data of the previous frame are calculated to obtain the prior estimation information.

[0020] Optionally, obtaining the feature point credibility corresponding to the image feature point includes:

[0021] Determining the actual number of extracted feature points of all the images, and defining a maximum number of extracted feature points corresponding to all the image features within a time window between the current frame and the next frame;

[0022] Projecting the position information corresponding to all the image feature points onto the prior motion model of the inertial sensor to obtain predicted position information, so as to determine position error information between the predicted position information and the current frame position information;

[0023] The reliability of the feature points is determined according to the position error information, the maximum extraction number and the actual extraction number.

[0024] Optionally, performing a posteriori estimation on the feature point credibility of each of the image feature points in the pose data to be optimized and allocating a weight value to be detected includes:

[0025] Determine a visual pose deviation value between any image feature point and the pose data to be optimized; determine an inertial pose deviation value between the inertial measurement data and the pose data to be optimized at any time point between the current frame and the next frame;

[0026] Determining a measurement quantity value corresponding to the inertial measurement information within a time window between the current frame and the next frame;

[0027] According to the visual pose deviation value, the inertial pose deviation value, the feature point credibility, the maximum extraction quantity and the measurement quantity value, a posteriori estimation is performed on all the image feature points to determine the weight value to be detected.

[0028] Optionally, performing large noise feature point detection on the weight values ​​to be detected of all the image feature points to eliminate image feature points that do not meet a preset observation threshold includes:

[0029] constructing a transformation matrix between the current frame and the next frame based on the inertial measurement information, estimating a next world coordinate system of the image acquisition device in the next frame based on the transformation matrix and the current world coordinate system of the image acquisition device in the current frame, and determining a coordinate transformation value between the next world coordinate system and the current world coordinate system;

[0030] If any image feature point is a stationary feature point, determining the next stationary point position information of the stationary feature point in the next world coordinate system according to the projection model of the image acquisition device and the coordinate transformation value;

[0031] Determine the current stationary point position information of the stationary feature point in the current world coordinate system, and judge whether the interval distance between the current stationary point position information and the next stationary point position information is greater than the preset observation threshold, so as to determine all stationary feature points that do not meet the preset observation threshold as dynamic reference points, and eliminate all dynamic reference points.

[0032] Optionally, uploading the pose data to be optimized, all target image feature points and the target weight value to a cloud server includes:

[0033] Constructing a local weight map according to all target image feature points and the corresponding target weight values;

[0034] The pose data to be optimized is fused with the local weight map, and the fused local weight map is uploaded to the cloud server, so that the cloud server constructs the connection relationship between all the target image feature points according to the proximity algorithm, and updates the target weight value based on the weight value of the image feature point within the adjacent range of the target image feature point.

[0035] A second aspect of an embodiment of the present application provides a positioning system for a heterogeneous robot, including:

[0036] a receiving unit, configured to receive current frame image information and next frame image information captured by the image capture device, and perform feature extraction on the current frame image information to obtain a plurality of image feature points that match the current frame and the next frame, as well as feature point credibility corresponding to the image feature points; wherein the current frame image information is used to describe observation constraint information of the current frame of the heterogeneous robot, and the next frame image information is used to describe observation constraint information of the next frame of the heterogeneous robot;

[0037] The receiving unit is further configured to receive inertial measurement information accumulated between a current frame and a next frame sent by the inertial sensor, so as to obtain a priori estimation information of the heterogeneous robot based on the inertial measurement information; wherein the inertial measurement information is used to describe motion constraint information of the heterogeneous robot;

[0038] an acquisition unit, configured to predict the heterogeneous robot based on the current frame image information of the current frame, the next frame image information, and the prior estimation information, to obtain estimated pose data of the heterogeneous robot in the next frame, and to perform weighted local pose optimization on the image feature points and the credibility of the feature points using the observation constraint information of the current frame, the observation constraint information of the next frame, and the motion constraint information as constraints on the estimated pose data, to obtain pose data to be optimized;

[0039] an allocating unit, configured to perform a posteriori estimation on the feature point credibility of each of the image feature points in the pose data to be optimized, and allocate a weight value to be detected; wherein the weight value to be detected is used to represent the degree of fit of the image feature point to the posteriori estimation;

[0040] a detection unit, configured to perform large noise feature point detection on the weight values ​​to be detected of all the image feature points, so as to eliminate image feature points that do not meet a preset observation threshold, and obtain target image feature points and target weight values ​​corresponding to the target image feature points;

[0041] An uploading unit is used to upload the pose data to be optimized, all target image feature points and the target weight value to a cloud server, so that the cloud service updates the target weight value, and based on the updated target weight value, globally splices all target image feature points according to the pose data to be optimized of the heterogeneous robot, to obtain the target pose data and global environment information of the heterogeneous robot in the next frame; wherein, the target pose data is used to describe the pose data with the smallest error.

[0042] The heterogeneous robot positioning system provided in the second aspect of the embodiments of the present application is used to execute the heterogeneous robot positioning method described in the first aspect.

[0043] A third aspect of the embodiments of the present application provides a positioning device for a heterogeneous robot, comprising:

[0044] CPU, memory, input and output interfaces, wired or wireless network interfaces, and power supply;

[0045] The memory is a transient storage memory or a persistent storage memory;

[0046] The central processing unit is configured to communicate with the memory and execute instruction operations in the memory to perform the positioning method of the heterogeneous robot described in the first aspect.

[0047] A fourth aspect of an embodiment of the present application provides a computer-readable storage medium, which includes instructions. When the instructions are executed on a computer, the computer executes the positioning method for heterogeneous robots described in the first aspect.

[0048] A fifth aspect of an embodiment of the present application provides a computer program product, which includes instructions. When the instructions are run on a computer, the computer executes the positioning method for heterogeneous robots described in the first aspect.

[0049] It can be seen from the above technical solutions that the embodiments of the present application have the following advantages: through the positioning method of a heterogeneous robot disclosed in the embodiments of the present application, the positioning accuracy of the entire robot system is improved by fusing inertial sensors. At the same time, during the working process, the heterogeneous robot evaluates the credibility of the reference system within its perception range, and uploads it to the cloud server for further sharing and splicing to complete the positioning of the heterogeneous robot. At the same time, it also predicts the position of each feature point in the image under the current posture of the image acquisition device, so that when the error between the expected position of the feature point on the image and the actual observed position is greater than the threshold, the feature point is regarded as a large noise feature point. This effectively reduces the impact of unreliable reference points. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments recorded in the present application. For ordinary technicians in this field, other drawings can also be obtained based on these drawings.

[0051] Figure 1 A schematic flow chart of a method for positioning a heterogeneous robot disclosed in an embodiment of the present application;

[0052] Figure 2 A schematic flow chart of another method for positioning heterogeneous robots disclosed in an embodiment of the present application;

[0053] Figure 3 A schematic flow chart of another method for positioning heterogeneous robots disclosed in an embodiment of the present application;

[0054] Figure 4 A schematic flow chart of another method for positioning heterogeneous robots disclosed in an embodiment of the present application;

[0055] Figure 5This is an overview diagram of a heterogeneous multi-robot inertial visual positioning system based on a cloud server disclosed in an embodiment of the present application;

[0056] Figure 6 A schematic diagram of an IMU and camera observation frequency disclosed in an embodiment of the present application;

[0057] Figure 7 This is a schematic diagram of large noise point detection disclosed in an embodiment of the present application;

[0058] Figure 8 A schematic structural diagram of a positioning system for a heterogeneous robot disclosed in an embodiment of the present application;

[0059] Figure 9 This is a schematic structural diagram of a positioning device for a heterogeneous robot disclosed in an embodiment of the present application. DETAILED DESCRIPTION

[0060] The terms "first," "second," "third," "fourth," and the like (if any) in the specification and claims of this application and in the accompanying drawings are used to distinguish similar objects and are not necessarily used to describe a particular order or sequential sequence. It should be understood that the terms used in this manner are interchangeable where appropriate so that the embodiments described herein can be implemented in an order other than that illustrated or described herein. In addition, the terms "including" and "having," and any variations thereof, are intended to cover non-exclusive inclusions, e.g., a process, method, system, product, or apparatus comprising a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0061] It should be noted that the descriptions of "first", "second", etc. in this application are for descriptive purposes only and should not be understood as indicating or implying their relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" or "second" may explicitly or implicitly include at least one of such features. In addition, the technical solutions between the various embodiments can be combined with each other, but this must be based on the fact that they can be implemented by ordinary technicians in this field. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such a combination of technical solutions does not exist and is not within the scope of protection required by this application.

[0062] The following will be combined with the drawings in the embodiments of this application to clearly and completely describe the technical solutions in the embodiments of this application. Obviously, the embodiments described are only part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0063] To resolve the technical issues described above, please refer to Figure 1 , Figure 1 This is a flow chart of a method for positioning a heterogeneous robot disclosed in an embodiment of the present application, including steps 101 to 105.

[0064] 101. Receive current frame image information and next frame image information captured by an image capture device, perform feature extraction on the current frame image information, obtain multiple image feature points that match the current frame and the next frame, and feature point credibility corresponding to the image feature points.

[0065] It should be noted that the present embodiment mainly describes a simultaneous localization and mapping (SLAM) system for heterogeneous multi-robots based on cloud server computing, which is dedicated to solving the positioning problem of multiple robots in dynamic scenes. Specifically, the SLAM system mainly includes heterogeneous robots and cloud servers. Among them, the heterogeneous robots at least include an image acquisition device (such as a camera or a camera, etc.) and an inertial sensor (IMU, inertial measurement unit). Furthermore, the cloud server can be an independent physical server, or a server cluster or distributed system composed of multiple physical servers, or a cloud server that provides basic cloud computing services such as cloud databases, cloud services, cloud computing, cloud functions, cloud storage, network services, cloud communications, middleware services, domain name services, security services, CDN, and big data and artificial intelligence platforms. There is no specific limitation here.

[0066] In this embodiment, in the SLAM system, each heterogeneous robot is equipped with a camera (image acquisition device, which will be described as a camera or a camera later). At the same time, since the execution steps of each heterogeneous robot are similar, one of the heterogeneous robots is described in detail here. Specifically, the heterogeneous robot can receive the current frame image information and the next frame image information acquired by the image acquisition device, and then perform feature extraction on the heterogeneous robot based on the current frame image information and the next frame image information, thereby obtaining multiple image feature points of the heterogeneous robot in the current frame and the next frame, as well as the feature point credibility corresponding to the image feature points. It should be noted that the current frame image information is used to describe the observation constraint information of the current frame of the heterogeneous robot, and the next frame image information is used to describe the observation constraint information of the next frame of the heterogeneous robot.

[0067] In one specific embodiment, in a positioning task, a heterogeneous robot collects image information of different frames in real time, including at least the current frame image information and the next frame image information. Furthermore, the current frame image information and the next frame image information can also reflect the position information of the camera in the entire coordinate system at this time. First, feature extraction and matching are performed on the current frame image information to find a series of image feature points that match the previous and next frames, as well as their corresponding credibility. Furthermore, after feature extraction, the error between the predicted value and the actual observed value of the image feature point in the coordinate system of the camera projection can be obtained. Therefore, after calculation based on the error, the credibility assigned to the feature point can be determined. It should be noted that in this embodiment, credibility can be understood as a weight.

[0068] 102. Receive accumulated inertial measurement information between a current frame and a next frame sent by an inertial sensor to obtain prior estimation information of the heterogeneous robot based on the inertial measurement information.

[0069] As shown in step 101, in the SLAM system, each heterogeneous robot is also equipped with an inertial sensor (IMU). Specifically, the heterogeneous robot can receive inertial measurement information accumulated between the current frame and the next frame sent by the inertial sensor. This inertial measurement information can then be used to obtain a priori estimation information for the heterogeneous robot. It should be noted that the inertial measurement information is used to describe the motion constraints of the heterogeneous robot.

[0070] In one specific embodiment, the heterogeneous robot also collects IMU observation information, that is, inertial measurement information. Among them, the inertial measurement information will feedback the speed information and rotation information of the heterogeneous robot, including at least real-time speed information, rotation information or speed error information, rotation error information, etc. Then, the inertial measurement information of the IMU mentioned above can be preprocessed. Since the data information measured by the IMU mainly includes acceleration and angular velocity (rotation related), the algorithm needs to be integrated to obtain translation and rotation information. The IMU is then frame-aligned with the image information, and the accumulated IMU information collected between the current frame and the next frame is pre-integrated to record the motion information of the heterogeneous robot. In combination with this motion information, a priori estimation of the heterogeneous robot can be obtained.

[0071] 103. Based on the current frame image information, the next frame image information and the prior estimation information of the current frame, the heterogeneous robot is predicted to obtain the estimated pose data of the heterogeneous robot in the next frame, and the observation constraint information of the current frame, the observation constraint information of the next frame and the motion constraint information are used as constraints for the estimated pose data. The image feature points and the feature point credibility are weightedly optimized to obtain the pose data to be optimized.

[0072] Therefore, the heterogeneous robot can be predicted based on the current frame image information, the next frame image information and the prior estimation information of the current frame, so as to obtain the estimated pose data of the heterogeneous robot in the next frame. The observation constraint information of the current frame, the observation constraint information of the next frame and the motion constraint information are used as constraints for the estimated pose data, and the image feature points and the feature point credibility are weightedly optimized to obtain the pose data to be optimized.

[0073] In one specific embodiment, based on steps 101 and 102, the current frame image information, the next frame image information, and the prior estimation information can be used to infer the operating logic or motion trajectory of the heterogeneous robot, thereby estimating the estimated pose data of the heterogeneous robot in the next frame. Furthermore, the observation constraint information of the current frame, the observation constraint information of the next frame, and the motion constraint information are used as constraints for the estimated pose data. Weighted local pose optimization is performed on the image feature points and their credibility, thereby further estimating the pose data to be optimized for the heterogeneous robot in the next frame.

[0074] 104. Perform a posteriori estimation on the feature point credibility of each image feature point in the pose data to be optimized, and assign a weight value to be detected.

[0075] Then, the credibility of the feature points of each image feature point in the pose data to be optimized is estimated a posteriori, and a weight value to be detected is assigned. It should be noted that the weight value to be detected is used to characterize the degree of fit of the image feature point to the posterior estimate (which can also be understood as the posterior pose estimate). It should also be noted that both the prior estimate and the posterior estimate are for the pose of the robot, which is the most important state maintained by the entire system. In the optimization problem, the pose of the Yigou robot with the minimum error can be optimized according to the observation constraints and inertia constraints, and the weight of the feature point can be updated according to the pose. Furthermore, the degree of fit is calculated by the deviation between the position of the feature point predicted by the posterior estimate in the image and the actual observation.

[0076] In one specific embodiment, during the optimization process, various parameters of the pose data to be optimized are adjusted, and different feature point credibility levels are screened to find the feature point credibility with the smallest error between the image information and the inertial measurement information. Consequently, the optimal pose data is estimated. Thus, the image feature points corresponding to the pose data to be optimized are determined, and the weight values ​​to be detected for these image feature points are then found. Furthermore, the weight values ​​to be detected for all image feature points can be found in the same manner.

[0077] In other feasible technical solutions, the weights of these feature points can be re-evaluated by detecting large noise feature points. Specifically, the weights to be detected are updated based on the deviation between their predicted and actual projections. For details, please refer to the description of large noise feature point detection in step 105, which will not be repeated here.

[0078] 105. Perform large noise feature point detection on the weight values ​​to be detected of all image feature points to eliminate image feature points that do not meet a preset observation threshold, and obtain target image feature points and target weight values ​​corresponding to the target image feature points.

[0079] Furthermore, large noise feature point detection can be performed on the weight values ​​of all image feature points to be detected, thereby eliminating image feature points that do not meet the preset observation threshold, and obtaining target image feature points and target weight values ​​corresponding to the target image feature points.

[0080] In one of the specific embodiments, during the movement of the feature point, there will be a large deviation between the actual projection and the predicted projection on the world coordinate system of the motion model. Therefore, it is necessary to perform large noise feature point detection on the weight values ​​to be detected of all image feature points. That is to say, when the error between the expected position of the feature point on the image and the actual observed position is greater than the threshold, the feature point is regarded as a large noise feature point. Specifically, by projecting the weight values ​​to be detected of the image feature points of different frames onto the motion model, the weight value of the predicted position point is determined, and compared with the weight value of the actually observed position point to determine whether the difference therein meets the preset observation threshold, and then determine the image feature point that does not meet the preset observation threshold. After eliminating the image feature points that do not meet the preset observation threshold, the target image feature point and the target weight value of the target image feature point can be obtained. Generally, the image feature points corresponding to the actually observed position points are retained.

[0081] 106. Upload the pose data to be optimized, all target image feature points and target weight values ​​to the cloud server, so that the cloud service updates the target weight values, and based on the updated target weight values, globally splices all target image feature points according to the pose data to be optimized of the heterogeneous robot, to obtain the target pose data and global environment information of the heterogeneous robot in the next frame.

[0082] Furthermore, in conjunction with step 105, the pose data to be optimized, all target image feature points, and target weight values ​​can be uploaded to the cloud server, allowing the cloud service to update the target weight values ​​and, based on the updated target weight values, globally stitch all target image feature points according to the pose data to be optimized for the heterogeneous robot, thereby obtaining the target pose data and global environment information for the heterogeneous robot in the next frame. It should be noted that the target pose data is used to describe the pose data with the smallest error.

[0083] In one specific embodiment, each heterogeneous robot will upload the local weight map (consisting of target image feature points and target weight values) obtained through local optimization and the pose data to be optimized to the cloud server. All feature points of the local map are globally spliced ​​according to the position of the robot, and the average value of the weights of the adjacent feature points is updated. Thus, based on the updated target weight value, the target image feature points are spliced ​​according to the pose data of the heterogeneous robot, thereby determining the target pose data and global environment information of the heterogeneous robot in the next frame. Furthermore, all feature points of the global map and the constraints of the IMU are also considered to globally optimize the pose and feature point positions of each robot. The positions that the robot has reached are identified through the traditional loop detection algorithm.

[0084] Through the positioning method of a heterogeneous robot disclosed in this embodiment, the positioning accuracy of the entire robot system is improved by fusing inertial sensors. At the same time, during the operation, the heterogeneous robot evaluates the credibility of the reference system within its perception range and uploads it to the cloud server for further sharing and splicing to complete the positioning of the heterogeneous robot. At the same time, it also predicts the position of each feature point in the image under the current posture of the image acquisition device. Therefore, when the error between the expected position of the feature point on the image and the actual observed position is greater than the threshold, the feature point is regarded as a large noise feature point. This effectively reduces the impact of unreliable reference points.

[0085] To explain in detail the embodiments of this application Figure 1 As shown in step 101, please refer to Figure 2 , Figure 2 This is a flow chart of another heterogeneous robot positioning method disclosed in an embodiment of the present application, including steps 201 to 205.

[0086] 201. Receive initial position information corresponding to each heterogeneous robot configured by the cloud server, and control the start and stop states of the corresponding heterogeneous robot according to the initial position information in the positioning task of the heterogeneous robot.

[0087] Depend on Figure 1 As can be seen from step 101, before receiving the image information, each heterogeneous robot needs to be started. Specifically, the heterogeneous robot can receive the initial position information corresponding to each heterogeneous robot configured by the cloud server and control the start and stop status of the corresponding heterogeneous robot according to the initial position information during the heterogeneous robot positioning task.

[0088] In one specific embodiment, each heterogeneous robot is configured according to the global initial position of the cloud server. Start. Denotes the position of the i-th heterogeneous robot at the j-th moment. Specifically, this global initial position facilitates subsequent heterogeneous robots to determine their initial positions in the world coordinate system of the entire motion model.

[0089] Then, the heterogeneous robot performs the positioning task, that is, the start and stop states of the related heterogeneous robots are controlled according to the initial position information. Furthermore, the start state of the heterogeneous robot can also be controlled without the initial position information, that is, it is only used for positioning recognition processing.

[0090] 202. Receive current frame image information and next frame image information captured by an image capture device, and inertial measurement information accumulated between the current frame and the next frame sent by an inertial sensor.

[0091] In this embodiment, step 202 is the same as the aforementioned Figure 1 The step 101 is similar and will not be described in detail here. However, it should be noted that the heterogeneous robot collects the t k Frame camera data and t k Frame (current frame) to t k+1 A series of IMU data (inertial measurement information) accumulated between frames (next frame) It should be noted that z represents the observation quantity of the heterogeneous robot, and its superscript c indicates that it is a camera observation. Similarly, the superscript IMU indicates that it is an IMU observation. is an image captured by a camera ( is the current frame image information, is the next frame image information). where a t is the acceleration in the IMU coordinate system, ω t Indicates the angular velocity between the two frames of IMU.

[0092] 203. Obtain acceleration error information and rotation error information of the heterogeneous robot between the current frame and the next frame based on the current frame image information, the next frame image information and the inertial measurement information.

[0093] Therefore, by analyzing the current frame image information, the next frame image information and the inertial measurement information, the acceleration error information and the rotation error information of the heterogeneous robot between the current frame and the next frame can be obtained.

[0094] In one specific embodiment, in the subsequent positioning task, the heterogeneous robot collects camera observation information and IMU observation information Maintain the state x of each key frame k ={p k ,v k ,qk ,b ak ,b ωk}. Among them, p k ,v k ,q k are the position, velocity and rotation of the robot at time k respectively. ak ,b ωk are IMU acceleration error and rotation error respectively.

[0095] 204. Based on the acceleration information, a first integral calculation is performed on the acceleration error information and the rotation information between the current frame and the next frame to obtain the velocity change of the heterogeneous robot between the current frame and the next frame; based on the acceleration information, a second integral calculation is performed on the acceleration error information and the rotation information between the current frame and the next frame to obtain the position change of the heterogeneous robot between the current frame and the next frame; based on the rotation information, the angular velocity information of different axes in the global coordinate system is determined, and based on the angular velocity information of different axes, a third integral calculation is performed on the rotation information and the rotation error information between the current frame and the next frame to obtain the rotation change of the heterogeneous robot between the current frame and the next frame.

[0096] Therefore, based on the above steps 201-203, it is necessary to perform a first integral calculation on the acceleration error information and rotation information between the current frame and the next frame according to the acceleration information, and obtain the speed change of the heterogeneous robot between the current frame and the next frame; perform a second integral calculation on the acceleration error information and rotation information between the current frame and the next frame according to the acceleration information, and obtain the position change of the heterogeneous robot between the current frame and the next frame; determine the angular velocity information of different axes in the global coordinate system according to the rotation information, and perform a third integral calculation on the rotation information and rotation error information between the current frame and the next frame according to the angular velocity information of different axes, and obtain the rotation change of the heterogeneous robot between the current frame and the next frame. Specifically, it is necessary to preprocess the data collected by the inertial sensor IMU. For ease of understanding and description, please refer to Figure 6 , Figure 6 This is a schematic diagram of an IMU and camera observation frequency disclosed in an embodiment of this application. Figure 6 As shown in the figure, the observation frequencies of IMU and camera are often inconsistent. Usually, the observation data collected by IMU is more "dense" than that of camera. Therefore, frame alignment is required before fusing the two observation data. Specifically, in a new frame t k Before the camera data is observed, this method saves the IMU data by integration. k is the kth frame of camera data, and t refers to t k to t k+1The data measured by IMU mainly includes acceleration and angular velocity. The algorithm needs to integrate to obtain translation and rotation information. In order to align the timestamps of camera data, the algorithm needs to perform the t-th k Frame to t k+1 The IMU data is accumulated and pre-integrated between frames.

[0097] in,

[0098]

[0099] The above calculation formulas correspond to the calculation formulas for the first integral calculation, the second integral calculation, and the third integral calculation. t k to t k+1 The change in velocity, position, and rotation in the global coordinate system at that moment. is the acceleration error of IMU, is the rotation error, is the IMU’s t k to the rotation of t, To put it simply, R is the rotation matrix and Ω is a matrix of angular velocities used to simplify rotation calculations.

[0100] 205. The velocity change, position change, and rotation change are used as the posture change, and the posture change and the estimated posture data of the previous frame are calculated to obtain prior estimation information.

[0101] Then, the velocity change, position change, and rotation change can be used as pose change, and the pose change and the estimated pose data of the previous frame can be used as input to predict the heterogeneous robot and obtain prior estimation information.

[0102] In one specific embodiment, the speed change, position change and rotation change can be used as posture change. By combining the estimated posture data of the heterogeneous robot in the previous frame, the speculation of the heterogeneous robot in the next frame can be completed, thereby obtaining the prior estimation information of the heterogeneous robot at this time.

[0103] Furthermore, in other feasible technical solutions, after the timestamps of the camera data and the observation data are aligned, the pose change obtained by pre-integration can be used as With t k+1 Frame of camera data As the input of the inertial visual odometry, t is estimated through weighted local optimization. k+1 The pose of the heterogeneous robot at the moment is the estimated pose data (i.e., the content of the estimated pose data obtained in step 103).

[0104] A positioning method for a heterogeneous robot disclosed in this embodiment pre-integrates the measurement data collected by the IMU and the image data collected by the camera, thereby estimating the position and posture of the heterogeneous robot in the next frame, providing prior help for subsequently solving the positioning problem of multiple robots in dynamic scenes and improving the feasibility of the solution.

[0105] To explain in detail the embodiments of this application Figure 1 For steps 102-103 shown in Figure 3 , Figure 3 This is a flow chart of another heterogeneous robot positioning method disclosed in an embodiment of the present application, including steps 301 to 306.

[0106] 301. Perform feature extraction on the current frame image information, determine the actual number of extracted feature points of all images, and define the maximum number of extracted feature points corresponding to all images in a time window between the current frame and the next frame.

[0107] In this embodiment, weighted local pose optimization is required. Specifically, it is necessary to first extract features from the current frame image information, determine the actual number of extracted feature points of all images, and define the maximum number of extracted feature points corresponding to all images in the time window between the current frame and the next frame.

[0108] In one specific embodiment, first, k Frame image information Perform feature extraction and matching to find a series of image feature points {p1, p2, ..., p n It should be noted that the image feature point p n As mentioned above Similarly, both can represent the positioning position, or reference points, of the heterogeneous robots at that moment. Specifically, the matched feature points are actually the reference points between the two frames. Classic visual SLAM uses these reference points to estimate the robot's motion. For example, if a person sees a tree moving backward, they can use this to infer that they are moving forward. Just as people use trees as reference points for positioning, robots do the same, using feature matching algorithms to find feature points to assist in positioning.

[0109] For newly constructed image feature points, it is necessary to determine the maximum number N of image feature points that can be extracted within the time window during the matching process and the actual number n of feature points. In short, N represents the maximum number of feature points within the time window; n represents the actual number of feature points.

[0110] 302. Project the position information corresponding to all image feature points onto the prior motion model of the inertial sensor to obtain predicted position information, so as to determine position error information between the predicted position information and the current frame position information.

[0111] Based on step 301, all image feature points can be projected in the prior motion model to obtain the predicted value (predicted position information) and actual observation value (current frame position information, which can also be the image feature point p) of the image feature point in the projection. n ) to obtain the position error information.

[0112] 303. Determine the credibility of the feature points based on the position error information, the maximum extraction quantity, and the actual extraction quantity.

[0113] Based on step 302 , the reliability of the feature points can be determined according to the position error information, the maximum extraction number, and the actual extraction number.

[0114] In one specific embodiment, the degree of fit of the image feature points to the past motion model can be evaluated based on the optimized motion model, and weights can be assigned to each feature point. The higher the degree of fit, the more accurate the data of the feature point, and the greater the weight (credibility) assigned accordingly. Among them, δρ i is the error between the predicted value and the actual observed value of the i-th image feature point in the projection (i.e., position error information). N is the maximum number of feature points that can be extracted, and n is the actual number of feature points. When calculating the weight, multiply This coefficient is to ensure that the feature point constraint is the "component" of the optimization problem. Otherwise, if it is difficult to extract feature points in the environment, the IMU constraint of the optimization problem will overwhelmingly affect the result. The advantage of this is that even if the noise interference of the environment causes an insufficient number of feature points, as long as there are several high-quality feature points, accurate positioning can be restored. It should also be noted that in this embodiment, the degree of fitting refers to the error δρ between the predicted value of the feature point in the projection and the actual observed value i The weight is calculated based on the degree of fit. This weight is used to adjust the focus of the optimization problem. In other words, the positioning is prioritized based on feature points that have a high degree of fit in the past.

[0115] 304. Use the observation constraint information of the current frame, the observation constraint information of the next frame, and the motion constraint information as constraints for estimating the pose data to determine the visual pose deviation value between any image feature point and the pose data to be optimized; determine the inertial pose deviation value between the inertial measurement data at any time point between the current frame and the next frame and the pose data to be optimized.

[0116] Based on step 303, after obtaining the feature point credibility corresponding to different image feature points, the current frame image information, the next frame image information and the motion constraint information can be used as constraints for the estimated pose data, thereby determining the visual pose deviation value between any image feature point and the estimated pose data; and determining the inertial pose deviation value between the inertial measurement data and the estimated pose data at any time point between the current frame and the next frame.

[0117] In one specific embodiment, the camera observation value (Current frame image information and next frame image information, i.e., relevant observation data of the i-th heterogeneous robot) and IMU observation value (Described as motion constraint information, since it includes the speed, position, or rotation of heterogeneous robots) as a constraint on the estimated pose X (which can be understood as a state), thereby determining the deviation between all image feature points and the estimated pose data (i.e., the visual pose deviation value). This visual pose deviation value also needs to be combined with the maximum number of image feature points that can be extracted within the time window. Furthermore, it is necessary to determine the deviation between the IMU observation data at time j and the estimated pose data X (i.e., the inertial pose deviation value).

[0118] 305. Determine a measurement quantity value corresponding to the inertial measurement information within a time window between the current frame and the next frame.

[0119] Based on the above steps, when executing step 301, step 305 may also be executed. Specifically, the measurement quantity value corresponding to the inertial measurement information in the time window between the current frame and the next frame may be determined.

[0120] In one specific embodiment, within a time window, the number of observations of the IMU corresponding to the inertial measurement information in the motion model is determined.

[0121] 306. Perform a posteriori estimation on all image feature points based on the visual pose deviation value, the inertial pose deviation value, the feature point credibility, the maximum extraction quantity and the measurement quantity value to determine the weight value to be detected.

[0122] Thus, based on the visual pose deviation value, inertial pose deviation value, feature point credibility, maximum extraction quantity and measurement quantity value obtained in the above steps, weighted local pose optimization can be performed on all image feature points to determine the weight value to be detected. It should be noted that both the prior estimation and the posterior estimation are for the robot's pose, which is the most important state maintained by the entire system. In the optimization problem, the pose data of the Yi Gou robot with the minimum error is obtained by optimizing according to the observation constraints and the inertial constraints, and the weight value of the image feature point is updated according to the pose data. Therefore, the weight value can be obtained by the calculation formula. Furthermore, it can be understood that the weight to be detected is the degree of fit of the image feature point to the posterior pose estimate. The degree of fit is calculated by the posterior estimation by predicting the deviation between the position of the image feature point in the image and the actual observation.

[0123] In one specific embodiment, the corresponding image feature points {p1, p2, ..., p n} are {ρ1,ρ2,...,ρ n For the newly constructed image feature points, its credibility is initialized to Camera observations and IMU observations As a constraint of the state X, in the optimization process, the parameters of the state vector X are adjusted to make the visual measurement error r c and IMU measurement error r b Reach the minimum and estimate the optimal state.

[0124] Among them, there are in, represents the deviation between the i-th feature point and the estimated pose X, and M is the number of observations of inertial measurement information in the time window. is the deviation between the IMU observation data at the jth moment and the estimated pose X. The above objective function uses weight ρ i (Weight value to be detected) The control algorithm finds a more reliable reference point, maximally eliminating the influence of dynamic objects on positioning.

[0125] The heterogeneous robot positioning method disclosed in this embodiment uses weighted local pose optimization to restore accurate positioning even when environmental noise results in an insufficient number of feature points, as long as a number of high-quality feature points are present. Furthermore, the optimized posterior estimates are used to calculate the degree of fit between each feature point and the motion, thereby determining its reliability.

[0126] To explain in detail the embodiments of this application Figure 1 For steps 104-105 shown in Figure 4 , Figure 4This is a flow chart of another heterogeneous robot positioning method disclosed in an embodiment of the present application, including steps 401 to 407.

[0127] 401. Construct a transformation matrix between the current frame and the next frame based on the inertial measurement information, estimate the next world coordinate system of the image acquisition device in the next frame based on the transformation matrix and the current world coordinate system of the image acquisition device in the current frame, and determine the coordinate transformation value between the next world coordinate system and the current world coordinate system.

[0128] In this embodiment, large noise feature point detection is required. Specifically, a transformation matrix between the current frame and the next frame can be constructed based on the inertial measurement information. Based on the transformation matrix and the current world coordinate system of the image acquisition device in the current frame, the next world coordinate system of the image acquisition device in the next frame can be estimated, and the coordinate transformation value between the next world coordinate system and the current world coordinate system can be determined.

[0129] In one specific embodiment, see Figure 7 , Figure 7 This is a schematic diagram of a large noise point detection disclosed in the embodiment of this application. Figure 7 As shown, the algorithm extracts an image feature point P on the dynamic object t It is in O t The projection on the camera is p1. Then the camera moves from O t Exercise to O t+1 , according to the motion model, the projection of the image feature points should be at O t+1 The camera's p′2 position. However, because the feature points move during this period, the actual projection will have a large deviation.

[0130] According to this rule, when the error between the expected position of an image feature point on the image and the actual observed position is greater than a threshold, the feature point is considered a large noise feature point. Therefore, in step 401 of this embodiment, the transformation from the current world system to the current camera coordinate system is known, and the transformation matrix is ​​constructed using the motion information collected by the IMU. And predict the transformation from the camera world coordinate system to the camera at the next moment, that is, the coordinate transformation value. represents a real matrix.

[0131] From this, we can know that It should be noted that It represents the current world coordinate system of the current frame. The world coordinate system described above can also be understood as the camera coordinate system in the camera projection model in this embodiment.

[0132] 402. If any image feature point is a static feature point, determine the next static point position information of the static feature point in the next world coordinate system according to the projection model and coordinate transformation value of the image acquisition device.

[0133] Combined with step 401, when any image feature point is a stationary feature point, the next stationary point position information of the stationary feature point in the next world coordinate system can be determined according to the projection model and coordinate transformation value of the image acquisition device.

[0134] In one specific embodiment, according to the camera projection model, if the image feature point P t Is a stationary reference object, then it can be t+1 The camera observes P t (world coordinate system) position p′2 on the image. Among them, It should be noted that K is the camera intrinsic parameter, and the corresponding p′2 is understood as the position information of the next stationary point.

[0135] 403. Determine the current stationary point position information of the stationary feature point in the current world coordinate system, and judge whether the interval distance between the current stationary point position information and the next stationary point position information is greater than a preset observation threshold, so as to determine all stationary feature points that do not meet the preset observation threshold as dynamic reference points, and eliminate all dynamic reference points to obtain target image feature points and target weight values ​​corresponding to the target image feature points.

[0136] In this way, the current stationary point position information of the stationary feature point in the current world coordinate system can be determined, and it can be judged whether the interval distance between the current stationary point position information and the next stationary point position information is greater than the preset observation threshold, so that all stationary feature points that do not meet the preset observation threshold are determined as dynamic reference points, and all dynamic reference points are eliminated to obtain the target image feature points and the target weight values ​​corresponding to the target image feature points.

[0137] In one specific embodiment, the comparison image P t The actual position p2 (current stationary point position information) of the predicted point. When the distance between the predicted point and the actual observed point is greater than the preset observation threshold, the point is judged to be a dynamic reference point, and ||p′2-p2|| 2 >ε. ε is the preset observation threshold described above. This allows filtering out a number of image feature points with large errors. At the same time, it is possible to determine whether the remaining image feature points after filtering are target image feature points, as well as the target weight values ​​corresponding to these target image feature points.

[0138] As can be seen from the above description, this embodiment provides a method for detecting high-noise feature points. To address the problem of unreliable reference points in dynamic scenes severely impacting positioning progress, this embodiment uses the IMU's prior motion model to predict the image positions of each reference point under the current camera pose. When the error between the expected position of a feature point in the image and its actual observed position is greater than a threshold, the feature point is considered a high-noise feature point. This effectively reduces the impact of unreliable reference points.

[0139] 404. Construct a local weight map based on all target image feature points and corresponding target weight values, fuse the pose data to be optimized with the local weight map, and upload the fused local weight map to the cloud server, so that the cloud server constructs the connection relationship between all target image feature points according to the proximity algorithm, and updates the target weight value based on the weight value of the image feature point within the adjacent range of the target image feature point.

[0140] Based on step 403, it is necessary to perform weighted map fusion, global pose optimization and loop detection. Among them, step 404 can also be run in the cloud server, and the specific details are not limited here. In this embodiment, a local weight map can be constructed based on all target image feature points and the corresponding target weight values, the pose data to be optimized is fused with the local weight map, and the fused local weight map is uploaded to the cloud server, so that the cloud server constructs the connection relationship between all target image feature points according to the proximity algorithm, and updates the target weight value based on the weight value of the image feature point within the adjacent range of the target image feature point.

[0141] In one specific embodiment, each heterogeneous robot can upload its locally optimized local weight map (a fusion of target image feature points and target weight values) and the pose data to be optimized to a cloud server. The cloud server can then globally stitch all target image feature points in the local map according to the position of the heterogeneous robot to determine the global environment information.

[0142] Furthermore, the target weight values ​​of the target image feature points are updated by taking the average value through the K-nearest neighbor algorithm. What needs to be distinguished is that i and j here represent the i and jth target image feature points (it should be noted that i and j here are adjacent and can also be understood as the image feature points of adjacent i and j frames). Figure 2 and Figure 3 The meanings of i and j shown in the figure are different. It should be noted that in order to optimize the update speed of the feature point weights, the algorithm only updates the range of the K nearest neighbors of the target image feature point.

[0143] The cloud server then performs global pose optimization and loop closure detection. Taking into account all feature points in the global map and IMU constraints, it globally optimizes the pose and feature point positions of each robot. It then uses traditional loop closure detection algorithms to identify locations previously reached by the robot.

[0144] For easier understanding, please refer to Figure 5 , Figure 5 This is a general diagram of a heterogeneous multi-robot inertial visual positioning system based on a cloud server disclosed in an embodiment of this application. Figure 5 It can be seen that heterogeneous robots can execute Figure 1 、 Figure 2 、 Figure 3 and Figure 4 Steps 401 to 403. The cloud server mainly performs Figure 4 Step 404.

[0145] Specifically, the camera and IMU of each heterogeneous robot (1 to n) can collect the i-th frame camera data Inertial sensor data of frame j Then, feature point extraction is performed to obtain {p1,p2,...,p n}, and perform pre-integration to obtain Then perform weighted local pose optimization, The credibility of each feature point is then calculated using a posterior estimation algorithm. The local map and global pose are then uploaded to the cloud server. The cloud server receives the local maps and global poses of all heterogeneous robots, performs weighted local point cloud map fusion, performs weighted global pose optimization, and finally performs loop closure detection. This allows the position of each heterogeneous robot to be determined.

[0146] Through the positioning method of a heterogeneous robot disclosed in this embodiment, the prior motion state of the IMU is used to predict the projection of feature points on the image, and feature points with large deviations between the predicted value and the observed value are filtered. At the same time, a weighted optimization objective function is used to allow the algorithm to establish a motion model in reliable reference points, thereby minimizing the impact of dynamic objects. At the same time, in response to the problem of unreliable reference points in dynamic scenes, this embodiment calculates weights based on the degree of matching between the posterior estimated pose and each reference point, and assigns greater weights to reliable reference points in subsequent optimization, making the positioning of the robot more robust. Furthermore, the K-nearest neighbor averaging method is used to update the feature point weights of the global map, allowing the algorithm to extract and filter out more reliable global feature points from different perspectives of multiple heterogeneous robots.

[0147] It should be understood that, although the various steps in the flowcharts involved in the various embodiments described above are displayed in sequence according to the instructions of the arrows, these steps are not necessarily executed in sequence in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order restriction on the execution of these steps, and these steps can be executed in other orders. Moreover, at least a portion of the steps in the flowcharts involved in the various embodiments described above can include multiple steps or multiple stages, and these steps or stages are not necessarily executed and completed at the same time, but can be executed at different times, and the execution order of these steps or stages is not necessarily to be carried out in sequence, but can be executed in turn or alternately with other steps or at least a portion of steps or stages in other steps.

[0148] See also Figure 8 , Figure 8 This is a schematic structural diagram of a positioning system for a heterogeneous robot disclosed in an embodiment of the present application.

[0149] A receiving unit 801 is configured to receive current frame image information and next frame image information captured by an image capture device, perform feature extraction on the current frame image information, and obtain a plurality of image feature points that match the current frame and the next frame, as well as feature point credibility corresponding to the image feature points; wherein the current frame image information is used to describe observation constraint information of the current frame of the heterogeneous robot, and the next frame image information is used to describe observation constraint information of the next frame of the heterogeneous robot;

[0150] The receiving unit 801 is further configured to receive inertial measurement information accumulated between a current frame and a next frame sent by an inertial sensor, so as to obtain a priori estimation information of the heterogeneous robot based on the inertial measurement information; wherein the inertial measurement information is used to describe motion constraint information of the heterogeneous robot;

[0151] An acquisition unit 802 is configured to predict the heterogeneous robot based on the current frame image information, the next frame image information, and the prior estimation information, to obtain estimated pose data of the heterogeneous robot in the next frame, and to perform weighted local pose optimization on the image feature points and the feature point credibility using the observation constraint information of the current frame, the observation constraint information of the next frame, and the motion constraint information as constraints on the estimated pose data to obtain pose data to be optimized;

[0152] An allocating unit 803 is configured to perform a posteriori estimation on the credibility of each image feature point in the pose data to be optimized, and allocate a weight value to be detected; wherein the weight value to be detected is used to represent the degree of fit of the image feature point to the posteriori estimation;

[0153] The detection unit 804 is configured to perform large noise feature point detection on the weight values ​​of all image feature points to be detected, so as to eliminate image feature points that do not meet a preset observation threshold, thereby obtaining target image feature points and target weight values ​​corresponding to the target image feature points;

[0154] The uploading unit 805 is used to upload the pose data to be optimized, all target image feature points and target weight values ​​to the cloud server, so that the cloud service updates the target weight values, and based on the updated target weight values, globally splices all target image feature points according to the pose data to be optimized of the heterogeneous robot, to obtain the target pose data and global environment information of the heterogeneous robot in the next frame; wherein, the target pose data is used to describe the pose data with the smallest error.

[0155] Exemplarily, the system further includes: a control unit 806;

[0156] The receiving unit 801 is further configured to receive initial position information corresponding to each heterogeneous robot configured by the cloud server;

[0157] The control unit 806 is used to control the start and stop states of the corresponding heterogeneous robot according to the initial position information in the positioning task of the heterogeneous robot, and execute the steps of receiving the current frame image information and the next frame image information captured by the image acquisition device, as well as the inertial measurement information accumulated between the current frame and the next frame sent by the inertial sensor.

[0158] Exemplarily, the system further includes: a calculation unit 807;

[0159] The acquisition unit 802 is specifically configured to acquire acceleration error information and rotation error information of the heterogeneous robot between the current frame and the next frame based on the current frame image information, the next frame image information, and the inertial measurement information;

[0160] A calculation unit 807 is configured to perform a first integral calculation on the acceleration error information and the rotation information between the current frame and the next frame based on the acceleration information to obtain a velocity change of the heterogeneous robot between the current frame and the next frame;

[0161] The calculation unit 807 is further configured to perform a second integral calculation on the acceleration error information and the rotation information between the current frame and the next frame based on the acceleration information to obtain a position change of the heterogeneous robot between the current frame and the next frame;

[0162] The calculation unit 806 is further configured to perform a second integral calculation on the acceleration error information and the rotation information between the current frame and the next frame based on the acceleration information to obtain a position change of the heterogeneous robot between the current frame and the next frame;

[0163] The acquisition unit 802 is further configured to determine angular velocity information of different axes in the global coordinate system based on the rotation information, and perform a third integral calculation on the rotation information and rotation error information between the current frame and the next frame based on the angular velocity information of the different axes to obtain a rotation change of the heterogeneous robot between the current frame and the next frame.

[0164] The acquisition unit 802 is further configured to use the velocity change, position change, and rotation change as the posture change, and calculate the posture change with the estimated posture data of the previous frame to obtain prior estimation information.

[0165] Exemplarily, the system further includes: a determining unit 808;

[0166] A determination unit 808 is configured to determine the actual number of extracted feature points of all images and define a maximum number of extracted feature points corresponding to all images within a time window between a current frame and a next frame;

[0167] The acquisition unit 802 is specifically configured to project the position information corresponding to all image feature points onto the prior motion model of the inertial sensor to obtain predicted position information, thereby determining position error information between the predicted position information and the current frame position information;

[0168] The determining unit 808 is further configured to determine the reliability of the feature points according to the position error information, the maximum extraction quantity and the actual extraction quantity.

[0169] Exemplarily, the system includes:

[0170] The determination unit 808 is specifically configured to determine a visual pose deviation value between any image feature point and the pose data to be optimized; and determine an inertial pose deviation value between the inertial measurement data and the pose data to be optimized at any time point between the current frame and the next frame.

[0171] The determining unit 808 is further configured to determine a measurement quantity value corresponding to the inertial measurement information within a time window between a current frame and a next frame;

[0172] The determination unit 808 is further configured to perform a posteriori estimation on all image feature points based on the visual pose deviation value, the inertial pose deviation value, the feature point credibility, the maximum extraction quantity and the measurement quantity value, and determine the weight value to be detected.

[0173] Exemplarily, the system includes:

[0174] The determination unit 808 is specifically configured to construct a transformation matrix between the current frame and the next frame based on the inertial measurement information, estimate a next world coordinate system of the image acquisition device in the next frame based on the transformation matrix and the current world coordinate system of the image acquisition device in the current frame, and determine a coordinate transformation value between the next world coordinate system and the current world coordinate system;

[0175] The determining unit 808 is further configured to determine, when any image feature point is a stationary feature point, the next stationary point position information of the stationary feature point in the next world coordinate system based on the projection model and coordinate transformation value of the image acquisition device;

[0176] The determination unit 808 is further used to determine the current stationary point position information of the stationary feature point in the current world coordinate system, and to judge whether the interval distance between the current stationary point position information and the next stationary point position information is greater than a preset observation threshold, so as to determine all stationary feature points that do not meet the preset observation threshold as dynamic reference points, and eliminate all dynamic reference points.

[0177] Exemplarily, the system further includes: a construction unit 809;

[0178] A construction unit 809 is configured to construct a local weight map based on all target image feature points and corresponding target weight values;

[0179] The uploading unit 805 is specifically used to fuse the posture data to be optimized with the local weight map, and upload the fused local weight map to the cloud server, so that the cloud server constructs the connection relationship between all target image feature points according to the proximity algorithm, and updates the target weight value based on the weight value of the image feature point within the adjacent range of the target image feature point.

[0180] See below Figure 9 , a structural diagram of a positioning device for a heterogeneous robot disclosed in an embodiment of the present application includes:

[0181] CPU 901, memory 905, input / output interface 904, wired or wireless network interface 903 and power supply 902;

[0182] The memory 905 is a temporary storage memory or a permanent storage memory;

[0183] The CPU 901 is configured to communicate with the memory 905 and execute the instructions in the memory 905 to perform the aforementioned Figures 1 to 4 The positioning method of the heterogeneous robot in the illustrated embodiment.

[0184] The embodiment of the present application also provides a chip system, which includes at least one processor and a communication interface, wherein the communication interface and the at least one processor are interconnected through a line, and the at least one processor is used to run a computer program or instruction to execute the aforementioned Figures 1 to 4 The positioning method of the heterogeneous robot in the illustrated embodiment.

[0185] The embodiment of the present application also provides a computer-readable storage medium, which includes instructions. When the instructions are executed on a computer, the computer executes the aforementioned Figures 1 to 4 The positioning method of the heterogeneous robot in the illustrated embodiment.

[0186] The present application also provides a computer program product comprising instructions, which, when executed on a computer, causes the computer to execute the aforementioned Figures 1 to 4 The positioning method of the heterogeneous robot in the illustrated embodiment.

[0187] Those skilled in the art will clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0188] In the several embodiments provided in this application, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are merely schematic. For example, the division of the units is merely a logical function division. In actual implementation, there may be other division methods, such as 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 an indirect coupling or communication connection through some interfaces, devices or units, which can be electrical, mechanical or other forms.

[0189] The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of these units may be selected to achieve the purpose of this embodiment according to actual needs.

[0190] In addition, the functional units in the various embodiments of the present application may be integrated into a single processing unit, or each unit may exist physically separately, or two or more units may be integrated into a single unit. The aforementioned integrated units may be implemented in the form of hardware or software functional units.

[0191] If the 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 application is essentially 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 enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, read-only memory), random access memory (RAM, random access memory), disk or optical disk, and other media that can store program code.

Claims

1. A positioning method for heterogeneous robots, characterized in that: Applied to a heterogeneous robot, the heterogeneous robot includes an image acquisition device and an inertial sensor, and the method includes: receiving current frame image information and next frame image information captured by the image capture device, performing feature extraction on the current frame image information to obtain a plurality of image feature points that match the current frame and the next frame, and feature point credibility corresponding to the image feature points; wherein the current frame image information is used to describe observation constraint information of the current frame of the heterogeneous robot, and the next frame image information is used to describe observation constraint information of the next frame of the heterogeneous robot; Receiving inertial measurement information accumulated between a current frame and a next frame sent by the inertial sensor, so as to obtain a priori estimation information of the heterogeneous robot based on the inertial measurement information; wherein the inertial measurement information is used to describe motion constraint information of the heterogeneous robot; Predicting the heterogeneous robot based on the current frame image information of the current frame, the next frame image information, and the prior estimation information to obtain estimated pose data of the heterogeneous robot in the next frame, and using the observation constraint information of the current frame, the observation constraint information of the next frame, and the motion constraint information as constraints on the estimated pose data, performing weighted local pose optimization on the image feature points and the credibility of the feature points to obtain pose data to be optimized; Performing a posteriori estimation on the feature point credibility of each of the image feature points in the pose data to be optimized, and assigning a weight value to be detected; wherein the weight value to be detected is used to represent the degree of fit of the image feature point to the posteriori estimation; Performing large noise feature point detection on the weight values ​​to be detected of all the image feature points to eliminate image feature points that do not meet a preset observation threshold, thereby obtaining target image feature points and target weight values ​​corresponding to the target image feature points; The pose data to be optimized, all target image feature points and the target weight value are uploaded to the cloud server, so that the cloud service updates the target weight value, and based on the updated target weight value, all target image feature points are globally spliced ​​according to the pose data to be optimized of the heterogeneous robot to obtain the target pose data and global environment information of the heterogeneous robot in the next frame; wherein, the target pose data is used to describe the pose data with the smallest error.

2. The positioning method of a heterogeneous robot according to claim 1, characterized in that: Before receiving the current frame image information and the next frame image information acquired by the image acquisition device, the method further includes: Receiving initial position information corresponding to each heterogeneous robot configured by the cloud server; In the positioning task of the heterogeneous robot, the start and stop states of the corresponding heterogeneous robot are controlled according to the initial position information, and the step of receiving the current frame image information and the next frame image information captured by the image acquisition device is performed.

3. The method for positioning heterogeneous robots according to claim 1, characterized in that: The obtaining the a priori estimation information of the heterogeneous robot according to the inertial measurement information includes: Acquire acceleration error information and rotation error information of the heterogeneous robot between the current frame and the next frame according to the current frame image information, the next frame image information, and the inertial measurement information; performing a first integral calculation on the acceleration error information and the rotation information between the current frame and the next frame according to the acceleration information to obtain a velocity change of the heterogeneous robot between the current frame and the next frame; performing a second integral calculation on the acceleration error information and the rotation information between the current frame and the next frame according to the acceleration information to obtain a position change of the heterogeneous robot between the current frame and the next frame; Determining angular velocity information about different axes in a global coordinate system based on the rotation information, and performing a third integral calculation on the rotation information and the rotation error information between the current frame and the next frame based on the angular velocity information about the different axes to obtain a rotation change of the heterogeneous robot between the current frame and the next frame; The speed change, the position change and the rotation change are used as posture change, and the posture change and the estimated posture data of the previous frame are calculated to obtain the prior estimation information.

4. The positioning method of a heterogeneous robot according to claim 1, characterized in that: The obtaining of the feature point credibility corresponding to the image feature point includes: Determining the actual number of extracted feature points of all images, and defining a maximum number of extracted feature points corresponding to all images within a time window between the current frame and the next frame; Projecting the position information corresponding to all the image feature points onto the prior motion model of the inertial sensor to obtain predicted position information, so as to determine position error information between the predicted position information and the current frame position information; The reliability of the feature points is determined according to the position error information, the maximum extraction number and the actual extraction number.

5. The method for positioning a heterogeneous robot according to claim 4, characterized in that: The step of performing a posteriori estimation on the feature point credibility of each of the image feature points in the pose data to be optimized and allocating a weight value to be detected comprises: Determine a visual pose deviation value between any image feature point and the pose data to be optimized; determine an inertial pose deviation value between the inertial measurement information and the pose data to be optimized at any time point between the current frame and the next frame; Determining a measurement quantity value corresponding to the inertial measurement information within a time window between the current frame and the next frame; According to the visual pose deviation value, the inertial pose deviation value, the feature point credibility, the maximum extraction quantity and the measurement quantity value, a posteriori estimation is performed on all the image feature points to determine the weight value to be detected.

6. The method for positioning heterogeneous robots according to claim 1, characterized in that: The performing large noise feature point detection on the weight values ​​to be detected of all the image feature points to eliminate image feature points that do not meet a preset observation threshold includes: constructing a transformation matrix between the current frame and the next frame based on the inertial measurement information, estimating a next world coordinate system of the image acquisition device in the next frame based on the transformation matrix and the current world coordinate system of the image acquisition device in the current frame, and determining a coordinate transformation value between the next world coordinate system and the current world coordinate system; If any image feature point is a stationary feature point, determining the next stationary point position information of the stationary feature point in the next world coordinate system according to the projection model of the image acquisition device and the coordinate transformation value; Determine the current stationary point position information of the stationary feature point in the current world coordinate system, and judge whether the interval distance between the current stationary point position information and the next stationary point position information is greater than the preset observation threshold, so as to determine all stationary feature points that do not meet the preset observation threshold as dynamic reference points, and eliminate all dynamic reference points.

7. The method for positioning a heterogeneous robot according to claim 1, wherein: The step of uploading the pose data to be optimized, all target image feature points, and the target weight value to a cloud server includes: Constructing a local weight map according to all target image feature points and the corresponding target weight values; The pose data to be optimized is fused with the local weight map, and the fused local weight map is uploaded to the cloud server, so that the cloud server constructs the connection relationship between all the target image feature points according to the proximity algorithm, and updates the target weight value based on the weight value of the image feature point within the adjacent range of the target image feature point.

8. A positioning system for heterogeneous robots, characterized in that: Applied to a heterogeneous robot, the heterogeneous robot includes an image acquisition device and an inertial sensor, and the system includes: a receiving unit, configured to receive current frame image information and next frame image information captured by the image capture device, and perform feature extraction on the current frame image information to obtain a plurality of image feature points that match the current frame and the next frame, as well as feature point credibility corresponding to the image feature points; wherein the current frame image information is used to describe observation constraint information of the current frame of the heterogeneous robot, and the next frame image information is used to describe observation constraint information of the next frame of the heterogeneous robot; The receiving unit is further configured to receive inertial measurement information accumulated between a current frame and a next frame sent by the inertial sensor, so as to obtain a priori estimation information of the heterogeneous robot based on the inertial measurement information; wherein the inertial measurement information is used to describe motion constraint information of the heterogeneous robot; an acquisition unit, configured to predict the heterogeneous robot based on the current frame image information of the current frame, the next frame image information, and the prior estimation information, to obtain estimated pose data of the heterogeneous robot in the next frame, and to perform weighted local pose optimization on the image feature points and the credibility of the feature points using the observation constraint information of the current frame, the observation constraint information of the next frame, and the motion constraint information as constraints on the estimated pose data, to obtain pose data to be optimized; an allocating unit, configured to perform a posteriori estimation on the feature point credibility of each of the image feature points in the pose data to be optimized, and allocate a weight value to be detected; wherein the weight value to be detected is used to represent the degree of fit of the image feature point to the posteriori estimation; a detection unit, configured to perform large noise feature point detection on the weight values ​​to be detected of all the image feature points, so as to eliminate image feature points that do not meet a preset observation threshold, and obtain target image feature points and target weight values ​​corresponding to the target image feature points; An uploading unit is used to upload the pose data to be optimized, all target image feature points and the target weight value to a cloud server, so that the cloud service updates the target weight value, and based on the updated target weight value, globally splices all target image feature points according to the pose data to be optimized of the heterogeneous robot, to obtain the target pose data and global environment information of the heterogeneous robot in the next frame; wherein, the target pose data is used to describe the pose data with the smallest error.

9. A positioning device for a heterogeneous robot, characterized in that: The device comprises: CPU, memory, input and output interfaces, wired or wireless network interfaces, and power supply; The memory is a transient storage memory or a persistent storage memory; The central processing unit is configured to communicate with the memory and execute instruction operations in the memory to perform the positioning method of a heterogeneous robot according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that The computer-readable storage medium includes instructions, and when the instructions are executed on a computer, the computer is caused to execute the positioning method for a heterogeneous robot according to any one of claims 1 to 7.

Images