An autonomous navigation method for a mobile robot across floors and in all areas

Abstract

The present invention relates to an autonomous navigation system for a cross-floor full-domain mobile robot, comprising a task planning module, a robot movement module, a manipulator module, an elevator state monitoring module, a map management module and a path planning module. The task planning module is used for decomposing a navigation task into a plurality of subtasks, and then selecting an adapted subtask according to a comparison result between a target floor and a current floor, and sending a control instruction to other modules. The robot movement module is used for receiving instructions and controlling the movement of the robot. Two manipulator modules are used for respectively controlling the manipulators to press an elevator door opening button and an elevator floor button. The elevator state monitoring module is used for detecting the switch state of an elevator door and judging the elevator floor where the robot is located. The map management module is used for constructing a floor map, and establishing a positional relationship between different floor maps to align and adjust the orientation of a target floor map, so as to prepare the target floor map in advance for the navigation task.

Description

Technical Field

[0001] The present invention relates to the technical field of robot autonomous navigation, and in particular to an autonomous navigation method for a cross-floor full-area mobile robot. Background Art

[0002] In modern intelligent robot technology, autonomous navigation across floors is a key function that provides robots with the ability to move freely between floors. The realization of this function usually relies on the data processing capabilities within the robot and the wireless communication mechanism with the elevator control system. The robot interacts with the elevator through wireless signals to control the elevator's door opening and closing and floor selection, thereby achieving transfer between floors.

[0003] At present, the common mobile robot cross-floor navigation system requires the use of monitoring equipment to build a multi-platform system of robots, elevator control motherboards and monitoring equipment. The robot splices the maps of each floor to form a unified map containing all floor information for full-field navigation. By monitoring the position of the robot and coordinating the wireless communication between the robot and the elevator control motherboard, the number of floors reached by the elevator is known so that continuous navigation can be carried out on the unified map.

[0004] However, this method requires the robot to store and process complex map navigation information, and the monitoring capability of the monitoring equipment determines the accuracy of the robot's position judgment. If the signal of the monitoring equipment is inaccurate, wrong or interrupted, the robot is very likely to receive wrong floor information when switching floors, resulting in navigation task failure. In addition, the robot directly interacts with the elevator control motherboard and intervenes in the elevator control motherboard in real time, which poses certain risks to the operation of the elevator.

[0005] The above problems are worth solving. Summary of the invention

[0006] In order to overcome the deficiencies of the existing technology, the present invention provides an autonomous navigation method for a cross-floor full-area mobile robot.

[0007] The technical solution of the present invention is as follows:

[0008] An autonomous navigation method for a cross-floor full-area mobile robot, comprising a task planning module, a robot movement module, a manipulator module, an elevator status monitoring module, a map management module and a path planning module;

[0009] The task planning module is used to decompose the navigation task into several subtasks, select the appropriate subtask according to the comparison result between the target floor and the current floor, and send control instructions to the robot movement module, the manipulator module, the elevator status monitoring module, the map management module and the path planning module;

[0010] The robot movement module is used to receive instructions and control the robot movement; the manipulator module includes a first manipulator module and a second manipulator module, which respectively control the corresponding manipulators to press the elevator door opening button and the elevator floor button;

[0011] The elevator state monitoring module is used to detect the switch state of the elevator door and determine the elevator floor where the robot is located, and the step of determining the elevator floor where the robot is located includes analyzing the air pressure difference between floors and obtaining the floor value matching the manipulator at the elevator door opening button;

[0012] The map management module is used to construct a floor map, establish a positional relationship between different floor maps to align and adjust the orientation of the target floor map, and execute map switching when the robot reaches a specified position in the elevator to prepare the target floor map in advance for the navigation task;

[0013] The path planning module is used to provide a navigation route for a target floor map.

[0014] The present invention according to the above scheme is characterized in that the navigation task includes target position information, and the target position information includes floor information and coordinate information.

[0015] The present invention according to the above solution is characterized in that the task planning module is used to decompose the navigation task into five subtasks, and the five subtasks include:

[0016] Subtask 1: Navigate and control the robot to move to the target point on the same floor;

[0017] Subtask 2: Navigate and control the robot to move to the target point at the elevator door of the current floor, and command the robot to wait for the elevator door to open;

[0018] Subtask 3: Control the first manipulator to press the elevator door opening button;

[0019] Subtask 4: Navigate and control the robot to move to the target point in the elevator, and command the robot to wait for the elevator door to open;

[0020] Subtask 5: Control the second manipulator to press the elevator floor button;

[0021] And when the target floor is equal to the current floor, call subtask 1; when the target floor is not equal to the current floor, call subtask 2, subtask 3, subtask 4, subtask 5, and subtask 1 in sequence.

[0022] Furthermore, the task planning module calls the subtask 3, including: the task planning module obtains the size comparison result between the target floor and the current floor, issues instructions according to the size comparison result, and controls the first manipulator to select the correct elevator door opening button, and the elevator door opening button includes an up button and a down button.

[0023] The present invention according to the above scheme is characterized in that the elevator state monitoring module detects the switch state of the elevator door by 2D laser radar scanning;

[0024] When the robot is located at the target point in the elevator or at the target point at the elevator door, the 2D laser radar is activated and scans the range of -30° to +30° in front to obtain ranging data;

[0025] The elevator state detection module determines whether the elevator door is in a fully open state according to the distance measurement data, and sends the result to the task planning module.

[0026] The present invention according to the above solution is characterized in that the process of analyzing the air pressure difference between floors includes:

[0027] Step A1: Record the pressure difference P between each floor in advance ij ,P ij represents the air pressure difference between floor i and floor j;

[0028] Step A2: The robot enters the elevator and records the air pressure value P_now of the current floor;

[0029] Step A3: The target floor pressure value formula P_tar = P_now + P ij ,i=now,j=tar, calculate the target floor air pressure value;

[0030] Step A4: Detect and refresh the air pressure value P_now every 0.5s;

[0031] Step A5: If |P_tar-P_now|<ε, it is preliminarily considered that the target floor has been reached, where ε>0.

[0032] Furthermore, the elevator state detection module is provided with a Bluetooth module, and the Bluetooth module is connected to a Bluetooth chip of one of the first manipulators; the process of obtaining the floor value matching the manipulator at the elevator door opening button includes:

[0033] Step B1, establishing a matching relationship between the ID of the first manipulator module on each floor and the corresponding floor;

[0034] Step B2: When the pressure difference value preliminarily determines that the target floor has been reached, the Bluetooth module is restarted to scan and search for the nearest Bluetooth signal;

[0035] Step B3: When the elevator door is opened, the Bluetooth module connects to the Bluetooth chip with the strongest signal, reads the ID of the corresponding first manipulator module, and obtains the floor corresponding to the ID;

[0036] Step B4: compare the acquired floor with the target floor in the navigation task. If the two are consistent, it means that the elevator has reached the correct target floor.

[0037] Furthermore, in step B4, if the acquired floor is not consistent with the target floor in the navigation task after comparison, step B5 is executed:

[0038] Start the air pressure detection emergency process, detect the current air pressure value, and record it as the check value Pcheck.

[0039] If |Pcheck- P_tar| < ε, the floor value in the initial judgment stage of the air pressure difference shall prevail;

[0040] If |Pcheck- P_tar| > ε, the floor value obtained by the Bluetooth module shall prevail, where ε > 0.

[0041] The present invention according to the above scheme is characterized in that the map management module is also used to set the four turning points of the elevator room as feature points on the constructed maps of each floor.

[0042] Furthermore, the process of aligning the target floor map by the map management module includes:

[0043] Step C1: Select the first layer map as the base map and define the feature points of the base map F = {f 1 , f 2 , f 3 , f 4}, f i ={(x i ,y i )}, f i Indicates the location of the feature point on the map;

[0044] Step C2: define the map of the target floor as the map to be aligned, and define the feature points of the map to be aligned F'={f 1 ´,f 2 ´,f 3 ´,f 4 '}, f i ´={(x i ´,y i ´)}, (x i ´,y i ´) is the position of the i-th feature point on the map to be aligned;

[0045] Step C3: Calculate f by rotation and translation transformation alignment i ´={(x i ´,y i ´)}, the formula is as follows:

[0046] Among them, R is the rotation matrix and t is the translation matrix;

[0047] The least squares method is used to minimize the objective function J to solve R and t. The formula is as follows:

[0048] By finding θ and obtaining the rotation relationship, the map can be aligned.

[0049] The present invention according to the above scheme is characterized in that the path planning module generates a global path on the target floor map according to the starting point and the target point, and then calculates the local path based on the global path; and the global path planning uses the A* algorithm, and the local path planning uses the DWA algorithm.

[0050] The present invention according to the above scheme has the following beneficial effects:

[0051] 1. The present invention no longer uses comprehensive and complex map information, but builds a separate map for each floor, and then provides a navigation map for the robot that reaches the target floor. Specifically, the position relationship between maps of different floors is established through the map management module, and the map of the target floor is prepared in advance for the navigation task. The map management module only performs map switching when the robot reaches the target point in the elevator. The robot's orientation is determined, and the position is determined after the cut target floor map is aligned, ensuring that the mobile robot quickly self-positions after switching the map, solving the problem that the robot cannot quickly reposition after switching the map;

[0052] Furthermore, when establishing the positional relationship between maps of different floors, the present invention uses the turning point of the elevator room as the feature point, takes the first floor elevator as the reference map, and the remaining floor maps as the maps to be aligned. It only needs to use a simple rotation formula to align the maps, and the calculation result is accurate, ensuring the timeliness and stability of map switching;

[0053] 2. The present invention monitors the floor where the elevator is located by correctly judging the floor where the robot is currently located through the elevator status monitoring module, so as to avoid the robot walking out of the elevator when the elevator has not reached the target floor;

[0054] Furthermore, when judging the current floor of the elevator where the robot is located, the present invention combines the air pressure difference judgment method and the Bluetooth connection judgment method to collaboratively judge the current floor of the elevator. The air pressure difference detection is first used as a preliminary judgment to start the Bluetooth module scanning search. There is no need to wait until the elevator door opens. The Bluetooth starts the search in advance. Once the elevator door opens, it quickly connects, thereby improving the efficiency of the Bluetooth connection pairing stage. The Bluetooth module connects to the Bluetooth chip when the elevator door opens, and can more accurately connect to the first manipulator on the floor where the elevator is located, thereby improving the accuracy of the Bluetooth connection pairing stage. Compared with a single judgment method, it can avoid errors in a certain method and give wrong navigation results. Furthermore, the elevator status monitoring module also has an air pressure detection emergency process, which self-checks abnormal results and gives the verified floor value.

[0055] 3. The robot of the present invention no longer uses the intervention method of responding to the elevator control mainboard to control the elevator, but uses the manipulator at the elevator button to achieve control instructions issued by the task planning module, and the corresponding button manipulator imitates the human hand to press the required elevator button;

[0056] 4. The present invention decomposes the entire process of full-field cross-floor autonomous navigation through the task planning module, detects the execution status of each module in real time and schedules each module to complete the task, so that each module runs smoothly, providing a guarantee for the robot to successfully complete full-field cross-floor autonomous navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 is a system flow chart of the present invention;

[0058] Figure 2 It is a flowchart of the task planning module in the present invention;

[0059] Figure 3 This is a simulation diagram of the elevator door closing;

[0060] Figure 4 This is the LiDAR detection image of the elevator door closed;

[0061] Figure 5 This is a simulation diagram of the elevator door being half-opened;

[0062] Figure 6 This is the LiDAR detection image of the elevator door half-opened;

[0063] Figure 7 This is a simulation diagram of the elevator door being fully opened;

[0064] Figure 8 This is the laser radar detection image of the elevator door fully opened;

[0065] Fig. 9This is a flow chart of the process of detecting the floor the elevator is currently on;

[0066] Fig.10 This is a flowchart of the map management module;

[0067] Fig.11 The target floor is not aligned with the reference map;

[0068] Fig.12 The state of the target floor being aligned with the reference map. DETAILED DESCRIPTION

[0069] In order to better understand the purpose, technical scheme and technical effect of the present invention, the present invention is further explained in conjunction with the accompanying drawings and embodiments. It should be noted that similar numbers and letters represent similar items in the following drawings. Therefore, once an item is defined in one drawing, it does not need to be further defined and explained in the subsequent drawings. At the same time, it is stated that the embodiments described below are only used to explain the present invention and are not used to limit the present invention.

[0070] It should be noted that when an element is referred to as being "fixed on" or "disposed on" another element, it may be directly on the other element or there may also be a central element, and when an element is considered to be "connected to" another element, it may be directly connected to the other element or there may be a central element at the same time.

[0071] The indicated orientation or position relationship is based on the orientation or position relationship shown in the drawings, or the orientation or position relationship in which the product of the application is usually placed when used, or the orientation or position relationship commonly understood by technical personnel in this field, or the orientation or position relationship in which the product of the application is usually placed when used. It is only for the convenience of describing the present application and simplifying the description, and does not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present application.

