Method and system for tracking position and orientation of a target object via sensor fusion

Abstract

A computer-implemented method for tracking the position and orientation of a target object in a work site, the work site having a target object and a mapping instrument configured to emit tracking light and measure the position of the tracked target object, the method being executed by a computer system and comprising: - predicting the position and orientation of the target object relative to a previous position and orientation by integrating over time a 3D acceleration measured by an accelerometer sensing unit and a 3D angular velocity measured by a gyroscope sensing unit attached to the target object; - correcting the predicted position and orientation at least once using a first position of the target object measured by the mapping instrument; and - further correcting the corrected position and orientation over time using another position of the target object measured over time by the mapping instrument and changes in position and orientation of the target object relative to the previous position and orientation of the target object measured by a first sensor attached to the target object.

Description

[0001] The present invention relates to a computer-implemented method for tracking the position and orientation of a target object as defined in claim 1, a computer program as defined in claim 2, and an apparatus for tracking the position and orientation of a target object as defined in claim 3. Background Technology

[0002] In surveying applications, measurements taken with a total station do not directly correspond to the target point to be measured. Current practice involves mounting a reflector on a pole, ensuring the pole is perfectly vertical above the point of interest, and compensating for positional variations based on pole length. However, leveling the pole takes time, and the desired outcome is to measure the target location without needing to level the pole.

[0003] However, surveyors are not concerned with the reflector itself, but rather with a point on the ground; the reflector is typically mounted on a pole to optimize the reception of reflected signals. Unless the pole is perfectly upright relative to the ground, the horizontal position of the reflector will be offset relative to the point of interest on the ground. Leveling devices (whether traditional bubble levels or more complex sensor arrangements) can be used to determine whether the pole is upright within a certain tolerance sufficient for measurement. Alternatively, if the angular orientation ("attitude") of the pole can be accurately measured, the error can be compensated for, assuming the pole's length is known.

[0004] Measuring the angle of the pole relative to the ground is not a simple task. While the angles of the pole relative to the vertical (pitch and roll) can be accurately measured using various methods (in particular, by measuring local gravity using accelerometers or inclinometers), measuring the pole's orientation relative to true north (yaw or azimuth) is much more difficult. Summary of the Invention

[0005] Therefore, what is desired is a method and system for tracking the position and orientation of a target object without the need for a leveling rod.

[0006] These objectives are achieved by implementing the features of the independent claims. The dependent claims describe features that further develop the invention in an advantageous manner.

[0007] According to one aspect of the present invention, a computer-implemented method is provided for tracking the position and orientation of a target object in a work site, wherein the target object and a mapping instrument configured to emit tracking light and measure the position of the tracked target object are deployed in the work site. The method is executed by the computer system and includes the following steps:

[0008] ■ The position and orientation of the target object relative to its previous position and orientation are predicted by integrating over time the 3D acceleration measured by the accelerometer sensing unit and the 3D angular velocity measured by the gyroscope sensing unit, which are attached to the target object.

[0009] ■ The predicted position and orientation are corrected at least once by using the first position of the target object measured by a surveying instrument, and

[0010] ■The position and orientation of the target object are further corrected over time by using the change in position and orientation of the target object relative to its previous position and orientation as measured by the surveying instrument over time and by the change in position and orientation of the target object relative to its previous position and orientation as measured by the first sensor. The first sensor is attached to the target object.

[0011] According to another aspect of the invention, a computer program is provided, comprising instructions that, when executed by a computer system, cause the computer system to perform the method according to the invention.

[0012] The computer program may be stored on the computer system or on a computer-readable medium communicatively connected to the computer system. The term "computer-readable medium" includes, but is not limited to, portable or non-portable storage devices, optical storage devices, and various other media capable of storing, containing, or carrying instructions and / or data.

[0013] According to another aspect of the present invention, an apparatus is provided, the apparatus comprising means for performing the method according to the present invention. The apparatus includes:

[0014] ■ A surveying instrument configured to emit a tracking light and measure the position of the tracked target object.

[0015] ■ A first sensor, attached to the target object and configured to measure changes in position and orientation of the target object relative to its previous position and orientation.

[0016] ■ An accelerometer sensing unit, which is attached to the target object and configured to measure 3D acceleration over time.

[0017] ■ A gyroscope sensing unit, which is attached to the target object and configured to measure 3D angular velocity over time, and

[0018] ■ Computer system. Attached Figure Description

[0019] The following description or explanation of various aspects of the invention is by way of example only, with reference to the schematic examples shown in the accompanying drawings. Identical elements are labeled with the same reference numerals in the drawings. The described embodiments are generally not shown to scale and should not be construed as limiting the invention. Specifically,

[0020] Figure 1 The image shows an operator using a surveying instrument deployed at the work site. The instrument includes a measuring unit mounted on a tripod and a remote control connected to the measuring unit via a communication link.

[0021] Figure 2A , Figure 2B It shows in Figure 1 An exemplary version of the measuring unit used in the surveying instrument ( Figure 2A ) and block diagram of the main components of the surveying instrument ( Figure 2B ),

[0022] Figure 3A , Figure 3B It shows in Figure 1 An exemplary version of the remote control used in the surveying instrument ( Figure 3A ) and such Figure 3A The diagram shows the block diagram of the main components of the remote control. Figure 3B ),as well as

[0023] Figure 4 An exemplary version of the method for tracking the position and orientation of a target object according to the present invention is illustrated in flowchart form. Detailed Implementation

[0024] Reference will now be made in detail to this preferred embodiment, examples of which are illustrated in the accompanying drawings. It should be understood that the technology disclosed herein is not intended to limit its application to the construction details and component arrangements set forth in the following description or shown in the drawings. The technology disclosed herein can have other embodiments and can be practiced or implemented in various ways.

[0025] Furthermore, it should be understood that the wording and terminology used herein are for descriptive purposes and should not be considered restrictive. As defined and used herein, all definitions should be understood to take precedence over dictionary definitions, definitions in incorporated documents by reference, and / or general meanings of the terms defined.

[0026] The use of “comprising,” “including,” or “having,” and its variations herein is intended to cover the items listed thereafter and their equivalents, as well as additional items. Unless otherwise limited, the terms “connection,” “linkage,” and “installation,” and their variations thereof, are used extensively herein and cover direct and indirect connections, links, and installations. Furthermore, the terms “connection” and “linkage,” and their variations thereof, are not limited to physical or mechanical connections or links.

[0027] Figure 1 The diagram illustrates an operator using a surveying instrument 10 deployed at a work site 11. The work site 11 can be a construction site (indoor or outdoor) or a surveying site, etc. In addition to the surveying instrument 10, a target object 12, located away from the surveying instrument 10, is deployed at the work site 11. The target object 12 may include a reflector 13, such as a prism and / or a peephole, mounted on top of a pole 14.

[0028] The lever 14 includes a body having a pointer tip for contacting a measurement point and a reflector 13 for enabling coordinate determination of a reference position. The reflector 13 is positioned on the body of the lever 14 in a defined spatial relationship relative to the pointer tip. The lever further includes an inertial measurement unit (IMU) positioned on the body in a defined spatial relationship relative to the reflector, and includes IMU sensors including an accelerometer sensing unit, a gyroscope sensing unit, and a first sensor. The accelerometer sensing unit is attached to the target object 12 and configured to measure 3D acceleration over time, the gyroscope sensing unit is attached to the target object 12 and configured to measure 3D angular velocity over time, and the first sensor is attached to the target object 12 and configured to measure changes in position and orientation of the target object 12 relative to its previous position and orientation.

[0029] The surveying instrument 10 is configured as a total station and includes a measuring unit 15 mounted on a support structure in the form of a tripod 16. The surveying instrument 10 also includes a removable control panel in the form of a remote controller 17, which can be used to remotely control the measuring unit 15 via a wireless connection 18.

[0030] Figure 2A , Figure 2B Shown in 3D Figure 1 An exemplary version of the measuring unit 15 used in the surveying instrument 10 ( Figure 2A ) and such Figure 2A The block diagram of the main components of the measuring unit 15 shown in the figure ( Figure 2B ).

