CN118149830B - Adaptive navigation method, device and equipment based on vehicle non-holonomic constraints - Google Patents

Abstract

The embodiment of the application discloses a self-adaptive navigation method, a device and equipment based on carrier non-integrity constraint, and relates to the technical field of inertial navigation, wherein the method comprises the following steps: respectively acquiring measurement data output by an inertia measurement unit and a wheel speed measurement unit, establishing an observation equation of the state of the carrier based on the measurement data, calculating lateral motion state parameters of the carrier, determining non-integrity constraint lateral constraint noise, and outputting an observation noise matrix; and calculating an error compensation vector through an observation equation and an observation noise matrix, and compensating the inertia measurement unit and the wheel speed measurement unit based on the error compensation vector. According to the application, through the error compensation vectors of the inertia measurement unit and the wheel speed measurement unit, the state parameters of the inertia measurement unit and the wheel speed measurement unit can be adjusted in real time in the running process of the carrier, so that the self-adaption correction of errors according to the change of the running state is realized, the error accumulation is avoided, the measurement precision is improved, and the navigation precision is improved.

Description

Adaptive navigation method, device and equipment based on vehicle non-holonomic constraints

Technical Field

The present application relates to the field of inertial navigation technology, and in particular to an adaptive navigation method, device and equipment based on vehicle non-integrity constraints.

Background technique

At present, the Inertial Navigation System (INS) is often used as the basis of the combined navigation system of land-based vehicles in dynamic urban scenes due to its good independence and high sampling rate, so as to realize the autonomous recursion of navigation status. However, due to the cost and volume of high-precision inertial sensors, the accuracy level of the inertial measurement unit (IMU) actually used for vehicle navigation is often low, which is particularly evident in the inertial system based on the Micro Electro Mechanical System Inertial Measurement Unit (MEMS IMU). Due to the rough manufacturing process of MEMS sensors, their zero bias will cause the positioning results of the vehicle to diverge rapidly over time, which will lead to the accumulation of position errors. Therefore, other types of sensors are needed to constrain their state estimation and suppress their error divergence process.

The non-holonomic constraint (NHC) means that when the vehicle is operating normally, the velocity components in the vertical and lateral directions are approximately zero, that is, it is assumed that there is almost no lateral sliding and vertical jumping during the normal driving of the ground vehicle. Using this prior condition, the accumulation of inertial navigation errors of the vehicle in motion can be improved to a certain extent. However, the non-holonomic constraint is too ideal. Under complex urban road conditions, the vehicle frequently turns and changes lanes, and its actual motion state does not strictly meet the NHC constraint, which will reduce the estimation accuracy of some state quantities and cannot meet the reliability and accuracy requirements of vehicle navigation in complex scenarios.

Summary of the invention

The embodiment of the present application provides an adaptive navigation method, device and apparatus based on vehicle non-holonomic constraints, which is used to solve the defect that only using the non-holonomic constraints of fixed noise in the related art cannot suppress navigation errors and lead to reduced navigation accuracy, so that the vehicle can adjust the state parameters of the inertial measurement unit and the wheel speed measurement unit in real time during operation, thereby realizing adaptive correction of the error according to the change of the driving state and avoiding error accumulation. The technical solution is as follows:

In a first aspect, an embodiment of the present application provides an adaptive navigation method based on vehicle non-holonomic constraints, wherein the vehicle is equipped with an inertial measurement unit and a wheel speed measurement unit, and the method comprises the following steps:

Acquire first measurement data output by the inertial measurement unit, and acquire second measurement data output by the wheel speed measurement unit;

Establishing an observation equation of a vehicle state based on the first measurement data and the second measurement data;

Calculating the lateral motion state parameters of the vehicle based on the first measurement data and the second measurement data, acquiring the non-holonomic constraint lateral constraint noise of the vehicle according to the lateral motion state parameters, and outputting an observation noise matrix based on the preset constraint noise and the non-holonomic constraint lateral constraint noise;

Obtaining an error compensation vector by calculating the observation equation and the observation noise matrix;

The error compensation vector is input into the inertial measurement unit and the wheel speed measurement unit so that the inertial measurement unit and the wheel speed measurement unit update corresponding state parameters, and the process proceeds to the step of obtaining the first measurement data output by the inertial measurement unit and the step of obtaining the second measurement data output by the wheel speed measurement unit.

In an optional solution of the first aspect, the first measurement data includes specific force and angular velocity, and the second measurement data includes the wheel speed of the driving wheel of the vehicle.

In an optional solution of the first aspect, establishing an observation equation of a vehicle state based on the first measurement data and the second measurement data includes:

Selecting an earth-centered earth-fixed coordinate system as a reference system, performing mechanical arrangement based on the first measurement data, and outputting the posture information of the inertial measurement unit; wherein the posture information includes position information, velocity information, and attitude information;

Projecting the speed information to a vehicle coordinate system, and outputting the speed information of the inertial measurement unit in the vehicle coordinate system;

Obtaining an observation value vector based on the driving wheel speed and non-holonomic constraint virtual observation values;

The observation equation is established based on the velocity information in the vehicle coordinate system and the observation value vector.

In an optional solution of the first aspect, the earth-centered earth-fixed coordinate system is selected as the reference system, and mechanical arrangement is performed based on the first measurement data, and the formula is applied:

;

Output the posture information of the inertial measurement unit including position information , speed information and posture information ;

The speed information is projected to the vehicle coordinate system, and the speed information in the vehicle coordinate system is output. , apply the formula:

;

;

in, is the position information of the inertial measurement unit in the Earth-centered Earth-fixed coordinate system, For location information The differential of Differentiation that represents position information The error, is the velocity information of the inertial measurement unit in the Earth-centered Earth-fixed coordinate system, Display speed information The error, For speed information The differential of Differentiation of velocity information The error, is the attitude information of the inertial measurement unit, For posture information The differential of is the Earth's rotation angular velocity, is the rotation matrix from the carrier coordinate system to the Earth-centered Earth-fixed coordinate system, is the acceleration due to gravity, is the error of gravitational acceleration, is the velocity information in the carrier coordinate system, is the specific force in the carrier coordinate system, is the error of the specific force, is the angular velocity in the carrier coordinate system, is the error of the angular velocity in the carrier coordinate system, is the rotation matrix from the carrier coordinate system to the vehicle coordinate system, is the velocity information in the vehicle coordinate system, the superscript T represents the transposed matrix, The lever arm information from the inertial measurement unit to the driving wheel.

In an optional solution of the first aspect, the acquiring the non-holonomic constraint lateral constraint noise of the vehicle based on the lateral motion state parameter includes:

If the lateral motion state parameter is less than or equal to a first threshold, outputting a first non-holonomic constraint lateral constraint noise;

If the lateral motion state parameter is greater than the first threshold and less than or equal to a second threshold, outputting a second non-holonomic constraint lateral constraint noise;

If the lateral motion state parameter is greater than the second threshold, outputting a third non-holonomic constraint lateral constraint noise;

The first threshold is smaller than the second threshold.

In an optional solution of the first aspect, if the lateral motion state parameter is greater than the first threshold and less than or equal to a second threshold, outputting a second non-holonomic constraint lateral constraint noise comprises:

A linear operation is performed based on the difference between the lateral motion state parameter and the first threshold, and the difference between the first threshold and the second threshold, and the second non-holonomic constraint lateral constraint noise is output.

In an optional solution of the first aspect, outputting an observation noise matrix based on the preset constraint noise and the non-holonomic constraint lateral constraint noise includes:

The preset constraint noise includes the observation noise of the wheel speed measurement unit and the non-holonomic constraint vertical constraint noise, and an observation noise matrix is output based on the observation noise of the wheel speed measurement unit, the non-holonomic constraint vertical constraint noise and the non-holonomic constraint lateral constraint noise. :

;

in, is the observation noise of the wheel speed measurement unit, is the non-holonomic constraint vertical constraint noise, The non-holonomic constraints laterally constrain noise.

In a second aspect, an embodiment of the present application further provides an adaptive navigation device based on vehicle non-holonomic constraints, wherein the vehicle is equipped with an inertial measurement unit and a wheel speed measurement unit, and the device comprises:

a data acquisition unit, used to acquire first measurement data output by the inertial measurement unit, and also used to acquire second measurement data output by the wheel speed measurement unit;

an observation equation unit, configured to establish an observation equation of a vehicle state based on the first measurement data and the second measurement data;

an observation noise unit, configured to calculate a lateral motion state parameter of the vehicle based on the first measurement data and the second measurement data, obtain a non-holonomic constraint lateral constraint noise of the vehicle according to the lateral motion state parameter, and output an observation noise matrix based on a preset constraint noise and the non-holonomic constraint lateral constraint noise;

a compensation vector calculation unit, used for calculating an error compensation vector through the observation equation and the observation noise matrix, and for inputting the error compensation vector into the inertial measurement unit and the wheel speed measurement unit so that the inertial measurement unit and the wheel speed measurement unit update corresponding state parameters; and the adaptive navigation device performs navigation based on the inertial measurement unit and the wheel speed measurement unit after error compensation.

In a third aspect, an embodiment of the present application further provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the method provided in the first aspect of the embodiment of the present application or any one of the implementation methods of the first aspect is implemented.

In a fourth aspect, the present application also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method provided by the first aspect of the embodiment of the present application or any one of the implementations of the first aspect.

The beneficial effects brought about by the technical solutions provided by some embodiments of the present application include at least:

An adaptive navigation method based on vehicle non-completeness constraints is provided in an embodiment of the present application. The acceleration and angular velocity of the vehicle motion are obtained by first measurement data output by an inertial measurement unit, and the linear velocity of the vehicle motion is obtained by collecting and outputting the wheel speed by a wheel speed measurement unit. This method can realize real-time monitoring of the vehicle motion state, and obtain the non-completeness constraint lateral constraint noise through the lateral motion state parameters calculated by the first measurement data and the second measurement data, thereby avoiding the defect that the non-completeness constraint conditions of the related technology are too ideal and cannot be applied to navigation under complex urban road conditions. The error compensation vector of the inertial measurement unit and the wheel speed measurement unit can be calculated by combining the observation equation and the observation noise matrix obtained by the lateral constraint noise. The inertial measurement unit and the wheel speed measurement unit can be adaptively compensated during the vehicle driving process, so that the inertial measurement unit and the wheel speed measurement unit can output more accurate information, which is beneficial to improving the accuracy of navigation.

