Motion track planning method, computer equipment and readable storage medium

Abstract

The invention provides a motion trajectory planning method, computer equipment and a readable storage medium, and relates to the technical field of trajectory planning. According to the method, spline curve construction is carried out on a reference discrete path given under any spatial dimension, curve re-parameterization is carried out on the constructed target continuous spline curve, and a continuous motion path related to the curve length is obtained; then, determining sampling point positions of a plurality of time sampling points which simultaneously meet numerous motion constraint conditions in a specific time period on the continuous motion path, and determining the motion constraint conditions according to path starting point motion state information and path end point motion state information of a reference discrete path; and performing continuous trajectory fitting based on the respective sampling point positions of the plurality of time sampling points to obtain a continuous optimal motion trajectory suitable for the corresponding spatial dimension in the target motion time period, so as to effectively expand the applicable spatial dimension range of the trajectory planning scheme and avoid unnecessary acceleration and / or deceleration phenomena.

Description

Technical Field

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

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

[0003] It is worth noting that the application scenarios of various existing trajectory planning algorithms are relatively limited. They are often only applicable to specific spatial dimensions and are not compatible with each other in different spatial dimensions. At the same time, the corresponding planned motion trajectories often have unnecessary acceleration and / or deceleration in the actual operation process, 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 the present application is to provide a motion trajectory planning method, a computer device and a readable storage medium, which can, on the basis of a given discrete path of any spatial dimension, plan an optimal motion trajectory that is suitable for the corresponding spatial dimension and continuous under the constraints of numerous motion constraints through the organic combination of path smoothing fitting and path sampling fitting, so as to improve the applicability of the corresponding trajectory planning scheme in different application scenarios, while avoiding unnecessary acceleration and / or deceleration, to improve the motion control effect and motion smoothness of the motion target, and effectively reduce the motion energy consumption of the corresponding motion target.

[0005] In order to achieve the above purpose, the technical solution adopted in the embodiment of the present application is as follows:

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

[0007] Obtaining a reference discrete path that requires trajectory planning, motion linear velocity constraints, motion acceleration constraints, motion angular velocity constraints, and expected trajectory running time, as well as path start point motion state information and path end point motion state information of the reference discrete path;

[0008] Constructing a spline curve based on the reference discrete path to obtain a target continuous spline curve having the same path starting point and path end point as the reference discrete path;

[0009] Re-parameterizing 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 path starting point of the reference discrete path;

[0010] Performing time sampling in a target motion time period corresponding to the expected trajectory running time at a preset time interval to obtain a plurality of time sampling points including a start time point and an end time point of the target motion time period;

[0011] Determine sampling point positions satisfying motion linear velocity constraint conditions, motion acceleration constraint conditions and motion angular velocity constraint conditions corresponding to each of the plurality of time sampling points on the continuous motion path, wherein the sampling point position corresponding to the start time point is the path starting point of the reference discrete path, and the sampling point position corresponding to the end time point is the path ending point of the reference discrete path;

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

[0013] In an optional embodiment, the step of constructing a spline curve based on the reference discrete path to obtain a target continuous spline curve having the same path starting point and path end point as the reference discrete path includes:

[0014] Obtaining an expected interpolation number for the target continuous spline curve, and constructing a target B-spline basis function satisfying the expected interpolation number for the reference discrete path including each discrete point of the path;

[0015] According to the target B-spline basis functions of all the discrete points on the path, B-spline curve fitting is performed on the spatial positions of all the discrete points on the path to obtain the target continuous spline curve.

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

[0017]

[0018] Among them, N i,d (u) is used to represent the target B-spline basis function of the path discrete point numbered i that is related to the curve independent variable parameter u and satisfies the expected interpolation order d, n is used to represent the path end point number of the reference discrete path, and the path starting point number of the reference discrete path is 0, where u∈[0,1].

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

[0020] The target continuous spline curve is sampled at equal independent variable intervals to obtain a plurality of curve sampling points including the path starting point and the path end point of the target continuous spline curve, and the curve length values ​​from all the curve sampling points to the path starting point are calculated, wherein the curve length value from each curve sampling point to the path starting point is obtained by accumulating the distances between all adjacent sampling points between the curve sampling point and the path starting point;

[0021] Performing a red-black tree query based on the curve length values ​​of all the curve sampling points, 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 according to the query result;

[0022] Based on the parameter association relationship between two adjacent curve sampling points among all the curve sampling points, a curve equivalent transformation is performed on the target continuous spline curve to obtain the continuous motion path.

[0023] In an optional embodiment, the step of determining the sampling point positions satisfying the motion linear velocity constraint, motion acceleration constraint and motion angular velocity constraint corresponding to each of the plurality of time sampling points on the continuous motion path comprises:

[0024] Taking all time sampling points except the starting time point and the ending time point as target time sampling points in sequence, and determining a linear velocity distribution interval of the target time sampling point that satisfies the motion acceleration constraint condition and the motion linear velocity constraint condition according to the theoretical linear velocity of the previous time sampling point of the target time sampling point;

[0025] Performing linear velocity sampling on the linear velocity distribution interval according to a preset linear velocity resolution to obtain an initial linear velocity sampling set at 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 of the previous sampling point, an initial curve length sampling set matching the initial linear velocity sampling set is constructed, wherein each curve length sampling value in the initial curve length sampling set corresponds to a linear velocity sampling value, and each curve length sampling value is obtained by adding the actual curve length of the previous sampling point and a target length displacement matching the corresponding linear velocity sampling value, wherein the target length displacement is a uniformly accelerated linear motion displacement of the corresponding linear velocity sampling value and the theoretical linear velocity of the previous sampling point within a unit time period, and the time 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, construct an initial angular velocity sampling set of the target time sampling point;

