Method and device for determining motion state of target and carrier

Abstract

The invention provides a method and device for determining the motion state of a target and a carrier, which can be applied to the field of intelligent driving. The method comprises the following steps: acquiring a corresponding relation between multi-frame point cloud data of a target and measurement data of a target point in multi-frame point cloud from a sensor; determining a base point of the target and determining a relationship between the point cloud data of the target and the motion state of the base point according to the relationship between the motion state of the base point and the motion state of the target point; and determining the motion state of the target based on the relationship between the point cloud data of the target and the motion state of the base point and the corresponding relationship between the measurement data of the target point in the multi-frame point cloud. The method can be applied to an intelligent driving system, and is beneficial to improving the accuracy of estimating the motion state of the target, thereby being beneficial to improving the driving safety and comfort of a user.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of intelligent driving, and more particularly, to a method and device for determining a target motion state and a carrier. BACKGROUND

[0002] In the intelligent driving system of a vehicle, a radar is usually configured to perceive the surrounding environment information. A typical vehicle-mounted radar can provide measurement data of a target relative to the sensor, usually including position measurement data and speed measurement data. For example, a millimeter wave radar can provide distance, azimuth angle (or azimuth angle and elevation angle), radial velocity, Doppler frequency information, and radar cross section (RCS) information. Similarly, a laser radar based on frequency-modulated continuous wave (FMCW) technology can also provide distance, azimuth angle (or azimuth angle and elevation angle) or rectangular coordinate position, radial velocity, and reflected intensity information. In addition, based on similar technologies, an ultrasonic radar (sonar) can also provide position and radial velocity measurement data by utilizing the Doppler effect.

[0003] With the improvement of sensor resolution, for a target relatively close to the distance sensor, a millimeter wave radar or a laser radar or a sonar sensor can generate multiple measurement data in one scanning process. Such a target is usually referred to as an extended target. Unlike a point target, the dimension or size of an extended target will span multiple sensor resolution units. In one scanning process of a millimeter wave radar or a laser radar or the like, multiple extended targets can be included. Based on the multiple measurement data of the extended target, the motion state and shape estimation of the target, especially the motion speed and the position of the key points of the target, are obtained, so as to realize tracking through the extended target, which is crucial for ADAS or autonomous driving. For example, the motion speed (instantaneous speed vector or angular velocity) and the transverse and longitudinal position information of the nearest point and the farthest point relative to the ego vehicle. A typical driving scenario and the extended target therein are shown in Figure 1 (a) and (b). When estimating the motion state of the extended target, the position of the instantaneous center of rotation (ICR) of the target motion needs to be relied on. In a typical case such as uniform motion or uniform acceleration motion, the position of the ICR of the target motion is located at infinity, and the angular velocity is close to 0. In addition, due to the change of the motion state, the ICR of the target will change instantaneously. Therefore, the above estimation of the motion state of the target relying on the ICR will have the case of unstable estimation value. In addition, it is also difficult to further improve the estimation accuracy of the ICR through filtering methods, and thus it is difficult to improve the estimation accuracy of the speed vector. SUMMARY

[0004] The application provides a method, device and carrier for determining a target motion state, which helps to improve the accuracy of the motion state estimation of an extended target, thereby helping to improve the driving safety and comfort of a user.

[0005] In a first aspect, the application provides a method for determining a target motion state, which comprises: obtaining, from a sensor, a plurality of frames of point cloud data of a target and a correspondence relationship between measurement data of target points in the plurality of frames of point cloud data; determining a base point of the target and determining a relationship between the point cloud data of the target and the motion state of the base point according to a relationship between the motion state of the base point and the motion state of the target points; and determining the motion state of the target based on the relationship between the point cloud data of the target and the motion state of the base point and the correspondence relationship between the measurement data of target points in the plurality of frames of point cloud data.

[0006] Based on the above technical solution, the relationship between the point cloud data of the target and the motion state of the base point is determined based on the relationship between the motion state of the target points and the motion state of the base point, so as to determine the motion state of the target based on the relationship between the point cloud data of the target and the motion state of the base point and the correspondence relationship between the measurement data of target points, which helps to improve the accuracy of the motion state estimation of the base point, thereby helping to improve the accuracy of the motion state of the target points, and thus improving the estimation accuracy of the motion state of the target based on the base point and the target points.

[0007] In combination with the first aspect, in some implementations of the first aspect, the method comprises: determining a relationship between the motion states of the base point in different frames; and wherein the determining the motion state of the target based on the relationship between the point cloud data of the target and the motion state of the base point and the correspondence relationship between the measurement data of target points in the plurality of frames of point cloud data comprises: determining the motion state of the target based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence relationship between the measurement data of target points.

[0008] Based on the above technical solution, the motion state of the target is determined based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence relationship between the measurement data of target points. In this way, the accuracy of the motion state estimation of the base point is improved, thereby helping to improve the accuracy of the motion state of the target points, and thus improving the estimation accuracy of the motion state of the target based on the base point and the target points.

[0009] In combination with the first aspect, in some implementations of the first aspect, the plurality of frames of point cloud data comprises one or more of position measurement data, velocity measurement data or acceleration measurement data of the target.

[0010] In conjunction with the first aspect, in some implementations of the first aspect, the velocity measurement data includes one or more of the target's radial velocity measurement data, optical flow measurement data, or scene flow measurement data.

[0011] In conjunction with the first aspect, in some implementations of the first aspect, the relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between the base point of the rigid body and the mass point on the rigid body.

[0012] Based on the above technical solution, the relationship between the motion states of the base point and the target point can be determined by the motion relationship between the base point and the mass points on the rigid body. On the one hand, the motion state of the base point can be observed using the data from the target point, thereby increasing the observability of the base point's motion state and improving the estimation accuracy. On the other hand, based on the relationship between their motion states, the number of state variables to be estimated can be reduced, thereby reducing the dimension of the state vector to be estimated, thus reducing the complexity of the problem and improving the estimation accuracy. Therefore, based on the rigid body relationship between the base point and the target point, the computational complexity and amount of computation for the target's motion state can be reduced, while simultaneously improving the accuracy of the target's motion state estimation.

[0013] In conjunction with the first aspect, in some implementations of the first aspect, the kinematic relationship between the base point of the rigid body and the mass point on the rigid body includes a relationship between one or more of the following: position, angular velocity, linear velocity, or acceleration.

[0014] In some possible implementations, the kinematic relationship between the base point of the rigid body and the particles on the rigid body includes the relationship between position, angular velocity, and linear velocity.

[0015] In some possible implementations, the kinematic relationship between the base point of the rigid body and the particles on the rigid body includes the relationship between position, angular velocity and acceleration.

[0016] In conjunction with the first aspect, in some implementations of the first aspect, the relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between two mass points on a composite rigid body, the composite rigid body comprising at least two rigid bodies, the at least two rigid bodies comprising a main body of the target and at least one accessory, the main body and the at least one accessory being connected by a shaft.

[0017] For example, the attachment rotates about the axis or moves in a scissor-like motion.

[0018] Based on the above technical solution, the relationship between the motion states of the base point and the target point can be determined by the motion relationship between the two mass points of the composite rigid body. On the one hand, the motion state of the base point can be observed using the data from the target point, thereby increasing the observability of the base point's motion state and improving the estimation accuracy. On the other hand, based on the relationship between their motion states, the number of state variables to be estimated can be reduced, thereby lowering the dimensionality of the state vector to be estimated, reducing the complexity of the problem, and improving the estimation accuracy. Therefore, based on the relationship between the states of the base point and the target point, the computational complexity and amount of computation for estimating the target's motion state can be reduced, while simultaneously improving the accuracy of the target's motion state estimation.

[0019] In some possible implementations, taking a vehicle as an example, the main body can be the vehicle body and the accessory can be the wheel.

[0020] In some possible implementations, taking a drone or flying car as an example, the main body can be the fuselage and the accessory can be the rotor.

[0021] In some possible implementations, taking a vehicle as an example, the main body can be the front of the vehicle, and the accessory can be the passenger compartment.

[0022] In some possible implementations, taking a vehicle as an example, the main body can be the vehicle body and the accessory can be the door.

[0023] In some possible implementations, taking the composite rigid body as a trailer vehicle as an example, the main body can be the cab and the trailer, and the accessory can be the trailer.

[0024] In conjunction with the first aspect, in some implementations of the first aspect, the relationship between the motion state of the base point and the motion state of the target point is determined by the motion relationship between the target point and the axis point and the motion relationship between the base point and the axis point, wherein the axis point is a point located on the axis and the target point is located in the attachment.

[0025] In conjunction with the first aspect, in some implementations of the first aspect, the relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between two mass points on a composite rigid body. The composite rigid body includes at least two rigid bodies, which include a main body of the target and at least one accessory. The main body and the at least one accessory are connected by a contact point.

[0026] In conjunction with the first aspect, in some implementations of the first aspect, the relationship between the motion state of the base point and the motion state of the target point is determined by the motion relationship between the target point and the contact point and the motion relationship between the base point and the contact point, wherein the target point is located in the attachment.

[0027] In conjunction with the first aspect, in certain implementations of the first aspect, the relationship between the motion states of the base point in different frames includes: the base point having one or more of the following equal velocity, angular velocity, or acceleration between adjacent frames; and / or, the base point having one or more of the following equal average velocity, angular velocity, or acceleration between adjacent frames; and / or, the base point having equal height or average height between adjacent frames; and / or, the base point having an average vertical velocity or acceleration of 0 between adjacent frames; and / or, the base point having one or two angular velocity components of 0 between adjacent frames.

[0028] In conjunction with the first aspect, in some implementations of the first aspect, determining the motion state of the target includes: determining the motion state of the base point and / or the motion state of the target point;

[0029] In conjunction with the first aspect, in some implementations of the first aspect, the objective is an extended objective, which is an objective whose size spans across the sensor resolution unit.

[0030] Based on the above technical solution, by determining the motion state of the extended target through this method, the intention of the extended target to change lanes or overtake can be accurately judged, thereby effectively avoiding collision risks or requiring emergency takeover.

[0031] For example, the sensor may include, but is not limited to, radar or camera devices, and the radar may include, but is not limited to, lidar, ultrasonic radar or millimeter-wave radar.

[0032] In conjunction with the first aspect, in some implementations of the first aspect, the method further includes: determining one or more of the shape, dimension, or size of the target based on the positions of the base point and the target point.

[0033] In conjunction with the first aspect, in some implementations of the first aspect, determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: determining the relative position measurement relationship between the target point and the base point based on the relationship between the motion state of the base point and the motion state of the target point, wherein the relative position vector measurement value is determined by the position measurement value of the target point and the position of the base point.

[0034] In conjunction with the first aspect, in some implementations of the first aspect, determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: determining the radial velocity measurement relationship between the target point and the base point based on the relationship between the motion state of the base point and the motion state of the target point, wherein the radial velocity measurement relationship represents the relationship between the radial velocity of the target point and the linear velocity and angular velocity of the base point.

[0035] In conjunction with the first aspect, in some implementations of the first aspect, determining the radial velocity measurement relationship between the target point and the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: determining the radial velocity measurement relationship based on the following relationship: the radial velocity of the target point is the radial projection component of the velocity vector of the target point, and the velocity vector of the target point is obtained based on the velocity vector of the base point, the position vector of the target point relative to the base point, and the angular velocity.

[0036] In conjunction with the first aspect, in some implementations of the first aspect, determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: determining the radial acceleration measurement relationship between the target point and the base point based on the relationship between the motion state of the base point and the motion state of the target point; determining the radial acceleration measurement relationship between the target point and the base point includes determining the radial acceleration measurement relationship based on the following relationship: the radial acceleration of the target point is the radial projection component of the acceleration vector of the target point, and the velocity vector of the target point is obtained based on the acceleration vector of the base point, the position vector of the target point relative to the base point, and the angular velocity.

[0037] In conjunction with the first aspect, in some implementations of the first aspect, determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: determining the optical flow measurement relationship between the point cloud data of the target and the base point based on the relationship between the motion state of the base point and the motion state of the target point; determining the optical flow measurement relationship between the point cloud data of the target and the base point includes determining the optical flow measurement relationship based on the following relationship: the optical flow of the target point is the perspective projection of the velocity vector of the target point on the camera plane, and the velocity vector of the target point is obtained from the velocity vector of the base point, the position vector of the target point relative to the base point, and the angular velocity.

[0038] In conjunction with the first aspect, in some implementations of the first aspect, the method further includes: determining the position of the target point relative to the base point based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion states of the base point, and the correspondence between the measurement data of the target point in the multi-frame point cloud.

[0039] In conjunction with the first aspect, in some implementations of the first aspect, the method further includes: obtaining the position of the target point; and determining the position of the target point relative to the base point based on the position of the target point and the motion state of the base point.

[0040] In conjunction with the first aspect, in certain implementations of the first aspect, determining the motion state of the target based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of the target points in the multi-frame point cloud, includes:

[0041] Using the relationship between the motion states of the base point in different frames as the motion equation, the relationship between the point cloud data of the target and the motion state of the base point as the measurement equation, and the motion state of the base point and the position of the target point relative to the base point as the state vector, the motion state of the base point and the position of the target point relative to the base point are determined based on the Iterative Extended Kalman Filter (IEKF). In conjunction with the first aspect, in some implementations of the first aspect, determining the motion state of the target based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of the target point in the multi-frame point cloud, includes: determining an objective function based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of the target point; and optimizing the solution based on the objective function to obtain the motion state of the base point and / or the position of the target point relative to the base point.

[0042] Based on the above technical solution, the optimal or good solution obtained through the objective function determined by the above relationships yields the motion state of the base point and / or the position of the target point relative to the base point. On the one hand, this simplifies the target's motion state to the motion state of the base point and the target point's position relative to the base point, eliminating the need to calculate the state vectors of all target points, thus significantly reducing the computational complexity and workload. On the other hand, based on the optimization of the above objective function, the aforementioned data, state relationships, and correspondences can be fully utilized, greatly reducing the errors in estimating the motion state of the base point and the position of the target point. Therefore, the above solution not only reduces the complexity and computational workload of the problem but also improves the accuracy of target motion state estimation.

[0043] In conjunction with the first aspect, in some implementations of the first aspect, the optimization of the objective function to obtain the motion state of the base point and / or the position of the target point relative to the base point includes: establishing a factor graph based on the relationship between the base point, the target point, the motion state of the base point in different frames, and the relationship between the point cloud data of the target and the motion state of the base point; and optimizing the solution based on the factor graph to obtain the motion state of the base point and / or the position of the target point relative to the base point.

