Path optimization method and related device

Abstract

A path optimization method is applied to the technical field of robots or automatic driving. The method comprises the following steps: acquiring a motion state of each sampling position point of a driving device; the motion state is represented based on a plurality of flat variables or derivatives of the flat variables; the motion state of the at least one sampling position point comprises curvature and curvature change rate; determining a target penalty function according to the motion state information; the target penalty function is related to driving feasibility and smoothness when the driving device drives along a plurality of sampling position points; and optimizing the driving path according to the target penalty function. According to the method, solving of a differential equation in the trajectory optimization process can be avoided, so that trajectory optimization difficulty is reduced, nonlinear constraint terms such as curvature and change rate thereof are directly described in a flat variable space, and displacement or time difference of repeated discretization can be avoided.

Description

Technical Field

[0001] The present application relates to the technical field of path optimization, and in particular to a path optimization method and related devices. Background Art

[0002] Path optimization technology refers to the process by which a driving device (such as a robot) perceives the driving environment based on its own sensors, thereby automatically planning a safe movement path from the current point to the target point. In addition to positioning and mapping, path optimization is a key function of any autonomous navigation system, mainly solving three major problems: 1. Connectivity, that is, the path must be able to connect the starting point and the end point, and ensure safe obstacle avoidance throughout the entire process; 2. Dynamic and kinematic feasibility, that is, the path meets the constraints of the driving device's own movement and dynamic constraints, such as the speed of the driving device should not exceed its upper speed limit; 3. Optimality, that is, on the premise of achieving the first two, the length of the path, driving time, and energy consumption of the driving device should be optimized as much as possible.

[0003] In the prior art, the predicted driving path is optimized through a penalty function, wherein the construction of the penalty function is related to information such as the curvature and curvature change rate of the path. The curvature is determined by the linear velocity and the angular velocity, and is a nonlinear constraint (involving arctan, sqrt, and exponential operations), while the curvature change rate is even more nonlinear and of a higher order (numerical operations involve acceleration, the fifth power of velocity, etc., and values ​​often appear in the range of 10^10, which can easily cause gradient explosion). This results in a high degree of complexity in modeling the curvature change rate, and is difficult to control in terms of restoration and numerical stability.

[0004] Based on this, a more efficient and accurate path optimization method is urgently needed. Summary of the invention

[0005] The first aspect of the present application provides a path optimization method, which is applied to a driving device. The method specifically includes: obtaining the motion state information of the driving device and the driving path to be optimized, the driving path includes multiple sampling positions; the motion state information represents the motion state of each of the sampling positions; wherein the motion state is represented based on multiple flat variables or derivatives of flat variables; each of the flat variables corresponds to a position of one dimension of the sampling position; the motion state of at least one of the sampling positions includes curvature and curvature change rate; according to the motion state information, determining the target penalty function; the target penalty function is related to the driving feasibility and smoothness of the driving device when driving along the multiple sampling positions; wherein the driving feasibility is related to the curvature, and the smoothness is related to the curvature and the curvature change rate; according to the target penalty function, optimizing the driving path.

[0006] For example, driving feasibility may be related to curvature (for example, a turning radius constraint may be constructed based on curvature), and smoothness may be related to curvature and the rate of change of curvature (for example, curvature itself is a representation of smoothness, and the rate of change of curvature may be used to construct an angular acceleration constraint).

[0007] This application expresses the motion state through flat variables, which can avoid solving differential equations in the trajectory optimization process on the one hand, and reduce the dimension of the expression space on the other hand, thereby reducing the difficulty of trajectory optimization. In addition, directly describing nonlinear constraints such as curvature and its rate of change in the flat variable space can avoid repeated discretization of displacement or time difference, so as to achieve reasonable constraint curvature jump while making the overall steering consistency as high as possible and the amplitude as small as possible.

[0008] In one possible implementation, the driving path is optimized based on the motion state information, including: determining a target penalty function based on the motion state information; the target penalty function is related to the driving performance of the driving device when driving along the multiple sampling position points; optimizing the driving path based on the target penalty function; wherein the goal of optimizing the driving path is to reduce the value of the target penalty function.

[0009] In one possible implementation, obtaining the motion state information of the traveling device includes: obtaining initial motion state information of the traveling device, wherein the initial motion state information is expressed in a first space; changing the expression space of the initial motion state information from the first space to a second space to obtain the motion state information, wherein the dimension of the second space is less than or equal to the dimension of the first space, and each dimension of the second space corresponds to a flat variable or a derivative of a flat variable.

[0010] When the dimension of the second space is smaller than the dimension of the first space, it is equivalent to the quantitative representation of the variables used to represent the motion state, thereby reducing the dimension of the expression space and reducing the difficulty of optimization.

[0011] In a possible implementation, the motion state is at least one of velocity, acceleration, angular velocity, angular acceleration, curvature, and curvature change rate.

[0012] In a possible implementation, the driving path is represented by a high-order polynomial with respect to a time variable, and the optimizing the driving path includes: optimizing the time variable and the polynomial coefficients of the high-order polynomial.

[0013] In the existing implementation, a plurality of sampling points can be sampled for a section of the driving route to be optimized, and the position of the sampling points is used to represent the section of the driving route. When optimizing the route, the position of each sampling point is optimized. On the one hand, in order to ensure the optimization accuracy, the number of sampling points cannot be too small, so the number of points that need to be optimized is also large, resulting in a large calculation overhead. On the other hand, since the position of the sampling point is only an approximate fit of the route, even if the number of sampling points is large, the accuracy and effect of the path optimization are still limited.

[0014] In the embodiment of the present application, the driving path to be optimized is represented by a high-order polynomial with respect to the time variable. When optimizing the driving path, the objects to be optimized are the time variable and the polynomial coefficients. Since the number of objects to be optimized is reduced, the calculation overhead during optimization can be reduced, and the shape of the driving path can be more accurately represented by the high-order polynomial, thereby improving the optimization accuracy and effect.

[0015] In a possible implementation, the multiple sampling position points include a first sampling point and a second sampling point, the curvature or curvature change rate corresponding to the first sampling point in the driving path is greater than that of the second sampling point, the first sampling point is obtained by sampling the driving path according to a first sampling resolution, the second sampling point is obtained by sampling the driving path according to a second sampling resolution, and the first sampling resolution is greater than the second sampling resolution.

[0016] In one possible implementation, variational rate sampling can be performed on each trajectory segment, so that the number of samples of points with larger curvature or curvature change rate is greater. Since the parts with larger curvature or curvature change rate are the objects that need to be optimized, the above-mentioned variational rate sampling method can improve the effect of subsequent optimization.

[0017] In a possible implementation, the driving path to be optimized is a path selected from a planned path of the driving device, and both ends of the driving path are sampled from the planned path based on curvature and / or curvature change rate being less than a threshold.

[0018] When optimizing the driving trajectory, in order to ensure the optimization effect, the endpoints of the segmented path (which may also be referred to as anchor points in the embodiment of the present application) cannot be points whose curvature or curvature change rate is greater than a threshold. The reason is that the endpoints are often not adjusted in the optimization algorithm.

[0019] In a possible implementation, the driving path includes sampling points whose curvature and / or curvature change rate is greater than a threshold.

[0020] When optimizing the driving trajectory, in order to ensure the optimization effect, it is often necessary to adjust the positions with larger curvature or larger curvature change rate in the driving trajectory. Therefore, when the initial path is divided, each divided path (including the driving path to be optimized in the embodiment of the present application) can include at least one sampling position point with larger curvature or larger curvature change rate (also referred to as a bad point in the embodiment of the present application).

[0021] In a possible implementation, the target penalty function is negatively correlated with the driving feasibility and / or smoothness of the driving device when driving along the multiple sampling position points.

[0022] In a possible implementation, the method also includes: obtaining point cloud data of the driving environment of the driving device at the multiple sampling positions, the point cloud data being used to characterize obstacles in the driving environment; obtaining collision risk information of the driving device at each of the sampling positions based on the interaction between each point cloud in the point cloud data and the Euclidean distance field ESDF of the driving device; and optimizing the driving path based on the motion state information, including: optimizing the driving path based on the motion state information and the collision risk information.

[0023] In one possible implementation, the collision risk information of the traveling device at each sampling position is obtained based on the interaction between each point cloud in the point cloud data and the Euclidean distance field ESDF of the traveling device, including: obtaining the collision risk information of the traveling device at each sampling position by determining the ESDF gradient based on the ESDF value at the corresponding position of each point cloud in the point cloud data in the ESDF, and the ESDF value at the nearby position of the corresponding position.

[0024] In a possible implementation, the traveling device includes a robot or a vehicle.

[0025] In a second aspect, the present application provides a path optimization device, comprising:

[0026] An acquisition module, used for acquiring motion state information of a driving device and a driving path to be optimized, wherein the driving path includes a plurality of sampling positions; the motion state information represents the motion state of each of the sampling positions; wherein the motion state is represented based on a plurality of flat variables or derivatives of flat variables; each of the flat variables corresponds to a position of one dimension of the sampling position; and the motion state of at least one of the sampling positions includes curvature and curvature change rate;

[0027] A processing module, configured to determine a target penalty function according to the motion state information; the target penalty function is related to driving feasibility and smoothness when the driving device drives along the plurality of sampling positions; wherein the driving feasibility is related to the curvature, and the smoothness is related to the curvature and the curvature change rate;

[0028] The driving path is optimized according to the target penalty function.

[0029] In a possible implementation, a goal of optimizing the driving path is to reduce a value of the target penalty function.

[0030] In a possible implementation, the processing module is specifically configured to:

[0031] Determining a target penalty function according to the motion state information; the target penalty function is related to the driving performance of the driving device when driving along the multiple sampling position points;

[0032] The driving path is optimized according to the target penalty function; wherein the goal of optimizing the driving path is to reduce the value of the target penalty function.

[0033] In a possible implementation, the acquisition module is specifically used to:

[0034] Acquiring initial motion state information of the traveling device, where the initial motion state information is expressed in a first space;

[0035] The expression space of the initial motion state information is changed from the first space to the second space to obtain the motion state information, wherein the dimension of the second space is less than or equal to the dimension of the first space, and each dimension of the second space corresponds to a flat variable or a derivative of a flat variable.

[0036] In a possible implementation, the motion state is at least one of velocity, acceleration, angular velocity, angular acceleration, curvature, and curvature change rate.

[0037] In a possible implementation, the driving path is represented by a high-order polynomial with respect to a time variable, and the optimizing the driving path includes: optimizing the time variable and the polynomial coefficients of the high-order polynomial.

[0038] In a possible implementation, the multiple sampling position points include a first sampling point and a second sampling point, the curvature or curvature change rate corresponding to the first sampling point in the driving path is greater than that of the second sampling point, the first sampling point is obtained by sampling the driving path according to a first sampling resolution, the second sampling point is obtained by sampling the driving path according to a second sampling resolution, and the first sampling resolution is greater than the second sampling resolution.

[0039] In a possible implementation, the driving path to be optimized is a path selected from a planned path of the driving device, and both ends of the driving path are sampled from the planned path based on curvature and / or curvature change rate being less than a threshold.

[0040] In a possible implementation, the driving path includes sampling points whose curvature and / or curvature change rate is greater than a threshold.

[0041] In a possible implementation, the target penalty function is negatively correlated with the driving feasibility and / or smoothness of the driving device when driving along the multiple sampling position points.

[0042] In a possible implementation, the acquisition module is further used to:

[0043] Acquire point cloud data of a driving environment where the driving device is located at the plurality of sampling positions, wherein the point cloud data is used to characterize obstacles in the driving environment;

[0044] The processing module is further used to: obtain collision risk information of the traveling device at each sampling position point according to the interaction between each point cloud in the point cloud data and the Euclidean distance field ESDF of the traveling device;

[0045] The processing module is specifically used for:

[0046] The driving path is optimized according to the motion state information and the collision risk information.

[0047] In a possible implementation, the processing module is specifically configured to:

[0048] According to the ESDF value of the corresponding position of each point cloud in the point cloud data in the ESDF, and the ESDF gradient determined by the ESDF value of the position near the corresponding position, the collision risk information of the driving device at each sampling position point is obtained.

[0049] In a possible implementation, the traveling device includes a robot or a vehicle.

[0050] The third aspect of the present application provides a driving device, which may include a processor, the processor and a memory are coupled, the memory stores program instructions, and when the program instructions stored in the memory are executed by the processor, the method of the first aspect or any implementation of the first aspect is implemented. For the steps in each possible implementation of the first aspect executed by the processor, the details can be referred to the first aspect, and will not be repeated here.

[0051] The fourth aspect of the present application provides a computer-readable storage medium, in which a computer program is stored. When the computer-readable storage medium is run on a computer, the computer executes the method of the first aspect or any implementation manner of the first aspect.

[0052] A fifth aspect of the present application provides a circuit system, the circuit system includes a processing circuit, and the processing circuit is configured to execute the method of the above-mentioned first aspect or any implementation manner of the first aspect.

[0053] The sixth aspect of the present application provides a computer program product, which, when executed on a computer, enables the computer to execute the method of the first aspect or any implementation manner of the first aspect.

[0054] The seventh aspect of the present application provides a chip system, which includes a processor for supporting a server or a threshold value acquisition device to implement the functions involved in the above-mentioned first aspect or any implementation of the first aspect, for example, sending or processing the data and / or information involved in the above-mentioned method. In one possible design, the chip system also includes a memory, which is used to store program instructions and data necessary for the server or communication device. The chip system can be composed of chips, or it can include chips and other discrete devices.

[0055] The beneficial effects of the second to seventh aspects mentioned above can be referred to the introduction of the first aspect mentioned above, and will not be repeated here. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 A schematic diagram of an inspection robot for the power transmission industry provided in an embodiment of the present application;

[0057] Figure 2 A schematic diagram of a mine car traveling in a mine provided in an embodiment of the present application;

[0058] Figure 3 A schematic diagram of the structure of a robot 500 provided in an embodiment of the present application;

[0059] Figure 4 A schematic diagram of the structure of a vehicle 100 provided in an embodiment of the present application;

[0060] Figure 5 A schematic diagram of a flow chart of a path optimization method provided in an embodiment of the present application;

[0061] Figure 6 A schematic diagram of the structure of a traveling device provided in an embodiment of the present application;

[0062] Figure 7 A schematic diagram of the structure of a traveling device provided in an embodiment of the present application;

[0063] Figure 8 A schematic diagram of the structure of a chip provided in an embodiment of the present application;

[0064] Fig. 9A schematic diagram of the structure of a computer-readable storage medium provided in an embodiment of the present application. DETAILED DESCRIPTION

[0065] The embodiments of the present application will be described below in conjunction with the accompanying drawings. A person skilled in the art will appreciate that, with the development of technology and the emergence of new scenarios, the technical solutions provided in the embodiments of the present application are also applicable to similar technical problems.

[0066] The terms "first", "second", etc. in the specification and claims of the present application and the above-mentioned drawings are used to distinguish similar objects, and need not be used to describe a specific order or sequential order. It should be understood that the terms used in this way can be interchangeable under appropriate circumstances, which is only to describe the distinction mode adopted by the objects of the same attributes when describing in the embodiments of the present application. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions, so that the process, method, system, product or equipment comprising a series of units need not be limited to those units, but may include other units that are not clearly listed or inherent to these processes, methods, products or equipment.

[0067] To facilitate understanding, the technical terms involved in the embodiments of the present application are first introduced below.

[0068] (1) Sign distance function (SDF)

[0069] SDF, also known as oriented distance function, determines the distance from a point to the boundary of a finite region in space and defines the sign of the distance at the same time. If the point is inside the boundary of the region, the sign of the distance is positive; if the point is outside the boundary of the region, the sign of the distance is negative; if the point is on the boundary of the region, the distance is 0.

[0070] (2) Euclidean Space

[0071] Euclidean space, also known as Euclidean space for short, is a generalization of the two-dimensional and three-dimensional space studied by Euclid in mathematics. This generalization converts Euclid's concepts of distance, length and angle into a coordinate system of any number of dimensions.

[0072] (3) Euclidea Signed Distance Field (ESDF)

[0073] ESDF is the result of applying SDF in Euclidean space and represents the shortest distance from a point in Euclidean space to the surrounding obstacles.

[0074] (4) Based on truncated signed distance function (TSDF)

[0075] TSDF is a penalty function obtained by adding a truncation zone to SDF. It also determines the distance from a point to the region boundary in a finite region in space and defines the sign of the distance at the same time. If the distance between the point and the region boundary is closer, the TSDF value is larger; if the distance between the point and the region boundary is farther, the TSDF value is smaller; if the distance between the point and the region boundary exceeds the truncation zone value (i.e. a certain threshold), the TSDF value is 0.

[0076] (5) Penalty function

[0077] The penalty function is an augmented objective function obtained by adding an obstacle function to the original objective function when solving an optimization problem (optimization without linear constraints and optimization without nonlinear constraints). The function of the penalty function is to assign a maximum value to infeasible points or points that attempt to cross the boundary and escape the feasible domain, that is, to transform the constrained optimization problem into an unconstrained optimization problem.

[0078] (6) Random sample consensus (RANSAC)

[0079] RANSAC is an algorithm that calculates the mathematical model parameters of the data based on a set of sample data sets containing abnormal data to obtain valid sample data.

[0080] (7) Bounding box

[0081] The bounding box algorithm is an algorithm for finding the optimal bounding space of a discrete point set. The basic idea is to use a geometric body with a slightly larger volume and simpler characteristics (called a bounding box) to approximately replace complex geometric objects.

[0082] Common bounding box algorithms include Axis-aligned bounding box (AABB), oriented bounding box (OBB), and fixed directions hull (FDH). Collision detection has a wide range of applications in virtual reality, computer-aided design and manufacturing, games, and robotics, and has even become a key technology. The bounding box algorithm is one of the important methods for preliminary detection of collision interference.

[0083] (8) Point cloud data

[0084] Point cloud data refers to a set of vectors in a three-dimensional coordinate system, which can reflect the real situation of the surface or objects with high accuracy, such as ground conditions, reflection characteristics of objects, etc. Generally speaking, point cloud data is obtained by emitting lasers to the objects to be scanned by a laser radar, and then receiving the lasers reflected by objects in the environment. The data obtained by the laser radar scanning the object is recorded in the form of points, each of which contains three-dimensional coordinates, and some may contain color information (RGB) or reflection intensity information (Intensity).

[0085] (9) Laser Radar

[0086] LiDAR is a radar system that uses laser beams to detect the position, speed and other characteristic quantities of a target. The working principle of LiDAR is to transmit a detection signal (laser beam) to the target, and then compare and appropriately process the received signal reflected from the target (target echo) with the transmitted signal to obtain relevant information about the target, such as target distance, direction, height, speed, attitude, and even shape parameters, thereby realizing the detection of targets such as the ground or obstacles on the surface. Generally speaking, LiDAR can be composed of a laser transmitter, an optical receiver and an information processing system. The laser transmitter converts electrical pulses into light pulses and transmits them. The optical receiver then converts the light pulses reflected from the target into electrical pulses and sends them to the information processing system for processing.

[0087] (10) Inertial sensor

[0088] An inertial sensor is a sensor, often referred to as an inertial measurement unit (IMU). Inertial sensors are mainly used to detect and measure acceleration, rotation, tilt, shock, vibration, and multi-degree-of-freedom (DoF) motion, and are important components for solving navigation, orientation, and motion carrier control. The principle of inertial sensors is realized by the law of inertia, and mainly includes accelerometers and angular velocity meters (gyroscopes), which can measure the acceleration and angular velocity of an object in three directions in a three-dimensional coordinate system.

[0089] (11) Signal-to-noise ratio

[0090] The signal-to-noise ratio refers to the ratio of the signal to the noise in an electronic device or electronic system. The signal here refers to the electronic signal from outside the device that needs to be processed by this device, and the noise refers to the irregular additional signal (or information) that does not exist in the original signal after passing through the device, and this signal does not change with the change of the original signal.

[0091] (12)Position

[0092] Pose is a description of the position and posture of an object in a specified coordinate system. Among them, position refers to the positioning of the object in space. The position of a rigid body can be represented by a 3x1 matrix, that is, the position of the rigid body in the three-dimensional coordinate system. Pose refers to the orientation of the object in space. The posture of a rigid body can also be represented by a 3x3 matrix, that is, the posture of the rigid body coordinate system in the base coordinate system.

[0093] (13) Grader

[0094] A motor grader is an earth-moving machine that uses a scraper to level the ground. The scraper is installed between the front and rear axles of the machine and can be raised and lowered, tilted, rotated and extended. It is flexible and accurate in movement, easy to operate, and can level the ground with high precision. It is suitable for building roadbeds and ground, building slopes, and digging ditches. It can also stir ground mixtures, clear snow, push loose materials, and maintain dirt and gravel roads.

[0095] (14) Elevation map

[0096] An elevation map is a map used to show the altitude (i.e. elevation) of a certain area.

[0097] (15) Covariance

[0098] Covariance is used in probability theory and statistics to measure the overall error of two variables. Variance is a special case of covariance, that is, when the two variables are equal.

[0099] Covariance represents the overall error of two variables, which is different from the variance of the error of only one variable. If the two variables have the same trend, that is, if one is greater than its expected value and the other is also greater than its expected value, then the covariance between the two variables is positive. If the two variables have opposite trends, that is, one is greater than its expected value and the other is less than its expected value, then the covariance between the two variables is negative.

[0100] (16) Variance

[0101] Variance is a measure of the degree of dispersion of a random variable or a set of data in probability theory and statistics. In probability theory, variance is used to measure the degree of deviation between a random variable and its mathematical expectation (i.e., mean). Variance (sample variance) in statistics is the average of the squared values ​​of the difference between each sample value and the mean of all sample values. In many practical problems, studying variance, i.e., the degree of deviation, is of great significance.

[0102] (17) Curvature (kappa): Curvature is defined as the derivative of the unit tangent vector function, that is, the rotation rate of the tangent direction angle at a point on the curve with respect to the arc length. It is defined by differentiation and indicates the degree to which the curve deviates from a straight line.

[0103] (18) Curvature change rate (dkappa): the derivative of curvature.

[0104] In this embodiment, the path optimization method can be applied to a driving device, such as a robot or a vehicle. Specifically, the robot is, for example, an indoor robot (such as a sweeping robot, a household robot, a welcoming robot, etc.), a patrol robot in the power transmission industry (such as a patrol robot inside a substation), a handling robot or a patrol robot in the coal mining industry, a handling robot or an underground detection robot in the logistics industry, and other robots. For these robots, the environment in which these robots travel is, for example, an indoor environment with obstacles of irregular non-convex shapes, or an outdoor environment with obstacles or uneven terrain. For example, please refer to Figure 1 , Figure 1 A schematic diagram of an inspection robot for the power transmission industry provided in an embodiment of the present application.