BRIEF DESCRIPTION OF THE DRAWINGS

In order to more clearly illustrate the technical solutions in the present application or related technologies, the drawings required for use in the embodiments or related technical descriptions are briefly introduced below. Obviously, the drawings described below are some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

FIG1 is a schematic diagram of the structure of a carrier according to an embodiment of the present application;

FIG2 is a flow chart of an adaptive navigation method based on vehicle non-holonomic constraints according to an embodiment of the present application;

FIG3 is a schematic diagram of the state of a carrier provided in an embodiment of the present application;

FIG4 is a schematic diagram of the structure of an adaptive navigation device based on vehicle non-holonomic constraints provided in an embodiment of the present application;

FIG. 5 is a schematic diagram of the structure of an electronic device provided in an embodiment of the present application.

Detailed ways

In order to make the purpose, technical solutions and advantages of this application clearer, the technical solutions in this application will be clearly and completely described below in conjunction with the drawings in this application. Obviously, the described embodiments are part of the embodiments of this application, not all of them. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

The terms "including" and "having" and any variations thereof in the specification and claims of this application and the above drawings are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device including a series of steps or modules is not limited to the listed steps or modules, but may optionally include steps or modules that are not listed, or may optionally include other steps or modules that are inherent to these processes, methods, products or devices.

It should be noted that the terms "first\second" involved in the present application are only used to distinguish similar objects, and do not represent a specific order for the objects. It is understandable that "first\second" can be interchanged with a specific order or sequence where permitted. It should be understood that the objects distinguished by "first\second" can be interchanged where appropriate, so that the embodiments of the present application described herein can be implemented in an order other than those described or illustrated herein.

Next, please refer to FIG1 , which is a schematic diagram of the architecture of a vehicle equipped with an adaptive navigation system provided by an exemplary embodiment of the present application. As shown in FIG1 , the vehicle 100 equipped with an adaptive navigation system includes a navigation module 110 and a computing module 120 .

The calculation module 120 may be a unit with computing capability such as an ECU electronic control unit or a vehicle chip mounted on the vehicle. A user version of the software may be installed in the calculation module 120 to obtain the first measurement data output by the inertial measurement unit 111 in the navigation module 110 and the second measurement data output by the wheel speed measurement unit 112; the calculation module 120 is also used to calculate the error compensation vector of the navigation module 110 based on the acquired data, and input the error compensation vector to the navigation module 110, so that the vehicle 100 can navigate through the navigation module 110 after error compensation.

It can be understood that the navigation module 110 includes an inertial measurement unit 111 and a wheel speed measurement unit 112. The inertial measurement unit 111 may include an accelerometer and a gyroscope. The accelerometer can output specific force, and the gyroscope can determine angular velocity. The wheel speed measurement unit 112 may include an odometer installed at the position of the vehicle driving wheel, or it may be in the form of a CAN bus and a wheel speed acquisition interface, which is used to measure the wheel speed of the driving wheel, that is, the linear velocity.

Among them, the calculation module 120 can also be connected to the Internet of Vehicles platform 130 through the network, including but not limited to uploading the first measurement data and the second measurement data obtained from the navigation module 110 to the Internet of Vehicles platform 130 through the network. The process of calculating the error compensation vector based on the first measurement data and the second measurement data can be completed in the calculation module 120 of the vehicle, and the calculation result can be fed back to the vehicle 100 after the calculation is completed by the Internet of Vehicles platform 130. It can also be calculated jointly by the calculation module 120 and the Internet of Vehicles platform 130, and this embodiment of the present application is not limited to this.

It can be understood that the Internet of Vehicles platform 130 is a cloud platform provided by the corresponding manufacturer of the vehicle 100 for providing Internet of Vehicles, cloud computing services, navigation and other functions for the corresponding type of vehicle, and can be a server, server cluster, etc. The number of vehicles 100 and Internet of Vehicles platforms in FIG. 1 is for illustration only. For example, but not limited to, the Internet of Vehicles platform 130 can be a server cluster composed of multiple servers, one Internet of Vehicles platform 130 can connect to multiple vehicles 100, and a single vehicle 100 can also request services from multiple Internet of Vehicles platforms 130.

The network may be a medium that provides a communication link between the cloud 120 and any terminal 110, or may be the Internet including network devices and transmission media, but is not limited thereto. The transmission medium may be a wired link, such as but not limited to coaxial cable, optical fiber, and digital subscriber line (DSL), or a wireless link, such as but not limited to wireless fidelity (WIFI), Bluetooth, and mobile device network.

The present application is described in detail below with reference to specific embodiments.