[0044] Based on the above technical solution, a factor graph is determined through the aforementioned relationships. The motion state of the base point and / or the position of the target point relative to the base point are then obtained through optimization based on the factor graph. On the one hand, the graph structure characteristics of the problem, particularly its sparsity, can be utilized to simplify the algorithm implementation and reduce computational complexity and volume. On the other hand, the optimal or good solution obtained by the graph optimization algorithm can effectively reduce the errors caused by the aforementioned relationships as constraints, thereby improving the accuracy of the motion state of the base point and / or the position of the target point relative to the base point. Therefore, the accuracy of the target's motion state estimation can be improved, thus contributing to enhanced carrier safety.

[0045] In conjunction with the first aspect, in some implementations of the first aspect, the factor graph includes multiple nodes and multiple edges connecting the nodes. The multiple nodes include the base point and the target point. The multiple edges include at least first-type edges and second-type edges. The first-type edges connect base points in different frames, corresponding to the relationship between the motion states of the base points in different frames. The second-type edges connect the target point and the base point, corresponding to the relationship between the point cloud data of the target and the motion state of the base point. The determination of the objective function based on the correspondence between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the measurement data of the target point includes: determining a state error term based on the relationship between the motion states of the base point corresponding to the edges of the factor graph in different frames; determining a measurement error term based on the relationship between the point cloud data of the target and the motion state of the base point corresponding to the edges of the factor graph; and determining the objective function corresponding to the factor graph based on the state error term and the measurement error term.

[0046] In conjunction with the first aspect, in some implementations of the first aspect, the motion state of the target includes one or more of the target point's position, velocity, angular velocity, and acceleration.

[0047] In conjunction with the first aspect, in some implementations of the first aspect, the base point is any point in the multi-frame point cloud; the base point is the point closest to the carrier; the base point is the centroid of multiple points in a certain frame of the multi-frame point cloud; or, the base point is an external input point.

[0048] In conjunction with the first aspect, in some implementations of the first aspect, the method further includes: determining the intention of the target based on the target's motion state; and controlling the vehicle's movement based on the target's intention.

[0049] In some possible implementations, the motion state of the target point can be used for the planning and control of the carrier.

[0050] Secondly, this application provides an apparatus for determining the motion state of a target, the apparatus including units or modules for implementing the methods in the first aspect and any possible implementation thereof.

[0051] Thirdly, this application provides an apparatus for determining the motion state of a target, the apparatus including a memory and a processor, the memory for storing a computer program and the processor for executing the computer program in the memory, such that the motion state estimation apparatus can implement the methods in the first aspect and any possible implementation thereof.

[0052] Fourthly, this application provides a system for determining the motion state of a target, the system comprising a sensing system and the apparatus described in the second or third aspect above.

[0053] Fifthly, this application provides a carrier that includes any of the devices possible in the second to third aspects above, or includes the system described in the fourth aspect above.

[0054] In conjunction with the fifth aspect, in some implementations of the fifth aspect, the carrier is a vehicle.

[0055] The term "vehicle" in this application is used in a broad sense and can refer to means of transportation (such as commercial vehicles, passenger cars, motorcycles, flying cars, trains, etc.), industrial vehicles (such as forklifts, trailers, tractors, etc.), engineering vehicles (such as excavators, bulldozers, cranes, etc.), agricultural equipment (such as lawnmowers, harvesters, etc.), amusement equipment, toy vehicles, etc. The embodiments of this application do not specifically limit the type of vehicle.

[0056] In a sixth aspect, this application provides a computer program product comprising: computer program code, which, when executed on a computer, causes the computer to perform the method in any possible implementation of the first aspect.

[0057] In a seventh aspect, this application provides a computer-readable storage medium storing a computer program that, when run on a computer, causes the computer to perform the method in any possible implementation of the first aspect.

[0058] Eighthly, this application provides a chip including circuitry for performing the method in any possible implementation of the first aspect described above. Attached Figure Description

[0059] Figure 1 This is a schematic diagram of a driving scenario and its extended objectives.

[0060] Figure 2 This is a functional block diagram of the vehicle provided in the embodiments of this application.

[0061] Figure 3 This is a schematic block diagram of the intelligent driving system provided in the embodiments of this application.

[0062] Figure 4 This is a schematic diagram of the extended target detection provided in the embodiments of this application.

[0063] Figure 5 This is a schematic flowchart illustrating a method for determining the motion state of a target, as provided in an embodiment of this application.

[0064] Figure 6 This is a schematic diagram of the base point and target point provided in the embodiments of this application.

[0065] Figure 7 This is a schematic diagram of the main body, accessories, base points, and target points in the composite rigid body provided in the embodiments of this application.

[0066] Figure 8 This is a schematic diagram of the main body, accessories, base points, and target points in another composite rigid body provided in the embodiments of this application.

[0067] Figure 9 This is a schematic diagram of the main body, accessories, base points, and target points in another composite rigid body provided in the embodiments of this application.

[0068] Figure 10 This is a schematic diagram of the main body, accessories, base points, and target points in another composite rigid body provided in the embodiments of this application.

[0069] Figure 11 This is a factor graph of an embodiment of this application.

[0070] Figure 12 This is a schematic flowchart illustrating the method for determining the motion state of a target provided in this application embodiment.

[0071] Figure 13 This is a schematic block diagram of the motion state estimation device provided in the embodiments of this application. Detailed Implementation

[0072] The technical solutions of the embodiments of this application will be described below with reference to the accompanying drawings. In the description of the embodiments of this application, unless otherwise stated, " / " means "or," for example, A / B can mean A or B; "and / or" in this document is merely a description of the association relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, and B existing alone. "At least one" refers to one or more. For example, "at least one of A and B," similar to "A and / or B," describes the association relationship between related objects, indicating that three relationships can exist. For example, at least one of A and B can represent: A existing alone, A and B existing simultaneously, and B existing alone.

[0073] The prefixes such as "first" and "second" used in this application embodiment are merely for distinguishing different descriptive objects and do not limit the position, order, priority, quantity, or content of the described objects. The use of ordinal numbers and other prefixes used to distinguish descriptive objects in this application embodiment does not constitute a limitation on the described objects. The description of the described objects is given in the claims or the context of the embodiments, and should not constitute unnecessary restrictions due to the use of such prefixes. Furthermore, in the description of this embodiment, unless otherwise stated, "multiple" means two or more.

[0074] Figure 2This is a functional block diagram of a vehicle 100 provided in an embodiment of this application. The vehicle 100 may include a perception system 110, a computing platform 120, and a display device 130. The perception system 110 may include environmental perception sensors, such as one or more sensors that sense or measure information about the environment surrounding the vehicle 100; it may also include body sensors that sense the vehicle's motion state. Environmental perception sensors, such as one or more of lidar, millimeter-wave radar, ultrasonic radar (sonar), and camera devices, can be used to acquire measurement information of stationary targets, obstacles, or moving targets in the environment. Body sensors can be used to determine the vehicle's pose and motion state, for example, an inertial measurement unit (IMU) and a satellite positioning system. The IMU may include an acceleration measurement unit and / or an angular velocity measurement unit and / or a magnetic measurement unit. The positioning system may be a global positioning system (GPS), a BeiDou system, or other positioning systems. The aforementioned environmental sensor data can also be fused with the body sensors to improve the estimation accuracy of the vehicle's pose and motion state; this is not limited here. What needs to be supported is that environmental perception sensors can be fixedly connected to the vehicle body. By using the vehicle body sensors to obtain the vehicle's pose and motion state, it can be used to compensate for the measurement or motion state of environmental targets obtained by the environmental perception sensors, thereby obtaining an estimate of targets or obstacles relative to the environment, such as the ground.

[0075] Some or all of the functions of vehicle 100 can be controlled by computing platform 120. Computing platform 120 may include one or more processors, such as processors 121 to 12n (n being a positive integer). A processor is a circuit with signal processing capabilities. In one implementation, the processor can be a circuit with instruction read and execute capabilities, such as a central processing unit (CPU), microprocessor, graphics processing unit (GPU) (which can be understood as a type of microprocessor), or digital signal processor (DSP). In another implementation, the processor can implement certain functions through the logical relationships of hardware circuits. These logical relationships are fixed or reconfigurable. For example, the processor may be a hardware circuit implemented using an application-specific integrated circuit (ASIC) or a programmable logic device (PLD), such as a field-programmable gate array (FPGA). In reconfigurable hardware circuits, the process of the processor loading a configuration document and configuring the hardware circuit can be understood as the process of the processor loading instructions to implement some or all of the functions of the aforementioned units. Furthermore, the processor can also be a hardware circuit designed for artificial intelligence, which can be understood as an ASIC, such as a neural network processing unit (NPU), tensor processing unit (TPU), deep learning processing unit (DPU), etc. In addition, the computing platform 120 may also include a memory for storing instructions. Some or all of the processors 121 to 12n can call the instructions in the memory to implement the corresponding functions.

[0076] The in-cabin display devices 130 are mainly divided into two categories: the first is the in-vehicle display screen; the second is the projection display screen, such as the head-up display (HUD). An in-vehicle display screen is a physical display screen and an important component of the in-vehicle infotainment system. Multiple displays can be installed in the cabin, such as the digital instrument cluster display, the central control screen, the display screen in front of the front passenger (also known as the front-seat passenger), the display screen in front of the left rear passenger, the display screen in front of the right rear passenger, and even the car window can be used as a display screen. A head-up display, also known as a head-up display system, is mainly used to display driving information such as speed and navigation on a display device in front of the driver (such as the windshield). This reduces the driver's eye-shift time, avoids pupil changes caused by eye-shifting, and improves driving safety and comfort. Examples of HUDs include combiner-HUD (C-HUD) systems, windshield-HUD (W-HUD) systems, and augmented reality HUD (AR-HUD) systems. It should be understood that HUDs can also evolve into other types of systems as technology progresses, and this application does not limit them. The motion state of the target obtained through the embodiments of this application, such as the target's position, speed, shape, or size, can be directly displayed to the user through the display device in the cockpit, or it can be displayed after processing, such as through 3D modeling or virtual reality, thereby improving the safety and comfort of driving.

[0077] The above description of the display device 130 uses an in-vehicle display screen and a projection display screen as examples, but the embodiments of this application are not limited thereto. For example, the display device 130 can also be a light display screen or a projection screen.

[0078] Optionally, the structure of the vehicle 100 described above is merely illustrative. In actual applications, various components of the vehicle 100 may be added or removed as needed.

[0079] Vehicle 100 may include an intelligent driving system, which may include an advanced driving assistant system (ADAS) and an autonomous driving system (ADS). The intelligent driving system uses various sensors on the vehicle (including but not limited to: lidar, millimeter-wave radar, camera devices, ultrasonic sensors, global positioning system, inertial measurement unit) to acquire information from the vehicle's surroundings, and analyzes and processes the acquired information to achieve functions such as obstacle perception, target recognition, vehicle positioning, path planning, and driver monitoring / alerts, thereby improving the safety, automation, and comfort of driving the vehicle.

[0080] For example, Figure 3 A schematic block diagram of an intelligent driving system provided in an embodiment of this application is shown. The intelligent driving system may include three functional modules: a perception module 210, a planning module 220, and a control module 230. The perception module 210 perceives the environment surrounding the vehicle through sensors and outputs corresponding perception data to the planning module 220. The planning module 220 obtains information such as road elements, stationary obstacles, or moving targets based on the information acquired by the perception module 210. Based on the vehicle's current position, information on road elements and stationary obstacles, and information on moving targets as described in this embodiment, the planning module 220 determines the physical connectivity of the vehicle from its current position to a sampling point. When the vehicle is physically connected to a sampling point, the planning module 220 plans the vehicle's trajectory to that sampling point. The planning module 220 can determine the vehicle's strategy space based on this trajectory. The planning module 220 can send this strategy space to the control module 230. The control module 230 can evaluate the strategy space in Euclidean space to make behavioral or interactive decisions for the vehicle.

[0081] The above-mentioned sensing module 210, planning module 220 and control module 230 can be located in the above-mentioned computing platform 120.

[0082] As mentioned earlier, vehicle-mounted radar is typically configured in intelligent driving systems to perceive information about the surrounding environment. A typical vehicle-mounted radar can provide measurement data of the target relative to the radar, including range, azimuth (or azimuth and elevation), radial velocity, Doppler frequency information, and radar cross-section (RCS). Among these, range and azimuth can also be referred to as the target's position.

[0083] For example, as the resolution of automotive radar improves, for a target relatively close to the radar, the radar will generate multiple measurement data points during a single scan. Such targets are typically called extended targets. Unlike point targets, extended targets span the resolution cells of multiple automotive radars.

[0084] For example, Figure 4 A schematic diagram of the extended target detection provided in an embodiment of this application is shown.

[0085] like Figure 4 As shown in (a), during the scanning process, the vehicle-mounted radar obtains multiple points on the target vehicle, where each point occupies a grid cell, and each grid cell corresponds to a resolution cell of the vehicle-mounted radar. Since the vehicle-mounted radar obtains multiple measurement data during the scanning process of the target vehicle, the target vehicle can be referred to as an extended target.

[0086] likeFigure 4 As shown in (b), the measurement data may include the distance r of each point on the target vehicle relative to the origin of the vehicle radar coordinate system, and the radial velocity of that point. Azimuth angle θ. Similarly, using 4D millimeter-wave radar or lidar, the target's 3D position (including range, azimuth, and elevation angle measurements) and radial velocity measurements can also be obtained; furthermore, radial acceleration information can be obtained using signal processing. The method by which the sensor obtains the above data is not limited here.

[0087] Obtaining the motion state and shape estimation of targets by extending target detection is crucial for intelligent driving systems. This is especially true for the target's speed and the position of key points, such as the speed (instantaneous velocity vector or angular velocity) relative to the nearest and farthest points of the vehicle, as well as lateral and longitudinal position information. This information can be provided to the planning or control modules to determine the target's lane-changing or cut-in intentions, thereby effectively avoiding collision risks or requiring emergency intervention, improving driving safety and comfort.

[0088] Currently, estimating the motion state of a target relies on the position of its Intermediate Caliper (ICR). Typically, in uniform or uniformly accelerated motion, the ICR is located at infinity, with an angular velocity close to zero. Therefore, this ICR-dependent estimation method can lead to unstable results. Furthermore, it is difficult to further improve the accuracy of ICR estimation using filtering methods.

