WO2019047639A1

WO2019047639A1 - Method and device for calculating curvature of vehicle trajectory

Info

Publication number: WO2019047639A1
Application number: PCT/CN2018/098609
Authority: WO
Inventors: 姜雨; 郁浩; 闫泳杉; 郑超; 唐坤; 张云飞
Original assignee: 百度在线网络技术（北京）有限公司
Priority date: 2017-09-05
Filing date: 2018-08-03
Publication date: 2019-03-14
Also published as: CN107817790A; CN107817790B

Abstract

A method and device for calculating the curvature of a vehicle trajectory, wherein the method comprises: according to GPS information of a vehicle that is acquired in real time, acquiring a travelling trajectory of the vehicle by means of clothoid curve fitting (S201); calculating an observed value of the curvature of a certain trajectory point in the travelling trajectory (S202); according to a turning angle of a steering wheel of the vehicle corresponding to the trajectory point that is monitored in real time, acquiring a predicted value of the curvature of the trajectory point (S203); according to the observed value and predicted value of the trajectory point, acquiring an optimal value of the curvature of the trajectory point by using Kalman filtering. The method and device greatly increase accuracy in calculating the curvature of a vehicle trajecotry and may increase the feasibility of automatic driving when applied in a self-driving vehicle.

Description

Method and device for calculating curvature of vehicle trajectory

Cross-reference to related applications

This patent application claims Chinese patent filed on September 5, 2017, with the application number of 201710792205.1, the applicant is Baidu Online Network Technology (Beijing) Co., Ltd., and the invention is entitled "A Method and Apparatus for Calculating the Curvature of Vehicle Tracks". Priority of the application, the entire contents of which are incorporated herein by reference.

Technical field

The present invention relates to the field of vehicle automatic driving technology, and more particularly to a technique for calculating the curvature of a vehicle trajectory.

Background technique

During the driving process of the vehicle, the curvature calculation of the trajectory points in the vehicle trajectory is very important, especially in the process of automatic driving of the vehicle, if the curvature of each trajectory point of the trajectory of the self-driving vehicle during the automatic driving process can be accurately calculated, Further precise determination of subsequent driving modes, driving directions, driving trajectories, and the like. However, the calculation method of the curvature of the existing track points is not accurate enough, in particular, the small curvature cannot be calculated.

Therefore, how to provide an efficient and accurate method for calculating the curvature of a vehicle trajectory has become one of the problems that those skilled in the art need to solve.

Summary of the invention

It is an object of the present invention to provide a method and apparatus for calculating the curvature of a vehicle trajectory.

According to an aspect of the invention, a method of calculating a curvature of a vehicle trajectory is provided, wherein the method comprises:

Obtaining a travel trajectory of the vehicle by a curve curve fitting according to GPS information of the vehicle collected in real time;

Calculating an observation value of a curvature of a track point in the travel track;

Obtaining a predicted value of the curvature of the track point according to a steering wheel angle corresponding to the track point in the real-time monitored vehicle;

According to the observed value and the predicted value of the curvature of the track point, the Kalman filter is used to obtain the optimal value of the curvature of the track point.

Preferably, the method further comprises:

The parameter values of the Kalman filter are obtained by deep neural network learning according to the true value of the curvature of the track point.

Preferably, the step b comprises:

An observation of the curvature of the track point in the travel trajectory is calculated using a Gauss-Newton method.

Preferably, the step c comprises:

According to the steering wheel angle corresponding to the track point in the real-time monitored vehicle, the predicted value of the curvature of the track point is obtained by using the Ackerman steering principle.

According to another aspect of the present invention, there is also provided an apparatus for calculating a curvature of a vehicle trajectory, wherein the apparatus comprises:

a fitting device, configured to obtain a travel trajectory of the vehicle by a curve curve fitting according to GPS information of the vehicle collected in real time;

a computing device configured to calculate an observation value of a curvature of a track point in the travel track;

a monitoring device, configured to obtain a predicted value of a curvature of the track point according to a steering wheel angle corresponding to the track point of the vehicle in real time;

And an optimizing device, configured to obtain an optimal value of the curvature of the track point by using Kalman filtering according to the observed value and the predicted value of the curvature of the track point.

Preferably, the device further comprises:

The learning device is configured to obtain the parameter value of the Kalman filter by deep neural network learning according to the true value of the curvature of the track point.

Preferably, said computing device is for:

Preferably, said detecting means is for:

According to still another aspect of the present invention, there is also provided a computer readable storage medium storing computer code, the method of any of the foregoing being executed when the computer code is executed .

According to still another aspect of the present invention, there is also provided a computer program product, the method of any of the preceding, when the computer program product is executed by a computer device.

According to still another aspect of the present invention, a computer device is provided, the computer device comprising:

One or more processors;

a memory for storing one or more computer programs;

When the one or more computer programs are executed by the one or more processors, the one or more processors are caused to implement the method of any of the preceding.

