Dual-filter based positioning scheme switching method, apparatus, device, and medium

Abstract

This invention provides a positioning scheme switching method, apparatus, device, and medium based on dual filters. The method includes: updating a state machine to the corresponding state according to the current state of the driving equipment; acquiring sensing data collected by a sensor group installed on the driving equipment; updating the state variables of a first filter or a second filter based on the current state of the state machine using the sensing data; when switching from the first filter to the second filter, updating the transformation matrix according to the state variables of the dual filters at the time of switching; converting the state variables of the currently used filter into a positioning result in a virtual coordinate system through the transformation matrix; and outputting the result to the downstream module of the autonomous driving system. This invention employs dual filters, introducing the positioning result into a virtual coordinate system for calculation, transferring the error to the virtual coordinate system, and outputting only a positioning result transformed to the virtual coordinate system. Therefore, there are no numerical jumps, and it will not have a serious impact on the downstream module.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to a method, apparatus, device, and medium for switching positioning schemes based on dual filters. Background Technology

[0002] Different levels of autonomous driving systems have varying requirements for positioning capabilities, and these capabilities also differ across scenarios. In Level 3 and higher autonomous driving systems that rely on high-precision maps, the downstream path planning and query modules depend on high-precision global positioning. However, in Level 2 autonomous driving systems that do not rely on high-precision maps, the downstream control modules depend more on local high-precision positioning than global positioning. Based on the different autonomous driving modes, switching between high-level global positioning and low-level local positioning can lead to two different positioning results with significant numerical differences, causing numerical jumps. Since downstream modules such as path planning and query cannot accept these large jumps in upstream positioning, when the autonomous driving system's positioning cannot provide high-precision global positioning, it often needs to exit autonomous driving and wait for positioning capabilities to recover, or switch to a Level 2 lane-keeping system for repositioning. However, such switching and resulting positional jumps can often harm the data of downstream modules.

[0003] Currently, positioning schemes based on high-precision maps cannot support changes in positioning capabilities or autonomous driving capabilities. In other words, it is impossible to switch positioning schemes to support changes in autonomous driving capabilities without disengaging autonomous driving. Therefore, how to switch between high-precision local positioning and global positioning to support the switching of autonomous driving capabilities without disengaging autonomous driving is an urgent problem to be solved in this field. Summary of the Invention

[0004] To address the problems existing in the prior art, the present invention provides a positioning scheme switching method, device, equipment and medium based on dual filters, so as to realize real-time positioning scheme switching, and the positioning results output to the downstream module will not change during the switching process.

[0005] This invention provides a positioning scheme switching method based on a dual-filter, wherein the dual filters include a first filter for local positioning and a second filter for global positioning, and the state machine includes multiple states of the dual filters from waiting for initialization to positioning initialization completion, to support the positioning system in switching positioning schemes between global positioning and local positioning. The method includes:

[0006] The state machine is updated to the corresponding state based on the current state of the driving equipment;

[0007] Acquire sensing data collected by the sensor group installed on the driving equipment;

[0008] Based on the current state of the state machine, the state variables of the first filter or the second filter are updated using the sensing data;

[0009] When the dual filter switches from the first filter to the second filter, the transformation matrix from the world coordinate system to the virtual coordinate system is updated according to the state variables of the dual filter at the time of the switch.

[0010] The state variables of the currently used filter in the dual filters are converted into positioning results in the virtual coordinate system through the transformation matrix, and then sent as the output of the positioning system to the downstream modules of the autonomous driving system.

[0011] According to a positioning scheme switching device based on a dual filter provided by the present invention, the step of switching from a first filter to a second filter and updating the transformation matrix from the world coordinate system to the virtual coordinate system based on the state variables of the dual filters during the switching includes:

[0012] Obtain the first state value of the first filter and the second state value of the second filter during switching;

[0013] The transformation matrix is ​​obtained by multiplying the first state variable by the inverse of the second state variable.

[0014] According to a positioning scheme switching device based on a dual-filter provided by the present invention, the step of converting the state variables of the currently used filter in the dual filters into positioning results in a virtual coordinate system through the transformation matrix includes:

[0015] The transformation matrix is ​​multiplied by the state variables of the currently used filter in the dual filters to convert it into a positioning result in a virtual coordinate system.

[0016] According to the positioning scheme switching device based on dual filters provided by the present invention, the initialization order of the dual-filter positioning is to initialize the first filter first and then initialize the second filter, and the state of the state machine is set accordingly as follows:

[0017] State 1: Waiting for dual filter initialization;

[0018] State 2: First filter initialization in progress;

[0019] State 3: Waiting for the second filter to initialize;

[0020] State 4: Second filter initialization in progress;

[0021] State 5: Dual-filter positioning initialization complete;

[0022] The condition for switching from state two to state three is that the first filter is successfully initialized, and the condition for switching from state three to state five is that the second filter is successfully initialized.