[0028] Determine a target angular velocity sampling value that does not satisfy the motion angular velocity constraint condition in the initial angular velocity sampling set, and remove sampling data corresponding to the target angular velocity sampling value in the initial linear velocity sampling set and the initial curve length sampling set;

[0029] Performing a union operation on the initial curve length sampling set after sampled data is removed and the theoretical curve length sampling set of the previous time sampling point to obtain a candidate curve length sampling set of the target time sampling point;

[0030] Eliminate the maximum curve length sampling value in the candidate curve length sampling set to obtain a theoretical curve length sampling set of the target time sampling point, and use the maximum curve length sampling value as the actual curve length of the target time sampling point;

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

[0032] In an optional implementation manner, the interval lower limit value of the linear velocity distribution interval of the kth target time sampling point other than the starting time point and the ending 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 kth target time sampling point is calculated by the following formula:

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

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

[0037] In an optional implementation, the initial angular velocity sampling set at the kth target time sampling point is expressed using the following equation:

[0038]

[0039] Among them, W k It is used to represent the initial angular velocity sampling set of the k-th target time sampling point, w k,r The angular velocity sampling value numbered r in the initial angular velocity sampling set at the kth target time sampling point is used to represent the angular velocity sampling value, s k,r The curve length sampling value numbered r in the initial curve length sampling set for the kth target time sampling point is used to represent the curve length sampling value, v k,r It is used to represent the linear velocity sampling value numbered r in the initial linear velocity sampling set at the kth target time sampling point, p(s k,r ) is used to represent the curve length sampling value s k,r a mapped path position on said continuous motion path, is p(s k,r ), is p(s k,r ), ||·||2 is used to represent the L2 norm, N k Used to represent the maximum value of the sampling value number of the k-th target time sampling point.

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

[0041] Constructing an initial sub-trajectory between the sampling point positions of two adjacent time sampling points among the multiple time sampling points;

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

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

[0044]

