Position determination method, device, operating machine and medium based on image and point cloud

Abstract

The present application relates to the field of computer vision technology, and discloses a method, device, operating machine and medium for determining posture based on images and point clouds. The method is applied to the operating machine, and the operating machine includes a laser radar and an image acquisition device. The method includes: determining the first posture of the image acquisition device based on visual identification; determining the visual constraint on the second posture of the operating machine according to the first posture and the first external parameter, wherein the first external parameter is the external parameter of the image acquisition device relative to the laser radar that is pre-calibrated; determining the point cloud constraint on the posture of the operating machine according to the multi-frame point cloud acquired by the laser radar; determining the second posture of the operating machine according to the visual constraint and the point cloud constraint. The method provided in the present application can obtain more robust, stable and accurate posture results because it introduces visual constraints and constructs multi-source information constraints of visual constraints and point cloud constraints.

Description

Technical Field

[0001] The present application relates to the field of computer vision technology, and specifically to a method, device, operating machine and medium for determining a posture based on an image and a point cloud. Background Art

[0002] The working environment in the field of operating machinery is often harsh, the work intensity is high, and there are great safety hazards. With the rapid development of science and technology, unmanned driving, as an emerging force in the field of operating machinery, is gradually changing the traditional working methods. Unmanned driving can accurately complete various work tasks through advanced control systems, which not only improves work efficiency and saves labor costs, but also effectively avoids safety hazards and provides better protection for personnel safety.

[0003] SLAM (Simultaneous Localization and Mapping) can solve the positioning and map building problems at the same time, and determine the map of the environment based on the position and posture of the working machine. However, the existing working machine's position and posture determination is not accurate enough, which leads to poor accuracy in map construction. Therefore, there is an urgent need for a position and posture determination method that can adapt to complex working scenarios in the field of working machinery to improve the accuracy of the working machinery's position and posture. Summary of the invention

[0004] The purpose of the embodiments of the present application is to provide a method, device, working machine and medium for determining posture based on images and point clouds, so as to solve the technical problem of inaccurate posture determination of working machines in the prior art.

[0005] In order to achieve the above-mentioned purpose, the first aspect of the present application provides a method for determining a posture based on an image and a point cloud, which is applied to an operating machine, wherein the operating machine includes a laser radar and an image acquisition device, and the method includes:

[0006] Based on the visual identification, determining a first pose of the image acquisition device;

[0007] Determining a visual constraint on a second posture of the working machine according to the first posture and a first external parameter, wherein the first external parameter is an external parameter of a pre-calibrated image acquisition device relative to a laser radar;

[0008] Determine the point cloud constraints on the position and posture of the operating machine based on the multi-frame point cloud collected by the laser radar;

[0009] The second pose of the operating machine is determined based on visual constraints and point cloud constraints.

[0010] In an embodiment of the present application, determining the first pose of the image acquisition device based on the visual identifier includes:

[0011] capturing images including visual identifiers;

[0012] According to the image, determine the coordinates of multiple key points in the visual identification;

[0013] The first position of the image acquisition device is obtained by performing registration according to the coordinates of multiple key points.

[0014] In an embodiment of the present application, a calibration method of the first external parameter includes:

[0015] Determine respectively a first direction vector and a second direction vector of the image acquisition device and the laser radar relative to the coordinate detection device;

[0016] According to the first direction vector and the second direction vector, respectively determine a second external parameter and a third external parameter of the image acquisition device and the laser radar relative to the coordinate detection device;

[0017] The first external parameter is determined according to the second external parameter and the third external parameter.

[0018] In the embodiment of the present application, the point cloud constraint includes an inter-frame registration constraint, and the point cloud constraint for the position and posture of the working machine is determined according to the multi-frame point cloud collected by the laser radar, including:

[0019] The multi-frame point clouds collected by the LiDAR are classified based on curvature to obtain edge point sets and plane point sets;

[0020] According to any current edge point in the point cloud of the current frame and the first target point in the point cloud of the previous frame, a point-line inter-frame registration constraint is constructed, wherein the current edge point belongs to the edge point set, and the first target point includes the two points closest to the current edge point;

[0021] According to any current plane point in the point cloud of the current frame and the second target point in the point cloud of the previous frame, a point-plane frame registration constraint is constructed, wherein the current plane point belongs to the plane point set, and the second target point includes the three points closest to the current edge point;

[0022] The point-line inter-frame registration constraint and the point-plane inter-frame registration constraint are used as the inter-frame registration constraints.

[0023] In an embodiment of the present application, the multi-frame point cloud collected by the laser radar is classified based on curvature to obtain edge point sets and plane point sets, including:

[0024] Performing regional division on the multi-frame point cloud collected by the laser radar, wherein each frame of the point cloud includes multiple points;

[0025] Determine the curvature at all points;

[0026] Selecting a first preset number of points with the largest curvature in each region as edge points to obtain an edge point set;

[0027] In each region, a second preset number of points with the smallest curvature are selected as plane points to obtain a plane point set.

[0028] In the embodiment of the present application, the multi-frame point cloud collected by the laser radar is divided into regions, including:

[0029] Mapping the multi-frame point cloud collected by the laser radar to the global coordinate system to obtain the coordinates of each point in the global coordinate system, wherein the global coordinate system is established according to the coordinate detection device;

[0030] According to the coordinates of each point in the global coordinate system, determine the number of channels and relative scanning time of each point;

[0031] All points are divided into regions according to the number of channels and relative scanning time.

[0032] In the embodiment of the present application, the point cloud constraint includes a plane constraint. According to the multi-frame point cloud collected by the laser radar, the point cloud constraint for the posture of the working machine is determined, including:

[0033] Extract the preset range of the current frame point cloud collected by the laser radar to obtain a local point cloud;

[0034] According to the plane equation of the previous frame point cloud and the inter-frame pose, the first plane hyperparameter of the current frame point cloud is solved;

[0035] According to the points in the local point cloud, determine the initial parameters of the plane of the current frame point cloud;

[0036] Optimize the initial parameters of the plane to obtain the second plane hyperparameters of the current frame point cloud observation;

[0037] A plane constraint is constructed based on the first plane hyperparameter and the second plane hyperparameter.

[0038] In an embodiment of the present application, the laser radar includes an inertial measurement unit, and the method further includes:

[0039] Pre-integrate the inertial measurement unit and determine the inertial navigation pre-integration constraints;

[0040] According to the visual constraints and point cloud constraints, the second pose of the operating machine is determined, including:

[0041] The second pose of the operating machine is determined based on visual constraints, point cloud constraints and inertial navigation pre-integration constraints.

[0042] A second aspect of the present application provides a posture determination device based on an image and a point cloud, comprising:

[0043] a memory configured to store instructions;

[0044] The processor is configured to call instructions from the memory and implement the aforementioned first aspect of the method for determining the pose based on images and point clouds when executing the instructions.

[0045] A third aspect of the present application provides a working machine, comprising:

[0046] The device for determining a position and posture based on an image and a point cloud according to the second aspect;

[0047] LiDAR;

[0048] Image acquisition device.

[0049] A fourth aspect of the present application provides a machine-readable storage medium having instructions stored thereon, the instructions being used to enable a machine to execute the image and point cloud-based pose determination method of the first aspect described above.

[0050] Through the above technical solution, the first pose of the image acquisition device is determined based on the visual identification, and then the visual constraint on the second pose of the operating machine is determined based on the first pose and the first external parameter. Due to the introduction of visual constraints, the cumulative error and environmental degradation scenes can be effectively reduced, and more robust, stable and accurate pose results can be obtained. Furthermore, multi-source information constraints of visual constraints and point cloud constraints are constructed, which can eliminate the limitations of single information constraints and combine the advantages of various information sources to obtain more robust, stable and accurate pose results.

[0051] Other features and advantages of the embodiments of the present application will be described in detail in the subsequent specific implementation section. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] The accompanying drawings are used to provide a further understanding of the embodiments of the present application and constitute a part of the specification. Together with the following specific implementations, they are used to explain the embodiments of the present application, but do not constitute a limitation on the embodiments of the present application. In the accompanying drawings:

[0053] Figure 1 A schematic diagram of a process of determining a posture based on an image and a point cloud according to an embodiment of the present application is shown;

[0054] Figure 2 A schematic diagram of a structure for implementing step 120 according to an embodiment of the present application is shown;

[0055] Figure 3 The schematic diagram schematically shows the principle of the calibration method of the first external parameter according to the embodiment of the present application;

[0056] Figure 4 A schematic diagram of constructing a plane constraint according to an embodiment of the present application is schematically shown;

[0057] Figure 5 The structural block diagram of a posture determination device based on image and point cloud according to an embodiment of the present application is schematically shown. DETAILED DESCRIPTION

[0058] In order to make the purpose, technical scheme and advantages of the embodiments of the present application clearer, the technical scheme in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. It should be understood that the specific implementation methods described herein are only used to illustrate and explain the embodiments of the present application, and are not used to limit the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this application.

[0059] It should be noted that if the embodiments of the present application involve directional indications (such as up, down, left, right, front, back...), such directional indications are only used to explain the relative position relationship, movement status, etc. between the components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly.

[0060] In addition, if there are descriptions involving "first", "second", etc. in the embodiments of the present application, the descriptions of "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or suggesting their relative importance or implicitly indicating the number of technical features indicated. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In addition, the technical solutions between the various embodiments can be combined with each other, but they must be based on the ability of ordinary technicians in the field to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such combination of technical solutions does not exist and is not within the scope of protection required by this application.

[0061] Figure 1 The following schematically shows a flow chart of a method for determining a posture based on an image and a point cloud according to an embodiment of the present application. Figure 1 As shown, an embodiment of the present application provides a posture determination method based on images and point clouds, which is applied to an operating machine, wherein the operating machine includes a laser radar and an image acquisition device. The method may include the following steps.

[0062] Step 110: Determine the first position of the image acquisition device based on the visual identifier.

[0063] The visual identifier can be fixedly set in the scene in the forward direction of the working machine, for example, it can be set on a wall or the ground. The visual identifier can be a QR code or an LED light board, etc. The number of visual identifiers can be determined according to the range of motion. By collecting an image including the visual identifier, the first position of the image acquisition device can be determined. It can be understood that the image acquisition device can be any one of a monocular camera, a binocular camera, and a depth camera, and the embodiments of the present application are not limited to this.

