Trajectory polynomial generation device and its trajectory sequence controller

Abstract

The present invention provides a trajectory polynomial generation device and a trajectory sequence controller thereof. The trajectory sequence controller includes a trajectory sequence generator, a preparation circuit, a forward circuit, a backward circuit, and a generation circuit. The trajectory generator sequence activates the preparation circuit, the forward circuit, the backward circuit, and the generation circuit. The preparation circuit schedules the trajectory floating-point calculation circuit to calculate the x-axis step length between any two adjacent path points among a plurality of path points, and uses the x-axis step length and the coordinates of the plurality of path points to prepare the coefficient matrix and constant vector in the linear equation group of the cubic spline interpolation algorithm. The forward circuit schedules the trajectory floating-point calculation circuit to calculate at least one element of the quadratic differential value vector in the linear equation group using the coefficient matrix and the constant vector. The backward circuit schedules the trajectory floating-point calculation circuit to calculate the remaining elements of the quadratic differential value vector using the coefficient matrix and the constant vector. The generation circuit schedules the trajectory floating-point calculation circuit to calculate the coefficients of the curve using the quadratic differential value vector.

Description

Technical Field

[0001] This invention relates to an electronic circuit, and more particularly to a trajectory polynomial generation device and its trajectory sequence controller. Background Technology

[0002] In robot navigation, controlling the robot to move from point A to point B is commonly referred to as motion planning. Motion planning generally consists of two steps: path planning and trajectory planning. Path planning refers to searching for a path from point A to point B on a map, where the path is composed of a series of discrete waypoints. The waypoints on the path may be sparse (not smooth). To better control the robot's motion, it is necessary to transform the sparse waypoints into smooth curves or dense trajectory points, which is trajectory planning. How to implement hardware to accelerate the generation of the coefficients of the curve between any two adjacent waypoints in a multi-waypoint system, thereby reducing the real-time computation latency in trajectory planning, is one of the many important issues in the field of robotics. Summary of the Invention

[0003] This invention provides a trajectory polynomial generation device and a trajectory sequential controller, which schedules a trajectory floating-point calculation circuit to calculate the coefficient of the curve between any two adjacent waypoints in a motion path.

[0004] In an embodiment of the invention, the trajectory sequence controller includes a trajectory sequential generator, a prepare circuit, a forward circuit, a backward circuit, and a generation circuit. The prepare circuit is coupled to the trajectory sequence generator. The prepare circuit is used to schedule a trajectory floating-point calculation circuit based on the activation of the trajectory sequence generator to calculate the x-axis step size between any two adjacent path points among a plurality of path points, and to schedule the trajectory floating-point calculation circuit to prepare the coefficient matrix and constant vector in the linear equation system of the Cubic Spline Interpolation Algorithm using the x-axis step size and the coordinates of the plurality of path points. The forward circuit is coupled to the trajectory sequence generator. The forward circuit is used to schedule the trajectory floating-point calculation circuit based on the activation of the trajectory sequence generator to calculate at least one element of the quadratic difference value vector in the linear equation system using the coefficient matrix and constant vector. The backward circuit is coupled to the trajectory sequence generator. The backward circuit is used to schedule the trajectory floating-point calculation circuit based on the activation of the trajectory sequence generator to calculate the remaining elements in the quadratic difference value vector using the coefficient matrix and constant vector. The generation circuit is coupled to the trajectory sequence generator. Based on the activation of the trajectory sequence generator, the generation circuit schedules the trajectory floating-point calculation circuit to calculate the coefficients of the curve using the x-axis step size, the coordinates of the plurality of path points, and the quadratic difference vector.

[0005] In an embodiment of the present invention, the trajectory polynomial generation apparatus includes a trajectory floating-point calculation circuit, a trajectory buffer, and a trajectory sequence controller. The trajectory buffer provides coordinates of multiple path points. The trajectory sequence controller is coupled to the trajectory buffer and the trajectory floating-point calculation circuit. The trajectory sequence controller schedules the trajectory floating-point calculation circuit to perform a cubic spline interpolation algorithm to convert the coordinates of the multiple path points into coefficients of a curve. The trajectory sequence controller schedules the trajectory floating-point calculation circuit to calculate the x-axis step size between any two adjacent path points among the multiple path points, schedules the trajectory floating-point calculation circuit to prepare the coefficient matrix and constant vector in the linear equation system of the cubic spline interpolation algorithm using the x-axis step size and the coordinates of the multiple path points, schedules the trajectory floating-point calculation circuit to calculate the elements of the quadratic difference vector in the linear equation system using the coefficient matrix and constant vector, and schedules the trajectory floating-point calculation circuit to calculate the coefficients of the curve using the x-axis step size, the coordinates of the multiple path points, and the quadratic difference vector.

[0006] Based on the above, the trajectory polynomial generation device can accelerate the generation of coefficients of multiple curves between any two adjacent path points using hardware, thereby reducing the real-time calculation delay in trajectory planning. Attached Figure Description

[0007] Figure 1 This is a schematic diagram of a circuit block of a trajectory polynomial generation device according to an embodiment of the present invention;

[0008] Figure 2 This is a circuit block diagram of a trajectory sequence controller according to an embodiment of the present invention;

[0009] Figure 3 This is a schematic diagram illustrating the configuration of the cache assembly according to an embodiment of the present invention.

[0010] Explanation of reference numerals in the attached figures

[0011] 10: External devices

[0012] 100: Trajectory Polynomial Generator

[0013] 110: Interface conversion circuit

[0014] 120, 130, 140: Ping-Pong Register

[0015] 150, 200: Trajectory Sequence Controller

[0016] 160: Trajectory Floating-Point Calculation Circuit

[0017] 161: Floating-point multiplication and addition calculation circuit

[0018] 162: Floating-point division circuit

[0019] 170: Direct Memory Access (DMA) Controller

[0020] 180: Track Cache

[0021] 181: Install Cache

[0022] 182: Intermediate Cache

[0023] 183: Delivery Cache

[0024] 210: Trajectory Sequence Generator

[0025] 220: Preparing the Circuit

[0026] 230: Forward Circuit

[0027] 240: Rear-entry circuit

[0028] 250: Generation Circuit

[0029] BANK1, BANK2, BANK3, BANKm: Library

[0030] TRDMA: Track Read Direct Memory Access Controller

[0031] TWDMA: Trace Write Direct Memory Access Controller

[0032] WP[1], WP[2], WP[3], WP[m], WP[m+1], WP[m+2], WP[m+3], WP[2m]: way point Detailed Implementation

[0033] Reference will now be made in detail to exemplary embodiments of the invention, examples of which are illustrated in the accompanying drawings. Wherever possible, the same component reference numerals are used in the drawings and description to denote the same or similar parts.

[0034] The term "coupled (or connected)" as used throughout this specification (including the claims) may refer to any direct or indirect means of connection. For example, if the text describes a first device coupled (or connected) to a second device, it should be interpreted as the first device being directly connected to the second device, or the first device being indirectly connected to the second device through other devices or some means of connection. The terms "first," "second," etc., used throughout this specification (including the claims) are used to name components, and are not intended to limit the upper or lower limit of the number of components, nor to limit the order of components. Furthermore, wherever possible, components / components / steps using the same reference numerals in the drawings and embodiments represent the same or similar parts. Components / components / steps using the same reference numerals or the same terms in different embodiments may be referred to mutually in the relevant descriptions.

