CN109917440B

CN109917440B - Combined navigation method, system and vehicle

Info

Publication number: CN109917440B
Application number: CN201910281099.XA
Authority: CN
Inventors: 王佩生; 刘凡凡; 田靓; 孙屹; 吴吉; 邓小烽
Original assignee: Guangzhou Xiaopeng Motors Technology Co Ltd
Current assignee: Guangzhou Xiaopeng Motors Technology Co Ltd
Priority date: 2019-04-09
Filing date: 2019-04-09
Publication date: 2021-07-13
Anticipated expiration: 2039-04-09
Also published as: CN109917440A

Abstract

A combined navigation method, a system and a vehicle are provided, the method comprises the following steps: in the running process of the vehicle, acquiring the vertical angular speed, first vehicle speed information and satellite positioning information of the vehicle to perform Kalman filtering fusion, and acquiring first position information and first course angle information of the vehicle; when the vehicle is identified to run to a gateway area of the target overhead according to the first position information and the first course angle information, monitoring the inclination angle information of the vehicle in real time; if the inclination angle information of the vehicle is matched with the inclination angle data of the target overhead, determining the overhead driving direction of the vehicle; and performing map matching on the first position information, the first course angle information and the inclination angle information of the vehicle according to the overhead driving direction of the vehicle so as to mark the position and/or the angle of the vehicle on a first road in the map. By implementing the embodiment of the invention, the problems of large satellite positioning error and inaccurate navigation route when the automobile runs to the elevated road section can be solved, so that the navigation precision of the automobile is improved.

Description

Combined navigation method, system and vehicle

Technical Field

The invention relates to the technical field of map navigation, in particular to a combined navigation method, a combined navigation system and a vehicle.

Background

With the development of navigation technology, most intelligent automobiles are provided with vehicle-mounted navigation. Currently, the mainstream vehicle navigation is mainly based on a Global Positioning System (GPS), and the vehicle position information is calculated by receiving satellite data returned by a GPS satellite, and then the specific position of the vehicle in an electronic map can be determined by matching the vehicle position information with electronic map data, so as to realize real-time Positioning and route navigation of the vehicle.

However, in practice, when the automobile runs to an elevated road section, it is difficult for the vehicle-mounted navigation to effectively identify the actions of the automobile on and off the elevated road section, so that the satellite positioning error is increased, the navigation route is inaccurate, and the navigation precision of the automobile is greatly reduced.

Disclosure of Invention

The embodiment of the invention discloses a combined navigation method, a system and a vehicle, which can solve the problems of large satellite positioning error and inaccurate navigation route when the vehicle runs to an elevated road section, thereby improving the navigation precision of the vehicle.

The first aspect of the embodiment of the invention discloses a combined navigation method, which comprises the following steps:

in the driving process of a vehicle, acquiring the vertical angular speed of the vehicle, first vehicle speed information of the vehicle and satellite positioning information of the vehicle;

performing Kalman filtering fusion on the satellite positioning information, the vertical angular rate and the first vehicle speed information to obtain first position information of the vehicle and first course angle information of the vehicle;

when the vehicle is identified to run to a gateway area of a target overhead according to the first position information and the first course angle information, monitoring the inclination angle information of the vehicle in real time;

if the inclination angle information of the vehicle is matched with the inclination angle data of the target overhead, determining the overhead driving direction of the vehicle;

and performing map matching on the first position information, the first course angle information and the inclination angle information of the vehicle according to the overhead driving direction of the vehicle to determine a first route in a map, and marking the position and/or the angle of the vehicle on the first route.

As an optional implementation manner, in the first aspect of this embodiment of the present invention, the method further includes:

if the inclination angle information of the vehicle is not matched with the inclination angle data of the target overhead and the vehicle runs on the target overhead currently, map matching is carried out on the first position information, the first course angle information and the inclination angle information of the vehicle by combining the road data of the target overhead so as to determine a second route in a map, and the position and/or the angle of the vehicle are/is marked on the second route;

or if the inclination angle information of the vehicle is not matched with the inclination angle data of the target overhead and the vehicle is currently running on a plane road, map matching is performed on the first position information, the first course angle information and the inclination angle information of the vehicle by combining the road data of the plane road so as to determine a third route in a map, and the position and/or the angle of the vehicle are/is marked on the third route.

As an alternative implementation, in the first aspect of the embodiments of the present invention, the map-matching the first position information, the first heading angle information, and the inclination angle information of the vehicle according to the overhead traveling direction of the vehicle to determine a first route in a map, and marking the vehicle position and/or the vehicle angle on the first route includes:

if the elevated driving direction of the vehicle is an uplink direction, map matching is carried out on the first position information, the first course angle information and the inclination angle information of the vehicle by combining the road data of the target elevated so as to determine a first route in a map, and the position and/or the angle of the vehicle are/is marked on the first route;

or if the overhead driving direction of the vehicle is a downlink direction, map matching is performed on the first position information, the first course angle information and the inclination angle information of the vehicle by combining road data of a plane road, so as to determine a first route in a map, and the position and/or the angle of the vehicle are/is marked on the first route.

As an alternative implementation, in the first aspect of the embodiment of the present invention, the obtaining the vertical angular velocity of the vehicle includes:

acquiring a vertical angular rate of the vehicle through a gyroscope;

the real-time monitoring of the inclination angle information of the vehicle comprises:

under a vehicle coordinate system, acquiring a transverse angular rate of the vehicle and a longitudinal angular rate of the vehicle through the gyroscope, and respectively performing integral operation on the transverse angular rate and the longitudinal angular rate to obtain a first pitch angle of the vehicle and a first roll angle of the vehicle;

under the vehicle coordinate system, acquiring a longitudinal ratio force value received by the vehicle and a transverse ratio force value received by the vehicle through an accelerometer, and respectively converting the longitudinal ratio force value and the transverse ratio force value to obtain a second pitch angle of the vehicle and a second roll angle of the vehicle;

carrying out angle correction on the first pitch angle by using the second pitch angle to obtain a third pitch angle of the vehicle;

carrying out angle correction on the first rolling angle by utilizing the second rolling angle so as to obtain a third rolling angle of the vehicle;

and obtaining the inclination angle information of the vehicle under a navigation coordinate system according to the third pitch angle and the third roll angle.

As an alternative implementation, in the first aspect of the embodiment of the present invention, the performing an angular correction on the first pitch angle by using the second pitch angle to obtain a third pitch angle of the vehicle includes:

combining a first angle correction formula, and performing angle correction on the first pitch angle by using the second pitch angle so as to obtain a third pitch angle of the vehicle when the correction times of the first pitch angle reach preset times;

wherein the first angle correction formula is:

S_t＝|(S_t-1+x₁)-y₁|×K+(S_t-1+x₁)；

said S_tIn order to correct the first pitch angle for t times, wherein t is an integer greater than or equal to 1, and S_t-1For a vehicle pitch angle after t-1 corrections of the first pitch angle, x₁Is the first pitch angle, y₁The second pitch angle is defined, and K is a preset gain parameter;

the performing angle correction on the first roll angle by using the second roll angle to obtain a third roll angle of the vehicle includes:

carrying out angle correction on the first rolling angle by utilizing the second rolling angle in combination with a second angle correction formula so as to obtain a third rolling angle of the vehicle when the correction times of the first rolling angle reach the preset times;

wherein the second angle correction formula is:

S_t'＝|(S_t-1'+x₂)-y₂|×K+(S_t-1'+x₂)；

said S_t' is a vehicle roll angle obtained by correcting the first roll angle t times, wherein t is an integer of 1 or more, and S_t-1' is a vehicle roll angle corrected t-1 times for the first roll angle, x₂For the first roll angle, y₂And K is the preset gain parameter for the second roll angle.

As an optional implementation manner, in the first aspect of the embodiment of the present invention, the performing kalman filter fusion on the satellite positioning information, the vertical angular rate, and the first vehicle speed information to obtain the first position information of the vehicle and the first heading angle information of the vehicle includes:

acquiring historical system state variables and historical error covariance;

obtaining a predicted system state variable according to the historical system state variable;

obtaining a prediction error covariance according to the historical error covariance:

constructing an observation matrix according to the satellite positioning information, the vertical angular rate and the first vehicle speed information;

correcting the predicted system state variable according to the observation matrix and the prediction error covariance to obtain a current system state variable;

and obtaining first position information of the vehicle and first course angle information of the vehicle according to the current system state variable.

As an optional implementation manner, in the first aspect of the embodiment of the present invention, the obtaining a predicted system state variable according to the historical system state variable includes:

obtaining a predicted system state variable according to the historical system state variable and by combining the following formula, namely:

X_n＝F_nX_n-1；

wherein, X is_nFor the predicted system state variables, said F_nFor a motion state transition matrix of the vehicle, X_n-1The historical system state variable is used as the state variable, and n is a natural number;

the satellite positioning information includes satellite state information, and the obtaining a prediction error covariance according to the historical error covariance includes:

obtaining a prediction error covariance according to the historical error covariance and combining the following formula, namely:

P_n＝F_nP_n-1F_n ^T+Q_n；

wherein, the P_nFor the prediction error covariance, the P_n-1For the historical error covariance, the F_n ^TIs the said F_nThe inverse matrix of, the Q_nA system noise matrix generated based on the satellite state information.

As an optional implementation manner, in the first aspect of the embodiment of the present invention, the satellite positioning information further includes second position information, second vehicle speed information, and second heading angle information, and the second position information includes longitude information and latitude information; the correcting the predicted system state variable according to the observation matrix and the prediction error covariance to obtain a current system state variable includes:

calculating a Kalman gain based on the prediction error covariance in combination with the following equation:

wherein, K is_nIs the Kalman gain, the H_nTo convert a matrix, the H_nT is the above-mentioned H_nThe inverse matrix of (1), the R_nMeasuring a noise covariance matrix;

correcting the predicted system state variable according to the observation matrix and the Kalman gain and by combining the following formula to obtain the current system state variable, namely:

X_n'＝X_n+K_n[Z_n-H_n·X_n]；

wherein, X is_n' is the current system state variable, the Z_nIs the observation matrix;

wherein the current system state variable X_n' is the system state variable corresponding to the current moment, and the historical system state variable X_n-1The calendar before the current timeSystem state variables corresponding to the history moments; the system state variable is a matrix X, and X ═ p_E p_N v h a ω δ k ε]^TSaid p is_EIs the east position of the vehicle, the p_NThe north position of the vehicle is shown, v is the vehicle speed of the vehicle, h is the course angle of the vehicle, alpha is the speed change rate of the vehicle in the vehicle running direction, omega is the course angle rate of the vehicle, delta is the vehicle speed residual error, k is the vehicle speed correction parameter, and epsilon is the gyro zero offset drift;

wherein the observation matrix Z_nComprises the following steps: z_n＝[long_GNSS lat_GNSS v_GNSS h_GNSS v_Car ω_Gyro]^TSaid long_GNSSFor the longitude information, the lat_GNSSFor the latitude information, the v_GNSSFor the second vehicle speed information, the h_GNSSIs the second course angle information, v_CarAs the first vehicle speed information, the ω_GyroIs the vertical angular velocity.

As an optional implementation manner, in the first aspect of the embodiments of the present invention, the acquiring vertical angular velocity of the vehicle, the first vehicle speed information of the vehicle, and the satellite positioning information of the vehicle during the driving of the vehicle includes:

in the driving process of a vehicle, acquiring a plurality of vertical angular rate data frames acquired by a gyroscope between a current observation time and a previous observation time, and performing timestamp adjustment on the plurality of vertical angular rate data frames according to an adjustment coefficient of the gyroscope so as to obtain a vertical angular rate corresponding to the vehicle at the current observation time according to the adjusted plurality of vertical angular rate data frames; the adjusting coefficient of the gyroscope is the ratio of the actual acquisition frequency of the gyroscope to the preset acquisition frequency;

acquiring a plurality of vehicle speed data frames acquired between the current observation time and the last observation time through a vehicle speed signal, and performing timestamp adjustment on the plurality of vehicle speed data frames according to an adjustment coefficient of the vehicle speed signal so as to obtain first vehicle speed information corresponding to the vehicle at the current observation time according to the adjusted plurality of vehicle speed data frames; the adjustment coefficient of the vehicle speed signal is the ratio of the actual acquisition frequency of the vehicle speed signal to the preset acquisition frequency; the time stamps of the plurality of adjusted vertical angular rate data frames correspond to the time stamps of the plurality of adjusted vehicle speed data frames one to one;

and acquiring satellite positioning information corresponding to the vehicle at the current observation time.

As an optional implementation manner, in the first aspect of the embodiment of the present invention, after the obtaining the first vehicle speed information of the vehicle and before the obtaining the satellite positioning information of the vehicle, the method further includes:

judging whether the vehicle can receive satellite signals or not;

if yes, executing the step of obtaining the satellite positioning information of the vehicle;

if not, performing integral operation on the vertical angular rate of the vehicle to obtain third course angle information of the vehicle; calculating third position information of the vehicle according to the first vehicle speed information; and performing map matching on the third position information and the third course angle information to determine a fourth road route in the map, and marking the vehicle position and/or the vehicle course angle on the fourth road route.

A second aspect of the embodiments of the present invention discloses a combined navigation system, including:

the vehicle positioning system comprises an acquisition unit, a positioning unit and a positioning unit, wherein the acquisition unit is used for acquiring the vertical angular speed of a vehicle, first vehicle speed information of the vehicle and satellite positioning information of the vehicle in the driving process of the vehicle;

the fusion unit is used for performing Kalman filtering fusion on the satellite positioning information, the vertical angular rate and the first vehicle speed information to obtain first position information of the vehicle and first course angle information of the vehicle;

the monitoring unit is used for monitoring the inclination angle information of the vehicle in real time when the vehicle is identified to run to the gateway area of the target overhead according to the first position information and the first course angle information;

a direction determination unit for determining an overhead traveling direction of the vehicle when the inclination information of the vehicle matches the inclination data of the target overhead;

and the first matching unit is used for carrying out map matching on the first position information, the first course angle information and the inclination angle information of the vehicle according to the overhead driving direction of the vehicle so as to determine a first route in a map, and marking the position and/or the angle of the vehicle on the first route.

As an optional implementation manner, in a second aspect of the embodiment of the present invention, the first matching unit includes:

the first matching subunit is used for performing map matching on the first position information, the first course angle information and the inclination angle information of the vehicle by combining the road data of the target overhead when the overhead driving direction of the vehicle is an uplink direction so as to determine a first route in a map, and marking the position and/or the angle of the vehicle on the first route;

and the second matching subunit is used for performing map matching on the first position information, the first course angle information and the inclination angle information of the vehicle by combining road data of a plane road when the overhead driving direction of the vehicle is a downlink direction so as to determine a first route in a map, and marking the position and/or the angle of the vehicle on the first route.

As an optional implementation manner, in the second aspect of the embodiment of the present invention, the system further includes:

a determining unit, configured to determine whether the vehicle can receive a satellite signal after the obtaining unit obtains the first vehicle speed information of the vehicle and before the obtaining unit obtains the satellite positioning information of the vehicle;

the computing unit is used for performing integral computation on the vertical angular rate of the vehicle to obtain third course angle information of the vehicle when the judging unit judges that the vehicle cannot receive the satellite signal;

an estimation unit configured to estimate third position information of the vehicle based on the first vehicle speed information;

the second matching unit is used for carrying out map matching on the third position information and the third course angle information so as to determine a fourth road route in a map and mark the vehicle position and/or the vehicle course angle on the fourth road route;

the manner for the obtaining unit to obtain the satellite positioning information of the vehicle is specifically:

the acquisition unit is used for acquiring the satellite positioning information of the vehicle when the judgment unit judges that the vehicle can receive the satellite signal.

A third aspect of the embodiments of the present invention discloses a vehicle including the integrated navigation system disclosed in the second aspect of the embodiments of the present invention.

A fourth aspect of the embodiments of the present invention discloses a computer-readable storage medium, which stores a computer program, wherein the computer program enables a computer to execute a combined navigation method disclosed in the first aspect of the embodiments of the present invention.

Compared with the prior art, the embodiment of the invention has the following beneficial effects:

by implementing the embodiment of the invention, Kalman filtering fusion is carried out by acquiring the satellite positioning information of the vehicle, the vertical angular rate of the vehicle and the speed information of the vehicle, whether the vehicle runs to an overhead road section is identified based on the vehicle position information and the vehicle course angular information acquired after the information fusion, and the overhead movement of the vehicle is identified based on the inclination angle information of the vehicle, so that the problems of large satellite positioning error and inaccurate navigation route when the vehicle runs to the overhead road section can be solved, the accuracy and robustness of vehicle positioning are improved, and the navigation precision of the vehicle is further improved; in addition, whether the current vehicle enters the elevated road on an elevated ramp or runs off the elevated road on a downhill by judging to determine whether the vehicle continues to navigate based on the elevated road or a plane road, and then map matching is performed on the vehicle positioning result obtained after information fusion, so that a road line which is more in line with the vehicle positioning result can be determined in the map, and the vehicle position and/or the vehicle angle are/is displayed on the road line, thereby realizing an accurate route navigation function and improving the driving navigation experience of a user.

Drawings

In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art that other drawings can be obtained according to these drawings without creative efforts.

FIG. 1 is a flow chart of a combined navigation method according to an embodiment of the present invention;

FIG. 2 is a flow chart of another integrated navigation method disclosed in the embodiment of the present invention;

FIG. 3 is a schematic structural diagram of a combined navigation system according to an embodiment of the present invention;

fig. 4 is a schematic structural diagram of another integrated navigation system disclosed in the embodiment of the present invention.

Detailed Description

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

It should be noted that the terms "first", "second", "third", "fourth", and the like in the description and the claims of the present invention are used for distinguishing different objects, and are not used for describing a specific order. The terms "comprises," "comprising," and any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed, but may include other steps or elements not expressly listed or inherent to such process, method, article, or apparatus.

The embodiment of the invention discloses a combined navigation method, a system and a vehicle, which can solve the problems of large satellite positioning error and inaccurate navigation route when the vehicle runs to an elevated road section, thereby improving the navigation precision of the vehicle. The following detailed description is made with reference to the accompanying drawings.

Example one

Referring to fig. 1, fig. 1 is a flow chart illustrating a combined navigation method according to an embodiment of the present invention. As shown in fig. 1, the integrated navigation method is applied to an integrated navigation system, and may specifically include the following steps.

101. The integrated navigation system acquires the vertical angular velocity of the vehicle, first vehicle speed information of the vehicle and satellite positioning information of the vehicle in the driving process of the vehicle.

In an embodiment of the present invention, the integrated navigation system may acquire angular velocity information and acceleration information of the vehicle through an inertial measurement unit, where the inertial measurement unit may include a gyroscope and an accelerometer, which is not particularly limited. It can be understood that, since the vertical angular velocity of the vehicle is related to the heading angle thereof, the vertical angular velocity of the vehicle can be obtained by subtracting the zero offset from the raw data output from the gyroscope and multiplying the result by the scale factor, so as to further perform time integration on the vertical angular velocity to obtain the heading angle information of the vehicle. In addition, the integrated navigation system may obtain the first vehicle speed information of the vehicle by converting a vehicle speed signal distributed by a CAN (Controller Area Network) bus.

In the embodiment of the invention, the integrated navigation system can receive the satellite observation data and solve the satellite observation data to obtain the satellite positioning information of the vehicle. The integrated Navigation System may receive Satellite observation data through a GNSS (Global Navigation Satellite System). Optionally, the GNSS module is a multimode satellite positioning module based on a global positioning system, a beidou second generation navigation system, a european galileo navigation system, a russian glonass navigation system, and a satellite-based augmentation system. Specifically, the integrated navigation system receives satellite observation data transmitted from a plurality of satellites through a GNSS module at a preset frequency (for example, 1Hz), performs related operations such as down-conversion, acquisition and tracking, and PVT solution (including position, speed, and time solution) on the satellite observation data, and can obtain pseudo ranges between the plurality of satellites and a vehicle according to information such as satellite positions, speeds, elevation angles, and inclination angles of the plurality of satellites at a specific observation time, so as to determine a position of the vehicle, a speed of the vehicle, and a heading angle of the vehicle. It can be seen that the availability and reliability of satellite positioning can be improved by maximizing the number of visible, available satellites.

It can be understood that, in order to ensure the satellite positioning and the time synchronization of the data collected by the different sensing devices, as an alternative implementation, the vertical angular rate of the vehicle, the first vehicle speed information of the vehicle, and the satellite positioning information of the vehicle may be obtained based on the current observation time of the GNSS module.

Thus, step 101 may comprise:

in the driving process of the vehicle, the integrated navigation system acquires a plurality of vertical angular rate data frames acquired by the gyroscope between the current observation time and the last observation time, and carries out timestamp adjustment on the plurality of vertical angular rate data frames according to the adjustment coefficient of the gyroscope so as to acquire the vertical angular rate corresponding to the vehicle at the current observation time according to the adjusted plurality of vertical angular rate data frames; the adjusting coefficient of the gyroscope is the ratio of the actual acquisition frequency of the gyroscope to the preset acquisition frequency;

the combined navigation system acquires a plurality of vehicle speed data frames acquired between the current observation time and the last observation time through the vehicle speed signal, and carries out timestamp adjustment on the plurality of vehicle speed data frames according to an adjustment coefficient of the vehicle speed signal so as to obtain first vehicle speed information corresponding to the vehicle at the current observation time according to the plurality of adjusted vehicle speed data frames; the adjustment coefficient of the vehicle speed signal is the ratio of the actual acquisition frequency of the vehicle speed signal to the preset acquisition frequency; the time stamps of the plurality of adjusted vertical angular rate data frames correspond to the time stamps of the plurality of adjusted vehicle speed data frames one to one;

and the integrated navigation system acquires satellite positioning information corresponding to the vehicle at the current observation time.

Optionally, the time stamp adjustment of the multiple vertical angular rate data frames by the integrated navigation system according to the adjustment coefficient of the gyroscope may include:

the combined navigation system utilizes the adjustment coefficient of the gyroscope to adjust the actual time stamp of each vertical angular rate data frame, and obtains a corresponding target time stamp so as to take the vertical angular rate data frame as a data frame corresponding to the target time stamp; for example, if the actual acquisition frequency of the gyroscope is 8Hz and the preset acquisition frequency is 10Hz, the adjustment coefficient of the gyroscope is 8 ÷ 10 ÷ 0.8. Therefore, assuming that data is acquired from 0ms, the vertical angular rate data frame acquired by the gyroscope at 125ms is the data frame corresponding to the target timestamp at 100 ms; the vertical angular velocity data frame acquired by the gyroscope at 250ms is the data frame corresponding to the target timestamp at 200 ms.

Similarly, the time stamp adjustment of the plurality of vehicle speed data frames by the integrated navigation system according to the adjustment coefficient of the vehicle speed signal may specifically include:

and the combined navigation system adjusts the actual time stamp of each vehicle speed data frame by using the adjustment coefficient of the vehicle speed signal to obtain a corresponding target time stamp so as to take the vehicle speed data frame as a data frame corresponding to the target time stamp.

Optionally, if the target timestamp corresponding to any vertical angular rate data frame/or vehicle speed data frame does not belong to the time interval between the current observation time and the last observation time, the combined navigation system removes the vertical angular rate data frame/or vehicle speed data frame;

if the actual frame number of the plurality of vertical angular rate data frames/or the vehicle speed data frames is less than the preset frame number, automatically supplementing the data frames with the corresponding number according to the difference between the preset frame number and the actual frame number of the plurality of vertical angular rate data frames/or the vehicle speed data frames; for example, if 9 vehicle speed data frames are collected between the current observation time and the last observation time, and the number of the preset frames is 10, one data frame is added at the 10 th timestamp between the current observation time and the last observation time, and the added data frame may be the same as the vehicle speed data frame corresponding to the 9 th timestamp.

It can be understood that the above logic is also applicable to other sensors of the vehicle, and can reduce the data frame loss rate of each target timestamp, and ensure the time synchronism of the multi-source observation data for vehicle positioning.

102. The combined navigation system performs Kalman filtering fusion on the satellite positioning information, the vertical angular rate and the first vehicle speed information to obtain first position information of the vehicle and first course angle information of the vehicle.

In the embodiment of the invention, the observation time of the GNSS module is taken as the same time reference, and Kalman filtering fusion is carried out on the satellite positioning information obtained at the observation time, the vertical angular rate of the vehicle and the vehicle speed information of the vehicle, so that the optimal estimation result of the position, the vehicle speed and the course angle of the vehicle can be solved based on the multi-source observation information, the accumulated error is eliminated, and the accuracy of vehicle positioning is further improved.

103. And the combined navigation system monitors the inclination angle information of the vehicle in real time when identifying that the vehicle runs to the gateway area of the target overhead according to the first position information and the first course angle information.

104. And if the inclination angle information of the vehicle is matched with the inclination angle data of the target overhead, determining the overhead driving direction of the vehicle by the integrated navigation system.

In the embodiment of the invention, it can be understood that the inclination information of the vehicle comprises the inclination size and the direction of the vehicle in the navigation coordinate system. Step 104 may specifically be: if the inclination angle of the vehicle is matched with the inclination angle data of the target overhead, and when the inclination angle direction of the vehicle is consistent with the uplink direction of the target overhead, determining that the overhead driving direction of the vehicle is the uplink direction; when the tilt direction of the vehicle coincides with the down direction of the target overhead, it is determined that the overhead traveling direction of the vehicle is the down direction.

105. And the combined navigation system performs map matching on the first position information, the first course angle information and the inclination angle information of the vehicle according to the overhead driving direction of the vehicle so as to determine a first route in a map, and marks the position and/or the angle of the vehicle on the first route.

It will be appreciated that by determining a first route in the map, the vehicle location may be marked on the first route; or, the vehicle angle can be marked on the first road; alternatively, the vehicle position and the vehicle angle may be marked simultaneously on the first route.

As an optional implementation manner, step 105 specifically includes:

if the elevated driving direction of the vehicle is an uplink direction, map matching is carried out on the first position information, the first course angle information and the inclination angle information of the vehicle by combining the road data of the target elevated so as to determine a first route in the map, and the position and/or the angle of the vehicle are/is marked on the first route;

or if the overhead driving direction of the vehicle is the downlink direction, map matching is performed on the first position information, the first course angle information and the inclination angle information of the vehicle by combining the road data of the plane road so as to determine a first route in the map, and the position and/or the angle of the vehicle are/is marked on the first route.

It can be understood that, since the position information and the heading angle information of the vehicle obtained based on the above-mentioned combined navigation method generally have errors with the road data in the map, and the vehicle needs to match the road data of the elevated road in the elevated upward direction, and needs to match the road data of the planar road in the elevated downward direction, by performing map matching in combination with the elevated traveling direction, a road line that most matches the position information and the heading angle information of the vehicle can be selected, and the vehicle is marked on the road line to match the vehicle to the road network, and the information such as the position, the angle, the vehicle speed, and the like of the vehicle can be displayed, thereby avoiding the situation that the vehicle still uses the planar road data for navigation after driving on the elevated, or uses the elevated data for navigation after driving off the elevated to cause a route deviation.

It can be seen that, by implementing the method described in fig. 1, kalman filter fusion is performed by obtaining the satellite positioning information of the vehicle, the vertical angular velocity of the vehicle, and the vehicle speed information of the vehicle, and identifying whether the vehicle is driven to an overhead road section based on the vehicle position information and the vehicle course angular information obtained after the information fusion, and identifying the overhead movement of the vehicle based on the vehicle inclination information, the problems of large satellite positioning error and inaccurate navigation route when the vehicle is driven to the overhead road section can be solved, the accuracy and robustness of vehicle positioning are improved, and the navigation accuracy of the vehicle is further improved; in addition, whether the current vehicle enters the elevated road on an elevated ramp or runs off the elevated road on a downhill by judging to determine whether the vehicle continues to navigate based on the elevated road or a plane road, and then map matching is performed on the vehicle positioning result obtained after information fusion, so that a road line which is more in line with the vehicle positioning result can be determined in the map, and the vehicle position and/or the vehicle angle are/is displayed on the road line, thereby realizing an accurate route navigation function and improving the driving navigation experience of a user.

Example two

Referring to fig. 2, fig. 2 is a flow chart illustrating another integrated navigation method according to an embodiment of the present invention. As shown in fig. 2, the integrated navigation method may include the following steps.

201. The integrated navigation system acquires the vertical angular velocity of the vehicle and first vehicle speed information of the vehicle in the driving process of the vehicle.

202. The integrated navigation system judges whether the vehicle can receive the satellite signal, if so, the steps 206 to 210 are executed; if not, step 203 to step 205 are executed.

203. And the integrated navigation system performs integral operation on the vertical angular rate of the vehicle to obtain third course angle information of the vehicle.

204. And the integrated navigation system calculates third position information of the vehicle according to the first vehicle speed information.

205. And the combined navigation system performs map matching on the third position information and the third course angle information to determine a fourth road route in the map, and marks the position and/or the course angle of the vehicle on the fourth road route.

As can be seen, by implementing the above steps 202 to 205, in a scene where the vehicle cannot receive the satellite signal, such as a tunnel, an underground parking lot, and the like, the third course angle information of the vehicle can be directly obtained according to the vertical angular rate obtained by the gyroscope, so as to identify a branch in the driving route; moreover, the CAN bus continuously outputs a vehicle speed signal, so that the acceleration and deceleration actions of the vehicle CAN be recognized; in addition, after the first vehicle speed information of the vehicle is acquired, the third position information of the vehicle at the current time can be calculated by combining the vehicle position, the vehicle course angle and other information at the historical time, and the known third position information, the first vehicle speed information and the third course angle information can be subjected to map matching to realize the vehicle positioning and navigation functions under the condition of no satellite signal.

206. The integrated navigation system acquires satellite positioning information.

207. The combined navigation system performs Kalman filtering fusion on the satellite positioning information, the vertical angular rate and the first vehicle speed information to obtain first position information of the vehicle and first course angle information of the vehicle.

As an optional implementation manner, step 207 specifically includes:

the combined navigation system acquires a historical system state variable and a historical error covariance;

the integrated navigation system obtains a prediction system state variable according to the historical system state variable;

and the combined navigation system obtains the prediction error covariance according to the historical error covariance:

the combined navigation system constructs an observation matrix according to the satellite positioning information, the vertical angular rate and the first vehicle speed information;

the combined navigation system corrects the state variable of the prediction system according to the observation matrix and the covariance of the prediction error so as to obtain the state variable of the current system;

the integrated navigation system obtains first position information of the vehicle and first course angle information of the vehicle according to the current system state variable.

It is understood that the kalman filter fusion algorithm uses state variables to characterize various physical quantities in the system, such as vehicle position, vehicle speed, angular rate, and so on. The historical system state variable is a system state variable corresponding to a historical moment before the current moment, and the historical error covariance is an error covariance corresponding to the historical moment.

Further, as an optional implementation manner, the obtaining, by the integrated navigation system, the predicted system state variable according to the historical system state variable includes:

the combined navigation system obtains the predicted system state variable according to the historical system state variable and by combining the following formula, namely:

X_n＝F_nX_n-1；

wherein, X_nTo predict system state variables, F_nThe motion state transition matrix for the vehicle is actually a mathematical model, X, established for the vehicle motion state transition_n-1Is a historical system state variable, and n is a natural number.

And, the satellite positioning information includes satellite state information, which may specifically include satellite number, satellite pitch angle, satellite azimuth angle, satellite carrier-to-noise ratio (CNo), and the like. The combined navigation system obtains the prediction error covariance according to the historical error covariance, and specifically comprises the following steps:

the combined navigation system obtains the prediction error covariance according to the historical error covariance and by combining the following formula, namely:

wherein, P_nTo predict error covariance, P_n-1In order to be the covariance of the historical error,

is F_nInverse matrix of, Q_nA system noise matrix generated based on the satellite state information is used to introduce uncertainty in the environment.

In addition, in the embodiment of the present invention, the satellite positioning information further includes second position information, second vehicle speed information, and second heading angle information, and the second position information includes longitude information and latitude information. As an optional implementation manner, the integrated navigation system corrects the predicted system state variable according to the observation matrix and the prediction error covariance to obtain the current system state variable, specifically:

the integrated navigation system calculates the kalman gain from the prediction error covariance in combination with the following equation, namely:

wherein, K_nAs Kalman gain, H_nA transformation matrix, which represents the relationship that connects the system state variables with the observed variables,

is H_nInverse matrix of R_nMeasuring a noise covariance matrix;

the integrated navigation system corrects the predicted system state variable according to the observation matrix and the Kalman gain and by combining the following formula to obtain the current system state variable, namely:

X_n'＝X_n+K_n[Z_n-H_n·X_n]；

wherein, X_n' is a current system state variable, Z_nIs an observation matrix;

wherein the current system state variable X_n' is a system state variable corresponding to the current moment; the system state variable is matrix X, and X ═ p_E p_N v h a ω δ k ε]^T，p_EEast-oriented position of vehicle, p_NIs the north position of the vehicle, v is the speed of the vehicleH is the course angle of the vehicle, alpha is the speed change rate of the vehicle in the driving direction of the vehicle, omega is the course angle rate of the vehicle, delta is the vehicle speed residual error, k is the vehicle speed correction parameter, and epsilon is the gyro zero offset drift;

wherein the observation matrix Z_nComprises the following steps: z_n＝[long_GNSS lat_GNSS v_GNSS h_GNSS v_Car ω_Gyro]^T，long_GNSSAs longitude information, lat_GNSSAs latitude information, v_GNSSFor the second vehicle speed information, h_GNSSIs the second course angle information, v_CarAs the first vehicle speed information, ω_GyroIs the vertical angular velocity.

It CAN be seen that, by implementing all the optional embodiments described above, the state variable of the prediction system CAN be calculated recursively directly according to the state variable of the historical system through the kalman filter fusion algorithm, the implementation is simple, and the state variable of the prediction system is corrected by combining the observation variables (including a plurality of observation physical quantities, such as the vehicle angular rate obtained through a gyroscope, the vehicle speed information obtained through a CAN bus, and the like) to obtain the current state variable of the system, so that the accumulated error CAN be eliminated, and the accuracy of vehicle positioning is further improved.

208. And the combined navigation system monitors the inclination angle information of the vehicle in real time when identifying that the vehicle runs to the gateway area of the target overhead according to the first position information and the first course angle information.

As an alternative, the integrated navigation system may acquire the vertical angular rate of the vehicle through a gyroscope. In addition, a three-axis gyroscope and a three-axis accelerometer may be provided on the vehicle to measure three-axis angular velocity and three-axis acceleration of the vehicle. In step 208, the integrated navigation system monitors the inclination angle information of the vehicle in real time, including:

under a vehicle coordinate system, the integrated navigation system acquires the transverse angular rate and the longitudinal angular rate of the vehicle through a gyroscope, and respectively performs time integral operation on the transverse angular rate and the longitudinal angular rate to obtain a first pitch angle of the vehicle and a first roll angle of the vehicle;

under a vehicle coordinate system, the integrated navigation system acquires a longitudinal ratio value and a transverse ratio value of the vehicle through an accelerometer, wherein the longitudinal ratio value of the vehicle is a component force value of the gravity acceleration on an X axis of the vehicle coordinate system, and the transverse ratio value of the vehicle is a component force value of the gravity acceleration on a Y axis of the vehicle coordinate system;

the combined navigation system converts the longitudinal ratio force value and the transverse ratio force value by using a cosine function respectively to obtain a second pitch angle of the vehicle and a second roll angle of the vehicle;

the combined navigation system corrects the angle of the first pitch angle by using the second pitch angle to obtain a third pitch angle of the vehicle;

the combined navigation system utilizes the second roll angle to carry out angle correction on the first roll angle so as to obtain a third roll angle of the vehicle;

and the integrated navigation system converts the third pitch angle and the third roll angle from the vehicle coordinate system to the navigation coordinate system according to the conversion relation between the vehicle coordinate system and the navigation coordinate system so as to obtain the inclination angle information of the vehicle in the navigation coordinate system.

Further, as an optional implementation, the integrated navigation system performs an angle correction on the first pitch angle by using the second pitch angle to obtain a third pitch angle of the vehicle, including:

the combined navigation system combines a first angle correction formula, and performs angle correction on the first pitch angle by using the second pitch angle, so as to obtain a third pitch angle of the vehicle when the correction times of the first pitch angle reach preset times;

wherein, the first angle correction formula is as follows:

S_t＝|(S_t-1+x₁)-y₁|×K+(S_t-1+x₁)；

S_tfor the vehicle pitch angle after the first pitch angle is corrected for t times, t is an integer greater than or equal to 1, S_t-1Vehicle pitch angle, x, after t-1 corrections to the first pitch angle₁Is a first pitch angle, y₁For the second pitch angle, K is a predetermined gain parameter, the predetermined gain parameter being a pass gainThe preset time parameter is obtained by dividing the output frequency of the gyroscope/accelerometer by the preset time parameter, and the preset time parameter is larger than the expected test time.

The combined navigation system carries out angle correction on the first roll angle by utilizing the second roll angle to obtain a third roll angle of the vehicle, and comprises:

the combined navigation system combines a second angle correction formula, and angle correction is carried out on the first rolling angle by using a second rolling angle, so that a third rolling angle of the vehicle is obtained when the correction times of the first rolling angle reach the preset times;

wherein the second angle correction formula is:

S_t'＝|(S_t-1'+x₂)-y₂|×K+(S_t-1'+x₂)；

S_t' is a vehicle roll angle obtained by correcting the first roll angle t times, t is an integer of 1 or more, and S_t-1' is a vehicle roll angle, x, corrected t-1 times for the first roll angle₂Is a first roll angle, y₂And K is a preset gain parameter, and is the second roll angle.

It will be appreciated that the number of corrections is related to the output frequency of the gyroscope and accelerometer, and that the output frequency of the gyroscope and the output frequency of the accelerometer are identical. Therefore, the optional implementation mode is implemented, the vehicle angle information obtained by the gyroscope is corrected through the vehicle angle information obtained by the accelerometer, the advantages of accuracy in short-time measurement of the gyroscope and long-time stability of the accelerometer can be integrated, errors of output angles are reduced, and accuracy of inclination angle matching is improved.

209. And if the inclination angle information of the vehicle is matched with the inclination angle data of the target overhead, determining the overhead driving direction of the vehicle by the integrated navigation system.

210. And the combined navigation system performs map matching on the first position information, the first course angle information and the inclination angle information of the vehicle according to the overhead driving direction of the vehicle so as to determine a first route in a map, and marks the position and/or the angle of the vehicle on the first route.

As an optional implementation manner, the manner of map-matching the first position information, the first heading angle information, and the inclination information of the vehicle by the integrated navigation system may specifically be:

the integrated navigation system judges whether the vehicle is preset with a destination or not;

if not, the combined navigation system uses the off-line road data under the vehicle cruising mode to carry out map matching on the first position information, the first course angle information and the inclination angle information of the vehicle by using the off-line road data;

if so, the integrated navigation system acquires destination information; and, in the vehicle navigation mode, when it is detected that the vehicle is connected to the network, performing on-line road and map calculation according to the first position information, the first course angle information, the inclination angle information of the vehicle, and the destination information; and when the fact that the vehicle is not connected with the network is detected, performing off-line road calculation and map matching on the first position information, the first course angle information, the inclination angle information of the vehicle and the destination information by using off-line road data.

It is to be understood that if the overhead traveling direction is the up direction, the off-line road data may include road data of the target overhead; if the overhead traveling direction is the down direction, the offline road data may include road data of a flat road. Therefore, the optional implementation mode is implemented, whether the vehicle is provided with the destination or not is detected, the navigation or cruise mode is automatically selected to enter, the online navigation state and the offline navigation state are automatically switched according to the network connection state of the vehicle, the map matching and road calculation functions can be flexibly adjusted based on the actual driving condition of the vehicle, and the driving experience of a user is improved.

As an optional implementation, the present solution may further include:

if the inclination angle information of the vehicle is not matched with the inclination angle data of the target overhead and the vehicle runs on the target overhead currently, map matching is carried out on the first position information, the first course angle information and the inclination angle information of the vehicle by combining the road data of the target overhead so as to determine a second road route in the map, and the position and/or the angle of the vehicle are/is marked on the second road route;

or if the inclination angle information of the vehicle is not matched with the inclination angle data of the target overhead and the vehicle is currently running on the plane road, map matching is carried out on the first position information, the first course angle information and the inclination angle information of the vehicle by combining the road data of the plane road so as to determine a third route in the map, and the position and/or the angle of the vehicle are/is marked on the third route.

Therefore, by implementing the optional implementation mode, under the condition that the inclination angle information of the vehicle is not matched with the inclination angle data of the target overhead, the vehicle can be considered to run forwards according to the original route, and the used road data does not need to be adjusted at the moment, so that the flexibility of map matching is ensured.

As an optional implementation manner, after step 206, the method may further include:

the combined navigation system carries out dead reckoning on the vertical angular speed of the vehicle and the first vehicle speed information of the vehicle so as to calculate and obtain fourth position information of the vehicle and fourth course angle information of the vehicle;

and the integrated navigation system performs Kalman filtering fusion on the satellite positioning information, the fourth position information, the first vehicle speed information and the fourth course angle information to obtain the vehicle position information and the vehicle course angle information after the information fusion so as to perform map matching.

The navigation position calculation is used for calculating the vehicle position at the current moment according to the vehicle course, the vehicle speed, the vehicle position at the historical moment and the relative time difference between the current moment and the historical moment. Therefore, the satellite positioning information acquired through the GNSS is added into the observation variable of the system, and the estimated vehicle position, the estimated vehicle speed and the estimated vehicle course information are added into the observation variable of the system through the navigation position operation on the measured vehicle angular rate and the measured vehicle speed, so that the effect of correcting the system state variable by using the observation variable can be further improved, and the positioning accuracy is improved.

It can be seen that, by implementing the method described in fig. 2, the state variable of the prediction system can be calculated recursively directly according to the state variable of the historical system through a kalman filter fusion algorithm, the implementation manner is simple, the state variable of the prediction system is corrected by combining with the observation variable to obtain the state variable of the current system, the accumulated error can be eliminated, the problem of large satellite positioning error when the automobile runs to a complex environment with low satellite signal quality is solved, the accuracy and robustness of vehicle positioning are improved, and the navigation precision of the vehicle is further improved; in addition, a road line which better accords with a vehicle positioning result can be determined in the map, and the vehicle position and/or the vehicle angle are/is displayed on the road line, so that an accurate route navigation function is realized, and the driving navigation experience of a user is improved; furthermore, the vehicle ascending and descending actions can be effectively recognized, the vehicle angle information obtained by the gyroscope is corrected through the vehicle angle information obtained by the accelerometer, the error of the output angle is reduced, and the accuracy of the inclination angle matching is improved. Furthermore, the fork and the acceleration and deceleration actions of the vehicle in the driving route can be identified, and the vehicle positioning and navigation functions without satellite signals are realized.

EXAMPLE III

Please refer to fig. 3, which is a schematic structural diagram of a combined navigation system according to an embodiment of the present invention. As shown in fig. 3, the integrated navigation system may include an acquisition unit 301, a fusion unit 302, a monitoring unit 303, a direction determination unit 304, and a first matching unit 305, wherein:

the acquiring unit 301 is configured to acquire a vertical angular rate of the vehicle, first vehicle speed information of the vehicle, and satellite positioning information of the vehicle during a driving process of the vehicle;

the fusion unit 302 is configured to perform kalman filtering fusion on the satellite positioning information, the vertical angular rate, and the first vehicle speed information to obtain first position information of the vehicle and first course angle information of the vehicle;

the monitoring unit 303 is configured to monitor the inclination angle information of the vehicle in real time when the vehicle is identified to travel to the gateway area of the target overhead according to the first position information and the first course angle information;

a direction determination unit 304 for determining an overhead traveling direction of the vehicle when the inclination information of the vehicle matches the inclination data of the target overhead;

a first matching unit 305, configured to perform map matching on the first position information, the first heading angle information, and the inclination angle information of the vehicle according to the overhead driving direction of the vehicle, to determine a first route in the map, and label the vehicle position and/or the vehicle angle on the first route.

As an alternative embodiment, the first matching unit 305 includes:

and the second matching subunit is used for performing map matching on the first position information, the first course angle information and the inclination angle information of the vehicle by combining the road data of the plane road when the overhead driving direction of the vehicle is the downlink direction so as to determine a first route in the map, and marking the position and/or the angle of the vehicle on the first route.

In this embodiment of the present invention, the obtaining unit 301 may be configured to receive satellite observation data and perform calculation on the satellite observation data to obtain satellite positioning information for the vehicle. The acquisition unit 301 may receive Satellite observation data through a GNSS (Global Navigation Satellite System). Optionally, the GNSS module is a multimode satellite positioning module based on a global positioning system, a beidou second generation navigation system, a european galileo navigation system, a russian glonass navigation system, and a satellite-based augmentation system. Specifically, the satellite positioning unit 403 is configured to receive satellite observation data from a plurality of satellites through a GNSS module at a preset frequency (for example, 1Hz), perform down-conversion, acquisition and tracking, PVT (including position, speed, and time solution) and other related operations on the satellite observation data, and obtain pseudoranges between the plurality of satellites and the vehicle according to information such as satellite positions, speeds, elevation angles, and inclination angles of the plurality of satellites at a specific observation time, so as to determine the position of the vehicle, the speed of the vehicle, and a vehicle heading angle. It can be seen that the availability and reliability of satellite positioning can be improved by maximizing the number of visible, available satellites.

As an optional implementation, the obtaining unit 301 may include:

the first acquisition subunit is used for acquiring a plurality of vertical angular rate data frames acquired by the gyroscope between the current observation time and the last observation time in the driving process of the vehicle;

the first adjusting subunit is used for adjusting the timestamps of the plurality of vertical angular rate data frames according to the adjusting coefficient of the gyroscope so as to obtain the vertical angular rate corresponding to the vehicle at the current observation time according to the adjusted plurality of vertical angular rate data frames; the adjusting coefficient of the gyroscope is the ratio of the actual acquisition frequency of the gyroscope to the preset acquisition frequency;

the second acquisition subunit is used for acquiring a plurality of vehicle speed data frames acquired between the current observation time and the last observation time through the vehicle speed signal;

the second adjusting subunit is used for performing timestamp adjustment on the plurality of vehicle speed data frames according to the adjustment coefficient of the vehicle speed signal so as to obtain first vehicle speed information corresponding to the vehicle at the current observation time according to the plurality of adjusted vehicle speed data frames; the adjustment coefficient of the vehicle speed signal is the ratio of the actual acquisition frequency of the vehicle speed signal to the preset acquisition frequency; the time stamps of the plurality of adjusted vertical angular rate data frames correspond to the time stamps of the plurality of adjusted vehicle speed data frames one to one;

and the satellite positioning subunit is used for acquiring satellite positioning information corresponding to the vehicle at the current observation time.

Optionally, the first adjusting subunit is configured to perform timestamp adjustment on the multiple vehicle speed data frames according to an adjustment coefficient of the vehicle speed signal, where a manner of performing timestamp adjustment on the multiple vehicle speed data frames may specifically be:

the first adjusting subunit is used for adjusting the actual time stamp of each vertical angular rate data frame by using the adjusting coefficient of the gyroscope to obtain a corresponding target time stamp so as to take the vertical angular rate data frame as a data frame corresponding to the target time stamp;

the second adjusting subunit is configured to, according to the adjustment coefficient of the vehicle speed signal, perform timestamp adjustment on the plurality of vehicle speed data frames in a specific manner:

and the second adjusting subunit is used for adjusting the actual time stamp of each vehicle speed data frame by using the adjusting coefficient of the vehicle speed signal, obtaining a corresponding target time stamp and taking the vehicle speed data frame as a data frame corresponding to the target time stamp.

Still optionally, the obtaining unit 301 may further include:

the third adjusting subunit is used for removing the vertical angular rate data frame and/or the vehicle speed data frame when the target timestamp corresponding to any vertical angular rate data frame and/or vehicle speed data frame does not belong to the time interval between the current observation time and the last observation time;

a fourth adjusting subunit, configured to, when the actual number of the plurality of vertical angular rate data frames/or vehicle speed data frames is smaller than the preset number of frames, automatically supplement data frames of a corresponding number according to a difference between the preset number of frames and the actual number of the plurality of vertical angular rate data frames/vehicle speed data frames; for example, if 9 vehicle speed data frames are collected between the current observation time and the last observation time, and the number of the preset frames is 10, one data frame is added at the 10 th timestamp between the current observation time and the last observation time, and the added data frame may be the same as the vehicle speed data frame corresponding to the 9 th timestamp.

It can be seen that, by implementing the system described in fig. 3, kalman filter fusion is performed by obtaining the satellite positioning information of the vehicle, the vertical angular velocity of the vehicle, and the vehicle speed information of the vehicle, and whether the vehicle is driven to an overhead road section is identified based on the vehicle position information and the vehicle course angular information obtained after the information fusion, and the overhead movement of the vehicle is identified based on the inclination information of the vehicle, the problems of large satellite positioning error and inaccurate navigation route when the vehicle is driven to the overhead road section can be solved, the accuracy and robustness of vehicle positioning are improved, and the navigation accuracy of the vehicle is further improved; in addition, whether the current vehicle enters the elevated road on an elevated ramp or runs off the elevated road on a downhill by judging to determine whether the vehicle continues to navigate based on the elevated road or a plane road, and then map matching is performed on the vehicle positioning result obtained after information fusion, so that a road line which is more in line with the vehicle positioning result can be determined in the map, and the vehicle position and/or the vehicle angle are/is displayed on the road line, thereby realizing an accurate route navigation function and improving the driving navigation experience of a user.

Example four

Referring to fig. 4, fig. 4 is a schematic structural diagram of another integrated navigation system according to an embodiment of the present invention. The integrated navigation system shown in fig. 4 is optimized by the integrated navigation system shown in fig. 3. Compared with the integrated navigation system shown in fig. 3, the integrated navigation system shown in fig. 4 further includes a determination unit 306, an arithmetic unit 307, an estimation unit 308, and a second matching unit 309, wherein:

a determining unit 306, configured to determine whether the vehicle can receive the satellite signal after the acquiring unit 301 acquires the first vehicle speed information of the vehicle and before the acquiring unit 301 acquires the satellite positioning information of the vehicle;

the operation unit 307 is used for performing integral operation on the vertical angular rate of the vehicle to obtain third course angle information of the vehicle when the judgment unit 306 judges that the vehicle cannot receive the satellite signal;

an estimation unit 308 for estimating third position information of the vehicle based on the first vehicle speed information;

a second matching unit 309, configured to perform map matching on the third position information and the third heading angle information, to determine a fourth road route in the map, and mark the vehicle position and/or the vehicle heading angle on the fourth road route;

the manner for acquiring the satellite positioning information of the vehicle by the acquiring unit 301 is specifically as follows:

an acquisition unit 301 configured to acquire satellite positioning information of the vehicle when the determination unit 306 determines that the vehicle can receive the satellite signal.

As an optional implementation, the fusion unit 302 includes:

a first obtaining subunit 3021, configured to obtain a historical system state variable and a historical error covariance;

a first operator unit 3022, configured to obtain a predicted system state variable according to a historical system state variable;

a second operator unit 3023, configured to obtain, according to the historical error covariance, a prediction error covariance:

the construction subunit 3024 is configured to construct an observation matrix according to the satellite positioning information, the vertical angular rate, and the first vehicle speed information;

a syndrome unit 3025 configured to correct the predicted system state variable according to the observation matrix and the prediction error covariance to obtain a current system state variable;

the second obtaining subunit 3026 is configured to obtain first position information of the vehicle and first heading angle information of the vehicle according to the current system state variable.

As an optional implementation manner, the first operator unit 3022 is specifically configured to obtain the predicted system state variable according to the historical system state variable and by combining the following formula, that is:

X_n＝F_nX_n-1；

wherein, X_nTo predict system state variables, F_nFor a motion state transition matrix of the vehicle, X_n-1Is a historical system state variable, and n is a natural number;

and, the satellite positioning information includes satellite state information, which may specifically include satellite number, satellite pitch angle, satellite azimuth angle, satellite carrier-to-noise ratio (CNo), and the like. The second operator unit 3023 is specifically configured to obtain the prediction error covariance according to the historical error covariance and by combining the following formula, that is:

is F_nInverse matrix of, Q_nA system noise matrix generated based on the satellite state information.

As an optional implementation manner, the satellite positioning information further includes second position information, second vehicle speed information and second heading angle information, and the second position information includes longitude information and latitude information;

a syndrome unit 3025, comprising:

a calculation module 30251 for calculating a kalman gain from the prediction error covariance in combination with the following equation, namely:

wherein, K_nAs Kalman gain, H_nIn order to convert the matrix, the first and second matrices,

is H_nInverse matrix of R_nMeasuring a noise covariance matrix;

a correcting module 30252, configured to correct the predicted system state variable according to the observation matrix and the kalman gain, and by combining the following formula, to obtain a current system state variable, that is:

X_n'＝X_n+K_n[Z_n-H_n·X_n]；

wherein, X_n' is a current system state variable, Z_nIs an observation matrix;

wherein the current system state variable X_n' is the system state variable corresponding to the current time, the historical system state variable X_n-1The system state variables corresponding to historical moments before the current moment; the system state variable is matrix X, and X ═ p_E p_Nv h a ω δ k ε]^T，p_EEast-oriented position of vehicle, p_NIs a vehicleThe method comprises the following steps that the north position of a vehicle, v is the speed of the vehicle, h is the course angle of the vehicle, alpha is the speed change rate of the vehicle in the driving direction of the vehicle, omega is the course angle rate of the vehicle, delta is the vehicle speed residual error, k is a vehicle speed correction parameter, and epsilon is the gyro zero offset drift;

As an alternative embodiment, the obtaining unit 301 may obtain the vertical angular rate of the vehicle through a gyroscope. In addition, a three-axis gyroscope and a three-axis accelerometer may be provided on the vehicle to measure three-axis angular velocity and three-axis acceleration of the vehicle; a monitoring unit 303 comprising:

the first operation subunit 3031 is configured to, when the vehicle is identified to travel to the gateway area of the target overhead according to the first position information and the first course angle information, perform integral operation on the lateral angular rate and the longitudinal angular rate of the vehicle through the gyroscope in the vehicle coordinate system, and obtain a first pitch angle of the vehicle and a first roll angle of the vehicle;

the second operation subunit 3032 is configured to obtain a longitudinal ratio force value received by the vehicle and a lateral ratio force value received by the vehicle through the accelerometer in the vehicle coordinate system, and convert the longitudinal ratio force value and the lateral ratio force value to obtain a second pitch angle of the vehicle and a second roll angle of the vehicle;

a first correcting subunit 3033, configured to perform angle correction on the first pitch angle by using the second pitch angle to obtain a third pitch angle of the vehicle;

a second correction subunit 3034, configured to perform angle correction on the first roll angle by using the second roll angle to obtain a third roll angle of the vehicle;

and the third operation subunit 3035 is configured to obtain the inclination information of the vehicle in the navigation coordinate system according to the third pitch angle and the third roll angle.

Further, as an optional implementation manner, the first correcting subunit 3033 is specifically configured to, in combination with the first angle correcting formula, perform angle correction on the first pitch angle by using the second pitch angle, so as to obtain a third pitch angle of the vehicle when the number of times of correcting the first pitch angle reaches a preset number of times;

wherein, the first angle correction formula is as follows:

S_t＝|(S_t-1+x₁)-y₁|×K+(S_t-1+x₁)；

S_tfor the vehicle pitch angle after the first pitch angle is corrected for t times, t is an integer greater than or equal to 1, S_t-1Vehicle pitch angle, x, after t-1 corrections to the first pitch angle₁Is a first pitch angle, y₁Is a second pitch angle, and K is a preset gain parameter;

the second correcting subunit 3034 is specifically configured to perform angle correction on the first roll angle by using the second roll angle in combination with a second angle correction formula, so as to obtain a third roll angle of the vehicle when the correction frequency of the first roll angle reaches a preset frequency;

wherein the second angle correction formula is:

S_t'＝|(S_t-1'+x₂)-y₂|×K+(S_t-1'+x₂)；

As an optional implementation manner, the way for the first matching unit 305 to perform map matching on the first position information, the first heading angle information, and the inclination angle information of the vehicle may specifically be:

a first matching unit 305 for determining whether a destination is set in advance for the vehicle; if not, calling the off-line road data in the vehicle cruising mode so as to carry out map matching on the first position information, the first course angle information and the inclination angle information of the vehicle by using the off-line road data; if yes, destination information is obtained; and, in the vehicle navigation mode, when it is detected that the vehicle is connected to the network, performing on-line road and map calculation according to the first position information, the first course angle information, the inclination angle information of the vehicle, and the destination information; and when the fact that the vehicle is not connected with the network is detected, performing off-line road calculation and map matching on the first position information, the first course angle information, the inclination angle information of the vehicle and the destination information by using off-line road data.

As an optional implementation, the system may further include:

a third matching unit 310, configured to, when the inclination information of the vehicle does not match the inclination data of the target overhead and the vehicle is currently running on the target overhead, perform map matching on the first position information, the first course angle information, and the inclination information of the vehicle in combination with the road data of the target overhead to determine a second route in the map, and mark the vehicle position and/or the vehicle angle on the second route;

and a fourth matching unit 311, configured to, when the inclination information of the vehicle does not match the inclination data of the target overhead and the vehicle is currently traveling on a planar road, perform map matching on the first position information, the first course angle information, and the inclination information of the vehicle in combination with the road data of the planar road, to determine a third route in the map, and mark the vehicle position and/or the vehicle angle on the third route.

As an optional implementation manner, the operation unit 307 is further configured to perform a dead-reckoning operation on the vertical angular rate of the vehicle and the first vehicle speed information of the vehicle after the acquisition unit 301 acquires the satellite positioning information of the vehicle, so as to derive fourth position information of the vehicle and fourth heading angle information of the vehicle;

the fusion unit 302 is further configured to perform kalman filtering fusion on the satellite positioning information, the fourth position information, the first vehicle speed information, and the fourth heading angle information, to obtain information-fused vehicle position information and vehicle heading angle information, so as to perform map matching.

It can be seen that, by implementing the system described in fig. 4, the state variable of the prediction system can be calculated by the kalman filter fusion algorithm directly according to the state variable of the historical system in a recursive manner, the implementation mode is simple, the state variable of the prediction system is corrected by combining with the observation variable to obtain the current state variable of the system, the accumulated error can be eliminated, the problem of large satellite positioning error when the automobile runs to a complex environment with low satellite signal quality is solved, the accuracy and robustness of vehicle positioning are improved, and the navigation precision of the vehicle is further improved; in addition, a road line which better accords with a vehicle positioning result can be determined in the map, and the vehicle position and/or the vehicle angle are/is displayed on the road line, so that an accurate route navigation function is realized, and the driving navigation experience of a user is improved; furthermore, the vehicle ascending and descending actions can be effectively recognized, the vehicle angle information obtained by the gyroscope is corrected through the vehicle angle information obtained by the accelerometer, the error of the output angle is reduced, and the accuracy of the inclination angle matching is improved. Furthermore, the fork and the acceleration and deceleration actions of the vehicle in the driving route can be identified, and the vehicle positioning and navigation functions without satellite signals are realized.

The embodiment of the invention discloses a vehicle which comprises a combined navigation system shown in any one of figures 3-4.

The embodiment of the invention also discloses a computer readable storage medium which stores a computer program, wherein the computer program enables a computer to execute any one of the combined navigation methods shown in the figures 1-2.

It will be understood by those skilled in the art that all or part of the steps in the methods of the embodiments described above may be implemented by hardware instructions of a program, and the program may be stored in a computer-readable storage medium, where the storage medium includes Read-Only Memory (ROM), Random Access Memory (RAM), Programmable Read-Only Memory (PROM), Erasable Programmable Read-Only Memory (EPROM), One-time Programmable Read-Only Memory (OTPROM), Electrically Erasable Programmable Read-Only Memory (EEPROM), Compact Disc Read-Only Memory (CD-ROM), or other Memory, such as a magnetic disk, or a combination thereof, A tape memory, or any other medium readable by a computer that can be used to carry or store data.

The above detailed description is provided for a combined navigation method, a system and a vehicle disclosed in the embodiments of the present invention, and the principle and the implementation of the present invention are explained in the present document by applying specific examples, and the above description of the embodiments is only used to help understanding the method and the core idea of the present invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, there may be variations in the specific embodiments and the application scope, and in summary, the content of the present specification should not be construed as a limitation to the present invention.

Claims

1. A combined navigation method, characterized in that the method comprises:

acquiring satellite positioning information corresponding to the vehicle at the current observation time;

2. The method of claim 1, further comprising:

3. The method of claim 1, wherein the map-matching the first position information, the first heading angle information, and the inclination angle information of the vehicle according to the overhead traveling direction of the vehicle to determine a first route in a map and labeling a vehicle position and/or a vehicle angle on the first route comprises:

4. The method of claim 1, wherein the monitoring of the tilt angle information of the vehicle in real time comprises:

acquiring a transverse angular rate of the vehicle and a longitudinal angular rate of the vehicle through a gyroscope in a vehicle coordinate system, and respectively performing integral operation on the transverse angular rate and the longitudinal angular rate to obtain a first pitch angle of the vehicle and a first roll angle of the vehicle;

5. The method of claim 4, wherein angularly correcting the first pitch angle using the second pitch angle to obtain a third pitch angle of the vehicle comprises:

wherein the first angle correction formula is:

S_t＝|(S_t-1+x₁)-y₁|×K+(S_t-1+x₁)；

said S_tIn order to correct the first pitch angle for t times, wherein t is an integer greater than or equal to 1, and S_t-1For performing t-1 times on the first pitch angleCorrected vehicle pitch angle, said x₁Is the first pitch angle, y₁The second pitch angle is defined, and K is a preset gain parameter;

wherein the second angle correction formula is:

S_t'＝|(S_t-1'+x₂)-y₂|×K+(S_t-1'+x₂)；

6. The method of claim 1, wherein the kalman filter fusing the satellite positioning information, the vertical angular rate, and the first vehicle speed information to obtain the first position information of the vehicle and the first heading angle information of the vehicle comprises:

acquiring historical system state variables and historical error covariance;

7. The method of claim 6, wherein obtaining a predicted system state variable from the historical system state variables comprises:

X_n＝F_nX_n-1；

wherein, the P_nFor the prediction error covariance, the P_n-1For the historical error covariance, the

Is the said F_nThe inverse matrix of, the Q_nA system noise matrix generated based on the satellite state information.

8. The method of claim 7, wherein the satellite positioning information further comprises second location information, second vehicle speed information, and second heading angle information, the second location information comprising longitude information and latitude information; the correcting the predicted system state variable according to the observation matrix and the prediction error covariance to obtain a current system state variable includes:

wherein, K is_nIs the Kalman gain, the H_nTo convert a matrix, said

Is the said H_nThe inverse matrix of (1), the R_nMeasuring a noise covariance matrix;

X_n'＝X_n+K_n[Z_n-H_n·X_n]；

wherein the current system state variable X_n' is the system state variable corresponding to the current moment, and the historical system state variable X_n-1The system state variables corresponding to historical moments before the current moment; the system state variable is a matrix X, and X ═ p_E p_N v h a ω δ k ε]^TSaid p is_EIs the east position of the vehicle, the p_NThe north position of the vehicle, v is the speed of the vehicle, h is the course angle of the vehicle, alpha is the speed change rate of the vehicle in the driving direction of the vehicle, omega is the course angle speed of the vehicle, delta is the residual speed of the vehicleThe difference is that k is a vehicle speed correction parameter, and epsilon is gyro zero offset drift;

9. The method of any of claims 1-8, wherein after the obtaining the first vehicle speed information of the vehicle and before the obtaining the satellite positioning information of the vehicle, the method further comprises:

judging whether the vehicle can receive satellite signals or not;

10. A combined navigation system, the system comprising:

the acquisition unit is used for acquiring a plurality of vertical angular rate data frames acquired by a gyroscope between a current observation time and a previous observation time in the driving process of a vehicle, and carrying out timestamp adjustment on the plurality of vertical angular rate data frames according to an adjustment coefficient of the gyroscope so as to acquire a vertical angular rate corresponding to the vehicle at the current observation time according to the adjusted plurality of vertical angular rate data frames; the adjusting coefficient of the gyroscope is the ratio of the actual acquisition frequency of the gyroscope to the preset acquisition frequency; acquiring a plurality of vehicle speed data frames acquired between the current observation time and the last observation time through a vehicle speed signal, and performing timestamp adjustment on the plurality of vehicle speed data frames according to an adjustment coefficient of the vehicle speed signal so as to obtain first vehicle speed information corresponding to the vehicle at the current observation time according to the adjusted plurality of vehicle speed data frames; the adjustment coefficient of the vehicle speed signal is the ratio of the actual acquisition frequency of the vehicle speed signal to the preset acquisition frequency; the time stamps of the plurality of adjusted vertical angular rate data frames correspond to the time stamps of the plurality of adjusted vehicle speed data frames one to one; acquiring satellite positioning information corresponding to the vehicle at the current observation time;

11. The system of claim 10, wherein the first matching unit comprises:

12. The system according to claim 10 or 11, characterized in that the system further comprises:

13. A vehicle characterized in that it comprises an integrated navigation system according to any one of claims 10 to 12.