Course angle initialization method, electronic equipment and storage medium

Abstract

The invention provides a course angle initialization method, electronic equipment and a storage medium. In the method, the combined navigation system establishes a second coordinate system based on a first coordinate system when target equipment is static, so that a course angle obtained based on the first coordinate system and the second coordinate system is 0. Error correction is carried out on the INS of the integrated navigation system in a static state, and when the target equipment starts to move, the integrated navigation system determines the movement direction of the target equipment based on the speed information or the position information calculated by the INS so as to initialize the course angle of the target equipment according to the movement direction of the target equipment. The INS performs error correction when the target equipment is in the static state, so that the speed information or the position information calculated by the INS is more accurate, the determination of the movement direction is more accurate, and the correctness of course angle initialization is ensured on the basis of not limiting the movement direction of the target equipment to improve the user experience.

Description

Technical Field

[0001] The present application relates to the field of navigation technology, and in particular to a heading angle initialization method, an electronic device and a storage medium. Background Art

[0002] The Global Navigation Satellite System (GNSS) and the Inertial Navigation System (INS) can be applied to various electronic equipment operating scenarios. Before the combined navigation system based on INS and GNSS works, the INS needs to be initialized. Specifically, the initialization of INS includes the initialization of position, speed and attitude. In the prior art, for the initialization of position and speed, the position information and speed information output by GNSS can be used as the initial position information and initial speed information of INS; the initialization of attitude includes the initialization of horizontal attitude and the initialization of heading angle. Among them, the initialization of horizontal attitude can be achieved based on the acceleration information of INS. For the initialization of heading angle, since the accuracy of the inertial measurement unit (IMU) based on the micro-electro-mechanical system (MEMS) in INS is limited, it is necessary to use the measurement information of GNSS for initialization. For example, the heading angle of INS can be initialized by relying on the track angle calculated by the Doppler velocity measured by GNSS.

[0003] Among them, the process of initializing the heading angle of INS using the track angle measured by GNSS has requirements on the movement direction of the electronic device. In order to ensure the correctness of the heading angle initialization, the electronic device needs to move forward, which is not conducive to user experience. In other existing technologies, in order to address the above defects, the movement direction of the vehicle can be determined by acceleration information to assist in the initialization of the heading angle.

[0004] However, when the electronic device is agricultural machinery (abbreviated as agricultural machinery), since the agricultural machinery has a large vibration amplitude and its movement speed is low, the acceleration information during the movement of the agricultural machinery is not accurate, which may lead to errors in the judgment of the movement direction, thereby causing the problem of heading initialization error. Summary of the invention

[0005] The present application provides a heading angle initialization method, which corrects the INS error caused by the vibration of the target device when the target device is in a stationary state, and determines the movement direction based on the speed information or position information solved by the INS after the error correction to achieve the initialization of the heading angle. In this way, the accuracy of the heading angle initialization is guaranteed on the basis of not limiting the movement direction of the target device to improve the user experience.

[0006] In a first aspect, the present application provides a heading angle initialization method, which is applied to an integrated navigation system, and the integrated navigation system is used to perform positioning and solving for a target device. The method comprises: when the target device is in a stationary state, establishing a second coordinate system based on a first coordinate system of the target device in the stationary state; in the stationary state, the y-axis of the first coordinate system coincides with that of the second coordinate system, the positive direction of the y-axis of the first coordinate system points to the front direction of the target device, and the positive direction of the y-axis of the second coordinate system is used to point to the north; wherein, when the y-axis of the first coordinate system coincides with that of the second coordinate system, the positive direction of the y-axis of the first coordinate system points to the front direction of the target device, and the positive direction of the y-axis of the second coordinate system is used to point to the north, the heading angle of the target device is 0; estimating the inertial navigation system in the integrated navigation system. The method comprises the steps of: calculating a state quantity of an INS of the integrated navigation system, and correcting an error of the INS in the static state based on the currently estimated state quantity; wherein the state quantity includes a three-dimensional position error of the target device in the second coordinate system, a three-dimensional velocity error of the target device in the second coordinate system, a three-dimensional attitude error of the target device in the second coordinate system, a three-dimensional constant drift of a gyroscope of the INS in the first coordinate system, and a three-dimensional constant zero bias of an accelerometer of the INS in the first coordinate system; based on determining that the target device starts to move, determining a movement direction of the target device according to speed information or position information solved by the INS, wherein the movement direction is forward movement or backward movement; and determining an initial heading angle of the target device based on the movement direction and a track angle measured by a global navigation satellite system (GNSS) in the integrated navigation system.

[0007] In some embodiments, the initial heading angle of the target device is determined based on the movement direction and the track angle measured by GNSS in the combined navigation system, including: when the movement direction is forward movement, determining the track angle measured by GNSS as the initial heading angle; when the movement direction is backward movement, determining the initial heading angle based on the track angle measured by GNSS and the angle difference between forward movement and backward movement.

[0008] In some embodiments, the speed information includes a first speed vector of the target device in a second coordinate system, and determining the movement direction of the target device according to the speed information includes: determining the forward speed of the target device in the first coordinate system in a current motion state based on the first speed vector of the target device in the second coordinate system; wherein the first speed vector is used to indicate the eastward speed, northward speed and celestial speed of the target device; when the forward speed is greater than a first threshold, determining that the movement direction of the target device is forward movement; wherein the first threshold is a preset positive number; when the forward speed is less than the opposite of the first threshold, determining that the movement direction of the target device is backward movement.

[0009] In some embodiments, determining the forward speed of the target device in the first coordinate system in the current motion state based on the first speed vector of the target device in the second coordinate system includes: determining the second speed vector of the target device in the first coordinate system in the current motion state based on the first speed vector and the transformation matrix; the second speed vector includes the right speed, forward speed and azimuth speed of the target device; wherein the transformation matrix is ​​used to characterize the posture conversion relationship between the first coordinate system and the second coordinate system of the target device in the current motion state.

[0010] In some embodiments, the position information includes a first position vector of the current target device in the second coordinate system, and determining the movement direction of the target device based on the position information includes: determining a vector difference between the first position vector and a second position vector of the target device in the second coordinate system when in the stationary state; determining that the movement direction of the target device is forward movement based on the vector difference being greater than a second threshold; wherein the second threshold is a preset positive number; and determining that the movement direction of the target device is backward movement based on the vector difference being less than the opposite number of the second threshold.

[0011] In some embodiments, the method also includes: obtaining an intermediate result of estimating the state quantity; determining whether the target device starts moving based on the intermediate result; if the target device has not started moving, returning to execute the operations of estimating the state quantity of the INS in the combined navigation system, correcting the error of the INS in the stationary state based on the state quantity, and obtaining an intermediate result of the estimated state quantity, until it is determined that the target device starts moving based on the last intermediate result obtained.

[0012] In some embodiments, in the current cycle, the state quantity of the INS in the integrated navigation system is estimated, including: determining the one-step prediction state matrix of the current cycle based on the initial state quantity of the INS or the state quantity estimated in the previous cycle, and the state transfer matrix; determining the one-step prediction mean square error matrix of the current cycle based on the state transfer matrix, the mean square error matrix of the state quantity in the previous cycle, the noise input matrix in the previous cycle, and the system noise variance matrix in the previous cycle; determining the filter gain in the current cycle based on the one-step prediction mean square error matrix of the current cycle, the measurement matrix corresponding to the measurement of the current cycle, and the measurement noise variance matrix; the measurement includes position measurement and heading measurement; the position measurement is based on the position information measured by the INS The heading vector measurement is determined based on the heading of the INS in the current cycle and the initial heading; the state quantity of the INS in the current cycle is determined based on the one-step predicted state matrix in the current cycle, the filter gain in the current cycle, the quantity measurement in the current cycle, and the measurement matrix corresponding to the quantity measurement in the current cycle; the mean square error matrix of the state quantity of the INS in the next cycle is determined based on the filter gain in the current cycle, the measurement matrix corresponding to the quantity measurement in the current cycle, and the one-step predicted mean square error matrix of the current cycle; the mean square error matrix of the INS in the next cycle is used to determine the filter gain in the next cycle, thereby determining the state quantity of the INS in the next cycle.

[0013] In some embodiments, the intermediate results include the one-step prediction state matrix within the current cycle and the one-step prediction mean square error matrix within the current cycle; determining whether the target device starts moving based on the intermediate results includes: determining the new information of the chi-square test based on the one-step prediction state matrix within the current cycle, the quantity measurement within the current cycle, and the measurement matrix within the current cycle; the new information of the chi-square test obeys the normal distribution with a position parameter of 0 and a scale parameter of a first matrix; wherein the first matrix is ​​determined based on the measurement matrix within the current cycle, the one-step prediction mean square error matrix within the current cycle and the measurement noise variance matrix corresponding to the current cycle; when the ratio of the new information of the chi-square test to the diagonal matrix of the first matrix is ​​greater than a preset chi-square test threshold, it is determined that the target device starts moving; when the ratio of the new information of the chi-square test to the diagonal matrix of the first matrix is ​​less than or equal to the chi-square test threshold, it is determined that the target device has not started moving.

[0014] In a second aspect, the present application provides a heading angle initialization device, including: a module used for the method described in the first aspect and its embodiments.

[0015] In a third aspect, the present application provides a chip comprising an interface circuit and a logic circuit, wherein the interface circuit is used to receive signals from other chips outside the chip and transmit them to the logic circuit, or to send signals from the logic circuit to other chips outside the chip, and the logic circuit is used to implement the method described in the first aspect and its embodiments.

[0016] In a fourth aspect, the present application provides an electronic device comprising a memory and a processor, wherein the memory stores a computer program executable on the processor, and wherein the processor implements the method described in the first aspect and its embodiments when executing the program.

[0017] In a fifth aspect, the present application provides a storage medium having a computer program stored thereon, which, when executed by a processor, implements the method described in the first aspect and its embodiments.

[0018] In a sixth aspect, the present application provides a computer program product comprising instructions, which, when executed on a computer, enables the computer to execute the steps of the method of the first aspect and its embodiments.

[0019] The present application provides a heading angle initialization method, an electronic device and a storage medium; in the method, the combined navigation system establishes a second coordinate system based on the first coordinate system when the target device is stationary, so that the heading angle obtained based on the first coordinate system and the second coordinate system is 0. Then, the INS is error corrected in a stationary state, and when the target device starts to move, the movement direction of the target device is determined based on the speed information or position information solved by the INS, so as to initialize the heading angle of the target device according to the movement direction of the target device. Since the INS performs error correction when the target device is in a stationary state, the speed information or position information solved by the INS is more accurate, so that the determination of the movement direction is more accurate, so that the accuracy of the heading angle initialization is guaranteed on the basis of not limiting the movement direction of the target device to improve the user experience. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 A flowchart of a method for initializing a heading angle provided in an embodiment of the present application;

[0021] Figure 2A A schematic diagram of a target device and a corresponding coordinate system provided in an embodiment of the present application;

[0022] Figure 2B A flowchart of a method for initializing a heading angle provided in an embodiment of the present application;

[0023] Figure 3 A flowchart of a method for initializing a heading angle provided in an embodiment of the present application;

[0024] Figure 4 A flowchart of a method for initializing a heading angle provided in an embodiment of the present application;

[0025] Figure 5 A schematic diagram of the target device provided in an embodiment of the present application moving in a second coordinate system;

[0026] Figure 6 A flowchart of a method for initializing a heading angle provided in an embodiment of the present application;

[0027] Figure 7 A flowchart of a method for initializing a heading angle provided in an embodiment of the present application;

[0028] Figure 8 A structural diagram of a heading angle initialization device provided in an embodiment of the present application;

[0029] Fig. 9 A schematic diagram of the structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0030] In this application, "at least one" means one or more, and "plurality" means two or more. "And / or" describes the association relationship of associated objects, indicating that three relationships may exist. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone, where A and B can be singular or plural. The character " / " generally indicates that the associated objects before and after are in an "or" relationship. "At least one of the following" or similar expressions refers to any combination of these items, including any combination of single items or plural items. For example, at least one of a, b, or c alone can represent: a, b, c, a and b, a and c, or a, b, and c, where a, b, and c can be single or multiple. In addition, the terms "first" and "second" are used for descriptive purposes only and cannot be understood as indicating or implying relative importance.

[0031] The directions or positional relationships indicated by terms such as "center", "longitudinal", "lateral", "up", "down", "left", "right", "front", and "back" are based on the directions or positional relationships shown in the accompanying drawings and are only for the convenience of describing the present application and simplifying the description. They do not indicate or imply that the device or element referred to must have a specific direction, be constructed and operated in a specific direction, and therefore should not be understood as a limitation on the present application.

[0032] The terms "connected" and "connected" should be understood in a broad sense. For example, the "connected" or "connected" of a circuit structure can refer to not only physical connection, but also electrical connection or signal connection. For example, it can be directly connected, that is, physically connected, or indirectly connected through at least one intermediate element, as long as the circuit is connected, or it can be the internal connection of two elements; signal connection can refer to signal connection through a circuit or through a media medium, such as radio waves. For ordinary technicians in this field, the specific meanings of the above terms in this application can be understood according to specific circumstances.

[0033] The INS and GNSS combined navigation system can be applied to agricultural machinery equipment (hereinafter referred to as agricultural machinery). The agricultural machinery combined navigation system based on INS and GNSS needs to be initialized before working. Specifically, the position, velocity and attitude of the IMU in the agricultural machinery navigation system are initialized. Among them, the initialization of the IMU attitude is the key to the initialization of the combined navigation system. The initialization of the horizontal attitude depends on the acceleration information. Due to the limited accuracy of the MEMS-IMU, the initialization of the heading needs to rely on GNSS related information.

[0034] The initialization of IMU heading mainly depends on the track angle obtained by Doppler velocity calculation of GNSS.

[0035] Among them, the heading angle of the IMU refers to the angle between the forward direction of the agricultural machinery and the north direction.

[0036] The GNSS track angle refers to the angle between the trajectory of agricultural machinery and the north direction.

[0037] Agricultural machinery has the characteristics of slow movement speed and large vibration amplitude, and the difficulty of combined navigation in the operation scenario of agricultural machinery is relatively high. Agricultural machinery may move forward and backward during daily operation. When the agricultural machinery moves forward, the heading angle of the IMU is the same as the track angle of the GNSS. When the agricultural machinery moves backward, the heading angle of the INU differs by 180° from the track angle of the GNSS. In the prior art, forward acceleration can be used to assist in determining the driving direction of the vehicle to help initialize the heading angle.

[0038] Specifically, this type of technology requires a preliminary estimation of the accelerometer zero bias at rest, and then the forward acceleration information is used to subtract its zero bias and then integrated to calculate the forward speed during the driving process of the agricultural machinery. If the forward speed is greater than the positive set threshold, it is judged to be forward, and if the forward speed is less than the negative set threshold, it is judged to be backward. Next, according to the judgment result of forward or backward, the track angle calculated by the Doppler velocity of GNSS is used to initialize the heading angle. If the agricultural machinery is moving forward, the heading angle of INS is the same as the track angle of GNSS. If the agricultural machinery is moving backward, the difference between the track angle and the heading angle needs to be 180 degrees.

[0039] The existing initialization process for agricultural machinery navigation systems requires the vehicle to move forward to ensure the correctness of the heading angle initialization, which is not conducive to the user experience. Some rely on acceleration information to determine whether the vehicle is moving forward or backward to assist in the initialization of the heading angle, but this method has low stability. Because the agricultural machinery environment has large vibrations, it is difficult to obtain the acceleration of the vehicle moving forward or backward, and the flatness of the road surface will also affect the judgment result. Therefore, problems such as initialization failure are prone to occur.

[0040] In order to solve the problem of heading initialization in typical application scenarios where agricultural machinery has a high vibration amplitude and a slow moving speed during operation, the present application provides a heading initialization method. The method determines the heading angle to be 0 when the target device is in a stationary state, and then periodically corrects the INS error. The movement direction is judged based on the corrected intermediate result satisfying the preset conditions, and the heading angle of the target device is initialized based on the movement direction of the target device and the GNSS track. In this way, the accuracy of the heading angle initialization is guaranteed without limiting the movement direction of the target device to initialize the heading angle.

[0041] The target equipment in this application is not limited and can be industrial equipment, mechanical equipment, etc.

[0042] The heading angle initialization method of the present application is applied to a GNSS and INS combined navigation system, which can be deployed on a target device, on the cloud, or on other devices different from the target device.

[0043] The INS in the present application may be a strap-down inertial navigation system (SINS). Specifically, an IMU (including a gyroscope and an accelerometer) is installed on the target device.

[0044] Figure 1 This is a flowchart of the method for initializing the heading angle provided in the embodiment of the present application. Figure 1 As shown, the method includes:

[0045] S101 . When a target device is in a stationary state, establish a second coordinate system based on a first coordinate system of the target device in the stationary state.

[0046] The present application does not limit the conditions for determining whether the target device is in a stationary state. In some embodiments, whether the target device is in a stationary state can be determined based on at least one of the fixed solution position information, floating point solution position information, speed information, gyro change information and accelerometer change information of the GNSS.

[0047] Specifically, if the fixed solution position information and / or the floating point solution position information does not change within a preset time, and at least one of the speed information, the gyro change information of the IMU, and the accelerometer change information is 0, it is determined that the target device is in a stationary state.

[0048] The origin of the first coordinate system is located on the target device. In order to more clearly illustrate the coordinate system involved in this application, Figure 2A The coordinate system involved in this application is explained.

[0049] Figure 2A Schematic diagram of the target device and the corresponding coordinate system provided in the embodiment of the present application. Figure 2A As shown, the integrated navigation system requires the IMU coordinate system to coincide with the vehicle body coordinate system when installed on the target device.

[0050] The first coordinate system includes the above-mentioned IMU coordinate system and the vehicle body coordinate system.

[0051] Specifically, the IMU coordinate system and the vehicle body coordinate system are both right front upper coordinate systems. In some embodiments, the origins of the IMU coordinate system and the vehicle body coordinate system are different. The origin of the IMU coordinate system is located on the IMU, for example, it can be the center of the IMU; the center of the vehicle body coordinate system is located on the target device, for example, the center of the target device. In other embodiments, the origins of the IMU coordinate system and the vehicle body coordinate system are the same.

[0052] The IMU coordinate system and the vehicle body coordinate system include an x-axis, a y-axis and a z-axis; the x-axis, y-axis and z-axis of the IMU coordinate system and the vehicle body coordinate system are parallel, and the positive directions of the x-axis, y-axis and z-axis of the IMU coordinate system and the vehicle body coordinate system are consistent.

[0053] Specifically, in the IMU coordinate system and the vehicle coordinate system, the positive direction of the x-axis points to the right of the target device, the positive direction of the y-axis points to the front of the target device, and the positive direction of the z-axis points to the top. The front of the target device can be defined as the front of the front of the target device, that is, the positive direction of the y-axis of the vehicle coordinate system or the IMU coordinate system is consistent with the forward movement direction of the target device.

[0054] The second coordinate system in the present application may be specifically established based on the vehicle body coordinate system.

[0055] Specifically, the origin of the second coordinate system constructed in the present application is consistent with the origin of the vehicle body coordinate system. The x-axis, y-axis and z-axis coincide. However, the positive directions of the x-axis, y-axis and z-axis of the second coordinate system and the vehicle body coordinate system are different.

[0056] Specifically, the positive direction of the x-axis of the second coordinate system is used to point to the due east, the positive direction of the y-axis is used to point to the due north (also described as north), and the z-axis is used to point to the due upwards.

[0057] It can be understood that the positive direction of the y-axis of the second coordinate system established according to the above method is consistent with the forward direction of the target device. In this case, the direction pointed by the positive direction of the y-axis is determined as the north.

[0058] It should be noted that in this application, the y-axis does not necessarily point to the direction of the real North Pole of the Earth. In this application, the positive direction of the y-axis is defined as pointing to the north, rather than the positive direction of the y-axis actually pointing to the north. Therefore, it can also be said that the positive direction of the y-axis is used to point to the north.

[0059] As described above, the heading angle refers to the angle between the target device's forward direction (i.e., the target device's forward movement direction or the target device's forward direction) and the true north direction. When the positive direction of the y-axis is used to point to the north, the heading angle determined by the target device based on the first coordinate system and the second coordinate system is 0.

[0060] It should be noted that the second coordinate system is established based on the first coordinate system in a stationary state. When the first coordinate system is in a stationary state, the x-axis, y-axis and z-axis are almost unchanged. When the first coordinate system changes from a stationary state to a moving state, the x-axis, y-axis and z-axis will also change. However, the second coordinate system will not change after it is established.

[0061] S102: Estimate the state quantity of the INS and correct the error of the INS in the static state based on the currently estimated state quantity.

[0062] The state quantities of INS include the three-dimensional position error of the target device in the second coordinate system, the three-dimensional velocity error of the target device in the second coordinate system, the three-dimensional attitude error of the target device in the second coordinate system, the three-dimensional constant drift of the INS's gyroscope in the first coordinate system, and the three-dimensional constant zero bias of the INS's accelerometer in the first coordinate system.

[0063] The above error of the target device in the second coordinate system can be specifically characterized by the above error of the IMU in the second coordinate system.

[0064] It can be understood that when the target device is in a stationary state, the velocity of the target device is 0. In this case, a zero velocity update (ZUPT) operation can be performed on the target device.

[0065] Specifically, the speed calculated by INS can be used as the observed value of the speed error of the integrated navigation system, the state quantity can be estimated periodically, and the navigation parameter error inside the INS system can be corrected using the most recently estimated state quantity feedback to complete the static zero-speed correction.

[0066] The method for periodically estimating INS state quantities is as follows Figure 2B The embodiments are described in detail and will not be described in detail here.

[0067] S103: Based on determining that the target device starts to move, determine the movement direction of the target device according to the speed information or position information obtained by INS solution; wherein the movement direction is forward movement or backward movement.

[0068] It should be noted that when the target device starts to move, the INS pure inertial navigation solution operation is performed, that is, the moving direction of the target device is determined only according to the speed information or position information solved by the INS.

[0069] After the heading angle is initialized, the INS pure inertial navigation solution operation is performed on the target device, and the INS uses the second coordinate system to locate the target device.

[0070] S104: Determine an initial heading angle of the target device based on the moving direction of the target device and the track angle obtained by GNSS measurement.

[0071] Specifically, if the direction of movement determined based on the intermediate result is forward movement, it means that the heading angle and the track angle of the target device are the same. Specifically, the directions corresponding to the heading angle and the track angle are the same. Then the track angle measured by GNSS is determined as the initial heading angle of the target device;

[0072] If the movement direction determined based on the intermediate result is backward movement, it means that the heading angle and the track angle of the target device differ by 180°. The initial heading angle of the target device is obtained by adding the track angle to 180°.

[0073] The heading angle initialization method provided by the present application is that when the target device is stationary, a second coordinate system is established based on the first coordinate system, so that the heading angle obtained based on the first coordinate system and the second coordinate system is 0. Then, the INS of the combined navigation system is error corrected in a stationary state, and when the target device starts to move, the movement direction of the target device is determined based on the speed information or position information solved by the INS, so as to initialize the heading angle of the target device according to the movement direction of the target device. Since the INS performs error correction when the target device is stationary, the speed information or position information solved by the INS is more accurate, so that the determination of the movement direction is more accurate, so that the accuracy of the heading angle initialization is guaranteed without limiting the movement direction of the target device to improve the user experience.

[0074] As mentioned above, the integrated navigation system periodically estimates the state of the INS and corrects the INS error. Figure 2B The embodiment further illustrates the way in which the integrated navigation system estimates the INS state quantity.

[0075] An embodiment of the present application provides a heading angle initialization method, which is used to illustrate a specific implementation method of estimating an INS state quantity of an integrated navigation system.

[0076] Figure 2B The following is a flowchart of the method for initializing the heading angle provided in this application; Figure 2B As shown, S102 can be implemented by following steps a1 to a2:

[0077] Step a1: estimate the state quantity of INS, correct the error of INS in the static state based on the current estimated state quantity, and obtain the intermediate result of the current estimated state quantity.

[0078] As described above, before the target device starts to move, the state quantity of the INS is estimated periodically. Step a1 is executed for one cycle of estimating the state quantity of the INS.

[0079] Step a2: determine whether the target device starts to move based on the intermediate result. If so, execute S103; otherwise, execute step a1.

[0080] In some embodiments, the integrated navigation system may use a Kalman filter algorithm to estimate the INS state quantity. In the process of estimating the INS state quantity, the intermediate result obtained obeys a normal distribution. Therefore, a chi-square test may be used to determine whether the target device has started to move based on the intermediate result. The method for determining whether the target device has started to move is shown in the following embodiment and will not be described in detail here.

[0081] If the target device starts to move, then the next step can be performed, and S103 can be executed to determine the moving direction of the target device. If the target device is still in a stationary state, that is, it has not started to move, then the process should return to step a1 to continue to estimate the state quantity and correct the static error of the INS based on the state quantity.

[0082] In this embodiment, the target device continuously corrects the error of INS when it is stationary. In this way, when the target device starts to move, it calculates the speed information and position information based on the INS whose error has been corrected in the stationary state, and then determines the direction of movement based on the speed information and position information, so that the determined direction of movement is more accurate, thereby ensuring the correctness of the initialization of the heading angle.

[0083] As described above, the moving direction of the target device can be determined based on the speed information and position information obtained by INS. Figure 3 Determining the moving direction of the target device based on the speed information obtained by INS solution is described. Figure 4 Determining the moving direction of the target device based on the position information obtained by INS solution is described.

[0084] The present application embodiment provides a method for initializing a heading angle. Figure 3 A flowchart of a method for initializing a heading angle provided in an embodiment of the present application is shown in FIG. Figure 3 As shown, S103, determining the moving direction of the target device based on the speed information obtained by INS solution, can be achieved through S1031 to S1032:

[0085] S1031. Determine a forward speed of the target device in the first coordinate system in a current motion state based on a first speed vector of the target device in the second coordinate system.

[0086] It can be understood that when the target device starts to move, the xyz axis of the first coordinate system of the target device changes, but the second coordinate system remains unchanged during the heading initialization process.

[0087] In this embodiment, the speed information obtained by INS calculation is: a first speed vector of the target device in the second coordinate system.

[0088] As described above, the x-axis of the second coordinate system is used to point to the east, the y-axis is used to point to the north, and the z-axis is used to point to the top. That is, the first velocity vector is used to indicate the eastward velocity of the target device in the second coordinate system. Northbound speed and celestial speed

[0089] Specifically, the eastward speed refers to the positive speed of the target device along the x-axis in the second coordinate system, the northward speed refers to the positive speed of the target device along the y-axis in the second coordinate system, and the skyward speed refers to the positive speed of the target device along the z-axis in the second coordinate system.

[0090] It is understandable that there is a difference between the postures of the first coordinate system and the second coordinate system in the motion state. In some embodiments, the first velocity vector can be transformed based on a transformation matrix for characterizing the transformation relationship between the postures of the first coordinate system and the second coordinate system to obtain the forward velocity of the target device in the first coordinate system.

[0091] Specifically: the transformation matrix Expressed as:

[0092] Among them, T ij is the element in the i-th row and j-th column of the matrix.

[0093] The first velocity vector and the second velocity vector satisfy formula 1:

[0094]

[0095] Among them, V bis the second velocity vector of the target device at the first coordinate in the current motion state, V n is the first velocity vector of the target device in the second coordinate system; (*) T Represents the transpose of a matrix.

[0096] Among them, V b Includes the rightward speed of the target device in the first coordinate system when it is in the current motion state Forward speed of the target device in the first coordinate system Celestial velocity of the target device in the first coordinate system

[0097] Specifically, the right speed of the target device in the first coordinate system in the current motion state refers to the positive speed of the target device on the x-axis in the first coordinate system in the current motion state; the forward speed of the target device in the first coordinate system in the current motion state refers to the positive speed of the target device on the y-axis in the first coordinate system in the current motion state; the celestial speed of the target device in the first coordinate system in the current motion state refers to the positive speed of the target device on the z-axis in the first coordinate system in the current motion state.

[0098] Formula 1 can be further expressed as Formula 2:

[0099]

[0100] According to formula 2, we can further get formula 3:

[0101]

[0102] Furthermore, the rightward speed of the target device in the second coordinate system can be obtained according to Formulas 4 to 6: Forward speed v f and celestial speed

[0103]

[0104]

[0105]

[0106] S1032: Determine a moving direction of the target device based on a relationship between the forward speed and the first threshold.

[0107] Specifically, the first threshold is a positive number greater than 0.

[0108] Forward velocity is a vector quantity, which has both direction and magnitude.

[0109] If the forward speed of the target device in the first coordinate system is greater than the first threshold, it means that the magnitude of the speed is greater than the first threshold, and the direction of the speed is consistent with the forward direction of the target device. Therefore, the moving direction of the target device is determined to be forward movement.

[0110] If the forward speed of the target device in the first coordinate system is less than the inverse of the first threshold, it means that the magnitude of the speed is greater than the first threshold, and the direction of the speed is opposite to the forward direction of the target device. Therefore, the movement direction of the target device is determined to be backward movement.

[0111] If there is any other relationship between the forward speed and the first threshold, this determination is invalid.

[0112] The first threshold can be preset according to actual needs.

[0113] In this embodiment, when the target device starts to move, the forward speed of the target device in the first coordinate system in the current motion state is determined based on the first velocity vector of the target device in the second coordinate system and the posture conversion matrix between the first coordinate system and the second coordinate system, so as to determine the movement direction of the target device based on the comparison result of the first forward speed and the first threshold.

[0114] The present application embodiment provides a method for initializing a heading angle. Figure 4 A flowchart of a method for initializing a heading angle provided in an embodiment of the present application is shown in FIG. Figure 4 As shown, the position information solved by INS includes the first position vector of the current target device in the second coordinate system. Then S103, determining the movement direction of the target device based on the position information solved by INS, can be implemented through S1033 to S1034:

[0115] S1033: Determine a vector difference between the first position vector and a second position vector of the target device in the second coordinate system when in the stationary state.

[0116] It can be understood that if the target device is currently in motion, the position of the target device currently calculated by the INS will change compared to the position calculated by the INS when the target device is in a stationary state.

[0117] When the target device is in a stationary state, the latitude position information of the target device calculated by the INS in the second coordinate system is represented by a second position vector.

[0118] The INS is used to calculate the position of the current target device in motion in the second coordinate system to obtain the latitude position information of the target device, which can be represented by the first position vector.

[0119] The vector difference between the first position vector and the second position vector can be expressed as first position vector-second position vector.

[0120] S1034: Determine a movement direction of the target device based on a relationship between a vector difference between the first position vector and the second position vector and a second threshold.

[0121] Specifically, the vector difference is a vector, including magnitude and direction.

[0122] If the vector difference between the first position vector and the second position vector is greater than the second threshold, it means that the magnitude of the vector difference is greater than the second threshold, and the direction of the vector difference is the same as the direction of the first position vector, then the movement direction of the target device is determined to be forward movement.

[0123] If the vector difference between the second position vector and the first position vector is less than the negative number of the second threshold, it means that the magnitude of the vector difference is greater than the second threshold, and the direction of the vector difference is opposite to the direction of the first position vector, then the movement direction of the target device is determined to be backward movement.

[0124] If there is any other relationship between the vector difference and the second threshold, this determination is invalid.

[0125] Figure 5 A schematic diagram showing the movement of the target device in the second coordinate system is shown. Figure 5 As shown, the target device moves from O0 at rest to O f , the angle between the moving direction of the target device and the north direction of the second coordinate system is not greater than 90 degrees, and the target device moves in a straight line. b , the angle between the moving direction of the target device and the north direction of the second coordinate system is greater than 90 degrees, and the target device is moving backward in a straight line.

[0126] In this embodiment, after the target device starts moving, the INS solves the position information of the target device in the second coordinate system to determine the vector difference between the first position vector during the movement and the second position vector in the stationary state, thereby determining the movement direction of the target device based on the vector difference between the first position vector and the second position vector.

[0127] The embodiment of the present application does not limit whether S1031 to S1032 are used to implement S103 or S1033 to S1034 are used to implement S103. In actual operation, it can be selected based on actual needs.

[0128] As described above, when estimating the INS state quantity, an intermediate result is generated. The present application can use a chi-square test method to determine whether the target device has started to move based on the intermediate result.

[0129] Next, through Figure 6The embodiment describes how to estimate the state quantity based on the Kalman filter method. Figure 7 The embodiment describes how to determine whether the target device is moving based on the intermediate result generated by estimating the state quantity.

[0130] Figure 6 The heading initialization method provided in the embodiment of the present application is as follows: Figure 6 As shown, step a1 estimates the state of INS in the current cycle through S1021 to S1025:

[0131] Before executing S1021, the state equation and measurement equation of INS need to be determined. Next, the state equation and measurement equation are introduced.

[0132] The state of INS is represented by X sins As mentioned above, the state quantity of INS can be expressed by Formula 7.

[0133]

[0134] Among them, δP n is the three-dimensional position error of the target device in the second coordinate system, δV n is the three-dimensional velocity error of the target device in the second coordinate system, φ n is the 3D posture error of the target device in the second coordinate system, ε b is the three-dimensional constant drift of the INS gyroscope in the first coordinate system, A three-dimensional constant zero bias of the accelerometer in the first coordinate system.

[0135] The superscript n represents the second coordinate system, and b represents the first coordinate system.

[0136] According to the traditional INS error model, formula 7 is differentiated to obtain formula 8:

[0137]

[0138] in, For X sins The differential of F sins is the system state matrix; G sins is the noise input matrix, and W is the system state noise matrix.

[0139] When the target device is stationary, the speed should be 0. However, when the target device is an agricultural machine, when the target device is stationary, if the engine is in the starting state, the target device will vibrate. When the vehicle is stationary, the INS error will be disturbed by the vibration, resulting in the true speed not being 0. Therefore, considering the impact of vibration on the target device in agricultural machinery application scenarios, the position measurement update method is used to perform ZUPT. That is, during the static measurement update, the fixed solution position information output by GNSS is used as the observation quantity to update the INS error.

[0140] Specifically, the position measurement equation is shown in Formula 9:

[0141] Z pos =P ins -P gnss =(δP n ) T Formula 9;

[0142] Among them, Z pos P is the position measurement; ins is the position information calculated by INS; P gnss The position information is obtained by GNSS solution.

[0143] At the same time, considering the low accuracy of MEMS-IMU, if the target device is stationary for a long time, the heading of INS will have a relative drift problem, which will also affect the judgment of the subsequent movement direction. Therefore, on the basis of the above measurement operation, a heading hold function is added to ensure the INS heading accuracy of the target device during the stationary state.

[0144] The heading measurement equation used to add the heading keeping function is shown in Formula 10:

[0145]

[0146] Among them, Z yaw is the heading vector measurement, ψ ins is the real-time heading angle measured by INS in the second coordinate system, ψ0 is the heading angle when the target device is in a stationary state, ψ0 = 0. φ E ,φ N and φ U is the 3D attitude error of INS, i.e. φ n =[φ E φ N φ U ] T , T ij As described above, these are the elements in the transformation matrix.

[0147] Based on Formula 9 and Formula 10, the total measurement equation is shown in Formula 11:

[0148] Z=HX+V;Formula 11;

[0149] Where Z = [(Z pos ) T Z yaw ] T , V is the measurement noise, and H is the measurement matrix. H is expressed as shown in Formula 11:

[0150]

[0151] Among them, I 3×3 is a 3D unit matrix;

[0152] Discretize Formula 8 and Formula 11, and obtain the discretized system equations as shown in Formula 13 and Formula 14:

[0153] X k =Φ k / k-1 X k-1 +Γ k-1 W k-1 Formula XIII;

[0154] Z k =H k X k +V k Formula 14;

[0155] Among them, the subscript k represents the current estimation period (referred to as period), X k Represents the state quantity of the current estimation cycle, Φ k / k-1 is the state transfer matrix, Γ k-1 is the noise input matrix of the previous cycle, W k-1 Represents the system state noise matrix of the previous cycle. Z k is the quantity measurement of the current period, H k is the measurement matrix of the current cycle, V k is the measurement noise of the current period.

[0156] Next, the Kalman filter algorithm estimation from S1021 to S1025 is performed based on Formula 13 and Formula 14.

[0157] S1021. Based on the initial state quantity of the INS or the state quantity estimated in the previous cycle, and the state transfer matrix, determine the one-step prediction state matrix of the current cycle.

[0158] Specifically, the one-step prediction state matrix is ​​determined as shown in Formula 15:

[0159] X k / k-1 =Φ k / k-1 Xk-1 Formula 15;

[0160] Among them, X k / k-1 is the one-step prediction state matrix, Φ k / k-1 is the state transfer matrix; if this update cycle is the first update when the target device is in a stationary state, then X k-1 is the initial state quantity. If this update cycle is not the first update when the target device is in a stationary state, then X k-1 The state quantity updated in the previous cycle.

[0161] S1022. Determine the one-step prediction mean square error matrix of the current cycle based on the state transfer matrix, the mean square error matrix of the state quantity of INS in the previous cycle, the noise input matrix in the previous cycle, and the system noise variance matrix in the previous cycle.

[0162] Specifically, the one-step prediction mean square error matrix is ​​determined as shown in Formula 16:

[0163]

[0164] Among them, P k / k-1 is the one-step prediction mean square error matrix of the current period, P k-1 is the mean square error matrix of the state quantity of INS in the previous cycle, Γ k-1 is the noise input matrix in the previous cycle, Q k-1 is the system noise variance matrix in the previous cycle.

[0165] S1023: Determine the filter gain in the current period based on the one-step prediction mean square error matrix of the current period, the measurement matrix corresponding to the measurement of the current period, and the measurement noise variance matrix.

[0166] The measurement includes position measurement and heading measurement; the position measurement is determined based on the difference between the position information measured by INS and the position information measured by GNSS; the heading measurement is determined based on the heading of INS in the current cycle and the initial heading.

[0167] The filter gain in the current cycle is determined as shown in Formula 17:

[0168]

[0169] Among them, K k is the filter gain in the current cycle, H k is the measurement matrix corresponding to the measurement of the current cycle, R k is the measurement noise variance matrix.

[0170] S1024, determining the state quantity of the INS in the current cycle based on the one-step prediction state matrix in the current cycle, the filter gain of the Kalman filter algorithm in the current cycle, the quantity measurement in the current cycle, and the measurement matrix corresponding to the quantity measurement in the current cycle.

[0171] The state quantity of INS in the current cycle is determined as shown in Formula 18:

[0172] X k =X k / k-1 +K k (Z k -H k X k / k-1 ) Formula XVIII;

[0173] Among them, X k is the state quantity of INS in the current cycle. k It is the quantity measurement in the current period.

[0174] X k Can be used to calculate the quantity measurement in the next cycle.

[0175] S1025. Determine the mean square error matrix of the state quantity of the INS in the next cycle based on the filter gain of the Kalman filter algorithm in the current cycle, the measurement matrix corresponding to the quantity measurement in the current cycle, and the one-step prediction mean square error matrix of the current cycle.

[0176] The mean square error matrix of the state quantity of INS in the next cycle can be determined by formula 19:

[0177] P k =(IK k H k ) k / k-1 Formula 19;

[0178] Among them, P k It is the mean square error matrix of the state quantity of INS in the next cycle.

[0179] P k It can be used to determine the filter gain of the Kalman filter algorithm in the next cycle, and thus to determine the state quantity of the INS in the next cycle.

[0180] In this embodiment, the state equation and measurement equation of INS are established. Based on the state equation and measurement equation of INS, the filter measurement update process from S1021 to S1025 is performed when the target device is in a static state to feedback correct the error of INS, and the intermediate results in the update process include: the one-step prediction state matrix X in the current cycle k / k-1 and the one-step prediction mean square error matrix P in the current period k / k-1, the intermediate result can be used later to determine whether the target device is moving.

[0181] An embodiment of the present application provides a heading initialization method for specifically explaining how to determine whether a target device starts moving based on an intermediate result.

[0182] Figure 7 A flow chart of the heading initialization method provided in this application, such as Figure 7 As shown, step a2 of determining whether the target device starts to move based on the intermediate result can be implemented through S1026 to S1027.

[0183] S1026. Determine new information of the chi-square test based on the one-step prediction state matrix in the intermediate results, the quantity measurement in the current cycle, and the measurement matrix in the current cycle.

[0184] Specifically, the new interest is determined as shown in Formula 20:

[0185] r k =Z k -H k X k / k-1 Formula 20;

[0186] The new information of the chi-square test follows a normal distribution, as shown in the following formula 21:

[0187]

[0188] in, is the first matrix, then r k It follows a normal distribution with location parameter 0 and scale parameter the first matrix.

[0189] S1027. Determine whether the target device starts to move based on a relationship between a ratio of the new information of the chi-square test to the diagonal matrix of the first matrix and a preset chi-square test threshold.

[0190] Among them, when the ratio of the new information of the chi-square test to the diagonal matrix of the first matrix is ​​greater than the preset chi-square test threshold, the target device starts to move; when the ratio of the new information of the chi-square test to the diagonal matrix of the first matrix is ​​less than or equal to the chi-square test threshold, the target device does not start to move and remains in a stationary state.

[0191] Specifically, r k It can be expressed as [r k (1)r k (2)r k (3)r k (4)] where r k (1) r k (2) r k (3) and rk (4) correspond to the x-axis, y-axis, z-axis and heading of the second coordinate system respectively.

[0192] Correspondingly, there are four chi-square test thresholds, which are related to r k (1) r k (2) r k (3) and r k (4) One-to-one correspondence.

[0193] The ratio of the new information of the chi-square test to the diagonal matrix of the first matrix is ​​greater than a preset chi-square test threshold, including: k (i)] 2 / M k (i,i)>β i ; i is 1 to 4, β i For r k (i) The corresponding chi-square test threshold. k (i,i) is the diagonal matrix of the first matrix.

[0194] The ratio of the new information of the chi-square test to the diagonal matrix of the first matrix is ​​greater than or equal to the chi-square test threshold, including: k (i)] 2 / M k (i,i)>β i .

[0195] Among them, the chi-square test threshold is set based on the requirements of missed detection rate and false detection rate.

[0196] In this embodiment, the intermediate result generated by updating the INS state quantity is fully utilized, and a chi-square test is used to determine whether the target device is moving based on the intermediate result, so as to subsequently initialize the heading angle based on the moving direction of the target device.

[0197] It should be noted that although the steps of the method in the present application are described in a specific order in the drawings, this does not require or imply that the steps must be performed in this specific order, or that all the steps shown must be performed to achieve the desired results. Additionally or alternatively, some steps may be omitted, multiple steps may be combined into one step, and / or one step may be decomposed into multiple steps, etc.; or, steps in different embodiments may be combined into a new technical solution.

[0198] Based on the foregoing embodiments, an embodiment of the present application provides a heading angle initialization device, which includes the modules included and the units included in the modules, and can be implemented by a processor; of course, it can also be implemented by a specific logic circuit; in the implementation process, the processor can be a central processing unit (CPU), a microprocessor (MPU), a digital signal processor (DSP) or a field programmable gate array (FPGA), etc.

[0199] Figure 8 A schematic diagram of the structure of the heading angle initialization device provided in the embodiment of the present application is shown in FIG. Figure 8 As shown, the heading angle initialization device 80 includes a coordinate system generation module 81, an error correction module 82 and a determination module 83, wherein:

[0200] The coordinate system generating module 81 is used to establish a second coordinate system based on the first coordinate system of the target device in the stationary state when the target device is in the stationary state; in the stationary state, the y-axis of the first coordinate system coincides with that of the second coordinate system, the positive direction of the y-axis of the first coordinate system points to the front direction of the target device, and the positive direction of the y-axis of the second coordinate system is used to point to the north; wherein, when the y-axis of the first coordinate system coincides with that of the second coordinate system, the positive direction of the y-axis of the first coordinate system points to the front direction of the target device, and the positive direction of the y-axis of the second coordinate system is used to point to the north, the heading angle of the target device is 0.

[0201] The error correction module 82 is used to estimate the state quantity of the inertial navigation system INS in the combined navigation system, and correct the error of the INS in the static state based on the currently estimated state quantity; wherein the state quantity includes the three-dimensional position error of the target device in the second coordinate system, the three-dimensional velocity error of the target device in the second coordinate system, the three-dimensional attitude error of the target device in the second coordinate system, the three-dimensional constant drift of the gyroscope of the INS in the first coordinate system, and the three-dimensional constant zero bias of the accelerometer of the INS in the first coordinate system.

[0202] The determination module 83 is used to determine the movement direction of the target device based on the determination that the target device starts to move, according to the speed information or position information obtained by the INS, and the movement direction is forward movement or backward movement; based on the movement direction and the track angle measured by the global navigation satellite system GNSS in the integrated navigation system, determine the initial heading angle of the target device.

[0203] In some embodiments, the determination module 83 is specifically used to determine the track angle obtained by the GNSS measurement as the initial heading angle when the movement direction is forward movement; when the movement direction is backward movement, determine the initial heading angle based on the track angle obtained by the GNSS measurement and the angle difference between the forward movement and the backward movement.

[0204] In some embodiments, the determination module 83 is specifically used to determine the forward speed of the target device in the first coordinate system in the current motion state based on the first speed vector of the target device in the second coordinate system; wherein the first speed vector is used to indicate the eastward speed, northward speed and celestial speed of the target device; when the forward speed is greater than a first threshold, the movement direction of the target device is determined to be forward movement; wherein the first threshold is a preset positive number; when the forward speed is less than the opposite of the first threshold, the movement direction of the target device is determined to be backward movement.

[0205] In some embodiments, the determination module 83 is specifically used to determine the second velocity vector of the target device in the first coordinate system in the current motion state based on the first velocity vector and the transformation matrix; the second velocity vector includes the right velocity, forward velocity and azimuth velocity of the target device; wherein the transformation matrix is ​​used to characterize the posture transformation relationship between the first coordinate system and the second coordinate system of the target device in the current motion state.

[0206] In some embodiments, the determination module 83 is specifically used to determine the vector difference between the first position vector and the second position vector of the target device in the second coordinate system when in the stationary state; based on the vector difference being greater than a second threshold, determining that the movement direction of the target device is forward movement; wherein the second threshold is a preset positive number; based on the vector difference being less than the opposite number of the second threshold, determining that the movement direction of the target device is backward movement.

[0207] In some embodiments, the determination module 83 is also used to obtain an intermediate result of estimating the state quantity; determine whether the target device starts moving based on the intermediate result; if the target device has not started moving, return to execute the operation of estimating the state quantity of the INS in the combined navigation system, correcting the error of the INS in the stationary state based on the state quantity, and obtaining an intermediate result of the estimated state quantity, until it is determined that the target device starts moving based on the last intermediate result obtained.

[0208] In some embodiments, the error correction module 82 is specifically used to determine the one-step predicted state matrix of the current cycle based on the initial state quantity of the INS or the state quantity estimated in the previous cycle, and the state transfer matrix; determine the one-step predicted mean square error matrix of the current cycle based on the state transfer matrix, the mean square error matrix of the state quantity in the previous cycle, the noise input matrix in the previous cycle, and the system noise variance matrix in the previous cycle; determine the filter gain in the current cycle based on the one-step predicted mean square error matrix of the current cycle, the measurement matrix corresponding to the quantity measurement of the current cycle, and the measurement noise variance matrix; the quantity measurement includes position quantity measurement and heading vector measurement; the position quantity measurement is determined based on the difference between the position information measured by the INS and the position information measured by the GNSS; the The heading vector measurement is determined based on the heading of the INS in the current cycle and the initial heading; based on the one-step prediction state matrix in the current cycle, the filter gain of the Kalman filter algorithm in the current cycle, the quantity measurement of the current cycle, and the measurement matrix corresponding to the quantity measurement of the current cycle, the state quantity of the INS in the current cycle is determined; based on the filter gain of the Kalman filter algorithm in the current cycle, the measurement matrix corresponding to the quantity measurement in the current cycle, and the one-step prediction mean square error matrix of the current cycle, the mean square error matrix of the state quantity of the INS in the next cycle is determined; the mean square error matrix of the INS in the next cycle is used to determine the filter gain of the Kalman filter algorithm in the next cycle, thereby determining the state quantity of the INS in the next cycle.

[0209] In some embodiments, the intermediate results include the one-step prediction state matrix within the current cycle and the one-step prediction mean square error matrix within the current cycle; the determination module 83 is specifically used to determine the new information of the chi-square test based on the one-step prediction state matrix within the current cycle, the quantity measurement within the current cycle, and the measurement matrix within the current cycle; the new information of the chi-square test obeys the normal distribution with a position parameter of 0 and a scale parameter of the first matrix; wherein the first matrix is ​​determined based on the measurement matrix within the current cycle, the one-step prediction mean square error matrix corresponding to the current cycle, and the measurement noise variance matrix corresponding to the current cycle; when the ratio of the new information of the chi-square test to the diagonal matrix of the first matrix is ​​greater than a preset chi-square test threshold, it is determined that the target device has started to move; when the ratio of the new information of the chi-square test to the diagonal matrix of the first matrix is ​​less than or equal to the chi-square test threshold, it is determined that the target device has not started to move.

[0210] It should be noted that in the embodiments of this application Figure 8The division of modules in the heading angle initialization device shown is schematic and is only a logical function division. There may be other division methods in actual implementation. In addition, each functional unit in each embodiment of the present application may be integrated into a processing unit, or may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of a software functional unit. It may also be implemented in the form of a combination of software and hardware.

[0211] An embodiment of the present application provides an electronic device, Fig. 9 A schematic diagram of the hardware entity of the electronic device according to the embodiment of the present application is shown in FIG. Fig. 9 As shown, the electronic device 90 includes a memory 91 and a processor 92, wherein the memory 91 stores a computer program that can be run on the processor 92, and the processor 92 implements the steps in the method provided in the above embodiment when executing the program.

[0212] It should be noted that the memory 91 is configured to store instructions and applications executable by the processor 92, and can also cache data to be processed or already processed by the processor 92 and various modules in the electronic device 90 (for example, image data, audio data, voice communication data, and video communication data), which can be implemented through flash memory (FLASH) or random access memory (Random Access Memory, RAM).

[0213] An embodiment of the present application provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the steps in the heading angle initialization method provided in the above embodiment are implemented.

[0214] An embodiment of the present application provides a computer program product comprising instructions, which, when executed on a computer, enables the computer to execute the steps in the heading angle initialization method provided in the above method embodiment.

[0215] It should be noted here that the description of the above storage medium and device embodiments is similar to the description of the above method embodiments, and has similar beneficial effects as the method embodiments. For technical details not disclosed in the storage medium, storage medium and device embodiments of this application, please refer to the description of the method embodiments of this application for understanding.

Claims

1. A heading angle initialization method, characterized in that: The heading angle initialization method is applied to an integrated navigation system, and the integrated navigation system is used to perform positioning and solving for a target device. The method comprises: When the target device is in a stationary state, a second coordinate system is established based on the first coordinate system of the target device in the stationary state; in the stationary state, the y-axis of the first coordinate system coincides with that of the second coordinate system, the positive direction of the y-axis of the first coordinate system points to the front direction of the target device, and the positive direction of the y-axis of the second coordinate system points to the north; wherein, when the y-axis of the first coordinate system coincides with that of the second coordinate system, the positive direction of the y-axis of the first coordinate system points to the front direction of the target device, and the positive direction of the y-axis of the second coordinate system points to the north, the heading angle of the target device is 0; Estimate the state quantity of the inertial navigation system INS in the integrated navigation system, and correct the error of the INS in the static state based on the currently estimated state quantity; wherein the state quantity includes the three-dimensional position error of the target device in the second coordinate system, the three-dimensional velocity error of the target device in the second coordinate system, the three-dimensional attitude error of the target device in the second coordinate system, the three-dimensional constant drift of the gyroscope of the INS in the first coordinate system, and the three-dimensional constant zero bias of the accelerometer of the INS in the first coordinate system; Based on determining that the target device starts to move, determining the movement direction of the target device according to the speed information or position information obtained by the INS, wherein the movement direction is forward movement or backward movement; An initial heading angle of the target device is determined based on the movement direction and a track angle measured by a global navigation satellite system (GNSS) in the integrated navigation system.

2. The method according to claim 1, characterized in that The determining the initial heading angle of the target device based on the motion direction and the track angle measured by the GNSS in the integrated navigation system includes: When the movement direction is forward movement, determining the track angle obtained by the GNSS measurement as the initial heading angle; In the case where the movement direction is backward movement, the initial heading angle is determined based on the track angle measured by the GNSS and the angle difference between the forward movement and the backward movement.

3. The method according to claim 2, characterized in that The speed information includes a first speed vector of the target device in the second coordinate system, and determining the moving direction of the target device according to the speed information includes: Determine, based on a first velocity vector of the target device in the second coordinate system, a forward velocity of the target device in the first coordinate system in a current motion state; wherein the first velocity vector is used to indicate an eastward velocity, a northward velocity, and a celestial velocity of the target device; When the forward speed is greater than a first threshold, determining that the moving direction of the target device is forward movement; wherein the first threshold is a preset positive number; When the forward speed is less than the negative number of the first threshold, the moving direction of the target device is determined to be backward movement.

4. The method according to claim 3, characterized in that The determining, based on the first velocity vector of the target device in the second coordinate system, a forward velocity of the target device in the first coordinate system in the current motion state includes: Based on the first velocity vector and the transformation matrix, determine the second velocity vector of the target device in the first coordinate system in the current motion state; the second velocity vector includes the right velocity, forward velocity and azimuth velocity of the target device; wherein the transformation matrix is ​​used to characterize the posture transformation relationship between the first coordinate system and the second coordinate system of the target device in the current motion state.

5. The method according to claim 2, characterized in that: The position information includes a first position vector of the current target device in the second coordinate system, and determining the movement direction of the target device according to the position information includes: determining a vector difference between the first position vector and a second position vector of the target device in the second coordinate system when in the stationary state; Based on the vector difference being greater than a second threshold, determining that the movement direction of the target device is forward movement; wherein the second threshold is a preset positive number; Based on the fact that the vector difference is smaller than the opposite number of the second threshold, it is determined that the moving direction of the target device is backward movement.

6. The method according to claim 1, characterized in that The method further comprises: Obtaining an intermediate result of estimating the state quantity; Determine whether the target device starts moving based on the intermediate result; if the target device has not started moving, return to execute the operation of estimating the state quantity of the INS in the combined navigation system, correcting the error of the INS in the stationary state based on the state quantity, and obtaining the intermediate result of the estimated state quantity, until it is determined that the target device starts moving based on the intermediate result obtained last time.

7. The method according to claim 6, characterized in that In the current cycle, estimating the state quantity of the INS in the integrated navigation system includes: Based on the initial state quantity of INS or the state quantity estimated in the previous cycle, and the state transfer matrix, determine the one-step prediction state matrix of the current cycle; Determine a one-step prediction mean square error matrix for the current cycle based on the state transfer matrix, the mean square error matrix of the state quantity in the previous cycle, the noise input matrix in the previous cycle, and the system noise variance matrix in the previous cycle; Determine the filter gain in the current period based on the one-step prediction mean square error matrix of the current period, the measurement matrix corresponding to the measurement of the current period, and the measurement noise variance matrix; the measurement includes position measurement and heading measurement; the position measurement is determined based on the difference between the position information measured by the INS and the position information measured by the GNSS; the heading measurement is determined based on the heading of the INS in the current period and the initial heading; Determine the state quantity of the INS in the current period based on the one-step prediction state matrix in the current period, the filter gain in the current period, the quantity measurement in the current period, and the measurement matrix corresponding to the quantity measurement in the current period; Based on the filter gain in the current cycle, the measurement matrix corresponding to the quantity measurement in the current cycle, and the one-step prediction mean square error matrix of the current cycle, the mean square error matrix of the state quantity of the INS in the next cycle is determined; the mean square error matrix of the INS in the next cycle is used to determine the filter gain in the next cycle, thereby determining the state quantity of the INS in the next cycle.

8. The method according to claim 7, characterized in that The intermediate results include the one-step prediction state matrix in the current cycle and the one-step prediction mean square error matrix in the current cycle; The determining whether the target device starts to move based on the intermediate result includes: Based on the one-step prediction state matrix in the current cycle, the quantity measurement in the current cycle, and the measurement matrix in the current cycle, determine the new information of the chi-square test; the new information of the chi-square test obeys the normal distribution with a position parameter of 0 and a scale parameter of the first matrix; wherein the first matrix is ​​determined based on the measurement matrix in the current cycle, the one-step prediction mean square error matrix in the current cycle, and the measurement noise variance matrix corresponding to the current cycle; In a case where a ratio of the new information of the chi-square test to the diagonal matrix of the first matrix is ​​greater than a preset chi-square test threshold, determining that the target device starts to move; When the ratio of the innovation of the chi-square test to the diagonal matrix of the first matrix is ​​less than or equal to the chi-square test threshold, it is determined that the target device has not started to move.

9. An electronic device comprising a memory and a processor, wherein the memory stores a computer program that can be run on the processor, characterized in that: When the processor executes the program, the method according to any one of claims 1 to 8 is implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 8 is implemented.

Images