[0105] Vehicles include mining cars in mines, trucks on construction sites, and transporters in warehouses. The environments in which these vehicles travel are prone to uneven surfaces or obstacles. Therefore, in order to ensure the normal driving of the vehicles, it is necessary to plan the driving paths of the vehicles to ensure that the vehicles can avoid obstacles. For example, please refer to Figure 2 , Figure 2 A schematic diagram of a mine car traveling in a mine provided in an embodiment of the present application.

[0106] In order to facilitate the understanding of this solution, the embodiments of this application are combined with Figure 3 and Figure 4 The structures of the robot and vehicle provided in this application are introduced.

[0107] Please refer to Figure 3 , Figure 3 Schematic diagram of the structure of a robot 500 provided in an embodiment of the present application. Figure 3 As shown, the robot 500 may include an image acquisition module 501 , a sensor 502 , a processor 503 , a memory 504 , and a communication module 505 .

[0108] The processor 503 may include one or more processing units, for example, the processor 503 may include an application processor (AP), a modem processor, a graphics processor (GPU), an image signal processor (ISP), a controller, a memory, a video codec, a digital signal processor (DSP), a baseband processor, and / or a neural-network processing unit (NPU), etc. Different processing units may be independent devices or integrated into one or more processors.

[0109] The controller may be the nerve center and command center of the robot 500. The controller may generate an operation control signal according to the instruction operation code and the timing signal to complete the control of fetching and executing instructions.

[0110] The processor 503 may also be provided with a memory for storing instructions and data. In some embodiments, the memory in the processor is a cache memory. The memory may store instructions or data that the processor has just used or circulated. If the processor needs to use the instruction or data again, it may be directly called from the memory. This avoids repeated access, reduces the waiting time of the processor 503, and thus improves the efficiency of the system.

[0111] In some embodiments, the processor 503 may include one or more interfaces. The interface may include an inter-integrated circuit (I2C) interface, a universal asynchronous receiver / transmitter (UART) interface, a mobile industry processor interface (MIPI), a general-purpose input / output (GPIO) interface, a subscriber identity module (SIM) interface, and / or a universal serial bus (USB) interface, etc.

[0112] The interface is a bidirectional synchronous serial bus, including a serial data line (SDA) and a serial clock line (SCL). In some embodiments, the processor may include multiple I2C buses. The processor may be coupled to sensors, cameras, etc. through different I2C bus interfaces.

[0113] The UART interface is a universal serial data bus for asynchronous communication. The bus can be a bidirectional communication bus. It converts the data to be transmitted between serial communication and parallel communication. In some embodiments, the UART interface is generally used to connect the processor 503 and the communication module 505. For example, the processor 503 communicates with the Bluetooth module in the wireless communication module through the UART interface to implement the Bluetooth function.

[0114] The IPI interface can be used to connect the processor to peripheral devices such as cameras. The MIPI interface includes a camera serial interface (CSI), etc. In some embodiments, the processor and the camera communicate through the CSI interface to realize the shooting function of the robot 500.

[0115] The GPIO interface can be configured by software. The GPIO interface can be configured as a control signal or as a data signal. In some embodiments, the GPIO interface can be used to connect the processor to a camera, a wireless communication module, a sensor module, etc. The GPIO interface can also be configured as an I2C interface, a UART interface, a MIPI interface, etc.

[0116] The USB interface is an interface that complies with USB standard specifications, and specifically may be a Mini USB interface, a Micro USB interface, a USB Type C interface, etc. The USB interface may be used to connect a charger to charge the robot 500, and may also be used to transmit data between the robot 500 and peripheral devices. The interface may also be used to connect other robots 500, etc.

[0117] The image acquisition module 501 can collect image information around the robot 500, for example, taking photos or videos, etc. The robot 500 can implement the image acquisition function through an ISP, a camera, a video codec, a GPU, and an application processor.

[0118] ISP is used to process the data fed back by the camera. For example, when taking a photo, the shutter is opened, and the light is transmitted to the camera photosensitive element through the lens. The light signal is converted into an electrical signal, and the camera photosensitive element transmits the electrical signal to the ISP for processing and converts it into an image visible to the naked eye. ISP can also perform algorithm optimization on the noise, brightness, etc. of the image. ISP can also optimize the exposure, color temperature and other parameters of the shooting scene. In some embodiments, ISP can be set in the camera.

[0119] The camera is used to capture still images or videos. The object generates an optical image through the lens and projects it onto the photosensitive element. The photosensitive element can be a charge coupled device (CCD) or a complementary metal-oxide-semiconductor (CMOS) phototransistor. The photosensitive element converts the light signal into an electrical signal, and then passes the electrical signal to the ISP to be converted into a digital image signal. The ISP outputs the digital image signal to the DSP for processing. The DSP converts the digital image signal into an image signal in a standard RGB, YUV or other format. In some embodiments, the robot 500 may include 1 or N cameras, where N is a positive integer greater than 1.

[0120] The sensor 502 can obtain information such as the moving speed, moving direction, and distance between the robot 500 and surrounding objects. Exemplarily, the sensor 502 can include a gyroscope sensor, a speed sensor, an acceleration sensor, a distance sensor, and the like.

[0121] Among them, the gyroscope sensor can be used to determine the motion posture of the robot 500. In some embodiments, the angular velocity of the robot 500 around three axes (i.e., x, y, and z axes) can be determined by the gyroscope sensor. The gyroscope sensor can be used for anti-shake shooting. Exemplarily, when the robot 500 is performing image acquisition, the gyroscope sensor detects the angle of the robot 500 shaking, calculates the distance that the lens module needs to compensate based on the angle, and allows the lens to offset the shaking of the robot 500 through reverse movement to achieve anti-shake. The gyroscope sensor can also be used for navigation or calculating the unevenness of the ground, judging whether the robot 500 is trapped, and other scenarios.

[0122] The speed sensor is used to measure the moving speed. In some embodiments, the robot 500 measures the moving speed at the current moment through the speed sensor, and can be combined with the distance sensor to predict the environment where the robot 500 will be at the next moment based on the environment where the robot 500 is at the current moment.

[0123] The acceleration sensor can detect the magnitude of acceleration in various directions (generally three axes) of the robot 500. When the robot 500 is stationary, the magnitude and direction of gravity can be detected.

[0124] The distance sensor is used to measure the distance. The robot 500 can measure the distance by infrared or laser. In some embodiments, when shooting a scene, the robot 500 can use the distance sensor to measure the distance to achieve fast focusing.

[0125] The memory 504 may include an external memory and an internal memory. The external memory interface may be used to connect an external memory card, such as a Micro SD card, to expand the storage capacity of the robot 500. The external memory card communicates with the processor via the external memory interface to implement a data storage function. For example, a sample information file is stored in the external memory card.

[0126] The internal memory can be used to store computer executable program codes, which include instructions. The processor executes various functional applications and data processing of the robot 500 by running the instructions stored in the internal memory. The internal memory may include a program storage area and a data storage area. Among them, the program storage area can store an operating system and an application required for at least one function. The data storage area can store data created during the use of the robot 500. In addition, the internal memory may include a high-speed random access memory, and may also include a non-volatile memory, such as at least one disk storage device, a flash memory device, a universal flash storage (UFS), etc.

[0127] The wireless communication function of the robot 500 can be implemented through the communication module 505. For example, through the communication module 505, the robot 500 can communicate with other devices, such as communication with a server. As an example, the communication module 505 may include an antenna 1, an antenna 2, a mobile communication module, a wireless communication module, a modem processor, a baseband processor, and the like.

[0128] Antenna 1 and antenna 2 are used to transmit and receive electromagnetic wave signals. Each antenna in robot 500 can be used to cover a single or multiple communication frequency bands. Different antennas can also be reused to improve the utilization of antennas. For example, antenna 1 can be reused as a diversity antenna for a wireless local area network. In some other embodiments, the antenna can be used in combination with a tuning switch.

[0129] The mobile communication module can provide solutions for wireless communications including 2G / 3G / 4G / 5G applied to the robot 500. The mobile communication module may include at least one filter, a switch, a power amplifier, a low noise amplifier (LNA), etc. The mobile communication module can receive electromagnetic waves from the antenna 1, and filter, amplify, and process the received electromagnetic waves, and transmit them to the modulation and demodulation processor for demodulation. The mobile communication module can also amplify the signal modulated by the modulation and demodulation processor, and convert it into electromagnetic waves for radiation through the antenna 1. In some embodiments, at least some of the functional modules of the mobile communication module may be arranged in the processor. In some embodiments, at least some of the functional modules of the mobile communication module may be arranged in the same device as at least some of the modules of the processor.

[0130] The wireless communication module can provide wireless communication solutions including wireless local area networks (WLAN) (such as wireless fidelity (Wi-Fi) networks), Bluetooth (BT), global navigation satellite system (GNSS), infrared technology (IR), etc., which are applied to the robot 500. The wireless communication module can be one or more devices integrating at least one communication processing module. The wireless communication module receives electromagnetic waves via antenna 2, modulates and filters the electromagnetic wave signals, and sends the processed signals to the processor. The wireless communication module can also receive the signal to be sent from the processor, modulate the frequency of the signal, amplify it, and convert it into electromagnetic waves for radiation through antenna 2.

[0131] In some embodiments, the antenna 1 of the robot 500 is coupled to the mobile communication module, and the antenna 2 is coupled to the wireless communication module, so that the robot 500 can communicate with the server and other devices through wireless communication technology. The wireless communication technology may include global system for mobile communications (GSM), general packet radio service (GPRS), code division multiple access (CDMA), wideband code division multiple access (WCDMA), time division code division multiple access (TD-SCDMA), long term evolution (LTE), BT, GNSS, WLAN, NFC, FM, and / or IR technology, etc. The GNSS may include global positioning system (GPS), global navigation satellite system (GLONASS), Beidou navigation satellite system (BDS), quasi-zenith satellite system (QZSS) and / or satellite based augmentation system (SBAS).

[0132] It is understood that the structure shown in this embodiment does not constitute a specific limitation on the robot 500. In other embodiments, the robot 500 may include more or fewer components than shown in the figure, or combine some components, or separate some components, or arrange the components differently. The components shown in the figure may be implemented in hardware, software, or a combination of software and hardware.

[0133] See also Figure 4 , Figure 4 A schematic diagram of the structure of a vehicle 100 provided in an embodiment of the present application.

[0134] like Figure 4As shown, in one embodiment, the vehicle 100 can be configured in a fully or partially automated driving mode. For example, the vehicle 100 can control itself while in the automated driving mode, and can determine the current state of the vehicle and its surroundings through human operation, determine the possible behavior of at least one other vehicle in the surrounding environment, and determine the confidence level corresponding to the possibility of the other vehicle performing the possible behavior, and control the vehicle 100 based on the determined information. When the vehicle 100 is in the automated driving mode, the vehicle 100 can be set to operate without human interaction.

[0135] The vehicle 100 may include various subsystems, such as a travel system 102, a sensor system 104, a control system 106, one or more peripheral devices 108, and a power supply 110, a computer system 101, and a user interface 116. Optionally, the vehicle 100 may include more or fewer subsystems, and each subsystem may include multiple components, such as each subsystem includes multiple electronic control units (ECUs). In addition, each subsystem and component of the vehicle 100 may be interconnected by wire or wirelessly.

