CN107817790B - Method and device for calculating curvature of vehicle track - Google Patents

Method and device for calculating curvature of vehicle track Download PDF

Info

Publication number
CN107817790B
CN107817790B CN201710792205.1A CN201710792205A CN107817790B CN 107817790 B CN107817790 B CN 107817790B CN 201710792205 A CN201710792205 A CN 201710792205A CN 107817790 B CN107817790 B CN 107817790B
Authority
CN
China
Prior art keywords
curvature
vehicle
track point
track
value
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201710792205.1A
Other languages
Chinese (zh)
Other versions
CN107817790A (en
Inventor
姜雨
郁浩
闫泳杉
郑超
唐坤
张云飞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Baidu Netcom Science and Technology Co Ltd
Original Assignee
Beijing Baidu Netcom Science and Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Baidu Netcom Science and Technology Co Ltd filed Critical Beijing Baidu Netcom Science and Technology Co Ltd
Priority to CN201710792205.1A priority Critical patent/CN107817790B/en
Publication of CN107817790A publication Critical patent/CN107817790A/en
Priority to PCT/CN2018/098609 priority patent/WO2019047639A1/en
Application granted granted Critical
Publication of CN107817790B publication Critical patent/CN107817790B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/02—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to ambient conditions
    • B60W40/06—Road conditions
    • B60W40/072—Curvature of the road
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0253—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting relative motion information from a plurality of images taken successively, e.g. visual odometry, optical flow
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0285—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using signals transmitted via a public communication network, e.g. GSM network
    • G—PHYSICS
    • G16—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR SPECIFIC APPLICATION FIELDS
    • G16Z—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR SPECIFIC APPLICATION FIELDS, NOT OTHERWISE PROVIDED FOR
    • G16Z99/00—Subject matter not provided for in other main groups of this subclass

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Mathematical Physics (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)

Abstract

The invention aims to provide a method and a device for calculating the curvature of a vehicle track. Compared with the prior art, the method has the advantages that the running track of the vehicle is obtained through clothoid fitting according to the GPS information of the vehicle collected in real time, the observed value of the curvature of a certain track point in the running track is calculated, the predicted value of the curvature of the track point is obtained according to the steering wheel corner, corresponding to the track point, of the vehicle monitored in real time, and then the optimal value of the curvature of the track point is obtained through Kalman filtering according to the observed value and the predicted value of the curvature of the track point; the accuracy of calculating the curvature of the vehicle running track is greatly improved, and when the method is applied to an automatic driving vehicle, the feasibility of automatic driving can be improved.

Description

Method and device for calculating curvature of vehicle track
Technical Field
The invention relates to the technical field of automatic driving of vehicles, in particular to a technology for calculating the curvature of a vehicle track.
Background
In the process of vehicle driving, the curvature calculation of the track points in the vehicle driving track is of great importance, and particularly in the process of automatic vehicle driving, if the curvature of each track point of the driving track of the automatic driving vehicle in the automatic driving process can be accurately calculated, the subsequent determination of the driving mode, the driving direction, the driving track and the like can be further accurately performed. However, the existing method for calculating the curvature of the track point has insufficient accuracy, and particularly cannot calculate the small curvature.
Therefore, how to provide an efficient and accurate method for calculating the curvature of the vehicle track becomes one of the problems that those skilled in the art need to solve.
Disclosure of Invention
The invention aims to provide a method and a device for calculating the curvature of a vehicle track.
According to an aspect of the present invention, there is provided a method of calculating a curvature of a vehicle trajectory, wherein the method comprises:
a, according to GPS information of a vehicle collected in real time, obtaining a running track of the vehicle through clothoid fitting;
b, calculating an observed value of the curvature of a certain track point in the driving track;
c, obtaining a predicted value of the curvature of the track point according to a steering wheel corner corresponding to the track point of the vehicle monitored in real time;
and d, obtaining the optimal value of the curvature of the track point by adopting Kalman filtering according to the observed value and the predicted value of the curvature of the track point.
Preferably, the method further comprises:
and obtaining the parameter value of Kalman filtering through deep neural network learning according to the actual value of the curvature of the track point.
Preferably, the step b includes:
and calculating the observed value of the curvature of the track point in the driving track by adopting a Gaussian-Newton method.
Preferably, the step c includes:
and obtaining a predicted value of the curvature of the track point by adopting an Ackerman steering principle according to the steering wheel corner corresponding to the track point of the vehicle monitored in real time.
According to another aspect of the present invention, there is also provided an apparatus for calculating a curvature of a trajectory of a vehicle, wherein the apparatus includes:
the fitting device is used for obtaining the running track of the vehicle through clothoid fitting according to the GPS information of the vehicle collected in real time;
the calculating device is used for calculating an observed value of the curvature of a certain track point in the driving track;
the monitoring device is used for acquiring a predicted value of the curvature of the track point according to the steering wheel corner corresponding to the track point of the vehicle monitored in real time;
and the optimization device is used for obtaining the optimal value of the curvature of the track point by adopting Kalman filtering according to the observed value and the predicted value of the curvature of the track point.
Preferably, the apparatus further comprises:
and the learning device is used for obtaining the parameter value of the Kalman filtering through deep neural network learning according to the actual value of the curvature of the track point.
Preferably, the computing device is configured to:
and calculating the observed value of the curvature of the track point in the driving track by adopting a Gaussian-Newton method.
Preferably, the detection means is for:
and obtaining a predicted value of the curvature of the track point by adopting an Ackerman steering principle according to the steering wheel corner corresponding to the track point of the vehicle monitored in real time.
According to yet another aspect of the invention, there is also provided a computer readable storage medium storing computer code which, when executed, performs a method as in any one of the preceding.
According to yet another aspect of the invention, there is also provided a computer program product, which when executed by a computer device, performs the method of any of the preceding claims.
According to still another aspect of the present invention, there is also provided a computer apparatus including:
one or more processors;
a memory for storing one or more computer programs;
the one or more computer programs, when executed by the one or more processors, cause the one or more processors to implement the method of any preceding claim.
Compared with the prior art, the method has the advantages that the running track of the vehicle is obtained through clothoid fitting according to the GPS information of the vehicle collected in real time, the observed value of the curvature of a certain track point in the running track is calculated, the predicted value of the curvature of the track point is obtained according to the steering wheel corner, corresponding to the track point, of the vehicle monitored in real time, and then the optimal value of the curvature of the track point is obtained through Kalman filtering according to the observed value and the predicted value of the curvature of the track point; the accuracy of calculating the curvature of the vehicle running track is greatly improved, and when the method is applied to an automatic driving vehicle, the feasibility of automatic driving can be improved.
Further, key parameters of Kalman filtering are obtained by deep neural network learning, and parameters are continuously adjusted through the deep neural network learning, so that the Kalman filtering algorithm is optimized, and accurate curvature of a track point closer to real data is obtained.
Drawings
Other features, objects and advantages of the invention will become more apparent upon reading of the detailed description of non-limiting embodiments made with reference to the following drawings:
FIG. 1 illustrates a block diagram of an exemplary computer system/server 12 suitable for use in implementing embodiments of the present invention;
FIG. 2 illustrates a flow diagram of a method for calculating curvature of a vehicle trajectory in accordance with an aspect of the present invention;
fig. 3 shows a schematic configuration of an apparatus for calculating a curvature of a vehicle trajectory according to another aspect of the present invention.
The same or similar reference numbers in the drawings identify the same or similar elements.
Detailed Description
Before discussing exemplary embodiments in more detail, it should be noted that some exemplary embodiments are described as processes or methods depicted as flowcharts. Although a flowchart may describe the operations as a sequential process, many of the operations can be performed in parallel, concurrently, or simultaneously. In addition, the order of the operations may be re-arranged. The process may be terminated when its operations are completed, but may have additional steps not included in the figure. The processes may correspond to methods, functions, procedures, subroutines, and the like.
The term "computer device" or "computer" in this context refers to an intelligent electronic device that can execute predetermined processes such as numerical calculation and/or logic calculation by running predetermined programs or instructions, and may include a processor and a memory, wherein the processor executes a pre-stored instruction stored in the memory to execute the predetermined processes, or the predetermined processes are executed by hardware such as ASIC, FPGA, DSP, or a combination thereof. Computer devices include, but are not limited to, servers, personal computers, laptops, tablets, smart phones, and the like.
The computer equipment comprises user equipment and network equipment. Wherein the user equipment includes but is not limited to computers, smart phones, PDAs, etc.; the network device includes, but is not limited to, a single network server, a server group consisting of a plurality of network servers, or a Cloud Computing (Cloud Computing) based Cloud consisting of a large number of computers or network servers, wherein Cloud Computing is one of distributed Computing, a super virtual computer consisting of a collection of loosely coupled computers. Wherein the computer device can be operated alone to implement the invention, or can be accessed to a network and implement the invention through interoperation 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 device, the network, etc. are only examples, and other existing or future computer devices or networks may also be included in the scope of the present invention, and are included by reference.
The methods discussed below, some of which are illustrated by flow diagrams, may be implemented by 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 perform the necessary tasks may be stored in a machine or computer readable medium such as a storage medium. The processor(s) may perform the necessary tasks.
Specific structural and functional details disclosed herein are merely representative and are provided for purposes of describing example embodiments of the present invention. The present invention may, however, be embodied in many alternate forms and should not be construed as limited to only the embodiments set forth herein.
It will be understood that, although the terms first, second, etc. may be used herein to describe various elements, these elements should not be limited by these terms. These terms are only used to distinguish one element from another. For example, a first element may be termed a second element, and, similarly, a second element may be termed a first element, without departing from the scope of example embodiments. As used herein, the term "and/or" includes any and all combinations of one or more of the associated listed items.
It will be understood that when an element is referred to as being "connected" or "coupled" to another element, it can be directly connected or coupled to the other element or intervening elements may be present. In contrast, when an element is referred to as being "directly connected" or "directly coupled" to another element, there are no intervening elements present. Other words used to describe the relationship between elements (e.g., "between" versus "directly between", "adjacent" versus "directly adjacent to", etc.) should be interpreted in a similar manner.
The terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of example embodiments. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will be further understood that the terms "comprises" and/or "comprising," when used herein, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.
It should also be noted that, in some alternative implementations, the functions/acts noted may occur out of the order noted in the figures. For example, two figures shown in succession may, in fact, be executed substantially concurrently, or the figures may sometimes be executed in the reverse order, depending upon the functionality/acts involved.
The present invention is described in further detail below with reference to the attached drawing figures.
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 only one example and should not be taken to limit the scope of use or the functionality of embodiments of the present invention.
As shown in FIG. 1, computer system/server 12 is 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, a system memory 28, and a bus 18 that couples various system components including the system memory 28 and the processing unit 16.
Bus 18 represents one or more of any of several types of bus structures, including a memory bus or memory controller, a peripheral bus, an accelerated graphics port, and a processor or local bus using any of a variety of bus architectures. By way of example, such architectures include, but are not limited to, Industry Standard Architecture (ISA) bus, micro-channel architecture (MAC) bus, enhanced ISA bus, Video Electronics Standards Association (VESA) local bus, and Peripheral Component Interconnect (PCI) bus.
Computer system/server 12 typically includes a variety of computer system readable media. Such media may be any available media that is accessible by computer system/server 12 and includes both volatile and nonvolatile media, removable and non-removable media.
The 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. The computer system/server 12 may further include other removable/non-removable, volatile/nonvolatile computer system storage media. By way of example only, storage system 34 may be used to read from and write to non-removable, nonvolatile magnetic media (not shown in FIG. 1, and commonly referred to as a "hard drive"). Although not shown in FIG. 1, a magnetic disk drive for reading from and writing to a removable, nonvolatile magnetic disk (e.g., a "floppy disk") and an optical disk drive for reading from or writing to a removable, nonvolatile optical disk (e.g., a CD-ROM, DVD-ROM, or other optical media) may be provided. In these cases, each drive may be connected to bus 18 by one or more data media interfaces. Memory 28 may include at least one program product having a set (e.g., at least one) of program modules that are configured to carry out the functions of embodiments of the 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 application programs, other program modules, and program data, each of which examples or some combination thereof may comprise an implementation of a network environment. Program modules 42 generally carry out the functions and/or methodologies of the described embodiments of the invention.
The computer system/server 12 may also communicate with one or more external devices 14 (e.g., keyboard, pointing device, display 24, etc.), with one or more devices that enable a user to interact with the computer system/server 12, and/or with any devices (e.g., network card, modem, etc.) that enable the computer system/server 12 to communicate with one or more other computing devices. Such communication may be through an input/output (I/O) interface 22. Also, the computer system/server 12 may 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) via the network adapter 20. As shown, network adapter 20 communicates with the other modules of computer system/server 12 via bus 18. It should be appreciated that although not shown in FIG. 1, other hardware and/or software modules may be used in conjunction with the 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, among others.
The processing unit 16 executes various functional applications and data processing by executing programs stored in the memory 28.
For example, the memory 28 stores a computer program for executing the functions and processes of the present invention, and the processing unit 16 executes the computer program, so that the present invention recognizes the intention of an incoming call on the network side.
The specific functions/steps of the present invention for calculating the curvature of the vehicle trajectory will be described in detail below.
FIG. 2 illustrates a flow diagram of a method for calculating curvature of a vehicle trajectory in accordance with an aspect of the present invention.
In step S201, the device 1 obtains the driving track of the vehicle through clothoid fitting according to the GPS information of the vehicle collected in real time.
Specifically, in step S201, the device 1 acquires, for example, GPS information of the vehicle, such as high-precision GPS position information, GPS time information, and the like, in real time through real-time interaction with a GPS device on the vehicle, and the GPS time information may be further refined into GPS weekend second time information and GPS nanosecond time information. The high-precision GPS information can be obtained, for example, by an RTK (Real-time kinematic) technique that uses a carrier phase observation of the GPS, and removes most of errors in observation data of the rover station by a differential method using a spatial correlation of an observation error between the reference station and the rover station, thereby realizing high-precision positioning that can obtain centimeter-level positioning precision in Real time in the field.
Subsequently, after the device 1 acquires sets of high-precision GPS information of the vehicle in real time, in step S201, the device 1 obtains a driving trajectory of the vehicle by clothoid spline fitting from these sets of high-precision GPS information acquired in real time. Here, since the curvature of the clothoid curve is linear and the vehicle trajectory is second-order conductive and the curvature is continuous, both are similar, the clothoid curve can be used to fit the travel trajectory of the vehicle.
Here, the vehicle may be a general vehicle, or may further be an autonomous vehicle. In the automatic driving process or the manual auxiliary driving process of the automatic driving vehicle, the vehicle-mounted GPS device continuously collects high-precision GPS information of the automatic driving vehicle, such as high-precision GPS position information, GPS weekly-second time information, GPS nanosecond time information, and the like, in step S201, the device 1 collects the high-precision GPS information of the vehicle in real time through interaction with the vehicle-mounted GPS device of the automatic driving vehicle, and obtains the driving track of the automatic driving vehicle through clothoid fitting.
The method is suitable for an end-to-end driving mode in automatic driving of the vehicle, for example, the end-to-end driving mode means that the automatic driving vehicle senses surrounding scenes by using a vehicle-mounted sensor, such as a vehicle-mounted camera, a vehicle-mounted radar and the like, so as to judge how to perform automatic driving, such as judging whether to step on an accelerator or a brake, judging how to turn on a steering wheel and the like, and the degree of freedom of automatic driving of the vehicle is high; the tracking driving mode is that the automatic driving vehicle acquires the position of the vehicle by using a high-precision GPS and automatically drives along a preset track, and although the tracking driving mode is safe relatively, the driving track is fixed and not flexible.
Further, the end-to-end driving mode or the tracking driving mode is performed in a closed park, for example, where the closed park refers to a limited scene with limited routes and limited physical areas, such as ports, parking lots, exhibition places, campus interiors, and the like, which are common in reality, and of course, the closed park may be customized.
It should be understood by those skilled in the art that the above-mentioned manner of collecting vehicle GPS information is only an example, and other manners of collecting vehicle GPS information that may be present or later come to be suitable for the present invention are also included within the scope of the present invention and are hereby incorporated by reference.
It should also be understood by those skilled in the art that the above-mentioned manner of fitting to obtain the driving trajectory of the vehicle is only an example, and other manners of fitting to obtain the driving trajectory of the vehicle, which may occur now or in the future, such as may be applicable to the present invention, are also included in the scope of the present invention and are hereby incorporated by reference.
In step S202, the device 1 calculates an observed value of the curvature of a certain track point in the travel locus.
Specifically, for the travel locus of the vehicle fitted in step S201, in step S202, the device 1 may calculate the curvature of a certain track point among the track points, and take the value obtained by the calculation as the observed value of the curvature of the track point. In this case, the device 1 can calculate the observed values of the curvature of a plurality of trajectory points in the travel trajectory. Here, the curvature of a certain track point is the instantaneous curvature of the finger track point.
Preferably, in step S202, the device 1 calculates the observed value of the curvature of the locus point in the travel locus by using the gauss-newton method.
Specifically, in step S202, the apparatus may calculate the curvature of a certain locus point in the travel locus obtained by clothoid fitting in step S201 by using the gauss-newton method, and use the value obtained by the calculation as the observed value of the curvature of the locus point. Here, a certain track point on the driving track of the vehicle is actually, for example, a corresponding GPS position point of the vehicle, and when the device 1 fits the driving track of the vehicle with a clothoid curve, it can be regarded that the GPS position points of the vehicle are fitted into a clothoid curve. Since not all GPS position points may be fitted on the clothoid curve when the travel locus of the vehicle is fitted with the clothoid curve from the GPS position points of the vehicle, and these GPS position points may be deviated from actual points in the clothoid curve, the device 1 may calculate the observed value of the curvature of these GPS position points, that is, the track points, using the gauss-newton method in step S202.
The basic idea of the gauss-newton method is to use a taylor series expansion to approximately replace a nonlinear regression model, then modify the regression coefficient for multiple times through multiple iterations, so that the regression coefficient continuously approaches to the optimal regression coefficient of the nonlinear regression model, and finally the sum of the squares of the residuals of the original model is minimized.
It will be understood by those skilled in the art that the above-described manner of calculating an observed value of curvature of a trace point is merely exemplary, and other manners of calculating an observed value of curvature of a trace point that may exist or may later become known are encompassed within the scope of the present invention, as applicable thereto, and are 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 corner corresponding to the track point of the vehicle monitored in real time.
Specifically, the curvature of the track point in the driving track of the vehicle is caused by the turning of the vehicle, and the turning of the vehicle necessarily corresponds to a certain turning angle of a steering wheel on the vehicle, wherein the vehicle can turn at a large angle, the curvature of the corresponding track point is large at the moment, or can turn at a small angle, and the curvature of the corresponding track point is small at the moment. In step S203, the device 1 may monitor the steering wheel angle of the steering wheel of the vehicle in real time, for example, the steering wheel of the vehicle may be pre-equipped with a corresponding sensor, and the device 1 interacts with the sensor in real time, and the steering wheel angle may be obtained through the sensor; after that, the device 1 may obtain the curvature of the track point through a certain conversion calculation according to the steering wheel rotation angle corresponding to the steering wheel of the vehicle when the vehicle travels to the track point, which is monitored in real time, and use the value obtained through the calculation as the 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 by using ackermann steering principle according to the steering wheel angle of the vehicle corresponding to the track point monitored in real time.
Specifically, Ackermann steering (Ackermann steering) is a geometry that the circle centers of the inner and outer steering wheel paths are different when the vehicle turns, and in step S203, the device 1 may use the Ackermann steering principle and construct an equation of curvature and steering wheel angle by combining vehicle dynamics parameters, so that the device 1 may calculate the curvature of a track point according to the steering wheel angle, which is monitored in real time, corresponding to the track point of the vehicle at the track point, using the equation, and use the value obtained by the calculation as a predicted value of the curvature of the track point.
For example, it is assumed that in step S203, the device 1 knows that the steering wheel angle corresponding to the steering wheel of the vehicle is 30 degrees when the vehicle travels to a certain track point of the driving track through real-time interaction with a sensor pre-installed on the steering wheel of the vehicle, so that the device 1 obtains the curvature of the track point as 0.12 through calculation according to the aforementioned ackerman steering principle by using the aforementioned established equation between the curvature and the steering wheel angle, and uses the value as the predicted value of the curvature of the track point.
It will be understood by those skilled in the art that the above-described manner of calculating the predicted value of curvature of the trace point is merely exemplary, and other manners of calculating the predicted value of curvature of the trace point, which may be present or may occur in the future, such as may be applicable to the present invention, are also included within the scope of the present invention and are 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 apparatus 1 calculates an optimal value for obtaining the curvature of the locus point using kalman filtering from the observed value of the curvature of the locus point obtained in step S202 and from the predicted value of the curvature of the locus point obtained in step S203.
Here, Kalman filtering (Kalman filtering) is an algorithm that performs optimal estimation of a system state by outputting observation data through a system input using a linear system state equation. The optimal estimation can also be seen as a filtering process, since the observed data includes the effects of noise and interference in the system. The data filtering is a data processing technology for removing noise and restoring real data, and the Kalman filtering can estimate the state of a dynamic system from a series of data with measurement noise under the condition that the measurement variance is known.
Kalman filtering is briefly introduced below:
a system for introducing a discrete control process, which can be described by a linear random differential equation:
X(k)＝AX(k-1)+B U(k)+W(k)
plus the system measurements:
Z(k)＝H X(k)+V(k)
in the above two equations, x (k) is the system state at time k, and u (k) is the control amount for the system at time k, which may be 0 if there is no control amount. A and B are system parameters, which for a multi-model system are matrices. Z (k) is the measured value at time k, H is a parameter of the measurement system, and H is a matrix for a multi-measurement system. W (k) and v (k) represent process and measured noise, respectively, which is assumed to be white gaussian noise with covariance Q, R, respectively, assuming that the white gaussian noise does not change with system state changes.
Assuming that the present system state is k, according to the model of the system, the present state can be predicted based on the last state of the system:
X(k|k-1)＝AX(k-1|k-1)+B U(k)………(1)
in the formula (1), X (k | k-1) is the result predicted by the previous state, X (k-1| k-1) is the optimum result of the previous state, and U (k) is the control amount of the current state, which may be 0 if there is no control amount.
The system results have been updated so far, but the covariance corresponding to X (k | k-1) has not been updated. Here, covariance is denoted by P:
P(k|k-1)＝AP(k-1|k-1)A’+Q………(2)
in the formula (2), P (k | k-1) is a covariance corresponding to X (k | k-1), P (k-1| k-1) is a covariance corresponding to X (k-1| k-1), A' represents a transposed matrix of A, and Q is a covariance of the system process.
The above equations (1) and (2) are predictions of the system, and after obtaining the prediction result of the present state, the measured values of the present state are collected. Combining the predicted values and the measured values, an optimized estimated value 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. However, in order to make the kalman filter continuously run until the system process is finished, the covariance of X (k | k) in the k state is updated:
P(k|k)＝(I-Kg(k)H)P(k|k-1)………(5)
where I is a matrix of 1, I ═ 1 for single model single measurements. When the system enters the k +1 state, P (k | k) is P (k-1| k-1) of equation (2). Thus, the algorithm can proceed with autoregressive operation.
Here, since the apparatus 1 has obtained the observed value of the curvature of the track point in step S202 and the predicted value of the curvature of the track point in step S203, the apparatus 1 may further obtain the optimal value of the curvature of the track point using the kalman filter in step S204 in combination with the parameter of the kalman filter.
Preferably, the method further comprises step S205 (not shown). In step S205, the device 1 obtains the parameter value of the kalman filter through deep neural network learning according to the true value of the curvature of the trajectory point.
Specifically, the curvature of each trace point of the travel trace of the vehicle may have a real value provided by, for example, a vehicle manufacturer, and in step S205, the device 1 obtains the parameter value of the kalman filter through deep neural network learning based on the real value of the curvature of the trace point. For example, the parameter value of the kalman filter is the above covariance, such as a covariance matrix, an initial value may be set for the covariance matrix, the initial value may be set empirically or even randomly, and convergence may be performed through the deep neural network learning, for example, after the initial value of the covariance matrix is set, an error may be obtained according to the calculation of the above formula, and the error is fed back, so as to modify the covariance matrix, and continuously feed back, so as to perform parameter adjustment, so that the covariance matrix is converged, and optimize the kalman filter algorithm, therefore, in step S205, the apparatus 1 obtains the parameter value of the kalman filter through the deep neural network learning. Subsequently, in step S204, the apparatus 1 calculates 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 obtained by the foregoing calculation.
The device 1 obtains a running track of the vehicle through clothoid curve fitting according to GPS information of the vehicle collected in real time, calculates an observed value of the curvature of a certain track point in the running track, obtains a predicted value of the curvature of the track point according to a steering wheel corner corresponding to the track point of the vehicle monitored in real time, and obtains an optimal value of the curvature of the track point by adopting Kalman filtering according to the observed value and the predicted value of the curvature of the track point; the accuracy of calculating the curvature of the vehicle running track is greatly improved, and when the method is applied to an automatic driving vehicle, the feasibility of automatic driving can be improved.
Further, key parameters of Kalman filtering are obtained by using deep neural network learning, and the device 1 continuously adjusts parameters through the deep neural network learning, so that the Kalman filtering algorithm is optimized to obtain accurate curvature of a track point closer to real data.
Fig. 3 shows a schematic configuration of an apparatus for calculating a curvature of a vehicle trajectory according to another aspect of the present invention.
The apparatus 1 comprises fitting means 301, calculation means 302, monitoring means 303 and optimization means 304. The apparatus 1 is located, for example, in a computer device, which is located, for example, in a vehicle, or further in an autonomous vehicle, or may be a network device connected to the vehicle or further to the autonomous vehicle via a network, further, the apparatus 1 may be partially located in the network device, and partially located in the vehicle, for example, the fitting means 301, the calculating means 302 and the optimizing means 304 are located in the network device, and the monitoring means 303 is located in the vehicle. It will be understood by those skilled in the art that the above-described arrangements are merely exemplary, and that other arrangements, now known or later developed, which may be suitable for use with the present invention, are also included within the scope of the present invention and are hereby incorporated by reference.
The fitting device 301 obtains the driving track of the vehicle through clothoid fitting according to the GPS information of the vehicle collected in real time.
Specifically, the fitting device 301 collects, for example, GPS information of the vehicle, such as high-precision GPS position information, GPS time information, and the like, in real time through real-time interaction with a GPS device on the vehicle, and the GPS time information may be further refined into GPS weekend second time information and GPS nanosecond time information. The high-precision GPS information can be obtained, for example, by an RTK (Real-time kinematic) technique that uses a carrier phase observation of the GPS, and removes most of errors in observation data of the rover station by a differential method using a spatial correlation of an observation error between the reference station and the rover station, thereby realizing high-precision positioning that can obtain centimeter-level positioning precision in Real time in the field.
Subsequently, after the device 1 acquires sets of high-precision GPS information of the vehicle in real time, the fitting device 301 obtains the driving track of the vehicle by fitting a clothoid spline according to the sets of high-precision GPS information acquired in real time. Here, since the curvature of the clothoid curve is linear and the vehicle trajectory is second-order conductive and the curvature is continuous, both are similar, the clothoid curve can be used to fit the travel trajectory of the vehicle.
Here, the vehicle may be a general vehicle, or may further be an autonomous vehicle. In the automatic driving process or the manual auxiliary driving process of the automatic driving vehicle, the vehicle-mounted GPS device on the automatic driving vehicle continuously acquires high-precision GPS information of the automatic driving vehicle, such as high-precision GPS position information, GPS weekly-second time information, GPS nanosecond time information and the like, the fitting device 301 acquires the high-precision GPS information of the vehicle in real time through interaction with the vehicle-mounted GPS device of the automatic driving vehicle, and obtains the driving track of the automatic driving vehicle through clothoid fitting.
The method is suitable for an end-to-end driving mode in automatic driving of the vehicle, for example, the end-to-end driving mode means that the automatic driving vehicle senses surrounding scenes by using a vehicle-mounted sensor, such as a vehicle-mounted camera, a vehicle-mounted radar and the like, so as to judge how to perform automatic driving, such as judging whether to step on an accelerator or a brake, judging how to turn on a steering wheel and the like, and the degree of freedom of automatic driving of the vehicle is high; the tracking driving mode is that the automatic driving vehicle acquires the position of the vehicle by using a high-precision GPS and automatically drives along a preset track, and although the tracking driving mode is safe relatively, the driving track is fixed and not flexible.
Further, the end-to-end driving mode or the tracking driving mode is performed in a closed park, for example, where the closed park refers to a limited scene with limited routes and limited physical areas, such as ports, parking lots, exhibition places, campus interiors, and the like, which are common in reality, and of course, the closed park may be customized.
It should be understood by those skilled in the art that the above-mentioned manner of collecting vehicle GPS information is only an example, and other manners of collecting vehicle GPS information that may be present or later come to be suitable for the present invention are also included within the scope of the present invention and are hereby incorporated by reference.
It should also be understood by those skilled in the art that the above-mentioned manner of fitting to obtain the driving trajectory of the vehicle is only an example, and other manners of fitting to obtain the driving trajectory of the vehicle, which may occur now or in the future, such as may be applicable to the present invention, are also included in the scope of the present invention and are hereby incorporated by reference.
The calculation means 302 calculates an observed value of the curvature of a certain track point in the travel locus.
Specifically, for the driving trajectory of the vehicle fitted by the fitting device 301, the calculation device 302 may calculate the curvature of a certain track point, and use the value obtained by the calculation as the observed value of the curvature of the track point. Here, the calculation device 1 can calculate the observed values of the curvatures of a plurality of trajectory points in the travel trajectory. Here, the curvature of a certain track point is the instantaneous curvature of the finger track point.
Preferably, the calculation means 302 calculates the observed value of the curvature of the trajectory point in the driving trajectory by using a gauss-newton method.
Specifically, the calculation means 302 may calculate the curvature of a certain track point in the travel track obtained by the fitting means 301 through clothoid curve fitting by using a gaussian-newton method, and use the value obtained by the calculation as the observed value of the curvature of the track point. Here, a certain track point on the driving track of the vehicle is actually, for example, a corresponding GPS position point of the vehicle, and when the fitting device 301 fits the driving track of the vehicle with a clothoid, it can be regarded that the GPS position points of the vehicle are fitted to a clothoid. Since not all GPS position points may be fitted on the clothoid curve when fitting the travel locus of the vehicle with the clothoid curve from the GPS position points of the vehicle, and these GPS position points may have a deviation from actual points in the clothoid curve, the calculation means 302 may calculate the observed value of the curvature of these GPS position points, that is, the track points, using the gauss-newton method.
The basic idea of the gauss-newton method is to use a taylor series expansion to approximately replace a nonlinear regression model, then modify the regression coefficient for multiple times through multiple iterations, so that the regression coefficient continuously approaches to the optimal regression coefficient of the nonlinear regression model, and finally the sum of the squares of the residuals of the original model is minimized.
It will be understood by those skilled in the art that the above-described manner of calculating an observed value of curvature of a trace point is merely exemplary, and other manners of calculating an observed value of curvature of a trace point that may exist or may later become known are encompassed within the scope of the present invention, as applicable thereto, and are hereby incorporated by reference.
The monitoring device 303 obtains a predicted value of the curvature of the track point according to the steering wheel corner corresponding to the track point of the vehicle monitored in real time.
Specifically, the curvature of the track point in the driving track of the vehicle is caused by the turning of the vehicle, and the turning of the vehicle necessarily corresponds to a certain turning angle of a steering wheel on the vehicle, wherein the vehicle can turn at a large angle, the curvature of the corresponding track point is large at the moment, or can turn at a small angle, and the curvature of the corresponding track point is small at the moment. The monitoring device 303 may monitor the steering wheel angle of the steering wheel of the vehicle in real time, for example, the steering wheel of the vehicle may be pre-installed with a corresponding sensor, and the monitoring device 303 interacts with the sensor in real time, and the steering wheel angle may be obtained through the sensor; thereafter, the monitoring device 303 may obtain the curvature of the track point through a certain conversion calculation according to the steering wheel rotation angle corresponding to the steering wheel of the vehicle when the vehicle travels to the track point, which is monitored in real time, and use the value obtained through the calculation as the predicted value of the curvature of the track point.
Preferably, the monitoring device 303 obtains the predicted value of the curvature of the track point by using an ackermann steering principle according to the steering wheel corner corresponding to the track point of the vehicle monitored in real time.
Specifically, Ackermann steering (Ackermann steering) is a geometry that the circle centers of the path pointing to the inner and outer steering wheels are different when the vehicle turns, and the monitoring device 303 may use the Ackermann steering principle and construct an equation of curvature and steering wheel angle by combining vehicle dynamics parameters, so that the monitoring device 303 may calculate the curvature of a track point according to the steering wheel angle corresponding to the vehicle at the track point monitored in real time, and use the calculated value as the predicted value of the curvature of the track point.
For example, assuming that the monitoring device 303 knows that a certain track point of the driving track of the vehicle is reached through real-time interaction with a sensor pre-installed on the steering wheel of the vehicle, the steering wheel angle corresponding to the steering wheel is 30 degrees, so that the monitoring device 303 obtains the curvature of the track point as 0.12 through calculation according to the ackermann steering principle by using the previously constructed equation between the curvature and the steering wheel angle, and uses the value as the predicted value of the curvature of the track point.
It will be understood by those skilled in the art that the above-described manner of calculating the predicted value of curvature of the trace point is merely exemplary, and other manners of calculating the predicted value of curvature of the trace point, which may be present or may occur in the future, such as may be applicable to the present invention, are also included within the scope of the present invention and are hereby incorporated by reference.
And the optimization device 304 obtains the optimal value of the curvature of the track point by adopting Kalman filtering according to the observed value and the predicted value of the curvature of the track point.
Specifically, the optimization device 304 calculates and obtains the optimal value of the curvature of the track point by using kalman filtering according to the observed value of the curvature of the track point obtained by the calculation device 302 and according to the predicted value of the curvature of the track point obtained by the monitoring device 303.
Here, Kalman filtering (Kalman filtering) is an algorithm that performs optimal estimation of a system state by outputting observation data through a system input using a linear system state equation. The optimal estimation can also be seen as a filtering process, since the observed data includes the effects of noise and interference in the system. The data filtering is a data processing technology for removing noise and restoring real data, and the Kalman filtering can estimate the state of a dynamic system from a series of data with measurement noise under the condition that the measurement variance is known.
Kalman filtering is briefly introduced below:
a system for introducing a discrete control process, which can be described by a linear random differential equation:
X(k)＝AX(k-1)+B U(k)+W(k)
plus the system measurements:
Z(k)＝H X(k)+V(k)
in the above two equations, x (k) is the system state at time k, and u (k) is the control amount for the system at time k, which may be 0 if there is no control amount. A and B are system parameters, which for a multi-model system are matrices. Z (k) is the measured value at time k, H is a parameter of the measurement system, and H is a matrix for a multi-measurement system. W (k) and v (k) represent process and measured noise, respectively, which is assumed to be white gaussian noise with covariance Q, R, respectively, assuming that the white gaussian noise does not change with system state changes.
Assuming that the present system state is k, according to the model of the system, the present state can be predicted based on the last state of the system:
X(k|k-1)＝A X(k-1|k-1)+B U(k)………(1)
in the formula (1), X (k | k-1) is the result predicted by the previous state, X (k-1| k-1) is the optimum result of the previous state, and U (k) is the control amount of the current state, which may be 0 if there is no control amount.
The system results have been updated so far, but the covariance corresponding to X (k | k-1) has not been updated. Here, covariance is denoted by P:
P(k|k-1)＝A P(k-1|k-1)A’+Q………(2)
in the formula (2), P (k | k-1) is a covariance corresponding to X (k | k-1), P (k-1| k-1) is a covariance corresponding to X (k-1| k-1), A' represents a transposed matrix of A, and Q is a covariance of the system process.
The above equations (1) and (2) are predictions of the system, and after obtaining the prediction result of the present state, the measured values of the present state are collected. Combining the predicted values and the measured values, an optimized estimated value 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. However, in order to make the kalman filter continuously run until the system process is finished, the covariance of X (k | k) in the k state is updated:
P(k|k)＝(I-Kg(k)H)P(k|k-1)………(5)
where I is a matrix of 1, I ═ 1 for single model single measurements. When the system enters the k +1 state, P (k | k) is P (k-1| k-1) of equation (2). Thus, the algorithm can proceed with autoregressive operation.
Here, since the calculation device 302 has already obtained the observed value of the curvature of the track point, and the monitoring device 303 has already obtained the predicted value of the curvature of the track point, the optimization device 304 may further obtain the optimal value of the curvature of the track point by using kalman filtering in combination with the parameter of the kalman filtering.
Preferably, the device 1 further comprises learning means (not shown). And the learning device obtains the parameter value of the Kalman filtering through deep neural network learning according to the actual value of the curvature of the track point.
Specifically, the curvature of each trace point of the travel trace of the vehicle may have a real value provided by, for example, a vehicle manufacturer, and the learning device obtains the parameter value of the kalman filter through deep neural network learning based on the real value of the curvature of the trace point. For example, the parameter value of the kalman filter is the above covariance, such as a covariance matrix, an initial value may be set for the covariance matrix, the initial value may be set empirically or even randomly, and convergence may be performed through the deep neural network learning, for example, after the initial value of the covariance matrix is set, an error may be obtained according to the calculation of the above formula, and the error may be fed back, so as to modify the covariance matrix, and continuously fed back, so as to perform a parameter adjustment, so that the covariance matrix is converged, and optimize the kalman filter algorithm, so that the learning apparatus obtains the parameter value of the kalman filter through the deep neural network learning. Then, the optimization device 304 calculates and 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 obtained by the calculation.
The device 1 obtains a running track of the vehicle through clothoid curve fitting according to GPS information of the vehicle collected in real time, calculates an observed value of the curvature of a certain track point in the running track, obtains a predicted value of the curvature of the track point according to a steering wheel corner corresponding to the track point of the vehicle monitored in real time, and obtains an optimal value of the curvature of the track point by adopting Kalman filtering according to the observed value and the predicted value of the curvature of the track point; the accuracy of calculating the curvature of the vehicle running track is greatly improved, and when the method is applied to an automatic driving vehicle, the feasibility of automatic driving can be improved.
Further, key parameters of Kalman filtering are obtained by using deep neural network learning, and the device 1 continuously adjusts parameters through the deep neural network learning, so that the Kalman filtering algorithm is optimized to obtain accurate curvature of a track point closer to real data.
The invention also provides a computer readable storage medium having stored thereon computer code which, when executed, performs a method as in any one of the preceding claims.
The invention also provides a computer program product, which when executed by a computer device, performs the method of any of the preceding claims.
The present invention also provides a computer device, comprising:
one or more processors;
a memory for storing one or more computer programs;
the one or more computer programs, when executed by the one or more processors, cause the one or more processors to implement the method of any preceding claim.
It is noted that the present invention may be implemented in software and/or in a combination of software and hardware, for example, the various means of the invention may be implemented using Application Specific Integrated Circuits (ASICs) or any other similar hardware devices. In one embodiment, the software program of the present invention may be executed by a processor to implement the steps or functions described above. Also, the software programs (including associated data structures) of the present invention can be stored in a computer readable recording medium, such as RAM memory, magnetic or optical drive or diskette and the like. Further, some of the steps or functions of the present invention may be implemented in hardware, for example, as circuitry that cooperates with the processor to perform various steps or functions.
It will be evident to those skilled in the art that the invention is not limited to the details of the foregoing illustrative embodiments, and that the present invention may be embodied in other specific forms without departing from the spirit or essential attributes thereof. The present embodiments are therefore to be considered in all respects as illustrative and not restrictive, the scope of the invention being indicated by the appended claims rather than by the foregoing description, and all changes which come within the meaning and range of equivalency of the claims are therefore intended to be embraced therein. Any reference sign in a claim should not be construed as limiting the claim concerned. Furthermore, it is obvious that the word "comprising" does not exclude other elements or steps, and the singular does not exclude the plural. A plurality of units or means recited in the system claims may also be implemented by one unit or means in software or hardware. The terms first, second, etc. are used to denote names, but not any particular order.

Claims (13)

1. A method of calculating curvature of a vehicle trajectory, wherein the method comprises:
a, according to GPS information of a vehicle collected in real time, obtaining a running track of the vehicle through clothoid fitting;
b, calculating an observed value of the curvature of a certain track point in the driving track;
c, obtaining a predicted value of the curvature of the track point according to a steering wheel corner corresponding to the track point of the vehicle monitored in real time;
and d, obtaining the optimal value of the curvature of the track point by adopting Kalman filtering according to the observed value and the predicted value of the curvature of the track point.
2. The method of claim 1, wherein the method further comprises:
and obtaining the parameter value of Kalman filtering through deep neural network learning according to the actual value of the curvature of the track point.
3. The method of claim 1, wherein the step b comprises:
and calculating the observed value of the curvature of the track point in the driving track by adopting a Gaussian-Newton method.
4. The method of claim 2, wherein the step b comprises:
and calculating the observed value of the curvature of the track point in the driving track by adopting a Gaussian-Newton method.
5. The method according to any one of claims 1 to 4, wherein said step c comprises:
and obtaining a predicted value of the curvature of the track point by adopting an Ackerman steering principle according to the steering wheel corner corresponding to the track point of the vehicle monitored in real time.
6. An apparatus for calculating a curvature of a trajectory of a vehicle, wherein the apparatus comprises:
the fitting device is used for obtaining the running track of the vehicle through clothoid fitting according to the GPS information of the vehicle collected in real time;
the calculating device is used for calculating an observed value of the curvature of a certain track point in the driving track;
the monitoring device is used for acquiring a predicted value of the curvature of the track point according to the steering wheel corner corresponding to the track point of the vehicle monitored in real time;
and the optimization device is used for obtaining the optimal value of the curvature of the track point by adopting Kalman filtering according to the observed value and the predicted value of the curvature of the track point.
7. The apparatus of claim 6, wherein the apparatus further comprises:
and the learning device is used for obtaining the parameter value of the Kalman filtering through deep neural network learning according to the actual value of the curvature of the track point.
8. The apparatus of claim 6, wherein the computing device is to:
and calculating the observed value of the curvature of the track point in the driving track by adopting a Gaussian-Newton method.
9. The apparatus of claim 7, wherein the computing device is to:
and calculating the observed value of the curvature of the track point in the driving track by adopting a Gaussian-Newton method.
10. The apparatus of any one of claims 6 to 9, wherein the monitoring device is to:
and obtaining a predicted value of the curvature of the track point by adopting an Ackerman steering principle according to the steering wheel corner corresponding to the track point of the vehicle monitored in real time.
11. A computer readable storage medium storing computer code which, when executed, performs the method of any of claims 1 to 5.
12. A computer program product, the method of any one of claims 1 to 5 being performed when the computer program product is executed by a computer device.
13. A computer device, the computer device comprising:
one or more processors;
a memory for storing one or more computer programs;
the one or more computer programs, when executed by the one or more processors, cause the one or more processors to implement the method of any of claims 1-5.
CN201710792205.1A 2017-09-05 2017-09-05 Method and device for calculating curvature of vehicle track Active CN107817790B (en)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201710792205.1A CN107817790B (en) 2017-09-05 2017-09-05 Method and device for calculating curvature of vehicle track
PCT/CN2018/098609 WO2019047639A1 (en) 2017-09-05 2018-08-03 Method and device for calculating curvature of vehicle trajectory

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710792205.1A CN107817790B (en) 2017-09-05 2017-09-05 Method and device for calculating curvature of vehicle track

Publications (2)

Publication Number Publication Date
CN107817790A CN107817790A (en) 2018-03-20
CN107817790B true CN107817790B (en) 2020-12-22

Family

ID=61600898

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710792205.1A Active CN107817790B (en) 2017-09-05 2017-09-05 Method and device for calculating curvature of vehicle track

Country Status (2)

Country Link
CN (1) CN107817790B (en)
WO (1) WO2019047639A1 (en)

Families Citing this family (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107817790B (en) * 2017-09-05 2020-12-22 百度在线网络技术（北京）有限公司 Method and device for calculating curvature of vehicle track
CN108445886B (en) * 2018-04-25 2021-09-21 北京联合大学 Automatic driving vehicle lane change planning method and system based on Gaussian equation
CN109795484A (en) * 2018-12-24 2019-05-24 百度在线网络技术（北京）有限公司 Control method for vehicle and equipment
CN109606467B (en) * 2018-12-29 2023-05-09 百度在线网络技术（北京）有限公司 Vehicle steering method and vehicle
CN110286671B (en) * 2019-04-29 2022-03-29 北京工业大学 Automatic driving vehicle path generation method based on clothoid curve
CN110262509B (en) * 2019-07-10 2022-06-28 百度在线网络技术（北京）有限公司 Automatic vehicle driving method and device
CN113742634A (en) * 2020-05-27 2021-12-03 华为技术有限公司 Track point filtering method and device and storage medium
CN111829540B (en) * 2020-06-04 2022-08-02 北京百度网讯科技有限公司 Driving track generation method and device, electronic equipment and storage medium
CN114312770B (en) * 2020-10-09 2023-07-07 宇通客车股份有限公司 Vehicle, vehicle running track prediction method and device
CN112622934B (en) * 2020-12-25 2022-06-24 上海商汤临港智能科技有限公司 Reference track point and reference track generation method, driving method and vehicle
CN113460084A (en) * 2021-06-11 2021-10-01 北京汽车研究总院有限公司 Method and device for determining vehicle driving intention, vehicle and storage medium
CN113650618B (en) * 2021-09-23 2022-09-30 东软睿驰汽车技术（上海）有限公司 Vehicle track determination method and related device
CN114139795B (en) * 2021-11-26 2024-09-13 苏州安智汽车零部件有限公司 Automobile track prediction method based on intelligent algorithm
CN114877904A (en) * 2022-05-07 2022-08-09 广西睛智汽车技术有限公司 Road curvature estimation method, road curvature measurement device and computer equipment
CN115035185B (en) * 2022-05-26 2024-02-27 郑州大学 Method for identifying flat curve by using curvature and curvature change rate
CN117584982B (en) * 2023-12-28 2024-04-23 上海保隆汽车科技股份有限公司 Curve radius estimation method, system, medium, electronic equipment, vehicle machine and vehicle
CN118153299A (en) * 2024-03-07 2024-06-07 北京易航远智科技有限公司 Vehicle position information determining method, electronic device, apparatus and storage medium

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3911492B2 (en) * 2003-06-26 2007-05-09 トヨタ自動車株式会社 Vehicle travel support device
JP4801637B2 (en) * 2007-07-19 2011-10-26 三菱重工業株式会社 Vehicle steering control method and apparatus
JP5429234B2 (en) * 2011-03-23 2014-02-26 トヨタ自動車株式会社 Information processing apparatus for vehicle
US9393996B2 (en) * 2012-01-06 2016-07-19 Honda Motor Co., Ltd. Reverse drive assist for long wheelbase dual axle trailers
CN202987136U (en) * 2012-12-07 2013-06-12 长安大学 Front-road curvature estimation device
WO2014149042A1 (en) * 2013-03-20 2014-09-25 International Truck Intellectual Property Company, Llc Smart cruise control system
US10202144B2 (en) * 2015-12-08 2019-02-12 Ford Global Technologies, Llc Vehicle curvature determination
CN106515722B (en) * 2016-11-08 2018-09-21 西华大学 A kind of method for planning track of vertically parking
CN106681335A (en) * 2017-01-22 2017-05-17 无锡卡尔曼导航技术有限公司 Obstacle-avoiding route planning and control method for unmanned agricultural machine driving
CN107817790B (en) * 2017-09-05 2020-12-22 百度在线网络技术（北京）有限公司 Method and device for calculating curvature of vehicle track

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
A Spatial Approach to Control of Platooning Vehicles Separating Path-Following from Tracking;E.Lefeber 等;《IFAC PapersOnLine》;20170731;全文 *
基于运动微分约束的无人车辆纵横向协同规划算法的研究;姜岩 等;《自动化学报》;20131231;全文 *
车辆主动安全系统关键预测算法研究;詹盛;《中国博士学位论文全文数据库工程科技Ⅱ辑》;20141215;全文 *

Also Published As

Publication number Publication date
CN107817790A (en) 2018-03-20
WO2019047639A1 (en) 2019-03-14

Similar Documents

Publication Publication Date Title
CN107817790B (en) Method and device for calculating curvature of vehicle track
EP3819194B1 (en) Automatic parking method and apparatus, and electronic device
US20240011776A9 (en) Vision-aided inertial navigation
KR102292277B1 (en) LIDAR localization inferring solutions using 3D CNN networks in autonomous vehicles
Tin Leung et al. A review of ground vehicle dynamic state estimations utilising GPS/INS
Amditis et al. A situation-adaptive lane-keeping support system: Overview of the safelane approach
Kim et al. Lane-level localization using an AVM camera for an automated driving vehicle in urban environments
Hasberg et al. Simultaneous localization and mapping for path-constrained motion
CN112639502A (en) Robot pose estimation
Ndjeng et al. Low cost IMU–Odometer–GPS ego localization for unusual maneuvers
CN111208814B (en) Memory-based optimal motion planning for an automatic vehicle using dynamic models
Gómez et al. Optimal motion planning by reinforcement learning in autonomous mobile vehicles
CN112433531A (en) Trajectory tracking method and device for automatic driving vehicle and computer equipment
Nilsson et al. Reliable vehicle pose estimation using vision and a single-track model
CN113933858A (en) Abnormal detection method and device of positioning sensor and terminal equipment
CN112099378B (en) Front vehicle lateral motion state real-time estimation method considering random measurement time lag
CN117222915A (en) System and method for tracking an expanded state of a moving object using a composite measurement model
CN111024124A (en) Multi-sensor information fusion combined navigation fault diagnosis method
CN111596329A (en) Vehicle positioning method, device and equipment and vehicle
Dang et al. Tightly-coupled data fusion of VINS and odometer based on wheel slip estimation
JP2022513511A (en) How to identify the range of integrity
CN116416277A (en) Multi-target tracking method and device based on motion equation track prediction
Hua et al. M2C-GVIO: motion manifold constraint aided GNSS-visual-inertial odometry for ground vehicles
Jiang et al. Modeling of unbounded long-range drift in visual odometry
CN117872416A (en) Positioning calibration method, positioning calibration device, computer equipment and storage medium

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant