Multi-sensor navigation

By estimating the spatial relationship between a camera and LiDAR through trajectory-based data alignment, the method enhances navigation by effectively combining their data, addressing the challenge of integrating these sensors.

JP2024543753AInactive Publication Date: 2024-11-26COMMONWEALTH SCI & IND RES ORG
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
JP2023537302
Authority / Receiving Office
JP · JP
Patent Type
Applications
Current Assignee / Owner
Priority Date
2021-10-25
Filing Date
2022-10-21
Publication Date
2024-11-26
Estimated Expiration
Not applicable · inactive patent

AI Technical Summary

Technical Problem

The challenge lies in effectively combining data from cameras and LiDAR sensors for vehicle navigation due to the unknown relative spatial relationship between these sensors, which hinders the integration of their complementary information.

Method used

A method is provided to estimate the spatial relationship between a camera and LiDAR by determining their respective trajectories based on sensor data, allowing for the combination of data from both sensors to enhance navigation.

Benefits of technology

This approach enables accurate and robust navigation by aligning the data from cameras and LiDAR, overcoming the limitations of unknown spatial relationships and improving the integration of their complementary information.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure 2024543753000001_ABST
    Figure 2024543753000001_ABST
Patent Text Reader

Abstract

The present disclosure relates to a method performed by a processor for navigating a moving vehicle in an environment. The processor captures an environment using a first sensor mounted on the moving vehicle to generate first sensor data, and uses a second sensor mounted on the moving vehicle to generate second sensor data. The processor then determines a first trajectory of the first sensor and a second trajectory of the second sensor. The processor then estimates a spatial relationship between the first sensor and the second sensor based on the first trajectory and the second trajectory, and combines the first sensor data and the second sensor data into a combined multi-sensor representation of the environment using the estimated spatial relationship. Finally, the processor navigates the moving vehicle based on the combined multi-sensor representation of the environment.
Need to check novelty before this filing date? Find Prior Art

Description

[Technical field]

[0001] CROSS-REFERENCE TO RELATED APPLICATIONS This application claims priority from Australian Provisional Patent Application No. 2021 / 903403, filed on October 25, 2021, the contents of which are incorporated herein by reference in their entirety.

[0002] This disclosure relates to navigation using multiple sensors, such as, but not limited to, the use of cameras and LiDAR sensors for navigation. [Background technology]

[0003] Cameras and Light Detection and Ranging (LiDAR) sensors (or simply "LiDAR") can be mounted on moving vehicles to provide complementary information about the vehicle's environment. While cameras capture an essentially 2D pixel representation of the environment in the form of a color image, LiDARs capture a 3D point cloud. The 3D point cloud includes distance information for each sampling point at each vertex and azimuth angle. Thus, in essence, LiDARs provide a 3D representation of the surroundings in a polar coordinate system, which can be transformed to a Cartesian coordinate system. However, LiDARs typically do not provide color information, but only the locations of the sampling points.

[0004] For effective navigation of a vehicle, it is desirable to obtain the vehicle's position and orientation with respect to the environment from data generated by the camera and LiDAR, but the relative spatial relationship between the camera and LiDAR is often unknown, making it difficult to combine these two data sources.

[0005] Any discussion of documents, acts, materials, devices, articles or the like included in this specification is not an admission that any or all of such matters formed part of the substrate of the prior art or were common general knowledge in the art relevant to the present disclosure as they existed prior to the priority date of each of the appended claims.

[0006] Throughout this specification, the word "comprise" or variations such as "comprises" or "comprising" will be understood to imply the inclusion of a stated element, integer or step, or group of elements, integers or steps, but not the exclusion of any other element, integer or step, or group of elements, integers or steps. Summary of the Invention [Means for solving the problem]

[0007] The present disclosure provides a method for estimating a spatial relationship between a first sensor, such as a camera, and a second sensor, such as a LiDAR. This spatial relationship can be used to combine data from both sensors. The combined data can then be used for navigation or other applications. Importantly, the spatial relationship between the two sensors is based on the respective trajectories of the two sensors, which can be determined based on the respective sensor data.

[0008] A method for navigating a moving vehicle within an environment includes: Capturing an environment using a first sensor mounted on a moving vehicle to generate first sensor data; Capturing the environment using a second sensor mounted on the moving vehicle to generate second sensor data; determining a first trajectory of the first sensor over time using the first sensor data, the first trajectory including a plurality of positions relative to a first world coordinate system for the first sensor; determining a second trajectory of the second sensor over time using the second sensor data, the second trajectory including a plurality of positions of the second sensor relative to a second world coordinate system; estimating a spatial relationship between the first sensor and the second sensor based on the first trajectory and the second trajectory; combining the first sensor data and the second sensor data into a combined multi-sensor representation of the environment using the estimated spatial relationship; and navigating the moving vehicle based on the combined multi-sensor representation of the environment.

[0009] A method for estimating a spatial relationship between a first sensor and a second sensor includes: determining a first trajectory of a first sensor over time using sensor data indicative of the scene, the first sensor being captured over time by a first sensor moving relative to the scene; determining a second trajectory of a second sensor over time using sensor data indicative of the scene, the second sensor being captured over time by a second sensor moving relative to the scene; and calculating a spatial relationship between the first sensor and the second sensor based on the first trajectory and the second trajectory.

[0010] Since the calculation is based on both trajectories, the mapping of the sensor to the environment is implicit in the decision.

[0011] In some embodiments estimating the spatial relationship comprises: two or more of the plurality of locations of the first trajectory; two or more of the plurality of locations of the second trajectory; and The method includes optimizing an objective function including:

[0012] In some embodiments, the reference transformation transforms a first world coordinate system into a second coordinate system, and the objective function is independent of the reference transformation.

[0013] In some embodiments, the objective function is based on positions on the first and second trajectories that correspond in time.

[0014] In some embodiments, a first position of the first trajectory corresponds in time to a first position of the second trajectory, and a second position of the first trajectory corresponds in time to a second position of the second trajectory, and the objective function is: a result of the transformation from the first point of the first trajectory to the second point of the first trajectory; and represents the difference between the result of a transformation from a first point on a second trajectory to a second point on the second trajectory according to a spatial relationship.

[0015] In some embodiments, the objective function represents the difference multiple times for additional corresponding positions in time to improve accuracy in the presence of noise.

[0016] In some embodiments, the method further includes synchronizing the first sensor data with the second sensor data to determine corresponding positions in time.

[0017] In some embodiments, the synchronizing includes interpolating the first trajectory.

[0018] In some embodiments, the interpolation is based on a quaternion representation of the position of the first trajectory.

[0019] In some embodiments, optimizing the objective function includes optimizing the modified objective function subject to an equality constraint.

[0020] In some embodiments, the objective function is modified by replacing the object transformed by the spatial relationship with a single variable in the second transformation, and the equality constraint is a constraint on the single variable that makes the single variable equal to the object transformed by the spatial relationship.

[0021] In some embodiments, optimizing the objective function comprises: optimizing a first subproblem to update the estimate of the spatial relationship given a current value of a single variable; optimizing a second subproblem to update the estimate of a single variable given a current estimate of the spatial relationship.

[0022] In some embodiments, the method includes optimizing the objective function using an alternating direction multiplier method (ADMM).

[0023] In some embodiments, the first trajectory and the second trajectory each include a plurality of Euclidean transformations, each of the plurality of Euclidean transformations including a rotation and a translation, to represent one of the positions.

[0024] The software, when executed by a computer, causes the computer to perform any of the methods described above.

[0025] The system is a first sensor and a second sensor; A processor, the processor comprising: determining a first trajectory of a first sensor over time using sensor data indicative of the scene, the first sensor being captured over time by a first sensor moving relative to the scene; determining a second trajectory of the second sensor over time using sensor data indicative of the scene, the second sensor being captured over time by a second sensor moving relative to the scene; and A spatial relationship between the first sensor and the second sensor is configured to be calculated based on the first trajectory and the second trajectory.

[0026] The optional features disclosed above as optional features of the method are also optional features of the system.

[0027] Examples will now be described with reference to the following drawings: [Brief description of the drawings]

[0028] [Figure 1] 1 illustrates a system for navigating a moving vehicle. [Diagram 2] FIG. 1 is a simplified representation. [Diagram 3] SUMMARY OF THE DISCLOSURE A method for navigating a moving vehicle within an environment is presented. [Figure 4] A visual representation of the equivalence between various transformations. [Diagram 5] A computer system is presented for estimating a spatial relationship between a first sensor and a second sensor and then navigating a vehicle. [Figure 6] 1 illustrates a method for estimating a spatial relationship between a first sensor and a second sensor. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0029] 1 shows a system 100 for navigating a moving vehicle (not shown for simplicity). The system 100 comprises a LiDAR 101 and a camera 102. The LiDAR 101 captures a 3D point cloud 103 of an object 104 by emitting a laser beam and measuring the time of flight of the reflected light sensed at the LiDAR 101. This measurement is repeated for multiple different angles of the laser beam to create the point cloud 103.

[0030] A camera 102 captures an image 105 of another object 106 by capturing illuminating light reflected from the object 106. In this manner, the camera 102 captures multiple pixel values ​​in an image 105. These pixel values ​​may represent red / green / blue (RGB) channels, a single intensity channel, or other channels, such as a hyperspectral image.

[0031] The processor 107 receives data from the LiDAR 101 and the camera 102 and performs the methods disclosed herein to determine the spatial relationship between the LiDAR 101 and the camera 102.

[0032] As the vehicle moves, the LiDAR 101 moves along a LiDAR trajectory 110 and the camera 102 moves along a camera trajectory 120. The trajectories 110 and 120 are defined by respective sequences of positions.

[0033] FIG. 2 is a simplified representation of FIG. 1 and shows a first position 111 of the LiDAR trajectory 110 and a second position 112 of the LiDAR trajectory 110. Similarly, FIG. 2 shows a first position 121 of the camera trajectory 120 and a second position 122 of the camera trajectory 120. In this context, the terms "first" and "second" have a temporal meaning, such that the LiDAR 101 and the camera 102 are at the first position before reaching the second position. Here, the first position 111 of the LiDAR trajectory 110 and the first position 121 of the camera trajectory have the same timestamp. This means that the positions 111 and 121 were captured at the same time. Similarly, the second positions 112 and 122 have the same timestamp since they were captured at the same time. If two positions with the same timestamp are not found, one of the two trajectories can be interpolated to generate the missing position.

[0034] Each of the positions is relative to a respective world coordinate system, also referred to as a "global reference." More specifically, the LiDAR positions 111 and 112 are relative to the world LiDAR coordinate system 201, while the camera positions 121 and 122 are relative to the world camera coordinate system 202. Note that in some examples disclosed herein, the positions 111, 112, 121, 122 are referred to as Euclidean transformations or simply transformations. These transformations include translation (movement) and rotation (orientation). For example, for position 112, a translation is shown at 203, which essentially transforms the origin of the LiDAR world coordinate system 201 to position 112. There may also be a rotation that reflects the orientation (view direction) of the LiDAR at position 112. Similarly, transformation 204 transforms the camera world coordinate system 202 to position 122, possibly including a rotation to represent the camera's orientation (view direction).

[0035] The world coordinate systems 201 and 202 are arbitrary and are set at the beginning of the execution of the method, for example, the origin and orientation of each of the world coordinate systems may be set as the position and orientation of the LiDAR and camera when they are first turned on.

[0036] Trajectories 110 and 120 may be determined based on data generated by LiDAR 101 and camera 102, respectively. These trajectory determinations may be performed by providing reference objects in the environment that the respective sensors can easily recognize. The reference objects may have a known stationary position and orientation in the environment so that the processor 107 can calculate the trajectories based on the point cloud and images, respectively. It is noted that both sensors (e.g., LiDAR 101 and camera 102) may capture different reference objects to calculate the respective trajectories, which has the advantage of being able to select the most suitable reference for each sensor.

[0037] Since the LiDAR 101 and the camera 102 are mounted on a vehicle (or another moving platform), it can be assumed that there is a fixed spatial relationship between the LiDAR 101 and the camera 102. In other words, there exists a Euclidean transformation (shown at 205 in FIG. 2 and referred to as “p” below) that maps the first position 121 of the camera to the first position 111 of the LiDAR. This transformation 205 should remain constant as the LiDAR 101 and the camera 102 move along their respective trajectories 110 and 120. We now aim to calculate this transformation 205. In a basic sense, with a fixed transformation 205, it can be said that any change between the positions 111 and 112 should also be a change between the positions 121 and 122. However, in the presence of noise in the sensor data, the observed changes will generally not be equal. Thus, the processor 107 can optimize the objective function to minimize the difference between the two changes (which can again be expressed as the respective transformations).

