A motion trajectory planning method, computer equipment and readable storage medium

Abstract

The application provides a motion trajectory planning method, computer equipment and a readable storage medium, and relates to the technical field of trajectory planning. The application constructs a spline curve for a given reference discrete path in any spatial dimension, re-parameterizes the constructed target continuous spline curve, obtains a continuous motion path with respect to the curve length, determines the sampling point positions of a plurality of time sampling points in a specific time period on the continuous motion path, which simultaneously satisfy a plurality of motion constraint conditions, and performs continuous trajectory fitting based on the sampling point positions of the plurality of time sampling points and the motion state information of the starting point and the terminal point of the reference discrete path, to obtain an optimal motion trajectory in the target motion time period, which is applicable to the corresponding spatial dimension and continuous, so that the application space dimension range of the trajectory planning scheme is effectively expanded, and unnecessary acceleration and / or deceleration is avoided.

Description

Technical Field

[0001] This application relates to the field of trajectory planning technology, and more specifically, to a motion trajectory planning method, a computer device, and a readable storage medium. Background Technology

[0002] With the continuous development of science and technology, trajectory planning algorithms are being used more and more widely in various fields (such as intelligent driving, industrial manufacturing, logistics and distribution, aerospace, etc.) because they can provide motion trajectories for moving targets (such as industrial robots, drones, autonomous vehicles, game characters, etc.) to complete predetermined tasks.

[0003] It is worth noting that the existing trajectory planning algorithms have relatively limited application scenarios. They are often only applicable to specific spatial dimensions and cannot be compatible with each other in different spatial dimensions. At the same time, the planned motion trajectories often exhibit unnecessary acceleration and / or deceleration during actual operation, which seriously affects the motion control effect and motion smoothness of the moving target and easily causes serious motion energy consumption problems. Summary of the Invention

[0004] In view of this, the purpose of this application is to provide a motion trajectory planning method, computer device and readable storage medium, which can plan the optimal motion trajectory that is suitable for the corresponding spatial dimension and continuous under the constraints of numerous motion constraints, based on a given discrete path of arbitrary spatial dimension, through the organic combination of path smoothing fitting and path sampling fitting. This improves the applicability of the corresponding trajectory planning scheme in different application scenarios, while avoiding unnecessary acceleration and / or deceleration, thereby improving the motion control effect and motion smoothness of the moving target, and effectively reducing the motion energy consumption of the corresponding moving target.

[0005] To achieve the above objectives, the technical solutions adopted in the embodiments of this application are as follows:

[0006] In a first aspect, this application provides a motion trajectory planning method, the method comprising:

[0007] Obtain the reference discrete path for trajectory planning, motion linear velocity constraints, motion acceleration constraints, motion angular velocity constraints, and desired trajectory runtime, as well as the motion state information of the starting point and ending point of the reference discrete path;

[0008] Based on the reference discrete path, a spline curve is constructed to obtain a target continuous spline curve that has the same path start and path end as the reference discrete path.

[0009] The target continuous spline curve is reparameterized to obtain a continuous motion path with respect to the curve length, wherein the curve length is based on the starting point of the reference discrete path;

[0010] Time sampling is performed within the target motion time period corresponding to the expected trajectory running time according to a preset time interval, to obtain multiple time sampling points including the start time point and the end time point of the target motion time period;

[0011] On the continuous motion path, determine the sampling point positions corresponding to each of the plurality of time sampling points that satisfy the motion linear velocity constraint, motion acceleration constraint, and motion angular velocity constraint, wherein the sampling point position corresponding to the start time point is the path start point of the reference discrete path, and the sampling point position corresponding to the end time point is the path end point of the reference discrete path.

[0012] Based on the motion state information of the starting point and the motion state information of the ending point of the path, continuous trajectory fitting is performed based on the sampling point positions of the multiple time sampling points to obtain the optimal motion trajectory within the target motion time period.

[0013] In an optional implementation, the step of constructing a spline curve based on the reference discrete path to obtain a target continuous spline curve with the same path start and path end points as the reference discrete path includes:

[0014] Obtain the desired interpolation degree for the target continuous spline curve, and construct target B-spline basis functions that satisfy the desired interpolation degree for each discrete point of the reference discrete path.

[0015] Based on the target B-spline basis functions of each of the discrete points along the path, B-spline curves are fitted to the spatial positions of each of the discrete points along the path to obtain the target continuous spline curve.

[0016] In an optional implementation, the target B-spline basis function of the path discrete point numbered i in the reference discrete path is calculated using the following equation:

[0017]

[0018] Where, N i,d (u) represents the objective B-spline basis function of the path discrete point numbered i, which is related to the curve independent variable parameter u and satisfies the expected interpolation degree d. n represents the path endpoint number of the reference discrete path, and the path start number of the reference discrete path is 0, where u∈[0,1].

[0019] In an optional implementation, the step of reparameterizing the target continuous spline curve to obtain a continuous motion path with respect to the curve length includes:

[0020] The target continuous spline curve is sampled at equal intervals to obtain multiple curve sampling points including the path start point and path end point of the target continuous spline curve. The curve length value from each curve sampling point to the path start point is calculated, wherein the curve length value from each curve sampling point to the path start point is obtained by accumulating the distance between all adjacent sampling points between the curve sampling point and the path start point.

[0021] A red-black tree query is performed based on the curve length values ​​of each of the curve sampling points, and a parameter association relationship between the curve length and the curve independent variable parameter of the target continuous spline curve is constructed between two adjacent curve sampling points according to the query results.

[0022] Based on the parameter correlation between two adjacent curve sampling points among all the curve sampling points, the target continuous spline curve is transformed into a curve equivalent to obtain the continuous motion path.

[0023] In an optional implementation, the step of determining the positions of the sampling points that satisfy the linear velocity constraint, acceleration constraint, and angular velocity constraint on each of the plurality of time sampling points along the continuous motion path includes:

[0024] All time sampling points other than the start time point and the end time point are sequentially used as target time sampling points. Based on the theoretical linear velocity of the previous time sampling point of the target time sampling point, the linear velocity distribution range of the target time sampling point that satisfies the motion acceleration constraint condition and the motion linear velocity constraint condition is determined.

[0025] Linear velocity sampling is performed on the linear velocity distribution interval according to a preset linear velocity resolution to obtain an initial linear velocity sampling set of the target time sampling point, wherein the linear velocity interval between two adjacent linear velocity sampling values ​​in the initial linear velocity sampling set is the preset linear velocity resolution.