Next, in conjunction with FIG1 , an adaptive navigation method based on vehicle non-holonomic constraints performed by a vehicle computing module is taken as an example to introduce an adaptive navigation method based on vehicle non-holonomic constraints provided by an embodiment of the present application. Please refer to FIG2 for details, which shows a flow chart of an adaptive navigation method based on vehicle non-holonomic constraints provided by an embodiment of the present application. As shown in FIG2 , the method includes the following steps:

S201, obtaining measurement data, including:

The first measurement data output by the inertial measurement unit is obtained, and the second measurement data output by the wheel speed measurement unit is obtained.

The first measurement data output by the inertial measurement unit includes specific force and angular velocity, and the second measurement data output by the wheel speed measurement unit includes the wheel speed of the driving wheel of the vehicle.

Specifically, the inertial measurement unit generally obtains specific force through the accelerometer carried by the unit, and the specific force is used to represent the acceleration relative to the inertial reference frame. The angular velocity is generally measured through a gyroscope.

It should be noted that the first measurement data output by the inertial measurement unit is relative to the carrier coordinate system b (bodyframe), which is recorded as specific force and angular velocity .

Specifically, the wheel speed measurement unit may include an odometer (OD) installed on a driving wheel or a non-steering wheel of the vehicle, through which the wheel speed in the vehicle frame can be obtained. The wheel speed can also be obtained through the wheel speed acquisition interface on the CAN bus.

S202, output observation equation, including:

An observation equation of the vehicle state is established based on the first measurement data and the second measurement data.

Specifically, firstly, an earth-centered earth-fixed frame is selected as a reference frame, mechanical arrangement is performed based on the first measurement data, and the position and posture information of the inertial measurement unit is output. and angular velocity For mechanical arrangement, the error state differential equation of mechanical arrangement can be expressed as:

;

The position information of the inertial measurement unit includes: position ,speed ,attitude .

Furthermore, according to the relationship between the center of a circle and the linear velocity of a point on the circle in circular motion, the center velocity of the inertial measurement unit is converted to the wheel speed measurement unit, and the installation angle error between the carrier coordinate system b and the vehicle coordinate system v is calculated. Project the center velocity of the inertial measurement unit to the v system and apply the formula:

;

;

in, is the position information of the inertial measurement unit in the Earth-centered Earth-fixed coordinate system, For location information The differential of Differentiation that represents position information The error, is the velocity information of the inertial measurement unit in the Earth-centered Earth-fixed coordinate system, Display speed information The error, For speed information The differential of Differentiation of velocity information The error, is the attitude information of the inertial measurement unit, For posture information The differential of is the Earth's rotation angular velocity, is the rotation matrix from the carrier coordinate system to the Earth-centered Earth-fixed coordinate system, is the acceleration due to gravity, is the error of gravitational acceleration, is the velocity information in the carrier coordinate system, is the specific force in the carrier coordinate system, is the error of the specific force, is the angular velocity in the carrier coordinate system, is the error of the angular velocity in the carrier coordinate system, is the rotation matrix from the carrier coordinate system to the vehicle coordinate system, is the velocity information in the vehicle coordinate system, the superscript T represents the transposed matrix, The lever arm information from the inertial measurement unit to the driving wheel.

Specifically, the installation angle error between the carrier coordinate system b and the vehicle coordinate system v can be calibrated online or offline and expressed in the form of a rotation matrix, denoted as ; The lever arm information from the inertial measurement unit to the drive wheel can be measured before the vehicle moves The lever arm information can also be obtained by searching in the product manual and other description documents of the carrier. This application does not limit the method of obtaining the lever arm information and the installation angle error.

It should be noted that for the experimental vehicle, the position of the components may be changed before and after the experiment. The arm information can be measured according to the specific installation position. and installation angle error .

Furthermore, the wheel speed under the V system obtained by the wheel speed measurement unit can be Combined with the non-holistic constraint virtual observation value NHC, we get the observation value vector :

;

It should be noted that the non-holonomic constraint virtual observation value NHC introduced here presupposes that the lateral slip and vertical jump of the vehicle are almost zero during driving, and together with the observation value of the wheel speed measurement unit, the above observation value vector is obtained .

Furthermore, based on the observation vector and the speed of the IMU conversion to the V frame It is not difficult to obtain the observation equation for observing the motion of the vehicle. The embodiment of the present application does not limit the specific form of the observation equation. The observation equation is only used to characterize the motion state of the carrier, and the motion state may include position information, speed information, and posture information.

S203, outputting the observation noise matrix, including:

The lateral motion state parameters of the vehicle are calculated based on the first measurement data and the second measurement data, and the non-holonomic constraint lateral constraint noise of the vehicle is obtained according to the lateral motion state parameters. The observation noise matrix is output based on the preset constraint noise and the non-holonomic constraint lateral constraint noise.

It should be noted that, in the related art, the default lateral sliding constraint and vertical runout constraint of the general non-completeness constraint are 0. The present application innovatively introduces lateral motion state parameters to adjust the noise of the non-completeness constraint, thereby realizing the adaptive adjustment of INS/OD joint positioning and attitude determination, inertial sensor/wheel speed sensor motion state monitoring and non-completeness constraint noise.

It can be understood that, as shown in FIG3 , is the angle between the center axis of the vehicle and the forward direction, and yaw is the heading angle of the vehicle.

Understandably, it is generally believed that the short period of time when the odometer is observed ( ), the angle between the vehicle's center axis and the forward direction The change in is approximately equal to the change in the vehicle's heading angle yaw, that is:

;

in, correspond The angle between the vehicle's central axis and the forward direction at the moment, correspond The heading angle at the moment, correspond The angle between the vehicle's central axis and the forward direction at the moment, correspond The heading angle at the moment.

As shown in Figure 3, the state diagram based on the assumption that the non-holonomic constraint lateral zero speed is not satisfied when the vehicle turns, where O is the center of the turning circle, , are the actual speeds of the midpoints of the steering axle and the driving axle, respectively. , They are The forward and lateral components of the drive axle can be assumed to be the midpoint speed of the drive axle in a short period of time. The change in lateral speed of the driving wheel of the carrier is expressed as:

;

Furthermore, it can be considered that when the vehicle is traveling, the b system is approximately parallel to the xOy plane of the local horizontal coordinate system l system (Local Frame) of the road surface. Differentiate the left and right sides of the above equation and apply the formula:

;

Ignore the changes of the l system relative to the inertial coordinate system i system (inertial frame) in a short period of time, defined as , apply the formula:

;

In the formula, the symbol Indicates the definition of parameters. is the projection of the angular velocity of the carrier coordinate system b relative to the horizontal coordinate system l on the b system, for The z-axis component of is the projection of the angular velocity of the carrier coordinate system b relative to the inertial coordinate system i on the b system, for The z-axis component of is the projection of the angular velocity of the carrier coordinate system b relative to the horizontal coordinate system l on the b system, for The z-axis component of

That is, the lateral motion state parameter of the vehicle, which is used to characterize the intensity of the lateral motion of the vehicle. The larger the value, the greater the displacement of the lateral motion of the vehicle, and vice versa.

Further, the wheel speed measured by the wheel speed measurement unit is substituted into The calculation formula is:

;

Then, according to Determine the non-holonomic restraint lateral restraint noise of the vehicle.

In some embodiments, the actual lateral speed of the vehicle may be , the motion state is solved by using the vertical component of the gyroscope output of the inertial measurement unit and the output of the wheel speed measurement unit , and conduct time series analysis, according to Divide several value intervals to evaluate the vehicle's motion state, such as setting the first lateral constraint noise , third lateral restraint noise And the corresponding The first threshold , the second threshold According to the threshold set above, the lateral constraint noise of the non-holonomic constraint is:

;

Optionally, when The value is less than the first threshold When the lateral constraint noise of the nonholonomic constraint for , assuming that the vehicle is approximately traveling in a straight line, a preset smaller threshold noise is used Constrain it, that is, the first non-holonomic constraint is the lateral constraint noise; when The value is greater than the second threshold When the lateral constraint noise of the nonholonomic constraint for , it is believed that the vehicle is turning violently, and a preset larger threshold noise is used It is constrained, namely the third non-holonomic constraint is the lateral constraint noise.

It should be noted that when The value is greater than the first threshold Less than the second threshold When the carrier is considered to have a slight turn, the Linearly correlated noise To constrain it, it is necessary to combine the lateral motion state parameters The value of , the first threshold and the second threshold are linearly operated to calculate the second non-holonomic constraint lateral constraint noise.

Furthermore, the noise can be constrained laterally according to the determined non-holonomic constraints Covariance matrix Assign the value, and the observed noise is as follows:

;

in, is the observation noise of the wheel speed measurement unit, is the non-holonomic constraint vertical constraint noise, through It can characterize the noise in the observation of the carrier state.

in, , The preset value can be determined before performing the calculation, and can be obtained based on multiple experiments or by consulting the carrier-related instruction manual, or can be measured by specific components during the travel of the carrier. The embodiments of the present application are not limited to this.

It should be noted that steps S202 and S203 can be performed simultaneously or separately. Step S202 can be executed first to obtain the observation equation, or step S203 can be executed first to obtain the observation noise matrix, or they can be calculated simultaneously. This application does not limit the execution order of S202 and S203.

S204, calculating an error compensation vector through the observation equation and the observation noise matrix.

Specifically, according to the observation equation and observation noise matrix obtained in the previous steps, the errors of the corresponding parameters measured by the inertial measurement unit and the wheel speed measurement unit during the vehicle's driving process can be calculated, and the error compensation vector can be calculated to reduce the cumulative errors of the inertial measurement unit and the wheel speed measurement unit due to not taking into account the lateral constraints.

Furthermore, the error compensation vector is input to the inertial measurement unit and the wheel speed measurement unit, so that the inertial measurement unit and the wheel speed measurement unit update corresponding state parameters.

Specifically, the state parameters of the inertial measurement unit and the wheel speed measurement unit can be adjusted through the error compensation vector. The state parameters include but are not limited to the position, speed, attitude, zero bias, proportional factor, etc. of the inertial measurement unit and the proportional factor of the wheel speed measurement unit, etc., which are not limited to the embodiments of the present application.

