Track planning method and device and medium

Through the trajectory planning method and quadratic planning technology based on grid map, combined with obstacle distribution optimization, the smoothness and responsiveness of trajectory planning in unstructured environments are solved, and the stability and tasks of robots in complex environments are improved. Success rate.

CN119043355APending Publication Date: 2024-11-29BEIJING KEYI TECH CO LTD
View PDF 0 Cites 2 Cited by

Patent Information

Application Number
CN202411081085.0
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-07
Publication Date
2024-11-29

AI Technical Summary

Technical Problem

Traditional trajectory planning methods are difficult to meet the needs of trajectory smoothness, continuity and fast response in unstructured environments, resulting in unstable operation of robots in complex and changeable environments, prone to collisions or task failures.

Method used

Using a trajectory planning method based on grid map, smooth trajectory is generated through initial path planning and secondary planning, and trajectory optimization is carried out in combination with obstacle avoidance errors in obstacle distribution to ensure the continuous trajectory in velocity, acceleration, orientation angle and orientation angular velocity. Sex and safety.

Benefits of technology

It improves the motion stability and task success rate of robots in unstructured environments, realizes efficient response and safe navigation in complex dynamic environments, reduces the use of computing resources, and enhances the path computing efficiency and trajectory planning security.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119043355A_ABST
    Figure CN119043355A_ABST
Patent Text Reader

Abstract

The embodiment of the invention provides a trajectory planning method, a trajectory planning device and a medium, which are used for solving the problem that a traditional trajectory algorithm is difficult to meet the requirements of trajectory smoothness, continuity and quick response at the same time in an unstructured environment. The method comprises the steps of performing trajectory planning based on a distance between a first trajectory point corresponding to each first target grid in a grid map and an obstacle to obtain an initial trajectory from a target position to a final position, and performing secondary planning based on the initial trajectory, a current speed, a current orientation angular speed and a preset trajectory final speed to obtain a second trajectory; determining a closed-form solution of the target function to obtain a smooth trajectory; and based on the distances between the plurality of second trajectory points and the obstacle, performing trajectory optimization on the smooth trajectory by using a sequential quadratic programming algorithm to obtain a planned trajectory. Further, dynamics inspection and point-by-point inspection are carried out on the planned trajectory, and the trajectory speed, the corner speed and the acceleration are adjusted to ensure that the trajectory is feasible and safe.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present application relates to the technical field of intelligent path planning, and in particular to a trajectory planning method, device and medium. Background Art

[0002] Robots are widely used in logistics, security inspection, home services, agricultural automation and other fields due to their simple mechanical structure and flexible motion control. In unstructured environments such as homes and offices, robots need to face highly dynamic and complex environmental changes. Compared with structured environments (such as industrial plants and warehouses), the obstacle distribution and environmental dynamics of unstructured environments are more unpredictable. In the prior art, trajectory planning mainly includes sampling-based methods, search-based methods and optimization-based methods. However, there are significant limitations in the application of the above trajectory planning methods in unstructured environments, especially in terms of path smoothness and continuity, real-time response capability and dynamic adaptability. The above methods are difficult to meet the requirements of high stability and rapid response at the same time, resulting in unstable operation of robots in complex and changing environments, prone to collisions or mission failures. Summary of the invention

[0003] The embodiments of the present application provide a trajectory planning method, device and medium to solve the problem that traditional trajectory algorithms are difficult to simultaneously meet trajectory smoothness, continuity and rapid response in unstructured environments.

[0004] In a first aspect, an embodiment of the present application provides a trajectory planning method, including:

[0005] Performing trajectory planning based on the distance between the first trajectory point corresponding to each first target grid in the grid map and the obstacle to obtain an initial trajectory from the target position to the end position, wherein the target position is the current position of the differential robot, the first target grid is the grid corresponding to the target position, and the initial trajectory includes a plurality of first trajectory points;

[0006] Based on the initial trajectory, the current speed of the differential robot, the current angular velocity of the orientation and the preset trajectory endpoint velocity, a secondary programming is performed to determine a closed-form solution of the objective function to obtain a smooth trajectory; wherein the smooth trajectory includes a plurality of second trajectory points, and the objective function is used to determine the corresponding trajectory when the set conditions are met;

[0007] The smooth trajectory is optimized based on the distances between the plurality of second trajectory points and obstacles, so as to obtain a planned trajectory from the current position to the end position.

[0008] In a possible implementation, performing trajectory planning based on the distance between the first trajectory point corresponding to the first target grid in the grid map and the obstacle to obtain an initial trajectory from the target position to the end position includes:

[0009] Selecting a first trajectory point from a plurality of first trajectory points based on the distance between the first trajectory point corresponding to each of the first target grids and the obstacle;

[0010] Determine a second target grid corresponding to the selected first trajectory point;

[0011] Selecting a first track point from the plurality of first track points based on the distance between the first track points corresponding to each of the second target grids and the obstacle, and continuing to determine the second target grid corresponding to the selected first track point until the selected first track point is the end point position;

[0012] Based on the selected first trajectory points, an initial path is obtained;

[0013] According to the trajectory direction represented by the line connecting any two adjacent first trajectory points in the initial path and the preset time from the target position to the end position, a first processing operation is performed on the plurality of first trajectory points in the initial path to obtain an initial trajectory from the target position to the end position;

[0014] The first processing operation includes pruning processing and interpolation processing.

[0015] In a possible implementation manner, the selecting a first trajectory point from a plurality of first trajectory points includes:

[0016] Determine an obstacle avoidance cost corresponding to each of the first trajectory points based on the distance between each of the first trajectory points and the nearest obstacle;

[0017] For each of the first trajectory points, the following operations are performed: a total cost is determined based on a first position cost, a second position cost, and the obstacle avoidance cost; wherein the first position cost is determined by a trajectory point of a grid where the target position is located and the first trajectory point, and the second position cost is determined by a trajectory point of a grid where the end position is located and the first trajectory point;

[0018] A first trajectory point whose total cost satisfies a cost setting condition is selected from the plurality of first trajectory points.

[0019] In a possible implementation, the performing of secondary programming based on the initial trajectory, the current speed of the differential robot, the current angular velocity of the orientation, and the preset trajectory endpoint velocity to determine a closed-form solution of the objective function includes:

[0020] Determine a path fitting error function for characterizing a trajectory error between an initial trajectory and a smooth trajectory based on the initial trajectory, determine an initial velocity error function for characterizing a velocity error between the current velocity and an initial velocity of a smooth trajectory based on the current velocity, determine an acceleration error function for characterizing an angular velocity error between the current angular velocity and an angular velocity corresponding to a smooth trajectory based on a correspondence between angular velocity and acceleration, and determine a terminal velocity error function for characterizing a velocity error between the terminal velocity of the trajectory and a terminal velocity corresponding to a smooth trajectory based on the terminal velocity of the trajectory;

[0021] Determine a Hessian matrix and linear terms in an objective function based on the path fitting error function, the initial speed error function, the acceleration error function, and the terminal speed error function;

[0022] A closed-form solution to the objective function is determined based on the Hessian matrix and the linear terms.

[0023] In a possible implementation, the performing trajectory optimization on the smooth trajectory based on the distances between the plurality of second trajectory points and obstacles to obtain a planned trajectory from the current position to the end position includes:

[0024] For each second trajectory point, determining an obstacle avoidance error based on a distance between the second trajectory point and a nearest obstacle and an obstacle avoidance safety threshold;

[0025] Determine a trajectory loss value corresponding to the smooth trajectory based on the obstacle avoidance error corresponding to each second trajectory point;

[0026] Performing N trajectory optimizations on the smooth trajectory based on the trajectory loss value, and determining trajectory loss values ​​corresponding to the N optimized smooth trajectories respectively;

[0027] The trajectory loss value corresponding to the smooth trajectory and the trajectory loss values ​​corresponding to the N optimized smooth trajectories respectively, the smooth trajectory corresponding to the trajectory loss value that meets the trajectory setting condition is used as the planned trajectory from the current position to the end position.

[0028] In a possible implementation, determining the trajectory loss value corresponding to the smooth trajectory based on the obstacle avoidance error corresponding to each predicted trajectory point includes:

[0029] determining a path fitting error based on the smooth trajectory and the initial trajectory, determining an initial speed error based on the smooth trajectory and the current speed, determining a terminal speed error based on the smooth trajectory and the terminal speed, and determining an acceleration error for characterizing a heading angular speed error based on the smooth trajectory, the heading angle, and the current speed;

[0030] The obstacle avoidance error, the path fitting error, the initial velocity error, the terminal velocity error, and the acceleration error are weighted to obtain a trajectory loss value of the smooth trajectory.

[0031] In a possible implementation, the smooth trajectory satisfies the condition shown in the following formula:

[0032] C=-H -1 f;

[0033] Wherein, C is used to represent the control point corresponding to the second trajectory point in the smooth trajectory, H is used to represent the Hessian matrix, and f is used to represent the linear term.

[0034] In a possible implementation, the performing a dynamic check on the planned trajectory includes: determining a speed range based on a preset time from the target position to the terminal position, the initial speed, and the terminal speed;

[0035] Determining whether each second trajectory point in the planned trajectory satisfies a restriction condition;

[0036] The restrictions include:

[0037] The speed of the second trajectory point is within the speed range; the acceleration of the second trajectory point is within the acceleration range; and the orientation angular velocity of the second trajectory point is within the orientation angular velocity range.

[0038] In a possible implementation, the method further includes: when there is at least one second trajectory point in the planned trajectory that does not satisfy at least one of the constraints, adjusting the preset duration, and re-planning the trajectory from the current position to the target position based on the adjusted preset duration.

[0039] In a second aspect, an embodiment of the present application provides a trajectory planning device, the device comprising:

[0040] a first planning unit, configured to perform trajectory planning based on the distance between the first trajectory point corresponding to each first target grid in the grid map and the obstacle, to obtain an initial trajectory from the target position to the terminal position, wherein the target position is the current position of the differential robot, the first target grid is the grid corresponding to the target position, and the initial trajectory includes a plurality of first trajectory points;

[0041] A second planning unit is used to perform secondary planning based on the initial trajectory, the current speed of the differential robot, the current angular velocity of the orientation and the preset trajectory endpoint velocity, determine the closed-form solution of the objective function, and obtain a smooth trajectory; wherein the smooth trajectory includes a plurality of second trajectory points, and the objective function is used to determine the corresponding trajectory when the set conditions are met;

[0042] Optimizing the smooth trajectory based on the distances between the plurality of second trajectory points and the obstacles, to obtain a planned trajectory from the current position to the end position;

[0043] A dynamic check is performed on the planned trajectory, and when the planned trajectory meets the restriction condition, the planned trajectory is used as the target planned trajectory.

[0044] In a possible implementation, the first planning unit, when performing trajectory planning based on the distance between the first trajectory point corresponding to the first target grid in the grid map and the obstacle to obtain an initial trajectory from the target position to the end position, is specifically used to:

[0045] Selecting a first trajectory point from a plurality of first trajectory points based on the distance between the first trajectory point corresponding to each of the first target grids and the obstacle;

[0046] Determine a second target grid corresponding to the selected first trajectory point;

[0047] Selecting a first track point from the plurality of first track points based on the distance between the first track points corresponding to each of the second target grids and the obstacle, and continuing to determine the second target grid corresponding to the selected first track point until the selected first track point is the end point position;

[0048] Based on the selected first trajectory points, an initial path is obtained;

[0049] According to the trajectory direction represented by the line connecting any two adjacent first trajectory points in the initial path and the preset time from the target position to the end position, a first processing operation is performed on the plurality of first trajectory points in the initial path to obtain an initial trajectory from the target position to the end position;

[0050] The first processing operation includes pruning processing and interpolation processing.

[0051] In a possible implementation manner, when the first planning unit selects a first trajectory point from the plurality of first trajectory points, it is specifically configured to:

[0052] Determine an obstacle avoidance cost corresponding to each of the first trajectory points based on the distance between each of the first trajectory points and the nearest obstacle;

[0053] For each of the first trajectory points, the following operations are performed: a total cost is determined based on a first position cost, a second position cost, and the obstacle avoidance cost; wherein the first position cost is determined by a trajectory point of a grid where the target position is located and the first trajectory point, and the second position cost is determined by a trajectory point of a grid where the end position is located and the first trajectory point;

[0054] A first trajectory point whose total cost satisfies a cost setting condition is selected from the plurality of first trajectory points.

[0055] In a possible implementation, the second planning unit performs secondary planning based on the initial trajectory, the current speed of the differential robot, the current angular velocity of the orientation, and the preset trajectory endpoint velocity to determine a closed-form solution of the objective function, specifically for:

[0056] Determine a path fitting error function for characterizing a trajectory error between an initial trajectory and a smooth trajectory based on the initial trajectory, determine an initial velocity error function for characterizing a velocity error between the current velocity and an initial velocity of a smooth trajectory based on the current velocity, determine an acceleration error function for characterizing an angular velocity error between the current angular velocity and an angular velocity corresponding to a smooth trajectory based on a correspondence between angular velocity and acceleration, and determine a terminal velocity error function for characterizing a velocity error between the terminal velocity of the trajectory and a terminal velocity corresponding to a smooth trajectory based on the terminal velocity of the trajectory;

[0057] Determine a Hessian matrix and linear terms in an objective function based on the path fitting error function, the initial speed error function, the acceleration error function, and the terminal speed error function;

[0058] A closed-form solution to the objective function is determined based on the Hessian matrix and the linear terms.

[0059] In a possible implementation, the second planning unit, when performing trajectory optimization on the smooth trajectory based on the distances between the plurality of second trajectory points and obstacles to obtain a planned trajectory from the current position to the end position, is specifically configured to:

[0060] For each second trajectory point, determining an obstacle avoidance error based on a distance between the second trajectory point and a nearest obstacle and an obstacle avoidance safety threshold;

[0061] Determine a trajectory loss value corresponding to the smooth trajectory based on the obstacle avoidance error corresponding to each second trajectory point;

[0062] Performing N trajectory optimizations on the smooth trajectory based on the trajectory loss value, and determining trajectory loss values ​​corresponding to the N optimized smooth trajectories respectively;

[0063] The trajectory loss value corresponding to the smooth trajectory and the trajectory loss values ​​corresponding to the N optimized smooth trajectories respectively, the smooth trajectory corresponding to the trajectory loss value that meets the trajectory setting condition is used as the planned trajectory from the current position to the end position.

[0064] In a possible implementation, the second planning unit, when determining the trajectory loss value corresponding to the smooth trajectory based on the obstacle avoidance error corresponding to each predicted trajectory point, is specifically configured to:

[0065] determining a path fitting error based on the smooth trajectory and the initial trajectory, determining an initial speed error based on the smooth trajectory and the current speed, determining a terminal speed error based on the smooth trajectory and the terminal speed, and determining an acceleration error for characterizing a heading angular speed error based on the smooth trajectory, the heading angle, and the current speed;

[0066] The obstacle avoidance error, the path fitting error, the initial velocity error, the terminal velocity error, and the acceleration error are weighted to obtain a trajectory loss value of the smooth trajectory.

[0067] In a possible implementation, the smooth trajectory satisfies the condition shown in the following formula:

[0068] C=-H -1 f;

[0069] Among them, the C is used to represent the control point corresponding to the second trajectory point in the smooth trajectory, the H is used to represent the Hessian matrix, and the f is used to represent the linear term.

[0070] In a possible implementation, the second planning unit is specifically configured to: determine a speed range based on a preset time duration from the target position to the terminal position, the initial speed, and the terminal speed;

[0071] Determining whether each second trajectory point in the planned trajectory satisfies a restriction condition;

[0072] The restrictions include:

[0073] The speed of the second trajectory point is within the speed range; the acceleration of the second trajectory point is within the acceleration range; and the orientation angular velocity of the second trajectory point is within the orientation angular velocity range.

[0074] In a possible implementation, the second planning unit is further used to: when there is at least one second trajectory point in the planned trajectory that does not satisfy at least one of the constraints, adjust the preset duration, and re-plan the trajectory from the current position to the target position based on the adjusted preset duration.

[0075] In a third aspect, an embodiment of the present application provides an execution device, including:

[0076] A memory for storing program instructions;

[0077] The processor is used to obtain the program instructions stored in the memory, and implement the method described in the first aspect and different implementation methods of the first aspect according to the obtained program instructions.

[0078] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium includes computer instructions. When a computer executes the computer instructions, the method described in the first aspect and different implementation methods of the first aspect are implemented.

[0079] The beneficial effects of this application are as follows:

[0080] When performing path planning, the present application first performs initial path planning based on the grid map to obtain the initial trajectory, and determines the Hessian matrix and linear terms of the objective function based on the initial trajectory, the current speed of the differential robot, the current heading angle and the preset trajectory terminal speed, so as to quickly solve the smooth trajectory. The present application greatly reduces the occupation of computing resources and improves the efficiency of path calculation by quickly solving the smooth trajectory, and can achieve efficient implementation of response in a complex dynamic environment. Further, the smooth trajectory is used as the initial value, and the trajectory is optimized by introducing the obstacle avoidance error of the obstacle distribution, which can reliably avoid randomly appearing or moving obstacles and ensure the safety of trajectory planning. In addition, when performing smooth trajectory planning, the present application introduces errors in speed, acceleration, heading angle and heading angular velocity to ensure the continuity of the smooth trajectory in speed, acceleration, heading angle and heading angular velocity, ensure the stability and safety of the movement, and improve the robot's motion stability and mission success rate in an unstructured environment.

[0081] Other features and advantages of the present application will be described in the following description, and partly become apparent from the description, or be understood by practicing the present application. The purpose and other advantages of the present application can be realized and obtained by the structures specifically pointed out in the written description, claims, and drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0082] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0083] Figure 1 A flow chart of a trajectory planning method provided in an embodiment of the present application;

[0084] Figure 2 A schematic diagram of a first target grid provided in an embodiment of the present application;

[0085] Figure 3 A schematic diagram of determining an initial trajectory provided in an embodiment of the present application;

[0086] Figure 4 A flowchart of the steps of trajectory planning provided in an embodiment of the present application;

[0087] Figure 5 A structural diagram of a trajectory planning device provided in an embodiment of the present application;

[0088] Figure 6 A structural diagram of an execution device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0089] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. The components of the embodiments of the present application described and shown in the drawings here can be arranged and designed in various different configurations.

[0090] Therefore, the following detailed description of the embodiments of the present application provided in the accompanying drawings is not intended to limit the scope of the present application for which protection is sought, but merely represents selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present application.

[0091] It should be noted that relational terms such as "first" and "second" are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0092] Differential Drive Robots are widely used in logistics, security inspection, home services and agricultural automation due to their simple mechanical structure and flexible motion control. These robots achieve flexible steering and precise positioning by independently controlling the speed difference between the left and right wheels, and adapt to a variety of application scenarios. However, in unstructured environments such as homes and offices, robots need to face highly dynamic and complex environmental changes. Compared with structured environments (such as industrial plants and warehouses), the obstacle distribution and environmental dynamics of unstructured environments are more unpredictable. The characteristics of these environments include: Dynamic changes in the environment: The layout and position of obstacles may change at any time, such as moving furniture in the home, pets or people walking around in the office. High trajectory planning requirements: When robots perform tasks, such as delivering food or carrying items, they need not only a smooth path, but also a smooth transition of speed and acceleration during movement to avoid damage to items.

[0093] However, the existing trajectory planning methods mainly include: sampling-based methods, search-based methods and optimization-based methods. However, the traditional trajectory planning algorithm cannot meet the continuity requirements of the trajectory, resulting in unstable robot movement and increasing the risk of mission failure. In addition, the traditional method is complex in calculation and cannot complete the path update in a short time, which affects the real-time navigation capability.

[0094] In view of the above problems, the embodiments of the present application provide a method, device and medium for trajectory planning, firstly perform initial path planning based on a grid map to obtain an initial trajectory, and then perform secondary planning based on the initial trajectory, current speed, current angular velocity and preset trajectory terminal velocity, determine the closed-form solution of the objective function and obtain a smooth trajectory. The method can quickly solve the smooth trajectory, greatly reduce the occupation of computing resources, improve the efficiency of path calculation, and achieve efficient implementation of response in complex dynamic environments. Furthermore, the smooth trajectory is used as the initial value, and the trajectory is optimized by introducing the obstacle avoidance error of the obstacle distribution, which can reliably avoid randomly appearing or moving obstacles and ensure the safety of trajectory planning. In addition, errors in speed, acceleration, and angular velocity are introduced to ensure the continuity of the trajectory in speed, acceleration, and angular velocity, ensure the smoothness and safety of the movement, and improve the robot's motion stability and task success rate in unstructured environments.

[0095] like Figure 1 As shown, Figure 1 A flow chart of a trajectory planning method provided in an embodiment of the present application is exemplarily provided. The method can be applied to a differential robot. The method comprises the following steps:

[0096] S101, performing trajectory planning based on the distance between the first trajectory point corresponding to each first target grid in the grid map and the obstacle, to obtain an initial trajectory from the target position to the end position.

[0097] In some embodiments, the grid map may be a distance field grid map, and each grid implements updating of the distance to the nearest obstacle. In some scenarios, each grid may represent its position information through a track point. The target position is the current position of the differential robot, and the initial track includes a plurality of first track points. The first target grid is the grid corresponding to the target position. Specifically, the first target grid may be a grid within a set range corresponding to the target position, or a grid adjacent to the target position, such as Figure 2 As shown. The dotted line represents the set range, and the first target grid is the grid within the dotted line range. In some scenarios, when the first target grid is the grid within the set range, all grids within the set range are the first target grids, that is, if part of a grid is within the set range and the rest is outside the set range, then the grid is the first target grid. In other scenarios, the first target grid is the grid where all grids fall within the set range, that is, when all grids are within the set range, then the grid is the first target grid. If part of a grid is within the set range and the rest is outside the set range, then the grid cannot be used as the first target grid.

[0098] In some scenarios, the set range may be a circle, rectangle or other shape centered on the target position, or the set range may be a designated area including the target position, which is not specifically limited in this application.

[0099] In a possible implementation, the initial path from the target position to the terminal position can be determined in the following manner: based on the distance between the first trajectory point corresponding to each first target grid and the obstacle, a first trajectory point is selected from a plurality of first trajectory points, and then the second target grid corresponding to the selected first trajectory point is determined. Further, based on the distance between the first trajectory point corresponding to each second target grid and the obstacle, a first trajectory point can be selected from a plurality of first trajectory points, and the second target grid corresponding to the selected first trajectory point can be continuously determined until the selected first trajectory point is the terminal position. Based on the selected first trajectory points, the initial path is obtained.