[0136] The propulsion system 102 may include components that provide powered movement for the vehicle 100. In one embodiment, the propulsion system 102 may include an engine 118, an energy source 119, a transmission 120, and wheels / tires 121. The engine 118 may be an internal combustion engine, an electric motor, an air compression engine, or other types of engine combinations, such as a hybrid engine consisting of a gasoline engine and an electric motor, or a hybrid engine consisting of an internal combustion engine and an air compression engine. The engine 118 converts the energy source 119 into mechanical energy.

[0137] Examples of energy source 119 include gasoline, diesel, other petroleum-based fuels, propane, other compressed gas-based fuels, ethanol, solar panels, batteries, and other sources of electricity. Energy source 119 may also provide energy to other systems of vehicle 100.

[0138] The transmission 120 can transmit mechanical power from the engine 118 to the wheels 121. The transmission 120 may include a gearbox, a differential, and a drive shaft. In one embodiment, the transmission 120 may also include other devices, such as a clutch. Among them, the drive shaft may include one or more shafts that can be coupled to one or more wheels 121.

[0139] The sensor system 104 may include several sensors that sense information about the environment surrounding the vehicle 100. For example, the sensor system 104 may include a global positioning system 122 (the positioning system may be a GPS system, or a Beidou system or other positioning systems), an inertial measurement unit (IMU) 124, a radar 126, a laser rangefinder 128, and a camera 130. The sensor system 104 may also include sensors of the internal systems of the monitored vehicle 100 (e.g., an in-vehicle air quality monitor, a fuel gauge, an oil temperature gauge, etc.). Sensor data from one or more of these sensors may be used to detect objects and their corresponding characteristics (position, shape, direction, speed, etc.). Such detection and recognition are key functions for the safe operation of the vehicle 100.

[0140] The global positioning system 122 may be used to estimate the geographic location of the vehicle 100. The IMU 124 is used to sense position and orientation changes of the vehicle 100 based on inertial acceleration. In one embodiment, the IMU 124 may be a combination of an accelerometer and a gyroscope.

[0141] Radar 126 may utilize radio signals to sense objects within the surrounding environment of vehicle 100. In some embodiments, in addition to sensing objects, radar 126 may also be used to sense the speed and / or heading of an object.

[0142] The laser rangefinder 128 may utilize laser light to sense objects in the environment in which the vehicle 100 is located. In some embodiments, the laser rangefinder 128 may include one or more laser sources, a laser scanner, and one or more processing modules, among other system components.

[0143] The camera 130 may be used to capture multiple images of the surrounding environment of the vehicle 100. The camera 130 may be a still camera or a video camera.

[0144] The control system 106 controls the operation of the vehicle 100 and its components. The control system 106 may include various elements, including a steering system 132 , a throttle 134 , a brake unit 136 , a computer vision system 140 , a path control system 142 , and an obstacle avoidance system 144 .

[0145] The steering system 132 is operable to adjust the forward direction of the vehicle 100. For example, in one embodiment, it may be a steering wheel system.

[0146] The throttle 134 is used to control the operating speed of the engine 118 and, in turn, the speed of the vehicle 100 .

[0147] The brake unit 136 is used to control the deceleration of the vehicle 100. The brake unit 136 can use friction to slow down the wheel 121. In other embodiments, the brake unit 136 can convert the kinetic energy of the wheel 121 into electric current. The brake unit 136 can also take other forms to slow down the rotation speed of the wheel 121 to control the speed of the vehicle 100.

[0148] The computer vision system 140 can be operated to process and analyze the images captured by the camera 130 in order to identify objects and / or features in the environment surrounding the vehicle 100. The objects and / or features may include traffic signs, road boundaries, and obstacles. The computer vision system 140 may use object recognition algorithms, Structure from Motion (SFM) algorithms, video tracking, and other computer vision techniques. In some embodiments, the computer vision system 140 may be used to map the environment, track objects, estimate the speed of objects, and the like.

[0149] The route control system 142 is used to determine the driving route of the vehicle 100. In some embodiments, the route control system 142 may combine data from the GPS 122 and one or more predetermined maps to determine the driving route for the vehicle 100.

[0150] The obstacle avoidance system 144 is used to identify, evaluate, and avoid or otherwise negotiate potential obstacles in the environment of the vehicle 100 .

[0151] Of course, in one example, the control system 106 may include additional or alternative components other than those shown and described, or may also reduce some of the components shown above.

[0152] The vehicle 100 interacts with external sensors, other vehicles, other computer systems, or users through peripherals 108. The peripherals 108 may include a wireless communication system 146, an onboard computer 148, a microphone 150, and / or a speaker 152.

[0153] In some embodiments, the peripheral device 108 provides a means for the user of the vehicle 100 to interact with the user interface 116. For example, the onboard computer 148 can provide information to the user of the vehicle 100. The user interface 116 can also operate the onboard computer 148 to receive input from the user. The onboard computer 148 can be operated via a touch screen. In other cases, the peripheral device 108 can provide a means for the vehicle 100 to communicate with other devices located in the vehicle. For example, the microphone 150 can receive audio (e.g., voice commands or other audio input) from the user of the vehicle 100. Similarly, the speaker 152 can output audio to the user of the vehicle 100.

[0154] The wireless communication system 146 can communicate wirelessly with one or more devices directly or via a communication network. For example, the wireless communication system 146 can use 3G cellular communication, such as CDMA, EVDO, GSM / GPRS, or 4G cellular communication, such as LTE. Or 5G cellular communication. The wireless communication system 146 can communicate with a wireless local area network (WLAN) using WiFi. In some embodiments, the wireless communication system 146 can communicate directly with the device using an infrared link, Bluetooth, or ZigBee. Other wireless protocols, such as various vehicle communication systems, for example, the wireless communication system 146 may include one or more dedicated short range communications (DSRC) devices, which may include public and / or private data communications between vehicles and / or roadside stations.

[0155] The power source 110 can provide power to various components of the vehicle 100. In one embodiment, the power source 110 can be a rechargeable lithium-ion or lead-acid battery. One or more battery packs of such batteries can be configured as a power source to provide power to various components of the vehicle 100. In some embodiments, the power source 110 and the energy source 119 can be implemented together, such as in some all-electric vehicles.

[0156] Some or all functions of the vehicle 100 are controlled by a computer system 101. The computer system 101 may include at least one processor 113 that executes instructions 115 stored in a non-transitory computer-readable medium such as a data storage device 114. The computer system 101 may also be a plurality of computing devices that control individual components or subsystems of the vehicle 100 in a distributed manner.

[0157] Processor 113 may be any conventional processor, such as a commercially available CPU. Alternatively, the processor may be a dedicated device such as an ASIC or other hardware-based processor. Figure 4 The processor, memory, and other elements of the computer 110 are functionally illustrated as being in the same block, but one of ordinary skill in the art will appreciate that the processor, computer, or memory may actually include multiple processors, computers, or memories that may or may not be stored within the same physical housing.

[0158] For example, the memory may be a hard drive or other storage medium located in a housing different from the computer 110. Thus, references to a processor or computer will be understood to include references to a collection of processors or computers or memories that may or may not operate in parallel. Rather than using a single processor to perform the steps described herein, some components, such as the steering assembly and the speed reduction assembly, may each have their own processor that performs only calculations related to the functions specific to the component.

[0159] In various aspects described herein, the processor may be located remote from the vehicle and in wireless communication with the vehicle. In other aspects, some of the processes described herein are performed on a processor disposed within the vehicle and others are performed by a remote processor, including taking the necessary steps to perform a single maneuver.

[0160] In some embodiments, the memory 114 may contain instructions 115 (e.g., program logic) that may be executed by the processor to perform various functions of the vehicle 100, including those described above. The data storage 114 may also contain additional instructions, including instructions to send data to, receive data from, interact with, and / or control one or more of the propulsion system 102, the sensor system 104, the control system 106, and the peripherals 108.

[0161] In addition to instructions 115, memory 114 may also store data such as road maps, route information, the vehicle's location, direction, speed, and other such vehicle data, and other information. Such information may be used by vehicle 100 and computer system 101 during operation of vehicle 100 in autonomous, semi-autonomous, and / or manual modes.

[0162] The user interface 116 is used to provide information to or receive information from a user of the vehicle 100. Optionally, the user interface 116 may include one or more input / output devices within the set of peripherals 108, such as a wireless communication system 146, a vehicle computer 148, a microphone 150, and a speaker 152.

[0163] Computer system 101 may control functions of vehicle 100 based on input received from various subsystems (e.g., travel system 102, sensor system 104, and control system 106) and from user interface 116. For example, computer system 101 may utilize input from control system 106 in order to control steering unit 132 to avoid obstacles detected by sensor system 104 and obstacle avoidance system 144. In some embodiments, computer system 101 may be operable to provide control over many aspects of vehicle 100 and its subsystems.

[0164] Alternatively, one or more of the above-mentioned components may be installed or associated separately from the vehicle 100. For example, the memory 114 may exist partially or completely separately from the vehicle 100. The above-mentioned components may be communicatively coupled together in a wired and / or wireless manner.

[0165] Optionally, the above components are only examples. In actual applications, the components in the above modules may be added or deleted according to actual needs. Figure 4 It should not be understood as limiting the embodiments of the present application.

[0166] An autonomous vehicle traveling on a road, such as vehicle 100 above, can identify objects in its surrounding environment to determine adjustments to the current speed. The objects can be other vehicles, traffic control devices, or other types of objects. In some examples, each identified object can be considered independently, and based on the object's respective characteristics, such as its current speed, acceleration, spacing from the vehicle, etc., can be used to determine the speed to be adjusted by the autonomous vehicle.

[0167] Optionally, the vehicle 100 or a computing device associated with the vehicle 100 can predict the behavior of the identified objects based on the characteristics of the identified objects and the state of the surrounding environment (e.g., traffic, rain, ice on the road, etc.). Optionally, each of the identified objects depends on the behavior of each other, so all of the identified objects can also be considered together to predict the behavior of a single identified object. The vehicle 100 can adjust its speed based on the predicted behavior of the identified objects.

[0168] In other words, the autonomous vehicle can determine what stable state the vehicle will need to adjust to (e.g., accelerate, decelerate, or stop) based on the predicted behavior of the object. In this process, other factors may also be considered to determine the speed of the vehicle 100, such as the lateral position of the vehicle 100 in the road it is traveling on, the curvature of the road, the proximity of static and dynamic objects, etc.

[0169] In addition to providing instructions to adjust the speed of the autonomous vehicle, the computing device may also provide instructions to modify the steering angle of vehicle 100 so that the autonomous vehicle follows a given trajectory and / or maintains a safe lateral and longitudinal distance from objects near the autonomous vehicle (e.g., cars in adjacent lanes on the road).

[0170] The vehicle 100 may be a car, a truck, a motorcycle, a bus, an entertainment vehicle, an amusement park vehicle, construction equipment, a tram, a golf cart, etc., and the embodiments of the present application do not make any particular limitation.