[0023] According to a positioning scheme switching device based on a dual filter provided by the present invention, the step of updating the state machine to the corresponding state according to the current state of the driving device includes:

[0024] When the driving equipment is powered on, the autonomous driving system is activated, and the state machine is in state one.

[0025] When the sensor information used for initializing the local positioning system is obtained and the driving mode is in manual driving mode, the state machine enters state two.

[0026] After the first filter is initialized, the state machine enters state three, at which point the autonomous driving system is allowed to enter the assisted driving mode with local positioning function;

[0027] When the sensor information used for global positioning system initialization is obtained and the driving mode is in manual driving mode or assisted driving mode, the state machine enters state four.

[0028] After the second filter is initialized, the state machine enters state five, at which point the autonomous driving system is allowed to enter a high-order autonomous driving mode with global positioning function.

[0029] When the autonomous driving system is downgraded from the high-level autonomous driving mode to the manual driving mode or the assisted driving mode due to a malfunction or environmental influence, the state machine switches from state five to state three.

[0030] When the autonomous driving system is downgraded from the high-level autonomous driving mode to the manual driving mode or the assisted driving mode due to a change in the current driving level of the driving device, the state machine switches from state three to state four.

[0031] When information from the sensors used for reinitializing the global positioning system is continuously acquired, and after the second filter is reinitialized, the state machine switches from state four to state five, allowing the autonomous driving system to enter the higher-order autonomous driving mode.

[0032] A positioning scheme switching device based on a dual filter according to the present invention further includes: updating the state machine when a positioning scheme anomaly occurs, specifically including:

[0033] If a global positioning scheme anomaly occurs in state five, the state machine switches to state three and requests to exit the advanced autonomous driving mode.

[0034] If both the global and local positioning schemes malfunction in state five, the state machine switches to state one and requests to exit autonomous driving.

[0035] If a local positioning scheme malfunctions in state four, the state machine switches to state one and requests to exit autonomous driving.

[0036] Furthermore, when the state machine switches from state five to state three due to a positioning scheme anomaly, all state variables in the second filter are copied to the first filter to provide initial state variables for the first filter for local positioning, and the second filter is attempted to be initialized.

[0037] According to a positioning scheme switching device based on dual filters provided by the present invention, the step of updating the state variables of the first filter or the second filter based on the current state of the state machine and using the sensing data includes:

[0038] Get the current state of the state machine;

[0039] If the current state is state two, then attempt to initialize the first filter;

[0040] If the current state is state three, then the state value of the first filter is updated according to the sensing data;

[0041] If the current state is state four, then attempt to initialize the second filter and update the state variables of the first filter based on the sensing data;

[0042] If the current state is state five, then the state value of the second filter is updated according to the sensing data.

[0043] According to the present invention, a positioning scheme switching device based on a dual filter is provided. The dual filter includes a first filter for local positioning and a second filter for global positioning. The state machine includes multiple states of the dual filter from waiting for initialization to positioning initialization completion, so as to support the positioning system to switch positioning schemes between global positioning and local positioning. The device includes:

[0044] The state machine state update module is used to update the state machine to the corresponding state according to the current state of the driving equipment;

[0045] The sensing data acquisition module is used to acquire sensing data collected by sensors installed on the driving equipment;

[0046] A filter state quantity update module is used to update the state quantity of the first filter or the second filter based on the current state of the state machine and using the sensing data.

[0047] The transformation matrix update module is used to update the transformation matrix from the world coordinate system to the virtual coordinate system based on the state variables of the dual filters when the first filter switches to the second filter.

[0048] The positioning result generation module is used to convert the state variables of the currently used filter in the dual filters into positioning results in the virtual coordinate system through the transformation matrix, so as to serve as the output of the positioning system and send it to the downstream modules of the autonomous driving system for use.

[0049] The present invention 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 executes the program to implement the positioning scheme switching method based on dual filters as described above.

[0050] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the positioning scheme switching method based on dual filters as described above.

[0051] This invention provides a positioning scheme switching method and apparatus based on a dual-filter system. It utilizes a dual-filter system consisting of a first filter for local positioning and a second filter for global positioning, along with a state machine that corresponds to multiple states from waiting for initialization to the completion of positioning initialization. The method updates the state machine to the corresponding state based on the current state of the driving device; acquires sensing data collected by a sensor group installed on the driving device; updates the state variables of the first or second filter using the sensing data based on the current state of the state machine; switches from the first filter to the second filter, updating the transformation matrix from the world coordinate system to the virtual coordinate system based on the state variables of the dual filters during the switch; and converts the state variables of the currently used filter in the dual-filter system into a positioning result in the virtual coordinate system using the transformation matrix, which is then used as the output of the positioning system and sent to downstream modules of the autonomous driving system. This invention employs a dual-filter scheme, enabling the switching between global and local positioning schemes through dual filters and a state machine. The positioning result is then incorporated into a virtual coordinate system for calculation. This eliminates the need to generate positioning results with significant numerical differences during the positioning scheme switching process, as the error can be transferred to the virtual coordinate system. Only a positioning result transformed to the virtual coordinate system is output, thus avoiding numerical jumps and preventing serious impact on downstream modules. Furthermore, downstream modules are unaware of the upstream positioning switch based on this positioning result, allowing for safe and seamless positioning scheme switching. Attached Figure Description