[0035] The cubic spline interpolation algorithm can be used for trajectory planning in autonomous driving algorithms. When an autonomous vehicle (or robot) is moving, it uses a map to calculate the shortest path to its destination (i.e., path planning), where the path consists of a series of discrete waypoints. After completing path planning, the autonomous vehicle (or robot) performs interpolation to calculate the curve between any two adjacent waypoints. This invention will illustrate, through the following embodiments, a dedicated hardware acceleration device designed for the cubic spline interpolation algorithm: a trajectory polynomial generation (TPG) device. For example, a central processing unit (CPU) or other external device can perform path planning to generate a series of waypoints for the motion path, and then provide the coordinates of the waypoints to the trajectory polynomial generation device described in the following embodiments. The trajectory polynomial generation device can calculate the coefficients of the curve (polynomial) between any two adjacent waypoints, and then feed these coefficients back to the external device. Therefore, the external device can create a polynomial between any two adjacent waypoints based on the coefficients generated by the trajectory polynomial generation device.

[0036] There are two types of methods for calculating cubic spline interpolation. One type uses the chasing method, which is a fast solution method designed for tridiagonal, pentagonal, and other diagonal matrix linear equation systems. The second type uses methods for solving common matrix linear equation systems. Solving common matrix linear equation systems involves more complex scenarios and requires more complex constraints, resulting in the matrix equation system not forming a tridiagonal matrix. The following embodiments use a calculation method based on the chasing method, which can reduce the use of hardware resources and computational latency during real-time cubic spline interpolation calculations.

[0037] Figure 1 This is a circuit block diagram of a trajectory polynomial generation device 100 according to an embodiment of the present invention. An external device 10 (e.g., a CPU) can perform path planning to generate multiple path points for a motion path, and then provide the coordinates of these path points to the trajectory polynomial generation device 100. The trajectory polynomial generation device 100 can calculate the coefficients of the curve (polynomial) between any two adjacent path points in the multiple path points of the motion path, and then return these curve coefficients to the external device 10. Figure 1The trajectory polynomial generation device 100 shown includes an interface conversion circuit 110, a ping-pong register 120, a ping-pong register 130, a ping-pong register 140, a trajectory sequential controller (TSC) 150, a trajectory floating-point calculation circuit 160, a direct memory access (DMA) controller 170, and a trajectory buffer 180. Depending on different design requirements, the interface conversion circuit 110, the trajectory sequence controller 150, the trajectory floating-point calculation circuit 160, and / or the direct memory access controller 170 can be implemented in hardware, firmware, software, or a combination of these three methods.

[0038] In hardware form, the interface conversion circuit 110, trajectory sequence controller 150, trajectory floating-point calculation circuit 160, and / or direct memory access controller 170 can be implemented as logic circuits on an integrated circuit. The functions of the interface conversion circuit 110, trajectory sequence controller 150, trajectory floating-point calculation circuit 160, and / or direct memory access controller 170 can be implemented as hardware using hardware description languages ​​(such as Verilog HDL or VHDL) or other suitable programming languages. For example, the functions of the interface conversion circuit 110, trajectory sequence controller 150, trajectory floating-point calculation circuit 160, and / or direct memory access controller 170 can be implemented in various logic blocks, modules, and circuits within one or more controllers, microcontrollers, microprocessors, application-specific integrated circuits (ASICs), digital signal processors (DSPs), field programmable gate arrays (FPGAs), and / or other processing units.

[0039] In software and / or firmware form, the functions of the interface conversion circuit 110, trajectory sequence controller 150, trajectory floating-point calculation circuit 160, and / or direct memory access controller 170 can be implemented as programming codes. For example, the interface conversion circuit 110, trajectory sequence controller 150, trajectory floating-point calculation circuit 160, and / or direct memory access controller 170 can be implemented using general programming languages ​​(such as C, C++, or assembly language) or other suitable programming languages. The programming code can be recorded / stored in a non-transitory readable medium. In some embodiments, the non-transitory readable medium includes, for example, semiconductor memory, programmable logic circuitry, and / or storage devices. A central processing unit (CPU), controller, microcontroller, or microprocessor can read and execute the programming code from the non-transitory readable medium to implement the functions of the interface conversion circuit 110, trajectory sequence controller 150, trajectory floating-point calculation circuit 160, and / or direct memory access controller 170.

[0040] exist Figure 1 In the illustrated embodiment, interface conversion circuit 110 provides a conversion interface from the Advanced Peripheral Bus (APB) to the Configuration Space Bus (CSB). Interface conversion circuit 110 obtains configuration and control signals from external device 10 via the APB bus. Interface conversion circuit 110 is coupled to ping-pong registers 120, 130, and 140, which are respectively coupled to direct memory access controller 170, trajectory sequence controller 150, and trajectory floating-point calculation circuit 160. Interface conversion circuit 110 can selectively provide the configuration and control signals to at least one of ping-pong registers 120, 130, and 140 via the CSB bus. Therefore, external device 10 can set / control the operation of trajectory sequence controller 150, trajectory floating-point calculation circuit 160, and direct memory access controller 170 using the configuration and control signals.

[0041] exist Figure 1In the illustrated embodiment, the ping-pong register 140 is used to obtain configuration information and control signals from the external device 10 from the interface conversion circuit 110, and to provide the configuration information and control signals to the direct memory access controller 170. The configuration information includes (but is not limited to) the "number of path points". The control information includes (but is not limited to) "path point reading start". The ping-pong register 140 is coupled to the direct memory access controller 170. The direct memory access controller 170 is coupled to the trajectory buffer 180 and the external device 10. The direct memory access controller 170 includes a trajectory read direct memory access controller (TRDMA) and a trajectory write direct memory access controller (TWDMA). The trajectory read direct memory access controller (TRDMA) is used to read the coordinates of multiple path points from a first external device, while the trajectory write direct memory access controller (TWDMA) is used to provide the coordinates to a second external device. Depending on the actual design, in some embodiments, the first external device and the second external device may be different devices. In other embodiments (e.g.) Figure 1 In the illustrated embodiment, the first external device and the second external device can be the same external device 10.

[0042] The trajectory buffer 180 includes an assembly buffer (ABUFF) 181, an intermediate buffer (IBUFF) 182, and a delivery buffer (DBUFF) 183. A trajectory read direct memory access controller (TRDMA) is coupled to the assembly buffer 181. The TRDMA can read the coordinates of multiple path points from the external device 10 via the Advanced eXtensible Interface 4.0 (AXI4) bus and store these coordinates in the assembly buffer 181. The trajectory polynomial generation device 100 can calculate the coefficients of the curve (polynomial) between any two adjacent path points and then temporarily store these coefficients in the delivery buffer 183. A trajectory write direct memory access controller (TWDMA) is coupled to the delivery buffer 183. The TWDMA can read the coefficients of these curves from the delivery buffer 183 and then provide / return the coefficients temporarily stored in the delivery buffer 183 to the external device 10 via the AXI4 bus.

[0043] Equation 1 below is a polynomial example of a curve between any two adjacent path points. Where g... i (x) represents the y-coordinate of the i-th curve in the motion path, and x represents the x-coordinate of the i-th curve. i Ca represents the x-coordinate of the i-th path point in the motion path.i Cb represents the constant term coefficient of the i-th curve. i Cc represents the coefficient of the linear term of the i-th curve. i Cd represents the coefficient of the quadratic term of the i-th curve. i Let represent the coefficient of the cubic term of the i-th curve.

