Adaptive distance detection method, system and medium based on TOF sensor

Abstract

The adaptive distance detection method, system and medium based on the TOF sensor include obtaining an original slant range measurement value and an original attitude angle measurement value; correcting the original slant range measurement value to obtain an effective slant range value; performing attitude solving processing on the original attitude angle measurement value to obtain an attitude angle component; performing time alignment processing based on the effective slant range value and the attitude angle component to obtain a correlated data tuple; calculating a calibrated straight-line distance based on the data tuple and a preset formula; performing effectiveness verification on the calibrated straight-line distance to obtain an effective straight-line distance; calculating a time sequence statistic based on a plurality of continuous effective straight-line distances, and calculating a time sequence change based on a plurality of continuous attitude angle components; comparing the time sequence statistic with a first static threshold value, and comparing the time sequence change with a second static threshold value; and adjusting a ranging frequency of the TOF sensor according to a comparison result.

Description

Technical Field

[0001] This invention relates to the field of distance detection technology, and in particular to an adaptive distance detection method, system and medium based on a TOF sensor. Background Technology

[0002] Time-of-Flight (TOF) sensors, a technology that detects distance by measuring the round-trip time of light signals, are widely used in smart control panels, contactless interactive devices, and smart homes. Their core function is to acquire the straight-line distance data between the target object and the device. Based on changes in this distance, the system executes control strategies such as adjusting light brightness, waking up the device, or triggering other functions.

[0003] Ideally, a TOF sensor needs to be installed horizontally to ensure that the measurement direction is perpendicular to the target detection plane, thereby directly obtaining the true straight-line distance. However, in practical applications, due to factors such as installation space limitations, construction errors, and equipment housing structure, TOF sensors often cannot maintain an ideal horizontal posture, resulting in a certain tilt angle between the transmitting and receiving optical axes and the direction to be measured, which directly affects the measurement accuracy.

[0004] Existing Time-of-Flight (TOF) ranging algorithms generally assume that the TOF sensor is in an ideal installation state, directly outputting the oblique distance along the optical axis without compensating for the system error introduced by the installation tilt angle. This results in a significant deviation between the measurement result and the actual required straight-line distance. Furthermore, existing solutions lack intelligent power management mechanisms, employing a fixed ranging frequency mode. Even in standby scenarios without target activity, they continue to operate at high frequency, causing unnecessary power waste and making it difficult to achieve a dynamic balance between low power consumption and fast response performance.

[0005] In summary, existing technologies cannot automatically calibrate and obtain the true straight-line distance when the TOF sensor is installed at an angle; and they lack the ability to dynamically adjust the ranging frequency according to environmental conditions, making it difficult to balance power consumption and response performance. Summary of the Invention

[0006] To address the aforementioned technical problems, this invention provides an adaptive distance detection method based on a Time-of-Flight (TOF) sensor, comprising the following steps:

[0007] Obtain the original slant range measurement value and the original attitude angle measurement value; The original slant range measurement value is corrected to obtain the effective slant range value; the original attitude angle measurement value is subjected to attitude calculation to obtain the attitude angle components. Based on the effective slant range value and attitude angle components, time alignment processing is performed to obtain the associated data tuple; Based on the data tuples and the preset formula, the calibration line distance is calculated; The validity of the calibrated straight-line distance is verified to obtain the valid straight-line distance; The temporal statistics are calculated based on multiple consecutive effective straight-line distances, and the temporal changes are calculated based on multiple consecutive attitude angle components. The time-series statistics are compared with a first static threshold, and the time-series change is compared with a second static threshold; when the time-series statistics are less than the first static threshold and the time-series change is less than the second static threshold, the ranging frequency of the TOF sensor is adjusted from the first ranging frequency to the second ranging frequency; the second ranging frequency is less than the first ranging frequency.

[0008] Optionally, time alignment processing is performed based on the effective slant range value and attitude angle components to obtain associated data tuples, including: The effective slant range value and the attitude angle component are respectively marked with corresponding timestamps; Based on the timestamp sequence, the effective slant range values ​​with a time difference less than a preset time difference threshold and the corresponding attitude angle components are selected and combined to obtain the associated data tuples.

[0009] Optionally, the original slope distance measurement value is corrected to obtain an effective slope distance value, including: The calibration function is called to perform nonlinear compensation or temperature drift correction on the original slope distance measurement value to obtain the corrected slope distance value. Based on threshold judgment, the corrected slope distance value is data-removed, and then the effective slope distance value is obtained through moving average filtering.

[0010] Optionally, the attitude angle components include a horizontal tilt angle component and a vertical tilt angle component; The original attitude angle measurement values ​​are subjected to attitude calculation processing to obtain attitude angle components. Specifically, the original measurement data are filtered and Euler angles are calculated to obtain the horizontal tilt angle component and the vertical tilt angle component.

[0011] Optionally, the calibration line distance is calculated based on the data tuple and a preset formula, including: Obtain the reference horizontal tilt angle and the reference vertical tilt angle; The horizontal offset of the horizontal tilt angle component relative to a reference horizontal tilt angle, and the vertical offset of the vertical tilt angle component relative to a reference vertical tilt angle are monitored; wherein, , This represents the horizontal offset. This represents the horizontal tilt angle component. Indicates the reference horizontal tilt angle; , This represents the vertical offset. This represents the vertical tilt angle component. Indicates the reference vertical tilt angle; When the absolute value of the horizontal offset is less than or equal to a first preset angle threshold, and the absolute value of the vertical offset is less than or equal to the first preset angle threshold, the calibration line distance is calculated according to a first preset formula, which is as follows: ; in, For the calibration straight-line distance, The effective slope distance value; When the absolute value of the horizontal offset or the absolute value of the vertical offset is greater than the first preset angle threshold, the calibration line distance is calculated according to the second preset formula, which is as follows: ; in, This is the system gain factor.

[0012] Optionally, when the absolute value of the horizontal offset is greater than the second preset angle threshold and the duration is greater than the first preset duration, the current horizontal tilt angle component is updated to a new reference horizontal tilt angle. When the absolute value of the vertical offset is greater than the second preset angle threshold and the duration is greater than the second preset duration, the current vertical tilt angle component is updated to a new reference vertical tilt angle. Wherein, the second preset angle threshold is less than the first preset angle threshold.

[0013] Optionally, the validity of the calibrated straight-line distance is verified to obtain the valid straight-line distance, including: Determine whether the current calibration linear distance is within the effective measurement range of the TOF sensor; Calculate the difference between the current calibration line distance and the previous valid line distance, and determine whether the difference is greater than a preset mutation threshold; If the calibration straight-line distance does not meet the effective range or the difference is greater than the preset mutation threshold, the current calibration straight-line distance is discarded, and the previous effective straight-line distance is taken as the effective straight-line distance for this time.

[0014] Optionally, the method further includes: When the change in the effective straight-line distance is greater than the third static threshold, or when the change in the attitude angle component is greater than the fourth static threshold, the ranging frequency of the TOF sensor is restored from the second ranging frequency to the first ranging frequency.

[0015] Corresponding to the aforementioned adaptive distance detection method based on a TOF sensor, this invention provides an adaptive distance detection system based on a TOF sensor, comprising: The data acquisition module acquires the original slant range measurement value and the original attitude angle measurement value; The data correction module is used to correct the original slope distance measurement value to obtain an effective slope distance value; The attitude calculation module is used to perform attitude calculation processing on the original attitude angle measurement values ​​to obtain attitude angle components; The time alignment module is used to perform time alignment processing based on the effective slant range value and attitude angle components to obtain associated data tuples. The first calculation module is used to calculate the calibration line distance based on the data tuple and the preset formula; The verification module is used to verify the validity of the calibrated straight-line distance to obtain the valid straight-line distance; The second calculation module is used to calculate the time-series statistics based on a series of consecutive effective straight-line distances and to calculate the time-series changes based on a series of consecutive attitude angle components. An adaptive module is used to compare the time-series statistics with a first static threshold and the time-series change with a second static threshold; when the time-series statistics are less than the first static threshold and the time-series change is less than the second static threshold, the ranging frequency of the TOF sensor is adjusted from the first ranging frequency to the second ranging frequency; the second ranging frequency is less than the first ranging frequency.

[0016] In addition, to achieve the above objectives, the present invention also provides a computer-readable storage medium storing an adaptive distance detection program based on a TOF sensor, wherein when the adaptive distance detection program based on a TOF sensor is executed by a processor, the program implements the steps of the adaptive distance detection method based on a TOF sensor as described above.

[0017] Compared with the prior art, the present invention has the following beneficial effects: (1) By correcting the original slant range measurement value and performing attitude calculation on the original attitude angle measurement value, the preprocessing of the original measurement data of the TOF sensor is realized, providing an accurate data basis for subsequent calculations; by time alignment processing, the effective slant range value and attitude angle component are associated to ensure that the data used to calculate the calibration straight distance is consistent in time; the calibration straight distance is calculated based on the data tuple and the preset formula to realize the compensation for the installation tilt angle, which solves the technical problem in the prior art that the measurement result has a significant deviation from the actual straight distance due to the installation tilt of the TOF sensor; by validating the calibration straight distance, the reliability of the output data is ensured; by calculating the time series statistics and time series changes and comparing them with the static threshold, the ranging frequency is dynamically adjusted according to the environmental state, which solves the technical problem in the prior art that there is a lack of intelligent power consumption management and that it is impossible to balance low power consumption and fast response performance; when the time series statistics of the effective straight distance and the time series changes of the attitude angle component are both less than the corresponding threshold, the ranging frequency is reduced, which synergistically realizes the technical effect of reducing power consumption in a stable environment and maintaining response performance in an active scenario.

[0018] (2) By marking the effective slant range value and attitude angle component with timestamps respectively, the time identification of the two types of heterogeneous data is realized; by selecting data with a time difference less than the preset time difference threshold for combination, the synchronization of slant range data and attitude data in the associated data tuple in the time dimension is ensured, the calculation error caused by the deviation of data acquisition time is avoided, and the calculation accuracy of the calibration straight line distance is improved.

[0019] (3) By calling the calibration function for nonlinear compensation or temperature drift correction, the systematic measurement error introduced by the TOF sensor due to hardware characteristics and changes in ambient temperature is eliminated; by using threshold judgment to remove data, the interference of abnormal measurement values ​​on subsequent processing is eliminated; by using moving average filtering, random noise is smoothed, and the accuracy and stability of the effective slant range value are further improved; the above multi-step synergistic effect significantly improves the reliability and anti-interference ability of slant range measurement.

[0020] (4) By filtering the original attitude angle measurement values, the influence of TOF sensor noise on attitude measurement is suppressed; by Euler angle calculation, the original measurement data is converted into horizontal tilt angle components and vertical tilt angle components with clear physical meaning, providing directly usable attitude parameters for subsequent distance compensation calculation based on tilt angle, and realizing the quantitative characterization of installation tilt state.

[0021] (5) By monitoring the horizontal and vertical offsets, the deviation of the current attitude of the TOF sensor from the reference attitude is quantified in real time. When the absolute value of the offset is less than or equal to the first preset angle threshold, the first preset formula is used for calculation, that is, the calibration straight distance is quickly obtained by simplifying the calculation in the small angle deviation scenario. When the absolute value of any offset is greater than the first preset angle threshold, the second preset formula containing the system gain factor is used for calculation, that is, the calculation accuracy is improved by gain compensation in the large angle deviation scenario. The synergistic effect of the two calculation modes enables accurate straight distance measurement results to be obtained under different tilt degrees, taking into account both calculation efficiency and measurement accuracy.

[0022] (6) When the absolute value of the horizontal offset or the vertical offset is greater than the second preset angle threshold and the duration exceeds the corresponding preset duration, the current tilt angle component is updated to a new reference tilt angle, thus realizing the adaptive dynamic adjustment of the reference attitude. The second preset angle threshold is less than the first preset angle threshold, which avoids frequent changes in the reference due to instantaneous disturbances. Therefore, the present invention can adapt to the long-term slow change of the TOF sensor installation attitude or the attitude change after reinstallation, maintaining the accuracy of distance calibration and the long-term stability of the system.

[0023] (7) By judging whether the calibration straight distance is within the effective range, invalid data caused by the target exceeding the measurement range is eliminated; by judging whether the difference with the previous effective straight distance is greater than the preset mutation threshold, unreliable data caused by measurement abnormality or target mutation is identified and eliminated; when the data is invalid, the previous effective straight distance is used as the output, ensuring the continuity and stability of the distance output; the above verification mechanisms work together to significantly improve the reliability of the effective straight distance and the robustness of the system output.

[0024] (8) By monitoring the changes in effective straight distance and attitude angle components, real-time perception of the target activity state and the attitude change state of the TOF sensor is realized; when any change exceeds the corresponding static threshold, the frequency is restored to a high ranging frequency, thus achieving the technical effect of rapid response and timely restoration of high-frequency measurement mode when environmental changes or target activity are detected, ensuring dynamic response performance. Attached Figure Description

[0025] The accompanying drawings, which are included to provide a further understanding of the invention and form part of this invention, illustrate exemplary embodiments of the invention and are used to explain the invention, but do not constitute an undue limitation of the invention. In the drawings: Figure 1 This is a simplified flowchart of an embodiment of the adaptive distance detection method based on a TOF sensor according to the present invention; Figure 2This is a framework diagram of an embodiment of the adaptive distance detection system based on a TOF sensor according to the present invention. Detailed Implementation

[0026] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0027] Current Time-of-Flight (TOF) ranging algorithms generally assume that the TOF sensor is in an ideal installation state and directly output the oblique distance along the optical axis. When the TOF sensor is installed at an angle, the "oblique distance" output by the algorithm does not match the "straight-line distance" actually required by the user, resulting in significant deviations in the detection results. For example, if the user is located at an actual straight-line distance of 50cm, an obliquely installed TOF sensor may output an oblique distance of 60cm.

[0028] like Figure 1 As shown, an adaptive distance detection method based on a TOF sensor according to the present invention includes the following steps: Obtain the original slant range measurement value and the original attitude angle measurement value; The original slant range measurement values ​​are corrected to obtain the effective slant range values; the original attitude angle measurement values ​​are subjected to attitude calculation to obtain the attitude angle components. Time alignment is performed based on the effective slant range value and attitude angle components to obtain the associated data tuples; The calibration line distance is calculated based on the data tuples and preset formulas; The validity of the calibrated straight-line distance is verified to obtain the effective straight-line distance; The time-series statistics are calculated based on multiple consecutive effective straight-line distances, and the time-series changes are calculated based on multiple consecutive attitude angle components. The time-series statistics are compared with the first static threshold, and the time-series change is compared with the second static threshold. When the time-series statistics are less than the first static threshold and the time-series change is less than the second static threshold, the ranging frequency of the TOF sensor is adjusted from the first ranging frequency to the second ranging frequency. The second ranging frequency is less than the first ranging frequency.

[0029] Preferably, the time-series statistic is variance, and the first static threshold is preset according to actual needs. By using the variance of the effective straight-line distance over a period of time, it is determined whether the user is stationary or moving slowly.

[0030] In this embodiment, the second static threshold is preset based on the attitude angle components that can be used to measure the steady state of the TOF sensor.

[0031] Understandably, when the time-series statistics are less than the first static threshold and the time-series change is less than the second static threshold, it is determined to be in an inactive state (variance is below the threshold and attitude angle is stable), triggering entry into a low-power mode. Preferably, the first ranging frequency is 10Hz and the second ranging frequency is 1Hz. Adjusting the ranging frequency of the TOF sensor from the first ranging frequency to the second ranging frequency can significantly reduce the power consumption of the TOF sensor. However, in this low-power mode, the distance calibration algorithm does not stop, but continues to run at an extremely low frequency (1Hz). Utilizing the sparse data points collected at this time, combined with the previously stored installation tilt angle reference, it maintains "lightweight perception" of the environment, ensuring that accurate ranging capability can be quickly restored upon wake-up.

[0032] This invention preprocesses the raw TOF sensor measurement data by correcting the original slant range measurement values ​​and performing attitude calculation on the original attitude angle measurement values, providing an accurate data foundation for subsequent calculations. Time alignment processing correlates the effective slant range value with the attitude angle components, ensuring temporal consistency of the data used to calculate the calibration straight-line distance. The calibration straight-line distance is calculated based on data tuples and preset formulas, compensating for the installation tilt angle and solving the technical problem in existing technologies where the measurement results deviate significantly from the actual straight-line distance due to TOF sensor installation tilt. The validity of the calibration straight-line distance is verified, ensuring the reliability of the output data. By calculating the temporal statistics of the effective straight-line distance and the temporal changes of the attitude angle components and comparing them with static thresholds, the ranging frequency is dynamically adjusted according to environmental conditions, solving the technical problems in existing technologies that lack intelligent power management and cannot balance low power consumption and fast response performance. When both the temporal statistics and the temporal changes are less than the corresponding thresholds, the ranging frequency is reduced, synergistically achieving the technical effect of reducing power consumption in stable environments and maintaining response performance in active scenarios.

[0033] In this embodiment, time alignment is performed based on the effective slant range value and attitude angle components to obtain the associated data tuple, including: Mark the effective slant range value and attitude angle component with the corresponding timestamps; Based on the timestamp sequence, the effective slant range values ​​with a time difference less than a preset time difference threshold and their corresponding attitude angle components are selected and combined to obtain the associated data tuples.

[0034] This invention achieves time identification of two types of heterogeneous data by marking the effective slant range value and attitude angle component with timestamps respectively; by selecting data with a time difference less than a preset time difference threshold for combination, it ensures the synchronization of slant range data and attitude data in the time dimension in the associated data tuple, avoids calculation errors caused by deviations in data acquisition time, and improves the calculation accuracy of calibration straight line distance.

[0035] In this embodiment, the original slope distance measurement value is corrected to obtain the effective slope distance value, including: The calibration function is called to perform nonlinear compensation or temperature drift correction on the original slant range measurement value to obtain the corrected slant range value; preferably, the calibration function is an internal calibration function provided by the TOF sensor manufacturer. Based on threshold judgment, the corrected slope distance values ​​are data-removed, and then the effective slope distance values ​​are obtained through moving average filtering.

[0036] This invention eliminates systematic measurement errors introduced by the hardware characteristics and ambient temperature changes of the TOF sensor by calling a calibration function for nonlinear compensation or temperature drift correction; it eliminates the interference of abnormal measurement values ​​on subsequent processing by using threshold judgment to remove data; and it smooths random noise through moving average filtering, further improving the accuracy and stability of the effective slope range value. The synergistic effect of the above multiple steps significantly improves the reliability and anti-interference capability of slope range measurement.

[0037] In this embodiment, the attitude angle components include a horizontal tilt angle component and a vertical tilt angle component; The original attitude angle measurements are processed to obtain attitude angle components. Specifically, the original measurement data are filtered and Euler angles are calculated to obtain horizontal tilt angle components and vertical tilt angle components.

[0038] This invention suppresses the influence of TOF sensor noise on attitude measurement by filtering the original attitude angle measurement values; and converts the original measurement data into horizontal and vertical tilt angle components with clear physical meaning through Euler angle calculation, providing directly usable attitude parameters for subsequent distance compensation calculation based on tilt angle, and realizing the quantitative characterization of the installation tilt state.

[0039] In this embodiment, the calibration line distance is calculated based on the data tuple and a preset formula, including: Obtain the reference horizontal tilt angle and the reference vertical tilt angle; Monitor the horizontal offset of the horizontal tilt angle component relative to the reference horizontal tilt angle, and the vertical offset of the vertical tilt angle component relative to the reference vertical tilt angle; wherein, , Indicates the horizontal offset. Indicates the horizontal tilt angle component. Indicates the reference horizontal tilt angle; , Indicates the vertical offset. Indicates the vertical tilt angle component. Indicates the reference vertical tilt angle; When the absolute value of the horizontal offset is less than or equal to the first preset angle threshold, and the absolute value of the vertical offset is less than or equal to the first preset angle threshold, the calibration straight-line distance is calculated according to the first preset formula, which is as follows: ; in, To calibrate the straight-line distance, The effective slope distance value; the first preset angle threshold is preferably 5°; When the absolute value of the horizontal offset or the absolute value of the vertical offset is greater than the first preset angle threshold, the calibration straight-line distance is calculated according to the second preset formula, which is as follows: ; in, This is the system gain factor.

[0040] Furthermore, if the horizontal tilt angle component is greater than the third preset angle threshold (preferably 45°), or the vertical tilt angle component is greater than the third preset angle threshold, it is determined to be an installation abnormality, an alarm is triggered, and calibration is paused.

[0041] In this embodiment, the system gain factor is used to compensate for the calibration error of the TOF sensor itself, the attitude calculation deviation, and the assembly tolerance between the two. The value can be determined through a simple on-site calibration procedure: at a known standard distance... Place the reflector at a point (e.g., D=50cm) and record the slope distance at that point. The horizontal tilt angle component at this time The vertical tilt angle component at this time ,calculate .

[0042] This invention achieves real-time quantification of the deviation of the current attitude of the TOF sensor from the reference attitude by monitoring horizontal and vertical offsets. When the absolute value of the offset is less than or equal to a first preset angle threshold, a first preset formula is used for calculation, which simplifies the calculation and quickly obtains the calibration straight-line distance in scenarios with small angle deviations. When the absolute value of any offset is greater than the first preset angle threshold, a second preset formula including a system gain factor is used for calculation, which improves the calculation accuracy through gain compensation in scenarios with large angle deviations. The synergistic effect of the two calculation modes enables accurate straight-line distance measurement results to be obtained under different tilt levels, balancing computational efficiency and measurement accuracy.

[0043] In this embodiment, when the absolute value of the horizontal offset is greater than the second preset angle threshold and the duration is greater than the first preset duration, the current horizontal tilt angle component is updated to a new reference horizontal tilt angle; the second preset angle threshold is preferably 1°, and the first preset duration is preferably 1 minute; When the absolute value of the vertical offset is greater than the second preset angle threshold and the duration is greater than the second preset duration, the current vertical tilt angle component is updated to a new reference vertical tilt angle. The second preset angle threshold is less than the first preset angle threshold.

[0044] When the absolute value of the horizontal or vertical offset is greater than the second preset angle threshold and the duration exceeds the corresponding preset duration, the present invention updates the current tilt angle component to a new reference tilt angle, thereby realizing adaptive dynamic adjustment of the reference attitude. The second preset angle threshold is less than the first preset angle threshold, which avoids frequent changes in the reference due to instantaneous disturbances. Therefore, the present invention can adapt to the long-term slow changes in the installation attitude of the TOF sensor or the attitude changes after reinstallation, maintaining the accuracy of distance calibration and the long-term stability of the system.

[0045] In this embodiment, the validity of the calibrated straight-line distance is verified to obtain the valid straight-line distance, including: Determine whether the current calibration line distance is within the effective range of the TOF sensor; preferably, the effective range is 5-150cm.

[0046] Calculate the difference between the current calibration straight line distance and the previous valid straight line distance, and determine whether the difference is greater than a preset mutation threshold; preferably, the preset mutation threshold can be set according to the maximum hand movement speed. If the calibration straight-line distance does not meet the effective range or the difference is greater than the preset mutation threshold, the current calibration straight-line distance will be discarded and the previous effective straight-line distance will be used as the effective straight-line distance for this measurement.

[0047] Furthermore, in practical applications, the effective straight-line distance is compared with a preset "distance-light parameter" mapping table to determine the corresponding control interval. To avoid frequent light jumps due to slight distance fluctuations at the interval boundaries, this invention introduces a hysteresis comparison mechanism. For example, if the switching boundary between the near field and the mid-field is 30cm, then the effective straight-line distance to enter the mid-field from the near field must be greater than 31cm, and the effective straight-line distance to return from the mid-field to the near field must be less than 29cm. Based on the determined interval, the system can generate a corresponding light control parameter package (target brightness, target color).

[0048] Furthermore, instead of directly sending the target parameters to the light strip, the system employs a linear gradation algorithm. For example, every 20ms, the current light value is approximated one step closer to the target value. This ensures that the light changes are smooth and fluid, without any abruptness.

[0049] By using a single-wire protocol, the gradually changing PWM control sequence is sent to the LED strip driver chip in real time, achieving seamless visual adjustment.

[0050] This invention eliminates invalid data caused by the target exceeding the measurement range by determining whether the calibration straight-line distance is within the effective measurement range; it identifies and eliminates unreliable data caused by measurement anomalies or target mutations by determining whether the difference with the previous effective straight-line distance is greater than a preset mutation threshold; and it uses the previous effective straight-line distance as the output when the data is invalid, ensuring the continuity and stability of the distance output. The above verification mechanisms work together to significantly improve the reliability of the effective straight-line distance and the robustness of the system output.

[0051] In this embodiment, the method further includes: When the change in effective straight-line distance exceeds the third static threshold, or the change in attitude angle components exceeds the fourth static threshold, the ranging frequency of the TOF sensor is restored from the second ranging frequency to the first ranging frequency. Preferably, the third and fourth static thresholds can be preset according to specific wake-up requirements. A change in attitude angle components exceeding the fourth static threshold means that the angle change exceeds the threshold.

[0052] If the change in effective straight-line distance is greater than the third static threshold (e.g., 2 cm), or the change in attitude angle components is greater than the fourth static threshold (e.g., 0.5°), the entire system can be immediately activated to full-speed operating mode to prepare for precise distance measurement and subsequent processing.

[0053] This invention achieves real-time perception of the target's activity state and the TOF sensor's attitude change state by monitoring the changes in the effective straight-line distance and the changes in the attitude angular components. When either change exceeds the corresponding static threshold, the measurement frequency is restored to a high ranging frequency. This synergistically achieves the technical effect of rapid response and timely restoration of high-frequency measurement mode when environmental changes or target activity are detected, ensuring dynamic response performance.

[0054] like Figure 2 As shown, the present invention also provides an adaptive distance detection system based on a TOF sensor, which includes: Data acquisition module 10 acquires the original slant range measurement value and the original attitude angle measurement value; Data correction module 20 is used to correct the original slope distance measurement value to obtain the effective slope distance value; The attitude calculation module 30 is used to perform attitude calculation processing on the original attitude angle measurement values ​​to obtain attitude angle components; The time alignment module 40 is used to perform time alignment processing based on the effective slant range value and attitude angle components to obtain the associated data tuples. The first calculation module 50 is used to calculate the calibration line distance based on the data tuple and the preset formula; Verification module 60 is used to verify the validity of the calibrated straight-line distance to obtain the valid straight-line distance; The second calculation module 70 is used to calculate its timing statistics based on multiple consecutive effective straight-line distances and to calculate its timing changes based on multiple consecutive attitude angle components. The adaptive module 80 is used to compare the time-series statistics with a first static threshold and the time-series change with a second static threshold; when the time-series statistics are less than the first static threshold and the time-series change is less than the second static threshold, the ranging frequency of the TOF sensor is adjusted from the first ranging frequency to the second ranging frequency; the second ranging frequency is less than the first ranging frequency.

[0055] Preferably, the data acquisition module 10 includes a TOF sensing unit for acquiring raw slant range measurements and an angle detection unit for acquiring raw attitude angle measurements. The TOF sensing unit uses a TOF sensor. It is connected to the main controller via an I²C bus (standard mode, 400kHz), responsible for emitting modulated infrared light and receiving target reflection signals, outputting the raw slant range measurement corresponding to the raw flight time. The angle detection unit samples at a fixed frequency of 20Hz, using a motion processing chip integrating a three-axis gyroscope and a three-axis accelerometer. It is physically connected to the TOF sensor, sharing the same I²C bus, and measures the static tilt angle or dynamic angular velocity of the TOF sensor relative to the horizontal plane in real time. These measurements are then processed by the attitude calculation module 30 to output high-precision horizontal and vertical tilt angle components.

[0056] In this embodiment, the TOF sensor triggers a single measurement based on the current operating mode (1Hz or 10Hz).

[0057] In this embodiment, after the system is powered on, it first initializes, completing the static configuration of peripherals such as clock, GPIO, and I²C. Then, the TOF sensing unit and angle detection unit perform power-on self-tests sequentially, verifying the device ID by sending a specific query command to confirm that the communication link is normal. Next, the angle detection unit performs a static zero-bias calibration for up to 2 seconds, during which the module remains absolutely still to eliminate the inherent error of the TOF sensor. Finally, the system state variables are initialized, including historical distance values, historical angle values, and operating mode flags. Among them, the historical distance value is a circular buffer of length 5, used to store the 5 most recent effective distances L(n), initialized to the midpoint of the range (e.g., 75cm); the historical angle value is the last effective attitude angle, initialized to 0°; the operating mode flag is initialized to STANDBY (standby mode); for the system gain factor, if field calibration has been performed, the calibration value is loaded, otherwise it defaults to 1.0.

[0058] Preferably, a specific query command is sent to verify the device ID to confirm that the communication link is normal. This includes sending a read device ID command (e.g., I²C address 0x52, register 0xC0) to the TOF sensing unit, expecting to receive a specific value (e.g., 0xEE). Similarly, a read WHO_AM_I command (register 0x75) is sent to the angle detection unit, expecting to receive 0x68. If any ID verification fails, the system enters a fault mode, which can be indicated by flashing LEDs.

[0059] This invention also provides a computer-readable storage medium, which may be a computer-readable storage medium included in the memory described in the above embodiments; or it may be a standalone computer-readable storage medium not assembled into a device. The computer-readable storage medium stores at least one instruction, which is loaded and executed by a processor to implement... Figure 1 The diagram illustrates an adaptive distance detection method based on a Time-of-Flight (TOF) sensor. The computer-readable storage medium can be a read-only memory, a hard disk, or an optical disk, etc.

[0060] It should be noted that the various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For system embodiments and storage medium embodiments, since they are basically similar to method embodiments, the descriptions are relatively simple; relevant parts can be referred to the descriptions in the method embodiments.

[0061] Furthermore, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0062] The foregoing description illustrates and describes preferred embodiments of the present invention. It should be understood that the present invention is not limited to the forms disclosed herein and should not be construed as excluding other embodiments. It can be used in various other combinations, modifications, and environments, and can be altered within the scope of the inventive concept by means of the foregoing teachings or techniques or knowledge in related fields. Any modifications and variations made by those skilled in the art that do not depart from the spirit and scope of the present invention should be within the protection scope of the appended claims.

Claims

1. An adaptive distance detection method based on a TOF sensor, characterized in that, Includes the following steps: Obtain the original slant range measurement value and the original attitude angle measurement value; The original slant range measurement value is corrected to obtain the effective slant range value; the original attitude angle measurement value is subjected to attitude calculation to obtain the attitude angle components. Based on the effective slant range value and attitude angle components, time alignment processing is performed to obtain the associated data tuple; Based on the data tuples and the preset formula, the calibration line distance is calculated; The validity of the calibrated straight-line distance is verified to obtain the valid straight-line distance; The temporal statistics are calculated based on multiple consecutive effective straight-line distances, and the temporal changes are calculated based on multiple consecutive attitude angle components. The time-series statistics are compared with a first static threshold, and the time-series change is compared with a second static threshold; when the time-series statistics are less than the first static threshold and the time-series change is less than the second static threshold, the ranging frequency of the TOF sensor is adjusted from the first ranging frequency to the second ranging frequency; the second ranging frequency is less than the first ranging frequency.

2. The adaptive distance detection method based on a TOF sensor according to claim 1, characterized in that, Based on the effective slant range value and attitude angle components, time alignment processing is performed to obtain the associated data tuple, including: The effective slant range value and the attitude angle component are respectively marked with corresponding timestamps; Based on the timestamp sequence, the effective slant range values ​​with a time difference less than a preset time difference threshold and the corresponding attitude angle components are selected and combined to obtain the associated data tuples.

3. The adaptive distance detection method based on a TOF sensor according to claim 1, characterized in that, The original slope distance measurement value is corrected to obtain the effective slope distance value, including: The calibration function is called to perform nonlinear compensation or temperature drift correction on the original slope distance measurement value to obtain the corrected slope distance value. Based on threshold judgment, the corrected slope distance value is data-removed, and then the effective slope distance value is obtained through moving average filtering.

4. The adaptive distance detection method based on a TOF sensor according to claim 1, characterized in that, The attitude angle components include horizontal tilt angle components and vertical tilt angle components; The original attitude angle measurement values ​​are subjected to attitude calculation processing to obtain attitude angle components. Specifically, the original measurement data are filtered and Euler angles are calculated to obtain the horizontal tilt angle component and the vertical tilt angle component.

5. The adaptive distance detection method based on a TOF sensor according to claim 4, characterized in that, The calibration line distance is calculated based on the data tuples and preset formulas, including: Obtain the reference horizontal tilt angle and the reference vertical tilt angle; The horizontal offset of the horizontal tilt angle component relative to a reference horizontal tilt angle, and the vertical offset of the vertical tilt angle component relative to a reference vertical tilt angle are monitored; wherein, , This represents the horizontal offset. This represents the horizontal tilt angle component. Indicates the reference horizontal tilt angle; , This represents the vertical offset. This represents the vertical tilt angle component. Indicates the reference vertical tilt angle; When the absolute value of the horizontal offset is less than or equal to a first preset angle threshold, and the absolute value of the vertical offset is less than or equal to the first preset angle threshold, the calibration line distance is calculated according to a first preset formula, which is as follows: ； in, For the calibration straight-line distance, The effective slope distance value; When the absolute value of the horizontal offset or the absolute value of the vertical offset is greater than the first preset angle threshold, the calibration line distance is calculated according to the second preset formula, which is as follows: ； in, This is the system gain factor.

6. The adaptive distance detection method based on a TOF sensor according to claim 5, characterized in that, When the absolute value of the horizontal offset is greater than the second preset angle threshold and the duration is greater than the first preset duration, the current horizontal tilt angle component is updated to a new reference horizontal tilt angle. When the absolute value of the vertical offset is greater than the second preset angle threshold and the duration is greater than the second preset duration, the current vertical tilt angle component is updated to a new reference vertical tilt angle. Wherein, the second preset angle threshold is less than the first preset angle threshold.

7. The adaptive distance detection method based on a TOF sensor according to claim 1, characterized in that, The validity of the calibrated straight-line distance is verified to obtain the valid straight-line distance, including: Determine whether the current calibration linear distance is within the effective measurement range of the TOF sensor; Calculate the difference between the current calibration line distance and the previous valid line distance, and determine whether the difference is greater than a preset mutation threshold; If the calibration straight-line distance does not meet the effective range or the difference is greater than the preset mutation threshold, the current calibration straight-line distance is discarded, and the previous effective straight-line distance is taken as the effective straight-line distance for this time.

8. The adaptive distance detection method based on a TOF sensor according to claim 1, characterized in that, Also includes: When the change in the effective straight-line distance is greater than the third static threshold, or when the change in the attitude angle component is greater than the fourth static threshold, the ranging frequency of the TOF sensor is restored from the second ranging frequency to the first ranging frequency.

9. An adaptive distance detection system based on a TOF sensor, characterized in that, include: The data acquisition module acquires the original slant range measurement value and the original attitude angle measurement value; The data correction module is used to correct the original slope distance measurement value to obtain an effective slope distance value; The attitude calculation module is used to perform attitude calculation processing on the original attitude angle measurement values ​​to obtain attitude angle components; The time alignment module is used to perform time alignment processing based on the effective slant range value and attitude angle components to obtain associated data tuples. The first calculation module is used to calculate the calibration line distance based on the data tuple and the preset formula; The verification module is used to verify the validity of the calibrated straight-line distance to obtain the valid straight-line distance; The second calculation module is used to calculate the time-series statistics based on a series of consecutive effective straight-line distances and to calculate the time-series changes based on a series of consecutive attitude angle components. An adaptive module is used to compare the time-series statistics with a first static threshold and the time-series change with a second static threshold; when the time-series statistics are less than the first static threshold and the time-series change is less than the second static threshold, the ranging frequency of the TOF sensor is adjusted from the first ranging frequency to the second ranging frequency; the second ranging frequency is less than the first ranging frequency.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores an adaptive distance detection program based on a TOF sensor, which, when executed by a processor, implements the steps of the adaptive distance detection method based on a TOF sensor as described in any one of claims 1 to 8.

Images