Cooperative positioning method and system based on four laser range finders

Abstract

The invention relates to the technical field of high-precision positioning, and discloses a cooperative positioning method and system based on four laser range finders, and the method comprises the steps: controlling the four laser range finders to synchronously measure the distance value to a target point, and enabling the four laser range finders to be in an asymmetric tetrahedron spatial layout; performing dynamic error compensation on the distance value; dynamic error compensation comprises environmental parameter correction and abnormal value filtering processing; establishing an overdetermined equation set based on the four compensated distance values, and solving the three-dimensional coordinates of the target point by adopting a weighted least square method; and according to the obtained three-dimensional coordinates of the target point at different moments, displaying the moving trajectory of the target point in the target time period. According to the scheme, cost can be reduced, and positioning precision and system reliability are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of high-precision positioning, and particularly relates to a collaborative positioning method and system based on four laser rangefinders. Background Art

[0002] As the core sensing unit of an automated system, the positioning technology of laser rangefinders directly determines the quality of equipment motion control and operation safety. Especially in fields such as high-end manufacturing, aerospace, etc., which have strict requirements for accuracy and reliability, the breakthrough of this technology will promote the development process of intelligent manufacturing and autonomous systems.

[0003] Traditional single laser rangefinders can only provide one-way distance information and cannot directly locate the coordinates of the target point. The three-laser rangefinder system (based on the trilateration principle) is vulnerable to sensor errors, environmental interference, or occlusion, resulting in a decrease or failure of positioning accuracy. Existing multi-sensor positioning systems mostly rely on complex algorithms or high-cost equipment (such as lidar), and it is difficult to achieve high-robustness positioning at low cost. The current laser rangefinder positioning technology mainly relies on a fixed configuration of multi-sensor layouts and static data fusion algorithms, and has the following significant defects: First, the sensor network lacks fault tolerance design, and a single point failure is likely to cause the system to crash, and the spatial layout of redundant sensors does not fully consider the geometric error amplification effect; Second, a fixed weight allocation strategy is adopted during the data fusion process, and it cannot be dynamically adjusted according to environmental disturbances or changes in sensor performance, resulting in insufficient ability to suppress outliers; Third, the environmental parameter compensation mechanism relies on a preset empirical model and is difficult to respond in real time to dynamic disturbances such as temperature and humidity changes and air refractive index fluctuations. In addition, traditional methods have insufficient monitoring of the long-term stability of sensors and cannot effectively identify progressive performance degradation (such as optical path contamination or component aging), resulting in a gradual decrease in positioning accuracy over time. These defects limit the application of existing technologies in complex industrial scenarios, especially in working conditions with vibration, temperature changes, or local occlusion, and the system reliability is significantly reduced.

[0004] Therefore, there is an urgent need to develop a collaborative positioning method and system based on four laser rangefinders, which can reduce costs, improve positioning accuracy, and system reliability. Summary of the Invention

[0005] In order to solve the above technical problems, the present invention provides a collaborative positioning method and system based on four laser rangefinders, which can reduce costs, improve positioning accuracy, and system reliability.

[0006] The present invention provides a collaborative positioning method based on four laser rangefinders, and the method includes the following steps: S1. Control four laser rangefinders to synchronously measure the distance values to the target point, and the four laser rangefinders are arranged in an asymmetric tetrahedral spatial layout; S2. Perform dynamic error compensation on the distance values; the dynamic error compensation includes environmental parameter correction and outlier filtering. S3. Establish an overdetermined system of equations based on the four compensated distance values, and use the weighted least squares method to solve for the three-dimensional coordinates of the target point. S4. According to the three-dimensional coordinates of the target point at different times obtained, display the running trajectory of the target point within the target time period.

[0007] Further, in S1, the asymmetric tetrahedral spatial layout of the four laser rangefinders includes: The elevations of the four laser rangefinders are not equal. The distances from the four laser rangefinders to the target point are not equal. The included angle between the optical axes of any two of the four laser rangefinders is greater than or equal to 30°.

[0008] Further, in S2, the environmental parameter correction includes: S21. Obtain the environmental temperature value through a temperature sensor and obtain the relative air humidity value through a humidity sensor. S22. Perform speed compensation on the distance values according to the environmental temperature value and the relative air humidity value.

