Trajectory polynomial generator

Abstract

The present invention provides a trajectory polynomial generation device, comprising a trajectory floating-point calculation circuit and a trajectory sequence controller. The trajectory sequence controller is coupled to the trajectory floating-point calculation circuit. The trajectory sequence controller performs a cubic spline interpolation algorithm to convert the coordinates of a plurality of path points into coefficients of a curve. The trajectory sequence controller schedules the trajectory floating-point calculation circuit to calculate the x-axis step length between any two adjacent path points among the plurality of path points, uses the x-axis step length to obtain multiple elements of a coefficient matrix in a linear equation system of the cubic spline interpolation algorithm, uses the x-axis step length and the coordinates of the plurality of path points to obtain multiple elements of a constant vector in the linear equation system, uses the coefficient matrix and the constant vector to obtain a quadratic difference value vector in the linear equation system, and uses the quadratic difference value vector to calculate the coefficients of the curve.

Description

Technical Field

[0001] The present invention relates to an electronic circuit, and in particular to a trajectory polynomial generating device. Background Art

[0002] During robot navigation, how to control the robot to move from point A to point B is usually called motion planning. Motion planning is generally divided into two steps: path planning and trajectory planning. Path planning refers to searching for a motion path from point A to point B in a map, where the motion path consists of a series of discrete waypoints. The waypoints of a motion path may be sparse (not smooth). In order to better control the movement of the robot, the sparse waypoints need to be converted 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 multiple waypoints, thereby reducing the real-time calculation delay in trajectory planning, is one of the many important topics in the field of robotics. Summary of the Invention

[0003] The present invention provides a trajectory polynomial generation device for calculating the coefficient of a curve between any two adjacent waypoints in a plurality of waypoints of a motion path.

[0004] In an embodiment according to the present invention, the trajectory polynomial generation device includes a trajectory floating-point calculation circuit and a trajectory sequential controller. The trajectory sequential controller is coupled to the trajectory floating-point calculation circuit. The trajectory sequential controller is used to schedule the trajectory floating-point calculation circuit to perform a cubic spline interpolation algorithm to convert the coordinates of multiple path points into coefficients of a curve. The trajectory sequential controller schedules the trajectory floating-point calculation circuit to calculate the x-axis step length between any two adjacent path points among the multiple path points, uses the x-axis step length to obtain multiple elements of the coefficient matrix in the linear equation group of the cubic spline interpolation algorithm, uses the x-axis step length and the coordinates of the multiple path points to obtain multiple elements of the constant vector in the linear equation group, uses the coefficient matrix and the constant vector to obtain a quadratic difference value vector in the linear equation group, and uses the x-axis step length, the coordinates of the multiple path points and the quadratic difference value vector to calculate the coefficients of the curve.

[0005] Based on the above, the trajectory polynomial generation device can use hardware to accelerate the generation of coefficients of multiple curves between any two adjacent path points, thereby reducing real-time calculation delays in trajectory planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0006] Figure 1 is a circuit block diagram of a trajectory polynomial generating device according to an embodiment of the present invention;

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

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

[0009] Description of Reference Numerals

[0010] 10: External devices

[0011] 100: Trajectory polynomial generation device

[0012] 110: Interface conversion circuit

[0013] 120, 130, 140: Ping-Pong registers

[0014] 150, 200: Trajectory sequence controller

[0015] 160: Trajectory floating-point calculation circuit

[0016] 161: Floating-point multiplication and addition circuit

[0017] 162: Floating-point division circuit

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

[0019] 180: Track Cache

[0020] 181: Assembly Cache

[0021] 182: Intermediate cache

[0022] 183: Delivery Cache

[0023] 210: Trajectory Sequence Generator

[0024] 220: Preparing the Circuit

[0025] 230: Forward Circuit

[0026] 240: Backward Circuit

[0027] 250: Generate Circuit

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

[0029] TRDMA: Trajectory Read Direct Memory Access Controller

[0030] TWDMA: Trace Write Direct Memory Access Controller

[0031] WP[1], WP[2], WP[3], WP[m], WP[m+1], WP[m+2], WP[m+3], WP[2m]: way point DETAILED DESCRIPTION

[0032] Reference will now be made in detail to exemplary embodiments of the present invention, examples of which are illustrated in the accompanying drawings. Whenever possible, the same reference numerals are used in the drawings and the description to refer to the same or like parts.

