A local path planning and guidance method for differential mobile robots in mixed environments

Abstract

The present invention discloses a local path planning and guidance method for a differential mobile robot in a mixed environment, which belongs to the field of robot path planning. The method includes: firstly, based on the perception input of the robot's current viewing angle sensor (including laser radar, camera, etc.), obstacles are identified through a segmentation detection algorithm, and a local obstacle grid occupancy map is generated; secondly, based on the obstacle grid occupancy map, the obstacle connection domain is extracted, the center of mass and direction of each obstacle area are calculated, the obstacle that most affects the current movement is determined, the major axis, minor axis, and offset of the parallel elliptic limit cycle are determined, the parallel elliptic limit cycle differential equation is constructed, and the local path sequence point information is obtained by solving the equation; finally, based on the current position point, the local target point, and the local path sequence point, a pure tracking control algorithm based on preview distance adaptation is applied to accurately track and guide the local path, thereby ensuring the real-time and safe navigation of the mobile robot in a mixed environment.

Description

Technical Field

[0001] The invention relates to a local path planning and guiding method for a differential mobile robot in a mixed environment, belonging to the field of robot path planning. Background Art

[0002] Autonomous robot navigation is a complex issue that has attracted much attention in the industrial, military, and civilian fields. Efficient and robust autonomous robot navigation systems are urgently needed to be studied and released. Although navigation technology is developing with the times and has achieved advanced results, new demands will continue to emerge in the actual application process to adapt to more targeted engineering applications. Therefore, it is necessary to further study more flexible and reliable navigation strategies to enable them to adapt to new scene environments and task requirements.

[0003] In recent years, reactive and cognitive navigation architectures applied to autonomous mobile machine navigation have been widely studied. Local path planning and guidance methods such as rapid search random tree (RRT), local window method (DWA), artificial potential field method, TEB, etc. are widely used. However, in complex and chaotic environments, these algorithms cannot take into account the requirements of computational efficiency, safe obstacle avoidance, and accurate and stable guidance. Reactive navigation draws on the "stimulus-response" principle and can respond to environmental information in real time. It does not need to fully know the entire environmental map information for other complex task planning, so it can be competent for safe navigation in more complex environments.

[0004] In a cluttered environment where the global map cannot be known in advance, the mobile robot needs to combine the sensor input of its own sensors, model and analyze the environment within the current local field of view, plan a local collision-free path, and apply a tracking control algorithm for accurate guidance. Therefore, it is necessary to study a local path planning and guidance method architecture that can model the environment in real time, plan efficiently and stably, and guide accurately and robustly. Summary of the invention

[0005] The technical problem to be solved by the present invention is that, in the case of an unknown global environment, based on the perception input of the mobile robot's own sensors, the local environmental obstacles in the field of view are optimized modeled based on elliptical geometry, local path planning is performed through the parallel elliptical limit cycle method, and the preview distance adaptive pure tracking guidance algorithm is used to accurately track the local path. In this way, a safe local path can be quickly planned under the conditions of a mixed environment, dense obstacles, low robustness and poor accuracy of the robot's own positioning, and the tracking and guidance along the local path can be quickly and efficiently performed.

[0006] In order to solve the above technical problems, the present invention provides a local path planning and guidance method for a differential mobile robot in a mixed environment, the method comprising the following steps:

[0007] S1: Based on the perception input of the robot's current viewing angle sensor (including lidar, camera, etc.), obstacles are identified through the segmentation detection algorithm to generate a local obstacle grid occupancy map;

[0008] S2: extracting the obstacle connected domain from the obstacle grid occupancy map input in step S1 to form an obstacle connected map;

[0009] S3: For the obstacle connectivity graph generated in step S2, calculate the centroid and direction of each obstacle area;

[0010] S4: Based on the obstacle mass center and direction calculated in step S3 and combined with the current position of the mobile robot, the obstacle impact levels are sorted to determine the obstacle that most affects the current movement;

[0011] S5: for the obstacle that most affects the current motion obtained in step S4, further determine the center of mass (center of the ellipse), direction, major axis, minor axis, and offset;

