Heterogeneous robot positioning method and related equipment

Abstract

The embodiment of the invention provides a positioning method of a heterogeneous robot and related equipment, which are used for solving the positioning problem of the heterogeneous robot in a dynamic scene. The method provided by the embodiment of the invention comprises the following steps: receiving current frame image information and next frame image information to perform feature extraction to obtain a plurality of image feature points and feature point credibility; receiving inertial measurement information to obtain priori estimation information of the heterogeneous robot; according to the current frame image information, the next frame image information and the prior estimation information, estimated pose data is obtained, local pose optimization with weights is carried out on the image feature points and the feature point credibility, and pose data to be optimized are obtained; performing posterior estimation on the credibility of the feature points to obtain a to-be-detected weight value; performing large noise feature point detection on the to-be-detected weight value to obtain a target image feature point and a target weight value corresponding to the target image feature point; and uploading the to-be-optimized pose data, all the target image feature points and the target weight value 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] Positioning is a key link in robot navigation. Traditional positioning algorithms are implemented on the assumption that the environment is static, that is, the robot will use the surrounding fixed objects as reference frames to estimate the motion state, such as road signs, fences, and buildings on the road. However, in actual scenarios, robots are often surrounded by a large number of dynamic objects, especially in warehouse scenarios with multiple heterogeneous robots, which include forklifts for transporting goods, jacking trucks for placing shelves, movable robotic arms for picking goods, and goods on conveyor belts. If the robot mistakenly uses these dynamic objects as reference frames, its estimated motion state will have a large error, which increases the risk of smart warehouse operations. 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 the heterogeneous robot in a dynamic scene.

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

[0005] Receive the current frame image information and the next frame image information collected by the image acquisition device to 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, and feature point credibility corresponding to the image feature points; wherein 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;

[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 according to the inertial measurement information; wherein the inertial measurement information is used to describe motion constraint information of the heterogeneous robot;

[0007] According to the current frame image information of the current frame, the next frame image information and the prior estimation information, the heterogeneous robot is predicted to obtain 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 of the estimated pose data, and the image feature points and the credibility of the feature points are weightedly optimized 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 characterize 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, and 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, so as 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 acquired by the image acquisition 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] According to 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 a velocity change of the heterogeneous robot between the current frame and the next frame;

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

[0018] Determine 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 the rotation error information between the current frame and the next frame according to 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;

[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 credibility of the feature point corresponding to the image feature point includes:

[0021] Determine the actual number of extracted feature points of all the images, and define the maximum number of extracted feature points corresponding to all the image features in the time window between the current frame and the next frame;

[0022] Projecting the position information corresponding to all the image feature points into 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 assigning 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] Determine 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 posture deviation value, the inertial posture 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, the performing large noise feature point detection on the weight values ​​to be detected of all the image feature points to remove 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 according to the inertial measurement information, estimating a next world coordinate system of the image acquisition device in the next frame according to 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 the 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, used to receive the current frame image information and the next frame image information collected by the image acquisition device, so as to perform feature extraction on the current frame image information, and obtain a plurality of image feature points matching 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 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;

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

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

[0039] An allocating unit, used for performing a posteriori estimation on the credibility of the feature point of each of the image feature points in the pose data to be optimized, and allocating a weight value to be detected; wherein the weight value to be detected is used to characterize the degree of fit of the image feature point to the a posteriori estimation;

[0040] A detection unit, used 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, 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, so as 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 an embodiment 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 short-term 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 a heterogeneous robot 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 executed on a computer, the computer executes the positioning method for a heterogeneous robot 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, by fusing inertial sensors, the positioning accuracy of the entire robot system is improved. 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 drawings required for use in the description of the embodiments will be briefly introduced below. 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 diagram of a 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 a heterogeneous robot disclosed in an embodiment of the present application;

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

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

[0055] Figure 5This is a general diagram of an inertial visual positioning system for heterogeneous multi-robots 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 A schematic diagram of large noise point detection disclosed in an embodiment of the present application;

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

[0059] Fig. 9 This is a schematic diagram of the structure 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", etc. (if any) in the specification and claims of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be 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, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0061] It should be noted that the descriptions involving "first", "second", etc. in this application are only for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the number of technical features indicated. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In addition, the technical solutions between the various embodiments can be combined with each other, but they must be based on the ability of ordinary technicians in this field to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such 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 the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

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