[0072] The terms "first" and "second" are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of technical features. "Several" means two or more, unless otherwise clearly and specifically defined.

[0073] like Figure 1 and Figure 2 As shown, a method for autonomous navigation of a cross-floor full-area mobile robot includes: a task planning module, a robot movement module, a manipulator module, an elevator status monitoring module, a map management module and a path planning module.

[0074] The task planning module is used to decompose the navigation task into several subtasks, select the appropriate subtask process according to the comparison result between the target floor and the current floor, and send the corresponding subtask instructions to control the robot to move, control the first manipulator to press the elevator door opening button, and control the second manipulator to select the elevator floor button. The task planning module is also used to monitor the execution of each module in real time. The robot's movement is controlled by the robot movement module, and the first manipulator and the second manipulator are controlled by the corresponding manipulator modules.

[0075] The navigation task has target position information, and the target position information includes at least floor information and coordinate information, such as L2 (5, 6), which means the intersection of the horizontal coordinate of 5 meters and the vertical coordinate of 6 meters in the 2nd floor. In this embodiment, the direction of the horizontal coordinate is parallel to the direction of the elevator door, the direction of the vertical coordinate is perpendicular to the direction of the elevator door, and the coordinate origin is the midpoint of the elevator room. In other optional embodiments, the horizontal and vertical coordinates can be adjusted to other suitable directions, and the zero point position can also be set in the elevator room or other places on the floor. It should be noted that a robot arrival point has been defined in any office area on each floor. When the robot reaches the navigation end point, it will park at the defined point instead of parking randomly in the target area. In addition, the system has entered the coordinate information of the arrival points of all office areas. The user only needs to select the office area as the navigation end point in the operation interface. The system can read the corresponding coordinate information and send it to the robot mobile module without accurately entering the coordinate position.

[0076] The navigation task can be broken down into five subtasks, including:

[0077] Subtask 1: Navigate and control the robot to move to the target point on the same floor;

[0078] Subtask 2: Navigate and control the robot to move to the target point at the elevator door of the current floor, and command the robot to wait for the elevator door to open;

[0079] Subtask 3: Control the first manipulator to press the elevator door opening button (up button or down button);

[0080] Subtask 4: Navigate and control the robot to move to the target point in the elevator, and command the robot to wait for the elevator door to open;

[0081] Subtask 5 - Control the second robot to press the elevator floor button.

[0082] The task planning module determines whether the task target floor is consistent with the robot's current floor, and selects subtask 1 or subtask 2-3-4-5-1 process based on the judgment result.

[0083] When it is determined that the target floor is the same floor as the robot's current floor, an instruction to execute subtask 1 is sent to the robot movement module. The robot movement module executes subtask 1, controls the robot to move to the target position, that is, the coordinate information of the arrival point in the office area, and completes the navigation task.

[0084] When it is determined that the target floor is not the same floor as the robot's current floor, the robot movement module is sent an instruction to execute subtask 2, and the robot movement module executes subtask 2 to control the robot to move to the elevator door. After the robot arrives at the elevator door, the robot movement module receives a standstill instruction so that the robot can stand still before the elevator door opens. After the robot movement module executes subtask 2, the task planning module sends the instructions of subtask 3, subtask 4, subtask 5, and subtask 1 in sequence. The detailed process is as follows.

[0085] The task planning module sends an instruction to execute subtask 3 to the first manipulator module. The instruction includes an instruction to select the correct elevator door opening key. Specifically, the task planning module obtains the size comparison result between the target floor and the current floor based on the previous judgment information, and issues an instruction to select the correct elevator door opening key based on the size comparison result. If the target floor is larger than the current floor, the first manipulator selects the up key; if the target floor is smaller than the current floor, the first manipulator selects the down key.

[0086] The robot moves to the elevator door, and the task planning module sends a command to start the elevator status monitoring module. When the elevator status monitoring module detects that the elevator door is open, it feeds back this information to the task planning module. The task planning module sends a command to execute subtask 4 to the robot movement module. The robot movement module executes subtask 4 and controls the robot to move to the target point in the elevator. The target point in the elevator is generally set in the middle of the elevator to facilitate the laser radar to scan the elevator door status.

[0087] After the robot reaches the target point in the elevator, it automatically turns around to point the laser radar installed on the front side of the robot toward the elevator door. Then, the robot movement module receives a standstill command so that the robot can stand still before the elevator door opens.

[0088] At this time, the task planning module sends an instruction to execute subtask 5 to the second manipulator module. The instruction contains the target floor information, and the second manipulator module selects the correct floor button based on the information.