[0012] S6: Based on the relevant parameters determined in step S5, construct a parallel elliptic limit cycle differential equation, and solve the differential equation to obtain local path sequence point information;

[0013] S7: Based on the local path sequence point information generated in step S6 and in combination with the environment, determine the phased local target point;

[0014] S8: For the phased local target point determined in step S7, combined with the current position point and the local path sequence point, a pure tracking control algorithm based on preview distance adaptation is applied to accurately track and guide the local path.

[0015] Preferably, the step S1 specifically includes the following process:

[0016] (1-1) Use hardware connection to synchronize the data of multiple sensors, or use the external interrupt of microprocessor to synchronize the data of multiple sensors;

[0017] (1-2) Perform binary projection on the collected fusion information to obtain a local obstacle grid occupancy map.

[0018] Preferably, the step S2 specifically includes the following process:

[0019] For the local obstacle grid occupancy map in step S1, a two-step fast scanning algorithm based on connected area extraction is used to extract connected domains.

[0020] (2-1) The first step is to traverse all row and column data, assign temporary labels and record equivalent items;

[0021] (2-2) The second step is to traverse each temporary label generated in the first step and replace it with the minimum label of the equivalent class.

[0022] Preferably, the step S3 specifically includes the following process:

[0023] (3-1) Based on the connected graph generated in step S2, the centroid of each connected domain is determined using the computational geometry distance algorithm, and the calculation formula is as follows:

[0024]

[0025] Among them, n is the number of rows, m is the number of columns, (x i ,y j )(i=0,1,...,n-1;j=0,1,...,m-1) are pixel coordinates.

[0026] (3-2) Based on the connectivity graph generated in step S2, the minimum second-order moment axis (the equivalent axis of the minimum inertia axis on the two-dimensional plane) is used as the direction of the connectivity domain. The formula is:

[0027] E=∫∫r 2 f(x,y)dxdy(2)

[0028] Where r is the vertical distance from the point (x, y) to the line.

[0029] Preferably, the step S4 specifically includes the following process:

[0030] (4-1) Based on the centroid of each obstacle connected domain in the local environment connected graph Calculate the current robot position (x now ,y now ) from each centroid: Where m = 1, 2, ..., n, n is the total number of connected domains in the connected graph;

[0031] (4-2) Based on the calculated distance set {dis1, dis2, dis3, ..., dis n} Sort from small to large and determine the minimum distance min{dis1,dis2,dis3,...,dis m} and obstructive material mind The kth obstacle connected domain is regarded as the obstacle that most affects the robot within the motion field of view.

[0032] Preferably, step S5 specifically includes the following process:

[0033] (5-1) Based on the obstacle k that most affects the movement of the robot determined in step S4, calculate its center of mass Maximum distance to the connected domain boundary (where j is the connected domain boundary point sequence), and takes it as the major semi-axis of the envelope ellipse A=lena , calculate the shortest distance from the centroid to the boundary of the connected domain Multiply it by a coefficient gain K greater than 1 cof The latter is the minor semi-axis of the enveloping ellipse B = K cof *len b ;

[0034] (5-2) Combined with the robot's physical size and its minimum safe collision distance to surrounding obstacles dis Considering the above, the offset K of the parallel ellipse is determined. p =safe dis .

[0035] Preferably, the step S6 specifically includes the following process:

[0036] (6-1) Based on the major semi-axis, minor semi-axis, and offset determined in step S5, establish the parallel elliptic limit cycle equation.

[0037]

[0038] in, z3=(ABK p ) 2 ; A and B represent the major and minor semi-axis of the envelope ellipse, respectively; K p ∈R + (K p ≠0) represents the offset of the elliptical parallel curve relative to the enveloping ellipse.

[0039] (6-2) Solve the differential equation to obtain the parallel trajectory of the ellipse;

[0040] (6-3) Based on the obtained trajectory, the center of mass coordinates are translated and the direction of the connected domain is rotated to obtain the final path trajectory.