[0038] For completeness, it should also be noted that there is a transformation, denoted at 206 and referred to below as “q”, that maps the LiDAR world coordinate system 201 to the camera world coordinate system 202.

[0039] 3 illustrates a method 300 for navigating a moving vehicle in an environment, performed by a processor 107. First, the processor 107 captures (301) the environment using a first sensor, such as a LiDAR 101 mounted on the moving vehicle, to create first sensor data, and captures (302) the environment using a second sensor, such as a camera 102 mounted on the moving vehicle, to create second sensor data. The processor 107 then uses the first sensor data to determine (303) a first trajectory 110 of the first sensor over time. As described above, the first trajectory 110 includes a plurality of positions 111 / 112 relative to the first world coordinate system 201 for the first sensor 101. The processor 107 further uses the second sensor data to determine (304) a second trajectory 120 of the second sensor over time. Also, as described above, the second trajectory 120 includes a number of positions relative to the second world coordinate system 202 for the second sensor.

[0040] Based on the first trajectory 110 and the second trajectory 120, the processor 107 estimates a spatial relationship 205 between the first sensor and the second sensor, and then combines the first sensor data and the second sensor data into a combined multi-sensor representation of the environment using the estimated spatial relationship. Finally, the processor 107 navigates the moving vehicle based on the combined multi-sensor representation of the environment.

[0041] The following discussion provides a more mathematical explanation of the method 300.

[0042] Background information The function f(x;p) represents the Euclidean transformation of a 3D point x according to parameters p. This applies to transformation 203 in the sense that position 111 can be seen as a transformation from world coordinate system 201. This also applies to positions 112, 121, 122, and transformations 205 and 206. This transformation is mathematically formulated as follows:

number

[0043] The parameter vector p contains six values ​​θ1, where p=[θ 1, θ 2, θ 3, t x, t y, t z ] T , θ2, and θ3 are parameters of the rotation matrix R(p), and t(p)=[t x ,t y ,t z ] T is a 3D transformation.

[0044] The transformation f(x;p) transforms any 3D shape X = [x1, x2, x3, ..., x P ] can be generalized to.

number

[0045] Orbit estimation As mentioned above with reference to the LiDAR trajectory 110 and the camera trajectory 120, the term trajectory refers to the movement of the sensor in 3D space.

number

number

[0046] For example, assume that a square 20mm x 20mm checkerboard pattern is placed in a fixed position in a room. The camera captures the image at a fixed time.

number

number

number

number

number

number

[0047] There is an ambiguity in Equation 3 regarding the definition of the world coordinate system of the sensor. For all Euclidean transformation parameters q, which indicates that the world coordinate system is arbitrary,

number

[0048] If p1=0, then p2 can be calculated by solving the following equation:

number

number

[0049] In fact, in Equation 6, the estimate of the 3D shape

number

[0050] It should be noted that using a calibration target such as a checkerboard pattern is one of many ways to estimate the trajectory of a sensor. Alternative methods such as odometry estimation or localization and mapping (SLAM) algorithms assume that the environment is fixed and identify correspondences between different observations of the same structure, for example assuming on the trajectory or observations that the sensor moves smoothly in 3D space. One example of a SLAM algorithm is the Wildcat algorithm by Data61 of the Commonwealth Scientific and Industrial Research Organization of Australia.

[0051] This disclosure provides a method to align two trajectories using estimates of the sensor trajectories under the assumption that the transformation between the sensors is always constant and the transformation between the world coordinate systems of each sensor trajectory is unknown.

[0052] Orbital alignment {c i} is the trajectory of the camera 120. As mentioned above, each c i is time

number

[0053] {d j} is the trajectory of the LiDAR110, where each d j represents the Euclidean transformation such as 111, 112, etc., which can convert the observation results obtained by the LiDAR into the position of the LiDAR in the world coordinate system.

[0054] From Equation 4, it can be seen that the camera and LiDAR world coordinate systems are related by an unknown Euclidean transformation q206.

[0055] The camera 102 is assumed to be rigidly connected to the LiDAR 101. A rigid relationship means that the position and orientation of the camera 102 with respect to the LiDAR 101 is constant. Mathematically, this is defined such that the 3D shape X observed by the LiDAR 101 is always observed by the camera 102 at a position f(X;p). The parameter p is also called the extrinsic parameter.

[0056] To derive the trajectory alignment algorithm, there are other assumptions, namely, the 3D shape X observed by the LiDAR 101 is rigidly attached to the LiDAR 101 and the camera 102, and the matrix X representing the 3D shape is rank 3.

[0057] With these assumptions it is possible to show the following relationships:

number

[0058] Therefore, in orbital alignment, any non-planar shape X and orbits {c i} and {d i} we need to solve for p and q.

[0059] Exclusion of q We can eliminate the transformation q 205 by considering how the 3D shape X in the local coordinate system at time i changes when transformed to the local coordinate system at time j. Below we show how this formula eliminates the Euclidean transformation q.

[0060] The proof requires the following notational conventions:

number

number

number

number

[0061] The proof starts with the noise-free case (Eq. 7).

number

number

number

number

[0062] For brevity,

number

number

number

[0063] From equation 16, it can be seen that the parameter q has been eliminated.

[0064] 4 shows the equivalence of Equation 16, which allows the calculation of p and later forms the objective function of the optimization problem. Equation 16 shows that on the left, at time i, a virtual object X is transformed (401) from the LiDAR position 111 to the LiDAR world coordinate system 201, and then transformed (402) back to the local LiDAR position at time j 112. On the right, the same object X is transformed (403) from the LiDAR viewpoint at time i 111 to the camera viewpoint at time i 121, and then transformed (404) to the camera world coordinate system 202. The virtual object X is then transformed (405) back to the camera viewpoint at time j 122, and finally transformed (406) back to the LiDAR viewpoint at time j 112.

[0065] In that sense, the spatial relationship is calculated using two positions c i , c j 111, 112, two positions d on the second orbit 120 i , d j It is clear that the mathematical expressions involving 121, 122, and the spatial relation p itself are used. Also, note that equation (16) is independent of the parameter q that transforms the LiDAR world coordinate system 201 to the camera world coordinate system 202.

[0066] Isotropic Gaussian model In most real-world applications, the sensor trajectory {c i} and {d j Due to noise in {\displaystyle \mathbb {R}\left(R)}, the equality does not hold in Equation 16. In this section, we use the error

number

[0067] This shows that the spatial relationship determination depends on four positions, including two positions on each of the two trajectories. Multiple transformations are performed between these four positions and the two world coordinate systems. These transformations essentially include two groups of transformations, both of which should ideally end up at the same endpoint. The first group of transformations includes the two LiDAR positions on the LiDAR trajectory and the LiDAR world coordinate system 201, and the second group of transformations includes the two camera positions on the camera trajectory and the camera world coordinate system 202. The second group also includes the spatial relationship between the LiDAR and the camera. Since points from both trajectories are included, the determination incorporates both trajectories as a basis for finding the spatial relationship between both sensors. The spatial relationship determination then aims to minimize this difference between the results of the two groups of transformations.

[0068] Furthermore, e k is an isotropic multivariate Gaussian distribution

number

[0069] Therefore, the joint probability of all correspondences, assuming that the correspondences are independent, is expressed as follows:

number

[0070] Therefore, the maximum likelihood solution for the external parameters p is

number

number

[0071] therefore,

number

number

[0072] Optimization using ADMM Introducing the dummy variable Y into equation 22, we obtain:

number

[0073] The ADMM takes the form of a decomposition-adjustment procedure that adjusts solutions of smaller local subproblems to find a solution to a larger global problem. The ADMM can be seen as an attempt to blend the advantages of dual decomposition and augmented Lagrangian methods for constrained optimization.

[0074] The scaled augmented Lagrangian L for the current problem is:

number

[0075] The next problem is a subproblem for variable p.

number

[0076] This problem has a closed-form solution, as shown in Olga Sorkine-Hornung and Michael Rabinovich. Least-squares rigid motion using SVD. https: / / igl.ethz.ch / projects / ARAP / svd_rot.pdf, 2017.

[0077] Partial question Y Below is a subproblem involving the variable Y.

number

number

number

number

[0078] Again, Y represents the variable Y for iteration k, and Y * represents the variable Y for iteration k+1. Therefore, the proposed algorithm updates p * Then use the updated value to update Y * Calculate p and use the updated value to update * Then, the solution is iteratively optimized until a termination criterion is met.

[0079] p * and Y * After is calculated, the scaled dual variables Z * is updated using the following formula:

number

[0080] The ADMM algorithm repeats the steps of equations (25) to (3) until convergence. The termination criterion for convergence is the primitive residual r * =vec(Y* -f(X;p * )) and double residuals

number

number

number

[0081] operator

number

[0082] λ i Selection λ i There are several strategies for choosing the values ​​of λ1=...λ N = 1. Unfortunately, this treats each correspondence equally and can lead to undesirable results if a small number of correspondences are incorrect.

[0083] To deal with outliers, an iterative strategy can be used, where we use the current estimates of the extrinsic parameters p to iteratively iterate over λ. i Calculate the new value of x. One such strategy is to use the following function:

number

number

[0084] The above expansion of the strategy is, after each iteration

Number

Number

Number

[0085] Synchronization The implicit assumption introduced above is that the LiDAR transformation d i at the estimated time

Number

Number

[0086] Below, c i is the i-th transformation,

Number

Number

number

number

number

[0087] LiDAR Timestamp

number

number

number

number

number

number

[0088] orbit

number

number

number

number

number

number

number

number

number

[0089] Time relationship with unknown parameters In practice, the function g that relates the LiDAR timestamp to the camera timestamp is a function of unknown parameters

number

number

[0090] In this case, the external parameter p is

number

number

number

[0091] Brute force search involves

number

number

number

[0092] After completion,

number

[0093] Computer Systems FIG. 5 shows a computer system 500 for estimating a spatial relationship between a first sensor and a second sensor and then navigating a vehicle. The computer system 500 comprises a processor 107, already characterized in FIG. 1, connected to a program memory 501, a data memory 502, a communication port 503, and a control output port 504. The program memory 501 is a non-transitory computer readable medium such as a hard drive, a solid state disk, or a CD-ROM. The software, i.e., the executable program stored in the program memory 501, causes the processor 107 to execute the method of FIG. 3 and FIG. 4, i.e., to calculate the spatial relationship between the two sensors based on their respective trajectories. The term "calculating the spatial relationship" refers to calculating a value indicative of the spatial relationship. This also applies to related terms.

[0094] The processor 107 may store the sensor data, trajectories, and spatial relationships in a data store 502, such as a RAM or processor registers. The processor 107 may also transmit the determined spatial relationships via a communication port 503 to a server or other computer, such as a central control room.

[0095] The processor 107 may receive data, such as LiDAR and camera data, from the data memory 502 and from the communication port 503. In one example, the processor 107 receives sensor data from the LiDAR 101 and the camera 102 via the communication port 503, such as by using a Wi-Fi network according to IEEE 802.11. The Wi-Fi network may be a distributed ad-hoc network such that no dedicated management infrastructure, such as a router, is required, or a centralized network with a router or access point that manages the network.

[0096] In one example, the processor 107 receives and processes the sensor data in real time, which means that the processor 107 calculates or updates the spatial relationship every time it receives sensor data from the LiDAR 101 and the camera 102, and completes this calculation before the LiDAR 101 and the camera 102 send the next sensor data update.

[0097] Although communication port 503 and control port 504 are shown as separate entities, it should be understood that any kind of data port may be used to receive data, such as a network connection, a memory interface, pins on the chip package of processor 107, or an IP socket, or logical ports such as parameters of functions stored in program memory 104 and executed by processor 107. These parameters may be stored on data memory 502 and may be treated in source code by value or by reference, i.e., as pointers.

[0098] Processor 107 may receive data through all these interfaces, including memory access of volatile memory, such as cache or RAM, or non-volatile memory, such as optical disk drives, hard disk drives, storage servers, or cloud storage. Computer system 500 may further be implemented in a cloud computing environment, such as a managed group of interconnected servers hosting a dynamic number of virtual machines.

[0099] It should be understood that any receiving step is preceded by determining or calculating the data to be subsequently received by the processor 107. For example, the processor 107 determines a point cloud and stores the point cloud in a data memory 502, such as a RAM or a processor register. The processor 107 then requests the data from the data memory 502, such as by providing a read signal along with a memory address. The data memory 502 provides the data as voltage signals on physical bit lines, and the processor 107 receives the point cloud via a memory interface.

[0100] 3 and 6 should be understood as blueprints of a software program, which may be implemented step by step, with each step in FIG. 3 and 6 being represented by a function in a programming language such as C++ or Java. The resulting source code is then compiled and stored as computer executable instructions on the program memory 501.

[0101] Method for determining spatial relationships 6 shows a method 600 for estimating a spatial relationship p403 between a first sensor and a second sensor. Typically, the processor 107 executes the method 600 in the sense that it determines (601) a first trajectory of a first sensor, such as a LiDAR 101, over time using sensor data indicative of a scene, the first sensor being captured over time by a first sensor moving relative to the scene. The processor 107 also determines (602) a second trajectory of a second sensor, such as a camera 102, over time using sensor data indicative of a scene, the second sensor being captured over time by a second sensor moving relative to the scene. Finally, the processor 107 calculates (603) a spatial relationship between the first sensor and the second sensor based on the first trajectory and the second trajectory.

[0102] Since the calculation is based on both trajectories, the decision implicitly incorporates the mapping of the sensor to the environment. As a result, the calculation is accurate and robust to sensor noise. It is noted that the more specific features discussed above with reference to method 300 of FIG. 3, and in particular to equations (20) and (23), and optimization using the ADMM, also apply to method 600 of FIG. 6.

[0103] Those skilled in the art will appreciate that numerous variations and / or modifications may be made to the above-described embodiments without departing from the broad general scope of the present disclosure, and the present embodiments are therefore to be considered in all respects as illustrative and not restrictive. [Explanation of symbols]

[0104] 100 Systems 101 First Sensor 102 Camera 103 point cloud 104 Object 105 images 106 Object 107 Processors 110 First Orbit 111 and 112 LiDAR locations 120 Second Orbit 121 and 122 Camera positions 201 and 202 World Coordinate System 203 Conversion 204 Conversion 205 Conversion 206 Conversion 500 Computer Systems 501 Program Memory 502 Datastore 503 communication port 504 control port

Claims

1. 1. A method for navigating a moving vehicle within an environment, comprising: capturing the environment using a first sensor mounted on the moving vehicle to generate first sensor data; capturing the environment using a second sensor mounted on the moving vehicle to generate second sensor data; determining a first trajectory of the first sensor over time using the first sensor data, the first trajectory including a plurality of positions relative to a first world coordinate system for the first sensor; determining a second trajectory of the second sensor over time using the second sensor data, the second trajectory including a plurality of positions relative to a second world coordinate system for the second sensor; estimating a spatial relationship between the first sensor and the second sensor based on the first trajectory and the second trajectory; combining the first sensor data and the second sensor data into a combined multi-sensor representation of the environment using the estimated spatial relationship; and navigating the moving vehicle based on the combined multi-sensor representation of the environment.

2. estimating the spatial relationship two or more of the plurality of locations of the first trajectory; and two or more of the plurality of locations of the second trajectory; and The method of claim 1 , further comprising optimizing an objective function comprising:

3. a reference transformation transforms the first world coordinate system into the second coordinate system; The method of claim 2 , wherein the objective function is independent of the reference transformation.

4. The method of claim 2 or 3, wherein the objective function is based on positions on the first and second trajectories that correspond in time.

5. a first position of the first trajectory corresponds in time to a first position of the second trajectory; a second position of the first trajectory corresponds in time to a second position of the second trajectory; The objective function is a result of a transformation from a first point of the first trajectory to a second point of the first trajectory; 5. The method of claim 4, further comprising expressing a difference between a result of a transformation from the first point on the second trajectory to the second point on the second trajectory according to the spatial relationship.

6. The method of any one of claims 2 to 5, wherein the objective function represents the difference multiple times for further corresponding positions in time to improve accuracy under noise.

7. The method of any one of claims 2 to 6, wherein optimizing the objective function comprises optimizing a modified objective function subject to an equality constraint.

8. 8. The method of claim 7, wherein the objective function is modified in the second transformation by replacing an object transformed by the spatial relationship with a single variable, and the equality constraint is a constraint on the single variable that makes the single variable equal to the object transformed by the spatial relationship.

9. optimizing the objective function, optimizing a first subproblem to update the estimate of the spatial relationship given a current value of the single variable; optimizing a second subproblem to update the estimate of the single variable given the current estimate of the spatial relationship.

10. The method of any one of claims 2 to 9, wherein the method comprises optimizing the objective function using the Alternating Directional Method of Multipliers (ADMM).

11. The method of any one of claims 1 to 10, further comprising synchronising the first sensor data with the second sensor data to determine corresponding positions in time.

12. The method of claim 11 , wherein synchronizing comprises interpolating the first trajectory.

13. The method of claim 12 , wherein the interpolation is based on a quaternion representation of the positions of the first trajectory.

14. 14. The method of claim 1, wherein the first trajectory and the second trajectory each include a plurality of Euclidean transformations, each of the plurality of Euclidean transformations including a rotation and a translation, to represent one of the positions.

15. 1. A method for estimating a spatial relationship between a first sensor and a second sensor, comprising: determining a first trajectory of the first sensor over time using sensor data indicative of a scene, the first sensor being captured over time by the first sensor moving relative to the scene; determining a second trajectory of the second sensor over time using sensor data indicative of a scene, the second sensor being captured over time by the second sensor moving relative to the scene; and calculating the spatial relationship between the first sensor and the second sensor based on the first trajectory and the second trajectory.

16. estimating the spatial relationship two or more of the plurality of locations of the first trajectory; and two or more of the plurality of locations of the second trajectory; and The method of claim 15, further comprising optimizing an objective function comprising:

17. a reference transformation transforms the first world coordinate system into the second coordinate system; The method of claim 16 , wherein the objective function is independent of the reference transformation.

18. 18. The method of claim 16 or 17, wherein the objective function is based on positions on the first and second trajectories that correspond in time.

19. a first position of the first trajectory corresponds in time to a first position of the second trajectory; a second position of the first trajectory corresponds in time to a second position of the second trajectory; The objective function is a result of a transformation from a first point of the first trajectory to a second point of the first trajectory; 20. The method of claim 18, further comprising expressing a difference between a result of a transformation from the first point on the second trajectory to the second point on the second trajectory according to the spatial relationship.

20. A method according to any one of claims 16 to 19, wherein the objective function represents the difference multiple times for further corresponding positions in time to improve accuracy under noise.

21. A method according to any one of claims 16 to 20, wherein optimizing the objective function comprises optimizing a modified objective function subject to an equality constraint.

22. 22. The method of claim 21, wherein the objective function is modified in the second transformation by replacing an object transformed by the spatial relationship with a single variable, and the equality constraint is a constraint on the single variable that makes the single variable equal to the object transformed by the spatial relationship.

23. optimizing the objective function, optimizing a first subproblem to update the estimate of the spatial relationship given a current value of the single variable; optimizing a second subproblem to update the estimate of the single variable given the current estimate of the spatial relationship.

24. A method according to any one of claims 16 to 23, wherein the method comprises optimising the objective function using the Alternating Directional Method of Multipliers (ADMM).

25. The method of any one of claims 15 to 24, further comprising synchronizing the first sensor data with the second sensor data to determine corresponding positions in time.

26. 26. The method of claim 25, wherein synchronizing comprises interpolating the first trajectory.

27. 27. The method of claim 26, wherein the interpolation is based on a quaternion representation of the positions of the first trajectory.

28. 28. The method of any one of claims 15 to 27, wherein the first trajectory and the second trajectory each include a plurality of Euclidean transformations, each of the plurality of Euclidean transformations including a rotation and a translation, to represent one of the positions.

29. Software which, when executed by a computer, causes the computer to carry out the method of any one of claims 1 to 28.

30. 1. A system comprising: a first sensor and a second sensor; a processor, the processor comprising: determining a first trajectory of the first sensor over time using sensor data indicative of a scene, the first sensor moving relative to the scene and captured over time by the first sensor; determining a second trajectory of the second sensor over time using sensor data indicative of a scene, the second sensor moving relative to the scene, the second trajectory being captured over time by the second sensor; and The system is configured to calculate a spatial relationship between the first sensor and the second sensor based on the first trajectory and the second trajectory.