[0026] Based on the actual curve length at the previous time sampling point, an initial curve length sampling set matching the initial linear velocity sampling set is constructed. Each curve length sampling value in the initial curve length sampling set corresponds to a linear velocity sampling value. Each curve length sampling value is obtained by adding the actual curve length at the previous time sampling point to the target length displacement that matches the corresponding linear velocity sampling value. The target length displacement is the uniformly accelerated linear motion displacement of the corresponding linear velocity sampling value and the theoretical linear velocity at the previous time sampling point within a unit time period. The length of the unit time period is consistent with the preset time interval.

[0027] Based on the initial linear velocity sampling set and the initial curve length sampling set, an initial angular velocity sampling set for the target time sampling point is constructed;

[0028] Identify the target angular velocity sample values ​​in the initial angular velocity sample set that do not meet the motion angular velocity constraint conditions, and remove the sample data corresponding to the target angular velocity sample values ​​from the initial linear velocity sample set and the initial curve length sample set;

[0029] The initial curve length sampling set after removing the sampled data is combined with the theoretical curve length sampling set of the previous time sampling point to obtain the candidate curve length sampling set of the target time sampling point.

[0030] The maximum curve length sample value in the candidate curve length sample set is removed to obtain the theoretical curve length sample set of the target time sample point, and the maximum curve length sample value is used as the actual curve length of the target time sample point.

[0031] The linear velocity sample value corresponding to the maximum curve length sample value in the initial linear velocity sample set after removing the sampled data is taken as the theoretical linear velocity of the target time sampling point, and the mapping path position of the maximum curve length sample value on the continuous motion path is taken as the sampling point position of the target time sampling point.

[0032] In an optional implementation, the lower limit of the linear velocity distribution interval of the k-th target time sampling point, excluding the start time point and the end time point, is calculated using the following formula:

[0033] v kmin =max(v k-1 -a max Δt,v min );

[0034] The upper limit of the linear velocity distribution interval of the k-th target time sampling point is calculated using the following formula:

[0035] vkmax =min(v k-1 +a max Δt,v max );

[0036] Among them, v kmin The lower limit of the interval used to represent the linear velocity distribution interval of the k-th target time sampling point, v kmax The upper limit of the interval used to represent the linear velocity distribution interval of the k-th target time sampling point, v k-1 a is used to represent the theoretical linear velocity of the previous time sampling point of the k-th target time sampling point. max v is used to represent the maximum acceleration that satisfies the aforementioned acceleration constraint. max v is used to represent the maximum linear velocity value that satisfies the aforementioned linear velocity constraint condition. min The minimum linear velocity value that satisfies the linear velocity constraint condition is used to represent the preset time interval.

[0037] In an optional implementation, the initial angular velocity sampling set of the k-th target time sampling point is expressed by the following equation:

[0038]

[0039] Among them, W k w is used to represent the initial angular velocity sampling set of the k-th target time sampling point. k,r The angular velocity sample value with the number r in the initial angular velocity sampling set used to represent the k-th target time sampling point is s. k,r The curve length sample value with index r in the initial curve length sample set used to represent the k-th target time sampling point is v. k,r p(s) represents the linear velocity sample value with index r in the initial linear velocity sample set of the k-th target time sampling point. k,r ) is used to represent the curve length sample value s k,r Mapped path position on the continuous motion path p(s) k,r The first derivative of ) p(s) k,r The second derivative of ), ||·||2, is used to denote the L2 norm, N k This is used to represent the maximum value of the sampled value number of the k-th target time sampling point.

[0040] In an optional implementation, the step of performing continuous trajectory fitting based on the sampling point positions of the plurality of time sampling points, according to the motion state information of the path starting point and the motion state information of the path ending point, to obtain the optimal motion trajectory within the target motion time period, includes:

[0041] An initial sub-trajectory is constructed between the sampling point positions of two adjacent time sampling points in the plurality of time sampling points;

[0042] Based on the motion state information of the starting point and the motion state information of the ending point of the path, multiple initial sub-trajectories are jointly optimized to obtain the optimal motion trajectory formed by splicing the optimized multiple initial sub-trajectories.

[0043] In an optional implementation, the i-th initial sub-trajectory among the plurality of initial sub-trajectories is expressed by the following formula:

[0044]

[0045] Where, σ i (tT i-1 ) is used to represent the i-th initial sub-trajectory with respect to the time parameter t, c i The trajectory basis matrix β(tT) is used to represent the i-th initial sub-trajectory. i-1 ) is used to represent the time parameter vector of the i-th initial sub-trajectory, Δt is used to represent the preset time interval, and T is used to represent the time parameter vector of the i-th initial sub-trajectory. i-1 T is used to represent the time sampling point corresponding to the starting point of the i-th initial sub-trajectory. i Used to represent the time sampling point corresponding to the endpoint of the i-th initial sub-trajectory;

[0046] At this point, the joint optimization equation for the multiple initial sub-trajectories is expressed by the following equation:

[0047]

[0048] in, M is used to represent the total number of trajectories of the multiple initial sub-trajectories. Used to represent the motion state information of the starting point of the path. β is used to represent the motion state information at the end of the path. (b) (t′) is the b-th derivative of β(t′), pj represents the sampling point position corresponding to the sampling point with sampling point number j among the plurality of time sampling points, N fThe sampling point number used to represent the termination time point, b, is obtained by subtracting one from the total number of motion parameter types of the path start-point motion state information or the path end-point motion state information, where j∈{1,2,…,N} f -1}, the sampling point number of the starting time point is 0.

[0049] Secondly, this application provides a computer device, including a processor and a memory, wherein the memory stores a computer program that can be executed by the processor, and the processor can execute the computer program to implement the motion trajectory planning method described in any of the foregoing embodiments.

[0050] Thirdly, this application provides a readable storage medium having a computer program stored thereon, wherein when the computer program is executed by a computer device, it implements the motion trajectory planning method described in any of the foregoing embodiments.

[0051] In this case, the beneficial effects of the embodiments of this application may include the following:

[0052] This application constructs a target continuous spline curve with the same starting and ending points as the reference discrete path by constructing a spline curve from a given reference discrete path in arbitrary spatial dimensions. The target continuous spline curve is then reparameterized to obtain a continuous motion path with respect to its length. Subsequently, for multiple time sampling points sampled within a specific time period (i.e., the target motion time period corresponding to the desired trajectory runtime), the corresponding sampling point positions satisfying the linear velocity, acceleration, and angular velocity constraints are determined on the continuous motion path. This is based on the motion state information of the starting point of the reference discrete path. Based on the motion state information of the path endpoint, continuous trajectory fitting is performed on the sampling point positions of the aforementioned multiple time sampling points to obtain the optimal motion trajectory that is applicable to the corresponding spatial dimension and continuous within the target motion time period. This effectively expands the applicable spatial dimension range of the motion trajectory planning scheme provided in this application, making it easier to improve the applicability of the corresponding trajectory planning scheme in different application scenarios. At the same time, the planned motion trajectory can effectively avoid unnecessary acceleration and / or deceleration of the controlled target (i.e., the motion target running the motion trajectory), thereby improving the motion control effect and motion smoothness of the controlled target and effectively reducing the motion energy consumption of the controlled target.