[0031] The surveying instrument 10 is designed as a robotic total station, and the measuring unit 15 includes a base 21, a support 22, and a measuring head 23. The measuring head 23 is enclosed by a housing 24, which includes an exit window 25. Within the housing 24, a distance measuring device capable of emitting a distance measuring beam and a tracking device capable of emitting radiation are arranged. The distance measuring beam and radiation are emitted out of the housing 24 through the exit window 25.

[0032] exist Figure 2AIn an exemplary version, the support 22 is U-shaped and includes a bottom portion 27, a first side portion 28, and a second side portion 29. The support 22 can rotate completely around its circumference about a first axis of rotation 31 at a full 360° angle relative to the base 21. The measuring head 23 is pivotally mounted to the support 22 about a second axis of rotation 32 and is arranged between the first side portion 28 and the second side portion 29 of the support. Typically, the first axis of rotation 31 is aligned parallel to the local gravity direction 33, and the second axis of rotation 32 is aligned perpendicular to the local gravity direction 33.

[0033] An azimuth motor and a first angle encoder may be located in the bottom portion 27 of the support 22, allowing the measuring unit 15 to rotate about a first rotation axis 31 and determine the direction of the distance measuring beam in a first plane perpendicular to the first rotation axis 31. An elevation motor and a second angle encoder may be located in the first side portion 28 of the support 22, allowing the measuring head 23 to pivot about a second rotation axis 32 and determine the direction of the distance measuring beam in a second plane perpendicular to the second rotation axis 32. To make the instrument 12 fully automatic, a self-leveling device may be included, which may be arranged in the bottom portion 27 of the support 22.

[0034] Figure 2B A block diagram of the main components of the measurement unit 15 of the surveying instrument 10 is shown. The measurement unit 15 includes a first electronic device 41, a distance measuring device 42, a first angle encoder 43, an azimuth motor device 44, a second angle encoder 45, and an elevation motor device 46. Optionally, the measurement unit 15 may additionally include one or more cameras, such as a tracking camera configured to track a target object, or a targeting camera configured to allow the user to aim at a point of interest.

[0035] In order to automatically track target objects using a total station 10, for example Figure 1 The target object 12 in the total station 10 may include a tracking camera and a tracking light source that emits a tracking beam. The tracking camera enables the measurement of static reflective targets and the tracking of moving reflective targets. The tracking camera provides information about the target's center of gravity, and this information is used to manipulate the drive system to guide the distance measuring beam to the center of the target and measure the target's 3D coordinates. To track reflective targets under all ambient lighting conditions, even in darkness, the tracking light source is used to generate the necessary illumination for the tracking camera. The tracking camera should have a filter suitable for the wavelength of the tracking light source.

[0036] The first electronic device 41 includes a first processing circuit (µP) 50, a first memory circuit 51 (which may include associated random access memory (RAM) and read-only memory (ROM)), a first communication circuit 52, and a first input / output (I / O) interface circuit 53. The first processing circuit 50 (also referred to as a device control unit) can communicate with the first memory circuit 51 and the first communication circuit 52 and is configured to control the laser instrument 12. The first communication circuit 52 includes a first transmitter circuit 54 and a first receiver circuit 55 and is configured to be connected to the communication circuitry of a remote controller via a communication link. The first input / output interface circuit 53 is the interface between the first processing circuit 50 and various types of motor driver circuits and sensor circuits of the measurement unit 15.

[0037] The distance measuring device 42 includes a laser emitter 56, a laser driver circuit 57, a photoelectric sensor 58, and a laser receiver interface circuit 59. The laser driver circuit 57 provides current to the laser emitter 56, which emits the distance measuring beam. The photoelectric sensor 58 receives at least a portion of the distance measuring beam reflected from a target or surface at the work site, and the current signal output by the photoelectric sensor 58 is directed to the laser receiver interface circuit 59. After appropriate amplification and demodulation, the signal is sent to the first processing circuit 50 via the first input / output interface circuit 53.