[0064] Schematically, the visual identification provided in the embodiment of the present application is a metal plate, on which a plurality of light-emitting devices are mounted. The metal plate may be an aluminum alloy plate, and the light-emitting devices may be LED lamps, the number of which may be four. The embodiment of the present application is described with the above example.

[0065] In an optional implementation, step 110 includes:

[0066] capturing images including visual identifiers;

[0067] According to the image, determine the coordinates of multiple key points in the visual identification;

[0068] The first position of the image acquisition device is obtained by performing registration according to the coordinates of multiple key points.

[0069] Illustratively, an embodiment of the present application provides an implementation form of determining the first pose of an image acquisition device based on a visual identifier. Specifically, the image acquisition device acquires an image including a visual identifier, and based on the image, the coordinates of key points in the image corresponding to the visual identifier can be obtained.

[0070] It should be noted that the embodiment of the present application uses image segmentation to obtain key point coordinates, and the key point coordinates are taken as the center of each LED lamp as an example for explanation. The four LED lamps in the image are segmented to obtain the boundary of each LED lamp, and then each boundary is fitted to determine whether each fitted figure matches the corresponding LED lamp figure. The LED lamp used in the embodiment of the present application is circular. If the fitted figure is circular, it means that the position of the LED lamp in the image has been determined, and the pixel coordinates of the center of each LED lamp in the image can be solved.

[0071] If the coordinates of the centers of the four LED lights are obtained in the image, the visual constraints of the frame image are considered to be available. Based on the pixel coordinates of the centers of the four LED lights in the image, the coordinates of the four LED lights in the visual identification coordinate system are PNP (Perspective-n-Point, multi-point perspective imaging) aligned to obtain the first position of the image acquisition device. It can be understood that the visual identification coordinate system is a coordinate system established based on the visual identification, and the first position is the position of the image acquisition device relative to the visual identification coordinate system.

[0072] Step 120: Determine a visual constraint on a second posture of the working machine based on the first posture and a first external parameter, wherein the first external parameter is an external parameter of a pre-calibrated image acquisition device relative to a laser radar.

[0073] In the embodiments of this application, please refer to Figure 2 , Figure 2 A schematic diagram of a structure for implementing step 120 according to an embodiment of the present application is shown schematically. Figure 2 As shown in the figure, the coordinate detection device 210, the image acquisition device 220 and the visual identification 230 are included. The visual identification 230 includes four LED lights, and the centers of each LED light are respectively point G, point H, point I and point J. A visual identification coordinate system is established based on the visual identification 230, with point K as the coordinate origin and the direction from point H to point G as x. S Axis, the direction from point H to point I is y S The z axis is perpendicular to the plane where the visual mark is located and points to the image acquisition device. S Axis, to make the diagram clearer, Figure 2 The middle coordinate detection device 210 is simplified as a point, and the z S Axis. Point G and point H are at x S The distance between point H and point I in the y-axis direction S If the distances in the axial direction are the same, the coordinates of the center points G, H, I and J of the four LED lights relative to the visual identification coordinate system are G S =(h / 2, -h / 2, 0), H S =(-h / 2,-h / 2,0),I S =(-h / 2, h / 2, 0), J S =(h / 2, h / 2, 0). Where h represents the distance between two adjacent LED lights.

[0074] Calibrate the external parameters of the visual marker 230 relative to the coordinate detection device 210. Use the coordinate detection device 210 to obtain the coordinates of the centers of the four LED lights relative to the coordinate detection device 210. Then the coordinate axis x in the visual marker coordinate system is S Axis, y S Axis and z S The direction vector of the axis relative to the coordinate detection device 210 is k=G W -H W ,m=I W -H W , l = k m,G W represents the coordinates of point G relative to the coordinate detection device 210, H Wrepresents the coordinates of point H relative to the coordinate detection device 210, I W represents the coordinates of point I relative to the coordinate detection device 210. Then the direction vector is normalized to obtain the rotation matrix of the visual marker 230 relative to the coordinate detection device 210, which can be expressed by the following formula (1):

[0075] (1)

[0076] In the formula, R s represents the rotation matrix of the visual marker 230 relative to the coordinate detection device 210, ( k x , k y , k z ) represents the coordinate axis x in the visual identification coordinate system s The direction vector of the axis relative to the coordinate detection device 210, ( m x , m y , m z ) represents the coordinate axis y in the visual identification coordinate system s The direction vector of the axis relative to the coordinate detection device 210, ( l x , l y , l z ) represents the coordinate axis z in the visual identity coordinate system s The direction vector of the axis relative to the coordinate detection device 210.

[0077] The translation vector of the visual marker 230 relative to the coordinate detection device 210 is the average of the centers of the four LED lights, which can be expressed by the following formula (2):

[0078] (2)

