Ultrasonic mapping positioning method and device, robot and electronic equipment

Abstract

The invention provides an ultrasonic mapping positioning method and device, a robot and electronic equipment, and belongs to the field of digital mapping, and the method comprises the following steps: respectively carrying out continuous sampling on an inertial measurement unit and each ultrasonic detection device to obtain a plurality of continuous measurement arrays; obtaining the position of the robot and the position of the obstacle at the target moment according to each measurement array; when the target moment is the starting moment, obtaining an environment map of the environment where the robot is located according to the position of the obstacle, and controlling the robot to move according to the environment map; and when the target moment is not the starting moment, updating the map, and performing movement obstacle avoidance according to the updated map. Therefore, through the cooperation of the ultrasonic detection data and the IMU measurement data, the positioning mapping process is not interfered by various environmental factors such as light, high-cost devices such as a laser detector do not need to be used, and the method can also adapt to various scenes, so that the cost, precision and universality requirements of robot mapping are met at the same time.

Description

Technical Field

[0001] The present invention relates to the field of digital mapping, and in particular to an ultrasonic mapping and positioning method, device, robot and electronic equipment. Background Art

[0002] With the development of industrial automation and intelligent service robots, the demand for autonomous navigation capabilities of robots is growing. Robots need to understand their environment and plan the best path to their destination without direct human intervention, which requires robots to understand and build a map of their surroundings.

[0003] Currently, common robot mapping methods include: (1) visual SLAM-based mapping solutions, which obtain the top-level and non-top-level information of the environment image separately and perform fusion processing to build a map; (2) laser radar-based detection to achieve mapping and positioning. However, these methods cannot meet the requirements of cost, accuracy and versatility at the same time. Summary of the invention

[0004] In view of this, an object of the present invention is to provide an ultrasonic mapping and positioning method, device, robot and electronic equipment, so as to simultaneously meet the cost, accuracy and versatility requirements of robot mapping.

[0005] In order to achieve the above object, the technical solution adopted by the present invention is as follows:

[0006] In a first aspect, the present invention provides an ultrasonic mapping and positioning method, which is applied to a robot host, wherein the robot is also provided with an inertial measurement unit and a plurality of ultrasonic detection devices in different orientations, and the host is respectively connected to the inertial measurement unit and each of the ultrasonic detection devices in communication, and the method comprises:

[0007] Continuously sampling the inertial measurement unit and each of the ultrasonic detection devices respectively to obtain a plurality of continuous measurement arrays; wherein the plurality of measurement arrays include measurement arrays at a target moment and at a plurality of detection moments after the target moment;

[0008] According to each of the measurement arrays, the robot position at the target moment is obtained, and according to the robot position and the measurement array, the obstacle position when the robot is located at the robot position is obtained;

[0009] When the target time is the starting time, constructing an environment map of the environment in which the robot is located according to the position of the obstacle, and controlling the movement of the robot according to the environment map;

[0010] When the target time is any time after the start time, the map is updated according to the obstacle position and the environment map, and the motion obstacle avoidance is performed according to the updated map.

[0011] Optionally, the measurement array includes a distance matrix and a position and posture of the robot;

[0012] The step of obtaining the robot position at the target moment according to each of the measurement arrays comprises:

[0013] For the measurement array at each detection moment, multiply the inverse matrix of the pose at the target moment by the pose and distance matrix in the measurement array to obtain the estimated pose of the robot in the target coordinate system at the detection moment; wherein the target coordinate system is the world coordinate system where the robot is located at the target moment;

[0014] Inputting each of the estimated postures into a preset disturbance analysis model, and generating a disturbance variance using the disturbance analysis model; wherein the disturbance analysis model is a model obtained by fitting a historical measurement array of the robot;

[0015] Based on the disturbance variance and each of the estimated poses, fusing the multiple estimated poses with Gaussian distribution to obtain a fused pose;

[0016] Generate a Gaussian distribution perturbation set with a mean of zero; wherein the Gaussian distribution perturbation set includes multiple perturbation values;

[0017] For each disturbance value, the robot position, motion command control amount and disturbance value of the robot at the last moment are integrated to obtain an estimated value;

[0018] The error between the fused pose and each of the estimated values ​​is calculated, and the estimated value with the smallest error is used as the robot position at the target moment.

[0019] Optionally, there are multiple disturbance variances, and the disturbance variances correspond one-to-one to the estimated poses;

[0020] The step of fusing Gaussian distribution of the plurality of estimated poses based on the disturbance variance and each of the estimated poses to obtain a fused pose comprises:

[0021] For each of the estimated postures, calculating the inverse of the disturbance variance corresponding to the estimated posture to obtain a weight of the estimated posture;

[0022] Multiplying each of the estimated poses by the corresponding weight, and adding multiple products obtained to obtain a rough pose;

[0023] The total weight of each weight is calculated, and the coarse pose is divided by the total weight to obtain a fused pose.

[0024] Optionally, the measurement array includes the distance of obstacles detected by each of the ultrasonic detection devices;

[0025] The step of obtaining the obstacle position when the robot is located at the robot position according to the robot position and the measurement array comprises:

[0026] taking the measurement array at the target time as a target measurement group;

[0027] For each obstacle distance in the target measurement group, an obstacle position at the target moment is obtained by combining the robot position, the obstacle distance, and a setting angle of the ultrasonic detection device for obtaining the obstacle distance.

[0028] Optionally, the step of constructing an environment map of the environment in which the robot is located according to the position of the obstacle includes:

[0029] Multiplying the preset map resolution by the robot position and the obstacle position respectively to obtain the robot coordinates and the obstacle coordinates on the map;

[0030] According to the adjacent relationship of the coordinates, the coordinates of the obstacles with adjacent coordinates are divided into a set to obtain a plurality of obstacle sets;

[0031] Eliminating the obstacle sets that do not form a closed loop area from the plurality of obstacle sets, and obtaining the area of ​​the closed loop area formed by each of the remaining obstacle sets;

[0032] Connecting the coordinates of each obstacle in the obstacle set with the largest area in sequence to obtain a map boundary;

[0033] Smoothing the map boundary, and dilating the smoothed map boundary according to a preset boundary dilation rule to obtain a preprocessed map;

[0034] The robot coordinates are set on the preprocessed map to obtain an environment map.

[0035] Optionally, the step of dilating the smoothed map boundary according to a preset boundary dilation rule to obtain a preprocessed map includes:

[0036] When the boundary expansion rule is to reduce the map, the map boundary is reduced inward according to a preset reduction radius;

[0037] When the boundary expansion rule is to expand the map, the map boundary is expanded outward according to a preset expansion radius.

[0038] Optionally, the step of updating the map according to the obstacle position and the environment map includes:

[0039] According to the coverage between each obstacle position and each position point in the environment map, updating the map value of each position point;