Specifically, the error compensation vector is input to a filter including the state of the inertial measurement unit and the state of the wheel speed measurement unit, so that the filter updates the state parameters of the inertial measurement unit and the wheel speed measurement unit.

Further, after the error compensation vector is input into the inertial measurement unit and the wheel speed measurement unit, the state parameters of the inertial measurement unit and the wheel speed measurement unit are updated, and the process proceeds to step S201 to obtain the first measurement data output by the inertial measurement unit and the second measurement data output by the wheel speed measurement unit. This can reduce the error between the first measurement data and the second measurement data obtained by the inertial measurement unit and the wheel speed measurement unit in the next measurement, thereby improving the accuracy of inertial navigation and combined navigation using inertial navigation.

It should be noted that the method provided in the present application can be run all the time during the operation of the vehicle, and the non-complete constraint lateral constraint noise is continuously calculated during the driving process, and then the error compensation vector is continuously generated, so that the vehicle can adjust the state parameters of the inertial measurement unit and the wheel speed measurement unit in real time during the operation, and then realize the adaptive correction of the error according to the change of driving state, avoid error accumulation, thereby improving the measurement accuracy, which is conducive to improving the navigation accuracy.

The following are device embodiments of the present application, which can be used to execute the method embodiments of the present application. For details not disclosed in the device embodiments of the present application, please refer to the method embodiments of the present application.

Next, please refer to Figure 4, which is a schematic diagram of the structure of an adaptive navigation device based on vehicle non-integrity constraints provided by an exemplary embodiment of the present application. The device can be implemented as all or part of the terminal through software, hardware, or a combination of both, and can also be integrated on the server as an independent module. The adaptive navigation device based on vehicle non-integrity constraints in the embodiment of the present application can be applied to the vehicle side or the vehicle networking platform side. The vehicle is equipped with an inertial measurement unit and a wheel speed measurement unit. The device 400 includes a data acquisition unit 410, an observation equation unit 420, an observation noise unit 430, and a compensation vector calculation unit 440. Specifically:

The data acquisition unit 410 is used to obtain the first measurement data output by the inertial measurement unit, and is also used to obtain the second measurement data output by the wheel speed measurement unit;

The observation equation unit 420 is used to establish an observation equation of the vehicle state based on the first measurement data and the second measurement data;

The observation noise unit 430 is used to calculate the lateral motion state parameters of the vehicle based on the first measurement data and the second measurement data, obtain the non-holonomic constraint lateral constraint noise of the vehicle according to the lateral motion state parameters, and output the observation noise matrix based on the preset constraint noise and the non-holonomic constraint lateral constraint noise;

The compensation vector calculation unit 440 is used to calculate the error compensation vector through the observation equation and the observation noise matrix, and is also used to input the error compensation vector into the inertial measurement unit and the wheel speed measurement unit so that the inertial measurement unit and the wheel speed measurement unit update corresponding state parameters; the adaptive navigation device 400 navigates based on the inertial measurement unit and the wheel speed measurement unit after error compensation.

It should be noted that when the device 400 provided in the above embodiment executes the adaptive navigation method based on vehicle non-integrity constraints, only the division of the above functional modules is used as an example. In actual applications, the above functions can be assigned to different functional modules as needed, that is, the internal structure of the device is divided into different functional modules to complete all or part of the functions described above. In addition, the device provided in the above embodiment and the adaptive navigation method embodiment based on vehicle non-integrity constraints belong to the same concept. The implementation process is detailed in the method embodiment, which will not be repeated here.

An embodiment of the present application also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of any of the above-mentioned method embodiments when executing the program.

Please refer to FIG5 , which is a structural block diagram of an electronic device provided in an embodiment of the present application.

As shown in FIG5 , the electronic device 500 includes a processor 501 and a memory 502 .

In the embodiment of the present application, the processor 501 is the control center of the computer system, which can be a processor of a physical machine or a processor of a virtual machine. The processor 501 may include one or more processing cores, such as a 4-core processor, an 8-core processor, etc. The processor 501 may be implemented in at least one hardware form of DSP (Digital Signal Processing), FPGA (Field-Programmable Gate Array), and PLA (Programmable Logic Array).

The processor 501 may also include a main processor and a coprocessor. The main processor is a processor for processing data in the awake state, also called a CPU (Central Processing Unit); the coprocessor is a low-power processor for processing data in the standby state.

The memory 502 may include one or more computer-readable storage media, which may be non-transitory. The memory 502 may also include a high-speed random access memory and a non-volatile memory, such as one or more disk storage devices and flash memory storage devices. In some embodiments of the present application, the non-transitory computer-readable storage medium in the memory 502 is used to store at least one instruction, which is used to be executed by the processor 501 to implement the method in the embodiment of the present application.

In some embodiments, the electronic device 500 further includes: a peripheral device interface 503 and at least one peripheral device. The processor 501, the memory 502 and the peripheral device interface 503 can be connected via a bus or a signal line. Each peripheral device can be connected to the peripheral device interface 503 via a bus, a signal line or a circuit board. Specifically, the peripheral devices include: a display screen 504, a camera 505 and an audio circuit 506. The peripheral device interface 503 can be used to connect at least one peripheral device related to I/O (Input/Output) to the processor 501 and the memory 502.

