Fusion positioning method based on dynamic weight distribution

Abstract

The invention discloses a fusion positioning method based on dynamic weight distribution, and belongs to the technical field of multi-sensor data fusion, and the method comprises the steps: obtaining multi-sensor data, and obtaining the laser point cloud density and illumination intensity change rate based on the multi-sensor data; acquiring environment complexity based on the laser point cloud density and the illumination intensity change rate; obtaining a weight constraint condition based on environment complexity, a Kalman filtering framework, a weight dynamic embedding item and an experimental calibration fine tuning method; obtaining dynamic weight distribution based on the weight constraint condition and a preset transition function; and obtaining multi-sensor fusion positioning based on the dynamic weight distribution, the multi-sensor data and a preset extended Kalman filtering fusion model. According to the fusion positioning method based on dynamic weight distribution, the problem that in the prior art, a fixed weight fusion positioning error is too large is solved, dynamic weight distribution is carried out according to the actual environment complexity, and the accuracy of multi-sensor data fusion positioning is effectively improved.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-sensor data fusion, and in particular to a fusion positioning method based on dynamic weight allocation. Background Art

[0002] Existing indoor mobile robots often use a fixed-weight extended Kalman filter (EKF) fusion solution. When fusing laser SLAM, visual odometry, and encoder data, each sensor is assigned a fixed weight (e.g., 0.5 for lidar, 0.3 for vision, and 0.2 for encoder).

[0003] However, existing fusion solutions often suffer from the following problems: Rigid weights lead to poor environmental adaptability, and fixed weights are unable to respond to dynamic environmental changes (such as sudden changes in illumination, obstructions, etc.). For example, if a visual sensor is still assigned a weight of 0.3 when 90% of sudden changes in illumination fail, the fusion result will drift. Similarly, if a lidar weight is still assigned 0.5 when the error in glass curtain wall reflections increases, the positioning error will be amplified. There is a lack of sensor data conflict resolution. When the lidar and visual odometry counts conflict (such as glass reflections leading to visual misjudgment), fixed weight fusion cannot suppress the conflict, resulting in excessive positioning error. Furthermore, errors accumulate during the prediction phase. Traditional EKFs do not utilize encoder odometry to correct the motion model in real time, causing linearization errors to increase with distance. For example, the tracking error for a 10-meter trajectory will exceed 10%. Therefore, a fusion positioning technology is urgently needed to address these technical issues, improve adaptability for real-time applications in complex environments, and eliminate data conflicts. Summary of the Invention

[0004] The present invention provides a fusion positioning method based on dynamic weight allocation, which can solve the problem in the existing technology that fixed weight fusion cannot suppress conflicts and causes excessive positioning errors. It realizes dynamic weight allocation and regulation according to the complexity of the actual application environment, and effectively improves the accuracy of multi-sensor data fusion positioning.

[0005] The present invention provides a fusion positioning method based on dynamic weight allocation, comprising:

[0006] Acquire multi-sensor data, and obtain laser point cloud density and light intensity change rate based on the multi-sensor data;

[0007] Obtain environmental complexity based on laser point cloud density and light intensity change rate;

[0008] Obtain weight constraints based on environment complexity, Kalman filter framework, weight dynamic embedding term and experimental calibration fine-tuning method;

[0009] Obtain dynamic weight distribution based on weight constraints and preset transition functions;

[0010] Multi-sensor fusion positioning is obtained based on dynamic weight allocation, multi-sensor data and a preset extended Kalman filter fusion model.

[0011] The present invention provides a fusion positioning method based on dynamic weight allocation. The method calculates the current environmental complexity through preliminary analysis of multi-sensor data to obtain corresponding weight constraints according to the environmental complexity, and dynamically allocates weights according to the constraints. Then, the fusion model is corrected and improved according to the dynamic weight allocation to improve the accuracy and adaptability of the fusion model in obtaining multi-sensor data fusion positioning. It solves the problem in the existing technology that fixed weight fusion cannot suppress conflicts, resulting in excessive positioning errors, and realizes dynamic weight allocation and regulation according to the environmental complexity in actual applications, effectively improving the accuracy of multi-sensor data fusion positioning.

[0012] Furthermore, multi-sensor data is obtained, and the laser point cloud density and the light intensity change rate are obtained based on the multi-sensor data, including: the multi-sensor data includes lidar data and light sensor data; the lidar data is obtained, and the number of valid point clouds is obtained based on the lidar data and a preset division statistical method, so as to obtain the laser point cloud density based on the number of valid point clouds; the light sensor data is obtained, and the light intensity change rate is obtained based on the light sensor data and a preset light intensity processing algorithm.

[0013] In the above scheme, by processing the lidar data and light sensor data in the multi-sensor data, the lidar data is first scanned, divided and counted using a preset division and statistical method to filter out noise points and obtain the number of valid point clouds, thereby realizing quantitative processing of multi-sensor data and providing data support for subsequent analysis of environmental complexity.

[0014] Furthermore, light sensor data is obtained, and the light intensity change rate is obtained based on the light sensor data and a preset light intensity processing algorithm, including: obtaining light sensor data, obtaining an image grayscale matrix based on the light sensor data; and obtaining the average brightness of the entire frame based on the image grayscale matrix. The calculation process satisfies the following formula:

[0015]

[0016] Where: L represents the average brightness of the entire frame, I represents the image grayscale matrix;

[0017] The light intensity change rate is obtained based on the average brightness of the entire frame, and its calculation process satisfies the following formula:

[0018]

[0019] Where: ΔL vision Indicates the rate of change of light intensity, L current Indicates the average brightness of the current frame.

[0020] L previous Indicates the average brightness of the entire historical frame.

[0021] In the above scheme, the image calf matrix is ​​first extracted from the light sensor data to analyze the average brightness of the current light sensor image, and the same algorithm is used to calculate the average brightness of adjacent frame images to achieve the adjacent frame change rate of light intensity, obtain the required light intensity change rate, and realize the quantitative analysis of light sensor data.

[0022] Furthermore, weight constraints are obtained based on the environmental complexity, the Kalman filter framework, the weight dynamic embedding terms and the experimental calibration fine-tuning method, including: obtaining theoretical constraints based on the Kalman filter framework and the weight dynamic embedding terms; obtaining fixed constraint boundaries based on the environmental complexity, the theoretical constraints and the experimental calibration fine-tuning method, and using the fixed constraint boundaries as weight constraints.

[0023] In the above scheme, based on the environmental complexity obtained after multi-sensor data processing, the Kalman filter framework and the weight dynamic embedding term are combined to obtain theoretical constraints to measure the uncertainty of the positioning results caused by the environmental complexity. The positioning error is dynamically eliminated through the weight constraint allocation. On the basis of the theoretical constraints, the experimental calibration and fine-tuning method is used to measure and optimize the boundaries of the theoretical constraints according to the actual application scenarios to improve the adaptability in complex real-time environments.

[0024] Furthermore, the weight constraint conditions are obtained based on the environmental complexity, the Kalman filter framework, the weight dynamic embedding terms and the experimental calibration fine-tuning method, and also include: when there is a first triggering scenario, an adaptive weight constraint boundary is obtained based on the fixed constraint boundary, the first triggering scenario and the preset dynamic relaxation boundary, so as to use the adaptive weight constraint boundary as the weight constraint condition.

[0025] Furthermore, the weight constraint conditions are obtained based on the environmental complexity, Kalman filter framework, weight dynamic embedding terms and experimental calibration fine-tuning methods, and also include: when there is a conflict triggering scenario, the conflict handling constraint boundary is obtained based on the fixed constraint boundary, the conflict triggering scenario and the preset setting method, so as to use the conflict handling constraint boundary as the weight constraint condition.

[0026] In the above scheme, a conflict handling mechanism is set up in the acquisition of weight constraints. When there are triggering conditions of the conflict handling mechanism, such as the first trigger scenario and the conflict trigger scenario, that is, when there are abnormalities in the multi-sensor data, the weight constraints are dynamically conditioned according to the actual abnormal situation, further improving the availability of dynamic weight allocation in complex real-time environments, so as to effectively improve the accuracy of multi-sensor data fusion.

[0027] Furthermore, dynamic weight distribution is obtained based on weight constraints and a preset transition function, including: obtaining basic weight distribution based on environmental complexity; obtaining dynamic weight distribution based on the basic weight distribution, weight constraints and a preset transition function, and the calculation process satisfies the following formula:

[0028]

[0029] W laser ∈[W lmin ,W lmax ]

[0030] W vision ∈[W vmin ,W vmin ]

[0031] W encoder =W o

[0032] Where: W laser Represents the lidar weight, W vision Represents the visual sensor weight, W encoder Indicates the programmer's fixed weight, D laser Indicates the laser point cloud density, ΔL vision Indicates the rate of change of light intensity, W lmin Represents the minimum reliability weight of the laser, W lmax Represents the laser dominant weight, W vmin Represents the minimum reliability weight of light intensity, W vmax Represents the dominant weight of light intensity, W o Represents a fixed weight value.

[0033] In the above scheme, in order to avoid the oscillation of the fusion positioning process caused by weight jumps due to sudden changes in the environment, a transition function is designed, and dynamic weight allocation is realized according to the weight constraint conditions, which improves the conflict resolution ability of multi-sensor data and ensures the stability of dynamic weight allocation, so as to further ensure the accuracy of multi-source sensor data fusion positioning.

[0034] Furthermore, encoder data is obtained, and multi-sensor fusion positioning is obtained based on dynamic weight allocation, multi-sensor data, encoder data and a preset extended Kalman filter fusion model, including: obtaining sensor data vectors according to a preset update frequency based on multi-sensor data and a preset fusion framework; obtaining dynamic weighted observation equations based on dynamic weight allocation, sensor data vectors and a preset weighted observation method; obtaining encoder data, and obtaining a revised fusion model based on encoder data, a preset extended Kalman filter fusion model and the dynamic weighted observation equation; obtaining multi-sensor fusion positioning based on multi-sensor data, encoder data and the revised fusion model.

[0035] In the above scheme, the improvement of the weighted observation equation and the dynamic scaling of the noise covariance matrix are achieved through dynamic weight allocation, and the prediction and update process of the extended Kalman filter is corrected. The fusion dynamic weight is integrated to achieve fusion adjustment according to the prediction of the observation state. The multi-sensor fusion positioning is obtained only according to the corrected fusion model, which effectively improves the adaptability, stability and accuracy of the fusion positioning.

[0036] Furthermore, a dynamic weighted observation equation is obtained based on dynamic weight allocation, sensor data vector and preset weighted observation method, including: obtaining a weighted observation vector based on dynamic weight allocation, sensor data vector and preset weighted observation method; obtaining a dynamic scaling matrix based on dynamic weight allocation and a preset noise covariance matrix; obtaining a dynamic weighted observation equation based on the weighted observation vector and the dynamic scaling matrix.

[0037] In the above scheme, dynamic weights are embedded in the observation equation to effectively suppress the influence of unreliable sensors, significantly improving the environmental adaptability of the fusion positioning method and the conflict resolution ability of multi-sensor data.

[0038] Furthermore, encoder data is obtained, and a revised fusion model is obtained based on the encoder data, a preset extended Kalman filter fusion model and a dynamic weighted observation equation, including: obtaining encoder data, obtaining encoder correction parameters based on the encoder data and the predicted motion model; obtaining an improved fusion model based on the encoder correction parameters, the dynamic weighted observation equation and the preset extended Kalman filter fusion model; obtaining Kalman gain based on the improved fusion model, and obtaining a revised fusion model based on the Kalman gain and the improved fusion model.

[0039] In the above scheme, the encoder data is combined to realize constraint state prediction, which realizes the correction of the motion model in the encoder, effectively reduces the linear error of motion trajectory tracking, suppresses the prediction error, and ensures the real-time performance of fusion positioning.

[0040] The present invention provides a fusion positioning method based on dynamic weight allocation, which calculates the current environmental complexity through preliminary analysis of multi-sensor data to obtain corresponding weight constraints according to the environmental complexity, and dynamically allocates weights according to the constraints, and then corrects and improves the fusion model according to the dynamic weight allocation, embeds dynamic weights into the observation equation, so as to improve the accuracy and adaptability of the fusion model in obtaining multi-sensor data fusion positioning, solves the problem in the existing technology that fixed weight fusion cannot suppress conflicts and causes excessive positioning errors, realizes dynamic weight allocation and regulation according to the environmental complexity in actual applications, effectively improves the accuracy of multi-sensor data fusion positioning, significantly improves the environmental adaptability, the conflict resolution ability of multi-sensor data and the suppression effect of prediction errors, and effectively reduces linear errors and ensures the real-time performance of fusion positioning by performing motion model correction through encoder data. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] In order to more clearly illustrate the technical solution of the present application, the following is a brief introduction to the drawings required for use in the implementation. Obviously, the drawings described below are only some implementation methods of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0042] Figure 1 This is a schematic diagram of a fusion positioning method based on dynamic weight allocation provided by this embodiment;

[0043] Figure 2 Schematic diagram of a multi-source sensor fusion framework based on dynamic weight allocation provided by this embodiment;

[0044] Figure 3 This embodiment provides a fusion positioning system based on dynamic weight allocation. DETAILED DESCRIPTION

[0045] To make the objectives, technical solutions, and advantages of this application more clear, the technical solutions in this application will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of this application. Obviously, the described embodiments are only part of the embodiments of this application, not all of them. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making any creative efforts are within the scope of protection of this application.

[0046] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art to which this application belongs; the terms used herein are only for the purpose of describing specific embodiments and are not intended to limit this application; the terms "including" and "having" and any variations thereof in the specification and claims of this application and the above-mentioned figure descriptions are intended to cover non-exclusive inclusions.

[0047] In the description of the embodiments of this application, the technical terms "first" and "second" are used only to distinguish different objects and should not be understood to indicate or imply relative importance or implicitly specify the quantity, specific order, or primary and secondary relationship of the indicated technical features. In the description of the embodiments of this application, the meaning of "plurality" is more than two, unless otherwise clearly and specifically defined.

[0048] References herein to "embodiments" mean that a particular feature, structure, or characteristic described in connection with the embodiments may be included in at least one embodiment of the present application. The appearance of this phrase in various places in the specification does not necessarily refer to the same embodiment, nor does it constitute an independent or alternative embodiment that is mutually exclusive of other embodiments. It is understood, both explicitly and implicitly, by those skilled in the art that the embodiments described herein may be combined with other embodiments.

[0049] In the description of the embodiments of this application, the term "and / or" is simply a description of the association relationship between associated objects, indicating that three relationships can exist. For example, A and / or B can represent the following three situations: A exists alone, A and B exist simultaneously, and B exists alone. In addition, the character " / " in this document generally indicates that the associated objects are in an "or" relationship.

[0050] In the description of the embodiments of the present application, the term "multiple" refers to more than two (including two). Similarly, "multiple groups" refers to more than two groups (including two groups), and "multiple pieces" refers to more than two pieces (including two pieces).

[0051] In the description of the embodiments of the present application, unless otherwise expressly specified or limited, technical terms such as "installed," "connected," "connected," and "fixed" should be understood in a broad sense. For example, they can refer to fixed connections, detachable connections, or integration; mechanical connections or electrical connections; direct connections or indirect connections through an intermediate medium; internal connections between two components or interactions between two components. Those skilled in the art can understand the specific meanings of the above terms in the embodiments of the present application based on specific circumstances.

[0052] Example 1:

[0053] This embodiment provides a fusion positioning method based on dynamic weight allocation, such as Figure 1Shown, including:

[0054] S1. Acquire multi-sensor data, and obtain laser point cloud density and light intensity change rate based on the multi-sensor data;

[0055] S2, obtain the environmental complexity based on the laser point cloud density and the light intensity change rate;

[0056] S3, obtain weight constraints based on environment complexity, Kalman filter framework, weight dynamic embedding term and experimental calibration fine-tuning method;

[0057] S4. Obtain dynamic weight distribution based on weight constraints and a preset transition function;

[0058] S5. Obtain multi-sensor fusion positioning based on dynamic weight allocation, multi-sensor data and a preset extended Kalman filter fusion model.

[0059] This embodiment provides a fusion positioning method based on dynamic weight allocation. It performs a preliminary analysis on multi-sensor data to calculate the current environmental complexity, obtains corresponding weight constraints based on the environmental complexity, and dynamically allocates weights based on the constraints. The fusion model is then corrected and improved based on the dynamic weight allocation to improve the accuracy and adaptability of the fusion model in obtaining multi-sensor data fusion positioning. This solves the problem in the prior art that fixed weight fusion cannot suppress conflicts, resulting in excessive positioning errors. It realizes dynamic weight allocation and regulation based on the environmental complexity in actual applications, effectively improving the accuracy of multi-sensor data fusion positioning.

[0060] Optionally, multi-sensor data is obtained, and the laser point cloud density and the light intensity change rate are obtained based on the multi-sensor data, including: the multi-sensor data includes lidar data and light sensor data; the lidar data is obtained, and the number of valid point clouds is obtained based on the lidar data and a preset division statistical method, so as to obtain the laser point cloud density based on the number of valid point clouds; the light sensor data is obtained, and the light intensity change rate is obtained based on the light sensor data and a preset light intensity processing algorithm.

[0061] In the specific implementation process, the laser radar data provided in this embodiment is sourced from the Leishen N10P laser radar (e.g., 10 Hz scanning frequency, ranging radius 10 m). In the process of obtaining the number of valid point clouds based on the laser radar data and the preset division statistical method, and obtaining the laser point cloud density based on the number of valid point clouds, the scanning area (area A) is first divided into scan area ) is divided into grids (such as 0.1m×0.1m), and then the number of valid point clouds N is counted valid points (Filter out noise points), and then calculate the point density per unit area, that is, the laser point cloud density: The following example shows that the scanning area is a sector with a radius of 10m and an area of ​​78.5㎡. 15,700 valid points are detected, so D is calculated. laser =200 points / m 2 The hardware implementation in actual application is usually as follows: using the STM32 main control board to process the point cloud data in real time (100Hz) and send it to Luban Cat 1s through the core communication protocol UART.

[0062] Optionally, light sensor data is obtained, and the light intensity change rate is obtained based on the light sensor data and a preset light intensity processing algorithm, including: obtaining light sensor data, obtaining an image grayscale matrix based on the light sensor data; and obtaining the average brightness of the entire frame based on the image grayscale matrix, wherein the calculation process satisfies the following formula:

[0063]

[0064] Where: L represents the average brightness of the entire frame, I represents the image grayscale matrix;

[0065] The light intensity change rate is obtained based on the average brightness of the entire frame, and its calculation process satisfies the following formula:

[0066]

[0067] Where: ΔL vision Indicates the rate of change of light intensity, L current Indicates the average brightness of the current frame.

[0068] L previous Indicates the average brightness of the entire historical frame.

[0069] In the specific implementation process, the data source of the light sensor data used in this embodiment is the Astra depth camera of Orbbec (such as 30fps RGB image). When calculating the light intensity change rate, the image grayscale matrix I is first extracted (such as using 640×480 resolution), and then the average brightness of the entire frame is calculated. The same method is used to calculate the average brightness of the entire frame of the historical frame to calculate the adjacent frame change rate, that is, the required light intensity change rate. The hardware implementation method in actual application is usually: use Luban Cat 1s to run the OpenCV library to calculate ΔL vision (Usually 30Hz updates).

[0070] In the specific implementation process, when obtaining the environmental complexity through the laser point cloud density and the light intensity change rate, an environmental complexity quantification (three-level classification) standard provided by this embodiment is shown in Table 1 below.

[0071] Table 1 Environmental complexity quantification (three-level classification) standards

[0072]

[0073] Optionally, weight constraints are obtained based on environmental complexity, Kalman filter framework, weight dynamic embedding items and experimental calibration fine-tuning methods, including: obtaining theoretical constraints based on Kalman filter framework and weight dynamic embedding items; obtaining fixed constraint boundaries based on environmental complexity, theoretical constraints and experimental calibration fine-tuning methods, and using the fixed constraint boundaries as weight constraints.

[0074] Optionally, obtaining weight constraints based on environmental complexity, Kalman filter framework, weight dynamic embedding terms and experimental calibration fine-tuning methods also includes: when there is a first triggering scenario, obtaining an adaptive weight constraint boundary based on the fixed constraint boundary, the first triggering scenario and the preset dynamic relaxation boundary, so as to use the adaptive weight constraint boundary as the weight constraint condition.

[0075] Optionally, weight constraints are obtained based on environmental complexity, Kalman filter framework, weight dynamic embedding terms and experimental calibration fine-tuning methods, and also include: when there is a conflict triggering scenario, conflict handling constraint boundaries are obtained based on fixed constraint boundaries, conflict triggering scenarios and preset setting methods, so as to use the conflict handling constraint boundaries as weight constraints.

[0076] In the specific implementation process, the basis for determining the constraints mainly includes theoretical analysis based on the Kalman filter framework and experimental calibration fine-tuning based on actual application scenarios. When the theoretical analysis is driven by the Kalman filter framework, the covariance of the system state estimation error needs to be minimized: min W trace(P k ), Among them, P k Represents the state estimation error covariance matrix, which is used to measure the uncertainty of the positioning result. The smaller the value, the higher the accuracy. k ) represents the matrix trace, which is used to represent the sum of the positioning errors (to be minimized); and the Kalman gain K k Strongly related to weight W: where K k Represents the Kalman gain, which is used to control the fusion ratio of sensor data. Represents the state prediction covariance, which is used to represent the uncertainty of the motion model. H represents the observation matrix, which is used to represent the mapping from state to observation. R represents the sensor noise covariance matrix (calibrated value). R / W 2represents the weighted dynamic embedding term, so a theoretical bound can be obtained through calculation. An example of a theoretical bound driven by theoretical analysis is shown in Table 2 below. Experimental calibration and fine-tuning are then performed based on the theoretical bound. For example, optimization was performed through field measurements in three typical classroom scenarios, as shown in Table 3 below. To more intuitively illustrate the impact of changes in weight distribution on multi-source sensor data fusion positioning, this embodiment uses the example values ​​above to simply illustrate the actual physical meaning of the weight values, as shown in Table 4 below.

[0077] Table 2 An example of theoretical boundaries driven by theoretical analysis

[0078] sensor Noise standard deviation σ <![CDATA[W 理论 ]]> LiDAR 0.01 [0.32,0.65] Vision Sensors 0.04 [0.18,0.55] encoder 0.005 0.20

[0079] Table 3 Typical classroom scene optimization based on theoretical boundaries

[0080]

[0081] Table 4 Actual physical meaning of weight values

[0082]

[0083] In actual applications, to cope with the ever-changing real-time complex environment, the weight constraints in different situations will change. First, for conventional situations, such as large lecture halls, lecture halls, and auditoriums, where multi-sensor data sources are relatively reliable, fixed constraint boundaries are usually used. For example, the fixed boundary constraint values ​​in the following example are: For special cases, adaptive constraint adjustments will be made based on actual conditions, including dynamic boundary relaxation, constraint failure protection, and conflict handling mechanisms.

[0084] First, the dynamic relaxation of the boundary is triggered when the first trigger scenario exists. The trigger mechanism usually includes an emergency obstacle avoidance scenario, such as detecting a close-range dynamic obstacle (<1m). At this time, the constraint condition changes to It also includes long-distance corridor scenes, which are triggered when the point cloud density is >200 points / ㎡ for 5 consecutive seconds. At this time, the constraint condition changes to

[0085] Second, constraint failure protection is usually triggered by sensor hardware failure, such as a LiDAR disconnection. At this time, the constraint conditions change as follows: In specific applications, the mathematical implementation of constraint changes is as follows:

[0086]

[0087]

[0088] An example of a weight constraint change adjustment in this case is: laser=180, ΔL vision =0.25;

[0089] Third, the conflict handling mechanism is to dynamically adjust when sensor data is abnormal. For example, when a LiDAR failure scenario occurs (such as glass curtain wall reflection), W is forced to be set. laser =0.3 (lower limit), W vision = 0.5; when a visual failure scenario occurs (such as a 90% illumination mutation), W is forced to be set vision =

[0090] 0.2 (lower limit), W laser =0.7. When determining sensor data anomalies, this is usually accomplished through multi-source information fusion and a hierarchical decision-making mechanism. The specific identification process for failure scenarios includes:

[0091] Step 1: Direct indicator monitoring (real-time detection layer). The following code implements the direct indicator monitoring and identification process of the "lidar failure scenario":

[0092]

[0093]

[0094] Step 2: Perform multi-sensor consistency check based on the decision fusion layer, as shown in Table 5 below.

[0095] Table 5 Multi-sensor consistency check based on decision fusion layer

[0096]

[0097] Among them, the characteristic case judgments of some typical failure scenarios include: (1) Glass curtain wall reflection, such as hospital corridor scenes, are judged based on three detection indicators: point cloud density, intensity variance, and motion continuity deviation. The normal range is point cloud density > 100 points / ㎡, and the failure characteristic value is point cloud density < 30 points / ㎡; the normal range of intensity variance is 200-800, and the failure characteristic value is intensity variance > 2000; the normal range is motion continuity deviation < 0.1m, and the failure characteristic value is motion continuity deviation > 0.3m. (2) Metal equipment occlusion, such as operating room scenes, are judged based on three detection indicators: point cloud density, dynamic object false detection, and posture deviation. The normal range is point cloud density > 800 points / ㎡, and the point cloud density failure characteristic value range is 25-50 points / ㎡; the normal range of degree variance is less than 2 / frame, and the failure characteristic value is dynamic object false detection > 8 / frame; the normal range is posture deviation < 0.08m, and the failure characteristic value is posture deviation > 0.25m. The conflict handling mechanism after failure determination includes, for example, when the laser radar is identified as failed (Failure_Flag=1): forcibly setting W laser =0.3 (constraint lower limit); compensation improvement W vision =0.5 (vision-dominated); encoder weight W encoder =0.2 (unchanged).

[0098] Optionally, dynamic weight distribution is obtained based on weight constraints and a preset transition function, including: obtaining basic weight distribution based on environmental complexity; obtaining dynamic weight distribution based on the basic weight distribution, weight constraints and a preset transition function, and the calculation process satisfies the following formula:

[0099]

[0100] W laser ∈[W lmin ,W lmax ]

[0101] W vision ∈[W vmin ,W vmin ]

[0102] W encoder =W o

[0103] Where: W laser Represents the lidar weight, W vision Represents the visual sensor weight, W encoder Indicates the programmer's fixed weight, D laser Indicates the laser point cloud density, ΔL vision Indicates the rate of change of light intensity, W lmin Represents the minimum reliability weight of the laser, W lmax Represents the laser dominant weight, Wvmin Represents the minimum reliability weight of light intensity, W vmax Represents the dominant weight of light intensity, W o Represents a fixed weight value.

[0104] In the specific implementation process, when performing dynamic weight calculation, the basic weight distribution is first obtained according to the complexity of the environment. A basic distribution provided by this embodiment is shown in Table 6 below:

[0105] Table 6 Basic weight distribution based on environmental complexity

[0106] Environmental complexity <![CDATA[W laser ]]> <![CDATA[W vision ]]> <![CDATA[W encoder ]]> Low 0.5 0.3 0.2 middle 0.4 0.4 0.2 high 0.3 0.5 0.2

[0107] Then, a continuous transition function (i.e., a preset transition function) is used to smoothly switch weights to avoid system oscillation caused by weight jumps. An example of a transition is as follows: And the weight constraint condition under this basic allocation is: W laser ∈[0.3, 0.7]; W vision ∈

[0108] [0.2, 0.6]; W encoder =0.2.

[0109] Optionally, encoder data is obtained, and multi-sensor fusion positioning is obtained based on dynamic weight allocation, multi-sensor data, encoder data and a preset extended Kalman filter fusion model, including: obtaining sensor data vectors according to a preset update frequency based on multi-sensor data and a preset fusion framework; obtaining dynamic weighted observation equations based on dynamic weight allocation, sensor data vectors and a preset weighted observation method; obtaining encoder data, and obtaining a corrected fusion model based on encoder data, a preset extended Kalman filter fusion model and the dynamic weighted observation equation; obtaining multi-sensor fusion positioning based on multi-sensor data, encoder data and the corrected fusion model.

[0110] Optionally, a dynamic weighted observation equation is obtained based on dynamic weight allocation, sensor data vector and a preset weighted observation method, including: obtaining a weighted observation vector based on dynamic weight allocation, sensor data vector and a preset weighted observation method; obtaining a dynamic scaling matrix based on dynamic weight allocation and a preset noise covariance matrix; obtaining a dynamic weighted observation equation based on the weighted observation vector and the dynamic scaling matrix.

[0111] In the specific implementation process, dynamic weights are embedded in the observation equation to effectively suppress the influence of unreliable sensors and significantly improve the environmental adaptability of the fusion positioning method and the conflict resolution ability of multi-sensor data. In the specific application process, the EKF fusion core obtains fusion positioning data such as positioning coordinates and positioning angles through the weighted fusion calculation of the sensor raw data using a certain frequency approximate data, such as Figure 2 As shown: First, the lidar, depth camera and encoder obtain sensor raw data at a certain frequency. The lidar obtains laser point cloud density and laser solution data based on a scanning frequency of 10 Hz, the depth camera obtains light intensity change rate and visual odometry data at a scanning frequency of 30 Hz, and the encoder obtains linear displacement increments and angular displacement increments at a frequency of 100 Hz. The dynamic weight distribution of the lidar, depth camera and encoder is obtained based on the laser point cloud density and light intensity change rate. Then, the EKF fusion core performs dynamic weighted fusion of laser solution data, visual odometry data, linear displacement increments and angular displacement increments based on dynamic weight distribution to output accurate fusion and positioning data. The multi-sensor data fusion framework is shown in Table 7 below.

[0112] Table 7 Multi-sensor data fusion framework

[0113]

[0114] When applied to medical mobile robots, the dynamic weight is embedded in the EKF observation equation. The observation equation transformation process of dynamic weight embedding includes: (1) obtaining the weighted observation vector based on the dynamic weight distribution, sensor data vector and preset weighted observation method, and performing weighted observation vector Constructed as follows:

[0115] in is the original observation vector, is the weight vector; L represents the lidar data, which comes from the sensor system identification and is used to distinguish the data source (such as ), is a data type identifier; V represents the visual odometry data identifier, which comes from the sensor system label and is also used to distinguish the data source (such as ), is a data type identifier; E represents the encoder data identifier, which comes from the sensor system tag and is also used to distinguish the data source (such as ), is a data type identifier; x represents the x coordinate of the robot in the world coordinate system output by EKF fusion, which is often used for the lateral position in navigation application scenarios (such as operating rooms); y represents the y coordinate of the robot in the world coordinate system output by EKF fusion, which is often used for the longitudinal position in navigation application scenarios (such as operating rooms), and the unit is meter (m); θ represents the heading angle of the robot output by EKF fusion, which is often used for orientation control during instrument transportation, and the unit is radian (rad). L The x coordinate calculated by the laser radar using laser SLAM (Simultaneous Localization and Mapping) is in meters and is often used for global positioning in open corridors. L Represents the y coordinate of the laser SLAM solution, which is often used for precise positioning in the ward area, θ L Indicates the heading angle calculated by laser SLAM, which is often used for the calibration of the operating table docking angle, x, y, θ, X L 、Y L ,θ L All of them belong to absolute posture; u represents the motion control input data obtained by the encoder solution, which is a control instruction with the unit of m / s or rad / s, and is often used to control the speed / angular velocity of the Ackerman chassis; Δx V (unit: m) represents the position increment in the x direction of the visual odometry in the depth camera, which is often used for motion tracking on texture-rich ground. V (unit: m) represents the position increment in the y direction of the tree fern odometry in the depth camera, which is often used to represent the lateral displacement during lateral obstacle avoidance. V (unit: rad) represents the change in the heading angle of the visual odometry in the depth camera, which is often used to express the change in angle when turning, Δs E (unit: m) represents the linear displacement increment measured by the photoelectric encoder, often used to express the wheel rotation distance, Δθ E (unit: rad) represents the angular displacement increment measured by the photoelectric encoder, which is often used to express the change of the steering angle of the servo, where Δx V , Δy V , Δθ V , Δθ E All are relative motion increments.

[0116] (2) Based on the dynamic weight allocation and the preset noise covariance matrix, a dynamic scaling matrix is ​​obtained to realize the noise covariance matrix Dynamic scaling, as shown below:

[0117]

[0118] The sub-matrix is the laser radar noise covariance (calibration value: σLx =σ Ly =0.01,σ Lθ =0.001), is the visual odometry noise covariance (calibration value: σ Vx =σ Vy =0.04,σ Vθ =0.004), is the u encoder noise covariance (calibration value: ).

[0119] Optionally, encoder data is obtained, and a modified fusion model is obtained based on the encoder data, a preset extended Kalman filter fusion model and a dynamic weighted observation equation, including: obtaining encoder data, obtaining encoder correction parameters based on the encoder data and the predicted motion model; obtaining an improved fusion model based on the encoder correction parameters, the dynamic weighted observation equation and the preset extended Kalman filter fusion model; obtaining Kalman gain based on the improved fusion model, and obtaining a modified fusion model based on the Kalman gain and the improved fusion model.

[0120] In the specific implementation process, the prediction and update process of the extended Kalman filter is used to realize the improvement and correction of the fusion model. The prediction and update process of the extended Kalman filter includes: (1) performing encoder correction in the prediction stage; such as: u k =[Δs E ,Δθ E ] T , where f(·) represents the predicted motion model such as the Ackerman chassis kinematic model, and w k Represents process noise The improved fusion model is obtained based on the preset extended Kalman filter fusion model. (2) Dynamic weight fusion is performed in the update phase. First, the Kalman gain is calculated based on the improved fusion model: Where H represents the observation Jacobian matrix (identity matrix, because h(x) = x); then the state update is performed: Finally, the covariance update is performed In the formula Represents the weight, and finally the improved EKF fusion model (i.e., the corrected fusion model) is obtained. The preferred implementation of the prediction update process is to perform multi-threaded priority scheduling through ROS node pseudo code. The following code implements the above multi-threaded priority scheduling:

[0121]

[0122]

[0123] Compared with the traditional fixed-weight EKF, the improved EKF fusion model provided in this embodiment effectively suppresses the fusion positioning error and improves the accuracy of fusion positioning. The fusion effect comparison with the traditional EKF is shown in Table 8 below, where the error of the fusion result of this embodiment is

[0124] Table 8 Comparison of fusion effects between the EKF of this embodiment and the traditional EKF

[0125] parameter Traditional fixed-weight EKF Improved EKF fusion model Visual weight <![CDATA[W vision =0.3]]> <![CDATA[W vision =0.2]]> <![CDATA[Noise matrix R V > diag(0.04,0.04,0.004) <![CDATA[diag(0.04 / 0.2 2 )=diag(1.0)]]> <![CDATA[Kalman gain K V > 0.12 0.02 (visual data is suppressed) Positioning error Heading angle deviation>3° Heading angle deviation <0.5°

[0126] This embodiment provides a fusion positioning method based on dynamic weight allocation, based on the laser point cloud density (such as: D laser ) and the rate of change of light intensity (such as: ΔL vision ) Quantify the complexity of the environment and strictly limit the weight range (such as: W laser ∈[0.3,0.7], W vision ∈[0.2,0.6]) to construct a dynamic weight mechanism and embed the dynamic weight into the EKF observation equation (such as: z k =ΣW i ·z i ), encoder data (such as: u k ) is used for both correction in the prediction phase and fusion in the update phase (e.g., W encoder = 0.2), in order to modify the state equation and construct the weighted observation vector to achieve the correction and improvement of the multi-sensor fusion architecture (e.g., z k =[W laser ·z laser ;W vision ·z vision ;W encoder ·z encoder The paper addresses the technical issues of dynamically allocating sensor weights based on real-time environmental complexity (obstacle density, light intensity), adjusting weights (in the range of 0.3-0.7) to suppress the influence of unreliable sensors, and combining encoder odometry with state prediction to reduce linearization errors. A multi-sensor fusion positioning method based on a dynamic weighted extended Kalman filter has been implemented, significantly improving environmental adaptive fusion capabilities. For example, in scenarios with sudden changes in lighting (such as strong light in an operating room), the visual weight is reduced from 0.3 to 0.1, while the laser weight is increased to 0.7, improving positioning stability by 45%. The paper also enhances multi-sensor conflict resolution capabilities. For example, when visual data is invalidated by reflections from glass curtain walls, the laser weight is automatically increased to 0.7, reducing static positioning error to less than 3cm. Prediction errors are effectively suppressed, and after the encoder odometry corrects the motion model, the maximum error of a 10-meter dynamic trajectory is less than 5cm. Real-time performance is guaranteed, and through ROS multi-threaded priority scheduling, the positioning update frequency reaches 20Hz, with a latency of ≤20ms.

[0127] Example 2:

[0128] Based on the above embodiment of a fusion positioning method based on dynamic weight allocation, this embodiment provides a fusion positioning system based on dynamic weight allocation, such as Figure 3 As shown, it includes a lidar, a depth camera, an environment complexity assessment module, an encoder, a dynamic weight calculation module and an improved EKF fusion module, where:

[0129] LiDAR is used to obtain LiDAR data and laser point cloud density;

[0130] The depth camera is used to obtain visual odometry data and light intensity change rate;

[0131] The environment complexity evaluation module is used to obtain the environment complexity based on the laser point cloud density and the light intensity change rate;

[0132] Encoder is used to obtain encoder data;

[0133] The dynamic weight calculation module is used to obtain real-time dynamic weights based on the environment complexity and encoder data;

[0134] Improved EKF fusion module to obtain pose output based on real-time dynamic weights, encoder data, lidar data and visual odometry data;

[0135] Among them, the improved EKF fusion module is composed of the improved EKF fusion model;

[0136] The interface design between the dynamic weight calculation module and the improved EKF fusion module is shown in the following code, including:

[0137] 1. Dynamic weight calculation results are published through ROS topics:

[0138] #ROS message definition (weight_calculation.msg)

[0139] float32 w_laser #range [0.3, 0.7]

[0140] float32 w_vision #range [0.2,0.6]

[0141] float32 w_encoder=0.2

[0142] 2. The EKF node subscribes to the topic and calls during the update phase:

[0143] / / 2.1, EKF update phase pseudo code

[0144] void ekfUpdateCallback(const SensorData&data,const Weights&w){

[0145] / / 2.2, construct weighted observation vector

[0146] Vector3d z_weighted;

[0147] z_weighted< <w.w_laser*data.laser_pose,

[0148] w.w_vision*data.vision_pose,

[0149] w.w_encoder*data.encoder_pose;

[0150] / / 2.3, update the noise matrix R

[0151] Matrix3d ​​R=Matrix3d::Zero();

[0152] R(0,0)=laser_noise / w.w_laser; / / The higher the weight, the lower the noise variance

[0153] R(1,1)=vision_noise / w.w_vision;

[0154] R(2,2)=encoder_noise / w.w_encoder;

[0155] / / 2.4, Kalman gain calculation

[0156] Matrix3d ​​K=P*H.transpose()*(H*P*H.transpose()+R).inverse();

[0157] / / 2.5, Status Update

[0158] x = x + K * (z_weighted - H * x);

[0159] }

[0160] Example 3:

[0161] Based on the above-mentioned embodiment of a fusion positioning method based on dynamic weight allocation, another embodiment of the present invention provides a terminal device, which includes a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor. When the processor executes the computer program, it implements a fusion positioning method based on dynamic weight allocation according to any embodiment of the present invention.

[0162] For example, in this embodiment, the computer program may be divided into one or more modules, which are stored in the memory and executed by the processor to implement the present invention. The one or more module elements may be a series of computer program instruction segments capable of performing specific functions, and the instruction segments are used to describe the execution process of the computer program in the terminal device.

[0163] The terminal device may be a computing device such as a desktop computer, a notebook computer, a PDA, a cloud server, etc. The terminal device may include, but is not limited to, a processor and a memory.

[0164] The processor may be a central processing unit (CPU), other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or any conventional processor, etc. The processor is the control center of the terminal device, connecting various parts of the entire terminal device using various interfaces and lines.

[0165] Based on the above-mentioned method embodiments, another embodiment of the present invention provides a computer-readable storage medium, including a stored computer program, wherein, when the computer program is running, the device where the computer-readable storage medium is located is controlled to execute a fusion positioning method based on dynamic weight allocation as described in any one of the above-mentioned method embodiments of the present invention.

[0166] Wherein, the module / unit integrated in the device / terminal equipment, if implemented in the form of a software functional unit and sold or used as an independent product, can be stored in a computer-readable storage medium. Based on this understanding, the present invention implements all or part of the process in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and when the computer program is executed by the processor, it can implement the steps of the above-mentioned various method embodiments. Wherein, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. The computer-readable medium may include: any entity or device that can carry the computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electric carrier signal, telecommunication signal and software distribution medium, etc.

[0167] The above is a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications are also considered to be within the scope of protection of the present invention.

Claims

1. A fusion positioning method based on dynamic weight allocation, characterized in that: include: Acquire multi-sensor data, and acquire laser point cloud density and light intensity change rate based on the multi-sensor data; Acquire environmental complexity based on the laser point cloud density and the light intensity change rate; Obtaining weight constraints based on the environment complexity, Kalman filter framework, weight dynamic embedding term and experimental calibration fine-tuning method; Obtaining dynamic weight distribution based on the weight constraint and a preset transition function; Encoder data is obtained, and multi-sensor fusion positioning is obtained based on the dynamic weight distribution, multi-sensor data, encoder data and a preset extended Kalman filter fusion model.

2. A fusion positioning method based on dynamic weight allocation according to claim 1, characterized in that: The acquiring of multi-sensor data and acquiring the laser point cloud density and the light intensity change rate based on the multi-sensor data includes: The multi-sensor data includes lidar data and light sensor data; Acquire laser radar data, obtain a valid point cloud quantity based on the laser radar data and a preset division statistical method, and obtain a laser point cloud density based on the valid point cloud quantity; Light sensor data is obtained, and a light intensity change rate is obtained based on the light sensor data and a preset light intensity processing algorithm.

3. The fusion positioning method based on dynamic weight allocation according to claim 2, characterized in that: The obtaining of light sensor data and obtaining a light intensity change rate based on the light sensor data and a preset light intensity processing algorithm include: Acquiring light sensor data, and acquiring an image grayscale matrix based on the light sensor data; The average brightness of the entire frame is obtained based on the image grayscale matrix, and the calculation process satisfies the following formula: Where: L represents the average brightness of the entire frame, I represents the image grayscale matrix; The illumination intensity change rate is obtained based on the average brightness of the entire frame, and the calculation process satisfies the following formula: Where: ΔL vision Indicates the rate of change of light intensity, L current Indicates the average brightness of the current frame, L previous Indicates the average brightness of the entire historical frame.

4. The fusion positioning method based on dynamic weight allocation according to claim 1, characterized in that: The method of obtaining weight constraints based on the environment complexity, the Kalman filter framework, the weight dynamic embedding term and the experimental calibration fine-tuning method includes: Obtain theoretical constraints based on the Kalman filter framework and weighted dynamic embedding terms; A fixed constraint boundary is obtained based on the environmental complexity, theoretical constraint conditions and an experimental calibration fine-tuning method, so as to use the fixed constraint boundary as a weight constraint condition.

5. The fusion positioning method based on dynamic weight allocation according to claim 4, characterized in that: The method of obtaining weight constraints based on the environment complexity, the Kalman filter framework, the weight dynamic embedding term and the experimental calibration fine-tuning method further includes: When the first triggering scenario exists, an adaptive weight constraint boundary is acquired based on the fixed constraint boundary, the first triggering scenario, and a preset dynamic relaxation boundary, so as to use the adaptive weight constraint boundary as a weight constraint condition.

6. A fusion positioning method based on dynamic weight allocation according to claim 4, characterized in that: The method of obtaining weight constraints based on the environment complexity, the Kalman filter framework, the weight dynamic embedding term and the experimental calibration fine-tuning method further includes: When a conflict triggering scenario exists, a conflict handling constraint boundary is obtained based on the fixed constraint boundary, the conflict triggering scenario and a preset setting method, so as to use the conflict handling constraint boundary as a weight constraint condition.

7. The fusion positioning method based on dynamic weight allocation according to claim 1, characterized in that: The obtaining of dynamic weight distribution based on the weight constraint condition and a preset transition function includes: Obtaining a basic weight distribution based on the environmental complexity; Based on the basic weight distribution, weight constraint conditions and preset transition function, dynamic weight distribution is obtained, and the calculation process satisfies the following formula: IN laser ∈[W lmin ,IN lmax ] IN vision ∈[W vmin ,IN vmin ] IN encoder =In o Where: W laser Represents the lidar weight, W vision Represents the visual sensor weight, W encoder represents the encoder fixed weight, D laser Indicates the laser point cloud density, ΔL vision Indicates the rate of change of light intensity, W lmin Represents the minimum reliability weight of the laser, W lmax Represents the laser dominant weight, W vmin Represents the minimum reliability weight of light intensity, W vmax Represents the dominant weight of light intensity, W o Represents a fixed weight value.

8. The fusion positioning method based on dynamic weight allocation according to claim 1, characterized in that: The acquiring encoder data, and acquiring multi-sensor fusion positioning based on the dynamic weight allocation, multi-sensor data, encoder data, and a preset extended Kalman filter fusion model, includes: Acquire sensor data vectors based on the multi-sensor data and a preset fusion framework according to a preset update frequency; Obtaining a dynamic weighted observation equation based on the dynamic weight allocation, the sensor data vector, and a preset weighted observation method; Obtaining encoder data, and obtaining a modified fusion model based on the encoder data, a preset extended Kalman filter fusion model, and a dynamic weighted observation equation; A multi-sensor fusion positioning is obtained based on the multi-sensor data, the encoder data and the modified fusion model.

9. The fusion positioning method based on dynamic weight allocation according to claim 8, characterized in that: The obtaining of a dynamic weighted observation equation based on the dynamic weight allocation, the sensor data vector and the preset weighted observation method includes: Obtaining a weighted observation vector based on the dynamic weight allocation, the sensor data vector, and a preset weighted observation method; Obtaining a dynamic scaling matrix based on the dynamic weight allocation and a preset noise covariance matrix; A dynamic weighted observation equation is obtained based on the weighted observation vector and the dynamic scaling matrix.

10. The fusion positioning method based on dynamic weight allocation according to claim 8, characterized in that: The acquiring encoder data and acquiring a modified fusion model based on the encoder data, a preset extended Kalman filter fusion model and a dynamic weighted observation equation include: Obtaining encoder data, and obtaining encoder correction parameters based on the encoder data and a predicted motion model; Based on the encoder correction parameters, the dynamic weighted observation equation and the preset extended Kalman filter fusion model, an improved fusion model is obtained; A Kalman gain is obtained based on the improved fusion model, so as to obtain a modified fusion model based on the Kalman gain and the improved fusion model.

Images