Course angle estimation method, equipment, device and storage medium

Abstract

The invention provides a course angle estimation method, equipment and device and a storage medium, and relates to the technical field of intelligent traffic, and the method comprises the following steps: executing the following steps for each frame of a tracking target: executing a measurement updating process based on a measurement course angle of a current frame under the condition that a credibility label corresponding to the measurement course angle of the current frame is credible, obtaining a filtering course angle of the current frame; taking the filtering course angle of the current frame as a course angle estimation result of the current frame; under the condition that the credibility label corresponding to the measurement course angle of the current frame is not credible, after the observation noise standard deviation is increased, the measurement updating process is executed; or, skipping a measurement updating process, and taking the predicted course angle of the current frame as the course angle estimation result of the current frame; or, replacing the measured course angle of the current frame with the historical stable course angle of the tracking target, and executing a measurement updating process. According to the invention, a stable and accurate course angle estimation result is provided in a complex real traffic scene.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of intelligent transportation, and in particular to a heading angle estimation method, device, apparatus and storage medium. BACKGROUND

[0002] As a key state quantity describing the instantaneous orientation of a tracked target in a geographic coordinate system, heading angle estimation is the basis for realizing high-precision vehicle navigation, lane-level path planning, multi-target tracking, and traffic participant behavior intention understanding, and other advanced intelligent driving applications.

[0003] Currently, mainstream heading angle estimation algorithms are mainly implemented through two technical paths: one is multi-source sensor fusion, such as the combination of Global Navigation Satellite System (GNSS) and Inertial Navigation System (INS) integrated navigation systems, continuous attitude observations or velocity vectors of inertial measurement units, radars and vision sensors; the other is single-source visual inference, which uses the geometric displacement of the tracking target detection box in consecutive video frames to calculate the motion direction.

[0004] However, when dealing with low-speed creep, trajectory jitter caused by partial occlusion, and multi-sensor space-time registration errors, the output results of mainstream heading angle estimation algorithms often exhibit significant jumps and noise, which cannot guarantee the continuous smoothness and high precision of the heading information. Therefore, in complex real traffic scenarios, the stability and accuracy of mainstream heading angle estimation algorithms are bottlenecked, and it is difficult to meet the stringent requirements of advanced automatic driving and other fields for continuous and reliable heading states. SUMMARY

[0005] The present application provides a heading angle estimation method, device, apparatus and storage medium to solve the defect that the prior art is difficult to provide stable and accurate heading angle estimation results in complex real traffic scenarios, and to achieve stable and accurate heading angle estimation results in complex real traffic scenarios.

[0006] In a first aspect, an embodiment of the present application provides a heading angle estimation method, which comprises: performing the following steps on each frame of a tracking target: in a case where a confidence label corresponding to a measured heading angle of a current frame is trusted, performing a measurement update process based on the measured heading angle of the current frame to obtain a filtered heading angle of the current frame; and taking the filtered heading angle of the current frame as a heading angle estimation result of the current frame; If the confidence label corresponding to the measured heading angle in the current frame is unreliable, the measurement update process is executed after increasing the standard deviation of the observation noise; or, Skip the measurement update process and use the predicted heading angle of the current frame as the heading angle estimation result of the current frame; or, The measured heading angle of the current frame is replaced with the historical stable heading angle of the tracked target, and the measurement update process is performed.

[0007] In some embodiments, the method further includes: When the tracked target is within the effective coverage area of ​​the map, obtain the road centerline heading angle of the current frame and the confidence level corresponding to the road centerline heading angle of the current frame; If the angle difference between the filtered heading angle of the current frame and the road centerline heading angle of the current frame is less than or equal to an angle difference threshold, and the confidence level is greater than or equal to a confidence level threshold, the filtered heading angle of the current frame is corrected using the road centerline heading angle and the confidence level to obtain the corrected filtered heading angle of the current frame; the corrected filtered heading angle of the current frame is used as the heading angle estimation result of the current frame. If the angle difference is greater than the angle difference threshold and / or the confidence level is less than the confidence level threshold, the filtered heading angle of the current frame is used as the heading angle estimation result of the current frame.

[0008] In some embodiments, the method further includes: If the speed of the tracked target in the current frame is greater than the upper limit of the low-speed hysteresis interval and the number of consecutive jitters is greater than the jitter threshold, the historical stable heading angle of the tracked target is used to replace the measured heading angle of the current frame in the measurement update process, and the credibility label is set to untrustworthy. If the speed of the tracked target in the current frame is greater than the upper limit of the low-speed hysteresis interval, and the number of consecutive jitters is less than or equal to the jitter threshold, the historical stable heading angle of the tracked target is refreshed according to the measured heading angle of the current frame, and the confidence label is set to confidence. If the speed of the tracked target in the current frame is less than the lower limit of the low-speed hysteresis interval, the historical stable heading angle of the tracked target is used as the heading angle estimation result of the current frame; When the velocity of the tracked target in the current frame is in the low-speed hysteresis range, the MCSM maintains the confidence label unchanged.

[0009] In some embodiments, the performing a measurement update process based on the measurement heading angle of the current frame to obtain the filtered heading angle of the current frame comprises: updating and normalizing the weight of each particle in the current frame based on the measurement heading angle of the current frame, the predicted heading angle of each particle in the current frame and the observation noise standard deviation to obtain the normalized weight of each particle in the current frame; performing a weighted average on the predicted heading angle of all particles in the current frame according to the normalized weight of all particles in the current frame to obtain the filtered heading angle of the current frame on the unit circle.

[0010] In some embodiments, the method further comprises: performing the following steps on any particle in any frame: obtaining the predicted heading angle of the current particle in the current frame according to the heading angle of the current particle in the previous frame, the angular velocity of the current particle in the previous frame, the frame time interval between the current frame and the previous frame and the process noise of the heading angle; obtaining the predicted angular velocity of the current particle in the current frame according to the angular velocity of the current particle in the previous frame and the process noise of the angular velocity.

[0011] In some embodiments, the method further comprises: in the case that the current particle is a pedestrian particle, obtaining the predicted angular velocity of the current particle in the current frame according to the angular velocity of the current particle in the previous frame, the angular velocity decay factor and the process noise of the angular velocity.

[0012] In some embodiments, the method further comprises: performing the following steps on each frame: calculating the effective particle number of the current frame according to the total particle number and the weight of all particles in the current frame; in the case that the effective particle number of the current frame is greater than or equal to the effective particle threshold, skipping the resampling process; in the case that the effective particle number of the current frame is less than the effective particle threshold and it is determined that the effective particle number after particle resampling is greater than or equal to the effective particle threshold, performing particle resampling in a fixed starting point and equal interval sampling manner and setting the weight of all resampled particles to the reciprocal of the total particle number; in the case that the effective particle number of the current frame is less than the effective particle threshold and it is determined that the effective particle number after particle resampling is less than the effective particle threshold, triggering the particle diversity self-recovery process.

[0013] In some embodiments, the method further comprises: In the initial detection of the tracking target, in the presence of a heading angle provided by a sensor not interfered by a magnetic field, the heading angle provided by the sensor is taken as the first frame of the measured heading angle; In the absence of a heading angle provided by a sensor not interfered by a magnetic field, and the tracking target is located in a lock angle area, a preset heading angle is taken as the first frame of the measured heading angle; In the absence of a heading angle provided by a sensor not interfered by a magnetic field, and the tracking target is not located in a lock angle area, the first frame of the measured heading angle is calculated according to the position points of the tracking target in two consecutive frames.

[0014] In some embodiments, the method further comprises: Based on the first frame of the measured heading angle, a particle filter instance is created and initialized; According to the type of the tracking target, the initialization variance of the particle filter instance is set; wherein the initialization variance corresponding to the vehicle tracking target is smaller than the initialization variance corresponding to the pedestrian tracking target.

[0015] In some embodiments, the observation noise standard deviation, the process noise of the heading angle, and the process noise of the angular velocity are adjusted according to the type of the tracking target and the speed of the tracking target.

[0016] In a second aspect, the embodiments of the present application also provide an electronic device, comprising a memory, a transceiver, and a processor; The memory is used to store a computer program; the transceiver is used to transceive data under the control of the processor; and the processor is used to read the computer program in the memory and execute the method of the first aspect.

[0017] In a third aspect, the embodiments of the present application also provide a heading angle estimation device, the device comprising: A measurement update module is used to execute the following steps for each frame of a tracking target: In the case that the credibility label corresponding to the measured heading angle of the current frame is credible, a measurement update process is executed based on the measured heading angle of the current frame to obtain a filtered heading angle of the current frame; and the filtered heading angle of the current frame is taken as the heading angle estimation result of the current frame; In the case that the credibility label corresponding to the measured heading angle of the current frame is not credible, the measurement update process is executed after increasing the observation noise standard deviation; or, The measurement update process is skipped, and the predicted heading angle of the current frame is taken as the heading angle estimation result of the current frame; or, The measured heading angle of the current frame is replaced by a historical stable heading angle of the tracking target, and the measurement update process is executed.

[0018] In a fourth aspect, the present application also provides a processor-readable storage medium, which stores a program for causing a processor to execute the method according to the first aspect.

[0019] In a fifth aspect, the present application also provides a chip, which stores a program for causing a chip to execute the method according to the first aspect.

[0020] In a sixth aspect, the present application also provides a computer program product, which comprises a computer program or instructions, and when the computer program or instructions are run on a computer, the computer program or instructions cause the computer to execute the method according to the first aspect.

[0021] The heading angle estimation method, device, apparatus and storage medium provided by the present application divide the processing flow of each frame into two completely different branches by measuring the credibility label corresponding to the heading angle. In the case that the credibility label is credible, the precision priority mode is entered, the credible measurement heading angle is fully fused through the standard measurement update flow, and the accurate filtered heading angle is output. In the case that the credibility label is not credible, the stability priority mode is switched to, and one of the three defensive strategies of weight reduction update, skip update and measurement value replacement is executed. The real-time evaluation of data quality and the dynamic switching of processing strategy ensure that stable and accurate heading angle estimation results are provided in complex real traffic scenarios. BRIEF DESCRIPTION OF DRAWINGS

[0022] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the related art, the drawings needed to be used in the embodiment or related art description will be briefly introduced. Obviously, the drawings in the following description are some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0023] Figure 1 One of the heading angle estimation method flowcharts provided by the embodiments of the present application.

[0024] Figure 2 The second heading angle estimation method flowchart provided by the embodiments of the present application.

[0025] Figure 3 The third heading angle estimation method flowchart provided by the embodiments of the present application.

[0026] Figure 4 The fourth heading angle estimation method flowchart provided by the embodiments of the present application.

[0027] Figure 5FIG. 5 is a flowchart of a method for estimating a heading angle according to an embodiment of the present application.

[0028] Figure 6 FIG. 6 is a comparison chart of a heading angle before and after optimization according to an embodiment of the present application.

[0029] Figure 7 FIG. 7 is a schematic diagram of an electronic device according to an embodiment of the present application.

[0030] Figure 8 FIG. 8 is a schematic diagram of a device for estimating a heading angle according to an embodiment of the present application. DETAILED DESCRIPTION

[0031] The technical solutions in the embodiments of the present application will be clearly and completely described with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, but not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative work fall within the scope of the present application.

[0032] Figure 1 FIG. 1 is a flowchart of a method for estimating a heading angle according to an embodiment of the present application. As shown in FIG. 1, the method comprises the following steps. Figure 1 The present application provides a method for estimating a heading angle. The execution subject is a tracker. The tracker comprises a Motion Consistency State Machine (MCSM) and a particle filter. The tracker performs the following steps on each frame of a tracking target. In step 110, it is determined whether the confidence label corresponding to the measured heading angle of the current frame is reliable.

[0033] Specifically, the MCSM is used to evaluate the stability of the motion of the tracking target. The MCSM maintains state quantities such as historical stable heading angle, continuous jitter count, and confidence label, and stores them according to the unique identifier of the tracking target, ensuring that each tracking target is independently tracked. The MCSM comprehensively determines whether the confidence label corresponding to the measured heading angle of the current frame is reliable in combination with various state quantities maintained by itself.

[0034] The confidence label being reliable means that the measured heading angle of the current frame is considered to be smooth, stable, and consistent with the motion law of the tracking target. The confidence label being unreliable means that the measured heading angle of the current frame may be affected by noise, jitter, occlusion, or an angle locking region, and is abnormal and unreliable.

[0035] If the confidence label corresponding to the measured heading angle of the current frame is reliable, step 120 is performed. If the confidence label corresponding to the measured heading angle of the current frame is unreliable, step 130 is performed.

[0036] In step 120, a measurement update process is performed based on the measured heading angle of the current frame to obtain a filtered heading angle of the current frame; and the filtered heading angle of the current frame is taken as the heading angle estimation result of the current frame.

[0037] Specifically, the particle filter periodically corrects the state predicted by the motion model to be consistent with the actual observation value. Since the measured heading angle of the current frame is reliable, the measurement update process can be performed using the measured heading angle of the current frame, that is, the weight of the particle in the particle filter is updated using the measured heading angle of the current frame, and the filtered heading angle of the current frame is obtained based on the updated particle weight. The filtered heading angle of the current frame is taken as the heading angle estimation result of the current frame.

[0038] In step 130, the measurement update process is performed after increasing the observation noise standard deviation; or the measurement update process is skipped, and the predicted heading angle of the current frame is taken as the heading angle estimation result of the current frame; or the measured heading angle of the current frame is replaced by the historical stable heading angle of the tracked target, and the measurement update process is performed.

[0039] Specifically, the MCSM has determined that the measured heading angle of the current frame is unreliable, and there are several ways: The first way is to perform the measurement update process after increasing the observation noise standard deviation, which is a "weight reduction update" strategy. The particle filter does not completely ignore the measured heading angle, but has a high distrust in the measured heading angle. By increasing the observation noise standard deviation, even if the measured heading angle has a large deviation from the prediction of the particle, the particle weight will not be severely punished. The particle group will be very slowly adjusted to the direction of the measured heading angle, which is suitable for handling the slight abnormality of a flexible tracking target such as a pedestrian.

[0040] The second way is to skip the measurement update process and take the predicted heading angle of the current frame as the heading angle estimation result of the current frame, which is a "skip update" strategy. The particle filter does not use the measured heading angle of the current frame at all. The particle filter only relies on its own motion model for "inertial prediction". This is suitable for situations where the motion model is relatively stable, such as a vehicle, or the measured heading angle is judged to be severely abnormal, to maximize the smoothness of the output.

[0041] It should be noted that the predicted heading angle of the current frame is the overall estimation of the target state of the current frame by the entire particle group, that is, the output of the particle filter. The predicted heading angle of the current frame can be obtained in the following way: weighted average of the predicted heading angles of all particles, or taking the predicted heading angle corresponding to the particle with the largest weight, etc.

[0042] Method 3: Replace the measured heading angle of the current frame with the historical stable heading angle of the tracked target and perform a measurement update process. This is a "measurement replacement" strategy. MCSM does not pass unreliable measured heading angles to the particle filter; instead, it uses its own maintained, reliable historical stable heading angles to replace the measured heading angles during the measurement update process. This is an ultimate stabilization measure, ensuring that the particle filter is always "pulled back" to a known, reliable state, and is particularly suitable for scenarios requiring forced output of a stable heading, such as angle-locked regions.

[0043] The heading angle estimation method provided in this application divides the processing flow of each frame into two distinct branches by measuring the credibility label corresponding to the heading angle. When the credibility label is credible, it enters a precision-priority mode, fully integrating credible measured heading angles through a standard measurement update process to output an accurate filtered heading angle. When the credibility label is unreliable, it switches to a stability-priority mode, selecting one of three defensive strategies—de-weighted update, skip update, and measurement value replacement—to execute. This real-time evaluation of data quality and dynamic switching of processing strategies ensures stable and accurate heading angle estimation results even in complex real-world traffic scenarios.

[0044] In tracking algorithms, when a target first enters the field of view, the tracker has no historical information about the target's motion state. To initiate subsequent tracking processes such as particle filtering, the heading angle of the first frame must be determined; this heading angle is also known as the initial heading angle. The quality of the initial heading angle is crucial to the entire tracking process: an accurate initial heading angle allows the particle filter to converge quickly, while an incorrect initial heading angle may lead to tracking failure or require multiple frames to correct.

[0045] Since the available data sources and their reliability vary in different scenarios, this application proposes a hierarchical, priority-based initialization method to ensure that the most reliable information source can always be selected to determine the initial heading angle.

[0046] Figure 2 This is a second schematic diagram of the heading angle estimation method provided in the embodiments of this application, as shown below. Figure 2 As shown, when the tracking target is initially detected, the heading angle estimation method provided in this application embodiment further includes the following steps: Step 210: Determine whether there is a heading angle provided by a sensor that is not affected by the magnetic field.

[0047] Specifically, upon initial detection of the target being tracked, the system first determines whether a heading angle provided by a sensor unaffected by magnetic field interference exists. Sensors unaffected by magnetic field interference are also known as high-reliability sensors, such as lidar or millimeter-wave radar.

[0048] If a heading angle is provided by a sensor unaffected by magnetic field interference, it indicates that the most reliable heading information is available, and step 220 is executed. If no heading angle is provided by a sensor unaffected by magnetic field interference, it indicates that direct heading information cannot be obtained from a highly reliable sensor, and other indirect methods are required, and step 230 is executed.

[0049] Step 220: Use the heading angle provided by the sensor as the measured heading angle of the first frame.

[0050] Specifically, the heading angle provided by the high-reliability sensor is directly adopted, and the heading angle provided by the high-reliability sensor is used as the measured heading angle of the first frame.

[0051] Step 230: Determine whether the tracked target is located in the corner lock area.

[0052] Specifically, if the tracked target is located in the corner-locked region, it indicates that the current environment is extremely harsh and no sensor-based heading calculations can be performed; proceed to step 240. If the tracked target is not located in the corner-locked region, it indicates that the environment is relatively normal and kinematic calculations can be attempted; proceed to step 250.

[0053] Step 240: Use the preset heading angle as the measured heading angle of the first frame.

[0054] Specifically, a preset heading angle is used as the measurement heading angle for the first frame. The preset heading angle is a safe default value based on prior knowledge. For example, if map information is available, it can be set to the heading of the road where the target is located; it can be set to the mainstream traffic direction of the scene; and it can be set to 0 degrees when there is no prior information.

[0055] When the target is located in the corner-locked region, the preset heading angle is used as the measured heading angle of the first frame. This is a defensive strategy that avoids using known, heavily contaminated data and instead uses a default value that is not precise but at least "harmless" to complete the initialization, preventing the entire tracking from crashing due to an incorrect initial value.

[0056] Step 250: Calculate the measured heading angle of the first frame based on the position points of the tracked target in two consecutive frames.

[0057] Specifically, the direction of the target displacement vector is calculated using kinematic formulas by utilizing the target's position points in two consecutive frames (i.e., the first and second frames in which the target is detected), and this direction is used as the measured heading angle in the first frame.

[0058] The first, calculated using kinematic formulas The expression for the measured heading angle of the frame is as follows: In the formula, Indicates the first a measured heading angle of the frame, denotes an angle normalization function for limiting the measured heading angle to an interval, , denotes a first a position point of the frame, , denotes a first a position point of the frame.

[0059] It should be noted that in the case where the trajectory point of the current frame is consistent with the trajectory point of the last frame, in order to avoid occlusion or positioning stagnation causing random direction, the measured heading angle of the last frame is directly inherited as the measured heading angle of the current frame, that is, the measured heading angle of the last frame is taken as the measured heading angle of the current frame.

[0060] The heading angle estimation method provided by the embodiment of the application firstly attempts to use the heading angle provided by the sensor not affected by the magnetic field, if not, checks whether the tracking target is located in the lock angle area, and if in the lock angle area, adopts the preset heading angle, and if not in the lock angle area, uses the general kinematics method. By constructing a decision chain from good to bad and layer by layer, it is ensured that no matter where the new tracking target appears, a most reasonable and safest initial heading angle can be generated for it, which embodies strong robustness and lays a solid foundation for subsequent stable tracking.

[0061] In the actual traffic environment, the motion state of the tracking target (such as a vehicle or a pedestrian) is complex and changeable. Especially in the case of low speed, static or poor sensor signal, the measured heading angle directly calculated through the kinematics formula is often full of noise and produces violent and unrealistic jitter. If these situations are not specially processed, the stability and reliability of the heading estimation will be seriously affected.

[0062] Therefore, by introducing a hysteresis comparison to stabilize the low-speed decision, and by monitoring the trajectory jitter to identify and suppress abnormal measurement, a reliable measured heading angle and a corresponding confidence label are output.

[0063] Figure 3 As shown in FIG. 3, the heading angle estimation method provided by the embodiment of the application further includes the following steps: Figure 3 Step 310, judging whether the speed of the tracking target in the current frame is less than the lower limit of the low-speed hysteresis interval.

[0064] Specifically, in order to avoid frequent decision flips due to slight fluctuations near the speed critical point, a low-speed hysteresis interval is introduced.

[0065] ​MCSM first determines whether the speed of the tracking target in the current frame is less than the lower limit of the low-speed hysteresis interval. If the speed of the tracking target in the current frame is less than the lower limit of the low-speed hysteresis interval, it indicates that the tracking target has entered a low-speed or stationary state, and the measured heading angle calculated by the kinematic formula is not reliable, and the MCSM performs step 320. If the speed of the tracking target in the current frame is greater than or equal to the lower limit of the low-speed hysteresis interval, it indicates that the speed of the tracking target may be in a high-speed state or in the low-speed hysteresis interval, and the MCSM performs step 330 for further determination.

[0066] In step 320, the historical stable heading angle of the tracking target is taken as the heading angle estimation result of the current frame.

[0067] Specifically, when the speed of the tracking target in the current frame is definitely lower than the lower limit of the hysteresis interval, the MCSM directly outputs the internally maintained historical stable heading angle as the final heading angle estimation result of the current frame. The historical stable heading angle is the most reliable heading value recorded when the tracking target was previously stably traveling. By inheriting the historical stable heading angle, the interference of positioning noise on the heading at low speed is effectively blocked, and the output is absolutely stable.

[0068] In step 330, it is determined whether the speed of the tracking target in the current frame is greater than the upper limit of the low-speed hysteresis interval.

[0069] Specifically, the MCSM then determines whether the speed of the tracking target in the current frame is greater than the upper limit of the low-speed hysteresis interval. If the speed of the tracking target in the current frame is greater than the upper limit of the low-speed hysteresis interval, it indicates that the tracking target has entered a high-speed motion state, and the measured heading angle has a high potential reliability, and the MCSM performs step 350 for more detailed jitter checking. If the speed of the tracking target in the current frame is less than or equal to the upper limit of the low-speed hysteresis interval, it indicates that the speed of the tracking target is between the lower limit and the upper limit of the low-speed hysteresis interval, i.e., in the low-speed hysteresis interval, and the MCSM performs step 340.

[0070] In step 340, the reliability label is maintained unchanged.

[0071] Specifically, the MCSM maintains the reliability label of the current frame consistent with that of the previous frame. If the reliability label of the previous frame is "reliable", the reliability label of the current frame is also "reliable"; if the reliability label of the previous frame is "unreliable", the reliability label of the current frame is also "unreliable". Correspondingly, the heading processing of the current frame also follows the logic of the previous frame.

[0072] By introducing the low-speed hysteresis interval, a decision "buffer zone" is created, and the decision state will only change when the speed of the tracking target crosses the upper and lower limits of the low-speed hysteresis interval, thereby avoiding invalid jitter near the critical point.

[0073] Step 350, judge whether the number of continuous shaking is greater than the shaking threshold value.

[0074] Specifically, secondly judge whether the number of continuous shaking is greater than the shaking threshold value. If the number of continuous shaking is greater than the shaking threshold value, it indicates that the trajectory appears continuous and abnormal shaking, and the MCSM determines that the measured heading angle of the current frame is not reliable, and executes step 360 for suppression processing. If the number of continuous shaking is less than or equal to the shaking threshold value, it indicates that the trajectory is smooth and there is no obvious abnormality, and the MCSM determines that the measured heading angle of the current frame is reliable, and executes step 370 for normal processing.

[0075] In order to improve the accuracy and robustness of shaking judgment, the consistency of displacement and the stability of heading change of the tracking target in the past n frames (for example, n=5) are investigated to comprehensively judge whether there is a real shaking phenomenon at present.

[0076] For example, the displacement vector of each frame relative to the last frame in the history n frames is obtained. It is checked whether the directions of these continuous displacement vectors are basically consistent. A significant shaking feature is that the direction of the displacement vector frequently reverses by a large angle. The angle standard deviation between the average direction of all displacement vectors in n frames and each displacement vector direction is calculated. If the angle standard deviation exceeds a threshold value, it indicates that the displacement direction is extremely unstable and there is a suspicion of shaking.

[0077] For example, the measured heading angles of the history n frames are extracted to form a history heading sequence. The heading angle change between adjacent two frames is calculated to obtain n-1 heading changes. The standard deviation of the n-1 heading changes is calculated. If the standard deviation is small, it means that the heading change amplitude between every two frames is similar, which indicates that the heading change is stable and predictable. If the standard deviation is large, it means that the heading change is large and small, positive and negative, which is a typical mathematical feature of high-frequency shaking. When the calculated heading change standard deviation exceeds a preset threshold value, the MCSM determines that shaking occurs.

[0078] Step 360, the history stable heading angle of the tracking target is replaced by the measured heading angle of the current frame to participate in the measurement update process, and the credibility label is set to be unreliable.

[0079] Specifically, the MCSM replaces the history stable heading angle with the measured heading angle of the current frame, and then sends this "fake" reliable measurement value into the particle filter to participate in the subsequent measurement update process. The MCSM explicitly sets the credibility label of the current frame to "unreliable" to inform the upper module or the subsequent process that there is an abnormality in the current heading state.

[0080] Step 370, refresh the history stable heading angle of the tracking target according to the measured heading angle of the current frame, and set the credibility label to be reliable.

[0081] Specifically, when no significant jitter is detected at medium and high speeds, the MCSM considers the measured heading angle of the current frame to be reliable, and thus the MCSM can use the measured heading angle of the current frame to update (or smooth refresh) the historically stable heading angle maintained internally, so as to maintain the timeliness and accuracy of the historically stable heading angle. The MCSM also explicitly sets the reliability label of the current frame to "reliable".

[0082] To adapt to different target types such as vehicles and pedestrians, the MCSM adaptively adjusts the upper and lower limits of the low-speed hysteresis interval and the jitter threshold and the like through a characteristic mapping function, realizes multi-target support under a single code framework, and does not need to maintain multiple sets of logic.

[0083] The heading angle estimation method provided in the embodiments of the present application first effectively solves the decision stability problem at the low-speed critical point through the hysteresis mechanism; secondly, the jitter monitoring mechanism acts as an abnormal data filter when running at medium and high speeds, and a double protection mechanism is constructed, so that the MCSM can accurately label each frame of the measured heading angle as "reliable" or "unreliable" to guide the subsequent adoption of the most appropriate processing strategy, thereby greatly improving the robustness and output quality of the entire heading angle estimation.

[0084] Each independently tracked target is estimated in state by a dedicated particle filter instance. After the first frame of the measured heading angle of the newly detected tracking target is determined, a particle filter instance must be created and correctly initialized for it, so that subsequent frame-by-frame tracking can begin.

[0085] The quality of initialization directly affects the performance of the particle filter. A good initialization should accurately reflect the prior knowledge and uncertainty of the initial state of the tracking target. Since different types of tracking targets (such as vehicles and pedestrians) have completely different motion characteristics, the present application adopts an adaptive initialization strategy based on the type of target.

[0086] In some embodiments, the heading angle estimation method provided in the embodiments of the present application further includes: creating and initializing a particle filter instance based on the measured heading angle of the first frame; setting the initialization variance of the particle filter instance according to the type of the tracking target; wherein the initialization variance corresponding to a vehicle tracking target is smaller than the initialization variance corresponding to a pedestrian tracking target.

[0087] Specifically, a brand new, independent particle filter instance is created in memory. This instance is bound to a unique identifier of the tracked target to ensure no interference during multi-target tracking. A set of N particles (e.g., N=100) is generated for the particle filter instance. The measured heading angle of the first frame is used as the center value of the initial state. The settings for all N particle states will revolve around the heading angle of the first frame. That is, the initial heading angle of each particle is sampled from a distribution with the measured heading angle of the first frame as the mean.

[0088] In statistics, variance represents the degree of dispersion of a data distribution. Here, the initial variance directly controls the dispersion range of the initial particle swarm around the central value of the measured heading angle of the first frame. A larger variance means a higher degree of dispersion in the particle distribution, covering a wider state space; a smaller variance makes the particle distribution more concentrated, converging to a region with a higher posterior probability density.

[0089] For vehicle-tracking targets (i.e., highly predictable targets), based on prior knowledge that vehicles have significant inertia and smooth heading changes, we have a high degree of confidence that their true heading lies within a small range around the measured heading angle in the first frame. By setting a small variance, most particles are closely distributed around the center value, which helps the filter converge quickly to an accurate estimate and maintain stability in the initial stage.

[0090] For pedestrian tracking targets (i.e., targets with high uncertainty), the prior knowledge that pedestrians possess extremely high flexibility, almost no motion inertia, and can turn at any time is insufficient. There is no absolute certainty about the pedestrian's behavior in the next second, and their actual heading may deviate significantly from the measured heading angle in the first frame. By setting a large variance, the particles are distributed over a wider angular range, increasing the likelihood of capturing sudden changes in the target's direction, thereby avoiding early tracking failures due to overconfidence in the initial state.

[0091] After initialization, MCSM marks the particle filter as "ready".

[0092] The heading angle estimation method provided in this application employs small variance initialization for vehicle tracking targets, ensuring rapid convergence and stability of tracking. For pedestrian tracking targets, it employs large variance initialization, ensuring adaptability to abrupt changes and improving tracking robustness. By embedding the motion characteristics of different tracking targets into the initialization variance, the initialization process is automated, improving the overall performance of the heading angle estimation method in mixed traffic scenarios.

[0093] In some embodiments, a measurement update process is performed based on the measured heading angle of the current frame to obtain the filtered heading angle of the current frame, including: Based on the measurement heading angle of the current frame, the predicted heading angle of each particle in the current frame, and the observation noise standard deviation, the weight of each particle in the current frame is updated and normalized to obtain the normalized weight of each particle in the current frame; On the unit circle, the predicted heading angle of all particles in the current frame is weighted and averaged according to the normalized weight of all particles in the current frame to obtain the filtered heading angle of the current frame.

[0094] Specifically, each particle generates a predicted heading angle in the prediction stage according to its own motion model and noise. The particle filter uses a Gaussian likelihood model to calculate the matching degree between the weight of each particle in the current frame and the measurement heading angle of the current frame. The higher the matching degree, the higher the weight.

[0095] The expression of the weight of the i-th particle in the j-th frame is as follows: The expression of the weight of the i-th particle in the j-th frame is as follows: In the formula, represents the weight of the i-th particle in the j-th frame, represents the measurement heading angle of the j-th frame, represents the predicted heading angle of the i-th particle in the j-th frame based on the j-th frame, represents the predicted heading angle of the i-th particle in the j-th frame based on the j-th frame, represents the observation noise standard deviation. The weights of all particles in the current frame are normalized to ensure that the total weight is 1, and the normalized weight of each particle in the current frame is obtained. The particle filter weights and averages the predicted heading angle of all particles in the current frame according to the normalized weight of all particles in the current frame to obtain a single filtered heading angle of the current frame representing the consensus of the particle group. This weighted average is not a simple arithmetic average, but a weighted average on the unit circle considering the wrap-around characteristics of the angle, avoiding the modal problem of angle averaging. In some embodiments, the observation noise standard deviation is adjusted according to the type of the tracked target and the speed of the tracked target. Specifically, in the measurement update stage of the particle filter, the observation noise standard deviation is dynamically and adaptively adjusted according to the type of the tracked target and the speed of the tracked target to ensure that the particle filter maintains optimal performance in different situations.

[0096] In some embodiments, the observation noise standard deviation is adjusted according to the type of the tracked target and the speed of the tracked target.

[0097] Specifically, in the measurement update stage of the particle filter, the observation noise standard deviation is dynamically and adaptively adjusted according to the type of the tracked target and the speed of the tracked target to ensure that the particle filter maintains optimal performance in different situations.

[0098]

[0099] ​​​​In the case of tracking a vehicle, the motion of the vehicle is usually stable and has large inertia, so the change in heading is relatively smooth. Therefore, the measured heading angle obtained by the vehicle-mounted sensor or geometric calculation has high credibility in most cases. At this time, the observation noise standard deviation is set to be small. A small observation noise standard deviation means a high degree of trust in the measured heading angle, and the particle filter will pull the particle group more strongly to the measured heading angle, thereby achieving fast convergence and high-precision heading estimation.

[0100] In the case of tracking a pedestrian, the motion of the pedestrian is flexible and variable, and may include frequent and rapid turning, pausing and other random actions. This makes the measured heading angle calculated by the change in the position of the tracking target itself contain a higher noise component, or its uncertainty is greater. At this time, the observation noise standard deviation is set to be large. A large observation noise standard deviation reflects a relative caution for the measured heading angle, and the particle filter will allow a larger difference between the particle group and the measured heading angle when updating. This helps to avoid the measured noise caused by the instantaneous and random actions of the pedestrian, so that the particle filter does not produce violent jitter, thereby improving the robustness of the particle filter for flexible tracking targets such as pedestrians.

[0101] For a high-speed moving tracking target, when the tracking target speed is high, the tracking target moves a longer distance in a unit of time. At this time, even a small positioning error can cause a large deviation in the calculated measured heading angle. In addition, in high-speed motion, a small fluctuation in the true heading can also bring significant visual or perceptual differences. In order to cope with this increased uncertainty, the observation noise standard deviation is increased accordingly. The observation noise standard deviation makes the filter more stable in modifying the measured heading angle at high speed, avoiding the instability of the filter output caused by the amplification of the measured noise at high speed.

[0102] For a low-speed or stationary tracking target, when the speed of the tracking target is low or even stationary, the position change of the tracking target between consecutive frames may be very small, even lower than the resolution of the sensor or positioning system. At this time, the measured heading angle calculated by the position change is extremely susceptible to positioning noise and becomes very unstable, easily producing random directions. In this case, the observation noise standard deviation is appropriately increased, and the MCSM may even directly determine that the measured heading angle is not reliable, in which case the observation noise standard deviation can be set to infinity or directly skipped to update, to block the disturbance of the positioning noise to the heading. This echoes the "inherited heading" or "de-weighted update" strategy mentioned earlier, and together guarantees the stability of the heading in the low-speed scenario.

[0103] In some embodiments, the heading angle estimation method provided by the embodiments of the present application further comprises: The following steps are performed on any particle in any frame: The predicted heading angle of the current particle in the current frame is obtained according to the heading angle of the current particle in the last frame, the angular velocity of the current particle in the last frame, the frame time interval between the current frame and the last frame, and the process noise of the heading angle; The predicted angular velocity of the current particle in the current frame is obtained according to the angular velocity of the current particle in the last frame and the process noise of the angular velocity.

[0104] Specifically, in the heading angle estimation, the tracker employs a particle filter to estimate the state of the tracked target. The core idea of the particle filter is to represent the posterior probability distribution of the target state by a large number of random particles with weights. Each particle contains a set of state variables, including at least the heading angle and the angular velocity.

[0105] When processing each frame of data, the particle filter first performs a prediction stage, which advances the state of each particle from the last frame to the current frame. The following steps are performed independently for each particle in the prediction stage: The prediction of the heading angle by the particle filter is usually based on a Nearly Constant Turn Rate Model. This model assumes that the tracked target will maintain approximately the turn rate of the last frame for a short period of time (i.e., the frame time interval Therefore, the expression of the predicted heading angle of the th particle in the th frame is as follows: In the expression, φ represents the predicted heading angle of the th particle in the th frame based on the th frame, represents the predicted heading angle of the th particle in the th frame, represents the predicted angular velocity of the th particle in the th frame, represents the frame time interval between the th frame and the th frame, represents the process noise of the heading angle.

[0106] The process noise of the heading angle is a random variable, which is used to model the uncertainty of the motion model. In the real world, the heading change of a target will not perfectly conform to the physical formula, but will be affected by various unpredictable factors such as the micro-operation of the driver, the road bumps, the wind resistance, etc. This noise term is a mathematical modeling of these uncertainties.

[0107] The predicted heading angle of each particle at the current frame is the "best guess" of the particle filter on the heading of the tracked target before it sees the measured heading angle of the current frame.

[0108] The prediction of the angular velocity by the particle filter usually adopts a random walk model. This model assumes that the angular velocity of the tracked target at the next time instance will be approximately equal to the angular velocity at the current time instance, but with a random perturbation added. Thus, the predicted angular velocity of the i-th particle at the k-th frame is given by: where In the above equation, denotes the predicted angular velocity of the i-th particle at the k-th frame based on the (k-1)-th frame, denotes the predicted angular velocity of the i-th particle at the k-th frame, denotes the process noise of the angular velocity.

[0109] The process noise of the angular velocity is a random variable that models the variation of the angular velocity itself. For example, when a vehicle is turning, the driver can increase or decrease the rotation rate of the steering wheel at any time. This noise term enables the particle filter to track and adapt to such variations of the angular velocity.

[0110] In some embodiments, the method for estimating the heading angle further comprises: In the case that the current particle is a pedestrian particle, the predicted angular velocity of the current particle at the current frame is obtained according to the angular velocity of the current particle at the last frame, the angular velocity decay factor and the process noise of the angular velocity.

[0111] Specifically, the turning of a pedestrian is usually a short and transient behavior, rather than a sustained motion state. For example, a pedestrian can quickly turn his / her head / body to avoid or observe something, but then quickly resume walking in a straight line. The standard random walk model cannot effectively capture the tendency of the angular velocity "returning to zero", which can lead to an overestimation of the duration of the pedestrian's turning. To this end, for the pedestrian particle, an angular velocity decay factor is introduced to more realistically model its motion characteristics.

[0112] First, the angular velocity at the last frame is multiplied by the angular velocity decay factor. This step mathematically models the physical process that after a pedestrian completes a turn, his / her body naturally returns to the straight position and the rotation rate gradually decreases. Then, the process noise of the angular velocity is added to model the occurrence of a new and unpredictable turning behavior. This results in a predicted angular velocity of the pedestrian particle at the current frame that is more consistent with its behavior characteristics. ​​​​​​

[0113] For pedestrian particles, the expression of the predicted angular velocity of the i-th particle at the (k+1)-th frame is as follows:

[0114] The angular velocity decay factor

[0115] The heading angle estimation method provided by the embodiments of the present application introduces an angular velocity decay factor to more realistically simulate the motion characteristics of pedestrians and achieve more accurate angular velocity prediction.

[0116] In some embodiments, the process noise of the heading angle and the process noise of the angular velocity are both adjusted according to the type of the tracked target and the speed of the tracked target.

[0117] Specifically, in the prediction stage of the particle filter, the process noise is used to simulate the uncertainty between the motion model and the real world. Traditional methods usually use fixed process noise parameters, but this is less adaptive in dynamic traffic environments. For example, a vehicle stably driving on a highway and a pedestrian slowly creeping in a parking lot have completely different uncertainties in motion.

[0118] To solve this problem, the present application proposes an adaptive process noise adjustment mechanism. The core idea of the mechanism is to dynamically adjust the basic process noise variance by combining a unified basic process noise variance with the type of the tracked target and the real-time speed of the tracked target. The adjusted basic process noise variance will be used to construct the process noise covariance matrix, thereby systematically changing the process noise of state quantities such as the heading angle and the angular velocity. This enables the prediction model of the particle filter to intelligently adapt to the behavior patterns of different targets in different motion states.

[0119] ​​​​​​​​​​​​​The mechanism first sets a baseline process noise level based on the type of target being tracked. For pedestrian targets with high motion pattern uncertainty, a relatively large baseline process noise variance is configured. This increases the model's tolerance to motion uncertainty, allowing it to cover a wider state space during the prediction phase, thus effectively capturing potential sudden turns by pedestrians. Conversely, for vehicle targets with more stable motion patterns, a smaller baseline process noise variance is configured to enhance the convergence and stability of state estimation, avoiding excessive divergence in predicted trajectories due to excessive noise.

[0120] Above this baseline noise level, the process noise adjustment mechanism further introduces a dynamic scaling factor that is positively correlated with the real-time velocity of the tracked target. As the target velocity increases, this scaling factor increases, proportionally amplifying the process noise variances of the heading angle and angular velocity derived from the basic process noise variance. This significantly improves the response speed of the particle filter in high-dynamic scenarios, enabling it to quickly follow heading changes during high-speed motion and effectively suppressing tracking lag. Conversely, when the target decelerates or is at a low speed, this scaling factor decreases, reducing the overall process noise level and ensuring smooth and stable state estimation, preventing unnecessary heading estimation jitter due to noise disturbances at low speeds.

[0121] Particle filters inherently suffer from "particle degradation" during iteration. After multiple weight updates, a few particles develop very large weights, while the weights of the vast majority approach zero. This leads to a sharp decrease in particle diversity and a reduction in the number of effective samples, affecting the accuracy of state estimation and potentially causing tracking failure. To proactively monitor and address this issue, this application proposes a closed-loop processing flow that includes state monitoring, selective resampling, and emergency self-recovery.

[0122] Figure 4 This is the fourth schematic diagram of the heading angle estimation method provided in the embodiments of this application, as shown below. Figure 4 As shown, the heading angle estimation method provided in this application embodiment further includes the following steps: Step 410: Calculate the effective number of particles in the current frame based on the total number of particles and the weights of all particles in the current frame.

[0123] Specifically, after each frame's measurement update, a quantitative assessment of the particle swarm's diversity is performed. The weights of all particles in the current frame are the set of weights after measurement updates and normalization. The total number of particles is the total number of particles configured in the particle filter.

[0124] In the formula, Indicates the first Effective number of particles per frame represents the weight of the th particle in the th frame, represents the total number of particles.

[0125] Step 420, determine whether the number of valid particles in the current frame is less than the valid particle threshold.

[0126] Specifically, if the number of valid particles in the current frame is greater than or equal to the valid particle threshold, it indicates that the diversity of the particle group is sufficient and no serious degradation has occurred. At this time, step 430 is executed. If the number of valid particles in the current frame is less than the valid particle threshold, it indicates that the particle degradation has reached a degree that requires intervention, and step 440 is executed to prepare for resampling.

[0127] Step 430, skip resampling.

[0128] Specifically, no resampling operation is performed. All particles and their weights will be directly used in the prediction stage of the next frame.

[0129] Step 440, determine whether the number of valid particles after particle resampling is greater than or equal to the valid particle threshold.

[0130] Specifically, a pre-judgment of resampling is performed to determine whether the execution of resampling can effectively solve the problem of particle diversity. In some extreme cases (for example, only one particle has a weight of 1 and the rest are 0), resampling will only infinitely duplicate this one particle and cannot restore diversity.

[0131] If it is determined that the number of valid particles after particle resampling is greater than or equal to the valid particle threshold, it indicates that resampling is sufficient to restore the diversity of the particle group, and step 450 is executed. If it is determined that the number of valid particles after particle resampling is still less than the valid particle threshold, it indicates that the particle group has fallen into deep degradation and the resampling means has failed, and step 460 is executed to start more powerful recovery measures.

[0132] Step 450, perform particle resampling in a fixed starting point and equal interval sampling manner, and set the weight of all resampled particles to the reciprocal of the total number of particles.

[0133] Specifically, particle resampling is performed in a fixed starting point and equal interval sampling manner, which ensures that the selection probability of a particle is proportional to its weight, thereby achieving the proliferation of high-weight particles and the rejection of low-weight particles. Compared to pure random roulette sampling, this manner can more uniformly extract samples from the old particle group, reducing the randomness of the sampling process and making the result more stable.

[0134] After resampling, the weights of all newly generated particles are reset to a uniform distribution, meaning that the weight of each particle becomes 1 / N, which puts all particles on the same starting line in the next round of competition.

[0135] Step 460 triggers the particle diversity self-recovery process.

[0136] Specifically, the particle diversity self-recovery process includes a series of progressively escalating interventions, such as: Re-injecting noise: After resampling, a small amount of random noise is artificially added to the state (heading angle, angular velocity) of all new particles, forcing them to "shake out" in order to create diversity.

[0137] Completely reset particle states: If injecting noise is still ineffective, discard all current particle states and regenerate a completely new, healthy set of particles centered on the most recent reliable measured heading angle or a historical stable heading angle. This is equivalent to performing a "hard reboot" on the particle filter.

[0138] The heading angle estimation method provided in this application first quantifies and monitors the effective particle count. When insufficient particle diversity is determined, efficient system resampling is selectively performed as a routine treatment. When system resampling is determined to be ineffective, an emergency self-recovery process is initiated as a fallback. This mechanism ensures that the particle filter will not gradually fail due to sample depletion, greatly enhancing the stability and reliability of the entire heading angle estimation method under long-term, complex scenarios.

[0139] Figure 5 This is the fifth schematic diagram of the heading angle estimation method provided in the embodiments of this application, as shown below. Figure 5 As shown, the heading angle estimation method provided in this application embodiment further includes the following steps: Step 510: When the tracked target is within the effective coverage area of ​​the map, obtain the heading angle of the road centerline in the current frame and the confidence level corresponding to the heading angle of the road centerline in the current frame.

[0140] Specifically, it determines whether the current location of the tracked target is within the effective coverage area of ​​the loaded high-precision map. If this condition is met, the internal road matching algorithm is invoked to match the current location of the tracked target to the most probable road centerline on the map, and the heading angle of the road centerline in the current frame and the corresponding confidence level are obtained.

[0141] The road centerline heading angle, which is the direction of the matched road centerline itself, is a theoretical reference value for heading. Confidence level is a quantified score representing the reliability of the road matching. For example, matching on a simple straight road results in high confidence; matching at complex intersections or when GPS signals are weak results in low confidence.

[0142] Step 520, judge whether the angle difference between the filtered heading angle of the current frame and the road centerline heading angle of the current frame is less than or equal to the angle difference threshold value.

[0143] Specifically, if the angle difference between the filtered heading angle of the current frame and the road centerline heading angle of the current frame is less than or equal to the angle difference threshold value, it indicates that the driving direction of the tracked target is basically consistent with the road direction, and the fusion is reasonable, then the process enters the next step of confidence check, that is, step 230 is executed.

[0144] If the angle difference between the filtered heading angle of the current frame and the road centerline heading angle of the current frame is greater than the angle difference threshold value, it indicates that the driving direction of the tracked target is significantly inconsistent with the road direction, which may mean that the tracked target is performing special operations not reflected on the map (such as U-turn, transverse parking), or the map matching itself has errors. In this abnormal case, blindly fusion is dangerous, then it is decided to give up this fusion, and step 540 is executed.

[0145] Step 530, judge whether the confidence corresponding to the road centerline heading angle of the current frame is greater than or equal to the confidence threshold value.

[0146] Specifically, if the confidence corresponding to the road centerline heading angle of the current frame is greater than or equal to the confidence threshold value, it indicates that the map matching result is very reliable and can be used with confidence, and the process enters the final correction step, that is, step 550 is executed. If the confidence corresponding to the road centerline heading angle of the current frame is less than the confidence threshold value, it indicates that the map matching result has great uncertainty and is not suitable for correcting the heading, and it is also decided to give up this fusion, and step 540 is executed.

[0147] Step 540, take the filtered heading angle of the current frame as the heading angle estimation result of the current frame.

[0148] Specifically, the filtered heading angle of the current frame output by the particle filter is directly taken as the heading angle estimation result of the current frame. This ensures that when the map information is unreliable or inconsistent with the behavior of the tracked target, it can fall back to the pure sensor filtering mode, ensuring the robustness of the algorithm.

[0149] Step 550, correct the filtered heading angle of the current frame using the road centerline heading angle of the current frame and the confidence corresponding to the road centerline heading angle of the current frame, to obtain the corrected filtered heading angle of the current frame.

[0150] Specifically, the filtered heading angle of the current frame is corrected using a weighted fusion strategy. The weight of the fusion is a function of the confidence. The higher the confidence, the greater the weight, and the stronger the force of the filtered heading being "pulled" to the map heading. The first The expression of the frame-corrected filtered heading angle is as follows: In the formula, represents the frame-corrected filtered heading angle, represents the frame-corrected filtered heading angle, represents the frame-corrected filtered heading angle, represents the frame-corrected filtered heading angle. represents the

[0151] Step 560, taking the frame-corrected filtered heading angle as the heading angle estimation result of the current frame.

[0152] Specifically, the filtered heading angle that is corrected by the map information and has improved precision and stability is taken as the final heading angle estimation result of the current frame.

[0153] The heading angle estimation method provided by the embodiment of the application ensures that the map information is introduced only under the premise of safety and reliability through the double-checking mechanism of the angle difference and the confidence. This not only effectively utilizes the advantages of the high-precision map to eliminate the cumulative error of the sensor and improve the absolute precision of the heading, but also intelligently gives up fusion and guarantees the robustness when the map information is missing, low in quality or inconsistent with the actual driving behavior, thereby achieving perfect balance between the precision and the reliability.

[0154] It should be noted that the tracker provides, to the outside (such as the upper module of path planning, behavior prediction, etc.), not only a single heading angle value, but also a structured data packet containing rich context information, and the main contents include: the heading angle estimation result, the confidence label, the map correction state flag and other additional information.

[0155] In the optimal case, the heading angle estimation result is the result corrected by the map information fusion; in the lock angle and other specific abnormal cases, the heading angle estimation result is the locked heading that is forced to be output; and in the normal case, the heading angle estimation result is the output of the particle filter.

[0156] The map correction state flag is used to explicitly indicate whether the heading angle estimation result is corrected by the map information. When this flag is true, it means that the heading angle estimation result has very high absolute precision and reliability.

[0157] To facilitate performance analysis, problem localization and continuous optimization of the algorithm, the tracker provides a set of fine-grained logging and output mechanisms, which can record and output detailed "snapshots" of its internal state at each frame for one or more specific tracking targets.

[0158] The log content includes: basic information (such as timestamp, tracking target identifier, current speed, etc.), heading evolution information (such as the heading angle of the last frame, the predicted heading angle of the current frame, the measured heading angle, and the finally output heading angle), and internal state information (such as particle weight distribution, effective particle number, credibility label, etc.).

[0159] When tracking abnormalities occur, the developer or test engineer can refer to this expert log to trace back the complete decision chain of the algorithm at each frame, thereby quickly and accurately locating the root cause of the problem, greatly improving the development and debugging efficiency.

[0160] To ensure long-term stable operation and effective use of computing and memory resources, the present application implements an automatic target lifecycle management mechanism. The tracker maintains a "presence duration" or "absence duration" counter for each target. When a tracking target is confirmed to leave (exit the sensor field of view) or lose tracking for too long (e.g., continuously undetected for multiple frames due to long-term occlusion), the entire set of resources created for that target, including its exclusive MCSM instance and particle filter instance, will be automatically destroyed and released.

[0161] This mechanism effectively avoids memory leaks and computing power waste caused by the persistent existence of "zombie targets", ensuring efficient and stable operation in scenarios with a large number of dynamic targets. When a released target reappears, it is treated as a completely new target, and the complete first-frame initialization process is performed again.

[0162] To deal with major systemic events that may occur, causing the particle filter state to completely fail, the present application designs a final and reliable "hard reset" process. Triggering conditions can be: a change in the main data source, or a target being detected again after a long time of losing tracking, with its internal prediction state severely diverging from the true state, or discovering through internal diagnosis (such as particle diversity consistently being zero) that the filter has entered an abnormal state that cannot be recovered through conventional means.

[0163] The execution operation can be: decisively discarding all current untrustworthy particle states, or considering the current time as a completely new starting point, calling the first-frame initialization process to obtain a most reliable new initial heading for the tracking target, or based on the new first-frame heading, recreating a clean particle filter instance.

[0164] The "re-initialization" mechanism gives strong self-healing ability. It ensures that the heading estimation can resolutely recover from a reliable new starting point after experiencing drastic changes or serious errors, thus maintaining the continuity and reliability of the heading estimation in highly dynamic and uncertain traffic environments.

[0165] Figure 6 The heading angle optimization before and after comparison chart provided for the embodiments of the application, Figure 6 The performance comparison of the heading angle estimation in the straight-acceleration-turning comprehensive scene is shown. As shown in Figure 6 The test object is an example vehicle (sampling frequency 10Hz), the short dotted line is the real-time dynamic differential positioning (Real-Time Kinematic, RTK) heading angle true value, the dotted line is the heading angle output by the algorithm before optimization, and the solid line is the output heading angle of the application. It can be observed that in the low-speed acceleration section from 7.5s to 10.5s, the application significantly suppresses the jitter caused by positioning noise in the traditional method. In the turning stage from 4s, the improvement of the heading lag and peak error of the application is more obvious. These results show that the motion consistency state machine and adaptive particle filtering combined strategy proposed in the application can continuously output a heading angle highly consistent with the true value, especially in difficult working conditions such as acceleration and turning, it still remains stable and reliable, verifying the actual effectiveness of the heading angle optimization method.

[0166] Figure 7 The structural schematic diagram of the electronic device provided for the embodiments of the application is shown in Figure 7 The electronic device includes a memory 720, a transceiver 710 and a processor 700; wherein the processor 700 and the memory 720 can also be arranged physically separately.

[0167] The memory 720 is used to store a computer program; the transceiver 710 is used to transceive data under the control of the processor 700.

[0168] Wherein, in Figure 7In particular embodiments, the bus architecture can include any number of interconnecting buses and bridges, depending on the specific application of processor 700 and the overall design constraints. The bus architecture can link various circuits of the various circuitries represented by the processor 700 and the memory 720, one or more processors and memories, respectively. The bus architecture can also link various other circuits, such as peripheral devices, voltage regulators, and power management circuitries, which are well known in the art and will not be further described. The bus interface provides an interface. The transceiver 710 can be a plurality of elements, i.e., including a transmitter and a receiver, providing a means for communicating with various other apparatus over a transmission medium, including a wireless channel, a wired channel, an optical cable, and the like. The user interface 730 can also be an interface capable of externally connecting the required devices for different user equipment, including but not limited to a keypad, a display, a speaker, a microphone, a joystick, and the like.

[0169] The processor 700 is responsible for managing the bus architecture and general processing, and the memory 720 can store data used by the processor 700 in the execution of operations.

[0170] The processor 700 can be a central processing unit (CPU), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA), or a complex programmable logic device (CPLD), and the processor can also adopt a multi-core architecture.

[0171] The processor 700, by invoking the computer program stored in the memory 720, is configured to execute any of the heading angle estimation methods provided by the embodiments of the present application according to the executable instructions obtained, including: For each frame of the tracking target, the following steps are performed: In the case that the credibility label corresponding to the measured heading angle of the current frame is credible, a measurement update process is performed based on the measured heading angle of the current frame to obtain a filtered heading angle of the current frame; and the filtered heading angle of the current frame is taken as the heading angle estimation result of the current frame. In the case that the credibility label corresponding to the measured heading angle of the current frame is not credible, the measurement update process is performed after increasing the observation noise standard deviation; or, The measurement update process is skipped, and the predicted heading angle of the current frame is taken as the heading angle estimation result of the current frame; or, The measured heading angle of the current frame is replaced by the historical stable heading angle of the tracking target, and the measurement update process is performed.

[0172] In some embodiments, the processor 700 further performs: when the tracking target is in the map valid coverage area, obtaining a road centerline heading angle of the current frame and a confidence degree corresponding to the road centerline heading angle of the current frame; when an angle difference between the filtered heading angle of the current frame and the road centerline heading angle of the current frame is less than or equal to an angle difference threshold, and the confidence degree is greater than or equal to a confidence degree threshold, correcting the filtered heading angle of the current frame by using the road centerline heading angle of the current frame and the confidence degree, to obtain a corrected filtered heading angle of the current frame; and taking the corrected filtered heading angle of the current frame as the heading angle estimation result of the current frame; when the angle difference is greater than the angle difference threshold, and / or the confidence degree is less than the confidence degree threshold, taking the filtered heading angle of the current frame as the heading angle estimation result of the current frame.

[0173] In some embodiments, the processor 700 further performs: when the speed of the tracking target in the current frame is greater than an upper limit of a low-speed hysteresis interval, and the number of continuous jittering is greater than a jittering threshold, replacing the measured heading angle of the current frame by the historical stable heading angle of the tracking target to participate in the measurement update process, and setting the credibility label as untrusted; when the speed of the tracking target in the current frame is greater than the upper limit of the low-speed hysteresis interval, and the number of continuous jittering is less than or equal to the jittering threshold, refreshing the historical stable heading angle of the tracking target according to the measured heading angle of the current frame, and setting the credibility label as trusted; when the speed of the tracking target in the current frame is less than a lower limit of the low-speed hysteresis interval, taking the historical stable heading angle of the tracking target as the heading angle estimation result of the current frame; when the speed of the tracking target in the current frame is in the low-speed hysteresis interval, the MCSM maintains the credibility label unchanged.

[0174] In some embodiments, the measurement update process based on the measured heading angle of the current frame is performed to obtain the filtered heading angle of the current frame, including: updating and normalizing the weight of each particle in the current frame based on the measured heading angle of the current frame, the predicted heading angle of each particle in the current frame and the observation noise standard deviation, to obtain the normalized weight of each particle in the current frame; On the unit circle, a weighted average of the predicted heading angles of all particles in the current frame is performed according to the normalized weights of all particles in the current frame, to obtain a filtered heading angle of the current frame.

[0175] In some embodiments, the method further comprises: For any particle in any frame, the following steps are performed: A predicted heading angle of the current particle in the current frame is obtained according to the heading angle of the current particle in the previous frame, the angular velocity of the current particle in the previous frame, a frame time interval between the current frame and the previous frame, and process noise of the heading angle. A predicted angular velocity of the current particle in the current frame is obtained according to the angular velocity of the current particle in the previous frame and process noise of the angular velocity.

[0176] In some embodiments, the processor 700 further performs: In the case that the current particle is a pedestrian particle, a predicted angular velocity of the current particle in the current frame is obtained according to the angular velocity of the current particle in the previous frame, an angular velocity decay factor, and process noise of the angular velocity.

[0177] In some embodiments, the processor 700 further performs: For each frame, the following steps are performed: An effective particle number of the current frame is calculated according to the total number of particles and the weights of all particles in the current frame. In the case that the effective particle number of the current frame is greater than or equal to an effective particle threshold, a resampling procedure is skipped. In the case that the effective particle number of the current frame is less than the effective particle threshold, and it is determined that the effective particle number after particle resampling is greater than or equal to the effective particle threshold, particle resampling is performed in a fixed starting point and equal interval sampling manner, and the weight of all resampled particles is set to the reciprocal of the total number of particles. In the case that the effective particle number of the current frame is less than the effective particle threshold, and it is determined that the effective particle number after particle resampling is less than the effective particle threshold, a particle diversity self-recovery procedure is triggered.

[0178] In some embodiments, the processor 700 further performs: When the tracking target is initially detected, in the case that there is a heading angle provided by a sensor that is not disturbed by a magnetic field, the heading angle provided by the sensor is taken as a measured heading angle of the first frame. In the case that there is no heading angle provided by a sensor that is not disturbed by a magnetic field, and the tracking target is located in a lock angle region, a preset heading angle is taken as a measured heading angle of the first frame. In the absence of a heading angle provided by a sensor that is not disturbed by a magnetic field, and in the absence of the tracking target being located in a lock angle region, a first frame measurement heading angle is calculated based on position points of the tracking target in two consecutive frames.

[0179] In some embodiments, the processor 700 also performs: Based on the first frame measurement heading angle, a particle filter instance is created and initialized; According to the type of the tracking target, the initialization variance of the particle filter instance is set; wherein the initialization variance corresponding to a vehicle tracking target is less than the initialization variance corresponding to a pedestrian tracking target.

[0180] In some embodiments, the observation noise standard deviation, the process noise of the heading angle, and the process noise of the angular velocity are all adjusted according to the type of the tracking target and the speed of the tracking target.

[0181] It should be noted that the electronic device provided by the embodiments of the present application can realize all the method steps realized by the corresponding method embodiments described above, and achieve the same technical effects. Therefore, the same parts and beneficial effects of the method embodiments will not be described in detail here.

[0182] Figure 8 The structure diagram of the heading angle estimation device provided by the embodiments of the present application is shown in Figure 8 The embodiments of the present application provide a heading angle estimation device, which comprises: The measurement update module 810 is configured to perform the following steps for each frame of the tracking target: In the case that the confidence label corresponding to the measurement heading angle of the current frame is trusted, a measurement update process is performed based on the measurement heading angle of the current frame to obtain a filtered heading angle of the current frame; and the filtered heading angle of the current frame is taken as the heading angle estimation result of the current frame; In the case that the confidence label corresponding to the measurement heading angle of the current frame is not trusted, the measurement update process is performed after increasing the observation noise standard deviation; or, The measurement update process is skipped, and the predicted heading angle of the current frame is taken as the heading angle estimation result of the current frame; or, The measurement heading angle of the current frame is replaced by a historical stable heading angle of the tracking target, and the measurement update process is performed.

[0183] In some embodiments, the device further comprises a map correction module, wherein the map correction module is configured to: When the tracking target is in a map effective coverage area, the road centerline heading angle of the current frame and the confidence corresponding to the road centerline heading angle of the current frame are obtained. In a case where the angle difference between the filtered heading angle of the current frame and the road centerline heading angle of the current frame is less than or equal to an angle difference threshold, and the confidence is greater than or equal to a confidence threshold, the filtered heading angle of the current frame is corrected based on the road centerline heading angle of the current frame and the confidence, to obtain a corrected filtered heading angle of the current frame; and the corrected filtered heading angle of the current frame is taken as the heading angle estimation result of the current frame. In a case where the angle difference is greater than the angle difference threshold, and / or, the confidence is less than the confidence threshold, the filtered heading angle of the current frame is taken as the heading angle estimation result of the current frame.

[0184] In some embodiments, the device further comprises a judging module configured to: In a case where the speed of the tracking target in the current frame is greater than the upper limit of the low-speed hysteresis interval, and the number of continuous jittering is greater than a jittering threshold, the historical stable heading angle of the tracking target is used to replace the measured heading angle of the current frame to participate in the measurement update process, and the confidence label is set as untrusted; In a case where the speed of the tracking target in the current frame is greater than the upper limit of the low-speed hysteresis interval, and the number of continuous jittering is less than or equal to the jittering threshold, the historical stable heading angle of the tracking target is refreshed according to the measured heading angle of the current frame, and the confidence label is set as trusted; In a case where the speed of the tracking target in the current frame is less than the lower limit of the low-speed hysteresis interval, the historical stable heading angle of the tracking target is taken as the heading angle estimation result of the current frame; In a case where the speed of the tracking target in the current frame is within the low-speed hysteresis interval, the confidence label is maintained unchanged by the MCSM.

[0185] In some embodiments, the measurement update module 810 is configured to: Based on the measured heading angle of the current frame, the predicted heading angle of each particle in the current frame, and the observation noise standard deviation, the weight of each particle in the current frame is updated and normalized to obtain the normalized weight of each particle in the current frame; On the unit circle, the predicted heading angle of all particles in the current frame is weighted and averaged according to the normalized weight of all particles in the current frame, to obtain the filtered heading angle of the current frame.

[0186] In some embodiments, the device further comprises a prediction module configured to: The following steps are performed on any particle in any frame: obtaining a predicted heading angle of the current particle in the current frame according to a heading angle of the current particle in a previous frame, an angular velocity of the current particle in the previous frame, a frame time interval between the current frame and the previous frame, and process noise of the heading angle; obtaining a predicted angular velocity of the current particle in the current frame according to the angular velocity of the current particle in the previous frame and process noise of the angular velocity.

[0187] In some embodiments, the prediction module is further configured to: in a case where the current particle is a pedestrian particle, obtaining a predicted angular velocity of the current particle in the current frame according to the angular velocity of the current particle in the previous frame, an angular velocity decay factor, and process noise of the angular velocity.

[0188] In some embodiments, the device further comprises a particle diversity module configured to: perform the following steps for each frame: calculating a number of effective particles in the current frame according to a total number of particles and weights of all particles in the current frame; in a case where the number of effective particles in the current frame is greater than or equal to an effective particle threshold, skipping a resampling procedure; in a case where the number of effective particles in the current frame is less than the effective particle threshold and a number of effective particles after particle resampling is determined to be greater than or equal to the effective particle threshold, performing particle resampling in a fixed starting point and equal interval sampling manner, and setting weights of all resampled particles to be inverses of the total number of particles; in a case where the number of effective particles in the current frame is less than the effective particle threshold and the number of effective particles after particle resampling is determined to be less than the effective particle threshold, triggering a particle diversity self-recovery procedure.

[0189] In some embodiments, the device further comprises an initialization module configured to: in a case where a heading angle provided by a sensor not affected by a magnetic field is present when the tracking target is initially detected, taking the heading angle provided by the sensor as a measured heading angle of a first frame; in a case where the tracking target is located in a lock angle region when the heading angle provided by the sensor not affected by the magnetic field is not present, taking a preset heading angle as the measured heading angle of the first frame; in a case where the tracking target is not located in the lock angle region when the heading angle provided by the sensor not affected by the magnetic field is not present, calculating the measured heading angle of the first frame according to position points of the tracking target in two consecutive frames.

[0190] In some embodiments, the device further comprises a creation module configured to: create and initialize a particle filter instance based on the measured heading angle of the first frame; set an initialization variance of the particle filter instance according to a type of the tracking target; wherein the initialization variance corresponding to a vehicle tracking target is smaller than the initialization variance corresponding to a pedestrian tracking target.

[0191] In some embodiments, the observation noise standard deviation, the process noise of the heading angle, and the process noise of the angular velocity are all adjusted according to the type of the tracking target and the speed of the tracking target.

[0192] It should be noted that the above apparatus provided by the embodiments of the present application can realize all the method steps achieved by the corresponding method embodiments, and achieve the same technical effects. Therefore, the same parts and beneficial effects of the method embodiments will not be described in detail herein.

[0193] It should be noted that the division of units in the embodiments of the present application is illustrative, and is only a logical functional division. In actual implementation, another division mode can be used. In addition, each functional unit in each embodiment of the present application can be integrated in one processing unit, or each unit can exist physically, or two or more units can be integrated in one unit. The integrated unit can be realized in the form of hardware or in the form of a software functional unit.

[0194] When the integrated unit is realized in the form of a software functional unit and sold or used as an independent product, it can be stored in a processor-readable storage medium. Based on this understanding, the technical solutions of the present application or the part that essentially contributes to the related art or the whole or part of the technical solutions can be embodied in the form of a software product. The computer software product is stored in a storage medium, and includes a plurality of instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) or a processor to execute all or part of the steps of the methods described in the embodiments of the present application. The aforementioned storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, and various storage medium that can store program codes.

[0195] Based on the same concept, in some embodiments, a processor-readable storage medium is also provided, which stores a program for causing a processor to execute the methods provided by the above-mentioned method embodiments.

[0196] The storage medium can be any available medium that the processor can access. For example, such a computer-readable medium can comprise a RAM, a ROM, an EEPROM, a CD-ROM, or other optical storage (such as CD, DVD, BD, HVD, etc.), a magnetic disk storage or other magnetic storage devices (such as a floppy disk, a hard disk, a magnetic tape, a magneto-optical disk (MO), or any other medium that can be used to carry or store desired program code in the form of instructions or data structures and that can be accessed by a computer.

[0197] The processor readable storage medium provided by the embodiments of the present application can realize all the method steps of the method embodiments and achieve the same technical effects. Here, the same parts and beneficial effects of the embodiments as the method embodiments will not be repeated in detail.

[0198] Based on the same idea, in some embodiments, a chip is also provided, wherein a program is stored in the chip, and the program is used to make the chip execute the method provided by the method embodiments.

[0199] Specifically, the chip provided by the embodiments of the present application can realize all the method steps of the method embodiments and achieve the same technical effects. Here, the same parts and beneficial effects of the embodiments as the method embodiments will not be repeated in detail.

[0200] Based on the same idea, in some embodiments, a computer program product is also provided, which comprises a computer program or instructions, and when the computer program or instructions run on a computer, the computer executes the method provided by the above embodiments.

[0201] Specifically, the computer program product provided by the embodiments of the present application can realize all the method steps of the method embodiments and achieve the same technical effects. Here, the same parts and beneficial effects of the embodiments as the method embodiments will not be repeated in detail.

[0202] It should also be understood that the memory mentioned in the embodiments of the present application can be a volatile memory and / or a non-volatile memory. Among them, the non-volatile memory can be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically EPROM (EEPROM) or a flash memory. The volatile memory can be a random access memory (RAM). For example, the RAM can be used as an external cache. As an example but not limitation, the RAM includes the following various forms: static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM) and direct rambus RAM (DR RAM). It should be noted that when the processor is a general processor, a DSP, an ASIC, a FPGA or other programmable logic device, a discrete gate or transistor logic device, a discrete hardware component, the memory (storage module) can be integrated in the processor.

[0203] Those skilled in the art should understand that the embodiments of the present application can be provided as a method, a system or a computer program product. Therefore, the present application can take the form of a complete hardware embodiment, a complete software embodiment or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage and optical storage, etc.) containing computer-usable program code.

[0204] The computer executable instructions can be provided to one or more processors of the computer, other programmable data processing apparatus, or other devices to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, implement the functions specified in the flowchart block or blocks and / or in conjunction with the other blocks in the flowcharts. Figure 1 one or more computer-executable instructions to implement one or more functions specified in the flowchart block or blocks. Figure 1 one or more computer-executable instructions to implement one or more functions specified in the flowchart block or blocks.

[0205] These processor-executable instructions can also be loaded onto a computer, other programmable data processing apparatus, or other devices to cause a series of operations to be performed on the computer, other programmable data processing apparatus, or other devices to produce a computer implemented process such that the instructions which execute on the computer or other programmable data processing apparatus implement the functions specified in the flowchart block or blocks. Figure 1 one or more computer-executable instructions to implement one or more functions specified in the flowchart block or blocks. Figure 1 one or more computer-executable instructions to implement one or more functions specified in the flowchart block or blocks.

[0206] Obviously, numerous modifications and variations of the present application are possible in light of the above teachings. It is therefore to be understood that within the scope of the claims and their equivalents, the application can be practiced otherwise than as specifically described.

Claims

1. A method for estimating heading angle, characterized in that, The method includes: Perform the following steps for each frame of the tracked target: If the confidence label corresponding to the measured heading angle of the current frame is reliable, a measurement update process is performed based on the measured heading angle of the current frame to obtain the filtered heading angle of the current frame; and the filtered heading angle of the current frame is used as the heading angle estimation result of the current frame. If the confidence label corresponding to the measured heading angle in the current frame is unreliable, the measurement update process is executed after increasing the standard deviation of the observation noise; or, Skip the measurement update process and use the predicted heading angle of the current frame as the heading angle estimation result of the current frame; or, The measured heading angle of the current frame is replaced with the historical stable heading angle of the tracked target, and the measurement update process is performed.

2. The heading angle estimation method according to claim 1, characterized in that, The method further includes: When the tracked target is within the effective coverage area of ​​the map, obtain the road centerline heading angle of the current frame and the confidence level corresponding to the road centerline heading angle of the current frame; If the angle difference between the filtered heading angle of the current frame and the road centerline heading angle of the current frame is less than or equal to an angle difference threshold, and the confidence level is greater than or equal to a confidence level threshold, the filtered heading angle of the current frame is corrected using the road centerline heading angle and the confidence level to obtain the corrected filtered heading angle of the current frame; the corrected filtered heading angle of the current frame is used as the heading angle estimation result of the current frame. If the angle difference is greater than the angle difference threshold and / or the confidence level is less than the confidence level threshold, the filtered heading angle of the current frame is used as the heading angle estimation result of the current frame.

3. The heading angle estimation method according to claim 1, characterized in that, The method further includes: If the speed of the tracked target in the current frame is greater than the upper limit of the low-speed hysteresis interval and the number of consecutive jitters is greater than the jitter threshold, the historical stable heading angle of the tracked target is used to replace the measured heading angle of the current frame in the measurement update process, and the credibility label is set to untrustworthy. If the speed of the tracked target in the current frame is greater than the upper limit of the low-speed hysteresis interval, and the number of consecutive jitters is less than or equal to the jitter threshold, the historical stable heading angle of the tracked target is refreshed according to the measured heading angle of the current frame, and the confidence label is set to confidence. If the speed of the tracked target in the current frame is less than the lower limit of the low-speed hysteresis interval, the historical stable heading angle of the tracked target is used as the heading angle estimation result of the current frame; When the velocity of the tracked target in the current frame is in the low-speed hysteresis range, the MCSM maintains the confidence label unchanged.

4. The heading angle estimation method according to claim 1, characterized in that, The measurement update process based on the measured heading angle of the current frame, to obtain the filtered heading angle of the current frame, includes: Based on the measured heading angle of the current frame, the predicted heading angle of each particle in the current frame, and the standard deviation of the observation noise, the weight of each particle in the current frame is updated and normalized to obtain the normalized weight of each particle in the current frame. On the unit circle, the predicted heading angles of all particles in the current frame are weighted and averaged according to their normalized weights in the current frame to obtain the filtered heading angle of the current frame.

5. The heading angle estimation method according to claim 4, characterized in that, The method further includes: Perform the following steps for any particle in any frame: The predicted heading angle of the current particle in the current frame is obtained based on the heading angle of the current particle in the previous frame, the angular velocity of the current particle in the previous frame, the frame time interval between the current frame and the previous frame, and the process noise of the heading angle. Based on the angular velocity and angular velocity process noise of the current particle in the previous frame, the predicted angular velocity of the current particle in the current frame is obtained.

6. The heading angle estimation method according to claim 5, characterized in that, The method further includes: When the current particle is a pedestrian particle, the predicted angular velocity of the current particle in the current frame is obtained based on the angular velocity of the current particle in the previous frame, the angular velocity decay factor, and the process noise of the angular velocity.

7. The heading angle estimation method according to claim 4, characterized in that, The method further includes: Perform the following steps for each frame: The effective number of particles in the current frame is calculated based on the total number of particles and the weights of all particles in the current frame. If the number of effective particles in the current frame is greater than or equal to the effective particle threshold, the resampling process is skipped. If the number of effective particles in the current frame is less than the effective particle threshold, and it is determined that the number of effective particles after particle resampling is greater than or equal to the effective particle threshold, particle resampling is performed with a fixed starting point and equal interval sampling, and the weight of all resampled particles is set to the reciprocal of the total number of particles. If the number of effective particles in the current frame is less than the effective particle threshold, and it is determined that the number of effective particles after particle resampling is less than the effective particle threshold, the particle diversity self-recovery process is triggered.

8. The heading angle estimation method according to claim 1, characterized in that, The method further includes: When the tracked target is initially detected, if a heading angle provided by a sensor that is not affected by the magnetic field exists, the heading angle provided by the sensor shall be used as the measured heading angle of the first frame. In the absence of a sensor that is not affected by magnetic field interference, and when the tracked target is located in the angle-locked region, the preset heading angle is used as the measured heading angle of the first frame. In the absence of a heading angle provided by a sensor unaffected by magnetic field interference, and when the tracked target is not located in the angle-locked region, the measured heading angle of the first frame is calculated based on the position points of the tracked target in two consecutive frames.

9. The heading angle estimation method according to claim 8, characterized in that, The method further includes: Based on the measured heading angle of the first frame, create and initialize a particle filter instance; The initial variance of the particle filter instance is set according to the type of the tracked target; wherein the initial variance corresponding to the vehicle tracking target is less than the initial variance corresponding to the pedestrian tracking target.

10. The heading angle estimation method according to claim 5, characterized in that, The observation noise standard deviation, the process noise of the heading angle, and the process noise of the angular velocity are all adjusted according to the type of the tracked target and the speed of the tracked target.

11. An electronic device, characterized in that, Includes memory, transceiver, and processor; Memory, used to store computer programs; Transceiver, used to send and receive data under the control of the processor; A processor is configured to read a computer program from the memory and execute the heading angle estimation method according to any one of claims 1-10.

12. A heading angle estimation device, characterized in that, The device includes: The measurement update module performs the following steps for each frame of the tracked target: If the confidence label corresponding to the measured heading angle of the current frame is reliable, a measurement update process is performed based on the measured heading angle of the current frame to obtain the filtered heading angle of the current frame; and the filtered heading angle of the current frame is used as the heading angle estimation result of the current frame. If the confidence label corresponding to the measured heading angle in the current frame is unreliable, the measurement update process is executed after increasing the standard deviation of the observation noise; or, Skip the measurement update process and use the predicted heading angle of the current frame as the heading angle estimation result of the current frame; or, The measured heading angle of the current frame is replaced with the historical stable heading angle of the tracked target, and the measurement update process is performed.

13. A processor-readable storage medium, characterized in that, The processor-readable storage medium stores a program for causing the processor to execute the heading angle estimation method according to any one of claims 1 to 10.

Images