[0045] Among them, σ 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 used to represent the i-th initial sub-trajectory, β(tT 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, T i-1 It is used to represent the time sampling point corresponding to the starting point of the i-th initial sub-trajectory, T i Used to represent the time sampling point corresponding to the trajectory end point of the i-th initial sub-trajectory;

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

[0047]

[0048] in,

[0049] M is used to represent the total number of trajectories of the multiple initial sub-trajectories, Used to indicate the motion state information of the starting point of the path, Used to represent the motion state information of the path endpoint, β (b) (t′) is the b-order derivative of β(t′), pj is used to represent the sampling point position corresponding to the time sampling point with sampling point number j among the multiple time sampling points, N fThe sampling point number used to represent the end 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.

[0050] In a second aspect, the present application provides a computer device, including a processor and a memory, wherein the memory stores a computer program executable by the processor, and the processor can execute the computer program to implement the motion trajectory planning method described in any one of the aforementioned embodiments.

[0051] In a third aspect, the present application provides a readable storage medium having a computer program stored thereon, and when the computer program is executed by a computer device, the motion trajectory planning method described in any one of the aforementioned embodiments is implemented.

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

[0053] The present application constructs a spline curve for a given reference discrete path in any spatial dimension to obtain a target continuous spline curve having the same path starting point and path ending point as the reference discrete path, and reparameterizes the target continuous spline curve to obtain a continuous motion path with respect to the curve length, and then determines the corresponding sampling point positions satisfying motion linear velocity constraints, motion acceleration constraints, and motion angular velocity constraints on the continuous motion path for multiple time sampling points sampled within a specific time period (i.e., a target motion time period corresponding to the expected trajectory running time), and determines the corresponding sampling point positions satisfying motion linear velocity constraints, motion acceleration constraints, and motion angular velocity constraints on the continuous motion path, and determines the motion state information of the path starting point of the reference discrete path according to the motion state information of the path starting point of the reference discrete path. and the motion state information of the path endpoint, and continuous trajectory fitting is performed based on the sampling point positions of each of the aforementioned multiple time sampling points to obtain an optimal motion trajectory that is suitable for the corresponding spatial dimension and continuous within the target motion time period, so as to effectively expand the applicable spatial dimension range of the motion trajectory planning scheme provided by the present application, facilitate improving the applicability of the corresponding trajectory planning scheme in different application scenarios, and at the same time, the planned motion trajectory can be used to effectively avoid unnecessary acceleration and / or deceleration of the controlled target (i.e., the motion target running the motion trajectory), so as to improve the motion control effect and motion smoothness of the controlled target, and effectively reduce the motion energy consumption of the controlled target.

[0054] In order to make the above-mentioned objects, features and advantages of the present application more obvious and easy to understand, preferred embodiments are specifically cited below and described in detail with reference to the attached drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for use in the embodiments will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.

[0056] Figure 1 A schematic diagram of the composition of a computer device provided in an embodiment of the present application;

[0057] Figure 2 A schematic diagram of a motion trajectory planning method provided in an embodiment of the present application;

[0058] Figure 3 A schematic diagram for comparing a target continuous spline curve and a reference discrete path provided in an embodiment of the present application;

[0059] Figure 4 for Figure 2 A schematic flow chart of the sub-steps included in step S220;

[0060] Figure 5 for Figure 2 A schematic flow chart of the sub-steps included in step S230;

[0061] Figure 6 for Figure 2 Schematic diagram of the flow chart of the sub-steps included in step S250.

[0062] Icons: 10 - computer device; 11 - memory; 12 - processor; 13 - communication unit. DETAILED DESCRIPTION

[0063] In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, rather than all 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.

[0064] 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 in the present application, all other embodiments obtained by ordinary technicians in the field without creative work are within the scope of protection of the present application.

[0065] It should be noted that similar reference numerals and letters denote similar items in the following drawings, and therefore, once an item is defined in one drawing, further definition and explanation thereof is not required in subsequent drawings.

[0066] In the description of the present application, it should be understood that the terms "center", "up", "down", "left", "right", "vertical", "horizontal", "inside", "outside", etc. indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, or are the orientations or positional relationships in which the product of the application is conventionally placed when in use, or are the orientations or positional relationships conventionally understood by those skilled in the art. They are only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on the present application.

[0067] In the description of this application, it should also be noted that, unless otherwise clearly specified and limited, the terms "set", "install", "connect", and "connect" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two elements. For ordinary technicians in this field, the specific meanings of the above terms in this application can be understood according to specific circumstances.

[0068] In addition, in the description of the present application, it is understood that the relational terms such as the term "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply that there is any such actual relationship or order between these entities or operations. Moreover, the term "include", "comprise" or any other variant thereof is intended to cover non-exclusive inclusion, so that the process, method, article or equipment including a series of elements includes not only those elements, but also includes other elements that are not clearly listed, or also includes elements inherent to such process, method, article or equipment. In the absence of more restrictions, the elements defined by the sentence "comprise one..." do not exclude the existence of other identical elements in the process, method, article or equipment including the elements. For those of ordinary skill in the art, the specific meanings of the above terms in the present application can be understood by specific circumstances.

[0069] In conjunction with the accompanying drawings, some embodiments of the present application are described in detail below. In the absence of conflict, the following embodiments and features in the embodiments can be combined with each other.

[0070] Please refer to Figure 1 , Figure 11 is a schematic diagram of the composition of the computer device 10 provided in the embodiment of the present application. In the embodiment of the present application, the computer device 10 can be used to implement the motion trajectory planning function, wherein the computer device 10 can be, but is not limited to: a personal computer, a laptop computer, a server, etc.

[0071] In the embodiment of the present 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, the memory 11, the processor 12, and the communication unit 13 may be electrically connected to each other via one or more communication buses or signal lines.

[0072] In the embodiment of the present application, the memory 11 may be, but not limited to, a random access memory (RAM), a read only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable read-only memory (EEPROM), etc. The memory 11 is used to store a computer program, and the processor 12 may execute the computer program accordingly after receiving an execution instruction.

[0073] In the embodiment of the present application, the processor 12 may be an integrated circuit chip with signal processing capability. The processor 12 may be a general-purpose processor, including a central processing unit (CPU), a graphics processing unit (GPU), a network processor (NP), a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA), or at least one of other programmable logic devices, discrete gates or transistor logic devices, and discrete hardware components. The general-purpose processor may be a microprocessor or the processor may be any conventional processor, etc., which may implement or execute the disclosed methods, steps, and logic block diagrams in the embodiment of the present application.

[0074] In the embodiment of the present application, 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 a wired communication network and a wireless communication network. For example, the computer device 10 can obtain a discrete path (i.e., composed of the path point positions of multiple path discrete points) specified by a 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.

[0075] In an embodiment of the present application, the computer device 10 may pre-store a specific computer program related to the motion trajectory planning function in the memory 11, and by driving the processor 12 to execute the specific computer program accordingly, on the basis of a given discrete path of any spatial dimension, through the organic combination of path smoothing fitting and path sampling fitting, under the constraints of numerous motion constraints, plan an optimal motion trajectory that is suitable for the corresponding spatial dimension and continuous, so as to effectively expand the applicable spatial dimension range of the motion trajectory planning scheme provided by the present application, facilitate improving the applicability of the corresponding trajectory planning scheme in different application scenarios, and at the same time, the planned motion trajectory can be used to effectively avoid unnecessary acceleration and / or deceleration of the controlled target, so as to improve the motion control effect and motion smoothness of the controlled target, and effectively reduce the motion energy consumption of the controlled target.

[0076] Understandably, Figure 1 The block diagram shown is only a schematic diagram of a composition of the computer device 10. The computer device 10 may also include Figure 1 More or fewer components as shown, or with Figure 1 Different configurations are shown. Figure 1 Each component shown in the figure can be implemented by hardware, software or a combination thereof.

[0077] In the present application, in order to ensure that the computer device 10 can perform continuous motion trajectory planning for a specified discrete path in any spatial dimension, so that the corresponding planned motion trajectory can avoid unnecessary acceleration and / or deceleration as much as possible, so as to effectively improve the motion control effect and motion smoothness of the controlled target, and effectively reduce the motion energy consumption of the controlled target, the embodiment of the present application achieves the above-mentioned purpose by providing a motion trajectory planning method. The motion trajectory planning method provided by the present application is described in detail below.

[0078] Please refer to Figure 2 , Figure 22 is a flow chart of a motion trajectory planning method provided in an embodiment of the present application. In an embodiment of the present application, the motion trajectory planning method may include steps S210 to S260.