[0064] 101. Receive current frame image information and next frame image information acquired by an image acquisition device to perform feature extraction on the current frame image information to obtain a plurality of image feature points matching 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 synchronous positioning and mapping (SLAM, simultaneous localization and mapping) system of 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 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, etc., which are not specifically limited here.

[0066] In this embodiment, in the SLAM system, each heterogeneous robot is equipped with a camera (image acquisition device, which will be described later as a camera or a camera). 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, so as to obtain multiple image feature points of the heterogeneous robot in the current frame and the next frame, and 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 of the specific embodiments, in the positioning task, the heterogeneous robot will collect 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 according to 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 the accumulated inertial measurement information between the current frame and the next frame sent by the inertial sensor. Thus, the prior estimation information of the heterogeneous robot is obtained based on the inertial measurement information. It should be noted that the inertial measurement information is used to describe the motion constraint information of the heterogeneous robot.

[0070] In one of the specific embodiments, 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, at least including 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. Then the IMU is frame aligned with the image information, and the accumulated IMU information between the current frame and the next frame is pre-integrated to record the motion information of the heterogeneous robot, so that the prior estimate of the heterogeneous robot can be obtained by combining the motion information.

[0071] 103. According to 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, and 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, and then 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 subjected to weighted local pose optimization to obtain the pose data to be optimized.

[0073] In one specific embodiment, based on step 101 and step 102, the operation logic or motion trajectory of the heterogeneous robot can be inferred by the current frame image information, the next frame image information and the prior estimation information, so as to estimate the estimated pose data of the heterogeneous robot in the next frame. Then, 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 credibility of the feature points are weighted for local pose optimization, so as to further estimate the pose data to be optimized of the heterogeneous robot in the next frame.

[0074] 104. Perform a posteriori estimation on the 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 the 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 Yigoo robot with the smallest 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, by adjusting various parameters of the pose data to be optimized and screening different feature point credibility, the feature point credibility with the smallest error in the image information and inertial measurement information is found as much as possible. Then, the optimal pose data is estimated. Thus, the image feature points corresponding to the pose data to be optimized are determined, and then the weight values ​​to be detected corresponding to the image feature points are found. Furthermore, the weight values ​​to be detected of all image feature points can be found in the same way.

[0077] In other feasible technical solutions, the weights of these feature points can also be reviewed by detecting large noise feature points. Specifically, the weight values ​​to be detected are updated according to the deviation between their predicted projection and actual projection. For details, please refer to the description of large noise feature point detection in 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. In other words, 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, it is determined whether the difference meets the preset observation threshold, and then the image feature points that do not meet the preset observation threshold are determined. After eliminating the image feature points that do not meet the preset observation threshold, the target image feature points and the target weight value of the target image feature points 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, combined 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, 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, and the target pose data and global environment information of the heterogeneous robot in the next frame are obtained. 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 by 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, so as to determine the target pose data and global environmental 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. And 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 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.

[0085] To explain in detail the embodiments of this application Figure 1 Step 101 shown in Figure 2 , Figure 2 This is a flow chart of another method for positioning a heterogeneous robot 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 robots according to the initial position information in the positioning task of the heterogeneous robots.