[0089] The robot moves into the elevator, and the task planning module sends a command to start the elevator status monitoring module. When the elevator status monitoring module detects that the elevator door is open, it feeds back this information to the task planning module. The task planning module sends a command to execute subtask 1 to the robot movement module. The robot movement module executes subtask 1, controls the robot to move to the target position, and completes the navigation task.

[0090] Among them, the coordinate information of the target point at the elevator door and the target point inside the elevator have also been entered into the system. After receiving relevant instructions, the robot can move to the corresponding coordinate position.

[0091] like Figures 3 to 8 As shown, it can be seen that the first function of the elevator status monitoring module is to detect the status of the elevator door opening and closing. Specifically, a 2D laser radar is used to determine the status of the elevator door opening and closing. 2D laser radar data has the characteristics of high precision and high stability, and is suitable for indoor ranging. First, after the robot 200 moves to the set target point, it stands in position and faces the detection port to the elevator door 100, so that the laser radar data of the elevator door status is obtained within the detection direction range of 30° to -30°. During the process of opening the elevator door, the laser radar data changes, and the data processing principle is as follows: the laser radar data is converted from polar coordinates to Cartesian coordinates, the X-axis represents the ranging data index within the range of 30° to -30° in the laser radar detection direction, and the Y-axis represents the measured distance value, refer to Figure 3 , Figure 5 and Figure 7 , which respectively represent the simulation diagrams of the elevator door states when the elevator door 100 is closed, the elevator door 100 is 50% open, and the elevator door 100 is 100% open; refer to Figure 4 , Figure 6 and Figure 8 , respectively representing the laser radar detection images when the elevator door is closed, the elevator door is 50% open, and the elevator door is 100% open. And the simulation image is displayed on the display interface of the robot 200. The visualization of the elevator state simulation image is convenient for the user to intuitively view and understand the operation status of the robot 200 at any time.

[0092] Depend on Figure 6 It can be seen that when the elevator door is not fully opened, the value measured by the 2D laser radar scanning has a distance difference and cannot be stably greater than a certain threshold. Figure 8 It can be seen that when the elevator door is fully opened, the value measured by the 2D LiDAR scan is always higher than a certain threshold. When the threshold is initially set, the elevator door is fully opened, the LiDAR obtains a measurement value, and the threshold is set to be slightly smaller than the measurement value. In subsequent use, if the 2D LiDAR scan distance measurement value remains steadily greater than the threshold, the elevator door is judged to be open, that is, Figure 8 Show the situation.

[0093] In order to prevent the robot from walking out of the elevator before the elevator reaches the target floor, the elevator status monitoring module can also correctly determine the current floor of the elevator where the robot is located. That is, the second function of the elevator status monitoring module is to detect the current floor of the elevator, including the preliminary judgment stage of the air pressure difference and the Bluetooth connection pairing stage.

[0094] like Fig. 9As shown, the initial judgment stage of the air pressure difference is:

[0095] Step A1: Record the pressure difference P between each floor in advance ij ,P ij represents the air pressure difference between floor i and floor j;

[0096] Step A2: The robot enters the elevator and records the air pressure value P_now of the current floor;

[0097] Step A3: The target floor pressure value formula P_tar = P_now + P ij ,i=now,j=tar, calculate the target floor air pressure value;

[0098] Step A4: Detect and refresh the air pressure value P_now every 0.5s;

[0099] Step A5: If |P_tar-P_now|<ε, ε is defined as a determination threshold greater than 0, it is preliminarily considered that the target floor has been reached; wherein, the smaller the set determination threshold ε is, the more accurate the determination result is, but the higher the air pressure detection accuracy requirement is.

[0100] About the Bluetooth connection pairing stage:

[0101] First, the first manipulator module of each floor has a unique ID and is matched with the corresponding floor; each first manipulator module has a built-in Bluetooth chip. After the robot's Bluetooth module is connected to the Bluetooth chip, it can obtain the ID of the corresponding first manipulator module to determine the floor it is on. This process does not require the participation of the elevator control mainboard. It can be divided into the following steps:

[0102] Step B1, establishing a matching relationship between the ID of the first manipulator module on each floor and the corresponding floor;

[0103] For example: the ID of the first robot on the second floor can index the floor L2, the ID of the first robot on the third floor can index the floor L3, the ID of the first robot on the fourth floor can index the floor L4, and so on.