[0052] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0053] Figure 1 A flowchart illustrating a positioning scheme switching method based on dual filters provided by the present invention;

[0054] Figure 2 This is a schematic diagram of the state switching mechanism of the state machine in the method provided in the embodiment of the present invention;

[0055] Figure 3 A flowchart illustrating step S2 of a positioning scheme switching method based on dual filters provided in an embodiment of the present invention;

[0056] Figure 4 A schematic diagram of a positioning scheme switching device based on dual filters provided by the present invention;

[0057] Figure 5 This is a schematic diagram of the structure of the electronic device provided by the present invention.

[0058] Figure label:

[0059] 21: State machine state update module; 22: Sensing data acquisition module; 23: Filter state quantity update module; 24: Transformation matrix update module; 25: Positioning result generation module. Detailed Implementation

[0060] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be described clearly and completely below with reference to specific embodiments and accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0061] It should be noted that those skilled in the art will understand, explicitly and implicitly, that the embodiments described in this invention can be combined with other embodiments without conflict. Unless otherwise defined, the technical or scientific terms used in this invention should be understood in their ordinary sense by those skilled in the art. The terms "a," "an," "an," "the," etc., used in this invention do not indicate quantity limitation and can represent singular or plural. The terms "comprising," "including," "having," and any variations thereof used in this invention are intended to cover non-exclusive inclusion; the terms "first," "second," "third," etc., used in this invention are merely to distinguish similar objects and do not represent a specific ordering of objects.

[0062] State Machines: The design of product business functions is fundamentally inseparable from the transitions between business states. As the business progresses, a business object will transition from one state to another; this state transition is the state machine. First, a state machine consists of several non-overlapping states, with at least one start state and one end state. Second, a state machine is a directed graph, with lines indicating actions that trigger state changes; that is, state changes are caused by external actions. Unlike flowcharts, which are suitable for describing different behaviors in the entire business process to connect various business objects, state machines focus more on the state changes of a specific object within a business process. Therefore, state machines can effectively guide the transitions of the business states of a single business object. Furthermore, for multiple related business objects, a combination of state machines and flowcharts can be used to clarify the state transitions of multiple business objects in a business process and how they influence each other.

[0063] Global localization outputs global pose, relying on global information and heavily depending on GNSS and high-precision maps. Global localization filters can be implemented using Kalman filtering (KF) and particle filtering (PF). Continuous localization outputs odometry attitude prediction, independent of global information. Continuous localization filters can also be implemented using KF and PF, but the sensors used for fusion differ.

[0064] Example 1

[0065] Reference Figure 1As shown, this embodiment provides a positioning scheme switching method based on dual filters. The dual filters include a first filter (global positioning filter) for local positioning and a second filter (local positioning filter) for global positioning. The state machine includes multiple states of the dual filters from waiting for initialization to the completion of positioning initialization, to support the positioning system in switching positioning schemes between global positioning and local positioning. The method includes:

[0066] Step S1: Update the state machine to the corresponding state based on the current state of the driving equipment;

[0067] Figure 2 A schematic diagram of the state machine's state transition mechanism is shown. (Refer to...) Figure 2 As shown, the initialization order of the dual-filter localization is to initialize the first filter first and then the second filter, correspondingly setting the state machine state as follows:

[0068] State 1: Waiting for dual filter initialization;

[0069] State 2: First filter initialization in progress;

[0070] State 3: Waiting for the second filter to initialize;

[0071] State 4: Second filter initialization in progress;

[0072] State 5: Dual-filter positioning initialization complete;

[0073] The initial state is state one, the ending state is state five, the condition for switching from state two to state three is that the first filter is successfully initialized, and the condition for switching from state three to state five is that the second filter is successfully initialized.

[0074] In this embodiment, step S1, which involves updating the request state machine to the corresponding state based on the current state of the driving device, includes:

[0075] When the driving equipment is powered on, the autonomous driving system starts, and the state machine is in state one.

[0076] When the sensor information used for initializing the local positioning system is obtained and the driving mode is manual driving mode, the state machine enters state two.

[0077] Once the first filter is initialized, the state machine enters state three, at which point the autonomous driving system is allowed to enter the assisted driving mode with local positioning function.

[0078] When the sensor information used for global positioning system initialization is obtained and the driving mode is in manual driving mode or assisted driving mode, the state machine enters state four.

[0079] Once the second filter is initialized, the state machine enters state five, at which point the autonomous driving system is allowed to enter a higher-order autonomous driving mode with global positioning capabilities.

[0080] When the autonomous driving system is downgraded from high-level autonomous driving mode to manual driving mode or assisted driving mode due to malfunction or environmental influence, the state machine switches from state five to state three.