[0089] In view of this, embodiments of this application provide a method, apparatus, and carrier for determining the motion state of a target. Based on measurement data provided by sensors, the motion state of the target can be estimated. This method does not require separate modeling of the target's shape or size, nor does it rely on the target's inter-vehicle collision detection (ICR), which helps improve the accuracy of the estimated motion state of the target. For example, taking a vehicle as the carrier, the vehicle can determine whether the target intends to change lanes or overtake by using the estimated motion state, thereby determining whether it needs to yield to the target. This helps avoid collisions caused by inaccurate estimation of the target's motion state, thus improving the user's driving safety and comfort.

[0090] The above description uses the example of a sensor (e.g., vehicle radar) located in a vehicle, but the embodiments of this application are not limited to this. For example, the sensor carrying platform can be other carriers, such as vehicle-mounted equipment, such as motorcycles or bicycles; or, the carrier can be airborne equipment, such as drones, helicopters, or jet aircraft; or, the carrier can be spaceborne equipment, such as satellites; or, the carrier can be an intelligent agent system, such as a robot system.

[0091] Figure 5 A schematic flowchart of a method 500 for determining the motion state of a target, provided in an embodiment of this application, is shown. This method 500 can be executed by the aforementioned carrier (e.g., vehicle 100), or by the aforementioned computing platform 120; or by a processor, chip, or circuit in the computing platform 120; or by the aforementioned intelligent driving system; or by the aforementioned perception module 210. The method 500 includes:

[0092] S510 acquires multi-frame point cloud data of the target from the sensor and the correspondence between the measurement data of the target points in the multi-frame point cloud;

[0093] Optionally, the sensor may be a millimeter-wave radar, lidar, ultrasonic radar (sonar), or imaging sensor, etc.

[0094] Optionally, the sensor can be one or more sensors of the same type, or a combination of multiple sensors of the same type or different types;

[0095] Alternatively, the target is an extended target, which is a target whose size spans across the sensor resolution unit.

[0096] Optionally, the target's point cloud data includes measurement data of multiple target points extending the target. The target's point cloud data can be obtained through methods such as clustering, segmentation, detection, or target recognition; the methods used for clustering, segmentation, detection, or recognition are not limited here. Optionally, the multi-frame point cloud data includes one or more of the following: position measurement data, velocity measurement data, or acceleration measurement data of multiple target points of the target.

[0097] Optionally, the velocity measurement data may include one or more of the following: radial velocity measurement data of the target, optical flow measurement data, or scene flow measurement data.

[0098] Optionally, taking this target as an extended target as an example, the perception module 210 can use multi-frame point cloud data to determine the state of one or more target points of the target, thereby obtaining the state of the extended target. The perception module 210 can establish the correspondence between the measurement data of target points in different frames of point clouds.

[0099] For example, the vehicle-mounted radar can acquire three frames of point cloud data. For instance, if the measurement data 'a' of a target point in the first frame of the point cloud and the measurement data 'b' of a target point in the second frame of the point cloud correspond to the same target point, then the perception module 210 can establish a correspondence between the measurement data 'a' and the measurement data 'b'. Similarly, if the measurement data 'b' of a target point and the measurement data 'c' of a target point in the third frame of the point cloud correspond to the same target point, then the perception module 210 can establish a correspondence between the measurement data 'a', the measurement data 'b', and the measurement data 'c'.

[0100] Optionally, the multi-frame point cloud data includes measurements of the target point in the multi-frame point cloud. For example, it includes the target point's location information and measurements of radial velocity and / or radial acceleration.

[0101] For example, the location information includes one or more of the following: the distance of the target point in the point cloud relative to the origin of the sensor coordinate system, the azimuth or pitch angle, or the depth of the target.

[0102] The correspondence between the measurement data of target points in the above multi-frame point cloud can be obtained using matching algorithms such as binary matching or graph matching algorithms, or it can be obtained based on machine learning or deep learning methods. No limitation is made here.

[0103] S520, determine the base point of the target and, based on the relationship between the motion state of the base point and the motion state of the target point, determine the relationship between the point cloud data of the target and the motion state of the base point.

[0104] For example, any point in a multi-frame point cloud can be selected as the base point.

[0105] For example, the centroids of multiple points in a point cloud in a certain frame can be selected as the base points.

[0106] For example, the point closest to the vehicle can be selected as the base point. For instance, a point at the rear of the target vehicle (e.g., the target vehicle is located in front of the vehicle).

[0107] For example, an external input point (e.g., a prediction point) can be selected as the base point.

[0108] For example, consider a point at the rear of the target vehicle as the base point. The position of this base point may be fixed relative to the target vehicle, but relative to the geodetic coordinate system, the state of the base point can evolve with the movement of the target in each frame.

[0109] The multiple point clouds described above can all use a common base point, or they can use different base points. For example, every three point clouds can share a base point, where the position and motion state of the base point can evolve over time. The following embodiments illustrate this using multiple frames sharing a single base point.

[0110] For example, Figure 6 A schematic diagram of the base point and target point provided in an embodiment of this application is shown. For example... Figure 6 As shown, o o Let O be the origin of the sensor coordinate system, and let P be the base point. j and P i To expand the target points on the target, O and P j The position vector between them is d j O and P j The position vector between them is d i O O and P j The position vector between them is r j O o and P i The position vector between them is r i .

[0111] Optionally, the relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between the base point of the rigid body and the mass point on the rigid body.

[0112] Optionally, the kinematic relationship between the base point of the rigid body and the particles on the rigid body includes the relationship between one or more of the following: position, angular velocity, linear velocity, or acceleration.

[0113] Optionally, the relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between two mass points on the composite rigid body. The composite rigid body includes at least two rigid bodies, which include a main body of the target and at least one accessory. The main body and the at least one accessory are connected by a shaft or a hinge.

[0114] Optionally, the base point is located on the main body of the target, and the target point is located on the attachment. The kinematic relationship between the two mass points on the composite rigid body includes the relationship between one or more of the following: position, angular velocity, linear velocity, or acceleration.

[0115] Optionally, determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: determining the relative position measurement relationship between the target point and the base point based on the relationship between the motion state of the base point and the motion state of the target point, wherein the relative position vector measurement value is determined by the position measurement value of the target point and the position of the base point.

[0116] The relative position measurement relationship between the target point and the base point can also be expressed by the position measurement equation of the target point relative to the base point. Optionally, the relative position measurement relationship of the target point relative to the base point can be called the position measurement equation, as shown in equation (1):

[0117] p i =d i +n p,i , i=1,…,N (1)

[0118] Where, d i p is the displacement vector or position vector of target point i relative to the base point. i For measurement data of displacement vector or position vector, n p,i For location measurement noise, optionally, its mean is 0 and its covariance is R. p,i p i The relative position measurement data of the target point with respect to the base point, where N is the number of target points.

[0119] Taking three dimensions as an example, d i Includes d i,x ,d i,y and d i,z The three coordinate components can be represented as shown in equation (2).

[0120] d i =[d i,x d i,y d i,z ] T (2)

[0121] For example, the relative position measurement data can be obtained according to the following formula (3).

[0122]

[0123] in For position measurement data obtained from sensors, The base point location data can be obtained from sensors, predicted, or input from external sources.

[0124] Optionally, the relationship between the point cloud data of the target and the motion state of the base point is determined based on the relationship between the motion state of the base point and the motion state of the target point, including: determining the radial velocity measurement relationship between the target point and the base point based on the relationship between the motion state of the base point and the motion state of the target point, wherein the radial velocity measurement relationship represents the relationship between the radial velocity measurement data of the target point and the linear velocity and angular velocity of the base point.

[0125] Optionally, determining the radial velocity measurement relationship between the target point and the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: determining the radial velocity measurement relationship based on the following relationship: the radial velocity of the target point is the radial projection component of the target point's velocity vector, and the target point's velocity vector is obtained based on the velocity vector of the base point, the position vector of the target point relative to the base point, and the angular velocity. The above radial velocity measurement relationship between the target point and the base point can be expressed by the radial velocity measurement equation between the target point and the base point.

[0126] Optionally, determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: determining the radial acceleration measurement relationship between the target point and the base point based on the relationship between the motion state of the base point and the motion state of the target point; determining the radial acceleration measurement relationship between the target point and the base point includes determining the radial acceleration measurement relationship based on the following relationship: the radial acceleration of the target point is the radial projection component of the acceleration vector of the target point, and the velocity vector of the target point is obtained based on the acceleration vector of the base point, the position vector of the target point relative to the base point, and the angular velocity.

[0127] Optionally, determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: determining the optical flow and / or scene flow measurement relationship between the point cloud data of the target and the base point based on the relationship between the motion state of the base point and the motion state of the target point.

[0128] Determining the optical flow measurement relationship between the point cloud data of the target and the base point includes determining the optical flow measurement relationship based on the following relationship: the optical flow of the target point is the perspective projection of the velocity vector of the target point onto the camera plane, and the velocity vector of the target point is obtained from the velocity vector of the base point, the position vector of the target point relative to the base point, and the angular velocity. Alternatively...

[0129] The determination of the scene flow measurement relationship between the point cloud data of the target and the base point includes determining the scene flow measurement relationship based on the following relationship: the scene flow of the target point is the velocity vector or projection component of the target point, and the velocity vector of the target point is obtained from the velocity vector of the base point, the position vector of the target point relative to the base point, and the angular velocity.

[0130] For example, the relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between the base point of the rigid body and the mass point on the rigid body. Determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: determining the relative position measurement relationship between the target point and the base point based on the motion relationship between the base point of the rigid body and the mass point on the rigid body, wherein the relative position vector measurement value is determined by the position measurement value of the target point and the position of the base point.

[0131] For example, the motion relationship between the base point of the rigid body and the mass point on the rigid body includes the relative positions remaining unchanged between at least two frames.

[0132] Specifically, the relative position measurement relationship between the target point and the base point in frame s and frame t can be called the position measurement equation, as shown in equation (4):

[0133] p i,k =d i +n p,i,k i = 1, ..., N k ,k=s,t (4)

[0134] Where, d i p is the displacement vector or position vector of target point i relative to the base point. i,k For the displacement or position vector measurement data in frame s, n p,i,k The noise measured at the position of the k-th frame can optionally have a mean of 0 and a covariance of R. p,i,k Where k = s, t, the s-th frame and the t-th frame can be adjacent, in which case t = s + 1; the s-th frame and the t-th frame can be adjacent but not adjacent, in which case t > s + 1.

[0135] For example, the relative position measurement data can be obtained from the target point position measurement data obtained from the sensor and the base point position, wherein the base point position can be obtained from the sensor, predicted, or input from an external source.

[0136] In this embodiment, the relationship between the motion state of the base point and the motion state of the target point is utilized. That is, according to the rigid body relationship, the relative position of the target point relative to the base point remains unchanged between at least two frames. Therefore, multiple frame position measurement data can be used to obtain multiple measurement data of the relative position of the target point relative to the base point. Thus, the accuracy of relative position estimation can be effectively improved by using the multiple measurement data.

[0137] For example, the relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between the base point of the rigid body and the mass on the rigid body. Determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target includes: determining the radial velocity measurement relationship between the target point and the base point based on the motion relationship between the base point of the rigid body and the mass on the rigid body, wherein the radial velocity measurement relationship represents the relationship between the radial velocity measurement data of the target point and the linear velocity and angular velocity of the base point.

[0138] For example, the kinematic relationship between the base point of the rigid body and the mass point on the rigid body includes the relationship between position, angular velocity and linear velocity.

[0139] For example, based on the motion relationship between the base point of the rigid body and the mass point on the rigid body, the relationship between the velocity and angular velocity of the base point and the linear velocity of the target point and the relative position of the target point with respect to the base point can be determined as shown in formula (5A):

[0140] v i =v base +ω×d i (5A)

[0141] Alternatively, it can be equivalently represented as shown in formula (5B):

[0142] v i =v base -d i ×ω (5B)

[0143] Where, d i v is the displacement vector or position vector of target point i relative to the base point. i v is the linear velocity vector of the target point. base Let ω be the velocity vector of the base point, and ω be the angular velocity of the base point. It should be noted that, as a rigid body, the target point and the base point have the same angular velocity here.

[0144] For example, based on the motion relationship between the base point of the rigid body and the mass point on the rigid body, the radial velocity measurement relationship between the target point and the base point can be determined as shown in equation (6):

[0145]

[0146] in, The radial velocity measurement value, Λ i Let d be the direction cosine vector. i v is the relative position vector or displacement vector of target point i relative to the base point. base Let ω be the velocity of the base point, and ω be the angular velocity of the base point. The radial velocity is used to measure noise. The velocity of the target point and the velocity and angular velocity of the base point are shown in equation (5B) above. The radial velocity is the radial component of the velocity of the target point, which can be expressed as Λ i v i .

[0147] Equivalently, exemplarily, the radial velocity measurement relationship between the target point and the base point can be called the radial velocity measurement equation, as shown in formula (7):

[0148]

[0149] in, Indicates the relationship with d i The corresponding antisymmetric matrix, for the three-dimensional case, It can be shown in equation (8)

[0150] Where d i =[d i,x d i,y d i,z ] T ,d i,x ,d i,y and d i,z For d i The three coordinate components.

[0151] Direction cosine vector Λ i It can be represented as Λ i =[Λ i,x Λ i,y Λ i,z ], where each component can be represented as in equation (9):

[0152]

[0153] in, and θ i These are the elevation and azimuth angles of target point i, respectively.

[0154] Alternatively, it can be equivalently represented as shown in equation (10):

[0155]

[0156] Where, x i ,y i ,z i and r i These are the three rectangular coordinate components and the distance to the target point i.

[0157] In the two-dimensional case, it can be further simplified, especially the angular velocity is simplified to a one-dimensional velocity component, and the other components are 0.

[0158] As one possible implementation, for a moving target in a two-dimensional plane, it can be simplified based on the following formulas (11) and (12):

[0159] Λ i =[Λ i,x Λ i,y 0] (11)

[0160] ω=[0 0 ω z ] T (12)

[0161] At this moment, the component of the angular velocity ω is ω x =ω y =0. By utilizing the above relationship, the number of state variables to be estimated can be reduced, thereby reducing the complexity of the problem and significantly reducing the amount of computation.