[0079] In the formula, t s represents the translation vector of the visual marker 230 relative to the coordinate detection device 210, ( ) represents the coordinates of point G relative to the coordinate detection device 210, ( ) represents the coordinates of point H relative to the coordinate detection device 210, ( ) represents the coordinates of point I relative to the coordinate detection device 210, ( ) represents the coordinates of point J relative to the coordinate detection device 210.

[0080] Thus, the visual marker extrinsic parameter T2 of the visual marker 230 relative to the coordinate detection device 210 is obtained = [R s , t s ] By combining the first pose, the first external reference and the visual marker external reference, we can get the absolute pose constraint , the visual constraint is = ,in, T is the second posture of the operating machine to be solved.

[0081] The embodiment of the present application introduces visual identification constraints, which can effectively reduce the cumulative error and environmental degradation scenarios, and obtain more robust and stable posture results.

[0082] In an optional implementation manner, the calibration method of the first external parameter includes:

[0083] Determine respectively a first direction vector and a second direction vector of the image acquisition device and the laser radar relative to the coordinate detection device;

[0084] According to the first direction vector and the second direction vector, respectively determine a second external parameter and a third external parameter of the image acquisition device and the laser radar relative to the coordinate detection device;

[0085] The first external parameter is determined according to the second external parameter and the third external parameter.

[0086] In the embodiment of the present application, the coordinate detection device may be a total station, namely an Electronic Tachometer Totalstation, which is an intelligent surveying and mapping instrument integrating optics, mechanics, and computers, and the data results obtained are highly accurate.

[0087] Please also read Figure 3 , Figure 3 The schematic diagram schematically shows the principle of the calibration method of the first external parameter according to the embodiment of the present application. Figure 3 As shown, the figure includes a coordinate detection device 210, an image acquisition device 220, a laser radar 240 and a calibration device 250, and the image acquisition device 220 and the laser radar 240 are both arranged in the calibration device 250. The coordinate detection device 210 is used to establish a first coordinate system of the coordinate detection device, the image acquisition device 220 is used to establish a second coordinate system of the image acquisition device, and the calibration device 250 arranged in the laser radar 240 is used to establish a third coordinate system of the laser radar 240. Point P represents the optical center of the image acquisition device 220, which is located on the z1 axis of the second coordinate system; point O represents a point on the calibration device 250. In order to make the diagram clearer, the coordinate detection device 210 and the image acquisition device 220 are simplified to one point, that is, the origin of each coordinate system, and the calibration device 250 corresponds one-to-one to the three coordinate axes of the third coordinate system.

[0088] The coordinates of the three coordinate systems are measured by using the coordinate detection device 210. The coordinates of the three coordinate systems are A and B on the x-axis, C and D on the y-axis, and E and F on the z-axis. The direction vectors of the three coordinate systems x, y, and z axes relative to the first coordinate system are n=BA, p=DC, and r=FE. Similarly, the direction vectors of the two coordinate systems x1, y1, and z1 axes relative to the first coordinate system are n1=r, p1=-p, and r1=r. The two direction vectors n, p, r, n1, p1, and r1 are normalized.

[0089] The six direction vectors mentioned above can be used to solve the rotation matrix R of the laser radar 240 relative to the coordinate detection device 210 and the rotation matrix R of the image acquisition device 220 relative to the coordinate detection device 210. v .

[0090] By measuring the coordinates of point O using the coordinate detection device 210, the translation vector t=[O x , O y， O z -ß] T , where (O x , O y , O z ) represents the coordinates of point O; ß represents the distance from point O to the center of the laser radar, which can be inferred by the mechanical model of the calibration device 250.

[0091] The coordinates of point P are measured by the coordinate detection device 210, and the translation vector t of the image acquisition device 220 relative to the coordinate detection device 210 can be obtained. v =[p x , p y， p z ] T , where (p x , p y , p z ) represents the coordinates of point P.

[0092] Thus, the external parameter T1=[R, t] of the laser radar 240 relative to the coordinate detection device 210 and the external parameter T of the image acquisition device 220 relative to the coordinate detection device 210 are obtained. v =[R v , t v ].

[0093] Furthermore, the external parameter T of the image acquisition device 220 relative to the laser radar 240 can be obtained. c_to_l =T1 -1 *Tv.

[0094] It is understandable that a similar method can be used for other sensors to obtain the external parameters of the sensor relative to the lidar, and the embodiments of the present application will not be described in detail one by one.

[0095] The embodiment of the present application provides an external parameter calibration method based on a coordinate detection device, which can obtain millimeter-level external parameter calibration results and unify various information sources into the same coordinate system.

[0096] Step 130: Determine the point cloud constraints on the position and posture of the working machine based on the multi-frame point cloud collected by the laser radar.

[0097] Based on the Time of Flight (TOF) measurement method, the laser radar can analyze the reflected energy size, amplitude, frequency and phase of the reflected spectrum of the target object surface, and output a point cloud, thereby presenting the accurate three-dimensional structural information of the target object. The multi-frame point cloud collected by the laser radar can determine the point cloud constraints for the posture of the operating machinery. The point cloud constraints provided in the embodiments of the present application may include inter-frame registration constraints and / or plane constraints, and these two constraints will be specifically introduced below.

[0098] In an optional implementation, the point cloud constraint includes an inter-frame registration constraint, and the point cloud constraint for the position and posture of the working machine is determined based on the multi-frame point cloud collected by the laser radar, including:

[0099] The multi-frame point clouds collected by the LiDAR are classified based on curvature to obtain edge point sets and plane point sets;

[0100] According to any current edge point in the point cloud of the current frame and the first target point in the point cloud of the previous frame, a point-line inter-frame registration constraint is constructed, wherein the current edge point belongs to the edge point set, and the first target point includes the two points closest to the current edge point;

[0101] According to any current plane point in the point cloud of the current frame and the second target point in the point cloud of the previous frame, a point-plane frame registration constraint is constructed, wherein the current plane point belongs to the plane point set, and the second target point includes the three points closest to the current edge point;

[0102] The point-line inter-frame registration constraint and the point-plane inter-frame registration constraint are used as the inter-frame registration constraints.

[0103] The embodiment of the present application takes a mechanical scanning laser radar as an example. The edge point set and the plane point set are extracted from the multi-frame cloud collected by the laser radar, and then the point-line frame registration constraint is constructed based on the distance from the point to the line, and the point-plane frame registration constraint is constructed based on the distance from the point to the plane, and the point-line frame registration constraint and the point-plane frame registration constraint are used as the frame registration constraint.

[0104] Specifically, for any current edge point in the current frame point cloudp k , select the two points closest to the current edge point in the previous frame point cloud p l and p m , assuming that these three points are distributed in a straight line, then the point-line frame registration constraint is constructed, which can be expressed as the following formula (3):

[0105] (3)

[0106] In the formula, represents the point-line frame registration constraint, p k Represents the coordinates of any current edge point in the current frame point cloud, p l and p m Represents the coordinates of the two points in the previous frame point cloud that are closest to the current edge point.

[0107] For any current plane point in the current frame point cloud p q , select the three points closest to the current edge point in the previous frame point cloud p n , p o and p p , assuming that these three points are distributed in a plane, then the point-plane frame registration constraint is constructed, which can be expressed as the following formula (4):

[0108] (4)

[0109] In the formula, represents the point-plane frame registration constraint, p q Represents the coordinates of any current plane point in the current frame point cloud, p n , p o and p p Represents the coordinates of the three points in the point cloud of the previous frame that are closest to the current edge point.

[0110] In an optional implementation, the multi-frame point cloud collected by the laser radar is classified based on curvature to obtain edge point sets and plane point sets, including:

[0111] Performing regional division on the multi-frame point cloud collected by the laser radar, wherein each frame of the point cloud includes multiple points;

[0112] Determine the curvature at all points;

[0113] Selecting a first preset number of points with the largest curvature in each region as edge points to obtain an edge point set;

[0114] In each region, a second preset number of points with the smallest curvature are selected as plane points to obtain a plane point set.

[0115] The embodiment of the present application schematically provides a method for classifying point clouds based on curvature. Schematically, the multi-frame point cloud collected by the laser radar is divided into regions to obtain multiple regions. It can be understood that the point cloud is essentially a data set composed of points. Therefore, each frame of the point cloud includes multiple points, and each divided region contains multiple points. For each point, the curvature needs to be calculated, which can be calculated according to the following formula (5):

[0116] (5)

[0117] In the formula, c represents the curvature, s Indicates the number of nearby points selected for calculating curvature at the current point. p i Represents the coordinates of the current point in the laser radar coordinate system, p j Represents the coordinates of the selected point in the laser radar coordinate system, l Represents the lidar coordinate system.

[0118] All points are sorted according to the curvature, and a first preset number of points with the largest curvature are selected in each region as edge points as an edge point set, and a second preset number of points with the smallest curvature are selected in each region as plane points as a plane point set. It can be understood that the first preset number and the second preset number can be set according to actual needs, and the embodiment of the present application does not limit this.

[0119] In an embodiment of the present application, by calculating the curvature characteristics of the point cloud, valuable registration points that can provide constraints are found in each frame of the point cloud, thereby significantly reducing the number of point clouds participating in the inter-frame registration, while taking into account the computational efficiency and obtaining a higher registration accuracy.

[0120] In an optional implementation, the multi-frame point cloud collected by the laser radar is divided into regions, including:

[0121] Mapping the multi-frame point cloud collected by the laser radar to the global coordinate system to obtain the coordinates of each point in the global coordinate system, wherein the global coordinate system is established according to the coordinate detection device;

[0122] According to the coordinates of each point in the global coordinate system, determine the number of channels and relative scanning time of each point;

[0123] All points are divided into regions according to the number of channels and relative scanning time.

[0124] The embodiment of the present application schematically provides a method for dividing a point cloud into regions based on the number of channels and the relative scanning time. Specifically, the point cloud is first mapped to a global coordinate system to obtain the coordinates of each point in the global coordinate system. The global coordinate system can be established according to the aforementioned coordinate detection device. The external parameters of the laser radar relative to the global coordinate system can be used for mapping.

[0125] In order to speed up data processing, it is necessary to sort all the points that are inherently disordered and store the chaotic point cloud data in time and space order. Based on the coordinates of each point in the global coordinate system, the number of channels and relative scanning time of each point are determined. Schematically, the number of channels and relative scanning time can be expressed as the following formula (6):

