CN117968657A - Repositioning method, repositioning device and readable storage medium - Google Patents
Repositioning method, repositioning device and readable storage medium Download PDFInfo
- Publication number
- CN117968657A CN117968657A CN202211306307.5A CN202211306307A CN117968657A CN 117968657 A CN117968657 A CN 117968657A CN 202211306307 A CN202211306307 A CN 202211306307A CN 117968657 A CN117968657 A CN 117968657A
- Authority
- CN
- China
- Prior art keywords
- self
- mobile device
- beacon
- coordinate system
- theoretical
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 80
- 230000009471 action Effects 0.000 claims abstract description 82
- 238000001514 detection method Methods 0.000 claims description 36
- 230000033001 locomotion Effects 0.000 claims description 17
- 239000011159 matrix material Substances 0.000 claims description 15
- 230000009466 transformation Effects 0.000 claims description 15
- 238000004590 computer program Methods 0.000 claims description 12
- 230000007246 mechanism Effects 0.000 claims description 9
- 238000012545 processing Methods 0.000 claims description 7
- 239000000523 sample Substances 0.000 claims 1
- 239000000758 substrate Substances 0.000 claims 1
- 230000000712 assembly Effects 0.000 abstract description 7
- 238000000429 assembly Methods 0.000 abstract description 7
- 238000006073 displacement reaction Methods 0.000 description 20
- 238000000354 decomposition reaction Methods 0.000 description 19
- 238000010586 diagram Methods 0.000 description 18
- 230000008569 process Effects 0.000 description 11
- 238000005259 measurement Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 4
- 239000000428 dust Substances 0.000 description 4
- 230000002159 abnormal effect Effects 0.000 description 3
- 238000013473 artificial intelligence Methods 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000004140 cleaning Methods 0.000 description 2
- 238000012423 maintenance Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000005856 abnormality Effects 0.000 description 1
- 230000006978 adaptation Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 239000003638 chemical reducing agent Substances 0.000 description 1
- 239000000470 constituent Substances 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005507 spraying Methods 0.000 description 1
- 238000010408 sweeping Methods 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
A repositioning method, apparatus, and readable storage medium perform a first advancement from a mobile device to advance from a first location to a second location relative to a target beacon assembly, determining N first theoretical position coordinates associated with the first location and N second theoretical position coordinates associated with the second location. Then, a second travel action is performed to travel from the second location to a third location, and N third theoretical position coordinates associated with the third location are determined according to a second mileage from the second location to the third location. And then, determining a first actual position coordinate according to third ranging information and N third theoretical position coordinates in the third position, and repositioning according to the actual position coordinate of the first position in a beacon coordinate system and the like. By adopting the scheme, the self-mobile device can finish relocation by only relying on one beacon assembly, and the relocation method is simple and low in cost without relying on a plurality of beacon assemblies or absolute positioning information such as GPS (global positioning system), RTK (real time kinematic) and the like.
Description
Technical Field
The present application relates to the field of artificial intelligence technology, and in particular, to a relocation method, apparatus, and readable storage medium.
Background
With the development of artificial intelligence (ARTIFICIAL INTELLIGENCE, AI) technology, various self-mobile devices are increasingly entering people's lives, such as logistics robots, sweeping robots, mowing robots, welcome robots, and the like.
In the moving process of the self-mobile device, the self-mobile device needs to be constantly positioned according to the environment map so as to plan or adjust the travelling path and the like.
When the self-mobile device is moved, restarted after shutdown, and the like, the positioning of the self-mobile device is lost, so that the self-mobile device cannot be positioned, and further the self-mobile device cannot work.
Disclosure of Invention
The application provides a repositioning method, equipment and a readable storage medium, wherein the repositioning mode is simple and has low cost.
In a first aspect, an embodiment of the present application provides a repositioning method for a self-mobile device that moves and works in a working area, where the self-mobile device is provided with a detection component, the method includes:
Controlling the self-mobile device to perform a first advancement to advance the self-mobile device from a first location to a second location relative to a target beacon component, the target beacon component being any one of the beacon components detected by the detection component;
According to first ranging information and second ranging information between the target beacon component and the self-mobile device, N first theoretical position coordinates of the self-mobile device, which are associated with the first position, and N second theoretical position coordinates of the self-mobile device, which are associated with the second position, are respectively obtained under a beacon coordinate system, wherein N is greater than or equal to 1, and N is a constant;
Controlling the self-moving device to perform a second travel action from the second location to cause the self-moving device to travel from the second location to a third location;
Acquiring N third theoretical position coordinates of the self-mobile device associated with the third position under a beacon coordinate system according to a second mileage and N second theoretical position coordinates, wherein the second mileage is a travelling mileage of the self-mobile device from the second position to the third position;
Acquiring a first actual position coordinate of the first position under the beacon coordinate system according to third ranging information and N third theoretical position coordinates when the target beacon component and the self-mobile device are positioned at a third position;
And acquiring the pose of the self-mobile device when the self-mobile device is positioned at the third position under the world coordinate system according to the first actual position coordinate of the first position under the beacon coordinate system, the position coordinate of the target beacon component under the world coordinate system, the first course angle of the self-mobile device for executing the first running action and the first mileage of the self-mobile device reaching the third position from the first position, wherein the first course angle is the included angle between the running direction of the self-mobile device for executing the first running action and the X W axis of the world coordinate system.
In a second aspect, an embodiment of the present application provides a self-mobile device, including:
a housing;
The travelling mechanism is used for supporting the shell and driving the self-moving equipment to move in a working area;
An actuator mounted on the housing for operation within the work area;
The detection assembly is arranged in the shell and is used for detecting the beacon assembly of the working area and measuring the distance;
a magnetometer mounted within the housing for determining the angle of rotation of the self-moving device;
An odometer mounted within the housing for detecting a range of travel of the self-mobile device;
a memory disposed on the housing for storing a computer program, the computer program comprising instructions;
a processor disposed on the housing and coupled with the memory, the processor executing the instructions to control the travel mechanism, the actuator, the detection assembly, the magnetometer, and the odometer to implement the method as described above in the first aspect or various possible implementations of the first aspect.
In a third aspect, an embodiment of the present application provides a relocating device including:
A first execution module for controlling the self-mobile device to execute a first travel action to cause the self-mobile device to travel from a first location to a second location relative to a target beacon component, the target beacon component being any one of the beacon components detected by the detection component;
The first acquisition module is used for respectively acquiring N first theoretical position coordinates of the self-mobile device associated with the first position under a beacon coordinate system and N second theoretical position coordinates associated with the second position according to first ranging information and second ranging information between the target beacon component and the self-mobile device, wherein N is greater than or equal to 1 and is a constant;
A second execution module for controlling the self-mobile device to execute a second traveling action from the second location to cause the self-mobile device to travel from the second location to a third location;
A second obtaining module, configured to obtain N third theoretical position coordinates of the self-mobile device associated with the third position under a beacon coordinate system according to a second mileage and N second theoretical position coordinates, where the second mileage is a travelling mileage of the self-mobile device from the second position to the third position;
The third acquisition module is used for acquiring a first actual position coordinate of the first position under the beacon coordinate system according to third ranging information and N third theoretical position coordinates when the target beacon component and the self-mobile device are positioned at a third position;
The processing module is used for acquiring the pose of the self-mobile device when the self-mobile device is located at the third position under the world coordinate system according to the first actual position coordinate of the first position under the beacon coordinate system, the position coordinate of the target beacon component under the world coordinate system, the first course angle of the self-mobile device for executing the first running action and the first mileage of the self-mobile device reaching the third position from the first position, wherein the first course angle is the included angle between the running direction of the self-mobile device for executing the first running action and the X W axis of the world coordinate system.
In a fourth aspect, embodiments of the present application provide a computer readable storage medium having stored therein computer instructions which, when executed by a processor, are adapted to carry out the method according to the first aspect or the various possible implementations of the first aspect.
In a fifth aspect, embodiments of the present application provide a computer program product comprising a computer program which, when executed by a processor, implements the method as described above in the first aspect or in the various possible implementations of the first aspect.
The embodiment of the application provides a repositioning method, equipment and a readable storage medium, wherein a detection component is arranged on mobile equipment, and a beacon component is arranged in a working area. When relocation is required, a first traveling action is performed from the mobile device to travel from a first location to a second location relative to the target beacon assembly, determining N first theoretical location coordinates associated with the first location and N second theoretical location coordinates associated with the second location. And after the first traveling action is executed, executing a second traveling action to travel from the second position to the third position, and determining N third theoretical position coordinates associated with the third position according to the second mileage from the second position to the third position. And then, according to third ranging information and N third theoretical position coordinates in the third position, determining a first actual position coordinate from N first theoretical position coordinates, and further according to the actual position coordinate of the first position in a beacon coordinate system and the like, determining the position coordinate of the third position in a world coordinate system and the course angle of the self-mobile equipment in the third position so as to finish repositioning. By adopting the scheme, the self-mobile device can finish relocation by only relying on one beacon assembly, and the relocation method is simple and low in cost without relying on a plurality of beacon assemblies or absolute positioning information such as GPS (global positioning system), RTK (real time kinematic) and the like.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for the description of the embodiments will be briefly described below, and it is apparent that the drawings in the following description are only some embodiments of the present application, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a schematic diagram of a conventional three-point positioning;
fig. 2 is a schematic structural diagram of an intelligent mower according to an embodiment of the present application;
FIG. 3 is a flow chart of a relocation method provided by an embodiment of the present application;
FIG. 4 is a schematic diagram of a target beacon component and a direction of motion of a self-mobile device in a world coordinate system in a repositioning method according to an embodiment of the present application;
fig. 5 is a schematic diagram of a beacon coordinate system in a relocation method according to an embodiment of the present application;
FIG. 6A is a schematic diagram of a path from a mobile device from a second location to a third location in a relocation method according to an embodiment of the present application;
FIG. 6B is another schematic diagram of a path from a mobile device from a second location to a third location in a relocation method according to an embodiment of the present application;
FIG. 6C is a schematic diagram of yet another path from a mobile device from a second location to a third location in a relocation method according to an embodiment of the present application;
FIG. 6D is a schematic diagram of yet another path from a mobile device from a second location to a third location in a relocation method according to an embodiment of the present application;
FIG. 7 is a flowchart of determining a first actual position coordinate in a repositioning method according to an embodiment of the present application;
FIG. 8 is another flow chart of a relocation method provided by an embodiment of the present application;
FIG. 9 is a further flowchart of a relocation method provided by an embodiment of the present application;
fig. 10 is a schematic diagram of a relocating device according to an embodiment of the present application.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the present application more apparent, the embodiments of the present application will be described in further detail with reference to the accompanying drawings.
After the robot enters a strange environment, an environment map is created by combining technologies such as instant positioning and map construction (Simultaneous Localization AND MAPPING, SLAM) and the like. The environment-based map is then positioned to plan or adjust the path and perform tasks.
If the robot is manually moved, the positioning of the robot is lost easily, so that the robot cannot be positioned. At this time, the robot is required to perform repositioning.
In a common repositioning method, a robot performs repositioning depending on absolute positioning information. The absolute positioning information comprises a global positioning system (Global Positioning System, GPS), a Real-time dynamic positioning (Real-TIME KINEMATIC, RTK), a visual map, a laser radar point cloud map, 3 or more Ultra Wide Band (UWB) base stations and the like.
The repositioning method depending on the absolute positioning information has high complexity and high cost.
In the mode based on UWB base station repositioning, the distance between the self-mobile device and at least 3 UWB base stations is determined, and then repositioning is performed according to the distances and the position coordinates of each UWB base station. This repositioning approach is also known as three-point positioning. For example, referring to fig. 1, fig. 1 is a schematic diagram of conventional three-point positioning.
Referring to fig. 1, black filled circles represent UWB1, UWB2 and UWB3, gray filled circles represent intelligent mowers, the moving tracks of the intelligent mowers are shown by black arrows in the figure, the positions of UWB1, UWB2 and UWB3 are known, the positions of the intelligent mowers are unknown, and the intelligent mowers determine the pose according to the distance r1 between the intelligent mowers and UWB1, the distance r2 between the intelligent mowers and UWB2 and the distance r3 between the intelligent mowers and UWB3, and the positions of UWB1, UWB2 and UWB3, so that repositioning is realized.
If the intelligent mower can only detect 1 or 2 UWB base stations, the intelligent mower cannot further determine the position of the intelligent mower, namely cannot realize repositioning. For example, although 3 or more UWB base stations are deployed around or within the operating region, only one or 2 of them can be detected by the self-mobile device, and relocation cannot be achieved at this time.
Based on the above, the embodiment of the application provides a repositioning method, a repositioning device and a readable storage medium, wherein the repositioning method is simple and has low cost.
In the embodiment of the application, the self-mobile device refers to electronic equipment which can autonomously move and can realize intelligent control. For example, the self-moving device is a mowing robot, and can automatically walk on a lawn to mow the lawn. For another example, the self-moving device is an automatic dust collector, and can automatically walk on the ground to perform dust collection work. For another example, the self-moving device is a cleaning robot, which can automatically walk on the ground, clean the floor and drag the floor at the same time.
It should be noted that the self-moving device according to the embodiment of the present application is not limited to a mowing robot, an automatic dust collector or a cleaning robot, but may be other types of devices, such as an automatic spraying device. By the self-mobile device, unattended operation of various works can be realized.
Next, the structure of the self-moving device will be described in detail using the self-moving device as an example of the intelligent mower. Fig. 2 is a schematic structural diagram of an intelligent mower according to an embodiment of the present application.
Referring to fig. 2, an intelligent mower 200 according to an embodiment of the present application includes: the housing 21, the travel mechanism 22 and the actuator 23 are provided on the housing 21.
In this case, the housing 21, also called a body, a casing, etc., and other constituent members may be provided on the surface or inside of the housing 21.
The travelling mechanism 22 comprises a driving wheel, a driven wheel and the like and is used for supporting the shell 21 and driving the intelligent mower to move in a working area.
An actuator 23 is mounted on the housing 21 for working in a working area. The travelling mechanism 22 drives the intelligent mower to move, and meanwhile, the executing mechanism 23 acts to achieve the mowing purpose.
The intelligent mower 200 further includes sensing components, magnetometers, and odometers, etc. sensors, not shown, mounted within the housing 21.
The detection component is, for example, a UWB base station, capable of detecting and ranging from beacon components of the work area. Multiple beacon components may be deployed for a work area, with the beacon components deployed inside, outside, or on the boundaries of the work area. For clarity, any one of the beacon components detected by the detection component is referred to as a target beacon component hereinafter.
During distance measurement, the intelligent mower 200 detects a distance signal emitted by the target beacon assembly by using the detection assembly, and determines the distance between the intelligent mower 200 and the target beacon assembly according to the distance signal; or the intelligent mower 200 transmits a distance signal to the target beacon assembly using the detection assembly and receives a feedback signal from the target beacon assembly, and determines the distance between the intelligent mower 200 and the target beacon assembly based on the feedback signal. When no shielding exists between the target beacon component and the detection component, the maximum ranging error is within 20 centimeters (cm), and the accuracy is high.
The magnetometer is, for example, an angle detector, a gyroscope, an inertial measurement unit (Inertial Measurement Unit, IMU) and the like, and functions similar to a compass, and can obtain magnetic field intensities in the three coordinate axes of the Xw axis, the Yw axis and the Zw axis in the world coordinate system, and after the intelligent mower 200 magnetically calibrates the magnetic field intensities, an accurate course angle can be obtained. The earth outdoors is not easily disturbed here compared with indoors, so that the heading angle calculated based on the magnetic force is more accurate and the repositioning accuracy is higher. Moreover, under outdoor working conditions, the condition of insufficient coverage of UWB base station signals easily occurs, and three-point positioning failure is caused. In the embodiment of the application, only one UWB base station, the magnetometer and the odometer are relied on, so that the repositioning success rate can be improved.
The odometer may be a displacement sensor, a wheel speed meter, also known as a mileage detector, an encoder, etc., and may be a wheel speed odometer, a visual odometer, etc. When the speedometer is used for measuring the wheel speed and the mileage, the speedometer is arranged on a driving motor or a speed reducer of the travelling mechanism 22, and the travelling mileage of the intelligent mower 200 is calculated by calculating the wheel speed and combining the wheel speed with the angle information of the intelligent mower 200, and the like, so that the calculation mode is simple and reliable. For example, a first mileage from a mobile device to a third location is measured with an odometer, and a second mileage from a mobile device to a third location is measured with an odometer. Therefore, the repositioning method provided by the embodiment of the application can realize repositioning by only relying on one beacon assembly, the magnetometer and the odometer, improves the success rate of positioning based on the beacon assembly, and has simple and reliable repositioning mode.
It will be appreciated by those skilled in the art that the configuration shown in fig. 2 is not limiting of the intelligent mower, and the intelligent mower may include more or fewer components than illustrated, or may combine certain components, or may have a different arrangement of components.
The repositioning method according to the embodiment of the present application will be described in detail below on the basis of the mower shown in fig. 2. For example, please refer to fig. 3.
Fig. 3 is a flowchart of a relocation method provided by an embodiment of the present application. The execution subject of the embodiment of the application is a self-mobile device with a detection component, and the embodiment comprises:
301. The method further includes controlling the self-mobile device to perform a first travel action to cause the self-mobile device to travel from a first location to a second location relative to a target beacon component, the target beacon component being any one of the beacon components detected by the detection component.
When the self-mobile device is manually moved, restarted and the like, and the self-mobile device is lost in positioning or abnormal in positioning, if the self-mobile device does not execute repositioning, the self-mobile device needs to reestablish an environment map, so that the defects of low operation speed and repeated operation of partial areas are caused. Or if no relocation is performed, causing the self-mobile device to stop the operation. Therefore, when the positioning is lost or abnormal, the self-mobile device executes the repositioning method disclosed by the embodiment of the application so as to determine the position, the orientation and the like of the self-mobile device, and further, the path planning and the operation are continued according to the environment map.
Scenarios in which relocation needs to be performed include, but are not limited to:
A. the mobile device is lifted and then placed on the ground.
In the travelling process of the mobile device, the mobile device is likely to be manually lifted and then lowered, the lifted position and the lowered position can be the same or different, and the time duration between the lifting and the lowering is not limited. In this case, the self-mobile device needs to perform relocation.
B. and restarting the mobile device after the mobile device is shut down, namely restarting the mobile device.
Typically, the self-moving device has a workstation, also known as a base station, a maintenance station, a charging dock, a charging post, a dust collection station, etc. The self-mobile device performs charging, maintenance and other tasks at the workstation. The self-moving device can determine its own position when in the workstation. If the self-mobile device is located at the workstation after each power-on, no relocation is required. If the self-mobile device is not located in the workstation after each power-on, the self-mobile device cannot determine the pose. In this case, the self-mobile device needs to perform relocation.
C. And locating the anomaly from the mobile device.
For example, an abnormality in positioning since the mobile device has moved for a long time, an inability to detect at least three beacon components at three-point positioning, and so on. At this time, relocation is performed from the mobile device.
In an embodiment of the application, at least one beacon component is deployed within the work area, near a boundary of the work area, or near an exterior of the work area. The location coordinates of the individual beacon components in the world coordinate system are pre-detected and stored from the mobile device. For example, a total of 3 beacon components are deployed, a list is stored on the self-mobile device indicating the location coordinates of each beacon component at world coordinates.
When relocation is required, the self-mobile device detects the beacon components by using the detection component to detect at least one beacon component, and any one of the detected at least one beacon component is taken as a target beacon component. Thereafter, a first travel action is performed from the mobile device to cause a change in its position relative to the target beacon assembly. For example from a first position to a second position.
302. And respectively acquiring N first theoretical position coordinates of the self-mobile device associated with the first position under a beacon coordinate system and N second theoretical position coordinates associated with the second position according to first ranging information and second ranging information between the target beacon component and the self-mobile device, wherein N is greater than or equal to 1, and N is a constant.
In an embodiment of the present application, the location coordinates of the target beacon component in the world coordinate system are known to the self-mobile device as (x wu,ywu). After the target beacon component is determined from the mobile device, a beacon coordinate system is constructed. The origin of the beacon coordinate system corresponds to the location coordinates of the target beacon component in the world coordinate system.
Although the position coordinate P 1(xwv1,ywv1 of the first position in the world coordinate system) is unknown, the position coordinate P 2(xwv2,ywv2 of the second position in the world coordinate system is also unknown. But based on the beacon coordinate system and the ranging information, the self-mobile device can determine possible positions of the first position, i.e., N first theoretical positions, and N second theoretical positions adjacent to the second position, in the beacon coordinate system. N is more than or equal to 1 and is an integer.
For example, when the self-mobile device is located at the first position, the detection component and the target beacon component on the self-mobile device communicate to obtain first ranging information, wherein the first ranging information comprises transmission speed, round trip time length and the like of a distance signal, and the distance signal is a laser or the like. N first theoretical position coordinates associated with the first position in the beacon coordinate system are obtained from the mobile device according to the first ranging information. In the beacon coordinate system, the first theoretical position coordinates are located on a circle taking the target beacon component as a center and taking the first distance as a radius. The first distance is the distance between the self-mobile device and the target beacon component when the self-mobile device is located at the first position, and the self-mobile device can calculate the first distance according to the first ranging information.
Similarly, when the self-mobile device is located at the second position, the detection component on the self-mobile device communicates with the target beacon component to obtain second ranging information, wherein the second ranging information comprises the transmission speed, the round trip time length and the like of the ranging signal. N second theoretical position coordinates associated with the second position in the beacon coordinate system are obtained from the mobile device according to the second ranging information. In the beacon coordinate system, the second theoretical position coordinates are located on a circumference taking the target beacon component as a center and taking the second distance as a radius. The second distance is the distance between the self-mobile device and the target beacon component when the self-mobile device is located at the second position, and the self-mobile device can calculate the second distance according to the second ranging information.
303. The self-mobile device is controlled to perform a second travel action from the second location to cause the self-mobile device to travel from the second location to a third location.
After the first travelling action is executed, the self-mobile device cannot determine the first actual position coordinates of the first position under the beacon coordinate system, but can determine a plurality of first theoretical position coordinates, wherein the first actual position coordinates are one of the plurality of first theoretical position coordinates. Thus, the self-mobile device needs to perform a second travel action.
A second travel action is performed from the mobile device to cause a change in its position relative to the target beacon component. For example from the second position to the third position.
304. And acquiring N third theoretical position coordinates of the self-mobile device associated with the third position under a beacon coordinate system according to a second mileage and N second theoretical position coordinates, wherein the second mileage is a travelling mileage of the self-mobile device from the second position to the third position.
And continuously acquiring the driving mileage by using an odometer and the like in the process of moving the mobile equipment from the second position to the third position, so as to acquire the driving mileage from the second position to the third position in the driving process, namely, the second mileage. Then, a possible third position, i.e. N third theoretical position coordinates, is determined from the second mileage and the N second theoretical position coordinates.
305. And acquiring a first actual position coordinate of the first position under the beacon coordinate system according to third ranging information and N third theoretical position coordinates when the target beacon component and the self-mobile device are positioned at a third position.
When the self-mobile device is located at the third position, the detection component on the self-mobile device is communicated with the target beacon component to obtain third ranging information, and the third ranging information comprises the transmission speed, the round trip time length and the like of the distance signal.
And then, the self-mobile device determines a first actual position coordinate of the first position under the beacon coordinate system from N first theoretical position coordinates according to the third ranging information and N third theoretical position coordinates.
306. And acquiring the pose of the self-mobile device when the self-mobile device is positioned at the third position under the world coordinate system according to the first actual position coordinate of the first position under the beacon coordinate system, the position coordinate of the target beacon component under the world coordinate system, the first course angle of the self-mobile device for executing the first running action and the first mileage of the self-mobile device reaching the third position from the first position.
Wherein the first course angle is an included angle between the advancing direction of the self-mobile device executing the first advancing action and the X W axis of the world coordinate system
In the embodiment of the application, the position of the mobile equipment needs to be obtained by the mobile equipment when the mobile equipment performs repositioning, and the position comprises position information and the gesture. The position information refers to position coordinates in a world coordinate system from a third position of the mobile device, and the gesture refers to a heading angle of the mobile device at the third position.
Repositioning is carried out by the self-mobile device according to the first actual position coordinate of the first position under the beacon coordinate system, the position coordinate of the target beacon component under the world coordinate system, the first course angle of the self-mobile device when the self-mobile device is at the first position, the first mileage of the self-mobile device reaching the third position from the first position, so that the position coordinate of the third position under the world coordinate system and the course angle of the self-mobile device when the self-mobile device is at the third position are determined, and then the repositioning is completed.
According to the repositioning method provided by the embodiment of the application, the detection component is arranged on the mobile equipment, and the beacon component is arranged in the working area. When relocation is required, a first traveling action is performed from the mobile device to travel from a first location to a second location relative to the target beacon assembly, determining N first theoretical location coordinates associated with the first location and N second theoretical location coordinates associated with the second location. And after the first traveling action is executed, executing a second traveling action to travel from the second position to the third position, and determining N third theoretical position coordinates associated with the third position according to the second mileage from the second position to the third position. And then, according to third ranging information and N third theoretical position coordinates in the third position, determining a first actual position coordinate from N first theoretical position coordinates, and further according to the actual position coordinate of the first position in a beacon coordinate system and the like, determining the position coordinate of the third position in a world coordinate system and the course angle of the self-mobile equipment in the third position so as to finish repositioning. By adopting the scheme, the self-mobile device can finish relocation by only relying on one beacon assembly, and the relocation method is simple and low in cost without relying on a plurality of beacon assemblies or absolute positioning information such as GPS (global positioning system), RTK (real time kinematic) and the like.
Optionally, in the above embodiment, when relocation is required, the number of beacon components detected by the detection component is determined from the mobile device. When 1 or 2 beacon components are detected, any one of the beacon components is taken as a target beacon component, and the relocation method according to the embodiment of the application is executed.
Illustratively, the detection component is, for example, a UWB base station or the like, capable of detecting and ranging a beacon component of the work area, the beacon component being deployed inside, outside, or on a boundary of the work area. In a conventional relocation scheme, the self-mobile device may implement the relocation according to at least three beacon components, as shown in fig. 1.
However, when the self-mobile device is operating outdoors, the beacon assembly cannot cover the operating area, and a phenomenon that the detection assembly cannot detect 3 or more beacon assemblies easily occurs. At this time, the self-mobile device cannot be repositioned by the three-point positioning process.
In an embodiment of the application, during relocation, the number of beacon components that can be detected is determined from the mobile device. When 1 or two beacon components are detected, namely that relocation can not be performed in a three-point positioning mode is detected, the self-mobile device performs relocation according to the relocation method provided by the embodiment of the application.
When the relocation condition is satisfied, if 3 or more beacon components can be detected from the mobile device, relocation is performed based on a three-point location manner.
By adopting the scheme, under the condition that the self-mobile device only detects one or two beacon assemblies, one beacon assembly is used as a target beacon assembly to further reposition, the limitation of the three-point positioning mode according to the positioning of at least three beacon assemblies is avoided, the mode is simple, the cost is low, and the repositioning success rate is improved.
Optionally, in the above embodiment, when relocation needs to be performed, the self-mobile device performs a calibration action to determine the first heading angle of the self-mobile device. That is, when the self-mobile device is lifted, restarted, etc. at the first location, resulting in a loss of positioning of the self-mobile device, and a repositioning needs to be performed, a calibration action is first performed by the self-mobile device to determine a first heading angle of the self-mobile device when in the first location. The first heading angle is also called an orientation angle and the like, and refers to an included angle between the motion direction of the mobile device and the X W axis of the world coordinate system. The direction of motion from the mobile device is typically the direction of a straight line pointing from the tail to the head of the mobile device.
By adopting the scheme, the self-mobile device executes the calibration action before executing the repositioning in the first position so as to determine the first course angle of the self-mobile device when the self-mobile device is in the first position, and the repositioning accuracy of the self-mobile device is improved.
Optionally, the calibration action is an action of rotating a preset angle in the first position, wherein the preset angle is greater than 90 °.
The magnetometer is calibrated from the mobile device at the first location to obtain a heading angle of the mobile device at the first location. To achieve calibration of the magnetometer of the self-moving device, the self-moving device is rotated at a first position by a preset angle, such as 90 degrees, 180 degrees, 360 degrees, etc., a circle is fitted according to the rotation angle, and calibration is performed on the magnetometer according to the fitted circle. Obviously, the circle fitted by the mobile device can be ensured to be more accurate by rotating at least 90 degrees at the first position, and the purpose of accurately determining the first course angle by the mobile device is realized.
Optionally, in the foregoing embodiment, before the self-mobile device performs the first travelling action to cause the self-mobile device to travel from the first position to the second position relative to the target beacon component, the beacon coordinate system is further constructed according to the first heading angle, and an origin of the beacon coordinate system corresponds to a position coordinate of the target beacon component under the world coordinate system.
In the embodiment of the application, when relocation is required to be executed, the self-mobile device takes any one of the detected beacon assemblies as a target beacon assembly, and constructs a beacon coordinate system by taking the position coordinate of the target beacon assembly under the world coordinate system as an origin. For example, referring to fig. 4, fig. 4 is a schematic diagram of a moving direction of a self-mobile device and a target beacon component in a world coordinate system in a repositioning method according to an embodiment of the present application.
Referring to fig. 4, the X-axis of the world coordinate system is denoted as Xw-axis, the Y-axis of the world coordinate system is denoted as Yw-axis, the X-axis of the beacon coordinate system is denoted as Xu-axis, the Y-axis of the beacon coordinate system is denoted as Yu-axis, the black filled circles in the figure represent the positions of the target beacon components in the world coordinate system, and the white filled circles represent the first positions of the self-mobile device in the world coordinate system.
In fig. 4, the position coordinates in the world coordinate system from the first position of the mobile device is P 1(xwv1,ywv1), the first position P 1(xwv1,ywv1) is unknown. The location coordinates of the target beacon component in the world coordinate system (x wu,ywu) are known from the direction of motion of the mobile device parallel to the Xu axis of the beacon coordinate system.
By adopting the scheme, the beacon coordinate system is constructed by taking the position coordinate of the target beacon component under the world coordinate system as the origin, so that the theoretical position coordinate of each position can be conveniently determined, and further, the repositioning is realized, the speed is high, and the accuracy is high.
Optionally, in the foregoing embodiment, in the process of acquiring N first theoretical position coordinates related to the first position and N second theoretical position coordinates related to the second position from the mobile device, first, determining a distance between the first position and the target beacon assembly according to first ranging information when the target beacon assembly and the self-mobile device are located at the first position; and determining the distance between the second position and the target beacon component according to the second ranging information when the target beacon component and the self-mobile device are located at the second position. And then, acquiring N first theoretical position coordinates associated with the first position and N second theoretical position coordinates associated with the second position according to the first distance, the second distance and a third distance between the first position and the second position by the self-mobile device.
Referring to fig. 4, the conditions that can be obtained from the mobile device in the first position are as follows:
a. a first distance R1 from the mobile device to the target beacon component when in the first position.
The self-mobile device detects the distance signal of the target beacon component by using the detection component, so as to obtain first distance measurement information, and further determines a first distance R1 according to the first distance measurement information. If the direction of motion of the self-mobile device is unknown, the first location is located on a circle with the origin as the center and the radius of R1 bit under the beacon coordinate system.
B. The direction of motion of the self-mobile device is parallel to the Xu axis of the beacon coordinate system.
The direction of travel from the mobile device is noted as the direction of a straight line pointing from the tail to the head.
From the above conditions a and b, it can be derived that:
The first theoretical position coordinates of the first position in the beacon coordinate system are P1 (x 1,y1) or P1' (x 1,-y1),x1 and y 1 are unknown, the two first theoretical position coordinates are two points which are symmetrical on the circumference with the origin as the center and the radius of the R1 bit, and the motion direction of the mobile device when the mobile device is positioned at the two positions is parallel to the Xu axis.
To determine the first theoretical position coordinates, i.e. to determine x 1 and y 1, a first movement is performed from the mobile device as an action such that the Xu coordinates of the first and second positions in the beacon coordinate system are different and the Yu coordinates are the same. For example, if the first theoretical position coordinate of the first position in the beacon coordinate system is P1 (x 1,y1) and the second theoretical position coordinate of the second position in the beacon coordinate system is P2 (x 2,y2), x 1 and x 2 are different, but y 1 and y 2 are the same. By adopting the scheme, the self-mobile device realizes the purpose of determining the theoretical position coordinate of the first position by executing the second action of enabling the Xu coordinates of the first position and the second position to be different and the Yu coordinates to be the same under the beacon coordinate system.
Fig. 5 is a schematic diagram of a beacon coordinate system in a relocation method according to an embodiment of the present application. Referring to fig. 5, when n=2, the 2 first theoretical position coordinates are P1 (x 1,y1) and P1' (x 1,-y1), respectively, and the traveling direction of the mobile device is parallel to the Xu axis of the beacon coordinate system, i.e. the straight line pointing from the tail to the head of the mobile device is parallel to the Xu axis.
The first travel action is performed from the first position by the mobile device, the second position is reached after the first travel action is performed, and 2 second theoretical position coordinates are respectively denoted as P2 (x 2,y2) and P2' (x 2,-y2),x1 and x 2 are different, but y 1 and y 2 are the same.
As shown in fig. 5, the self-moving device may travel along any of path ①, path ②, and path ③ to reach the second location from the first location. When the self-moving device moves along the path ①, the self-moving device moves along the path, which is equivalent to straight line, and the direction does not need to be adjusted, so that the purpose of quickly executing the first moving action is achieved. As the self-moving device travels along path ② or path ③, the direction needs to be adjusted during travel.
In fig. 5, the third distance d represents the distance between the first position and the second position.
The conditions that can be obtained from the mobile device when in the second position are as follows:
c. The second distance from the mobile device to the target beacon component when in the second position is R2.
D. a third distance d between the first position and said second position.
Referring to fig. 5, in performing the second action, the third distance d is equal to the travel distance of the self-mobile device when the self-mobile device travels along the path ①. When the self-moving device moves along the path ②, the moving mileage of the self-moving device is a semicircle with d as a diameter, and the third distance d can be determined according to the length of the semicircle. The third distance d is acquired from the mobile device according to a displacement sensor or the like as the mobile device travels along the path ③.
After the mobile device arrives at the second location, the origin, the first theoretical position coordinates P1 (x 1,y1) and the second theoretical position coordinates form a triangle OP1P2, and 2 first theoretical position coordinates P1 (x 1,y1) and P1 '(x 1,-y1), and 2 second theoretical position coordinates P2 (x 2,y2) and P2' (x 2,-y2) can be determined by solving the triangle and the like.
For example, from the mobile device according to the first theoretical position coordinate P1 (x 1,y1), the second theoretical position coordinate P2 (x 2,y2), the first distance R1, the second distance R2, and the third distance d, the following formula (1) can be obtained:
Equation (2), equation (3) and equation (4) are obtainable according to equation (1):
by adopting the scheme, the self-mobile device executes the first travelling action, and determines N first theoretical position coordinates and N second theoretical position coordinates according to the first ranging information at the first position and the second ranging information at the second position and the third distance between the first position and the second position, so that the speed is high and the accuracy is high.
And acquiring a pose transformation matrix of the self-mobile device moving from the second position to the third position according to the second mileage after acquiring a plurality of first theoretical coordinates of the first position and a plurality of second theoretical position coordinates of the second position from the mobile device. And then, acquiring N third theoretical position coordinates of the self-mobile device associated with the third position under a beacon coordinate system according to the pose transformation matrix and the N second theoretical position coordinates.
For example, the second traveling action is performed after the first traveling action is performed from the mobile device in order to determine a first actual position coordinate of the first position in the beacon coordinate system from the plurality of first theoretical position coordinates or in order to determine a second actual position coordinate of the second position in the beacon coordinate system from the plurality of second theoretical position coordinates. The second action is an action of making Yu coordinates of the second position and the third position in the beacon coordinate system different.
Referring to fig. 5, if a second theoretical position coordinate is P2 (x 2,y2), and a third position coordinate corresponding to the second theoretical position coordinate is P3 (x 3,y3), x 2 is the same as x 3, but y 2 is different from y 3. Similarly, when the second position coordinate is P2 '(x 2,-y2) and the third theoretical position coordinate is P3' (x 3,-y3), then x 2 and x 3 are the same, but-y 2 and-y 3 are different.
By adopting the scheme, the self-mobile device can realize the purpose of quickly and accurately determining the first actual position coordinate of the first position by executing the second traveling action which enables the Yu coordinates of the second position and the third position to be different under the beacon coordinate system.
The self-moving device may travel along any path such that y 2 and y 3 are different, -y 2 and-y 3 are different. Paths include, but are not limited to: the L-shaped path, the arc-shaped path, the connecting line of the path perpendicular to the first position and the second position, and the included angle between the path and the connecting line is larger than 0 degree. For example, please refer to fig. 6A-6D.
Fig. 6A is a schematic diagram of a path from a mobile device to a third location from a second location in a relocation method according to an embodiment of the present application. Referring to fig. 6A, the path ④ is an arc path, for example, a quarter arc.
Fig. 6B is another schematic diagram of a path from a mobile device to a third location from a second location in a relocation method according to an embodiment of the present application. Referring to fig. 6B, the path ⑤ is an L-shaped path.
Fig. 6C is a schematic diagram of yet another path from the mobile device from the second location to the third location in the relocation method according to the embodiment of the present application. Referring to fig. 6C, a path ⑥ is perpendicular to a line connecting the first position and the second position, and the first position is not shown.
Fig. 6D is a schematic diagram of a path from a mobile device to a third location from a second location in a relocation method according to an embodiment of the present application. Referring to fig. 6D, an included angle exists between the path ⑦ and a line connecting the first position and the second position, and the first position is not shown in the drawing.
By adopting the scheme, the self-mobile device flexibly reaches the third position from the second position, so that the flexibility is high and the mode is simple.
Fig. 7 is a flowchart of determining a first actual position coordinate in the repositioning method according to the embodiment of the present application. The embodiment comprises the following steps:
701. And acquiring a fourth distance according to third ranging information when the target beacon component and the self-mobile device are positioned at a third position.
Wherein the fourth distance R3 is a distance from the target beacon component when the self-mobile device is located at the third location. The fourth distance R3 may be derived from third ranging information between the beacon component and the detection component.
702. And acquiring N theoretical distances between the self-mobile device and the target beacon component according to the N third theoretical position coordinates.
Illustratively, the first theoretical position coordinates are P1 (x 1,y1) and P1' (x 1,-y1). If the mobile device starts from the first theoretical position coordinate P1 (x 1,y1), the second position is reached after the first action is performed. The second travel action is performed starting from the second position, thereby reaching the third position. In this process, the second theoretical position coordinate of the second position is P2 (x 2,y2), and the third theoretical position coordinate of the third position is P3 (x 3,y3). From the third theoretical position coordinate P3 (x 3,y3), a theoretical distance S 3 can be determined from the mobile device, which theoretical distance S 3 represents the distance from the third position P3 to the origin.
Similarly, if the mobile device starts from the first theoretical position coordinate P1' (x 1,-y1), the mobile device reaches the second position after the first action is performed. The second travel action is performed starting from the second position, thereby reaching the third position. In this process, the second theoretical position coordinate of the second position is P2 '(x 2,-y2), and the third theoretical position coordinate of the third position is P3' (x 3, -y 3). From the third theoretical position coordinates P3' (x 3,-y3), a theoretical distance S 3 ' can be determined from the mobile device, which theoretical distance S 3 ' represents the distance from the third position P3 to the origin.
703. And acquiring a first actual position coordinate of the first position under the beacon coordinate system according to the N theoretical distances and the fourth distance.
Referring to fig. 5, when the self-mobile device is located at the third position, the detection component detects the target beacon component, so as to determine the fourth distance R3. Then, a first difference between the theoretical distance S 3 and the fourth distance R3 and a second difference between the theoretical distance S 3' and the fourth distance R3 are determined from the mobile device, and a minimum difference is determined from the first difference and the second difference. For example, the first difference is the smallest, and since the first difference corresponds to the theoretical distance S 3, the theoretical distance S 3 corresponds to the third theoretical position coordinate P3 (x 3,y3), the first actual position coordinate of the first position is the first theoretical position coordinate P1 (x 1,y1). According to the above formula (2) and formula (4), it can be determined that the first actual position coordinate of the first position under the beacon coordinate system is P1 (x 1,y1) when the mobile device is located at the first position.
For another example, the second difference is the smallest, and since the second difference corresponds to the theoretical distance S 3 ', the theoretical distance S 3' corresponds to the third theoretical position coordinate P3 '(x 3,-y3), the first actual position coordinate of the first position is the first theoretical position coordinate P1' (x 1,-y1). According to the above formula (2) and formula (4), it can be determined that the first actual position coordinate of the first position under the beacon coordinate system is P1' (x 1,-y1) when the self-mobile device is located at the first position.
By adopting the scheme, the theoretical distance between the self-mobile device and the target beacon component when the self-mobile device is positioned at the third position is determined according to the theoretical position coordinates, and the theoretical distances and the fourth distance R3 are compared, so that the actual coordinate position of the first position under the beacon coordinate system is determined, and the speed is high and the accuracy is high.
Optionally, in the foregoing embodiment, in order to determine each theoretical distance, the self-mobile device determines, according to the second mileage, an Xu axis decomposition displacement m and a Yu axis decomposition displacement n of the self-mobile device under the beacon coordinate system after the self-mobile device reaches the third position from the second position. And then, determining the theoretical distance according to each second theoretical position coordinate, the Xu axis decomposition displacement and the Yu axis demarcation displacement by the self-moving device.
From the mobile device, the second theoretical position coordinate P2 (x 2,y2) and the second theoretical position coordinate P2' (x 2,-y2) can be determined according to the above formula (2) and formula (3). Further, after the self-mobile device reaches the third position from the second position, the self-mobile device can determine the travel mileage from the second position to the third position from the odometer, the wheel speed meter, or the like.
For example, the travel path from the mobile device is as shown in fig. 6A, the travel range is a quarter arc. And determining the decomposition displacement m of the Xu axis and the decomposition displacement n of the Yu axis from the second position to the third position according to the circular arc by the self-moving equipment.
As another example, as shown in fig. 6B, the travel distance is the sum of the distance m traveled along the Xu axis and the distance n traveled along the Yu axis. The decomposition displacement of the Xu axis is m and the decomposition displacement of the Yu axis is n.
As another example, as shown in fig. 6C, the travel distance is the distance n traveled along the Yu axis, and the travel distance m=0 along the Xu axis from the travel path of the mobile device. Decomposition displacement m=0 on the Xu axis, and the decomposition displacement on the yu axis is n.
As another example, the travel path from the mobile device is shown in fig. 6D, the travel range is the straight line distance from P2 to P3. And determining the decomposition displacement m of the Xu axis and the decomposition displacement n of the Yu axis from the second position to the third position according to the linear distance by the self-moving equipment.
Since the abscissa x 3 of the third theoretical position coordinate P3 (x 3,y3) can be determined from the x 2 and Xu axis decomposition displacement m, the ordinate y 3 can be determined from the x 2 and yu axis decomposition displacement n. Therefore, the theoretical distance S3 corresponding to the third theoretical position coordinate P3 (x 3,y3) can be derived according to the following formula (5):
similarly, the theoretical distance S 3 'corresponding to the third theoretical position coordinate P3' (x 3,-y3) can be obtained according to the following formula (6):
by adopting the scheme, the self-moving equipment obtains the decomposition displacement m of the Xu axis and the decomposition position n of the Yu according to the second mileage from the second position to the third position, and then combines the second theoretical position coordinates and the like to determine the theoretical distance, so that the speed is high and the accuracy is high.
Optionally, in the foregoing embodiment, in a process that the self-mobile device obtains N third theoretical position coordinates associated with the third position in the beacon coordinate system according to the second mileage and N second theoretical position coordinates, first, according to the second mileage, a pose transformation matrix of the self-mobile device moving from the second position to the third position is obtained. And then, acquiring N third theoretical position coordinates of the self-mobile device associated with the third position under a beacon coordinate system according to the pose transformation matrix and the N second theoretical position coordinates.
In the above embodiment, the self-mobile device is further capable of determining the pose transformation matrix T v2v3 moving from the second position to the third position based on the decomposition displacement m of the Xu axis, the decomposition position n of the Yu, and the like, which can be measured by the odometer. As shown in equation (7).
In the formula (7), β represents the amount of change in the heading angle after moving from the second position to the third position. For example, referring to fig. 6B, when the self-mobile device is located at the second position under the beacon coordinate system, the movement direction is parallel to the Xu axis, i.e. the heading angle is 0 degrees. After reaching the third position, the direction of motion is perpendicular to the Xu axis, i.e. the heading angle is 90 degrees. From this, it can be seen that: the change amount β of the heading angle is 90 degrees. In this embodiment, the pose transformation matrix T v2v3 is measured according to an odometer.
According to the pose transformation matrix and the second theoretical position coordinate P2 (x 2,y2), the self-mobile device can obtain the pose transformation matrix T uv3 of the self-mobile device when the third position of the self-mobile device is the third theoretical position coordinate P3 (x 3,y3) under the beacon coordinate system, as shown in formula (8).
According to the formula (8), the second theoretical position coordinate P2 (x 2,y2) and the decomposition displacements m and n corresponding to the second mileage can determine the third theoretical position coordinate P3 (x 3,y3) corresponding to the second theoretical position coordinate P2 (x 2,y2) as
Similarly, when the third position of the self-mobile device is the third theoretical position coordinate P3 '(x 3,-y3), the pose transformation matrix T' uv3 of the self-mobile device may be obtained as shown in formula (9).
According to the formula (8), the second theoretical position coordinate P2' (x 2,-y2) and the decomposition displacements m and n corresponding to the second mileage can determine that the third theoretical position coordinate P3' (x 3,-y3) corresponding to the second theoretical position coordinate P2' (x 2,-y2) is
By adopting the scheme, the pose transformation matrix of the mobile equipment moving from the second position to the third position is determined, and the third theoretical position coordinate is determined according to the pose transformation matrix, the second theoretical position coordinate and the like, so that the speed is high and the accuracy is high.
Optionally, in the foregoing embodiment, in a process that the self-mobile device obtains the pose of the self-mobile device when the self-mobile device arrives at the third position from the first position according to the first actual position coordinate of the first position in the beacon coordinate system, the position coordinate of the target beacon component in the world coordinate system, and the first heading angle, the position coordinate of the first position in the world coordinate system is determined according to the first actual position coordinate of the first position in the beacon coordinate system, the position coordinate of the target beacon component in the world coordinate system, and the first heading angle.
Illustratively, the self-mobile device determines the location coordinate (x wv1,ywv1, 1) of the first location in the world coordinate system from the first actual location coordinate P1 (x 1,y1) of the first location in the beacon coordinate system, the location coordinate (x wu,ywu, 1) of the target beacon component in the world coordinate system, and the first heading angle θ. The following formula (10) shows:
And then, the self-mobile device acquires the pose T wv3 of the self-mobile device when the self-mobile device is positioned at the third position under the world coordinate system according to the position coordinate (x wv1,ywv1, 1) of the first position under the world coordinate system, the first course angle theta and the first mileage T v1v3 of the self-mobile device reaching the third position from the first position.
The following formula (11) shows:
And (3) determining the position coordinates and the course angle of the self-mobile device under the world coordinate system when the self-mobile device is positioned at the third position according to the formula (11).
By adopting the scheme, the data of the odometer, the single detection component and the magnetometer are combined for repositioning, so that the process is simple and the cost is low.
Fig. 8 is another flowchart of a relocation method according to an embodiment of the present application. The embodiment comprises the following steps:
801. a relocation is determined from the mobile device to be performed.
When the self-mobile device is manually moved, restarted, etc., resulting in loss of positioning or abnormal positioning of the self-mobile device, the self-mobile device determines that repositioning needs to be performed.
802. A self-mobile device performs a calibration action at a first location to determine a first heading angle of the self-mobile device and construct a beacon coordinate system.
Illustratively, the angular data is collected and the heading angle is determined from the mobile device rotated at least 90 degrees from the first position using a magnetometer. Thereafter, a beacon coordinate system is constructed.
803. The method includes performing a first travel action from the mobile device and detecting distance information with a detection component and detecting a travel range with an odometer.
By performing the first travelling action, the self-mobile device is able to determine N first theoretical position coordinates of the first position in the beacon coordinate system and N second theoretical position coordinates of the second position in the beacon coordinate system.
804. And executing a second traveling action from the mobile device, detecting distance information by using the detection component, and detecting traveling mileage by using the odometer.
By performing the second travel action, N third theoretical position coordinates associated with the third position can be determined from the mobile device. And further determining a first actual position coordinate of the first position in the beacon coordinate system according to the third theoretical position coordinate and the like.
805. And the self-mobile device determines the position coordinate and the course angle of the third position under the world coordinate system according to the course angle, the first actual position coordinate of the first position under the beacon coordinate system, the position coordinate of the target beacon component under the world coordinate system and the first mileage from the first position to the third position, thereby finishing repositioning.
It should be noted that, although in the embodiment shown in fig. 3, after the mobile device performs step 304, step 305 and step 306 are described by taking as an example the determination of the first actual position coordinate of the first position in the beacon coordinate system, and then determining the pose of the mobile device when the mobile device is located in the third position in the world coordinate system according to the first actual position coordinate and the like. However, the embodiment of the present application is not limited to the above, and in other possible implementations, after the mobile device performs step 304, the second actual position coordinate of the second position in the beacon coordinate system may be determined, and then the pose of the mobile device when the mobile device is located in the third position in the world coordinate system may be determined according to the second actual position coordinate and the like. For example, referring to fig. 9, fig. 9 is a flowchart illustrating a repositioning method according to an embodiment of the present application. The embodiment comprises the following steps:
901. the self-mobile device is controlled to perform a first travel action to cause the self-mobile device to travel from a first location to a second location relative to a target beacon assembly.
Wherein the target beacon component is any one of the beacon components detected by the detection component.
902. And respectively acquiring N first theoretical position coordinates of the self-mobile device associated with the first position under a beacon coordinate system and N second theoretical position coordinates associated with the second position according to first ranging information and second ranging information between the target beacon component and the self-mobile device, wherein N is greater than or equal to 1, and N is a constant.
903. The self-mobile device is controlled to perform a second travel action from the second location to cause the self-mobile device to travel from the second location to a third location.
904. And acquiring N third theoretical position coordinates of the self-mobile device associated with the third position under a beacon coordinate system according to the second mileage and the N second theoretical position coordinates.
Wherein the second range is a range of travel of the self-mobile device from the second location to the third location.
Steps 901 to 904 can be referred to above in steps 301 to 304 of fig. 3, and will not be described here again.
905. And acquiring a second actual position coordinate of the second position under the beacon coordinate system according to the third ranging information and N third theoretical position coordinates when the target beacon component and the self-mobile device are positioned at a third position.
Illustratively, from equations (3) and (4) above, the second theoretical position coordinates P2 (x 2,y2) and P2' (x 2,-y2) of the second position in the beacon coordinate system may be determined.
If the second travel action is performed from the mobile device starting from the second theoretical position coordinate P2 (x 2,y2), the third theoretical position coordinate of the third position is P3 (x 3,y3). From the third theoretical position coordinate P3 (x 3,y3), a theoretical distance S 3 can be determined from the mobile device, which theoretical distance S 3 represents the distance from the third position P3 to the origin.
If the second travel action is performed from the mobile device starting from the second theoretical position coordinate P2 '(x 2,-y2), the third theoretical position coordinate of the third position is P3' (x 3,-y3). From the third theoretical position coordinates P3' (x 3,-y3), a theoretical distance S 3 ' can be determined from the mobile device, which theoretical distance S 3 ' represents the distance from the third position P3 to the origin.
Referring to fig. 5, when the self-mobile device is located at the third position, the detection component detects the target beacon component, so as to determine the fourth distance R3. Thereafter, from the mobile device comparing the fourth distance R3 with the theoretical distance S 3, comparing the fourth distance R3 with the theoretical distance S 3 ', determining who the theoretical distance S 3 and the theoretical distance S 3' are closer to the fourth distance R3. If the theoretical distance S 3 is closest to the fourth distance R3, the third theoretical position coordinate P3 (x 3,y3) is taken as the third actual position of the third position under the beacon coordinate system, and further, the second actual position coordinate of the second position under the beacon coordinate system is determined to be the second theoretical position coordinate P2 (x 2,y2).
Similarly, if the theoretical distance S 3 ' is closest to the fourth distance R3, the third theoretical position coordinate P3' (x 3,-y3) is taken as the third actual position of the third position in the beacon coordinate system, and further, the second actual position coordinate of the second position in the beacon coordinate system is determined to be the second theoretical position coordinate P2' (x 2,-y2).
906. And acquiring the pose of the self-mobile device when the self-mobile device is positioned at the third position under the world coordinate system according to the second actual position coordinate of the second position under the beacon coordinate system, the position coordinate of the target beacon component under the world coordinate system, the first course angle of the self-mobile device for executing the first running action and the first mileage of the self-mobile device reaching the third position from the first position.
The first course angle is an included angle between a traveling direction of the self-mobile device executing a first traveling action and an axis of a world coordinate system X W.
Assume that the second actual position coordinate of the second position in the beacon coordinate system is the second theoretical position coordinate P2 (x 2,y2). When step 806 is performed, the self-mobile device first determines the position coordinates (x wv2,ywv2, 1) of the second location in the world coordinate system according to the second actual position coordinates P2 (x 2,y2) of the second location in the beacon coordinate system, the position coordinates (x wu,ywu, 1) of the target beacon component in the world coordinate system, and the first heading angle θ. The following formula (12) shows:
and then, the self-mobile device acquires the pose T wv3 of the self-mobile device when the self-mobile device is positioned at the third position under the world coordinate system according to the position coordinate (x wv2,ywv2, 1) of the second position under the world coordinate system, the first course angle theta and the first mileage T v1v3 of the self-mobile device reaching the third position from the first position.
The following formula (11) shows:
the following are examples of the apparatus of the present application that may be used to perform the method embodiments of the present application. For details not disclosed in the embodiments of the apparatus of the present application, please refer to the embodiments of the method of the present application.
Fig. 10 is a schematic diagram of a relocating device according to an embodiment of the present application. The relocating device 1000 is integrated on a self-moving device for moving and working in a work area, the relocating device 1000 comprising: a first execution module 101, a first acquisition module 102, a second execution module 103, a second acquisition module 104, a third acquisition module 105, and a processing module 106.
A first execution module 101 for controlling the self-mobile device to execute a first travelling action to cause the self-mobile device to travel from a first position to a second position relative to a target beacon component, the target beacon component being any one of the beacon components detected by the detection component;
a first obtaining module 102, configured to obtain N first theoretical position coordinates of the self-mobile device associated with the first position and N second theoretical position coordinates of the self-mobile device associated with the second position in a beacon coordinate system according to first ranging information and second ranging information between the target beacon component and the self-mobile device, where N is greater than or equal to 1 and N is a constant;
A second execution module 103 for controlling the self-mobile device to execute a second traveling action from the second location to cause the self-mobile device to travel from the second location to a third location;
A second obtaining module 104, configured to obtain N third theoretical position coordinates of the self-mobile device associated with the third position under a beacon coordinate system according to a second mileage and N second theoretical position coordinates, where the second mileage is a travelling mileage of the self-mobile device from the second position to the third position;
A third obtaining module 105, configured to obtain a first actual position coordinate of the first position in the beacon coordinate system according to third ranging information and N third theoretical position coordinates when the target beacon component and the self-mobile device are located at a third position
And the processing module 106 is configured to obtain a pose of the self-mobile device when the self-mobile device is located in the third position in the world coordinate system according to a first actual position coordinate of the first position in the beacon coordinate system, a position coordinate of the target beacon component in the world coordinate system, a first heading angle of the self-mobile device for executing a first movement, and a first mileage of the self-mobile device reaching the third position from the first position, where the first heading angle is an included angle between a travel direction of the self-mobile device for executing the first movement and an axis of the world coordinate system X W.
In a possible implementation manner, a first obtaining module 102 is configured to obtain a first distance and a second distance according to first ranging information and second ranging information between the target beacon component and the self-mobile device, where the first distance and the second distance are distances between the first location and the second location and the target beacon component, respectively; and acquiring N first theoretical position coordinates associated with the first position and N second theoretical position coordinates associated with the second position according to the first distance, the second distance and a third distance between the first position and the second position.
In a possible implementation manner, the second obtaining module 104 is configured to obtain, according to a second mileage, a pose transformation matrix of the self-mobile device moving from the second location to the third location; and acquiring N third theoretical position coordinates of the self-mobile device associated with the third position under a beacon coordinate system according to the pose transformation matrix and the N second theoretical position coordinates.
In a possible implementation manner, a third obtaining module 105 is configured to obtain a fourth distance according to third ranging information when the target beacon component and the self-mobile device are located at a third position, where the fourth distance is a distance between the self-mobile device and the target beacon component when the self-mobile device is located at the third position; acquiring N theoretical distances between the self-mobile device and the target beacon component according to N third theoretical position coordinates; and acquiring a first actual position coordinate of the first position under the beacon coordinate system according to the N theoretical distances and the fourth distance.
In a possible implementation manner, the processing module 105 is configured to determine, according to a first actual position coordinate of the first position in the beacon coordinate system, a position coordinate of the target beacon component in the world coordinate system, and the first heading angle, a position coordinate of the first position in the world coordinate system; and acquiring the pose of the self-mobile device when the self-mobile device is positioned at the third position under the world coordinate system according to the position coordinate of the first position under the world coordinate system, the first course angle and the first mileage of the self-mobile device from the first position to the third position.
In a possible implementation, before the first execution module 101 controls the self-mobile device to execute the first movement to make the self-mobile device travel from the first position to the second position relative to the target beacon component, the processing module 105 is further configured to construct the beacon coordinate system according to the first heading angle, where an origin of the beacon coordinate system corresponds to a position coordinate of the target beacon component in the world coordinate system.
In a possible implementation, the processing module 105 is further configured to control the self-mobile device to perform a calibration action to determine the first heading angle of the self-mobile device before constructing the beacon coordinate system according to the first heading angle.
In a possible implementation, the calibration action is an action of rotating a preset angle in the first position, the preset angle being greater than 90 °.
In a possible implementation, the first movement is an action of making Xu coordinates of the first position and the second position in the beacon coordinate system different and Yu coordinates identical.
In a possible implementation manner, the travel path corresponding to the first travel action is a straight line.
In a possible implementation, the second travelling action is an action that makes Yu coordinates of the second position and the third position in the beacon coordinate system different.
In a possible implementation manner, the travel path corresponding to the second travel action is any one of the following paths: the L-shaped path, the arc-shaped path, the connecting line of the path perpendicular to the first position and the second position, and the included angle between the path and the connecting line is larger than 0 degree.
In a possible implementation, the target beacon components are 1.
In a possible implementation, the heading angle is measured by a magnetometer mounted on the self-moving device.
In a possible implementation manner, an odometer is further arranged on the self-mobile device, and the odometer is used for measuring the first mileage and/or the second mileage.
The repositioning device provided by the embodiment of the application can execute the actions of the self-mobile device in the above embodiment, and the implementation principle and the technical effect are similar, and are not repeated here.
Embodiments of the present application also provide a computer readable storage medium having stored therein computer instructions which, when executed by a processor, are operable to implement a relocation method as implemented from a mobile device as above.
Embodiments of the present application also provide a computer program product comprising a computer program which, when executed by a processor, implements a relocation method as implemented by a mobile device as described above.
Other embodiments of the application will be apparent to those skilled in the art from consideration of the specification and practice of the application disclosed herein. This application is intended to cover any variations, uses, or adaptations of the application following, in general, the principles of the application and including such departures from the present disclosure as come within known or customary practice within the art to which the application pertains. It is intended that the specification and examples be considered as exemplary only, with a true scope and spirit of the application being indicated by the following claims.
It is to be understood that the application is not limited to the precise arrangements and instrumentalities shown in the drawings, which have been described above, and that various modifications and changes may be effected without departing from the scope thereof. The scope of the application is limited only by the appended claims.
Claims (19)
1. A repositioning method, characterized by a self-moving device for movement and operation in an operating area, the self-moving device being provided with a detection component, the method comprising:
Controlling the self-mobile device to perform a first advancement to advance the self-mobile device from a first location to a second location relative to a target beacon component, the target beacon component being any one of the beacon components detected by the detection component;
According to first ranging information and second ranging information between the target beacon component and the self-mobile device, N first theoretical position coordinates of the self-mobile device, which are associated with the first position, and N second theoretical position coordinates of the self-mobile device, which are associated with the second position, are respectively obtained under a beacon coordinate system, wherein N is greater than or equal to 1, and N is a constant;
Controlling the self-moving device to perform a second travel action from the second location to cause the self-moving device to travel from the second location to a third location;
Acquiring N third theoretical position coordinates of the self-mobile device associated with the third position under a beacon coordinate system according to a second mileage and N second theoretical position coordinates, wherein the second mileage is a travelling mileage of the self-mobile device from the second position to the third position;
Acquiring a first actual position coordinate of the first position under the beacon coordinate system according to third ranging information and N third theoretical position coordinates when the target beacon component and the self-mobile device are positioned at a third position;
And acquiring the pose of the self-mobile device when the self-mobile device is positioned at the third position under the world coordinate system according to the first actual position coordinate of the first position under the beacon coordinate system, the position coordinate of the target beacon component under the world coordinate system, the first course angle of the self-mobile device for executing the first running action and the first mileage of the self-mobile device reaching the third position from the first position, wherein the first course angle is the included angle between the running direction of the self-mobile device for executing the first running action and the X W axis of the world coordinate system.
2. The method of claim 1, wherein the obtaining N first theoretical position coordinates of the self-mobile device associated with the first position in a beacon coordinate system and N second theoretical position coordinates of the self-mobile device associated with the second position according to the first ranging information and the second ranging information between the target beacon component and the self-mobile device, respectively, comprises:
According to first ranging information and second ranging information between the target beacon assembly and the self-mobile device, respectively obtaining a first distance and a second distance, wherein the first distance and the second distance are respectively the distances between the first position and the target beacon assembly and the second position;
And acquiring N first theoretical position coordinates associated with the first position and N second theoretical position coordinates associated with the second position according to the first distance, the second distance and a third distance between the first position and the second position.
3. The method of claim 1, wherein the obtaining N third theoretical position coordinates of the self-mobile device associated with the third position in a beacon coordinate system from the second mileage and the N second theoretical position coordinates comprises:
acquiring a pose transformation matrix of the self-mobile device moving from the second position to the third position according to a second mileage;
And acquiring N third theoretical position coordinates of the self-mobile device associated with the third position under a beacon coordinate system according to the pose transformation matrix and the N second theoretical position coordinates.
4. A method according to any one of claims 1 to 3, wherein the obtaining a first actual position coordinate of the first position in the beacon coordinate system according to the third ranging information when the target beacon component and the self-mobile device are located at a third position and N third theoretical position coordinates comprises:
acquiring a fourth distance according to third ranging information when the target beacon component and the self-mobile device are located at a third position, wherein the fourth distance is the distance between the self-mobile device and the target beacon component when the self-mobile device is located at the third position;
Acquiring N theoretical distances between the self-mobile device and the target beacon component according to N third theoretical position coordinates;
and acquiring a first actual position coordinate of the first position under the beacon coordinate system according to the N theoretical distances and the fourth distance.
5. A method according to any one of claims 1-3, wherein the obtaining the pose of the self-mobile device in the third location in the world coordinate system from the first actual location coordinates of the first location in the beacon coordinate system, the location coordinates of the target beacon component in the world coordinate system, the first heading angle of the self-mobile device performing the first travel action, and the first range of the self-mobile device from the first location to the third location comprises:
determining a position coordinate of the first position in the world coordinate system according to a first actual position coordinate of the first position in the beacon coordinate system, a position coordinate of the target beacon component in the world coordinate system and the first course angle;
And acquiring the pose of the self-mobile device when the self-mobile device is positioned at the third position under the world coordinate system according to the position coordinate of the first position under the world coordinate system, the first course angle and the first mileage of the self-mobile device from the first position to the third position.
6. A method according to any of claims 1-3, wherein prior to said controlling the self-mobile device to perform a first travel action to cause the self-mobile device to travel from a first location to a second location relative to a target beacon assembly, further comprising:
And constructing the beacon coordinate system according to the first course angle, wherein the origin point of the beacon coordinate system corresponds to the position coordinate of the target beacon component under the world coordinate system.
7. The method of claim 6, wherein prior to constructing a beacon coordinate system from the first heading angle, further comprising:
the self-mobile device is controlled to perform a calibration action to determine a first heading angle of the self-mobile device.
8. The method of claim 7, wherein the calibration action is an action of rotating a preset angle in the first position, the preset angle being greater than 90 °.
9. A method according to any one of claim 1 to 3, wherein,
The first movement is a movement in which Xu coordinates of the first position and the second position in the beacon coordinate system are different and Yu coordinates are the same.
10. The method of claim 9, wherein the step of determining the position of the substrate comprises,
And the travel path corresponding to the first travel action is a straight line.
11. A method according to any one of claim 1 to 3, wherein,
The second traveling action is an action of making Yu coordinates of the second position and the third position in the beacon coordinate system different.
12. The method of claim 11, wherein the step of determining the position of the probe is performed,
The travel path corresponding to the second travel action is any one of the following paths: the L-shaped path, the arc-shaped path, the connecting line of the path perpendicular to the first position and the second position, and the included angle between the path and the connecting line is larger than 0 degree.
13. A method according to any of claims 1-3, wherein the target beacon component is 1.
14. A method according to any of claims 1-3, characterized in that the heading angle is measured by a magnetometer, which is mounted on the self-moving device.
15. A method according to any of claims 1-3, wherein an odometer is further provided on the self-mobile device, the odometer being adapted to measure the first mileage and/or the second mileage.
16. A self-moving device, comprising:
a housing;
The travelling mechanism is used for supporting the shell and driving the self-moving equipment to move in a working area;
An actuator mounted on the housing for operation within the work area;
The detection assembly is arranged in the shell and is used for detecting the beacon assembly of the working area and measuring the distance;
a magnetometer mounted within the housing for determining the angle of rotation of the self-moving device;
An odometer mounted within the housing for detecting a range of travel of the self-mobile device;
a memory disposed on the housing for storing a computer program, the computer program comprising instructions;
A processor disposed on the housing and coupled with the memory, the processor executing the instructions to control the travel mechanism, the actuator, the detection assembly, the magnetometer, and the odometer to implement the method of any of claims 1-15.
17. A relocating device integrated on a self-moving device having a detection component, the device comprising:
A first execution module for controlling the self-mobile device to execute a first travel action to cause the self-mobile device to travel from a first location to a second location relative to a target beacon component, the target beacon component being any one of the beacon components detected by the detection component;
The first acquisition module is used for respectively acquiring N first theoretical position coordinates of the self-mobile device associated with the first position under a beacon coordinate system and N second theoretical position coordinates associated with the second position according to first ranging information and second ranging information between the target beacon component and the self-mobile device, wherein N is greater than or equal to 1 and is a constant;
A second execution module for controlling the self-mobile device to execute a second traveling action from the second location to cause the self-mobile device to travel from the second location to a third location;
A second obtaining module, configured to obtain N third theoretical position coordinates of the self-mobile device associated with the third position under a beacon coordinate system according to a second mileage and N second theoretical position coordinates, where the second mileage is a travelling mileage of the self-mobile device from the second position to the third position;
The third acquisition module is used for acquiring a first actual position coordinate of the first position under the beacon coordinate system according to third ranging information and N third theoretical position coordinates when the target beacon component and the self-mobile device are positioned at a third position;
The processing module is used for acquiring the pose of the self-mobile device when the self-mobile device is located at the third position under the world coordinate system according to the first actual position coordinate of the first position under the beacon coordinate system, the position coordinate of the target beacon component under the world coordinate system, the first course angle of the self-mobile device for executing the first running action and the first mileage of the self-mobile device reaching the third position from the first position, wherein the first course angle is the included angle between the running direction of the self-mobile device for executing the first running action and the X W axis of the world coordinate system.
18. A computer readable storage medium, on which a computer program is stored, characterized in that the computer program, when being executed by a processor, implements the method according to any one of claims 1 to 15.
19. A computer program product comprising a computer program which, when executed by a processor, implements the method of any of claims 1 to 15.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211306307.5A CN117968657A (en) | 2022-10-24 | 2022-10-24 | Repositioning method, repositioning device and readable storage medium |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211306307.5A CN117968657A (en) | 2022-10-24 | 2022-10-24 | Repositioning method, repositioning device and readable storage medium |
Publications (1)
Publication Number | Publication Date |
---|---|
CN117968657A true CN117968657A (en) | 2024-05-03 |
Family
ID=90844744
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211306307.5A Pending CN117968657A (en) | 2022-10-24 | 2022-10-24 | Repositioning method, repositioning device and readable storage medium |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117968657A (en) |
-
2022
- 2022-10-24 CN CN202211306307.5A patent/CN117968657A/en active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
EP3168705B1 (en) | Domestic robotic system | |
US20200191973A1 (en) | Navigation for a Robotic Work Tool | |
CN107463173B (en) | Storage AGV navigation method and device, computer equipment and storage medium | |
US10136576B2 (en) | Navigation for a robotic working tool | |
CA2831682C (en) | Measuring system for determining 3d coordinates of an object surface | |
US6454036B1 (en) | Autonomous vehicle navigation system and method | |
US20170120445A1 (en) | Navigation for a robotic working tool | |
CN111164380B (en) | Method for determining position of robot, position determining device for robot, and robot | |
CN106647741A (en) | Laser-navigation-based omnibearing motion mechanism control system | |
JP2018502296A (en) | 3D map generation by robot tool | |
JPH07281753A (en) | Moving robot | |
CN105044754A (en) | Mobile platform outdoor positioning method based on multi-sensor fusion | |
CN108489382A (en) | A kind of AGV dynamic pose measuring methods based on space multi-point constraint | |
CN104777452B (en) | Positioning system and positioning method of mobile equipment | |
CN114322980A (en) | Method for obtaining position coordinates and drawing electronic map, computer-readable storage medium, and autonomous operating apparatus | |
EP3761141B1 (en) | Method for mapping a working area of a mobile device | |
CN113899376B (en) | Self-mobile device map generation method, system and automatic working system | |
Xu et al. | A new positioning method for indoor laser navigation on under-determined condition | |
CN117968657A (en) | Repositioning method, repositioning device and readable storage medium | |
CN116917826A (en) | automatic working system | |
KR100962674B1 (en) | The method for estimating location of moble robot and mobile robot thereof | |
CN114562994A (en) | Positioning method of mobile robot in dynamic environment | |
CN114147723A (en) | Automatic lofting robot system and operation method thereof | |
Kaewkorn et al. | High-accuracy position-aware robot for agricultural automation using low-cost imu-coupled triple-laser-guided (TLG) system | |
RU131276U1 (en) | DEVICE FOR COORDINATION OF AUTOMATED DEVICES |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |