CN108829996B

CN108829996B - Method and device for obtaining vehicle positioning information

Info

Publication number: CN108829996B
Application number: CN201810658364.7A
Authority: CN
Inventors: 李超
Original assignee: HoloMatic Technology Beijing Co Ltd
Current assignee: Heduo Technology Guangzhou Co ltd
Priority date: 2018-06-25
Filing date: 2018-06-25
Publication date: 2020-01-31
Anticipated expiration: 2038-06-25
Also published as: CN108829996A

Abstract

The invention relates to the technical field of vehicle positioning, and provides methods and devices for obtaining vehicle positioning information.

Description

Method and device for obtaining vehicle positioning information

Technical Field

The invention relates to the field of vehicle positioning, in particular to methods and devices for obtaining vehicle positioning information.

Background

An automatic driving automobile, also called an unmanned automobile, a computer driving automobile or a wheeled mobile robot, is kinds of intelligent automobiles which realize unmanned driving through a computer system, has already been in the history of decades in the 20 th century, and has a trend of approaching to practicality in the beginning of the 21 st century.

In the automatic driving technology, it is important to obtain the positioning information of the vehicle in real time, however, in the prior art, a method for obtaining accurate positioning information is still lacked, which brings a huge obstacle to the research and development of the automatic driving technology and deduction.

Disclosure of Invention

In view of the above, embodiments of the present invention provide a method and an apparatus for obtaining vehicle positioning information, so as to solve the above technical problems.

In order to achieve the purpose, the invention provides the following technical scheme:

, embodiments of the present invention provide methods of obtaining vehicle location information, comprising:

constructing a factor graph for representing the navigation state of the vehicle at a plurality of moments and the constraint relation between the navigation states at the plurality of moments, wherein the navigation state at each moment comprises the pose, the speed and the zero offset of the IMU of the vehicle at the moment, and the plurality of moments comprise target moments;

determining initial values of navigation states at a plurality of moments in a factor graph;

optimizing and solving the factor graph based on the initial values of the navigation states at a plurality of moments and the constraint relation among the navigation states at a plurality of moments;

and determining the optimized value of the navigation state at the target moment as the positioning information of the vehicle at the target moment.

With reference to the th aspect, in an th possible implementation manner of the th aspect, the factor graph includes:

the system comprises a plurality of node groups and a factor graph, wherein the node groups are used for representing navigation states at a plurality of moments, each node group comprises an X node, a V node and a B node and respectively corresponds to the poses and speeds of the navigation states at moments in the navigation states at the plurality of moments and the zero offset of an IMU (inertial measurement Unit), and the node groups are arranged in the factor graph according to the time sequence of the moments;

and a plurality of constraint factors for representing constraint relationship correspondence between the navigation states at the plurality of time instants, each constraint factor being connected with at least nodes in the factor graph for representing constraint relationships existing between at least nodes.

With reference to the possible implementation manner of the aspect, in a second possible implementation manner of the aspect, the plurality of constraint factors includes at least constraint factors of a priori factor, GPS factor, absolute pose factor, relative pose factor, IMU factor, and IMU offset factor, wherein,

the prior factor can be connected with an X node, a V node or a B node of an th node group in the factor graph, and the prior factor is a constraint equation constructed based on the navigation state or the observed quantity before the earliest moment in a plurality of moments;

the GPS factor can be connected with any X nodes of the factor graph, and is a constraint equation constructed based on the observation quantity of the GPS;

the absolute pose factor can be connected with any X nodes of the factor graph, and is a constraint equation constructed based on the observed quantity of the visual positioning system;

the relative pose factors can be respectively connected with any two different X nodes of the factor graph, and are constraint equations constructed on the basis of observed quantities of the wheel speed odometer;

the IMU factors can be respectively connected with an X node, a V node and a B node of any node groups in the factor graph, and respectively connected with the X node and the V node of the next node groups adjacent to the node groups, and the IMU factors are constraint equations constructed based on the observed quantity of the IMU;

the IMU offset factors can be connected to any two adjacent node bs of the factor graph, respectively, and are constraint equations constructed based on the zero offset of the IMU.

With reference to the second possible implementation manner of the aspect , in a third possible implementation manner of the aspect , constructing a factor graph representing a navigation state of the vehicle at a plurality of time instants and a constraint relationship between the navigation states at the plurality of time instants includes:

judging whether the target time is the latest time in the plurality of times;

if yes, obtaining a partial factor graph comprising other node groups at other moments except the target moment in the multiple moments and other constraint factors connected with nodes in the other node groups;

constructing a target node group at a target time after the last node groups of the partial factor graph;

constructing a target constraint factor connected with nodes in the target node group based on the observed quantity at the target moment;

a factor graph including the partial factor graph, the target node group, and the target constraint factor is obtained.

With reference to the third possible implementation manner of the , in a fourth possible implementation manner of the , the method further includes:

if not, obtaining a partial factor graph of other node groups at other moments except the target moment in the multiple moments;

constructing a target node group of a target moment between two adjacent node groups of the target moment in the partial factor graph;

constructing other constraint factors connected with nodes in other node groups based on the observed quantities at other moments;

a factor graph is obtained that includes the partial factor graph, the other constraint factors, the target node group, and the target constraint factors.

With reference to any possible implementation manners of the through the fourth aspects of the or the , in a fifth possible implementation manner of the , determining initial values of navigation states at multiple time instants in a factor graph includes:

determining the history optimized values of the navigation states at other moments except the target moment in the multiple moments as initial values of the navigation states at other moments;

the initial value of the navigation state at the target time is obtained by integration based on the history optimized value of the navigation state at the front time adjacent to the target time among other times and the observed quantity of the IMU after the front time.

With reference to any possible implementation manners of the second to fourth aspects of the , in a sixth possible implementation manner of the , the multiple times are all times within or after a preset time window, and the preset time window has a preset duration and is capable of sliding over time.

With reference to the sixth possible implementation manner of the aspect, in a seventh possible implementation manner of the aspect, before constructing a factor graph representing a navigation state of the vehicle at a plurality of time instants and a constraint relationship between the navigation states at the plurality of time instants, the method further includes:

determining a prior factor connected to the th node group within the preset time window based on the constrained relationship between the navigational state at the time before the preset time window and the navigational state at the time before the preset time window.

With reference to the , in an eighth possible implementation manner of the , after determining the optimized value of the navigation state at the target time as the positioning information of the vehicle at the target time, the method further includes:

integrating values of navigational states at least intermediate times after the target time and before the next target time based on the optimized value of navigational state at the target time and the observed amount of IMU after the target time;

the values of the navigational state of at least intermediate times are determined as the positioning information of the vehicle at least intermediate times.

In a second aspect, an embodiment of the present invention provides an apparatus for obtaining vehicle location information, including:

the factor graph construction module is used for constructing a factor graph for representing the navigation state of the vehicle at a plurality of moments and the constraint relation between the navigation states at the plurality of moments, wherein the navigation state at each moment comprises the pose and the speed of the vehicle at the moment and the zero offset of the IMU, and the plurality of moments comprise target moments;

the initial value determining module is used for determining the initial values of the navigation states at a plurality of moments in the factor graph;

the factor graph solving module is used for carrying out optimization solving on the factor graph based on the initial values of the navigation states at multiple moments and the constraint relation among the navigation states at multiple moments;

and the positioning information determining module is used for determining the optimized value of the navigation state at the target moment as the positioning information of the vehicle at the target moment.

In a third aspect, an embodiment of the present invention provides computer storage media, where the computer storage media stores computer program instructions, and the computer program instructions, when read and executed by a processor of a computer, perform the method provided by any possible implementation manner of the or aspect.

In a fourth aspect, an embodiment of the present invention provides electronic devices, including a processor and a computer storage medium, where the computer storage medium stores computer program instructions, and when the computer program instructions are read and executed by the processor, the computer storage medium executes the method provided by any possible implementation manners of the aspect or the aspect.

According to the method and the device for obtaining the vehicle positioning information, provided by the embodiment of the invention, a factor graph used for representing the navigation states of the vehicle at multiple moments and the constraint relation among the navigation states at multiple moments is firstly constructed, then the initial values of the navigation states at multiple moments in the factor graph are determined, finally the factor graph is optimized and solved based on the initial values of the navigation states at multiple moments and the constraint relation among the navigation states at multiple moments, and further the optimized value of the navigation state at the target moment in the multiple moments is determined as the positioning information of the vehicle at the target moment. In the scheme, the constraint relation among the navigation states is fully considered, so that the solved optimized value of the navigation state can accurately estimate the real navigation state of the vehicle at the target moment, and the optimized value is used as the positioning information of the vehicle at the target moment, so that the vehicle can be accurately positioned, and the automatic driving effect is improved.

In order to make the above objects, technical solutions and advantages of the present invention more comprehensible, embodiments accompanied with figures are described in detail below.

Drawings

In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings needed to be used in the embodiments will be briefly described below, it should be understood that the following drawings only illustrate some embodiments of the present invention and therefore should not be considered as limiting the scope, and for those skilled in the art, other related drawings can be obtained according to the drawings without inventive efforts.

FIG. 1 shows a block diagram of electronic devices that may be used in embodiments of the invention;

FIG. 2 illustrates a flow chart of a method of obtaining vehicle location information provided by an embodiment of the invention;

FIG. 3 shows a factor graph provided by embodiment of the present invention;

fig. 4 is a functional block diagram showing an apparatus for obtaining vehicle location information according to a second embodiment of the present invention.

Detailed Description

The embodiments of the present invention generally described and illustrated in the figures herein can be arranged and designed in a wide variety of different configurations and, thus, the following detailed description of the embodiments of the present invention provided in the figures is not intended to limit the scope of the invention as claimed, but is merely representative of selected embodiments of the invention.

It should be noted that like reference numerals and letters refer to like items in the following figures, and thus once a item is defined in figures, it need not be further defined and explained by in subsequent figures.

Referring to fig. 1, an block diagram of an electronic device 100 that may be used in embodiments of the invention is shown, with reference to fig. 1, the electronic device 100 includes a memory 102, a memory controller 104, or more (only shown) processors 106, a peripheral interface 108, an input device 110, an output device 112, etc. these components communicate with each other via or more communication buses/signal lines 114.

The memory 102 may be used to store program instructions and/or modules, such as program instructions and/or modules corresponding to the method and apparatus for obtaining vehicle positioning information according to the embodiments of the present invention, and the processor 106 executes various functional applications and data processing, such as the method and apparatus for obtaining vehicle positioning information according to the embodiments of the present invention, by executing the program instructions and/or modules stored in the memory 102.

The Memory 102 may be, but is not limited to, a Random Access Memory (RAM), a Read Only Memory (ROM), a Programmable Read-Only Memory (PROM), an Erasable Read-Only Memory (EPROM), an electrically Erasable Read-Only Memory (EEPROM), and the like. Access to the memory 102 by the processor 106, and possibly other components, may be under the control of the memory controller 104.

The Processor 106 may be a type Integrated circuit chip having Signal Processing capability, and may specifically be a general-purpose Processor including a Central Processing Unit (CPU), a Micro Control Unit (MCU), a Network Processor (NP), or other conventional processors, or a special-purpose Processor including a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field Programmable array (FPGA), or other Programmable logic device, discrete , or a transistor logic device, or discrete hardware component.

Peripheral interface 108 couples various input/output devices to processor 106 and memory 102 in embodiments, peripheral interface 108, processor 106 and memory controller 104 may be implemented in a single chip in other examples, each of which may be implemented by a separate chip.

The input device 110 is used for inputting data and information to the terminal, and is which is a main device for exchanging information between the user and the terminal, and the input device 110 may be a keyboard, a mouse, a touch panel, a voice input device, and the like.

The output device 112 is used to represent various data, information or calculation results in the form of numbers, characters, images, sounds, etc., and the output device 112 may be a display panel, a video output system, a voice output system, etc.

It will be appreciated that the configuration shown in FIG. 1 is merely illustrative and that electronic device 100 may include more or fewer components than shown in FIG. 1 or have a different configuration than shown in FIG. 1. The components shown in fig. 1 may be implemented in hardware, software, or a combination thereof. In the embodiment of the present invention, the electronic device 100 may be a device with computing capability, such as a server, a personal computer, a mobile device, a wearable device, and a vehicle-mounted device.

th embodiment

Referring to fig. 2, a flow chart of a method of obtaining vehicle location information according to an embodiment of the present invention includes:

step S10: the processor 106 of the electronic device 100 constructs a factor graph representing a navigational state of the vehicle at a plurality of time instants and a constraint relationship between the navigational states at the plurality of time instants.

The values of the three parameters can be obtained by a device or system installed on the vehicle and then obtained by optimizing the solution in combination with the method provided in embodiment .

The observation quantity obtained by a Global Positioning System (GPS) is mainly the absolute pose of a vehicle.

The observation quantity obtained by the vision positioning system is mainly the absolute pose of the vehicle. The visual positioning system generally refers to hardware and software devices or systems for obtaining the vehicle pose by a visual positioning method, and at present, many existing algorithms are available for visual positioning, for example, including but not limited to ORB-SLAM 2 algorithm, the algorithm published by Torsten Sattler et al in the ICCV '11 conference under the name of Fast Image-Based Localization using Direct 2D-to-3D Matching, the algorithm published by Torsten Sattler et al in the ECCV' 12 conference under the name of Improving Image-Based Localization by Active Corresponsence Search, and the like, which are all available in the visual positioning system.

The wheel speed odometer obtains observation quantities mainly including the absolute speed of the vehicle and the relative pose of the vehicle. The wheel speed odometer includes a wheel speed meter of the vehicle and a steering wheel (steering wheel angle for obtaining a determination posture).

The IMU obtains the observed quantity which is mainly the relative speed and the relative pose of the vehicle, and in addition, the IMU has a system error in the measurement process, and the system error is generally called as a zero offset quantity of the IMU.

The observation quantity obtained by each device or system only represents the observation result of the device or system, and the finally obtained navigation state can be combined with the observation results of various devices or systems, so that the obtained navigation state is more valuable.

As optional embodiments, in the process of obtaining the observed quantity, the newly obtained observed quantity may be subjected to outlier filtering based on the variance of the observed quantity obtained before, so that abnormal observed quantities with large deviation are excluded, and the abnormal observed quantities are prevented from affecting the construction of the subsequent factor graph.

FIG. 3 illustrates a factor graph provided by an th embodiment of the present invention, it is to be understood that FIG. 3 is only implementations of the factor graph and not only, referring to FIG. 3, the factor graph includes a plurality of node groups, wherein each node group corresponds to a navigation state at time instants, specifically, each node group includes an X node, a V node, and a B node, which correspond to three parameters of pose, velocity, and zero offset of the IMU, respectively, and in subsequent steps, the nodes may be populated with values.

For example, in fig. 3, three circles above navigation state 0 (representing the navigation state at time 0) form a corresponding node group, and there are three nodes X0, V0 and B0, which respectively represent the pose, speed and zero offset of the IMU at time 0. other navigation states are similar to the corresponding relationship of the node group and navigation state 0, and are not described in , 6 times corresponding to navigation states 0 to 5 are sequentially occurring times, where time 0 is the earliest time and time 5 is the latest time, in fig. 3, the node groups corresponding to the navigation states are sequentially arranged from left to right according to the sequence of the times corresponding to the navigation states, so that the time sequence relationship between the navigation states can be embodied.

In FIG. 3, the constraint factors are shown in black dots, each constraint factor is connected with at least nodes in the factor graph and is used for representing the constraint relation existing between at least nodes, and the constraint relation is embodied by constraint equations constructed based on observed quantities (or other input quantities) and is used for constraining the values of the nodes connected with the constraint equations.

In fig. 3, a total of 6 constraint factors including an a priori factor, a GPS factor, an absolute pose factor, a relative pose factor, an IMU factor, and an IMU offset factor are shown.

For example, in fig. 3, a priori factor 1 may be connected to the X0 node, a priori factor 2 may be connected to the V0 node, and a priori factor 3 may be connected to the node B0 node, where the a priori factor 1, the a priori factor 2, and the a priori factor 3 represent different a priori factors for respectively constraining values of the X0 node, the V0 node, and the B0 node

The prior factor is a constraint equation constructed based on the navigation state or observation before the navigation state at the earliest moment in the factor graph. Specifically, if the navigation state at any historical time (time before the earliest time in the factor graph) is not cached before the factor graph is constructed, the obtained observation can be used as an input quantity for constructing the prior factor. If the navigation state at the historical moment is cached before the factor graph is constructed, a marginalization method can be adopted to calculate the prior factor based on the cached navigation state value.

For example, in fig. 3, the GPS factor may be connected to the X0 node, and constructed based on the GPS observation obtained at time 0, so as to constrain the value of the X0 node.

For example, in fig. 3, the absolute pose factor can be connected to the X1 node, and is constructed based on the observed quantity of the visual positioning system obtained at time 1, so as to constrain the value of the X1 node.

The relative pose factors can be respectively connected with the X nodes of any two different node groups of the factor graph, and the relative pose factors are constraint equations constructed on the basis of observed quantities of the wheel speed odometer. For example, in fig. 3, the relative pose factors are respectively connected to the X0 node and the X2 node, and are constructed based on the observed quantities of the wheel speed odometer obtained at 0 time and 2 time, so as to constrain the values of the X0 node and the X2 node.

For example, in FIG. 3, IMU factors are respectively connected with an X0 node, a V0 node and a B0 node of a navigation state 0, and simultaneously connected with an X1 node and a V1 node of a navigation state 1, and simultaneously constructed based on the observation quantities of the IMU obtained at the time instants of 0 and 1, and are used for restricting the values of an X0 node, a V0 node, a B0 node, an X1 node and a V1 node.

For example, in FIG. 3, the IMU offset factors are respectively connected with the B0 node in navigation state 0 and the B1 node in navigation state 1, and are used for constraining the values of the B0 node and the B1 node.

It is understood that the factor graph may further include more or less constraint factors than those in fig. 3, and the connection manner of each factor and the node may be different from that shown in fig. 3, so that the configuration is very flexible and can be configured according to requirements. For example, the IMU offset factor of FIG. 3 may be removed when the IMU is determined to be in a stable operating state (IMU zero offset remains substantially unchanged).

When the factor graph is constructed, firstly, the nodes of the factor graph are determined, then, the constraint factors are constructed based on the observed quantity and connected to the corresponding nodes, and the construction of the factor graph is completed.

, when constructing the factor graph, it can also be detected whether there is an excessive loss of the observed quantity, and when the observed quantity is seriously lost, the construction can be quitted.

And , after the constraint factor is constructed, the constraint quality can be detected, if the constraint quality is qualified, the subsequent steps are executed, and if the constraint quality is not qualified, the construction can be quitted.

And step , after the factor graph is constructed, the connectivity of the factor graph can be detected, if the factor graph is a connected graph, namely each node of the factor graph is connected with at least another nodes, the subsequent steps are executed, otherwise, the construction can be quitted, so that the situation that the unconstrained nodes exist in the factor graph and logic errors are caused is avoided.

The navigation state of a plurality of moments is included in the factor graph, moments are target moments, and moments except the target moments are other moments.

This is often the case if the newly received observations are collected after all other times, i.e. the target time is the latest of the times in the factor graph. The factor graph can be constructed in the following way, which is called as an increment construction way:

the target constraint factors are then constructed based on the observed quantities at the target time.

The above process is explained by taking fig. 3 as an example, and assuming that the observed quantity acquired at 6 times after 5 times is received at present, the target time is 6 times after 5 times, and other times are 0 to 5 times, and the corresponding navigation state is navigation state 6. when constructing, a partial factor graph, namely, current fig. 3 is obtained first, the obtaining referred to herein may be reconstruction, or may be direct reading, for example, when solving the navigation state corresponding to a certain time among other times, fig. 3 has already been constructed, after completing the solving, fig. 3 may be cached as a partial factor graph when constructing the factor graph for the next times.

This is often the case if the acquisition time of the newly received observations is before the latest of all other times, i.e. the target time is not the latest of the times in the factor graph. The factor graph can be constructed in the following way, which is called as a batch construction way:

a partial factor graph is first obtained that includes other node groups at other times. And then constructing a target node group of the target time between the node groups of two time moments adjacent to the target time in the partial factor graph. And then constructing target constraint factors connected with the nodes in the target node group based on the observed quantities at the target moment. Other constraint factors are then constructed that connect with nodes in other node groups based on the observations at other times. Finally, a factor graph including the partial factor graph, the other constraint factors, the target node group and the target constraint factors is obtained.

Taking fig. 3 as an example to explain the above process, assuming that the observed quantity acquired at the time 3.5 before the time 5 is currently received, the target time is the time 3.5 before the time 5, and the other times are 0 to 5, and the corresponding navigation state is the navigation state 3.5.

When building, a partial factor graph is obtained first, but it should be noted that, unlike the incremental building method, only nodes are included in the partial factor graph, and there is no constraint factor, that is, only a part of all nodes is included in fig. 3. The obtaining referred to herein may be reconstructing or directly reading, but after reading the previously cached fig. 3, the constraint factors (including the connection relationship thereof) therein need to be deleted, or at least a part that may change after inserting the navigation state 3.5 needs to be deleted, for example, the constraint factors between the navigation state 3 and the navigation state 4 need to be deleted, and the constraint factors between the navigation state 1 and the navigation state 2 may be retained, so as to improve the efficiency of constructing the factor graph.

And then adding a target node group corresponding to the navigation state 3.5 between the time 3 and the time 4 adjacent to the time 3.5, wherein the three nodes are X3.5, V3.5 and B3.5 nodes. And then constructing a target constraint factor based on the observed quantity acquired at the moment of 3.5, wherein the target constraint factor connects the nodes X3.5, V3.5 and B3.5 with other nodes in the graph 3 to form a target constraint relation. Other constraint factors connected to nodes of other navigational states are then constructed based on previously collected observations, for example in fig. 3, the other constraint factors refer to constraint factors between navigational state 0 to navigational state 3, and constraint factors between navigational state 4 to navigational state 5, although these constraint factors do not need to be reconstructed if they have not been previously deleted. Thus, the factor graph is constructed.

The above-mentioned factor graph is constructed only by target time points, however, in practice, the target time points may be a plurality of time points, and for each of the time points, the factor graph can be constructed by using which is one of the above-mentioned two construction methods.

In alternative embodiments, after receiving the observation from each observation collecting device or system, it can be packaged as observation message and inserted into the message queue, and then the observation message in the message queue is read to construct the factor graph according to of the above two construction methods.

Step S11: the processor 106 of the electronic device 100 determines initial values of the navigational state for a plurality of time instants in the factor graph.

The initialization value for determining the navigation state is a filling value in a node of the factor graph. The determination method of the initial value is different for the navigation state at the target time and the navigation state at the other times.

And for the navigation states at other moments, determining the historical optimization values obtained in the optimization solving process as initial values of the navigation states at other moments.

For the navigation state at the target time, firstly, the historical optimized value of the navigation state at the previous time adjacent to the target time in other times and the observed quantity of the IMU continuously obtained from the previous time are determined, and the initial value of the navigation state at the target time can be obtained by integration.

As special cases, if the factor graph is constructed for the first time, only navigation states are available, such as navigation state 0 in FIG. 3, at this time, if there is a navigation state in the factor graph constructed before caching, the initial value of navigation state can be extracted from the factor graph, if the navigation state in the factor graph constructed before caching or the factor graph not constructed before, in processing mode, the initial values of navigation states are calculated after the observation quantities of GPS and IMU are processed by a filtering method, and in the second processing mode, the pose of the navigation state is determined by a visual positioning system, and the speed of the navigation state and the zero offset of the IMU are obtained by other observation quantity acquisition devices or systems.

Step S12: the processor 106 of the electronic device 100 optimally solves the factor graph based on the initial values of the navigational states at the plurality of time instants and the constraint relationship between the navigational states at the plurality of time instants.

The optimized value referred to herein can refer to an optimal value under a certain convergence condition, but is not limited to an optimal value, such as a suboptimal value, etc. note that although the optimized value of the navigation state at the target time is generally only required to be output, the value of each navigation state in the factor graph is optimized in step during the solving process.

In embodiments of the , in order to speed up the construction and solution of the factor graph, a preset time window that has a preset time duration and can slide along with the lapse of time may be set on the factor graph, where the preset time duration may be a fixed time duration or a variable time duration.

The significance of the preset time window is explained below in connection with fig. 3. In fig. 3, the predetermined time window covers a time period between 2 and 5. Firstly, when the factor graph is constructed, only a part in a preset time window or a part behind the preset time window needs to be constructed, and the part outside the window is considered to be expired and does not need to be constructed.

For example, for the incremental construction mode, assuming that the target time is 6 times after 5 times, only a factor graph formed by navigation states 2 to 6 and the constraint factors thereof needs to be constructed; for the batch construction mode, assuming that the target time is 3.5 times between 3 times and 4 times, only a factor graph formed by navigation states 2 to 5 (including navigation state 3.5) and the constraint factors thereof needs to be constructed.

However, the navigation state 0, the navigation state 1 and the constraint factors thereof can still be converted into the prior factors of the navigation state 2 by a marginalization method, that is, the prior factors of the th navigation state in the preset time window can be determined according to the constraint relationship between the navigation state at the time before the preset time window and the navigation state at the time before the preset time window.

When the preset time window is adopted, the factor graph is naturally solved only by solving the factor graph constructed based on the preset time window, because the preset time window is sections of limited duration, the number of navigation states in the preset time window is not too large, the solving speed can be greatly accelerated, old navigation states are abandoned, but the states are generally far away from the target moment and have small influence on the optimization solving of the target moment, and in addition, the abandoned navigation states and constraint factors thereof are actually used for generating the prior factors of the factor graph, so that the information of the factor graph is retained to a certain extent, the optimization solving of the factor graph still can ensure higher quality, and the obtained optimized value of the navigation state still has higher reference value.

Step S13: the processor 106 of the electronic device 100 determines the optimized value of the navigation state at the target time as the positioning information of the vehicle at the target time.

The optimized value of the navigation state is determined as the positioning information of the target moment and is output to a corresponding software and hardware module, equipment or system, such as an automatic driving module and the like for further processing .

In practice, the optimal solution of the factor graph consumes a large amount of computing resources, which may result in a low frequency of the output positioning information, for example, within 10Hz, and such output frequency cannot meet the external requirement of the positioning information. Therefore, it is necessary to try to increase the output frequency, and the following measures can be taken:

after a certain optimization solution, based on the optimized value of the navigation state at the target time and the observed quantity of the IMU after the target time, integrating to obtain values of the navigation state at least intermediate times after the target time and before the lower target time, and then determining the values of the navigation state at least intermediate times as the positioning information of the vehicle at least intermediate times.

In summary, according to the method for obtaining vehicle positioning information provided by the embodiment of the present invention, because the constraint relationship between the navigation states is fully considered, the solved optimized value of the navigation state can accurately estimate the real navigation state of the vehicle at the target time, and further, the optimized value is used as the positioning information of the vehicle at the target time, so that the vehicle can be accurately positioned, and the effect of automatic driving is improved.

Second embodiment

Fig. 4 is a functional block diagram showing an apparatus 200 for obtaining vehicle location information according to a second embodiment of the present invention. Referring to fig. 4, the apparatus includes a factor graph construction module 210, an initial value determination module 220, a factor graph solving module 230, and a location information determination module 240.

The factor graph constructing module 210 is configured to construct a factor graph representing a navigation state of the vehicle at multiple times and a constraint relationship between the navigation states at the multiple times, where the navigation state at each time includes a pose and a speed of the vehicle at the time and a zero offset of the IMU, and the multiple times include a target time;

the initial value determining module 220 is configured to determine initial values of navigation states at multiple times in the factor graph;

the factor graph solving module 230 is configured to perform optimization solving on the factor graph based on initial values of the navigation states at multiple times and constraint relationships among the navigation states at multiple times;

the positioning information determining module 240 is configured to determine the optimized value of the navigation state at the target time as the positioning information of the vehicle at the target time.

The second embodiment of the present invention provides an apparatus 200 for obtaining vehicle positioning information, which implements the same principle and produces the same technical effects as the foregoing method embodiments, and for the sake of brief description, reference may be made to the corresponding contents in the foregoing method embodiments for the parts where the apparatus embodiments are not mentioned.

Third embodiment

The third embodiment of the present invention provides computer storage media, which stores computer program instructions, and when the computer program instructions are read and executed by a processor of a computer, the computer program instructions execute the method for obtaining vehicle positioning information provided by the embodiment of the present invention.

Fourth embodiment

The fourth embodiment of the present invention provides electronic devices, which include a processor and a computer storage medium, wherein the computer storage medium stores computer program instructions, and the computer program instructions are read by the processor and executed to execute the method for obtaining vehicle positioning information according to the present invention.

It should be noted that, in the present specification, the embodiments are all described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments may be referred to each other. For the device-like embodiment, since it is basically similar to the method embodiment, the description is simple, and for the relevant points, reference may be made to the partial description of the method embodiment.

The above-described apparatus embodiments are merely illustrative, and for example, the flowcharts and block diagrams in the figures may illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods and computer program products according to various embodiments of the present invention.

In addition, each functional module in each embodiment of the present invention may be integrated in to form independent parts, or each module may exist separately, or two or more modules may be integrated to form independent parts.

The functions may be stored in a computer-readable storage medium if they are implemented in the form of software functional modules and sold or used as separate products. Based on such understanding, the technical solution of the present invention may be embodied in the form of a software product, which is stored in a storage medium and includes several instructions for causing a computer device to execute all or part of the steps of the method according to the embodiments of the present invention. The aforementioned computer device includes: various devices having the capability of executing program codes, such as a personal computer, a server, a mobile device, an intelligent wearable device, a network device, and a virtual device, the storage medium includes: u disk, removable hard disk, read only memory, random access memory, magnetic disk, magnetic tape, or optical disk.

It should be noted that like reference numerals and letters refer to like elements in the following figures, and thus , once is defined in figures, it is not necessary to further define or interpret in the following figures.

The above description is only for the specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present invention, and all the changes or substitutions should be covered within the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

It should be noted that, in this document, relational terms such as , second and the like are only used to distinguish entities or operations from another entities or operations, and no necessarily requires or implies that any such actual relationship or order exists between the entities or operations.

Claims

A method of obtaining vehicle location information of species, comprising:

constructing a factor graph representing the navigation states of the vehicle at a plurality of moments and constraint relations among the navigation states at the plurality of moments, wherein the navigation state at each moment comprises the pose and the speed of the vehicle at the moment and the zero offset of an inertial measurement unit IMU, and the plurality of moments comprise target moments;

determining initial values of navigational states at the plurality of time instants in the factor graph;

performing optimization solution on the factor graph based on the initial values of the navigation states at the multiple moments and the constraint relation among the navigation states at the multiple moments;

determining the optimized value of the navigation state at the target moment as the positioning information of the vehicle at the target moment;

wherein the factor graph comprises:

a plurality of node groups, configured to represent the navigation states at the multiple times, where each node group includes an X node, a V node, and a B node, and corresponds to a pose, a speed, and a zero offset of the IMU of navigation states in the navigation states at the multiple times, respectively, and the node groups are arranged in the factor graph according to a chronological order of the multiple times;

a plurality of constraint factors representing a constraint relationship correspondence between navigational states at the plurality of time instants, each constraint factor being connected to at least nodes in the factor graph for representing a constraint relationship existing between the at least nodes, the plurality of constraint factors including at least constraint factors among a priori factor, a Global Positioning System (GPS) factor, an absolute pose factor, a relative pose factor, an IMU factor, and an IMU offset factor, wherein,

the prior factor can be connected with an X node, a V node or a B node of an th node group in the factor graph, and the prior factor is a constraint equation constructed based on a navigation state or an observed quantity before the earliest moment in the plurality of moments;

the GPS factor can be connected with any X nodes of the factor graph, and the GPS factor is a constraint equation constructed based on GPS observation;

the absolute pose factor can be connected with any X nodes of the factor graph, and the absolute pose factor is a constraint equation constructed based on the observation quantity of a visual positioning system;

the relative pose factors can be respectively connected with any two different X nodes of the factor graph, and the relative pose factors are constraint equations constructed on the basis of observed quantities of the wheel speed odometer;

the IMU factors can be respectively connected with an X node, a V node and a B node of any node groups of the factor graph, and respectively connected with an X node and a V node of a next node groups adjacent to the node groups, and are constraint equations constructed based on observed quantities of IMUs;

the IMU offset factors can be respectively connected with any two adjacent node Bs of the factor graph, and the IMU offset factors are constraint equations constructed based on zero offset of IMUs.
2. The method of obtaining vehicle positioning information according to claim 1, wherein said constructing a factor graph representing a navigation state of the vehicle at a plurality of time instants and a constraint relationship between the navigation states at the plurality of time instants comprises:

judging whether the target time is the latest time in the plurality of times;

if yes, obtaining a partial factor graph comprising other node groups at other moments except the target moment in the multiple moments and other constraint factors connected with nodes in the other node groups;

constructing a target node group for the target time after the last node groups of the partial factor graph;

constructing a target constraint factor connected with the nodes in the target node group based on the observed quantity of the target moment;

obtaining the factor graph including the partial factor graph, the target node group, and the target constraint factor.
3. The method of obtaining vehicle positioning information according to claim 2, wherein the determining whether the target time is after a latest time of the plurality of times further comprises:

if not, obtaining a partial factor graph of other node groups at other moments except the target moment in the multiple moments;

constructing a target node group of the target time between node groups of two times adjacent to the target time in the partial factor graph;

constructing a target constraint factor connected with the nodes in the target node group based on the observed quantity of the target moment;

constructing other constraint factors connected with nodes in the other node groups based on the observed quantities at other moments;

obtaining the factor graph including the partial factor graph, the other constraint factors, the target node group, and the target constraint factor.
4. The method of obtaining vehicle positioning information of any of claims 1-3, wherein the determining initial values for navigation states at the plurality of times in the factor graph includes:

determining the historical optimized values of the navigation states at other moments except the target moment in the multiple moments as initial values of the navigation states at the other moments;

and integrating to obtain an initial value of the navigation state of the target time based on the historical optimized value of the navigation state of the former time adjacent to the target time in the other time and the observed quantity of the IMU after the former time.
5. The method of obtaining vehicle positioning information of any of claims 1-3, wherein the plurality of times are each times within or after a preset time window, the preset time window having a preset duration and being slidable over time.
6. The method of obtaining vehicle positioning information of claim 5, wherein before constructing the factor graph representing the navigational state of the vehicle at the plurality of time instants and the constraint relationship between the navigational states at the plurality of time instants, the method further comprises:

determining a prior factor connected to the th node group within the preset time window based on a constraint relationship between the navigational state at the time before the preset time window and the navigational state at the time before the preset time window.
7. The method of obtaining vehicle positioning information according to claim 1, wherein the determining the optimized value of the navigation state at the target time as being subsequent to the positioning information of the vehicle at the target time, the method further comprises:

integrating values of navigational states at least intermediate times after the target time and before a lower target time based on the optimized value of navigational state at the target time and the observed amount of IMU after the target time;

determining the value of the navigational state of the at least intermediate time instants as the positioning information of the vehicle at the at least intermediate time instants.
An apparatus for obtaining vehicle location information of , comprising:

the factor graph construction module is used for constructing a factor graph for representing the navigation states of the vehicle at a plurality of moments and the constraint relation among the navigation states at the plurality of moments, wherein the navigation state at each moment comprises the pose, the speed and the zero offset of the IMU of the vehicle at the moment, and the plurality of moments comprise target moments;

an initial value determining module, configured to determine initial values of navigation states at the multiple times in the factor graph;

the factor graph solving module is used for carrying out optimization solving on the factor graph based on the initial values of the navigation states at the multiple moments and the constraint relation among the navigation states at the multiple moments;

the positioning information determining module is used for determining the optimized value of the navigation state at the target moment as the positioning information of the vehicle at the target moment;

wherein the factor graph comprises:

a plurality of node groups, configured to represent the navigation states at the multiple times, where each node group includes an X node, a V node, and a B node, and corresponds to a pose, a speed, and a zero offset of the IMU of navigation states in the navigation states at the multiple times, respectively, and the node groups are arranged in the factor graph according to a chronological order of the multiple times;

a plurality of constraint factors representing a constraint relationship correspondence between navigational states at the plurality of time instants, each constraint factor being connected to at least nodes in the factor graph for representing a constraint relationship existing between the at least nodes, the plurality of constraint factors including at least constraint factors among a priori factor, a Global Positioning System (GPS) factor, an absolute pose factor, a relative pose factor, an IMU factor, and an IMU offset factor, wherein,

the prior factor can be connected with an X node, a V node or a B node of an th node group in the factor graph, and the prior factor is a constraint equation constructed based on a navigation state or an observed quantity before the earliest moment in the plurality of moments;

the GPS factor can be connected with any X nodes of the factor graph, and the GPS factor is a constraint equation constructed based on GPS observation;

the absolute pose factor can be connected with any X nodes of the factor graph, and the absolute pose factor is a constraint equation constructed based on the observation quantity of a visual positioning system;

the relative pose factors can be respectively connected with any two different X nodes of the factor graph, and the relative pose factors are constraint equations constructed on the basis of observed quantities of the wheel speed odometer;

the IMU factors can be respectively connected with an X node, a V node and a B node of any node groups of the factor graph, and respectively connected with an X node and a V node of a next node groups adjacent to the node groups, and are constraint equations constructed based on observed quantities of IMUs;

the IMU offset factors can be respectively connected with any two adjacent node Bs of the factor graph, and the IMU offset factors are constraint equations constructed based on zero offset of IMUs.