Compared with the prior art, the present invention obtains the travel trajectory of the vehicle by the curve curve fitting according to the GPS information of the vehicle collected in real time, and calculates the observation value of the curvature of a certain trajectory point in the travel trajectory according to real-time. Observing a steering wheel angle corresponding to the track point of the vehicle, obtaining a predicted value of the curvature of the track point, and obtaining the track by Kalman filtering according to the observed value and the predicted value of the curvature of the track point The optimal value of the curvature of the point; greatly improves the accuracy of calculating the curvature of the vehicle's trajectory, and when applied to an autonomous vehicle, the feasibility of automatic driving can be improved.

Further, the key parameters of the Kalman filter are learned by using a deep neural network. The present invention continuously learns the parameters through deep neural network learning, thereby optimizing the Kalman filter algorithm to obtain an accurate curvature of the track point closer to the real data.

DRAWINGS

Other features, objects, and advantages of the present invention will become more apparent from the Detailed Description of Description

1 shows a block diagram of an exemplary computer system/server 12 suitable for implementing embodiments of the present invention;

2 shows a flow diagram of a method for calculating a curvature of a vehicle trajectory in accordance with an aspect of the present invention;

3 shows a block diagram of an apparatus for calculating the curvature of a vehicle trajectory in accordance with another aspect of the present invention.

The same or similar reference numerals in the drawings denote the same or similar components.

Detailed ways

Before discussing the exemplary embodiments in more detail, it should be noted that some exemplary embodiments are described as a process or method depicted as a flowchart. Although the flowcharts describe various operations as a sequential process, many of the operations can be implemented in parallel, concurrently or concurrently. In addition, the order of operations can be rearranged. The process may be terminated when its operation is completed, but may also have additional steps not included in the figures. The processing may correspond to methods, functions, procedures, subroutines, subroutines, and the like.

By "computer device", also referred to as "computer" in the context, is meant an intelligent electronic device that can perform predetermined processing, such as numerical calculations and/or logical calculations, by running a predetermined program or instruction, which can include a processor and The memory is executed by the processor to execute a predetermined process pre-stored in the memory to execute a predetermined process, or is executed by hardware such as an ASIC, an FPGA, a DSP, or the like, or a combination of the two. Computer devices include, but are not limited to, servers, personal computers, notebook computers, tablets, smart phones, and the like.

The computer device includes a user device and a network device. The user equipment includes, but is not limited to, a computer, a smart phone, a PDA, etc.; the network device includes but is not limited to a single network server, a server group composed of multiple network servers, or a cloud computing based computer Or a cloud composed of a network server, wherein cloud computing is a type of distributed computing, a super virtual computer composed of a group of loosely coupled computers. Wherein, the computer device can be operated separately to implement the present invention, and can also access the network and implement the present invention by interacting with other computer devices in the network. The network in which the computer device is located includes, but is not limited to, the Internet, a wide area network, a metropolitan area network, a local area network, a VPN network, and the like.

It should be noted that the user equipment, the network equipment, the network, and the like are merely examples, and other existing or future possible computer equipment or networks, such as those applicable to the present invention, are also included in the scope of the present invention. It is included here by reference.

The methods discussed below, some of which are illustrated by flowcharts, can be implemented in hardware, software, firmware, middleware, microcode, hardware description languages, or any combination thereof. When implemented in software, firmware, middleware or microcode, the program code or code segments to carry out the necessary tasks can be stored in a machine or computer readable medium, such as a storage medium. The processor(s) can perform the necessary tasks.

The specific structural and functional details disclosed are merely representative and are for the purpose of describing exemplary embodiments of the invention. The present invention may, however, be embodied in many alternative forms and should not be construed as being limited only to the embodiments set forth herein.

It should be understood that although the terms "first," "second," etc. may be used herein to describe the various elements, these elements should not be limited by these terms. These terms are used only to distinguish one unit from another. For example, a first unit could be termed a second unit, and similarly a second unit could be termed a first unit, without departing from the scope of the exemplary embodiments. The term "and/or" used herein includes any and all combinations of one or more of the associated listed items.

It will be understood that when a unit is referred to as "connected" or "coupled" to another unit, it can be directly connected or coupled to the other unit, or an intermediate unit can be present. In contrast, when a unit is referred to as being "directly connected" or "directly coupled" to another unit, there is no intermediate unit. Other words used to describe the relationship between the units should be interpreted in a similar manner (eg "between" and "directly between" and "adjacent to" Than "directly adjacent to", etc.).