[0162] In this embodiment, the relationship between the motion state of the base point and the motion state of the target point—that is, the relationship between the linear velocity of the target point and the linear velocity, angular velocity, and relative position of the base point according to rigid body relations—can be used to establish the relationship between the radial velocity measurement data of the target point and the linear velocity, angular velocity, and relative position of the base point. Therefore, multiple measurements of the linear velocity, angular velocity, and relative position of the base point can be obtained using the radial velocity measurement data of one or more target points in multiple frames of point cloud data. This multi-frame point cloud data can effectively improve the accuracy of the motion state estimation of the base point, and the aforementioned relationship can further improve the accuracy of the motion state estimation of the target point.

[0163] For example, the relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between the base point of the rigid body and the mass on the rigid body. Determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: determining the radial acceleration measurement relationship between the target point and the base point based on the motion relationship between the base point of the rigid body and the mass on the rigid body, wherein the radial acceleration measurement relationship represents the relationship between the radial velocity measurement data of the target point and the linear acceleration, angular velocity, and / or relative position of the base point.

[0164] For example, the kinematic relationship between the base point of the rigid body and the mass point on the rigid body includes the relationship between position, angular velocity and acceleration.

[0165] For example, based on the motion relationship between the base point of the rigid body and the mass point on the rigid body, the relationship between the acceleration and angular velocity of the base point and the acceleration of the target point and the relative position of the target point with respect to the base point can be determined as shown in formula (13A):

[0166] a i=a base +ω×(ω×d i (13A)

[0167] Alternatively, it can be represented as shown in formula (13B):

[0168] a i =a base +(d i ×ω)×ω (13B)

[0169] Where, d i Let a be the displacement vector or position vector of target point i relative to the base point. i Let a be the acceleration vector of the target point. base Let ω be the acceleration vector at the target point, and ω be the angular velocity at the base point. It should be noted that, considering the actual motion capabilities of vehicles, drones, and other carriers, the rate of change of angular velocity here is usually negligible.

[0170] For example, based on the motion relationship between the base point of the rigid body and the mass point on the rigid body, the radial acceleration measurement relationship between the target point and the base point can be called the radial acceleration measurement equation, as determined by equation (14):

[0171]

[0172] in, The radial acceleration measurement value is Λ. i Let d be the direction cosine vector. i Let a be the relative position vector or displacement vector of target point i with respect to the base point. base Let ω be the acceleration at the base point and ω be the angular velocity at the base point. The radial acceleration is used to measure noise. The acceleration of the target point and the acceleration and angular velocity of the base point are shown in equation (13B) above. The radial acceleration is the radial component of the acceleration of the target point, i.e., Λ. i a i The direction cosine vector and angular velocity are as described above. Indicates the relationship with d i The antisymmetric matrix corresponding to ×ω.

[0173] In this embodiment, the relationship between the motion state of the base point and the motion state of the target point—that is, the relationship between the acceleration of the target point and the acceleration, angular velocity, and relative position of the base point according to the rigid body relationship—can be used to establish the relationship between the radial acceleration measurement data of the target point and the acceleration, angular velocity, and relative position of the base point. Therefore, multiple measurements of the acceleration, angular velocity, and relative position of the base point can be obtained using radial acceleration measurement data of one or more target points from multiple frames of point cloud data. This multi-frame point cloud data can effectively improve the accuracy of the motion state estimation of the base point, and the aforementioned relationship can further improve the accuracy of the motion state estimation of the target point.

[0174] For example, the relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between the base point of the rigid body and the mass points on the rigid body. Determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target includes: determining the optical flow or scene flow measurement relationship between the target point and the base point based on the motion relationship between the base point of the rigid body and the mass points on the rigid body, wherein the optical flow or scene flow measurement relationship represents the relationship between the optical flow or scene flow measurement data of the target point and the linear velocity and / or acceleration and angular velocity of the base point.

[0175] For example, the kinematic relationship between the base point of the rigid body and the mass point on the rigid body includes the relationship between position, angular velocity and linear velocity.

[0176] For example, the motion relationship between the base point of the rigid body and the mass point on the rigid body is shown in (5A) or (5B).

[0177] For example, based on the motion relationship between the base point of the rigid body and the mass point on the rigid body, the optical flow or scene flow measurement data between the target point and the base point can be obtained by using the motion relationship between the base point of the rigid body and the mass point on the rigid body, and thus obtaining the relationship between the optical flow or scene flow measurement data of the target point and the linear velocity and / or acceleration and angular velocity of the base point.

[0178] In this embodiment, the relationship between the motion state of the base point and the motion state of the target point—that is, the relationship between the linear velocity of the target point and the linear velocity and / or acceleration, angular velocity, and relative position of the base point according to rigid body relations—can be used to establish the relationship between the optical flow or scene flow measurement data of the target point and the linear velocity and / or acceleration, angular velocity, and relative position of the base point. Therefore, multiple measurements of the linear velocity and / or acceleration, angular velocity, and relative position of the base point can be obtained using optical flow or scene flow measurement data of one or more target points in multiple frames of point cloud data. This multi-frame point cloud data can effectively improve the accuracy of motion state estimation of the base point, and the aforementioned relationship can further improve the accuracy of motion state estimation of the target point.

[0179] Optionally, the relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between two mass points on the composite rigid body. The composite rigid body includes at least two rigid bodies, which include a main body of the target and at least one accessory. The main body and the at least one accessory are connected by a shaft.

[0180] For example, the attachment rotates about the axis or moves in a scissor-like motion.

[0181] For example, taking a vehicle as an example, the main body can be the vehicle body, and the accessory can be a wheel, such as... Figure 7 As shown.

[0182] For example, if the composite rigid body is a drone or a flying car, the main body can be the fuselage and the accessory can be the rotor.

[0183] For example, taking a vehicle as an example, the main body can be the front of the vehicle, and the accessory can be the passenger compartment, such as... Figure 8 As shown.

[0184] For example, taking a vehicle as an example, the main body can be the vehicle body, and the accessory can be a door, such as... Figure 9 As shown.

[0185] For example, if the composite rigid body is a trailer vehicle, the main body can be the cab and the trailer, and the accessory can be the trailer.

[0186] For example, taking a concrete mixer truck as an example, the main body can be the vehicle body, and the accessory can be the mixing rotor, such as... Figure 10 As shown.

[0187] Optionally, the relationship between the motion state of the base point and the motion state of the target point is determined by the motion state relationship between the target point and the axis point and the motion state relationship between the base point and the axis point, wherein the axis point is a point located on the axis.

[0188] For example, the base point is located on the body of the target, and the target point is located on the periphery of the target.

[0189] For example, the motion of the target point relative to the axis can be rotational motion, or a scissor-like motion, or rotation around the axis while translating together.

[0190] Optionally, the motion relationship between the target point and the axis point includes the relationship between one or more of the following: position, angular velocity, linear velocity, or acceleration.

[0191] Optionally, the relationship between the motion state of the base point and the axis point includes the relationship between one or more of the following: position, angular velocity, linear velocity, or acceleration.

[0192] Taking ground-based vehicles as an example, Figure 7 This illustration shows a schematic diagram of the main body, attachments, base points, and target points in a composite rigid body provided in an embodiment of this application. (See attached diagram.) Figure 7 As shown, the target is a vehicle, with wheels and the vehicle body forming a composite rigid body. The wheels, as attachments, are connected to the vehicle body via axles. Target point 1 is located on the wheel, i.e., attachment 1, and the base point is located on the vehicle body near the rear axle. In this example, axle point 1 is located at the center of rotation of wheel 1, and simultaneously, axle point 1 becomes part of the vehicle body. Axle point 2 is located at the center of rotation of wheel 2, and simultaneously, axle point 2 becomes part of the vehicle body.

[0193] Optionally, the motion relationship between the target point and the axis point is determined according to the motion relationship between the base point of the rigid body and the mass point on the rigid body.

[0194] Based on the relationship between the motion state of the axis point and the motion state of the target point, the relationship between the point cloud data of the target and the motion state of the axis point can be determined.

[0195] For example, the relative position measurement relationship between the target point and the axis point is determined based on the motion relationship between the base point of the rigid body and the mass point on the rigid body.

[0196] For example, the relative position measurement relationship of the target point in the appendix with respect to the axis point, or the position measurement equation, is shown in equation (15):

[0197] p app =d app +n p,app (15)

[0198] Where, d app p is the displacement vector or position vector of the target point relative to the axis point. app For displacement vector or position vector measurement data, n p,app The noise for the location measurement can optionally have a mean of 0 and a covariance of R. p,app .

[0199] For example, based on the motion relationship between the base point of the rigid body and the mass point on the rigid body, the radial velocity measurement relationship between the target point and the axis point can be determined, as shown in (16).

[0200]

[0201] in, Λ is the measured radial velocity of target i. app,i Let d be the direction cosine vector. app,i v is the relative position vector or displacement vector of target point i with respect to the axis point. axle Let ω be the velocity of the pivot point. axle The angular velocity of the pivot point. Noise for radial velocity measurement. Velocity v at target point i. app,i The velocity and angular velocity of the pivot point, and the relative position d of the target point with respect to the pivot point. app,i The relationship is shown in the following formula (17A):

[0202] v app,i =v axle +ω axle ×d app,i (17A)

[0203] Alternatively, it can be represented as shown in formula (17B):

[0204] v app,i =v axle -d app,i ×ω axle (17B)

[0205] For example, based on the motion relationship between the base point of the rigid body and the mass point on the rigid body, the radial acceleration measurement relationship between the target point and the axis point can be determined, which can be called the radial acceleration measurement equation, as shown in (18).

[0206]

[0207] in, The radial acceleration measurement value for target point i. Noise is measured for radial acceleration. The acceleration a at target point i. app,i acceleration a at the axis axle angular velocity ω axle The relative position d of the target point with respect to the pivot point app,i The relationship is shown in the following formula (19A):

[0208] a app,i =a axle +ω axle ×(ωaxle ×d app,i (19A)

[0209] Alternatively, it can be represented as shown in formula (19B):

[0210] a app,i =a axle +(d app,i ×ω axle )×ω axle (19B)

[0211] For example, based on the motion relationship between the base point of the rigid body and the mass points on that rigid body, the optical flow or scene flow measurement relationship between the target point and the pivot point can be determined. For example, the optical flow of the target point is the perspective projection of the target point's velocity vector onto the camera plane, and the target point's velocity vector is obtained from the velocity vector of the base point, the target point's position vector relative to the base point, and its angular velocity. Alternatively, for example, the scene flow of the target point is the target point's velocity vector or its projection component, and the target point's velocity vector is obtained from the velocity vector of the base point, the target point's position vector relative to the base point, and its angular velocity.

[0212] Optionally, the relationship between the motion state of the base point and the axis point is determined according to the relationship between the motion of the base point of the rigid body and the mass point on the rigid body.

[0213] For example, based on the motion relationship between the base point of the rigid body and the mass points on the rigid body, the relative position vector of the axis point relative to the base point can be determined as follows:

[0214] p axle,k =d axle ,k=s,t

[0215] Where d axle p is the relative position vector of the axis point with respect to the base point. axle,k It represents the relative position vector of the axis point with respect to the base point in the k-th frame, where t ≥ s + 1, and t and s represent different frames.

[0216] In this embodiment, the relationship between the motion state of the base point and the motion state of the target point is utilized. That is, the relative position relationship between the target point and the axis point and the relative position between the axis point and the base point in the composite rigid body relationship remain unchanged between at least two frames. Therefore, multiple frame position measurement data can be used to obtain multiple measurement data of the relative position of the axis point relative to the base point. Thus, the accuracy of the axis point position estimation can be effectively improved by using the multiple measurement data.

[0217] For example, based on the motion relationship between the base point of the rigid body and the particle on the rigid body, the velocity v of the base point on the body can be determined. base Angular velocity ω and velocity v of the axis pointaxle The relative position d of the pivot point with respect to the base point axle The relationship can be shown in formula (20A):

[0218] v axle =v base +ω×d axle (20A)

[0219] Alternatively, it can be represented as shown in formula (20B):

[0220] v axle =v base -d axle ×ω (20B)

[0221] For example, the acceleration 'a' of the base point can be determined based on the motion relationship between the base point of the rigid body and the particles on the rigid body. base Angular velocity ω and acceleration a at that axis point axle The relative position d of the pivot point with respect to the base point axle The relationship is shown in formula (21A):

[0222] a axle =a base +ω×(ω×d axle (21A)

[0223] Alternatively, it can be represented as shown in formula (21B):

[0224] a axle =a base +(d axle ×ω)×ω (21B)

[0225] As one implementation method, the radial velocity measurement relationship between the target point and the base point can be determined, which can be called the radial velocity measurement equation, as shown in (22).

[0226]

[0227] In this embodiment, the relationship between the motion state of the base point and the motion state of the target point is utilized. Specifically, the relationship between the velocity of the target point and the velocity and angular velocity of the axis point, and the relationship between the velocity of the axis point and the linear velocity, angular velocity, and relative position of the base point, based on the composite rigid body relationship, can establish the relationship between the radial velocity measurement data of the target point and the linear velocity, angular velocity, and relative position of the base point. Therefore, multiple measurements of the linear velocity, angular velocity, and relative position of the base point can be obtained using the radial velocity measurement data of one or more target points in multiple frames of point cloud data. This multi-frame point cloud data can effectively improve the accuracy of the motion state estimation of the base point. Furthermore, the aforementioned relationship can be used to further improve the accuracy of the motion state estimation of the target point.

[0228] As one implementation method, the radial acceleration measurement relationship between the target point and the base point can be determined, which can be called the radial acceleration measurement equation, as shown in (23).

[0229]

[0230] Where a axle Given by (21A) or (21B).

[0231] In this embodiment, the relationship between the motion state of the base point and the motion state of the target point is utilized. Specifically, the relationship between the target point's acceleration and the acceleration and angular velocity of the axis point, and between the axis point's acceleration and the base point's acceleration, angular velocity, and relative position, based on the complex rigid body relationship, can establish the relationship between the radial acceleration measurement data of the target point and the acceleration, angular velocity, and relative position of the base point. Therefore, multiple measurements of the base point's acceleration, angular velocity, and relative position can be obtained using radial acceleration measurement data of one or more target points from multiple frames of point cloud data. This multi-frame point cloud data effectively improves the accuracy of the base point's motion state estimation, and the aforementioned relationship further enhances the accuracy of the target point's motion state estimation.

[0232] As another implementation, based on the embodiments of this application, the motion state of the pivot point can be obtained according to one or more of the relative position measurement relationship, radial velocity measurement relationship, radial acceleration measurement relationship, and optical flow or scene flow measurement relationship shown in formulas (15)-(19B). The motion state of the pivot point can be one or more of the estimated position, estimated velocity, estimated angular velocity, and estimated acceleration of the pivot point.

[0233] Based on the rigid body motion relationship between the axis point and the base point, and based on one or more of the above-mentioned position estimate, velocity estimate, angular velocity estimate, and acceleration estimate of the axis point, determine one or more of the relative position measurement relationship, velocity vector measurement relationship, and angular and acceleration measurement relationship between the axis point and the base point.

[0234] For example, based on the motion relationship between the base point of the rigid body and the mass point on the rigid body, the relative position measurement relationship of the axis point with respect to the base point can be determined as shown in formula (24):

[0235]

[0236] Where d axle This is the relative position vector of the axis point with respect to the base point. This represents the estimated relative position vector of the point on that axis with respect to the base point in the k-th frame. The vector estimation error of the relative position of the axis point with respect to the base point in the k-th frame is denoted by t ≥ s + 1, where t and s represent different frames.

[0237] In this embodiment, the relationship between the motion state of the base point and the motion state of the axis point is utilized, that is, the relative position of the axis point relative to the base point remains unchanged between at least two frames according to the rigid body relationship. Therefore, multiple frame position measurement data can be used to obtain multiple measurement data of the relative position of the axis point relative to the base point. Thus, the accuracy of relative position estimation can be effectively improved by using the multiple measurement data.

[0238] For example, based on the motion relationship between the base point of the rigid body and the mass point on the rigid body, the velocity vector measurement relationship of the axis point relative to the base point can be determined, which can be called the velocity vector measurement equation, as shown in formula (25A):

[0239]

[0240] Alternatively, it can be equivalently represented as shown in formula (25B):

[0241]

[0242] in This is the estimated velocity value for that axis point. This indicates the velocity vector estimation error at that axis point.

[0243] In this embodiment, the relationship between the motion state of the base point and the motion state of the axis point—that is, the relationship between the velocity of the axis point and the linear velocity, angular velocity, and relative position of the base point according to rigid body relations—can be used to establish the relationship between the estimated velocity of the axis point and the linear velocity, angular velocity, and relative position of the base point. Therefore, multiple measurements of the linear velocity, angular velocity, and relative position of the base point can be obtained using the velocity estimates of one or more axis points from multiple frames of point cloud data. This multi-frame point cloud data can effectively improve the accuracy of the motion state estimation of the base point. Furthermore, the aforementioned relationship can be used to further improve the accuracy of the motion state estimation of the target point.

[0244] For example, based on the motion relationship between the base point of the rigid body and the mass point on the rigid body, the acceleration vector measurement relationship of the axis point relative to the base point can be determined, which can be called the acceleration vector measurement equation, as shown in formula (26A):

[0245]

[0246] Alternatively, it can be equivalently represented as shown in formula (26B):

[0247]

[0248] in This is the estimated acceleration value for that axis point. This represents the acceleration vector estimation error at that axis point.

[0249] In this embodiment, the relationship between the motion state of the base point and the motion state of the axis point—that is, the relationship between the acceleration of the axis point and the acceleration, angular velocity, and relative position of the base point according to rigid body relations—can be used to establish a relationship between the estimated acceleration value of the axis point and the acceleration, angular velocity, and relative position of the base point. Therefore, multiple measurements of the acceleration, angular velocity, and relative position of the base point can be obtained using the acceleration estimates of one or more axis points from multiple frames of point cloud data. This multi-frame point cloud data can effectively improve the accuracy of the motion state estimation of the base point. Furthermore, the aforementioned relationship can be used to further improve the accuracy of the motion state estimation of the target point.

[0250] Optionally, the relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between two mass points on the composite rigid body. The composite rigid body includes at least two rigid bodies, which include a main body of the target and at least one accessory. The main body and the at least one accessory are connected by a contact point.

[0251] Optionally, the relationship between the motion state of the base point and the motion state of the target point is determined by the motion state relationship between the target point and the contact point, as well as the motion state relationship between the base point and the contact point, with the target point located in the attachment.

[0252] Optionally, similar to the axis points mentioned above, by utilizing the rigid body relationship between the contact point and the target point, the measurement data of the target point, and the rigid body relationship between the contact point and the base point, one or more of the following can be determined: the positional relationship between the target point and the base point, the radial velocity measurement relationship, the radial acceleration measurement relationship, and the optical flow or scene flow measurement relationship.

[0253] In this embodiment, the relationship between the motion state of the base point and the motion state of the target point is established using the relationship between the rigid body relationship between the target point and the axis point, and the rigid body relationship between the axis point and the base point, based on the composite rigid body relationship. Therefore, multiple frames of point cloud measurement data can be used to obtain multiple measurements of the linear velocity, angular velocity, and relative position of the base point, effectively improving the accuracy of the motion state estimation of the base point. Simultaneously, the aforementioned relationship can be used to further improve the accuracy of the target point's state estimation.

[0254] Optionally, similar to the aforementioned pivot point, the motion state estimate of the contact point can be determined using the rigid body relationship between the contact point and the target point, and the measurement data of the target point. For example, the motion state estimate may include one or more of the following: position vector estimate, velocity vector estimate, and acceleration vector estimate. Based on the rigid body relationship between the contact point and the base point, and based on the aforementioned motion state estimate of the contact point, the motion state measurement relationship between the contact point and the base point can be determined. For example, the motion state measurement relationship may include one or more of the following: position vector measurement relationship, velocity vector measurement relationship, angular velocity vector measurement relationship, and acceleration vector measurement relationship.

[0255] It should be noted that, considering the motion of the carrier or sensor, as a natural extension of the embodiments of this application, in the relationship between the motion states of the above-mentioned base point or target point in different frames or the measurement relationship between the target point and the base point, motion compensation of the sensor or vehicle body can be further introduced, such as the position, attitude (rotation and translation), speed and angular velocity of the carrier.

[0256] In this embodiment, the state estimate of the axis point is obtained from the target point using rigid body relationships. The relationship between the motion state of the base point and the rigid body motion state of the axis point is used to establish a measurement relationship between the motion state estimate of the axis point and the motion state of the base point. Therefore, multiple frames of point cloud data can be used to obtain multiple measurements of the linear velocity, angular velocity, and relative position of the base point, thereby effectively improving the accuracy of the motion state estimation of the base point. Simultaneously, the aforementioned relationship can be used to further improve the state estimation accuracy of the target point.

[0257] S530, based on the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of the target points in the multi-frame point cloud, the motion state of the target is determined.

[0258] Optionally, determining the motion state of the target includes: determining the motion state of the base point and / or the motion state of the target point.

[0259] For example, the motion state of the target includes one or more of the position, velocity, angular velocity, and acceleration of the base point and / or the target point.

[0260] Optionally, the method 500 further includes: determining the relationship between the motion states of the base point in different frames; wherein, determining the motion state of the target based on the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of the target points in the multi-frame point cloud, includes: determining the motion state of the target based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of the target points in the multi-frame point cloud.

[0261] Optionally, the method 500 further includes: determining the position of the target point relative to the base point based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion states of the base point, and the correspondence between the measurement data of the target point in the multi-frame point cloud.

[0262] Optionally, the relationship between the motion states of the base point in different frames includes: the base point having one or more of the following: equal velocity, angular velocity, or acceleration between adjacent frames; and / or, the base point having one or more of the following: equal average velocity, average angular velocity, or average acceleration between adjacent frames; and / or, the base point having equal height or average height between adjacent frames; and / or, the base point having a vertical velocity or average acceleration of 0 between adjacent frames; and / or, one or two angular velocity components of the base point being 0 between adjacent frames.

[0263] For example, the motion state of the base point may include one or more of the following: the base point's position, linear velocity, acceleration, and angular velocity.

[0264] The relationship between the motion states of the above base point in different frames can also be represented by state equations.

[0265] For example, the base point has equal velocity, angular velocity and height between adjacent frames or multiple frames, or has equal average velocity, average angular velocity and average height.

[0266] For example, the base point has equal acceleration, angular velocity and height between adjacent frames or multiple frames, or has equal mean acceleration, mean angular velocity and mean height.

[0267] For example, the motion of the target can be modeled in a 2D plane, and the motion state of the target can be described by a constant turn rate and velocity (CTRV) model; in this case, the base point or target point has equal velocity, angular velocity, and height in adjacent frames or multiple frames, or has equal average velocity, average angular velocity, and average height; for example, the state vector of the base point or target point can be [x k y k ψ k v k ω k ] T , where x k ,y k For position, ψ k For the heading angle, v k For velocity, ω kFor velocity; the above motion model CTRV can be described by the corresponding state equations, which will not be elaborated here.

[0268] Alternatively, the target's motion can be modeled in a 2D plane, and its motion state can be described using a constant turn rate and acceleration (CTRA) model. In this case, the base point or target point has equal acceleration and angular velocity across adjacent frames or multiple frames, or equal average velocity and average angular velocity. For example, the state vector of the base point or target point can be [x...]. k y k ψ k v k ω k a k ] T The motion models CTRV or CRTA described above can be described by the corresponding state equations, which will not be elaborated here.

[0269] For example, the motion of the target can be motion in 3D space, such as the motion of a drone, where the target's motion in 3D space has equal velocity vector, angular velocity vector, and height, or equal velocity vector, angular velocity vector, and height.

[0270] For example, the motion of the target can be in 3D space, where the motion model of the target in the horizontal plane dimension can be a CTRV model or a CTRA model; in the vertical dimension, it can have equal height or average height, or the vertical velocity or acceleration is 0, or one or two angular velocity components in the horizontal plane are 0. Taking a ground-based vehicle as an example, when passing through a roundabout area, turning, changing lanes, or cutting, the motion state of the target can usually be approximated by having equal acceleration and angular velocity, or equal average acceleration and average angular velocity. In this case, the motion of the base point or target point has equal average acceleration, average angular velocity, and average height between adjacent frames or multiple frames; for example, the state vector of the base point or target point can be [x k y k z k ψ k v k ω k a k ] T The 3D motion model is located at the same x k ,y k Different z k The target points have equal ψ k ,v k ,ω k ,a k .

[0271] Optionally, the relationship between the motion states of the base point or the target point in different frames can be called the state equation, as shown in formula (27):

[0272] X k+1 =f(X) k )+n k (27)

[0273] Among them, X k+1 X is the state vector of the base point or target point in the (k+1)th frame; k This is the state vector of the base point or target point in frame k. For example, the state vector of the base point or target point in frame k may include one or more of the following: position, heading angle, linear velocity, angular velocity, and acceleration; f(X) k ) represents the state transition function. For example, the state transition function can be the state transition function in the CTRV model or the CTRA model. The state transition function can also be the state transition model of a three-dimensional spatial object; there is no limitation here. k The noise is the process noise, which can have a mean of 0 and a covariance of Q.

[0274] For example, the state vector of the base point in the k-th frame can be as shown in equation (28):

[0275] X k =[x k y k ψ k v k ω k a k ] T (28)

[0276] Where, x k and y k Let ψ be the two-dimensional coordinates of the base point in the k-th frame. k v is the heading angle of the base point in the kth frame. k Let ω be the linear velocity of the base point in the kth frame. k Let a be the angular velocity of the base point in the kth frame. k The acceleration of the base point in the kth frame is given.

[0277] The state vector of the base point in the above formula (28) is illustrated using the example of the target moving in a two-dimensional plane, but the embodiments of this application are not limited to this. For example, the state of the base point can also be described by a three-dimensional state vector, which may include one or more of the base point's height, roll angle, or pitch angle.

[0278] State transition function f(X) kf(X) can be a linear or nonlinear function of the state vector, depending on the target's motion model. For ease of processing, it is usually necessary to perform a subtraction on the nonlinear function f(X). k A linear approximation is performed based on Taylor expansion. For example, f(X) k It can be approximated as shown in equation (29):

[0279]

[0280] in For X k A reference estimate is obtained based on existing information, such as filtered or predicted values ​​based on previous states, or measurement data or data provided by other sensors. f(X) k )exist The Jacobian matrix at that location.