[0009] Further, in S3, the formula for establishing the overdetermined system of equations is as follows: ; where (x, y, z) represents the three-dimensional coordinates of the target point, (x i , y i , z i ) represents the three-dimensional coordinates of the i-th laser rangefinder, i represents the i-th laser rangefinder, and d i ' represents the compensated distance value of the i-th laser rangefinder.

[0010] Further, in S3, using the weighted least squares method to solve for the three-dimensional coordinates of the target point includes: S31. Establish a distance residual function with the target point coordinates as unknowns based on the overdetermined system of equations and the compensated distance values. S32. Generate dynamic weight factors for each laser rangefinder based on the historical error rates of each laser rangefinder. S33. Perform a weighted sum of squares minimization operation on the dynamic weight factors and the distance residual function to obtain the three-dimensional coordinates of the target point.

[0011] Further, in S32, generating dynamic weight factors for each laser rangefinder based on the historical error rates of each laser rangefinder, the calculation formula is as follows: ; where w i(t) represents the dynamic weight factor of the i-th laser rangefinder at time t. N represents the size of the sliding window, k represents the time index within the sliding window, t represents the current time, and d i ’ (k) represents the compensated distance value of the i-th laser rangefinder at the time corresponding to the k-th time index, μ i (t) represents the mean value of the compensated distance values of the i-th laser rangefinder within the sliding window. represents the smoothing constant.

[0012] Furthermore, when controlling the four laser rangefinders to synchronously measure the distance values to the target point, it further includes: When it is detected that the data of any one laser rangefinder fails, it automatically switches to the three-laser rangefinder solution mode and triggers an alarm, re-establishes an overdetermined system of equations based on the three compensated distance values, and uses the weighted least squares method to solve the three-dimensional coordinates of the target point.

[0013] The present invention also provides a collaborative positioning system based on four laser rangefinders for performing the above-mentioned collaborative positioning method based on four laser rangefinders. The system includes the following modules: Four laser rangefinders for measuring the distance values between the laser rangefinders and the target point. The four laser rangefinders are arranged in an asymmetric tetrahedral spatial layout; A synchronization control module for controlling the four laser rangefinders to perform synchronous measurement; A data processing unit for performing dynamic error compensation on the distance values; the dynamic error compensation includes environmental parameter correction and outlier filtering processing; and establishing an overdetermined system of equations based on the four compensated distance values and using the weighted least squares method to solve the three-dimensional coordinates of the target point; A trajectory display module for showing the running trajectory of the target point during the target period according to the three-dimensional coordinates of the target point at different times obtained.

[0014] The embodiments of the present invention have the following technical effects: The present invention is based on an asymmetric tetrahedron spatial layout design. By maximizing the measurement baseline length and optimizing the non-coplanar angle of the optical axes, it effectively suppresses the geometric error amplification effect, and solves the problem of positioning sensitivity attenuation of traditional symmetric layout systems at specific azimuth angles. It adopts a dynamic weighted fusion algorithm, combines sliding window statistics and historical error analysis, and establishes a dynamic mapping relationship between sensor weights and data credibility, enabling the system to autonomously identify and suppress abnormal data caused by environmental mutations or device aging. At the same time, it eliminates instantaneous noise interference through time series smoothing processing. For the risk of single-point failure, it designs a redundant degradation mechanism and a residual consistency test algorithm to achieve fast isolation of faulty sensors and seamless switching of the solution mode. Its core principle is to transform the static sensing network into a dynamic collaborative system with self-sensing and self-correcting capabilities. Through the synergistic effects of spatial layout optimization, data credibility evaluation, environmental interference suppression, and fault-tolerant logic control, it breaks through the dependence of traditional positioning technologies on ideal environments and fixed weight strategies, and significantly improves the positioning stability and anti-interference ability under complex working conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] In order to more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the following will briefly introduce the drawings required for the description of the specific embodiments or the prior art. Obviously, the drawings in the following description are some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0016] Figure 1 It is a flowchart of a collaborative positioning method based on four laser rangefinders provided by an embodiment of the present invention; Figure 2 It is a schematic diagram of an asymmetric tetrahedron layout of four laser rangefinders provided by an embodiment of the present invention; Figure 3 It is a schematic diagram of the structure of a collaborative positioning system based on four laser rangefinders provided by an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0017] To make the objectives, technical solutions, and advantages of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of them. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts fall within the scope of protection of the present invention.