[0038] The first angle encoder 43 provides an input signal to the first processing circuit 50, enabling the first processing circuit to know precisely the horizontal angle at which the laser emitter 56 is positioned in the horizontal plane; the output signal of the first angle encoder 43 is directed to the first input / output interface circuit 53. The azimuth motor device 44 includes an azimuth motor 62 and an azimuth motor driver circuit 63. The azimuth motor is the driving force for rotating the main housing 22 of the measuring unit 15 about the first rotation axis 31, and the azimuth motor driver circuit provides appropriate current and voltage to drive the azimuth motor 62.

[0039] The second angle encoder 45 provides an input signal to the first processing circuit 50, enabling the first processing circuit to know precisely the second angle at which the laser emitter 56 is arranged in the second plane; the output signal of the second angle encoder 45 is directed to the first input / output interface circuit 53. The elevation motor device 46 includes an elevation motor 65 and an elevation motor driver circuit 66, which is the driving force for pivoting the measuring head 23 about the second rotation axis 32, and the elevation motor driver circuit provides appropriate current and voltage to drive the elevation motor 65.

[0040] Figure 3A , Figure 3B Shown in front view Figure 1 An exemplary version of the remote control 17 used in the surveying instrument 10 ( Figure 3A ) and such Figure 3A The block diagram of the main components of the remote controller 17 shown in the figure ( Figure 3B ).

[0041] The remote control 17 is designed as a tablet computer and includes a housing 81, a touch screen display 82, a battery 83, a set of buttons 84 (e.g., volume control buttons, power on / off buttons, and display control buttons), a set of indicators 85 (e.g., indicators for operating status, data storage status, and battery status), a set of connectors 86 (e.g., connectors for docking, data storage, and USB), and a card slot 87.

[0042] Figure 3B A block diagram of the main components of the remote controller 17 is shown. The remote controller 17 may include a second electronic device 91, a display device 92, and an input device 93.

[0043] The second electronic device 91 includes a second processing circuit (µP) 94, a second memory circuit 95 (which may include associated random access memory (RAM), read-only memory (ROM), and some type of bulk memory (BULK)), a second communication circuit 96, and a second input / output (I / O) interface circuit 97. The second processing circuit 94 can communicate with the second memory circuit 95 and the second communication circuit 96 and is configured to control the remote controller 17. The second communication circuit 96 includes a second transmitter circuit 98 and a second receiver circuit 99 and is configured to be connected to the first communication circuit 52 of the measurement unit 15 via a wireless connection 18. The second input / output interface circuit 97 is the interface between the second processing circuit 94 and various driver circuits of the remote controller 17.

[0044] The second memory circuit 95 can store a plurality of program codes having computer-executable instructions for performing methods. The stored program code may include program code for performing methods for tracking the position and orientation of a target object.

[0045] The method for tracking the position and orientation of a target object is performed by a computer system having evaluation, data processing, and / or control functions. In an exemplary version of the mapping instrument 10, the computer system is integrated into the second processing circuitry 94 of the second electronic device 91 of the remote controller 17. Alternatively, the computer system may be integrated into the first processing circuitry 50, or into both the first and second processing circuitries 50 and 94, or into any other suitable type of processing circuitry.

[0046] Display device 92 includes a display 101 and a display driver circuit 102. The display driver circuit communicates with a second I / O interface circuit 97 and provides the correct interface and data signals to the display 101. For example, if the remote control 17 is a laptop computer, this would be the standard display seen in most laptop computers. Alternatively, if the remote control 17 is a tablet computer or smartphone, in which case the display device is a much smaller physical device, the display device 101 could be a touchscreen display.

[0047] The user-operated input device 93 includes a keyboard 103 and a keyboard driver circuit 104. The keyboard driver circuit communicates with the second I / O interface circuit 97 and controls the signals interfaced with the keyboard 103. If the display device 101 is a touchscreen display, the remote control 17 may not have a separate keyboard, as most commands or data for input functions will be available through the touchscreen display itself, and the keyboard is integrated into the touchscreen display. There may be some type of power on / off button, but this is not necessarily considered a true keyboard and is generally not used for data input.