[0079] Step S210, obtaining a reference discrete path requiring trajectory planning, motion linear velocity constraints, motion acceleration constraints, motion angular velocity constraints and expected trajectory running time, as well as path starting point motion state information and path ending point motion state information of the reference discrete path.

[0080] In this embodiment, the reference discrete path Q can be expressed as in n is used to represent the path end number of the reference discrete path, m is used to represent the actual space dimension where the reference discrete path is located, q i It is used to represent the path point position of the path discrete point with the discrete point number i in the reference discrete path, and the path starting point number of the reference discrete path is 0. For example, when m=2, it means that the path represented by the reference discrete path is in a two-dimensional space, and the path point position of any path discrete point on the reference discrete path can be represented by 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 path discrete point on the reference discrete path can be expressed as q i =[x i ,y i ,z i ] T .

[0081] In this embodiment, the motion linear velocity constraint is used to limit the motion velocity value of the finally planned motion trajectory at any time point to between the set minimum linear velocity value and the maximum linear velocity value; the motion acceleration constraint is used to limit the absolute value of the acceleration of the finally planned motion trajectory at any time point to be lower than the set maximum acceleration; the motion angular velocity constraint is used to limit the absolute value of the angular velocity of the finally planned motion trajectory at any time point to be lower than the set maximum angular velocity; the expected trajectory running time is used to indicate the length of time required for the finally planned motion trajectory to complete one run.

[0082] In this embodiment, the motion state information of the path starting point of the reference discrete path It is used to indicate the specific motion state that the final planned motion trajectory needs to meet at the path starting point of the reference discrete path, and the path end motion state information of the reference discrete path Used to represent the specific motion state that the finally planned motion trajectory needs to meet at the path end point of the reference discrete path; wherein the path starting point motion state information and the path end point motion state information both involve a combination of the same multiple motion parameter types (for example, motion position, motion linear velocity, motion acceleration, etc.) (for example, a combination of motion position and motion linear velocity, or a combination of motion position, motion linear velocity and motion acceleration).

[0083] Step S220 , constructing a spline curve based on the reference discrete path to obtain a target continuous spline curve having the same path starting point and path end point as the reference discrete path.

[0084] In this embodiment, Figure 3 As shown in the figure, for any spatial dimension, the reference discrete path (such as Figure 3 For example, the dotted path composed of 8 discrete points q0 to q7 in the path, the corresponding target continuous spline curve (such as Figure 3 The solid line in the figure) will pass through the path starting point and the path end 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 the path starting point and the path end point, but will be very close to these path discrete points to ensure that the target continuous spline curve can effectively characterize the smooth state of the reference discrete path.

[0085] Optionally, see Figure 4 , Figure 4 yes Figure 2 Schematic diagram of the flow of sub-steps included in step S220. In the embodiment of the present application, step S220 may include sub-steps S221 to S222 to ensure that the constructed target continuous spline curve can effectively represent the smooth state of the reference discrete path.

[0086] Sub-step S221, obtaining the expected interpolation times for the target continuous spline curve, and constructing target B-spline basis functions satisfying the expected interpolation times for the reference discrete path including each discrete point of the path.

[0087] In this embodiment, the target continuous spline curve can be obtained by fitting in the form of a B-spline curve, so 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) is used to represent the target B-spline basis function of the path discrete point numbered i that satisfies the expected interpolation number d and is related to the curve independent variable parameter u, u∈[0,1], and the expected interpolation number is a positive integer. In this process, the target B-spline basis function of the path discrete point numbered i in the reference discrete path is calculated using the following equation:

[0088]

[0089] Among them, N i,d (u) is used to represent the target B-spline basis function of the path discrete point numbered i that is associated with the curve independent variable parameter u and satisfies the desired interpolation order d, and n is used to represent the path endpoint number of the reference discrete path.

[0090] Step S222, performing B-spline curve fitting on the spatial positions of all the path discrete points according to the target B-spline basis functions of all the path discrete points, to obtain the target continuous spline curve.

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

[0092] Step S230 , re-parameterizing 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 path starting point of the reference discrete path.

[0093] 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 is It is used to represent the curve length value from the path end point on the target continuous spline curve to the corresponding path starting point when it is used as a curve sampling point.

[0094] Optionally, see Figure 5 , Figure 5 yes Figure 2 In the embodiment of the present application, the step S230 may include sub-steps S231 to S233 to ensure that the constructed continuous motion path can show a mapping relationship between a smooth motion position and a curve length.

[0095] Sub-step S231, sampling the target continuous spline curve with equal independent variable intervals to obtain a plurality of curve sampling points including the path start point and path end point of the target continuous spline curve, and calculating the curve length values ​​from all the curve sampling points to the path start point.

[0096] In this embodiment, the curve independent variable parameter u∈[0,1] 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 ], The corresponding curve sampling point set is The distance between two adjacent curve sampling points can be expressed as D i =||h(u′ i-1 )-h(u′ i )||2,||·||2 is used to represent the L2 norm. Among them, for the curve sampling point h(u′0) corresponding to the path starting point of the target continuous spline curve, its curve length value s0=0, and for other curve sampling points except the path starting point, its curve length value can be expressed as At this point, the sampling set of independent variable parameters of the curve can be obtained Matched curve length dataset

[0097] Sub-step S232, performing a red-black tree query based on the curve length values ​​of all curve sampling points, 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 according to the query result.