[0100] In some embodiments, when selecting a first trajectory point from multiple first trajectory points, it can be achieved in the following manner: based on the distance between each first trajectory point and the nearest obstacle, determine the obstacle avoidance cost corresponding to each first trajectory point. For each first trajectory point, perform the following operations: determine the total cost based on the first position cost, the second position cost, and the obstacle avoidance cost. Select a first trajectory point from multiple first trajectory points whose total cost meets the cost setting condition. Among them, the first position cost is determined by the trajectory point of the grid where the target position is located and the first trajectory point, and the second position cost is determined by the trajectory point of the grid where the end position is located and the first trajectory point, and the cost setting condition is that the cost value is the minimum.

[0101] Specifically, the improved Astar algorithm can be used to generate the optimal path from the target position to the end position in the grid map. The improved Astar algorithm not only takes into account the first position cost (i.e., path cost) and the second position cost (i.e., heuristic cost) in the traditional algorithm, but also takes into account the obstacle avoidance cost. As an example, the total cost satisfies the conditions shown in the following formula:

[0102] f′(n)=g(n)+h(n)+o(n);

[0103] Among them, g(n) represents the first position cost from the trajectory point of the grid where the target position is located to the first trajectory point corresponding to grid n, h(n) represents the second position cost from the first trajectory point corresponding to grid n to the trajectory point of the grid where the end position is located, and o(n) represents the distance-related penalty between the first trajectory point corresponding to grid n and the obstacle, that is, the obstacle avoidance cost.

[0104] As an example, Figure 3As shown, taking the grid where the current position is located as grid 5, and the first target grids as grids 1-4, 6-9 adjacent to the current position as an example, for the first trajectory point corresponding to each first target grid, the total cost corresponding to each first trajectory point is calculated. Further, from the total costs corresponding to each first trajectory point, the first trajectory point with the smallest total cost is selected, and then the second target grid corresponding to the first trajectory point is determined. Assuming that the total cost of the first trajectory point 9 corresponding to the first target grid 9 is the smallest, the second target grid corresponding to the first trajectory point 9 is determined. Among them, the grids adjacent to grid 9 are grids 5-6, 8, 10-14, respectively, and grids 5-6, 8, 10-14 are used as the second target grids. Further, the total cost of the first trajectory point corresponding to each grid in grids 5-6, 8, 10-14 is calculated, and then a trajectory point is selected from the first trajectory points corresponding to grids 5-6, 8, 10-14. For example, when the total cost of the first trajectory point corresponding to grid 13 is the smallest, the first trajectory point 13 corresponding to grid 13 is selected, and then the second target grid corresponding to the first trajectory point 13 is determined, until the selected first trajectory point is the end point position, and the initial path is obtained.

[0105] Further, after the initial path is obtained, a first processing operation can be performed on multiple first trajectory points in the initial path according to the trajectory direction represented by the line connecting any two adjacent first trajectory points in the initial path and the preset time from the target position to the end position to obtain the initial trajectory from the target position to the end position. The first processing operation is pruning and interpolation.

[0106] As an example, this application plans and solves the trajectory based on the B-spline curve. The pruning algorithm can be used to remove redundant intermediate points in the initial path and retain the key turning points and direction change points of the initial path. When the number of first trajectory points is insufficient, the interpolation method is used to supplement additional first trajectory points so that the number and distribution of the first trajectory points meet the requirements of subsequent B-spline curve fitting.

[0107] S102, performing secondary programming based on the initial trajectory, the current speed of the differential robot, the current angular velocity of the orientation and the preset trajectory endpoint velocity, determining a closed-form solution of the objective function, and obtaining a smooth trajectory.

[0108] The smooth trajectory includes a plurality of second trajectory points, and the objective function is used to determine the corresponding trajectory when a set condition is met, and the set condition is that the value of the objective function is minimized.

[0109] In some embodiments, a path fitting error function for characterizing the trajectory error between the initial trajectory and the smooth trajectory can be determined based on the initial trajectory, an initial velocity error function for characterizing the velocity error between the current velocity and the initial velocity of the smooth trajectory can be determined based on the current velocity, an acceleration error function for characterizing the angular velocity error between the current angular velocity and the initial angular velocity corresponding to the smooth trajectory can be determined based on the correspondence between the angular velocity and the acceleration, and a terminal velocity error function for characterizing the velocity error between the terminal velocity of the trajectory and the terminal velocity corresponding to the smooth trajectory can be determined based on the terminal velocity of the trajectory; further, the Hessian matrix and linear terms in the objective function can be determined based on the path fitting error function, the initial velocity error function, the acceleration error function and the terminal velocity error function. Specifically, the Hessian matrix and linear terms in the objective function can be determined based on the path fitting error function, the initial velocity error function, the acceleration error function and the terminal velocity error function. Further, a closed-form solution to the objective function is determined based on the Hessian matrix and the linear terms.

[0110] In some embodiments, a smooth trajectory can be represented by a B-spline curve. A B-spline curve is a piecewise polynomial curve consisting of control points and basis functions. Assume that there are N control points, and the set of control points is C = {C0, C1, ..., C N-1}. The B-spline curve can be expressed as:

[0111]

[0112] in, It is used to represent the B-spline basis function of the ith control point, k represents the order of the basis function, and t is a parameter related to the preset duration.

[0113] The elements of the basis function matrix B are expressed as Assume that there are M discrete time points {t1, t2, ..., t M}, then the basis function matrix B is expressed as:

[0114]

[0115] In some embodiments, in order to ensure the safety of the trajectory, the initial trajectory constraints planned by the fitting search algorithm can be used to ensure preliminary safety, and then a smooth trajectory can be quickly solved.

[0116] As an example, the first set of trajectory points in the initial trajectory can be expressed as P target = {P1, P2, ..., P M}, then the sum of squares of the path fitting error function is expressed as: ‖E‖ 2 =(P target -BC) T (Ptarget -BC).

[0117] In some embodiments, in order to ensure the continuity, smoothness and stability of the smooth trajectory, the nonlinear angular velocity constraint may be converted into a linear lateral acceleration constraint to solve a closed-form solution.

[0118] Specifically, the constraint of the initial velocity v0 can be expressed by the basis function derivative matrix. Assuming that the basis function derivative matrix is ​​B′, the initial velocity constraint can be expressed as: v0 = B′(t0)C. In order to make the initial velocity close to the ideal velocity v0 * , then the sum of squares of the initial velocity error function is expressed as ‖v0-v0 * ‖ 2 = ‖B′(t0)C-v0 * ‖ 2 .

[0119] Terminal velocity v f It can be expressed by the derivative of the trajectory at the end point: f =B′(t f )C, in order to make the terminal speed close to the ideal speed v f * , then the sum of squares of the terminal velocity error function is expressed as ‖v f -v f * ‖ 2 =‖B′(t f )Cv f * ‖ 2 .

[0120] The following is a detailed description of the derivation process for converting the angular velocity into the linear lateral acceleration:

[0121] The curvature k indicates the curvature of the trajectory and is expressed as Among them, θ represents the tangent direction angle of the trajectory, and s represents the arc length along the trajectory. The velocity v is the moving speed along the trajectory, and its relationship with the arc length is expressed as ds=v dt. Substituting it into the curvature formula, we get in, is the robot's heading angular velocity in the local coordinate system.