[0048] Figure 4 An exemplary version of the method for tracking the position and orientation of a target object according to the present invention is illustrated in flowchart form. The method is executed by the computer system and includes the following steps:

[0049] ■ The position and orientation of the target object 12 relative to the previous position and orientation are predicted by integrating the 3D acceleration measured by the accelerometer sensing unit and the 3D angular velocity measured by the gyroscope sensing unit over time. The accelerometer sensing unit and the gyroscope sensing unit are attached to the target object 12 (step S10).

[0050] ■ The predicted position and orientation are corrected at least once by using the first position of the target object 12 measured by the surveying instrument 10 (step S20), and

[0051] ■ Using the change in position and orientation of the target object 12 measured over time by the surveying instrument 10 and the change in position and orientation of the target object relative to the previous position and orientation of the target object measured by the first sensor, the corrected position and orientation are further corrected over time, and the first sensor is attached to the target object 12 (step S30).

[0052] The device according to the invention includes a target object (e.g., a prism) to be tracked in space, a total station (TS) for measuring the position of the prism, and an IMU (3D accelerometer and 3D gyroscope sensor) attached to the prism. In a preferred embodiment, another sensor system (e.g., a camera sensor with a super fisheye lens) may be attached to the prism. This sensor system may be able to measure the angle of incidence of light emitted by the total station toward the prism (e.g., a standard infrared flash used for prism tracking). In another preferred embodiment, this additional sensor system may be configured to perform visual odometry using natural features from the work site.

[0053] 3D gyroscopes and 3D accelerometers measure the angular velocity and acceleration of the target object to which they are attached, respectively; these measurements are given in a so-called (rotating) sensor coordinate system. To track position and orientation, the accelerometer and gyroscope signals are typically used to perform what is known as "strapdown integration," or inertial dead reckoning. Strapdown integration is a well-known method in other fields, such as navigation and guidance applications. Dead reckoning allows the IMU signal to be integrated in a non-rotating reference frame (outside the IMU) and a new position and orientation to be calculated from the previous one (in the external reference frame). However, to calculate the actual position, the gravity vector (measured by the accelerometer) must first be removed so that only the so-called free acceleration (by rotating the acceleration component to a local (gravity-referenced) reference frame and subtracting its gravity component) is integrated to obtain the actual velocity and position.

[0054] It is worth noting that even when gravity is completely subtracted, dead reckoning alone will result in drift that increases over time due to the high susceptibility of integration to sensor noise and other non-ideals. Therefore, dead reckoning alone will only provide accurate estimates for short time periods.

[0055] When drift-free orientation and position tracking are of interest, dead reckoning of IMU signals is therefore combined with auxiliary measurement updates using so-called sensor fusion methods. Sensor fusion is typically performed within a statistical estimation framework (e.g., filtering techniques such as the well-known Kalman filter), in which states of interest (e.g., the position and orientation of the tracked target object) are not directly measured; instead, they are defined as states and estimated via statistical methods using various (noisy) measurements that provide their statistical characteristics.

[0056] For example, by initializing the position and orientation states to arbitrary values ​​with significant uncertainty, it is possible to begin performing IMU strapdown integration (also known as prediction in the sensor fusion framework) in an arbitrary but non-rotating coordinate system. This solution continuously predicts the position and orientation (with increasing uncertainty) by simply continuing the dead reckoning process. When a first position measurement from an auxiliary technology (such as a total station), with some measurement uncertainty associated with it, becomes available, this measurement is used to correct the predicted position using the actual measured position. This brings the estimated position (the tracked state) very close to the ground truth (and correspondingly reduces the initial uncertainty).

