Sweeping robot path planning control system and method

Abstract

The invention relates to a sweeping robot path planning control system which comprises a motion module and a sweeping module and further comprises a position sensing module used for detecting the distance between a sweeping robot and surrounding obstacles; the attitude sensing module is used for acquiring a yaw angle of the sweeping robot; the collision sensor is circumferentially arranged on the side of the sweeping robot and used for sensing collision of the sweeping robot; and the main control module is used for controlling the motion module to advance, retreat or turn according to information transmitted by the position sensing module, the posture sensing module and the collision sensor. According to the invention, the defects of low intelligence and low practicability of a floor sweeping robot manufactured by a single infrared sensor or ultrasonic sensor in the market are overcome; an STM32 single chip microcomputer is adopted as a main processor, the coverage rate and the real-time performance are improved, accurate obstacle avoidance is achieved, and meanwhile the requirements for low cost and high performance are met; and the collision sensor is adopted, so that the sweeping robot stops in time when colliding with an obstacle, and the service life of the device is prolonged.

Description

Technical Field

[0001] The present invention relates to the field of intelligent control technology, and in particular to a sweeping robot path planning control system and method. Background Art

[0002] With the continuous development of science and technology, smart home appliances are also changing with each passing day. As a kind of service robot, sweeping robots have now penetrated into people's home life and can clean the floors of homes and public places. Floor cleaning is a highly repetitive and time-consuming job. The emergence of sweeping robots has largely liberated people from the tedious cleaning work, saving labor costs and improving work efficiency, and has a good development prospect.

[0003] At present, there are relatively few intelligent sweeping robots on the market that are cost-effective and accepted by people. There are sweeping robots on the market that are made of a single infrared sensor or ultrasonic sensor. Although they are cheap, they are not intelligent, resulting in incomplete cleaning and low practicality. There are also sweeping robots on the market that use multiple high-performance sensor fusions, involving map construction (SLAM technology), path planning, operating systems, deep learning, communication transmission and many other contents. The cost of software and hardware is too high, resulting in a very expensive price and is not accepted by people. Therefore, designing a high-performance, low-cost sweeping robot path planning control system has become a problem that needs to be solved urgently by technicians in this field. Summary of the invention

[0004] The technical problem to be solved by the present invention is to overcome the deficiencies in the prior art and provide a path planning control system and method for a sweeping robot.

[0005] The present invention is achieved through the following technical solutions:

[0006] A path planning and control system for a sweeping robot, comprising a motion module and a cleaning module, and further comprising:

[0007] The position sensing module is used to detect the distance between the sweeping robot and surrounding obstacles and transmit it to the main control module;

[0008] The posture perception module is used to obtain the yaw angle of the sweeping robot and transmit it to the main control module;

[0009] Collision sensors are arranged around the sides of the sweeping robot to sense collisions of the sweeping robot and transmit the information to the main control module;

[0010] The main control module is electrically connected to the motion module, the cleaning module, the position sensing module, the posture sensing module, and the collision sensor, and is used to control the motion module to move forward, backward, or turn according to the information transmitted by the position sensing module, the posture sensing module, and the collision sensor.

[0011] According to the above technical solution, preferably, the motion module includes:

[0012] A motor connected to the driving wheel, used to drive the sweeping robot forward, backward or turn through the main control module;

[0013] A rotary steering device is used to control the sweeping robot to turn through a motor;

[0014] An encoder, mounted on the motor, for obtaining the speed of the driving wheel by reading the number of pulses of the encoder;

[0015] Driving wheels are used to drive the sweeping robot forward, backward or turn.

[0016] According to the above technical solution, preferably, the main control module is an STM32 main control chip.

[0017] According to the above technical solution, preferably, the position sensing module includes an ultrasonic sensor module and an infrared sensor module.

[0018] According to the above technical solution, preferably, three infrared sensor modules are provided and respectively installed at the front ends of the sweeping robot, and the ultrasonic sensor module is installed above the sweeping robot.

[0019] According to the above technical solution, preferably, the posture perception module includes an MPU6050 six-axis sensor.

[0020] According to the above technical solution, preferably, it also includes a power supply module for supplying power to the main control module, the motion module, the cleaning module, the position sensing module, the posture sensing module, and the collision sensor.

[0021] This patent also discloses a path planning control method for a sweeping robot, which is based on the above-mentioned path planning control system for a sweeping robot and includes the following steps:

[0022] S1. The position sensing module detects the distance between the sweeping robot and the surrounding obstacles and transmits it to the main control module;

[0023] S2. The main control module determines whether the distance is less than the safety distance threshold and controls the motion module to work;

[0024] S3. When the sweeping robot collides with an obstacle, the collision sensor senses the collision of the sweeping robot and transmits it to the main control module. The main control unit controls the motion module to stop and retreat, and at the same time controls the motion module to rotate the sweeping robot to continue to move forward;

[0025] S4. The posture perception module obtains the yaw angle of the sweeping robot and transmits it to the main control module; the main control module adjusts the rotation speed of the two driving wheels to correct the yaw according to the yaw angle in cooperation with the encoder of the motion module.

[0026] According to the above technical solution, preferably, step S2 includes: when the position sensing module does not detect the existence of an obstacle in front, the sweeping robot moves in a straight line; when the position sensing module detects the existence of an obstacle in front, the main control module determines whether the distance is less than the safety distance threshold; when the main control module determines that the distance is not less than the safety distance threshold, the sweeping robot moves in a straight line; when the main control module determines that the distance is less than the safety distance threshold, the sweeping robot slows down and turns, and then continues to move forward.

[0027] The beneficial effects of the present invention are:

[0028] The present invention solves the shortcomings of low intelligence, incomplete cleaning and low practicality of sweeping robots on the market which are manufactured with single infrared sensors or ultrasonic sensors through the coordinated use of ultrasonic sensors and infrared sensors; at the same time, the present invention adopts the STM32 single-chip microcomputer as the main processor to improve the coverage and real-time performance, realize accurate obstacle avoidance, and also meet the requirements of low cost and high performance; in addition, the present invention adopts a collision sensor to make the sweeping robot stop in time when it collides with an obstacle, thereby avoiding damage to the sweeping robot caused by continuous collision, thereby improving the service life of the device and having a high market promotion value. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 It is a principle block diagram of the path planning control system of the present invention;

[0030] Figure 2 is a system flow chart of the path planning control method of the present invention;

[0031] Figure 3 It is a flowchart of the driver program of the motion module of the present invention;

[0032] Figure 4 It is a flow chart of ultrasonic ranging of the position sensing module of the present invention. DETAILED DESCRIPTION

[0033] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings and the best embodiment. Based on the embodiments in the invention, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the invention.

[0034] Embodiment 1:

[0035] like Figure 1 As shown, the present invention includes:

[0036] (1) Position sensing module, used to detect the distance between the sweeping robot and the surrounding obstacles and transmit it to the main control module. The position sensing module includes an ultrasonic sensor module and an infrared sensor module. In this example, there are three infrared sensor modules (Sharp GP2Y0E03KOF infrared ranging sensors), which are installed at the front end of the sweeping robot (the interval angle of each infrared sensor is 40 degrees) to detect the environment within a range of 120 degrees in front. The ultrasonic sensor module (HC-SR04 ultrasonic sensor) is installed above the sweeping robot to obtain the obstacles around the sweeping robot and the distance to the obstacles. Figure 4 This is the flowchart of ultrasonic ranging. First, initialize each module. The ultrasonic sensor sends a signal to detect whether an echo signal is received. If not, continue to trigger. When the ultrasonic sensor sends a signal, the microcontroller starts timing. When the echo signal is received, the timing ends. The formula is: s=340t / 2, where s is the distance and t is the time counted by the timer.

[0037] (2) A posture sensing module, which is used to obtain the yaw angle of the sweeping robot and transmit it to the main control module. The posture sensing module is preferably but not limited to the MPU6050 six-axis sensor. The MPU6050 six-axis sensor needs to be firmly connected to the sweeping robot. The layout position connected to the fuselage must also be located at the center of gravity. Since the sweeping robot mainly moves in a two-dimensional plane, the main purpose is to detect the yaw angle and angular acceleration of the sweeping robot, so the pitch angle and angular acceleration perpendicular to the ground should be zero. The motion data measurement of the pitch angle is to ensure that the sweeping robot is not in a lifted or fallen state. At the same time, the yaw angle detected in real time by the MPU6050 sensor should be used in conjunction with the encoder to adjust the rotation speed of the two wheels to correct the yaw and achieve straight-line driving.

[0038] (3) Collision sensors are arranged around the sides of the sweeping robot to sense collisions of the sweeping robot and transmit the information to the main control module. In this example, preferably, but not limited to, five collision sensors are arranged at the front, left front, left rear, right front and right rear of the sweeping robot. When the sweeping robot collides with an obstacle, the collision switch RS216 is triggered, the robot stops running and moves back a certain distance. The sweeping robot rotates to a suitable angle and continues running, thus avoiding damage caused by another collision and increasing the service life of the sweeping robot.