[0104] Step B2: When the pressure difference value preliminarily determines that the target floor has been reached, the Bluetooth module is restarted to scan and search for the nearest Bluetooth signal;

[0105] Step B3: When the elevator door is opened, the Bluetooth module connects to the Bluetooth chip with the strongest signal, reads the ID of the corresponding first manipulator module, and obtains the floor corresponding to the ID;

[0106] Step B4: compare the acquired floor with the target floor in the navigation task. If the two are consistent, it means that the elevator has reached the correct target floor. If the two are inconsistent, execute step B5.

[0107] Step B5, start the air pressure detection emergency process, detect the current air pressure value, record it as the check value Pcheck, if |Pcheck- P_tar| < ε, the floor value in the initial judgment stage of the air pressure difference is used as the standard; if |Pcheck- P_tar| > ε, the floor value obtained by the Bluetooth module is used as the standard.

[0108] The present invention first uses air pressure difference detection as a preliminary judgment to start the Bluetooth module scanning search. There is no need to wait until the elevator door opens. The Bluetooth starts the search in advance. Once the elevator door opens, it quickly connects, thereby improving the efficiency of the Bluetooth connection pairing stage. The Bluetooth module connects to the Bluetooth chip when the elevator door opens, and can more accurately connect to the first manipulator on the floor where the elevator is located, thereby improving the accuracy of the Bluetooth connection pairing stage. The present invention combines the air pressure difference judgment method and the Bluetooth connection judgment method to collaboratively judge the current floor of the elevator. Compared with a single judgment method, it can avoid errors in a certain method and give erroneous navigation results; finally, through the air pressure detection emergency process, the abnormal results are self-checked and the verified floor value is given.

[0109] The present invention builds a separate map for each floor and adopts a map switching method to provide the robot with a navigation map of the target floor without processing complex map navigation information. Moreover, the present invention can quickly and accurately switch to the map of the target floor through the map management module, and ensure that the robot quickly self-positions after switching the map.

[0110] like Figures 10 to 12 As shown in the figure, the map management module uses the Cartographer algorithm to build a complete global simulation map for each floor. The algorithm is an existing mature technology, and the specific process and principle are not described in detail. Using the Cartographer mapping method, the direction information of each floor map depends on the initial position of the mobile robot when building the map, so there is directional uncertainty in the maps of each floor. Subsequently, the map management module sets feature points in the global simulation map of each floor and establishes the positional relationship between the maps of different floors. Because the structure of the elevator and the elevator room will not change in a multi-story building, the inflection point of the elevator room is used as a feature point to align the cut-out floor map with the reference map.

[0111] Specifically, in each floor map, the four turning points of the elevator room are set as feature points ( Fig.11 and Fig.12 The four marked points in the figure represent the feature points); select the first-layer map as the reference map, and define the feature points of the reference map F={f 1 , f 2 , f 3 , f 4}, f i ={(x i ,yi )}, f i represents the position of the feature point on the map; the map of the target floor is used as the map to be aligned, and the feature point F'={f 1 ´,f 2 ´,f 3 ´,f 4 '}, f i ´={(x i ´,y i ´)}, (x i ´,y i ´) is the position of the i-th feature point on the map to be aligned; f i ´={(x i ´,y i ´)} is calculated by rotation and translation transformation alignment, the formula is as follows:

[0112] Among them, R is the rotation matrix and t is the translation matrix.

[0113] R and t can be solved by minimizing the objective function J through the least squares method. The formula is as follows:

[0114] By finding θ and obtaining the rotation relationship, the map can be aligned. Fig.11 Indicates the state when the target floor Ln map (below) is not aligned with the ground floor L1 map (above). Fig.12 Indicates the state when the target floor Ln map (lower figure) is aligned with the first floor L1 map (upper figure).

[0115] In addition, when the robot reaches the target point in the elevator, the task planning module sends instructions to the map management module. The map management module will switch the map only after receiving the instructions. It can be seen that the map management module will switch the map only when the robot reaches the target point in the elevator. At this time, the robot's orientation is determined, and the position of the cut-out floor map is also determined after alignment. The navigation route autonomously planned by the robot between the elevator and the target coordinates can accurately correspond to the floor where it is located, ensuring that the mobile robot can quickly self-position after switching the map.