[0053] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, preferred embodiments are described below in detail with reference to the accompanying drawings. Attached Figure Description

[0054] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0055] Figure 1 A schematic diagram of the composition of a computer device provided in the embodiments of this application;

[0056] Figure 2 A flowchart illustrating the motion trajectory planning method provided in an embodiment of this application;

[0057] Figure 3 A schematic diagram comparing the target continuous spline curve and the reference discrete path provided in the embodiments of this application;

[0058] Figure 4 for Figure 2 A flowchart illustrating the sub-steps included in step S220;

[0059] Figure 5 for Figure 2 A flowchart illustrating the sub-steps included in step S230;

[0060] Figure 6 for Figure 2 The flowchart of the sub-steps included in step S250 is shown below.

[0061] Icons: 10-Computer equipment; 11-Memory; 12-Processor; 13-Communication unit. Detailed Implementation

[0062] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. The components of the embodiments of this application described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0063] Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely to illustrate selected embodiments of the application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0064] It should be noted that similar labels and letters in the following figures indicate similar items. Therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures.

[0065] In the description of this application, it should be understood that the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, or the orientation or positional relationship commonly used when the product is in use, or the orientation or positional relationship commonly understood by those skilled in the art. They are used only for the convenience of describing this application and simplifying the description, and are not intended to indicate or imply that the equipment or component referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this application.

[0066] In the description of this application, it should also be noted that, unless otherwise expressly specified and limited, the terms "set up," "install," "connect," and "link" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in this application based on the specific circumstances.

[0067] Furthermore, it is understood in the description of this application that relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element. Those skilled in the art will understand the specific meaning of the above terms in this application based on the specific circumstances.

[0068] The following detailed description of some embodiments of this application is provided in conjunction with the accompanying drawings. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0069] Please refer to Figure 1 , Figure 1This is a schematic diagram of the composition of the computer device 10 provided in an embodiment of this application. In this embodiment, the computer device 10 can be used to implement motion trajectory planning functions, and the computer device 10 may be, but is not limited to, a personal computer, a laptop computer, a server, etc.

[0070] In this embodiment of the application, the computer device 10 may include a memory 11, a processor 12, and a communication unit 13. The memory 11, the processor 12, and the communication unit 13 are electrically connected to each other directly or indirectly to achieve data transmission or interaction. For example, these components can be electrically connected to each other via one or more communication buses or signal lines.

[0071] In this embodiment, the memory 11 may be, but is not limited to, random access memory (RAM), read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), etc. The memory 11 is used to store computer programs, and the processor 12 can execute the computer programs accordingly after receiving execution instructions.

[0072] In this embodiment, the processor 12 can be an integrated circuit chip with signal processing capabilities. The processor 12 can be a general-purpose processor, including at least one of a Central Processing Unit (CPU), Graphics Processing Unit (GPU), Network Processor (NP), Digital Signal Processor (DSP), Application-Specific Integrated Circuit (ASIC), Field-Programmable Gate Array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. The general-purpose processor can be a microprocessor or any conventional processor, capable of implementing or executing the methods, steps, and logic block diagrams disclosed in this embodiment.

[0073] In this embodiment, the communication unit 13 is used to establish a communication connection between the computer device 10 and other electronic devices through a network, and to send and receive data through the network, wherein the network includes wired communication networks and wireless communication networks. For example, the computer device 10 can obtain a discrete path (i.e., composed of the path point positions of multiple discrete path points) specified by the user through a terminal device in any spatial dimension (e.g., a two-dimensional spatial dimension or a three-dimensional spatial dimension) through the communication unit 13, so as to plan a continuous motion trajectory suitable for the corresponding spatial dimension based on the obtained discrete path; the computer device 10 can also transmit the planned motion trajectory to the controlled target through the communication unit 13 to drive the controlled target to move according to the motion trajectory.

[0074] In this embodiment, the computer device 10 may pre-store a specific computer program related to the motion trajectory planning function in the memory 11. By driving the processor 12 to execute the specific computer program, based on a given discrete path of arbitrary spatial dimension, the device plans an optimal motion trajectory that is suitable for the corresponding spatial dimension and continuous under numerous motion constraints through the organic combination of path smoothing fitting and path sampling fitting. This effectively expands the applicable spatial dimension range of the motion trajectory planning scheme provided in this application, making it easier to improve the applicability of the corresponding trajectory planning scheme in different application scenarios. At the same time, the planned motion trajectory can effectively avoid unnecessary acceleration and / or deceleration of the controlled target, thereby improving the motion control effect and motion smoothness of the controlled target and effectively reducing the motion energy consumption of the controlled target.

[0075] Understandable Figure 1 The block diagram shown is only a schematic diagram of one configuration of the computer device 10. The computer device 10 may also include components such as... Figure 1 The more or fewer components shown, or having the same Figure 1 The different configurations shown. Figure 1 The components shown can be implemented using hardware, software, or a combination thereof.

[0076] In this application, to ensure that the computer device 10 can perform continuous motion trajectory planning for a specified discrete path in any spatial dimension, and to minimize unnecessary acceleration and / or deceleration in the planned motion trajectory, thereby effectively improving the motion control effect and smoothness of the controlled target, and effectively reducing the motion energy consumption of the controlled target, this application provides a motion trajectory planning method to achieve the aforementioned objectives. The motion trajectory planning method provided in this application will be described in detail below.

[0077] Please refer to Figure 2 , Figure 2This is a flowchart illustrating the motion trajectory planning method provided in an embodiment of this application. In this embodiment, the motion trajectory planning method may include steps S210 to S260.

[0078] Step S210: Obtain the reference discrete path, motion linear velocity constraints, motion acceleration constraints, motion angular velocity constraints, and desired trajectory runtime for trajectory planning, as well as the motion state information of the starting point and ending point of the reference discrete path.

