CN115366102A - Navigation method and system of mobile robot in indoor unknown dynamic environment - Google Patents
Navigation method and system of mobile robot in indoor unknown dynamic environment Download PDFInfo
- Publication number
- CN115366102A CN115366102A CN202211011136.3A CN202211011136A CN115366102A CN 115366102 A CN115366102 A CN 115366102A CN 202211011136 A CN202211011136 A CN 202211011136A CN 115366102 A CN115366102 A CN 115366102A
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- distance
- angle
- terminal
- formula
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Abstract
The invention discloses a navigation method and a system of a mobile robot in an indoor unknown dynamic environment, which comprises the following steps: determining an end point position and an initial end point angle, and obtaining an initial-end point distance according to the initial position and the end point position; planning a movement route according to the initial-end point distance; the mobile robot moves according to the movement route, and obtains the real-time rotation angle and the real-time movement distance of the mobile robot; obtaining the accumulated moving distance of the mobile robot according to the obtained real-time rotating angle and moving distance, and obtaining a terminal point distance and a terminal point angle according to the accumulated moving distance; and continuously updating the movement route according to the terminal distance and the terminal angle, and continuously moving according to the updated movement route until the terminal position is reached. The method is used for solving the technical problem that the traditional navigation method cannot accurately navigate in the indoor unknown dynamic environment, so that the aims of accurate navigation in the indoor unknown dynamic environment, low cost and low power consumption are fulfilled.
Description
Technical Field
The invention relates to the technical field of path planning and navigation of a mobile robot, in particular to a navigation method and a navigation system of the mobile robot in an indoor unknown dynamic environment.
Background
With the rapid development of computer and automatic control technologies, mobile robots have been applied to the fields of industrial manufacturing, medical services, logistics sorting, and the like. Compared with the traditional application with stable working environment and single content, the application scene of the mobile robot is more and more complex, which also puts higher requirements on the key technology of autonomous navigation. Generally, a mobile robot system senses the surrounding environment through sensors, such as laser sensors, ultrasonic sensors, visual sensors and the like, but most of the systems have insufficient decision and control capability due to the limited carrying of the sensors. Therefore, the intelligence and the autonomy of system navigation are improved, and the method is very important for the mobile robot to quickly adapt to complex working environments.
The collision prevention function of the mobile robot is an important component of navigation control, and how to handle the problem of collision prevention between the robot and a movable barrier in a complex unknown environment is a difficult point and a hot point in the related field. The current major mobile robot navigation technologies mainly include three types:
the first is the navigation technology of the automatic driving automobile based on multi-sensor fusion, which needs to navigate outdoors by means of sensors such as GPS, gyroscope, electronic compass, accelerometer, etc., which can be used outdoors but have low precision and large error when used indoors.
The second is SLAM technology (instantaneous positioning and mapping technology), which first needs to build a map of the environment to complete the navigation work, but in an unknown dynamic environment, it is not suitable for the unknown dynamic environment because an accurate map cannot be built.
In addition, most of the mobile robots in the prior art are based on Intel-structured CPU + Linux system + ROS middleware, and the mobile robots with the architectures are slow in response, high in cost, easy to consume electricity Chi Dianliang and overlarge in size.
Disclosure of Invention
In order to overcome the defects of the prior art, the invention provides a navigation method and a navigation system of a mobile robot in an indoor unknown dynamic environment, which are used for solving the technical problem that the traditional navigation method cannot accurately navigate in the indoor unknown dynamic environment, thereby achieving the purposes of accurate navigation in the indoor unknown dynamic environment and low cost and low power consumption of the mobile robot.
In order to solve the problems, the technical scheme adopted by the invention is as follows:
a navigation method of a mobile robot in an indoor unknown dynamic environment comprises the following steps:
determining an end point position and an initial end point angle, and obtaining an initial-end point distance according to the initial position and the end point position;
planning a movement route according to the initial-end point distance, the initial end point angle and the distance and angle of the obstacle detected by the laser probe;
the mobile robot moves according to the movement route, and the real-time rotation angle and the real-time movement distance of the mobile robot are obtained by utilizing the information of a photoelectric encoder of a motor;
along with the accumulation of the moving distance, obtaining the accumulated moving distance of the mobile robot according to the obtained real-time rotating angle and moving distance, and obtaining the end point distance and end point angle of the mobile robot at any position according to the accumulated moving distance;
the mobile robot continuously updates the movement route according to the terminal distance, the terminal angle and the distance and the angle of the barrier which is detected by the laser probe most recently, and continues to move according to the updated movement route until the terminal distance is 0, and then the mobile robot reaches the terminal position;
the terminal distance is the distance between the mobile robot and the terminal position at any position; the terminal angle is an angle of the mobile robot relative to the terminal position at any position; the mobile robot comprises a laser probe and a motor, wherein the motor comprises a photoelectric encoder.
As a preferred embodiment of the present invention, the method for obtaining the travel distance accumulated by the mobile robot includes:
establishing a navigation observation model of the mobile robot in an indoor unknown dynamic environment;
substituting the acquired real-time rotation angle and the acquired real-time moving distance of the mobile robot into a formula 1 and a formula 2 to obtain the moving distance accumulated by the mobile robot under the navigation observation model, wherein the formula 1 and the formula 2 are as follows:
wherein the navigation observation model comprises an X axis and a Y axis, i is the number of times that the mobile robot changes the motion state, C X Is the accumulated moving distance of the mobile robot in the X-axis direction, C Y Is the accumulated moving distance of the mobile robot in the Y-axis direction, S i For obtaining the real-time moving distance, beta, of the mobile robot i And obtaining the real-time rotation angle of the mobile robot.
As a preferred embodiment of the present invention, the method for obtaining the end point distance of the mobile robot at an arbitrary position includes:
substituting the initial-end point distance into a formula 3 and a formula 4 to obtain the initial-end point distance of the mobile robot in the directions of the X axis and the Y axis, wherein the formula 3 and the formula 4 are as follows:
B X = AB × cos α (formula 3);
B Y = AB × sin α (formula 4);
wherein AB is the initial-end distance, B X Is the initial-end distance in the X-axis direction, B Y Is the initial-end distance in the Y axis direction, and alpha is the initial end angle;
subjecting said C to X The C is Y The B X And said B Y Substituting the distance into a formula 5 to obtain the end point distance of the mobile robot at any position, wherein the formula 5 is as follows:
BC＝((B X -C X ) 2 +(B Y -C Y ) 2 ) 1/2 (equation 5);
and BC is the end point distance of the mobile robot at any position.
As a preferred embodiment of the present invention, the method for obtaining the end point angle of the mobile robot at an arbitrary position includes:
obtaining an accumulated rotation angle according to the obtained real-time rotation angle, as shown in formula 6:
wherein i is the number of times the mobile robot changes the motion state, and β is the accumulated rotation angle;
moving the accumulated moving distance C in the X-axis direction X A distance B from the initial-end point in the X-axis direction X And (3) comparison:
if C X ≤B X Then, a total angle is obtained through formula 7, where formula 7 is specifically as follows:
γ＝tan((Cy-By)/Cx-Bx)) -1 (equation 7);
if C X ＞B X Then, a total angle is obtained through the formula 8, wherein the formula 8 is specificThe following:
wherein the total angle is the sum of the accumulated rotation angle and the terminal angle of the mobile robot at any position, and gamma is the total angle;
obtaining an estimated end point angle according to the total angle and the accumulated rotation angle, as shown in formula 9:
θ 1 = γ - β (formula 9);
wherein, theta 1 Estimating the terminal angle;
judging whether the estimated end point angle is positive or negative, if theta 1 > 0, then θ = θ 1 If theta 1 If < 0, then θ = θ 1 +2π；
And theta is an end point angle of the mobile robot at any position.
As a preferred embodiment of the present invention, the method for acquiring a real-time moving distance of a mobile robot by using photoelectric encoder information of a motor includes:
obtaining the number of rotating turns of the wheel of the mobile robot according to the total number of feedback pulse numbers obtained by the photoelectric encoder and the number of feedback pulses obtained by one turn of rotation of the wheel of the mobile robot, as shown in formula 10:
m = P/F (equation 10);
wherein, P is the total number of feedback pulse numbers obtained by the photoelectric encoder, F is the feedback pulse number obtained by one rotation of the wheel of the mobile robot, and M is the number of rotation of the wheel of the mobile robot;
substituting the obtained number of turns of the wheel of the mobile robot into a formula 11 to obtain the real-time moving distance of the mobile robot, wherein the formula 11 is as follows:
s = M × pi D (formula 11);
the mobile robot comprises wheels, S is the real-time moving distance of the mobile robot, D is the diameter of the wheels of the mobile robot, and pi D is the perimeter of the wheels of the mobile robot.
As a preferred embodiment of the present invention, the method for acquiring a real-time rotation angle of a mobile robot by using photoelectric encoder information of a motor includes:
obtaining the distance from the center of gravity position of the mobile robot to the center point of the left upper wheel of the mobile robot according to the measured distance from the center point of the center of gravity position of the mobile robot to the center point of the left edge of the mobile robot and the distance from the center point of the left edge of the mobile robot to the center point of the left upper wheel of the mobile robot, as shown in formula 12:
ac＝(ab 2 +bc 2 ) 1/2 (equation 12);
ac is the distance from the center of gravity position of the mobile robot to the center point of the left edge of the mobile robot, ab is the distance from the center point of the left edge of the mobile robot to the center point of the left edge of the mobile robot, bc is the distance from the center of gravity position of the mobile robot to the center point of the upper left wheel of the mobile robot, and the wheels comprise upper left wheels;
obtaining a real-time rotation angle of the mobile robot according to the real-time movement distance of the mobile robot and the distance from the gravity center position of the mobile robot to the center point of the upper left wheel of the mobile robot, as shown in formula 13:
β i = S/ac (formula 13);
wherein, beta i And the real-time rotation angle of the mobile robot is obtained.
As a preferred embodiment of the present invention, when the mobile robot moves according to the movement route, the method includes:
the mobile robot judges the motion track of the mobile robot according to the end point angle theta of the mobile robot at any position until the mobile robot reaches the end point position, and the specific judgment conditions are as follows:
if theta is larger than or equal to 40 degrees and smaller than or equal to 320 degrees, and the laser probe of the mobile robot does not detect the obstacle within one meter, the mobile robot moves forwards;
if theta is larger than or equal to 40 degrees and smaller than or equal to 140 degrees and the laser probe of the mobile robot does not detect the obstacle within one meter, turning to the right;
if theta is larger than or equal to 220 degrees and smaller than or equal to 320 degrees, and the laser probe of the mobile robot does not detect the obstacle within one meter, turning left;
if theta is larger than or equal to 140 degrees and smaller than or equal to 220 degrees and the laser probe of the mobile robot does not detect the obstacle within one meter, the mobile robot moves backwards;
and if the obstacle is detected in the angle range, updating the movement route according to the detected obstacle, and moving along the updated movement route.
As a preferred embodiment of the present invention, the method for updating the movement route includes:
obtaining an included angle between the obstacles according to the angle of the obstacles detected by the laser probe, as shown in formula 14:
θ clip ＝θ B1 -θ A1 (equation 14);
wherein the obstacles comprise a first obstacle and a second obstacle, θ Clip Is the angle between the first obstacle and the second obstacle, theta B1 For the angle, θ, of the second obstacle detected by the laser probe A1 An angle of the first obstacle detected by the laser probe;
and obtaining the size of the gap between the obstacles according to the included angle between the obstacles and the distance between the obstacles detected by the laser probe, as shown in formula 15:
A 1 B 1 ＝(OA 1 2 +OB 1 2 -2*OA 1 *OB 1 *cosθ clip ) 1/2 (equation 15);
wherein A is 1 B 1 The size of the gap between the obstacles, OA 1 Distance of first obstacle, OB, detected by the laser probe 1 Is the distance of the second obstacle detected by the laser probe.
As a preferred embodiment of the present invention, when updating the movement route, the method further includes:
according to the obtained size of the gap between the obstacles and the terminal angle of the mobile robot at any position, finding an angle which is closest to the terminal angle and the size of the gap between the obstacles is larger than the width of the mobile robot, and obtaining the midline direction of the angle, wherein the mobile robot moves along the midline direction to avoid the obstacles.
A navigation system of a mobile robot in an indoor unknown dynamic environment, comprising:
an initial-end distance acquisition unit: the system comprises a terminal position acquisition unit, a terminal angle acquisition unit, a terminal position acquisition unit and a terminal-to-terminal distance acquisition unit, wherein the terminal position acquisition unit is used for acquiring a terminal position and an initial terminal angle;
a movement route planning unit: the system is used for planning a movement route according to the initial-end point distance, the initial end point angle and the distance and angle of the obstacle detected by the laser probe;
rotation angle and movement distance acquisition unit: the mobile robot moves according to the movement route, and obtains the real-time rotation angle and the real-time movement distance of the mobile robot by utilizing the information of the photoelectric encoder of the motor;
an end distance and end angle acquisition unit: the system comprises a mobile robot, a control module and a display module, wherein the mobile robot is used for acquiring a rotation angle and a movement distance of the mobile robot in real time along with the accumulation of the movement distance, acquiring the accumulated movement distance of the mobile robot according to the acquired real-time rotation angle and the acquired real-time movement distance, and acquiring a terminal distance and a terminal angle of the mobile robot at any position according to the accumulated movement distance;
a movement route updating unit: the mobile robot is used for continuously updating the movement route according to the terminal distance, the terminal angle and the distance and angle of the barrier which is newly detected by the laser probe, and continuously moving according to the updated movement route until the terminal distance is 0, and then reaching the terminal position;
the terminal distance is the distance of the mobile robot relative to the terminal position at any position; the terminal angle is an angle of the mobile robot relative to the terminal position at any position; the mobile robot comprises a laser probe and a motor, wherein the motor comprises a photoelectric encoder.
Compared with the prior art, the invention has the beneficial effects that:
(1) The method does not need to establish an environmental map in advance, so that the problem of low navigation precision and large error caused by inaccurate map established in advance under an unknown dynamic environment is solved;
(2) The invention carries out navigation in an indoor unknown dynamic environment without the aid of data of a sensor, thereby further improving the navigation precision and reducing the navigation error;
(3) The invention can make real-time judgment according to the actual moving barrier condition in the whole navigation process, is suitable for navigation in an indoor unknown dynamic environment, is simple and efficient, can be realized by using a CPU (central processing unit) with lower cost and even an MCU (microprogrammed control unit), and can effectively reduce the power consumption and the volume of the mobile robot.
The present invention will be described in further detail with reference to the accompanying drawings and specific embodiments.
Drawings
FIG. 1 is a diagram of steps of a method for navigating a mobile robot in an indoor unknown dynamic environment, in accordance with an embodiment of the present invention;
FIG. 2-is a schematic view of a mobile robot according to an embodiment of the present invention when moving according to a movement path;
FIG. 3 is a schematic diagram of a mobile robot according to an embodiment of the present invention, moving and rotating in real time;
FIG. 4-is a schematic diagram of a mobile robot according to an embodiment of the present invention detecting the distance and angle of a surrounding obstacle by a laser probe;
FIG. 5-is a schematic view of a mobile robot of an embodiment of the present invention as it passes over an obstacle;
fig. 6-is a schematic view of a mobile robot in navigating an observation model according to an embodiment of the present invention.
Detailed Description
The invention provides a navigation method of a mobile robot in an indoor unknown dynamic environment, which comprises the following steps:
step S1: determining an end point position and an initial end point angle, and obtaining an initial-end point distance according to the initial position and the end point position;
step S2: planning a movement route according to the initial-end point distance, the initial end point angle and the distance and angle of the obstacle detected by the laser probe;
and step S3: the mobile robot moves according to the movement route, and the real-time rotation angle and the real-time movement distance of the mobile robot are obtained by utilizing the information of the photoelectric encoder of the motor;
and step S4: along with the accumulation of the moving distance, obtaining the accumulated moving distance of the mobile robot according to the obtained real-time rotating angle and moving distance, and obtaining the end point distance and end point angle of the mobile robot at any position according to the accumulated moving distance;
step S5: the mobile robot continuously updates the movement route according to the terminal distance, the terminal angle and the distance and the angle of the barrier which is detected by the laser probe most recently, and continues to move according to the updated movement route until the terminal distance is 0, and then the mobile robot reaches the terminal position;
the terminal distance is the distance between the mobile robot and the terminal position at any position; the terminal angle is an angle of the mobile robot relative to the terminal position at any position; the mobile robot comprises a laser probe and a motor, wherein the motor comprises a photoelectric encoder.
The method can be applied to the mobile robots of a non-ROS system, a CPU based on a Coretex-A8 framework and an embedded Linux operating system, and effectively reduces the cost and the battery power consumption of the mobile robot on the premise of ensuring the navigation precision and errors.
In the step S4, as shown in fig. 6, the method for obtaining the moving distance accumulated by the mobile robot includes:
establishing a navigation observation model of the mobile robot in an indoor unknown dynamic environment;
substituting the acquired real-time rotation angle and the acquired real-time moving distance of the mobile robot into a formula 1 and a formula 2 to obtain the moving distance accumulated by the mobile robot under a navigation observation model, wherein the formula 1 and the formula 2 are as follows:
wherein the navigation observation model comprises an X axis and a Y axis, i is the number of times of changing the motion state of the mobile robot, C X Is the accumulated moving distance of the mobile robot in the X-axis direction, C Y Is the accumulated moving distance of the mobile robot in the Y-axis direction, S i For obtaining the real-time moving distance, beta, of the mobile robot i The obtained real-time rotation angle of the mobile robot is obtained.
Further, S i Is the distance the mobile robot moves in forward and backward directions, S in forward direction i Positive, at the time of retreat S i Is a negative value, beta i The angle of the mobile robot rotating during left turning and right turning is positive value during right turning and negative value during left turning.
In the step S4, the method for obtaining the end point distance of the mobile robot at the arbitrary position includes:
substituting the initial-end point distance into a formula 3 and a formula 4 to obtain the initial-end point distance of the mobile robot in the X-axis and Y-axis directions, wherein the formula 3 and the formula 4 are as follows:
B X = AB × cos α (formula 3);
B Y AB × sin α (formula 4);
wherein AB is the initial-end distance, B X Is the initial-end distance in the X-axis direction, B Y Is the initial-end distance in the Y axis direction, and alpha is the initial end angle;
c is to be X 、C Y 、B X And B Y Substituting into formula 5 to obtain the terminal distance of the mobile robot at any position, wherein formula 5 is as follows:
BC＝((B X -C X ) 2 +(B Y -C Y ) 2 ) 1/2 (equation 5);
wherein BC is an end point distance of the mobile robot at any position.
Further, as shown in fig. 2, the initial-end point distance AB is a distance between the initial position and the end point position, in fig. 2, the position1 is the initial position of the mobile robot, in the position1, an included angle between a center line of the mobile robot and an end point center point is calculated according to a counterclockwise direction, and an angle between the center line and the end point center point is the initial end point angle α.
When the method is used, the initial-end point distance and the initial end point angle are sent to the mobile robot through the mobile equipment APP, so that B X And B Y Then can be directly obtained by formula 3 and formula 4, and then according to the above obtained C X And C Y The end point distance of the mobile robot at any position can be obtained through formula 5.
As shown in fig. 6, the step S4 includes, when obtaining the end point angle of the mobile robot at an arbitrary position:
obtaining an accumulated rotation angle according to the obtained real-time rotation angle, as shown in formula 6:
wherein i is the number of times the mobile robot changes the motion state, and β is the accumulated rotation angle;
moving the accumulated distance C in the X-axis direction X From the initial-final point in the direction of the X-axis X And (3) comparison:
if C X ≤B X Then, a total angle is obtained through equation 7, where equation 7 is as follows:
γ＝tan((Cy-By)/Cx-Bx)) -1 (equation 7);
if C X ＞B X Then, a total angle is obtained through equation 8, where equation 8 is as follows:
wherein the total angle is the sum of the accumulated rotation angle and the terminal angle of the mobile robot at any position, and gamma is the total angle;
as shown in fig. 6, the estimated end point angle is obtained according to the total angle and the accumulated rotation angle, which is specifically shown in formula 9:
θ 1 = γ - β (formula 9);
wherein, theta 1 Estimating the terminal angle;
judging whether the estimated end point angle is positive or negative, if theta 1 > 0, then θ = θ 1 If theta 1 If < 0, then θ = θ 1 +2π；
And the angle theta is the terminal angle of the mobile robot at any position, the included angle between the central line of the mobile robot and the terminal central point at any position is calculated according to the anticlockwise direction, and the angle between the central line and the terminal central point is the terminal angle theta at any position.
By the method, the distance and the end point angle of the mobile robot relative to the end point position at any position can be calculated, so that navigation to the end point position is realized under the conditions of no sensor, no map and dynamic obstacle avoidance.
In the step S3, when the real-time moving distance of the mobile robot is obtained by using the photoelectric encoder information of the motor, the method includes:
obtaining the number of rotating turns of the wheel of the mobile robot according to the total number of feedback pulses obtained by the photoelectric encoder and the number of feedback pulses obtained by one turn of the wheel of the mobile robot, as shown in formula 10:
m = P/F (formula 10);
wherein, P is the total number of feedback pulse numbers obtained by the photoelectric encoder, F is the feedback pulse number obtained by one rotation of the wheel of the mobile robot, and M is the number of rotation turns of the wheel of the mobile robot;
substituting the obtained number of turns of the wheel of the mobile robot into a formula 11 to obtain the real-time moving distance of the mobile robot, wherein the formula 11 is as follows:
s = M × pi D (formula 11);
the mobile robot comprises wheels, S is the real-time moving distance of the mobile robot, D is the diameter of the wheels of the mobile robot, and pi D is the perimeter of the wheels of the mobile robot.
Because sensors such as a GPS sensor, a gyroscope, an accelerometer, a magnetometer and an electronic compass cannot be used indoors, the invention calculates the moving distance of the robot by using information of the photoelectric encoder.
In the step S3, as shown in fig. 3, the method for acquiring the real-time rotation angle of the mobile robot by using the photoelectric encoder information of the motor includes:
according to the measured distance from the center of gravity position of the mobile robot to the center point of the left edge of the mobile robot and the distance from the center point of the left edge of the mobile robot to the center point of the upper left wheel of the mobile robot, the distance from the center of gravity position of the mobile robot to the center point of the upper left wheel of the mobile robot is obtained, as shown in formula 12:
ac＝(ab 2 +bc 2 ) 1/2 (equation 12);
ab is the distance from the center point of the center of gravity position of the mobile robot to the center point of the left edge of the mobile robot, bc is the distance from the center point of the left edge of the mobile robot to the center point of the left edge of the mobile robot, ac is the distance from the center point of the center of gravity position of the mobile robot to the center point of the left upper wheel of the mobile robot, and the wheels comprise the left upper wheels.
Further, as can be seen from fig. 3, the position of the center of gravity of the mobile robot is α, and α is the center position of the robot for a robot that is symmetric front-back and left-right, and if not, the position of α can be obtained by measurement, b is the midpoint of the left edge of the robot, and c is the center point of the upper left wheel, so that the lengths of ab and bc can be obtained by measurement, and the length of ac can be obtained by the cosine theorem:
according to the real-time moving distance of the mobile robot and the distance from the gravity center position of the mobile robot to the central point of the upper left wheel of the mobile robot, the real-time rotating angle of the mobile robot is obtained, and the real-time rotating angle is specifically shown in formula 13:
β i = S/ac (formula 13);
wherein, beta i The real-time rotation angle of the mobile robot is obtained.
Because sensors such as a GPS sensor, a gyroscope, an accelerometer, a magnetometer and an electronic compass cannot be used indoors, the rotation angle of the robot is calculated by using information of the photoelectric encoder.
In the above steps S1 and S5, as shown in fig. 4, when the mobile robot moves according to the movement route, the method includes:
the mobile robot judges the motion track of the mobile robot according to the end point angle theta of the mobile robot at any position until the mobile robot reaches the end point position, and the specific judgment conditions are as follows:
if theta is larger than or equal to 40 degrees and smaller than or equal to 320 degrees, and the laser probe of the mobile robot does not detect the obstacle within one meter, the mobile robot moves forwards;
if theta is larger than or equal to 40 degrees and smaller than or equal to 140 degrees, and the laser probe of the mobile robot does not detect the obstacle within one meter, turning to the right;
if theta is larger than or equal to 220 degrees and smaller than or equal to 320 degrees, and the laser probe of the mobile robot does not detect the obstacle within one meter, turning left;
if theta is larger than or equal to 140 degrees and smaller than or equal to 220 degrees and the laser probe of the mobile robot does not detect the obstacle within one meter, the mobile robot moves backwards;
and if the obstacle is detected in the angle range, updating the movement route according to the detected obstacle, and moving along the updated movement route.
In a dynamic environment, the obstacles move ceaselessly, the invention guides the mobile robot to make the decision of the next movement according to the distance and the angle of the surrounding obstacles detected by the laser probe, and further utilizes the data output by the laser probe, thereby planning a reasonable route under the condition that the moving obstacles are fully distributed around.
Further, the present invention sets four variables, respectively, including: stop _ forward, stop _ right, stop _ left, and stop _ backward. As shown in fig. 4, when the terminal angle is in the range of 40 ° ≦ θ ≦ 320 °, and there is a moving obstacle within a range of 1 meter from the laser probe, the variable stop _ forward =1, otherwise the variable is 0, and the angle and the distance can be adjusted accordingly according to the situation of the actual moving robot. Similarly, when the laser probe finds that there is an obstacle within a range of 1 meter from the laser probe, the variable stop _ right =1, otherwise, the variable is 0, and if the variable stop _ forward =1, the robot needs to rotate to a certain angle at this time to get out of a gap of a surrounding obstacle.
The relationship between the end point angle and the rotation direction is specifically shown in table 1:
table 1: relation between end angle and direction of rotation
| End point angle | Direction of rotation |
| 320-40 ° simultaneous stop _ forward =0 | Forward |
| 40-140 ° simultaneous stop _ right =0 | Right turn |
| 220-320 degrees simultaneous stop _ left =0 | Left turn |
| 140-220 degrees and simultaneously stop _ backward =0 | Retreat |
In the above step S5, as shown in fig. 5, when updating the movement route, the method includes:
obtaining an included angle between obstacles according to the angle of the obstacles detected by the laser probe, as shown in formula 14:
θ clamp ＝θ B1 -θ A1 (equation 14);
wherein the obstacles include a first obstacle and a second obstacle, θ Clip Is the angle between the first obstacle and the second obstacle, theta B1 Angle of second obstacle theta detected by laser probe A1 Is the angle of the first obstacle detected by the laser probe;
and obtaining the size of the gap between the obstacles according to the included angle between the obstacles and the distance between the obstacles detected by the laser probe, which is specifically shown in a formula 15:
A 1 B 1 ＝(OA 1 2 +OB 1 2 -2*OA 1 *OB 1 *cosθ clip ) 1/2 (equation 15);
wherein A is 1 B 1 The size of the gap between the obstacles, OA 1 Distance of the first obstacle, OB, detected by the laser probe 1 Is the distance of the second obstacle detected by the laser probe.
In the step S5, when updating the movement route, the method further includes:
according to the obtained size of the gap between the obstacles and the terminal angle of the mobile robot at any position, finding an angle which is closest to the terminal angle and the size of the gap between the obstacles is larger than the width of the mobile robot, obtaining the middle line direction of the angle, and moving the mobile robot along the middle line direction to avoid the obstacles.
According to the content in table 1, the mobile robot can determine whether the next second is forward, right turn, left turn or backward, if there is an obstacle along the direction of the end point angle, according to the determination in table 2, the mobile robot calculates an angle which is closest to the end point angle and has a gap interval greater than the width of the robot, and moves away along the centerline direction of the angle, and table 2 is as follows:
TABLE 2 judgment of obstacle in front of 1 m
Further, as shown in fig. 2, the mobile robot should advance at the initial position1, but there is an obstacle in front of the mobile robot, and then turn left to the position2 according to table 1 and then move forward, during the advancing process, the mobile robot is monitoring the end point angle all the time, when moving to the position3, the mobile robot suddenly appears a mobile obstacle 3 ahead, combining table 1 and table 2, the mobile robot should turn right, during the rotation process, the end point angle is monitored all the time, when the end point angle satisfies the advancing of table 1, the mobile robot advances to the end point position, if other obstacles appear during the advancing process, the mobile robot iteratively makes a decision according to the end point angle monitored in real time and the contents of table 1 and table 2 until the end point position is reached.
The invention provides a navigation system of a mobile robot in an indoor unknown dynamic environment, which comprises:
an initial-end distance acquisition unit: the system is used for determining an end point position and an initial end point angle and obtaining an initial-end point distance according to the initial position and the end point position;
a movement route planning unit: the system is used for planning a movement route according to the initial-end point distance, the initial end point angle and the distance and angle of the obstacle detected by the laser probe;
rotation angle and movement distance acquisition unit: the system is used for moving the mobile robot according to a movement route and acquiring the real-time rotation angle and movement distance of the mobile robot by utilizing the information of a photoelectric encoder of a motor;
an end distance and end angle acquisition unit: the system comprises a mobile robot, a control module and a display module, wherein the control module is used for acquiring a rotation angle and a movement distance of the mobile robot in real time along with the accumulation of the movement distance, acquiring the accumulated movement distance of the mobile robot according to the acquired real-time rotation angle and the acquired real-time movement distance, and acquiring a terminal distance and a terminal angle of the mobile robot at any position according to the accumulated movement distance;
a movement route updating unit: the mobile robot is used for continuously updating the movement route according to the terminal distance, the terminal angle and the distance and the angle of the barrier which is newly detected by the laser probe, and continuously moving according to the updated movement route until the terminal distance is 0, and then reaching the terminal position;
the terminal distance is the distance between the mobile robot and the terminal position at any position; the terminal angle is an angle of the mobile robot relative to the terminal position at any position; the mobile robot comprises a laser probe and a motor, and the motor comprises a photoelectric encoder.
Compared with the prior art, the invention has the beneficial effects that:
(1) The method does not need to establish an environmental map in advance, so that the problem of low navigation precision and large error caused by inaccurate map established in advance under an unknown dynamic environment is solved;
(2) The invention carries out navigation in an indoor unknown dynamic environment without the aid of data of a sensor, thereby further improving the navigation precision and reducing the navigation error;
(3) The invention can make real-time judgment according to the actual moving barrier condition in the whole navigation process, is suitable for navigation in an indoor unknown dynamic environment, is simple and efficient, can be realized by using a CPU (central processing unit) with lower cost and even an MCU (microprogrammed control unit), and can effectively reduce the power consumption and the volume of the mobile robot.
The above embodiments are only preferred embodiments of the present invention, and the protection scope of the present invention is not limited thereby, and any insubstantial changes and substitutions made by those skilled in the art based on the present invention are within the protection scope of the present invention.
Claims (10)
1. A navigation method of a mobile robot in an indoor unknown dynamic environment is characterized by comprising the following steps:
determining an end point position and an initial end point angle, and obtaining an initial-end point distance according to the initial position and the end point position;
planning a movement route according to the initial-end point distance, the initial end point angle and the distance and angle of the obstacle detected by the laser probe;
the mobile robot moves according to the movement route, and obtains the real-time rotation angle and movement distance of the mobile robot by using the photoelectric encoder information of the motor;
along with the accumulation of the moving distance, obtaining the accumulated moving distance of the mobile robot according to the obtained real-time rotating angle and moving distance, and obtaining the end point distance and end point angle of the mobile robot at any position according to the accumulated moving distance;
the mobile robot continuously updates the movement route according to the terminal distance, the terminal angle and the distance and the angle of the barrier which is detected by the laser probe most recently, and continues to move according to the updated movement route until the terminal distance is 0, and then the mobile robot reaches the terminal position;
the terminal distance is the distance of the mobile robot relative to the terminal position at any position; the terminal angle is an angle of the mobile robot relative to the terminal position at any position; the mobile robot comprises a laser probe and a motor, wherein the motor comprises a photoelectric encoder.
2. The method for navigating the mobile robot in the unknown indoor dynamic environment according to claim 1, wherein the method for navigating the mobile robot in the unknown indoor dynamic environment comprises the following steps:
establishing a navigation observation model of the mobile robot in an indoor unknown dynamic environment;
substituting the acquired real-time rotation angle and the acquired real-time moving distance of the mobile robot into a formula 1 and a formula 2 to obtain the moving distance accumulated by the mobile robot under the navigation observation model, wherein the formula 1 and the formula 2 are as follows:
wherein the navigation observation model comprises an X axis and a Y axis, i is the number of times that the mobile robot changes the motion state, C X Is the accumulated moving distance of the mobile robot in the X-axis direction, C Y Is the accumulated moving distance of the mobile robot in the Y-axis direction, S i For obtaining the real-time moving distance, beta, of the mobile robot i And obtaining the real-time rotation angle of the mobile robot.
3. The method for navigating the mobile robot in the unknown indoor dynamic environment according to claim 2, wherein when obtaining the end point distance of the mobile robot at any position, the method comprises:
substituting the initial-end point distance into a formula 3 and a formula 4 to obtain the initial-end point distance of the mobile robot in the directions of the X axis and the Y axis, wherein the formula 3 and the formula 4 are as follows:
B X = AB × cos α (formula 3);
B Y = AB × sin α (formula 4);
wherein AB is the initial-end distance, B X Is the initial-end distance in the X-axis direction, B Y Is the initial-end distance in the Y axis direction, alpha is the initial end angle;
subjecting the C to X The C is Y The B X And said B Y Substituting the equation 5 into the terminal distance of the mobile robot at any position, where the equation 5 is as follows:
BC＝((B X -C X ) 2 +(B Y -C Y ) 2 ) 1/2 (equation 5);
and BC is the end point distance of the mobile robot at any position.
4. The method for navigating the mobile robot in the unknown indoor dynamic environment according to claim 3, wherein when obtaining the terminal angle of the mobile robot at any position, the method comprises:
obtaining an accumulated rotation angle according to the obtained real-time rotation angle, as shown in formula 6:
wherein i is the number of times the mobile robot changes the motion state, and β is the accumulated rotation angle;
moving the accumulated moving distance C in the X-axis direction X A distance B from the initial-end point in the X-axis direction X And (3) comparison:
if C X ≤B X Then, a total angle is obtained through formula 7, where formula 7 is specifically as follows:
γ＝tan((Cy-By)/Cx-Bx)) -1 (equation 7);
if C X ＞B X Then, a total angle is obtained through formula 8, where formula 8 is specifically as follows:
wherein the total angle is the sum of the accumulated rotation angle and the terminal angle of the mobile robot at any position, and gamma is the total angle;
obtaining an estimated end point angle according to the total angle and the accumulated rotation angle, as shown in formula 9:
θ 1 = γ - β (formula 9);
wherein, theta 1 Estimating the terminal angle;
judgment stationPositive or negative of the estimated end point angle if theta 1 > 0, then θ = θ 1 If theta 1 If < 0, then θ = θ 1 +2π；
And theta is an end point angle of the mobile robot at any position.
5. The method for navigating the mobile robot in the unknown indoor dynamic environment according to claim 1, wherein the method for acquiring the real-time moving distance of the mobile robot by using the photoelectric encoder information of the motor comprises the following steps:
obtaining the number of rotating turns of the wheel of the mobile robot according to the total number of feedback pulse numbers obtained by the photoelectric encoder and the number of feedback pulses obtained by one turn of rotation of the wheel of the mobile robot, as shown in formula 10:
m = P/F (formula 10);
wherein, P is the total number of feedback pulses obtained by the photoelectric encoder, F is the number of feedback pulses obtained by one rotation of the wheel of the mobile robot, and M is the number of rotation turns of the wheel of the mobile robot;
substituting the obtained number of turns of the wheel of the mobile robot into a formula 11 to obtain the real-time moving distance of the mobile robot, wherein the formula 11 is as follows:
s = M × pi D (formula 11);
the mobile robot comprises wheels, S is the real-time moving distance of the mobile robot, D is the diameter of the wheels of the mobile robot, and pi D is the perimeter of the wheels of the mobile robot.
6. The method for navigating the mobile robot in the unknown indoor dynamic environment according to claim 5, wherein the method for acquiring the real-time rotation angle of the mobile robot by using the photoelectric encoder information of the motor comprises the following steps:
obtaining the distance from the center of gravity position of the mobile robot to the center point of the left upper wheel of the mobile robot according to the measured distance from the center point of the center of gravity position of the mobile robot to the center point of the left edge of the mobile robot and the distance from the center point of the left edge of the mobile robot to the center point of the left upper wheel of the mobile robot, as shown in formula 12:
ac＝(ab 2 +bc 2 ) 1/2 (equation 12);
wherein ab is the distance from the center point of the gravity center position of the mobile robot to the center point of the left edge of the mobile robot, bc is the distance from the center point of the left edge of the mobile robot to the center point of the left edge of the mobile robot, ac is the distance from the center point of the gravity center position of the mobile robot to the center point of the upper left wheel of the mobile robot, and the wheels comprise upper left wheels;
obtaining a real-time rotation angle of the mobile robot according to the real-time moving distance of the mobile robot and the distance from the gravity center position of the mobile robot to the center point of the upper left wheel of the mobile robot, as shown in formula 13:
β i = S/ac (formula 13);
wherein beta is i And the real-time rotation angle of the mobile robot is obtained.
7. The method for navigating the mobile robot in the unknown indoor dynamic environment according to claim 4, wherein when the mobile robot moves according to the movement route, the method comprises the following steps:
the mobile robot judges the motion track of the mobile robot according to the end point angle theta of the mobile robot at any position until the mobile robot reaches the end point position, and the specific judgment conditions are as follows:
if theta is larger than or equal to 40 degrees and smaller than or equal to 320 degrees, and the laser probe of the mobile robot does not detect the obstacle within one meter, the mobile robot moves forwards;
if theta is larger than or equal to 40 degrees and smaller than or equal to 140 degrees and the laser probe of the mobile robot does not detect the obstacle within one meter, turning to the right;
if theta is larger than or equal to 220 degrees and smaller than or equal to 320 degrees, and the laser probe of the mobile robot does not detect the obstacle within one meter, turning left;
if theta is larger than or equal to 140 degrees and smaller than or equal to 220 degrees and the laser probe of the mobile robot does not detect the obstacle within one meter, the mobile robot moves backwards;
and if the obstacle is detected in the angle range, updating the movement route according to the detected obstacle, and moving along the updated movement route.
8. The method for navigating a mobile robot in an indoor unknown dynamic environment as claimed in claim 7, comprising, in updating the movement route:
obtaining an included angle between the obstacles according to the angle of the obstacles detected by the laser probe, as shown in formula 14:
θ clamp ＝θ B1 -θ A1 (equation 14);
wherein the obstacles comprise a first obstacle and a second obstacle, θ Clip Is the angle between the first obstacle and the second obstacle, theta B1 For the angle, θ, of the second obstacle detected by the laser probe A1 An angle of the first obstacle detected by the laser probe;
and obtaining the size of the gap between the obstacles according to the included angle between the obstacles and the distance between the obstacles detected by the laser probe, as shown in formula 15:
A 1 B 1 ＝(OA 1 2 +OB 1 2 -2*OA 1 *OB 1 *cosθ clip ) 1/2 (equation 15);
wherein A is 1 B 1 The size of the gap between the obstacles, OA 1 Distance, OB, of a first obstacle detected by the laser probe 1 Is the distance of the second obstacle detected by the laser probe.
9. The method for navigating a mobile robot in an indoor unknown dynamic environment as claimed in claim 8, further comprising, when updating the movement route:
according to the obtained size of the gap between the obstacles and the terminal angle of the mobile robot at any position, finding an angle which is closest to the terminal angle and the size of the gap between the obstacles is larger than the width of the mobile robot, and obtaining the midline direction of the angle, wherein the mobile robot moves along the midline direction to avoid the obstacles.
10. A navigation system for a mobile robot in an indoor unknown dynamic environment, comprising:
an initial-end distance acquisition unit: the system comprises a terminal position acquisition unit, a terminal angle acquisition unit, a terminal position acquisition unit and a terminal-to-terminal distance acquisition unit, wherein the terminal position acquisition unit is used for acquiring a terminal position and an initial terminal angle;
a movement route planning unit: the system is used for planning a movement route according to the initial-end point distance, the initial end point angle and the distance and angle of the obstacle detected by the laser probe;
a rotation angle and movement distance acquisition unit: the mobile robot moves according to the movement route, and obtains the real-time rotation angle and the real-time movement distance of the mobile robot by utilizing the information of the photoelectric encoder of the motor;
an end distance and end angle acquisition unit: the system comprises a mobile robot, a control module and a display module, wherein the mobile robot is used for acquiring a rotation angle and a movement distance of the mobile robot in real time along with the accumulation of the movement distance, acquiring the accumulated movement distance of the mobile robot according to the acquired real-time rotation angle and the acquired real-time movement distance, and acquiring a terminal distance and a terminal angle of the mobile robot at any position according to the accumulated movement distance;
a movement route updating unit: the mobile robot is used for continuously updating the movement route according to the terminal distance, the terminal angle and the distance and angle of the barrier which is newly detected by the laser probe, and continuously moving according to the updated movement route until the terminal distance is 0, and then reaching the terminal position;
the terminal distance is the distance of the mobile robot relative to the terminal position at any position; the terminal angle is an angle of the mobile robot relative to the terminal position at any position; the mobile robot comprises a laser probe and a motor, wherein the motor comprises a photoelectric encoder.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211011136.3A CN115366102A (en) | 2022-08-23 | 2022-08-23 | Navigation method and system of mobile robot in indoor unknown dynamic environment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211011136.3A CN115366102A (en) | 2022-08-23 | 2022-08-23 | Navigation method and system of mobile robot in indoor unknown dynamic environment |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115366102A true CN115366102A (en) | 2022-11-22 |
Family
ID=84068589
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211011136.3A Pending CN115366102A (en) | 2022-08-23 | 2022-08-23 | Navigation method and system of mobile robot in indoor unknown dynamic environment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115366102A (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104460666A (en) * | 2014-10-27 | 2015-03-25 | 上海理工大学 | Robot autonomous obstacle avoidance moving control method based on distance vectors |
| CN106525053A (en) * | 2016-12-28 | 2017-03-22 | 清研华宇智能机器人（天津）有限责任公司 | Indoor positioning method for mobile robot based on multi-sensor fusion |
| CN107024210A (en) * | 2017-03-09 | 2017-08-08 | 深圳市朗空亿科科技有限公司 | A kind of Indoor Robot barrier-avoiding method, device and navigation system |
| WO2018053942A1 (en) * | 2016-09-20 | 2018-03-29 | 深圳市银星智能科技股份有限公司 | Mobile robot and navigation method therefor |
| CN112720451A (en) * | 2019-10-28 | 2021-04-30 | 深圳市行知行机器人技术有限公司 | Automatic operation robot and control method thereof for automatic operation according to planned path |
-
2022
- 2022-08-23 CN CN202211011136.3A patent/CN115366102A/en active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104460666A (en) * | 2014-10-27 | 2015-03-25 | 上海理工大学 | Robot autonomous obstacle avoidance moving control method based on distance vectors |
| WO2018053942A1 (en) * | 2016-09-20 | 2018-03-29 | 深圳市银星智能科技股份有限公司 | Mobile robot and navigation method therefor |
| CN106525053A (en) * | 2016-12-28 | 2017-03-22 | 清研华宇智能机器人（天津）有限责任公司 | Indoor positioning method for mobile robot based on multi-sensor fusion |
| CN107024210A (en) * | 2017-03-09 | 2017-08-08 | 深圳市朗空亿科科技有限公司 | A kind of Indoor Robot barrier-avoiding method, device and navigation system |
| CN112720451A (en) * | 2019-10-28 | 2021-04-30 | 深圳市行知行机器人技术有限公司 | Automatic operation robot and control method thereof for automatic operation according to planned path |
Non-Patent Citations (1)
| Title |
|---|
| YUPEI YAN等: "A navigation algorithm of the mobile robot in the indoor and dynamic environment based on the PF-SLAM algorithm" * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN103592944B (en) | A kind of supermarket shopping robot and travel path planing method thereof | |
| CN102519400B (en) | Large slenderness ratio shaft part straightness error detection method based on machine vision | |
| CN107943042B (en) | Automatic construction method and device for geomagnetic fingerprint database | |
| CN205655844U (en) | Robot odometer based on ROS | |
| CN105737853B (en) | A kind of drift calibration method of robot inertial navigation system | |
| CN205950750U (en) | Transformer station inspection robot control system that navigates based on inertial navigation | |
| CN105004336A (en) | Robot positioning method | |
| CN104197935A (en) | Indoor localization method based on mobile intelligent terminal | |
| CN111380573A (en) | Method for calibrating the orientation of a moving object sensor | |
| CN111047531B (en) | Monocular vision-based storage robot indoor positioning method | |
| CN114964236A (en) | Mapping and vehicle positioning system and method for underground parking lot environment | |
| CN109387198B (en) | Inertia/vision milemeter combined navigation method based on sequential detection | |
| Wang et al. | Automatic reading system for analog instruments based on computer vision and inspection robot for power plant | |
| Xu et al. | Indoor vision/ins integrated mobile robot navigation using multimodel-based multifrequency kalman filter | |
| CN109375631A (en) | A kind of system and method for realizing high speed magnetic navigation identification | |
| Zhang et al. | Self-positioning for mobile robot indoor navigation based on wheel odometry, inertia measurement unit and ultra wideband | |
| CN115366102A (en) | Navigation method and system of mobile robot in indoor unknown dynamic environment | |
| WO2022237375A1 (en) | Positioning apparatus calibration method, odometer calibration method, program product, and calibration apparatus | |
| CN115540854A (en) | Active positioning method, equipment and medium based on UWB assistance | |
| Wang et al. | Rotary Encoder Based Self-Positioning Method for Mobile Robot | |
| CN112013868B (en) | Adaptive parameter police dog attitude estimation method based on visual inertial navigation odometer | |
| CN114562994A (en) | Positioning method of mobile robot in dynamic environment | |
| CN114485613A (en) | Multi-information fusion underwater robot positioning method | |
| Bao et al. | A sensor fusion strategy for indoor target three-dimensional localization based on ultra-wideband and barometric altimeter measurements | |
| Research on the positioning and recognition of an intelligent inspection robot in substations |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |