Method for determining the motion state of a rigid body

Abstract

The invention relates to a method for determining a motion state of a rigid body (2) relative to an environment of objects (4) by means of a plurality of measurement data sets for the objects (4) in the environment of the rigid body (2), wherein each measurement data set comprises a measurement time point and a Doppler velocity (d) and an azimuth angle (θ) relative to a respective sensor reference system (S), the method comprising: determining the motion state of the rigid body relative to the environment as a velocity vector (v x , v y ) and an angular velocity vector (ω z ) in a body reference system (B), wherein each sensor reference system can be transformed into the body reference system by a non-singular transformation (R, t), wherein at least one condition set is formed, the at least one condition set comprising a plurality of measurement data sets, wherein a function depending on a Doppler velocity deviation between an estimated Doppler velocity and the Doppler velocity of a measurement data set comprised in the at least one condition set is minimized for the at least one condition set in a regression analysis, wherein the estimated Doppler velocity is represented as a dependent variable in the regression analysis.

Description

Technical Field

[0001] The present invention relates to a method for determining the motion state of a rigid body relative to its environment by means of a large set of measurement data, a method for determining relative position and / or relative orientation, and a computing unit and a computer program for performing these methods. Background Technology

[0002] Odometry addresses the problem of determining the position and orientation of a moving body (e.g., a vehicle and / or a robot). To this end, the motion state of the moving body can be determined, such as velocity and / or angular velocity, where continuous temporal differences in position or orientation correspond to the velocity and / or angular velocity. To determine the motion state, inertial measurement units, i.e., accelerometers and gyroscopes, can be used, which determine the motion state or its components directly through measurement. Cameras mounted on the moving body can also be used; the images or video recordings from these cameras enable the evaluation of the motion state using suitable image processing software. Summary of the Invention

[0003] According to the present invention, a method for determining the motion state of a rigid body relative to its environment by means of a large set of measurement data, a method for determining relative position and / or relative orientation, and a computing unit and a computer program for performing these methods are proposed, having the features of the independent claims. Advantageous designs are the subject matter of the dependent claims and the description thereof below.

[0004] This invention employs a method of determining a set of conditions from a set of measurement data and then using regression analysis to determine the motion state based on these conditions. By forming a set of conditions, the measurement data set can be targeted, thereby achieving high accuracy in determining the motion state.

[0005] Specifically, the method uses a large set of measurement data for an object in a rigid body environment, where each set of measurement data includes a measurement time point, Doppler velocity, and azimuth angle relative to a (three-dimensional) sensor reference frame. The measurement data sets can be detected by one or more sensors, each having a sensor reference frame, and the measurement results included in the measurement data sets are related to that sensor reference frame. Each measurement data set is related to an object detected by the sensor; that is, each measurement data set for an object includes a measurement time point, Doppler velocity, and azimuth angle relative to the sensor reference frame of the sensor detecting the object. The measurement data sets can be provided in a suitable manner. The method preferably includes a corresponding providing step.

[0006] Generally, the present invention can utilize a set of measurement data from sensors that determine the Doppler velocity of a defined object (i.e., the radial velocity toward or away from the sensor). Therefore, these sensors can also be referred to as Doppler sensors. The sensors are preferably radar sensors and / or lidar sensors. Specifically, the use of FMCW radar sensors and / or FMCW lidar sensors is specified (Radar: "Radio Detection and Ranging"; LiDAR: "Light Detection and Ranging" or "Light Imaging, Detection and Ranging"; FMCW: "Frequency Modulated Continuous Wave"). Correspondingly, the set of measurement data is preferably a radar measurement data set and / or a lidar measurement data set.

[0007] Preferably, one or more (especially all) sets of measurement data include an elevation angle (relative to the sensor reference frame of the respective sensor). By taking the elevation angle into account, the accuracy of determining the motion state can be significantly improved. More preferably, it is specified that at least some of the one or more (especially all) sets of measurement data are determined from measurements that only include the measurement time point, Doppler velocity, and azimuth angle, wherein the elevation angle is set to be equal to a predetermined value, particularly equal to zero. Older radar or lidar sensors typically do not measure or are uncertain about the elevation angle. The corresponding measurements therefore initially only include Doppler velocity and azimuth angle. In order to be able to use measurements with and without an elevation angle, it is assumed that the elevation angle has a predetermined value, preferably zero (this predetermined value is sensor-dependent, i.e., the predetermined value is the same for different measurements from the same sensor). By setting the elevation angle to zero, or more generally to a predetermined value, uniform processing can be achieved. For simplicity, the following description will only address the case where the sensor detects the elevation angle. If the sensor does not do this, then as mentioned above, the elevation angle should be equal to the predetermined value or equal to zero, without needing to be mentioned separately. Of course, the set of measurement data obtained from measurements where the elevation angle is set to a predetermined value can be used together with a set of measurement data that already includes the elevation angle from the beginning. Even when measuring the elevation angle, it can be set to a predetermined value, especially zero, to simplify the equations used in regression analysis and thus achieve shorter computation time, for example.

[0008] The method also includes determining the motion state of the rigid body relative to the environment as velocity and angular velocity vectors in a volume reference frame, wherein each sensor reference frame can be transformed (i.e. mapped) to a volume reference frame through a non-singular transformation.

[0009] The term "velocity vector" refers to linear velocity (as opposed to angular velocity) and is also referred to as linear velocity vector within the scope of this specification. Linear velocity vectors and angular velocity vectors are described in a three-dimensional volume reference frame. Since they are calculated based on (Doppler) measurements of objects in the environment, they describe the velocity relative to said object. In principle, these (stationary) objects again define the environmental reference frame, such that the linear velocity vector and angular velocity vector describe the state of motion relative to the environment or the environmental reference frame (represented in the volume reference frame).

[0010] Because the (3D) sensor reference frame and the (3D) volume reference frame can be connected or mapped to each other through corresponding nonsingular transformations (i.e., particularly reversible) for each sensor reference frame or each sensor, the relative orientation and position of the sensors are taken into account when determining the motion state. For example, if the three-dimensional measurement data is projected onto a plane, information will be lost and the accuracy of determining the motion state will be reduced. Both the sensor reference frame and the volume reference frame have three dimensions. Correspondingly, the velocity vector and angular velocity vector both have three dimensions or three components. Therefore, the motion state in three-dimensional space is determined by 3 + 3 = 6 components.

[0011] When determining the motion state, at least one set of conditions is formed, comprising multiple sets of measurement data. In a regression analysis, for at least one set of conditions, a function is minimized that depends on the deviation of the estimated Doppler velocity from the Doppler velocities of the Doppler velocities included in the at least one set of measurement data. The estimated Doppler velocity is included or represented as a dependent variable in the regression analysis (which depends on the azimuth angle of the corresponding measurement data set and, if necessary, the elevation angle of the corresponding measurement data set). This regression analysis determines at least one component of the velocity vector and / or angular velocity vector. The Doppler velocity representing the dependent variable is correlated with the velocity vector and angular velocity vector respectively through mappings that depend on the azimuth angle of the corresponding measurement data set and, if necessary, the elevation angle of the corresponding measurement data set, and on a corresponding transformation between the sensor reference frame and the body reference frame.

[0012] Within the scope of the regression analysis, the motion state of the rigid body is calculated, i.e., the velocity vector and angular velocity vector, or at least one of their components. The estimated Doppler velocity depends on the azimuth of the corresponding set of measurement data and, if necessary, the elevation angle, where this should not be interpreted as the azimuth and elevation angles having to be independent variables in the regression analysis. In particular, the estimated Doppler velocity in the regression method or regression included in the regression analysis can be represented by equations as functions of the azimuth and elevation angles of the corresponding set of measurement data and as functions of the velocities to be determined in the regression method or regression (i.e., the velocity vector and angular velocity vector or the sensor's velocity vector), see equation set (2), (5) below. If the regression method or regression is considered an optimization problem or a variation problem, the velocity to be determined represents the variable to be varied.

[0013] Preferably, all (six in total) components of the velocity vector and / or angular velocity vector are determined through regression analysis (i.e., representing the variables to be varied). However, it is also possible that one or more components are set in a restrictive sense (e.g., in a rail vehicle, lateral movement may be restricted) or pre-given by measurements independent of the sensors (e.g., by a rotational rate sensor). In this case, the corresponding components can be determined by the restrictive or independent measurements and cannot be used as variables to be varied in the regression analysis. However, it is also preferable to consider, in the regression analysis, the components with restrictive or independent measurements, including additional conditions corresponding to the restrictive or independent measurements. These additional conditions are assigned weights relative to the set of measurement data, the weights indicating how strongly the restriction or the independent measurement should be considered.

[0014] The term "condition" should refer to the meaning of regression analysis, that is, the meaning of a function, in which these conditions should be met as much as possible. Therefore, "condition" does not mean an absolute condition that must be completely satisfied.

[0015] Preferably, one or more sensors are mounted on the rigid body, such that these sensors detect the environment of the rigid body. The set of measurement data is preferably detected by means of one or more sensors mounted on the rigid body, which detect the environment of the rigid body. The method preferably includes performing measurements by means of one or more sensors to detect the set of measurement data. Detecting the set of measurement data according to these statements can at least be considered as part of a providing step, which is preferably included in the method.

[0016] The measurement data set preferably includes at least two measurement data sets from the same sensor with different measurement time points. Detecting multiple measurement data sets with different measurement time points enables a more accurate determination of the motion state, and in particular, it also enables the determination of the motion state at different (determined) time points.

[0017] Preferably, the measurement time points of the measurement data sets from at least two different sensors are different from each other. This avoids interference between different sensors, which could lead to unreliable measurement data sets.

[0018] Preferably, the motion state at a given time point is determined at the calculation time point; wherein the at least one set of conditions is formed by a set of measurement data whose measurement time points fall within a defined time period, suitably the determined time point also falling within the defined time period. This allows the motion state to be determined by considering only specific sets of measurement data whose measurement time points are, for example, temporally close to the determined time point. Measurement data sets whose measurement time points are outside the defined time period are not included in the set of conditions. This, for example, prevents the consideration of measurement data sets that are temporally distant and from which other motion states still exist.

[0019] The term "calculation time point" should be understood as the time point at which the corresponding data (measurement data set) is determined, and the motion state is determined based on this data. When determining the motion state, only measurement data sets with measurement time points prior to or equal to the calculation time point may be considered (i.e., included in the corresponding condition set). The calculation itself obviously requires a continuous calculation period (the length of which depends on the available computing power). The calculation time point can be viewed as the start time of the calculation or the calculation period.

[0020] Preferably, the Doppler velocity deviation is multiplied by a positive time weight in the function, which is a function of the time difference between the measurement time point of the corresponding measurement data set and the determination time point for determining the motion state. More preferably, the time weight decreases as the absolute value of the time difference increases. In this way, by appropriately selecting the weights, the measurement data set that is temporally close to the determination time point and therefore better characterizes the motion state to be determined can be given stronger consideration in regression analysis than measurement data sets that are more temporally distant.

[0021] Preferably, the motion state is determined for a plurality of consecutive defined time points, wherein each defined time period among the plurality of consecutive defined time points has a lower limit time and an upper limit time, wherein the lower limit time is equal to a previous defined time point among the plurality of consecutive defined time points, and the upper limit time is equal to a subsequent defined time point or a corresponding calculation time point among the plurality of consecutive defined time points. Therefore, in the case of multiple consecutive defined time points, to achieve a high level of accuracy, only those sets of measurement data explicitly assigned to a defined time point or not located outside of previous or subsequent defined time points are considered.

[0022] Furthermore, preferably, the motion state is first determined at the determined time point based on a first determined time period, and the motion state is second determined at the determined time point based on a second determined time period; wherein the second determined time period is different from the first determined time period and includes at least one measurement time point not included in the first determined time period. More preferably, the first determination is performed at a first calculation time point, while the second determination is performed at a later second calculation time point, wherein the upper limit of the second determined time period is temporally located after the first calculation time point. This allows the motion state to be determined multiple times for the same determined time point when a new set of measurement data exists. The first determination can be considered as a first estimate performed at an earlier time point, and the first estimate is improved at a later second calculation time point when a new set of measurement data exists to obtain an improved estimate, i.e., the second determination.

[0023] Preferably, in the regression analysis, a transformation between the corresponding sensor reference frame and the volume reference frame is used to express the estimated Doppler velocity as dependent on the velocity vector and the angular velocity vector, and the velocity vector and the angular velocity vector are determined such that the function is minimized. This process considers all sets of measurement data simultaneously in the regression analysis, resulting in high accuracy.

[0024] Preferably, multiple sets of conditions are formed, each set including only a group of measurement data from a single sensor. Multiple sensor velocity vectors are determined in the corresponding sensor reference frame by expressing the estimated Doppler velocity as dependent on the sensor velocity vector in the regression analysis for each condition set, and determining the volume sensor velocity vector such that the corresponding function is minimized. In the regression, the velocity vector and angular velocity vector, or one or more components thereof, are determined such that the regression function of the sensor deviation between the sensor velocity vector and the estimated sensor velocity vector is minimized, wherein the sensor velocity vector is expressed as dependent on the velocity vector and angular velocity vector using a transformation between the volume reference frame and the corresponding sensor reference frame. This process first determines the sensor velocity vector, and then determines the motion state of the rigid body based on it. This two-stage process requires relatively little computation because the set of equations or conditions to be satisfied is relatively small.

[0025] Preferably, each condition set includes only groups of measurement data with the same measurement time point; wherein, if necessary, the sensor bias is multiplied by weights in the regression function, the weights being a function of the time difference between the measurement time point of the corresponding condition set and the determined time point for determining the motion state. This allows for the consideration of different measurement time points.

[0026] Preferably, the set of conditions includes at least one component constraint on at least one component of the velocity vector and / or angular velocity vector in the form of at least one component pre-set value. The function, or if necessary, the regression function, also depends on the component deviation between the at least one component pre-set value and at least one estimated component pre-set value. The component constraint represents a condition independent of the measurement data set, thus the component constraint is unaffected by errors or inaccuracies involving multiple measurement data sets, thereby contributing to improved accuracy in motion state determination.

[0027] Preferably, at least one angular velocity component is used as a pre-defined component in the body reference frame, said at least one angular velocity component being determined, in particular, by a rotational rate sensor; wherein, more preferably, if necessary, the deviation of at least one component is multiplied by a weight in a function or regression function, said weight being a function of the time difference between the measurement time point of the rotational rate sensor and the determination time point of the motion state. Rotational rate sensors typically have relatively high measurement accuracy, allowing for improved accurate determination of the motion state by introducing a corresponding determined or measured angular velocity component.

[0028] Preferably, the measurement data set includes at least one additional parameter selected from the object being measured, including distance, distance variance, azimuth variance, elevation variance, Doppler velocity variance, received signal strength, cross-section (i.e., the effective cross-section of the object, particularly the radar cross-section or lidar cross-section), and sensor type, as well as, if necessary, the arrangement of the sensor on the rigid body. The distance, distance variance, azimuth variance, elevation variance, Doppler velocity variance, received signal strength, and cross-section of the object being measured are respectively related to the object in the corresponding measurement data set. The Doppler velocity deviation is multiplied by an additional weight, which is a function of the corresponding additional parameter. Alternatively or additionally, if at least one additional parameter is outside at least one predetermined range, the measurement data set is discarded. In this way, measurement data sets that can be assumed to have high reliability based on the additional parameters can be given more consideration. Similarly, unreliable measurement data sets can be given less consideration or not considered at all. This allows for a more accurate determination of the rigid body's motion state.

[0029] Preferably, a so-called variable error regression method is used for regression analysis, where the azimuth angle is optimized and / or, if necessary, the elevation angle is optimized. This allows for further improvement in the accuracy of determining the rigid body motion state.

[0030] Iterative reweighted least squares regression analysis is preferred. This avoids inaccurate determination of motion states caused by outliers in the measurement data set, especially outliers of dynamic and non-static objects in the environment.

[0031] The method for determining the relative position and / or relative orientation of a rigid body according to the present invention includes determining multiple motion states of the rigid body for multiple consecutive determined time points according to the present invention, and integrating the motion states over time between an initial determined time point and an end determined time point among the multiple determined time points to obtain the relative position and / or relative orientation as the result of the integration.

[0032] The computing unit according to the invention, such as the control device of a motor vehicle or robot, is particularly configured with programming techniques to obtain multiple sets of measurement data and execute the method according to the invention having the above aspects.

[0033] Vehicles and / or robots according to the invention, particularly land vehicles, aerial vehicles, or water vehicles and / or land robots, aerial robots, or water robots, have a computing unit according to the invention and one or more sensors mounted on the vehicle body and / or robot body. These sensors detect the environment of the vehicle body and are configured to measure objects in the environment and send sets of measurement data detected in the process to the computing unit. The sensors are preferably radar sensors and / or lidar sensors.

[0034] It is also advantageous to implement the method according to the invention in the form of a computer program or computer program product having program code for performing all method steps, particularly those related to regression analysis and the formation of condition sets, because this results in particularly low costs, especially where the execution control device is also used for further tasks and is therefore present anyway. Suitable data carriers for providing the computer program are, in particular, magnetic, optical, and electrical memories, such as hard disk drives, flash memory, EEPROM, DVDs, etc. The program can also be downloaded via computer networks (Internet, intranet, etc.).

[0035] Further advantages and designs of the present invention will become apparent from the description and drawings. Attached Figure Description

[0036] The invention is schematically illustrated in the accompanying drawings based on embodiments, and the invention is described below with reference to the accompanying drawings.

[0037] Figure 1 A top view of a vehicle equipped with sensors is shown;

[0038] Figure 2 The reference frame of the sensor for detecting the object is shown;

[0039] Figure 3 A graph with multiple defined and measured time points is shown; and

[0040] Figure 4A flowchart illustrating a preferred embodiment of the present invention is shown. Detailed Implementation

[0041] Figure 1 A top view illustrates an example of a motor vehicle 2 as a rigid body, with sensors S1, S2, S3, and S4 (e.g., radar sensors and / or lidar sensors) mounted on it. It is also conceivable to use other vehicles (e.g., aerial vehicles and water vehicles) as rigid bodies. Robots are also possible. In particular, the vehicle can be automated (robotic vehicle). Generally, the invention can also be applied to non-automatically driven rigid bodies. The motor vehicle (more generally, the body) defines a body reference system B with three mutually orthogonal axes (i.e., x-axis, y-axis, and z-axis), which extends perpendicularly to the plane of the drawing in the top view. The origin U of the body reference system B is... B For motor vehicles, the center of rotation is preferably located at the center of rotation of the vehicle. In motor vehicles employing an Ackermann steering system, this is the center of the rear axle, as shown in the figure.

[0042] A rigid body (i.e., vehicle 2) moves within an environment containing objects 4 (e.g., trees, buildings, other vehicles). Static objects, i.e., objects that do not move in the environment, and moving objects may also exist. Static objects are located at fixed positions within the environment or in the environment's frame of reference.

[0043] A rigid body (vehicle 2) moves in the environment. This motion, that is, the state of motion of the rigid body in the environmental reference frame, can be represented in the body reference frame B by linear velocity vectors (Vx, Vy, Vz) and angular velocity vectors (ωx, ωy, ωz). The components of the velocity vector represent the velocities of the rigid body in the environmental reference frame along the corresponding axes x, y, and z of the body reference frame, while the components of the angular velocity vector represent the angular velocities of the rigid body in the environmental reference frame around the corresponding axes x, y, and z of the body reference frame. Figure 1 The view only shows Vx, Vy, and ωz.

[0044] Sensors S1, S2, S3, and S4 are mounted at specific locations on the rigid body, in this case, the four corners of the vehicle 2. Other numbers of sensors are also possible; preferably, the number ranges from 1 to 10, and more preferably from 3 to 6. Unlike what is shown, the sensors can be mounted not only at the corners but also at any other part of the rigid body.

[0045] Each sensor defines a sensor reference frame. Figure 1 The sensor reference frame S is shown as one of the representative sensors S1, which has mutually orthogonal axes, namely the x-axis and x-axis. S y-axis S and z-axis zS The z-axis is not shown in the top view and may, but is not required to, extend perpendicular to the plane of the drawing or parallel to the z-axis of the body reference frame.

[0046] Each sensor's sensor reference frame is connected to the volume reference frame via a linear transformation, meaning it can be mapped to the volume reference frame through a transformation. Therefore, vectors / coordinates in each sensor reference frame can be mapped to their corresponding vectors / coordinates in the volume reference frame through a corresponding transformation. The transformation of each sensor reference frame is achieved by a translation t (from the origin U of the volume reference frame). B With respect to the origin U of the sensor reference frame S The transformation is given by a combination of the three-dimensional translation vector representation (represented by the translation vector) and the rotation R (represented by Euler angles, for example) (again, representatively shown for one of the sensors). Therefore, this is a transformation between two three-dimensional spaces. This transformation should be non-singular.

[0047] also, Figure 1 The vehicle preferably includes a computing unit 12 (e.g., a control device) configured to acquire a set of measurement data from sensors and determine the motion state based thereon, wherein the computing unit is configured to perform regression analysis, particularly as described in more detail below.

[0048] Figure 2 The reference frame S of the sensor (radar sensor and / or lidar sensor) for the object being detected 4 is shown; this is the sensor reference frame. The sensor reference frame S has mutually orthogonal x-axis x... S y-axis S and z-axis z S The sensor surface 6, schematically drawn here, is exemplarily arranged in, for example, the yz plane, and detects the x-coordinate. S A half-space with a value greater than 0 is used as the field of view. Of course, these sensors can also have other fields of view (different from the half-space). The sensor, or the sensor used in this invention, is configured to use the Doppler effect to determine the velocity of the detected object toward or away from the sensor; this velocity is called the Doppler velocity, i.e., using a Doppler sensor. These sensors are preferably radar sensors or Doppler radar sensors and / or lidar sensors.

[0049] The position of object 4 is given by the object position vector P, which originates from the sensor reference frame at point U. S Extending to object 4, the object's position vector can be described in spherical coordinates (θ, φ, r) in the sensor reference frame. The radius r = |P|² is the length of the object's position vector P, i.e., the origin U. S The distance to object 4. The azimuth angle θ is the x-axis. SThe angle between the object's position vector P and its projection onto the xy-plane. The elevation angle φ is the angle between the xy-plane and the object's position vector P. Correspondingly:

[0050] P=(x s y s , z s )=(rcosθcosφ, rsinθcosφ, rsinφ)

[0051] Object 4 has a velocity vector V relative to the sensor or in the sensor reference frame. O The velocity represented. The velocity of the sensor relative to the object is correspondingly given by V. S =-V O Given. The Doppler velocity d is derived from the object's velocity vector V. O The projection length onto the object's position vector P is given. This length is equal to the object's velocity vector V. O The unit vector p along the direction of the object's position vector P 0 scalar product d = p 0 ·V O , where p 0 =P / r. In general, the Doppler velocity d can be obtained from the velocity vector V of the sensor relative to the object. S =(V Sx V Sy V Sz To express:

[0052]

[0053] The sensor detects a large number of measurement data sets (e.g., 50 to 150 sets) at a single measurement time point, so that each measurement by the sensor determines a large number of Doppler velocities and the corresponding azimuth and elevation angles. A static object defines the environment, relative to which the sensor's velocity, and more broadly, the velocity of the rigid body, can be determined using the measured Doppler velocities.

[0054] Despite the large set of measurement data, appropriate notation is introduced below in order to express the relevant equations in a clear form.

[0055] Vectors, matrices, and scalars: Vectors and matrices are shown in bold to more easily distinguish scalars.

[0056] A point in three-dimensional (3D) space: A point p that should be expressed in Cartesian coordinates relative to a coordinate system or reference system A is written as A p.

[0057] The vector between two points in 3D space: The translation vector between two points B and C relative to reference frame A is written as A tB,C .

[0058] Velocity of a reference frame in 3D space: If reference frame C moves relative to reference frame B with a specific linear velocity or angular velocity, then the corresponding velocity relative to reference frame A is written as follows for linear velocity:

[0059] A v B，C

[0060] Angular velocity is written as:

[0061] A ω B，C

[0062] Rigid body rotation and transformation: The transformation between two reference frames A and B is written as:

[0063] T A,B

[0064] This transformation converts the points represented in reference frame B to reference frame A:

[0065] T A，B · B p = A p

[0066] Since the transformation includes rotation and translation, it can also be written as:

[0067] R A，B · B p+ A t A，B = A p

[0068] Where R A,B T is the corresponding rotation matrix. A，B It can be viewed as the position and orientation of one reference frame relative to another; in English it is also called "Pose".

[0069] The rigid body's reference frame, or volume reference frame, is represented by the letter "B". The sensor's reference frame, or sensor reference frame, is represented by "S", "S1", "S2", ..., "Si", ..., "SI". For simplicity, the sensor itself is also partially represented by S, S1, S2, ..., Si, ..., SI. The environment's reference frame is represented by the letter "W".

[0070] Use this notation to express the position of an object in the sensor reference frame Si in Cartesian or polar coordinates:

[0071] Si p j = (x, y, z) T=r(cosθcosφ, sinθcosφ, sinφ) T

[0072] Where "j" represents one of the (multiple) sets of measurement data for the object (e.g., the results of radar or lidar measurements). If the sensor Si is at a velocity relative to the environment W... Si v W,Si If the object is in motion and is a static object in the environment, then equation (1) applies as described above:

[0073]

[0074] in

[0075]

[0076] It is the normalized point coordinate of the object, that is, the unit vector in the direction of the object (|·|2 represents the usual 2-norm, that is, the length of the vector).

[0077] From multiple Doppler velocities d j Multiple measurement data sets, where j is taken from 1, 2, ..., J (J is the number of measurement data sets used, which can include multiple measurement data sets of a single object and / or measurement data sets of different objects at a single measurement time point), yield the following system of equations (2):

[0078]

[0079] If the measurement data set does not include the elevation angle φ, then assume or set the corresponding elevation angle φ in the equation set. j Equal to a predetermined angle. These elevation angles are preferably set to zero (for measurement data set j where no elevation angle exists, φ). j =0). This leads to a simplification of the system of equations, since the cosine and sine of these elevation angles are 1 and 0 respectively. In simplified notation, the system of equations (2) can be written as:

[0080]

[0081] A· Si v W，Si =D

[0082] A and D serve as names for the corresponding vectors in the equations; these names are given by the sensor measurements. A is derived from the azimuth angle θ and elevation angle φ of the measured data set, and D is derived from the corresponding measured Doppler velocity d. i The conclusion is as follows.

[0083] Therefore, if multiple sets of measurement data exist for sensor Si, the speed of the sensor can be estimated or determined by solving these equations. Si vW,Si These equations typically form an overdetermined system of equations. In the sense of the aforementioned system of equations, the set of measurement data can be considered as a condition that the sensor velocity must satisfy. Therefore, the set of measurement data, or at least a portion thereof, represents a set of conditions.

[0084] The above system of equations (1) can be solved by regression, where the sensor velocity... Si v W,Si The error metric (i.e., the regression function F) is determined or changed to minimize the measured Doppler velocity d. j Compared with the estimated Doppler velocity The difference between them (Doppler velocity deviation) This difference is given by the left-hand side of the equations for the corresponding sensor velocity. For example, the mean square error can be minimized:

[0085]

[0086] Of course, other functional dependencies besides quadratic functional dependencies are also possible.

[0087] In addition, the weight g can be provided here. j The weights depend on additional parameters detected by the sensor during the measurement (e.g., radar or lidar measurement). Weight g j This can also be called parameter weighting. In terms of the function, the measured Doppler velocity d is then... j Compared with the estimated Doppler velocity The difference between them is multiplied by the corresponding weight g. j Therefore, we can conclude that:

[0088]

[0089] For example, compared to measurement data sets with low variance, a lower weight can be assigned to measurement data sets where azimuth and / or elevation angles have high uncertainty or variance.

[0090] Depending on additional parameters, such as if the distance to the object is higher than a pre-defined threshold, the individual measurement data sets can be disregarded, i.e., not included in the above equation set (2). Correspondingly, the weights g (parameters) are... j Setting it to zero has a similar effect.

[0091] Generally, measurement data sets contain errors; in particular, not all detected objects are static, resulting in outliers in the measurement data sets that distort the determination of sensor velocity. To minimize this effect, robust regression methods are preferred. Specifically, iteratively reweighted least squares (IRLS) can be used. This IRLS method is known to those skilled in the art.

[0092] To determine the linear velocity vector of a rigid body B v W,B and angular velocity vector B ω W,B The sensor's velocity is related to the (linear and angular) velocity vector of the rigid body, where the relative rotation R of the sensor, which is fixedly mounted on the rigid body, is used. B，Si Peaceful relocation B t B,Si Equation (3):

[0093] Si v W，Si =R Si，B ( B v W，B + B ω W，B × B t B，Si )

[0094] Here, "x" represents the common cross product of two three-dimensional vectors.

[0095] Use writing style

[0096]

[0097] The equation can be written as follows:

[0098]

[0099] Therefore, the velocity of the sensor can be expressed by a vector consisting of the linear velocity vector and the angular velocity vector of the rigid body.

[0100] For multiple sensors 1, 2, ..., I, the corresponding equations can be written together as a system of equations (4):

[0101]

[0102] The various transformations (relative rotation and translation) from different sensors are summarized in matrix M. Since matrix M is known, the velocity vector of the rigid body can be determined. B v W，B and B ωW，B The system of equations, which is generally overdetermined when there are a sufficient number of sensors.

[0103] Determine the motion state of a rigid body (i.e., determine the linear velocity vector) B v W，B and angular velocity vector B ω W，B One possibility is to first determine the sensor's velocity as described above. Si v W，Si That is, for each sensor, the above system of equations (2) is solved using a regression method, and then the above system of equations (4) is solved again using regression (e.g., least squares method). In general, this two-stage process represents a regression analysis. This two-stage process can be called a weakly coupled scheme and has the advantage of being able to be executed relatively quickly (i.e., compared with other schemes with relatively small computational power).

[0104] Another process, the so-called strongly coupled scheme, involves directly determining the linear velocity vector from the Doppler velocity using regression analysis or at least a portion thereof. B v W，B and angular velocity vector B ω W，B That is, there is no need to determine the sensor velocity from the Doppler velocity beforehand. Strongly coupled schemes can generally determine the motion state more accurately.

[0105] Therefore, the above equation (1) for the Doppler velocity and the above equation (3) combining the sensor velocity with the velocity vector of the rigid body are:

[0106]

[0107]

[0108] In the case of multiple measurement data sets 1, ..., J, these equations can be summarized into the following set of equations (5):

[0109]

[0110]

[0111] Here, multiple sets of measurement data from each of the multiple sensors can each include multiple sets of measurement data. For example, if four sensors are set, 50 to 150 sets of measurement data from each sensor can be included in the equation set, resulting in a total of 200 to 600 sets of measurement data. The subscript "Si" is used here in a general sense, meaning that it should be understood in each line of the equation set as the corresponding sensor or its sensor reference frame to which the set of measurement data is assigned. Therefore, sets of measurement data from different sensors can be included in this equation set.

[0112] This system of equations can be solved using regression methods. Therefore, as mentioned above, a pre-given function F depends on the measured Doppler velocity d. j and estimated Doppler velocity The difference or Doppler velocity deviation between them This function minimizes the solution (again, exemplarily using squared distance):

[0113]

[0114] Similarly, this can be based on weights or parameter weights g. j Additional parameters for the measurement data sets apply different weightings to different measurement data sets:

[0115]

[0116] The individual sets of measurement data based on the additional parameters in equation (5) can also be ignored.

[0117] As mentioned above, the expression "estimated Doppler velocity" here refers to: a given linear velocity vector estimated (within the range of regression methods or optimization). B v W，B and the given angular velocity vector B ω W，B The estimated Doppler velocity can be calculated from the left-hand side of the above equation system (5). Then, in the regression method, the linear velocity vector is determined. B v W，B and angular velocity vector B ω W，B This allows the corresponding function to be optimized or minimized. This represents the determination or estimation of the rigid body's motion state.

[0118] Figure 3 The graph illustrates time series for multiple determination and measurement points. Several determination points T0, T1, and T3 are plotted along the time axis t, representing the points in time when the motion state of the rigid body should be determined or estimated. These determination points are typically different from the calculation points used to determine or calculate the motion state.

[0119] The corresponding motion states are represented by Z0 = Z(T0), Z1 = Z(T1), and Z2 = Z(T2). At time point T... k The motion state Z(T) at the location k The rigid body at the corresponding time point T k linear velocity vector at the location B v W,B (T k ) and angular velocity vector B ω W,B (T k ) is used to characterize it.

[0120] As shown in the figure, the time points can be spaced out regularly, with the time interval ΔT between two consecutive time points being ΔT = T. k+1 -T k Given that the reciprocal of the time interval ΔT, 1 / ΔT, can be considered as a definite frequency. However, more generally, definite time points with irregular intervals can also be conceived.

[0121] Furthermore, the figure shows multiple measurement time points, i.e., the time points at which at least one sensor detects the object. At each measurement time point, the sensor will determine multiple individual sets of measurement data, each set including at least the Doppler velocity, azimuth, and elevation angle of the detected object. Additionally, each set of measurement data may include further parameters.

[0122] In addition to Doppler velocity, azimuth, and elevation, each set of measurement data also includes the measurement time points at which these data were measured. For example, displaying two sensors S1 (measurement time point t) -1 The measurement time points t of S2 (measurement time points t0, t2, t5) are: t1, t3, t4, t2, t5. -1 t0, t1, t2, t3, t4, t5, where t -1 It coincides exactly with T0. The measurement time points of the sensors can be independent of each other and / or independent of the determined time point. If necessary, the measurement frequencies of different sensors can also be different and / or different from the determined frequency. For example, in the figure, the measurement frequency of sensor S2 is equal to the determined frequency, while sensor S1 has a higher measurement frequency. Preferably, the measurement time points of different sensors are different from each other to avoid interference.

[0123] The determining frequency or the average number of determining time points per second can be in the range of, for example, 5-10 Hz. For instance, the sensor's measurement frequency or the average number of measurement time points per second can be in the range of 10-20 Hz. The sensor's measurement frequency (or the average number of measurement time points per second) is preferably greater than the determining frequency (or the average number of determining time points per second).

[0124] The determination or estimation of the motion state is in principle carried out by the regression method or regression as described above, that is, according to a loose coupling scheme or a tight coupling scheme. This includes measurement data sets with measurement time points that occur temporally close to the determination time point, in particular within a specific time limit around the determination time point. For example, all measurement data sets that occur within a predefined time interval dT can be included, that is: |t j -T k |<dT; here it is also possible to have an asymmetric limit with a lower time interval d u T and an upper time interval d o T: -d u T<t j -T k <d o T. Another possibility is to include all measurement data sets whose measurement time point t i lies between the previously determined time point T k-1 and the subsequent determined time point T k+1 : T k-1 <t j <T k+1 or expressed as upper / lower time intervals, d o T=T k+1 -T k and d u T=T k -T k-1 . For regularly spaced determination time points, the following formula applies: d o T=d u T=ΔT.

[0125] Therefore, in general, measurement data sets whose measurement time points are within a determination time period are included in the determination of the motion state for the determination time point T k . This determination time period is defined by a lower limit time and an upper limit time, for example, as described above, the interval [T k -d u T; T k +d o T] is the determination time period. The determination time point T k preferably lies within the determination time period. It is also generally conceivable that the determination time point T k lies outside the determination time period, that is, d u T<0 or d o T<0 in the above example.

[0126] The motion state is calculated at at least one calculation time point for a given time point. This calculation time point is typically different from the given time point at which the motion state is determined, so that it can also include a set of measurement data that is measured after the given time point. The figure exemplarily shows two calculation time points λ1 and λ2 for a given time point T1 to determine the motion state Z1, where the first calculation time point λ1 is between the two measurement time points t2 and t3, and the second calculation time point λ2 is after the measurement time point t3, here exemplarily between the measurement time point t3 and the given time point T2. However, the second calculation time point may also coincide with the measurement time point t3. Regarding the time constraints around the given given time points, the calculation time points represent an upper limit, i.e., the set of measurement data to be considered is limited, such that only additionally satisfying condition t is considered. j ≤λ b (λ b A set of measurement data (representing the calculation time point), meaning the measurement time points of these data sets are at or before the calculation time point. To determine... Figure 3 The motion state Z1 in the figure, for example, at a certain time point λ1, the measurement data set with the measurement time point t3 is not considered, while at a certain time point λ2, the measurement data set with the measurement time point t3 is considered.

[0127] Determining the motion state at a given time point can be performed multiple times at several different calculation time points. For example, determination can always be triggered when a new set of measurement data exists within the time limit of the determination time point from the measurement time point. Alternatively, determination can be performed only when the measurement time point is after the determination time point. However, in principle, the calculation time point can also be located before the determination time point for determining the motion state. For example, the motion state Z3 at the determination time point T3 (not shown in the figure) can be determined or estimated using the set of measurement data from measurement time points t2 and t3 at the determination time point λ2. It is also conceivable that the calculation time points are regularly spaced in time. The calculation time points can also coincide with the determination time point, where the motion state is determined for each previous determination time point. How the calculation time point is determined also depends in particular on the available computing power.

[0128] To account for cases where the measurement and determination time points do not coincide, the measurement data set is weighted within the function F to be minimized based on both the measurement and determination time points. Therefore, a weight W, which can be referred to as the time weight, is set. j The weights depend on the measurement time point t. j With a determined time point T k The difference between them: w j =w j (t j -T k Preferably, the weight depends on the absolute value of the difference: w j =wj (|t j -T k These weights should always be positive or equal to zero.

[0129] The functional dependency of the weights should be chosen such that, at measurement time t, j With a determined time point T k When the difference between them is 0, the weight takes its maximum value: w j (0)=w max Furthermore, it monotonically decreases as the absolute value increases, preferably strictly monotonically decreasing, until it reaches zero, wherein preferably it reaches the upper limit T of the considered measurement time point. k +d o T or lower limit T k -d u It reaches zero at time T:

[0130] w j (T k +d o TT k ) = w j (d o T)=0=w j (T k -d u TT k ) = w j (-d u T).

[0131] The maximum value can be any positive number, for example, equal to 1: W max =1. Maximum value W max The same applies to all sets of measurement data (and, if necessary, to other additional measurements, see below).

[0132] In particular, the following may apply:

[0133]

[0134] More generally, the linear decrease described above with increasing intervals can also be modified by a function f that monotonically increases with increasing independent variable, preferably strictly monotonically:

[0135]

[0136] For example, a quadratic function f(x) = x can be used. 2 .

[0137] Then, in the function F that should be minimized in the regression method, the measured data set is weighted accordingly, i.e., the difference between the measured Doppler velocity and the estimated Doppler velocity. In this function, the corresponding time weights w of the measurement data set j Multiply, therefore Used in this function:

[0138]

[0139] If the difference is further weighted based on additional parameters of the measurement data set, then the difference must also be weighted by the corresponding parameter weights g. j Multiplication, therefore, is used in this function.

[0140]

[0141] Here, in both cases, the preferred quadratic function in the independent variable (i.e., the weighted difference) is illustrated again as an example. This function can be used for the solution of equation system (5) (in the sense of the regression method) as well as the solution of equation system (2).

[0142] Each line of equation set (5) or equation set (2) represents a condition for the linear velocity vector and angular velocity vector or the corresponding velocity of the sensor. Since each line of the corresponding equation set corresponds to a set of measurement data, each set of measurement data can be regarded as a condition.

[0143] To determine the motion state for a given time point, a set of conditions is selected from the set of all measured data sets at the calculation time point; this set is used to determine the motion state. Specifically, these are the intervals [T] around the determined time point Tk where the measured time points are located. k -d u T;T k +d o The measurement data set within [T] (where the measurement time point is located before or at the calculation time point).

[0144] Here, additional conditions not derived from the measurement data set can be added to the set of conditions. These could be measurements from other measuring devices and / or pre-defined constraints on the rigid body motion.

[0145] Preferably, a rotation rate sensor or gyroscope is provided, particularly positioned at the origin of the body reference frame. Then, the following additional conditions or equations for equation set (5) are derived:

[0146]

[0147] in It is the angular velocity measured by the rotation rate sensor, and I is the three-dimensional identity matrix.

[0148] For example, the constraints on the motion of a rigid body (e.g., a motor vehicle moving in a plane) can be shown below, which is formulated here as conditions for the individual components of the velocity vector and angular velocity vector:

[0149]

[0150] Each of these conditions is considered in the function of the regression method with a corresponding (additional condition) weight. The magnitude of the weight determines the degree to which the additional condition or constraint is considered. Furthermore, in the case of measurement, such as angular velocity measurement via a rotation rate sensor, a (time) weight can be set depending on the measurement time point associated with the determined time point. Specifically, such time weights can be determined as described in conjunction with the sensor's measurement time points.

[0151] For example, in the case of measuring angular velocity using a rotation rate sensor, an additional function F can be added. G Add to the function F above to obtain the total function: F 总 =F+F G ,in

[0152]

[0153] represents the estimated angular velocity vector, and |·| represents the vector norm.

[0154] G is the weight, which is used to consider the measurement value of the rotation rate sensor.

[0155] w G The time weight is determined based on the measurement time point of the rotation rate sensor.

[0156] Since rotational rate sensors typically have higher accuracy than sensors, and given the generally large number of measurement data sets and relatively few rotational rate sensor measurements, the weight G is preferably chosen to be much higher than the corresponding weights of the sensor measurements. In the formula for the function F above, the corresponding weights of the sensor measurements are 1, or expressed differently, because the constant (positive) factor in the function is irrelevant to optimization and only represents normalization. Therefore, the function can be normalized over the measurement data sets so that the corresponding weights of the sensor measurements are equal to 1. Thus, the weight G is preferably chosen to be much greater than 1: G >> 1; for example, G > 10 or more preferably G > 100. The weight G can also be constrained upwards, for example, G < 1000.

[0157] As described above, robust regression methods are preferably used to minimize the impact of outliers, particularly those given by non-static objects. Iterative reweighted least squares (IRLS) is preferably used.

[0158] In this case, based on the aforementioned quadratic function and with iterative weights h for each set of measured data or for each condition within the function. j，k The squared deviations between the estimated and measured Doppler velocities are weighted, where k is the iteration parameter and j is used to number the measurement data sets as usual. For example, the following equation is then derived, where an optional time weight w is considered here. j and parameter weights g j :

[0159]

[0160] For a given iterative weight h j,k This function is minimized using the least squares method. Based on the found solution, that is, based on the found... Determine the new iterative weight h j,k+1 Here, the iterative weights that have a high deviation between the estimated Doppler velocity and the measured Doppler velocity are reduced relative to the iterative weights that have a small deviation. A specific value h can be pre-given as the initial value of the iterative weights. j,0 For example, 1:h j,0 =1, for all j.

[0161] So far, the azimuth θ and elevation φ contained in the measurement data set are considered accurate, and these angles are respectively included on the left-hand side of the system of equations to be solved. However, in general, they represent erroneous values ​​as measurements. To account for this, error-in-variables regression is preferably also used.

[0162] The process can be performed as follows: First, the motion state is determined or estimated using the strongly coupled scheme described above. Then, variable error regression is performed using the determined motion state to optimize the azimuth angle θ and / or elevation angle φ. Using the optimized azimuth angle θ and / or elevation angle φ, the motion state is re-determined or estimated using the strongly coupled scheme, and the azimuth angle θ and / or elevation angle φ are re-optimized again. This process is repeated until the results converge, for example, until the continuously determined motion state no longer changes within a pre-given tolerance, and / or until a pre-given maximum number of repetitions is reached. Overall, this process represents regression analysis.

[0163] If the motion state of a rigid body is determined for multiple consecutive, defined time points, it can be determined using linear velocity vectors. B v W，B and / or angular velocity vector B ω W,B The time integral is used to calculate the rigid body at the final determined time point T. End Compared to the initial determined time point TAnfang The relative position and / or orientation of the initial position and / or initial orientation. Since the motion state is determined only for discrete, specific points in time, this time integral is typically given as a sum.

[0164] Figure 4 A flowchart illustrating a preferred embodiment of the present invention is shown.

[0165] In step 102, a set of measurement data is obtained through one or more sensors. Similarly, additional measurement results can be detected, particularly rotational rate measurements from a rotational rate sensor or gyroscope. This detection can be performed at multiple measurement points; specifically, each sensor can perform measurements and detect the results continuously, for example, at a specific measurement frequency.

[0166] In step 104, the measurement data set, along with any additional measurement results if necessary, is transmitted to the computing unit. Here, the measurement data set is sent from the sensor to the computing unit and received by the computing unit. Similarly, additional measurement results are sent from the corresponding measurement sensor to the computing unit and received by the computing unit if necessary. In both cases, the sensor can be directly or indirectly connected to the computing unit, and temporary storage can be provided for later evaluation (i.e., determining the motion state). The computing unit can be located on or within the rigid body whose motion state should be determined. It can also be a computing unit located away from the rigid body, such as a remote computer or server. Measurement results can be transmitted to the computing unit via a communication network (e.g., wirelessly) or on a suitable data carrier (e.g., in the case of later evaluation).

[0167] Steps 102 and 104 can be considered together as providing steps. This providing can also be done in other ways, for example, if the measurement data set and additional measurement results already exist as stored data, or if a simulated measurement data set is used. In this respect, steps 102 and 104 are preferred steps.

[0168] In step 106, at least one set of conditions is formed from the set of measurement data and, if necessary, additional measurement results. Here, the process is combined as described above. Figure 3 As described, this is done. Therefore, for a specific time point where the state of motion should be determined or calculated, the set of measurement data within a defined time period, along with additional measurement results if necessary, is included in the condition set. This can be done once or multiple times at the corresponding calculation time point.

[0169] In preferred step 108, weights are determined and used to weight the measurement data sets included in the condition set, as well as any additional measurements, in the function used in the regression analysis. As described above, these are time weights and / or parameter weights and / or, if necessary, weights used to weight the additional measurements relative to the measurement data sets. These weights are determined before the actual regression method is performed and remain constant during the execution of the regression method (for each computation time point). Furthermore, additional weights different from those described above can be used during the regression analysis; these additional weights may change as necessary during multiple iterations of the regression method, such as the iterative weights described above in the IRLS case. Filtering may also be performed here, in which specific measurement data sets are excluded from the condition set based on additional parameters of the measurement data sets.

[0170] Then, a regression analysis is performed based on at least one set of conditions to determine the motion state (linear velocity vector and angular velocity vector) of the rigid body.

[0171] Here, on the one hand, step 110 can be carried out according to the strong coupling scheme, that is, the Doppler velocity is directly related to the motion state in the regression analysis (equation set (5)).

[0172] Alternatively, a weakly coupled scheme can be used. Here, in step 112, the sensor velocity vector is first determined by correlating the sensor velocity vector with the Doppler velocity in a regression method. Subsequently, in step 114, the motion state is calculated from the sensor velocity vector in a further regression.

[0173] In steps 110, 112, and 114, as explained above in step 108, additional variable weights may be used, if necessary, within the scope of the respective regression method used (e.g., within the scope of IRLS), which are different from time weights, parameter weights, and weights used to weight additional measurements.

[0174] In preferred step 116, it can be determined whether the vehicle's motion state should be determined for another defined time point and / or based on another defined time period (e.g., when a new set of measurement data exists during this period). If so, the process can jump back to step 106 (arrow 118), where at least one new set of conditions is determined for another defined time point and / or based on another defined time period. Independently, optional step 120 can continue for the already calculated motion state.

[0175] In preferred step 120, one or more calculated motion states may be output and / or passed to further processing. In particular, if motion states have been determined for multiple consecutive defined time points, these motion states can be integrated in time to determine the relative position and orientation of the rigid body between its initial state at an earlier defined time point and its final state at a later defined time point.

Claims

1. A method for determining the motion state of a rigid body relative to its environment by means of a large set of measurement data of an object (4) in an environment for the rigid body (2), Each set of measurement data includes the measurement time point, Doppler velocity (d), and azimuth angle (θ) relative to the sensor reference frame (S) of the sensors (S1, S2, S3, S4). The method includes: The motion state of the rigid body relative to the environment is defined as the velocity vector (v) in the body reference frame (B). x v y ) and angular velocity vector (ω) z Each sensor reference frame can be transformed into the volume reference frame through a nonsingular transformation (R, t). This forms at least one set of conditions, which includes multiple sets of measurement data. In the regression analysis, minimization of the at least one set of conditions depends on the Doppler velocity deviation between the estimated Doppler velocity and the Doppler velocity of the measurement data set included in the at least one set of conditions. The estimated Doppler velocity is represented as the dependent variable in the regression analysis. The regression analysis is used to determine one or more components of the velocity vector and / or angular velocity vector. This involves forming multiple sets of conditions, each set of conditions consisting of only a group of measurement data from a single sensor; In the corresponding sensor reference frame (S), multiple sensor velocity vectors (V) are determined. S The method involves, in the regression analysis, further representing the estimated Doppler velocity as dependent on the sensor velocity vector for each condition set, and determining the sensor velocity vector such that the corresponding function is minimized; and In the regression, the velocity vector and the angular velocity vector are determined such that the regression function that depends on the sensor deviation between the sensor velocity vector and the estimated sensor velocity vector is minimized, wherein the sensor velocity vector is expressed as depending on the velocity vector and the angular velocity vector using a transformation between the body reference frame and the corresponding sensor reference frame.

2. The method according to claim 1, wherein, One or more sets of measurement data include elevation angle ( ); wherein at least some of the measurement data sets are determined from the measurement results which only include the measurement time point, the Doppler velocity, and the azimuth angle, wherein the elevation angle is set to be equal to a predetermined value.

3. The method according to any one of the preceding claims, wherein, The measurement data set is detected by means of one or more sensors (S1, S2, S3, S4) mounted on the rigid body (2), the sensors detecting the environment of the rigid body.

4. The method according to any one of claims 1 to 2, wherein, The measurement data set includes data from a sensor at different measurement time points (t). -1 At least two sets of measurement data (t0, t1, t2, t3, t4, t5).

5. The method according to any one of claims 1 to 2, wherein, Measurement time points (t) of at least two different sensor (S1, S2) measurement data sets -1 t0, t1, t2, t3, t4, t5) are different from each other.

6. The method according to any one of claims 1 to 2, wherein, The motion state is determined at at least one calculation time point (λ1, λ2) for a given time point (T1); wherein the at least one set of conditions is formed by a group of measurement data whose measurement time points are located within a given time period.

7. The method according to claim 6, wherein, The function multiplies the Doppler velocity deviation by a positive time weight, where the positive time weight is the measurement time point (t) of the corresponding measurement data set. -1 The function of the time difference between t0, t1, t2, t3, t4, t5 and the determined time point (T1) that determines the motion state.

8. The method according to claim 7, wherein, The time weight decreases as the absolute value of the time difference increases.

9. The method according to claim 6, wherein, The motion state is determined for multiple consecutive defined time points (T0, T1, T2), wherein each defined time period of the multiple consecutive defined time points (T0, T1, T2) has a lower limit time and an upper limit time, wherein the lower limit time is equal to the previous defined time point among the multiple consecutive defined time points (T0, T1, T2), and the upper limit time is equal to the subsequent defined time point or the corresponding calculated time point among the multiple consecutive defined time points (T0, T1, T2).

10. The method according to claim 6, in, The motion state is first determined based on a first determined time period for the determined time point (T1), and the motion state is second determined based on a second determined time period for the determined time point; wherein the second determined time period is different from the first determined time period and includes at least one measurement time point not included in the first determined time period.

11. The method according to claim 10, wherein, The first determination is performed at a first calculation time point (λ1), while the second determination is performed at a later second calculation time point (λ2), wherein the upper limit of the second determination time period is located after the first calculation time point in time.

12. The method according to claim 6, wherein, Each set of conditions includes only groups of measurement data that have the same measurement time point; In the regression function, the sensor deviation is multiplied by a weight, which is a function of the time difference between the measurement time point of the corresponding condition set and the determination time point of the motion state.

13. The method according to claim 12, wherein, The set of conditions includes at least one component constraint on at least one component of the velocity vector and / or the angular velocity vector in the form of at least one component pre-given value, wherein the regression function also depends on the component deviation between the at least one component pre-given value and at least one estimated component pre-given value.

14. The method according to claim 13, wherein, At least one angular velocity component is used as a component pre-given value in the body reference frame, and the at least one angular velocity component is determined by a rotation rate sensor.

15. The method according to claim 14, wherein, In the regression function, the deviation of at least one component is multiplied by a weight, which is a function of the time difference between the measurement time of the angular velocity component and the determination time of the motion state.

16. The method according to claim 3, wherein, The measurement data set includes at least one additional parameter selected from the distance, distance variance, azimuth variance, elevation variance, Doppler velocity variance, signal strength of the received signal, cross-section, and / or sensor type and sensor arrangement on a rigid body of the object being measured, wherein the distance, distance variance, azimuth variance, elevation variance, Doppler velocity variance, signal strength of the received signal, and cross-section of the object being measured are respectively related to the object being measured in the corresponding measurement data set; The Doppler velocity deviation is multiplied by an additional weight, which is a function of a corresponding additional parameter, and / or the measurement data set is discarded if the at least one additional parameter is outside at least one predetermined range.

17. The method according to claim 2, The regression analysis is performed using a variable error regression method, wherein the azimuth angle and / or the elevation angle are optimized. The regression analysis is performed using the iterative reweighted least squares method and / or the method described above.

18. A method for determining the relative position and / or relative orientation of a rigid body (2), comprising: The method according to any one of the preceding claims is used to determine multiple motion states of the rigid body for multiple consecutive defined time points (T0, T1, T2); The motion state is integrated over the time between the initial determined time point and the end determined time point among the plurality of determined time points to obtain the relative position and / or the relative orientation as the result of the integration.

19. A computing unit (12) configured to acquire a large set of measurement data and perform the method according to any one of claims 1 to 18.

20. A vehicle and / or robot, comprising The computing unit (12) according to claim 19, and One or more sensors (S1, S2, S3, S4) are installed on the vehicle and / or robot body, the sensors detect the environment of the vehicle body and are configured to measure objects in the environment and send the sets of measurement data detected in the process to the computing unit.

21. The vehicle and / or robot according to claim 20, wherein, The sensor is a radar sensor and / or a lidar sensor.

22. A computer program product, when executed on a computing unit, causes the computing unit to perform all method steps of the method according to any one of claims 1 to 18.

23. A machine-readable storage medium having a computer program product according to claim 22 stored thereon.

Images