Mobile robot trajectory planning method and device based on two-step optimization strategy

Abstract

The present invention relates to a mobile robot trajectory planning method based on a two-step optimization strategy, comprising the following steps: using a maximum inscribed circle to represent the mobile robot's collision range, simplifying the modeling of collision constraints, using control points of a spline as optimization variables, and adaptively adjusting the number of control points based on the environment. The optimized polynomial curve serves as the initial value for the next optimization step; using a Euclidean distance field to accurately describe the mobile robot's chassis information, and using adjacent connected path points in the cubic polynomial curve as the optimization objective function. The algorithm is then used to solve the robot's optimized posture sequence and path curve. The present invention also relates to a computer device comprising a memory and a processor, wherein the processor implements the trajectory planning method when executing instructions stored in the memory.

Description

Technical Field

[0001] The present invention relates to a technical solution for mobile robot trajectory planning, in particular to a mobile robot trajectory planning method and device based on a two-step optimization strategy, and belongs to the technical field of robot automatic control. Background Art

[0002] Mobile robots have broad applications and development prospects in areas such as home services and factory logistics. However, complex environments place higher demands on mobile robots' navigation planning. Navigating extremely confined environments, such as narrow passages like doors and corridors, remains a challenging challenge for mobile robots. Traditional path sampling methods are unable to achieve navigation in such environments due to limited sampling resolution. Conventional optimization methods often struggle to generate collision-free navigation trajectories due to inappropriate constraint modeling and initial value settings, making stable navigation in confined spaces impossible.

[0003] Furthermore, real-time online planning and navigation for mobile robots in confined environments is a significant challenge, placing higher demands on the accuracy and real-time performance of planning algorithms. Traditional trajectory planning optimization methods, due to improper selection of initial values, consume more computing resources to iterate and obtain a suitable trajectory, increasing the consumption of onboard computing platform resources and computational time, impacting the real-time performance of planning. Summary of the Invention

[0004] The present invention provides a mobile robot trajectory planning technology solution, aiming to solve at least one of the technical problems existing in the prior art, such as the technical problem that current navigation planning algorithms occupy a large amount of computing resources and are time-consuming in narrow channel environments.

[0005] In some aspects, the technical solution of the present invention is a path planning method for a mobile robot, comprising the following steps:

[0006] S100, obtaining geometric data of a maximum inscribed circle according to the chassis shape of the mobile robot, and configuring a set of control points of a preset number at an initial sampling resolution to associate with a trajectory line of path planning of the mobile robot;

[0007] S200, updating the collision data of the mobile robot, constructing collision avoidance constraints, and optimizing the positions of a set of control points based on a first objective function of minimum path length consumption under a collision-free condition, and fitting the control points obtained by the optimized calculation into an updated trajectory line;

[0008] S300, determining that the maximum inscribed circle of the mobile robot does not collide with an obstacle when moving along all travels of the updated trajectory line, obtaining polynomial curve trajectory geometry data under the inscribed circle constraint model as an initial value of the refined optimized trajectory; otherwise, adding more control points in the collision area, and returning to step S200;

[0009] S400, establishing a Euclidean distance field model of the mobile robot based on the precise geometric shape data obtained from the chassis shape of the mobile robot, adding a collision risk area, and constructing collision constraints;

[0010] S500, using the curve generated by the polynomial curve trajectory geometric data as a line connecting the path points of the mobile robot, establishing a posture sequence of the mobile robot in the path points as an optimization variable, solving the optimized posture sequence based on the optimization equation of the second objective function of minimum energy consumption, and fitting the posture sequence obtained by the optimization solution into an updated polynomial curve;

[0011] S600. If the precise geometric shape of the mobile robot does not collide with any obstacle along all travels of the fitted polynomial curve, the coefficient parameters of the polynomial curve and the optimized pose sequence are obtained as the path planning result of the mobile robot. Otherwise, the pose sampling density in the collision area is increased to generate a pose sequence, and the process returns to step S400.

[0012] Furthermore, the step S100 includes:

[0013] Set the risk area and provide the risk penalty value calculation function f through the following formula obs , the first objective function optimization calculation for minimum energy consumption,

[0014]

[0015] Where, ρ represents the distance between the chassis center of the mobile robot and the edge of the obstacle, r m represents the maximum inscribed circle radius of the chassis shape of the mobile robot, c represents the maximum value of the risk area, δ represents the maximum distance from the edge of the risk area to the edge where the collision occurs,

[0016] If ρ <r m Then the mobile robot is in the collision zone if r m <ρ <r m +δ then the mobile robot is in the risk area.

[0017] Furthermore, the step S100 further includes:

[0018] Preliminary curve sampling is performed along the paths of the mobile robot's movement starting point, movement target point, and obstacle detour points to evenly configure a set of control points, the initial configuration number of which is substantially equal to the total number of points of the obstacle detour, movement starting point, and movement target point.

[0019] Furthermore, the step S200 includes the following steps:

[0020] S210, the first objective function of minimum energy consumption is obtained by the following formula The gradient descent algorithm is used to optimize the position vector q of the control point.

[0021]

[0022]

[0023] Among them, J b represents the collision constraint, J sm Indicates the intensity of the change of the control point, J le represents the length constraint of the trajectory line, λ b ,λ sm ,λ le They represent the constraint coefficient, smoothness coefficient and path length coefficient of collision avoidance, respectively. obs Represents the risk penalty value calculation function, o j Represents the obstacle position vector. i, j, n, and m are natural numbers and are used for subscript and subscript counting of parameters in the formula.

[0024] Furthermore, the step S200 further includes the following steps:

[0025] S220, fitting the control points obtained by optimization calculation into a spline curve p using the following formula i (τ) and as the trajectory line,

[0026]

[0027] Among them, τ is the function variable used to generate the spline curve, τ∈[0,1],n p is the number of path points of the mobile robot, The constant matrix representing the spline curve, s(τ) is the vector of polynomial terms constituting the spline curve, π i is the control point matrix, i is a natural number used to count the subscripts and subscripts of the parameters in the formula, i≤n q -n p , n q is the number of control points, which must not be less than the number of path points.

[0028] Furthermore, the step S400 includes:

[0029] Based on the Euclidean distance field model, the collision risk value calculation function is provided by the following formula To establish collision constraints, solve the optimization equation for the second objective function of minimum energy consumption,

[0030]

[0031] Among them, o represents the obstacle vector, c represents the maximum value of the risk area, Represents the Euclidean distance field model operation, represents the shape space of the mobile robot chassis, represents the expanded shape space of the mobile robot chassis after superimposing the risk area, Indicates that the obstacle is within the collision range. Indicates that the obstacle is within the risk area, c represents the maximum value of the risk area, δ represents the maximum distance from the edge of the risk area to the collision edge, and min{||op||} represents the closest distance from the obstacle to the boundary.

[0032] Furthermore, step S500 includes the following steps:

[0033] S510, the second objective function of minimizing energy consumption is obtained by the following formula Optimally solve the pose sequence z of the mobile robot,

[0034]

[0035] J end =λ end ||z i -z e ||

[0036]

[0037] Among them, J smo represents the smoothness constraint, j end represents the end pose constraint, J kin Indicates arc constraint, J v represents the no-collision constraint, λ smo represents the smoothness penalty coefficient matrix, λ end Represents the penalty coefficient of the optimization objective, λ kin represents the arc constraint coefficient, λ v represents the non-collision constraint coefficient, δ represents the maximum distance from the edge of the risk area to the edge of the collision, h represents the kinematic constraint function of the mobile robot, z i represents the pose vector, z e represents the end pose vector, Represents the collision risk value calculation function, Ri Represents the posture matrix of the mobile robot, o j Represents the position vector of the obstacle, p i Represents the position vector of the mobile robot. i and j are natural numbers and are used for subscript and subscript counting of parameters in the formula.

