Track optimization method and device, electronic equipment and medium
Through the method of combining sliding window and GTSAM optimization library, a trajectory optimization factor diagram is generated, which solves the problem of poor trajectory optimization results caused by severe changes in object positions, and achieves more efficient trajectory optimization and position tracking.
Patent Information
- Application Number
- CN202411951082.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-12-26
- Publication Date
- 2025-05-30
AI Technical Summary
In the field of object detection, the severe change in position of objects leads to unsatisfactory trajectory optimization results and even worse results.
The sliding window method is adopted and the trajectory optimization factor graph is used to optimize the trajectory segment by segment based on the GTSAM optimization library, and a trajectory optimization factor graph including multi-dimensional state variables, prior information, and prior factors is generated.
Effectively improve the effect of trajectory optimization, offset the fallback caused by position mutations and loop-free constraints, and achieve more accurate position tracking of target objects.
Smart Images
Figure CN120067486A_ABST
Abstract
Description
Technical Field
[0001] This application relates to the field of object detection, and particularly to a trajectory optimization method, device, electronic device, and medium. Background Art
[0002] During a period of time, the position change of a moving object in space at each moment is not fixed. Sometimes it moves at a relatively fast speed, and sometimes it moves relatively slowly. This results in relatively drastic position changes of the object, thus making the trajectory relatively complex.
[0003] If the entire trajectory is optimized, the effect of trajectory optimization may be unsatisfactory or even deteriorate due to the drastic position change of the object. Summary of the Invention
[0004] This application provides a trajectory optimization method, including: obtaining trajectory data of a target object; generating a trajectory optimization factor graph including multi-dimensional state variables, prior information, and prior factors based on the trajectory data; performing window sliding on the trajectory data, and optimizing the trajectory data based on the trajectory optimization factor graph using the GTSAM (Georgia Tech Smoothing and Mapping, a C++ library based on factor graph) optimization library to obtain optimized data corresponding to the trajectory data.
[0005] In some embodiments, the obtaining trajectory data of the target object includes: obtaining trajectory data including the position and velocity of the target object; the generating a trajectory optimization factor graph including multi-dimensional state variables, prior information, and prior factors based on the trajectory data includes: obtaining the multi-dimensional state variables including the position and velocity of the target object, the prior information including the position and velocity of the target object, and the prior factors including the position and velocity of the target object based on the trajectory data, and generating the trajectory optimization factor graph.
[0006] In some embodiments, the generating a trajectory optimization factor graph including multi-dimensional state variables, prior information, and prior factors based on the trajectory data includes: generating the trajectory optimization factor graph including the multi-dimensional state variables, the prior information, the prior factors, a four-factor of uniform acceleration motion model, and a two-factor of velocity smoothing motion model based on the trajectory data; wherein, the four-factor of uniform acceleration motion model includes the position, acceleration, velocity, and heading angle of the target object, and the two-factor of velocity smoothing motion model includes the acceleration and velocity of the target object.
[0007] In some embodiments, obtaining the trajectory data of the target object includes: obtaining trajectory data including the position, length, width, height, heading angle, and speed of the target object; the multi-dimensional state variables include the position and speed of the target object, the prior information includes the position and speed of the target object, and the prior factor includes the position and speed of the target object.
[0008] In some embodiments, the uniformly accelerated motion model is:
[0009]
[0010] yaw 2 = yaw 1 + wv 1 * Δt
[0011] In the formula, p 1 refers to the position of the target object in the trajectory at time t 1 p 2 refers to the position of the target object in the trajectory at time t 2 v 1 refers to the speed of the target object in the trajectory at time t 1 a 1 refers to the acceleration of the target object in the trajectory at time t 1 Δt refers to the time difference between time t 1 and time t 2 yaw 1 refers to the heading angle of the target object in the trajectory at time t 1 yaw 2 refers to the heading angle of the target object at time t in the trajectory 2 wv 1 refers to the angular velocity of the target object in the trajectory at time t 1 in the trajectory.
[0012] In some embodiments, the speed smoothing motion model is:
[0013] v 2 = v 1 + a 1 * Δt
[0014] wv 2 = wv 1
[0015] a 2 = a 1
[0016] In the formula, v 1 refers to the speed of the target object in the trajectory at time t1 The speed, v 2 refers to the speed of the target object in the trajectory at time t 2 The acceleration, a 1 refers to the acceleration of the target object in the trajectory at time t 1 The acceleration, a 2 refers to the acceleration of the target object in the trajectory at time t 2 The acceleration, Δt refers to the time t 1 and the time difference between time t 2 The angular velocity, wv 1 refers to the angular velocity of the target object in the trajectory at time t 1 The angular velocity, wv 2 refers to the angular velocity of the target object in the trajectory at time t 2 The angular velocity.
[0017] In some embodiments, the window sliding of the trajectory data and the optimization using the trajectory optimization factor graph based on the GTSAM optimization library to obtain the optimized data corresponding to the trajectory data include: in response to the result that the number of the trajectory data is greater than the size of the window, performing the window sliding on the trajectory data, and optimizing the trajectory data within the window of each sliding based on the GTSAM optimization library using the trajectory optimization factor graph to obtain the optimized data corresponding to the trajectory data.
[0018] In some embodiments, the window sliding of the trajectory data and the optimization using the trajectory optimization factor graph based on the GTSAM optimization library to obtain the optimized data corresponding to the trajectory data include: in response to the result that the number of the trajectory data is less than the size of the window and greater than a preset length threshold, optimizing the trajectory data based on the GTSAM optimization library using the trajectory optimization factor graph to obtain the optimized data corresponding to the trajectory data.
[0019] In some embodiments, the window sliding of the trajectory data and the optimization using the trajectory optimization factor graph based on the GTSAM optimization library to obtain the optimized data corresponding to the trajectory data include: in response to the result that the number of the trajectory data is less than the preset length threshold, directly using the trajectory data as the optimized data of the trajectory data.
[0020] The present application provides a trajectory optimization device, including: an acquisition module, configured to acquire trajectory data of a target object; a factor graph generation module, configured to generate a trajectory optimization factor graph including multi-dimensional state variables, prior information, and prior factors based on the trajectory data; a sliding window module, configured to perform window sliding on the trajectory data; and an optimization module, configured to perform optimization using the trajectory optimization factor graph based on the GTSAM optimization library during the window sliding process to obtain optimized data corresponding to the trajectory data.
[0021] The present application provides an electronic device, including a memory and a processor, where the memory and the processor are communicatively connected to each other, the memory stores a computer program that can run on the processor, and when the processor executes the computer program, the method described above is implemented.
[0022] The present application provides a computer-readable storage medium, where the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method described above is implemented.
[0023] The beneficial effects brought by the present application are as follows: The present application adopts a sliding window method and performs trajectory optimization segment by segment using the trajectory optimization factor graph based on the GTSAM optimization library, which can effectively improve the effect of trajectory optimization and offset the regression caused by position mutations and / or lack of loop constraints. Description of the Drawings
[0024] To more clearly illustrate the technical solutions in the embodiments of the present application, the following will briefly introduce the drawings required for the description of the embodiments. Obviously, the following drawings are only some embodiments of the present application. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.
[0025] Figure 1 It is a schematic diagram of the application scenario in some embodiments of the present application;
[0026] Figure 2 It is a flowchart of the trajectory optimization method in some embodiments of the present application;
[0027] Figure 3 It is a schematic diagram of the framework of the trajectory optimization factor in some embodiments of the present application;
[0028] Figure 4 It is a schematic diagram of the framework of the trajectory optimization factor in some embodiments of the present application;
[0029] Figure 5 It is a schematic diagram of the framework of the trajectory optimization device in some embodiments of the present application;
[0030] Figure 6Schematic diagram of the frame of an electronic device in some embodiments of the present application;
[0031] Figure 7 Schematic diagram of the frame of a computer-readable storage medium in an embodiment of the present application. Detailed implementation manners
[0032] The present application will be further described in detail below with reference to the accompanying drawings and embodiments. It should be specifically noted that the following embodiments are only used to illustrate the present application, but do not limit the scope of the present application. Similarly, the following embodiments are only partial embodiments of the present application rather than all embodiments. All other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present application.
[0033] When the present application mentions "embodiment", it means that the specific features, structures or characteristics described in combination with the embodiment may be included in at least one embodiment of the present application. Those skilled in the art explicitly and implicitly understand that the embodiments described in the present application can be combined with other embodiments.
[0034] In addition, the technical solutions between various embodiments can be combined with each other, but it must be based on the fact that those of ordinary skill in the art can implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the protection scope required by the present application.
[0035] The present application describes a trajectory optimization method, which can be applied in the field of object detection, such as the 3D (Three Dimensions) object detection field. Generally, 3D object detection is to first perform 3D object detection, then track the 3D object, and then a trajectory can be obtained. This trajectory optimization method can optimize the trajectory. In addition, it can also effectively improve the effect of trajectory optimization in the trajectory optimization and offset the regression caused by position mutation and / or lack of loop constraint.
[0036] Please refer to Figure 1 , Figure 1 is a schematic diagram of an application scenario in some embodiments of the present application. This application scenario may include a terminal device 1, a terminal device 2, a terminal device 3, a server 4, and a network 5.
[0037] The terminal device 1, the terminal device 2, and the terminal device 3 can be hardware or software.
[0038] When the terminal devices 1, 2, and 3 are hardware, they can specifically be various electronic devices with a display screen and supporting communication with the server 4, including but not limited to smartphones, tablets, laptop computers, desktop computers, etc. When the terminal devices 1, 2, and 3 are software, they can specifically be installed in the above-mentioned electronic devices. The terminal devices 1, 2, and 3 can be implemented as multiple software or software modules, or can also be implemented as a single software or software module, and the embodiments of the present application do not limit this.
[0039] In some embodiments, various applications can be installed on the terminal devices 1, 2, and 3, such as data processing applications, instant messaging tools, social platform software, search applications, shopping applications, etc. Sensor devices for data collection can be configured in the terminal devices 1, 2, and 3, and the collected data, such as point cloud data or photos, can be converted into trajectories.
[0040] The server 4 can be a server providing various services. For example, it can be a background server that establishes a communication connection with the terminal devices and receives the requests sent by the terminal devices. The background server can receive and analyze the requests sent by the terminal devices and generate processing results.
[0041] The server 4 can be a single server, or can also be a server cluster composed of several servers, or can also be a cloud computing service center. The present application does not limit this.
[0042] It should be noted that the server 4 can be hardware or software. When the server 4 is hardware, it can specifically be various electronic devices providing various services for the terminal devices 1, 2, and 3. When the server 4 is software, it can specifically be multiple software or software modules providing various services for the terminal devices 1, 2, and 3, or can also be a single software or software module providing various services for the terminal devices 1, 2, and 3. The embodiments of the present application do not limit this.
[0043] The network 5 can be a wired network connected by coaxial cables, twisted pairs, and / or optical fibers, or can also be a wireless network that can interconnect various communication devices without wiring, such as Bluetooth, Near Field Communication (NFC), Infrared, etc. The embodiments of the present application do not limit this.
[0044] The server 4 can receive the point cloud data or photos collected by terminal devices such as terminal device 1, terminal device 2, and / or terminal device 3, process and reconstruct the point cloud data or photos to obtain a trajectory, and optimize the already constructed trajectory.
[0045] In some embodiments, the server 4 can detect the target object in the picture or point cloud data through methods described by those skilled in the art, such as neural networks, and represent it as 3D information in space (i.e., pose information), which may include information such as the center point, length, width, height, heading angle, and speed of the target object. Then, based on the pose information, the target object is tracked to obtain the trajectory of the target object for at least a period of time. It can be understood that the trajectory acquisition methods listed here are only one of the trajectory acquisition methods in this application and do not limit the actual trajectory acquisition methods in this application.
[0046] Furthermore, the server can be used to implement the above-mentioned trajectory optimization method.
[0047] It should be noted that the specific types, quantities, and combinations of terminal device 1, terminal device 2, terminal device 3, server 4, and network 5 can be adjusted according to the actual requirements of the application scenario, and the embodiments of this application do not limit this.
[0048] Please refer to Figure 2 , Figure 2 , which is a flowchart of the trajectory optimization method in some embodiments of this application. The trajectory optimization method may include:
[0049] Step S201: Obtain the trajectory data of the target object.
[0050] In some embodiments, the target object can be a moving product such as a car, a drone, a wearable device, etc. Of course, it can also be other products, which will not be elaborated here.
[0051] In some embodiments, the trajectory data can be sourced from terminal devices such as terminal device 1, terminal device 2, and / or terminal device 3. Specifically, the server 4 can receive the point cloud data or photos about the target object collected by terminal devices such as terminal device 1, terminal device 2, and / or terminal device 3, and process and reconstruct the point cloud data or photos to obtain a trajectory. Of course, the server 4 can also perform a preliminary optimization on the already constructed trajectory. The trajectory data can be the trajectory constructed by the server 4 or the data obtained after the already constructed trajectory is preliminarily optimized by the server 4.
[0052] In some embodiments, the trajectory of the target object can be obtained by tracking the target object. For example, the target object is tracked by the Kalman filtering method, and the motion trajectory of the target object over a period of time is obtained. For example, the trajectory composed of the poses of the object at multiple moments obtained by the SLAM (Simultaneous Localization and Mapping) mapping method. Of course, other well-known tracking methods in the art can also be used to obtain the trajectory data of the target object, and it is not limited to the methods listed here.
[0053] In some embodiments, the trajectory data may at least include the position of the target object. Here, since the target object can have at least length, width, and height, the center of the target object can be used to represent the position of the target object. Furthermore, in a coordinate system, the coordinates of the center point of the target object can be marked, and these coordinates are used to represent the position of the target object. In some embodiments, the coordinate system can be the world coordinate system or other coordinate systems similar to the world coordinate system, and specifically, the coordinate system can also be determined according to the well-known technical solutions in the art. In some embodiments, the coordinate system can be other coordinate systems converted from the world coordinate system.
[0054] In some embodiments, the trajectory data may further include the length, width, and height of the target object.
[0055] In some embodiments, the trajectory data may further include the heading angle of the target object.
[0056] In some embodiments, the trajectory data may further include the speed of the target object. In some embodiments, the speed of the target object can be represented by the component speeds in the three directions of the x-axis, y-axis, and z-axis in the coordinate system.
[0057] In some embodiments, the trajectory data may further include the acceleration of the target object. In some embodiments, the acceleration of the target object can be represented by the component accelerations in the three directions of the x-axis, y-axis, and z-axis in the coordinate system.
[0058] In some embodiments, at least one of the position, length, width, height, heading angle, speed, and acceleration of the target object constitutes the position information of the target object.
[0059] It can be understood that the trajectory data can be characterized by different forms of content. In addition, the specific content of the trajectory data can also be adjusted according to the needs of those skilled in the art, and furthermore, it is not limited to the embodiments listed here.
[0060] Step S202: Generate a trajectory optimization factor graph including multi-dimensional state variables, prior information, and prior factors based on the trajectory data.
[0061] In some embodiments, the multi-dimensional state variable may include the position of the target object.
[0062] In some embodiments, the multi-dimensional state variable may further include the heading angle of the target object.
[0063] In some embodiments, the multi-dimensional state variable may further include the speed of the target object.
[0064] In some embodiments, the multi-dimensional state variable may further include the acceleration of the target object.
[0065] It can be understood that the multi-dimensional state variable can be characterized by different forms of content. In addition, the specific content of the multi-dimensional state variable can also be adjusted according to the needs of those skilled in the art. Furthermore, it is not limited to the embodiments listed here.
[0066] In some embodiments, the prior information and the prior factor can be determined based on the trajectory optimization factor graph. In some embodiments, the prior information may include position prior information. In some embodiments, the prior information may include speed prior information. In some embodiments, the prior information may include heading angle prior information. In some embodiments, the prior factor may include a position prior factor. In some embodiments, the prior information may include a speed prior factor. In some embodiments, the position prior information may include the position of the target object. In some embodiments, the position prior information may include the heading angle of the target object. In some embodiments, the position prior information may include the position and heading angle of the target object.
[0067] It can be understood that the prior information and the prior factor can be characterized by different forms of content. In addition, the specific content of the prior information and the prior factor can also be adjusted according to the needs of those skilled in the art. Furthermore, it is not limited to the embodiments listed here.
[0068] In some embodiments, during the process of establishing the trajectory optimization factor graph, variables, factors, etc. can be determined first, and connections can be established between them. In some embodiments, the trajectory optimization factor graph can be constructed using the GTSAM optimization library (GTSAM library), Python library, Open3D, NetworkX, MATLAB, etc.
[0069] Step S203: Perform window sliding on the trajectory data, and optimize it using the trajectory optimization factor graph based on the GTSAM optimization library to obtain the optimized data corresponding to the trajectory data.
[0070] The window sliding (sliding window) technique can process the trajectory data within a fixed-size window, and then gradually move the window to process continuous trajectory data. It can avoid processing the trajectory data simultaneously.
[0071] Since the trajectory data can be more or less, when there is a large amount of trajectory data, it is very easy to have data mutations such as speed mutations and position mutations when optimizing the trajectory data together, resulting in complex trajectories. When further using conventional methods to optimize the entire trajectory together, the effect of trajectory optimization may be unsatisfactory or even deteriorate due to the drastic change in the position of the target object. However, the present application uses a sliding window method and performs trajectory optimization segment by segment based on the GTSAM optimization library using a trajectory optimization factor graph, making these trajectories smoother and the position of the target object at each moment more accurate, thereby effectively improving the effect of trajectory optimization.
[0072] In some embodiments, the present application uses a sliding window (window sliding) method, which can batch process the places with drastic position changes by moving the sliding window, so as to better smooth the entire trajectory.
[0073] In some embodiments, for a vehicle object moving on a road, it generally travels in a certain fixed direction and does not form a closed loop. For such a closed trajectory, due to the existence of loop constraints, the effect of optimizing the entire trajectory together using conventional methods is also good. However, when the closed trajectory does not exist, this loop constraint disappears, and then the regression problem caused by the lack of loop constraints will occur, and conventional methods cannot be used. The present application uses a sliding window method to optimize the trajectory segment by segment, which can offset the regression problem caused by the lack of loop constraints.
[0074] In some embodiments, before performing the optimization using the trajectory optimization factor graph based on the GTSAM optimization library in step S202, step S203 may be performed, and a trajectory optimization factor graph is constructed in the GTSAM optimization library.
[0075] It can be understood that steps S202 and S203 may not be in a specific order and can be adjusted according to the needs of those skilled in the art. In some scenarios, step S203 may be performed after step S202. In some embodiments, step S202 may be performed after the first window sliding in step S203, and then step S202 is no longer performed during the other window sliding processes in step S203, and the optimization is performed using the trajectory optimization factor graph based on the GTSAM optimization library.
[0076] In addition, steps S201 and S202 may not follow a specific order and can be adjusted according to the needs of those skilled in the art. In some scenarios, step S202 may be performed after step S201. In some scenarios, step S201 may be performed after step S202. In some scenarios, steps S201 and S202 may be performed simultaneously. In some scenarios, step S202 may be performed after step S201, and then step S203 may be executed. In some scenarios, step S201 may be performed after step S202, and then step S203 may be executed.
[0077] In some embodiments, step S201 may include: obtaining trajectory data including the position and velocity of the target object.
[0078] The acquisition of the trajectory data may be determined based on the trajectory optimization factor graph in step S202. Please refer to Figure 3 , Figure 3 which is a schematic framework diagram of the trajectory optimization factor in some embodiments of the present application. When the trajectory optimization factor involves position prior information and velocity prior information, further, trajectory data including the position and velocity of the target object may be obtained in step S201 to facilitate the smooth progress of step S203.
[0079] In some scenarios, there will be at least one piece of trajectory data within a time segment. Then, each piece of trajectory data is processed in sequence. One piece of trajectory data is taken out from the trajectory data. Each piece of trajectory data is composed of the position information of the target object at different times. The position information of the object includes (x, y, z, l, w, h, yaw, v x , v y , v z ), that is, the center point coordinates (x, y, z) of the object, the length, width and height (l, w, h), the heading angle (yaw) and the velocity (v x , v y , v z ). Factors can be added to the trajectory optimization factor graph in Figure 3 , and an initial estimate value can be created. As can be seen from Figure 3 , the variables to be optimized are the position x and the velocity v. The added factors are: position prior factor and velocity prior factor.
[0080] After adding the factors to the trajectory optimization factor graph, it is also necessary to create an initial estimate value for each factor. The initial value of the position prior factor is the position information (x, y, z, yaw) of the target object at each moment. The velocity prior factor can obtain the velocity based on the adjacent two position information: v = (p 2 - p 1 ) / t.
[0081] In some embodiments, trajectory data including the position, heading angle, and speed of the target object may be obtained in step S201. In some embodiments, trajectory data including the position, heading angle, speed, acceleration, etc. of the target object may be obtained in step S201.
[0082] In some embodiments, referring to Figure 2 , step S202 may include: obtaining multi-dimensional state variables including the position and speed of the target object, prior information including the position and speed of the target object, and prior factors including the position and speed of the target object based on the trajectory data, and generating a trajectory optimization factor graph.
[0083] In some embodiments, multi-dimensional state variables including the position, speed, and heading angle of the target object, prior information including the position, speed, and heading angle of the target object, and prior factors including the position and speed of the target object may be obtained in step S202 based on the trajectory data, and a trajectory optimization factor graph may be generated.
[0084] In some embodiments, referring to Figure 4 , Figure 4 is a schematic framework diagram of the trajectory optimization factor in some embodiments of the present application. Step S202 may include: generating a trajectory optimization factor graph including multi-dimensional state variables, prior information, prior factors, a four-factor uniform acceleration motion model, and a two-factor velocity smoothing motion model based on the trajectory data; wherein, the four-factor uniform acceleration motion model includes the position, acceleration, speed, and heading angle of the target object, and the two-factor velocity smoothing motion model includes the acceleration and speed of the target object.
[0085] In some scenarios, there will be at least one piece of trajectory data within a period of time segment. Then, each piece of trajectory data is processed in sequence. One piece of trajectory data is taken out from the trajectory data. Each piece of trajectory data is composed of the position information of the target object at different times. The position information of the object includes (x, y, z, l, w, h, yaw, v x , v y , v z ), that is, the center point coordinates (x, y, z) of the object, the length, width, and height (l, w, h), the heading angle (yaw), and the speed (v x , v y , v z ). Factors may be added to the trajectory optimization factor graph in Figure 4 , and an initial estimate value may be created. As can be seen from Figure 4 , the variables to be optimized are the position x and the speed v. The added factors include: a position prior factor, a speed prior factor, a four-factor kinematic model, and a two-factor velocity smoothing factor.
[0086] After adding factors to the trajectory optimization factor graph, it is also necessary to create initial estimates for each factor. The initial value of the position prior factor is the position information (x, y, z, yaw) of the target object at each moment. The velocity prior factor can obtain the velocity based on the position information of two adjacent moments. The kinematic model quaternion factor and the velocity smoothing binary factor do not require the addition of initial values.
[0087] In some embodiments, step S201 may include: obtaining trajectory data including the position, length, width, height, heading angle, and velocity of the target object; the multi-dimensional state variable includes the position and velocity of the target object, the prior information includes the position and velocity of the target object, and the prior factor includes the position and velocity of the target object. In some embodiments, the prior information includes the position, velocity, and heading angle of the target object, and the prior factor includes the position, velocity, and heading angle of the target object.
[0088] In some embodiments, the trajectory data is sampled at a preset sampling frequency, that is, the time difference between two adjacent positions in the same trajectory can be determined by the preset sampling frequency. Therefore, within a certain period of time, such as 1s, 2s, 3s, 4s, 5s, etc., it can be assumed that the target object is in uniformly accelerated linear motion. Therefore, it is assumed here that the motion of the target object conforms to the uniformly accelerated motion model.
[0089] Assume t 1 The position of the object at the moment is p 1 , p 1 Contains the position in three directions (x 1 , y 1 , z 1) And the heading angle yaw 1， The velocity of the object at this time is (v x1 , v y1 , v z1 , w v1 ), and the acceleration is (a x1 , a y1 , a z1 ); At t 2 The position of the object at the moment is p 2 , p 2 Contains the position in three directions (x 2 , y 2 , z 2 ) and the heading angle yaw 2， The velocity of the object is (v x2 , v y2 , v z2 , w v2 ), and the acceleration is (a x2 , a y2 , a z2 ).
[0090] The uniformly accelerated motion model is:
[0091]
[0092] yaw 2 = yaw 1 + wv 1 * Δt
[0093] In the formula, p 1 refers to the position of the target object in the trajectory at time t 1 p 2 refers to the position of the target object in the trajectory at time t 2 v 1 refers to the velocity of the target object in the trajectory at time t 1 a 1 refers to the acceleration of the target object in the trajectory at time t 1 Δt refers to the time difference between time t 1 and time t 2 yaw 1 refers to the heading angle of the target object in the trajectory at time t 1 yaw 2 refers to the heading angle of the target object at time t in the trajectory 2 wv 1 refers to the angular velocity of the target object in the trajectory at time t 1
[0094] In some embodiments, the velocity and acceleration constraint relationship can be obtained from uniformly accelerated linear motion. Here, the linear velocity is uniformly accelerated linear motion, while the angular velocity is uniform motion. The velocity smoothing motion model is:
[0095] v 2 = v 1 + a 1 * Δt
[0096] wv 2 = wv 1
[0097] a 2 = a 1
[0098] In the formula, v 1 refers to the velocity of the target object in the trajectory at time t 1 v 2 refers to the velocity of the target object in the trajectory at time t 2 a 1 refers to the acceleration of the target object in the trajectory at time t 1 a 2 refers to the acceleration of the target object in the trajectory at time t 2 The acceleration, where Δt refers to the time t 1 and the time t 2 the time difference between them, wv 1 refers to the angular velocity of the target object in the trajectory at time t 1 the angular velocity, wv 2 refers to the angular velocity of the target object in the trajectory at time t 2 the angular velocity.
[0099] In some embodiments, step S203 may include:
[0100] Step S301: In response to the result that the number of trajectory data is greater than the size of the window, perform window sliding on the trajectory data, and optimize the trajectory data located within the window of each sliding using a trajectory optimization factor graph based on the GTSAM optimization library to obtain optimized data corresponding to the trajectory data.
[0101] In some embodiments, the size of the window may be determined according to the trajectory data. Specifically, it can be determined according to data mutations such as speed mutations and position mutations to improve the optimization effect. In some embodiments, the sliding window may be a fixed-size window, that is, the window size is fixed and moves one unit each time. In some embodiments, the sliding window may be a variable-size window, that is, the window size can be dynamically adjusted as needed. For example, the window size can be changed, such as reduced, when there are data mutations such as speed mutations and position mutations. For example, the window size can be changed, such as increased, when the data is smooth.
[0102] In some embodiments, the sliding window in step S203 may be a fixed-size window.
[0103] In some embodiments, starting from the initial position 0 in a single trajectory, set the size of the window to m. If the number of trajectory data is greater than m, sequentially select the first m trajectory data, optimize the trajectory data located within the window using a trajectory optimization factor graph based on the GTSAM optimization library, and save the optimized result; then move one position backward, starting from position 1, sequentially select the first m trajectory data, optimize the trajectory data located within the window using a trajectory optimization factor graph based on the GTSAM optimization library, and update / save the optimized result; and so on until the window reaches the last position of the trajectory data, update / save the optimized result to obtain optimized data corresponding to the trajectory data.
[0104] In some embodiments, before step S301, step S203 may further include: determining the matching degree between the trajectory data and the window. Specifically, if the number of trajectory data is greater than the size of the window, then they match, and thus step S301 can be performed.
[0105] In some embodiments, step S203 may include:
[0106] Step S302: In response to the result that the number of trajectory data is less than the size of the window and greater than the preset length threshold, optimize the trajectory data using a trajectory optimization factor graph based on the GTSAM optimization library to obtain optimized data corresponding to the trajectory data.
[0107] In some embodiments, before step S301, step S203 may further include: determining the matching degree between the trajectory data and the window. Specifically, if the number of trajectory data is less than the size of the window and greater than the preset length threshold, it is a low match, and then step S302 can be performed.
[0108] In some embodiments, step S203 may include:
[0109] Step S303: In response to the result that the number of trajectory data is less than the preset length threshold, directly use the trajectory data as the optimized data of the trajectory data.
[0110] In some embodiments, before step S301, step S203 may further include: determining the matching degree between the trajectory data and the window. Specifically, if the number of trajectory data is less than the preset length threshold, it is not a match, and then step S303 can be performed.
[0111] In some embodiments, step S203 may include:
[0112] Step S401: Perform a window sliding on the trajectory data;
[0113] Step S402: During the window sliding process, optimize using a trajectory optimization factor graph based on the GTSAM optimization library to obtain optimized data corresponding to the trajectory data.
[0114] In some embodiments, step S202 may be performed before step S402.
[0115] In some embodiments, before step S201, the method may include: determining a trajectory corresponding to a target object. After step S203, the method further includes determining a trajectory corresponding to the next target object, and then sequentially performing steps S201, S202, etc. until all trajectories are optimized.
[0116] Next, a trajectory optimization device is described, which is used to execute the trajectory optimization method in the above embodiments. In some embodiments, the trajectory optimization device may be the server 4 in the above embodiments, or it may be others.
[0117] Please refer to Figure 5 , Figure 5This is a schematic framework diagram of a trajectory optimization device in some embodiments of the present application. The trajectory optimization device 100 may include an acquisition module 101, a sliding window module 102, a factor graph generation module 103, and an optimization module 104.
[0118] In some embodiments, the acquisition module 101 may be configured to acquire trajectory data of a target object.
[0119] In some embodiments, the sliding window module 102 may be configured to perform window sliding on the trajectory data.
[0120] In some embodiments, the factor graph generation module 103 may be configured to generate a trajectory optimization factor graph including multi-dimensional state variables, prior information, and prior factors based on the trajectory data.
[0121] In some embodiments, the optimization module 104 may be configured to perform optimization using the trajectory optimization factor graph based on the GTSAM optimization library during the window sliding process to obtain optimized data corresponding to the trajectory data.
[0122] In some embodiments, the acquisition module 101 may be configured to acquire trajectory data including the position and velocity of the target object;
[0123] In some embodiments, the factor graph generation module 103 may be configured to obtain multi-dimensional state variables including the position and velocity of the target object, prior information including the position and velocity of the target object, and prior factors including the position and velocity of the target object based on the trajectory data, and generate a trajectory optimization factor graph.
[0124] In some embodiments, the factor graph generation module 103 may be configured to generate a trajectory optimization factor graph including multi-dimensional state variables, prior information, prior factors, a four-factor of uniform acceleration motion model, and a two-factor of velocity smoothing motion model based on the trajectory data;
[0125] Among them, the four-factor of uniform acceleration motion model includes the position, acceleration, velocity, and heading angle of the target object, and the two-factor of velocity smoothing motion model includes the acceleration and velocity of the target object.
[0126] In some embodiments, the acquisition module 101 may be configured to acquire trajectory data including the position, length, width, height, heading angle, and velocity of the target object;
[0127] The multi-dimensional state variables include the position and velocity of the target object, the prior information includes the position and velocity of the target object, and the prior factors include the position and velocity of the target object.
[0128] In some embodiments, the uniform acceleration motion model is:
[0129]
[0130] yaw2 = yaw 1 + wv 1 * Δt
[0131] In the formula, p 1 refers to the position of the target object in the trajectory at time t 1 p 2 refers to the position of the target object in the trajectory at time t 2 v 1 refers to the velocity of the target object in the trajectory at time t 1 a 1 refers to the acceleration of the target object in the trajectory at time t 1 Δt refers to the time difference between time t 1 and time t 2 yaw 1 refers to the heading angle of the target object in the trajectory at time t 1 yaw 2 refers to the heading angle of the target object at time t in the trajectory 2 wv 1 refers to the angular velocity of the target object in the trajectory at time t 1
[0132] In some embodiments, the velocity smoothing motion model is:
[0133] v 2 = v 1 + a 1 * Δt
[0134] wv 2 = wv 1
[0135] a 2 = a 1
[0136] In the formula, v 1 refers to the velocity of the target object in the trajectory at time t 1 v 2 refers to the velocity of the target object in the trajectory at time t 2 a 1 refers to the acceleration of the target object in the trajectory at time t 1 a 2 refers to the acceleration of the target object in the trajectory at time t 2 Δt refers to the time difference between time t 1 and time t 2 wv 1 refers to the angular velocity of the target object in the trajectory at time t 1 wv 2 refers to the angular velocity of the target object in the trajectory at time t 2 of the trajectory.
[0137] In some embodiments, the sliding window module 102 can be used to perform window sliding on the trajectory data in response to the result that the number of trajectory data is greater than the size of the window. The optimization module 104 can be used to optimize the trajectory data located within the window of each sliding window based on the GTSAM optimization library using a trajectory optimization factor graph to obtain optimized data corresponding to the trajectory data.
[0138] In some embodiments, the sliding window module 102 can be used in response to the result that the number of trajectory data is less than the size of the window and greater than a preset length threshold. The optimization module 104 can be used to optimize the trajectory data based on the GTSAM optimization library using a trajectory optimization factor graph to obtain optimized data corresponding to the trajectory data.
[0139] In some embodiments, the sliding window module 102 can be used in response to the result that the number of trajectory data is less than the preset length threshold. The optimization module 104 can be used to directly use the trajectory data as the optimized data of the trajectory data.
[0140] Next, an electronic device for performing the trajectory optimization method in the above embodiments is described. In some embodiments, the trajectory optimization device can be the server 4 in the above embodiments, or of course it can also be others.
[0141] Please refer to Figure 6 , Figure 6 which is a schematic framework diagram of an electronic device in some embodiments of the present application. The electronic device 200 includes a memory 210, a processor 220, and a computer program stored on the memory 210 and executable on the processor 220. When the processor 220 executes the computer program, it implements the steps of any of the above-described knock detection methods.
[0142] Among them, the processor 220 can also be referred to as a CPU (Central Processing Unit, central processing unit). The processor 220 may be an integrated circuit chip with signal processing capabilities. The processor 220 can also be a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components. The general-purpose processor can be a microprocessor or the processor 220 can also be any conventional processor, etc.
[0143] The memory 210 may include a random access memory (RAM), a read-only memory (ROM), a flash memory, an erasable programmable read-only memory 210 (EPROM), an electrically erasable programmable read-only memory (EEPROM), registers, a hard disk, a removable disk, a CD-ROM, and the like. The memory 210 may store program data, which may include, for example, a single instruction or many instructions, and may be distributed across several different code segments, across different programs, and across multiple memories. The memory 210 may be coupled to the processor 220 such that the processor 220 can read from / write information to the memory 210. Of course, the memory 210 may be integrated into the processor 220, and this application does not limit this, and those skilled in the art can make a choice according to actual needs.
[0144] The following introduces a computer-readable storage medium. Please refer to Figure 7 , Figure 7 which is a schematic framework diagram of a computer-readable storage medium in an embodiment of this application. A computer program 301 is stored on the computer-readable storage medium 300, and when the computer program 301 is executed by a processor, the above method is implemented. In some embodiments, the computer-readable storage medium 300 may be the memory 210 in the above embodiments.
[0145] Specifically, the computer-readable storage medium 300 may be a USB flash drive, a removable hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a magnetic disk, or an optical disc, etc., which can store the computer program 301, or may also be a server storing the computer program 301. The server may send the stored computer program 301 to other devices for running, or may also run the stored computer program 301 itself.
[0146] In several implementation manners provided in this application, it should be understood that the disclosed methods and devices can be implemented in other ways. For example, the device implementation manners described above are only illustrative. For example, the division of modules or units is only a logical function division. In actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed.
[0147] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place, or may be distributed to multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this implementation manner.
[0148] In addition, in each embodiment of the present application, each functional unit can be integrated into one processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit. The above-mentioned integrated units can be implemented in the form of hardware or in the form of software functional units.
[0149] The above are only the embodiments of the present application, and do not limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made by using the content of the specification and drawings of the present application, or directly or indirectly applied in other related technical fields, shall be similarly included in the patent protection scope of the present application.
Claims
1. A trajectory optimization method, characterized in that: include: Obtain trajectory data of the target object; Generate a trajectory optimization factor graph including multi-dimensional state variables, prior information, and prior factors based on the trajectory data; The trajectory data is windowed and optimized using the trajectory optimization factor graph based on a GTSAM optimization library to obtain optimized data corresponding to the trajectory data.
2. The method according to claim 1, characterized in that: The step of obtaining the trajectory data of the target object includes: Acquiring trajectory data including the position and velocity of the target object; The step of generating a trajectory optimization factor graph including multi-dimensional state variables, prior information, and prior factors based on the trajectory data includes: The multidimensional state variables including the position and velocity of the target object, the prior information including the position and velocity of the target object, and the prior factors including the position and velocity of the target object are acquired based on the trajectory data, and the trajectory optimization factor graph is generated.
3. The method according to claim 1, characterized in that The step of generating a trajectory optimization factor graph including multi-dimensional state variables, prior information, and prior factors based on the trajectory data includes: Generate the trajectory optimization factor graph including the multidimensional state variables, the prior information, the prior factors, the uniform acceleration motion model quaternary factors and the velocity smoothing motion model binary factors based on the trajectory data; The quaternary factors of the uniform acceleration motion model include the position, acceleration, velocity and heading angle of the target object, and the binary factors of the velocity smoothing motion model include the acceleration and velocity of the target object.
4. The method according to claim 3, characterized in that: The step of obtaining the trajectory data of the target object includes: Acquire trajectory data including the position, length, width, height, heading angle and speed of the target object; The multidimensional state variable includes the position and velocity of the target object, the prior information includes the position and velocity of the target object, and the prior factor includes the position and velocity of the target object.
5. The method according to claim 3 or 4, characterized in that: The uniform acceleration motion model is: yaw2=yaw1+wv1*Δt Wherein, p1 refers to the position of the target object in the trajectory at time t1, p2 refers to the position of the target object in the trajectory at time t2, v1 refers to the velocity of the target object in the trajectory at time t1, a1 refers to the acceleration of the target object in the trajectory at time t1, Δt refers to the time difference between time t1 and time t2, yaw1 refers to the heading angle of the target object in the trajectory at time t1, yaw2 refers to the heading angle of the target object in the trajectory at time t2, and wv1 refers to the angular velocity of the target object in the trajectory at time t1.
6. The method according to claim 3 or 4, characterized in that: The speed smoothing motion model is: v2=v1+a1*Δt wv2=wv1 a2=a1 Wherein, v1 refers to the velocity of the target object in the trajectory at time t1, v2 refers to the velocity of the target object in the trajectory at time t2, a1 refers to the acceleration of the target object in the trajectory at time t1, a2 refers to the acceleration of the target object in the trajectory at time t2, Δt refers to the time difference between time t1 and time t2, wv1 refers to the angular velocity of the target object in the trajectory at time t1, and wv2 refers to the angular velocity of the target object in the trajectory at time t2.
7. The method according to claim 1, characterized in that The window sliding of the trajectory data and optimizing using the trajectory optimization factor graph based on the GTSAM optimization library to obtain optimized data corresponding to the trajectory data includes: In response to the result that the amount of the trajectory data is greater than the size of the window, the window sliding is performed on the trajectory data, and based on the GTSAM optimization library, the trajectory data located in the window of each sliding is optimized using the trajectory optimization factor graph to obtain optimized data corresponding to the trajectory data.
8. The method according to claim 1, characterized in that: The window sliding of the trajectory data and optimizing using the trajectory optimization factor graph based on the GTSAM optimization library to obtain optimized data corresponding to the trajectory data includes: In response to the result that the amount of the trajectory data is less than the size of the window and greater than a preset length threshold, the trajectory data is optimized using the trajectory optimization factor graph based on the GTSAM optimization library to obtain optimized data corresponding to the trajectory data.
9. The method according to claim 1, characterized in that: The window sliding of the trajectory data and optimizing using the trajectory optimization factor graph based on the GTSAM optimization library to obtain optimized data corresponding to the trajectory data includes: In response to the result that the amount of the trajectory data is less than the preset length threshold, the trajectory data is directly used as the optimization data of the trajectory data.
10. A trajectory optimization device, characterized in that: include: An acquisition module, used to acquire trajectory data of a target object; A factor graph generation module, used to generate a trajectory optimization factor graph including multi-dimensional state variables, prior information, and prior factors based on the trajectory data; A sliding window module, used for performing window sliding on the trajectory data; The optimization module is used to optimize the trajectory optimization factor graph based on the GTSAM optimization library during the window sliding process to obtain optimized data corresponding to the trajectory data.
11. An electronic device, comprising a memory and a processor, wherein the memory and the processor are connected to each other for communication, and the memory stores a computer program that can be run on the processor, wherein: When the processor executes the computer program, the method according to any one of claims 1 to 9 is implemented.
12. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method according to any one of claims 1 to 9 is implemented.