[0122] Lateral acceleration a y is the acceleration perpendicular to the direction of travel. According to the definition of curvature and the relationship between velocity, the lateral acceleration can be expressed as: y =v 2 k, substituting the curvature formula into the equation, we get

[0123] In trajectory planning, the constraints of initial velocity and initial acceleration exist at the same time. When converting the initial angular velocity constraint to a linearized initial lateral acceleration constraint, this can be achieved by increasing the weight value of the initial velocity constraint. Assume that at the initial moment, the linear velocity of the robot along the trajectory direction is v0, that is, the initial velocity of the robot along the trajectory direction is close to the ideal value v0 * , the initial angular velocity can be expressed as a y0 ≈v0 * ·ω0. Furthermore, the initial acceleration constraint is expressed as a y0 =B″(t0)C. Wherein, B″ is the second-order derivative matrix of the basis function. In order to make the initial lateral acceleration consistent with the approximate initial lateral acceleration represented by the initial velocity and the heading angle, a heading angular velocity error function is constructed. The square sum of the acceleration error function used to represent the heading angular velocity error is expressed as ‖a y0 -v0 * ·ω0‖ 2 =‖B″(t0)CB′(t0)C·ω0‖ 2 .

[0124] Furthermore, the path fitting error function, initial speed error function, terminal speed error function and acceleration error function are integrated into one optimization problem, and the final objective function can be expressed as:

[0125]

[0126] Among them, λ v1 represents the weight value corresponding to the initial velocity error function, λ a1 Represents the weight value corresponding to the acceleration error function, λ f1 Indicates the weight value corresponding to the terminal speed error function.

[0127] Furthermore, the objective function can be converted into a standard quadratic optimization problem, and the objective function can be expressed as: H represents the Hessian matrix, including the quadratic coefficients, and f represents the linear terms.

[0128] Among them, H is composed of the quadratic terms of the path fitting error function, the initial velocity error function, the terminal velocity error function and the initial acceleration error function. H is expressed as:

[0129] H=B T B+λ v (B′(t0)) T B′(t0)+λ a (B″(t0)) T B″(t0)+λ f (B′(t f )) TB′(t f ).

[0130] The linear term f is a linear term consisting of the initial trajectory, the ideal initial velocity, the ideal initial angular velocity and the ideal terminal velocity. The linear term is expressed as:

[0131]

[0132] By solving the above quadratic optimization problem, the smooth trajectory satisfies the conditions shown in the following formula:

[0133] C=-H -1 f;

[0134] Wherein, C is used to represent the control point corresponding to the second trajectory point in the smooth trajectory, H is used to represent the Hessian matrix, and f is used to represent the linear term.

[0135] S103, optimizing the smooth trajectory based on the distances between the plurality of second trajectory points and the obstacles, to obtain a planned trajectory from the current position to the end position.

[0136] In some embodiments, for each second trajectory point, the obstacle avoidance error can be determined based on the distance between the second trajectory point and the nearest obstacle, and the obstacle avoidance safety threshold, and the trajectory loss value corresponding to the smooth trajectory can be determined based on the obstacle avoidance error corresponding to each second trajectory point. Further, the smooth trajectory can be optimized N times based on the trajectory loss value, and the trajectory loss values ​​corresponding to the N optimized smooth trajectories can be determined. Then, among the trajectory loss value corresponding to the smooth trajectory and the trajectory loss values ​​corresponding to the N optimized smooth trajectories, the smooth trajectory corresponding to the trajectory loss value that meets the trajectory setting condition is used as the planned trajectory from the current position to the end position. Among them, the trajectory setting condition is that the trajectory loss value is minimized.

[0137] In some embodiments, determining the trajectory loss value corresponding to the smooth trajectory based on the obstacle avoidance error corresponding to each predicted trajectory point can be achieved in the following manner: determining the path fitting error based on the smooth trajectory and the initial trajectory, determining the initial speed error based on the smooth trajectory and the current speed, determining the terminal speed error based on the smooth trajectory and the terminal speed, and determining the acceleration error used to characterize the angular velocity error based on the smooth trajectory, the heading angle and the current speed; weighting the obstacle avoidance error, path fitting error, initial speed error, terminal speed error and acceleration error to obtain the trajectory loss value of the smooth trajectory.

[0138] In some embodiments, when adjusting the smooth trajectory, a nonlinear programming problem can be constructed. Considering the path fitting error, the initial speed error, the terminal speed error, the acceleration error, and the obstacle avoidance error, the constructed nonlinear programming problem can be expressed as: Then, the sequential quadratic programming algorithm (SQP) can be used to determine the planning trajectory. Where X represents the set of optimization variables, represents the control points of the B-spline, and is the final planning trajectory.

[0139] The objective function J(X) consists of the following parts:

[0140]

[0141] Among them, J track represents the path fitting error, represents the initial velocity error, represents the acceleration error, represents the terminal velocity error, J obs Represents the obstacle avoidance error.

[0142] In some embodiments, the construction of the path fitting error, the initial velocity error, the acceleration error, and the terminal velocity error is the same as the quadratic programming when determining the smooth trajectory, which can be expressed as:

[0143] J track =‖P target -BC‖ 2 ;

[0144]

[0145] Among them, λ v2 Represents the weight value of the initial velocity error, λ a2 represents the weight value of the acceleration error used to characterize the angular velocity error, λ f2 Represents the weight value of the terminal velocity error, λ obs Represents the weight value of obstacle avoidance error. dist(x i ,y i ) represents the distance between the trajectory point and the obstacle, d safe Indicates the safety distance threshold.

[0146] In some embodiments, the trajectory planning method further includes step S104:

[0147] S104, performing a dynamic check on the planned trajectory, and when the planned trajectory meets the restriction conditions, taking the planned trajectory as the target planned trajectory.

[0148] In some embodiments, the planned trajectory is dynamically checked, including: determining a speed range based on a preset time from the target position to the terminal position, an initial speed, and a terminal speed. After determining the planned trajectory, determining whether each second trajectory point in the planned trajectory satisfies a restriction condition. The restriction condition includes: the speed of the second trajectory point is within the speed range; the acceleration of the second trajectory point is within the acceleration range; and the heading angular velocity of the second trajectory point is within the heading angular velocity range.

[0149] In some embodiments, when there is at least one second trajectory point in the planned trajectory that does not satisfy at least one of the constraints, the preset duration is adjusted, and the trajectory from the current position to the target position is replanned based on the adjusted preset duration.

[0150] For example, when the speed of a second trajectory point exceeds the speed range, or the acceleration exceeds the acceleration range, or the angular velocity exceeds the angular velocity range, it is necessary to adjust the preset duration and re-solve the trajectory.

[0151] In this application, the velocity, angular velocity, and acceleration (toward angular velocity) of the trajectory are checked point by point to see if they meet the preset constraints, ensuring that the generated trajectory is feasible and safe in actual execution. Based on the inspection results, if the trajectory does not meet the constraints, the adjustment ratio will be calculated to optimize the trajectory to ensure that the final trajectory can effectively avoid obstacles and meet dynamic constraints in actual applications. In some scenarios, the motion information of the trajectory generated by local trajectory planning can be provided to the trajectory tracking module for tracking.