In some embodiments of the present application, the processor 501, the memory 502, and the peripheral device interface 503 are integrated on the same chip or circuit board; in some other embodiments of the present application, any one or two of the processor 501, the memory 502, and the peripheral device interface 503 can be implemented on a separate chip or circuit board. This embodiment of the present application does not specifically limit this.

The display screen 504 is used to display a UI. The UI may include graphics, text, icons, videos, and any combination thereof. When the display screen 504 is a touch display screen, the display screen 504 also has the ability to collect touch signals on the surface or above the surface of the display screen 504. The touch signal may be input as a control signal to the processor 501 for processing. At this time, the display screen 504 may also be used to provide virtual buttons and/or virtual keyboards, also known as soft buttons and/or soft keyboards.

In some embodiments of the present application, the display screen 504 may be one, which is arranged on the front panel of the electronic device 500; in other embodiments of the present application, the display screen 504 may be at least two, which are arranged on different surfaces of the electronic device 500 or are folded; in still other embodiments of the present application, the display screen 504 may be a flexible display screen, which is arranged on a curved surface or a folded surface of the electronic device 500. Even more, the display screen 504 may be arranged in a non-rectangular irregular shape, that is, a special-shaped screen. The display screen 504 may be made of materials such as LCD (Liquid Crystal Display), OLED (Organic Light-Emitting Diode) and the like.

The camera 505 is used to capture images or videos. Optionally, the camera 505 includes a front camera and a rear camera. Usually, the front camera is arranged on the front panel of the electronic device, and the rear camera is arranged on the back of the electronic device. In some embodiments, there are at least two rear cameras, which are any one of a main camera, a depth of field camera, a wide-angle camera, and a telephoto camera, so as to realize the fusion of the main camera and the depth of field camera to realize the background blur function, the fusion of the main camera and the wide-angle camera to realize the panoramic shooting and VR (Virtual Reality) shooting function or other fusion shooting functions. In some embodiments of the present application, the camera 505 may also include a flash. The flash can be a monochrome temperature flash or a dual-color temperature flash. The dual-color temperature flash refers to a combination of a warm light flash and a cold light flash, which can be used for light compensation at different color temperatures.

The audio circuit 506 may include a microphone and a speaker. The microphone is used to collect sound waves from the user and the environment, and convert the sound waves into electrical signals and input them into the processor 501 for processing. For the purpose of stereo sound collection or reduction, there may be multiple microphones, which are respectively arranged at different parts of the electronic device 500. The microphone may also be an array microphone or an omnidirectional collection microphone.

The power supply 507 is used to power various components in the electronic device 500. The power supply 507 can be an alternating current, a direct current, a disposable battery, or a rechargeable battery. When the power supply 507 includes a rechargeable battery, the rechargeable battery can be a wired rechargeable battery or a wireless rechargeable battery. A wired rechargeable battery is a battery that is charged through a wired line, and a wireless rechargeable battery is a battery that is charged through a wireless coil. The rechargeable battery can also be used to support fast charging technology.

The electronic device structure block diagram shown in the embodiment of the present application does not constitute a limitation on the electronic device 500. The electronic device 500 may include more or fewer components than shown in the figure, or combine certain components, or adopt a different component arrangement.

The present application also provides a computer-readable storage medium on which a computer program is stored, and when the program is executed by a processor, the steps of the method of any of the above embodiments are implemented. The computer-readable storage medium may include, but is not limited to, any type of disk, including a floppy disk, an optical disk, a DVD, a CD-ROM, a micro drive, and a magneto-optical disk, a ROM, a RAM, an EPROM, an EEPROM, a DRAM, a VRAM, a flash memory device, a magnetic card or an optical card, a nanosystem (including a molecular memory IC), or any type of medium or device suitable for storing instructions and/or data.

Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, can also be implemented by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the relevant technology can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM/RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiment.

Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit it. Although the present application has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some of the technical features therein with equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims (10)

1. An adaptive navigation method based on carrier non-integrity constraints, characterized in that the carrier is mounted with an inertial measurement unit and a wheel speed measurement unit, the adaptive navigation method comprising:

Acquiring first measurement data output by the inertia measurement unit and second measurement data output by the wheel speed measurement unit;

establishing a vehicle state observation equation based on the first measurement data and the second measurement data;

Calculating a lateral motion state parameter of the carrier based on the first measurement data and the second measurement data, acquiring incomplete constraint lateral constraint noise of the carrier according to the lateral motion state parameter, and outputting an observation noise matrix based on preset constraint noise and the incomplete constraint lateral constraint noise;

calculating to obtain an error compensation vector through the observation equation and the observation noise matrix;

and inputting the error compensation vector to the inertia measurement unit and the wheel speed measurement unit so that the inertia measurement unit and the wheel speed measurement unit update corresponding state parameters, and transferring to the step of acquiring first measurement data output by the inertia measurement unit and acquiring second measurement data output by the wheel speed measurement unit.

2. The method of claim 1, wherein the first measurement data comprises a specific force and an angular velocity and the second measurement data comprises a wheel speed of a drive wheel of the vehicle.