The terminology used herein is for the purpose of describing the particular embodiments, The singular forms "a", "an", and, It is also to be understood that the terms "comprising" and """ Other features, integers, steps, operations, units, components, and/or combinations thereof.

It should also be noted that in some alternative implementations, the functions/acts noted may occur in a different order than that illustrated in the drawings. For example, two figures shown in succession may in fact be executed substantially concurrently or sometimes in the reverse order, depending on the function/acts involved.

The invention is further described in detail below with reference to the accompanying drawings.

FIG. 1 illustrates a block diagram of an exemplary computer system/server 12 suitable for use in implementing embodiments of the present invention. The computer system/server 12 shown in FIG. 1 is merely an example and should not impose any limitation on the function and scope of use of the embodiments of the present invention.

As shown in FIG. 1, computer system/server 12 is embodied in the form of a general purpose computing device. The components of computer system/server 12 may include, but are not limited to, one or more processors or processing units 16, system memory 28, and bus 18 that connects different system components, including system memory 28 and processing unit 16.

Bus 18 represents one or more of several types of bus structures, including a memory bus or memory controller, a peripheral bus, a graphics acceleration port, a processor, or a local bus using any of a variety of bus structures. For example, these architectures include, but are not limited to, an Industry Standard Architecture (ISA) bus, a Micro Channel Architecture (MAC) bus, an Enhanced ISA Bus, a Video Electronics Standards Association (VESA) local bus, and peripheral component interconnects ( PCI) bus.

Computer system/server 12 typically includes a variety of computer system readable media. These media can be any available media that can be accessed by computer system/server 12, including both volatile and non-volatile media, removable and non-removable media.

Memory 28 may include computer system readable media in the form of volatile memory, such as random access memory (RAM) 30 and/or cache memory 32. Computer system/server 12 may further include other removable/non-removable, volatile/non-volatile computer system storage media. By way of example only, storage system 34 may be used to read and write non-removable, non-volatile magnetic media (not shown in FIG. 1, commonly referred to as "hard disk drives"). Although not shown in FIG. 1, a disk drive for reading and writing to a removable non-volatile disk (such as a "floppy disk"), and a removable non-volatile disk (such as a CD-ROM, DVD-ROM) may be provided. Or other optical media) read and write optical drive. In these cases, each drive can be coupled to bus 18 via one or more data medium interfaces. Memory 28 can include at least one program product having a set (e.g., at least one) of program modules configured to perform the functions of various embodiments of the present invention.

A program/utility 40 having a set (at least one) of program modules 42 may be stored, for example, in memory 28, such program modules 42 including, but not limited to, an operating system, one or more applications, other programs Modules and program data, each of these examples or some combination may include an implementation of a network environment. Program module 42 typically performs the functions and/or methods of the described embodiments of the present invention.

Computer system/server 12 may also be in communication with one or more external devices 14 (e.g., a keyboard, pointing device, display 24, etc.), and may also be in communication with one or more devices that enable a user to interact with the computer system/server 12. And/or in communication with any device (e.g., network card, modem, etc.) that enables the computer system/server 12 to communicate with one or more other computing devices. This communication can take place via an input/output (I/O) interface 22. Also, computer system/server 12 may also communicate with one or more networks (e.g., a local area network (LAN), a wide area network (WAN), and/or a public network, such as the Internet) through network adapter 20. As shown, network adapter 20 communicates with other modules of computer system/server 12 via bus 18. It should be understood that although not shown in FIG. 1, other hardware and/or software modules may be utilized in conjunction with computer system/server 12, including but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems. , tape drives, and data backup storage systems.

Processing unit 16 executes various functional applications and data processing by running programs stored in memory 28.

For example, the memory 28 stores therein a computer program for performing the functions and processes of the present invention, and when the processing unit 16 executes the corresponding computer program, the identification of the incoming call intention at the network side by the present invention is implemented.

Specific functions/steps of the present invention for calculating the curvature of a vehicle trajectory will be described in detail below.

2 shows a flow diagram of a method for calculating the curvature of a vehicle trajectory in accordance with an aspect of the present invention.

In step S201, the device 1 obtains the travel trajectory of the vehicle by the curve curve fitting according to the GPS information of the vehicle collected in real time.

Specifically, in step S201, the device 1 collects GPS information of the vehicle in real time, for example, by real-time interaction with a GPS device on the vehicle, such as high-precision GPS position information, GPS time information, etc., and GPS time information is further It can be further refined into GPS weekly second time information and GPS nanosecond time information. Here, the high-precision GPS information can be obtained, for example, by RTK (Real-time kinematic) technology, which uses GPS carrier phase observation and utilizes observation errors between the reference station and the mobile station. The spatial correlation removes most of the errors in the observation data of the mobile station by differential means, thereby achieving high-precision positioning, which can obtain centimeter-level positioning accuracy in the field in real time.

Then, after the device 1 collects a plurality of sets of high-precision GPS information of the vehicle in real time, in step S201, the device 1 obtains a set by means of a clothoid spline fitting according to the plurality of sets of high-precision GPS information collected in real time. The trajectory of the vehicle. Here, since the curvature of the convolution curve is linear, and the vehicle trajectory is second-order and the curvature is continuous, the two are similar, and therefore, the gyro curve can be used to fit the trajectory of the vehicle.

Here, the vehicle may be an ordinary vehicle or may be a self-driving vehicle. The self-driving vehicle continuously collects high-precision GPS information of the self-driving vehicle during the automatic driving process or by the manual assisted driving process, such as high-precision GPS position information, GPS weekly second time information, GPS nanometer. Second time information or the like, in step S201, the device 1 acquires high-precision GPS information of the vehicle in real time through interaction with the in-vehicle GPS device of the self-driving vehicle, and obtains the traveling locus of the self-driving vehicle by the curve curve fitting.

Here, the method is applicable to, for example, an end-to-end driving mode in automatic driving of a vehicle, and the end-to-end driving mode refers to an autonomous driving vehicle using an in-vehicle sensor, such as an in-vehicle camera, a vehicle-mounted radar, etc., to sense a surrounding scene to determine how to perform automatic driving. If it is judged whether it is stepping on the accelerator or stepping on the brakes, judging how to drive the steering wheel, etc., the degree of freedom of automatic driving of the vehicle is high; the opposite is the tracking driving mode, which is that the self-driving vehicle uses high-precision GPS to know. Its own position, along the preset trajectory for automatic driving, although relatively safe, but the trajectory is fixed, not so flexible.

Further, the above-mentioned end-to-end driving mode or the tracking driving mode is performed, for example, in a closed campus. Here, the closed campus refers to a limited scenario with a limited route and a limited physical area. In reality, a port such as a port is more common. , parking lots, fairs, campus interiors, etc. Of course, the closed park can also be customized.

Those skilled in the art should understand that the above manner of collecting GPS information of the vehicle is only an example, and other existing or future possible methods for collecting GPS information of the vehicle, if applicable to the present invention, should also be included in the protection scope of the present invention. And is hereby incorporated by reference.

It should also be understood by those skilled in the art that the manner in which the above-mentioned fitting obtains the traveling trajectory of the vehicle is only an example, and other existing or future possible fitting manners for obtaining the traveling trajectory of the vehicle, as applicable to the present invention, should also be It is intended to be included within the scope of the invention and is hereby incorporated by reference.

In step S202, the device 1 calculates an observation value of the curvature of a certain track point in the traveling track.

Specifically, for the traveling trajectory of the vehicle obtained in step S201, in step S202, the device 1 may calculate the curvature of the trajectory point for one of the trajectory points, and use the calculated value as the The observed value of the curvature of the track point. Here, the device 1 can calculate an observation of the curvature of a plurality of track points in the travel trajectory. Here, the curvature of a track point is the instantaneous curvature of the point track point.

Preferably, in step S202, the device 1 calculates an observation value of the curvature of the track point in the travel track using a Gauss-Newton method.

Specifically, in step S202, the device may use a Gauss-Newton method to calculate the curvature of a track point in the travel track obtained by the curve curve fitting in step S201, and use the calculated value as the track. The observed value of the curvature of the point. Here, a certain trajectory point on the trajectory of the vehicle is actually, for example, a corresponding GPS position point of the vehicle, and when the device 1 fits the trajectory of the vehicle by using a gyro curve, it can be regarded as the GPS position of the vehicle. The points are fitted into a convoluted curve. Since when the vehicle's driving trajectory is fitted according to the GPS position point of the vehicle, not all GPS position points may be fitted on the gyro curve, and the actual points in the GPS position point and the gyro curve may exist. The deviation, therefore, in step S202, the apparatus 1 can calculate the observation of the curvature of these GPS position points, that is, the track points, using the Gauss-Newton method.

Here, the basic idea of the Gauss-Newton method is to use the Taylor series expansion to approximate the nonlinear regression model, and then to modify the regression coefficients multiple times through multiple iterations, so that the regression coefficients are continually approaching the best of the nonlinear regression model. The regression coefficient finally minimizes the sum of the residuals of the original model.

It should be understood by those skilled in the art that the manner of calculating the observation value of the curvature of the track point is only an example, and other existing or future possible ways of calculating the curvature of the track point may be applied to the present invention. It is intended to be included within the scope of the invention and is hereby incorporated by reference.

In step S203, the device 1 obtains a predicted value of the curvature of the track point according to the steering wheel angle corresponding to the track point of the vehicle in real time.

Specifically, the curvature of the track point in the trajectory of the vehicle is caused by the turning of the vehicle, and the turning of the vehicle necessarily corresponds to a certain angle of rotation of the steering wheel on the vehicle, where the vehicle can turn at a larger angle. Then, the curvature of the corresponding track point is larger, and the curve can be turned only by a small angle, and the curvature of the corresponding track point is smaller. In step S203, the device 1 can monitor the steering wheel angle of the steering wheel of the vehicle in real time. For example, the steering wheel of the vehicle can be pre-loaded with a corresponding sensor, and the device 1 interacts with the sensor in real time, and the steering angle of the steering wheel can be obtained by the sensor. After that, the device 1 can obtain the curvature of the track point by a certain conversion calculation according to the steering wheel angle corresponding to the upper steering wheel of the vehicle when the vehicle travels to the track point, and obtain the calculated value. As a predicted value of the curvature of the track point.

Preferably, in step S203, the device 1 obtains a predicted value of the curvature of the track point according to the steering wheel angle corresponding to the track point corresponding to the track point in real time using the Ackerman steering principle.

Specifically, the Ackermann steering is a geometry to solve the difference in the center of the inner and outer steering wheel paths when the vehicle turns, and in step S203, the device 1 can adopt the Ackerman steering principle and combine The vehicle dynamics parameter constructs an equation of curvature and steering wheel angle, so that the device 1 can calculate the curvature of the track point according to the steering wheel angle corresponding to the vehicle at the track point in real time, and obtain the curvature of the track point by using the equation, and obtain the calculation The value is taken as the predicted value of the curvature of the track point.

For example, suppose that in step S203, the device 1 learns that the vehicle is traveling to a certain trajectory point of the traveling trajectory by real-time interaction with the sensor pre-installed on the steering wheel of the vehicle, and the steering wheel corresponding to the upper steering wheel has a corner angle of 30 degrees, thereby According to the foregoing Ackerman steering principle, the apparatus 1 calculates the curvature of the track point by 0.12 using the equation between the curvature and the steering wheel angle constructed as described above, and uses the value as the predicted value of the curvature of the track point.

It should be understood by those skilled in the art that the manner of calculating the predicted value of the curvature of the track point is only an example, and other existing or future possible methods for calculating the predicted value of the curvature of the track point, as applicable to the present invention, It is intended to be included within the scope of the invention and is hereby incorporated by reference.

In step S204, the device 1 obtains an optimal value of the curvature of the track point by using Kalman filtering according to the observed value and the predicted value of the curvature of the track point.

Specifically, in step S204, the device 1 calculates based on the observed value of the curvature of the track point obtained in step S202 and the predicted value of the curvature of the track point obtained in step S203, using Kalman filtering. An optimal value of the curvature of the track point is obtained.

Here, Kalman filtering is an algorithm that uses the linear system state equation to optimally estimate the state of the system by inputting and outputting observation data through the system. Since the observed data includes the effects of noise and interference in the system, the optimal estimate can also be considered as a filtering process. Data filtering is a data processing technique that removes noise and restores real data. Kalman filtering can estimate the state of a dynamic system from a series of data with measurement noise when the measurement variance is known.

The following is a brief introduction to Kalman filtering:

Introducing a system of discrete control processes that can be described by a linear stochastic differential equation:

X(k)=A X(k-1)+B U(k)+W(k)

Plus the measured value of the system:

Z(k)=H X(k)+V(k)

In the above two equations, X(k) is the system state at time k, U(k) is the control amount of the system at time k, and if there is no control amount, it can be 0. A and B are system parameters, which are matrices for multi-model systems. Z(k) is the measured value at time k, and H is the parameter of the measurement system. For multi-measurement systems, H is a matrix. W(k) and V(k) represent the process and measured noise, respectively, which are assumed to be Gaussian white noise whose covariance is Q, R, respectively, assuming that the Gaussian white noise does not change with system state changes.

Assuming that the current system state is k, depending on the model of the system, it can be predicted to appear in the state based on the previous state of the system:

X(k|k-1)=A X(k-1|k-1)+B U(k)...........(1)

In equation (1), X(k|k-1) is the result of prediction using the previous state, X(k-1|k-1) is the result of the previous state, and U(k) is the current state. The amount of control, if there is no control, it can be zero.

The system results have been updated so far, but the covariance corresponding to X(k|k-1) has not been updated. Here, P is used to indicate covariance:

P(k|k-1)=A P(k-1|k-1)A’+Q.........(2)

In equation (2), P(k|k-1) is the covariance corresponding to X(k|k-1), and P(k-1|k-1) is the correspondence of X(k-1|k-1) Covariance, A' denotes the transposed matrix of A, and Q is the covariance of the system process.

The above equations (1) and (2) are predictions of the system, and after obtaining the prediction result of the current state, the measured values of the current state are collected. Combining the predicted and measured values, the optimal estimate X(k|k) of the current state (k) can be obtained:

X(k|k)=X(k|k-1)+Kg(k)(Z(k)-H X(k|k-1))...(3)

Where Kg is the Kalman gain:

Kg(k)=P(k|k-1)H'/(H P(k|k-1)H'+R)......(4)

Here, the optimum estimated value X(k|k) in the k state has been obtained. But in order to keep the Kalman filter running until the end of the system process, we also update the covariance of X(k|k) in the k state:

P(k|k)=(I-Kg(k)H)P(k|k-1)......(5)

A matrix in which I is 1, for a single model single measurement, I=1. When the system enters the k+1 state, P(k|k) is P(k-1|k-1) of equation (2). In this way, the algorithm can proceed to the autoregressive operation.

Here, since the device 1 has obtained the observation value of the curvature of the track point in step S202, the predicted value of the curvature of the track point has been obtained in step S203, and the device 1 may further combine the parameters of the Kalman filter, in step S204. The Kalman filter is used to obtain the optimal value of the curvature of the track point.

Preferably, the method further comprises a step S205 (not shown). In step S205, the device 1 learns the parameter values of the Kalman filter by deep neural network learning according to the true value of the curvature of the track point.

Specifically, the curvature of each track point of the travel trajectory of the vehicle may have a real value, which is provided, for example, by the vehicle manufacturer. In step S205, the device 1 passes the deep neural network according to the true value of the curvature of the track point. Learn to obtain the parameter values of the Kalman filter. For example, the parameter value of the Kalman filter here is the aforementioned covariance, and if it is a covariance matrix, an initial value may be set for the covariance matrix, and the initial value may be set according to experience or even randomly, and the deep neural network is learned. Convergence can be performed. For example, after setting the initial value of the covariance matrix, according to the calculation of the foregoing formula, an error can be obtained, and then the error is fed back, thereby correcting the covariance matrix, and continuously feedback, thereby performing parameter adjustment, so that The covariance matrix is converged, and the Kalman filter algorithm is optimized. Therefore, in step S205, the device 1 learns through the deep neural network to obtain the parameter values of the Kalman filter. Then, in step S204, the device 1 calculates the optimal value of the curvature of the track point by using the Kalman filter according to the observation value and the predicted value of the curvature of the track point obtained by the foregoing calculation.

Here, the device 1 obtains the travel trajectory of the vehicle by the curve curve fitting according to the GPS information of the vehicle collected in real time, and calculates the observed value of the curvature of a certain trajectory point in the travel trajectory according to the real-time monitored position. Obtaining a steering wheel angle corresponding to the track point of the vehicle, obtaining a predicted value of the curvature of the track point, and obtaining a curvature of the track point by using Kalman filtering according to the observed value and the predicted value of the curvature of the track point. The optimal value greatly improves the accuracy of calculating the curvature of the vehicle's travel trajectory, and when applied to an autonomous vehicle, the feasibility of the automatic driving can be improved.

Further, the key parameters of the Kalman filter are learned using a deep neural network. The device 1 learns through the deep neural network and continuously adjusts the parameters to optimize the Kalman filter algorithm to obtain an accurate curvature of the track point closer to the real data.

The device 1 comprises a fitting device 301, a computing device 302, a monitoring device 303 and an optimization device 304. The device 1 is, for example, located in a computer device, for example located in a vehicle, or further located in an autonomous vehicle, or a network device connected to the vehicle or, further, an autonomous vehicle, via a network, further The device 1 may be partially located in a network device, some of which are located in the vehicle. For example, the aforementioned fitting device 301, computing device 302 and optimization device 304 are located in a network device, and the aforementioned monitoring device 303 is located in the vehicle. It should be understood by those skilled in the art that the location of the above device is merely an example, and other existing or future possible devices may be included in the scope of the present invention, and may be included in the scope of the present invention. It is included here by reference.

The fitting device 301 obtains the traveling trajectory of the vehicle by the curve curve fitting according to the GPS information of the vehicle collected in real time.

Specifically, the fitting device 301 collects GPS information of the vehicle in real time through real-time interaction with the GPS device on the vehicle, such as high-precision GPS position information, GPS time information, etc., and the GPS time information can be further fined. It is converted into GPS weekly time information and GPS nanosecond time information. Here, the high-precision GPS information can be obtained, for example, by RTK (Real-time kinematic) technology, which uses GPS carrier phase observation and utilizes observation errors between the reference station and the mobile station. The spatial correlation removes most of the errors in the observation data of the mobile station by differential means, thereby achieving high-precision positioning, which can obtain centimeter-level positioning accuracy in the field in real time.

Then, after the device 1 collects a plurality of sets of high-precision GPS information of the vehicle in real time, the fitting device 301 obtains the vehicle by the clothoid spline fitting according to the plurality of sets of high-precision GPS information collected in real time. Driving track. Here, since the curvature of the convolution curve is linear, and the vehicle trajectory is second-order and the curvature is continuous, the two are similar, and therefore, the gyro curve can be used to fit the trajectory of the vehicle.

Here, the vehicle may be an ordinary vehicle or may be a self-driving vehicle. The self-driving vehicle continuously collects high-precision GPS information of the self-driving vehicle during the automatic driving process or by the manual assisted driving process, such as high-precision GPS position information, GPS weekly second time information, GPS nanometer. The second time information and the like, the fitting device 301 acquires high-precision GPS information of the vehicle in real time through interaction with the in-vehicle GPS device of the self-driving vehicle, and obtains the traveling trajectory of the self-driving vehicle by the curve curve fitting.

The computing device 302 calculates an observation of the curvature of a certain trajectory point in the travel trajectory.

Specifically, for the traveling trajectory of the vehicle obtained by the fitting device 301, the calculating device 302 may calculate the curvature of the trajectory point for one of the trajectory points, and use the calculated value as the curvature of the trajectory point. Observations. Here, the computing device 1 can calculate an observation of the curvature of a plurality of track points in the travel trajectory. Here, the curvature of a track point is the instantaneous curvature of the point track point.

Preferably, computing device 302 calculates an observation of the curvature of the track point in the travel trajectory using a Gauss-Newton method.

Specifically, the computing device 302 may use a Gauss-Newton method to calculate the curvature of a track point in the travel track obtained by the fitting device 301 by the curve curve fitting, and use the calculated value as the curvature of the track point. Observations. Here, a certain trajectory point on the trajectory of the vehicle is actually, for example, a corresponding GPS position point of the vehicle, and when the fitting device 301 fits the trajectory of the vehicle by using a gyro curve, it can be regarded as the vehicle The GPS position points are fitted into a convoluted curve. Since when the vehicle's driving trajectory is fitted according to the GPS position point of the vehicle, not all GPS position points may be fitted on the gyro curve, and the actual points in the GPS position point and the gyro curve may exist. The deviation, therefore, computing device 302 can calculate the observed values of the curvature of these GPS position points, i.e., track points, using a Gauss-Newton method.

The monitoring device 303 obtains a predicted value of the curvature of the track point according to the steering wheel angle corresponding to the track point of the vehicle in real time.

Specifically, the curvature of the track point in the trajectory of the vehicle is caused by the turning of the vehicle, and the turning of the vehicle necessarily corresponds to a certain angle of rotation of the steering wheel on the vehicle, where the vehicle can turn at a larger angle. Then, the curvature of the corresponding track point is larger, and the curve can be turned only by a small angle, and the curvature of the corresponding track point is smaller. The monitoring device 303 can monitor the steering wheel angle of the steering wheel of the vehicle in real time. For example, the steering wheel of the vehicle can be pre-loaded with a corresponding sensor, and the monitoring device 303 interacts with the sensor in real time, and the rotation angle of the steering wheel can be obtained by the sensor; thereafter, The monitoring device 303 can obtain the curvature of the track point by a certain conversion calculation according to the steering wheel angle corresponding to the upper steering wheel of the vehicle when the vehicle travels to the track point, and use the calculated value as the The predicted value of the curvature of the track point.

Preferably, the monitoring device 303 obtains a predicted value of the curvature of the track point according to the steering wheel angle corresponding to the vehicle at the track point in real time and adopts the Ackerman steering principle.

Specifically, Ackermann steering (Ackermann steering) is a geometry to solve the different angles of the inner and outer steering wheel paths when the vehicle turns, and the monitoring device 303 can adopt the Ackerman steering principle and combine the vehicle dynamic parameters. An equation of curvature and steering wheel angle is constructed, so that the monitoring device 303 can calculate the curvature of the track point according to the steering wheel angle corresponding to the track point of the vehicle in real time, and calculate the obtained track point by using the equation. The predicted value of the curvature of the track point.

For example, if the monitoring device 303 knows in real-time interaction with the sensor preloaded by the steering wheel of the vehicle that the vehicle is traveling to a certain trajectory point of the driving trajectory, the steering wheel corresponding to the upper steering wheel has a corner angle of 30 degrees, thereby the monitoring device 303 According to the foregoing Ackerman steering principle, using the equation between the curvature and the steering wheel angle constructed above, the curvature of the track point is calculated to be 0.12, and the value is used as the predicted value of the curvature of the track point.

The optimizing means 304 obtains an optimum value of the curvature of the track point by using Kalman filtering according to the observed value and the predicted value of the curvature of the track point.

Specifically, the optimization device 304 calculates the obtained track point by using Kalman filtering according to the observation value of the curvature of the track point obtained by the calculation device 302 and the predicted value of the curvature of the track point obtained by the monitoring device 303. The optimal value of the curvature.

Here, Kalman filtering is an algorithm that uses the linear system state equation to estimate the state of the system through input and output of the system. Since the observed data includes the effects of noise and interference in the system, the optimal estimate can also be considered as a filtering process. Data filtering is a data processing technique that removes noise and restores real data. Kalman filtering can estimate the state of a dynamic system from a series of data with measurement noise when the measurement variance is known.

The following is a brief introduction to Kalman filtering:

X(k)=A X(k-1)+B U(k)+W(k)

Plus the measured value of the system:

Z(k)=H X(k)+V(k)

X(k|k-1)=A X(k-1|k-1)+B U(k)...........(1)

P(k|k-1)=A P(k-1|k-1)A’+Q.........(2)

X(k|k)=X(k|k-1)+Kg(k)(Z(k)-H X(k|k-1))...(3)

Where Kg is the Kalman gain:

Kg(k)=P(k|k-1)H'/(H P(k|k-1)H'+R)......(4)

P(k|k)=(I-Kg(k)H)P(k|k-1)......(5)

A matrix in which I is 1, for a single model single measurement, I=1. When the system enters the k+1 state, P(k|k) is P(k-1|k-1) of the equation (2). In this way, the algorithm can proceed to the autoregressive operation.

Here, since the computing device 302 has obtained the observed value of the curvature of the track point, the monitoring device 303 has obtained the predicted value of the curvature of the track point, and the optimizing device 304 can further combine the parameters of the Kalman filter to obtain the track using Kalman filtering. The optimal value of the curvature of the point.

Preferably, the device 1 further comprises a learning device (not shown). The learning device learns the parameter values of the Kalman filter by deep neural network learning according to the true value of the curvature of the track point.

Specifically, the curvature of each track point of the travel trajectory of the vehicle may have a real value, which is provided, for example, by a vehicle manufacturer, and the learning device learns through the deep neural network according to the true value of the curvature of the track point to obtain the Karl. The parameter value of the Manchester filter. For example, the parameter value of the Kalman filter here is the aforementioned covariance, and if it is a covariance matrix, an initial value may be set for the covariance matrix, and the initial value may be set according to experience or even randomly, and the deep neural network is learned. Convergence can be performed. For example, after setting the initial value of the covariance matrix, according to the calculation of the foregoing formula, an error can be obtained, and then the error is fed back, thereby correcting the covariance matrix, and continuously feedback, thereby performing the parameter adjustment, so that the parameter The covariance matrix is converged and the Kalman filter algorithm is optimized. Therefore, the learning device learns the deep neural network to obtain the parameter values of the Kalman filter. Then, the optimization device 304 calculates the optimal value of the curvature of the track point by using the Kalman filter according to the observation value and the predicted value of the curvature of the track point obtained by the foregoing calculation.

The present invention also provides a computer readable storage medium storing computer code, the method of any of which is performed when the computer code is executed.

The invention also provides a computer program product, the method of any of the preceding one being performed when the computer program product is executed by a computer device.

The invention also provides a computer device, the computer device comprising:

One or more processors;

a memory for storing one or more computer programs;

It should be noted that the present invention can be implemented in software and/or a combination of software and hardware. For example, the various devices of the present invention can be implemented using an application specific integrated circuit (ASIC) or any other similar hardware device. In one embodiment, the software program of the present invention may be executed by a processor to implement the steps or functions described above. Likewise, the software program (including related data structures) of the present invention can be stored in a computer readable recording medium such as a RAM memory, a magnetic or optical drive or a floppy disk and the like. Additionally, some of the steps or functions of the present invention may be implemented in hardware, for example, as a circuit that cooperates with a processor to perform various steps or functions.

It is apparent to those skilled in the art that the present invention is not limited to the details of the above-described exemplary embodiments, and the present invention can be embodied in other specific forms without departing from the spirit or essential characteristics of the invention. Therefore, the present embodiments are to be considered as illustrative and not restrictive, and the scope of the invention is defined by the appended claims instead All changes in the meaning and scope of equivalent elements are included in the present invention. Any reference signs in the claims should not be construed as limiting the claim. In addition, it is to be understood that the word "comprising" does not exclude other elements or steps. A plurality of units or devices recited in the system claims can also be implemented by a unit or device by software or hardware. The first, second, etc. words are used to denote names and do not denote any particular order.

Claims

A method of calculating a curvature of a vehicle trajectory, wherein the method comprises:

Obtaining a travel trajectory of the vehicle by a curve curve fitting according to GPS information of the vehicle collected in real time;

Calculating an observation value of a curvature of a track point in the travel track;

Obtaining a predicted value of the curvature of the track point according to a steering wheel angle corresponding to the track point in the real-time monitored vehicle;

According to the observed value and the predicted value of the curvature of the track point, the Kalman filter is used to obtain the optimal value of the curvature of the track point.
The method of claim 1 wherein the method further comprises:

The parameter values of the Kalman filter are obtained by deep neural network learning according to the true value of the curvature of the track point.
The method of claim 1 or 2, wherein said step b comprises:

An observation of the curvature of the track point in the travel trajectory is calculated using a Gauss-Newton method.
The method according to any one of claims 1 to 3, wherein the step c comprises:

According to the steering wheel angle corresponding to the track point in the real-time monitored vehicle, the predicted value of the curvature of the track point is obtained by using the Ackerman steering principle.
An apparatus for calculating a curvature of a vehicle trajectory, wherein the apparatus comprises:

a fitting device, configured to obtain a travel trajectory of the vehicle by a curve curve fitting according to GPS information of the vehicle collected in real time;

a computing device configured to calculate an observation value of a curvature of a track point in the travel track;

a monitoring device, configured to obtain a predicted value of a curvature of the track point according to a steering wheel angle corresponding to the track point of the vehicle in real time;

And an optimizing device, configured to obtain an optimal value of the curvature of the track point by using Kalman filtering according to the observed value and the predicted value of the curvature of the track point.
The device of claim 5, wherein the device further comprises:

The learning device is configured to obtain the parameter value of the Kalman filter by deep neural network learning according to the true value of the curvature of the track point.
Apparatus according to claim 5 or claim 6, wherein said computing means is for:

An observation of the curvature of the track point in the travel trajectory is calculated using a Gauss-Newton method.
The device according to any one of claims 5 to 7, wherein the detecting means is for:

According to the steering wheel angle corresponding to the track point in the real-time monitored vehicle, the predicted value of the curvature of the track point is obtained by using the Ackerman steering principle.
A computer readable storage medium storing computer code, the method of any one of claims 1 to 4 being executed when the computer code is executed.
A computer program product, the method of any one of claims 1 to 4 being executed when the computer program product is executed by a computer device.
A computer device, the computer device comprising:

One or more processors;

a memory for storing one or more computer programs;

When the one or more computer programs are executed by the one or more processors, the one or more processors are caused to implement the method of any one of claims 1 to 4.