[0079] In this embodiment, the reference discrete path Q can be represented as in n represents the path endpoint number of the reference discrete path, m represents the actual spatial dimension of the reference discrete path, and q represents the path endpoint number of the reference discrete path. i This is used to represent the path point position of the path discrete point with discrete point number i in the reference discrete path, where the path start number of the reference discrete path is 0. For example, when m = 2, it indicates that the reference discrete path represents a path in two-dimensional space, and the path point position of any path discrete point on the reference discrete path can be represented as q. i =[x i ,y i ] T When m = 3, it indicates that the reference discrete path represents a path in three-dimensional space, and the path point position of any discrete point on the reference discrete path can be represented as q. i =[x i ,y i ,z i ] T .

[0080] In this embodiment, the linear velocity constraint condition is used to limit the velocity value of the final planned trajectory at any time point to between a set minimum linear velocity value and a set maximum linear velocity value; the acceleration constraint condition is used to limit the absolute value of the acceleration of the final planned trajectory at any time point to be lower than a set maximum acceleration; the angular velocity constraint condition is used to limit the absolute value of the angular velocity of the final planned trajectory at any time point to be lower than a set maximum angular velocity; and the desired trajectory running time is used to represent the time required for the final planned trajectory to complete one run.

[0081] In this embodiment, the motion state information of the starting point of the reference discrete path This is used to represent the specific motion state that the final planned motion trajectory needs to satisfy at the starting point of the reference discrete path, and the motion state information at the ending point of the reference discrete path. This is used to represent the specific motion state that the final planned motion trajectory needs to satisfy at the end point of the reference discrete path; wherein, the motion state information of the path start point and the motion state information of the path end point both involve the same combination of multiple motion parameter types (e.g., motion position, motion linear velocity, motion acceleration, etc.) (e.g., a combination of motion position and motion linear velocity, or a combination of motion position, motion linear velocity and motion acceleration).

[0082] Step S220: Construct a spline curve based on the reference discrete path to obtain a target continuous spline curve with the same path start and path end as the reference discrete path.

[0083] In this embodiment, with Figure 3 As shown in the figure, for a reference discrete path in any spatial dimension (such as...) Figure 3 For the dashed path consisting of 8 discrete points q0 to q7 in the image, the corresponding target continuous spline curve (such as...) Figure 3 The solid lines in the target continuous spline curve will pass through the starting point and ending point of the reference discrete path. At the same time, the target continuous spline curve is continuous and differentiable everywhere. The target continuous spline curve will not pass through other path discrete points of the reference discrete path except for the starting point and ending point, but will be very close to these path discrete points to ensure that the target continuous spline curve can effectively represent the smoothness of the reference discrete path.

[0084] Alternatively, please refer to Figure 4 , Figure 4 yes Figure 2 The flowchart of step S220 is shown below. In the embodiments of this application, step S220 may include sub-steps S221 to S222 to ensure that the constructed target continuous spline curve can effectively characterize the smoothness of the reference discrete path.

[0085] Sub-step S221: Obtain the expected interpolation degree for the target continuous spline curve, and construct the target B-spline basis function that satisfies the expected interpolation degree for each discrete point of the reference discrete path.

[0086] In this embodiment, the target continuous spline curve can be obtained by fitting a B-spline curve, and thus the target continuous spline curve can be expressed as h(u) = C(u). T Q, where C(u) = [N 0,d (u),…,N i,d (u),…,N n,d (u)] T N i,d(u) represents the objective B-spline basis function of the path discrete point numbered i, which satisfies the expected interpolation degree d and is related to the curve independent variable parameter u, where u ∈ [0,1], and the expected interpolation degree is a positive integer. In this process, the objective B-spline basis function of the path discrete point numbered i in the reference discrete path is calculated using the following equation:

[0087]

[0088] Where, N i,d (u) represents the objective B-spline basis function of the path discrete point numbered i, which satisfies the expected interpolation degree d and is related to the curve independent variable parameter u, and n represents the path endpoint number of the reference discrete path.

[0089] Step S222: Based on the target B-spline basis functions of each of the discrete points along the path, perform B-spline curve fitting on the spatial positions of each of the discrete points along the path to obtain the target continuous spline curve.

[0090] Therefore, by executing the above sub-steps S221 to S222, this application can ensure that the constructed target continuous spline curve can effectively characterize the smoothness of the reference discrete path.

[0091] Step S230: Perform curve reparameterization on the target continuous spline curve to obtain a continuous motion path with respect to the curve length, wherein the curve length is based on the starting point of the reference discrete path.

[0092] In this embodiment, the target continuous spline curve h(u) can be converted into a continuous motion path p(s) with respect to the curve length by performing path sampling fitting on the target continuous spline curve h(u), where the curve length... This is used to represent the curve length value from the path endpoint on the target continuous spline curve to the corresponding path start point when it is used as a curve sampling point.

[0093] Alternatively, please refer to Figure 5 , Figure 5 yes Figure 2 The flowchart of step S230 includes the sub-steps. In the embodiments of this application, step S230 may include sub-steps S231 to S233 to ensure that the constructed continuous motion path can show the mapping relationship between the smooth motion position and the curve length.

[0094] Sub-step S231 involves sampling the target continuous spline curve at equal intervals to obtain multiple curve sampling points, including the path start and path end points of the target continuous spline curve, and calculating the curve length value from each curve sampling point to the path start point.

[0095] In this embodiment, the independent variable parameter u∈[0,1] of the curve can be sampled according to the independent variable interval δ to obtain the corresponding curve independent variable parameter sampling set. Where u′ i =iδ, i∈[0,1,…,N d ], At this time, the corresponding set of curve sampling points is: The distance between adjacent sampling points of two adjacent curve sampling points can be expressed as D. i =||h(u′) i-1 )-h(u′ i ||2, ||·||2 are used to represent the L2 norm. For the curve sampling point h(u′0) corresponding to the starting point of the path to the target continuous spline curve, its curve length value s0 = 0. For other curve sampling points besides the starting point, their curve length values ​​can be expressed as... At this point, the sample set of the independent variable parameters of the curve can be obtained. Matched curve length dataset

[0096] Sub-step S232 involves performing a red-black tree query based on the curve length values ​​of each curve sampling point, and constructing a parameter association relationship between the curve length and the curve independent variable parameter of the target continuous spline curve between two adjacent curve sampling points based on the query results.

[0097] In this embodiment, the computer device 10 can use a red-black tree to store the curve length dataset that matches the sample set of the curve's independent variable parameters. Based on the red-black tree's property that "the left leaf node of any subtree is always smaller than the root node of that subtree, and the right leaf node of any subtree is always larger than the root node of that subtree," the red-black tree query logic can be used to quickly search for any... Query a set (s) j ,s j+1 To ensure s j <s<s j+1 This is to improve the overall efficiency of trajectory planning.

[0098] Then, the computer device 10 will be based on (s) j ,s j+1 (u′) j ,u′ j+1 ), and construct the parametric relationship between the curve length and the curve's independent variable parameter at two curve sampling points numbered j and j+1. This parametric relationship can then be expressed as:

[0099] Sub-step S233: Based on the parameter correlation between two adjacent curve sampling points among all curve sampling points, perform curve equivalent transformation on the target continuous spline curve to obtain the continuous motion path.

[0100] Therefore, by executing the above sub-steps S231 to S233, this application can ensure that the constructed continuous motion path can exhibit the mapping relationship between the smooth motion position and the curve length.

[0101] Step S240: Time sampling is performed within the target motion time period corresponding to the expected trajectory running time according to a preset time interval, to obtain multiple time sampling points including the start time point and the end time point of the target motion time period.

[0102] In this embodiment, the plurality of time sampling points can be represented as Where T0 represents the starting time point of the target motion time period. T is used to indicate the end time point of the target motion time period. k -T k-1 =Δt, where Δt represents the preset time interval, N f The sampling point number used to indicate the termination time point.

[0103] Step S240: Determine the sampling point positions that satisfy the linear velocity constraint, acceleration constraint, and angular velocity constraint conditions for each of the multiple time sampling points on the continuous motion path.

[0104] In this embodiment, for the starting time point T0 among the plurality of time sampling points, the sampling point position of the starting time point T0 is the path point position of the starting point of the reference discrete path, the theoretical linear velocity of the starting time point T0 is the L2 norm of the linear velocity in the motion state information of the starting point of the path, the actual curve length of the starting time point T0 is s0 = 0, and the sampling set of the theoretical curve length of the starting time point T0 is {s0}; while for the ending time point among the plurality of time sampling points... In other words, the end time point The sampling point location is the path point location of the endpoint of the reference discrete path, and the termination time point. The theoretical linear velocity is the L2 norm value of the linear velocity in the motion state information at the end of the path.

[0105] Alternatively, please refer to Figure 6 , Figure 6 yes Figure 2The flowchart of step S250 is shown below. In this embodiment, step S250 may include sub-steps S251 to S258, to solve for the sampling point positions that satisfy the linear velocity constraint, acceleration constraint, and angular velocity constraint for each time sampling point other than the start and end time points of the target time period. This ensures that the motion evolution trend of the sampling point positions at each time sampling point is smooth and stable in the time dimension, so that the final planned motion trajectory can avoid unnecessary acceleration and / or deceleration of the controlled target as much as possible.

[0106] Sub-step S251: Take all time sampling points other than the start time point and the end time point as target time sampling points in sequence, and determine the linear velocity distribution range of the target time sampling points that satisfies the motion acceleration constraint and motion linear velocity constraint based on the theoretical linear velocity of the previous time sampling point of the target time sampling point.

[0107] In this embodiment, the lower limit of the linear velocity distribution interval for the k-th target time sampling point, excluding the start time point and the end time point, is calculated using the following formula:

[0108] v k min =max(v k-1 -a max Δt,v min );

[0109] The upper limit of the linear velocity distribution interval of the k-th target time sampling point is calculated using the following formula:

[0110] v k max =min(v k-1 +a max Δt,v max );

[0111] Among them, v kmin The lower limit of the interval used to represent the linear velocity distribution interval of the k-th target time sampling point, v kmax The upper limit of the interval used to represent the linear velocity distribution interval of the k-th target time sampling point, v k-1 a is used to represent the theoretical linear velocity of the previous time sampling point of the k-th target time sampling point. max v is used to represent the maximum acceleration that satisfies the aforementioned acceleration constraint. max v is used to represent the maximum linear velocity value that satisfies the aforementioned linear velocity constraint condition. minThe minimum linear velocity value that satisfies the motion linear velocity constraint is used, and Δt is used to represent the preset time interval. Specifically, for the first target time sampling point, the previous time sampling point is the starting time point.

[0112] Sub-step S252: Perform linear velocity sampling on the linear velocity distribution interval according to the preset linear velocity resolution to obtain the initial linear velocity sampling set of the target time sampling point.

[0113] In this embodiment, the linear velocity interval between two adjacent linear velocity sample values ​​in the initial linear velocity sampling set of any target time sampling point is the preset linear velocity resolution; wherein, the initial linear velocity sampling set of the kth target time sampling point can be expressed by the following equation:

[0114]

[0115] Among them, V k This is used to represent the initial linear velocity sampling set for the k-th target time sampling point. Used to represent the preset linear velocity resolution, v k,r N is used to represent the linear velocity sample value with the number r in the initial linear velocity sample set of the k-th target time sampling point. k This is used to represent the maximum value of the sampled value number of the k-th target time sampling point.

[0116] Sub-step S253: Based on the actual curve length of the previous time sampling point, construct an initial curve length sampling set that matches the initial linear velocity sampling set.

[0117] In this embodiment, each curve length sample value in the initial linear velocity sample set of any target time sampling point corresponds to a separate linear velocity sample value. Each curve length sample value is obtained by adding the actual curve length of the previous time sampling point to the target length displacement that matches the corresponding linear velocity sample value. The target length displacement is the uniformly accelerated linear motion displacement of the corresponding linear velocity sample value and the theoretical linear velocity of the previous time sampling point within a unit time period. The length of the unit time period is consistent with the preset time interval. The initial curve length sample set of the k-th target time sampling point is expressed by the following equation:

[0118]

[0119] Among them, S k The initial curve length sampling set, s, is used to represent the k-th target time sampling point. k-1 s is used to represent the actual curve length of the previous time sampling point of the kth target time sampling point. k,rThe curve length sample value with index r in the initial curve length sample set used to represent the k-th target time sampling point is v. k-1 v is used to represent the theoretical linear velocity of the previous time sampling point of the k-th target time sampling point. k,r The linear velocity sample value with the number r in the initial linear velocity sampling set of the k-th target time sampling point is used to represent the linear velocity sample value. Δt is used to represent the preset time interval.

[0120] The initial curve length sampling set constructed by the computer device 10 for any target time sampling point can be stored using a max-heap structure. This allows the maximum curve length sampling value in the corresponding initial curve length sampling set to be quickly found in subsequent steps by utilizing the characteristics of the max-heap structure: "the parent node of each subtree is greater than the child node, each parent node has 1 to 2 child nodes, the root node in the entire tree structure is the largest node among all nodes, and only the root node and the last level nodes in the entire tree structure can be without sibling nodes, while other nodes must have sibling nodes." This effectively improves the overall trajectory planning efficiency.