[0098] In this embodiment, the computer device 10 can use a red-black tree to store the curve length data set that matches the curve independent variable parameter sampling set, so as to quickly query any left leaf node of a subtree based on the characteristics of the red-black tree: "the left leaf node of any subtree must be smaller than the root node of the subtree, and the right leaf node of any subtree must be larger than the root node of the subtree". Query a group (s j ,s j+1 ) to ensure j <s<s j+1 , in order to improve the overall trajectory planning efficiency.

[0099] Then, the computer device 10 will be based on (s j ,s j+1 ) corresponds to (u′ j ,u′ j+1 ), a parameter association relationship between the curve length and the curve independent variable parameter is constructed at two curve sampling points numbered j and j+1. At this time, the parameter association relationship can be expressed as

[0100] Sub-step S233, based on the parameter association relationship between two adjacent curve sampling points among all the curve sampling points, a curve equivalent transformation is performed on the target continuous spline curve to obtain a continuous motion path.

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

[0102] Step S240 , performing time sampling in a target motion time period corresponding to the expected trajectory running time according to a preset time interval, and obtaining a plurality of time sampling points including a start time point and an end time point of the target motion time period.

[0103] In this embodiment, the multiple time sampling points can be expressed as T0 is used to represent the starting time point of the target movement time period, It is used to indicate the end time point of the target movement time period, T k -T k-1 =Δt, Δt is used to represent the preset time interval, N f The sampling point number used to indicate the end time point.

[0104] Step S240, determining sampling point positions satisfying motion linear velocity constraint conditions, motion acceleration constraint conditions and motion angular velocity constraint conditions corresponding to a plurality of time sampling points on the continuous motion path.

[0105] In this embodiment, for the starting time point T0 among the multiple time sampling points, the sampling point position of the starting time point T0 is the path point position of the path starting point of the reference discrete path, the theoretical linear velocity of the starting time point T0 is the L2 norm value of the motion linear velocity in the motion state information of the path starting point, the actual curve length of the starting time point T0 is s0=0, and the theoretical curve length sampling set of the starting time point T0 is {s0}; and for the ending time point T0 among the multiple time sampling points For example, the end time point The sampling point position is the path point position of the path end point of the reference discrete path, and the end time point The theoretical linear velocity is the L2 norm value of the motion linear velocity in the motion state information of the path endpoint.

[0106] Optionally, see Figure 6 , Figure 6 yes Figure 2Schematic diagram of the flow chart of the sub-steps included in step S250 in the embodiment of the present application, the step S250 may include sub-steps S251 to S258, so as to respectively solve the sampling point positions that satisfy the motion linear velocity constraint conditions, motion acceleration constraint conditions and motion angular velocity constraint conditions for each time sampling point other than the start time point and the end time point of the target time period, and ensure that the motion evolution trend of the sampling point positions of each time sampling point at the time dimension level is smooth and stable, so that the final planned motion trajectory can avoid unnecessary acceleration and / or deceleration of the controlled target as much as possible.

[0107] Sub-step S251, taking all time sampling points except the start time point and the end time point as target time sampling points in sequence, and determining the linear velocity distribution interval of the target time sampling point that satisfies the motion acceleration constraint condition and the motion linear velocity constraint condition according to the theoretical linear velocity of the previous time sampling point of the target time sampling point.

[0108] In this embodiment, the interval lower limit value of the linear velocity distribution interval of the kth target time sampling point other than the starting time point and the ending time point is calculated by the following formula:

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

[0110] The upper limit of the linear velocity distribution interval of the kth target time sampling point is calculated by the following formula:

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

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

[0113] Sub-step S252, performing linear velocity sampling on the linear velocity distribution interval according to a preset linear velocity resolution to obtain an initial linear velocity sampling set at a target time sampling point.

[0114] In this embodiment, the linear velocity interval between two adjacent linear velocity sampling 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:

[0115]

[0116] Among them, V k The initial linear velocity sampling set used to represent the k-th target time sampling point, It is used to represent the preset linear velocity resolution, v k,r It is used to represent the linear velocity sampling value numbered r in the initial linear velocity sampling set at the kth target time sampling point, N k Used to represent the maximum value of the sampling value number of the k-th target time sampling point.

[0117] Sub-step S253, constructing an initial curve length sampling set matching the initial linear velocity sampling set based on the actual curve length of the previous time sampling point.

[0118] In this embodiment, each curve length sampling value in the initial linear velocity sampling set of any target time sampling point corresponds to a linear velocity sampling value, and each curve length sampling value is obtained by adding the actual curve length of the previous time sampling point and the target length displacement matching the corresponding linear velocity sampling value, and the target length displacement is the uniformly accelerated linear motion displacement of the corresponding linear velocity sampling value and the theoretical linear velocity of the previous time sampling point in a unit time period, and the time length of the unit time period is consistent with the preset time interval. Among them, the initial curve length sampling set of the kth target time sampling point is expressed by the following equation:

[0119]

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

[0121] The initial curve length sampling set constructed by the computer device 10 for any target time sampling point can be stored in a maximum heap structure, so as to utilize the maximum heap structure characteristics that "the parent node of each subtree is larger than the child node, each parent node corresponds to 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 layer of nodes in the entire tree structure can have no sibling nodes, and other nodes must have sibling nodes" to quickly find the maximum curve length sampling value in the corresponding initial curve length sampling set in the subsequent step process, so as to effectively improve the overall trajectory planning efficiency.