3. The method of claim 2, wherein the establishing an observation equation for a vehicle state based on the first measurement data and the second measurement data comprises:

selecting a geocentric earth fixed coordinate system as a reference system, mechanically arranging based on the first measurement data, and outputting pose information of the inertial measurement unit; wherein the pose information comprises position information, speed information and pose information;

Projecting the speed information to a carrier coordinate system, and outputting the speed information of the inertial measurement unit under the carrier coordinate system;

Constraining a virtual observer vector based on the wheel speed of the drive wheel and the non-integrity;

And establishing the observation equation based on the speed information in the carrier coordinate system and the observation value vector.

4. A method according to claim 3, wherein the selecting a geocentric, fixed coordinate system as a reference system is mechanically programmed based on the first measurement data by applying the formula:

；

outputting pose information of the inertial measurement unit including position information Speed informationAnd gesture information；

The speed information is projected to a carrier coordinate system, and the speed information under the carrier coordinate system is outputThe formula applies:

；

；

Wherein, For the position information of the inertial measurement unit in a geocentric fixed coordinate system,To locate informationIs used for the differentiation of the (c) and (d),Differentiation of representing position informationIs used for the error of (a),For velocity information of the inertial measurement unit in a geocentric fixed coordinate system,Representing speed informationIs used for the error of (a),For speed informationIs used for the differentiation of the (c) and (d),Differentiation of information representing speedIs used for the error of (a),For the attitude information of the inertial measurement unit,For gesture informationIs used for the differentiation of the (c) and (d),Is the rotational angular velocity of the earth,For a rotation matrix of the carrier coordinate system to the geocentric fixed coordinate system,The acceleration of the gravity is that,The error in the acceleration of the gravity,For the speed information in the carrier coordinate system,For the specific force in the carrier coordinate system,As an error in the specific force of the material,For the angular velocity in the coordinate system of the carrier,Is the error in angular velocity in the carrier coordinate system,For a rotation matrix from carrier coordinate system to carrier coordinate system,For speed information in the carrier coordinate system, the superscript T represents the transposed matrix,Lever arm information for the inertial measurement unit to the drive wheel.

5. The method of claim 2, wherein the acquiring non-integrity-constrained lateral constraint noise of the vehicle based on the lateral motion state parameter comprises:

Outputting first non-integrity-constrained lateral constraint noise if the lateral motion state parameter is less than or equal to a first threshold;

Outputting second non-integrity-constrained lateral constraint noise if the lateral motion state parameter is greater than the first threshold and less than or equal to a second threshold;

Outputting third non-integrity-constrained lateral constraint noise if the lateral motion state parameter is greater than the second threshold;

Wherein the first threshold is less than the second threshold.

6. The method of claim 5, wherein outputting a second non-integrity-constrained lateral constraint noise if the lateral motion state parameter is greater than the first threshold and less than or equal to a second threshold comprises:

And carrying out linear operation based on the difference value between the lateral motion state parameter and the first threshold value and the difference value between the first threshold value and the second threshold value, and outputting the second incomplete constraint lateral constraint noise.

7. The method of any of claims 1-6, wherein outputting an observed noise matrix based on a preset constrained noise and the non-integrity constrained lateral constrained noise comprises:

the obtaining of the preset constraint noise comprises the observation noise and the incomplete constraint vertical constraint noise of the wheel speed measuring unit, and the observation noise matrix is output based on the observation noise, the incomplete constraint vertical constraint noise and the incomplete constraint lateral constraint noise of the wheel speed measuring unit ：

；

Wherein,For the observation noise of the wheel speed measurement unit,Vertical constraint noise for the non-integrity constraint,And constraining noise laterally for the non-integrity constraint.

8. An adaptive navigation device based on carrier non-integrity constraints, wherein the carrier is mounted with an inertial measurement unit and a wheel speed measurement unit, the device comprising:

The data acquisition unit is used for acquiring first measurement data output by the inertia measurement unit and second measurement data output by the wheel speed measurement unit;

an observation equation unit for establishing an observation equation of a vehicle state based on the first measurement data and the second measurement data;

The observation noise unit is used for calculating lateral motion state parameters of the carrier based on the first measurement data and the second measurement data, acquiring incomplete constraint lateral constraint noise of the carrier according to the lateral motion state parameters, and outputting an observation noise matrix based on preset constraint noise and the incomplete constraint lateral constraint noise;

The compensation vector calculation unit is used for calculating an error compensation vector through the observation equation and the observation noise matrix, and inputting the error compensation vector into the inertia measurement unit and the wheel speed measurement unit so as to enable the inertia measurement unit and the wheel speed measurement unit to update corresponding state parameters; the self-adaptive navigation device carries out navigation based on the inertial measurement unit and the wheel speed measurement unit after error compensation.

9. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the processor implements the steps of the method according to any one of claims 1 to 7 when the program is executed by the processor.

10. A non-transitory computer readable storage medium, on which a computer program is stored, characterized in that the computer program, when being executed by a processor, implements the steps of the method according to any one of claims 1 to 7.