[0081] When the autonomous driving system is downgraded from high-level autonomous driving mode to manual driving mode or assisted driving mode due to a change in the current driving level of the driving device, the state machine switches from state three to state four.

[0082] When the sensor information used for reinitializing the global positioning system is continuously acquired, and after the second filter is reinitialized, the state machine switches from state four to state five, allowing the autonomous driving system to enter the high-level autonomous driving mode.

[0083] Specifically, the fault is a hardware or software malfunction in the global positioning module of the autonomous driving system's positioning system.

[0084] Therefore, this invention adopts a dual-filter scheme: a second filter (global filter) that supports global positioning and a first filter (continue filter) that only supports local positioning. The dual filters of the positioning system are updated through a state machine to support real-time switching of positioning schemes between global and local positioning. This allows the positioning system to switch positioning schemes and restore positioning capabilities without having to forcibly exit autonomous driving, and enables the management of the state of autonomous driving capabilities supported under the current positioning scheme.

[0085] Step S2: Acquire the sensing data collected by the sensor group installed on the driving equipment;

[0086] Optionally, the driving device includes, but is not limited to, any type and function of vehicle that allows the deployment of multiple sensors and whose movement trajectory can be controlled, such as: vehicles (vehicles, ships, airplanes, etc.), measuring tools (drones, probes, etc.). In this embodiment, a vehicle is used as an example to illustrate the processing procedure of the dual-filter-based positioning scheme switching method. The processing procedure for other types of vehicles is similar and will not be described in detail here.

[0087] A sensor is a detection device deployed on a vehicle. When it senses information being measured, it transforms that information into an electrical signal or other desired form for output. Examples include: IMU (Inertial Measurement Unit), GNSS (Global Navigation Satellite System), vehicle odometer, knock sensor, water temperature sensor, ultrasonic radar, ADAS (Advanced Driving Assistance System) camera, millimeter-wave radar, and lidar, etc. A sensor array consists of multiple sensors deployed on a vehicle.

[0088] Sensing data is acquired through sensors in a sensor array deployed on the vehicle. This includes, but is not limited to, the transformation of information obtained by sensors into the target format when they sense the information to be acquired. It also includes, but is not limited to, controlling the vehicle by transmitting, processing, storing, displaying, and recording the sensing data. For example, the inertial navigation unit deployed on the vehicle first acquires corresponding measurement data. Since the inertial navigation unit measures the object's three-axis attitude angles (or angular rates) and acceleration, the acceleration of the vehicle at the current moment can be recorded based on the acquired inertial measurement data. The driving state of the vehicle can be determined based on this inertial measurement data. By combining the measurement data from multiple sensors, the acceleration of the vehicle can be adjusted to control its movement within a reasonable speed range.

[0089] Step S3: Based on the current state of the state machine, update the state variables of the first filter or the second filter using the sensing data;

[0090] It should be noted that the state variables of the dual filters include, but are not limited to, those obtained by transforming the sensing data collected by the sensor array deployed on the driving device through the Kalman filter equation.

[0091] In this embodiment, refer to Figure 3 As shown, step S3 specifically includes:

[0092] Step S301: Obtain the current state of the state machine;

[0093] If the current state is state two, then execute step S302: attempt to initialize the first filter;

[0094] If the current state is state three, then execute step S303: update the state quantity of the first filter according to the sensing data;

[0095] If the current state is state four, then execute step S304: attempt to initialize the second filter and update the state variables of the first filter according to the sensing data;

[0096] If the current state is state five, then execute step S305: update the state of the second filter based on the sensing data.

[0097] Further, after step S304 is completed, step S306 is executed: determine whether the second filter initialization is successful. If yes, proceed to step S3; otherwise, reprocess the sensing data based on the current state of the state machine.

[0098] Furthermore, when a positioning scheme anomaly occurs, the state machine state is updated, specifically including:

[0099] If a global positioning scheme anomaly occurs in state five, the state machine switches to state three and requests to exit the advanced autonomous driving mode.

[0100] If both the global and local positioning schemes fail in state five (i.e., both the global and continue positioning schemes fail), the state machine switches to state one and requests to exit autonomous driving.

[0101] If a local positioning scheme anomaly occurs in state four (i.e., a continue positioning scheme anomaly), the state machine switches to state one and requests to exit autonomous driving.

[0102] Furthermore, when the state machine switches from state five (where the positioning scheme malfunctions) to state one or state three, the autonomous driving level will decrease. It is necessary to reinitialize the dual filters or the second filter to complete the positioning initialization of the dual filters. At this time, the positioning scheme also switches from global positioning to local positioning. In order to avoid updating the state variables of the dual filters based on the sensing data, it is only necessary to copy all the state variables in the second filter to the first filter to provide the initial state variables for the first filter for local positioning, and continue to attempt to initialize the second filter.