[0057] Using dead reckoning predictions and measurement updates offers several advantages in performing sensor fusion compared to directly using measurements (e.g., estimating the tracked position versus directly using total station positions). These advantages include higher resolution: the position is available whenever IMU samples are available, and IMU sampling can reach thousands of Hz (compared to tens of Hz for total station measurements). Additionally, position estimation is available even when total station measurements are temporarily unavailable, such as when the line of sight is blocked for a short period. Furthermore, higher accuracy can be achieved compared to individual measurements because sensor fusion performs an averaging process across measurements. Additionally, since the prediction of the tracked position is always available (with associated uncertainty), measurements containing outliers can be detected and removed (e.g., in cases where the total station has locked onto an incorrect target). Another important benefit is that, given the complementary nature of the combined sensing data, it is often possible to statistically model and estimate additional states such as sensor errors (e.g., accelerometer / gyroscope bias or gain error), measurement update offsets (e.g., prism constants, etc.), axis misalignment, etc.

[0058] Furthermore, when using a so-called tightly coupled sensor fusion frame (i.e., a frame in which all different states and measurements are statistically correlated together), correcting the position state with position updates will additionally correct for the orientation components perpendicular to the horizontal plane (commonly referred to as tilt, or roll and pitch angles). This can be intuitively understood as follows: once the position is known, the orientation relative to gravity (i.e., tilt) will also be known, allowing gravity to be correctly subtracted and the measured accelerometer signal to be properly integrated to obtain the position; on the other hand, this position should (statistically) match the measured position. If the tilt is incorrect, there will be a large residual error when subtracting gravity before integrating the measured acceleration to obtain the position, resulting in a large position error via accelerometer integration (compared to the measured position provided by the total station).

[0059] It should be noted that the above considerations hold true independently of the movement of the prism (e.g., when the prism is stationary), but will only allow the tilt component of the orientation to be observed.

[0060] Using a similar argument as above, it can be seen that in cases where the prism experiences another external acceleration with a component along the horizontal plane (e.g., as typically during travel towards the loft point with the prism), such acceleration will make the orientation heading (also known as the yaw angle, i.e., the component of the orientation projected onto the horizontal plane relative to gravity) component known when the sensor fusion frame receives a position measurement update. For this reason, in these cases, the desired full 6-DOF (6DOF, 3D orientation + 3D position) pose of the prism will generally be known (allowing desired operations such as rod tilt compensation to be performed for the application). From this moment on, dead reckoning can continue in time by further integrating the accelerometer and gyroscope signals until further position measurement updates from the total station become available.

[0061] While many methods may exist for performing sensor fusion (e.g., Kalman filtering), in a preferred embodiment, the sensor fusion employed is based on convex nonlinear optimization. This method offers several advantages compared to so-called "Divided Difference Filtering," which is typically a filtering technique. The first advantage is the possibility of employing arbitrary noise distributions (rather than the usual Gaussian assumption) in the optimization problem of this application. For example, measurements from a total station can be modeled using, for instance, a Student-T distribution, which is a so-called heavy-tailed distribution. A key benefit of this method is the inherent robustness to outliers (inherently captured by the long-tailed distribution), reducing the need for complex tuning and outlier removal schemes. Another crucial benefit is the possibility of modeling fixed or slowly varying parameters as constants rather than as slowly varying time processes (such as Gaussian-Markov or random walk processes), leading to better estimation performance. Additionally, because the framework is based on nonlinear optimization, the same framework can be used for both real-time and offline applications. The latter may relate to estimating relevant parameters (e.g., axis misalignment) during the initial calibration run, and then using the same sensor fusion framework and formulas to treat these parameters as constants for real-time applications.

[0062] Another significant benefit is the possibility of formulating the optimization problem as a moving horizontal estimation, which allows for real-time or near-real-time estimation while using a temporal data window for the optimization problem. Compared to filtering techniques, moving horizontal estimation is known to offer significant benefits in improving the robustness of the estimation. It should be further noted that when formulating this problem as a moving horizontal estimation, the optimization problem can inherently be sparse, for which efficient algebraic implementations are available (e.g., suite sparse), thus reducing the implementation requirements on resource-constrained platforms.

[0063] Further observation revealed that precise synchronization and timing between different data points are crucial when employing a tightly coupled sensor fusion framework. It is conceivable that a delay of only 10 ms between measurements would result in a 1 cm inconsistency between different data points when the prism moves at 1 m / s (normal walking speed). This is particularly problematic in a tightly coupled sensor fusion framework because these errors are then attributed in a rather counterintuitive way to the different tracked states (e.g., tilt might be affected).