[0281] For example, as an approximate linear implementation, the above state equation (27) can be represented as shown in equation (30):

[0282]

[0283] It should be noted that, considering the motion of the carrier or sensor, as a natural extension of the embodiments of this application, motion compensation of the sensor or vehicle body can be further introduced in the relationship between the motion states of the above-mentioned base point or target point in different frames, such as the position, attitude (rotation and translation), speed and angular velocity of the carrier.

[0284] For example, the motion state of the base point includes at least one of the base point's velocity, angular velocity, and acceleration.

[0285] For example, the speed of the base point can be the absolute speed of the base point, or it can be the relative speed of the base point with respect to the vehicle.

[0286] Optionally, the motion state of the target is determined based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of the target points in the multi-frame point cloud. This includes: establishing constraints or equations based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of the target points; and obtaining the motion state of the base point and / or the position of the target point relative to the base point based on the constraints or equations.

[0287] The constraints include the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion states of the base point, and the correspondence between the measurement data of the target point.

[0288] As one implementation, obtaining the motion state of the base point and the position of the target point relative to the base point based on the constraints can be achieved by solving equations based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of the target point.

[0289] For example, the method 500 may include: the motion relationship between the target point and the base point satisfies a rigid body relationship; the relationship between the motion states of the base point in different frames is used as the motion equation; the relationship between the point cloud data of the target and the motion state of the base point is used as the measurement equation; and the motion state of the base point and the position of the target point relative to the base point are used as the state vector. The motion state of the base point and the position of the target point relative to the base point are determined based on Kalman filtering (KF), extended Kalman filtering (EKF), iterative extended Kalman filtering (IEKF), or unscented Kalman filtering (UKF).

[0290] In the Extended Kalman Filter (EKF), the nonlinear terms in the equations of motion or measurement can be linearized based on Taylor expansion. For the IEKF, the nonlinear terms can be linearized multiple times using the filtered state estimates, thus obtaining a more accurate linearized approximation.

[0291] Furthermore, based on the correspondence between the measurement data of target points in multiple frames, the predicted values ​​of EKF, IEKF, or UKF and the corresponding measurement values ​​in the next frame can be used to further determine the motion state of the base point and the position of the target point relative to the base point. For example, this can include: the target is a composite rigid body, comprising the target body and at least one accessory, the at least one accessory being connected to the body via an axis or contact point. The target body can satisfy a rigid body relationship based on the motion relationship between the target point and the base point on the body, and the motion state of the base point and the position of the target point on the body can be determined using the aforementioned EKF, IEKF, or UKF; the at least one accessory can satisfy a rigid body relationship based on the motion relationship between the target point and the axis point or contact point on the accessory, and the motion state of the axis point or contact point and the position of the target point on the accessory can be determined using the aforementioned EKF, IEKF, or UKF; using the motion state of the axis point or contact point and the measurement equation of the base point, the motion state of the base point and the positions of the target point and the axis point or contact point on the body can be further determined using the aforementioned EKF, IEKF, or UKF.

[0292] In this embodiment, the relationship between the motion states of base points in different frames is used as the motion equation, and the relationship between the point cloud data of the target and the motion state of the base points is used as the measurement equation. Using KF, EFK, IEFK, or UFK, the motion state of the base points and the position of the target points relative to the base points can be determined. Therefore, the motion state of the target can be determined based on the motion state of the base points and the position of the target points relative to the base points. On one hand, the measurement relationship between multiple target points and base points can be utilized to improve the state estimation accuracy of the base points. On the other hand, the correspondence between target point measurement data in multiple frames can be used to accumulate data over time through filtering methods, thereby improving the estimation accuracy of the base points in each frame. While improving the state estimation accuracy of the base points, the relationship between the target points and base points can also improve the estimation accuracy of the target, thus improving the overall state estimation accuracy of the target. Furthermore, linearization based on the motion equation or measurement equation helps reduce the computational requirements of the carrier, and iterative filtering based on this can further improve the estimation accuracy.

[0293] Optionally, the motion state of the target is determined based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of the target points in the multi-frame point cloud. This includes: determining an objective function based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of the target points; and optimizing the objective function to obtain the motion state of the base point and / or the position of the target point relative to the base point. As one implementation, obtaining the motion state of the base point and the position of the target point relative to the base point based on the constraints can be achieved by transforming the relationship between the motion states of the base point in different frames and the relationship between the point cloud data of the target and the motion state of the base point (including the constraints) into an objective function, and then obtaining the motion state of the base point and the position of the target point relative to the base point through an optimization method.

[0294] Optionally, the optimization solution based on the objective function to obtain the motion state of the base point and / or the position of the target point relative to the base point includes: establishing a factor map based on the relationship between the motion states of the base point, the target point, and the base point in different frames, and the relationship between the point cloud data of the target and the motion state of the base point; and optimizing the solution based on the factor map to obtain the motion state of the base point and / or the position of the target point relative to the base point.

[0295] For example, the motion state of the base point and the position of the target point relative to the base point can be obtained through an optimization method using a factor graph.

[0296] Optionally, the motion state of the base point and the position of the target point relative to the base point are determined based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of the target point in the multi-frame point cloud. The motion state of the target point is determined based on the motion state of the base point and the position of the target point relative to the base point.

[0297] Optionally, the motion state of the base point and the position of the target point relative to the base point can be determined based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion states of the base point, and the correspondence between the measurement data of the target point. This can be achieved through the following steps (a) and (b):

[0298] Step (a): Based on the relationship between the motion states of the base points in different frames, the relationship between the point cloud data of the target and the motion states of the base points, and the correspondence between the measurement data of the target points, a factor graph is established.

[0299] Optionally, the factor graph includes multiple nodes and multiple edges. The multiple nodes include the base point and the target point. The multiple edges include first-type edges and second-type edges. The first-type edges connect base points in different frames, and the second-type edges connect the target point and the base point. The determination of the objective function based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion states of the base point, and the correspondence between the measurement data of the target point, includes: determining a state error term based on the relationship between the motion states of the base point corresponding to the edges of the factor graph in different frames; determining a measurement error term based on the relationship between the point cloud data of the target and the motion states of the base point corresponding to the edges of the factor graph; and determining the objective function corresponding to the factor graph based on the state error term and / or the measurement error term.

[0300] For example, Figure 11 A factor graph of an embodiment of this application is shown. For example... Figure 11 As shown, the factor graph is constructed from base points and target points in three frames of point cloud data. Multiple nodes in the factor graph can include state nodes X. k and target point P i k = 1, 2, 3, i = 1, 2, 3, 4, 5. Factors are established between each pair of state nodes through the state equation f. State node X k and target point P i Factors are established between them by measuring the equation h. Figure 11 It can be seen that target point P2 appears in the point cloud of frame 1 and frame 2 respectively, and target point P4 appears in the point cloud of frame 2 and frame 3 respectively.

[0301] For example, the relationship between the point cloud data of the target and the motion state of the base point may include the position measurement equation and the radial velocity measurement equation, or one of them.

[0302] Taking the combination of the two into a single measurement equation as an example, it can be shown in equations (31) and (32):

[0303]

[0304] in,

[0305]

[0306] Among them, Z j For the measurement data of position and radial velocity, p j For the measurement data of the position, This is the measurement data for radial velocity.

[0307] Step (b) is to obtain the motion state of the base point and the position of the target point relative to the base point based on the factor graph.

[0308] Optionally, the method 500 further includes: obtaining the position of the target point; and determining the position of the target point relative to the base point based on the position of the target point and the motion state of the base point.

[0309] For example, the above factor graph can be solved by transforming it into an objective function optimization problem. For instance, the above factor graph optimization problem can be expressed as the following formula (33):

[0310]

[0311] Where X * This represents the state estimate that minimizes the above objective function. In formula (16) above, {} represents the objective function.

[0312] Based on the motion state of the base point and the position of the target point, the position of the target point relative to the base point can be obtained, where the position of the target point relative to the base point can be as shown in formula (34):

[0313]

[0314] Where, d j Indicates the location of the target point, d includes d j j = 1, ..., N, one or more of them, K ≤ N; d * To find the minimum value of d that minimizes the above objective function.

[0315] In this embodiment, the motion state of the base point can be determined first by the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of the target point in the multi-frame point cloud. Then, based on the position of the target point and the motion state of the base point, the position of the target point relative to the base point can be determined. On the one hand, the relationship between the measurement data of the target point and the motion state of the base point can be used to improve the motion state of the base point. On the other hand, the motion state of the base point can be used to further improve the accuracy of the position estimation of the target point relative to the base point, while significantly reducing computational complexity and computing power requirements.

[0316] Optionally, the method 500 further includes: determining the position of the target point relative to the base point based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion states of the base point, and the correspondence between the measurement data of the target point. For example, the above factor graph can be solved by transforming it into an objective function optimization problem. For instance, the above factor graph optimization problem can be expressed as the following formula (35):

[0317]

[0318] Where X * and d* Let represent the state estimate that minimizes the above objective function and the position estimate of the target point relative to the base point, respectively. Figure 11 For example, X0 is the prior estimate of the motion state of the base point in frame 0, X * It can include The motion state estimates of the base points at three time points, d * It can include The estimated positions of five target points relative to the base point.

[0319] In the objective function described above, the norm is the norm of a vector or matrix. Optionally, it can be the 1-norm, 2-norm, p-norm, or infinity-norm, L1-norm, L2-norm, etc. Preferably, it can be the 2-norm, or the 2-norm of the error vector after covariance normalization.

[0320] For example, in formulas (33)-(35), f(X) i-1 )-X i and The 2-norm can be represented by formulas (36) and (37), respectively:

[0321] ||f(X i-1 )-X i || 2 =(f(X) i-1 )-X i ) T (f(X i-1 )-X i (36)

[0322]

[0323] For example, in formulas (33)-(35), f(X) i-1 )-X i and The 2-norm of the covariance normalization can be represented by equations (38) and (39), respectively:

[0324]

[0325] Where the covariance matrix P i It can be a prior estimate, such as one obtained based on statistical estimation, machine learning, or deep learning; alternatively, it can be based on X. i-1 estimation error covariance, X i The estimation error covariance and from X i-1 To X i The covariance matrix is ​​obtained from one or more of the process noise covariance. It can be a prior estimate, such as one obtained based on statistical estimation, machine learning, or deep learning; alternatively, it can be based on... The estimation error covariance, from To Z j The measurement noise covariance is obtained.

[0326] As one implementation method, the nonlinear functions f(·) and / or h(·,·) in the objective functions of the above formulas (33)-(35) can be optimized after linearization approximation, thereby reducing the computational difficulty and computational requirements.

[0327] For example, the linearized approximation of the objective function f(·) in the above formulas (33)-(35) is shown in (29).

[0328] For example, the functions in formulas (33)-(35) above It can be linearized and approximated as any one of the terms in formulas (40)-(42):

[0329]

[0330] or

[0331]

[0332] or

[0333]

[0334] in For d j A reference estimate, and They are respectively and This is a reference estimate. The aforementioned reference estimate can be obtained based on existing information, such as filtered or predicted values ​​from previous states, measurement data, or data provided by other sensors.

[0335] In this embodiment, a factor graph is established by considering the relationships between the motion states of the base point in different frames, the relationships between the point cloud data of the target and the motion states of the base point, and the correspondence between the measurement data of the target points in the multi-frame point cloud. On one hand, the graph structure characteristics of the problem, particularly its sparsity, can be utilized to simplify the algorithm implementation and reduce computational complexity and volume. On the other hand, the optimal or good solution obtained based on the graph optimization algorithm can effectively reduce the errors caused by the various relationships used as constraints, thereby improving the accuracy of the motion state of the base point and / or the position of the target point relative to the base point. Therefore, the accuracy of the target's motion state estimation can be improved, thus contributing to enhanced carrier safety.

[0336] For example, in the above optimization or factor graph method, the motion relationship between the target point and the base point satisfies a rigid body relationship or a composite rigid body relationship.

[0337] For example, as one implementation, the target can be a composite rigid body, including a main body and at least one attachment, wherein the at least one attachment is connected to the main body via an axis or contact point. The main body of the target can satisfy a rigid body relationship based on the motion relationship between the target point and the base point on the main body, and the motion state of the base point and the position of the target point on the main body are determined using the aforementioned optimization or factor graph method. The at least one attachment can satisfy a rigid body relationship based on the motion relationship between the target point on the attachment and the axis point or contact point, and the motion state of the axis point or contact point and the position of the target point on the attachment are determined using the aforementioned optimization or factor graph method. Using the motion state of the axis point or contact point and the measurement equation of the base point, the motion state of the base point and the positions of the target point and the axis point or contact point on the main body are further determined using the aforementioned optimization or factor graph method.

[0338] Optionally, in a hybrid method that can be based on EKF, IEKF, UKF, and optimization or factor graphs, the motion state of the base point and the position of the target can be determined by utilizing the motion relationship between the target point and the base point to satisfy a rigid body relationship or a composite rigid body relationship.

[0339] For example, as one implementation, the target can be a composite rigid body, including the target body and at least one attachment, wherein the at least one attachment is connected to the body via an axis or contact point. The target body can satisfy a rigid body relationship based on the motion relationship between the target point and the base point on the body, and the motion state of the base point and the position of the target point on the body are determined using the above-mentioned optimization or factor graph method; the at least one attachment can satisfy a rigid body relationship based on the motion relationship between the target point on the attachment and the axis point or contact point, and the motion state of the axis point or contact point and the position of the target point on the attachment are determined using the above-mentioned EKF, IEKF, or UKF; using the motion state of the axis point or contact point and the measurement equation of the base point, the motion state of the base point and the positions of the target point and axis point or contact point on the body are further determined using the above-mentioned optimization or factor graph method.

[0340] In this embodiment, the target is decomposed into a composite rigid body, including a main body and at least one accessory, with the accessory connected to the main body via an axis or contact point. On one hand, the states of the main body's base point and axis or contact point can be determined through rigid body relationships. On the other hand, the measurement relationship between the axis and / or contact point and the base point can be determined using the motion states of the axis and / or contact point. This further establishes a factor graph. On one hand, the graph structure characteristics of the problem, especially its sparsity, can be utilized to simplify the algorithm implementation and reduce computational complexity and volume. On the other hand, the optimal or good solution obtained based on the graph optimization algorithm can effectively reduce the errors caused by the various relationships used as constraints, thereby improving the accuracy of the motion state of the base point and / or the position of the target point relative to the base point. Therefore, the accuracy of the target's motion state estimation can be improved, thus contributing to enhanced carrier safety.

[0341] Optionally, the method 500 further includes: determining the motion state of the target point based on the motion state of the base point and the position of the target point, and based on the relationship between the motion state of the base point and the motion state of the target point.

[0342] Optionally, the motion state of the target point includes one or more of the target point's position, velocity, and angular velocity.

[0343] For example, the motion state of the target point may include the position and instantaneous speed of the target point closest to the vehicle and the target point farthest from the vehicle.

[0344] For example, the motion state of the target point may include the position and instantaneous speed of each point of the target closest to the vehicle.

[0345] For example, taking the rigid body relationship between a target point and a base point on the vehicle body or drone body as an example, given the position d of any given target point relative to the base point, the velocity of the target point can be obtained by the following formula (43A) or (43B):

[0346] v = v base +ω×d(43A) or, equivalently,

[0347] v = v base -d×ω(43B)

[0348] Among them, v base Let ω be the velocity of the base point, ω be the angular velocity of the base point, and v be the velocity of the target point.

[0349] Optionally, taking the rigid body relationship between the target point and the base point on the vehicle body or the drone body as an example, given the position d of the target point relative to the base point, the acceleration of the target point can be obtained by the following formula (44A) or (44B):

[0350] a = a base +ω base ×(ω base ×d)(44A) or, equivalently,

[0351] a = a base +(d×ω base )×ω base (44B)

[0352] Where a is the acceleration of the target point, a base The acceleration is based on the reference point. The angular velocity of the vehicle body or drone body is usually approximately constant in adjacent frames or multiple frames during its movement.

[0353] Optionally, taking the target point and the base point on the vehicle body or the main body of the drone as an example of a rigid body relationship, the angular velocity of the target point is equal to the angular velocity of the base point.

[0354] It should be noted that for composite rigid bodies, the motion state of the contact points or pivot points connected to the target body can also be obtained in a similar manner. For at least one attachment on a composite rigid body, the state of the target point on the attachment can be obtained from the motion state of the contact points or pivot points connected to the attachment in a similar manner, which will not be elaborated here.

[0355] In this embodiment, motion state estimation based on a base point utilizes the motion state relationship between the base point and the target point, axis point, or contact point to obtain the motion state of the target point, axis point, or contact point. Therefore, the accuracy of the base point's state estimation can be fully utilized to improve the accuracy of the target's motion state estimation, thereby contributing to improved driving safety and comfort.

[0356] Optionally, the method 500 further includes: determining the motion state of the target point based on the motion state of the base point and the position of the target point, and based on the relationship between the motion state of the base point and the motion state of the target point.

[0357] Optionally, the method 500 further includes: determining one or more of the shape, dimension, or size of the target based on the positions of the base point and the target point. Optionally, the target can be the extended target described above, and the method 500 further includes: determining the shape of the extended target based on the positions of each target point.

[0358] For example, based on the position of each target point, the minimum bounding polygon, polyhedron, bounding box, bounding ellipse, ellipsoid, or polygon outline of the extended target can be obtained.

[0359] Optionally, the method 500 further includes: determining the intention of the target based on the target's motion state; and controlling the vehicle to move according to the target's intention.

[0360] Optionally, the method 500 further includes: sending the motion state of the target to the planning module 220, wherein the motion state of the target may include one or more of the motion state of the base point, the shape of the target, the position of the target point, velocity, angular velocity, etc.

[0361] For example, the state of motion can be one or more of the following: position, orientation, velocity, angular velocity, acceleration, shape, etc.

[0362] For example, if the execution subject of the method 500 is the perception module 210, the perception module 210 can send the estimated value of the motion state of the target to the planning module 220 after obtaining the estimated value of the motion state of the target.

[0363] Optionally, the sensing module 210 can also send the shape of the extended target to the planning module 220.

[0364] Optionally, the method 500 further includes: controlling the vehicle to travel according to the motion state of the target.

[0365] For example, the carrier can be a vehicle 100.

[0366] For example, taking the method 500 being executed by the intelligent driving system 200 in the vehicle 100, after obtaining the motion state of the target point, the intelligent driving system 200 can determine the target's intention to change lanes or overtake based on the position and speed of each point obtained above.

[0367] This application provides a method for determining the motion state of a target. Based on position and radial velocity measurement data and / or radial acceleration measurement provided by sensors, this method can provide the target's motion state, particularly the velocity and angular velocity estimates of the base point, as well as the position, velocity, and / or acceleration estimates of key points. Furthermore, a more accurate estimate of the target's shape can be obtained based on this. This information can be provided to prediction, planning, or control subsystems to determine the target's lane-changing or cut-in intentions, thereby effectively avoiding collision risks or requiring emergency takeover, improving driving safety and comfort.

[0368] Optionally, the method 500 further includes: determining the motion state of the base point and the position of the target point relative to the base point based on the relationship between the single-frame point cloud data of the target and the motion state of the base point. This is a supplement to the above-mentioned filtering-based, optimization-based, or factor graph-based methods. Figure 12This illustration shows a schematic flowchart of a method 1200 for determining the motion state of a target according to an embodiment of this application. The method can determine the motion state of a base point or target point based on single-frame data, thereby being used in the initialization method for the motion state of the base point or target point in the aforementioned method. The method 1200 includes:

[0369] S1210, acquire single-frame point cloud data of the target from the sensor, wherein the point cloud data includes measurement data of multiple target points of the target;

[0370] As mentioned above, the target can be an extended target. The sensors and measurement data are as described above.

[0371] S1220, determine the base point of the target and, based on the relationship between the motion state of the base point and the motion state of the target point, determine the relationship between the point cloud data of the target and the motion state of the base point;

[0372] Optionally, determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: determining the radial acceleration measurement relationship between the target point and the base point based on the relationship between the motion state of the base point and the motion state of the target point; determining the radial acceleration measurement relationship between the target point and the base point includes determining the radial acceleration measurement relationship based on the following relationship: the radial acceleration of the target point is the radial projection component of the acceleration vector of the target point, and the velocity vector of the target point is obtained based on the acceleration vector of the base point, the position vector of the target point relative to the base point, and the angular velocity.

[0373] Optionally, determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: determining the optical flow and / or scene flow measurement relationship between the point cloud data of the target and the base point based on the relationship between the motion state of the base point and the motion state of the target point.

[0374] Determining the optical flow measurement relationship between the point cloud data of the target and the base point includes determining the optical flow measurement relationship based on the following relationship: the optical flow of the target point is the perspective projection of the velocity vector of the target point onto the camera plane, and the velocity vector of the target point is obtained from the velocity vector of the base point, the position vector of the target point relative to the base point, and the angular velocity. Alternatively...

[0375] The determination of the scene flow measurement relationship between the point cloud data of the target and the base point includes determining the scene flow measurement relationship based on the following relationship: the scene flow of the target point is the velocity vector or projection component of the target point, and the velocity vector of the target point is obtained from the velocity vector of the base point, the position vector of the target point relative to the base point, and the angular velocity.

[0376] S1230, Based on the relationship between the point cloud data of the target and the motion state of the base point, determine the motion state of the target.

[0377] For example, the relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between the base point of the rigid body and the mass point on the rigid body. Determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: determining the relative position measurement relationship between the target point and the base point based on the motion relationship between the base point of the rigid body and the mass point on the rigid body, wherein the relative position vector measurement value is determined by the position measurement value of the target point and the position of the base point.

[0378] Determining the motion state of the target may include determining the motion state of the base point and the position of the target point relative to the base point.

[0379] For example, the motion relationship between the base point of the rigid body and the mass point on the rigid body includes the relative positions remaining unchanged between at least two frames.

[0380] For example, based on the relative position measurement relationship between the target point and the base point, as shown in equation (1-3), the position of the target point relative to the base point can be determined as shown in equation (45):

[0381]

[0382] For example, based on the relative position measurement relationship between the target point and the base point, and the radial velocity measurement relationship between the target point and the base point, the estimated values ​​of the velocity and angular velocity of the base point and the position of the target point relative to the base point are obtained based on equations (1)-(12); for example, based on the radial velocity measurement relationship between the target point and the base point, as shown in equations (7)-(12), the estimated values ​​of the velocity and angular velocity of the base point are obtained; for example, they can be obtained based on equations (7)-(12) and (45) using Newton's method, Gauss-Newton method, or LM (Levenberg-Marquardt) algorithm, etc.

[0383] For example, based on the relative position measurement relationship between the target point and the base point, the radial velocity measurement relationship between the target point and the base point, and the radial acceleration measurement relationship between the target point and the base point, the estimated values ​​of the velocity, acceleration and angular velocity of the base point and the position of the target point relative to the base point are obtained based on (1)-(14), for example, by using Newton's method, Gauss-Newton method or LM (Levenberg-Marquardt algorithm, etc.);

[0384] In this embodiment, the relationship between the motion state of the base point and the motion state of the target point—that is, the relationship between the acceleration of the target point and the acceleration, angular velocity, and relative position of the base point according to rigid body relations—can be used to establish a relationship between the radial acceleration measurement data of the target point and the acceleration, angular velocity, and relative position of the base point. Therefore, the acceleration, angular velocity, and relative position of the base point can be obtained using the radial acceleration measurement data of one or more target points in the point cloud. This point cloud data can effectively improve the accuracy of the motion state estimation of the base point, and the aforementioned relationship can further improve the accuracy of the motion state estimation of the target point.

[0385] Figure 13 A schematic block diagram of a device 1300 for determining the motion state of a target, according to an embodiment of this application, is shown. The device 1300 includes: an acquisition unit, configured to acquire multi-frame point cloud data of the target from a sensor and the correspondence between measurement data of target points in the multi-frame point cloud; a determination unit 1320, configured to determine a base point of the target and, based on the relationship between the motion state of the base point and the motion state of the target points, determine the relationship between the point cloud data of the target and the motion state of the base point; the determination unit 1320 is further configured to determine the motion state of the target based on the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of target points in the multi-frame point cloud.

[0386] Optionally, the determining unit 1320 is further configured to: determine the relationship between the motion states of the base point in different frames; and determine the motion state of the target based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of the target points in the multi-frame point cloud.

[0387] Optionally, the multi-frame point cloud data includes one or more of the target's position measurement data, velocity measurement data, or acceleration measurement data.

[0388] Optionally, the velocity measurement data may include one or more of the following: radial velocity measurement data of the target, optical flow measurement data, or scene flow measurement data.

[0389] Optionally, the relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between the base point of the rigid body and the mass point on the rigid body.

[0390] Optionally, the kinematic relationship between the base point of the rigid body and the particles on the rigid body includes the relationship between one or more of the following: position, angular velocity, linear velocity, or acceleration.

[0391] Optionally, the relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between two mass points on the composite rigid body. The composite rigid body includes at least two rigid bodies, which include a main body of the target and at least one accessory. The main body and the at least one accessory are connected by a shaft.

[0392] Optionally, the relationship between the motion state of the base point and the motion state of the target point is determined by the motion state relationship between the target point and the axis point and the motion state relationship between the base point and the axis point, where the axis point is a point located on the axis and the target point is located in the attachment.

[0393] Optionally, the relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between two mass points on the composite rigid body. The composite rigid body includes at least two rigid bodies, which include a main body of the target and at least one accessory. The main body and the at least one accessory are connected by a contact point.

[0394] Optionally, the relationship between the motion state of the base point and the motion state of the target point is determined by the motion state relationship between the target point and the contact point and the motion state relationship between the base point and the contact point, wherein the target point is located in the attachment.

[0395] Optionally, the relationship between the motion states of the base point in different frames includes: the base point having one or more of the following: equal velocity, angular velocity, or acceleration between adjacent frames; and / or, the base point having one or more of the following: equal average velocity, average angular velocity, or average acceleration between adjacent frames; and / or, the base point having equal height or average height between adjacent frames; and / or, the base point having a vertical velocity or average acceleration of 0 between adjacent frames; and / or, one or two angular velocity components of the base point being 0 between adjacent frames.

[0396] Optionally, the determining unit is specifically used to: determine the motion state of the base point and / or the motion state of the target point;

[0397] Alternatively, the target is an extended target, which is a target whose size spans across the sensor resolution unit.

[0398] Optionally, one or more of the shape, dimension, or size of the target can be determined based on the positions of the base point and the target point.

[0399] Optionally, the determining unit 1320 is specifically used to: determine the relative position measurement relationship between the target point and the base point based on the relationship between the motion state of the base point and the motion state of the target point, wherein the relative position vector measurement value is determined by the position measurement value of the target point and the position of the base point.

[0400] Optionally, the determining unit 1320 is specifically used to: determine the radial velocity measurement relationship between the target point and the base point based on the relationship between the motion state of the base point and the motion state of the target point, wherein the radial velocity measurement relationship is used to represent the relationship between the radial velocity of the target point and the linear velocity and angular velocity of the base point.

[0401] Optionally, the determining unit 1320 is specifically used to: determine the radial velocity measurement relationship according to the following relationship: the radial velocity of the target point is the radial projection component of the velocity vector of the target point, and the velocity vector of the target point is obtained based on the velocity vector of the base point, the position vector of the target point relative to the base point, and the angular velocity.

[0402] Optionally, the determining unit 1320 is specifically used to: determine the radial acceleration measurement relationship between the target point and the base point based on the relationship between the motion state of the base point and the motion state of the target point; the determining unit can determine the radial acceleration measurement relationship based on the following relationship: the radial acceleration of the target point is the radial projection component of the acceleration vector of the target point, and the velocity vector of the target point is obtained based on the acceleration vector of the base point, the position vector of the target point relative to the base point, and the angular velocity.

[0403] Optionally, the determining unit 1320 is specifically used to: determine the optical flow measurement relationship between the point cloud data of the target and the base point based on the relationship between the motion state of the base point and the motion state of the target point; the determining unit can determine the optical flow measurement relationship based on the following relationship: the optical flow of the target point is the perspective projection of the velocity vector of the target point on the camera plane, and the velocity vector of the target point is obtained from the velocity vector of the base point, the position vector of the target point relative to the base point, and the angular velocity.

[0404] Optionally, the determining unit 1320 is further configured to: determine the position of the target point relative to the base point based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion states of the base point, and the correspondence between the measurement data of the target point in the multi-frame point cloud.

[0405] Optionally, the acquisition unit 1310 is further configured to: acquire the position of the target point; the determination unit 1320 is specifically configured to: determine the position of the target point relative to the base point based on the position of the target point and the motion state of the base point.

[0406] Optionally, determining the motion state of the target based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion state of the base point, and the correspondence between the measurement data of the target points in the multi-frame point cloud includes:

[0407] The relationship between the motion states of the base point in different frames is used as the motion equation, the relationship between the point cloud data of the target and the motion state of the base point is used as the measurement equation, and the motion state of the base point and the position of the target point relative to the base point are used as the state vector. Based on the iterative extended Kalman filter (IEKF), the motion state of the base point and the position of the target point relative to the base point are determined.

[0408] Optionally, the determining unit 1320 is specifically used to: determine an objective function based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion states of the base point, and the correspondence between the measurement data of the target point; and optimize the solution based on the objective function to obtain the motion state of the base point and / or the position of the target point relative to the base point.

[0409] Optionally, the determining unit 1320 is specifically used to: establish a factor map based on the relationship between the base point, the target point, the motion state of the base point in different frames, and the relationship between the point cloud data of the target and the motion state of the base point; and optimize the solution based on the factor map to obtain the motion state of the base point and / or the position of the target point relative to the base point.

[0410] Optionally, the factor graph includes multiple nodes and multiple edges. The multiple nodes include the base point and the target point. The multiple edges include first-type edges and second-type edges. The first-type edges connect base points in different frames, and the second-type edges connect the target point and the base point. Specifically, the determining unit 1320 is used to: determine a state error term based on the relationship between the motion states of the base point corresponding to the edge of the factor graph in different frames; determine a measurement error term based on the relationship between the target point cloud data corresponding to the edge of the factor graph and the motion state of the base point; and determine the objective function corresponding to the factor graph based on the state error term and the measurement error term.

[0411] Optionally, the motion state of the target includes one or more of the target point's position, velocity, angular velocity, and acceleration.

[0412] Optionally, the base point can be any point in a multi-frame point cloud; the base point can be the point closest to the carrier; the base point can be the centroid of multiple points in a single frame of a multi-frame point cloud; or the base point can be an external input point.

[0413] Optionally, the device further includes a control unit, the determining unit 1320, which is further configured to determine the intention of the target based on the target's motion state; the control unit is configured to control the vehicle's movement based on the target's intention.

[0414] It should be understood that the division of units in the above device is only a logical functional division. In actual implementation, they can be fully or partially integrated into a single physical entity, or they can be physically separated. Furthermore, the units in the device can be implemented by a processor calling software; for example, the device includes a processor connected to memory, which stores instructions. The processor calls the instructions stored in memory to implement any of the above methods or to implement the functions of each unit in the device. The processor can be, for example, a general-purpose processor, such as a CPU or microprocessor, and the memory can be internal or external to the device. Alternatively, the units in the device can be implemented as hardware circuits. The functions of some or all units can be implemented through the design of the hardware circuits, which can be understood as one or more processors. For example, in one implementation, the hardware circuit is an ASIC, and the functions of some or all units are implemented through the design of the logical relationships between the components within the circuit. In another implementation, the hardware circuit can be implemented using a PLD, such as an FPGA, which can include a large number of logic gates. The connection relationships between the logic gates are configured through configuration files, thereby implementing the functions of some or all units. All units of the above devices can be implemented entirely through processor calling software, or entirely through hardware circuits, or partially through processor calling software with the remaining parts implemented through hardware circuits.

[0415] In this application embodiment, a processor is a circuit with signal processing capabilities. In one implementation, the processor can be a circuit with instruction reading and execution capabilities, such as a CPU, microprocessor, GPU, or DSP. In another implementation, the processor can implement certain functions through the logical relationships of hardware circuits. These logical relationships are fixed or reconfigurable. For example, the processor may be a hardware circuit implemented as an ASIC or PLD, such as an FPGA. In a reconfigurable hardware circuit, the process of the processor loading a configuration document and configuring the hardware circuit can be understood as the processor loading instructions to implement the functions of some or all of the above units. Furthermore, it can also be a hardware circuit designed for artificial intelligence, which can be understood as an ASIC, such as an NPU, TPU, or DPU.

[0416] As can be seen, each unit in the above device can be one or more processors (or processing circuits) configured to implement the above methods, such as: CPU, GPU, NPU, TPU, DPU, microprocessor, DSP, ASIC, FPGA, or a combination of at least two of these processor forms.

[0417] Furthermore, the units in the above devices can be integrated in whole or in part, or they can be implemented independently. In one implementation, these units are integrated together as a System-on-a-Chip (SoC). The SoC may include at least one processor for implementing any of the above methods or implementing the functions of the units in the device. The at least one processor may be of different types, such as CPU and FPGA, CPU and AI processor, CPU and GPU, etc.

[0418] This application also provides a motion state estimation device, which includes a processing unit and a storage unit. The storage unit is used to store instructions, and the processing unit executes the instructions stored in the storage unit to enable the device to perform the methods or steps described in the above embodiments.

[0419] Optionally, if the motion state estimation device is located in the vehicle, the aforementioned processing unit may be... Figure 2 One or more of the processors 121-12n shown.

[0420] This application also provides a system for determining the motion state of a target. The system includes a sensing system and a computing platform, the computing platform including the aforementioned device 1300 for determining the motion state of a target.

[0421] This application also provides a vehicle that may include the device 1300 for determining the motion state of a target or the system for determining the motion state of a target.

[0422] This application also provides a computer program product, which includes computer program code that, when run on a computer, causes the computer to perform the methods described in the above embodiments.

[0423] This application also provides a computer-readable medium storing program code that, when run on a computer, causes the computer to perform the methods described in the above embodiments.

[0424] This application also provides a chip, which includes a circuit for performing the methods described in the above embodiments.

[0425] In implementation, each step of the above method can be completed by integrated logic circuits in the processor's hardware or by instructions in software. The method disclosed in the embodiments of this application can be directly implemented by a hardware processor, or by a combination of hardware and software modules within the processor. The software modules can reside in random access memory, flash memory, read-only memory, programmable read-only memory, power-on erasable programmable memory, registers, or other mature storage media in the art. This storage medium is located in memory, and the processor reads information from the memory and, in conjunction with its hardware, completes the steps of the above method. To avoid repetition, detailed descriptions are omitted here.

[0426] It should be understood that in the embodiments of this application, the memory may include read-only memory and random access memory, and provides instructions and data to the processor.

[0427] It should also be understood that, in the various embodiments of this application, the order of the above-mentioned processes does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0428] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0429] Those skilled in the art will understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0430] In the several embodiments provided in this application, it should be understood that the disclosed systems, apparatuses, and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between apparatuses or units may be electrical, mechanical, or other forms.

[0431] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0432] In addition, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.

[0433] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0434] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in this application should be covered. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A method for determining the motion state of a target, characterized in that, include: The correspondence between multi-frame point cloud data of the target obtained from the sensor and the measurement data of the target points in the multi-frame point cloud; Determine the base point of the target and, based on the relationship between the motion state of the base point and the motion state of the target point, determine the relationship between the point cloud data of the target and the motion state of the base point; The motion state of the target is determined based on the relationship between the point cloud data of the target and the motion state of the base point, as well as the correspondence between the measurement data of the target points in the multi-frame point cloud.

2. The method according to claim 1, characterized in that, The method includes: Determine the relationship between the motion states of the base points in different frames; The determination of the target's motion state based on the relationship between the target's point cloud data and the motion state of the base point, as well as the correspondence between the target point measurement data in the multi-frame point cloud, includes: The motion state of the target is determined based on the relationship between the motion states of the base points in different frames, the relationship between the point cloud data of the target and the motion states of the base points, and the correspondence between the measurement data of the target points in the multi-frame point cloud.

3. The method according to claim 1 or 2, characterized in that, The multi-frame point cloud data includes one or more of the target's position measurement data, velocity measurement data, or acceleration measurement data.

4. The method according to claim 3, characterized in that, The velocity measurement data includes one or more of the following: radial velocity measurement data of the target, optical flow measurement data, or scene flow measurement data.

5. The method according to any one of claims 1-4, characterized in that, The relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between the base point of the rigid body and the mass point on the rigid body.

6. The method according to claim 5, characterized in that, The kinematic relationship between the base point of the rigid body and the mass point on the rigid body includes the relationship between one or more of the following: position, angular velocity, linear velocity, or acceleration.

7. The method according to any one of claims 1-4, characterized in that, The relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between two mass points on the composite rigid body. The composite rigid body includes at least two rigid bodies, and the at least two rigid bodies include a main body and at least one accessory of the target. The main body and the at least one accessory are connected by a shaft.

8. The method according to claim 7, characterized in that, The relationship between the motion state of the base point and the motion state of the target point is determined by the motion state relationship between the target point and the axis point and the motion state relationship between the base point and the axis point. The axis point is a point located on the axis, and the target point is located in the attachment.

9. The method according to any one of claims 1-4, characterized in that, The relationship between the motion state of the base point and the motion state of the target point is determined according to the motion relationship between two mass points on the composite rigid body. The composite rigid body includes at least two rigid bodies, and the at least two rigid bodies include a main body of the target and at least one accessory. The main body and the at least one accessory are connected by a contact point.

10. The method according to claim 9, characterized in that, The relationship between the motion state of the base point and the motion state of the target point is determined by the motion state relationship between the target point and the contact point, and the motion state relationship between the base point and the contact point, wherein the target point is located in the attachment.

11. The method according to any one of claims 2-10, characterized in that, The relationship between the motion states of the base points in different frames includes: The base point has one or more of the following: equal average velocity, average angular velocity, or average acceleration between adjacent frames; and / or The vertical velocity or acceleration of the base point is 0 between adjacent frames.

12. The method according to any one of claims 1-11, characterized in that, Determining the motion state of the target includes: Determine the motion state of the base point and / or the motion state of the target point.

13. The method according to any one of claims 1-12, characterized in that, The target is an extended target, which is a target whose size spans across the sensor resolution unit.

14. The method according to claim 13, characterized in that, The method further includes: The shape, dimension, or size of the target is determined based on the positions of the base point and the target point.

15. The method according to any one of claims 1-14, characterized in that, Determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: The relative position measurement relationship between the target point and the base point is determined based on the relationship between the motion state of the base point and the motion state of the target point, wherein the relative position vector measurement value is determined by the position measurement value of the target point and the position of the base point.

16. The method according to any one of claims 1-15, characterized in that, Determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: The radial velocity measurement relationship between the target point and the base point is determined based on the relationship between the motion state of the base point and the motion state of the target point. The radial velocity measurement relationship is used to represent the relationship between the radial velocity of the target point and the linear velocity and angular velocity of the base point.

17. The method according to claim 16, characterized in that, Determining the radial velocity measurement relationship between the target point and the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: The radial velocity measurement relationship is determined based on the following relationship: The radial velocity of the target point is the radial projection component of the velocity vector of the target point, and the velocity vector of the target point is obtained based on the velocity vector of the base point, the position vector of the target point relative to the base point, and the angular velocity.

18. The method according to any one of claims 1-17, characterized in that, Determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: The radial acceleration measurement relationship between the target point and the base point is determined based on the relationship between the motion state of the base point and the motion state of the target point; Determining the radial acceleration measurement relationship between the target point and the base point includes determining the radial acceleration measurement relationship based on the following relationship: The radial acceleration of the target point is the radial projection component of the acceleration vector of the target point, and the velocity vector of the target point is obtained based on the acceleration vector of the base point, the position vector of the target point relative to the base point, and the angular velocity.

19. The method according to any one of claims 1-18, characterized in that, Determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: Based on the relationship between the motion state of the base point and the motion state of the target point, the optical flow measurement relationship between the point cloud data of the target and the base point is determined; Determining the optical flow measurement relationship between the point cloud data of the target and the base point includes determining the optical flow measurement relationship based on the following relationship: The optical flow of the target point is the perspective projection of the velocity vector of the target point onto the camera plane. The velocity vector of the target point is obtained from the velocity vector of the base point, the position vector of the target point relative to the base point, and the angular velocity.

20. The method according to any one of claims 1-19, characterized in that, Determining the relationship between the point cloud data of the target and the motion state of the base point based on the relationship between the motion state of the base point and the motion state of the target point includes: Based on the relationship between the motion state of the base point and the motion state of the target point, the scene flow measurement relationship between the point cloud data of the target and the base point is determined; Determining the scene flow measurement relationship between the point cloud data of the target and the base point includes determining the scene flow measurement relationship based on the following relationship: The scene flow of the target point is the velocity vector of the target point or the projection component of the velocity vector of the target point. The velocity vector of the target point is obtained from the velocity vector of the base point, the position vector of the target point relative to the base point, and the angular velocity.

21. The method according to any one of claims 2-20, characterized in that, The method further includes: The position of the target point relative to the base point is determined based on the relationship between the motion states of the base point in different frames, the relationship between the point cloud data of the target and the motion states of the base point, and the correspondence between the measurement data of the target point in the multi-frame point cloud.

22. The method according to any one of claims 2-21, characterized in that, The method further includes: Obtain the location of the target point; The position of the target point relative to the base point is determined based on the position of the target point and the motion state of the base point.

23. The method according to any one of claims 2-22, characterized in that, The determination of the target's motion state based on the relationship between the motion states of the base points in different frames, the relationship between the target's point cloud data and the motion states of the base points, and the correspondence between the measurement data of the target points in the multi-frame point cloud, includes: The relationship between the motion states of the base point in different frames is used as the motion equation, the relationship between the point cloud data of the target and the motion state of the base point is used as the measurement equation, and the motion state of the base point and the position of the target point relative to the base point are used as the state vector. Based on the iterative extended Kalman filter (IEKF), the motion state of the base point and the position of the target point relative to the base point are determined.

24. The method according to any one of claims 2-22, characterized in that, The determination of the target's motion state based on the relationship between the motion states of the base points in different frames, the relationship between the target's point cloud data and the motion states of the base points, and the correspondence between the measurement data of the target points in the multi-frame point cloud, includes: The objective function is determined based on the relationship between the motion states of the base points in different frames, the relationship between the point cloud data of the target and the motion states of the base points, and the correspondence between the measurement data of the target points. The motion state of the base point and / or the position of the target point relative to the base point are obtained by optimizing the objective function.

25. The method according to claim 24, characterized in that, The step of optimizing and solving the objective function to obtain the motion state of the base point and / or the position of the target point relative to the base point includes: A factor graph is established based on the relationship between the base point, the target point, the motion state of the base point in different frames, and the relationship between the point cloud data of the target and the motion state of the base point. The motion state of the base point and / or the position of the target point relative to the base point are obtained by optimizing the solution based on the factor graph.

26. The method according to claim 25, characterized in that, The factor graph includes multiple nodes and multiple edges. The multiple nodes include the base point and the target point. The multiple edges include first-type edges and second-type edges. The first-type edges connect base points in different frames, and the second-type edges connect the target point and the base point. The step of determining the objective function based on the relationship between the motion states of the base points in different frames, the relationship between the point cloud data of the target and the motion states of the base points, and the correspondence between the measurement data of the target points includes: Based on the relationship between the motion states of the base points corresponding to the edges of the factor graph in different frames, the state error term is determined; The measurement error term is determined based on the relationship between the point cloud data of the target corresponding to the edge of the factor graph and the motion state of the base point; The objective function corresponding to the factor graph is determined based on the state error term and / or the measurement error term.

27. The method according to any one of claims 1-26, characterized in that, The method further includes: Based on the relationship between the single-frame point cloud data of the target and the motion state of the base point, the motion state of the base point and the position of the target point relative to the base point are determined.

28. The method according to any one of claims 1-27, characterized in that, The motion state of the target includes one or more of the target point's position, velocity, angular velocity, and acceleration.

29. The method according to any one of claims 1 to 28, characterized in that, The base point is any point in the multi-frame point cloud; The base point is the point closest to the carrier; The base point is the centroid of multiple points in a single frame of a multi-frame point cloud; or... The base point is an external input point.

30. The method according to any one of claims 1 to 29, characterized in that, The method further includes: Based on the target's motion state, determine the target's intent; Control the vehicle's movement according to the intended purpose of the target.

31. A device for determining the motion state of a target, characterized in that, The apparatus includes modules or units for performing the method as described in any one of claims 1 to 30.

32. A device for determining the motion state of a target, characterized in that, include: A processor for executing a computer program stored in memory to cause the apparatus to perform the method as described in any one of claims 1 to 30.

33. The apparatus according to claim 32, characterized in that, The device also includes the memory.

34. A system for determining the motion state of a target, characterized in that, The control system includes a sensing system and a computing platform, the computing platform including the apparatus as described in any one of claims 31-33.

35. A carrier, characterized in that, Includes the apparatus as described in any one of claims 31-33, or includes the system as described in claim 34.

36. The carrier according to claim 35, characterized in that, The carrier is a vehicle.

37. A computer-readable storage medium, characterized in that, It stores instructions that, when executed by a processor, cause the processor to implement the method as described in any one of claims 1 to 30.

38. A computer program product, characterized in that, The computer program product includes computer program code that, when run on a computer, causes the computer to perform the method as described in any one of claims 1 to 30.

39. A chip, characterized in that, The chip includes circuitry for performing the method as described in any one of claims 1 to 30.

Images