[0038] Furthermore, step S500 includes the following steps:

[0039] S520, perform cubic polynomial curve fitting on the pose sequence z obtained by optimization in the X and Y directions using the following formula:

[0040]

[0041] Among them, τ is the function variable used to generate the cubic polynomial curve, s(τ) is the vector of polynomial terms that constitute the cubic polynomial curve, and d a and d b are the parameter matrices of the polynomial terms in the X and Y directions respectively;

[0042] Among them, the constraint condition AX is given according to the requirement that the starting and ending points of the trajectory of adjacent posture points in the posture sequence of the mobile robot need to meet the requirement that the position and posture coincide. k = 0, and energy optimization is used as the objective function through the following formula Optimally solve the optimization variable X of the cubic polynomial curve k ,

[0043]

[0044] stAX k =0

[0045] in,

[0046]

[0047] η k and γ k is the relaxation factor coefficient, B and G are constant matrices, and k is the formula letter subscript.

[0048] The technical solution of the present invention also relates to a computer device, comprising a memory and a processor, wherein the processor implements the above method when executing a computer program stored in the memory.

[0049] The technical solution of the present invention also relates to a mobile robot, comprising: an environmental sensor for obtaining position and shape data of obstacles; a posture sensor for feeding back position and posture data of the mobile robot; and the above-mentioned computing device, which is respectively connected to the environmental sensor and the posture sensor.

[0050] Therefore, the proposed solution mainly involves a two-step optimization: the first step uses the maximum inscribed circle to represent the collision range of the mobile robot, simplifying the modeling of the collision constraints, and uses the control points of the cubic spline as the optimization variables. The number of control points can also be adaptively adjusted according to the environment. The optimized polynomial curve is used as the initial value for the next optimization step; the second step uses the Euclidean distance field to accurately describe the chassis information of the mobile robot, and uses the adjacent connected path points in the cubic polynomial curve as the optimization objective function, and solves the problem through the LBFGS (Limited-memory Broyden Fletcher Goldfarb Shanno) algorithm. The beneficial effect is that in narrow space channels, the use of rough collision modeling in the first step expands the solution space, avoids considering the optimization modeling of attitude angles, greatly reduces computational time, and can obtain a relatively good initial value for the second step; in the second step, an accurate mobile robot collision model can be established, making the planning results more optimized and accurate.

[0051] In addition, additional aspects and advantages of the present invention will be set forth in part in the following description and in part will be obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 is a flowchart of a planning method according to an embodiment of the present invention.

[0053] Figure 2 It is a schematic diagram of the theoretical shape of the mobile robot and its largest inscribed circle, and represents its relationship with obstacles.

[0054] Figure 3 It is a schematic diagram representing the impact of risk areas on mobile robot planning.

[0055] FIG4 is a schematic diagram showing the influence of control points of different resolutions on planning.

[0056] Figure 5 It is a schematic diagram of inserting control points between the control points where collision occurs.

[0057] Figure 6 It is the numerical value of the Euclidean distance field of the mobile robot chassis and its gradient diagram.

[0058] Figure 7 Schematic diagram of obstacle avoidance control point planning with a sampling interval of 0.5 m in an example according to the present invention.

[0059] Figure 8 Schematic diagram of obstacle avoidance planning with adaptive control point adjustment at a sampling interval of 0.5 m according to an example of the present invention.

[0060] Figure 9It is a planning diagram in a random obstacle environment.

[0061] Figure 10 It is a schematic diagram of planning in a channel environment with a width of 0.7m according to an example of the present invention.

[0062] Figure 11 It is a schematic diagram of planning in a narrow and curved channel according to an example of the present invention.

[0063] Figure 12 4 is a diagram of a physical test scene of a mobile robot in an example according to the present invention. DETAILED DESCRIPTION