[0152] In the trajectory planning method provided by the present application, the traditional Astar algorithm is improved based on the distance field grid map, and the obstacle avoidance cost term is added. The initial trajectory from the current position to the target position is obtained by pruning and interpolation; the trajectory duration is estimated based on the current initial velocity, the given average velocity and the final velocity, and a quadratic programming optimization problem without considering obstacle constraints is constructed to obtain the closed-form solution of the control point C of the B-spline curve to obtain a smooth trajectory. Furthermore, obstacle constraints are constructed based on the distance field grid information, and a nonlinear programming optimization problem is constructed in combination with the constraints of the quadratic programming optimization problem. A sequential quadratic programming algorithm is used to solve the problem and obtain the planned trajectory. Finally, a dynamic check is performed on the planned trajectory, and the trajectory velocity, angular velocity and acceleration are checked and adjusted point by point to ensure that the trajectory is feasible and safe.

[0153] like Figure 4 As shown, the trajectory planning method provided in the embodiment of the present application is illustrated by way of example, and the specific process is as follows:

[0154] First, the target point is obtained and the distance field grid map is updated in real time. The target point includes the target position and the end point position. The local planning algorithm is planned based on the local map, and a distance field grid map is constructed and updated. Each grid updates the distance to the nearest obstacle in real time.

[0155] Then, the path is planned by the search algorithm, and the initial trajectory is obtained by pruning and interpolation. Specifically, based on the improved Astar algorithm, the optimal path from the target position to the end position can be generated in the local map by considering the path cost, heuristic cost and obstacle avoidance cost. The number and distribution of the first trajectory points in the initial path obtained by pruning and interpolation meet the requirements of subsequent B-spline curve fitting.

[0156] Then, sensor data is obtained. The status data of the differential robot is obtained in real time through sensors (wheel encoders, IMU), including wheel speed and orientation angle information.

[0157] Next, the trajectory duration is estimated. The trajectory duration is determined based on the current wheel speed, the given average speed and the terminal speed.

[0158] Then, the first-stage quadratic programming optimization problem is constructed, and the first-stage closed-form solution is solved. In this stage, the path fitting error, velocity error (initial velocity and terminal velocity), and heading angular velocity constraint (initial heading angular velocity) are considered. Among them, the nonlinear term (heading angle) is processed, and the quadratic programming problem constructed in the first stage can quickly solve a continuous and smooth smooth trajectory. The quadratic programming problem ensures the safety of the smooth trajectory by fitting the path fitting error determined by the initial trajectory planned by the front-end search algorithm.

[0159] Furthermore, the second stage nonlinear optimization problem constructs and solves the approximation. This stage considers the path fitting error, velocity error (including initial velocity and terminal velocity), heading angular velocity error and obstacle avoidance error. Since the quadratic programming problem in the first stage gives a safe initial guess, the nonlinear programming problem in the second stage can adjust the trajectory away from obstacles in a short time, achieving efficient real-time response and safe and smooth navigation in complex dynamic environments.

[0160] Furthermore, the trajectory dynamics check and constraint adjustment are performed. Specifically, this step checks the velocity, angular velocity, and acceleration of the trajectory point by point to see if they meet the preset constraints, ensuring that the generated trajectory is feasible and safe in actual execution. Based on the check results, if the trajectory does not meet the constraints, the adjustment ratio will be calculated to optimize the trajectory, ensuring that the final trajectory can effectively avoid obstacles and meet dynamic constraints in actual applications.

[0161] Finally, the trajectory motion information is obtained: the local trajectory planning generates the trajectory motion information and gives it to the trajectory tracking module for tracking.

[0162] The trajectory planning method provided in this application has the following beneficial effects:

[0163] 1. Improve the smoothness and continuity of the path:

[0164] This application uses B-spline curves to represent trajectories and generates smooth continuous paths through a two-stage optimization algorithm. Unlike traditional sampling or search-based methods, B-spline curves can naturally generate smooth trajectories, avoiding sharp angles and mutations in the path. Through special processing of nonlinear terms, the quadratic programming problem can quickly solve the initial smooth trajectory, ensuring the continuity of the trajectory in terms of speed, acceleration, heading angle, and heading angular velocity, significantly improving the robot's motion stability and mission success rate in unstructured environments.

[0165] 2. Enhance real-time response capabilities:

[0166] This application uses a two-stage optimization algorithm. In the first stage, a smooth trajectory is quickly solved by constructing a quadratic programming problem, and in the second stage, a nonlinear programming problem is used to make detailed adjustments. Compared with the high computational complexity and slow response of traditional optimization methods, this application can quickly adjust the trajectory in complex and dynamic environments. The rapid solution in the first stage ensures the real-time nature of path planning, can quickly respond to environmental changes, and ensures the robot's efficient navigation and flexible obstacle avoidance in a dynamic environment.

[0167] 3. Improve the safety and reliability of the trajectory:

[0168] This application combines dynamic avoidance of obstacles and processing of complex nonlinear constraints in the trajectory planning process. By introducing dynamic constraints on obstacle distribution and position changes, this application can reliably avoid randomly appearing or moving obstacles and ensure the safety of trajectory planning. Compared with the problem of low computational efficiency of traditional methods when dealing with complex nonlinear constraints, this application optimizes the calculation process, can generate safe and reliable paths in real time, and reduces the risk of collision of robots in complex environments.

[0169] 4. Enhance the ability to adapt to unstructured environments:

[0170] This application takes into full account the dynamic characteristics of unstructured environments, ensuring that the trajectory planning algorithm can maintain stable performance in a variety of complex situations. This application can adapt to diverse environmental changes and random obstacle arrangements, and the flexibility and environmental adaptability of trajectory planning have been significantly improved. Whether it is dealing with moving furniture in a home environment or changes in the flow of people in an office space, the robot can maintain stable navigation performance, improving reliability and adaptability in practical applications.

[0171] 5. Optimize path calculation efficiency and reduce computing resource usage:

[0172] This application improves the efficiency of calculation by decomposing the optimization problem into two parts: quadratic programming and nonlinear programming. Compared with traditional methods that often consume a lot of computing resources in the process of high computational complexity path planning, this application greatly reduces the occupation of computing resources by decomposing the problem into two stages, preliminary solution and detailed adjustment respectively. In this way, not only the efficiency of path calculation is improved, but also the algorithm can run efficiently on resource-constrained embedded systems.

[0173] 6. Achieve precise control of dynamic constraints:

[0174] This application introduces constraints of velocity, acceleration, and angular velocity during the optimization process, and uses the properties of B-spline curves to ensure the continuity and smoothness of the path. Therefore, this application can accurately control the speed and acceleration changes of the robot on the path, ensure the smoothness and safety of the movement, especially avoid sudden changes and unnecessary acceleration in complex path tasks, and ensure the adaptability and operational stability of the robot in different task scenarios.

[0175] Based on the same technical concept, see Figure 5 As shown, Figure 5 The present invention provides a trajectory planning device 500 as an example. The device 500 can perform any step in the above trajectory planning method, which will not be described in detail here to avoid repetition. The device 500 includes a first planning unit 501 and a second planning unit 502.

[0176] A first planning unit 501 is used to perform trajectory planning based on the distance between the first trajectory point corresponding to each first target grid in the grid map and the obstacle, to obtain an initial trajectory from the target position to the end position, wherein the target position is the current position of the differential robot, the first target grid is the grid corresponding to the target position, and the initial trajectory includes a plurality of first trajectory points;

[0177] The second planning unit 502 is used to perform secondary planning based on the initial trajectory, the current speed of the differential robot, the current angular velocity of the orientation and the preset trajectory endpoint velocity, determine the closed-form solution of the objective function, and obtain a smooth trajectory; wherein the smooth trajectory includes a plurality of second trajectory points, and the objective function is used to determine the corresponding trajectory when the set conditions are met;

[0178] The second planning unit 502 is used to optimize the smooth trajectory based on the distances between the multiple second trajectory points and the obstacles, so as to obtain a planned trajectory from the current position to the end position.