[0040] The location point whose current map value is greater than the obstacle threshold is regarded as an obstacle point;

[0041] According to a preset expansion radius, all expansion points of each obstacle point are determined from the environment map;

[0042] On the environment map, the obstacle points and the expansion points are updated to obtain a new environment map.

[0043] Optionally, the step of determining all expansion points of each obstacle point from the environment map according to a preset expansion radius includes:

[0044] Taking the obstacle point as its own expansion origin, and putting the obstacle point into the first set;

[0045] Extract a position point from the first set as a point to be measured, and put the point to be measured into a second set;

[0046] Calculating the distance between the point to be measured and the expansion origin of the point to be measured, and determining whether the distance is less than the expansion radius;

[0047] If yes, then update the map value of the point to be measured, select the position point that is closest to the point to be measured and is not in the second set, put it into the first set as a new point, use the expansion origin of the point to be measured as the expansion origin of the new point, and return to execute the steps of extracting a position point from the first set as the point to be measured and putting the point to be measured into the second set;

[0048] If not, the map value of the point to be measured is updated, and when the second set is not empty, the process returns to the step of extracting a location point from the first set as the point to be measured and putting the point to be measured into the second set.

[0049] Optionally, the step of updating the map value of the point to be measured includes:

[0050] When the spacing is not less than the radius of the robot, the expansion value of the point to be measured is reduced by a preset value to obtain a base value, and a decay rate is obtained according to the spacing and the radius of the robot, and the product of the base value and the decay rate is used as a new expansion value of the point to be measured;

[0051] When the distance is smaller than the radius of the robot and the distance is not zero, updating the expansion value of the point to be measured according to a preset attenuation value;

[0052] When the spacing is equal to zero, the expansion value of the point to be measured is set to a maximum value;

[0053] From the current expansion value of the point to be measured and the historical map value, a maximum value is selected as the current map value of the point to be measured.

[0054] Optionally, the step of controlling the movement of the robot according to the environment map comprises:

[0055] Determine a target point in the environment map, take the robot position in the environment map as a starting point, and use an A* algorithm to obtain a planned path;

[0056] The robot is controlled to move according to the planned path.

[0057] In a second aspect, the present invention provides an ultrasonic mapping and positioning device, which is applied to a host of a robot, wherein the robot is also provided with an inertial measurement unit and a plurality of ultrasonic detection devices in different orientations, wherein the host is respectively connected to the inertial measurement unit and each of the ultrasonic detection devices in communication, and the ultrasonic mapping and positioning device comprises a data sampling module, an obstacle acquisition module, a map establishment module and a map update module;

[0058] The data sampling module is used to continuously sample the inertial measurement unit and each of the ultrasonic detection devices to obtain a plurality of continuous measurement arrays; wherein the plurality of measurement arrays include measurement arrays at a target moment and at a plurality of detection moments after the target moment, and the measurement arrays include a distance matrix and the position and posture of the robot;

[0059] The obstacle acquisition module is used to obtain the robot position at the target moment according to each of the measurement arrays, and to obtain the obstacle position when the robot is located at the robot position according to the robot position and the measurement array;

[0060] The map building module is used to build an environment map of the environment where the robot is located according to the position of the obstacle when the target time is the starting time, and control the movement of the robot according to the environment map;

[0061] The map updating module is used to update the map according to the obstacle position and the environment map when the target moment is any moment after the start moment, and to perform motion obstacle avoidance according to the updated map.

[0062] In a third aspect, the present invention provides a robot, comprising a body, and a host, an inertial measurement unit and a plurality of ultrasonic detection devices installed on the body, wherein the plurality of ultrasonic detection devices are arranged at different positions of the body, and the host is respectively connected to the inertial measurement unit and the ultrasonic detection device for communication;

[0063] The inertial measurement unit is used to obtain the current position and posture of the robot in real time;

[0064] The ultrasonic detection device is used to emit ultrasonic waves in real time to detect the distance of obstacles;

[0065] The host is used to implement the ultrasonic mapping and positioning method as described in the first aspect.

[0066] In a fourth aspect, the present invention provides an electronic device, comprising a processor and a memory, wherein the memory stores a computer program executable by the processor, and the processor can execute the computer program to implement the ultrasonic mapping and positioning method as described in the first aspect.

[0067] In a fifth aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the ultrasonic mapping and positioning method as described in the first aspect.

[0068] The ultrasonic mapping and positioning method, device, robot and electronic device provided by the embodiment of the present invention include: continuously sampling the inertial measurement unit and each ultrasonic detection device respectively to obtain multiple continuous measurement arrays; wherein the multiple measurement arrays include measurement arrays at the target moment and multiple detection moments after the target moment; according to each measurement array, the robot position at the target moment is obtained, and according to the robot position and the measurement array, the obstacle position when the robot is located at the robot position is obtained; when the target moment is the starting moment, according to the obstacle position, the environment map of the robot's environment is obtained, and according to the environment map, the robot movement is controlled; when the target moment is any moment after the starting moment, the map is updated according to the obstacle position and the environment map, and the movement and obstacle avoidance are performed according to the updated map. In this way, positioning and mapping can be realized by the cooperation of ultrasonic detection data and IMU measurement data, and the positioning and mapping process is not interfered by various environmental factors such as light, and there is no need to use high-cost devices such as laser detectors. It can also adapt to various scenarios, thereby meeting the cost, accuracy and versatility requirements of robot mapping at the same time.

[0069] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, preferred embodiments are given below and described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0071] Figure 1 A schematic diagram of the structure of an ultrasonic mapping and positioning system provided by an embodiment of the present invention is shown.

[0072] Figure 2 A block diagram of an electronic device provided by an embodiment of the present invention is shown.

[0073] Figure 3 A schematic flow chart of an ultrasonic mapping and positioning method provided in an embodiment of the present invention is shown.

[0074] Figure 4 Shows Figure 3 Schematic diagram of the process flow of some sub-steps of step 13.

[0075] Figure 5 Shows Figure 4 A flowchart of some sub-steps of step 133 in FIG.

[0076] Figure 6 Shows Figure 3 Schematic diagram of the process flow of some sub-steps of step 15.

[0077] Figure 7 Shows Figure 3 Schematic diagram of the process of some sub-steps of step 17.

[0078] Figure 8 Shows Figure 7 A flowchart of some sub-steps of step 175.

[0079] Fig. 9 A block diagram of an ultrasonic mapping and positioning device provided by an embodiment of the present invention is shown.

[0080] Explanation of the reference numerals: 10 - ultrasonic mapping and positioning system; 110 - robot body; 120 - inertial measurement unit; 130 - host; 140 - ultrasonic detection device; 20 - electronic device; 210 - memory; 220 - processor; 230 - communication module; 30 - ultrasonic mapping and positioning device; 310 - data sampling module; 320 - obstacle acquisition module; 330 - map building module; 340 - map updating module. DETAILED DESCRIPTION

[0081] The following will be combined with the accompanying drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings here can be arranged and designed in various different configurations.

[0082] Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the invention claimed for protection, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without making creative work are within the scope of protection of the present invention.

[0083] It should be noted that relational terms such as "first" and "second" are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0084] The ultrasonic mapping and positioning method provided by the embodiment of the present invention can be applied to Figure 1 In the ultrasonic mapping and positioning system 10 shown, the ultrasonic mapping and positioning system 10 includes a robot body 110, and an inertial measurement unit 120, a host 130 and a plurality of ultrasonic detection devices 140 installed on the robot body 110. The host 130 can be respectively communicated with the inertial measurement unit 120 and the ultrasonic detection device 140 by wired connection or wireless connection.

[0085] Multiple ultrasonic detection devices 140 are arranged at different positions of the robot body 110 so that the detection range can cover the robot. For example, there can be four ultrasonic detection devices 140, which are arranged on the front, back, left and right sides of the robot body 110 respectively.

[0086] The inertial measurement unit 120 is used to measure the current position and posture of the robot in real time.

[0087] The ultrasonic detection device 140 is used to emit ultrasonic waves in real time and obtain the distance of obstacles within the detection range according to the reflected ultrasonic waves, wherein the obstacle distance is the distance between the robot body 110 and the obstacle.

[0088] The host 130 is used to implement the ultrasonic mapping and positioning method provided in the embodiment of the present invention.

[0089] The robot may be any robot such as a lawn mowing robot, a patrol robot, a sweeping robot, etc. The host 130 may be, but is not limited to, a computer device, a single chip microcomputer, a programmable logic controller (PLC), or a field programmable gate array (FPGA).

[0090] Please refer to Figure 2 , is a block diagram of an electronic device 20, which may be Figure 1 The host 130 in the ultrasonic mapping and positioning system 10 is shown. The electronic device 20 includes a memory 210, a processor 220 and a communication module 230. The memory 210, the processor 220 and the communication module 230 are electrically connected to each other directly or indirectly to achieve data transmission or interaction. For example, these components can be electrically connected to each other through one or more communication buses or signal lines.

[0091] The memory 210 is used to store programs or data and can be, but not limited to, a random access memory, a read-only memory, a programmable read-only memory, an erasable read-only memory, an electrically erasable read-only memory, and the like.

[0092] The processor 220 is used to read / write data or programs stored in the memory 210 and execute corresponding functions. For example, Figure 1 In the ultrasonic mapping and positioning system 10 shown, the processor 220 of the host 130 reads the computer program stored in the memory 210 to implement the ultrasonic mapping and positioning method provided in the embodiment of the present invention.

[0093] The communication module 230 is used to establish a communication connection between the electronic device 20 and other communication terminals through the network, and to send and receive data through the network. Figure 1 In the ultrasonic mapping and positioning system 10 shown, the host 130 transmits and receives data with the inertial measurement unit 120 and the ultrasonic detection device 140 respectively through the network.

[0094] It should be understood that Figure 2 The structure shown is only a schematic diagram of the structure of the electronic device 20. The electronic device 20 may also include Figure 2 More or fewer components as shown, or with Figure 2 Different configurations shown. Figure 2 Each component shown in the figure can be implemented by hardware, software or a combination thereof.

[0095] In order to improve the problem that the current robot mapping method cannot meet the requirements of cost, accuracy and versatility at the same time, the embodiment of the present invention provides an ultrasonic mapping positioning method, referring to Figure 3 , including steps 11 to 17. Thus,: Figure 1 In the ultrasonic mapping and positioning system 10 shown in FIG. Figure 2 The structure shown implements steps 11 to 17 when the processor 220 executes the computer program stored in the memory 210.

[0096] Step 11, continuously sampling the inertial measurement unit and each ultrasonic detection device respectively to obtain a plurality of continuous measurement arrays.

[0097] The multiple measurement arrays include measurement arrays at the target time and multiple detection times after the target time.

[0098] Step 13, obtaining the robot position at the target time according to each measurement array, and obtaining the obstacle position when the robot is located at the robot position according to the robot position and the measurement array.

[0099] Step 15, when the target time is the starting time, the environment map of the robot's environment is obtained according to the position of the obstacle, and the robot movement is controlled according to the environment map.

[0100] Among them, the environment map includes a map boundary.

[0101] Step 17, when the target time is any time after the start time, the map is updated according to the obstacle position and the environment map, and the movement obstacle avoidance is performed according to the updated map.

[0102] For example, in combination Figure 1In the ultrasonic mapping and positioning system 10 shown, when the robot is powered on to start mowing, sweeping or inspecting a certain area, the inertial measurement unit 120 installed on the robot obtains the robot's position in real time, and each ultrasonic detection device 140 emits ultrasonic waves in real time, and obtains the obstacle distance between the robot and the obstacle based on the time from the ultrasonic wave being sent to the echo being received. The host 130 of the robot samples the inertial measurement unit 120 and the ultrasonic detection device 140 in real time to obtain a measurement array of multiple consecutive moments.

[0103] The measurement data includes a measurement array of a target time and a plurality of consecutive detection times after the target time, and each measurement time includes the measurement value of the inertial measurement unit 120 and the measurement value of each ultrasonic detection device 140 at the corresponding time. For example, when the work is just started, the target time is t 1 At time t, the measurement data obtained can be 1 ,t 2 ,t 3 ,t 4 The measurement array at time t 1 When the environment map at time t is 2 , then the measurement array can be t 2 ,t 3 ,t 4 ,t 5 Array of measurements at time instant.

[0104] Then, the host 130 obtains the robot position at the target time according to the sampled measurement array, and obtains the obstacle position when the robot is at the robot position according to the robot position and the measurement array. Thus, when the target time is the starting time, the host 130 determines the map boundary according to the obstacle position, and then obtains the environment map of the robot's environment according to the map boundary, and controls the robot movement according to the environment map. When the target time is any time after the starting time (i.e., not the starting time), since the map boundary and the initial environment map have been obtained at the starting time, the host 130 can directly update the environment map according to the obstacle position, and control the robot movement and obstacle avoidance according to the updated map.

[0105] Traditional mapping solutions based on visual SLAM are highly dependent on light. They are easily unable to distinguish static and dynamic obstacles in environmental images due to insufficient light and other environmental conditions. In addition, the computational load is large, and the constructed maps cannot be directly used for path planning and navigation. They cannot solve the cumulative drift during positioning, resulting in poor positioning and mapping accuracy. LiDAR-based positioning and mapping solutions are highly dependent on hardware, have high hardware costs, and cannot adapt to all application scenarios.

[0106] In the ultrasonic mapping and positioning method provided by the present invention, positioning and mapping are achieved through the joint processing of ultrasonic detection data and IMU measurement data. The positioning and mapping process is not affected by various environmental factors such as light, and there is no need to use high-cost devices such as laser detectors. It can also be adapted to various scenarios, thereby meeting the cost, accuracy and versatility requirements of robot mapping at the same time.

[0107] In order to facilitate subsequent data processing, the obstacle distance detected by each ultrasonic detection device at the same time can be represented by a distance matrix, which contains the obstacle distance detected by the ultrasonic detection device. Similarly, the posture acquired by the inertial measurement unit at the same time can be represented by a posture matrix, which includes data such as the robot's posture, acceleration, and angular velocity.

[0108] The method of obtaining the robot position at the target time according to the measurement array in step 13 can be flexibly set. For example, a pre-trained neural network model or a deep learning model can be used to generate the robot position for the measurement array, or a preset rule algorithm can be used to obtain the robot position. The above method is only an example, and its implementation method is not limited.

[0109] In order to obtain a more accurate robot position, in the process of obtaining the robot position in step 13, each measurement array is introduced to infer the estimated position of the robot at the target time, and the multiple estimated positions are subjected to perturbation analysis and Gaussian distribution fusion to obtain a more accurate robot position at the target time. Figure 4 The process of obtaining the robot position at the target time in step 13 includes steps 131 to 136.

[0110] Step 131 , for each measurement array at the detection moment, multiply the inverse matrix of the pose at the target moment by the pose and distance matrix in the measurement array to obtain the estimated pose of the robot in the target coordinate system at the detection moment.

[0111] The target coordinate system is the world coordinate system where the robot is located at the target time. The estimated pose can be understood as the inverse pose of the robot at the detection time in the coordinate system at the target time.

[0112] Take the target time as t 1 , the detection time after the target time is t 2 ,t 3 ,t 4 For example, t 2 The robot is at t 1 The estimated pose in the world coordinate system can be expressed as: represents the estimated pose, Indicates t 1The inverse pose matrix at time t, Indicates t 3 The pose matrix at the moment, Indicates t 2 The distance matrix at time t 3 The robot is at t 1 The estimated pose in the world coordinate system can be expressed as: t 3 The robot is at t 1 The estimated pose in the world coordinate system can be expressed as:

[0113] It should be understood that the estimated position and posture of the target at a certain moment, that is, the product of the distance matrix at the target moment and the identity matrix, is the distance matrix itself.

[0114] Step 132: input each estimated posture into a preset disturbance analysis model, and generate a disturbance variance using the disturbance analysis model.

[0115] Among them, the perturbation analysis model is a model obtained by fitting and iteratively training the robot's historical measurement array, and has learned the relationship between the robot's various poses and perturbation errors. Therefore, the perturbation analysis model can be used to process the input estimated poses to obtain the perturbation variance corresponding to each estimated pose.

[0116] Step 133: Based on the disturbance variance and each estimated pose, multiple estimated poses are fused with Gaussian distribution to obtain a fused pose.

[0117] Here, the inverse of the perturbation variance can be used as the weight of each estimated pose, and then the estimated poses are weighted and summed according to the weights to obtain the fused pose. Alternatively, the weights can be calculated based on the perturbation variance using a preset rule, and then the estimated poses are weighted fused. The above two methods of obtaining the fused pose are examples, and their implementation methods are not limited.

[0118] In order to make the fusion pose more accurate, refer to Figure 5 The process of obtaining the fused posture in step 133 may include steps 1331 to 1335.

[0119] Step 1331: For each estimated posture, calculate the inverse of the disturbance variance corresponding to the estimated posture to obtain the weight of the estimated posture.

[0120] Step 1333, multiply each estimated pose by the corresponding weight, and add the multiple products obtained to obtain a rough pose.

[0121] Step 1335, calculate the total weight of each weight, divide the coarse pose by the total weight, and obtain the fused pose.

[0122] The above steps 1331 to 1335 can be expressed by the formula: σ n represents the perturbation variance of the nth estimated pose, μ n represents the nth estimated pose, and μ represents the perturbation pose.

[0123] Step 134, generating a Gaussian distribution disturbance set with a mean of zero.

[0124] The Gaussian distribution disturbance set includes multiple disturbance values, which can also be understood as each disturbance value in the Gaussian distribution disturbance set conforming to a Gaussian distribution with a mean of zero.

[0125] Step 135 , for each disturbance value, the robot position, motion command control amount and disturbance value of the robot at the previous moment are integrated to obtain an estimated value.

[0126] Here, the estimated value includes the x-axis coordinate, y-axis coordinate, and angle coordinate in the world coordinate system, which can be expressed as: 2n =x 1 +dx+Δx,y′ 2n =y 1 +dy+Δy,θ′ 2n =θ 1 +dθ+Δθ, where (x 1 ,y 1 ,θ 1 ) represents the coordinates of the previous moment, (x′ 2n , y′ 2n ,θ′ 2n ) represents the nth estimated value, and (Δx, Δy, Δθ) represents the disturbance value. It should be understood that both the estimated value and the disturbance value are coordinates, and the coordinates of the previous moment before the start moment can be any default value.

[0127] Step 136, calculate the error between the fused pose and each estimated value, and use the estimated value with the smallest error as the robot position at the target moment.

[0128] Here, the fusion pose can be expressed as (x 2 ,y 2 ,θ 2 ), the error between the estimated value and the fused pose can be: It can also be the sum of differences, or any other method of calculating the error, which is not limited here.

[0129] In the above steps 131 to 136 and related sub-steps, the estimated pose at the target moment is inferred through the measurement arrays at multiple moments, and disturbance analysis, Gaussian fusion distribution and Gaussian distribution disturbance set are introduced to consider the influence of disturbance noise during the robot motion process in the calculation of the robot position, so that the final robot position is closer to the position of the actual motion state of the robot, weakening the influence of errors such as ultrasound in actual applications, and greatly improving the accuracy of the robot position.

[0130] After obtaining the robot position at the target time, in step 13, the obstacle position of each obstacle in the world coordinate system can be obtained according to the obstacle distance and the setting angle of the ultrasonic detection device 140. Please continue to refer to Figure 4 , the process includes step 137 to step 138.

[0131] Step 137: Use the measurement array at the target time as the target measurement group.

[0132] Step 138, for each obstacle distance in the target measurement group, an obstacle position at the target time is obtained by combining the robot position, the obstacle distance, and the setting angle of the ultrasonic detection device for obtaining the obstacle distance.

