| CPC G01S 19/47 (2013.01) [G01S 19/35 (2013.01); G01S 19/393 (2019.08); G01S 19/40 (2013.01); G01S 19/396 (2019.08)] | 9 Claims |
|
1. A multi-source data fusion method for a navigation board, performed by the navigation board, wherein the navigation board includes a printed circuit board, a Global Navigation Satellite System (GNSS) module, an inertial sensor, a processor, and a data interface, wherein the data interface of the navigation board comprises an external sensor interface configured to receive from an external sensor at least one of odometer data, millimeter-wave radar data, lidar data, visual sensor data, or map data, wherein the GNSS module, the inertial sensor and the processor are integrated on the printed circuit board, and the GNSS module and the inertial sensor are electrically connected to the processor through a circuit on the printed circuit board, and the method comprising:
acquiring synchronously, by the processor, observation data of the external sensor, the inertial sensor and the GNSS module, wherein the external sensor includes at least one of an odometer, a millimeter wave radar, a lidar or a visual sensor;
executing, by the processor, a large misalignment angle initialization algorithm to initialize a navigation and positioning state, and determining an initial attitude, an initial velocity, and an initial position;
acquiring periodically, by the processor, observation values of an angular velocity and an acceleration of the inertial sensor by taking the initial attitude, the initial velocity and the initial position respectively as an attitude, a velocity and a position at a starting moment of inertial navigation and positioning, and executing an inertial strapdown solution algorithm to perform an inertial navigation and positioning solution on the basis of the observation values of the angular velocity and the acceleration of the inertial sensor to obtain a first attitude, a first velocity and a first position; and
obtaining, by the processor, a new attitude, a new velocity and a new position by performing a fusion solution on observation data of a second position, a second velocity and a second attitude solved by the GNSS module, observation data of a third position, a third velocity and a third attitude acquired by the external sensor, and the first attitude, the first velocity and the first position; and
taking the new attitude, the new velocity and the new position as navigation results for output,
wherein executing, by the processor, the large misalignment angle initialization algorithm to initialize the navigation and positioning state, and determining the initial attitude, the initial velocity, and the initial position, comprising:
acquiring, by the GNSS module, a position and a velocity of the GNSS in a fixed solution state, and setting the initial position and the initial velocity; setting, by the processor, an initial pitch angle and an initial roll angle according to a pitch angle and a roll angle calculated by a tri-axial acceleration and a ti-axial angular velocity measured by the inertial sensor, and setting an initial heading angle to an arbitrary value; and
configuring two filters, wherein initial heading values of the two filters are set differently, and the two filters simultaneously operate,
wherein a case where the two filters each reaches a steady state and a determining condition of a heading angle estimation is satisfied, the processor interrupting solution of the large misalignment angle initialization algorithm, and taking a heading angle estimated by the processor as an initial heading of navigation and positioning solution in response to interrupting the solution of the large misalignment angle initialization algorithm.
|