[0171] Figure 4The vehicle 100 shown may be equipped with an advanced driver assistance system for realizing the automatic driving function, and the advanced driver assistance system contains a large number of parameters that need to be calibrated. Specifically, the calibration implementation process of the vehicle's advanced driver assistance system mainly includes the calibration of the parameters of each subsystem in the execution layer, the perception layer and the functional layer. The execution layer involves the calibration of the power system, the braking system, the steering system, the four-wheel alignment parameters and the suspension system. The perception layer involves the calibration of GNSS and INS (Initial Navigation System, inertial navigation system), camera calibration, lidar calibration, millimeter wave radar calibration, ultrasonic radar calibration, etc. GNSS includes GPS (Global Position System, Global Positioning System), GLONASS (Global Navigation Satellite System, GLONASS satellite navigation system), Galileo (Galileo navigation satellite system, Galileo satellite navigation system), BDS (Beidou satellite navigation system, Beidou satellite navigation system). The functional layer involves vehicle longitudinal control module calibration, lateral control module calibration, ADAS basic function calibration, ADAS driving style calibration, etc. Longitudinal control is mainly speed control, which controls the vehicle speed by controlling the brakes, throttle, gear, etc. Lateral control is mainly to control the heading, by changing the steering wheel torque or angle, etc., so that the vehicle can travel in the desired direction. ADAS basic functions include, for example, ACC (Adaptive Cruising System), LCC (Lane Center Control), ALC (Auto Lane Change), etc. Driving style refers to the way of driving or the habitual driving method, which includes the choice of driving speed, the choice of driving distance, etc. Driving styles include, for example, aggressive, stable, cautious, etc.

[0172] The above introduces the scenarios and driving devices to which the method provided in the embodiments of the present application is applied. The following will introduce the path optimization method provided in the present application in detail.

[0173] See also Figure 5 , Figure 5 A schematic diagram of a path optimization method provided in an embodiment of the present application. Figure 5 As shown, the path optimization method includes the following steps 501-503.

[0174] 501. Obtain motion state information of a driving device and a driving path to be optimized, wherein the driving path includes a plurality of sampling position points; the motion state information represents the motion state of each of the sampling position points; wherein the motion state is represented based on a plurality of flat variables or derivatives of flat variables; each of the flat variables corresponds to a position of one dimension of the sampling position point; and the motion state of at least one of the sampling position points includes curvature and curvature change rate.

[0175] In a possible implementation, a driving path to be optimized may be obtained, wherein the driving path to be optimized may be a prediction result for the driving path of the driving device (for example, an initial prediction result or an intermediate result of an optimization process).

[0176] In the existing implementation, a plurality of sampling points can be sampled for a section of the driving route to be optimized, and the position of the sampling points is used to represent the section of the driving route. When optimizing the route, the position of each sampling point is optimized. On the one hand, in order to ensure the optimization accuracy, the number of sampling points cannot be too small, so the number of points that need to be optimized is also large, resulting in a large calculation overhead. On the other hand, since the position of the sampling point is only an approximate fit of the route, even if the number of sampling points is large, the accuracy and effect of the path optimization are still limited.

[0177] In the embodiment of the present application, the driving path to be optimized is represented by a high-order polynomial with respect to the time variable. When optimizing the driving path, the objects to be optimized are the time variable and the polynomial coefficients. Since the number of objects to be optimized is reduced, the calculation overhead during optimization can be reduced, and the shape of the driving path can be more accurately represented by the high-order polynomial, thereby improving the optimization accuracy and effect.

[0178] For example, for the driving path to be optimized, let the arc length be L and the expected average driving speed be v mean , the endpoints of the driving path are anchor points qk and qk+1 respectively, then the duration of the corresponding driving path is T k is the path length N q is the compensation coefficient (determined by the expected speed and expected acceleration of the endpoint); the corresponding polynomial coefficients are then solved by the trajectory planning algorithm (such as MinimumSnap) to obtain the parameterized representation result of the driving path to be optimized, which can be used as the initial value of the optimization process.

[0179] Since the original path to be optimized is very long, it is necessary to split the original path into multiple sub-paths during optimization, and optimize each sub-path at a specific granularity.

[0180] In a possible implementation, the original path may be sampled (eg, randomly sampled) to obtain a plurality of position sampling points, and the original path may be split based on the curvature or curvature change rate characteristics of the position sampling points.

[0181] Next, we will introduce how to divide the original path:

[0182] When optimizing the driving trajectory, in order to ensure the optimization effect, it is often necessary to adjust the positions with larger curvature or larger curvature change rate in the driving trajectory. Therefore, when the initial path is divided, each divided path (including the driving path to be optimized in the embodiment of the present application) can include at least one sampling position point with larger curvature or larger curvature change rate (also referred to as a bad point in the embodiment of the present application).

[0183] In a possible implementation, the driving path includes sampling points whose curvature or curvature change rate is greater than a threshold.

[0184] When optimizing the driving trajectory, in order to ensure the optimization effect, the endpoints of the segmented path (which may also be referred to as anchor points in the embodiment of the present application) cannot be points whose curvature or curvature change rate is greater than a threshold. The reason is that the endpoints are often not adjusted in the optimization algorithm.

[0185] In a possible implementation, the driving path to be optimized is a path selected from a planned path of the driving device, and both ends of the driving path are sampled from the planned path based on a curvature or a curvature change rate being less than a threshold.

[0186] Next, we introduce a process diagram of path division:

[0187] Take the initial path, allocate anchor points and time according to the change of its curvature, and split the original path. For the acquired initial path, it can be but not limited to uniform sampling with a certain step length on it. For each sampling point, the approximate curvature and curvature change rate are calculated in conjunction with its adjacent front and rear points. If the two exceed the preset upper threshold, the sampling point is counted as a bad point and grouped according to the degree of proximity. From the two ends of each group of bad points, advance to both sides until the above-mentioned curvature and curvature change rate are found to be lower than the preset lower threshold, mark it as an anchor point, and use the anchor point as the dividing point to divide the original path into multiple sections according to the order of the front and rear anchor points. It should be understood that if there is overlap between sections, the anchor points can be merged or cut according to the severity of the overlapping parts until there is no overlap between the sections. Afterwards, each section can be parameterized by time and polynomial coefficients, and time is allocated according to the arc length of each section of the trajectory, and the entire trajectory is initialized as a piecewise polynomial trajectory.

[0188] The curvature-guided spatiotemporal parameterization of trajectories is implemented as follows:

[0189] After obtaining the initial path, for each path point except the first and last points, combine its adjacent points before and after to approximate its curvature with a second-order polynomial:

[0190]

[0191]

[0192] Calculate the curvature and its rate of change of each sampling point. or κ>κ u , then mark the current point as bad point i; if two or more bad points are close to each other, group them together to ensure that there are no repeated bad points between groups; start from the two ends of each group of bad points and expand in the forward and backward directions with a step length of ds, Until it is confirmed that the actual curvature change rate of the extension point satisfies And κ<κ l ; Register the corresponding positions of the two end points of the extension as anchor points q i and q i+1 , the part between the two anchor points (inclusive) is taken as the road section corresponding to the bad point i; if there is an intersection between the front and rear sections, the center point of the intersection is used to replace the anchor points at both ends, and this iteration is carried out until all sections have no overlap, and finally the arc length of each section is calculated

[0193] In a possible implementation, the motion state information of the driving device may be acquired, and the driving path includes a plurality of sampling position points; the motion state information indicates the motion state of each of the sampling position points.

[0194] For example, the motion state may be at least one of velocity, acceleration, angular velocity, angular acceleration, curvature, and curvature change rate.