[0121] Sub-step S254: Based on the initial linear velocity sampling set and the initial curve length sampling set, construct the initial angular velocity sampling set for the target time sampling point.

[0122] In this embodiment, the initial angular velocity sampling set of the k-th target time sampling point is expressed by the following equation:

[0123]

[0124] Among them, W k w is used to represent the initial angular velocity sampling set of the k-th target time sampling point. k,r The angular velocity sample value with the number r in the initial angular velocity sampling set used to represent the k-th target time sampling point is s. k,r The curve length sample value with index r in the initial curve length sample set used to represent the k-th target time sampling point is v. k,r p(s) represents the linear velocity sample value with index r in the initial linear velocity sample set of the k-th target time sampling point. k，r The curve length sample value s in the initial curve length sample set representing the k-th target time sampling point is used to represent the curve length sample value s. k，r Mapped path position on the continuous motion path p(s) k,r The first derivative of ) For p(s) k，r The second derivative of ), ||·||2, is used to denote the L2 norm, N. kThis is used to represent the maximum value of the sampled value number of the k-th target time sampling point.

[0125] Sub-step S255: Determine the target angular velocity sample values ​​in the initial angular velocity sample set that do not meet the motion angular velocity constraint conditions, and remove the sample data corresponding to the target angular velocity sample values ​​from the initial linear velocity sample set and the initial curve length sample set.

[0126] Specifically, for any initial linear velocity sampling set, the sampled data to be discarded are the linear velocity sampled values ​​corresponding to the target angular velocity sampled values; for any initial curve length sampling set, the sampled data to be discarded are the curve length sampled values ​​corresponding to the target angular velocity sampled values. After the sampled data of an initial curve length sampling set has been discarded, it will be rearranged and stored according to a max-heap structure to conform to the storage format of a max-heap structure.

[0127] Sub-step S256: Perform a union operation on the initial curve length sampling set after removing the sampled data and the theoretical curve length sampling set of the previous time sampling point to obtain the candidate curve length sampling set of the target time sampling point.

[0128] In this embodiment, the candidate curve length sampling set for any target time sampling point is also stored using a max-heap structure to quickly find the maximum curve length sampling value in the corresponding candidate curve length sampling set, thereby effectively improving the overall trajectory planning efficiency.

[0129] Sub-step S257: Remove the maximum curve length sample value from the candidate curve length sample set to obtain the theoretical curve length sample set for the target time sample point, and use the maximum curve length sample value as the actual curve length of the target time sample point.

[0130] In this embodiment, the computer device 10 can utilize the maximum value fast lookup characteristic of the max-heap structure (i.e., the root node of the corresponding max-heap structure is the maximum value node, and the maximum value can be determined by directly accessing the root node of the corresponding max-heap structure) to quickly find the maximum curve length sample value in the candidate curve length sample set for any target time sampling point. Then, by deleting the found maximum curve length sample value and rearranging the candidate curve length sample set according to the max-heap structure, the theoretical curve length sample set for the target time sampling point is obtained. At this time, the deleted maximum curve length sample value can be used as the actual curve length for the target time sampling point.

[0131] Sub-step S258: The linear velocity sample value corresponding to the maximum curve length sample value in the initial linear velocity sample set after removing the sampled data is taken as the theoretical linear velocity of the target time sampling point, and the mapping path position of the maximum curve length sample value on the continuous motion path is taken as the sampling point position of the target time sampling point.

[0132] In this embodiment, for any target time sampling point, when the actual curve length of the target time sampling point (i.e., the maximum curve length sampling value that has been deleted) is determined, the linear velocity sampling value corresponding to the actual curve length of the target time sampling point can be found in the initial linear velocity sampling set after the completion of the sampling data removal operation of the target time sampling point. The found linear velocity sampling value is directly used as the theoretical linear velocity of the target time sampling point. At the same time, the actual curve length of the target time sampling point can be substituted into the curve expression corresponding to the continuous motion path to solve for the mapping path position of the actual curve length of the target time sampling point on the continuous motion path. Then, the solved mapping path position is directly used as the sampling point position of the corresponding target time sampling point.

[0133] Therefore, by executing the above sub-steps S251 to S258, this application can solve for the sampling point positions that satisfy the linear velocity constraint, acceleration constraint, and angular velocity constraint for each time sampling point other than the start and end time points of the target time period. This ensures that the motion evolution trend of the sampling point positions at each time sampling point is smooth and stable in the time dimension, so that the final planned motion trajectory can avoid unnecessary acceleration and / or deceleration of the controlled target as much as possible.

[0134] Step S260: Based on the motion state information of the starting point and the motion state information of the ending point of the path, continuous trajectory fitting is performed based on the sampling point positions of multiple time sampling points to obtain the optimal motion trajectory within the target motion time period.

[0135] In this embodiment, after obtaining the sampling point positions of multiple time sampling points including the start and end times of the target time period, the planning task of the optimal motion trajectory can be decomposed into the planning tasks of multiple sub-trajectories. Then, by jointly solving the planning tasks of the multiple sub-trajectories, the optimal motion trajectory formed by splicing together multiple continuous sub-trajectories is finally obtained. Wherein, the two endpoints of any sub-trajectory are the sampling point positions of two adjacent time sampling points, and the optimal motion trajectory is... It can be expressed as σ(t)={σ1(t-T0),…,σ i (tT i-1 ),…,σ M (tT M-1)}, where M represents the total number of the multiple sub-trajectories, M = N f , σ i (tT i-1 () is used to represent the i-th sub-trajectory with respect to the time parameter t, where the i-th sub-trajectory must satisfy t∈[T] i-1 ,T i ],T i -T i-1 =Δt.

[0136] In this case, step S260 may include: constructing an initial sub-trajectory between the sampling point positions of two adjacent time sampling points among the plurality of time sampling points; and jointly optimizing the plurality of initial sub-trajectory according to the motion state information of the path starting point and the motion state information of the path ending point, with the aim of ensuring that the plurality of initial sub-trajectory simultaneously satisfy multiple motion state constraints, to obtain the optimal motion trajectory formed by splicing the optimized plurality of initial sub-trajectory.

[0137] In this process, the i-th initial sub-trajectory among the multiple initial sub-trajectories is expressed by the following formula:

[0138]