[0133] It should be understood that the increments on each coordinate axis can be obtained by combining trigonometric functions, obstacle distance and setting angle, so as to obtain the obstacle position. The calculation formula of the obstacle position can be: obs_n =x 2 +cos(θ 2 +β n )×L n ,y obs_n =y+sin(θ 2 +β n )×L n ,θ obs_n =θ 2 +β n ,(x obs_n ,y obs_n ,θ obs_n ) represents the position of the nth obstacle, β n represents the setting angle of the ultrasonic detection device 140 for detecting the distance of the nth obstacle, L n Indicates the distance to the nth obstacle.

[0134] The obstacle position and robot position obtained above can be mapped according to the preset map resolution to obtain the corresponding map coordinates. The mapping formula can be: map=(X,Y)×resolution, where (X,Y) represents the x and y coordinates of the obstacle position or the robot position, resolution represents the map resolution, and pose map Represents map coordinates.

[0135] After obtaining the robot position and the obstacle position in the above manner, the method of constructing the environment map in step 15 can be flexibly set. For example, the map can be constructed using preset rules, or the robot position and the obstacle position can be input into a pre-trained map generation model, and the environment map can be generated using the map generation model. Here, there is no restriction on the implementation method of constructing the environment map.

[0136] In order to make the environment map more accurate and facilitate the subsequent movement and obstacle avoidance of the robot, the idea of ​​removing obstacle points in the inaccessible area is introduced in the process of building the environment map. Figure 6 The process of constructing the environment map in step 155 includes steps 151 to 156.

[0137] Step 151 , multiplying the preset map resolution by the robot position and the obstacle position respectively to obtain the robot coordinates and the obstacle coordinates on the map.

[0138] Step 152 , according to the adjacent relationship of the coordinates, the coordinates of the obstacles with adjacent coordinates are divided into a set to obtain a plurality of obstacle sets.

[0139] Here, for each obstacle coordinate in the obstacle set, there is at least one obstacle coordinate adjacent to the obstacle coordinate in the obstacle set, that is, the obstacle set is composed of a plurality of obstacle coordinates adjacent to each other.

[0140] All obstacle coordinates can be put into a coordinate set. An obstacle coordinate is extracted from the coordinate set as a test coordinate, and the coordinate set is checked to see if there are adjacent obstacle coordinates of the test coordinate.

[0141] If not, the process returns to the step of extracting an obstacle coordinate from the coordinate set as the test coordinate to continue the above process.

[0142] If yes, both the adjacent obstacle coordinates and the test coordinates are put into an obstacle set. Then, if there are obstacle coordinates in the coordinate set that are adjacent to any obstacle coordinate in the adjacent set, the adjacent obstacle coordinates found are put into the obstacle set again, and the above process is executed repeatedly. If there are no coordinates in the coordinate set that are adjacent to each coordinate in the adjacent set, the process returns to the step of extracting an obstacle coordinate from the coordinate set as the test coordinate to continue the above process.

[0143] In this way, multiple obstacle sets can be obtained, and the area of ​​each obstacle set can be calculated.

[0144] Step 153: Eliminate obstacle sets that do not form a closed loop area from the multiple obstacle sets, and obtain the area of ​​the closed loop area formed by each remaining obstacle set.

[0145] In an optional manner, for each obstacle set, the adjacent distances between every two adjacent obstacle coordinates in the obstacle set can be calculated. When one or more adjacent distances are greater than a preset value (the preset value can be equal to the width or length of the robot, etc.), it can be determined that the obstacle set does not form a closed loop area, otherwise, a closed loop area is formed. A pre-trained neural network model can also be used to determine whether the obstacle set forms a closed loop area. The above manner is only an example, and its implementation manner is not limited.

[0146] Step 154, the coordinates of each obstacle in the obstacle set with the largest area are connected in sequence to obtain a map boundary.

[0147] It should be noted that here, the coordinates of each obstacle in the obstacle set with the largest area among the obstacle sets are taken as key obstacles, and the key obstacles are connected in sequence according to the adjacent order to obtain the map boundary.

[0148] Step 155, smoothing the map boundary, and dilating the smoothed map boundary according to a preset boundary dilation rule to obtain a preprocessed map.

[0149] Here, the smoothing process can adopt any smoothing method, such as L1 norm smoothing, weighted least squares method, or by reducing the size, median filtering, deleting points that are very close, and using Bezier curve formula to smooth the edge. There is no restriction on the implementation method.

[0150] In addition, developers can set boundary expansion rules in advance based on the robot's ultrasonic error and positioning error. For example, when the robot is sensitive to boundaries, if the area near the boundary is not allowed to pass, the boundary expansion rule can be to shrink the map. On the contrary, if the robot is required to visit every location on the map as much as possible, and can dynamically avoid obstacles based on the real-time map during subsequent operation, the boundary expansion rule can be to expand the map.

[0151] Thus, the process of border expansion can be: when the border expansion rule is to reduce the map, the map border is reduced inward according to the preset reduction radius; when the border expansion rule is to expand the map, the map border is expanded outward according to the preset expansion radius.

[0152] Step 156, setting the robot coordinates on the preprocessed map to obtain an environment map.

[0153] Through the above steps 151 to 156, an environment map adapted to the robot's passage requirements is obtained quickly and accurately.

[0154] After the above steps 151 to 156 are used at the start time to construct an environmental map of the robot's environment, any planning method can be used to obtain a planned path for the robot on the environmental map, and the robot can be controlled to move according to the planned path.

[0155] The process of controlling the robot movement in step 15 may be: determining a target point in the environment map, taking the robot position in the environment map as the starting point, using the A* algorithm to obtain a planned path, and then controlling the robot to move according to the planned path. The target point may be a manually set point or a point selected by the robot in the environment map. For example, when the robot is a lawn mowing robot, a preset lawn mowing end point may be used as the target point.

[0156] It should be noted that after determining the target point and the starting point, the fast expanding random tree algorithm can be used for planning to obtain the planned path.

[0157] After the environment map is constructed at the start time, the host continuously executes steps 11 to 13 in a loop to obtain the robot position and obstacle position at the subsequent detection time. For example, according to t 1 , t 2 ,…,t n The measurement array at the moment, construct t 1 The environment map at the moment, and after starting to move according to the planned path, according to t 2 , t 2 ,…,t n+1 The measurement array at time t 2 The robot position and obstacle position at the moment are used to update the environment map. Then, according to t 3 , t 2 ,…,t n+2 The measurement array at time t 3 The environment map is updated based on the robot position and obstacle position at the moment. In this way, the above process is continuously repeated to execute steps 11 to 17 to continuously update the environment map at each moment, and the robot moves to avoid obstacles according to the updated map until it reaches the target point and the operation is completed.

[0158] Among them, the method of updating the map in step 17 can be flexibly selected. For example, the update can be completed by directly replacing the robot position and obstacle position in the environment map with the current latest robot position and obstacle position. It can also be updated according to preset rules, and its implementation method is not restricted.

[0159] In actual applications, ultrasonic waves may have errors, and for some obstacles, ultrasonic information cannot obtain their full picture. Therefore, in the subsequent moments, if the robot movement is controlled based on the environment map built at the start moment, it is very easy to cause dangerous situations such as collisions due to obstacle disturbances or ultrasonic errors that do not obtain the full picture. Therefore, the idea of ​​filtering obstacles and expanding boundaries is introduced in the process of updating the map in step 17. Figure 7 , the process of updating the map includes steps 171 to 177.

[0160] Step 171, updating the map value of each position point according to the coverage between each obstacle position and each position point in the environment map.

[0161] It should be understood that the environment map is composed of a plurality of location points (which can be understood as unit grids).

[0162] In the above step 171, the preset map resolution is multiplied by the current obstacle position to obtain the latest obstacle coordinates. Then, the location points (i.e., unit grids) where the environment map coincides with the latest obstacle coordinates are determined, and the map values ​​of these location points are increased by the preset values. This process can be expressed as follows: k =obstacle k-1 +τ, where τ represents the preset value, obstacle k-1 Indicates the map value at the previous moment, obstacle k Represents the updated map value.

[0163] For the locations that do not coincide with the obstacle coordinates, the map values ​​of these locations are subtracted from the preset values. This process can be expressed by the formula: obstacle k =obstacle k-1 -τ.

[0164] It should be noted that initially, the map values ​​of all location points are all initial values, that is, they are the same.

[0165] Step 173: The position point whose current map value is greater than the obstacle threshold is regarded as an obstacle point.

[0166] The essence of the above steps 171 and 173 is: according to the latest obstacle position at each moment, the map value of each position point in the environment map is continuously updated, so that in the latest environment map, only the position point whose map value is greater than the obstacle threshold is used as a position point. That is, the same position point will be used as an obstacle point only when it coincides with the obstacle coordinates at multiple consecutive moments. In this way, the obstacles that are mistakenly identified and dynamic obstacles can be filtered out, and the recognition accuracy of obstacles can be improved.

[0167] Step 175 , according to the preset expansion radius, all expansion points of each obstacle point are determined from the environment map.

[0168] Step 177, updating obstacle points and expansion points on the environment map to obtain a new environment map.

[0169] In the above steps 175 and 177, the points around the obstacle point whose distance from the obstacle point is less than the preset expansion radius are used as the expansion points of the obstacle point to expand the obstacle point. Figure 8 The process of determining all expansion points of the obstacle point includes the following steps 1751 to 1759.

[0170] Step 1751, taking the obstacle point as its own expansion origin, and putting the obstacle point into the first set.

[0171] Step 1753: extract a location point from the first set as a point to be measured, and put the point to be measured into the second set.

[0172] The first set may be a set set, which represents a set of location points to be measured, and the second set may be a visited set, which is used to place points in the first set that have been visited and processed.

[0173] Step 1755, calculate the distance between the measured point and the expansion origin of the measured point, and determine whether the distance is less than the expansion radius. If yes, execute step 1757, if not, execute step 1759.

[0174] Step 1757, update the map value of the point to be measured, select the position point closest to the point to be measured and not in the second set, put it into the first set as a new point, and use the expansion origin of the point to be measured as the expansion origin of the new point. If the first set is not empty, return to step 1753.

[0175] Step 1759, update the map value of the point to be measured. After step 1759, if the second set is not empty, return to step 1753, otherwise end the process.

[0176] For example, assuming that the point to be measured is the obstacle point (0, 0), the expansion origin of the obstacle point (0, 0) is extracted as the point to be measured, and (0, 0) is placed in the second set. Thus, the distance between the point to be measured (0, 0) and its expansion origin (i.e., the point (0, 0) itself) is calculated to be 0, which must be smaller than the expansion radius, so step 1757 is executed: the map value of the point (0, 0) is updated, and the position points closest to the point to be measured and not in the second set are (0, 1), (0, -1), (1, 0), (-1, 0), and the new points are placed in the first set, and the expansion origins of these new points are consistent with the expansion origin of the point to be measured (0, 0), all of which are (0, 0). Furthermore, since the first set is not empty, the execution of step 1753 is continued until the first set is empty.

[0177] Among them, in order to facilitate the subsequent movement and obstacle avoidance of the robot, the process of updating the map value of the test point in the above steps 1757 and 1759 is introduced based on the spacing, setting an expansion value inversely proportional to the spacing, and taking the maximum value between the new expansion value and the original map value as the idea of ​​the latest map value of the test point.

[0178] Specifically, when the spacing is not less than the radius of the robot, the expansion value of the point to be measured is reduced by a preset value to obtain a base value, and the attenuation rate is obtained according to the spacing and the radius of the robot, and the product of the base value and the attenuation rate is used as the new expansion value of the point to be measured; when the spacing is less than the radius of the robot and the spacing is not zero, the expansion value of the point to be measured is updated according to the preset attenuation value; when the spacing is equal to zero, the expansion value of the point to be measured is set to the maximum value; from the current expansion value of the point to be measured and the historical map value, the maximum value is selected as the current map value of the point to be measured.

[0179] The attenuation value may be 1 or any other value, which is not limited here.

[0180] When the spacing is not less than the radius of the robot, the new expansion value of the point to be measured can be expressed by the following formula: k =(inflate k-1 -decay)*exp(-1×γ)×(dr), where γ is the preset weight, d is the spacing, r is the robot radius, decay is the decay value, inflation k-1 Indicates the previous expansion value of the point to be tested, inflation k Indicates the new expansion value of the point to be tested.

[0181] When the spacing is smaller than the radius of the robot and is not zero, the new expansion value of the point to be measured can be expressed as: inflation k =(inflate k-1-decay). When the spacing is equal to zero, the new expansion value of the point to be tested can be expressed as: inflate k =inflate max , inflate max Indicates the maximum value of the expansion.

[0182] It should be noted that the map value represents the probability of an obstacle. The larger the map value of the location point, the greater the probability that the location point is an obstacle, and the more likely the robot will avoid the location point when walking. That is, in step 17, the robot can determine whether it needs to be avoided based on the map value of each location point in the updated map. For example, if the map value is greater than the avoidance threshold, it needs to be avoided. When planning the path, these points are directly regarded as inaccessible areas, otherwise there is no need to avoid them. In this way, during the path adjustment and movement of the robot after the map is updated, there is no need to calculate whether it will collide with obstacles, which reduces the amount of calculation and improves the safety of the robot's walking.

[0183] The obstacle point filtering and expansion methods in the above steps 171 to 177 and their related sub-steps can also be applied to the construction of the environment map in step 15. That is, after the environment map is obtained in step 15, the above steps 171 to 177 and their related sub-steps can be used to filter and expand the obstacle points, and then perform path planning.