[0033] The term "coupled (or connected)" used in the entire specification of this case (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. mentioned in the entire specification of this case (including the claims) are used to name the components (element), and are not used to limit the upper or lower limit of the number of components, nor are they used to limit the order of components. In addition, wherever possible, components / members / steps with the same numbers in the drawings and embodiments represent the same or similar parts. Components / members / steps with the same numbers or the same terms in different embodiments can refer to the relevant descriptions of each other.

[0034] The cubic spline interpolation algorithm can be used for trajectory planning in autonomous driving algorithms. During motion, an unmanned vehicle (or robot) uses a map to calculate the shortest path to its destination (i.e., path planning). This path consists of a series of discrete waypoints. After completing path planning, the unmanned vehicle (or robot) performs interpolation to calculate the curve between any two adjacent waypoints. The present invention will illustrate, using 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 transmit these curve coefficients back to the external device. Therefore, the external device can use the coefficients generated by the trajectory polynomial generation device to create a polynomial between any two adjacent waypoints.

[0035] There are two types of calculation methods for cubic spline interpolation, one of which is to use the catch-up method. The catch-up method is a fast solution method designed for tridiagonal, pentadiagonal and other diagonal matrix linear equations. The second type is to use a method for solving a common matrix linear equation system. The method for solving a common matrix linear equation system is more complex and requires more complex constraints, resulting in the matrix equation system not forming a tridiagonal matrix. The following embodiments use a calculation method for the catch-up method, which can use fewer hardware resources and less calculation delay in the real-time operation process of cubic spline interpolation.

[0036] Figure 1 Figure 1 is a circuit block diagram of a trajectory polynomial generation device 100 according to one embodiment of the present invention. An external device 10 (e.g., a CPU) can perform path planning to generate multiple path points along a motion path, 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 a curve (polynomial) between any two adjacent path points along the motion path, and then transmit these curve coefficients back to the external device 10. Figure 1The trajectory polynomial generation device 100 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 sequential controller 150, the trajectory floating-point calculation circuit 160, and / or the DMA controller 170 can be implemented in hardware, firmware, software, or a combination of these.

[0037] In hardware, the interface conversion circuit 110, the trace sequence controller 150, the trace floating-point calculation circuit 160, and / or the direct memory access controller 170 can be implemented as logic circuits on an integrated circuit. The functions of the interface conversion circuit 110, the trace sequence controller 150, the trace floating-point calculation circuit 160, and / or the 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, the trace sequence controller 150, the trace floating-point calculation circuit 160, and / or the direct memory access controller 170 can be implemented as various logic blocks, modules, and circuits in 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.

[0038] In software and / or firmware form, the functions associated with the interface conversion circuit 110 , the trace sequence controller 150 , the trace floating-point calculation circuit 160 , and / or the direct memory access controller 170 can be implemented as programming codes. For example, the interface conversion circuit 110 , the trace sequence controller 150 , the trace floating-point calculation circuit 160 , and / or the direct memory access controller 170 can be implemented using a common programming language (e.g., C, C++, or assembly language) or other suitable programming language. The programming codes can be recorded / stored in a non-transitory readable medium. In some embodiments, the non-transitory readable medium includes, for example, a semiconductor memory, a programmable logic circuit, and / or a storage device. A central processing unit (CPU), a controller, a microcontroller, or a microprocessor can read and execute the programming codes from the non-transitory readable medium, thereby implementing the functions associated with the interface conversion circuit 110 , the trace sequence controller 150 , the trace floating-point calculation circuit 160 , and / or the direct memory access controller 170 .

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

[0040] 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) "the start of path point reading". The ping-pong register 140 is coupled to the direct memory access controller 170. The direct memory access controller 170 is coupled to the trace cache 180 and the external device 10. The direct memory access controller 170 includes a trace read direct memory access controller TRDMA and a trace write direct memory access controller TWDMA. The trace read direct memory access controller TRDMA is used to read the coordinates of multiple path points from a first external device, and the trace write direct memory access controller TWDMA is used to provide the coefficients to a second external device. According to the actual design, in some embodiments, the first external device and the second external device may be different devices. In other embodiments (for example Figure 1 In the embodiment shown in FIG, the first external device and the second external device may be the same external device 10.

[0041] 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 trajectory read direct memory access controller 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 path point coordinates in the assembly buffer 181. The trajectory polynomial generation device 100 can calculate the coefficients of a curve (polynomial) between any two adjacent path points and temporarily store these curve coefficients in the delivery buffer 183. A trajectory write direct memory access controller TWDMA is coupled to the delivery buffer 183. The trajectory write direct memory access controller TWDMA can read these curve coefficients from the delivery buffer 183 and then provide / transmit the coefficients temporarily stored in the delivery buffer 183 to / from the external device 10 via the AXI4 bus.

[0042] The following equation 1 is an example of a polynomial curve between any two adjacent path points. i (x) represents the y-axis coordinate of the i-th curve in the motion path, x represents the x-axis coordinate of the i-th curve, and x i Indicates the x-axis coordinate of the i-th path point in the motion path, Cai Represents the constant coefficient of the i-th curve, Cb i Represents the linear coefficient of the i-th curve, Cc i represents the quadratic coefficient of the i-th curve, Cd i Represents the cubic coefficient of the i-th curve.

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

[0044] The ping-pong register 120 is used to obtain configuration information and control signals from the external device 10 from the interface conversion circuit 110. The configuration information includes (but is not limited to) "the number of path points", "the boundary conditions and boundary cases of the cubic spline interpolation" and (or) other configuration information. The control signal includes (but is not limited to) "the start of the trajectory sequence controller 150". The trajectory sequence controller 150 is coupled to the ping-pong register 120 and the trajectory floating-point calculation circuit 160. 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 multiple path points into the coefficients Ca of the 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 length between any two adjacent path points, use the x-axis step length to obtain multiple elements of the coefficient matrix in the linear equation system of the cubic spline interpolation algorithm, use the x-axis step length 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 value vector in the linear equation system, and use the x-axis step length, the coordinates of the multiple path points and the quadratic difference value vector to calculate the coefficient Ca of the curve i 、Cb i 、Cc i with cd i The trajectory sequence controller 150 may temporarily store the coefficients of these curves between any two adjacent path points in the delivery buffer 183. The trajectory write direct memory access controller TWDMA may temporarily store the coefficients Ca in the delivery buffer 183. i 、Cb i 、Cc iwith cd i Provided / transmitted back to the external device 10.

[0045] The linear equations of the cubic spline interpolation algorithm can be the following equation 2. Wherein, CM represents the coefficient matrix in the linear equations (Equation 3), MV represents the quadratic difference value vector in the linear equations (Equation 4), and YV represents the constant vector in the linear equations (Equation 5). It is assumed here that the number of path points of the motion path is n+1, where n is an arbitrary integer determined according to the actual application. The coefficient matrix CM is an n*n diagonal matrix, where a2, a3, ..., a n-1 with a n are the elements of the first diagonal of the coefficient matrix CM, b1, b2, b3, ..., b n-1 with b n are the elements of the second diagonal of the coefficient matrix CM, c1, c2, c3, ..., c n-1 are the elements on the third diagonal of the coefficient matrix CM, f1, f2, f3, ..., f n-1 With f n are the elements of the quadratic difference vector MV, and d1, d2, d3, ..., d n-1 with d n 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 value vector MV.

[0046] CM*MV=YV Equation 2

[0047]

[0048]

[0049]

[0050] The linear equations (Equation 2) of the cubic spline interpolation algorithm can be determined according to the actual application conditions. For example, under the application condition of free boundary, the coefficient matrix CM (Equation 3) can be the following Equation 6, and the constant vector YV (Equation 5) can be the following Equation 7. Under the application condition of fixed boundary, the coefficient matrix CM (Equation 3) can be the following Equation 8, and the constant vector YV (Equation 5) can be the following Equation 9. It is assumed here that the motion path has the 1st path point (x-axis coordinate and y-axis coordinate are x1 and y1) to the n+1th path point (x-axis coordinate and y-axis coordinate are x n+1 with y n+1 ). The following is x i with y iIn equations 6 to 9, h1 to h n Indicates the x-axis step length between any two adjacent path points in the motion path. For example, the i-th x-axis step length h in the motion path i is x i+1 -x i A and B in Equation 9 are arbitrary real numbers determined according to actual application conditions.

[0051]

[0052]

[0053]

[0054]

[0055] The ping-pong register 130 is used to obtain configuration information from the external device 10 from the interface conversion circuit 110. The configuration information includes (but is not limited to) "the calculation configuration information of each floating-point calculation unit in the trace floating-point calculation circuit 160". The ping-pong register 130 is coupled to the trace floating-point calculation circuit 160. Figure 1 In the illustrated embodiment, the trajectory floating-point calculation circuit 160 includes a plurality of floating-point multiplication-addition calculation circuits 161 and a plurality of floating-point division circuits 162. The number of floating-point multiplication-addition calculation circuits 161 and the number of floating-point division circuits 162 are related to the computational parallelism of the trajectory polynomial generation device 100. For example, assuming that the parallelism of the trajectory polynomial generation device 100 is m, the number of floating-point multiplication-addition calculation circuits 161 is 2*m, and the number of floating-point division circuits 162 is 2*m. The floating-point multiplication-addition calculation circuits 161 are coupled to the trajectory sequence controller 150. The trajectory sequence controller 150 can schedule the floating-point multiplication-addition calculation circuits 161 to calculate the x-axis step lengths 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 n .

[0056] 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 multiple curve (polynomial) coefficients Ca i 、Cb i 、Cc i with cdi Specifically, the trajectory sequence controller 150 can schedule the trajectory floating point calculation circuit 160 to perform the following operations: calculating the x-axis step lengths h1 to h2 between any two adjacent path points. n , use x-axis step size h1~h n Prepare the coefficient matrix CM and constant vector YV with the coordinates of these path points, and use the coefficient matrix CM and constant vector YV to calculate the elements f1 to f2 of the quadratic difference vector MV. n , and use the x-axis step h1~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 process of scheduling the trajectory floating-point calculation circuit 160, when a calculation exception occurs in the trajectory floating-point calculation circuit 160, the trajectory floating-point calculation circuit 160 may send an exception interrupt signal to the external device 10. When the cubic spline interpolation algorithm completes a calculation, the trajectory sequence controller 150 sends a completion interrupt signal to the external device 10.

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

[0058] This embodiment does not limit the implementation of the intermediate buffer 182. For example (but not limited to this), the intermediate buffer 182 may include a first buffer unit, a second buffer unit, a third buffer unit, a fourth buffer unit, a fifth buffer unit, and a sixth buffer unit. The first buffer unit of the intermediate buffer 182 may temporarily store the elements a2 to a1 of the first diagonal of the coefficient matrix CM. n (Equation 3) The third buffer unit of the intermediate buffer 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 set the x-axis step length h1 to h2 between any two adjacent path points.n As the elements a2 to a2 on the first diagonal of the coefficient matrix CM n And the elements c1 to c2 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 the x-axis step lengths h1 to h n Calculate the elements b1 to b1 on the second diagonal n (See Equation 6 or Equation 8 for details.) The second buffer unit of the intermediate buffer 182 can temporarily store the elements b1 to b2 of 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 the x-axis step size h1~h n and the coordinates y1~y of multiple path points n+1 Calculate multiple quotients [(y2–y1) / h1] to [(y n+1 -y n ) / h n ]. The trajectory sequence controller 150 can use these quotient values ​​[(y2−y1) / h1] to [(y n+1 -y n ) / h n ] Obtain the elements d1 to d2 of the constant vector YV n (See Equation 7 or Equation 9 for details). The fourth buffer unit of the intermediate buffer 182 can temporarily store the multiple elements d1-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 buffer unit, the second buffer unit, the third buffer unit, and the fourth buffer unit to obtain the elements f1 to f2 of the secondary difference value vector MV. n (Equation 4) The sixth buffer unit of the intermediate buffer 182 can temporarily store the elements f1 to f2 of the secondary differential value vector MV. n (Equation 4). The fifth buffer unit of the intermediate buffer 182 can temporarily store the quotient values ​​[(y2-y1) / h1] to [(y n+1 -y n ) / h n ]. The trace sequence controller 150 can schedule the trace floating point calculation circuit 160 to multiplex these quotient values ​​[(y2−y1) / h1] to [(y n+1 -y n ) / h n ] Obtain the coefficient Ca of each curve i 、Cb i 、Cc i with cd i .

[0059] Figure 2FIG. 1 is a circuit block diagram of a trajectory sequence controller 200 according to an embodiment of the present invention. Figure 2 The trajectory sequence controller 200 shown can be used as Figure 1 An example implementation of the trajectory sequence controller 150 is shown. 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. 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 trace sequence generator 210 and the trace buffer 180 .

[0060] Based on the actual design, in some embodiments, the trajectory sequence generator 210 (trajectory sequence controller 150) can divide the multiple path points into at least one batch to perform the 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 coordinates and y-axis coordinates of the multiple path points from the assembly buffer 181 of the trajectory buffer 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 lengths h1 to h2. n , the scheduling trajectory floating point calculation circuit 160 uses the x-axis step size h1~h n and the coordinates y1~y of multiple path points n+1 Prepare the coefficient matrix CM and the constant vector YV.

[0061] For example, assuming that the parallelism of the trajectory polynomial generating apparatus 100 is m, each batch of the waypoints includes m waypoints. Figure 3 FIG. 1 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 degree of parallelism m, the assembly cache 181 can be configured with m banks BANK1, BANK2, BANK3, ..., BANKm. The coordinates of the first batch of waypoints WP[1], WP[2], WP[3], ..., WP[m] are stored in the banks BANK1 to BANKm of the assembly cache 181 in a one-to-one manner, and the coordinates of the second batch of waypoints WP[m+1], WP[m+2], WP[m+3], ..., WP[2m] are also stored in the banks BANK1 to BANKm of the assembly cache 181 in a one-to-one manner, as shown in FIG. Figure 3The coordinates of the remaining path points can refer to the relevant descriptions of path points WP[1] to WP[2m] and be deduced by analogy, so they will not be repeated here.

[0062] In any batch, the circuit 220 can be prepared in multiple steps to calculate the x-axis step lengths h1 to h n , coefficient matrix CM and constant vector YV. It is assumed here 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 multiplication and addition calculation circuit 161 of the trajectory floating-point calculation circuit 160 to calculate the corresponding x-axis step lengths of multiple path points belonging to the current batch. For example, in the first batch, assuming that the number of floating-point multiplication and addition calculation circuits 161 is m, the preparation circuit 220 can parallelly access the coordinates of the path points WP[1]~WP[m] of the first batch (assuming it is the current batch) from the banks BANK1~BANKm of the assembly cache 181, and schedule the floating-point multiplication and addition calculation circuit 161 to calculate the corresponding x-axis step lengths h1~h m These corresponding x-axis steps are used as the first part of the elements of the coefficient matrix CM, for example, as the first diagonal part of the elements a2 to a m and some elements c1 to c2 on the third diagonal m The preparation circuit 220 may temporarily store the elements of the first diagonal and the elements of the third diagonal of the coefficient matrix CM in the first and third cache units of the intermediate cache 182 .

[0063] In the second step, the preparation circuit 220 can schedule the floating-point multiplication and addition circuit 161 to calculate multiple y-axis steps between any two adjacent path points in the current batch of path points. For example, the i-th y-axis step in the motion path is y i+1 -y i For example, in the first batch, assuming that the number of floating-point multiplication-addition calculation circuits 161 is m, the preparation circuit 220 can schedule the floating-point multiplication-addition calculation circuits 161 to calculate the y-axis step lengths [y2–y1] to [y m+1 –y m ].

[0064] In the third step, the preparation circuit 220 may schedule the floating-point multiplication and addition circuit 161 to calculate the first multiple value of each corresponding x-axis step size of the first step. For example, in the first batch, assuming that the number of floating-point multiplication and addition circuits 161 is m, the preparation circuit 220 may schedule the floating-point multiplication and addition circuit 161 to calculate each corresponding x-axis step size h1~h m The first multiple value of , for example 2*h1~2*h mIn addition, the preparation circuit 220 may also schedule the floating-point division circuit 162 of the trajectory floating-point calculation circuit 160 to calculate multiple quotients of these y-axis steps and these corresponding x-axis steps in the third step. For example, in the first batch, assuming that the number of floating-point division circuits 162 is m, the preparation circuit 220 may schedule the floating-point division circuit 162 to calculate the y-axis steps [y2–y1] to [y2–y1] belonging to the current batch in the third step. m+1 –y m ] and these corresponding x-axis step lengths h1~h m Multiple quotient values, such as [y2–y1] / h1~[y m+1 –y m ] / h m The preparation circuit 220 can temporarily store these quotient values ​​in the fifth buffer unit of the intermediate buffer 182. In addition to being used in the fifth step later, the generation circuit 250 can also demultiplex these quotient values ​​to obtain the coefficients Ca of each curve. i 、Cb i 、Cc i with cd i .