[0041] Preferably, the step S7 specifically includes the following process:

[0042] Based on the parallel elliptical limit cycle trajectory obtained in step S6, combined with the requirements of task orientation and obstacle avoidance, the stage target point on the limit cycle is determined.

[0043] Preferably, the step S8 specifically includes the following process:

[0044] (8-1) Based on the parallel elliptic limit cycle trajectory obtained in step S6 and the stage target point determined in step S7, a pure tracking control algorithm based on preview distance adaptation is applied from the current position of the robot to accurately track and guide the local path;

[0045] (8-2) In each control cycle, the curvature of the curve formed by the current point of the path and the next four points is calculated, and an adaptive preview distance function is established based on the curvature, and the preview distance and preview path point at the current moment are determined by solving the function;

[0046] (8-3) The adaptive preview distance and preview path point are input into the pure tracking algorithm, and the speed and angular velocity information of each control cycle is calculated and output, and sent to the robot chassis motor to achieve smooth, accurate and stable tracking guidance.

[0047] Compared with the prior art, the present invention has the following advantages:

[0048] The local path planning and guidance method based on parallel elliptic limit cycles proposed in the present invention can solve the problems of safe planning, efficient obstacle avoidance and accurate guidance of robots in local environments when the global environment is unknown. First, based on the cognitive navigation method of parallel elliptic limit cycles, with the help of the idea of ​​periodic trajectory of limit cycles and its inherent system stability, the effective safe distance of the mobile robot relative to the envelope obstacle ellipse in the perception field of view can be always maintained without complex calculations, thereby ensuring that the robot avoids obstacles as much as possible. Secondly, in a mixed environment, the convergence of the robot to its own orbit and the safe offset from obstacles can be flexibly adjusted by modeling the differential equation of parallel elliptic limit cycles, so that a smooth navigation path can be obtained in different environments. Finally, based on the pure tracking guidance algorithm with adaptive preview distance, the curvature of the robot's future path to be tracked is calculated, and the preview distance and preview point are adaptively calculated to form a smooth control quantity, thereby ensuring a stable and accurate following path for the robot and obtaining a smooth and flexible navigation result. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 A schematic flow chart of the method of the present invention;

[0050] Figure 2 Schematic diagram of the solution results of parallel elliptic limit cycles;

[0051] Figure 3 Flowchart of the pure tracking algorithm based on preview distance adaptation. DETAILED DESCRIPTION

[0052] The present invention will be further explained below in conjunction with the accompanying drawings and specific implementation methods. It should be understood that the following specific implementation methods are only used to illustrate the present invention and are not intended to limit the scope of the present invention.

[0053] Example 1: Figure 1 As shown, a local path planning and guidance method for a differential mobile robot in a mixed environment comprises the following steps:

[0054] Step S1: Based on the perception input of the robot's current viewing angle sensor (including laser radar, camera, etc.), obstacles are identified through a segmentation detection algorithm to generate a local obstacle grid occupancy map;

[0055] (1-1) Use hardware connection to synchronize the data of multiple sensors, or use the external interrupt of microprocessor to synchronize the data of multiple sensors;

[0056] (1-2) Perform binary projection on the collected fusion information to obtain a local obstacle grid occupancy map.

[0057] Step S2: For the obstacle grid occupancy map input in step S1, the obstacle connected domain is extracted to form an obstacle connected map.

[0058] For the local obstacle grid occupancy map in step S1, a two-step fast scanning algorithm based on connected area extraction is used to extract connected domains.

[0059] (2-1) The first step is to traverse all row and column data, assign temporary labels and record equivalent items;

[0060] (2-2) The second step is to traverse each temporary label generated in the first step and replace it with the minimum label of the equivalent class.

[0061] Step S3: for the obstacle connectivity graph generated in step S2, calculate the centroid and direction of each obstacle area;

[0062] (3-1) Based on the connected graph generated in step S2, the centroid of each connected domain is determined using the computational geometry distance algorithm, and the calculation formula is as follows:

[0063]

[0064] Among them, n is the number of rows, m is the number of columns, (x i ,y j )(i=0,1,...,n-1;j=0,1,…,m-1) are pixel coordinates.

[0065] (3-2) Based on the connectivity graph generated in step S2, the minimum second-order moment axis (the equivalent axis of the minimum inertia axis on the two-dimensional plane) is used as the direction of the connectivity domain. The formula is:

[0066] E=∫∫r 2 f(x,y)dxdy (2)

[0067] Where r is the vertical distance from the point (x, y) to the line.

[0068] Step S4: Based on the obstacle mass center and direction calculated in step S3 and combined with the current position of the mobile robot, the obstacle impact levels are sorted to determine the obstacle that most affects the current movement;

[0069] (4-1) Based on the centroid of each obstacle connected domain in the local environment connected graph Calculate the current robot position (x now ,y now ) from each centroid: Where m = 1, 2, ..., n, n is the total number of connected domains in the connected graph;

[0070] (4-2) Based on the calculated distance set {dis1, dis2, dis3, ..., dis n} Sort from small to large and determine the minimum distance min{dis1,dis2,dis3,...,dis m} and obstructive material mind The kth obstacle connected domain is regarded as the obstacle that most affects the robot within the motion field of view.

[0071] Step S5: For the obstacle that most affects the current motion obtained in step S4, further determine the ellipse center (h, k), determine the ellipse direction Ω, determine the major semi-axis A, determine the minor semi-axis B, and the offset K p .

[0072] (5-1) Based on the obstacle k that most affects the movement of the robot determined in step S4, calculate its center of mass Maximum distance to the connected domain boundary (where j is the connected domain boundary point sequence), and takes it as the major semi-axis of the envelope ellipse A=len a , calculate the shortest distance from the centroid to the boundary of the connected domain Multiply it by a coefficient gain K greater than 1 cof The latter is the minor semi-axis of the enveloping ellipse B = K cof *len b ;

[0073] (5-2) Combined with the robot's physical size and its minimum safe collision distance to surrounding obstacles dis Considering the above, the offset K of the parallel ellipse is determined. p =safe dis .

[0074] Step S6: Based on the relevant parameters determined in step S5, construct a parallel elliptic limit cycle differential equation, and solve the differential equation to obtain local path sequence point information;

[0075] (6-1) Based on the major semi-axis, minor semi-axis, and offset determined in step S5, establish the parallel elliptic limit cycle equation.

[0076]

[0077] in, z3=(ABK p ) 2 ; A and B represent the major and minor semi-axis of the envelope ellipse SE, respectively; K p ∈R + (K p ≠0) represents the offset of the elliptical parallel curve relative to the enveloping ellipse. μ∈R + is a positive constant that can be used to adjust the convergence of the parallel elliptic limit cycle to its own orbit. r = 1 represents a clockwise trajectory, and r = -1 represents a counterclockwise trajectory.

[0078] (6-2) Solve the differential equation to obtain the parallel trajectory of the ellipse;

[0079] (6-3) Based on the obtained trajectory, the center of the mass ellipse (h, k) is translated and the direction of the connected domain is rotated Ω to obtain the final path trajectory path.

[0080] In this embodiment, the effect generated by completing the above steps is as follows: Figure 2 Schematic diagram of the solution results of parallel elliptic limit cycles shown.

[0081] Step S7: Based on the local path sequence point information of the parallel elliptic limit cycle generated in step S6, combined with the needs of task orientation and obstacle avoidance, determine the staged local target point goal.

[0082] Step S8: For the phased local target point goal determined in step S7, based on the current point P and the local path path, a pure tracking control algorithm with preview distance adaptation is applied to accurately track and guide the local path.

[0083] (8-1) Based on the parallel elliptic limit cycle trajectory obtained in step S6 and the stage target point determined in step S7, a pure tracking control algorithm based on preview distance adaptation is applied from the current position of the robot to accurately track and guide the local path;