[0103] Step S4: The dual filters switch from the first filter to the second filter, and the transformation matrix from the world coordinate system to the virtual coordinate system is updated according to the state variables of the dual filters at the time of the switch.

[0104] Specifically, step S4 includes:

[0105] Step S401: Obtain the first state variable T of the first filter during switching. wc The second state quantity T of the second filter wg ;

[0106] Step S402: Set the first state variable T wc With the second state quantity T wg The inverse multiplication is used to obtain the transformation matrix T from the world coordinate system to the virtual coordinate system. wv .

[0107] When the positioning system switches positioning schemes, for example, when the dual filter switches from the first filter to the second filter, the transformation matrix T from the world coordinate system to the virtual coordinate system is calculated using equation (1). wv :

[0108]

[0109] Among them, the first state variable T wc The first state variable (the continue filter) represents the transformation matrix from local positioning to the world coordinate system; the second state variable T... wg The output state variable of the second filter (global filter) represents the transformation matrix from global positioning to the world coordinate system.

[0110] It is important to note that when switching positioning schemes, if it is a switch from a global positioning scheme to a local positioning scheme, that is, when the dual filters switch from the second filter to the first filter, the state variables of the second filter are simply copied to the first filter to provide the initial state variables for the first filter.

[0111] Step S5: Convert the state variables of the currently used filter in the dual filters into the positioning results in the virtual coordinate system through the transformation matrix, so as to serve as the output of the positioning system and send it to the downstream module of the autonomous driving system for use;

[0112] Specifically, the transformation matrix is ​​multiplied by the state variables of the currently used filter in the dual filters to convert it into a positioning result in the virtual coordinate system.

[0113] If the positioning scheme of the positioning system in the current state machine is a local positioning scheme, then according to the first state quantity T of the currently used first filter... wc With transformation matrix T wv The positioning result T in the virtual coordinate system is calculated using equation (2). vl :

[0114] T vl =T wv ×T wc (2)

[0115] The current positioning system in the state machine uses a global positioning scheme. Therefore, based on the second state variable T of the currently used second filter... wg With transformation matrix T wvThe positioning result T in the virtual coordinate system is calculated using equation (3). vl :

[0116] T vl =T wv ×T wg (3)

[0117] Among them, T vl The transmitted positioning result represents the transformation matrix from the vehicle body to the virtual coordinate system.

[0118] Therefore, by introducing a virtual coordinate system, the positioning scheme switching process does not require generating positioning results with large numerical differences. The error can be transferred to the virtual coordinate system, and only a positioning result transformed to the virtual coordinate system is output. This ensures that the positioning result output to the downstream does not undergo numerical jumps during the positioning scheme switching process, eliminating the serious impact of numerical jumps on the downstream module. At the same time, the downstream module does not perceive that the upstream positioning has switched based on the positioning result. Therefore, the positioning scheme switching can be achieved safely and seamlessly.

[0119] In summary, the present invention provides a positioning scheme switching method based on dual filters, applicable to rapid global position initialization in all scenarios on highways. The method is based on a dual filter consisting of a first filter for local positioning and a second filter for global positioning, and a state machine with multiple states corresponding to the dual filters from waiting for initialization to the completion of positioning initialization. The positioning system is updated to the corresponding state by the state machine according to the current state of the driving device. Based on the current state of the state machine, the state variables of the first or second filter are updated using sensing data. When the dual filters switch from the first filter to the second filter, the transformation matrix from the world coordinate system to the virtual coordinate system is updated according to the state variables of the dual filters at the time of the switch. The currently used state variables in the dual filters are converted into positioning results in the virtual coordinate system through the transformation matrix, which are then used as the output of the positioning system for downstream modules of the autonomous driving system. This invention employs a dual-filter scheme, enabling the switching between global and local positioning schemes through dual filters and a state machine. The positioning results are then incorporated into a virtual coordinate system for calculation. This eliminates the need to generate positioning results with significant numerical differences during the positioning scheme switching process, as the errors can be transferred to the virtual coordinate system. Only a positioning result transformed to the virtual coordinate system is output, thus avoiding numerical jumps and preventing serious impacts on downstream modules. Furthermore, downstream modules are unaware of the upstream positioning switch based on this positioning result, allowing for safe and seamless positioning scheme switching.

[0120] The positioning scheme switching method provided by the present invention includes, but is not limited to, applications on the aforementioned driving equipment, and can also be applied to the server corresponding to the driving equipment, with the server controlling the driving equipment.

[0121] Example 2

[0122] Based on the same inventive concept as the above method, and referring to... Figure 4 As shown, this embodiment provides a positioning scheme switching device based on dual filters. The dual filters include a first filter for local positioning and a second filter for global positioning. The state machine includes multiple states of the dual filters from waiting for initialization to the completion of positioning initialization, so as to support the positioning system to switch positioning schemes between global positioning and local positioning. The device includes:

[0123] The state machine state update module 21 is used to update the state machine to the corresponding state according to the current state of the driving equipment;