[0126] (6)

[0127] In the formula, m Indicates the number of channels, t Indicates the relative scan time, p x , p y , p z Represents the coordinates of each point in the global coordinate system.

[0128] In order to ensure that the environmental information can be expressed to the maximum extent and the environmental features can be retained, each frame of the point cloud is divided into four sub-areas s1, s2, s3 and s4 according to the channel and scanning order. In schematic form, the division can be based on the horizontal field of view, that is, based on the following formula (7):

[0129] (7)

[0130] In an optional implementation, the point cloud constraint includes a plane constraint, and the point cloud constraint for the position and posture of the working machine is determined based on the multi-frame point cloud collected by the laser radar, including:

[0131] Extract the preset range of the current frame point cloud collected by the laser radar to obtain a local point cloud;

[0132] According to the plane equation of the previous frame point cloud and the inter-frame pose, the first plane hyperparameter of the current frame point cloud is solved;

[0133] According to the points in the local point cloud, determine the initial parameters of the plane of the current frame point cloud;

[0134] Optimize the initial parameters of the plane to obtain the second plane hyperparameters of the current frame point cloud observation;

[0135] A plane constraint is constructed based on the first plane hyperparameter and the second plane hyperparameter.

[0136] Specifically, the current frame point cloud may be first subjected to a through filtering process, and then a preset range in the current frame point cloud may be extracted to obtain a local point cloud Q.

[0137] For the previous frame point cloud, assume is the ground point in the point cloud of the previous frame, is the ground point in the point cloud of the current frame, then Satisfies the plane equation, that is, the following formula (8):

[0138] (8)

[0139] In the formula, Represents the coordinates of the ground point of the previous frame point cloud, Represents the ground normal vector of the previous frame point cloud, Indicates the distance from the origin to the ground in the point cloud of the previous frame.

[0140] If the inter-frame pose is known, that is, the following formula (9):

[0141] (9)

[0142] In the formula, Represents the coordinates of the ground point of the previous frame point cloud, Represents the coordinates of the ground point of the current frame point cloud, and Represents the rotation matrix and translation vector of the current frame relative to the previous frame.

[0143] Combining formula (8) and formula (9), we can get the following formula (10):

[0144] (10)

[0145] Based on formula (10), the ground normal vector of the current frame point cloud can be obtained: = And the distance from the origin to the ground in the current frame point cloud - , and then get the first plane hyperparameter of the current frame point cloud = .

[0146] The points in the local point cloud Q are extracted based on RANSAC (RANdom SAmple Consensus) to obtain the initial parameters of the plane of the current frame point cloud. w 0Since the initial parameters of the plane may contain noise, the least squares problem can be constructed by using the minimum distance from the ground point to the plane, and then the initial parameters of the plane can be optimized to obtain the second plane hyperparameters of the current frame point cloud observation, that is, the following formula (11):

[0147] (11)

[0148] in,

[0149] In the formula, represents the second plane hyperparameter, p n Represents each point in the point cloud of the current frame, w Represents the second plane hyperparameter to be optimized.

[0150] Please combine Figure 4 , Figure 4 A schematic diagram of constructing a plane constraint according to an embodiment of the present application is shown schematically. Figure 4 As shown, the first plane hyperparameters of the current frame are obtained by computing the point cloud of the previous frame w b , and the second plane hyperparameters of the current frame point cloud observation , the plane constraint can be constructed, that is, the following formula (12):

[0151] = (12)

[0152] in, =

[0153] In the formula, represents a plane constraint, w b represents the first plane hyperparameter, represents the second plane hyperparameter.

[0154] In the embodiment of the present application, it is assumed that the ground within a small area of ​​the current frame point cloud is an absolute single plane, and a plane constraint is constructed thereby to obtain a robust and accurate pose result.

[0155] Step 140: Determine the second posture of the working machine according to the visual constraints and the point cloud constraints.

[0156] The visual constraints and point cloud constraints are fused, and the Gauss-Newton minimization error function is used. The second posture of the operating machine is taken as the goal to minimize the fused constraints, and the second posture of the operating machine can be obtained.

[0157] The embodiments of the present application provide several schematic illustrations of solutions for the second posture of the working machine.

[0158] (1) When the point cloud constraint includes the inter-frame registration constraint, the solution of the second pose of the working machine can refer to the following formula (13):

[0159] (13)

[0160] In the formula, e( · ) represents the Gauss-Newton minimization error function, represents the point cloud constraint including the inter-frame registration constraint. represents the point cloud constraint including the inter-frame registration constraint. Represents visual constraints, i It is a local point cloud Q These three constraints are all about the second posture of the working machine. T function.

[0161] (2) When the point cloud constraint includes a plane constraint, the solution of the second pose of the working machine can refer to the following formula (14):

[0162] (14)

[0163] In the formula, e( · ) represents the Gauss-Newton minimization error function, Indicates the plane constraints included in the point cloud constraints, Represents visual constraints, i It is a local point cloud Q These two constraints are about the second posture of the working machine. T function.