[0044] g i (x)=Ca i +Cb i *(x–x i )+Cc i *(x–x i ) 2 +Cd i *(x–x i ) 3 Equation 1

[0045] Ping-pong register 120 is used to obtain configuration information and control signals from external device 10 from interface conversion circuit 110. Configuration information includes (but is not limited to) the number of path points, boundary conditions and boundary situations for cubic spline interpolation, and / or other configuration information. Control signals include (but are not limited to) the start of trajectory sequence controller 150. Trajectory sequence controller 150 is coupled to ping-pong register 120 and trajectory floating-point calculation circuit 160. Trajectory sequence controller 150 can schedule trajectory floating-point calculation circuit 160 to perform a cubic spline interpolation algorithm to convert the coordinates of multiple path points into coefficients Ca of a curve (polynomial). i Cb i 、Cc i with cd i In detail, the trajectory sequence controller 150 can schedule the trajectory floating-point calculation circuit 160 to perform the following operations: calculate the x-axis step size between any two adjacent path points; use the x-axis step size to obtain multiple elements of the coefficient matrix in the linear equation system of the cubic spline interpolation algorithm; use the x-axis step size and the coordinates of these path points to obtain multiple elements of the constant vector in the linear equation system; use the coefficient matrix and the constant vector to obtain the quadratic difference vector in the linear equation system; and use the x-axis step size, the coordinates of the multiple path points, and the quadratic difference vector to calculate the coefficient Ca of the curve. i Cb i 、Cc i with cd i The trajectory sequence controller 150 can temporarily store the coefficients of these curves between any two adjacent path points in the delivery cache 183. The trajectory write direct memory access controller (TWDMA) can store the coefficients Ca in the delivery cache 183. i Cb i 、Cc iwith cd i Provide / return data to external device 10.

[0046] The linear equations of the cubic spline interpolation algorithm can be represented by Equation 2 below. Here, CM represents the coefficient matrix of the linear equations (Equation 3), MV represents the quadratic difference vector of the linear equations (Equation 4), and YV represents the constant vector of the linear equations (Equation 5). It is assumed here that the number of path points in the motion path is n+1, where n is any integer determined based on the actual application. The coefficient matrix CM is an n*n diagonal matrix, where a2, a3, ..., a... n-1 With a n The elements of the first diagonal of the coefficient matrix CM are b1, b2, b3, ..., b n-1 With b n The elements of the second diagonal of the coefficient matrix CM are c1, c2, c3, ..., c n-1 The elements on the third diagonal of the coefficient matrix CM are f1, f2, f3, ..., f n-1 with f n Let be the elements of the quadratic difference vector MV, where d1, d2, d3, ..., d n-1 with d n The elements are constant vector YV. The trajectory sequence controller 150 can schedule the trajectory floating-point calculation circuit 160 to prepare the coefficient matrix CM and the constant vector YV, and use the coefficient matrix CM and the constant vector YV to obtain the quadratic difference vector MV.

[0047] Equation 2: CM*MV=YV

[0048]

[0049]

[0050]

[0051] The linear equations (Equation 2) for the cubic spline interpolation algorithm can be determined according to the actual application conditions. For example, under the application conditions of free boundaries, the coefficient matrix CM (Equation 3) can be Equation 6 below, and the constant vector YV (Equation 5) can be Equation 7 below. Under the application conditions of fixed boundaries, the coefficient matrix CM (Equation 3) can be Equation 8 below, and the constant vector YV (Equation 5) can be Equation 9 below. Here, it is assumed that the motion path has the first path point (x-axis coordinates x1 and y-axis coordinates x1 and y1) to the (n+1)th path point (x-axis coordinates x1 and y-axis coordinates x2 and y2 respectively). n+1 With y n+1 ). The following uses x i With y iLet h1 represent the x-coordinate and y-coordinate of the i-th path point in the motion path. In equations 6 to 9, h1 to h... n This represents the x-axis step size between any two adjacent points on the motion path. For example, the i-th x-axis step size h on the motion path... i For x i+1 -x i In Equation 9, A and B are arbitrary real numbers determined according to the actual application conditions.

[0052]

[0053]

[0054]

[0055]

[0056] Ping-pong register 130 is used to obtain configuration information from external device 10 from interface conversion circuit 110. The configuration information includes (but is not limited to) "computation configuration information for each floating-point calculation unit in trajectory floating-point calculation circuit 160". Ping-pong register 130 is coupled to trajectory floating-point calculation circuit 160. Figure 1 In the illustrated embodiment, the trajectory floating-point calculation circuit 160 includes multiple floating-point multiply-accumulate calculation circuits 161 and multiple floating-point divider circuits 162. The number of floating-point multiply-accumulate calculation circuits 161 and the number of floating-point divider circuits 162 are related to the parallelism of the trajectory polynomial generation device 100. For example, assuming the parallelism of the trajectory polynomial generation device 100 is m, then the number of floating-point multiply-accumulate calculation circuits 161 is 2*m, and the number of floating-point divider circuits 162 is 2*m. The floating-point multiply-accumulate calculation circuits 161 are coupled to the trajectory sequence controller 150. The trajectory sequence controller 150 can schedule the floating-point multiply-accumulate calculation circuits 161 to calculate the x-axis step sizes h1 to h2. n And the elements of the coefficient matrix CM. The floating-point division circuit 162 is coupled to the trajectory sequence controller 150. The trajectory sequence controller 150 can schedule the floating-point division circuit 162 to calculate the elements d1~d of the constant vector YV. n .

[0057] The trajectory sequence controller 150 is coupled to the trajectory buffer 180 and the trajectory floating-point calculation circuit 160. The assembly buffer 181 of the trajectory buffer 180 can provide the coordinates of multiple path points to the trajectory sequence controller 150. The trajectory sequence controller 150 can schedule the trajectory floating-point calculation circuit 160 to perform a cubic spline interpolation algorithm to convert the coordinates of these path points into the coefficients Ca of multiple curves (polynomials). i Cb i 、Cc i with cdi In detail, the trajectory sequence controller 150 can schedule the trajectory floating-point calculation circuit 160 to perform the following operations: calculate the x-axis step size h1 to h2 between any two adjacent path points. n Use x-axis step size h1~h n Prepare a coefficient matrix CM and a constant vector YV with the coordinates of these path points. Use the coefficient matrix CM and the constant vector YV to calculate the elements f1 to f of the quadratic difference vector MV. n and using the x-axis step size h1 to h n The coordinates of these path points and the quadratic difference vector MV are used to calculate the coefficients Ca of these curves. i Cb i 、Cc i with cd i During the scheduling of the trajectory floating-point calculation circuit 160, if a calculation error occurs in the trajectory floating-point calculation circuit 160, the trajectory floating-point calculation circuit 160 can send an error interrupt signal to the external device 10. When the cubic spline interpolation algorithm completes one calculation, the trajectory sequence controller 150 sends a completion interrupt signal to the external device 10.

[0058] The trajectory cache 180 can provide the coordinates of multiple path points to the trajectory sequence controller 150, and temporarily store intermediate data from the cubic spline interpolation algorithm. Figure 1 In the illustrated embodiment, the trajectory cache includes an assembly cache 181, an intermediate cache 182, and a delivery cache 183. The assembly cache 181, intermediate cache 182, and delivery cache 183 are coupled to the trajectory sequence controller 150. The assembly cache 181 can provide the coordinates of multiple path points to the trajectory sequence controller 150. The intermediate cache 182 can temporarily store intermediate data from the cubic spline interpolation algorithm. The delivery cache 183 can receive the coefficient Ca of each curve output by the trajectory sequence controller 150. i Cb i 、Cc i with cd i .