[0122] Sub-step S254, constructing an initial angular velocity sampling set of a target time sampling point based on the initial linear velocity sampling set and the initial curve length sampling set.

[0123] In this embodiment, the initial angular velocity sampling set of the kth target time sampling point is expressed by the following equation:

[0124]

[0125] Among them, W k It is used to represent the initial angular velocity sampling set of the k-th target time sampling point, w k,r The angular velocity sampling value numbered r in the initial angular velocity sampling set at the kth target time sampling point is used to represent the angular velocity sampling value, s k,r The curve length sampling value numbered r in the initial curve length sampling set for the kth target time sampling point is used to represent the curve length sampling value, v k,r It is used to represent the linear velocity sampling value numbered r in the initial linear velocity sampling set at the kth target time sampling point, p(s k，r ) is used to represent the curve length sampling value s in the initial curve length sampling set of the kth target time sampling point k，r a mapped path position on said continuous motion path, is p(s k,r ), is p(s k，r ), ||·||2 is used to represent the L2 norm, N kUsed to represent the maximum value of the sampling value number of the k-th target time sampling point.

[0126] Sub-step S255, determining the target angular velocity sampling value that does not satisfy the motion angular velocity constraint condition in the initial angular velocity sampling set, and removing the sampling data corresponding to the target angular velocity sampling value in the initial linear velocity sampling set and the initial curve length sampling set.

[0127] For any initial linear velocity sampling set, the sampling data to be removed is the linear velocity sampling value corresponding to the target angular velocity sampling value; for any initial curve length sampling set, the sampling data to be removed is the curve length sampling value corresponding to the target angular velocity sampling value. After the sampling data of an initial curve length sampling set is removed, it will be rearranged and stored according to the maximum heap structure to conform to the storage form of the maximum heap structure.

[0128] Sub-step S256, performing a union operation on the initial curve length sampling set after the sampled data is removed and the theoretical curve length sampling set of the previous time sampling point to obtain a candidate curve length sampling set of the target time sampling point.

[0129] In this embodiment, the candidate curve length sampling set of any target time sampling point is also stored in a maximum heap structure, so as to quickly find the maximum curve length sampling value in the corresponding candidate curve length sampling set, so as to effectively improve the overall trajectory planning efficiency.

[0130] Sub-step S257, eliminating the maximum curve length sampling value in the candidate curve length sampling set, obtaining the theoretical curve length sampling set of the target time sampling point, and taking the maximum curve length sampling value as the actual curve length of the target time sampling point.

[0131] In this embodiment, the computer device 10 can use the maximum value fast query feature of the maximum heap structure (i.e., the root node of the corresponding maximum heap structure is the maximum value node, and the maximum value can be determined by directly accessing the root node of the corresponding maximum heap structure) to quickly find the maximum curve length sampling value in the candidate curve length sampling set of any target time sampling point, and then delete the found maximum curve length sampling value and re-arrange and store the candidate curve length sampling set according to the maximum heap structure to obtain the theoretical curve length sampling set of the target time sampling point. At this time, the deleted maximum curve length sampling value can be used as the actual curve length of the target time sampling point.

[0132] In sub-step S258, the linear speed sampling value corresponding to the maximum curve length sampling value in the initial linear speed sampling set after the sampling data is removed is used as the theoretical linear speed of the target time sampling point, and the mapping path position of the maximum curve length sampling value on the continuous motion path is used as the sampling point position of the target time sampling point.

[0133] 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 to be deleted) is determined, the linear speed sampling value corresponding to the actual curve length of the target time sampling point can be found in the initial linear speed sampling set of the target time sampling point for completing the sampling data elimination operation, and the found linear speed sampling value is directly used as the theoretical linear speed 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 the mapping path position of the actual curve length of the target time sampling point on the continuous motion path, and then the solved mapping path position is directly used as the sampling point position corresponding to the target time sampling point.

[0134] Therefore, the present application can solve the sampling point positions that satisfy the motion linear velocity constraints, motion acceleration constraints and motion angular velocity constraints for each time sampling point other than the start time point and the end time point of the target time period by executing the above sub-steps S251 to S258, thereby ensuring that the motion evolution trend of the sampling point positions of each time sampling point is smooth and stable at the time dimension level, so that the final planned motion trajectory can minimize unnecessary acceleration and / or deceleration of the controlled target.

[0135] Step S260 , performing continuous trajectory fitting based on the respective sampling point positions of a 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 an optimal motion trajectory within the target motion time period.

[0136] In this embodiment, after obtaining the sampling point positions of multiple time sampling points including the start time point and the end time point of the target time period, the planning task of the optimal motion trajectory can be decomposed into the planning tasks of multiple sub-trajectories, and then the planning tasks of the multiple sub-trajectories are jointly solved to finally obtain the optimal motion trajectory formed by splicing multiple continuous sub-trajectories. Among them, the two trajectory endpoints of any sub-trajectory are the sampling point positions of two adjacent time sampling points. At this time, the optimal motion trajectory It can be expressed as σ(t)={σ1(t-T0),…,σ i (tT i-1 ),…,σ M (tT M-1)}, where M is used to represent the total number of trajectories of the plurality of 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. For the i-th sub-trajectory, t∈[T i-1 ,T i ],T i -T i-1 =Δt.

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

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

[0139]