[0179] In a possible implementation, the first planning unit 501, when performing trajectory planning based on the distance between the first trajectory point corresponding to the first target grid in the grid map and the obstacle to obtain an initial trajectory from the target position to the end position, is specifically configured to:

[0180] Selecting a first trajectory point from a plurality of first trajectory points based on the distance between the first trajectory point corresponding to each of the first target grids and the obstacle;

[0181] Determine a second target grid corresponding to the selected first trajectory point;

[0182] Selecting a first track point from the plurality of first track points based on the distance between the first track points corresponding to each of the second target grids and the obstacle, and continuing to determine the second target grid corresponding to the selected first track point until the selected first track point is the end point position;

[0183] Based on the selected first trajectory points, an initial path is obtained;

[0184] According to the trajectory direction represented by the line connecting any two adjacent first trajectory points in the initial path and the preset time from the target position to the end position, a first processing operation is performed on the plurality of first trajectory points in the initial path to obtain an initial trajectory from the target position to the end position;

[0185] The first processing operation includes pruning processing and interpolation processing.

[0186] In a possible implementation, when the first planning unit 501 selects a first trajectory point from the plurality of first trajectory points, it is specifically configured to:

[0187] Determine an obstacle avoidance cost corresponding to each of the first trajectory points based on the distance between each of the first trajectory points and the nearest obstacle;

[0188] For each of the first trajectory points, the following operations are performed: a total cost is determined based on a first position cost, a second position cost, and the obstacle avoidance cost; wherein the first position cost is determined by a trajectory point of a grid where the target position is located and the first trajectory point, and the second position cost is determined by a trajectory point of a grid where the end position is located and the first trajectory point;

[0189] A first trajectory point whose total cost satisfies a cost setting condition is selected from the plurality of first trajectory points.

[0190] In a possible implementation, the second planning unit 502, when performing secondary planning based on the initial trajectory, the current speed of the differential robot, the current angular velocity of the orientation, and the preset trajectory endpoint velocity to determine the closed-form solution of the objective function, is specifically used to:

[0191] Determine a path fitting error function for characterizing a trajectory error between an initial trajectory and a smooth trajectory based on the initial trajectory, determine an initial velocity error function for characterizing a velocity error between the current velocity and an initial velocity of a smooth trajectory based on the current velocity, determine an acceleration error function for characterizing an angular velocity error between the current angular velocity and the current angular velocity corresponding to the smooth trajectory based on a correspondence between angular velocity and acceleration, and determine a terminal velocity error function for characterizing a velocity error between the terminal velocity of the trajectory and the terminal velocity corresponding to the smooth trajectory based on the terminal velocity of the trajectory;

[0192] Determine a Hessian matrix and linear terms in an objective function based on the path fitting error function, the initial speed error function, the acceleration error function, and the terminal speed error function;

[0193] A closed-form solution to the objective function is determined based on the Hessian matrix and the linear terms.

[0194] In a possible implementation, the second planning unit 502, when performing trajectory optimization on the smooth trajectory based on the distances between the plurality of second trajectory points and obstacles to obtain a planned trajectory from the current position to the end position, is specifically configured to:

[0195] For each second trajectory point, determining an obstacle avoidance error based on a distance between the second trajectory point and a nearest obstacle and an obstacle avoidance safety threshold;

[0196] Determine a trajectory loss value corresponding to the smooth trajectory based on the obstacle avoidance error corresponding to each second trajectory point;

[0197] Performing N trajectory optimizations on the smooth trajectory based on the trajectory loss value, and determining trajectory loss values ​​corresponding to the N optimized smooth trajectories respectively;

[0198] The trajectory loss value corresponding to the smooth trajectory and the trajectory loss values ​​corresponding to the N optimized smooth trajectories respectively, the smooth trajectory corresponding to the trajectory loss value that meets the trajectory setting condition is used as the planned trajectory from the current position to the end position.

[0199] In a possible implementation, the second planning unit 502, when determining the trajectory loss value corresponding to the smooth trajectory based on the obstacle avoidance error corresponding to each predicted trajectory point, is specifically configured to:

[0200] determining a path fitting error based on the smooth trajectory and the initial trajectory, determining an initial speed error based on the smooth trajectory and the current speed, determining a terminal speed error based on the smooth trajectory and the terminal speed, and determining an acceleration error for characterizing a heading angular speed error based on the smooth trajectory, the heading angle, and the current speed;

[0201] The obstacle avoidance error, the path fitting error, the initial velocity error, the terminal velocity error, and the acceleration error are weighted to obtain a trajectory loss value of the smooth trajectory.

[0202] In a possible implementation, the smooth trajectory satisfies the condition shown in the following formula:

[0203] C=-H -1 f;

[0204] Wherein, C is used to represent the control point corresponding to the second trajectory point in the smooth trajectory, H is used to represent the Hessian matrix, and f is used to represent the linear term.

[0205] In a possible implementation, the second planning unit 502 is further configured to: determine a speed range based on a preset time duration from the target position to the terminal position, the initial speed, and the terminal speed;

[0206] Determine whether each second trajectory point in the planned trajectory satisfies a restriction condition; wherein the restriction condition includes: a speed of the second trajectory point is within the speed range; an acceleration of the second trajectory point is within the acceleration range; and a heading angular velocity of the second trajectory point is within the heading angular velocity range.

[0207] In a possible implementation, the second planning unit is further used to: when there is at least one second trajectory point in the planned trajectory that does not satisfy at least one of the constraints, adjust the preset duration, and re-plan the trajectory from the current position to the target position based on the adjusted preset duration.

[0208] Based on the same technical concept, see Figure 6 As shown, an execution device 600 provided in an embodiment of the present application is provided. The execution device 600 can execute any step of the above trajectory planning method, and in order to avoid repetition, it is not described here. The execution device 600 includes a memory 601 and a processor 602.

[0209] The memory 601 is used to store program instructions;

[0210] The processor 602 is used to obtain the program instructions in the memory and execute each step in the above trajectory planning method according to the obtained program instructions.

[0211] In the embodiment of the present application, the processor 602 may be a general-purpose processor, a digital signal processor, an application-specific integrated circuit, a field programmable gate array or other programmable logic device, a discrete gate or transistor logic device, or a discrete hardware component, and may implement or execute the methods, steps, and logic block diagrams disclosed in the embodiment of the present application. The general-purpose processor may be a microprocessor or any conventional processor, etc. The steps of the method disclosed in the embodiment of the present application may be directly embodied as being executed by a hardware processor, or may be executed by a combination of hardware and software modules in the processor.

[0212] Memory 601, as a non-volatile computer-readable storage medium, can be used to store non-volatile software programs, non-volatile computer executable programs and modules. Memory 601 may include at least one type of storage medium, such as flash memory, hard disk, multimedia card, card-type memory, random access memory (Random Access Memory, RAM), static random access memory (Static Random Access Memory, SRAM), programmable read-only memory (Programmable Read Only Memory, PROM), read-only memory (Read Only Memory, ROM), electrically erasable programmable read-only memory (Electrically Erasable Programmable Read-Only Memory, EEPROM), magnetic storage, disk, optical disk, etc. Memory 601 is any other medium that can be used to carry or store a desired program code in the form of an instruction or data structure and can be accessed by a computer, but is not limited thereto. The memory 601 in the embodiment of the present application can also be a circuit or any other device that can realize a storage function, for storing program instructions and / or data.

[0213] Based on the same technical concept, an embodiment of the present application provides a computer-readable storage medium, which stores computer instructions. When the computer instructions are executed on a computer, the computer executes each step of the above-mentioned trajectory planning method.