[0059] This embodiment does not limit the implementation of the intermediate cache 182. For example (but not limited to), the intermediate cache 182 may include a first cache unit, a second cache unit, a third cache unit, a fourth cache unit, a fifth cache unit, and a sixth cache unit. The first cache unit of the intermediate cache 182 may temporarily store the elements a2 to a3 of the first diagonal of the coefficient matrix CM. n (Equation 3). The third cache unit of intermediate cache 182 can temporarily store the elements c1 to c2 on the third diagonal of the coefficient matrix CM. n-1 (Equation 3). The trajectory sequence controller 150 can control the x-axis step size h1 to h2 between any two adjacent path points.n As elements a2~a on the first diagonal of the coefficient matrix CM n And the elements c1 to c on the third diagonal n-1 (See Equation 6 or Equation 8 for details). The trajectory sequence controller 150 can schedule the trajectory floating-point calculation circuit 160 to use x-axis step sizes h1 to h2. n Calculate the elements b1 to b2 on the second diagonal. n (See Equation 6 or Equation 8 for details). The second cache unit of intermediate cache 182 can temporarily store the elements b1 to b2 on the second diagonal of the coefficient matrix CM. n (Equation 3). The trajectory sequence controller 150 can schedule the trajectory floating-point calculation circuit 160 to use x-axis step sizes h1 to h2. n Coordinates of multiple path points y1~y n+1 Calculate multiple quotients [(y2–y1) / h1]~[(y n+1 -y n ) / h n The trajectory sequence controller 150 can use these quotients [(y2–y1) / h1]~[(y n+1 -y n ) / h n Find the elements d1~d of the constant vector YV. n (See Equation 7 or Equation 9 for details). The fourth cache unit of intermediate cache 182 can temporarily store the multiple elements d1 to d2 of the constant vector YV. n (Equation 5). The trajectory sequence controller 150 can schedule the trajectory floating-point calculation circuit 160 to use the contents of the first cache unit, the second cache unit, the third cache unit, and the fourth cache unit to obtain the elements f1 to f2 of the quadratic difference vector MV. n (Equation 4). The sixth cache unit of intermediate cache 182 can temporarily store the elements f1 to f2 of the quadratic difference vector MV. n (Equation 4). The fifth cache unit of intermediate cache 182 can temporarily store the quotient [(y2–y1) / h1]~[(y n+1 -y n ) / h n The trajectory sequence controller 150 can schedule the trajectory floating-point calculation circuit 160 to multiplex these quotients [(y2–y1) / h1]~[(y n+1 -y n ) / h n The coefficient Ca of each curve is obtained. i Cb i 、Cc i with cd i .

[0060] Figure 2This is a circuit block diagram of a trajectory sequence controller 200 according to an embodiment of the present invention. A practical design, Figure 2 The trajectory sequence controller 200 shown can be used as Figure 1 The illustrated example is an implementation of the trajectory sequence controller 150. Figure 2 In the illustrated embodiment, the trajectory sequence controller 200 includes a trajectory sequence generator 210, a prepare circuit 220, a forward circuit 230, a backward circuit 240, and a generation circuit 250. Please refer to... Figure 1 and Figure 2 The preparation circuit 220, the forward circuit 230, the backward circuit 240, and the generation circuit 250 are coupled to the trajectory sequence generator 210 and the trajectory buffer 180.

[0061] Based on practical design, in some embodiments, the trajectory sequence generator 210 (trajectory sequence controller 150) can divide multiple path points into at least one batch for cubic spline interpolation algorithm according to the parallelism of the trajectory floating-point calculation circuit 160. The preparation circuit 220 can extract the x-axis and y-axis coordinates of multiple path points from the assembly cache 181 of the trajectory cache 180. Based on the activation of the trajectory sequence generator 210, the preparation circuit 220 can schedule the trajectory floating-point calculation circuit 160 to calculate the x-axis step size h1~h1. n The scheduling trajectory floating-point calculation circuit 160 uses x-axis step sizes h1 to h2. n Coordinates of multiple path points y1~y n+1 Prepare the coefficient matrix CM and the constant vector YV.

[0062] For example, assuming the parallelism of the trajectory polynomial generation device 100 is m, then each batch of these path points contains m path points. Figure 3 This is a schematic diagram illustrating the configuration of the assembly cache 181 of the trajectory cache 180 according to an embodiment of the present invention. Based on the parallelism m, the assembly cache 181 can be configured with m banks BANK1, BANK2, BANK3, ..., BANKm. The coordinates of the path points WP[1], WP[2], WP[3], ..., WP[m] of the first batch are stored in banks BANK1 to BANKm of the assembly cache 181 in a one-to-one manner, and the coordinates of the path points WP[m+1], WP[m+2], WP[m+3], ..., WP[2m] of the second batch are also stored in banks BANK1 to BANKm of the assembly cache 181 in a one-to-one manner, as follows. Figure 3As shown. The coordinates of the other path points can be deduced by referring to the relevant descriptions of path points WP[1]~WP[2m], so they will not be repeated here.

[0063] In any batch, the preparation of circuit 220 can involve multiple steps to calculate the x-axis step size h1 to h2. n The coefficient matrix CM and the constant vector YV. Here it is assumed that the preparation circuit 220 can perform the first to sixth steps in the current batch. In the first step, the preparation circuit 220 can schedule the floating-point multiply-accumulate calculation circuit 161 of the trajectory floating-point calculation circuit 160 to calculate the corresponding x-axis step length of multiple path points belonging to the current batch. For example, in the first batch, assuming that the number of floating-point multiply-accumulate calculation circuits 161 is m, the preparation circuit 220 can retrieve the coordinates of the path points WP[1] to WP[m] of the first batch (assuming to be the current batch) in parallel from the library BANK1 to BANKm of the assembly cache 181, and schedule the floating-point multiply-accumulate calculation circuit 161 to calculate the corresponding x-axis step length h1 to h2 belonging to the current batch. m These corresponding x-axis step sizes are used as the first part of the coefficient matrix CM, for example, as the partial elements a2 to a1 of the first diagonal of the coefficient matrix CM. m And some elements c1 to c on the third diagonal m The preparation circuit 220 can temporarily store the elements of the first and third diagonals of the coefficient matrix CM in the first and third cache units of the intermediate cache 182.

[0064] In the second step, the preparation circuit 220 can schedule the floating-point multiply-accumulate calculation circuit 161 to calculate multiple y-axis step sizes between any two adjacent path points in the current batch of multiple path points. For example, the i-th y-axis step size in the motion path is y i+1 -y i For example, in the first batch, assuming the number of floating-point multiply-accumulate calculation circuits 161 is m, the preparation circuit 220 can schedule the floating-point multiply-accumulate calculation circuits 161 to calculate the y-axis step size [y2–y1] to [y] of multiple path points belonging to the current batch. m+1 –y m ].

[0065] In the third step, the preparation circuit 220 can schedule the floating-point multiply-accumulate calculation circuit 161 to calculate the first multiple of each corresponding x-axis step size in the first step. For example, in the first batch, assuming the number of floating-point multiply-accumulate calculation circuits 161 is m, the preparation circuit 220 can schedule the floating-point multiply-accumulate calculation circuit 161 to calculate each corresponding x-axis step size h1 to h1 belonging to the current batch. m The first multiple of the value, for example, 2*h1 to 2*h mFurthermore, the preparation circuit 220 can also schedule the floating-point division circuit 162 of the trajectory floating-point calculation circuit 160 in the third step to calculate multiple quotients between these y-axis steps and these corresponding x-axis steps. For example, in the first batch, assuming the number of floating-point division circuits 162 is m, the preparation circuit 220 can schedule the floating-point division circuit 162 in the third step to calculate the y-axis steps [y2–y1] to [y2–y1] belonging to the current batch. m+1 –y m [and these corresponding x-axis step sizes h1 to h m Multiple quotients, such as [y2–y1] / h1~[y m+1 –y m ] / h m The preparation circuit 220 can temporarily store these quotients in the fifth cache unit of the intermediate cache 182. Besides being used in the later fifth step, the generation circuit 250 can also reuse these quotients to obtain the coefficient Ca of each curve. i Cb i 、Cc i with cd i .

[0066] In the fourth step, preparation circuit 220 can schedule floating-point multiply-accumulate circuit 161 to calculate the sum of any two adjacent multipliers in the first multiplier value from the third step. These sums can be used as the second part of the coefficient matrix CM. For example, in the first batch, assuming the number of floating-point multiply-accumulate circuits 161 is m, preparation circuit 220 can schedule floating-point multiply-accumulate circuits 161 to calculate the first multiplier value 2*h1 to 2*h belonging to the current batch. m The sum of any two adjacent multiples, for example, 2*[h1+h2]~2*[h m-1 +h m These sums are 2*[h1+h2]~2*[h m-1 +h m Elements b1 to b2 can be used as the second diagonal elements of the coefficient matrix CM. m The preparation circuit 220 can temporarily store the elements of the second diagonal of the coefficient matrix CM in the second cache unit of the intermediate cache 182.

[0067] In the fifth step, the preparation circuit 220 can schedule the floating-point multiply-accumulate calculation circuit 161 to calculate the difference between any two adjacent quotients from the quotients in the third step. For example, in the first batch, assuming the number of floating-point multiply-accumulate calculation circuits 161 is m, the preparation circuit 220 can schedule the floating-point multiply-accumulate calculation circuit 161 to calculate the quotients [y2–y1] / h1~[y m+1 –y m ] / h mThe difference between any two adjacent quotients, for example, {[y3–y2] / h2-[y2–y1] / h1}~{[y m+1 –y m ] / h m -[y m –y m-1 ] / h m-1}

[0068] In the sixth step, the preparation circuit 220 can schedule the floating-point multiply-accumulate calculation circuit 161 to calculate the second value of each of these differences from the fifth step, wherein these second values ​​can be used as elements d1 to d2 of the constant vector YV. n For example, in the first batch, assuming the number of floating-point multiply-accumulate calculation circuits 161 is m, the preparation circuit 220 can schedule the floating-point multiply-accumulate calculation circuits 161 to calculate the differences {[y3–y2] / h2-[y2–y1] / h1} belonging to the current batch to {[y3–y2] / h2-[y2–y1] / h1}~{y3–y2] / h1}~{y2 ... m+1 –y m ] / h m -[y m –y m-1 ] / h m-1 The second value of each of}, for example, 6*{[y3–y2] / h2-[y2–y1] / h1}~6*{[y m+1 –y m ] / h m -[y m –y m-1 ] / h m-1 These second-multiplied values ​​are 6*{[y3–y2] / h2-[y2–y1] / h1}~6*{[y m+1 –y m ] / h m -[y m –y m-1 ] / h m-1 Elements d1 to d2 can be used as constant vectors YV. m The preparation circuit 220 can convert the elements d1 to d2 of the constant vector YV. m It is temporarily stored in the fourth cache unit of intermediate cache 182.

[0069] After the sixth step is completed, the preparation operation for a batch (e.g., the first batch) is finished. At this time, the preparation circuit 220 can reply with a "complete" signal to the trajectory sequence generator 210. The trajectory sequence generator 210 can activate the preparation circuit 220 to retrieve the coordinates of the path points WP[m+1] to WP[2m] of the second batch in parallel from the libraries BANK1 to BANKm of the assembly cache 181, and the preparation circuit 220 can perform the preparation operation for the second batch (performing the first to sixth steps again). After the preparation circuit 220 completes the preparation operation for the current batch (e.g., the first batch), the trajectory sequence generator 210 can activate the forward circuit 230 to perform forward calculation on the current batch (the contents of the intermediate cache 182). Based on the activation of the trajectory sequence generator 210, the forward circuit 230 can schedule the trajectory floating-point calculation circuit 160 to use the coefficient matrix CM and the constant vector YV to calculate at least one element of the quadratic difference value vector MV.

[0070] The forward circuit 230 can perform multiple iterations to obtain at least one element of the quadratic difference vector MV. In the i-th iteration of the multiple iterations, the forward circuit 230 can schedule the trajectory floating-point calculation circuit 160 to calculate tmp = b. i –a i *c i-1 Where tmp is the intermediate number in the i-th iteration, and a i b is the i-th element of the first diagonal of the coefficient matrix CM (that is, the i-th data in the first cache unit of intermediate cache 182). i Let be the i-th element of the second diagonal of the coefficient matrix CM (that is, the i-th data in the second cache unit of intermediate cache 182), and c i-1 This is the (i-1)th element on the third diagonal of the coefficient matrix CM (i.e., the (i-1)th data in the third cache unit of intermediate cache 182). In the i-th iteration, the forward circuit 230 can schedule the trajectory floating-point calculation circuit 160 to calculate c. i =c i / tmp. Where c i This is the i-th element on the third diagonal of the coefficient matrix CM (i.e., the i-th data in the third cache unit of intermediate cache 182). In the i-th iteration, the forward circuit 230 can schedule the trajectory floating-point calculation circuit 160 to calculate d. i =[d i –a i *d i-1 ] / tmp. Where, d i-1 This refers to the (i-1)th element of the constant vector YV (i.e., the (i-1)th data in the fourth cache unit of intermediate cache 182), and d iIt is the i-th element of the constant vector YV (that is, the i-th data in the fourth cache unit of intermediate cache 182).

[0071] For example, the forward circuit 230 can perform the following pseudocode to obtain the last element f of the quadratic difference vector MV. n After completing the pseudocode, the forward circuit 230 can obtain the last element f of the quadratic difference value vector MV. n Temporarily stored in the sixth cache unit of intermediate cache 182.

[0072]

[0073]

[0074] After the forward circuit 230 completes its forward operation, it can send a "complete" signal to the trajectory sequence generator 210. Following this, the trajectory sequence generator 210 can activate the follow-up circuit 240 to perform follow-up operations on the contents of the intermediate buffer 182. Based on the activation of the trajectory sequence generator 210, the follow-up circuit 240 can schedule the trajectory floating-point calculation circuit 160 to use the coefficient matrix CM and the constant vector YV to calculate the remaining elements in the quadratic difference vector MV. The follow-up circuit 240 can perform multiple iterations to calculate the remaining elements in the quadratic difference vector MV. In the i-th iteration of these iterations, the follow-up circuit 240 can schedule the trajectory floating-point calculation circuit 160 to calculate f. i =d i -c i *f i+1 Among them, f i f is the i-th element of the quadratic difference vector MV. i+1 Let d be the (i+1)th element of the quadratic difference vector MV. i Let be the i-th element of the constant vector YV (i.e., the i-th data in the fourth cache unit of intermediate cache 182), and c i This is the i-th element on the third diagonal of the coefficient matrix CM (i.e., the i-th data in the third cache unit of intermediate cache 182). The subsequent circuit 240 can then use the i-th element f of the quadratic difference vector MV. i Temporarily stored in the sixth cache unit of intermediate cache 182.

[0075] For example, the follow-up circuit 240 can perform the following pseudocode to obtain the remaining elements f1 to f2 of the quadratic difference vector MV. n-1 After completing the pseudocode, the follow-up circuit 240 can extract the remaining elements f1 to f2 of the quadratic difference vector MV. n-1 Temporarily stored in the sixth cache unit of intermediate cache 182.

[0076]

[0077] After the follow-up circuit 240 completes its forward operation, it can send a "complete" signal to the trajectory sequence generator 210. After the follow-up circuit 240 completes its forward operation, the trajectory sequence generator 210 can activate the generation circuit 250 to perform coefficient generation operations on the contents of the intermediate buffer 182. Based on the activation of the trajectory sequence generator 210, the generation circuit 250 can schedule the trajectory floating-point calculation circuit 160 to use the quadratic difference value vector MV (i.e., the data in the sixth buffer unit of the intermediate buffer 182) to calculate the coefficient Ca of each curve. i Cb i 、Cc i with cd i When the cubic spline interpolation algorithm completes one calculation, the trajectory sequence generator 210 can output the coefficients Ca of each curve. i Cb i 、Cc i with cd i The delivery cache 183 is temporarily stored in the trajectory cache 180, and a completion interrupt signal is sent to the external device 10.

[0078] The generation circuit 250 can perform multiple iterations to calculate the coefficients of the curve. In the i-th iteration of the multiple iterations, the generation circuit 250 can calculate the following equations 10 to 13. In equation 10, the generation circuit 250 calculates the y-axis coordinate of the i-th path point. i Ca, the constant term coefficient of the i-th curve i The generation circuit 250 can schedule the trajectory floating-point calculation circuit 160 to calculate equation 11 as the coefficient Cb of the first-order term of the i-th curve. i Among them, y i+1 Let y be the y-coordinate of the (i+1)th path point. i h is the y-coordinate of the i-th path point. i f is the x-axis step size between the (i+1)th path point and the ith path point. i Let f be the i-th element of the quadratic difference vector MV (that is, the i-th data in the sixth cache unit of intermediate cache 182), and f i+1 This refers to the (i+1)th element of the quadratic difference vector MV (i.e., the (i+1)th data in the sixth cache unit of intermediate cache 182). In Equation 10, "(y i+1 –y i ) / h i"The intermediate data calculated by the preparation circuit 220 in the preparation operation (fourth step) (i.e., the data in the fifth cache unit of the intermediate cache 182) can be reused to reduce computational work. The generation circuit 250 can schedule the trajectory floating-point calculation circuit 160 to calculate Equation 12 as the quadratic coefficient Cc of the i-th curve." i The generation circuit 250 can schedule the trajectory floating-point calculation circuit 160 to calculate equation 13 as the coefficient Cd of the cubic term of the i-th curve. i .

[0079] Ca i =y i Equation 10

[0080] Cb i =[(y i+1 –y i ) / h i ]–[(h i / 2)*f i ]–[(h i / 6)*(f i+1 -f i Equation 11

[0081] Cc i =f i / 2 Equation 12

[0082] Cd i =(f i+1 -f i ) / (6*h i Equation 13

[0083] In summary, the trajectory polynomial generation device 100 can accelerate the generation of coefficients for multiple curves between any two adjacent path points using hardware, such as the constant term coefficient Ca of the i-th curve. i The coefficient of the linear term Cb i The coefficient of the quadratic term, Cc i With the coefficient of the cubic term Cd i Hardware can accelerate the generation of coefficients Ca. i Cb i 、Cc i with cd i This reduces the real-time computation delay in trajectory planning.

[0084] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A trajectory sequence controller for scheduling a trajectory floating-point calculation circuit to calculate the coefficients of the curve between any two adjacent path points among multiple path points of a motion path, characterized in that, The trajectory sequence controller includes: Trajectory sequence generator; A preparation circuit, coupled to the trajectory sequence generator, is used to schedule the trajectory floating-point calculation circuit to calculate the x-axis step size between any two adjacent path points among the plurality of path points based on the activation of the trajectory sequence generator, and to schedule the trajectory floating-point calculation circuit to prepare the coefficient matrix and constant vector in the linear equation system of the cubic spline interpolation algorithm using the x-axis step size and the coordinates of the plurality of path points; A forward circuit, coupled to the trajectory sequence generator, is used to schedule the trajectory floating-point calculation circuit to calculate at least one element of the quadratic difference value vector in the linear equation system based on the activation of the trajectory sequence generator; The subsequent circuitry, coupled to the trajectory sequence generator, is used to schedule the trajectory floating-point calculation circuitry, based on the activation of the trajectory sequence generator, to calculate the remaining elements in the quadratic difference value vector using the coefficient matrix and the constant vector; and A generation circuit, coupled to the trajectory sequence generator, is used to schedule the trajectory floating-point calculation circuit to calculate the coefficients of the curve using the x-axis step size, the coordinates of the plurality of path points, and the quadratic difference vector, based on the activation of the trajectory sequence generator. The trajectory floating-point calculation circuit includes: A floating-point multiply-accumulate calculation circuit is coupled to the trajectory sequence controller, wherein the trajectory sequence controller schedules the floating-point multiply-accumulate calculation circuit to calculate the x-axis step size and multiple elements of the coefficient matrix; and A floating-point division circuit is coupled to the trajectory sequence controller, wherein the trajectory sequence controller schedules the floating-point division circuit to calculate multiple elements of the constant vector.

2. The trajectory sequence controller according to claim 1, characterized in that, When the cubic spline interpolation algorithm completes one calculation, the trajectory sequence generator sends a completion interrupt signal to the external device.

3. The trajectory sequence controller according to claim 1, characterized in that, The preparation circuit extracts the coordinates of the plurality of path points from the trajectory cache.

4. The trajectory sequence controller according to claim 1, characterized in that, The trajectory sequence generator divides the multiple path points into at least one batch for the cubic spline interpolation algorithm based on the parallelism of the trajectory floating-point calculation circuit.

5. The trajectory sequence controller according to claim 4, characterized in that, In any of the at least one batch, the preparation circuit performs multiple steps to calculate the x-axis step size, the coefficient matrix, and the constant vector, wherein, In the first step of the plurality of steps, the preparation circuit schedules the trajectory floating-point calculation circuit to calculate a plurality of corresponding x-axis step lengths for the plurality of path points belonging to the current batch, wherein the plurality of corresponding x-axis step lengths are used as the first part of the coefficient matrix; In the second step of the plurality of steps, the preparation circuit schedules the trajectory floating-point calculation circuit to calculate the plurality of y-axis step lengths between any two adjacent path points in the plurality of path points belonging to the current batch. In the third step of the plurality of steps, the preparation circuit schedules the trajectory floating-point calculation circuit to calculate the first multiple value of each of the plurality of corresponding x-axis steps, and the preparation circuit schedules the trajectory floating-point calculation circuit to calculate the plurality of quotient values ​​of the plurality of y-axis steps and the plurality of corresponding x-axis steps. In the fourth step of the plurality of steps, the preparation circuit schedules the trajectory floating-point calculation circuit to calculate the sum of any two adjacent multiple values ​​in the first multiple value, wherein the sum value is used as the second part element of the coefficient matrix; In the fifth step of the plurality of steps, the preparation circuit schedules the trajectory floating-point calculation circuit to calculate the difference between any two adjacent quotients among the plurality of quotients, wherein the plurality of quotients are multiplexed by the generation circuit; and In the sixth step of the plurality of steps, the preparation circuit schedules the trajectory floating-point calculation circuit to calculate a second multiple value for each of the differences, wherein the second multiple value is used as an element of the constant vector.

6. The trajectory sequence controller according to claim 4, characterized in that, The forward circuit performs multiple iterations to obtain at least one element of the quadratic difference vector, wherein in the i-th iteration of the multiple iterations... The forward circuit schedules the trajectory floating-point calculation circuit to calculate tmp = b i –a i *c i-1 , where tmp is the intermediate number in the i-th iteration, a i b is the i-th element of the first diagonal of the coefficient matrix. i The i-th element of the second diagonal of the coefficient matrix, and c i-1 It is the (i-1)th element of the third diagonal of the coefficient matrix; The forward circuit schedules the trajectory floating-point calculation circuit to calculate c. i =c i / tmp, where c i The i-th element of the third diagonal of the coefficient matrix; as well as The forward circuit schedules the trajectory floating-point calculation circuit to calculate d. i =[d i –a i *d i-1 ] / tmp, where d i-1 Let be the (i-1)th element of the constant vector, and d i Let i be the i-th element of the constant vector.

7. The trajectory sequence controller according to claim 4, characterized in that, The subsequent circuit performs multiple iterations to calculate the remaining elements in the quadratic difference vector, wherein in the i-th iteration of the multiple iterations... The subsequent circuit schedules the trajectory floating-point calculation circuit to calculate f. i =d i -c i *f i+1 , where f i f is the i-th element of the quadratic difference value vector. i+1 Let d be the (i+1)th element of the quadratic difference value vector. i Let i be the i-th element of the constant vector, and c i It is the i-th element of the third diagonal of the coefficient matrix.

8. The trajectory sequence controller according to claim 4, characterized in that, The generation circuit performs multiple iterations to calculate the coefficients of the curve, wherein in the i-th iteration of the multiple iterations, The generation circuit uses the y-axis coordinate of the i-th path point as the constant term coefficient of the curve; The generation circuit schedules the trajectory floating-point calculation circuit to calculate [(y i+1 –y i ) / h i ]–[(h i / 2)*f i ]–[(h i / 6)*(f i+1 -f i )] is the coefficient of the first-order term of the curve, where y i+1 Let y be the y-coordinate of the (i+1)th path point. i h is the y-coordinate of the i-th path point. i f is the x-axis step size between the (i+1)th path point and the ith path point. i Let f be the i-th element of the quadratic difference value vector, and f i+1 It is the (i+1)th element of the quadratic difference value vector; The generation circuit schedules the trajectory floating-point calculation circuit to calculate f. i / 2 serves as the coefficient of the quadratic term of the curve; and The generation circuit schedules the trajectory floating-point calculation circuit to calculate (f i+1 -f i ) / (6*h i ) is used as the coefficient of the cubic term of the curve.

9. The trajectory sequence controller according to claim 1, characterized in that, The coefficient matrix is ​​a diagonal matrix.

10. A trajectory polynomial generation device for calculating the coefficients of a curve between any two adjacent path points among multiple path points of a motion path, characterized in that, The trajectory polynomial generation device includes: Trajectory floating-point calculation circuit; A trajectory cache, used to provide the coordinates of the plurality of path points; and A trajectory sequence controller, coupled to the trajectory buffer and the trajectory floating-point calculation circuit, is used to schedule the trajectory floating-point calculation circuit to perform a cubic spline interpolation algorithm to convert the coordinates of the plurality of path points into the coefficients of the curve. Specifically, the trajectory sequence controller schedules the trajectory floating-point calculation circuit to calculate the x-axis step size between any two adjacent path points; schedules the trajectory floating-point calculation circuit to prepare the coefficient matrix and constant vector in the linear equation system of the cubic spline interpolation algorithm using the x-axis step size and the coordinates of the plurality of path points; schedules the trajectory floating-point calculation circuit to calculate the elements of the quadratic difference vector in the linear equation system using the coefficient matrix and the constant vector; and schedules the trajectory floating-point calculation circuit to calculate the coefficients of the curve using the x-axis step size, the coordinates of the plurality of path points, and the quadratic difference vector. The trajectory floating-point calculation circuit includes: A floating-point multiply-accumulate calculation circuit is coupled to the trajectory sequence controller, wherein the trajectory sequence controller schedules the floating-point multiply-accumulate calculation circuit to calculate the x-axis step size and multiple elements of the coefficient matrix; and A floating-point division circuit is coupled to the trajectory sequence controller, wherein the trajectory sequence controller schedules the floating-point division circuit to calculate multiple elements of the constant vector. The trajectory sequence controller includes: Trajectory sequence generator; A preparation circuit, coupled to the trajectory sequence generator and the trajectory buffer, is used to schedule the trajectory floating-point calculation circuit to calculate the x-axis step size based on the activation of the trajectory sequence generator, and to schedule the trajectory floating-point calculation circuit to prepare the coefficient matrix and the constant vector using the x-axis step size and the coordinates of the plurality of path points; A forward circuit, coupled to the trajectory sequence generator and the trajectory buffer, is used to schedule the trajectory floating-point calculation circuit to calculate at least one element of the quadratic difference value vector using the coefficient matrix and the constant vector based on the activation of the trajectory sequence generator; The subsequent circuitry, coupled to the trajectory sequence generator and the trajectory buffer, is used to schedule the trajectory floating-point calculation circuitry to calculate the remaining elements in the quadratic difference value vector using the coefficient matrix and the constant vector based on the activation of the trajectory sequence generator; and A generation circuit, coupled to the trajectory sequence generator and the trajectory buffer, is used to schedule the trajectory floating-point calculation circuit to calculate the coefficients of the curve using the x-axis step size, the coordinates of the plurality of path points, and the quadratic difference vector based on the activation of the trajectory sequence generator.

11. The trajectory polynomial generation apparatus according to claim 10, characterized in that, When the cubic spline interpolation algorithm completes one calculation, the trajectory sequence generator sends a completion interrupt signal to the external device.

12. The trajectory polynomial generation apparatus according to claim 10, characterized in that, The preparation circuit extracts the coordinates of the plurality of path points from the trajectory cache.

13. The trajectory polynomial generation apparatus according to claim 10, characterized in that, The trajectory sequence generator divides the multiple path points into at least one batch for the cubic spline interpolation algorithm based on the parallelism of the trajectory floating-point calculation circuit.

14. The trajectory polynomial generation apparatus according to claim 13, characterized in that, In any of the at least one batch, the preparation circuit performs multiple steps to calculate the x-axis step size, the coefficient matrix, and the constant vector, wherein, In the first step of the plurality of steps, the preparation circuit schedules the trajectory floating-point calculation circuit to calculate a plurality of corresponding x-axis step lengths for the plurality of path points belonging to the current batch, wherein the plurality of corresponding x-axis step lengths are used as the first part of the coefficient matrix; In the second step of the plurality of steps, the preparation circuit schedules the trajectory floating-point calculation circuit to calculate the plurality of y-axis step lengths between any two adjacent path points in the plurality of path points belonging to the current batch. In the third step of the plurality of steps, the preparation circuit schedules the trajectory floating-point calculation circuit to calculate the first multiple value of each of the plurality of corresponding x-axis steps, and the preparation circuit schedules the trajectory floating-point calculation circuit to calculate the plurality of quotient values ​​of the plurality of y-axis steps and the plurality of corresponding x-axis steps. In the fourth step of the plurality of steps, the preparation circuit schedules the trajectory floating-point calculation circuit to calculate the sum of any two adjacent multiple values ​​in the first multiple value, wherein the sum value is used as the second part element of the coefficient matrix; In the fifth step of the plurality of steps, the preparation circuit schedules the trajectory floating-point calculation circuit to calculate the difference between any two adjacent quotients among the plurality of quotients, wherein the plurality of quotients are multiplexed by the generation circuit; and In the sixth step of the plurality of steps, the preparation circuit schedules the trajectory floating-point calculation circuit to calculate a second multiple value for each of the differences, wherein the second multiple value is used as an element of the constant vector.

15. The trajectory polynomial generation apparatus according to claim 13, characterized in that, The forward circuit performs multiple iterations to obtain at least one element of the quadratic difference vector, wherein in the i-th iteration of the multiple iterations... The forward circuit schedules the trajectory floating-point calculation circuit to calculate tmp = b i –a i *c i-1 , where tmp is the intermediate number in the i-th iteration, a i b is the i-th element of the first diagonal of the coefficient matrix. i The i-th element of the second diagonal of the coefficient matrix, and c i-1 It is the (i-1)th element of the third diagonal of the coefficient matrix; The forward circuit schedules the trajectory floating-point calculation circuit to calculate c. i =c i / tmp, where c i The i-th element of the third diagonal of the coefficient matrix; as well as The forward circuit schedules the trajectory floating-point calculation circuit to calculate d. i =[d i –a i *d i-1 ] / tmp, where d i-1 Let be the (i-1)th element of the constant vector, and d i Let i be the i-th element of the constant vector.

16. The trajectory polynomial generation apparatus according to claim 13, characterized in that, The subsequent circuit performs multiple iterations to calculate the remaining elements in the quadratic difference vector, wherein in the i-th iteration of the multiple iterations... The subsequent circuit schedules the trajectory floating-point calculation circuit to calculate f. i =d i -c i *f i+1 , where f i f is the i-th element of the quadratic difference value vector. i+1 Let d be the (i+1)th element of the quadratic difference value vector. i Let i be the i-th element of the constant vector, and c i It is the i-th element of the third diagonal of the coefficient matrix.

17. The trajectory polynomial generation apparatus according to claim 13, characterized in that, The generation circuit performs multiple iterations to calculate the coefficients of the curve, wherein in the i-th iteration of the multiple iterations, The generation circuit uses the y-axis coordinate of the i-th path point as the constant term coefficient of the curve; The generation circuit schedules the trajectory floating-point calculation circuit to calculate [(y i+1 –y i ) / h i ]–[(h i / 2)*m i ]–[(h i / 6)*(m i+1 -m i )] is the coefficient of the first-order term of the curve, where y i+1 Let y be the y-coordinate of the (i+1)th path point. i h is the y-coordinate of the i-th path point. i f is the x-axis step size between the (i+1)th path point and the ith path point. i Let f be the i-th element of the quadratic difference value vector, and f i+1 It is the (i+1)th element of the quadratic difference value vector; The generation circuit schedules the trajectory floating-point calculation circuit to calculate f. i / 2 serves as the coefficient of the quadratic term of the curve; and The generation circuit schedules the trajectory floating-point calculation circuit to calculate (f i+1 -f i ) / (6*h i ) is used as the coefficient of the cubic term of the curve.

18. The trajectory polynomial generation apparatus according to claim 10, characterized in that, The coefficient matrix is ​​a diagonal matrix.

19. The trajectory polynomial generation apparatus according to claim 10, characterized in that, When the trajectory floating-point calculation circuit experiences a calculation error, it sends an error interrupt signal to the external device.

20. The trajectory polynomial generation apparatus according to claim 10, characterized in that, The trajectory cache includes: An assembly buffer is coupled to the trajectory sequence controller to provide the coordinates of the plurality of path points to the trajectory sequence controller; An intermediate buffer, coupled to the trajectory sequence controller, is used to temporarily store intermediate data from the cubic spline interpolation algorithm; and A delivery buffer is coupled to the trajectory sequence controller to receive the coefficients of the curve.

21. The trajectory polynomial generation apparatus according to claim 20, characterized in that, The intermediate cache includes: The first cache unit is used to temporarily store the elements of the first diagonal of the coefficient matrix, wherein the trajectory sequence controller uses the x-axis step size between any two adjacent path points among the plurality of path points as the elements of the first diagonal and the third diagonal of the coefficient matrix. The second cache unit is used to temporarily store the elements of the second diagonal of the coefficient matrix, wherein the trajectory sequence controller schedules the trajectory floating-point calculation circuit to calculate the elements of the second diagonal using the x-axis step size; The third cache unit is used to temporarily store the elements of the third diagonal of the coefficient matrix; The fourth cache unit is used to temporarily store multiple elements of the constant vector, wherein the trajectory sequence controller schedules the trajectory floating-point calculation circuit to calculate multiple quotients using the x-axis step size and the coordinates of the multiple path points, and the trajectory sequence controller uses the multiple quotients to obtain the multiple elements of the constant vector; A fifth cache unit is used to temporarily store the plurality of quotient values, wherein the trajectory sequence controller schedules the trajectory floating-point calculation circuit to reuse the plurality of quotient values ​​to obtain the coefficients of the curve; and The sixth cache unit is used to temporarily store the elements of the quadratic difference vector.

22. The trajectory polynomial generation apparatus according to claim 20, characterized in that, The trajectory polynomial generation device further includes: A direct memory access controller, coupled to the trace cache and external devices, wherein the external devices include a first external device and a second external device, and the direct memory access controller includes: A trajectory reading direct memory access controller, coupled to the assembly cache, is used to read the coordinates of the plurality of path points from the first external device and store the coordinates in the assembly cache; and The trajectory is written to a direct memory access controller, coupled to the delivery cache, to read the coefficients of the curve from the delivery cache and provide the coefficients to the second external device.

23. The trajectory polynomial generation apparatus according to claim 22, characterized in that, The trajectory reading direct memory access controller reads the coordinates of the plurality of path points from the first external device via the AXI4 bus, and the trajectory writing direct memory access controller provides the coefficients to the second external device via the AXI4 bus.

24. The trajectory polynomial generation apparatus according to claim 22, characterized in that, The trajectory polynomial generation device further includes: The first ping-pong register is coupled to the trajectory floating-point calculation circuit; The second ping-pong register is coupled to the trajectory sequence controller; A third ping-pong register, coupled to the direct memory access controller; and An interface conversion circuit is coupled to the first ping-pong register, the second ping-pong register, and the third ping-pong register, wherein the interface conversion circuit obtains configuration signals and control signals from an external device via the APB bus, and the interface conversion circuit provides the configuration signals and control signals to at least one of the first ping-pong register, the second ping-pong register, and the third ping-pong register via the CSB bus.

Images