[0116] When the robot reaches the target floor, the task planning module sends instructions to the path planning module. The path planning module of the system provides a navigation route for the robot. Specifically, a global path is generated based on the starting point and the target point, and then the algorithm is used to calculate the optimal linear velocity and angular velocity in the local stage to generate a local path plan. The global path planning can provide the optimal path from the starting point to the end point, while the local path planning can respond quickly to the instant changes in the robot's current position and environment. When the environment changes or encounters obstacles, the local path planning can enable the robot to avoid obstacles and re-plan the path, while the global path planning ensures that the robot will not deviate too far from the final goal. During the process of planning the path movement, the system obtains the real-time positioning of the robot through the adaptive Monte Carlo positioning algorithm.

[0117] The global path planning of the present invention uses the A* algorithm, and the local path planning uses the DWA algorithm, wherein the A* (A-Star) algorithm is the most effective direct search method for solving the shortest path in a static road network, and is widely used in indoor robot path search, game animation path search, etc.; the principle of the DWA (Dynamic Window Approach) algorithm is mainly to sample multiple groups of speeds in the speed space (v, w) with a certain resolution, and simulate the motion trajectory of these speeds in a certain time. After obtaining the feasible trajectory, the trajectory is scored by the evaluation function, and after the scoring, the (v, w) corresponding to the optimal trajectory is selected to drive the robot to move. These two technologies are mature existing technologies, and their principles and algorithm processes are not described in detail.

[0118] In the present invention, the mobile robot (referred to as the robot) includes a host computer, a control panel and a robot chassis, the task planning module, the map management module and the path planning module are all arranged on the host computer, the elevator state monitoring module and the robot movement module are arranged on the control panel, wherein the robot movement module is used to control the movement of the robot chassis, including the movement speed and the movement direction. The response of the robot movement module comes from the host computer of the robot, and can receive the instruction of the task planning module of the host computer. The elevator state monitoring module includes a 2D laser radar, an air pressure detector and a Bluetooth module, the 2D laser radar and the air pressure detector are arranged on the robot chassis and are electrically connected to the control panel; the Bluetooth module is integrated on the control panel.

[0119] The first manipulator and the second manipulator are independent manipulators outside the robot, wherein the first manipulator module is installed next to the up and down elevator buttons outside the elevator, and the second manipulator module is installed next to the floor buttons inside the elevator. Both manipulator modules are connected to the mobile robot through low-power Bluetooth communication, and are used to assist the robot in completing the operation of pressing elevator-related buttons, so as to achieve cross-floor movement in a multi-story building. It can be seen that the robot of the present invention no longer controls the elevator by interacting with the elevator control motherboard, but realizes precise operation of the elevator buttons through the intelligent manipulator, which reduces potential technical risks, and also reduces dependence on the elevator control system, ensuring that the normal operation of the elevator is not disturbed.

[0120] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0121] The above embodiments only express several implementation methods of the present invention, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for those of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present invention, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention shall be subject to the attached claims.

Claims