[0039] (4) The main control module is preferably an STM32 main control chip (STM32F103C8T6 single chip microcomputer), which is electrically connected to the motion module, the cleaning module, the position sensing module, the posture sensing module, and the collision sensor, and is used to control the motion module to move forward, backward, or turn according to the information transmitted by the position sensing module, the posture sensing module, and the collision sensor. The STM32 main control chip is used to integrate the data of multiple sensors, thereby improving accuracy and real-time performance.

[0040] (5) A motion module, comprising: a motor connected to a driving wheel, used to drive the sweeping robot forward, backward or turn through the main control module; a rotary steering gear, used to control the sweeping robot to turn through the motor; an encoder, installed on the motor, used to obtain the speed of the driving wheel by reading the number of pulses of the encoder; and a driving wheel, used to drive the sweeping robot forward, backward or turn.

[0041] The current driving wheel speed is calculated by reading the number of pulses of the encoder. The formula is: driving wheel speed = the distance traveled by the robot after one pulse × program refresh frequency × number of pulses × resolution. Figure 3 As shown, the speed of the driving wheel of the sweeping robot is achieved through pulse width modulation. When the left wheel speed = the right wheel speed, the sweeping robot moves forward or backward. When the left wheel speed ≠ the right wheel speed, the robot turns left or right differentially. When the left wheel speed > the right wheel speed, the robot turns right on the spot. When the left wheel speed < the right wheel speed, the robot turns left on the spot.

[0042] (6) A power module, used to supply power to the main control module, motion module, cleaning module, position sensing module, posture sensing module, and collision sensor.

[0043] (7) Cleaning module, used for cleaning garbage.

[0044] Embodiment 2:

[0045] This patent also discloses a path planning control method for a sweeping robot, which is based on the above-mentioned path planning control system for a sweeping robot and includes the following steps:

[0046] S1. The position sensing module detects the distance between the sweeping robot and the surrounding obstacles and transmits it to the main control module;

[0047] S2. The main control module determines whether the distance is less than the safety distance threshold and controls the motion module to work;

[0048] S3. When the sweeping robot collides with an obstacle, the collision sensor senses the collision of the sweeping robot and transmits it to the main control module. The main control unit controls the motion module to stop and retreat, and at the same time controls the motion module to rotate the sweeping robot to continue to move forward;

[0049] S4. The posture perception module obtains the yaw angle of the sweeping robot and transmits it to the main control module; the main control module adjusts the rotation speed of the two driving wheels to correct the yaw according to the yaw angle in cooperation with the encoder of the motion module.

[0050] Among them, step S2 includes: when the position sensing module does not detect the existence of an obstacle in front, the sweeping robot moves in a straight line; when the position sensing module detects the existence of an obstacle in front, the main control module determines whether the distance is less than the safety distance threshold; when the main control module determines that the distance is not less than the safety distance threshold, the sweeping robot moves in a straight line; when the main control module determines that the distance is less than the safety distance threshold, the sweeping robot slows down and turns, and then continues to move forward.

[0051] In actual use, the system is first powered on, each module is initialized, and the ultrasonic sensor and infrared sensor are used together. First, the microcontroller can receive the infrared signal. If not, the ultrasonic sensor starts working; the main control chip STM32 fuses the data detected by each sensor, and performs real-time path planning for multiple obstacles indoors; when the sensor detects that there is no obstacle in front, the sweeping robot will walk in a straight line, but when an obstacle appears in front, it will determine whether the obstacle distance is less than the safety distance threshold. If not, the sweeping robot will continue to drive in a straight line. If it is less, it will determine whether it collides with the obstacle. If no collision occurs, the robot will slow down, turn, and continue cleaning. If a collision occurs, the robot will stop moving forward, retreat a distance, and then turn to continue cleaning. This process continues until the cleaning is completed.

[0052] The present invention solves the shortcomings of low intelligence, incomplete cleaning and low practicality of sweeping robots on the market which are manufactured with single infrared sensors or ultrasonic sensors through the coordinated use of ultrasonic sensors and infrared sensors; at the same time, the present invention adopts the STM32 single-chip microcomputer as the main processor to improve the coverage and real-time performance, realize accurate obstacle avoidance, and also meet the requirements of low cost and high performance; in addition, the present invention adopts a collision sensor to make the sweeping robot stop in time when it collides with an obstacle, thereby avoiding damage to the sweeping robot caused by continuous collision, thereby improving the service life of the device and having a high market promotion value.

[0053] Through the description of the above implementation mode, it can be clearly understood by those skilled in the art that the embodiments described herein can be implemented in hardware, software, firmware, middleware, code or any appropriate combination thereof. For hardware implementation, the processor can be implemented in one or more of the following units: application specific integrated circuit (ASIC), digital signal processor (DSP), digital signal processing device (DSPD), programmable logic device (PLD), field programmable gate array (FPGA), processor, controller, microcontroller, microprocessor, other electronic units designed to implement the functions described herein or their combination. For software implementation, part or all of the process of the embodiment can be completed by instructing the relevant hardware through a computer program. When implemented, the above program can be stored in a computer-readable medium or transmitted as one or more instructions or codes on a computer-readable medium. Computer-readable media include computer storage media and communication media, wherein the communication medium includes any medium that facilitates the transmission of a computer program from one place to another. The storage medium can be any available medium that can be accessed by a computer. Computer-readable media can include, but are not limited to, RAM, ROM, EEPROM, CD-ROM or other optical disk storage, disk storage media or other magnetic storage devices, or any other medium that can be used to carry or store the desired program code in the form of instructions or data structures and can be accessed by a computer.

[0054] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A path planning and control system for a sweeping robot, comprising a motion module and a cleaning module, characterized in that: Also includes: The position sensing module is used to detect the distance between the sweeping robot and surrounding obstacles and transmit it to the main control module; The posture perception module is used to obtain the yaw angle of the sweeping robot and transmit it to the main control module; Collision sensors are arranged around the sides of the sweeping robot to sense collisions of the sweeping robot and transmit the information to the main control module; The main control module is electrically connected to the motion module, the cleaning module, the position sensing module, the posture sensing module, and the collision sensor, and is used to control the motion module to move forward, backward, or turn according to the information transmitted by the position sensing module, the posture sensing module, and the collision sensor.

2. A path planning and control system for a sweeping robot according to claim 1, characterized in that: The motion module comprises: A motor connected to the driving wheel, used to drive the sweeping robot forward, backward or turn through the main control module; A rotary steering device is used to control the sweeping robot to turn through a motor; An encoder, mounted on the motor, for obtaining the speed of the driving wheel by reading the number of pulses of the encoder; Driving wheels are used to drive the sweeping robot forward, backward or turn.

3. A path planning and control system for a sweeping robot according to claim 1, characterized in that: The main control module is an STM32 main control chip.

4. A path planning and control system for a sweeping robot according to claim 3, characterized in that: The position sensing module includes an ultrasonic sensor module and an infrared sensor module.

5. A path planning and control system for a sweeping robot according to claim 4, characterized in that: There are three infrared sensor modules, which are respectively installed at the front ends of the sweeping robot, and the ultrasonic sensor module is installed above the sweeping robot.

6. A path planning and control system for a sweeping robot according to claim 3, characterized in that: The posture sensing module includes an MPU6050 six-axis sensor.

7. The path planning and control system for a sweeping robot according to any one of claims 1 to 6, characterized in that: It also includes a power module for supplying power to the main control module, the motion module, the cleaning module, the position sensing module, the posture sensing module, and the collision sensor.

8. A path planning control method for a sweeping robot, based on the path planning control system for a sweeping robot according to any one of claims 1 to 7, characterized in that: The steps include: S1. The position sensing module detects the distance between the sweeping robot and the surrounding obstacles and transmits it to the main control module; S2. The main control module determines whether the distance is less than the safety distance threshold and controls the motion module to work; S3. When the sweeping robot collides with an obstacle, the collision sensor senses the collision of the sweeping robot and transmits it to the main control module. The main control unit controls the motion module to stop and retreat, and at the same time controls the motion module to make the sweeping robot rotate and continue to move forward.

9. A path planning and control method for a sweeping robot according to claim 8, characterized in that: It is characterized in that Step S2 includes: When the position sensing module does not detect an obstacle ahead, the sweeping robot moves in a straight line; When the position sensing module detects that there is an obstacle ahead, the main control module determines whether the distance is less than a safety distance threshold; When the main control module determines that the distance is not less than the safety distance threshold, the sweeping robot moves in a straight line; When the main control module determines that the distance is less than the safety distance threshold, the sweeping robot slows down and turns, and then continues to move forward.

10. A path planning and control method for a sweeping robot according to claim 8, characterized in that: It is characterized in that it also includes: S4. The posture perception module obtains the yaw angle of the sweeping robot and transmits it to the main control module; the main control module adjusts the rotation speed of the two driving wheels to correct the yaw according to the yaw angle and cooperates with the encoder of the motion module.

Images