[0064] The following will provide a clear and complete description of the concept, specific structure and technical effects of the present invention in conjunction with the embodiments and drawings to fully understand the purpose, scheme and effects of the present invention.

[0065] Reference Figure 1 In some embodiments, the method according to the present invention basically comprises the following steps:

[0066] S100, obtaining geometric data of a maximum inscribed circle according to the chassis shape of the mobile robot, and configuring a set of control points of a preset number at an initial sampling resolution to associate with a trajectory line of path planning of the mobile robot;

[0067] S200, updating the collision data of the mobile robot, constructing collision avoidance constraints, and optimizing the positions of a set of control points based on a first objective function of minimum energy consumption, and fitting the control points obtained by the optimization calculation into an updated trajectory line;

[0068] S300, determining that the maximum inscribed circle of the mobile robot does not collide with an obstacle when moving along all travels of the updated trajectory line, obtaining polynomial curve trajectory geometry data under the inscribed circle constraint model as an initial value of the refined optimized trajectory; otherwise, adding more control points in the collision area, and returning to step S200;

[0069] S400, establishing a Euclidean distance field model of the mobile robot based on the precise geometric shape data obtained from the chassis shape of the mobile robot, adding a collision risk area, and constructing collision constraints;

[0070] S500, using the curve generated by the polynomial curve trajectory geometric data as a line connecting the path points of the mobile robot, establishing a posture sequence of the mobile robot in the path points as an optimization variable, solving the optimized posture sequence based on the optimization equation of the second objective function of minimum energy consumption, and fitting the posture sequence obtained by the optimization solution into an updated polynomial curve;

[0071] S600. If the precise geometric shape of the mobile robot does not collide with any obstacle along all travels of the fitted polynomial curve, the coefficient parameters of the polynomial curve and the optimized pose sequence are obtained as the path planning result of the mobile robot. Otherwise, the pose sampling density in the collision area is increased to generate a pose sequence, and the process returns to step S400.

[0072] The following describes in detail how to implement the above steps.

[0073] Specific implementation of step S100

[0074] As attached Figure 2 As shown, the chassis of the mobile robot is a polygon that is approximately rectangular. First, the maximum inscribed circle of the rectangle is obtained, where the maximum radius of the inscribed circle is r m , and establish a risk area, δ represents the width of the risk area, c represents the maximum penalty value of the risk area, ensuring that the mobile robot keeps a certain distance from obstacles as much as possible under the premise of ensuring safety. If the passing area is relatively narrow, the planned trajectory will tend to the middle of the narrow space, and the obstacle may fall into the risk area, but will not fall into the inscribed circle. If the passing area is wide, the planned trajectory will be as far away from the obstacle as possible, so that the obstacle will not fall into the risk area. The established prohibited area and risk area function f obs (ρ) is as follows:

[0075]

[0076] Among them, if ρ <r m Then the mobile robot will collide with the obstacle, satisfying r m <ρ <r m +δ, then the mobile robot is in the risk area and will not collide with obstacles. However, if the control accuracy of the mobile robot is insufficient, there is a risk of collision. For example, Figure 3 The schematic diagram of the impact of risk areas on mobile robot planning is shown in Figure 1 (the dotted area in the figure is the prohibited area, and the expanded area outside the dotted line is the risk area). When the mobile robot passes through two obstacles, r m <ρ <r m +δ case.

[0077] Furthermore, as shown in Figure 4, using the control point sampling resolution in Figure 4(a) results in more optimization target variables, increasing computation time. Using the larger spacing in Figure 4(b) to set the control point distribution reduces the scale of the optimization problem and computation time, but the trajectory lines between the control points may collide with obstacles. Using a larger spacing for the initial control points reduces computation time, and in step S300, adaptively inserting control points to achieve a reasonable control point distribution, ensuring shorter computation time and collision-free safety.

[0078] Specific implementation of step S200