[0164] (3) When the point cloud constraints include both inter-frame registration constraints and plane constraints, the solution of the second pose of the working machine can refer to the following formula (15):

[0165] (15)

[0166] In the formula, e( · ) represents the Gauss-Newton minimization error function, Indicates that the point cloud constraint includes the point-line inter-frame registration constraint, Indicates that the point cloud constraint includes the point-surface inter-frame registration constraint, Indicates the plane constraints included in the point cloud constraints, Represents visual constraints, i It is a local point cloudQ These four constraints are all about the second posture of the working machine. T function.

[0167] In the field of operating machinery, the operating conditions are generally complex, and a single sensor is often unable to meet its usage requirements. Therefore, the posture determination method based on images and point clouds provided in the embodiments of the present application constructs multi-source information constraints of visual constraints and point cloud constraints, which can eliminate the limitations of single information constraints, combine the advantages of various information sources, and obtain more robust and stable posture results, significantly improving the adaptability to complex working environments. Furthermore, due to the improvement of the accuracy of the posture, the accuracy of the map of the environment in which the operating machinery is located can also be improved. In addition, path planning can be performed based on the map to obtain the optimal path, realize autonomous navigation of the operating machinery, and improve the intelligence level of the operating machinery.

[0168] On this basis, in an optional implementation manner, the laser radar includes an inertial measurement unit, and the method further includes:

[0169] Pre-integrate the inertial measurement unit and determine the inertial navigation pre-integration constraints;

[0170] According to the visual constraints and point cloud constraints, the second pose of the operating machine is determined, including:

[0171] The second pose of the operating machine is determined based on visual constraints, point cloud constraints and inertial navigation pre-integration constraints.

[0172] The positioning method based on LiDAR is greatly affected by the environment. Therefore, in order to improve the stability of the positioning system, inertial navigation pre-integration constraints are added to further calibrate the posture. Inertial Measurement Unit (IMU) is a device that measures the three-axis attitude angle and acceleration of an object.

[0173] In order to avoid the need for re-integration each time after optimizing the pose, the inertial measurement unit needs to be pre-integrated to calculate the inertial navigation pre-integration constraints of adjacent frames of the point cloud.

[0174] In this case, the solution of the second posture of the working machine can refer to the following formula (16):

[0175] (16)

[0176] In the formula, e( · ) represents the Gauss-Newton minimization error function, Indicates that the point cloud constraint includes the point-line inter-frame registration constraint, Indicates that the point cloud constraint includes the point-surface inter-frame registration constraint, Indicates the plane constraints included in the point cloud constraints, represents the inertial navigation pre-integration constraint, Represents visual constraints, i It is a local point cloud Q These five constraints are all about the second posture of the working machine. T function.

[0177] In the embodiment of the present application, on the basis of visual constraints and point cloud constraints, the second posture of the working machine is determined according to the inertial navigation pre-integration constraints, thereby further improving the accuracy of the posture results.

[0178] The method for determining the pose based on images and point clouds provided in the embodiment of the present application determines the first pose of the image acquisition device based on the visual identifier, and then determines the visual constraint on the second pose of the operating machine based on the first pose and the first external parameter. Due to the introduction of visual constraints, it can effectively reduce the cumulative error and environmental degradation scenes, and obtain more robust, stable and accurate pose results. Furthermore, multi-source information constraints of visual constraints and point cloud constraints are constructed, which can eliminate the limitations of single information constraints and combine the advantages of various information sources to obtain more robust, stable and accurate pose results.

[0179] Figure 5 The structure block diagram of a device for determining a posture based on an image and a point cloud according to an embodiment of the present application is schematically shown. Figure 5 As shown, an embodiment of the present application provides a posture determination device based on an image and a point cloud, which may include:

[0180] Memory 510, configured to store instructions;

[0181] The processor 520 is configured to call instructions from the memory 510 and implement the above-mentioned method for determining the pose based on the image and point cloud when executing the instructions.

[0182] The image and point cloud-based posture determination device provided in the embodiment of the present application can implement each process of the image and point cloud-based posture determination method in the method embodiment, and can achieve the same technical effect. To avoid repetition, it will not be repeated here.

[0183] The present application also provides a working machine, which may include:

[0184] The aforementioned image and point cloud based posture determination device;

[0185] LiDAR;

[0186] Image acquisition device.

[0187] It is understandable that the operating machinery may be engineering machinery, agricultural machinery or an operating robot, etc.

[0188] An embodiment of the present application also provides a machine-readable storage medium, on which instructions are stored, and the instructions are used to enable a machine to execute the above-mentioned image and point cloud-based posture determination method.

[0189] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems or computer program products. Therefore, the present application may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present application may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program codes.

[0190] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of the processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0191] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0192] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0193] In a typical configuration, a computing device includes one or more processors (CPU), input / output interfaces, network interfaces, and memory.

[0194] The memory may include non-permanent memory in a computer-readable medium, random access memory (RAM) and / or non-volatile memory in the form of read-only memory (ROM) or flash RAM. The memory is an example of a computer-readable medium.

[0195] Computer readable media include permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. Information can be computer readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disk read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassettes, magnetic tape disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer readable media does not include temporary computer readable media (transitory media), such as modulated data signals and carrier waves.

[0196] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0197] The above are only embodiments of the present application and are not intended to limit the present application. For those skilled in the art, the present application may have various changes and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included within the scope of the claims of the present application.

Claims

1. A method for determining a pose based on an image and a point cloud, characterized in that: Applied to an operating machine, the operating machine includes a laser radar and an image acquisition device, and the method includes: Based on the visual identification, determining a first position of the image acquisition device; Determine a visual constraint on a second posture of the working machine according to the first posture, a first external parameter and a visual identification external parameter, wherein the first external parameter is a pre-calibrated external parameter of the image acquisition device relative to the laser radar, and the visual identification external parameter is a pre-calibrated external parameter of the visual identification relative to a coordinate detection device; Determining point cloud constraints on the position and posture of the working machine according to the multi-frame point cloud collected by the laser radar; A second posture of the working machine is determined according to the visual constraint and the point cloud constraint.

2. The method according to claim 1, characterized in that The step of determining the first position of the image acquisition device based on the visual identifier includes: capturing an image including the visual identifier; Determining, based on the image, coordinates of a plurality of key points in the visual identifier; The first position of the image acquisition device is obtained by performing registration according to the coordinates of the multiple key points.

3. The method according to claim 1, characterized in that The calibration method of the first external parameter includes: Respectively determining a first direction vector and a second direction vector of the image acquisition device and the laser radar relative to a coordinate detection device; Determine, according to the first direction vector and the second direction vector, a second external parameter and a third external parameter of the image acquisition device and the laser radar relative to the coordinate detection device, respectively; The first external parameter is determined according to the second external parameter and the third external parameter.

4. The method according to claim 1, characterized in that: The point cloud constraint includes an inter-frame registration constraint, and the point cloud constraint for the position and posture of the working machine is determined based on the multi-frame point cloud collected by the laser radar, including: Classifying the multi-frame point clouds collected by the laser radar based on curvature to obtain edge point sets and plane point sets; According to any current edge point in the point cloud of the current frame and the first target point in the point cloud of the previous frame, constructing a point-line inter-frame registration constraint, wherein the current edge point belongs to the edge point set, and the first target point includes two points closest to the current edge point; According to any current plane point in the current frame point cloud and a second target point in the previous frame point cloud, constructing a point-plane frame registration constraint, wherein the current plane point belongs to the plane point set, and the second target point includes three points closest to the current edge point; The point-line inter-frame registration constraint and the point-plane inter-frame registration constraint are used as inter-frame registration constraints.

5. The method according to claim 4, characterized in that The multi-frame point cloud collected by the laser radar is classified based on curvature to obtain edge point sets and plane point sets, including: Dividing the multiple frames of point cloud collected by the laser radar into regions, wherein each frame of the point cloud includes multiple points; determining the curvature of all said points; Selecting a first preset number of points with the largest curvature in each region as edge points to obtain an edge point set; In each region, a second preset number of points with the smallest curvature are selected as plane points to obtain a plane point set.

6. The method according to claim 5, characterized in that The step of dividing the multi-frame point cloud collected by the laser radar into regions includes: Mapping the multi-frame point cloud collected by the laser radar to a global coordinate system to obtain the coordinates of each point in the global coordinate system, wherein the global coordinate system is established according to a coordinate detection device; Determine the number of channels and relative scanning time of each point according to the coordinates of each point in the global coordinate system; All points are divided into regions according to the number of channels and the relative scanning time.

7. The method according to any one of claims 1, 4-6, characterized in that: The point cloud constraint includes a plane constraint, and the point cloud constraint for the position and posture of the working machine is determined based on the multi-frame point cloud collected by the laser radar, including: Extracting a preset range of the current frame point cloud collected by the laser radar to obtain a local point cloud; According to the plane equation of the previous frame point cloud and the inter-frame pose, the first plane hyperparameter of the current frame point cloud is solved; Determining initial plane parameters of the current frame point cloud according to the points in the local point cloud; Optimizing the initial parameters of the plane to obtain the second plane hyperparameters of the current frame point cloud observation; A plane constraint is constructed according to the first plane hyperparameter and the second plane hyperparameter.

8. The method according to claim 1, characterized in that The laser radar includes an inertial measurement unit, and the method further includes: Pre-integrating the inertial measurement unit to determine inertial navigation pre-integration constraints; Determining the second posture of the working machine according to the visual constraint and the point cloud constraint includes: A second posture of the working machine is determined according to the visual constraint, the point cloud constraint and the inertial navigation pre-integration constraint.

9. A posture determination device based on image and point cloud, characterized in that: include: a memory configured to store instructions; A processor is configured to call the instructions from the memory and implement the method for determining a pose based on an image and a point cloud according to any one of claims 1 to 8 when executing the instructions.

10. A working machine, characterized in that: include: The apparatus for determining a position and posture based on an image and a point cloud according to claim 9; LiDAR; Image acquisition device.

11. A machine-readable storage medium, characterized in that: The machine-readable storage medium stores instructions for causing a machine to execute the method for determining a pose based on an image and a point cloud according to any one of claims 1 to 8.

Images