[0139] Where, σ i (tT i-1 ) is used to represent the i-th initial sub-trajectory with respect to the time parameter t, c i The trajectory basis matrix β(tT) is used to represent the i-th initial sub-trajectory. i-1 ) is used to represent the time parameter vector of the i-th initial sub-trajectory, Δt is used to represent the preset time interval, and T is used to represent the time parameter vector of the i-th initial sub-trajectory. i-1 T is used to represent the time sampling point corresponding to the starting point of the i-th initial sub-trajectory. i The time sampling point corresponding to the endpoint of the i-th initial sub-trajectory is used, where N is a positive integer.

[0140] The various motion state constraints can be expressed as follows: "The actual motion state information of the starting point of the reference discrete path on the corresponding initial sub-trajectory is consistent with the motion state information of the starting point of the path", "The actual motion state information of the ending point of the reference discrete path on the corresponding initial sub-trajectory is consistent with the motion state information of the ending point of the path", and "The actual motion state information of the ending point of any initial sub-trajectory is consistent with the actual motion state information of the starting point of the next initial sub-trajectory".

[0141] The motion state constraint condition "the actual motion state information of the path starting point of the reference discrete path on the corresponding initial sub-trajectory is consistent with the motion state information of the path starting point" can be expressed as follows: The motion state constraint condition "the actual motion state information of the path endpoint of the reference discrete path on the corresponding initial sub-trajectory is consistent with the motion state information of the path endpoint" can be expressed as follows: The motion state constraint condition "the actual motion state information of the endpoint of any initial sub-trajectory is consistent with the actual motion state information of the starting point of the next initial sub-trajectory" can be expressed as "β(Δt)". T c i =p i T "and "β [b] (Δt) T c i =β [b] (0) T c i+1 ", where p i This is used to indicate the sampling point position corresponding to the sampling point numbered i among the plurality of time sampling points, wherein β (b) (t′) is the b-th derivative of β(t′), where b is obtained by subtracting one from the total number of motion parameter types of the motion state information at the starting point or the ending point of the path.

[0142] Based on this, the joint optimization equation for the multiple initial sub-trajectories is expressed by the following equation:

[0143]

[0144] in, M is used to represent the total number of trajectories of the multiple initial sub-trajectories. Used to represent the motion state information of the starting point of the path. β is used to represent the motion state information at the end of the path. (b) (t′) is the b-th derivative of β(t′), pj represents the sampling point position corresponding to the sampling point with sampling point number j among the plurality of time sampling points, N f The sampling point number used to represent the termination time point, b, is obtained by subtracting one from the total number of motion parameter types of the path start-point motion state information or the path end-point motion state information, where j∈{1,2,…,N} f -1}, the sampling point number of the starting time point is 0, t′∈[0,Δt].

[0145] When faced with the above joint optimization equation, the computer device 10 can record the joint optimization equation as "Ac = b, where..." The computer device 10 can determine the elements in A and b based on the sampling point positions of the determined multiple time sampling points, and obtain the optimal motion trajectory that is continuous with the reference discrete path in the same spatial dimension within the target motion time period by solving c (i.e. obtaining the trajectory basis matrix of each of the multiple initial sub-trajectories).

[0146] It is worth noting that, due to The dimension is relatively large, so we can directly solve for c = A. -1 b might be slower. In this case, we can see from the matrix expansion of A that A satisfies the property of a sparse matrix. We can reduce the memory required for computation by using the compact data representation of sparse matrices, and we can avoid calculating A by decomposing the sparse matrix. -1 This effectively improves the efficiency of solving c, thereby improving the overall trajectory planning efficiency.

[0147] Therefore, by executing the above steps S210 to S260, this application can perform continuous motion trajectory planning for a specified discrete path in any spatial dimension, so that the planned motion trajectory avoids unnecessary acceleration and / or deceleration as much as possible, thereby effectively improving the motion control effect and motion smoothness of the controlled target, effectively reducing the motion energy consumption of the controlled target, and effectively expanding the applicable spatial dimension range of the motion trajectory planning scheme provided by this application, so as to improve the applicability of the corresponding trajectory planning scheme in different application scenarios.

[0148] In the embodiments provided in this application, it should be understood that the disclosed apparatus and methods can also be implemented in other ways. The apparatus embodiments described above are merely illustrative. For example, the flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods, and computer program products according to embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code, which contains one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions marked in the blocks may occur in a different order than those marked in the drawings. For example, two consecutive blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in a block diagram and / or flowchart, and combinations of blocks in block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or action, or using a combination of dedicated hardware and computer instructions.