[0079] Construct collision avoidance constraints and use trajectory smoothness and shortest trajectory as the objective function to be solved to obtain the optimal control point position, where J b represents the collision constraint, J sm Indicates the intensity of the change of the control point, J le represents the length constraint of the trajectory line, λ b ,λ sm ,λ le They represent the constraint coefficient, smoothness coefficient, and path length coefficient for collision avoidance, respectively, as shown in the following formulas:

[0080]

[0081]

[0082] And use the gradient descent algorithm to optimize the solution. The spline formed by the control points can be obtained as follows,

[0083]

[0084] where τ∈[0,1],i=0,1,…,n q -n p , The constant matrix representing the spline, the spline curve formed according to the above control points, is used as the initial value in the fine optimization process.

[0085] Specific implementation of step S300

[0086] It is necessary to perform collision check on the spline obtained by optimizing the control points to check whether the trajectory line obtained by optimizing the calculation collides with the obstacle. If a collision occurs, a new control point is inserted between the colliding control points, such as Figure 5 As shown, and return to step S200 to calculate again. If no collision occurs, then the polynomial curve trajectory under the rough inscribed circle constraint model is obtained as the initial value of the fine optimization. For example, Figure 7 The following is a schematic diagram of obstacle avoidance control point planning based on a sampling interval of 0.5m. Figure 7 If the calculated trajectory line collides with an obstacle during the inspection, the interval adaptive control points need to be adjusted to generate a new obstacle avoidance planning curve. The effect is as follows: Figure 8 shown.

[0087] The specific implementation of step S400 is as follows:

[0088] Establish an accurate Euclidean distance field (EDF) model for mobile robots. The Euclidean distance field can easily query the distance and gradient information of obstacles, which plays an important role in the online motion planning of mobile robots. Directly using the Euclidean distance field for the optimization process will cause the mobile robot to be too close to the obstacle. Therefore, a risk area is added to keep the obstacle as far away from the mobile robot as possible. If the obstacle falls outside the risk area, its collision penalty value is 0. The collision penalty function used for the constraint The build is as follows,

[0089]

[0090] If the obstacle is within the risk area, c represents the maximum value of the risk area, δ represents the maximum distance from the edge of the risk area to the edge of the collision, so its value in the risk area is Where min{||op||} represents the shortest distance from the obstacle to the boundary. The specific field diagram is shown in Figure 6 .

[0091] Specific implementation of step S500

[0092] The pose sequence is used as the optimization target variable, and smoothness is also used as the optimization equation of the optimization objective function.

[0093]

[0094] Among them J smo The smoothness constraint is expressed as follows, where λ smo represents the penalty coefficient matrix,

[0095]

[0096] Among them J end Represents the end pose constraint. If the final target point is not reached, the penalty coefficient λ of this optimization objective end It can be set to a smaller value as follows

[0097] J end =λ end ||z i -z e || (5b)

[0098] The mobile robot also needs to satisfy the arc constraint as much as possible, but this constraint is not a mandatory constraint and is a very loose constraint, as shown below, where λ kin is the constraint coefficient,

[0099]

[0100] The mobile robot also needs to meet the no-collision constraint as much as possible and try to drive in the middle of adjacent obstacles, so the constraints are set as follows, where λ v Represents the constraint coefficient, R i Represents the posture matrix of the mobile robot, o j Indicates an obstacle, p i Indicates the position information of the mobile robot.

[0101]

[0102] The meanings of other characters in the above variables are as follows: i represents the pose vector, p represents the position information, is a two-dimensional real number space, representing a point on the plane, S 1 represents the parameterization of the circle, which is often used to describe rotation or direction. h(*,*) represents the kinematic constraint function of the mobile robot.

[0103]

[0104] p=[x i ,y i ] T (6a)

[0105] Δz i =z i+1 -z i (6b)

[0106] δz i,i+1 =Δz i+1 -Δz i (6c)

[0107]

[0108] h(z i ,z i+1 )=|(V θ,i +V θ,i+1 )×V p,i |2 (6e)

[0109] V θ,i =[cos(θ i ),sin(θ i )] T (6f)

[0110] V p,i =[x i+1 -x i ,y i+1 -y i ] T (6g)

[0111] In the above steps, the planning of the pose point is completed, which is a discrete path point. Based on the pose sequence obtained by optimization, the cubic polynomial fitting and interpolation based on energy optimization are used. The formulas of the cubic polynomial to be fitted in the XY direction are respectively expressed as

[0112]

[0113] Where s(τ), They are expressed as follows:

[0114] d a =[a0,a1,a2,a3],d b =[b0,b1,b2,b3] (7a)

[0115] s(τ)=[1,τ,τ 2 ,τ 3 ],τ∈[0,1] (7b) The starting and ending points of the trajectory lines of adjacent postures need to meet the requirements of position and posture coincidence. Therefore, the following constraints are imposed:

[0116]

[0117] The above η k , γ k is the relaxation factor, then the optimization variable can be written as follows,

[0118]

[0119] Then (7c) and (7d) can be written as follows

[0120] AX k =0 (7f)

[0121] Then there is

[0122]

[0123] Use energy optimization as the objective function as follows

[0124]

[0125] Similarly, there are

[0126]

[0127] stAX k =0

[0128] Where G can be expressed as follows

[0129]

[0130] The target equation is a quadratic problem, or QP problem, and can therefore be solved quickly, taking less than 2ms to compute. In this step, the polynomial fitting process for adjacent pose points is completed.

[0131] Specific implementation of step S600

[0132] If the energy-optimal polynomial curve fitted in step S500 collides with an obstacle, the pose sampling density at the collision point is increased, and steps S400 and S500 are executed again. If no collision occurs, the planning process ends, and the coefficient parameters of the polynomial curve and the optimized pose sequence are used as the path planning results for the mobile robot and transmitted to the motion controller of the mobile robot.

[0133] Figures 9 to 12 The robot trajectory planning method using the two-step optimization strategy of the present invention is verified in simulation scenarios and actual scenarios respectively. Figure 9 It is a planning diagram in a random obstacle environment; Figure 10 This is a schematic diagram of planning in a channel environment with a width of 0.7m according to an example of the present invention; Figure 11 This is a schematic diagram of planning in a narrow and curved passage according to an example of the present invention; Figure 12 Figure 1 is a diagram of a physical test scenario of a mobile robot in an example of the present invention, which verifies the safe real-time navigation of the mobile robot in a confined space.

[0134] It should be appreciated that the method steps in the embodiments of the present invention can be implemented or executed by computer hardware, a combination of hardware and software, or by computer instructions stored in a non-transitory computer-readable memory. The method can use standard programming techniques. Each program can be implemented in a high-level procedural or object-oriented programming language to communicate with the computer system. However, if desired, the program can be implemented in assembly or machine language. In any case, the language can be a compiled or interpreted language. In addition, for this purpose, the program can be run on a programmed application-specific integrated circuit.

[0135] Furthermore, the operations of the processes described herein may be performed in any suitable order unless otherwise indicated herein or otherwise clearly contradicted by the context. The processes described herein (or variations and / or combinations thereof) may be performed under the control of one or more computer systems configured with executable instructions and may be implemented as code (e.g., executable instructions, one or more computer programs, or one or more applications) that is executed collectively on one or more processors, by hardware, or a combination thereof. The computer program includes a plurality of instructions that can be executed by one or more processors.

[0136] Further, the methods can be implemented in any type of computing platform that is operably connected to a suitable computer, including but not limited to a personal computer, a minicomputer, a mainframe, a workstation, a network or distributed computing environment, a separate or integrated computer platform, or in communication with a charged particle tool or other imaging device, etc. Various aspects of the present invention can be implemented as machine-readable code stored on a non-transitory storage medium or device, whether removable or integrated into a computing platform, such as a hard disk, an optical read and / or write storage medium, RAM, ROM, etc., so that it can be read by a programmable computer, and when the storage medium or device is read by the computer, it can be used to configure and operate the computer to perform the processes described herein. In addition, the machine-readable code, or portions thereof, can be transmitted over a wired or wireless network. When such media includes instructions or programs that implement the steps described above in conjunction with a microprocessor or other data processor, the invention described herein includes these and other different types of non-transitory computer-readable storage media. When programmed according to the methods and techniques of the present invention, the present invention can also include the computer itself.

[0137] The computer program can be applied to input data to perform the functions described herein, thereby converting the input data to generate output data that is stored in a non-volatile memory. The output information can also be applied to one or more output devices such as a display. In a preferred embodiment of the present invention, the converted data represents a physical and tangible object, including a specific visual depiction of the physical and tangible object produced on the display.

[0138] It should be noted that, if not otherwise specified, when a certain feature is referred to as "fixed", "connected" to another feature, it can be directly fixed, connected to another feature, or it can be indirectly fixed, connected to another feature. It should be understood that although the terms first, second, third, etc. may be used to describe various elements herein, these elements should not be limited to these terms. These terms are only used to distinguish elements of the same type from each other. For example, without departing from the scope of this article, the first element may also be referred to as the second element, and similarly, the second element may also be referred to as the first element. The use of any and all examples or exemplary languages ​​("for example", "such as", etc.) provided herein is intended only to better illustrate embodiments of the present invention, and unless otherwise required, will not impose limitations on the scope of the present invention.

[0139] The above description is merely a preferred embodiment of the present invention. The present invention is not limited to the aforementioned embodiments. As long as the technical effects of the present invention are achieved by the same means, any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention shall be included within the scope of protection of the present invention. Within the scope of protection of the present invention, various modifications and variations of the technical solutions and / or implementation methods are possible.

Claims

1. A path planning method for a mobile robot, characterized in that: The method comprises the following steps: S100, obtaining geometric data of a maximum inscribed circle according to the chassis shape of the mobile robot, and configuring a set of control points of a preset number at an initial sampling resolution to associate with a trajectory line of path planning of the mobile robot; S200, updating the collision data of the mobile robot, constructing collision avoidance constraints, and optimizing the positions of a set of control points based on a first objective function of minimum path length consumption under a collision-free condition, and fitting the control points obtained by the optimized calculation into an updated trajectory line; S300, determining that the maximum inscribed circle of the mobile robot does not collide with an obstacle when moving along all travels of the updated trajectory line, obtaining polynomial curve trajectory geometry data under the inscribed circle constraint model as an initial value of the refined optimized trajectory; otherwise, adding more control points in the collision area, and returning to step S200; S400, establishing a Euclidean distance field model of the mobile robot based on the precise geometric shape data obtained from the chassis shape of the mobile robot, adding a collision risk area, and constructing collision constraints; S500, using the curve generated by the polynomial curve trajectory geometric data as a line connecting the path points of the mobile robot, establishing a posture sequence of the mobile robot in the path points as an optimization variable, solving the optimized posture sequence based on the optimization equation of the second objective function of minimum energy consumption, and fitting the posture sequence obtained by the optimization solution into an updated polynomial curve; S600: Determine that the precise geometric shape of the mobile robot does not collide with any obstacle along all travels of the fitted polynomial curve, obtain coefficient parameters of the polynomial curve and an optimized pose sequence as the path planning result of the mobile robot; otherwise, increase the pose sampling density in the collision area to generate a pose sequence, and return to step S400; Wherein, the step S100 includes: Set the risk area and provide the risk penalty value calculation function through the following formula , the first objective function optimization calculation for minimum energy consumption, in, represents the distance between the center of the chassis of the mobile robot and the edge of the obstacle, represents the maximum inscribed circle radius of the chassis shape of the mobile robot, represents the maximum value of the risk area, Indicates the maximum distance from the edge of the risk area to the edge where the collision occurs. if Then the mobile robot is in the collision zone if Then the mobile robot is in the risk zone; The step S200 includes the following steps: S210, the first objective function of minimum energy consumption is obtained by the following formula , using the gradient descent algorithm to optimize the position vector of the control point , in, represents the collision constraint, Indicates the intensity of the change of the control point. represents the length constraint of the trajectory line, 、 、 They represent the constraint coefficient, smoothness coefficient and path length coefficient of collision avoidance respectively. represents the risk penalty value calculation function, represents the obstacle position vector, i 、 j 、 n and m It is a natural number and is used for counting the subscripts and subscripts of parameters in formulas; The step S200 further includes the following steps: S220, fitting the control points obtained by optimization calculation into a spline curve using the following formula And as a trajectory line, in, is the function variable used to generate the spline curve, , is the polynomial order, A constant matrix representing the spline curve, is the vector of polynomial terms that make up the spline curve, is the control point matrix, i is a natural number used to count the subscripts and subscripts of the parameters in the formula, , is the number of control points.

2. The method according to claim 1, wherein The step S100 further includes: Preliminary curve sampling is performed along the paths of the mobile robot's movement starting point, movement target point, and obstacle detour points to evenly configure a set of control points, the initial configuration number of which is substantially equal to the total number of points of the obstacle detour, movement starting point, and movement target point.

3. The method according to claim 1, wherein The step S400 includes: Based on the Euclidean distance field model, the collision risk value calculation function is provided by the following formula To establish collision constraints, solve the optimization equation for the second objective function of minimum energy consumption, in, represents the obstacle vector, represents the maximum value of the risk area, Represents the Euclidean distance field model operation, represents the shape space of the mobile robot chassis, represents the expanded shape space of the mobile robot chassis after superimposing the risk area, Indicates that the obstacle is within the collision range. Indicates that the obstacle is within the risk area. Indicates the maximum distance from the edge of the risk area to the edge where the collision occurs. Indicates the shortest distance from the obstacle to the boundary.

4. The method according to claim 1 or 3, wherein The step S500 includes the following steps: S510, the second objective function of minimizing energy consumption is obtained by the following formula , optimally solve the pose sequence of the mobile robot , in, represents the smoothness constraint, represents the end pose constraint, represents the kinematic constraints, Indicates no collision constraints. represents the smoothness penalty coefficient matrix, represents the penalty coefficient of the optimization objective, represents the arc constraint coefficient, represents the no-collision constraint coefficient, Indicates the maximum distance from the edge of the risk area to the edge where the collision occurs. represents the kinematic constraint function of the mobile robot, represents the pose vector, represents the end pose vector, represents the collision risk value calculation function, represents the posture matrix of the mobile robot, represents the position vector of the obstacle, represents the position vector of the mobile robot, i and j It is a natural number used for counting subscripts and subscripts of parameters in formulas.

5. The method according to claim 4, wherein The step S500 includes the following steps: S520, optimize the pose sequence obtained by the following formula Perform cubic polynomial curve fitting in the X and Y directions. in, is the function variable used to generate the cubic polynomial curve, is the vector of polynomial terms that constitute the cubic polynomial curve, and are the parameter matrices of the polynomial terms in the X and Y directions respectively; Among them, the constraint condition is given according to the requirement that the starting and ending points of the trajectory of adjacent posture points in the posture sequence of the mobile robot need to meet the position and posture coincidence requirement. , energy optimization is used as the objective function through the following formula , optimally solve the optimization variables of the cubic polynomial curve , in, and is the relaxation factor coefficient, and is a constant matrix, k It is the formula letter subscript.

6. A computing device comprising a memory and a processor, characterized in that: The method according to any one of claims 1 to 5 is implemented when the processor executes the computer application stored in the memory.

7. A mobile robot, characterized in that: include: Environmental sensors, used to obtain the location and shape data of obstacles; Posture sensor, used to feedback the position and posture data of the mobile robot; The computing device according to claim 6, wherein the computing device is connected to the environmental sensor and the posture sensor respectively.

Images