[0214] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0215] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0216] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0217] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0218] Obviously, those skilled in the art can make various changes and modifications to the present application without departing from the spirit and scope of the present application. Thus, if these modifications and variations of the present application fall within the scope of the claims of the present application and their equivalents, the present application is also intended to include these modifications and variations.

Claims

1. A trajectory planning method, It is characterized in that The method comprises: Performing trajectory planning based on the distance between the first trajectory point corresponding to each first target grid in the grid map and the obstacle to obtain an initial trajectory from the target position to the end position, wherein the target position is the current position of the differential robot, the first target grid is the grid corresponding to the target position, and the initial trajectory includes a plurality of first trajectory points; Based on the initial trajectory, the current speed of the differential robot, the current angular velocity of the orientation and the preset trajectory endpoint velocity, a secondary programming is performed to determine a closed-form solution of the objective function to obtain a smooth trajectory; wherein the smooth trajectory includes a plurality of second trajectory points, and the objective function is used to determine the corresponding trajectory when the set conditions are met; Optimizing the smooth trajectory based on the distances between the plurality of second trajectory points and the obstacles, to obtain a planned trajectory from the current position to the end position; A dynamic check is performed on the planned trajectory, and when the planned trajectory meets the restriction condition, the planned trajectory is used as the target planned trajectory.

2. The method according to claim 1, It is characterized in that The step of performing trajectory planning based on the distance between the first trajectory point corresponding to the first target grid in the grid map and the obstacle to obtain an initial trajectory from the target position to the end position includes: Selecting a first trajectory point from a plurality of first trajectory points based on the distance between the first trajectory point corresponding to each of the first target grids and the obstacle; Determine a second target grid corresponding to the selected first trajectory point; Selecting a first track point from the plurality of first track points based on the distance between the first track points corresponding to each of the second target grids and the obstacle, and continuing to determine the second target grid corresponding to the selected first track point until the selected first track point is the end point position; Based on the selected first trajectory points, an initial path is obtained; According to the trajectory direction represented by the line connecting any two adjacent first trajectory points in the initial path and the preset time from the target position to the end position, a first processing operation is performed on the plurality of first trajectory points in the initial path to obtain an initial trajectory from the target position to the end position; The first processing operation includes pruning processing and interpolation processing.

3. The method according to claim 2, It is characterized in that The selecting a first trajectory point from the plurality of first trajectory points comprises: Determine an obstacle avoidance cost corresponding to each of the first trajectory points based on the distance between each of the first trajectory points and the nearest obstacle; For each of the first trajectory points, the following operations are performed: a total cost is determined based on a first position cost, a second position cost, and the obstacle avoidance cost; wherein the first position cost is determined by a trajectory point of a grid where the target position is located and the first trajectory point, and the second position cost is determined by a trajectory point of a grid where the end position is located and the first trajectory point; A first trajectory point whose total cost satisfies a cost setting condition is selected from the plurality of first trajectory points.

4. The method according to claim 1, It is characterized in that The method of performing secondary programming based on the initial trajectory, the current speed of the differential robot, the current angular velocity and the preset trajectory endpoint velocity to determine a closed-form solution of the objective function includes: Determine a path fitting error function for characterizing a trajectory error between an initial trajectory and a smooth trajectory based on the initial trajectory, determine an initial velocity error function for characterizing a velocity error between the current velocity and an initial velocity of a smooth trajectory based on the current velocity, determine an acceleration error function for characterizing an angular velocity error between the current angular velocity and an initial angular velocity corresponding to a smooth trajectory based on a correspondence between angular velocity and acceleration, and determine a terminal velocity error function for characterizing a velocity error between a terminal velocity of the trajectory and a terminal velocity corresponding to a smooth trajectory based on the terminal velocity of the trajectory; Determine a Hessian matrix and linear terms in an objective function based on the path fitting error function, the initial speed error function, the acceleration error function, and the terminal speed error function; A closed-form solution to the objective function is determined based on the Hessian matrix and linear terms.

5. The method according to claim 1, It is characterized in that The step of optimizing the smooth trajectory based on the distances between the plurality of second trajectory points and the obstacles to obtain a planned trajectory from the current position to the end position includes: For each second trajectory point, determining an obstacle avoidance error based on a distance between the second trajectory point and a nearest obstacle and an obstacle avoidance safety threshold; Determine a trajectory loss value corresponding to the smooth trajectory based on the obstacle avoidance error corresponding to each second trajectory point; Based on the trajectory loss value, the smooth trajectory is optimized N times, and trajectory loss values ​​corresponding to the N optimized smooth trajectories are determined; The trajectory loss value corresponding to the smooth trajectory and the trajectory loss values ​​corresponding to the N optimized smooth trajectories respectively, the smooth trajectory corresponding to the trajectory loss value that meets the trajectory setting condition is used as the planned trajectory from the current position to the end position.

6. The method according to claim 5, It is characterized in that The determining the trajectory loss value corresponding to the smooth trajectory based on the obstacle avoidance error corresponding to each predicted trajectory point includes: determining a path fitting error based on the smooth trajectory and the initial trajectory, determining an initial speed error based on the smooth trajectory and the current speed, determining a terminal speed error based on the smooth trajectory and the terminal speed, and determining an acceleration error for characterizing a heading angular speed error based on the smooth trajectory, the heading angle, and the current speed; The obstacle avoidance error, the path fitting error, the initial velocity error, the terminal velocity error, and the acceleration error are weighted to obtain a trajectory loss value of the smooth trajectory.

7. The method according to claim 1, It is characterized in that The smooth trajectory satisfies the conditions shown in the following formula: C=-H -1 f; Wherein, C is used to represent the control point corresponding to the second trajectory point in the smooth trajectory, H is used to represent the Hessian matrix, and f is used to represent the linear term.

8. The method according to any one of claims 1 to 7, It is characterized in that The performing a dynamics check on the planned trajectory includes: determining a speed range based on a preset time duration from the target position to the end position, the initial speed, and the end speed; Determining whether each second trajectory point in the planned trajectory satisfies a restriction condition; The restrictions include: The speed of the second trajectory point is within the speed range; The acceleration of the second trajectory point is within the acceleration range; The orientation angular velocity of the second trajectory point is within the orientation angular velocity range.

9. A trajectory planning device, It is characterized in that The device comprises: a first planning unit, configured to perform trajectory planning based on the distance between the first trajectory point corresponding to each first target grid in the grid map and the obstacle, to obtain an initial trajectory from the target position to the terminal position, wherein the target position is the current position of the differential robot, the first target grid is the grid corresponding to the target position, and the initial trajectory includes a plurality of first trajectory points; A second planning unit is used to perform secondary planning based on the initial trajectory, the current speed of the differential robot, the current angular velocity of the orientation and the preset trajectory endpoint velocity, determine the closed-form solution of the objective function, and obtain a smooth trajectory; wherein the smooth trajectory includes a plurality of second trajectory points, and the objective function is used to determine the corresponding trajectory when the set conditions are met; Optimizing the smooth trajectory based on the distances between the plurality of second trajectory points and the obstacles, to obtain a planned trajectory from the current position to the end position; A dynamic check is performed on the planned trajectory, and when the planned trajectory meets the restriction condition, the planned trajectory is used as the target planned trajectory.

10. A computer-readable storage medium, It is characterized in that The computer-readable storage medium includes computer instructions. When a computer executes the computer instructions, the method according to any one of claims 1 to 8 is implemented.

Citation Information

Cited By

  • Rapid solution optimization method and system for planning occupied grid trajectory

    CN121502924A

  • A fast solution optimization method and system for occupying grid trajectory planning

    CN121502924B