[0018] An embodiment of the present invention provides a collaborative positioning method based on four laser rangefinders, Figure 1 It is a flowchart of a collaborative positioning method based on four laser rangefinders provided by an embodiment of the present invention. SeeFigure 1 , the method includes the following steps: S1. Control four laser rangefinders to synchronously measure the distance values to the target point. The four laser rangefinders are arranged in an asymmetric tetrahedral spatial layout.

[0019] In some embodiments, Figure 2 is a schematic diagram of an asymmetric tetrahedral layout of four laser rangefinders provided by an embodiment of the present invention. Refer to Figure 2 , the four laser rangefinders being arranged in an asymmetric tetrahedral spatial layout includes: The elevations of the four laser rangefinders are not equal, that is, the heights of the four laser rangefinders from the ground are different; The distances from the four laser rangefinders to the target point are not equal; The included angle between the optical axes of any two of the four laser rangefinders is greater than or equal to 30°; the purpose is to enable the four laser rangefinders to cover the three-dimensional space of the target area, avoid the solution ambiguity caused by coplanarity, and ensure the maximization of the measurement baseline length.

[0020] Let the position coordinates of the four laser rangefinders be P i (x i ,y i ,z i ) (i = 1, 2, 3, 4), the position of the target point is (x, y, z), and each rangefinder synchronously emits a laser pulse to measure the distance value d i . Through the non-coplanar optical axis design and spatial topology optimization, the geometric sensitivity limitation of the traditional symmetric layout is broken through, the error amplification effect caused by unreasonable spatial distribution of the sensors is significantly reduced, the robustness of the system to the change of the target azimuth is enhanced, and the redundancy and complementarity of the measurement baseline in any direction are ensured by using the spatial coverage characteristics of the tetrahedral structure, laying a high signal-to-noise ratio foundation for subsequent data fusion.

[0021] S2. Perform dynamic error compensation on the distance values.

[0022] Among them, the dynamic error compensation includes environmental parameter correction and outlier filtering; the environmental parameter correction includes: S21. Obtain the environmental temperature value T through a temperature sensor and obtain the relative air humidity value H through a humidity sensor.

[0023] S22. Perform velocity compensation on the distance values according to the environmental temperature value and the relative air humidity value.

[0024] In some embodiments, the propagation speed v of the laser in the air is affected by the environmental temperature value T and the relative air humidity value H. The correction formula for performing velocity compensation on the distance values according to the environmental temperature value and the relative air humidity value is: ; ; where c represents the speed of light in vacuum, n(T, H) represents the refractive index of air, and d i ' represents the distance value after compensation of the i-th laser rangefinder, and d i represents the distance value from the i-th laser rangefinder to the target point, and n0 represents the refractive index under standard conditions.

[0025] Exemplarily, the refractive index of air n(T, H) can be calculated by an empirical formula: ; where λ represents the laser wavelength, P dry represents the dry air pressure, and P sat (T) represents the saturated water vapor pressure at the environmental temperature value T, 77.6×10 -6 represents the temperature dependence of the refractive index of dry air, 7.52×10 -3 represents the influence of the laser wavelength on the refractive index (dispersion effect), and 4810 represents the correction intensity of humidity on the refractive index.

[0026] In some embodiments, the outlier filtering process includes: Using Mahalanobis distance to detect abnormal data. Let the covariance matrix of historical measurement data be Σ, and the vector of the currently measured distance value be d = [d1, d2, d3, d4] ⊤ , and the mean of the currently measured distance value be μ. Then the Mahalanobis distance is: ; where D M represents the Mahalanobis distance, represents the transpose operation; when D M is greater than a preset threshold, it is determined as an outlier, and the distance value data of this group of measurements is excluded.

[0027] Based on real-time feedback of multi-source environmental parameters, a dynamic correction model for the physical characteristics of the laser propagation path is constructed to systematically eliminate the cumulative errors introduced by environmental factors such as temperature and humidity gradients and air disturbances.

[0028] S3. Establish an overdetermined system of equations based on the four compensated distance values, and use the weighted least squares method to solve the three-dimensional coordinates of the target point.

[0029] In some embodiments, the theoretical formula for establishing the overdetermined system of equations is as follows: ; where (x, y, z) represents the three-dimensional coordinates of the target point, and (x i , y i , z i ) represents the three-dimensional coordinates of the i-th laser rangefinder, and i represents the i-th laser rangefinder, di represents the distance value after compensation of the i-th laser rangefinder.

[0030] Due to the accuracy error of the laser rangefinder, as well as the calibration deviation of the installation position of the rangefinder, the deformation of the target reflection surface, etc., the established overdetermined equation system formula is a theoretical model, and there will be an error between the calculation result of the actual overdetermined equation system formula and the compensated distance value d i ', so the optimal approximate solution is found through the following steps.

[0031] S31. Establish a distance residual function with the target point coordinates as unknowns according to the overdetermined equation system and the compensated distance value.

[0032] In some embodiments, the distance residual function is defined as: ; where f i (x, y, z) represents the residual between the theoretical distance from the i-th laser rangefinder to the target point and the compensated distance value, that is, the residual between the theoretical distance and the measured distance.

[0033] S32. Generate dynamic weight factors for each laser rangefinder based on the historical error rates of each laser rangefinder.

[0034] In some embodiments, the calculation formula of the dynamic weight factor is as follows: ; where w i (t) represents the dynamic weight factor of the i-th laser rangefinder at time t, N represents the size of the sliding window, for example, N = 10 represents the results of the last 10 measurements, k represents the time index within the sliding window, t represents the current time, and d i ’ (k) represents the compensated distance value of the i-th laser rangefinder at the time corresponding to the k-th time index, and μ i (t) represents the mean value of the compensated distance values of the i-th laser rangefinder within the sliding window, represents the smoothing constant, which is used to prevent division by zero.

[0035] S33. Perform a weighted sum of squares minimization operation on the dynamic weight factor and the distance residual function to obtain the three-dimensional coordinates of the target point.

[0036] In some embodiments, the objective function for performing a weighted sum of squares minimization operation on the dynamic weight factor and the distance residual function is: ; Exemplarily, the Levenberg-Marquardt algorithm can be used to iteratively solve the three-dimensional coordinates (x, y, z) of the target point.

[0037] Furthermore, the positioning result can be smoothed by Kalman filtering to improve the stability of dynamic target tracking.

[0038] By adopting a dynamic weight allocation strategy, the historical reliability characteristics of the sensor are fused with the real-time residual information, and a higher-confidence rangefinder is given stronger dominance in the solution, significantly improving the positioning stability under complex working conditions.

[0039] Furthermore, when controlling the four laser rangefinders to synchronously measure the distance values to the target point, when it is detected that the data of any one laser rangefinder fails, it automatically switches to the three-laser rangefinder solution mode and triggers an alarm. An overdetermined system of equations is re-established based on the three compensated distance values, and the weighted least squares method is used to solve the three-dimensional coordinates of the target point.

[0040] In some embodiments, the criterion for the data failure of any one laser rangefinder may include: (1) Statistically calculate the historical measurement variance σ of a single laser rangefinder within a preset time window i 2 , if it exceeds the preset variance threshold σ th 2 , then it is determined that the data of this laser rangefinder has failed. For example, this occurs in the case of detecting progressive faults such as sensor aging and optical path pollution; (2) The measurement value of a single laser rangefinder undergoes a jump, the data mutation amplitude significantly exceeds the nominal accuracy of the sensor, or the continuous sampling values show irregular fluctuations. For example, this occurs in the case of detecting sudden interferences such as short-term occlusion and environmental disturbances.

[0041] When the above abnormal state continuously lasts for M sampling periods, it will trigger a failure to avoid false alarms caused by accidental misjudgments due to noise fluctuations.

[0042] Since there may be a situation of symmetric solutions in the three-laser rangefinder solution mode, when switching to the three-laser rangefinder solution mode, an alarm is automatically triggered and manual judgment of the final solution is required.

[0043] The calculation method in the three-laser rangefinder solution mode is the same as that in the four-laser rangefinder solution mode and will not be elaborated here.

[0044] By designing a redundant degradation and weight reallocation linkage mechanism, it seamlessly switches to a three-mode solution architecture when a single sensor fails, and reconstructs the optimal weight configuration through historical data backtracking, breaking through the bottleneck of functional interruption of traditional systems under local faults, and achieving a balance between rapid isolation of faulty sensors and continuous high-precision operation of the system.

[0045] In some embodiments, if the data of more than one laser rangefinder fails, the operation is stopped and an alarm is triggered.

[0046] S4. According to the three-dimensional coordinates of the target point at different times obtained, display the running track of the target point within the target time period.

[0047] The system binds the three-dimensional coordinates solved at each moment with the corresponding timestamp to form a continuous time-space data set. This process ensures the temporal correlation of the target point position and provides a basis for trajectory construction.

[0048] In some embodiments, linear interpolation technology can be adopted to generate smooth transition line segments between the coordinates of adjacent time points. When the target point moves smoothly, the path presents a soft curve; when the movement direction changes suddenly, the turning characteristics are clearly reflected through the break points, and the original movement characteristics are completely retained. Thus, it is applicable to scenarios with high-precision requirements such as industrial automation, warehousing logistics, and deformation of large-area yards.

[0049] The present invention is based on the design of an asymmetric tetrahedron spatial layout. By maximizing the measurement baseline length and optimizing the non-coplanar angle of the optical axes, the geometric error amplification effect is effectively suppressed, and the problem of positioning sensitivity attenuation of the traditional symmetric layout system at a specific azimuth angle is solved; a dynamic weighted fusion algorithm is adopted, combined with sliding window statistics and historical error analysis, to establish a dynamic mapping relationship between the sensor weight and data credibility, enabling the system to autonomously identify and suppress abnormal data caused by environmental mutations or device aging, and at the same time eliminating instantaneous noise interference through time series smoothing processing; for the risk of single-point failure, a redundant degradation mechanism and a residual consistency test algorithm are designed to achieve fast isolation of faulty sensors and seamless switching of the calculation mode; its core principle is to transform the static sensing network into a dynamic collaborative system with self-sensing and self-correction capabilities. Through the synergistic effect of spatial layout optimization, data credibility evaluation, environmental interference suppression, and fault tolerance logic control, it breaks through the dependence of traditional positioning technologies on ideal environments and fixed weight strategies, and significantly improves the positioning stability and anti-interference ability under complex working conditions.

[0050] An embodiment of the present invention also provides a collaborative positioning system based on four laser rangefinders for performing the above-mentioned collaborative positioning method based on four laser rangefinders. Figure 3 is a schematic structural diagram of a collaborative positioning system based on four laser rangefinders provided by an embodiment of the present invention. Refer to Figure 3 , the system includes the following modules: Four laser rangefinders for measuring the distance value between the laser rangefinder and the target point, and the four laser rangefinders are arranged in an asymmetric tetrahedron spatial layout; among them, the wavelength range of the four high-precision laser rangefinders is 905nm - 1550nm, and the accuracy is ±0.1mm.

[0051] A synchronization control module, which is used to control four laser rangefinders to perform synchronous measurements with an error < 1 ms; A data processing unit, which can use an embedded processor (such as an FPGA or an ARM Cortex-A series), is used to perform dynamic error compensation on the distance values; the dynamic error compensation includes environmental parameter correction and outlier filtering; and an overdetermined system of equations is established based on the four compensated distance values, and the weighted least squares method is used to solve the three-dimensional coordinates of the target point.

[0052] A trajectory display module, which is used to display the running trajectory of the target point within the target time period according to the three-dimensional coordinates of the target point at different times.

[0053] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the technical solutions of the embodiments of the present invention.

Claims

1. A collaborative positioning method based on four laser rangefinders, characterized in that: The method comprises the following steps: S1, controlling four laser rangefinders to synchronously measure the distance to the target point, wherein the four laser rangefinders are arranged in an asymmetric tetrahedron space; S2, performing dynamic error compensation on the distance value; the dynamic error compensation includes environmental parameter correction and outlier filtering; S3, establishing an overdetermined set of equations based on the four compensated distance values, and using a weighted least squares method to solve the three-dimensional coordinates of the target point; S4. Based on the obtained three-dimensional coordinates of the target point at different times, the running trajectory of the target point within the target period is displayed.

2. The collaborative positioning method based on four laser rangefinders according to claim 1, characterized in that: In S1, the four laser rangefinders are arranged in an asymmetric tetrahedral space, including: The elevations of the four laser rangefinders are not equal; The distances between the four laser rangefinders and the target point are not equal; The angle between the optical axes of any two laser rangefinders among the four laser rangefinders is greater than or equal to 30°.

3. The collaborative positioning method based on four laser rangefinders according to claim 1, characterized in that: In S2, the environmental parameter correction includes: S21, obtaining the ambient temperature value through the temperature sensor, and obtaining the relative humidity value of the air through the humidity sensor; S22. Perform speed compensation on the distance value according to the ambient temperature value and the air relative humidity value.

4. The collaborative positioning method based on four laser rangefinders according to claim 1, characterized in that: In S3, the formula for establishing the overdetermined equations is as follows: ； Where (x, y, z) represents the three-dimensional coordinates of the target point, (x i ,y i ,z i ) represents the three-dimensional coordinates of the i-th laser rangefinder, i represents the i-th laser rangefinder, d i ' represents the distance value after compensation of the i-th laser rangefinder.

5. The collaborative positioning method based on four laser rangefinders according to claim 4, characterized in that: In S3, using the weighted least squares method to solve the three-dimensional coordinates of the target point includes: S31, establishing a distance residual function with the target point coordinates as unknown quantities according to the overdetermined equation group and the compensated distance value; S32, generating a dynamic weight factor for each laser rangefinder based on the historical error rate of each laser rangefinder; S33, performing a weighted square sum minimization operation on the dynamic weight factor and the distance residual function to obtain the three-dimensional coordinates of the target point.

6. The collaborative positioning method based on four laser rangefinders according to claim 5, characterized in that: In S32, a dynamic weight factor of each laser rangefinder is generated based on the historical error rate of each laser rangefinder, and the calculation formula is as follows: ； Among them, w i (t) represents the dynamic weight factor of the i-th laser rangefinder at time t, N represents the size of the sliding window, k represents the time index within the sliding window, t represents the current time, and d i ' (k) represents the compensated distance value of the i-th laser rangefinder at the time corresponding to the k-th time index, μ i (t) represents the mean of the compensated distance values ​​of the i-th laser rangefinder within the sliding window, represents the smoothing constant.

7. The collaborative positioning method based on four laser rangefinders according to claim 1, characterized in that: When controlling four laser rangefinders to synchronously measure the distance to the target point, it also includes: When it is detected that the data of any laser rangefinder is invalid, it automatically switches to the three-laser rangefinder solution mode and triggers an alarm. The overdetermined equation group is re-established based on the three compensated distance values, and the weighted least squares method is used to solve the three-dimensional coordinates of the target point.

8. A collaborative positioning system based on four laser rangefinders, used to execute the collaborative positioning method based on four laser rangefinders as described in any one of claims 1 to 7, characterized in that: The system includes the following modules: Four laser rangefinders, used to measure the distance between the laser rangefinder and a target point, wherein the four laser rangefinders are arranged in an asymmetric tetrahedron space; A synchronous control module is used to control four laser rangefinders to perform synchronous measurement; A data processing unit, used for performing dynamic error compensation on the distance value; the dynamic error compensation includes environmental parameter correction and abnormal value filtering; and establishing an overdetermined set of equations based on the four compensated distance values, and using a weighted least squares method to solve the three-dimensional coordinates of the target point; The trajectory display module is used to display the running trajectory of the target point within the target period according to the obtained three-dimensional coordinates of the target point at different times.

Images