[0140] Among them, σ 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 used to represent the i-th initial sub-trajectory, β(tT 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, T i-1 It is used to represent the time sampling point corresponding to the starting point of the i-th initial sub-trajectory, T i It is used to represent the time sampling point corresponding to the end point of the trajectory of the i-th initial sub-trajectory, N is a positive integer,

[0141] The multiple motion state constraints can be respectively expressed as "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", "the actual motion state information of the path end point of the reference discrete path on the corresponding initial sub-trajectory is consistent with the motion state information of the path end point" and "the actual motion state information of the trajectory end point of any initial sub-trajectory is consistent with the actual motion state information of the trajectory starting point of the next initial sub-trajectory".

[0142] Among them, 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 The motion state constraint condition "the actual motion state information of the path end point of the reference discrete path on the corresponding initial sub-trajectory is consistent with the motion state information of the path end point" can be expressed as The motion state constraint condition "the actual motion state information of the end point 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 is used to indicate the sampling point position corresponding to the time sampling point with sampling point number i among the multiple time sampling points, wherein β (b) (t′) is the b-order derivative of β(t′), and b is obtained by subtracting one from the total number of motion parameter types of the motion state information at the starting point of the path or the motion state information at the end point of the path.

[0143] On this basis, the joint optimization equation of the multiple initial sub-trajectories is expressed by the following equation:

[0144]

[0145] in, M is used to represent the total number of trajectories of the multiple initial sub-trajectories, Used to indicate the motion state information of the starting point of the path, Used to represent the motion state information of the path endpoint, β (b) (t′) is the b-order derivative of β(t′), pj is used to represent the sampling point position corresponding to the time sampling point with sampling point number j among the multiple time sampling points, N f The sampling point number used to represent the end 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].

[0146] When the computer device 10 faces the above joint optimization equation, it 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 determined sampling point positions of each of the multiple time sampling points, and obtain the optimal motion trajectory in the target motion time period that is in the same spatial dimension and continuous with the reference discrete path by solving c (i.e., obtaining the trajectory basis matrix of each of the above-mentioned multiple initial sub-trajectories).

[0147] It is worth noting that due to The dimension is relatively large, so we can directly solve c = A -1 b may be slower. In this case, we can know from the matrix expansion of A that A satisfies the properties of sparse matrices. The compact data representation of sparse matrices can be used to reduce the memory required for calculations, and the decomposition of sparse matrices can be used to avoid the need to calculate A. -1 , thereby effectively improving the efficiency of solving c and further improving the overall trajectory planning efficiency.

[0148] Therefore, the present application can perform continuous motion trajectory planning for a specified discrete path in any spatial dimension by executing the above steps S210 to S260, so that the corresponding planned motion trajectory can avoid unnecessary acceleration and / or deceleration as much as possible, so as to effectively improve the motion control effect and motion smoothness of the controlled target, effectively reduce the motion energy consumption of the controlled target, and at the same time effectively expand the applicable spatial dimension range of the motion trajectory planning scheme provided by the present application, so as to facilitate the improvement of the applicability of the corresponding trajectory planning scheme in different application scenarios.

[0149] In the embodiments provided in the present application, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely schematic, for example, the flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to the embodiments of the present application. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and a module, a program segment or a part of a code 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 box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or the flowchart, and the combination of boxes in the block diagram and / or the flowchart can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0150] In addition, each functional module in each embodiment of the present application can be integrated together to form an independent part, or each module can exist separately, or two or more modules can be integrated to form an independent part. If the various functions provided by the present 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 such an understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or the part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including several instructions to enable a computer device (for example, a laptop, an industrial robot, a wheeled robot, a drone, etc.) to perform all or part of the steps of the method recorded in each embodiment of the present application. The aforementioned readable storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or CD-ROM and other various media that can store program code.

[0151] The above are only various implementations of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art who is familiar with the present technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

Claims

1. A motion trajectory planning method, characterized in that: The method comprises: Obtaining a reference discrete path that requires trajectory planning, motion linear velocity constraints, motion acceleration constraints, motion angular velocity constraints, and expected trajectory running time, as well as path start point motion state information and path end point motion state information of the reference discrete path; Constructing a spline curve based on the reference discrete path to obtain a target continuous spline curve having the same path starting point and path end point as the reference discrete path; Re-parameterizing 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 path starting point of the reference discrete path; Performing time sampling in a target motion time period corresponding to the expected trajectory running time at a preset time interval to obtain a plurality of time sampling points including a start time point and an end time point of the target motion time period; Determine sampling point positions satisfying motion linear velocity constraint conditions, motion acceleration constraint conditions and motion angular velocity constraint conditions corresponding to each of the plurality of time sampling points on the continuous motion path, wherein the sampling point position corresponding to the start time point is the path starting point of the reference discrete path, and the sampling point position corresponding to the end time point is the path ending point of the reference discrete path; According to the motion state information of the path starting point and the motion state information of the path ending point, continuous trajectory fitting is performed based on the sampling point positions of each of the multiple time sampling points to obtain an 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 having the same path starting point and path end point as the reference discrete path includes: Obtaining an expected interpolation number for the target continuous spline curve, and constructing a target B-spline basis function satisfying the expected interpolation number for the reference discrete path including each discrete point of the path; According to the target B-spline basis functions of all the discrete points on the path, B-spline curve fitting is performed on the spatial positions of all the discrete points on 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 numbered i in the reference discrete path is calculated using the following equation: Among them, N i,d (u) is used to represent the target B-spline basis function of the path discrete point numbered i that is related to the curve independent variable parameter u and satisfies the expected interpolation order d, n is used to represent the path end point number of the reference discrete path, and the path starting point 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 re-parameterizing the target continuous spline curve to obtain a continuous motion path with respect to the length of the curve comprises: The target continuous spline curve is sampled at equal independent variable intervals to obtain a plurality of curve sampling points including the path starting point and the path end point of the target continuous spline curve, and the curve length values ​​from all the curve sampling points to the path starting point are calculated, wherein the curve length value from each curve sampling point to the path starting point is obtained by accumulating the distances between all adjacent sampling points between the curve sampling point and the path starting point; Performing a red-black tree query based on the curve length values ​​of all the curve sampling points, 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 according to the query result; Based on the parameter association relationship between two adjacent curve sampling points among all the curve sampling points, a curve equivalent transformation is performed on the target continuous spline curve to obtain the continuous motion path.