[0064] To offset the aforementioned limitations, in one embodiment, the different data are synchronized in time with an inaccuracy of less than 1 ms to reduce potential errors. Note that one benefit of sensor fusion methods is that the data do not need to arrive at the same time; however, it is important that they do share a common time reference frame (e.g., via timestamps), with errors as described above.

[0065] However, in different embodiments, time misalignment can also be modeled statistically and estimated within the sensor fusion framework. The preferred method based on nonlinear optimization in this application is highly beneficial for this purpose, especially when the timing error itself exhibits an unknown constant offset, as these offsets can be naturally modeled in the optimization method, as previously stated.

[0066] However, there may be some applications that require higher performance (e.g., in low-dynamic or quasi-static applications where heading may not be accurately observed using the previously disclosed embodiments).

[0067] To avoid the aforementioned limitation of requiring prism movement, a further embodiment proposes a system incorporating additional means for directly measuring the heading angle relative to the total station. As an example, it is proposed to additionally use a camera sensor combined with a wide-angle lens. In a specific embodiment, this could be a super fisheye lens (e.g., with a FOV of 270 degrees or similar). In a preferred embodiment, the camera could be sensitive to infrared light emitted by a flash.

[0068] Due to its large field of view, the angle of incidence of the incoming flash can typically be recorded on the camera sensor. Since the camera's tilt relative to gravity is known (as is the tilt of the prism, see the argument above), the recorded heading of the flash can usually be determined. This measurement can then be further used in a sensor fusion estimation framework to correct for the heading component of the orientation state. In this way, the heading will always be observable, even under completely static conditions (where an IMU plus total station alone cannot provide heading observability).

[0069] The proposed solution therefore allows for the consistent and robust tracking of both the position and orientation of the prism in the total station's coordinate system.

[0070] Importantly, it should be noted that position updates and heading updates do not need to be performed simultaneously, although these measurements need to be expressed in a common time reference frame. In fact, upon receiving a measurement update (e.g., a position update), the prism's position and orientation can be calculated over time using further dead reckoning (prediction) from the IMU signal. This can be done at frequencies up to approximately kHz, as the IMU can be sampled at such frequencies. Whenever available, additional position or heading measurements from the total station can be used to further correct the dead reckoning solution.

[0071] The points mentioned above highlight the key difference and benefits of the proposed solution, as it does not rely on explicit calculations of tilt using, for example, an accelerometer as an inclinometer. This is a crucial benefit because directly using an accelerometer as an inclinometer would require the accelerometer to be highly accurate and have very small sensor errors (e.g., bias), which would otherwise directly result in large tilt errors. Instead, this solution uses only the accelerometer to predict velocity and position over time, and then corrects these predictions through updates from the total station. Through these corrections, the method can accurately and seamlessly estimate bias as a state during operation. For example, this solution could be applied to consumer-grade IMUs, such as the Invensense 42688. This IMU has an initial accelerometer bias of 20 mg, which would result in approximately 1 degree of tilt error when the accelerometer is used directly as an inclinometer; this is an unacceptable error for this application.

[0072] Another crucial benefit is that the tilt angle can always be tracked in this method, under both static and dynamic conditions. This is essential for simplifying usability and streamlining customer workflows, as users can use a simple handheld stick without needing a tripod for stability.

[0073] Regarding the solution described above, it is worth noting that IMU dead reckoning is prone to error. Results will remain accurate over time intervals (e.g., the mm and subdegree levels required for tilt bar compensation) before further measurement updates are applied, and these intervals typically depend on the overall sensor performance. For example, the integration may remain accurate in position for several seconds before receiving further position updates from the total station, and in orientation for tens of seconds before receiving updates from the total station or camera angle of incidence. Therefore, a further benefit of the described system is that a continuous line of sight between the prism and the total station is not required. Instead, due to IMU prediction, the solution will provide accurate tracking and robustness to relocking after loss of sight. Additionally, position and heading updates can arrive at different times with different update rates.

[0074] To further enhance the robustness of the proposed solutions, in alternative embodiments, camera data can be used to additionally perform so-called visual odometry, i.e., by tracking feature movement between different recorded image frames to calculate changes in position and orientation relative to previous positions and orientations.

[0075] Visual odometry information can be further included in the aforementioned sensor fusion framework, for example, as incremental updates to position or orientation, to provide an exemplary formula. While this alone will not provide absolute orientation and position information (at least a total station position update is still required), visual odometry can be used to strongly limit drift in IMU predictions in cases where measurement updates from the total station are missing for extended periods. For example, with such an update, position accuracy may remain within millimeters for tens of seconds; and when such an update is additionally used, orientation accuracy may remain within a small fraction of a degree for about a minute. Note that when visual odometry is performed using a monocular camera, the positional variations are known except for the unknown scale factor. Due to the use of the IMU and position updates, this scale factor can be included as a state in the estimation problem and becomes readily observable via such an update.

[0076] Note that when the visual odometry is used in combination with the IMU, total station position updates, and total station incident angle updates, the system will be configured to provide maximum robustness, as described above. However, in alternative implementations, the system can use only the visual odometry, IMU, and total station position updates. As discussed above, heading will typically be observable only if the prism moves; however, the addition of the visual odometry will achieve significantly higher robustness (strong limitation on drift) compared to the case of only IMU and total station position measurements. Where desired, this can lead to viable alternative embodiments where potentially different implementations of the first sensor become possible (e.g., a camera with a narrower field of view not aimed at the PLT, or a camera sensor not tuned to sense infrared light from the total station).

[0077] The proposed sensor fusion framework allows for the use of additional measurements within the estimation framework, where available. For example, when stationary conditions are detected, updates using so-called zero velocity or zero rotation can be employed, thereby enabling further robustness improvements. This solution can be further integrated with other auxiliary technologies, such as (RTK)-GNSS, magnetometers, UWB-based radio positioning, lidar, and stereo camera systems, where measurements from such systems will be further incorporated into the sensor fusion framework.

[0078] Similarly, and as previously mentioned, the state of the sensor fusion framework can be extended to include (IMU) sensor bias, gain error, camera extrinsic or intrinsic parameters, timing and synchronization errors (to name just a few) to increase overall performance and robustness.

[0079] While the camera sensor and super fisheye configuration have been considered above as devices for measuring heading relative to a total station, other solutions can be considered. These include, to name just a few, multiple camera systems with narrower fields of view, linear arrays, or photodetector arrays.

Claims

1. A computer-implemented method for tracking the position and orientation of a target object (12) in a work site (11), wherein the target object (12) is deployed and a mapping instrument (10) configured to emit tracking light and measure the position of the tracked target object is deployed, the method being executed by a computer system and comprising: ■ The position and orientation of the target object (12) relative to its previous position and orientation are predicted by integrating the 3D acceleration measured by the accelerometer sensing unit and the 3D angular velocity measured by the gyroscope sensing unit over time. The accelerometer sensing unit and the gyroscope sensing unit are attached to the target object (12). ■ The predicted position and orientation are corrected at least once by using the first position of the target object (12) measured by the surveying instrument (10), and ■The corrected position and orientation are further corrected over time by using another position of the target object (12) measured over time by the surveying instrument (10) and the changes in position and orientation of the target object relative to the previous position and orientation of the target object measured by the first sensor, which is attached to the target object (12).

2. A computer program comprising instructions that, when executed by a computer system, cause the computer system to perform the method according to claim 1.

3. An apparatus comprising means for performing the method according to claim 1, the apparatus comprising: ■ Surveying instrument (10), which is configured to emit a tracking light and measure the position of the tracked target object (12). ■ A first sensor, which is attached to the target object (12) and configured to measure changes in position and orientation of the target object relative to its previous position and orientation. ■ An accelerometer sensing unit, which is attached to the target object (12) and configured to measure 3D acceleration over time. ■ A gyroscope sensing unit, which is attached to the target object (12) and configured to measure 3D angular velocity over time, and ■ Computer system.

Images