[0149] Furthermore, the functional modules in the various embodiments of this application can be integrated together to form an independent part, or each module can exist independently, or two or more modules can be integrated to form an independent part. If the various functions provided in this application are implemented in the form of software functional modules and sold or used as independent products, they can be stored in a storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (e.g., a laptop, industrial robot, wheeled robot, drone, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned readable storage medium includes: USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, optical disks, and other media capable of storing program code.

[0150] The above descriptions are merely various embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A motion trajectory planning method, characterized in that, The method includes: Obtain the reference discrete path for trajectory planning, motion linear velocity constraints, motion acceleration constraints, motion angular velocity constraints, and desired trajectory runtime, as well as the motion state information of the starting point and ending point of the reference discrete path; Based on the reference discrete path, a spline curve is constructed to obtain a target continuous spline curve that has the same path start and path end as the reference discrete path. The target continuous spline curve is reparameterized to obtain a continuous motion path with respect to the curve length, wherein the curve length is based on the starting point of the reference discrete path; Time sampling is performed within the target motion time period corresponding to the expected trajectory running time according to a preset time interval, to obtain multiple time sampling points including the start time point and the end time point of the target motion time period; On the continuous motion path, determine the sampling point positions corresponding to each of the plurality of time sampling points that satisfy the motion linear velocity constraint, motion acceleration constraint, and motion angular velocity constraint, wherein the sampling point position corresponding to the start time point is the path start point of the reference discrete path, and the sampling point position corresponding to the end time point is the path end point of the reference discrete path. Based on the motion state information of the starting point and the motion state information of the ending point of the path, continuous trajectory fitting is performed based on the sampling point positions of the multiple time sampling points to obtain the optimal motion trajectory within the target motion time period.

2. The method according to claim 1, characterized in that, The step of constructing a spline curve based on the reference discrete path to obtain a target continuous spline curve with the same path start and path end as the reference discrete path includes: Obtain the desired interpolation degree for the target continuous spline curve, and construct target B-spline basis functions that satisfy the desired interpolation degree for each discrete point of the reference discrete path. Based on the target B-spline basis functions of each of the discrete points along the path, B-spline curves are fitted to the spatial positions of each of the discrete points along the path to obtain the target continuous spline curve.

3. The method according to claim 2, characterized in that, The target B-spline basis function of the path discrete point with the corresponding number i in the reference discrete path is calculated using the following equation: Where, N i,d (u) represents the objective B-spline basis function of the path discrete point numbered i, which is related to the curve independent variable parameter u and satisfies the expected interpolation degree d. n represents the path endpoint number of the reference discrete path, and the path start number of the reference discrete path is 0, where u∈[0,1].

4. The method according to claim 1, characterized in that, The step of reparameterizing the target continuous spline curve to obtain a continuous motion path with respect to the curve length includes: The target continuous spline curve is sampled at equal intervals to obtain multiple curve sampling points including the path start point and path end point of the target continuous spline curve. The curve length value from each curve sampling point to the path start point is calculated, wherein the curve length value from each curve sampling point to the path start point is obtained by accumulating the distance between all adjacent sampling points between the curve sampling point and the path start point. A red-black tree query is performed based on the curve length values ​​of each of the curve sampling points, and a parameter association relationship between the curve length and the curve independent variable parameter of the target continuous spline curve is constructed between two adjacent curve sampling points according to the query results. Based on the parameter correlation between two adjacent curve sampling points among all the curve sampling points, the target continuous spline curve is transformed into a curve equivalent to obtain the continuous motion path.

5. The method according to claim 1, characterized in that, The step of determining the positions of the sampling points that satisfy the linear velocity constraint, acceleration constraint, and angular velocity constraint on the plurality of time sampling points along the continuous motion path includes: All time sampling points other than the start time point and the end time point are sequentially used as target time sampling points. Based on the theoretical linear velocity of the previous time sampling point of the target time sampling point, the linear velocity distribution range of the target time sampling point that satisfies the motion acceleration constraint condition and the motion linear velocity constraint condition is determined. Linear velocity sampling is performed on the linear velocity distribution interval according to a preset linear velocity resolution to obtain the initial linear velocity sampling set of the target time sampling point; Based on the actual curve length of the previous time sampling point, an initial curve length sampling set matching the initial linear velocity sampling set is constructed. Based on the initial linear velocity sampling set and the initial curve length sampling set, an initial angular velocity sampling set for the target time sampling point is constructed; Identify the target angular velocity sample values ​​in the initial angular velocity sample set that do not meet the motion angular velocity constraint conditions, and remove the sample data corresponding to the target angular velocity sample values ​​from the initial linear velocity sample set and the initial curve length sample set; The initial curve length sampling set after removing the sampled data is combined with the theoretical curve length sampling set of the previous time sampling point to obtain the candidate curve length sampling set of the target time sampling point. The maximum curve length sample value in the candidate curve length sample set is removed to obtain the theoretical curve length sample set of the target time sample point, and the maximum curve length sample value is used as the actual curve length of the target time sample point. The linear velocity sample value corresponding to the maximum curve length sample value in the initial linear velocity sample set after removing the sampled data is taken as the theoretical linear velocity of the target time sampling point, and the mapping path position of the maximum curve length sample value on the continuous motion path is taken as the sampling point position of the target time sampling point.

6. The method according to claim 5, characterized in that, The lower limit of the linear velocity distribution interval of the k-th target time sampling point, excluding the start and end time points, is calculated using the following formula: v kmin =max(v k-1 -a max Δt,v min ); The upper limit of the linear velocity distribution interval of the k-th target time sampling point is calculated using the following formula: v kmax =min(v k-1 +a max Δt,v max ); Among them, v kmin The lower limit of the interval used to represent the linear velocity distribution interval of the k-th target time sampling point, v kmax The upper limit of the interval used to represent the linear velocity distribution interval of the k-th target time sampling point, v k-1 a is used to represent the theoretical linear velocity of the previous time sampling point of the k-th target time sampling point. max v is used to represent the maximum acceleration that satisfies the aforementioned acceleration constraint. max v is used to represent the maximum linear velocity value that satisfies the aforementioned linear velocity constraint condition. min The minimum linear velocity value that satisfies the linear velocity constraint condition is used to represent the preset time interval.

7. The method according to claim 6, characterized in that, The initial angular velocity sampling set of the k-th target time sampling point is expressed by the following equation: Among them, W k w is used to represent the initial angular velocity sampling set of the k-th target time sampling point. k,r The angular velocity sample value with the number r in the initial angular velocity sampling set used to represent the k-th target time sampling point is s. k,r The curve length sample value with index r in the initial curve length sample set used to represent the k-th target time sampling point is v. k,r p(s) represents the linear velocity sample value with index r in the initial linear velocity sample set of the k-th target time sampling point. k,r ) is used to represent the curve length sample value s k，r Mapped path position on the continuous motion path p(s) k,r The first derivative of ) p(s) k,r The second derivative of ), ||·||2, is used to denote the L2 norm, N k This is used to represent the maximum value of the sampled value number of the k-th target time sampling point.

8. The method according to any one of claims 1-7, characterized in that, The step of performing continuous trajectory fitting based on the motion state information of the path start point and the motion state information of the path end point, and based on the sampling point positions of the multiple time sampling points, to obtain the optimal motion trajectory within the target motion time period includes: An initial sub-trajectory is constructed between the sampling point positions of two adjacent time sampling points in the plurality of time sampling points; Based on the motion state information of the starting point and the motion state information of the ending point of the path, multiple initial sub-trajectories are jointly optimized to obtain the optimal motion trajectory formed by splicing the optimized multiple initial sub-trajectories.

9. The method according to claim 8, characterized in that, The i-th initial sub-trajectory among the multiple initial sub-trajectories is expressed by the following formula: Where, σ i (tT i-1 ) is used to represent the i-th initial sub-trajectory with respect to the time parameter t, c i The trajectory basis matrix β(tT) is used to represent the i-th initial sub-trajectory. i-1 ) is used to represent the time parameter vector of the i-th initial sub-trajectory, Δt is used to represent the preset time interval, and T is used to represent the time parameter vector of the i-th initial sub-trajectory. i-1 T is used to represent the time sampling point corresponding to the starting point of the i-th initial sub-trajectory. i The time sampling point is used to represent the endpoint of the i-th initial sub-trajectory, where N is a positive integer.

10. A computer device, characterized in that, It includes a processor and a memory, the memory storing a computer program that can be executed by the processor, the processor executing the computer program to implement the motion trajectory planning method according to any one of claims 1-9.

11. A readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a computer device, it implements the motion trajectory planning method according to any one of claims 1-9.

Images