[0195] In existing implementations, the motion state of each position sampling point is obtained by the physical motion equation of the system (usually a set of ordinary differential equations, such as Find the expressions of all state quantities and input quantities.

[0196] Among them, the curvature is determined by the linear velocity and the angular velocity (Δθ / Δs), which is a nonlinear constraint (involving arctan, sqrt, and exponential operations), and the curvature change rate is more nonlinear and of higher order (numerical operations involve acceleration, the fifth power of velocity, etc., and values ​​in the range of 10^10 appear frequently, which can easily cause gradient explosion). As a result, the modeling of the curvature change rate is highly complex and difficult to control in terms of restoration and numerical stability (discrete curvature calculation, a 0.5m differential displacement may cause the local curvature to change by more than 10 times). Therefore, accurate curvature constraint modeling and efficient calculation methods are required.

[0197] In a possible implementation, after the driving path to be optimized is obtained, position points of the driving path to be optimized may be sampled, and the motion states of the sampled position points may be used to construct a penalty term.

[0198] First, we introduce the sampling method of position sampling points:

[0199] In one possible implementation, variational rate sampling can be performed on each trajectory segment, so that the number of samples of points with larger curvature or curvature change rate is greater. Since the parts with larger curvature or curvature change rate are the objects that need to be optimized, the above-mentioned variational rate sampling method can improve the effect of subsequent optimization.

[0200] Specifically, the multiple sampling position points include a first sampling point and a second sampling point, the curvature or curvature change rate corresponding to the first sampling point in the driving path is greater than that of the second sampling point, the first sampling point is obtained by sampling the driving path according to a first sampling resolution, the second sampling point is obtained by sampling the driving path according to a second sampling resolution, and the first sampling resolution is greater than the second sampling resolution.

[0201] In the embodiment of the present application, the motion state is represented based on a plurality of flat variables or derivatives of flat variables; each of the flat variables corresponds to a position of one dimension of the sampling position point.

[0202] The embodiments of the present application can use differential flatness to model the motion system, and directly perform a unified differential parameterized description of motion state quantities such as curvature κ and curvature change rate κ, velocity v, acceleration a, angular velocity ω, angular acceleration α in the flat variable space.

[0203] Expressing the motion state through flat variables can avoid solving differential equations in the trajectory optimization process, thereby reducing the difficulty of optimization.

[0204] In a possible implementation, the initial motion state information of the driving device can be obtained, the initial motion state information is expressed in a first space, and the expression space of the initial motion state information is changed from the first space to a second space to obtain the motion state information, the dimension of the second space is less than or equal to the dimension of the first space, and each dimension of the second space corresponds to a flat variable or a derivative of a flat variable. When the dimension of the second space is less than the dimension of the first space, it is equivalent to the quantitative representation of the variables used to represent the motion state, thereby reducing the dimension of the expression space and reducing the difficulty of optimization.

[0205] For example, the system can be solved based on the physical equations of motion of the system (usually a set of ordinary differential equations, such as Find the expressions of all state quantities and input quantities (that is, the motion state in the embodiment of the present application); by mathematically transforming the above state quantities and input quantities, find a set of flat output variables y so that the original system variables can be represented by this set of flat variables and their linearly independent derivatives of various orders A complete representation is made, thereby completing the differential flat modeling of the original system.

[0206] In addition, differential flatness is used to model the motion system, and a unified description of state quantities such as velocity, acceleration, angular velocity, angular acceleration, curvature and curvature change rate is directly performed in a low-dimensional flat variable space, avoiding the use of geometric differences and state variables such as steering angle and yaw angle.

[0207] For example, the physical parameters of the moving subject, such as the turning radius and wheelbase of the vehicle body, and the constants in the kinematic model, can be obtained. Based on the above flat output and the basis vectors composed of its derivatives of various orders, the key state quantities such as the curvature κ and the curvature change rate κ·, velocity v, acceleration a, angular velocity ω, and angular acceleration α of its motion trajectory are differentially parameterized. describe.

[0208] Taking the vehicle system as an example, a schematic diagram of the specific implementation of differential flat modeling is as follows:

[0209] The vehicle's position, motion state, and wheelbase can be described by the vehicle's motion equations to find a set of flat outputs σ x ,σ y =p x ,p y , the main state quantities and input quantities of the original system are differentially parameterized, expressed as a unified description of the flat output and its multi-order derivatives:

[0210]

[0211] 502. Determine a target penalty function according to the motion state information; the target penalty function is related to driving feasibility and smoothness when the driving device drives along the multiple sampling positions; wherein the driving feasibility is related to the curvature, and the smoothness is related to the curvature and the curvature change rate;

[0212] 503. Optimize the driving path according to the target penalty function.

[0213] In one possible implementation, a target penalty function can be determined based on the motion state information; the target penalty function is related to the driving performance of the driving device when driving along the multiple sampling position points, and the driving path is optimized based on the target penalty function; wherein the goal of optimizing the driving path is to reduce the value of the target penalty function.

[0214] The target penalty function may include multiple penalty items, and one or more penalty items may be obtained according to the motion state information.

[0215] Next, the target penalty function in the embodiment of the present application is introduced:

[0216] In a possible implementation, the target penalty function (one or more penalty terms therein) is negatively correlated with the driving feasibility of the driving device when driving along the multiple sampling location points.

[0217] In a possible implementation, the target penalty function (one or more penalty terms therein) is negatively correlated with the smoothness of the driving device when driving along the multiple sampling position points.

[0218] For example, nonlinear inequality constraints can be directly constructed for curvature, curvature change rate, and angular acceleration, and their regularization terms can be set, combined with other constraints such as velocity, acceleration, angular velocity, etc., and converted into feasibility and smoothness penalty terms.

[0219] In addition, curvature, curvature change rate and angular acceleration can be used as hard constraints to construct inequality constraints, and upper and lower limits can be restricted. Constraints can be removed by softening (such as barrier function, Lagrange multiplier, enhanced Lagrange, etc.) and converted into kinematic penalty terms.

[0220] At the same time, regularization is implemented for instantaneous curvature, curvature change rate / angular acceleration The accumulated result in the whole trajectory is used as the regular penalty term related to the curvature change of the whole interval

[0221] At the same time, hard inequality constraints are constructed for dynamics-related variables such as velocity, acceleration, angular velocity, angular acceleration, etc., and upper and lower limits are imposed. The constraints can be removed by softening and relaxing (such as barrier function, Lagrange multiplier, enhanced Lagrange, etc.) and converted into dynamic penalty terms.

[0222] The above kinematic, dynamic, and regularization penalty terms are linearly weighted as the overall feasibility and smoothness penalty terms

[0223]

[0224] Next, we will introduce the schematic diagram of constructing the penalty term through the motion state represented by the flat variable in combination with the formula:

[0225] In a possible implementation, the expression of the rate of change (derivative) can be obtained based on the differential parameterization description of the angular velocity and curvature:

[0226]

[0227] Apply upper and lower limits (inequality) constraints to curvature, curvature change rate, and / or angular velocity and angular acceleration, and convert them into kinematic penalty terms

[0228] The penalty term corresponding to the curvature constraint can be:

[0229]

[0230] The penalty term corresponding to the curvature change rate constraint can be:

[0231]

[0232] The penalty term corresponding to the angular velocity constraint can be:

[0233]

[0234] The penalty term corresponding to the angular acceleration constraint can be:

[0235]

[0236] Regularization terms can be added to the curvature, curvature change rate, angular velocity, and angular acceleration (select according to actual conditions), such as: The accumulated result in the whole trajectory is used as the regular penalty term related to the curvature change of the whole interval (from time 0 to time T) Nonlinear inequality constraints are directly constructed for curvature, curvature change rate / angular acceleration, and regularization terms are set for curvature and its rate of change to jointly construct a smoothness penalty term, while constraining curvature jumps, steering consistency and amplitude.

[0237] The above introduces some penalty items (about feasibility and smoothness). In addition to the above penalty items, it is also necessary to construct collision risk information related to driving safety and construct corresponding safety penalty items based on the collision risk information.

[0238] Safe obstacle avoidance involves collision detection near a large number of online trajectory points (the request frequency often reaches hundreds of thousands) and changes in the path topology. Especially when the body and the environment have non-convex configurations or complex structures, the results often have a significant impact on the optimization time, stability and trajectory smoothness. Therefore, it is sensitive to the efficiency of collision gradient calculation and the selection of trajectory control points, requiring efficient collision constraint modeling and adaptive tuning of control points.

[0239] In a possible implementation, point cloud data of the driving environment of the driving device at the multiple sampling positions can be obtained, and the point cloud data is used to characterize obstacles in the driving environment. According to the interaction between each point cloud in the point cloud data and the Euclidean distance field ESDF of the driving device, the collision risk information of the driving device at each sampling position is obtained.

[0240] In this embodiment, the point cloud data is actually a collection of multiple points, each of which is used to represent the coordinates of the ground or the obstacle surface in the driving environment. Therefore, based on the multiple points in the point cloud data, the coordinates of each point on the ground and the obstacle surface in the driving environment can be determined.

[0241] The point cloud data of the driving environment can be obtained by real-time scanning by the driving device using a laser radar during driving. The point cloud data of the driving environment can also be obtained in advance by other driving devices through laser radar scanning. This embodiment does not limit the source of the point cloud data.

[0242] The driving device may be, for example, a robot or a vehicle. The driving environment may be, for example, an indoor environment or an outdoor environment, and the driving environment mainly depends on the environment in which the driving device operates. For example, when the driving device is a cleaning robot, the driving environment may be, for example, a user's home; when the driving device is a welcoming robot, the driving environment may be, for example, an indoor public place such as a bank or a restaurant; when the driving device is an inspection robot, the driving environment may be, for example, an outdoor environment such as a substation. When the driving device is a vehicle, the driving environment may be, for example, an outdoor environment such as a mine.

[0243] In a possible implementation, the collision risk information of the traveling device at each sampling position point may be obtained according to the interaction between each point cloud in the point cloud data and the Euclidean distance field ESDF of the traveling device.

[0244] The Euclidean distance field ESDF of the driving device can be constructed by, but not limited to, the following methods: find the circumscribed bounding box (generally a convex bounding box) of its surface contour, and ensure that the actual contour of the body is completely enclosed in it. Methods such as AABB, OBB, FDH, CH, etc. can be used (but not limited to). The area inside the outer bounding box is further subdivided, and the area surrounded by the edge of the actual contour is divided into the ROI area, and the rest of the area is the non-ROI area. The ESDF is generated by the EDT algorithm, and the inside of the ROI area is set to a negative value, and the non-ROI area is set to a non-negative value.

[0245] In a possible implementation, the collision risk information of the traveling device at each sampling position point can be obtained based on the ESDF value of the corresponding position of each point cloud in the point cloud data in the ESDF, and the ESDF gradient determined by the ESDF value of the position near the corresponding position.

[0246] For example, the entire ESDF can be stored as a discretized lookup table, and when used, continuous ESDF values ​​are obtained according to coordinate interpolation, and the mobile body positioning information is used. (position information and posture information), register the point cloud in the grid map centered on the body, and maintain and update the occupancy status of the grid in real time, detect the point cloud covered by the bounding box of the mobile body, and query the occupancy status of the grid corresponding to each point cloud i. If the state is occupied, use the lookup table interpolation to obtain the ESDF value di corresponding to the point cloud, and calculate its gradient based on the ESDF value of the neighborhood. The ESDF value and gradient of all occupied point clouds within the above bounding box are weighted as the collision cost and gradient corresponding to the current positioning of the body, and the result is used as the safety penalty item

[0247] Through the above method, a 3D potential field such as the Euclidean Signed Distance Field (ESDF) is used to finely describe the internal collision trend of the moving body, and a safety penalty term is constructed. In the robot body coordinate system, for the environmental obstacle points that invade its contour (confirmed by the occupancy state), the ESDF costs and gradients of all intrusion points are queried and summed. An intrusive 3D collision calculation model based on the environmental occupancy grid and the moving body ESDF can be established to construct a safety penalty term.

[0248] In a possible implementation, the driving path may be optimized according to the target penalty function; wherein the goal of optimizing the driving path is to reduce the value of the target penalty function.

[0249] Wherein, when the driving path is represented by a high-order polynomial with respect to a time variable, the correcting of the driving path includes: optimizing the time variable and the polynomial coefficients of the high-order polynomial.

[0250] When performing path optimization, it is optional to construct an optimal control problem with the minimum control and optimal time as the goal, and add the above penalty term for weighting, convert it into an unconstrained optimization problem, and perform nonlinear optimization on the piecewise polynomial trajectory. For example, dynamic variational rate sampling can be performed according to the amplitude of the curvature and its rate of change on each segment of the trajectory, and the above penalty term cost weighting and gradient calculation can be performed on all sampling points, and the two-dimensional gradient of time and space can be transmitted. The time variable and polynomial coefficients are optimized synchronously to obtain the optimized path trajectory.

[0251] Among them, the optimal control problem is constructed with the minimum control and optimal time as the goals, and the above-mentioned penalty terms are added for weighting, which is converted into an unconstrained optimization problem. When the piecewise polynomial trajectory is nonlinearly optimized, a schematic form of the unconstrained optimization problem finally constructed is as follows:

[0252]

[0253] in, To minimize the control input, To minimize the trajectory time;

[0254] are the security, smoothness and feasibility penalties and regularization terms.

[0255] Among them, the specific implementation of unconstrained optimization problem construction and solution is as follows:

[0256] (I) The energy penalty function makes the control input as smooth as possible to ensure energy optimality:

[0257]

[0258] (II) The total time penalty function makes the total time of the trajectory as short as possible:

[0259]

[0260] (III) The dynamic penalty function makes the expected velocity and acceleration of the mobile body at any time on the trajectory meet the dynamic constraints of the mobile body itself:

[0261]

[0262]

[0263]

[0264] Where C(*)=max{*,0} 3 is the cubic penalty function, v max is the maximum speed limit, a max is the maximum acceleration limit, and K is the number of segments in each trajectory.

[0265] (IV) The collision safety penalty function is as follows:

[0266]

[0267] in, is the current pose obtained from the body contour ESDF The corresponding collision penalty term is calculated as the square of the sum of the ESDF values ​​of all obstacle points that collide with the body contour in the current pose. i,j The ESDF value at p i,j The grid coordinates are calculated using the stereo linear interpolation method.

[0268] (V) The smoothness penalty function is as follows:

[0269]

[0270] in, G g (p i,j ) is the moving body on the trajectory p i,j The kinematic penalty terms (curvature, curvature change rate, angular velocity, angular acceleration, etc.) at are weighted together, and C is a custom cost-barrier function.

[0271] (VI) The regular penalty function is as follows:

[0272]

[0273] That is, the above regularized penalty term integral operation is realized by weighted discrete point sampling instead. G ρ (p i,j ) is the moving body on the trajectory p i,j The curvature, curvature change rate, angular velocity, and angular acceleration are weighted together.

[0274] (VII) The trajectory of the mobile body is represented by a quintic polynomial function. Suppose the trajectory has M segments, and the polynomial coefficient of the kth segment (k = 1, 2, ..., M) is c k , is a 6×1 vector with a duration of T k , is a one-dimensional vector, β(t)=[1,t,t 2 ,t 3 ,t 4 ,t 5 ] T is a function of time t, the position of the kth trajectory at time t Let c = [c1, c2, ..., c M ],T=[T1,T2,...,T M ] T ,remember P k The first derivative of (t), β (*) is the first-order derivative of β(t), then the unconstrained optimization problem finally modeled is:

[0275] minc,T λ e J e +λ t J t +λ d J d +λ r J r +λ g J g +λ ρ J ρ ;

[0276] Where λ* is the weight coefficient of the * penalty.

[0277] (VIII) Using the quasi-Newton method or the trust region method to solve unconstrained optimization problems.

[0278] (IX) Perform safety check on the trajectory generated in (II), that is, check the generated trajectory every L seconds. c The length is taken from the trajectory point. If there is a trajectory point, the grid is occupied; otherwise, the trajectory is generated successfully and the generated trajectory is returned.

[0279] The embodiment of the present application optimizes the initial driving path to a trajectory with continuous curvature without mutation and without collision between the vehicle body and the environment through efficient and high-quality optimization solution of the nonlinear minimum control problem.

[0280] This application expresses the motion state through flat variables, which can avoid solving differential equations in the trajectory optimization process on the one hand, and reduce the dimension of the expression space on the other hand, thereby reducing the difficulty of trajectory optimization. In addition, directly describing nonlinear constraints such as curvature and its rate of change in the flat variable space can avoid repeated discretization of displacement or time difference, so as to achieve reasonable constraint curvature jump while making the overall steering consistency as high as possible and the amplitude as small as possible.

[0281] A path optimization method provided by an embodiment of the present application is introduced above, and a driving device for executing the above path optimization method will be introduced below.

[0282] See also Figure 6 , Figure 6 This is a schematic diagram of the structure of a driving device provided in an embodiment of the present application. Figure 6 As shown, the traveling device comprises:

[0283] The acquisition module 601 is used to acquire the motion state information of the driving device and the driving path to be optimized, wherein the driving path includes a plurality of sampling positions; the motion state information represents the motion state of each of the sampling positions; wherein the motion state is represented based on a plurality of flat variables or derivatives of flat variables; each of the flat variables corresponds to a position of one dimension of the sampling position; and the motion state of at least one of the sampling positions includes curvature and curvature change rate;

[0284] The specific introduction of the acquisition module 601 can refer to the description of step 501 in the above embodiment, which will not be repeated here.

[0285] Processing module 602 is used to determine a target penalty function based on the motion state information; the target penalty function is related to the driving feasibility and smoothness of the driving device when driving along the multiple sampling position points; wherein the driving feasibility is related to the curvature, and the smoothness is related to the curvature and the curvature change rate; according to the target penalty function, the driving path is optimized.

[0286] For a detailed introduction to the processing module 602, reference may be made to the description of steps 502 and 503 in the above embodiment, which will not be repeated here.

[0287] In a possible implementation, a goal of optimizing the driving path is to reduce a value of the target penalty function.

[0288] In one possible implementation, the processing module 602 is specifically used to: determine a target penalty function based on the motion state information; the target penalty function is related to the driving performance of the driving device when it travels along the multiple sampling position points; optimize the driving path based on the target penalty function; wherein the goal of optimizing the driving path is to reduce the value of the target penalty function.

[0289] In a possible implementation, the acquisition module 601 is specifically used to: obtain initial motion state information of the traveling device, wherein the initial motion state information is expressed in a first space; change the expression space of the initial motion state information from the first space to a second space to obtain the motion state information, wherein the dimension of the second space is less than or equal to the dimension of the first space, and each dimension of the second space corresponds to a flat variable or a derivative of a flat variable.

[0290] In a possible implementation, the motion state is at least one of velocity, acceleration, angular velocity, angular acceleration, curvature, and curvature change rate.

[0291] In a possible implementation, the driving path is represented by a high-order polynomial with respect to a time variable, and the optimizing the driving path includes: optimizing the time variable and the polynomial coefficients of the high-order polynomial.

[0292] In a possible implementation, the multiple sampling position points include a first sampling point and a second sampling point, the curvature or curvature change rate corresponding to the first sampling point in the driving path is greater than that of the second sampling point, the first sampling point is obtained by sampling the driving path according to a first sampling resolution, the second sampling point is obtained by sampling the driving path according to a second sampling resolution, and the first sampling resolution is greater than the second sampling resolution.

[0293] In a possible implementation, the driving path to be optimized is a section of a path selected from a planned path of the driving device, and both ends of the driving path are sampled from the planned path based on curvature and / or curvature change rate being less than a threshold.

[0294] In a possible implementation, the driving path includes sampling points whose curvature and / or curvature change rate is greater than a threshold.

[0295] In a possible implementation, the target penalty function is negatively correlated with the driving feasibility and / or smoothness of the driving device when driving along the multiple sampling position points.

[0296] In a possible implementation, the acquisition module 601 is further used to:

[0297] Acquiring point cloud data of a driving environment where the driving device is located at the plurality of sampling locations, wherein the point cloud data is used to characterize obstacles in the driving environment;

[0298] The processing module 602 is further configured to obtain collision risk information of the driving device at each sampling position point according to the interaction between each point cloud in the point cloud data and the Euclidean distance field ESDF of the driving device;

[0299] The processing module 602 is specifically used for:

[0300] The driving path is optimized according to the motion state information and the collision risk information.

[0301] In a possible implementation, the processing module 602 is specifically configured to:

[0302] The collision risk information of the traveling device at each sampling position is obtained according to the ESDF value of the corresponding position of each point cloud in the point cloud data in the ESDF, and the ESDF gradient determined by the ESDF value of the position near the corresponding position.

[0303] In a possible implementation, the traveling device includes a robot or a vehicle.

[0304] Next, a driving device provided by an embodiment of the present application is introduced. Figure 7 , Figure 7 A schematic diagram of a driving device provided in an embodiment of the present application, the driving device 700 may include a microcontroller unit (MCU), a chip, a chip system or a circuit system, etc., which are not limited here. Specifically, the driving device 700 includes: a transceiver 701, a processor 702 and a memory 703 (wherein the number of processors 702 in the driving device 700 can be one or more, Figure 7 In the example, one processor is used as an example), wherein the processor 702 may include an application processor 7021 and a communication processor 707. In some embodiments of the present application, the transceiver 701, the processor 702 and the memory 703 may be connected via a bus or other means.

[0305] The memory 703 may include a read-only memory and a random access memory, and provides instructions and data to the processor 702. A portion of the memory 703 may also include a non-volatile random access memory (NVRAM). The memory 703 stores processor and operation instructions, executable modules or data structures, or subsets thereof, or extended sets thereof, wherein the operation instructions may include various operation instructions for implementing various operations.

[0306] Processor 702 controls the operation of path optimization. In a specific application, various components of the driving device are coupled together through a bus system, wherein the bus system includes not only a data bus but also a power bus, a control bus, and a status signal bus, etc. However, for the sake of clarity, various buses are referred to as bus systems in the figure.

[0307] The method disclosed in the above embodiment of the present application can be applied to the processor 702, or implemented by the processor 702. The processor 702 can be an integrated circuit chip with signal processing capabilities. In the implementation process, each step of the above method can be completed by the hardware integrated logic circuit or software instructions in the processor 702. The above processor 702 can be a general processor, a digital signal processor (digital signal processing, DSP), a microprocessor or a microcontroller, and can further include an application specific integrated circuit (application specific integrated circuit, ASIC), a field programmable gate array (field-programmable gate array, FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components. The processor 702 can implement or execute the various methods, steps and logic block diagrams disclosed in the embodiments of the present application. The general processor can be a microprocessor or the processor can also be any conventional processor, etc. The steps of the method disclosed in the embodiment of the present application can be directly embodied as a hardware decoding processor to execute, or the hardware and software modules in the decoding processor can be combined and executed. The software module may be located in a storage medium mature in the art such as a random access memory, a flash memory, a read-only memory, a programmable read-only memory, or an electrically erasable programmable memory, a register, etc. The storage medium is located in the memory 703, and the processor 702 reads the information in the memory 703 and completes the steps of the above method in combination with its hardware.

[0308] The transceiver 701 (such as a network card) can be used to receive input digital or character information and generate signal input related to the relevant settings and function control of the driving device. The transceiver 701 can also be used to output digital or character information through the first interface; and send instructions to the disk group through the first interface to modify the data in the disk group.

[0309] The driving device provided in the embodiment of the present application may specifically include a chip, and the chip includes: a processing unit and a communication unit, the processing unit may be, for example, a processor, and the communication unit may be, for example, an input / output interface, a pin or a circuit, etc. The processing unit may execute the computer execution instructions stored in the storage unit so that the chip in the processing module executes the path optimization method described in the above embodiment. Optionally, the storage unit is a storage unit in the chip, such as a register, a cache, etc., and the storage unit may also be a storage unit located outside the chip in the wireless access device end, such as a read-only memory (ROM) or other types of static storage devices that can store static information and instructions, a random access memory (RAM), etc.

[0310] For details, please refer to Figure 8 , Figure 8 A schematic diagram of the structure of a chip provided in an embodiment of the present application; the chip can be expressed as a neural network processor NPU 800, which is mounted on the host CPU (Host CPU) as a coprocessor and assigned tasks by the Host CPU. The core part of the NPU is the operation circuit 803, which is controlled by the controller 804 to extract matrix data from the memory and perform multiplication operations.

[0311] In some implementations, the operation circuit 803 includes multiple processing units (Process Engine, PE) inside. In some implementations, the operation circuit 803 is a two-dimensional systolic array. The operation circuit 803 can also be a one-dimensional systolic array or other electronic circuits capable of performing mathematical operations such as multiplication and addition. In some implementations, the operation circuit 803 is a general-purpose matrix processor.

[0312] For example, assume there is an input matrix A, a weight matrix B, and an output matrix C. The operation circuit takes the corresponding data of matrix B from the weight memory 802 and caches it on each PE in the operation circuit. The operation circuit takes the matrix A data from the input memory 801 and performs matrix operation with matrix B, and the partial result or final result of the matrix is ​​stored in the accumulator 808.

[0313] The unified memory 806 is used to store input data and output data. The weight data is directly transferred to the weight memory 802 through the direct memory access controller (DMAC) 805. The input data is also transferred to the unified memory 806 through the DMAC.

[0314] BIU stands for Bus Interface Unit, i.e., bus interface unit 810 , which is used for interaction between AXI bus, DMAC and instruction fetch buffer (IFB) 809 .

[0315] The bus interface unit 810 (BIU) is used for the instruction fetch memory 809 to obtain instructions from the external memory, and is also used for the storage unit access controller 805 to obtain the original data of the input matrix A or the weight matrix B from the external memory.

[0316] DMAC is mainly used to transfer input data in the external memory DDR to the unified memory 806 or to transfer weight data to the weight memory 802 or to transfer input data to the input memory 801.

[0317] The vector calculation unit 807 includes multiple operation processing units, and further processes the output of the operation circuit 803 when necessary, such as vector multiplication, vector addition, exponential operation, logarithmic operation, size comparison, etc. It is mainly used for non-convolutional / fully connected layer network calculations in neural networks, such as Batch Normalization, pixel-level summation, upsampling of feature planes, etc.

[0318] In some implementations, the vector calculation unit 807 can store the processed output vector to the unified memory 806. For example, the vector calculation unit 807 can apply a linear function; or a nonlinear function to the output of the operation circuit 803, such as linear interpolation of the feature plane extracted by the convolution layer, and then, for example, a vector of accumulated values ​​to generate an activation value. In some implementations, the vector calculation unit 807 generates a normalized value, a pixel-level summed value, or both. In some implementations, the processed output vector can be used as an activation input to the operation circuit 803, for example, for use in a subsequent layer in a neural network.

[0319] An instruction fetch buffer 809 connected to the controller 804 is used to store instructions used by the controller 804;

[0320] Unified memory 806, input memory 801, weight memory 802 and instruction fetch memory 809 are all On-Chip memories. External memories are private to the NPU hardware architecture.

[0321] The processor mentioned in any of the above places may be a general-purpose central processing unit, a microprocessor, an ASIC, or one or more integrated circuits for controlling the execution of the above program.

[0322] See also Fig. 9 , Fig. 9 A schematic diagram of the structure of a computer-readable storage medium provided in an embodiment of the present application. The present application also provides a computer-readable storage medium. In some embodiments, the method disclosed above can be implemented as computer program instructions encoded in a machine-readable format on a computer-readable storage medium or encoded on other non-transitory media or products.

[0323] Fig. 9Schematically illustrates a conceptual partial view of an example computer-readable storage medium including a computer program for executing a computer process on a computing device, arranged in accordance with at least some embodiments presented herein.

[0324] In one embodiment, the computer readable storage medium 900 is provided using a signal bearing medium 901. The signal bearing medium 901 may include one or more program instructions 902, which when executed by one or more processors may provide the functions or part of the functions described above for the embodiments. Fig. 9 Program instructions 902 in also describe example instructions.

[0325] In some examples, the signal bearing medium 901 may include a computer readable medium 903 such as, but not limited to, a hard drive, a compact disk (CD), a digital video disk (DVD), a digital tape, a memory, a ROM or RAM, and the like.

[0326] In some embodiments, the signal bearing medium 901 may include a computer recordable medium 904, such as, but not limited to, a memory, a read / write (R / W) CD, a R / W DVD, etc. In some embodiments, the signal bearing medium 901 may include a communication medium 905, such as, but not limited to, a digital and / or analog communication medium (e.g., a fiber optic cable, a waveguide, a wired communication link, a wireless communication link, etc.). Thus, for example, the signal bearing medium 901 may be communicated by a wireless form of the communication medium 905 (e.g., a wireless communication medium complying with the IEEE 802 standard or other transmission protocol).

[0327] The one or more program instructions 902 may be, for example, computer executable instructions or logic implementation instructions. In some examples, the computing device of the computing device may be configured to provide various operations, functions, or actions in response to the program instructions 902 communicated to the computing device via one or more of the computer readable medium 903, the computer recordable medium 904, and / or the communication medium 905.

[0328] It should also be noted that the device embodiments described above are merely schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed over multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. In addition, in the drawings of the device embodiments provided by the present application, the connection relationship between the modules indicates that there is a communication connection between them, which may be specifically implemented as one or more communication buses or signal lines.

[0329] Through the description of the above implementation mode, the technicians in the field can clearly understand that the present application can be implemented by means of software plus necessary general hardware, and of course, it can also be implemented by special hardware including special integrated circuits, special CPUs, special memories, special components, etc. In general, all functions completed by computer programs can be easily implemented by corresponding hardware, and the specific hardware structure used to implement the same function can also be various, such as analog circuits, digital circuits or special circuits. However, for the present application, software program implementation is a better implementation mode in more cases. Based on such an understanding, the technical solution of the present application is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a readable storage medium, such as a computer floppy disk, a U disk, a mobile hard disk, a ROM, a RAM, a disk or an optical disk, etc., including a number of instructions to enable a computer device (which can be a personal computer, a training device, or a network device, etc.) to execute the methods described in each embodiment of the present application.

[0330] In the above embodiments, all or part of the embodiments may be implemented by software, hardware, firmware or any combination thereof. When implemented by software, all or part of the embodiments may be implemented in the form of a computer program product.

[0331] The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, the process or function described in the embodiment of the present application is generated in whole or in part. The computer may be a general-purpose computer, a special-purpose computer, a computer network, or other programmable devices. The computer instructions may be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions may be transmitted from a website site, a computer, a training device, or a data center by wired (e.g., coaxial cable, optical fiber, digital subscriber line (DSL)) or wireless (e.g., infrared, wireless, microwave, etc.) mode to another website site, computer, training device, or data center. The computer-readable storage medium may be any available medium that a computer can store or a data storage device such as a training device, a data center, etc. that includes one or more available media integrations. The available medium may be a magnetic medium, (e.g., a floppy disk, a hard disk, a tape), an optical medium (e.g., a DVD), or a semiconductor medium (e.g., a solid-state drive (SSD)), etc.

Claims

1. A path optimization method, characterized in that: include: Acquire motion state information of a driving device and a driving path to be optimized, wherein the driving path includes a plurality of sampling positions; the motion state information indicates the motion state of each of the sampling positions; wherein the motion state is indicated based on a plurality of flat variables or derivatives of flat variables; each of the flat variables corresponds to a position of one dimension of the sampling position; and the motion state of at least one of the sampling positions includes curvature and curvature change rate; Determine a target penalty function according to the motion state information; the target penalty function is related to driving feasibility and smoothness when the driving device drives along the multiple sampling position points; wherein the driving feasibility is related to the curvature, and the smoothness is related to the curvature and the curvature change rate; The driving path is optimized according to the target penalty function.

2. The method according to claim 1, characterized in that The goal of optimizing the driving path is to reduce the value of the target penalty function.

3. The method according to claim 1 or 2, characterized in that: The obtaining of the motion state information of the traveling device includes: Acquiring initial motion state information of the traveling device, wherein the initial motion state information is expressed in a first space; The expression space of the initial motion state information is changed from the first space to the second space to obtain the motion state information, wherein the dimension of the second space is less than or equal to the dimension of the first space, and each dimension of the second space corresponds to a flat variable or a derivative of a flat variable.

4. The method according to any one of claims 1 to 3, characterized in that: The motion state is at least one of velocity, acceleration, angular velocity, angular acceleration, curvature and curvature change rate.

5. The method according to any one of claims 1 to 4, characterized in that: The driving path is represented by a high-order polynomial with respect to a time variable, and the optimizing the driving path includes: optimizing the time variable and the polynomial coefficients of the high-order polynomial.

6. The method according to any one of claims 1 to 5, characterized in that: The multiple sampling position points include a first sampling point and a second sampling point, the curvature or curvature change rate corresponding to the first sampling point in the driving path is greater than that of the second sampling point, the first sampling point is obtained by sampling the driving path according to a first sampling resolution, the second sampling point is obtained by sampling the driving path according to a second sampling resolution, and the first sampling resolution is greater than the second sampling resolution.

7. The method according to any one of claims 1 to 6, characterized in that: The driving path to be optimized is a section of the path selected from the planned path of the driving device, and both ends of the driving path are sampled from the planned path based on the curvature and / or curvature change rate being less than a threshold.

8. The method according to claim 7, characterized in that The driving path includes sampling points whose curvature and / or curvature change rate is greater than a threshold value.

9. The method according to any one of claims 2 to 8, characterized in that: The target penalty function is negatively correlated with driving feasibility and / or smoothness when the driving device drives along the multiple sampling position points.

10. The method according to any one of claims 1 to 9, characterized in that: The method further comprises: Acquire point cloud data of a driving environment where the driving device is located at the plurality of sampling locations, wherein the point cloud data is used to characterize obstacles in the driving environment; Obtaining collision risk information of the traveling device at each sampling position point according to the interaction between each point cloud in the point cloud data and the 3D potential field of the traveling device; The optimizing the driving path according to the motion state information includes: The driving path is optimized according to the motion state information and the collision risk information.

11. The method according to claim 10, characterized in that Obtaining collision risk information of the traveling device at each sampling position point according to the interaction between each point cloud in the point cloud data and the Euclidean distance field ESDF of the traveling device includes: The collision risk information of the traveling device at each sampling position is obtained according to the ESDF value of the corresponding position of each point cloud in the point cloud data in the ESDF, and the ESDF gradient determined by the ESDF value of the position near the corresponding position.

12. The method according to any one of claims 1 to 11, characterized in that: The traveling device includes a robot or a vehicle.

13. A path optimization device, characterized in that: include: An acquisition module, used for acquiring motion state information of a driving device and a driving path to be optimized, wherein the driving path includes a plurality of sampling positions; the motion state information represents the motion state of each of the sampling positions; wherein the motion state is represented based on a plurality of flat variables or derivatives of flat variables; each of the flat variables corresponds to a position of one dimension of the sampling position; and the motion state of at least one of the sampling positions includes curvature and curvature change rate; A processing module, configured to determine a target penalty function according to the motion state information; the target penalty function is related to driving feasibility and smoothness when the driving device drives along the plurality of sampling positions; wherein the driving feasibility is related to the curvature, and the smoothness is related to the curvature and the curvature change rate; The driving path is optimized according to the target penalty function.

14. The device according to claim 13, characterized in that The goal of optimizing the driving path is to reduce the value of the target penalty function.

15. The device according to claim 13 or 14, characterized in that The acquisition module is specifically used for: Acquiring initial motion state information of the traveling device, wherein the initial motion state information is expressed in a first space; The expression space of the initial motion state information is changed from the first space to the second space to obtain the motion state information, wherein the dimension of the second space is less than or equal to the dimension of the first space, and each dimension of the second space corresponds to a flat variable or a derivative of a flat variable.

16. The device according to any one of claims 13 to 15, characterized in that The motion state is at least one of velocity, acceleration, angular velocity, angular acceleration, curvature and curvature change rate.

17. The device according to any one of claims 13 to 16, characterized in that The driving path is represented by a high-order polynomial with respect to a time variable, and the optimizing the driving path includes: optimizing the time variable and the polynomial coefficients of the high-order polynomial.

18. The device according to any one of claims 13 to 17, characterized in that The multiple sampling position points include a first sampling point and a second sampling point, the curvature or curvature change rate corresponding to the first sampling point in the driving path is greater than that of the second sampling point, the first sampling point is obtained by sampling the driving path according to a first sampling resolution, the second sampling point is obtained by sampling the driving path according to a second sampling resolution, and the first sampling resolution is greater than the second sampling resolution.

19. The device according to any one of claims 13 to 18, characterized in that The driving path to be optimized is a section of the path selected from the planned path of the driving device, and both ends of the driving path are sampled from the planned path based on the curvature and / or curvature change rate being less than a threshold.

20. The device according to claim 19, characterized in that The driving path includes sampling points whose curvature and / or curvature change rate is greater than a threshold value.

21. The device according to any one of claims 13 to 20, characterized in that The target penalty function is negatively correlated with driving feasibility and / or smoothness when the driving device drives along the multiple sampling position points.

22. The device according to any one of claims 13 to 21, characterized in that The acquisition module is further used for: Acquire point cloud data of a driving environment where the driving device is located at the plurality of sampling locations, wherein the point cloud data is used to characterize obstacles in the driving environment; The processing module is further used to: obtain collision risk information of the traveling device at each sampling position point according to the interaction between each point cloud in the point cloud data and the Euclidean distance field ESDF of the traveling device; The processing module is specifically used for: The driving path is optimized according to the motion state information and the collision risk information.

23. The device according to claim 22, characterized in that The processing module is specifically used for: The collision risk information of the traveling device at each sampling position is obtained according to the ESDF value of the corresponding position of each point cloud in the point cloud data in the ESDF, and the ESDF gradient determined by the ESDF value of the position near the corresponding position.

24. The device according to any one of claims 13 to 23, characterized in that The traveling device includes a robot or a vehicle.

25. A traveling device, characterized in that: The device comprises a memory and a processor; the memory stores codes, the processor is configured to execute the codes, and when the codes are executed, the device executes the method according to any one of claims 1 to 12.

26. A computer storage medium, characterized in that The computer storage medium stores instructions, which, when executed by a computer, cause the computer to implement the method of any one of claims 1 to 12.

27. A computer program product, characterized in that The computer program product stores instructions, which, when executed by a computer, cause the computer to implement the method according to any one of claims 1 to 12.

Images