5. The method according to claim 1, characterized in that The step of determining the sampling point positions satisfying the motion linear velocity constraint, motion acceleration constraint and motion angular velocity constraint corresponding to each of the plurality of time sampling points on the continuous motion path comprises: Taking all time sampling points except the starting time point and the ending time point as target time sampling points in sequence, and determining a linear velocity distribution interval of the target time sampling point that satisfies the motion acceleration constraint condition and the motion linear velocity constraint condition according to the theoretical linear velocity of the previous time sampling point of the target time sampling point; Performing linear velocity sampling on the linear velocity distribution interval according to a preset linear velocity resolution to obtain an initial linear velocity sampling set at the target time sampling point; Based on the actual curve length at the previous time sampling point, construct an initial curve length sampling set matching the initial linear velocity sampling set; Based on the initial linear velocity sampling set and the initial curve length sampling set, construct an initial angular velocity sampling set of the target time sampling point; Determine a target angular velocity sampling value that does not satisfy the motion angular velocity constraint condition in the initial angular velocity sampling set, and remove sampling data corresponding to the target angular velocity sampling value in the initial linear velocity sampling set and the initial curve length sampling set; Performing a union operation on the initial curve length sampling set after sampled data is removed and the theoretical curve length sampling set of the previous time sampling point to obtain a candidate curve length sampling set of the target time sampling point; Eliminate the maximum curve length sampling value in the candidate curve length sampling set to obtain a theoretical curve length sampling set of the target time sampling point, and use the maximum curve length sampling value as the actual curve length of the target time sampling point; The linear speed sampling value corresponding to the maximum curve length sampling value in the initial linear speed sampling set after the sampling data is eliminated is used as the theoretical linear speed of the target time sampling point, and the mapping path position of the maximum curve length sampling value on the continuous motion path is used 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 kth target time sampling point other than the starting time point and the ending time point is calculated by 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 kth target time sampling point is calculated by the following formula: v kmax =min(v k-1 +a max Δt,v max ); Among them, v kmin It is used to represent the lower limit of the linear velocity distribution interval of the k-th target time sampling point, v kmax It is used to represent the upper limit of the linear velocity distribution interval of the k-th target time sampling point, v k-1 It is used to represent the theoretical linear velocity of the previous time sampling point of the kth target time sampling point, a max It is used to represent the maximum acceleration that satisfies the motion acceleration constraint, v max It is used to represent the maximum linear velocity value that satisfies the linear velocity constraint condition of the motion, v min It is used to represent the minimum linear velocity value that satisfies the linear velocity constraint condition of the movement, and Δt 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 kth target time sampling point is expressed by the following equation: Among them, W k It is used to represent the initial angular velocity sampling set of the k-th target time sampling point, w k,r The angular velocity sampling value numbered r in the initial angular velocity sampling set for the kth target time sampling point is used to represent the angular velocity sampling value, s k,r The curve length sampling value numbered r in the initial curve length sampling set for the kth target time sampling point is used to represent the curve length sampling value, v k,r It is used to represent the linear velocity sampling value numbered r in the initial linear velocity sampling set at the kth target time sampling point, p(s k,r ) is used to represent the curve length sampling value s k，r a mapped path position on said continuous motion path, is p(s k,r ), is p(s k,r ), ||·||2 is used to represent the L2 norm, N k Used to represent the maximum value of the sampling value number of the k-th target time sampling point.

8. The method according to any one of claims 1 to 7, characterized in that: The step of performing continuous trajectory fitting based on the motion state information of the path starting point and the motion state information of the path ending point and the sampling point positions of the plurality of time sampling points to obtain the optimal motion trajectory within the target motion time period comprises: Constructing an initial sub-trajectory between the sampling point positions of two adjacent time sampling points among the multiple time sampling points; According to the motion state information of the path starting point and the motion state information of the path ending point, a plurality of initial sub-trajectories are jointly optimized to obtain the optimal motion trajectory formed by splicing the plurality of optimized 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: Among them, σ 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 used to represent the i-th initial sub-trajectory, β(tT 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, T i-1 It is used to represent the time sampling point corresponding to the starting point of the i-th initial sub-trajectory, T i It is used to represent the time sampling point corresponding to the end point of the trajectory of the i-th initial sub-trajectory, and N is a positive integer.

10. A computer device, characterized in that: It includes a processor and a memory, wherein the memory stores a computer program executable by the processor, and the processor can execute the computer program to implement the motion trajectory planning method described in any one of claims 1 to 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, the motion trajectory planning method described in any one of claims 1 to 9 is implemented.

Images