[0184] In addition, for each established map boundary and the environment map that is dynamically updated in real time according to obstacles during operation, the robot host 130 can set the corresponding Name for database caching and indexing. The information recorded in the database includes but is not limited to: the last update time, the original boundary map Name (empty if there is none), the map size, the map area, etc. When the robot starts to perform a task in an area where a map has been built, the map loaded by the specified Name can be directly loaded from the database and used without building the map again.

[0185] Based on the same inventive concept as the above-mentioned ultrasonic mapping and positioning method, refer to Fig. 9 The embodiment of the present invention also provides an ultrasonic mapping and positioning device 30, which can be applied to Figure 1 The host 130 and the ultrasonic mapping and positioning device 30 in the ultrasonic mapping and positioning system 10 include a data sampling module 310 , an obstacle acquisition module 320 , a map building module 330 and a map updating module 340 .

[0186] The data sampling module 310 is used to continuously sample the inertial measurement unit and each ultrasonic detection device to obtain a plurality of continuous measurement arrays.

[0187] The multiple measurement arrays include measurement arrays at the target moment and multiple detection moments after the target moment, and the measurement arrays include a distance matrix and the position and posture of the robot.

[0188] The obstacle acquisition module 320 is used to obtain the robot position at the target time according to each measurement array, and to obtain the obstacle position when the robot is located at the robot position according to the robot position and the measurement array.

[0189] The map building module 330 is used to build an environment map of the robot's environment according to the obstacle positions when the target time is the start time, and control the robot's movement according to the environment map, wherein the environment map includes a map boundary.

[0190] The map updating module 340 is used to update the map according to the obstacle position and the environment map when the target time is any time after the start time, and perform motion obstacle avoidance according to the updated map.

[0191] The above-mentioned ultrasonic mapping and positioning device 30, under the coordinated action of the data sampling module 310, the obstacle acquisition module 320, the map building module 330 and the map updating module 340, can achieve positioning and mapping through the coordination of ultrasonic detection data and IMU measurement data. The positioning and mapping process is not affected by various environmental factors such as light, and there is no need to use high-cost devices such as laser detectors. It can also be adapted to various scenarios, thereby meeting the cost, accuracy and versatility requirements of robot mapping at the same time.

[0192] For the specific implementation and effect of the ultrasonic mapping and positioning device 30, please refer to the description of the implementation of the ultrasonic mapping and positioning method above, such as the specific implementation and effect of the sampling module, please refer to the description of the relevant content of step 11 above, the specific implementation and effect of the obstacle acquisition module 320, please refer to the description of the relevant content of step 13 above, the specific implementation and effect of the map building module 330, please refer to the description of the relevant content of step 15 above, and the specific implementation and effect of the map updating module 340, please refer to the description of the relevant content of step 17 above, which will not be repeated here.

[0193] In addition, each module of the ultrasonic mapping and positioning device 30 can be implemented in whole or in part by software, hardware, or a combination thereof. The above modules can be embedded in or independent of the processor 220 in the electronic device 20 in the form of hardware, or can be stored in the memory 210 of the electronic device 20 in the form of software, so that the processor 220 can call and execute the operations corresponding to the above modules to implement the ultrasonic mapping and positioning method provided above.

[0194] Based on the same inventive concept as the above-mentioned ultrasonic mapping and positioning method, an embodiment of the present invention also provides a robot, including a main body, and a host 130, an inertial measurement unit 120 and a plurality of ultrasonic detection devices 140 installed on the main body, wherein the plurality of ultrasonic detection devices 140 are arranged at different positions of the main body, and the host 130 is respectively communicated with the inertial measurement unit 120 and the ultrasonic detection device 140.

[0195] The inertial measurement unit 120 is used to obtain the current position and posture of the robot in real time.

[0196] The ultrasonic detection device 140 is used to emit ultrasonic waves in real time to detect the distance of obstacles.

[0197] The host 130 is used to implement the ultrasonic mapping and positioning method provided above.

[0198] The embodiment of the present invention also provides an electronic device 20, including a processor 220 and a memory 210, wherein the memory 210 stores a computer program executable by the processor 220, and the processor 220 can execute the computer program to implement the ultrasonic mapping and positioning method proposed in the embodiment of the present invention.

[0199] The embodiment of the present invention further provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed by the processor 220, the ultrasonic mapping and positioning method proposed in the embodiment of the present invention is implemented.

[0200] In summary, the ultrasonic mapping and positioning method, device, robot, and electronic device provided by the embodiments of the present invention have at least the following beneficial effects:

[0201] (1) The positioning and mapping process will not be affected by the lighting environment, the cost is lower, and it can be adapted to all scenarios with decimeter-level accuracy requirements;

[0202] (ii) Ultrasonic waves can be used to identify key obstacles in an area, and the boundaries of the area can be determined based on the key obstacles. At the same time, the errors caused by ultrasonic waves in actual applications can be weakened through obstacle filtering and boundary expansion, helping robots to travel along a given route within a certain area and automatically avoid obstacles, so that the mapping and positioning solution can achieve the same effect as the traditional SLAM solution with the help of ultrasonic waves alone.

[0203] In several embodiments provided by the present invention, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely schematic. For example, the flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to multiple embodiments of the present invention. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and the module, a program segment or a part of a code contains one or more executable instructions for implementing the specified logical functions. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, and the combination of boxes in the block diagram and / or flowchart can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0204] In addition, the functional modules in the various embodiments of the present invention may be integrated together to form an independent part, or each module may exist independently, or two or more modules may be integrated to form an independent part.

[0205] If the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory, a random access memory, a magnetic disk or an optical disk.

[0206] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. An ultrasonic mapping and positioning method, characterized in that: A host applied to a robot, the robot is also provided with an inertial measurement unit and a plurality of ultrasonic detection devices in different orientations, the host is respectively connected to the inertial measurement unit and each of the ultrasonic detection devices in communication, the method comprising: Continuously sampling the inertial measurement unit and each of the ultrasonic detection devices respectively to obtain a plurality of continuous measurement arrays; wherein the plurality of measurement arrays include measurement arrays at a target moment and at a plurality of detection moments after the target moment; According to each of the measurement arrays, the robot position at the target moment is obtained, and according to the robot position and the measurement array, the obstacle position when the robot is located at the robot position is obtained; When the target time is the starting time, constructing an environment map of the environment in which the robot is located according to the position of the obstacle, and controlling the movement of the robot according to the environment map; When the target time is any time after the start time, the map is updated according to the obstacle position and the environment map, and the motion obstacle avoidance is performed according to the updated map.

2. The ultrasonic mapping and positioning method according to claim 1, characterized in that: The measurement array includes a distance matrix and a position and posture of the robot; The step of obtaining the robot position at the target moment according to each of the measurement arrays comprises: For the measurement array at each detection moment, multiply the inverse matrix of the pose at the target moment by the pose and distance matrix in the measurement array to obtain the estimated pose of the robot in the target coordinate system at the detection moment; wherein the target coordinate system is the world coordinate system where the robot is located at the target moment; Inputting each of the estimated postures into a preset disturbance analysis model, and generating a disturbance variance using the disturbance analysis model; wherein the disturbance analysis model is a model obtained by fitting a historical measurement array of the robot; Based on the disturbance variance and each of the estimated poses, fusing the multiple estimated poses with Gaussian distribution to obtain a fused pose; Generate a Gaussian distribution perturbation set with a mean of zero; wherein the Gaussian distribution perturbation set includes multiple perturbation values; For each disturbance value, the robot position, motion command control amount and disturbance value of the robot at the last moment are integrated to obtain an estimated value; The error between the fused pose and each of the estimated values ​​is calculated, and the estimated value with the smallest error is used as the robot position at the target moment.

3. The ultrasonic mapping and positioning method according to claim 1, characterized in that: The measurement array includes the distance of obstacles detected by each of the ultrasonic detection devices; The step of obtaining the obstacle position when the robot is located at the robot position according to the robot position and the measurement array comprises: taking the measurement array at the target time as a target measurement group; For each obstacle distance in the target measurement group, an obstacle position at the target moment is obtained by combining the robot position, the obstacle distance, and a setting angle of the ultrasonic detection device for obtaining the obstacle distance.

4. The ultrasonic mapping and positioning method according to any one of claims 1 to 3, characterized in that: The step of constructing an environment map of the environment in which the robot is located according to the obstacle positions comprises: Multiplying the preset map resolution by the robot position and the obstacle position respectively to obtain the robot coordinates and the obstacle coordinates on the map; According to the adjacent relationship of the coordinates, the coordinates of the obstacles with adjacent coordinates are divided into a set to obtain a plurality of obstacle sets; Eliminating the obstacle sets that do not form a closed loop area from the plurality of obstacle sets, and obtaining the area of ​​the closed loop area formed by each of the remaining obstacle sets; Connecting the coordinates of each obstacle in the obstacle set with the largest area in sequence to obtain a map boundary; Smoothing the map boundary, and dilating the smoothed map boundary according to a preset boundary dilation rule to obtain a preprocessed map; The robot coordinates are set on the preprocessed map to obtain an environment map.

5. The ultrasonic mapping and positioning method according to claim 4, characterized in that: The step of dilating the smoothed map boundary according to a preset boundary dilation rule to obtain a preprocessed map comprises: When the boundary expansion rule is to reduce the map, the map boundary is reduced inward according to a preset reduction radius; When the boundary expansion rule is to expand the map, the map boundary is expanded outward according to a preset expansion radius.

6. The ultrasonic mapping and positioning method according to any one of claims 1 to 3, characterized in that: The step of updating the map according to the obstacle position and the environment map comprises: According to the coverage between each obstacle position and each position point in the environment map, updating the map value of each position point; The location point whose current map value is greater than the obstacle threshold is regarded as an obstacle point; According to a preset expansion radius, all expansion points of each obstacle point are determined from the environment map; On the environment map, the obstacle points and the expansion points are updated to obtain a new environment map.

7. The ultrasonic mapping and positioning method according to claim 6, characterized in that: The step of determining all expansion points of each obstacle point from the environment map according to a preset expansion radius comprises: Taking the obstacle point as its own expansion origin, and putting the obstacle point into the first set; Extract a position point from the first set as a point to be measured, and put the point to be measured into a second set; Calculating the distance between the point to be measured and the expansion origin of the point to be measured, and determining whether the distance is less than the expansion radius; If yes, then update the map value of the point to be measured, select the position point that is closest to the point to be measured and is not in the second set, put it into the first set as a new point, use the expansion origin of the point to be measured as the expansion origin of the new point, and return to execute the steps of extracting a position point from the first set as the point to be measured and putting the point to be measured into the second set; If not, the map value of the point to be measured is updated, and when the second set is not empty, the process returns to the step of extracting a location point from the first set as the point to be measured and putting the point to be measured into the second set.

8. The ultrasonic mapping and positioning method according to claim 7, characterized in that: The step of updating the map value of the point to be measured comprises: When the spacing is not less than the radius of the robot, the expansion value of the point to be measured is reduced by a preset value to obtain a base value, and a decay rate is obtained according to the spacing and the radius of the robot, and the product of the base value and the decay rate is used as a new expansion value of the point to be measured; When the distance is smaller than the radius of the robot and the distance is not zero, updating the expansion value of the point to be measured according to a preset attenuation value; When the spacing is equal to zero, the expansion value of the point to be measured is set to a maximum value; From the current expansion value and the historical map value of the point to be measured, a maximum value is selected as the current map value of the point to be measured.

9. An ultrasonic mapping and positioning device, characterized in that: A host applied to a robot, wherein the robot is also provided with an inertial measurement unit and a plurality of ultrasonic detection devices in different orientations, wherein the host is respectively connected to the inertial measurement unit and each of the ultrasonic detection devices in communication, and the ultrasonic mapping and positioning device comprises a data sampling module, an obstacle acquisition module, a map building module and a map updating module; The data sampling module is used to continuously sample the inertial measurement unit and each of the ultrasonic detection devices to obtain a plurality of continuous measurement arrays; wherein the plurality of measurement arrays include measurement arrays at a target moment and at a plurality of detection moments after the target moment, and the measurement arrays include a distance matrix and the position and posture of the robot; The obstacle acquisition module is used to obtain the robot position at the target moment according to each of the measurement arrays, and to obtain the obstacle position when the robot is located at the robot position according to the robot position and the measurement array; The map building module is used to build an environment map of the environment where the robot is located according to the position of the obstacle when the target time is the starting time, and control the movement of the robot according to the environment map; The map updating module is used to update the map according to the obstacle position and the environment map when the target moment is any moment after the start moment, and to perform motion obstacle avoidance according to the updated map.

10. A robot, characterized in that: It comprises a body, and a host, an inertial measurement unit and a plurality of ultrasonic detection devices installed on the body, wherein the plurality of ultrasonic detection devices are arranged at different positions of the body, and the host is respectively connected to the inertial measurement unit and the ultrasonic detection device for communication; The inertial measurement unit is used to obtain the current position and posture of the robot in real time; The ultrasonic detection device is used to emit ultrasonic waves in real time to detect the distance of obstacles; The host is used to implement the ultrasonic mapping and positioning method according to any one of claims 1 to 8.

11. An electronic device, characterized in that: The method comprises a processor and a memory, wherein the memory stores a computer program executable by the processor, and the processor can execute the computer program to implement the ultrasonic mapping and positioning method according to any one of claims 1 to 8.

Images