[0124] The sensing data acquisition module 22 is used to acquire sensing data collected by sensors installed on the driving equipment;

[0125] The filter state quantity update module 23 is used to update the state quantity of the first filter or the second filter based on the current state of the state machine and using sensing data.

[0126] The transformation matrix update module 24 is used to switch the first filter to the second filter in the dual filter, and update the transformation matrix from the world coordinate system to the virtual coordinate system according to the state variables of the dual filters at the time of switching.

[0127] The positioning result generation module 25 is used to convert the state variables of the currently used filter in the dual filter into positioning results in the virtual coordinate system through a transformation matrix, so as to serve as the output of the positioning system and send it to the downstream modules of the autonomous driving system for use.

[0128] Specifically, the filter state quantity update module 23 includes: a state judgment unit for obtaining the current state of the state machine; a first processing unit for attempting to initialize the first filter when the current state of the state machine is state two; a second processing unit for updating the state quantity of the first filter based on the observation data when the current state of the state machine is state three; a third processing unit for attempting to initialize the second filter and updating the state quantity of the first filter based on the observation data when the current state of the state machine is state four; and a fourth processing unit for updating the state quantity of the second filter based on the observation data when the current state of the state machine is state five.

[0129] The transformation matrix update module 24 specifically includes: a filter state quantity acquisition unit, used to acquire the first state quantity of the first filter and the second state quantity of the second filter when the dual filters switch from the first filter to the second filter; and a transformation matrix calculation unit, used to multiply the first state quantity and the second state quantity inversely to obtain the transformation matrix.

[0130] The positioning result generation module 25 is specifically used to multiply the transformation matrix with the state variables of the currently used filter in the dual filters to convert it into a positioning result in a virtual coordinate system.

[0131] The state machine state update module 21 specifically includes: a first switching unit, used when the driving equipment is powered on, the autonomous driving system is started, and the state machine is in state one; a second switching unit, used when the sensor information for initializing the local positioning system is obtained and the driving mode is manual driving mode, the state machine enters state two; a third switching unit, used when the first filter initialization is completed, the state machine enters state three, at which time the autonomous driving system is allowed to enter the assisted driving mode with local positioning function; a fourth switching unit, used when the sensor information for initializing the global positioning system is obtained and the driving mode is manual driving mode or assisted driving mode, the state machine enters state four; a fifth switching unit, used when the second filter initialization is completed, the state machine... Entering state five, at which point the autonomous driving system is allowed to enter a high-level autonomous driving mode with global positioning function; the sixth switching unit is used to switch the state machine from state five to state three when the autonomous driving system degrades from the high-level autonomous driving mode to manual driving mode or assisted driving mode due to a fault or environmental influence; the seventh switching unit is used to switch the state machine from state five to state three when the autonomous driving system degrades from the high-level autonomous driving mode to manual driving mode or assisted driving mode due to a fault or environmental influence; the eighth switching unit is used to switch the state machine from state four to state five when information from the sensors used for re-initialization of the global positioning system is continuously acquired, and after the second filter is re-initialized, and the autonomous driving system is allowed to enter the high-level autonomous driving mode.

[0132] Furthermore, the device also includes an anomaly switching module, specifically comprising: a first anomaly switching unit, used to switch the state machine to state three and request to exit the high-level autonomous driving mode if a global positioning scheme anomaly occurs in state five; a second anomaly switching unit, used to switch the state machine to state one and request to exit autonomous driving if both the global positioning scheme and the local positioning scheme are anomalies in state five; and a third anomaly switching unit, used to switch the state machine to state one and request to exit autonomous driving if a local positioning scheme anomaly occurs in state four.

[0133] Furthermore, the device also includes: a positioning scheme exception handling module, which is used to copy all the state quantities in the second filter to the first filter when the state machine switches from state five to state one or state three due to a positioning scheme exception, so as to provide initial state quantities for the first filter for local positioning, and continue to attempt to initialize the second filter.

[0134] The specific implementation process of the functions and roles of each module in the above device can be found in the implementation process of the corresponding steps in the above method. Therefore, relevant parts can be referred to in the description of the method embodiment, and will not be repeated here.

[0135] The device embodiments described above are merely illustrative. For example, the division of modules is only a logical functional division. In actual implementation, there may be other division methods. In the embodiments, each functional module can be integrated into a processor, or each module can be a separate device, or two or more modules can be integrated into a device. Each functional module in each embodiment can be implemented in hardware or in the form of hardware plus software functional units.

[0136] Example 3

[0137] Reference Figure 5 As shown, this embodiment provides an electronic device, which includes a processor 310, a communication interface 320, a memory 330, and a communication bus 340. The processor 310, communication interface 320, and memory 330 communicate with each other via the communication bus 340. The processor 310 can call logical instructions in the memory 330. The processor 310 executes the dual-filter-based positioning scheme switching method described in the above embodiment. This method is based on a dual filter consisting of a first filter for local positioning and a second filter for global positioning, and a state machine with multiple states corresponding to the dual filter from waiting for initialization to the completion of positioning initialization, to support the positioning system in switching positioning schemes between global and local positioning. Specifically, it includes:

[0138] Update the state machine to the corresponding state based on the current state of the driving equipment;

[0139] Acquire sensing data collected by the sensor group installed on the driving equipment;

[0140] Based on the current state of the state machine, the state variables of the first filter or the second filter are updated using the sensing data;

[0141] When the dual filters switch from the first filter to the second filter, the transformation matrix from the world coordinate system to the virtual coordinate system is updated according to the state variables of the dual filters at the time of the switch.

[0142] The state variables of the currently used filter in the dual-filter system are transformed into positioning results in the virtual coordinate system through a transformation matrix, which are then used as the output of the positioning system and sent to the downstream modules of the autonomous driving system.

[0143] Furthermore, the logical instructions in the aforementioned memory 330 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, essentially, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0144] On the other hand, the present invention also provides a computer program product, which includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the positioning scheme switching method based on a dual filter as described in the above-described method embodiments. This method is based on a dual filter consisting of a first filter for local positioning and a second filter for global positioning, and a state machine with multiple states corresponding to the dual filter from waiting for initialization to the completion of positioning initialization, to support the positioning system in switching positioning schemes between global positioning and local positioning. Specifically, it includes:

[0145] Update the state machine to the corresponding state based on the current state of the driving equipment;

[0146] Acquire sensing data collected by the sensor group installed on the driving equipment;

[0147] Based on the current state of the state machine, the state variables of the first filter or the second filter are updated using the sensing data;

[0148] When the dual filters switch from the first filter to the second filter, the transformation matrix from the world coordinate system to the virtual coordinate system is updated according to the state variables of the dual filters at the time of the switch.

[0149] The state variables of the currently used filter in the dual-filter system are transformed into positioning results in the virtual coordinate system through a transformation matrix, which are then used as the output of the positioning system and sent to the downstream modules of the autonomous driving system.

[0150] Example 4

[0151] This embodiment provides a non-transitory computer-readable storage medium storing a computer program. When the computer program is executed by a processor, it implements the dual-filter-based positioning scheme switching method described in the above-described method embodiment. This method is based on a dual filter consisting of a first filter for local positioning and a second filter for global positioning, and a state machine with multiple states corresponding to the dual filter from waiting for initialization to the completion of positioning initialization, to support the positioning system in switching positioning schemes between global positioning and local positioning. Specifically, it includes:

[0152] Update the state machine to the corresponding state based on the current state of the driving equipment;

[0153] Acquire sensing data collected by the sensor group installed on the driving equipment;

[0154] Based on the current state of the state machine, the state variables of the first filter or the second filter are updated using the sensing data;

[0155] When the dual filters switch from the first filter to the second filter, the transformation matrix from the world coordinate system to the virtual coordinate system is updated according to the state variables of the dual filters at the time of the switch.

[0156] The state variables of the currently used filter in the dual-filter system are transformed into positioning results in the virtual coordinate system through a transformation matrix, which are then used as the output of the positioning system and sent to the downstream modules of the autonomous driving system.

[0157] The various embodiments in this invention are described in a progressive manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on describing the differences from other embodiments. In particular, the device and medium embodiments are relatively simple in description because they are fundamentally similar to the method embodiments; relevant parts can be referred to the descriptions in the method embodiments.

[0158] The devices, media, and methods provided in the embodiments of the present invention are one-to-one correspondences. Therefore, the devices and media also have similar beneficial technical effects as their corresponding methods. Since the beneficial technical effects of the methods have been described in detail above, the beneficial technical effects of the devices and media will not be repeated here.

[0159] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process method or product that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process method or product. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process method or product that includes that element.

[0160] The above are merely embodiments of the present invention and are not intended to limit the invention. Although the present invention has been described in detail above with general descriptions and specific embodiments, modifications or improvements can be made to it, which will be obvious to those skilled in the art. Therefore, all such modifications or improvements made without departing from the spirit of the present invention fall within the scope of protection claimed by the present invention.

Claims

1. A positioning scheme switching method based on dual filters, characterized in that, The dual filter includes a first filter for local positioning and a second filter for global positioning. The state machine includes multiple states of the dual filter from waiting for initialization to the completion of positioning initialization, to support the positioning system in switching positioning schemes between global and local positioning. The method includes: The state machine is updated to the corresponding state based on the current state of the driving equipment; Acquire sensing data collected by the sensor group installed on the driving equipment; Based on the current state of the state machine, the state variables of the first filter or the second filter are updated using the sensing data; When the dual filter switches from the first filter to the second filter, the transformation matrix from the world coordinate system to the virtual coordinate system is updated according to the state variables of the dual filter at the time of the switch. The state variables of the currently used filter in the dual filters are converted into positioning results in the virtual coordinate system through the transformation matrix, and sent as the output of the positioning system to the downstream module of the autonomous driving system for use. The step of switching from the first filter to the second filter in the dual-filter system, and updating the transformation matrix from the world coordinate system to the virtual coordinate system based on the state variables of the dual filters at the time of switching, includes: Obtain the first state value of the first filter and the second state value of the second filter during switching; The transformation matrix is ​​obtained by multiplying the first state variable by the inverse of the second state variable.

2. The positioning scheme switching method according to claim 1, characterized in that, The step of converting the state variables of the currently used filter in the dual filters into positioning results in the virtual coordinate system using the transformation matrix includes: The transformation matrix is ​​multiplied by the state variables of the currently used filter in the dual filters to convert it into a positioning result in a virtual coordinate system.

3. The positioning scheme switching method according to claim 1, characterized in that, The initialization order of the dual-filter positioning is to initialize the first filter first and then the second filter, and correspondingly set the state machine state as follows: State 1: Waiting for dual filter initialization; State 2: First filter initialization in progress; State 3: Waiting for the second filter to initialize; State 4: Second filter initialization in progress; State 5: Dual filter positioning initialization complete; The condition for switching from state two to state three is that the first filter is successfully initialized, and the condition for switching from state three to state five is that the second filter is successfully initialized.

4. The positioning scheme switching method according to claim 3, characterized in that, The step of updating the state machine to the corresponding state based on the current state of the driving equipment includes: When the driving equipment is powered on, the autonomous driving system is activated, and the state machine is in state one. When the sensor information used for initializing the local positioning system is obtained and the driving mode is in manual driving mode, the state machine enters state two. After the first filter is initialized, the state machine enters state three, at which point the autonomous driving system is allowed to enter the assisted driving mode with local positioning function; When the sensor information used for global positioning system initialization is obtained and the driving mode is in manual driving mode or assisted driving mode, the state machine enters state four. After the second filter is initialized, the state machine enters state five, at which point the autonomous driving system is allowed to enter a high-order autonomous driving mode with global positioning function. When the autonomous driving system is downgraded from the high-level autonomous driving mode to the manual driving mode or the assisted driving mode due to a malfunction or environmental influence, the state machine switches from state five to state three. When the autonomous driving system is downgraded from the high-level autonomous driving mode to the manual driving mode or the assisted driving mode due to a change in the current driving level of the driving device, the state machine switches from state three to state four. When information from the sensors used for reinitializing the global positioning system is continuously acquired, and after the second filter is reinitialized, the state machine switches from state four to state five, allowing the autonomous driving system to enter the higher-order autonomous driving mode.

5. The positioning scheme switching method according to claim 4, characterized in that, Also includes: When a positioning scheme malfunctions, the state machine is updated, specifically including: If a global positioning scheme anomaly occurs in state five, the state machine switches to state three and requests to exit the advanced autonomous driving mode. If both the global and local positioning schemes malfunction in state five, the state machine switches to state one and requests to exit autonomous driving. If a local positioning scheme malfunctions in state four, the state machine switches to state one and requests to exit autonomous driving. Furthermore, when the state machine switches from state five to state three due to a positioning scheme anomaly, all state variables in the second filter are copied to the first filter to provide initial state variables for local positioning, and the second filter is attempted to be initialized.

6. The positioning scheme switching method according to claim 1, characterized in that, The step of updating the state variables of the first filter or the second filter based on the current state of the state machine and using the sensing data includes: Get the current state of the state machine; If the current state is state two, then attempt to initialize the first filter; If the current state is state three, then the state value of the first filter is updated according to the sensing data; If the current state is state four, then attempt to initialize the second filter and update the state variables of the first filter based on the sensing data; If the current state is state five, then the state value of the second filter is updated according to the sensing data.

7. A positioning scheme switching device based on dual filters, characterized in that, For implementing the positioning scheme switching method based on a dual filter as described in any one of claims 1-6, wherein the dual filter includes a first filter for local positioning and a second filter for global positioning, and the state machine includes multiple states of the dual filter from waiting for initialization to positioning initialization completion, to support the positioning system in switching positioning schemes between global positioning and local positioning, the apparatus includes: The state machine state update module is used to update the state machine to the corresponding state according to the current state of the driving equipment; The sensing data acquisition module is used to acquire sensing data collected by sensors installed on the driving equipment; A filter state quantity update module is used to update the state quantity of the first filter or the second filter based on the current state of the state machine and using the sensing data. The transformation matrix update module is used to update the transformation matrix from the world coordinate system to the virtual coordinate system based on the state variables of the dual filters when the first filter switches to the second filter. The positioning result generation module is used to convert the state variables of the currently used filter in the dual filters into positioning results in the virtual coordinate system through the transformation matrix, so as to serve as the output of the positioning system and send it to the downstream modules of the autonomous driving system for use.

8. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the dual-filter-based positioning scheme switching method as described in any one of claims 1-6.

9. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the dual-filter-based positioning scheme switching method as described in any one of claims 1-6.

Images