[0065] In the fourth step, the preparation circuit 220 can schedule the floating-point multiplication and addition calculation circuit 161 to calculate the sum of any two adjacent multiple values ​​of the first multiple value in the third step. These sums can be used as the second part of the elements of the coefficient matrix CM. For example, in the first batch, assuming that the number of floating-point multiplication and addition calculation circuits 161 is m, the preparation circuit 220 can schedule the floating-point multiplication and addition calculation circuit 161 to calculate the first multiple values ​​2*h1~2*h belonging to the current batch. m The sum of any two adjacent multiple values, such as 2*[h1+h2]~2*[h m-1 +h m These sums are 2*[h1+h2]~2*[h m-1 +h m ] can be used as the elements b1~b1 on the second diagonal of the coefficient matrix CM m The preparation circuit 220 may temporarily store the elements of the second diagonal of the coefficient matrix CM in the second cache unit of the intermediate cache 182 .

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

[0067] In the sixth step, the preparation circuit 220 can schedule the floating-point multiplication and addition calculation circuit 161 to calculate the second multiple value of each of the difference values ​​in the fifth step, wherein the second multiple values ​​can be used as the elements d1 to d n For example, in the first batch, assuming that the number of floating-point multiplication and addition circuits 161 is m, the preparation circuit 220 can schedule the floating-point multiplication and addition circuits 161 to calculate the differences {[y3–y2] / h2-[y2–y1] / h1}~{[y m+1 –y m ] / h m -[y m –y m-1 ] / h m-1}, for example, 6*{[y3–y2] / h2-[y2–y1] / h1} to 6*{[y m+1 –y m ] / h m -[y m –y m-1 ] / h m-1}. These second multiple values ​​6*{[y3–y2] / h2-[y2–y1] / h1}~6*{[y m+1 –y m ] / h m -[y m –y m-1 ] / h m-1} can be used as the elements d1~d of the constant vector YV m The preparation circuit 220 can convert the elements d1 to d m Temporarily stored in the fourth cache unit of the intermediate cache 182 .

[0068] After the sixth step is completed, the preparation operation for a batch (e.g., the first batch) is complete. The preparation circuit 220 can then send a "complete" signal back to the trajectory sequence generator 210. The trajectory sequence generator 210 can then activate the preparation circuit 220 to concurrently retrieve the coordinates of the second batch's waypoints WP[m+1]-WP[2m] from banks BANK1-BANKm of the assembly buffer 181. The preparation circuit 220 can then perform preparation operations on the second batch (repeating steps 1 through 6). 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 operations on the current batch (the contents of the intermediate buffer 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.

[0069] The forward circuit 230 may perform multiple iterations to obtain at least one element of the secondary difference value vector MV. In the i-th iteration of the multiple iterations, the forward circuit 230 may schedule the trace floating point calculation circuit 160 to calculate tmp=b i –a i *c i-1 . Among them, tmp is the middle number in the i-th iteration, a i is the i-th element of the first diagonal of the coefficient matrix CM (i.e., the i-th data of the first buffer unit of the intermediate buffer 182), b i is the i-th element of the second diagonal of the coefficient matrix CM (ie, the i-th data of the second buffer unit of the intermediate buffer 182), and c i-1 is the i-1th element of the third diagonal of the coefficient matrix CM (ie, the i-1th data of the third cache unit of the intermediate cache 182). In the i-th iteration, the forward circuit 230 can schedule the trace floating-point calculation circuit 160 to calculate c i =c i / tmp. Among them, c i is the i-th element of the third diagonal of the coefficient matrix CM (ie, the i-th data of the third cache unit of the intermediate cache 182). In the i-th iteration, the forward circuit 230 can schedule the trace floating-point calculation circuit 160 to calculate d i =[d i –a i *d i-1 ] / tmp. Among them, d i-1 is the i-1th element of the constant vector YV (ie, the i-1th data of the fourth cache unit of the intermediate cache 182), and d iis the i-th element of the constant vector YV (ie, the i-th data of the fourth cache unit of the intermediate cache 182).

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

[0071]

[0072] After the forward circuit 230 completes the forward operation, the forward circuit 230 may reply a "complete" signal to the trace sequence generator 210. After the forward circuit 230 completes the forward operation, the trace sequence generator 210 may activate the backward circuit 240 to perform a backward operation on the contents of the intermediate buffer 182. Based on the activation of the trace sequence generator 210, the backward circuit 240 may schedule the trace floating-point calculation circuit 160 to calculate the remaining elements in the secondary differential value vector MV using the coefficient matrix CM and the constant vector YV. The backward circuit 240 may perform multiple iterations to calculate the remaining elements in the secondary differential value vector MV. In the i-th iteration of the multiple iterations, the backward circuit 240 may schedule the trace floating-point calculation circuit 160 to calculate f i =d i –c i *f i+1 Among them, f i is the i-th element of the quadratic difference vector MV, f i+1 is the i+1th element of the quadratic difference vector MV, d i is the i-th element of the constant vector YV (ie, the i-th data of the fourth cache unit of the intermediate cache 182), and c i is the i-th element of the third diagonal of the coefficient matrix CM (ie, the i-th data of the third buffer unit of the intermediate buffer 182). The backward circuit 240 can convert the i-th element f of the secondary difference value vector MV into i Temporarily stored in the sixth cache unit of the intermediate cache 182 .

[0073] For example, the backward circuit 240 can perform the following pseudo code to obtain the remaining elements f1-f n-1 After completing the pseudo code, the backward circuit 240 can convert the remaining elements f1 to f n-1 Temporarily store in the sixth cache unit of the intermediate cache 182. for (i=n-1; i>0; i--);

[0074] {

[0075] f i =d i –c i *f i+1 ;

[0076] }

[0077] After the backward circuit 240 completes the forward operation, the backward circuit 240 can return a "complete" signal to the trajectory sequence generator 210. After the backward circuit 240 completes the forward operation, the trajectory sequence generator 210 can activate the generation circuit 250 to perform coefficient generation operation 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 secondary differential value vector MV (i.e., the data of 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 a calculation, the trajectory sequence generator 210 can calculate the coefficients of each curve Ca i 、Cb i 、Cc i with cd i The data is temporarily stored in the delivery buffer 183 of the trajectory buffer 180 , and a completion interrupt signal is sent to the external device 10 .

[0078] The generating circuit 250 may perform a plurality of iterations to calculate the coefficients of the curve. In the i-th iteration of the plurality of iterations, the generating circuit 250 may calculate the following equations 10 to 13. In equation 10, the generating circuit 250 uses the y-axis coordinate y of the i-th path point as i As the constant coefficient Ca of the i-th curve i The generating circuit 250 can schedule the trajectory floating point calculation circuit 160 to calculate Equation 11 as the linear coefficient Cb of the i-th curve. i Among them, y i+1 is the y-axis coordinate of the i+1th path point, y i is the y-axis coordinate of the i-th path point, h i is the x-axis step length between the i+1th path point and the i-th path point, f i is the i-th element of the secondary differential value vector MV (ie, the i-th data of the sixth buffer unit of the intermediate buffer 182), and f i+1 is the i+1th element of the secondary differential value vector MV (ie, the i+1th data of the sixth cache unit of the intermediate cache 182). In equation 10, “(y i+1 –yi ) / h i The intermediate data calculated by the preparation circuit 220 in the preparation operation (the fourth step) (i.e., the data of the fifth buffer unit of the intermediate buffer 182) can be reused to reduce the computational work. The generation circuit 250 can schedule the trajectory floating-point calculation circuit 160 to calculate Equation 12 as the quadratic term coefficient Cc of the i-th curve. i The generating circuit 250 can schedule the trajectory floating point calculation circuit 160 to calculate Equation 13 as the cubic coefficient Cd 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 of multiple curves between any two adjacent path points using hardware, such as the constant term coefficient Ca of the i-th curve. i , linear coefficient Cb i , quadratic coefficient Cc i and the cubic coefficient Cd i Hardware can accelerate the generation of coefficients Ca i 、Cb i 、Cc i with cd i , thereby reducing real-time computation delays 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, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements 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 polynomial generation device for calculating the coefficients of a curve between any two adjacent path points among a plurality of path points of a motion path, characterized in that: The trajectory polynomial generating device comprises: Trajectory floating-point calculation circuit; and a trajectory sequence controller, coupled to the trajectory floating-point calculation circuit, for scheduling the trajectory floating-point calculation circuit to perform a cubic spline interpolation algorithm to convert the coordinates of the multiple path points into the coefficients of the curve, wherein the trajectory sequence controller schedules the trajectory floating-point calculation circuit to calculate the x-axis step length between any two adjacent path points among the multiple path points, uses the x-axis step length to obtain multiple elements of the coefficient matrix in the linear equation group of the cubic spline interpolation algorithm, uses the x-axis step length and the coordinates of the multiple path points to obtain multiple elements of the constant vector in the linear equation group, uses the coefficient matrix and the constant vector to obtain a quadratic difference value vector in the linear equation group, and uses the x-axis step length, the coordinates of the multiple path points, and the quadratic difference value vector to calculate the coefficients of the curve, Wherein, the trajectory floating-point calculation circuit includes: a floating-point multiplication-addition calculation circuit coupled to the trajectory sequence controller, wherein the trajectory sequence controller schedules the floating-point multiplication-addition calculation circuit to calculate the x-axis step size and the plurality of elements of the coefficient matrix; and A floating-point division circuit is coupled to the trace sequence controller, wherein the trace sequence controller schedules the floating-point division circuit to calculate the plurality of elements of the constant vector.

2. The trajectory polynomial generating device according to claim 1, characterized in that The coefficient matrix is ​​a diagonal matrix.

3. The trajectory polynomial generating device according to claim 1, wherein: When a calculation exception occurs in the trajectory floating-point calculation circuit, the trajectory floating-point calculation circuit sends an abnormal interrupt signal to an external device.

4. The trajectory polynomial generating device according to claim 1, wherein: When the cubic spline interpolation algorithm completes one calculation, the trajectory sequence controller sends a completion interrupt signal to an external device.

5. The trajectory polynomial generating device according to claim 1, wherein: The trajectory sequence controller divides the plurality of path points into at least one batch according to the parallelism of the trajectory floating-point calculation circuit to perform the cubic spline interpolation algorithm.

6. The trajectory polynomial generating device according to claim 1, wherein: The trajectory polynomial generating device further comprises: The trajectory buffer is coupled to the trajectory sequence controller and is used to provide the coordinates of the plurality of path points to the trajectory sequence controller and temporarily store intermediate data of the cubic spline interpolation algorithm.

7. The trajectory polynomial generating device according to claim 6, characterized in that: The trajectory cache includes: an assembly buffer, coupled to the trajectory sequence controller, for providing the coordinates of the plurality of path points to the trajectory sequence controller; an intermediate buffer, coupled to the trajectory sequence controller, for temporarily storing the intermediate data of the cubic spline interpolation algorithm; and A delivery buffer is coupled to the trajectory sequence controller to receive the coefficients of the curve.

8. The trajectory polynomial generating device according to claim 7, characterized in that: The intermediate cache includes: a first cache unit for temporarily storing elements of a first diagonal line of the coefficient matrix, wherein the trajectory sequence controller uses the x-axis step length between any two adjacent path points among the plurality of path points as the elements of the first diagonal line and the elements of a third diagonal line in the coefficient matrix; a second cache unit for temporarily storing elements of a second diagonal line of the coefficient matrix, wherein the trajectory sequence controller schedules the trajectory floating-point calculation circuit to calculate the elements of the second diagonal line using the x-axis step size; a third cache unit, configured to temporarily store the elements of the third diagonal of the coefficient matrix; a fourth cache unit for temporarily storing the plurality of elements of the constant vector, wherein the trajectory sequence controller schedules the trajectory floating-point calculation circuit to calculate a plurality of quotients using the x-axis step length and the coordinates of the plurality of path points, and the trajectory sequence controller uses the plurality of quotients to obtain the plurality of elements of the constant vector; a fifth cache unit, configured to temporarily store the plurality of quotients, wherein the trajectory sequence controller schedules the trajectory floating-point calculation circuit to multiplex the plurality of quotients to obtain the coefficients of the curve; and The sixth cache unit is used to temporarily store elements of the secondary difference value vector.

9. The trajectory polynomial generating device according to claim 7, characterized in that: The trajectory polynomial generating device further comprises: a direct memory access controller coupled to the trace cache and an external device, wherein the external device comprises a first external device and a second external device, and the direct memory access controller comprises: a trace read direct memory access controller, coupled to the assembly cache, for reading the coordinates of the plurality of path points from the first external device and storing the coordinates in the assembly cache; and A trace write direct memory access controller is coupled to the commit buffer and is configured to read the coefficients of the curve from the commit buffer to provide the coefficients to the second external device.

10. The trajectory polynomial generating device according to claim 9, characterized in that: The trace read direct memory access controller reads the coordinates of the plurality of waypoints from the first external device through an AXI4 bus, and the trace write direct memory access controller provides the coefficients to the second external device through the AXI4 bus.

11. The trajectory polynomial generating device according to claim 9, characterized in that: The trajectory polynomial generating device further comprises: a first ping-pong register coupled to the trace floating-point calculation circuit; a second ping-pong register 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 an APB bus, and 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 a CSB bus.

Images