[0084] (8-2) In each control cycle, the current point position A is first determined now If the threshold is less than the set navigation success threshold, the car is considered to have reached the target point and the execution is completed.

[0085] Otherwise, the curvature of the curve formed by the current point of the path combined with the next four points is calculated according to the following formula:

[0086]

[0087] The following adaptive preview distance function is established based on the curvature, and is solved to obtain the current preview distance.

[0088]

[0089] Among them, L d is the preview distance, c∈R + (c≠0) is the gain coefficient, e is a positive constant, low is the lower limit of the preview distance, and up is the upper limit of the preview distance.

[0090] Then based on the preview distance L d Determine the preview point C.

[0091] (8-3) Figure 3 As shown, the adaptive preview distance L d and the preview point C are input into the pure tracking algorithm, based on the current position A of the mobile robot now and the vehicle heading angle θ pre Calculate the heading angle increment α required for the vehicle this time:

[0092] α=atan2((C y -A now_y ),(C x -A now_x ))-θ pre (6)

[0093] According to the heading angle increment α and the preview distance L d Calculate the turning radius R according to the following formula.

[0094]

[0095] Based on the turning radius R, the angular velocity ω is calculated according to the given speed V, or the speed V is calculated according to the given angular velocity ω. The calculation formula is:

[0096]

[0097] The speed V and angular velocity ω information of each control cycle output is sent to the robot chassis motor to achieve smooth, accurate and stable tracking guidance.

[0098] After the control cycle is completed, the current position of the robot is updated. now , continue with step (8-2).

[0099] It should be noted that the above embodiments are not intended to limit the protection scope of the present invention, and equivalent changes or substitutions made on the basis of the above technical solutions all fall within the protection scope of the claims of the present invention.

Claims

1. A local path planning and guidance method for a differential mobile robot in a mixed environment, characterized in that: The method comprises the following steps: S1: Based on the perception input of the robot's current viewing angle sensor, obstacles are identified through the segmentation detection algorithm, and a local obstacle grid occupancy map is generated; S2: extracting the obstacle connected domain from the obstacle grid occupancy map input in step S1 to form an obstacle connected map; S3: For the obstacle connectivity graph generated in step S2, calculate the centroid and direction of each obstacle area; S4: Based on the obstacle mass center and direction calculated in step S3 and combined with the current position of the mobile robot, the obstacle impact levels are sorted to determine the obstacle that most affects the current movement; S5: for the obstacle that most affects the current movement obtained in step S4, further determine the center of mass, that is, the center of the ellipse, determine the direction, determine the major axis, determine the minor axis, and the offset; S6: Based on the relevant parameters determined in step S5, construct a parallel elliptic limit cycle differential equation, and solve the differential equation to obtain local path sequence point information; S7: Based on the local path sequence point information generated in step S6 and in combination with the environment, determine the phased local target point; S8: For the phased local target point determined in step S7, combined with the current position point and the local path sequence point, a pure tracking control algorithm based on preview distance adaptation is applied to accurately track and guide the local path.

2. The local path planning and guidance method for a differential mobile robot in a mixed environment according to claim 1 is characterized in that: Specifically in step S1 The process includes: (1-1) Use hardware connection to synchronize the data of multiple sensors, or use external interrupts of microprocessors to synchronize the data of multiple sensors; (1-2) Perform binary projection on the collected fusion information to obtain a local obstacle grid occupancy map.

3. The local path planning and guidance method for a differential mobile robot in a mixed environment according to claim 1 is characterized in that: Specifically in step S2 The process includes: For the local obstacle grid occupancy map generated in step S1, a two-step fast scanning algorithm based on connected area extraction is used to extract the connected domain. (2-1) The first step is to traverse all row and column data, assign temporary labels and record equivalent items; (2-2) The second step is to traverse each temporary label generated in the first step and replace it with the minimum label of the equivalent class.