[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 in the positioning task of the heterogeneous robot, control the start and stop state of the corresponding heterogeneous robot according to the initial position information.

[0088] In one specific embodiment, each heterogeneous robot is configured according to the global initial position of the cloud server. Start. Represents the position of the ith heterogeneous robot at the jth moment. Specifically, the global initial position facilitates the 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 may not be controlled by the initial position information, that is, it is only processed as positioning identification.

[0090] 202. Receive current frame image information and next frame image information acquired by an image acquisition 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 similar to the above Figure 1 The step 101 is similar to that in step 101, and will not be described in detail here. However, it should be noted that the heterogeneous robot collects the tth k Frame camera data and t k Frame (current frame) to tth frame k+1 A series of IMU data (inertial measurement information) accumulated between frames (next frame) It should be noted that z represents the observation of the heterogeneous robot, and its superscript c refers to the camera observation. Similarly, the superscript IMU refers to the 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 previous and next two frames of IMU.

[0092] 203. 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.

[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. According to 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; according to 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; according to the rotation information, the angular velocity information of different axes in the global coordinate system is determined, and according to 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, so as to 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, so as to 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, so as to 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 Schematic diagram of an IMU and camera observation frequency disclosed in an embodiment of the present 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 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 timestamp of the camera data, the algorithm needs to perform a k Frame to t k+1 The IMU data is accumulated between frames for pre-integration.

[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 k+1 The change in speed, position, and rotation in the global coordinate system at the moment. is the acceleration error of IMU, is the rotation error, is the t of IMU k The rotation to 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, the position change and the 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 the posture change, and the posture change and the estimated posture 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, and 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 With t k+1 Frame of camera data As the input of the inertial visual odometer, t is estimated through weighted local optimization. k+1 The pose of the heterogeneous robot at the moment, that is, the estimated pose data (that is, 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 FIG. Figure 3 , Figure 3 This is a flow chart of another method for positioning a heterogeneous robot 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, it is necessary to perform weighted local pose optimization. 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 point of the heterogeneous robot at this time. Specifically, the matched feature points are actually the reference points of the two frames of images. Classical visual SLAM estimates the movement of the robot through these reference points. For example, if a person sees a tree moving backward, he will use this to judge that he is moving forward. People use trees as reference objects for positioning, and robots are similar. They use some feature matching algorithms to find feature points to assist in positioning.

[0109] For the 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. Projecting the position information corresponding to all the image feature points into the prior motion model of the inertial sensor to obtain predicted position information, so as to determine the 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 observed value (current frame position information, which can also be the image feature point p n ) to obtain the position error information.

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

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

[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 the "component" of the feature point constraint in 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 leads to 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 a value calculated by the degree of fit. This weight is used to adjust the focus of the optimization problem. That is, the feature points with a higher degree of fit in the past are given priority for positioning.

[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, 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 (Because it includes the speed, position or rotation of heterogeneous robots, it is described as motion constraint information) as a constraint on the estimated pose X (which can be understood as a state), so as to determine the deviation between all image feature points and the estimated pose data (i.e., the visual pose deviation value), where the 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 also necessary to determine the deviation between the IMU observation data at the jth moment 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 in a time window between a current frame and a 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. According to the visual posture deviation value, the inertial posture deviation value, the feature point credibility, the maximum extraction quantity and the measurement quantity value, all image feature points are estimated a posteriori 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, all image feature points can be optimized with weighted local pose, so as to determine the weight value to be detected. It should be noted that both the prior estimation and the posterior estimation are for the pose of the robot, which is the most important state maintained by the entire system. In the optimization problem, the pose data of the Yiguo robot with the smallest 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 estimation. 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 real observation.

[0123] In one specific embodiment, the corresponding image feature points {p1, p2, ..., p n The credibility of} is respectively {ρ1,ρ2,...,ρ n}. For the newly constructed image feature points, its credibility is initialized as Camera Observations and IMU observations As a constraint of the state X, the visual measurement error r is adjusted by adjusting the parameters of the state vector X during the optimization process. 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 ith 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, maximizing the elimination of the impact of dynamic objects on positioning.

[0125] Through the positioning method of a heterogeneous robot disclosed in this embodiment, through weighted local pose optimization, even if the number of feature points is insufficient due to environmental noise interference, as long as there are several feature points of good quality, accurate positioning can be restored. At the same time, the posterior estimation obtained by optimization is used to calculate the degree of fit between each feature point and the motion to obtain their credibility.

[0126] To explain in detail the embodiments of this application Figure 1 For steps 104-105 shown in FIG. Figure 4 , Figure 4This is a flow chart of another method for positioning a heterogeneous robot 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 according to the inertial measurement information, estimate the next world coordinate system of the image acquisition device in the next frame according to 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, the transformation matrix between the current frame and the next frame can be constructed according to the inertial measurement information, so as to estimate the next world coordinate system of the image acquisition device in the next frame according to 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.

[0129] In one specific embodiment, see Figure 7 , Figure 7 Schematic diagram of 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 Movement to O t+1 According to the motion model, the projection of the image feature points should be on 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 regarded as 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 through 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. Among them, 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 internal 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 P t The actual position p2 (the current static 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 >ε. Wherein, ε is the preset observation threshold described above. Thus, a batch of image feature points with large errors can be filtered out. At the same time, it can be determined that the remaining image feature points after filtering are target image feature points, as well as the target weight value corresponding to the target image feature point.

[0138] Therefore, it can be seen from the above description that this embodiment provides a method for detecting large noise feature points. In order to solve the problem that unreliable reference points in dynamic scenes seriously affect the positioning progress, this embodiment predicts the position of each reference point in the image under the current camera posture based on the prior motion model of the IMU. 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.

[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 in 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 there is no specific restriction here. In this embodiment, a local weight map can be constructed based on all target image feature points and 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 in the adjacent range of the target image feature point.

[0141] In one specific embodiment, each heterogeneous robot can upload the local weight map (the target image feature points and the target weight values ​​are fused) obtained by local optimization and the pose data to be optimized to the cloud server. Then, the cloud server can globally splice all the target image feature points in the local map according to the positions of the heterogeneous robots 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. It should be noted 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 image feature points of adjacent i and j frames). Figure 2 and Figure 3 The meanings of i and j shown 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] Then, the cloud server performs global posture optimization and loop detection. Considering all feature points of the global map and IMU constraints, the posture and feature point positions of each robot are globally optimized. The traditional loop detection algorithm is used to identify the locations that the robot has reached.

[0144] For easier understanding, please refer to Figure 5 , Figure 5 This is a general diagram of an inertial visual positioning system for heterogeneous multi-robots based on a cloud server disclosed in an embodiment of the present 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 in the above. The cloud server mainly performs Figure 4 Step 404.

[0145] Specifically, the camera and IMU of each heterogeneous robot (1-n) can collect the i-th frame of camera data Inertial sensor data of frame j Then, feature points are extracted to obtain {p1,p2,...,p n}, and perform pre-integration to obtain Then perform weighted local pose optimization. Then, the credibility of each feature point is calculated according to the posterior estimation algorithm. Then, the local map and global pose are uploaded to the cloud server. The cloud server receives the local map and global pose of all heterogeneous robots, performs weighted local point cloud map fusion, performs weighted global pose optimization, and finally performs loop detection. Thus, the position of each heterogeneous robot is located.

[0146] Through a positioning method for 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 a larger weight 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 above-mentioned embodiments are displayed in sequence according to the indication of the arrows, these steps are not necessarily executed in sequence according to the order indicated by the arrows. Unless there is a clear explanation in this article, the execution of these steps does not have a strict order restriction, and these steps can be executed in other orders. Moreover, at least a part of the steps in the flowcharts involved in the above-mentioned embodiments can include multiple steps or multiple stages, and these steps or stages are not necessarily executed 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 part of the steps or stages in other steps.

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

[0149] The receiving unit 801 is used to receive the current frame image information and the next frame image information collected by the image acquisition device, so as to perform feature extraction on the current frame image information, and obtain a plurality of image feature points matching 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 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;

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

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

[0152] The allocating unit 803 is used 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 characterize the degree of fit of the image feature point to the posteriori estimation;

[0153] The detection unit 804 is used to 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;

[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 used to receive the 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, and the accumulated inertial measurement information 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 used to acquire the acceleration error information and the 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;

[0160] The calculation unit 807 is used to perform 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, so as to obtain a speed change of the heterogeneous robot between the current frame and the next frame;

[0161] The calculation unit 807 is further used to perform 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, so as to obtain a position change of the heterogeneous robot between the current frame and the next frame;

[0162] The calculation unit 806 is further used to perform 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, so as to obtain a position change of the heterogeneous robot between the current frame and the next frame;

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

[0164] The acquisition unit 802 is further used to use the velocity change, position change and rotation change as the posture change, and calculate the posture change and 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, used 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 in a time window between a current frame and a next frame;

[0167] The acquisition unit 802 is specifically used to project the position information corresponding to all the image feature points into the prior motion model of the inertial sensor to obtain the predicted position information, so as to determine the position error information between the predicted position information and the current frame position information;

[0168] The determination unit 808 is further configured to determine the credibility 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 used to determine the visual posture deviation value between any image feature point and the posture data to be optimized; determine the inertial posture deviation value between the inertial measurement data at any time point between the current frame and the next frame and the posture data to be optimized;

[0171] The determination 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 used to perform a posteriori estimation on all image feature points according to the visual posture deviation value, the inertial posture 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 used to construct a transformation matrix between the current frame and the next frame according to the inertial measurement information, estimate the next world coordinate system of the image acquisition device in the next frame according to 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;

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

[0176] The determination unit 808 is also used to determine the current stationary point position information of the stationary feature point in the current world coordinate system, and to determine 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 used to construct a local weight map according to all target image feature points and corresponding target weight values;

[0179] The uploading unit 805 is specifically used to 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.

[0180] See below Fig. 9 , a structural schematic 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, the chip system includes at least one processor and a communication interface, 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, the computer-readable storage medium 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 including instructions, which, when executed on a computer, enables 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 can 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 the present 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 only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, 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 separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0190] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[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 to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the 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 method for positioning a heterogeneous robot, characterized in that: Applied to a heterogeneous robot, the heterogeneous robot includes an image acquisition device and an inertial sensor, and the method includes: Receive the current frame image information and the next frame image information collected by the image acquisition device to 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, and feature point credibility corresponding to the image feature points; wherein 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; 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 according to the inertial measurement information; wherein the inertial measurement information is used to describe motion constraint information of the heterogeneous robot; According to the current frame image information of the current frame, the next frame image information and the prior estimation information, the heterogeneous robot is predicted to obtain 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 of the estimated pose data, and the image feature points and the credibility of the feature points are weightedly optimized 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 characterize 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, and 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, so as 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 method for positioning 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 a heterogeneous robot according to claim 1, characterized in that: The obtaining the prior 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; According to 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 a velocity change of the heterogeneous robot between the current frame and the next frame; According to the acceleration information, performing a second integral calculation on the acceleration error information and the rotation information between the current frame and the next frame to obtain a position change of the heterogeneous robot between the current frame and the next frame; Determine 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 the rotation error information between the current frame and the next frame according to 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; 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 method for positioning a heterogeneous robot according to claim 1, characterized in that: The obtaining the credibility of the feature point corresponding to the image feature point includes: Determine the actual number of extracted feature points of all the images, and define the maximum number of extracted feature points corresponding to all the image features in the time window between the current frame and the next frame; Projecting the position information corresponding to all the image feature points into 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 credibility of each of the image feature points in the pose data to be optimized and assigning 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 data and the pose data to be optimized at any time point between the current frame and the next frame; Determine 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 posture deviation value, the inertial posture 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 a heterogeneous robot according to claim 1, characterized in that: The step of 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 according to the inertial measurement information, estimating a next world coordinate system of the image acquisition device in the next frame according to 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, characterized in that: 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 the 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, used to receive the current frame image information and the next frame image information collected by the image acquisition device, so as to perform feature extraction on the current frame image information, and obtain a plurality of image feature points matching 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 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; The receiving unit is further used to receive the accumulated inertial measurement information between the current frame and the next frame sent by the inertial sensor, so as to obtain the prior estimation information of the heterogeneous robot according to the inertial measurement information; wherein the inertial measurement information is used to describe the motion constraint information of the heterogeneous robot; an acquisition unit, configured to predict the heterogeneous robot according to the current frame image information of the current frame, the next frame image information and the prior estimation information, obtain estimated pose data of the heterogeneous robot in the next frame, and use the observation constraint information of the current frame, the observation constraint information of the next frame and the motion constraint information as constraints of the estimated pose data, perform weighted local pose optimization on the image feature points and the credibility of the feature points, and obtain pose data to be optimized; An allocating unit, used for performing a posteriori estimation on the credibility of the feature point of each of the image feature points in the pose data to be optimized, and allocating a weight value to be detected; wherein the weight value to be detected is used to characterize the degree of fit of the image feature point to the a posteriori estimation; A detection unit, used 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, 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, so as 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 short-term 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 as described in 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 executes the positioning method for a heterogeneous robot according to any one of claims 1 to 7.

Images