1. A method for autonomous navigation of a mobile robot across floors and in the entire field, characterized in that: It includes task planning module, robot movement module, manipulator module, elevator status monitoring module, map management module and path planning module; The task planning module is used to decompose the navigation task into five subtasks, select the appropriate subtask according to the comparison result between the target floor and the current floor, and send control instructions to the robot movement module, the manipulator module, the elevator status monitoring module, the map management module and the path planning module; The five subtasks include: Subtask 1: Navigate and control the robot to move to the target point on the same floor; Subtask 2: Navigate and control the robot to move to the target point at the elevator door of the current floor, and command the robot to wait for the elevator door to open; Subtask 3: Control the first manipulator to press the elevator door opening button; Subtask 4: Navigate and control the robot to move to the target point in the elevator, and command the robot to wait for the elevator door to open; Subtask 5: Control the second manipulator to press the elevator floor button; And when the target floor is equal to the current floor, call subtask 1; when the target floor is not equal to the current floor, call subtask 2, subtask 3, subtask 4, subtask 5, and subtask 1 in sequence; The robot movement module is used to receive instructions and control the robot movement; the manipulator module includes a first manipulator module and a second manipulator module, which respectively control the corresponding manipulators to press the elevator door opening button and the elevator floor button; The first manipulator module is installed next to the up and down elevator buttons outside the elevator, and the second manipulator module is installed next to the floor buttons inside the elevator, and both are connected to the mobile robot through low-power Bluetooth communication; The elevator state monitoring module is used to detect the switch state of the elevator door and determine the elevator floor where the robot is located, and the step of determining the elevator floor where the robot is located includes analyzing the air pressure difference between floors and obtaining the floor value matching the manipulator at the elevator door opening button; The process of analyzing the air pressure difference between floors includes: Step A1: Record the pressure difference P between each floor in advance ij ,P ij represents the air pressure difference between floor i and floor j; Step A2: The robot enters the elevator and records the air pressure value P_now of the current floor; Step A3: The target floor pressure value formula P_tar = P_now + P ij ,i=now,j=tar, calculate the target floor air pressure value; Step A4: Detect and refresh the air pressure value P_now every 0.5s; Step A5: If |P_tar-P_now| < ε, it is preliminarily considered that the target floor has been reached, where ε > 0; The elevator status monitoring module is provided with a Bluetooth module, and the Bluetooth module is connected to a Bluetooth chip of one of the first manipulators; the process of obtaining the floor value matched with the manipulator at the elevator door opening button includes: Step B1, establishing a matching relationship between the ID of the first manipulator module on each floor and the corresponding floor; Step B2: When the pressure difference value preliminarily determines that the target floor has been reached, the Bluetooth module is restarted to scan and search for the nearest Bluetooth signal; Step B3: When the elevator door is opened, the Bluetooth module connects to the Bluetooth chip with the strongest signal, reads the ID of the corresponding first manipulator module, and obtains the floor corresponding to the ID; Step B4: compare the acquired floor with the target floor in the navigation task. If the two are consistent, it means that the elevator has reached the correct target floor. If the acquired floor is not consistent with the target floor in the navigation task after comparison, execute step B5: Start the air pressure detection emergency process, detect the current air pressure value, and record it as the check value Pcheck. If |Pcheck- P_tar| < ε, the floor value in the initial judgment stage of the air pressure difference shall prevail; If |Pcheck- P_tar| > ε, the floor value obtained by the Bluetooth module shall prevail, where ε > 0; The map management module is used to construct a floor map, establish a positional relationship between different floor maps to align and adjust the orientation of the target floor map, and execute map switching when the robot reaches a specified position in the elevator to prepare the target floor map in advance for the navigation task; The path planning module is used to provide a navigation route for a target floor map.

2. The cross-floor full-area mobile robot autonomous navigation method according to claim 1 is characterized in that: The navigation task includes target location information, and the target location information includes floor information and coordinate information.

3. The cross-floor full-area mobile robot autonomous navigation method according to claim 1 is characterized in that: The task planning module calls the subtask 3, including: the task planning module obtains the size comparison result between the target floor and the current floor, issues instructions according to the size comparison result, and controls the first manipulator to select the correct elevator door opening button, and the elevator door opening button includes an up button and a down button.

4. The cross-floor full-area mobile robot autonomous navigation method according to claim 1 is characterized in that: The elevator status monitoring module detects the switch status of the elevator door by 2D laser radar scanning; When the robot is located at the target point in the elevator or at the target point at the elevator door, the 2D laser radar is activated and scans the range of -30° to +30° in front to obtain ranging data; The elevator state monitoring module determines whether the elevator door is in a fully open state according to the distance measurement data, and sends the result to the task planning module.

5. The cross-floor full-area mobile robot autonomous navigation method according to claim 1 is characterized in that: The map management module is also used to set the four turning points of the elevator room as feature points on the constructed maps of each floor.

6. The cross-floor full-area mobile robot autonomous navigation method according to claim 5 is characterized in that: The process of aligning the target floor map by the map management module includes: Step C1: Select the first layer map as the reference map and define the feature points of the reference map F = {f1, f2, f3, f4}, f i ={(x i ,y i )}, f i Indicates the location of the feature point on the map; Step C2: define the map of the target floor as the map to be aligned, and define the feature points of the map to be aligned F'={f1', f2', f3', f4'}, f i ´={(x i ´,y i ´)}, (x i ´,y i ´) is the position of the i-th feature point on the map to be aligned; Step C3: Calculate f by rotation and translation transformation alignment i ´={(x i ´,y i ´)}, the formula is as follows: Among them, R is the rotation matrix and t is the translation matrix; The least squares method is used to minimize the objective function J to solve R and t. The formula is as follows: By finding θ and obtaining the rotation relationship, the map can be aligned.

7. The cross-floor full-area mobile robot autonomous navigation method according to claim 1 is characterized in that: The path planning module generates a global path on the target floor map according to the starting point and the target point, and then calculates the local path based on the global path; and the global path planning uses the A* algorithm, and the local path planning uses the DWA algorithm.

Images