4. The local path planning and guidance method for a differential mobile robot in a mixed environment according to claim 1 is characterized in that: Step S3 specifically includes the following process: (3-1) Based on the connected graph generated in step S2, the centroid of each connected domain is determined using the computational geometry distance algorithm, and the calculation formula is as follows: Among them, n is the number of rows, m is the number of columns, (x i ,y j ), i = 0, 1, ..., n-1; j = 0, 1, ..., m-1, are pixel coordinates, (3-2) Based on the connectivity graph generated in step S2, the minimum second-order moment axis, i.e., the equivalent axis of the minimum inertia axis on the two-dimensional plane, is used as the direction of the connectivity domain. The formula is: E=∫∫r 2 f(x,y)dxdy (2) Where r is the vertical distance from the point (x, y) to the line.

5. The local path planning and guidance method for a differential mobile robot in a mixed environment according to claim 1 is characterized in that: Specifically in step S4 The process includes: (4-1) Based on the centroid of each obstacle connected domain in the local environment connected graph Calculate the current robot position (x now ,y now ) from each centroid: Where m = 1, 2, ..., n, n is the total number of connected domains in the connected graph; (4-2) Based on the calculated distance set {dis1, dis2, dis3, ..., dis n } Sort from small to large and determine the minimum distance min{dis1,dis2,dis3,...,dis m } and the corresponding obstacle material center The kth obstacle connected domain is regarded as the obstacle that most affects the robot within the motion field of view.

6. The local path planning and guidance method for a differential mobile robot in a mixed environment according to claim 1 is characterized in that: Step S5 specifically includes the following process: (5-1) Based on the obstacle k that most affects the movement of the robot determined in step S4, calculate its center of mass Maximum distance to the connected domain boundary (where j is the connected domain boundary point sequence), and takes it as the major semi-axis of the envelope ellipse A=len a , calculate the shortest distance from the centroid to the boundary of the connected domain Multiply it by a coefficient gain K greater than 1 cof The latter is the minor semi-axis of the enveloping ellipse B = K cof *len b ; (5-2) Combined with the robot's physical size and its minimum safe collision distance to surrounding obstacles dis Considering the above, the offset K of the parallel ellipse is determined. p =safe dis .

7. The local path planning and guidance method for a differential mobile robot in a mixed environment according to claim 1 is characterized in that: In step S6, The process includes: (6-1) Based on the major semi-axis, minor semi-axis, and offset determined in step S5, establish the parallel ellipse limit cycle equation, in, A and B represent the major and minor axes of the envelope ellipse SE, respectively; K p ∈R + (K p ≠0) represents the offset of the elliptical parallel curve relative to the envelope ellipse, μ∈R + is a positive constant that can be used to adjust the convergence of the parallel elliptic limit cycle to its own orbit. r = 1 represents a clockwise trajectory, and r = -1 represents a counterclockwise trajectory. (6-2) Solve the differential equation to obtain the parallel trajectory of the ellipse; (6-3) Based on the obtained trajectory, the coordinates of the prime ellipse (h, k) are translated and the direction Ω of the connected domain is rotated to obtain the final path trajectory path.

8. The local path planning and guidance method for a differential mobile robot in a mixed environment according to claim 1 is characterized in that: In step S7, The process includes: Based on the parallel elliptical limit cycle trajectory obtained in step S6, combined with the requirements of task orientation and obstacle avoidance, the stage target point on the limit cycle is determined.

9. The local path planning and guidance method for a differential mobile robot in a mixed environment according to claim 1, characterized in that: In step S8, The process includes: (8-1) Based on the parallel elliptic limit cycle trajectory obtained in step S6 and the stage target point determined in step S7, a pure tracking control algorithm based on preview distance adaptation is applied from the current position of the robot to accurately track and guide the local path; (8-2) In each control cycle, the curvature of the curve formed by the current point of the path and the next four points is calculated, and an adaptive preview distance function is established based on the curvature, and the preview distance and preview path point at the current moment are determined by solving the function; (8-3) The adaptive preview distance and preview path point are input into the pure tracking algorithm, and the speed and angular velocity information of each control cycle is calculated and output, and sent to the robot chassis motor to achieve smooth, accurate and stable tracking guidance.

Images