Navigation task scheduling method and system based on state machine and storage medium
By introducing a state machine-based navigation task scheduling method in the navigation system, using the global state queue to manage navigation task status, the problem of inefficiency in traditional navigation systems during task switching and state synchronization is solved, and more efficient navigation task processing is achieved and stability.
Patent Information
- Application Number
- CN202411202503.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-29
- Publication Date
- 2024-12-06
AI Technical Summary
Traditional navigation systems have processing conflicts during task switching and state synchronization, resulting in inefficient processing of complex tasks.
The navigation task scheduling method based on the state machine is adopted, and the navigation task state events are queued for execution by maintaining the global state queue and using the queue's first-in-first-out feature to queue up the navigation task status events to be executed, improving the processing capability and efficiency of the navigation system. Specific steps include entering the global planning state, rotating to the starting point state, local control state, rotating to the target point state, slowing down state and restoring behavior state to ensure the stability and efficiency of navigation tasks.
It effectively solves the problem of handling conflicts in the navigation system during task switching and state synchronization, improves the processing capabilities and efficiency of the navigation system, and ensures the stability and efficient operation of the navigation task.
Smart Images

Figure CN119088016A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of navigation task scheduling, and more specifically, to a navigation task scheduling method, system and storage medium based on a state machine. Background Art
[0002] With the rapid advancement of science and technology, navigation technology is one of the core technologies in the fields of self-driving cars, logistics robots, drones, etc. In the process of task switching, traditional navigation systems will produce different results when processing different events, resulting in processing conflicts. Summary of the invention
[0003] In view of the above problems, the purpose of the present invention is to provide a scheduling method, system and storage medium for navigation tasks based on a state machine. By maintaining a global state queue, the navigation task state is scheduled and managed, and the first-in-first-out feature of the queue is utilized to put different navigation task state events into the state queue to wait for execution, thereby improving the processing power and efficiency of the navigation system and solving the problems of task switching, state synchronization, etc. faced by traditional navigation systems when processing complex tasks.
[0004] A first aspect of the present invention provides a method for scheduling navigation tasks based on a state machine, the method comprising:
[0005] Enter the global planning state and obtain the first coordinate information and the second coordinate information;
[0006] Obtaining first path information according to the first coordinate information, the second coordinate information and preset map information;
[0007] Determining first rotation angle information according to the first path information;
[0008] adjusting the movement direction according to the first rotation angle information;
[0009] Acquire third coordinate information, and send a first angular velocity control instruction according to the third coordinate information and the first path information;
[0010] Determining whether the third coordinate information is the same as the second coordinate information;
[0011] If so, the end point direction is set according to the first path information, and a second angular velocity control instruction is sent.
[0012] This plan also includes:
[0013] When the finite state machine is awakened and enters the global planning state;
[0014] Start path planning based on coordinate information and preset map information;
[0015] Determining whether the first path information is obtained;
[0016] If yes, put the state rotated to the starting point into the state queue;
[0017] If not, obtain the first-time information;
[0018] Determining whether the first time information exceeds a preset time threshold;
[0019] If so, put the recovery behavior state into the state queue.
[0020] This plan also includes:
[0021] When the finite state machine is awakened and enters the state of rotating to the starting point;
[0022] Acquire first angle information, and determine third coordinate information according to the first path information;
[0023] Obtaining second angle information according to the first coordinate information and the third coordinate information;
[0024] Determine the first rotation angle information according to the first angle information and the second angle information;
[0025] A third angular velocity control instruction is sent according to the first rotation angle information to achieve turning on the spot to adjust the movement direction.
[0026] In this solution, the recovery behavior state is specifically:
[0027] Rotate in place to a preset angle threshold;
[0028] Determine whether there is an obstacle within a preset first distance threshold;
[0029] If yes, continue to rotate by the preset angle threshold;
[0030] If not, send the first angular velocity control instruction.
[0031] This plan also includes:
[0032] When the finite state machine is awakened and enters the deceleration and slow stop state;
[0033] Acquire the first coordinate information, and determine the third coordinate information according to the first path information;
[0034] Obtaining first distance information according to the first coordinate information and the third coordinate information;
[0035] Obtaining first speed information according to the first distance information and a preset deceleration algorithm;
[0036] The first angular velocity control instruction is sent according to the first velocity information to control the running speed.
[0037] This plan also includes:
[0038] When exercising according to the first path information,
[0039] Acquire second distance information, and determine whether the second distance information is lower than the first distance threshold;
[0040] If so, the first coordinate information is obtained and recorded, the state of the current state machine is ended, and the global planning state is placed in the state queue.
[0041] A second aspect of the present invention provides a navigation task scheduling system based on a state machine, including a navigation task scheduling method program based on a state machine, wherein the navigation task scheduling method program based on a state machine implements the following steps when executed by the processor:
[0042] Enter the global planning state and obtain the first coordinate information and the second coordinate information;
[0043] Obtaining first path information according to the first coordinate information, the second coordinate information and preset map information;
[0044] Determining first rotation angle information according to the first path information;
[0045] adjusting the movement direction according to the first rotation angle information;
[0046] Acquire third coordinate information, and send a first angular velocity control instruction according to the third coordinate information and the first path information;
[0047] Determining whether the third coordinate information is the same as the second coordinate information;
[0048] If so, the end point direction is set according to the first path information, and a second angular velocity control instruction is sent.
[0049] This plan also includes:
[0050] When the finite state machine is awakened and enters the global planning state;
[0051] Start path planning based on coordinate information and preset map information;
[0052] Determining whether the first path information is obtained;
[0053] If yes, put the state rotated to the starting point into the state queue;
[0054] If not, obtain the first-time information;
[0055] Determining whether the first time information exceeds a preset time threshold;
[0056] If so, put the recovery behavior state into the state queue.
[0057] This plan also includes:
[0058] When the finite state machine is awakened and enters the state of rotating to the starting point;
[0059] Acquire first angle information, and determine third coordinate information according to the first path information;
[0060] Obtaining second angle information according to the first coordinate information and the third coordinate information;
[0061] Determine the first rotation angle information according to the first angle information and the second angle information;
[0062] A third angular velocity control instruction is sent according to the first rotation angle information to achieve turning on the spot to adjust the movement direction.
[0063] A third aspect of the present invention provides a computer-readable storage medium, which includes a scheduling method program for a navigation task based on a state machine. When the scheduling method program for a navigation task based on a state machine is executed by a processor, the steps of the scheduling method for a navigation task based on a state machine as described in any one of the above items are implemented.
[0064] The present invention provides a scheduling method, system and storage medium for navigation tasks based on a state machine. First, a global planning state is entered to plan a path according to first coordinate information and second coordinate information; secondly, a rotation to a starting point state is entered to rotate in situ to the direction of the starting point of the path according to the planned path; then, a local control state is entered to adjust the travel speed according to third coordinate information and the planned path; finally, a rotation to a target point state is entered to rotate in situ to the direction of the target point after reaching the target position; in addition, a deceleration and slow stop state and a recovery behavior state are also provided, and the stability of the navigation operation is improved by decelerating to stop to keep the motion posture stable and achieving the purpose of getting out of trouble through recovery behavior. BRIEF DESCRIPTION OF THE DRAWINGS
[0065] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and should not be regarded as limiting the scope.
[0066] Figure 1A flowchart of a method for scheduling navigation tasks based on a state machine of the present invention is shown;
[0067] Figure 2 The operating flow chart of the global planning state provided by the embodiment of the present invention is shown;
[0068] Figure 3 The operation flow chart of rotating to the starting point state provided by an embodiment of the present invention is shown;
[0069] Figure 4 A block diagram of a navigation task scheduling system based on a state machine of the present invention is shown. DETAILED DESCRIPTION
[0070] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0071] Unless otherwise defined, all terms (including technical and scientific terms) used in the embodiments of the present invention have the same meanings as commonly understood by those skilled in the art to which the present invention belongs. It should also be understood that terms such as those defined in common dictionaries should be interpreted as having meanings consistent with their meanings in the context of the relevant technology, and should not be interpreted in an idealized or extremely formal sense, unless the embodiments of the present invention are clearly defined in this way.
[0072] The words "first", "second" and similar words used in the embodiments of the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. Words such as "one", "an" or "the" and similar words do not indicate a quantity limitation, but indicate the existence of at least one. Similarly, words such as "include" or "comprise" mean that the elements or objects appearing before the word include the elements or objects listed after the word and their equivalents, without excluding other elements or objects.
[0073] "Connected" or "connected" and the like are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. The steps preceding or following the method of the embodiment of the present invention are not necessarily performed in exact order. Instead, the various steps may be processed in reverse order or simultaneously. At the same time, other operations may be added to these processes, or one or more steps may be removed from these processes.
[0074] In addition, the functional modules in the various embodiments of the present invention may be integrated together to form an independent part, or each module may exist independently, or two or more modules may be integrated to form an independent part.
[0075] Figure 1 A flow chart of a method for scheduling navigation tasks based on a state machine of the present invention is shown.
[0076] like Figure 1 As shown, the present invention discloses a method for scheduling navigation tasks based on a state machine, the method comprising:
[0077] S102, entering a global planning state, obtaining first coordinate information and second coordinate information;
[0078] S104, obtaining first path information according to the first coordinate information, the second coordinate information and preset map information;
[0079] S106, determining first rotation angle information according to the first path information;
[0080] S108, adjusting the movement direction according to the first rotation angle information;
[0081] S110, acquiring third coordinate information, and sending a first angular velocity control instruction according to the third coordinate information and the first path information;
[0082] S112, determining whether the third coordinate information is the same as the second coordinate information;
[0083] S114: If yes, set the end point direction according to the first path information, and send a second angular velocity control instruction.
[0084] It should be noted that in the state machine provided in the embodiment of the present invention, the core state of the navigation task is divided into the global planning state, the state of rotating to the starting point, the local control state and the state of rotating to the target point; the auxiliary state of the navigation task includes the deceleration and slow stop state and the recovery behavior state. After the navigation system is started, the navigation finite state machine is in a suspended state. When the navigation target point is obtained, the global planning state is put into the state queue list; and the navigation finite state machine is awakened, and the navigation task scheduling is started according to the state queue list.
[0085] Wake up the navigation finite state machine, enter the global planning state according to the state queue list, and pop the global planning state from the state queue list; first, obtain the first coordinate information and the second coordinate information according to the electronic map information preset in the system, the first coordinate information is the current coordinate of the navigation device, and the second coordinate information is the target point coordinate of the navigation device; then, according to the first coordinate information, the second coordinate information and the electronic sub-map information, obtain the first path information through the preset path planning algorithm; finally, after completing the global planning state, put the next state into the state queue list, and suspend the navigation finite state machine.
[0086] Wake up the navigation finite state machine, enter the rotation to the starting point state according to the state queue list, and pop the rotation to the starting point state from the state queue list; obtain the travel path of the navigation device according to the first path information, and obtain the angle at which the navigation device will turn to the travel path before moving according to the running path, which is recorded as the third angle information; perform an in-place rotation operation according to the third angle information, so that the navigation device turns to the specified direction; after completing the rotation to the starting point state, put the next state into the state queue list, and suspend the navigation finite state machine.
[0087] Wake up the navigation finite state machine, enter the local control state according to the state queue list, and pop the local control state from the state queue list; in an embodiment of the present invention, the navigation path in the first path information is composed of at least one straight path; the third coordinate information is the end point coordinate of the straight path on which the navigation device is traveling, and the remaining distance of the current straight path is obtained according to the third coordinate information, and a first angular velocity control instruction is sent to the brake motor according to the distance to control the travel speed of the navigation device; if the third coordinate information is the same as the second coordinate information, a second angular velocity control instruction is sent, so that the navigation device gradually decelerates to the position of the third coordinate according to the third coordinate information and then stops traveling; after completing the local control state, the next state is placed in the state queue list, and the navigation finite state machine is suspended.
[0088] Wake up the navigation finite state machine, enter the rotation to target point state according to the state queue list, and pop the rotation to target point state from the state queue list; obtain the destination orientation of the device according to the first path information, perform an in-place rotation operation, so that the orientation of the navigation device turns to the specified direction; after completing the rotation to target point state, suspend the navigation finite state machine and end the current navigation task.
[0089] Figure 2 The operating flow chart of the global planning state provided by the embodiment of the present invention is shown.
[0090] According to an embodiment of the present invention, Figure 2 As shown, it also includes:
[0091] S202, when the finite state machine is awakened and enters the global planning state;
[0092] S204, starting path planning according to the coordinate information and preset map information;
[0093] S206, determining whether the first path information is obtained;
[0094] S208, if yes, put the state of rotating to the starting point into the state queue;
[0095] S210, if not, obtaining first time information;
[0096] S212, determining whether the first time information exceeds a preset time threshold;
[0097] S214: If yes, put the recovery behavior state into the state queue.
[0098] It should be noted that when the navigation finite state machine is awakened and enters the global planning state, path planning is performed through a preset path planning algorithm based on the coordinate information of the navigation target point and the electronic map information preset in the system. If the first path information can be obtained through the path planning algorithm, it means that the navigation device can travel to the target point according to the first path information, and after putting the rotation to the starting point state into the state queue, exit the current state. If the first path information cannot be obtained through the path planning algorithm, the path planning is performed again and the path planning time is recorded as the first time information. When the first time information exceeds the preset time threshold, it means that the navigation device is trapped in an area, and after putting the recovery behavior state into the state queue, exit the current state.
[0099] Figure 3 The operating flow chart of rotating to the starting point state provided by an embodiment of the present invention is shown.
[0100] According to an embodiment of the present invention, Figure 3 As shown, it also includes:
[0101] S302, when the finite state machine is awakened and enters the state of rotating to the starting point;
[0102] S304, acquiring first angle information, and determining third coordinate information according to the first path information;
[0103] S306, obtaining second angle information according to the first coordinate information and the third coordinate information;
[0104] S308, determining the first rotation angle information according to the first angle information and the second angle information;
[0105] S310: Send a third angular velocity control instruction according to the first rotation angle information to achieve turning on the spot to adjust the movement direction.
[0106] It should be noted that when the navigation finite state machine is awakened and enters the state of rotating to the starting point, the navigation device obtains its current orientation information, that is, the first angle information; wherein the first angle information is the angle between the line connecting the current coordinates of the navigation device and the coordinates of the map point 0 and the horizontal coordinate of the map. The third coordinate information of the first straight path segment is confirmed based on the first straight path segment in the first path information. The second angle information is calculated based on the first coordinate information and the third coordinate information, and the second angle information is the angle between the coordinates of the first straight path segment on the map based on the horizontal coordinate point coordinates. The difference between the first angle information and the second angle information is the angle that the navigation device needs to rotate, that is, the first rotation angle information. According to the required rotation angle, the third angular velocity control instruction is sent to the brake motor to drive the navigation device to rotate in place, so that the device's orientation turns to the specified direction.
[0107] According to an embodiment of the present invention, the recovery behavior state is specifically:
[0108] Rotate in place to a preset angle threshold;
[0109] Determine whether there is an obstacle within a preset first distance threshold;
[0110] If yes, continue to rotate by the preset angle threshold;
[0111] If not, send the first angular velocity control instruction.
[0112] It should be noted that when the navigation finite state machine is awakened and enters the recovery behavior state, the navigation device is identified as trapped in an area, and it is turned in place and moved forward to get out of trouble. First, the third angular velocity control instruction is used to drive the navigation device to rotate in place at a preset angle, which is usually set to 5 degrees or 10 degrees in actual applications; then, the distance of the obstacle directly in front of the navigation device is collected. If the distance of the obstacle in front is lower than the preset first distance threshold, it means that there is an obstacle in front of the navigation device within a safe distance and it cannot move forward, then the in-place rotation operation is performed again. If there is no obstacle in front within a safe distance, the navigation device is driven forward by sending the first angular velocity control instruction. Through repeated operations, until the navigation device is identified as out of trouble, the global planning state is placed in the state list and the current state is exited.
[0113] According to an embodiment of the present invention, it also includes:
[0114] When the finite state machine is awakened and enters the deceleration and slow stop state;
[0115] Acquire the first coordinate information, and determine the third coordinate information according to the first path information;
[0116] Obtaining first distance information according to the first coordinate information and the third coordinate information;
[0117] Obtaining first speed information according to the first distance information and a preset deceleration algorithm;
[0118] The first angular velocity control instruction is sent according to the first velocity information to control the running speed.
[0119] It should be noted that when the navigation finite state machine is awakened and enters the deceleration and slow stop state, the third coordinate information is confirmed according to the current straight path in the first path information. The first coordinate information of the location of the navigation device is obtained, and combined with the third coordinate information, the remaining travel distance of the current straight path can be calculated, which is recorded as the first distance information. The first distance information is used as a calculation parameter and added to the preset deceleration algorithm to obtain the operating speed value that should be set when the first distance is obtained, which is recorded as the first speed information. According to the first speed information and the physical characteristics of the motor, the angular velocity of the motor is converted, and the operating speed of the navigation device is controlled by the first angular velocity control instruction.
[0120] According to an embodiment of the present invention, it also includes:
[0121] When exercising according to the first path information,
[0122] Acquire second distance information, and determine whether the second distance information is lower than the first distance threshold;
[0123] If so, the first coordinate information is obtained and recorded, the state of the current state machine is ended, and the global planning state is placed in the state queue.
[0124] It should be noted that the second distance information is the distance between the navigation device and the obstacle in front of it when the navigation device moves according to the first path information. When the second distance information is lower than the first distance threshold, it means that there is an obstacle outside the path planning, then the current state machine state is terminated, and the global planning state is put into the state queue, and the path is replanned.
[0125] It is worth mentioning that it also includes:
[0126] When exercising according to the first path information,
[0127] Acquire second distance information, and determine whether the second distance information is lower than the first distance threshold;
[0128] Determine the fourth coordinate information according to the first path information;
[0129] Update the map information according to the fourth coordinate information
[0130] It should be noted that when the navigation device is navigating according to the first path information, if the distance of the obstacle in front is lower than the preset first distance threshold, it means that there is an obstacle outside the path planning, and the electronic map needs to be updated. According to the current coordinates of the navigation device, the direction of travel of the first path information and the second distance information, the coordinate information of the obstacle can be obtained, which is recorded as the fourth coordinate information; finally, the electronic map can be updated according to the fourth coordinate information.
[0131] It is worth mentioning that it also includes:
[0132] Obtaining starting coordinate information and ending coordinate information of at least two sets of the first path information in the record;
[0133] Determine whether there is repeated movement within the first area according to a preset position detection algorithm;
[0134] If yes, set the preset second coordinate information and enter the global planning state.
[0135] It should be noted that after completing a round of navigation tasks, the coordinates of the starting point and the end point of the navigation device are recorded, and combined with the coordinate information, the distance traveled, the running time and other information, through the preset position detection algorithm, it is determined whether the navigation device is repeatedly moving in the same area of a preset size. If so, it means that the navigation device may be trapped. At this time, the second coordinate information is set to a preset position, such as the map origin, the base station device coordinate point, etc., so that the navigation device plans a navigation task of a larger distance; and the global planning state is placed in the state queue list.
[0136] Figure 4 A block diagram of a navigation task scheduling system based on a state machine of the present invention is shown.
[0137] like Figure 4 As shown, the present invention discloses a navigation task scheduling system 4 based on a state machine, comprising a memory 41 and a processor 42, wherein the memory comprises a navigation task scheduling method program based on a state machine, and the navigation task scheduling method program based on a state machine is executed by the processor to implement the following steps:
[0138] Enter the global planning state and obtain the first coordinate information and the second coordinate information;
[0139] Obtaining first path information according to the first coordinate information, the second coordinate information and preset map information;
[0140] Determining first rotation angle information according to the first path information;
[0141] adjusting the movement direction according to the first rotation angle information;
[0142] Acquire third coordinate information, and send a first angular velocity control instruction according to the third coordinate information and the first path information;
[0143] Determining whether the third coordinate information is the same as the second coordinate information;
[0144] If so, the end point direction is set according to the first path information, and a second angular velocity control instruction is sent.
[0145] It should be noted that in the state machine provided in the embodiment of the present invention, the core state of the navigation task is divided into the global planning state, the state of rotating to the starting point, the local control state and the state of rotating to the target point; the auxiliary state of the navigation task includes the deceleration and slow stop state and the recovery behavior state. After the navigation system is started, the navigation finite state machine is in a suspended state. When the navigation target point is obtained, the global planning state is put into the state queue list; and the navigation finite state machine is awakened, and the navigation task scheduling is started according to the state queue list.
[0146] Wake up the navigation finite state machine, enter the global planning state according to the state queue list, and pop the global planning state from the state queue list; first, obtain the first coordinate information and the second coordinate information according to the electronic map information preset in the system, the first coordinate information is the current coordinate of the navigation device, and the second coordinate information is the target point coordinate of the navigation device; then, according to the first coordinate information, the second coordinate information and the electronic sub-map information, obtain the first path information through the preset path planning algorithm; finally, after completing the global planning state, put the next state into the state queue list, and suspend the navigation finite state machine.
[0147] Wake up the navigation finite state machine, enter the rotation to the starting point state according to the state queue list, and pop the rotation to the starting point state from the state queue list; obtain the travel path of the navigation device according to the first path information, and obtain the angle at which the navigation device will turn to the travel path before moving according to the running path, which is recorded as the third angle information; perform an in-place rotation operation according to the third angle information, so that the navigation device turns to the specified direction; after completing the rotation to the starting point state, put the next state into the state queue list, and suspend the navigation finite state machine.
[0148] Wake up the navigation finite state machine, enter the local control state according to the state queue list, and pop the local control state from the state queue list; in an embodiment of the present invention, the navigation path in the first path information is composed of at least one straight path; the third coordinate information is the end point coordinate of the straight path on which the navigation device is traveling, and the remaining distance of the current straight path is obtained according to the third coordinate information, and a first angular velocity control instruction is sent to the brake motor according to the distance to control the travel speed of the navigation device; if the third coordinate information is the same as the second coordinate information, a second angular velocity control instruction is sent, so that the navigation device gradually decelerates to the position of the third coordinate according to the third coordinate information and then stops traveling; after completing the local control state, the next state is placed in the state queue list, and the navigation finite state machine is suspended.
[0149] Wake up the navigation finite state machine, enter the rotation to target point state according to the state queue list, and pop the rotation to target point state from the state queue list; obtain the destination orientation of the device according to the first path information, perform an in-place rotation operation, so that the orientation of the navigation device turns to the specified direction; after completing the rotation to target point state, suspend the navigation finite state machine and end the current navigation task.
[0150] According to an embodiment of the present invention, it also includes:
[0151] When the finite state machine is awakened and enters the global planning state;
[0152] Start path planning based on coordinate information and preset map information;
[0153] Determining whether the first path information is obtained;
[0154] If yes, put the state rotated to the starting point into the state queue;
[0155] If not, obtain the first-time information;
[0156] Determining whether the first time information exceeds a preset time threshold;
[0157] If so, put the recovery behavior state into the state queue.
[0158] It should be noted that when the navigation finite state machine is awakened and enters the global planning state, path planning is performed through a preset path planning algorithm based on the coordinate information of the navigation target point and the electronic map information preset in the system. If the first path information can be obtained through the path planning algorithm, it means that the navigation device can travel to the target point according to the first path information, and after putting the rotation to the starting point state into the state queue, exit the current state. If the first path information cannot be obtained through the path planning algorithm, the path planning is performed again and the path planning time is recorded as the first time information. When the first time information exceeds the preset time threshold, it means that the navigation device is trapped in an area, and after putting the recovery behavior state into the state queue, exit the current state.
[0159] According to an embodiment of the present invention, it also includes:
[0160] When the finite state machine is awakened and enters the state of rotating to the starting point;
[0161] Acquire first angle information, and determine third coordinate information according to the first path information;
[0162] Obtaining second angle information according to the first coordinate information and the third coordinate information;
[0163] Determine the first rotation angle information according to the first angle information and the second angle information;
[0164] A third angular velocity control instruction is sent according to the first rotation angle information to achieve turning on the spot to adjust the movement direction.
[0165] It should be noted that when the navigation finite state machine is awakened and enters the state of rotating to the starting point, the navigation device obtains its current orientation information, that is, the first angle information; wherein the first angle information is the angle between the line connecting the current coordinates of the navigation device and the coordinates of the map point 0 and the horizontal coordinate of the map. The third coordinate information of the first straight path segment is confirmed based on the first straight path segment in the first path information. The second angle information is calculated based on the first coordinate information and the third coordinate information, and the second angle information is the angle between the coordinates of the first straight path segment on the map based on the horizontal coordinate point coordinates. The difference between the first angle information and the second angle information is the angle that the navigation device needs to rotate, that is, the first rotation angle information. According to the required rotation angle, the third angular velocity control instruction is sent to the brake motor to drive the navigation device to rotate in place, so that the device's orientation turns to the specified direction.
[0166] According to an embodiment of the present invention, the recovery behavior state is specifically:
[0167] Rotate in place to a preset angle threshold;
[0168] Determine whether there is an obstacle within a preset first distance threshold;
[0169] If yes, continue to rotate by the preset angle threshold;
[0170] If not, send the first angular velocity control instruction.
[0171] It should be noted that when the navigation finite state machine is awakened and enters the recovery behavior state, the navigation device is identified as trapped in an area, and it is turned in place and moved forward to get out of trouble. First, the third angular velocity control instruction is used to drive the navigation device to rotate in place at a preset angle, which is usually set to 5 degrees or 10 degrees in actual applications; then, the distance of the obstacle directly in front of the navigation device is collected. If the distance of the obstacle in front is lower than the preset first distance threshold, it means that there is an obstacle in front of the navigation device within a safe distance and it cannot move forward, then the in-place rotation operation is performed again. If there is no obstacle in front within a safe distance, the navigation device is driven forward by sending the first angular velocity control instruction. Through repeated operations, until the navigation device is identified as out of trouble, the global planning state is placed in the state list and the current state is exited.
[0172] According to an embodiment of the present invention, it also includes:
[0173] When the finite state machine is awakened and enters the deceleration and slow stop state;
[0174] Acquire the first coordinate information, and determine the third coordinate information according to the first path information;
[0175] Obtaining first distance information according to the first coordinate information and the third coordinate information;
[0176] Obtaining first speed information according to the first distance information and a preset deceleration algorithm;
[0177] The first angular velocity control instruction is sent according to the first velocity information to control the running speed.
[0178] It should be noted that when the navigation finite state machine is awakened and enters the deceleration and slow stop state, the third coordinate information is confirmed according to the current straight path in the first path information. The first coordinate information of the location of the navigation device is obtained, and combined with the third coordinate information, the remaining travel distance of the current straight path can be calculated, which is recorded as the first distance information. The first distance information is used as a calculation parameter and added to the preset deceleration algorithm to obtain the operating speed value that should be set when the first distance is obtained, which is recorded as the first speed information. According to the first speed information and the physical characteristics of the motor, the angular velocity of the motor is converted, and the operating speed of the navigation device is controlled by the first angular velocity control instruction.
[0179] According to an embodiment of the present invention, it also includes:
[0180] When exercising according to the first path information,
[0181] Acquire second distance information, and determine whether the second distance information is lower than the first distance threshold;
[0182] If so, the first coordinate information is obtained and recorded, the state of the current state machine is ended, and the global planning state is placed in the state queue.
[0183] It should be noted that the second distance information is the distance between the navigation device and the obstacle in front of it when the navigation device moves according to the first path information. When the second distance information is lower than the first distance threshold, it means that there is an obstacle outside the path planning, then the current state machine state is terminated, and the global planning state is put into the state queue, and the path is replanned.
[0184] It is worth mentioning that it also includes:
[0185] When exercising according to the first path information,
[0186] Acquire second distance information, and determine whether the second distance information is lower than the first distance threshold;
[0187] Determine the fourth coordinate information according to the first path information;
[0188] Update the map information according to the fourth coordinate information
[0189] It should be noted that when the navigation device is navigating according to the first path information, if the distance of the obstacle in front is lower than the preset first distance threshold, it means that there is an obstacle outside the path planning, and the electronic map needs to be updated. According to the current coordinates of the navigation device, the direction of travel of the first path information and the second distance information, the coordinate information of the obstacle can be obtained, which is recorded as the fourth coordinate information; finally, the electronic map can be updated according to the fourth coordinate information.
[0190] It is worth mentioning that it also includes:
[0191] Obtaining starting coordinate information and ending coordinate information of at least two sets of the first path information in the record;
[0192] Determine whether there is repeated movement within the first area according to a preset position detection algorithm;
[0193] If yes, set the preset second coordinate information and enter the global planning state.
[0194] It should be noted that after completing a round of navigation tasks, the coordinates of the starting point and the end point of the navigation device are recorded, and combined with the coordinate information, the distance traveled, the running time and other information, through the preset position detection algorithm, it is determined whether the navigation device is repeatedly moving in the same area of a preset size. If so, it means that the navigation device may be trapped. At this time, the second coordinate information is set to a preset position, such as the map origin, the base station device coordinate point, etc., so that the navigation device plans a navigation task of a larger distance; and the global planning state is placed in the state queue list.
[0195] A third aspect of the present invention provides a computer-readable storage medium, which includes a scheduling method program for a navigation task based on a state machine. When the scheduling method program for a navigation task based on a state machine is executed by a processor, the steps of the scheduling method for a navigation task based on a state machine as described in any one of the above items are implemented.
[0196] The present invention provides a scheduling method, system and storage medium for navigation tasks based on a state machine. First, a global planning state is entered to plan a path according to first coordinate information and second coordinate information; secondly, a rotation to a starting point state is entered to rotate in situ to the direction of the starting point of the path according to the planned path; then, a local control state is entered to adjust the travel speed according to third coordinate information and the planned path; finally, a rotation to a target point state is entered to rotate in situ to the direction of the target point after reaching the target position; in addition, a deceleration and slow stop state and a recovery behavior state are also provided, and the stability of the navigation operation is improved by decelerating to stop to keep the motion posture stable and achieving the purpose of getting out of trouble through recovery behavior.
[0197] If the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.
[0198] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.
Claims
1. A method for scheduling navigation tasks based on a state machine, characterized in that: The method comprises: Enter the global planning state and obtain the first coordinate information and the second coordinate information; Obtaining first path information according to the first coordinate information, the second coordinate information and preset map information; Determining first rotation angle information according to the first path information; adjusting the movement direction according to the first rotation angle information; Acquire third coordinate information, and send a first angular velocity control instruction according to the third coordinate information and the first path information; Determining whether the third coordinate information is the same as the second coordinate information; If so, the end point direction is set according to the first path information, and a second angular velocity control instruction is sent.
2. A method for scheduling navigation tasks based on a state machine according to claim 1, characterized in that: Also includes: When the finite state machine is awakened and enters the global planning state; Start path planning based on coordinate information and preset map information; Determining whether the first path information is obtained; If yes, put the state rotated to the starting point into the state queue; If not, obtain the first-time information; Determining whether the first time information exceeds a preset time threshold; If so, put the recovery behavior state into the state queue.
3. The method for scheduling navigation tasks based on a state machine according to claim 1, characterized in that: Also includes: When the finite state machine is awakened and enters the state of rotating to the starting point; Acquire first angle information, and determine third coordinate information according to the first path information; Obtaining second angle information according to the first coordinate information and the third coordinate information; Determine the first rotation angle information according to the first angle information and the second angle information; A third angular velocity control instruction is sent according to the first rotation angle information to achieve turning on the spot to adjust the movement direction.
4. The method for scheduling navigation tasks based on a state machine according to claim 2, characterized in that: The recovery behavior state is specifically: Rotate in place to a preset angle threshold; Determine whether there is an obstacle within a preset first distance threshold; If yes, continue to rotate by the preset angle threshold; If not, send the first angular velocity control instruction.
5. The method for scheduling navigation tasks based on a state machine according to claim 1, characterized in that: Also includes: When the finite state machine is awakened and enters the deceleration and slow stop state; Acquire the first coordinate information, and determine the third coordinate information according to the first path information; Obtaining first distance information according to the first coordinate information and the third coordinate information; Obtaining first speed information according to the first distance information and a preset deceleration algorithm; The first angular velocity control instruction is sent according to the first velocity information to control the running speed.
6. The method for scheduling navigation tasks based on a state machine according to claim 1, characterized in that: Also includes: When exercising according to the first path information, Acquire second distance information, and determine whether the second distance information is lower than the first distance threshold; If so, the first coordinate information is obtained and recorded, the state of the current state machine is ended, and the global planning state is placed in the state queue.
7. A navigation task scheduling system based on a state machine, characterized in that: The system includes a memory and a processor, wherein the memory includes a navigation task scheduling method program based on a state machine, and the navigation task scheduling method program based on a state machine implements the following steps when executed by the processor: Enter the global planning state and obtain the first coordinate information and the second coordinate information; Obtaining first path information according to the first coordinate information, the second coordinate information and preset map information; Determining first rotation angle information according to the first path information; adjusting the movement direction according to the first rotation angle information; Acquire third coordinate information, and send a first angular velocity control instruction according to the third coordinate information and the first path information; Determining whether the third coordinate information is the same as the second coordinate information; If so, the end point direction is set according to the first path information, and a second angular velocity control instruction is sent.
8. A navigation task scheduling system based on a state machine according to claim 7, characterized in that: Also includes: When the finite state machine is awakened and enters the global planning state; Start path planning based on coordinate information and preset map information; Determining whether the first path information is obtained; If yes, put the state rotated to the starting point into the state queue; If not, obtain the first-time information; Determining whether the first time information exceeds a preset time threshold; If so, put the recovery behavior state into the state queue.
9. A navigation task scheduling system based on a state machine according to claim 7, characterized in that: Also includes: When the finite state machine is awakened and enters the state of rotating to the starting point; Acquire first angle information, and determine third coordinate information according to the first path information; Obtaining second angle information according to the first coordinate information and the third coordinate information; Determine the first rotation angle information according to the first angle information and the second angle information; A third angular velocity control instruction is sent according to the first rotation angle information to achieve turning on the spot to adjust the movement direction.
10. A computer-readable storage medium having a computer program stored thereon, characterized in that: The computer-readable storage medium includes a method program for scheduling navigation tasks based on a state machine. When the method program for scheduling navigation tasks based on a state machine is executed by a processor, the steps of the method for scheduling navigation tasks based on a state machine as described in any one of claims 1 to 6 are implemented.
Citation Information
Patent Citations
Sweeping device target point navigation method based on finite-state machine
CN111329399A
Train inspection robot distributed scheduling method based on finite-state machine
CN116341880A
Method for improving robot path planning
CN116954216A
Method and apparatus for providing robot navigation
KR1020110046291A
Navigation method and apparatus, computer device and storage medium
US20220214178A1
