Obstacle position detection method, device, control equipment and storage medium

Abstract

The embodiment of the present invention provides an obstacle position detection method, device, control equipment and storage medium, which relates to the field of computer technology. The method includes: obtaining an initial point cloud; determining the to-be-processed three-dimensional points of the body that do not belong to the carriage from the three-dimensional points contained in the initial point cloud; clustering the to-be-processed three-dimensional points to obtain at least one target three-dimensional point set; controlling the robot to travel to a detection position in the carriage that is adjacent to the spatial position represented by the obtained target three-dimensional point set, obtaining the image to be detected and the corresponding point cloud to be matched through a three-dimensional camera; detecting the image area occupied by the obstacle in the image to be detected; determining the spatial position represented by the three-dimensional points corresponding to the pixel points contained in the detected image area in the corresponding point cloud to be matched as the position of the obstacle in the carriage. The rough position of the obstacle is first detected based on the laser radar, and then the spatial position of the obstacle is detected based on the three-dimensional camera, so that the accurate position of the obstacle can be located.

Description

Technical Field

[0001] The present invention relates to the field of computer technology, and in particular to an obstacle position detection method, device, control equipment and storage medium. Background Art

[0002] In the fields of factories, warehouses, logistics, etc., with the shortage of loading and unloading workers and the rising labor costs, it has become a trend to use robots to replace manual labor for automated cargo loading and unloading and handling. For example, in the intelligent loading scenario, when the goods need to be stacked in the truck compartment, the space area inside the truck compartment can be determined by scanning equipment such as radar, and the space area inside the truck compartment can be used as the cargo stacking area; the cargo stacking position is planned for the stacking area, and then the cargo handling path is planned for the robot based on the planned cargo stacking position. Subsequently, the server controls the robot to move along the planned cargo handling path and stack the cargo in the planned cargo stacking position.

[0003] In the prior art, the spatial area inside the carriage is often directly determined as the cargo stacking area. However, in actual application scenarios, there may be obstacles such as baffles, latches, and air-conditioning units in the carriage, which may cause the planned cargo stacking position to interfere with the position of the obstacles. Stacking the goods at the cargo stacking position may cause the cargo to be placed unstable, and then cause damage to the cargo. The planned cargo handling path of the robot may also interfere with the position of the obstacle, and the robot may collide with the obstacle during movement, causing damage to the robot. Obviously, in order to avoid the above problems, it is necessary to determine the position of the obstacle in the truck compartment. Therefore, there is an urgent need for a detection method that can determine the position of obstacles in the truck compartment. Summary of the invention

[0004] The purpose of the embodiments of the present invention is to provide an obstacle position detection method, device, control equipment and storage medium to accurately locate the obstacle. The specific technical solution is as follows:

[0005] In a first aspect of an embodiment of the present invention, a method for detecting an obstacle position is first provided, the method comprising: obtaining an initial point cloud; wherein the initial point cloud is obtained by scanning the carriage outside the carriage by a laser radar in a robot when a target side of the carriage is closed and opened; the robot is also provided with a three-dimensional camera; three-dimensional points of a carriage body that do not belong to the carriage are determined from the three-dimensional points contained in the initial point cloud as three-dimensional points to be processed; the three-dimensional points to be processed are clustered to obtain at least one target three-dimensional point set; the robot is controlled to travel to a detection position in the carriage that is adjacent to the spatial position represented by the obtained target three-dimensional point set when the target side is open, and the spatial position represented by the obtained target three-dimensional point set is scanned by the three-dimensional camera to obtain an image to be detected and a corresponding point cloud to be matched; the image area occupied by the obstacle in the obtained image to be detected is detected; and the spatial position represented by the three-dimensional points corresponding to the pixel points contained in the detected image area in the corresponding point cloud to be matched is determined as the position of the obstacle in the carriage.

[0006] Optionally, the robot is controlled to drive to a detection position in the car that is adjacent to the spatial position represented by the obtained target three-dimensional point set when the target side is open, and the spatial position represented by the obtained target three-dimensional point set is scanned by the three-dimensional camera to obtain an image to be detected and a corresponding point cloud to be matched, including: determining a current spatial area to be detected from multiple spatial areas to be detected in a specified direction; wherein the multiple spatial areas to be detected are: multiple rectangular spatial areas obtained by dividing the spatial area in the car in the specified direction; the specified direction is the direction in which the face opposite to the target side points to the target side; when the current spatial area to be detected intersects with the spatial position represented by any obtained target three-dimensional point set, the robot is controlled to drive to a detection position at the specified side of the current spatial area to be detected when the target side is open, and obtain an image containing the current spatial area to be detected by the three-dimensional camera. and the point cloud of the current spatial area to be detected, respectively as the current image to be detected and the corresponding point cloud to be matched; wherein the designated side represents the side of the current spatial area to be detected close to the side of the target; the image area occupied by the obstacle in the detected image to be detected comprises: detecting the image area occupied by the obstacle in the current image to be detected as the current image area to be processed; the spatial position represented by the three-dimensional points corresponding to the pixel points contained in the detected image area in the corresponding point cloud to be matched is determined as the position of the obstacle in the compartment, comprising: determining the spatial position represented by the three-dimensional points corresponding to the pixel points contained in the current image area to be processed in the point cloud to be matched corresponding to the current image to be detected as the position of the obstacle in the current spatial area to be detected; and returning to execute the step of determining the current spatial area to be detected from multiple spatial areas to be detected in the designated direction until the position of the obstacle in the spatial area to be detected is determined, and the position of the obstacle in the compartment is obtained.

[0007] Optionally, the target side is one of the six body faces of the body; the origin of the robot coordinate system is located at the specified position of the robot; the position of the obstacle in the current space area to be detected is the position in the car coordinate system of the car; after determining the spatial position represented by the three-dimensional points corresponding to the pixel points contained in the current image area to be processed in the point cloud to be matched corresponding to the current image to be detected as the position of the obstacle in the current space area to be detected, the method further includes: based on the position of the obstacle in the current space area to be detected in the car coordinate system, the position of the current space area to be detected in the car coordinate system, and the sizes of various types of goods to be stacked in the current space area to be detected, planning the current space area to be detected in the car through a stacking generation algorithm The cargo stacking position in the coordinate system of the vehicle body; wherein, the position of the current space area to be detected in the vehicle body coordinate system is: obtained based on the distance between the vehicle body surface and the relative vehicle body surface; when the robot is parked at the detection position at the specified side of the current space area to be detected, based on the distance between the origin of the vehicle body coordinate system and the origin of the current robot coordinate system, and the angle between the vehicle body surface and the coordinate plane of the current robot coordinate system, the cargo stacking position in the current space area to be detected in the vehicle body coordinate system is converted to obtain the cargo stacking position in the current space area to be detected in the current robot coordinate system; the robot is controlled to stack the cargo according to the cargo stacking position in the current space area to be detected in the current robot coordinate system until the cargo stacking in the current space area to be detected is completed.

[0008] Optionally, the relative box faces are determined in the following manner: when the robot is docked outside the target side, the three-dimensional points contained in the initial point cloud are processed based on a plane segmentation algorithm to obtain plane equations of multiple box faces in the current robot coordinate system; wherein a plane equation in the current robot coordinate system is expressed in the following form: ax+by+cz+d=0; a represents the coefficient of the variable x; b represents the coefficient of the variable y; c represents the coefficient of the variable z; d represents the constant term in the plane equation; for two box faces, a×d corresponding to one box face>0, and , the corresponding a×d of the other box surface is <0, and , the two body surfaces are determined to be opposite body surfaces; wherein, represents the maximum angle allowed between the relative box surface and the yoz coordinate plane of the current robot coordinate system; o represents the origin of the current robot coordinate plane; for two box surfaces, b×d corresponding to one box surface>0, and , the other box surface corresponding to b×d<0, and , the two body surfaces are determined to be opposite body surfaces; wherein, Indicates the maximum angle allowed between the relative box surface and the zox coordinate plane of the current robot coordinate system; for two box surfaces, c×d>0 on one box surface, and , the c×d corresponding to the other box surface is <0, and , the two body surfaces are determined to be opposite body surfaces; wherein, Indicates the maximum angle allowed between the relative box surface and the xoy coordinate plane of the current robot coordinate system.

[0009] Optionally, the robot includes a robotic arm for grabbing goods; the three-dimensional camera is arranged at the end of the robotic arm; the acquisition of an image containing the current spatial area to be detected and a point cloud of the current spatial area to be detected, respectively serving as the current image to be detected and the corresponding point cloud to be matched, includes: acquiring an image in the direction of the current spatial area to be detected by the three-dimensional camera, and obtaining an image containing the current spatial area to be detected, as the current image to be detected; scanning the direction of the current spatial area to be detected by the three-dimensional camera, and determining the three-dimensional points belonging to the current spatial area to be detected from the scanning results, and obtaining the current point cloud to be matched.

[0010] Optionally, the three-dimensional points in the initial point cloud are three-dimensional points in the robot coordinate system; obtaining the initial point cloud includes: scanning the car body at multiple scanning positions outside the car body by the laser radar respectively, to obtain a point cloud to be converted corresponding to each scanning position; wherein the union of the scanning ranges at each scanning position can cover the spatial area occupied by the car body; for the point cloud to be converted corresponding to each scanning position, converting the three-dimensional points contained in the point cloud to be converted according to the target transformation relationship, to obtain the three-dimensional points in the initial point cloud; wherein the target transformation relationship represents: the transformation relationship between the radar coordinate system and the robot coordinate system.

[0011] Optionally, the robot includes a mechanical arm for grabbing goods; the laser radar is arranged at the end of the mechanical arm; the point cloud to be converted corresponding to each scanning position, the three-dimensional points contained in the point cloud to be converted are converted according to the target conversion relationship to obtain the three-dimensional points in the initial point cloud, including: for the point cloud to be converted corresponding to each scanning position, according to the preset conversion formula between the radar coordinate system and the robot coordinate system, the three-dimensional points contained in the point cloud to be converted are converted to obtain the three-dimensional points in the initial point cloud; wherein the preset conversion formula includes:

[0012] ;

[0013] st

[0014] ;

[0015] st

[0016] ;

[0017] ;

[0018] A first transformation matrix representing the radar coordinate system and the tool coordinate system of the manipulator; A second transformation matrix representing the tool coordinate system and the robot coordinate system; Represents the three-dimensional coordinates of the three-dimensional points contained in the point cloud to be converted; represents the three-dimensional coordinates of the three-dimensional points in the initial point cloud obtained by transformation; r represents the rotation matrix from the tool coordinate system to the robot coordinate system; t represents the translation matrix from the tool coordinate system to the robot coordinate system; represents the coordinates of the origin of the tool coordinate system in the robot coordinate system; represents the Euler angle of the tool coordinate system relative to the robot coordinate system.

[0019] Optionally, a three-dimensional point that does not belong to the body of the vehicle is determined from the three-dimensional points included in the initial point cloud as a three-dimensional point to be processed, including: processing the three-dimensional points included in the initial point cloud based on a plane segmentation algorithm to obtain multiple planes; for each three-dimensional point included in the initial point cloud, if the distance between the three-dimensional point and each obtained plane is not less than a preset distance, then the three-dimensional point is used as a three-dimensional point to be processed that does not belong to the body of the vehicle.

[0020] Optionally, clustering the to-be-processed 3D points to obtain at least one target 3D point set includes: taking any unclustered to-be-processed 3D point as the current cluster center; determining, from the currently unclustered to-be-processed 3D points, the to-be-processed 3D points that are within the extended space occupied by the candidate 3D point set to which the current cluster center belongs; adding the determined to-be-processed 3D points to the candidate 3D point set to which the current cluster center belongs, and returning to execute the step of determining, from the currently unclustered to-be-processed 3D points, the candidate 3D points that are located within the current cluster center. The step of selecting the to-be-processed 3D points within the outer expansion space occupied by the set until, among the currently unclustered to-be-processed 3D points, there are no to-be-processed 3D points within the outer expansion space occupied by the candidate 3D point set to which the current cluster center belongs; and returning to execute the step of taking any unclustered to-be-processed 3D point as the current cluster center until there are no unclustered to-be-processed 3D points; and determining, from the candidate 3D point sets to which each cluster center belongs, a candidate 3D point set whose number of included 3D points is not less than a preset number threshold as the target 3D point set.

[0021] Optionally, the detected image area occupied by the obstacle in the image to be detected comprises: inputting the obtained image to be detected into a pre-trained target detection model with DenseNet121 as the backbone network to obtain the image area occupied by the obstacle; the activation function in the DenseNet121 comprises a Mish function, and / or the number of output channels of each network layer of the DenseNet121 increases in the following manner:

[0022] ;

[0023] Where i=1, 2, ..., n; % represents the remainder operation. represents the number of output channels of the i-th network layer, n represents the number of network layers included in the DenseNet121; and / or, the target detection model is built based on the YOLOv4 network.

[0024] In a second aspect of an embodiment of the present invention, an obstacle position detection system is provided, which includes a control device and a robot, wherein: the control device is used to execute any obstacle position detection method described in the first aspect above; the robot is used to travel under the control of the control device and stack goods in a carriage.

[0025] In a third aspect of an embodiment of the present invention, an obstacle position detection device is provided, the device comprising: a first acquisition module, for acquiring an initial point cloud; wherein the initial point cloud is obtained by scanning the compartment outside the compartment through a laser radar in a robot when the target side of the compartment is closed and opened; the robot is also provided with a three-dimensional camera; a three-dimensional point determination module, for determining three-dimensional points of a compartment body that do not belong to the compartment from the three-dimensional points contained in the initial point cloud as three-dimensional points to be processed; a clustering module, for clustering the three-dimensional points to be processed to obtain at least one target three-dimensional point set; a second acquisition module, for controlling the robot to drive to a detection position in the compartment adjacent to a spatial position represented by the obtained target three-dimensional point set when the target side is open, and scanning the spatial position represented by the obtained target three-dimensional point set through the three-dimensional camera to obtain an image to be detected and a corresponding point cloud to be matched; a detection module, for detecting an image area occupied by an obstacle in the obtained image to be detected; a position determination module, for determining a spatial position represented by a three-dimensional point corresponding to a pixel point contained in the corresponding point cloud to be matched as the position of the obstacle in the compartment.

[0026] Optionally, the second acquisition module is specifically used to: determine the current spatial area to be detected from multiple spatial areas to be detected according to a specified direction; wherein the multiple spatial areas to be detected are: multiple rectangular spatial areas obtained by dividing the spatial area in the car in the specified direction; the specified direction is the direction in which the surface opposite to the target side points to the target side; when the current spatial area to be detected intersects with the spatial position represented by any obtained target three-dimensional point set, control the robot to drive to the detection position at the specified side of the current spatial area to be detected with the target side opened, and obtain an image containing the current spatial area to be detected and a point cloud of the current spatial area to be detected through the three-dimensional camera, respectively. as the current image to be detected and the corresponding point cloud to be matched; wherein the designated side represents the side of the current spatial area to be detected close to the side of the target; the detection module is specifically used to: detect the image area occupied by the obstacle in the current image to be detected as the current image area to be processed; the position determination module is specifically used to: determine the spatial position represented by the three-dimensional points corresponding to the pixel points contained in the current image area to be processed in the point cloud to be matched corresponding to the current image to be detected as the position of the obstacle in the current spatial area to be detected; and return to execute the step of determining the current spatial area to be detected from multiple spatial areas to be detected in the designated direction until the position of the obstacle in the spatial area to be detected is determined, and the position of the obstacle in the car is obtained.

[0027] Optionally, the target side is one of the six body faces of the body; the origin of the robot coordinate system is located at the specified position of the robot; the position of the obstacle in the current space area to be detected is the position in the car coordinate system of the car; the device also includes: a stacking planning module, which is used to execute the position determination module to determine the spatial position represented by the three-dimensional points corresponding to the pixel points contained in the current image area to be processed in the point cloud to be matched corresponding to the current image to be detected as the position of the obstacle in the current space area to be detected, and then execute the position of the obstacle in the current space area to be detected in the car coordinate system, the position of the current space area to be detected in the car coordinate system, and the sizes of various types of goods to be stacked in the current space area to be detected, and plan the current space area to be detected in the car through a stacking generation algorithm. The cargo stacking position in the coordinate system of the car; wherein the position of the current space area to be detected in the car coordinate system is: obtained based on the distance between the car body surface relative to the car; a conversion module, used to execute the detection position of the robot at the specified side of the current space area to be detected, based on the distance between the origin of the car coordinate system and the origin of the current robot coordinate system, and the angle between the car body surface of the car and the coordinate plane of the current robot coordinate system, to convert the cargo stacking position in the current space area to be detected in the car coordinate system, so as to obtain the cargo stacking position in the current space area to be detected in the current robot coordinate system; a control module, used to execute control of the robot to stack the cargo according to the cargo stacking position in the current space area to be detected in the current robot coordinate system until the cargo stacking is completed in the current space area to be detected.

[0028] Optionally, the relative box faces are determined in the following manner: when the robot is docked outside the target side, the three-dimensional points contained in the initial point cloud are processed based on a plane segmentation algorithm to obtain plane equations of multiple box faces in the current robot coordinate system; wherein a plane equation in the current robot coordinate system is expressed in the following form: ax+by+cz+d=0; a represents the coefficient of the variable x; b represents the coefficient of the variable y; c represents the coefficient of the variable z; d represents the constant term in the plane equation; for two box faces, a×d corresponding to one box face>0, and , the corresponding a×d of the other box surface is <0, and , the two body surfaces are determined to be opposite body surfaces; wherein, represents the maximum angle allowed between the relative box surface and the yoz coordinate plane of the current robot coordinate system; o represents the origin of the current robot coordinate plane; for two box surfaces, b×d corresponding to one box surface>0, and , the other box surface corresponding to b×d<0, and , the two body surfaces are determined to be opposite body surfaces; wherein, Indicates the maximum angle allowed between the relative box surface and the zox coordinate plane of the current robot coordinate system; for two box surfaces, c×d>0 on one box surface, and , the c×d corresponding to the other box surface is <0, and , the two body surfaces are determined to be opposite body surfaces; wherein, Indicates the maximum angle allowed between the relative box surface and the xoy coordinate plane of the current robot coordinate system.

[0029] Optionally, the robot includes a robotic arm for grabbing goods; the three-dimensional camera is arranged at the end of the robotic arm; the second acquisition module is specifically used to: collect an image in the direction of the current spatial area to be detected through the three-dimensional camera, and obtain an image containing the current spatial area to be detected as the current image to be detected; scan the direction of the current spatial area to be detected through the three-dimensional camera, and determine the three-dimensional points belonging to the current spatial area to be detected from the scanning results, to obtain the current point cloud to be matched.

[0030] Optionally, the three-dimensional points in the initial point cloud are three-dimensional points in the robot coordinate system; the first acquisition module is specifically used to: scan the car body at multiple scanning positions outside the car body by the laser radar respectively, to obtain a point cloud to be converted corresponding to each scanning position; wherein the union of the scanning ranges at each scanning position can cover the spatial area occupied by the car body; for the point cloud to be converted corresponding to each scanning position, the three-dimensional points contained in the point cloud to be converted are converted according to the target transformation relationship to obtain the three-dimensional points in the initial point cloud; wherein the target transformation relationship represents: the transformation relationship between the radar coordinate system and the robot coordinate system.

[0031] Optionally, the robot includes a mechanical arm for grabbing goods; the laser radar is arranged at the end of the mechanical arm;

[0032] The first acquisition module is specifically used to: for each scanning position corresponding to the point cloud to be converted, according to the preset conversion formula between the radar coordinate system and the robot coordinate system, convert the three-dimensional points contained in the point cloud to be converted to obtain the three-dimensional points in the initial point cloud; wherein the preset conversion formula includes:

[0033] ;

[0034] st

[0035] ;

[0036] st

[0037] ;

[0038] ;

[0039] A first transformation matrix representing the radar coordinate system and the tool coordinate system of the manipulator; A second transformation matrix representing the tool coordinate system and the robot coordinate system; Represents the three-dimensional coordinates of the three-dimensional points contained in the point cloud to be converted; represents the three-dimensional coordinates of the three-dimensional points in the initial point cloud obtained by transformation; r represents the rotation matrix from the tool coordinate system to the robot coordinate system; t represents the translation matrix from the tool coordinate system to the robot coordinate system; represents the coordinates of the origin of the tool coordinate system in the robot coordinate system; represents the Euler angle of the tool coordinate system relative to the robot coordinate system.

[0040] Optionally, the three-dimensional point determination module is specifically used to: process the three-dimensional points contained in the initial point cloud based on a plane segmentation algorithm to obtain multiple planes; for each three-dimensional point contained in the initial point cloud, if the distance between the three-dimensional point and each obtained plane is not less than a preset distance, then the three-dimensional point is used as a to-be-processed three-dimensional point that does not belong to the body of the vehicle.

[0041] Optionally, the clustering module is specifically used to: take any unclustered three-dimensional point to be processed as the current clustering center; determine, from the current unclustered three-dimensional points to be processed, the three-dimensional points to be processed that are within the extended space range occupied by the set of candidate three-dimensional points to which the current clustering center belongs; add the determined three-dimensional points to be processed to the set of candidate three-dimensional points to which the current clustering center belongs, and return to execute the step of determining, from the current unclustered three-dimensional points to be processed, the three-dimensional points to be processed that are within the extended space range occupied by the set of candidate three-dimensional points to which the current clustering center belongs, until there are no three-dimensional points to be processed that are within the extended space range occupied by the set of candidate three-dimensional points to which the current clustering center belongs among the current unclustered three-dimensional points to be processed; and return to execute the step of taking any unclustered three-dimensional point to be processed as the current clustering center until there are no unclustered three-dimensional points to be processed; and determine, from the set of candidate three-dimensional points to which each clustering center belongs, a set of candidate three-dimensional points whose number of three-dimensional points is not less than a preset number threshold as the target three-dimensional point set.

[0042] Optionally, the detection module is specifically used to: input the obtained image to be detected into a pre-trained target detection model with DenseNet121 as the backbone network to obtain the image area occupied by the obstacle; the activation function in the DenseNet121 includes the Mish function, and / or the number of output channels of each network layer of the DenseNet121 increases in the following manner:

[0043] ;

[0044] Where i=1, 2, ..., n; % represents the remainder operation. represents the number of output channels of the i-th network layer, n represents the number of network layers included in the DenseNet121; and / or, the target detection model is built based on the YOLOv4 network.

[0045] In a fourth aspect of an embodiment of the present invention, a control device is provided, comprising a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory communicate with each other via the communication bus; the memory is used to store computer programs; and the processor is used to implement the obstacle position detection method described in any one of the first aspects above when executing the program stored in the memory.

[0046] In a fifth aspect of the embodiments of the present invention, a computer-readable storage medium is provided, wherein a computer program is stored in the computer-readable storage medium, and when the computer program is executed by a processor, the obstacle position detection method described in any one of the first aspects is implemented. The embodiments of the present invention also provide a computer program product containing instructions, which, when executed on a computer, enables the computer to execute any one of the obstacle position detection methods described in the first aspect.

[0047] The present invention provides an obstacle position detection method, wherein a control device acquires an initial point cloud; the initial point cloud is obtained by scanning the carriage from outside the carriage by a laser radar in a robot when a target side of the carriage is closed and opened; a 3D camera is also provided in the robot; 3D points of a carriage body that do not belong to the carriage are determined from the 3D points included in the initial point cloud as 3D points to be processed; the 3D points to be processed are clustered to obtain at least one target 3D point set; the robot is controlled to travel to a detection position in the carriage that is adjacent to a spatial position represented by the obtained target 3D point set in a state where the target side is opened, and the spatial position represented by the obtained target 3D point set is scanned by a 3D camera to obtain an image to be detected and a corresponding point cloud to be matched; an image area occupied by an obstacle in the obtained image to be detected is detected; and a spatial position represented by a 3D point in the corresponding point cloud to be matched is determined as the position of the obstacle in the carriage.

[0048] Based on the above processing, when the target side of the car is closed and opened, the car is scanned outside the car by the laser radar to obtain the initial point cloud. Obviously, when there is an obstacle inside the car, the three-dimensional points contained in the initial point cloud do not belong to the to-be-processed three-dimensional points of the car body, that is, the three-dimensional points scanned from the obstacles inside the car. Then the to-be-processed three-dimensional points are clustered, and the target three-dimensional point set obtained includes the three-dimensional points obtained by scanning the obstacles based on the laser radar. Since the laser radar scans outside the car, the distance between the laser radar and the obstacles inside the car is far, so the three-dimensional points of the obstacles scanned by the laser radar are not accurate, that is, the rough position of the obstacles detected based on the laser radar. After the robot equipped with a three-dimensional camera enters the car, the three-dimensional camera is used to scan the spatial position represented by the three-dimensional point set at the detection position adjacent to the spatial position represented by the target three-dimensional point set. At this time, the three-dimensional camera is already inside the car, and the scanned point cloud to be matched is more accurate than the three-dimensional points to be processed obtained by scanning outside the car. Therefore, the position of the obstacle determined from the point cloud to be matched according to the image to be detected is more accurate. That is, the exact position of the obstacle can be located by detecting the spatial position of the obstacle based on the three-dimensional camera.

[0049] Of course, it is not necessary to achieve all of the advantages described above at the same time to implement any product or method of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present invention. For ordinary technicians in this field, other embodiments can also be obtained based on these drawings.

[0051] Figure 1 A first flow chart of an obstacle position detection method provided by an embodiment of the present invention;

[0052] Figure 2 A top view of a truck compartment provided by an embodiment of the present invention;

[0053] Figure 3 A second flow chart of the obstacle position detection method provided by an embodiment of the present invention;

[0054] Figure 4 A third flow chart of the obstacle position detection method provided by an embodiment of the present invention;

[0055] Figure 5 A fourth flow chart of the obstacle position detection method provided by an embodiment of the present invention;

[0056] Figure 6 A structural diagram of obstacle position detection provided by the present invention;

[0057] Figure 7 A structural diagram of a control device provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0058] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field based on the present invention belong to the scope of protection of the present invention.

[0059] In the intelligent loading scenario, when the goods need to be stacked in the truck compartment, the space area occupied by the truck compartment can be determined by radar, and the space area inside the truck compartment can be used as the stacking area for the goods; the stacking position of the goods is planned in the stacking area, and then the robot is planned to carry out the cargo transportation path based on the planned cargo stacking position. Subsequently, the server controls the robot to move along the planned cargo transportation path and stack the goods in the planned cargo stacking position.

[0060] In the prior art, the spatial area inside the carriage is often directly determined as the cargo stacking area. However, in actual application scenarios, there may be obstacles such as baffles, latches, and air-conditioning units in the carriage, which may cause the planned cargo stacking position to interfere with the position of the obstacles, and subsequently lead to cargo damage or robot damage. Obviously, if the above problems are to be avoided, it is necessary to determine the position of the obstacles in the truck compartment. That is, a key link in realizing intelligent loading is to accurately detect and locate the position of obstacles that interfere with cargo stacking. Only in this way can prior information be provided for cargo stacking position planning and robot motion planning, and the stability of the intelligent loading system can be improved. Therefore, there is an urgent need for a detection method that can determine the position of obstacles in truck compartments.

[0061] The obstacle position detection method provided by the present invention is applied to control equipment. For example, the control equipment can be a central server of an intelligent loading system, and the central server can communicate with robots, trucks, etc. in the intelligent loading system, such as controlling robots to carry goods, notifying trucks to travel, etc. The control equipment can first detect the rough position of obstacles based on laser radar according to the method provided by the present invention, and then detect the spatial position of obstacles based on a three-dimensional camera. In this way, the accurate position of the obstacle can be detected.

[0062] See also Figure 1 , Figure 1A first flow chart of an obstacle position detection method provided by an embodiment of the present invention, the method comprising the following steps:

[0063] S101: Obtaining an initial point cloud.

[0064] The initial point cloud is obtained by scanning the cabin from outside the cabin through the laser radar in the robot when the target side of the cabin is closed and opened; a three-dimensional camera is also provided in the robot.

[0065] S102: Determine the three-dimensional points of the body that do not belong to the carriage from the three-dimensional points included in the initial point cloud as the three-dimensional points to be processed.

[0066] S103: Cluster the three-dimensional points to be processed to obtain at least one target three-dimensional point set.

[0067] S104: Control the robot to drive to a detection position in the vehicle compartment that is adjacent to the spatial position represented by the obtained target three-dimensional point set with the target side open, and scan the spatial position represented by the obtained target three-dimensional point set with a three-dimensional camera to obtain an image to be detected and a corresponding point cloud to be matched.

[0068] S105: Detecting the image area occupied by the obstacle in the image to be detected.

[0069] S106: Determine the spatial position represented by the three-dimensional point corresponding to the pixel point contained in the detected image area in the corresponding point cloud to be matched as the position of the obstacle in the vehicle compartment.

[0070] Based on the obstacle position detection method provided by the present invention, when the target side of the carriage is closed and opened, the carriage is scanned by the laser radar outside the carriage to obtain an initial point cloud. Obviously, when there is an obstacle inside the carriage, the three-dimensional points contained in the initial point cloud do not belong to the to-be-processed three-dimensional points of the carriage body of the carriage, that is, the three-dimensional points scanned from the obstacles inside the carriage. Then the to-be-processed three-dimensional points are clustered, and the target three-dimensional point set obtained includes the three-dimensional points obtained by scanning the obstacles based on the laser radar. Since the laser radar scans outside the carriage, the distance between the laser radar and the obstacles inside the carriage is far, so the three-dimensional points of the obstacles scanned by the laser radar are not accurate, that is, the rough position of the obstacles detected based on the laser radar. After the robot equipped with a three-dimensional camera enters the carriage, the three-dimensional camera is used to scan the spatial position represented by the three-dimensional point set at the detection position adjacent to the spatial position represented by the target three-dimensional point set. At this time, the three-dimensional camera is already inside the carriage, and the scanned point cloud to be matched is more accurate than the three-dimensional points to be processed obtained by scanning outside the carriage. Therefore, the position of the obstacle determined from the point cloud to be matched according to the image to be detected is more accurate. That is, the exact position of the obstacle can be located by detecting the spatial position of the obstacle based on the three-dimensional camera.

[0071] With respect to step S101, the robot moves the goods to the compartment of a truck used to transport the goods, which is the loading process in the intelligent loading scenario. Subsequently, the truck loaded with goods can travel to the unloading address to move the goods from the loading address to the unloading address. The model of the truck, the size and shape of the space area in the compartment, the model of the robot, and the type of goods can all be set according to the actual application scenario requirements, and the present invention is not limited to this. For example, the space area inside the compartment can be a rectangular parallelepiped; the goods to be stacked can be a material box; the robot can include a mechanical arm for grabbing the goods and a placement platform for placing the loaded goods.

[0072] For ease of description, the present invention will be described below by taking the example that the carriage includes six carriage faces and the space area in the carriage is a rectangular parallelepiped. Figure 2 , Figure 2 A top view of a truck compartment provided in an embodiment of the present invention. The compartment surface close to the front of the compartment is the front side surface, the compartment surface at the rear of the compartment is the rear side surface, the compartment surface at the bottom of the compartment is the bottom surface, and the compartment surface at the top of the compartment is the top surface; correspondingly, the remaining two compartment surfaces are collectively referred to as side surfaces. The side with an openable door is the target side surface of the compartment.

[0073] Since only scanning the outside of the car cannot obtain the point cloud of the obstacles in the car, it is impossible to locate the obstacles; scanning only the inside of the car makes it difficult to distinguish between the obstacles and the car body. Therefore, it is necessary to obtain an initial point cloud by scanning both the outside and the inside of the car. For the specific method of obtaining the initial point cloud, please refer to the detailed description of the subsequent embodiments, which will not be repeated here. Obviously, the position of the laser radar can be changed. For example, the laser radar can change its position as the robot moves. Alternatively, the laser radar can be set on the robot's mechanical arm and change its position as the mechanical arm swings. Obviously, the way the laser radar changes its position is not limited to this.

[0074] Exemplarily, the laser radar is arranged at the end of the mechanical arm. After the target side is turned on, the control device changes the position of the laser radar by controlling the swing of the mechanical arm; the laser radar scans the interior of the car at different positions through the target side to obtain the point cloud of the interior of the car; and scans the exterior of the car at different positions to obtain the point cloud of the other car body surfaces of the car except the target side. After the target side is turned off, the laser radar scans the target side at different positions to obtain the point cloud of the target side.

[0075] The robot in the present invention may also be provided with a three-dimensional camera. Later, after the robot enters the carriage, the three-dimensional camera may be used to detect the exact position of obstacles in the carriage. For details, please refer to the detailed description of the subsequent embodiments.

[0076] Regarding step S102, it is obvious that the three-dimensional points that do not belong to the body of the carriage in the three-dimensional points included in the initial point cloud may be three-dimensional points to be processed that belong to obstacles. The specific way in which the control device determines the three-dimensional points to be processed is described in detail in the subsequent embodiments. The absence of three-dimensional points to be processed indicates that there are no obstacles in the carriage. At this time, the control device performs motion path planning and cargo stacking planning for the robot, and then controls the robot according to the planning results.

[0077] In the case where there are other objects outside the car, the initial point cloud may contain 3D points of other objects. In this case, the control device can first determine the 3D points belonging to the space area occupied by the car from the 3D points contained in the initial point cloud, and then determine the 3D points to be processed from the determined 3D points. For example, if a truck is parked at a designated parking position, the 3D points in the space area occupied by the parking position are the 3D points in the space area occupied by the car.

[0078] For step S103 and step S104, cluster the 3D points to be processed that may belong to obstacles to obtain at least one target 3D point set, and obstacles may exist in the spatial position represented by the target 3D point set (hereinafter referred to as the spatial position to be detected). The spatial position to be detected can be: the spatial area occupied by the minimum circumscribed cube of each 3D point included in the target 3D point set.

[0079] When the target side is open, the control device controls the robot to drive to the detection position in the car that is adjacent to the space to be detected (hereinafter referred to as the adjacent detection position), and uses the three-dimensional camera to scan the space to be detected at close range to obtain a more accurate image to be detected and the corresponding point cloud to be matched. For example, the pixel coordinates of the corresponding pixel points in the image to be detected of the three-dimensional points in the point cloud to be matched are obtained. The detection position is: the distance between the space to be detected and the position in the car is small, and the robot can be docked at present.

[0080] In some embodiments, the control device plans the motion path of the robot based on the spatial position to be detected, and determines a moving path whose destination is adjacent to the detection position and does not pass through the spatial position to be detected. Then, when the robot subsequently drives along the planned motion path, it can bypass possible obstacles, further reducing the probability of damage to the robot.

[0081] For step S105, the control device performs target detection on the image to be detected collected by the 3D camera at a close distance, and obtains the image area occupied by the obstacle in the image to be detected. For example, the target detection is performed on the image to be detected based on a pre-trained target detection model to obtain the image area occupied by the minimum circumscribed rectangle of the obstacle in the image to be detected. For example, the target detection model can be a YOLO (You Only Look Once) model, an SSD (Single Shot MultiBoxDetector) model, etc.

[0082] For step S106, according to the pixel coordinates of the pixel points in the image area occupied by the obstacle in the image to be detected, the three-dimensional points corresponding to the pixel points contained in the image area in the point cloud to be matched can be determined; the spatial position represented by the determined three-dimensional points is the spatial position of the obstacle in the image to be detected. For example, the spatial position represented by the determined three-dimensional points can be: the spatial area occupied by the minimum circumscribed cube of the determined three-dimensional points. Based on the more accurate image to be detected and the corresponding point cloud to be matched, the accuracy of the position of the obstacle in the car is also higher.

[0083] In some embodiments, the three-dimensional points in the initial point cloud are three-dimensional points in the robot coordinate system. At this time, the robot is located outside the side of the target, and the origin of the current robot coordinate system is at a specified position on the robot. Step S101 may include the following steps: Step 1: Scan the car body at multiple scanning positions outside the car body by means of a laser radar respectively, and obtain a point cloud to be converted corresponding to each scanning position. Among them, the union of the scanning ranges at each scanning position can cover the spatial area occupied by the car body. Step 2: For each point cloud to be converted corresponding to a scanning position, the three-dimensional points contained in the point cloud to be converted are converted according to the target conversion relationship to obtain the three-dimensional points in the initial point cloud. Among them, the target conversion relationship represents: the conversion relationship between the radar coordinate system and the robot coordinate system.

[0084] There are only a limited number of truck models that may appear in actual application scenarios. Therefore, the control device can pre-record the scanning positions of each model of truck. The union of the scanning ranges of the laser radar at the scanning position of a model of truck can cover the space area occupied by the compartment of the model of truck. For example, the scanning position can indicate the rotation angle and swing amplitude of the robot arm. After the truck stops, the control device searches for the scanning position of the model of truck, and controls the laser radar to scan the compartment in turn at these scanning positions to obtain the point cloud to be converted corresponding to each scanning position.

[0085] Obviously, the control device needs to integrate the point clouds to be converted at each scanning position into a unified coordinate system. Since the laser radar is set on the robot, and the position of the robot body will not change when the car is scanned by the laser radar, the point cloud to be converted can be integrated into the robot coordinate system. At this time, the robot is located outside the side of the target, such as docked at the target docking position; the origin of the robot coordinate system is the point in the target docking position. In order to distinguish it from the robot coordinate system involved after the subsequent robot drives into the car, the robot coordinate system at this time will be referred to as the initial coordinate system in the future.

[0086] In some embodiments, the robot includes a mechanical arm for grabbing goods; the laser radar is arranged at the end of the mechanical arm. The above step S1012 may include the following steps: for each point cloud to be converted corresponding to the scanning position, according to the preset conversion formula between the radar coordinate system and the robot coordinate system, the three-dimensional points included in the point cloud to be converted are converted to obtain the three-dimensional points in the initial point cloud.

[0087] The preset conversion formulas include:

[0088] ;

[0089] st

[0090] ;

[0091] st

[0092] ;

[0093] ;

[0094] A first transformation matrix representing the radar coordinate system and the tool coordinate system of the manipulator; A second transformation matrix representing the tool coordinate system and the robot coordinate system; Represents the three-dimensional coordinates of the three-dimensional points contained in the point cloud to be converted; represents the 3D coordinates of the 3D points in the initial point cloud obtained by transformation; r represents the rotation matrix from the tool coordinate system to the robot coordinate system; t represents the translation matrix from the tool coordinate system to the robot coordinate system; Indicates the coordinates of the origin of the tool coordinate system in the robot coordinate system; Represents the Euler angle of the tool coordinate system relative to the robot coordinate system.

[0095] The first transformation matrix represents the transformation relationship between the radar coordinate system and the tool coordinate system, which can be obtained by hand-eye calibration of the laser radar and the robotic arm. The second transformation matrix represents the transformation relationship between the tool coordinate system and the robot coordinate system, which can be obtained according to the rotation angle of the robotic arm and the position of the robotic arm on the robot when the laser radar is located at a scanning position. Obviously, r is a matrix of size 3×3, and t is a matrix of size 3×1. For each scanning position corresponding to the point cloud to be transformed, according to the first transformation matrix and the second transformation matrix of the scanning position, the three-dimensional points contained in the point cloud to be transformed corresponding to the scanning position can be transformed into the initial coordinate system.

[0096] Based on the above processing, the union of the scanning ranges of the lidar at each scanning position can cover the spatial area occupied by the carriage. The point cloud to be converted at each scanning position, that is, the point cloud obtained by a comprehensive scan of the carriage, can avoid missed detections due to incomplete scanning, improve the safety of subsequent stacking of goods in the carriage based on the position of obstacles, further improve the safety of the goods, and further reduce the probability of damage to the robot.

[0097] In some embodiments, Figure 1 Based on Figure 3 , step S102 may include the following steps:

[0098] S1021: Processing the three-dimensional points included in the initial point cloud based on a plane segmentation algorithm to obtain multiple planes.

[0099] S1022: For each 3D point included in the initial point cloud, if the distance between the 3D point and each obtained plane is not less than a preset distance, the 3D point is regarded as a to-be-processed 3D point of the body that does not belong to the carriage.

[0100] It is understandable that when the three-dimensional point has three-dimensional coordinates and the car body includes six car body faces, the three-dimensional points contained in the initial point cloud are processed based on the plane segmentation algorithm to obtain six plane equations in the initial coordinate system, that is, six planes representing the six car body faces of the car body are determined. For example, the plane segmentation algorithm can be an open source algorithm in PCL (Point Cloud Library), and the technician configures the plane segmentation algorithm selected from PCL in the control device according to the requirements. Then, for each three-dimensional point contained in the initial point cloud, the distance between the three-dimensional coordinates of the three-dimensional point and each plane is calculated. Since the car body has thickness, and the plane represented by the plane equation may deviate from the plane of the actual car body face, the distance between the three-dimensional coordinates of the three-dimensional point and any plane is not less than the preset distance, and it can be considered that the three-dimensional point does not belong to the car body. The preset distance can be determined based on the accuracy of the plane segmentation algorithm and the thickness of the car body. If the car body is thicker, the preset distance can be a larger value, such as 10 cm; when the car body is thinner and the accuracy of the plane segmentation algorithm is higher, the preset distance can be a smaller value, such as 3 cm.

[0101] Based on the above processing, the control device uses the plane obtained by processing the initial point cloud based on the plane segmentation algorithm as the plane of the car body, and the distance between the three-dimensional point and the plane can indicate whether the three-dimensional point belongs to the car body. The three-dimensional points to be processed determined in this way are more accurate, and the target three-dimensional point set obtained subsequently is more accurate, reducing the number of subsequent detections based on the three-dimensional camera and improving the efficiency of determining the accurate position of obstacles in the car body.

[0102] In some embodiments, step S103 may include the following steps: Step 1: Take any unclustered 3D point to be processed as the current cluster center. Step 2: From the current unclustered 3D points to be processed, determine the 3D points to be processed that are within the extended space range occupied by the candidate 3D point set to which the current cluster center belongs. Step 3: Add the determined 3D points to be processed to the candidate 3D point set to which the current cluster center belongs, and return to execute the step of determining the 3D points to be processed that are within the extended space range occupied by the candidate 3D point set to which the current cluster center belongs from the current unclustered 3D points to be processed, until there are no 3D points to be processed that are within the extended space range occupied by the candidate 3D point set to which the current cluster center belongs among the current unclustered 3D points to be processed. Step 4: Return to execute the step of taking any unclustered 3D point to be processed as the current cluster center, until there are no unclustered 3D points to be processed. Step 5: From the candidate 3D point sets to which each cluster center belongs, determine a candidate 3D point set whose number of included 3D points is not less than a preset number threshold as the target 3D point set.

[0103] The obstacle in the present invention is an object that actually exists and has a certain volume, and the three-dimensional positions of the three-dimensional points belonging to an obstacle are similar. Therefore, the spatial positions represented by the three-dimensional points contained in the candidate three-dimensional point set obtained by clustering the three-dimensional points to be processed may include the position of the obstacle.

[0104] Specifically, the control device uses any currently unclustered 3D point to be processed as the current cluster center. The candidate 3D point set (hereinafter referred to as the current point cloud cluster) to which the current cluster center belongs includes the current cluster center, and the external expansion space occupied by the current point cloud cluster is: a spherical space with the current cluster center as the center and a radius of the preset neighborhood distance.

[0105] Then, other 3D points to be processed (hereinafter referred to as first cluster points) existing in the external expansion space range occupied by the current point cloud cluster are added to the current point cloud cluster. At this time, the current point cloud cluster includes: the current cluster center and the first cluster point. The external expansion space range occupied by the current point cloud cluster is: multiple spherical spaces with each first cluster point as the sphere center and a radius of the preset neighborhood distance.

[0106] Then, other 3D points to be processed (hereinafter referred to as second cluster points) within the external expansion space occupied by the current point cloud cluster are added to the current point cloud cluster. At this time, the current point cloud cluster includes: the current cluster center, the first cluster point, and the second cluster point. The external expansion space occupied by the current point cloud cluster is: multiple spherical spaces with each second cluster point as the sphere center and a radius of the preset neighborhood distance.

[0107] By analogy, other 3D points to be processed within the extended space range occupied by the current point cloud cluster are added to the current point cloud cluster, and the extended space range occupied by the current point cloud cluster is expanded: a spherical space with each newly added 3D point to be processed as the center and a radius of the preset neighborhood distance.

[0108] This cycle is repeated until there are no un-clustered 3D points to be processed that are within the extended space occupied by the current point cloud cluster, and the candidate 3D point set to which the current cluster center belongs is obtained. The preset neighborhood distance can be an empirical value, such as 5 cm; or it can be determined based on the radar accuracy of the laser radar, the distance relative to the car, etc. If the laser radar accuracy is high, the obtained 3D points to be processed are denser, and the preset neighborhood distance is smaller, such as 3 cm; if the laser radar is far away from the car, the obtained 3D points to be processed are sparse, and the preset neighborhood distance is larger, such as 8 cm.

[0109] Then, any currently un-clustered 3D point to be processed is used as the current cluster center, and the candidate 3D point set to which the current cluster center belongs is determined in the above manner. This cycle is repeated until there are no un-clustered 3D points to be processed. The candidate 3D point sets to which each cluster center belongs are obtained, that is, the positions of each possible obstacle inside the car are obtained.

[0110] Furthermore, the initial point cloud may include noise points generated by interference in the actual scene, and the obtained alternative three-dimensional point set may also contain an alternative three-dimensional point set obtained by clustering the noise points (hereinafter referred to as the noise point set). Since noise points are usually not clustered, the alternative three-dimensional point set that includes a small number of three-dimensional points is the noise point set. Therefore, the alternative three-dimensional point set that includes a number of three-dimensional points that is not less than a preset number threshold can be used as the target three-dimensional point set. The preset number threshold can be data measured through experiments, such as 50. Obviously, the absence of a target three-dimensional point set indicates that there are no obstacles in the car. At this time, the control device performs motion path planning and cargo stacking planning for the robot, and then controls the robot according to the planning results.

[0111] Based on the above processing, the control device determines the positions of each possible obstacle inside the car by clustering the three-dimensional points to be processed, and determines the target three-dimensional point set according to the number of three-dimensional points contained in each candidate three-dimensional point set. Avoid detecting the spatial position of the falsely detected noise point set, reduce the number of subsequent detections based on the three-dimensional camera, and improve the efficiency of determining the accurate position of obstacles in the car.

[0112] In some embodiments, Figure 1 Based on Figure 4 , step S104 may include the following steps:

[0113] S1041: Determine a current space region to be detected from multiple space regions to be detected according to a specified direction.

[0114] Among them, the multiple space areas to be detected are: multiple rectangular space areas obtained by dividing the space area in the car in a specified direction; the specified direction is the direction in which the surface opposite to the target side surface points to the target side surface.

[0115] S1042: When there is an intersection between the current spatial area to be detected and the spatial position represented by any target three-dimensional point set obtained, the robot is controlled to drive to the detection position on the specified side of the current spatial area to be detected with the target side open, and an image containing the current spatial area to be detected and a point cloud of the current spatial area to be detected are obtained through a three-dimensional camera, which are used as the current image to be detected and the corresponding point cloud to be matched, respectively.

[0116] The designated side indicates the side of the current space area to be detected that is close to the side of the target.

[0117] Accordingly, step S105 may include the following steps:

[0118] S1051: Detect the image area occupied by the obstacle in the current image to be detected, and use it as the current image area to be processed.

[0119] Accordingly, step S106 may include the following steps:

[0120] S1061: Determine the spatial position represented by the three-dimensional point corresponding to the pixel point contained in the current image area to be processed in the point cloud to be matched corresponding to the current image to be detected as the position of the obstacle in the current spatial area to be detected; and return to execute the step of determining the current spatial area to be detected from multiple spatial areas to be detected in the specified direction until the position of the obstacle in the spatial area to be detected is determined, thereby obtaining the position of the obstacle in the vehicle.

[0121] The specified direction is the direction from the surface opposite to the target side to the target side. For example, when the target side is the rear side, the specified direction is the direction from the front side to the rear side. And obviously, the robot needs to start stacking the goods from a position far away from the target side. Figure 2 As shown, when the target side is the rear side, the robot needs to first stack the goods near the front side (i.e., in the space area 1 to be detected). Otherwise, the goods will be piled up near the target side, making it difficult for the subsequent robots to enter the carriage, thereby wasting the space area in the carriage.

[0122] In order to avoid detecting obstacles at a long distance through the three-dimensional camera and improve the accuracy of the detected obstacle position, the space area in the car can be divided into multiple space areas to be detected, and the position of the obstacle in the current space area to be detected is determined each time.

[0123] Specifically, the space area in the car is divided in a specified direction to obtain a plurality of rectangular space areas to be detected, and then the current space area to be detected is determined from the plurality of space areas to be detected. The division method can be determined according to the accuracy of the three-dimensional camera and the length of the car in the specified direction. If the length of the car in the specified direction is long, a larger number of space areas to be detected can be obtained by division, such as 10 space areas to be detected; when the accuracy of the three-dimensional camera is high, a smaller number of space areas to be detected can be obtained by division, such as 5 space areas to be detected. But it is not limited to this.

[0124] like Figure 2 As shown, Figure 2 The space area to be detected 1, space area to be detected 2, and space area to be detected 3 in the figure represent the space area to be detected in a rectangular parallelepiped. The length of the space area to be detected is the width of the carriage, the height of the space area to be detected is the height of the carriage, and the width of the space area to be detected is the division distance when dividing the space area in the carriage. For example, the division distance can be set to 1 meter. Figure 2 The black circular area in the figure represents the obstacle in the car. The spatial position represented by the target three-dimensional point set (i.e., the spatial position to be detected) includes the spatial area occupied by the obstacle.

[0125] The current space area to be detected is: the space area to be detected that is farthest from the side of the target and has not been detected (i.e., space area to be detected 1). There is no intersection between space area to be detected 1 and the space position to be detected, that is, there is no obstacle in space area to be detected 1. At this time, the control device plans the robot to stack the goods, and then controls the robot according to the planning results.

[0126] Then, the space area 2 to be detected that is adjacent to the space area 1 to be detected in the specified direction is used as the current space area to be detected. If the space area 2 to be detected intersects with the space position to be detected, then when the target side is open, the robot is controlled to travel to the detection position at the specified side of the current space area to be detected. The specified side is the side of the current space area to be detected close to the target side, that is, the side to which the robot can currently travel. The detection position at the specified side of the current space area to be detected can be: on the moving path of the robot when entering the car, at the specified side and at a preset distance from the current space area to be detected, such as Figure 2Then, the three-dimensional camera is used to obtain an image of the current space area to be detected (i.e., the current image to be detected) and a point cloud of the current space area to be detected (i.e., the point cloud to be matched corresponding to the current image to be detected).

[0127] For example, Indicates: The three-dimensional coordinates of the center point are , the length is , the width is , the height is The cubic space region. Then , it means that the current space area to be detected and the space position to be detected have an intersection. Indicates the current space area to be detected. Represents any spatial position to be detected.

[0128] It is understandable that the current image to be detected acquired by the three-dimensional camera may include images of other spatial areas to be detected; the point cloud obtained by scanning the direction of the current spatial area to be detected may include point clouds in other spatial areas to be detected. For example, the image acquired by the three-dimensional camera at detection position 1 may include spatial area 1 to be detected; scanning the direction of spatial area 2 to be detected may scan the point cloud in spatial area 1 to be detected. As mentioned above, in order to improve the accuracy of the position of the detected obstacle, the control device only determines the position of the obstacle in the current spatial area to be detected based on the point cloud currently scanned by the three-dimensional camera. Specifically, step S1042 may include the following steps: acquiring an image in the direction of the current spatial area to be detected by the three-dimensional camera, and obtaining an image containing the current spatial area to be detected as the current image to be detected. Scanning the direction of the current spatial area to be detected by the three-dimensional camera, and determining the three-dimensional points belonging to the current spatial area to be detected from the scanning results to obtain the current point cloud to be matched.

[0129] The control device determines the current point cloud to be matched from the scanning results, such as taking the three-dimensional points whose three-dimensional coordinates belong to the current spatial area to be detected as the current point cloud to be matched.

[0130] Then, the image area occupied by the obstacle in the current image to be detected is detected, such as detecting the current image area to be processed in the image to be detected based on the pre-trained target detection model. Then, the spatial position represented by the three-dimensional point corresponding to the pixel point contained in the current image area to be processed in the current point cloud to be matched is determined as the position of the obstacle in the current spatial area to be detected. The accurate position of the obstacle in the current spatial area to be detected is obtained.

[0131] Based on the accurate position of the obstacles in the current space area to be detected, the robot is controlled to stack the goods in the current space area to be detected. Then, according to the specified direction, the space area to be detected that is farthest from the side of the target (i.e., space area to be detected 3) is determined from the space areas to be detected that have not been detected. Continue to detect the accurate position of the obstacles in space area to be detected 3 according to the above method until the position of the obstacles in each space area to be detected is determined, that is, the position of the obstacles in the car is obtained.

[0132] Based on the above processing, for each obstacle, the exact position of the obstacle can be determined based on the current image to be detected and the corresponding point cloud to be matched acquired at a close distance. In addition, the control device detects the current space area to be detected where obstacles may exist based on the three-dimensional camera, reducing the number of detections based on the three-dimensional camera and improving the efficiency of determining the exact position of obstacles in the car.

[0133] It is understandable that during intelligent loading, the truck usually stops at a designated location and the position of obstacles in the carriage usually does not change. In actual application scenarios, the robot will move, that is, the position of the origin of the robot coordinate system in the world coordinate system will change. Therefore, the control device can plan the cargo stacking position in the carriage coordinate system, and after the robot is currently docked, the planned cargo stacking position in the carriage coordinate system (hereinafter referred to as the intermediate stacking position) is converted to the current robot coordinate system.

[0134] In some embodiments, the target side is one of the six body faces of the car body; the origin of the robot coordinate system is located at the designated position of the robot; the position of the obstacle in the current space area to be detected is the position in the car body coordinate system of the car body; after step S1061, the method may further include the following steps: Step 1: Based on the position of the obstacle in the current space area to be detected in the car body coordinate system, the position of the current space area to be detected in the car body coordinate system, and the size of various types of goods to be stacked in the current space area to be detected, the stacking generation algorithm is used to plan the stacking position of the goods in the current space area to be detected in the car body coordinate system. Among them, the position of the current space area to be detected in the car body coordinate system is: obtained based on the distance between the body faces of the car body relative to the car body. Step 2: When the robot is parked at the detection position at the designated side of the current space area to be detected, based on the distance between the origin of the car coordinate system and the origin of the current robot coordinate system, and the angle between the body surface of the car and the coordinate plane of the current robot coordinate system, the cargo stacking position in the current space area to be detected in the car coordinate system is converted to obtain the cargo stacking position in the current space area to be detected in the current robot coordinate system. Step 3: Control the robot to stack the cargo according to the cargo stacking position in the current space area to be detected in the current robot coordinate system until the cargo is stacked in the current space area to be detected.

[0135] The point cloud to be matched corresponding to the current image to be detected is the point cloud in the current robot coordinate system; the position of the obstacle (hereinafter referred to as the target obstacle) in the current spatial area to be detected is the position in the current robot coordinate system.

[0136] According to the positional relationship between the car coordinate system and the current robot coordinate system, the position of the target obstacle in the current robot coordinate system can be converted into the position of the target obstacle in the car coordinate system (hereinafter referred to as the target obstacle position).

[0137] The size of the car can be obtained based on the distance between the car body surfaces relative to the car body; as mentioned above, the size of the space area to be detected can be obtained based on the size of the car. According to the current size of the space area to be detected and the order of the current space area to be detected in the multiple space areas to be detected, the position of the current space area to be detected in the car coordinate system (hereinafter referred to as the target area position) can be obtained. There are various ways to obtain the size of the car, and the present invention is not limited to this. For example, the size of the car can be measured manually by a technician, the size of the car can be measured using a laser radar, etc.

[0138] In addition, according to the business needs in the actual scenario, the size of various types of goods that need to be transported by trucks can be determined. Through the stacking generation algorithm, the target obstacle position, the target area position, and the size of various types of goods to be stacked in the current space area to be detected can be planned to obtain the intermediate stacking position in the current space area to be detected. If the goods are stacked according to the planned cargo stacking position at this time, the space utilization rate can be maximized.

[0139] In some embodiments, goods stacking rules can also be set, such as setting that goods A cannot be rotated, goods B cannot be stacked on the bottom layer, and requiring the maximum number of goods C to be stacked, etc. Correspondingly, the stacking generation algorithm can also plan the goods stacking positions that meet the set goods stacking rules to meet the business needs in actual scenarios.

[0140] In some embodiments, the origin of the car coordinate system may be any vertex of the car; based on the distance from the car body surface to the current origin of the robot coordinate system, the distance between the origin of the car coordinate system and the current origin of the robot coordinate system is obtained.

[0141] A box surface is represented in the current robot coordinate system in the form of the following plane equation: ax+by+cz+d=0; a represents the coefficient of variable x; b represents the coefficient of variable y; c represents the coefficient of variable z; d represents the constant term in the plane equation. The plane equation determined based on the plane segmentation algorithm can be converted into a plane equation in the current robot coordinate system according to the translation matrix between the initial coordinate system and the current robot coordinate system. Alternatively, after the robot is docked at the detection position at the specified side of the current space area to be detected, the plane equation for characterizing the box surface in the current robot coordinate system is obtained based on the laser radar in accordance with the method of determining the plane equation in the aforementioned embodiment, which can reduce the interference caused by the change of the orientation during the robot's driving process, and improve the accuracy of the cargo stacking position (hereinafter referred to as the target stacking position) in the current space area to be detected in the current robot coordinate system obtained by subsequent conversion.

[0142] According to the following formula (1), calculate the distance D between the plane equation of the box surface and the origin of the current robot coordinate system:

[0143] …… (1)

[0144] According to the following formula (2), the angle between the body surface and the coordinate plane of the current robot coordinate system is calculated:

[0145] …… (2)

[0146] Indicates the angle between the body surface and the xoy surface of the current robot coordinate system; Indicates the angle between the body surface and the yoz surface of the current robot coordinate system; It represents the angle between the body surface and the zox surface of the current robot coordinate system.

[0147] After the robot stops at the detection position at the designated side of the current space area to be detected, the intermediate stacking position is converted to the current robot coordinate system according to the above distance and angle to obtain the target stacking position. Compared with the stacking position in the current robot coordinate system obtained by directly converting the distance between the origin of the car coordinate system and the origin of the current robot coordinate system in the prior art, the interference caused by the tilt of the car can be reduced; stacking goods according to the target stacking position reduces the probability of squeezing, collapse and other problems when stacking goods.

[0148] In some embodiments, the relative cabin faces are determined in the following manner: when the robot is parked outside the target side, the three-dimensional points contained in the initial point cloud are processed based on the plane segmentation algorithm to obtain the plane equations of multiple cabin faces in the current robot coordinate system; wherein a plane equation in the current robot coordinate system is expressed in the following form: ax+by+cz+d=0; a represents the coefficient of the variable x; b represents the coefficient of the variable y; c represents the coefficient of the variable z; d represents the constant term in the plane equation; for two cabin faces, a×d corresponding to one cabin face>0, and , the corresponding a×d of the other box surface is <0, and , the two body surfaces are determined to be opposite body surfaces; wherein, represents the maximum angle allowed between the relative box surface and the yoz coordinate plane of the current robot coordinate system; o represents the origin of the current robot coordinate plane; for two box surfaces, b×d corresponding to one box surface>0, and , the other box surface corresponding to b×d<0, and , the two body surfaces are determined to be opposite body surfaces; wherein, Indicates the maximum angle allowed between the relative box surface and the zox coordinate plane of the current robot coordinate system; for two box surfaces, c×d>0 on one box surface, and , the c×d corresponding to the other box surface is <0, and , the two body surfaces are determined to be opposite body surfaces; wherein, Indicates the maximum angle allowed between the relative box surface and the xoy coordinate plane of the current robot coordinate system.

[0149] According to the mathematical relationship between the coefficients of the plane equation, the corresponding relationship between the plane equation and the box surface can be determined. For example, the corresponding a×d>0, and When , the box body surface is the right side surface; the corresponding a×d<0, and When , the box body surface is the left side surface; the corresponding b×d>0, and When , the box body surface is the front side; the corresponding b×d<0, and When , the body surface is the rear side surface; the corresponding c×d>0, and When , the box body surface is the top surface; the corresponding c×d<0, and When the two body surfaces meet the above conditions, it can be determined that the two body surfaces are opposite to each other. , , You can gain experience points. , , The specific setting can be determined according to the model of the truck used in the actual application scenario. Obviously, the size of the space area in the car can be obtained based on the distance between the relative car body surfaces. Subsequently, the robot motion path planning and cargo stacking planning can be carried out based on the size of the space area in the car.

[0150] Based on the above processing, the control device determines the relative body surface according to the mathematical relationship between the coefficients of the plane equation of the body surface; then calculates the size of the space area in the car based on the relative body surface; finally, based on the calculated size of the space area in the car, the robot motion path planning and cargo stacking planning are carried out. The use of the above method not only improves the measurement accuracy, but also reduces the probability of problems such as squeezing and collapse when stacking goods.

[0151] In some embodiments, step S105 may include the following steps: inputting the obtained image to be detected into a pre-trained target detection model with DenseNet121 as the backbone network to obtain the image area occupied by the obstacle; the activation function in DenseNet121 includes a Mish function, and / or the number of output channels of each network layer of DenseNet121 increases in the following manner:

[0152] ;

[0153] Where i=1, 2, ..., n; % represents the remainder operation. represents the number of output channels of the i-th network layer, n represents the number of network layers included in DenseNet121; and / or, the target detection model is built based on the YOLOv4 network.

[0154] The target detection model in the embodiment of the present invention can be obtained by improving the existing target detection model. For example, the backbone network of the existing SSD model can be replaced with DenseNet121, and then the improved SSD model can be trained to obtain the target detection model in the embodiment of the present invention.

[0155] The YOLOv4 network includes a backbone network for feature extraction of the input image, and a recognition network for detecting targets and positions based on features. DenseNet121 includes multiple processing units, each of which includes a convolution layer, a pooling layer, and an activation layer. The multiple processing units are connected in series; and the input data of each processing unit includes the output data of each processing unit located before the processing unit in the serial connection. When the target detection model is built based on the YOLOv4 network, based on DenseNet121, the image features in the image to be detected can be extracted more deeply to obtain richer image features. Subsequently, when the target and target position are detected based on richer image features, the results obtained will be more accurate.

[0156] The activation layer in the existing DenseNet121 usually uses the Relu function. Experimental results show that compared with using the Relu function as the activation function, when DenseNet121 uses the Mish function as the activation function, the accuracy of the extracted features is higher, and the image area occupied by the obstacles detected later is more accurate. Therefore, when DenseNet121 uses the Mish function as the activation function, the target detection model with DenseNet121 as the backbone network detects the image area occupied by obstacles in the image to be detected more accurately.

[0157] The number of output channels of the network layer in the existing DenseNet121 increases according to a fixed channel growth number. If the number of channels of the input feature is , then the number of feature channels (i.e., the number of output channels) of the Lth network layer in the existing DenseNet121 is: , k is the number of fixed channel growth. When the number of output channels of the network layer in DenseNet121 increases according to the growth method provided by the present invention, since DenseNet121 includes hundreds of network layers, the final number of output channels is greater than the final number of output channels of the existing DenseNet121. In this way, richer image features can be obtained based on DenseNet121, and the image area occupied by obstacles detected later is more accurate.

[0158] In some embodiments, before detecting the image area occupied by the obstacle in the image to be detected, the resolution of the image to be detected can be normalized to a larger resolution, such as 800×800. Compared with the prior art that usually normalizes the image to a resolution of 640×640, it is obvious that the image features retained by the image to be detected normalized to a resolution of 800×800 are more accurate; thus, the accuracy of the target and target position detected based on more accurate image features is higher.

[0159] See also Figure 5 , Figure 5 A fourth flow chart of the obstacle position detection method provided by an embodiment of the present invention. The method may include the following steps:

[0160] S501: Reconstruction of the three-dimensional data of the interior of the carriage and hand-eye fusion.

[0161] In this step, data reconstruction and hand-eye fusion, that is, the three-dimensional points contained in the point cloud to be converted in the aforementioned embodiment are converted to obtain the three-dimensional points in the initial point cloud. The laser radar is set on the robot's mechanical arm. By controlling the swing of the mechanical arm, the position of the laser radar is changed, so that the laser radar can scan the car at different scanning positions. The union of the scanning range of the laser radar at each scanning position can cover the space area occupied by the car. Then, based on the aforementioned preset conversion formula, the control device converts the three-dimensional points contained in the point cloud to be converted scanned by the laser radar at different scanning positions to obtain the three-dimensional points in the initial point cloud.

[0162] S502: Calculate the plane and spatial position of the carriage.

[0163] In this step, the car plane is the plane obtained based on the plane segmentation algorithm in the above embodiment; the spatial pose is the distance between the origin of the car coordinate system in the above embodiment and the origin of the current robot coordinate system, and the angle between the car body surface and the coordinate plane of the current robot coordinate system. The control device processes the three-dimensional points contained in the initial point cloud based on the plane segmentation algorithm to obtain multiple planes; and then calculates the spatial pose in the manner provided in the above embodiment.

[0164] S503: Segment the point cloud data inside the vehicle compartment.

[0165] In this step, the point cloud data of the interior of the carriage is the 3D point to be processed in the above-mentioned embodiment. For each 3D point included in the initial point cloud, the control device calculates the distance between the 3D coordinates of the 3D point and each obtained plane. Then, the 3D point whose distance to each plane is not less than the preset distance is taken as the 3D point to be processed, that is, the point cloud data of the interior of the carriage is segmented from the initial point cloud.

[0166] S504: Determine whether the point cloud data inside the carriage is empty.

[0167] In this step, it is determined whether the point cloud data inside the carriage is empty, that is, whether there are 3D points to be processed. If the judgment result is yes, it indicates that there are no obstacles in the carriage. At this time, jump to step S510 and the process ends. If the judgment result is no, there may be obstacles inside the carriage. At this time, jump to step S505.

[0168] S505: Clustering the point cloud in the carriage to obtain point cloud clusters, and screening the point cloud clusters.

[0169] In this step, the point cloud in the car is the 3D point to be processed in the aforementioned embodiment, and the point cloud cluster is the candidate 3D point set to which each cluster center belongs in the aforementioned embodiment. The process of screening the point cloud cluster is the process of determining the target 3D point set from the candidate 3D point set in the aforementioned embodiment. That is, the control device clusters the 3D points to be processed to obtain the candidate 3D point set to which each cluster center belongs; then, from the candidate 3D point set to which each cluster center belongs, a candidate 3D point set containing a number of 3D points not less than a preset number threshold is determined as the target 3D point set.

[0170] S506: Determine whether the number of the screened point cloud clusters is 0.

[0171] In this step, the selected point cloud cluster is the target three-dimensional point set in the above embodiment. The number of selected point cloud clusters is 0, that is, there is no target three-dimensional point. When the judgment result is yes, it indicates that there is no obstacle in the car, jump to step S510, and the process ends. When the judgment result is no, there may be an obstacle inside the car, in this case, jump to step S507.

[0172] S507: Calculate the circumscribed cube of the point cloud cluster to obtain the rough position of the obstacle.

[0173] In this step, as mentioned above, the circumscribed cube of the point cloud cluster can be expressed as: ,in, is the 3D coordinate of the center point of the circumscribed cube of the point cloud cluster, and the length of the circumscribed cube of the point cloud cluster is , the width is , the height is The spatial area occupied by the circumscribed cube of the point cloud cluster is the rough location of the obstacle.

[0174] S508: Determine whether there are obstacles in the current stacking area.

[0175] In this step, there are obstacles in the current stacking area, that is, in the aforementioned embodiment: whether there is an intersection between the current spatial area to be detected and the spatial position represented by any target three-dimensional point set obtained. When the judgment result is yes, it indicates that there is an obstacle in the current stacking area. In order to detect the exact position of the obstacle, jump to step S509. When the judgment result is no, that is, there is no obstacle in the current stacking area, the process of detecting the current stacking area ends. Subsequently, continue to determine the current spatial area to be detected, and detect whether there is an intersection between the current spatial area to be detected and the spatial position represented by any target three-dimensional point set obtained, until the position of the obstacle in the spatial area to be detected is determined, and the position of the obstacle in the car is obtained. At this time, the process ends and you can jump to step S510.

[0176] S509: Collect obstacle data at close range using a three-dimensional camera, and detect and locate the exact position of the obstacle using an improved deep learning method.

[0177] In this step, the obstacle data is the image to be detected and the corresponding point cloud to be matched in the aforementioned embodiment. The three-dimensional camera collects obstacle data at close range, that is, the control robot in the aforementioned embodiment drives to a detection position in the car that is adjacent to the spatial position represented by the obtained target three-dimensional point set, and the spatial position represented by the obtained target three-dimensional point set is scanned by the three-dimensional camera to obtain the image to be detected and the corresponding point cloud to be matched. The improved deep learning method is the target detection model with DenseNet121 as the backbone network provided by the present invention. The target detection model is constructed based on the YOLOv4 network, and / or the number of output channels of each network layer of DenseNet121 increases according to the growth method described above in the present invention. In addition, the use of the Mish function as the activation function in DenseNet121 can improve the accuracy of the target and target position detected by the target detection model.

[0178] S510: The process ends.

[0179] In this step, the accurate positions of the obstacles in the carriage have been detected. Subsequently, the cargo stacking positions in the carriage can be planned based on the positions of the obstacles, and the cargo can be stacked according to the planned cargo stacking positions.

[0180] Based on the above processing, when there is an obstacle inside the carriage, the rough position of the obstacle determined by the laser radar can be obtained based on the three-dimensional points scanned from the obstacle inside the carriage contained in the initial point cloud. Then, based on the close-range scanning of the rough position of the obstacle by the three-dimensional camera, a more accurate image to be detected and the corresponding point cloud to be matched are obtained, and the position of the obstacle determined from the point cloud to be matched is more accurate. It can be seen that the present invention can detect and locate the exact position of the obstacle in real time during the intelligent loading process, reduce the probability of interference between the stacking position of the goods and the position of the obstacle, and the interference between the movement path of the robot and the position of the obstacle during the loading process, and improve the stability of the loading system. By combining multiple sensors (i.e., laser radar and three-dimensional camera), the rough position of the obstacle is first detected based on the laser radar, prompting the loading system to pay special attention to these areas; during the stacking process, the obstacles within the working range (i.e., the current space area to be detected) are accurately located based on the three-dimensional camera. The experimental results show that based on the obstacle position detection method provided by the present invention, the accuracy of the detected obstacle position is high, and the positioning accuracy is about 1 cm.

[0181] The embodiment of the present invention further provides an obstacle position detection system, which includes a control device and a robot, wherein: the control device is used to execute the obstacle position detection method described in any of the above embodiments. The robot is used to travel under the control of the control device and stack goods in the carriage.

[0182] Based on the above processing, when the target side of the car is closed and opened, the car is scanned outside the car by the laser radar to obtain the initial point cloud. Obviously, when there is an obstacle inside the car, the three-dimensional points contained in the initial point cloud do not belong to the to-be-processed three-dimensional points of the car body, that is, the three-dimensional points scanned from the obstacles inside the car. Then the to-be-processed three-dimensional points are clustered, and the target three-dimensional point set obtained includes the three-dimensional points obtained by scanning the obstacles based on the laser radar. Since the laser radar scans outside the car, the distance between the laser radar and the obstacles inside the car is far, so the three-dimensional points of the obstacles scanned by the laser radar are not accurate, that is, the rough position of the obstacles detected based on the laser radar. After the robot equipped with a three-dimensional camera enters the car, the three-dimensional camera is used to scan the spatial position represented by the three-dimensional point set at the detection position adjacent to the spatial position represented by the target three-dimensional point set. At this time, the three-dimensional camera is already inside the car, and the scanned point cloud to be matched is more accurate than the three-dimensional points to be processed obtained by scanning outside the car. Therefore, the position of the obstacle determined from the point cloud to be matched according to the image to be detected is more accurate. That is, the exact position of the obstacle can be located by detecting the spatial position of the obstacle based on the three-dimensional camera.

[0183] Based on the same inventive concept as the above-mentioned obstacle position detection method, the present invention also provides an obstacle position detection device. Figure 6 , Figure 6 A structural diagram of the obstacle position detection device provided by the present invention. The device includes: a first acquisition module 601, used to acquire an initial point cloud; wherein the initial point cloud is obtained by scanning the car body outside the car body through a laser radar in the robot when the target side of the car body is closed and opened; the robot is also provided with a three-dimensional camera; a three-dimensional point determination module 602, used to determine the three-dimensional points of the car body that do not belong to the car body from the three-dimensional points included in the initial point cloud as the three-dimensional points to be processed; a clustering module 603, used to cluster the three-dimensional points to be processed to obtain at least one target three-dimensional point set; a second acquisition module 604, used to control When the target side is open, the robot drives to a detection position in the car that is adjacent to the spatial position represented by the obtained target three-dimensional point set, and scans the spatial position represented by the obtained target three-dimensional point set through the three-dimensional camera to obtain an image to be detected and a corresponding point cloud to be matched; a detection module 605 is used to detect the image area occupied by the obstacle in the obtained image to be detected; a position determination module 606 is used to determine the spatial position represented by the three-dimensional points corresponding to the pixel points contained in the detected image area in the corresponding point cloud to be matched as the position of the obstacle in the car.

[0184] Optionally, the second acquisition module 604 is specifically used to: determine the current spatial area to be detected from multiple spatial areas to be detected according to a specified direction; wherein the multiple spatial areas to be detected are: multiple rectangular spatial areas obtained by dividing the spatial area in the car in the specified direction; the specified direction is the direction in which the surface opposite to the target side points to the target side; when the current spatial area to be detected intersects with the spatial position represented by any obtained target three-dimensional point set, control the robot to drive to the detection position at the specified side of the current spatial area to be detected with the target side opened, and obtain an image containing the current spatial area to be detected and a point cloud of the current spatial area to be detected through the three-dimensional camera, respectively as The current image to be detected and the corresponding point cloud to be matched; wherein the specified side represents the side of the current spatial area to be detected close to the side of the target; the detection module 605 is specifically used to: detect the image area occupied by the obstacle in the current image to be detected as the current image area to be processed; the position determination module 606 is specifically used to: determine the spatial position represented by the three-dimensional points corresponding to the pixel points contained in the current image area to be processed in the point cloud to be matched corresponding to the current image to be detected as the position of the obstacle in the current spatial area to be detected; and return to execute the step of determining the current spatial area to be detected from multiple spatial areas to be detected in the specified direction until the position of the obstacle in the spatial area to be detected is determined, and the position of the obstacle in the car is obtained.

[0185] Optionally, the target side is one of the six body faces of the body; the origin of the robot coordinate system is located at the specified position of the robot; the position of the obstacle in the current space area to be detected is the position in the car coordinate system of the car; the device also includes: a stacking planning module, which is used to execute the position determination module 606 to determine the spatial position represented by the three-dimensional points corresponding to the pixel points contained in the current image area to be processed in the point cloud to be matched corresponding to the current image to be detected as the position of the obstacle in the current space area to be detected, and then execute the position of the obstacle in the current space area to be detected in the car coordinate system, the position of the current space area to be detected in the car coordinate system, and the sizes of various types of goods to be stacked in the current space area to be detected, and plan the current space area to be detected in the stacking generation algorithm through the stacking generation algorithm. The cargo stacking position in the car coordinate system; wherein, the position of the current space area to be detected in the car coordinate system is: obtained based on the distance between the car body surface relative to the car; a conversion module, used to execute the detection position of the robot at the specified side of the current space area to be detected, based on the distance between the origin of the car coordinate system and the origin of the current robot coordinate system, and the angle between the car body surface of the car and the coordinate plane of the current robot coordinate system, to convert the cargo stacking position in the current space area to be detected in the car coordinate system, so as to obtain the cargo stacking position in the current space area to be detected in the current robot coordinate system; a control module, used to execute control of the robot to stack the cargo according to the cargo stacking position in the current space area to be detected in the current robot coordinate system until the cargo stacking in the current space area to be detected is completed.

[0186] Optionally, the relative box faces are determined in the following manner: when the robot is docked outside the target side, the three-dimensional points contained in the initial point cloud are processed based on a plane segmentation algorithm to obtain plane equations of multiple box faces in the current robot coordinate system; wherein a plane equation in the current robot coordinate system is expressed in the following form: ax+by+cz+d=0; a represents the coefficient of the variable x; b represents the coefficient of the variable y; c represents the coefficient of the variable z; d represents the constant term in the plane equation; for two box faces, a×d corresponding to one box face>0, and , the corresponding a×d of the other box surface is <0, and , the two body surfaces are determined to be opposite body surfaces; wherein, represents the maximum angle allowed between the relative box surface and the yoz coordinate plane of the current robot coordinate system; o represents the origin of the current robot coordinate plane; for two box surfaces, b×d corresponding to one box surface>0, and , the other box surface corresponding to b×d<0, and , the two body surfaces are determined to be opposite body surfaces; wherein, Indicates the maximum angle allowed between the relative box surface and the zox coordinate plane of the current robot coordinate system; for two box surfaces, c×d>0 on one box surface, and , the c×d corresponding to the other box surface is <0, and , the two body surfaces are determined to be opposite body surfaces; wherein, Indicates the maximum angle allowed between the relative box surface and the xoy coordinate plane of the current robot coordinate system.

[0187] Optionally, the robot includes a robotic arm for grabbing goods; the three-dimensional camera is arranged at the end of the robotic arm; the second acquisition module 604 is specifically used to: collect an image in the direction of the current spatial area to be detected through the three-dimensional camera, and obtain an image containing the current spatial area to be detected as the current image to be detected; scan the direction of the current spatial area to be detected through the three-dimensional camera, and determine the three-dimensional points belonging to the current spatial area to be detected from the scanning results, to obtain the current point cloud to be matched.

[0188] Optionally, the three-dimensional points in the initial point cloud are three-dimensional points in the robot coordinate system; the first acquisition module 601 is specifically used to: scan the car body at multiple scanning positions outside the car body by the laser radar respectively, and obtain the point cloud to be converted corresponding to each scanning position; wherein the union of the scanning ranges at each scanning position can cover the spatial area occupied by the car body; for the point cloud to be converted corresponding to each scanning position, the three-dimensional points contained in the point cloud to be converted are converted according to the target transformation relationship to obtain the three-dimensional points in the initial point cloud; wherein the target transformation relationship represents: the transformation relationship between the radar coordinate system and the robot coordinate system.

[0189] Optionally, the robot includes a mechanical arm for grabbing goods; the laser radar is arranged at the end of the mechanical arm;

[0190] The first acquisition module 601 is specifically used to: for each scanning position corresponding to the point cloud to be converted, according to the preset conversion formula between the radar coordinate system and the robot coordinate system, convert the three-dimensional points contained in the point cloud to be converted to obtain the three-dimensional points in the initial point cloud; wherein the preset conversion formula includes:

[0191] ;

[0192] st

[0193] ;

[0194] st

[0195] ;

[0196] ;

[0197] A first transformation matrix representing the radar coordinate system and the tool coordinate system of the manipulator; A second transformation matrix representing the tool coordinate system and the robot coordinate system; Represents the three-dimensional coordinates of the three-dimensional points contained in the point cloud to be converted; represents the three-dimensional coordinates of the three-dimensional points in the initial point cloud obtained by transformation; r represents the rotation matrix from the tool coordinate system to the robot coordinate system; t represents the translation matrix from the tool coordinate system to the robot coordinate system; represents the coordinates of the origin of the tool coordinate system in the robot coordinate system; represents the Euler angle of the tool coordinate system relative to the robot coordinate system.

[0198] Optionally, the three-dimensional point determination module 602 is specifically used to: process the three-dimensional points contained in the initial point cloud based on a plane segmentation algorithm to obtain multiple planes; for each three-dimensional point contained in the initial point cloud, if the distance between the three-dimensional point and each obtained plane is not less than a preset distance, then the three-dimensional point is used as a to-be-processed three-dimensional point that does not belong to the body of the vehicle.

[0199] Optionally, the clustering module 603 is specifically used to: take any unclustered three-dimensional point to be processed as the current cluster center; determine the three-dimensional points to be processed that are located within the extended space occupied by the candidate three-dimensional point set to which the current cluster center belongs from the current unclustered three-dimensional points to be processed; add the determined three-dimensional points to be processed to the candidate three-dimensional point set to which the current cluster center belongs, and return to execute the step of determining the three-dimensional points to be processed that are located within the extended space occupied by the candidate three-dimensional point set to which the current cluster center belongs from the current unclustered three-dimensional points to be processed, until there are no three-dimensional points to be processed that are located within the extended space occupied by the candidate three-dimensional point set to which the current cluster center belongs among the current unclustered three-dimensional points to be processed; and return to execute the step of taking any unclustered three-dimensional point to be processed as the current cluster center until there are no unclustered three-dimensional points to be processed; and determine, from the candidate three-dimensional point sets to which each cluster center belongs, a candidate three-dimensional point set whose number of three-dimensional points is not less than a preset number threshold as the target three-dimensional point set.

[0200] Optionally, the detection module 605 is specifically used to: input the obtained image to be detected into a pre-trained target detection model with DenseNet121 as the backbone network to obtain the image area occupied by the obstacle; the activation function in the DenseNet121 includes the Mish function, and / or the number of output channels of each network layer of the DenseNet121 increases in the following manner:

[0201] ;

[0202] Where i=1, 2, ..., n; % represents the remainder operation. represents the number of output channels of the i-th network layer, n represents the number of network layers included in the DenseNet121; and / or, the target detection model is built based on the YOLOv4 network.

[0203] Based on the obstacle position detection device provided by the present invention, when the target side of the carriage is closed and opened, the carriage is scanned by the laser radar outside the carriage to obtain an initial point cloud, that is, the carriage is scanned more comprehensively by the laser radar. Naturally, when there is an obstacle inside the carriage, the initial point cloud will also include the three-dimensional points scanned from the obstacle. Then the three-dimensional points included in the initial point cloud do not belong to the carriage body of the carriage, that is, the three-dimensional points scanned from the obstacles inside the carriage. Obviously, the three-dimensional points to be processed are clustered, and the target three-dimensional point set obtained includes the three-dimensional points obtained by scanning the obstacles based on the laser radar. Since the laser radar scans outside the carriage, that is, the distance between the laser radar and the obstacles inside the carriage is far, the three-dimensional points of the obstacles scanned by the laser radar are not accurate. That is, the rough position of the obstacle is detected based on the laser radar first. After the robot equipped with a three-dimensional camera enters the carriage, the spatial position represented by the three-dimensional point set is scanned by the three-dimensional camera at the detection position adjacent to the spatial position represented by the target three-dimensional point set. At this time, the 3D camera is already inside the car, so the scanned point cloud to be matched is more accurate than the 3D points to be processed scanned outside the car. Therefore, the position of the obstacle determined from the point cloud to be matched according to the image to be detected is more accurate. That is, based on the 3D camera to detect the spatial position of the obstacle, the exact position of the obstacle can be located.

[0204] The embodiment of the present invention also provides a control device, such as Figure 7As shown, the system includes a processor 701, a communication interface 702, a memory 703 and a communication bus 704, wherein the processor 701, the communication interface 702 and the memory 703 communicate with each other through the communication bus 704. The memory 703 is used to store computer programs; the processor 701 is used to implement the steps of any obstacle position detection method in the above embodiments when executing the program stored in the memory 703.

[0205] The communication bus mentioned in the above control device can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. The communication bus can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, only one thick line is used in the figure, but it does not mean that there is only one bus or one type of bus.

[0206] The communication interface is used for communication between the above control device and other devices.

[0207] The memory may include a random access memory (RAM) or a non-volatile memory (NVM), such as at least one disk memory. Optionally, the memory may also be at least one storage device located away from the aforementioned processor.

[0208] The above-mentioned processor can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it can also be a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components.

[0209] In another embodiment of the present invention, a computer-readable storage medium is provided, in which a computer program is stored. When the computer program is executed by a processor, the steps of any of the above obstacle position detection methods are implemented.

[0210] In another embodiment of the present invention, a computer program product including instructions is provided. When the computer program product is run on a computer, the computer executes any obstacle position detection method in the above embodiments.

[0211] In the above embodiments, it can be implemented in whole or in part by software, hardware, firmware or any combination thereof. When implemented by software, it can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, the process or function described in the embodiment of the present invention is generated in whole or in part. The computer may be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions may be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions may be transmitted from one website site, computer, server or data center to another website site, computer, server or data center by wired (e.g., coaxial cable, optical fiber, digital subscriber line (DSL)) or wireless (e.g., infrared, wireless, microwave, etc.) means. The computer-readable storage medium may be any available medium that a computer can access or a data storage device such as a server or data center that includes one or more available media integrated. The available medium may be a magnetic medium (e.g., a floppy disk, a hard disk, a tape), an optical medium (e.g., a DVD), or a semiconductor medium (e.g., a solid-state drive Solid State Disk (SSD)), etc.

[0212] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article 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, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0213] Each embodiment in this specification is described in a related manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system, device, control device, computer-readable storage medium, and computer program product embodiments, since they are basically similar to the method embodiments, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiments.

[0214] The above description is only a preferred embodiment of the present invention and is not intended to limit the protection scope of the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention are included in the protection scope of the present invention.

Claims

1. A method for detecting an obstacle position, characterized in that: The method comprises: Acquire an initial point cloud; wherein the initial point cloud is obtained by scanning the compartment outside the compartment through a laser radar in the robot when the target side of the compartment is closed and opened; the robot is also provided with a three-dimensional camera; Determine, from the three-dimensional points included in the initial point cloud, three-dimensional points of the body of the carriage that do not belong to the carriage as three-dimensional points to be processed; Clustering the three-dimensional points to be processed to obtain at least one target three-dimensional point set; Controlling the robot to drive to a detection position in the vehicle compartment adjacent to a spatial position represented by the obtained target three-dimensional point set in a state where the target side is opened, and scanning the spatial position represented by the obtained target three-dimensional point set by the three-dimensional camera to obtain an image to be detected and a corresponding point cloud to be matched; The image area occupied by the obstacle in the detected image; Determine the spatial position represented by the three-dimensional point corresponding to the pixel point contained in the detected image area in the corresponding to-be-matched point cloud as the position of the obstacle in the vehicle compartment; The robot is controlled to drive to a detection position in the vehicle compartment adjacent to a spatial position represented by the obtained target three-dimensional point set in a state where the target side is opened, and the spatial position represented by the obtained target three-dimensional point set is scanned by the three-dimensional camera to obtain an image to be detected and a corresponding point cloud to be matched, including: According to the specified direction, determine the current space area to be detected from multiple space areas to be detected; wherein the multiple space areas to be detected are: multiple rectangular space areas obtained by dividing the space area in the vehicle compartment in the specified direction; the specified direction is the direction in which the surface opposite to the target side surface points to the target side surface; In the case where the current spatial region to be detected and the spatial position represented by any obtained target three-dimensional point set have an intersection, the robot is controlled to drive to the detection position at the designated side of the current spatial region to be detected with the target side opened, and an image containing the current spatial region to be detected and a point cloud of the current spatial region to be detected are obtained through the three-dimensional camera, as the current image to be detected and the corresponding point cloud to be matched, respectively; wherein the designated side refers to a side of the current spatial region to be detected close to the target side; The image area occupied by the obstacle in the detected image includes: Detect the image area occupied by the obstacle in the current image to be detected as the current image area to be processed; The step of determining the spatial position represented by the three-dimensional point corresponding to the pixel point contained in the detected image area in the corresponding to-be-matched point cloud as the position of the obstacle in the vehicle compartment comprises: The spatial position represented by the three-dimensional point corresponding to the pixel point contained in the current image area to be processed in the point cloud to be matched corresponding to the current image to be detected is determined as the position of the obstacle in the current spatial area to be detected; And return to execute the step of determining the current space area to be detected from multiple space areas to be detected according to the specified direction, until the position of the obstacle in the space area to be detected is determined, and the position of the obstacle in the car is obtained.

2. The method according to claim 1, characterized in that The target side surface is one of the six body surfaces of the body; the origin of the robot coordinate system is located at the specified position of the robot; the position of the obstacle in the current space area to be detected is the position in the body coordinate system of the body; After determining the spatial position represented by the three-dimensional point corresponding to the pixel point contained in the current image area to be processed in the to-be-detected point cloud corresponding to the current image to be detected as the position of the obstacle in the current spatial area to be detected, the method further includes: Based on the positions of obstacles in the current space area to be detected in the car coordinate system, the current position of the space area to be detected in the car coordinate system, and the sizes of various types of goods to be stacked in the current space area to be detected, the cargo stacking position in the current space area to be detected in the car coordinate system is planned by a stacking generation algorithm; wherein the current position of the space area to be detected in the car coordinate system is obtained based on the distance between the car body surfaces of the car body and the car body surface; When the robot is parked at the detection position at the designated side of the current space area to be detected, based on the distance between the origin of the car coordinate system and the origin of the current robot coordinate system, and the angle between the body surface of the car and the coordinate plane of the current robot coordinate system, the cargo stacking position in the current space area to be detected in the car coordinate system is converted to obtain the cargo stacking position in the current space area to be detected in the current robot coordinate system; The robot is controlled to stack the goods according to the goods stacking position in the current space area to be detected in the current robot coordinate system until the goods are stacked in the current space area to be detected.

3. The method according to claim 2, characterized in that The opposing cabin faces are determined as follows: When the robot is parked outside the side of the target, the three-dimensional points included in the initial point cloud are processed based on the plane segmentation algorithm to obtain plane equations of multiple cabin surfaces in the current robot coordinate system; wherein a plane equation in the current robot coordinate system is expressed in the following form: ax+by+cz+d=0; a represents the coefficient of the variable x; b represents the coefficient of the variable y; c represents the coefficient of the variable z; d represents the constant term in the plane equation; For two box faces, a×d>0 on one box face, and , the corresponding a×d of the other box surface is <0, and , the two body surfaces are determined to be opposite body surfaces; wherein, Indicates the maximum angle allowed between the relative box surface and the yoz coordinate plane of the current robot coordinate system; o indicates the origin of the current robot coordinate plane; For two box faces, b×d>0 on one box face, and , the other box surface corresponding to b×d<0, and , the two body surfaces are determined to be opposite body surfaces; wherein, Indicates the maximum angle allowed between the relative box surface and the zox coordinate plane of the current robot coordinate system; For two box faces, the corresponding c×d on one box face is greater than 0, and , the c×d corresponding to the other box surface is < 0, and , the two body surfaces are determined to be opposite body surfaces; wherein, Indicates the maximum angle allowed between the relative box surface and the xoy coordinate plane of the current robot coordinate system.

4. The method according to claim 1, characterized in that: The robot comprises a mechanical arm for grabbing goods; the three-dimensional camera is arranged at the end of the mechanical arm; The step of acquiring an image containing the current spatial region to be detected and a point cloud of the current spatial region to be detected, respectively serving as the current image to be detected and the corresponding point cloud to be matched, comprises: The three-dimensional camera collects an image in the direction of the current spatial region to be detected, and obtains an image containing the current spatial region to be detected as the current image to be detected; The three-dimensional camera is used to scan the direction of the current space area to be detected, and the three-dimensional points belonging to the current space area to be detected are determined from the scanning results to obtain the current point cloud to be matched.

5. The method according to claim 1, characterized in that The three-dimensional points in the initial point cloud are three-dimensional points in the robot coordinate system; The obtaining of the initial point cloud comprises: The carriage is scanned by the laser radar at multiple scanning positions outside the carriage to obtain a point cloud to be converted corresponding to each scanning position; wherein the union of the scanning ranges at each scanning position can cover the spatial area occupied by the carriage; For the point cloud to be converted corresponding to each scanning position, the three-dimensional points contained in the point cloud to be converted are converted according to the target conversion relationship to obtain the three-dimensional points in the initial point cloud; wherein the target conversion relationship represents: the conversion relationship between the radar coordinate system and the robot coordinate system.

6. The method according to claim 5, characterized in that The robot comprises a mechanical arm for grabbing goods; the laser radar is arranged at the end of the mechanical arm; The step of converting the three-dimensional points contained in the point cloud to be converted corresponding to each scanning position according to the target conversion relationship to obtain the three-dimensional points in the initial point cloud includes: For the point cloud to be converted corresponding to each scanning position, the three-dimensional points contained in the point cloud to be converted are converted according to the preset conversion formula between the radar coordinate system and the robot coordinate system to obtain the three-dimensional points in the initial point cloud; Wherein, the preset conversion formula includes: ； st ； st ； ； A first transformation matrix representing the radar coordinate system and the tool coordinate system of the manipulator; A second transformation matrix representing the tool coordinate system and the robot coordinate system; Represents the three-dimensional coordinates of the three-dimensional points contained in the point cloud to be converted; represents the three-dimensional coordinates of the three-dimensional points in the initial point cloud obtained by transformation; r represents the rotation matrix from the tool coordinate system to the robot coordinate system; t represents the translation matrix from the tool coordinate system to the robot coordinate system; represents the coordinates of the origin of the tool coordinate system in the robot coordinate system; represents the Euler angle of the tool coordinate system relative to the robot coordinate system.

7. The method according to claim 1, characterized in that Determining a three-dimensional point of a body that does not belong to the carriage from the three-dimensional points included in the initial point cloud as a three-dimensional point to be processed, including: Processing the three-dimensional points included in the initial point cloud based on a plane segmentation algorithm to obtain multiple planes; For each three-dimensional point included in the initial point cloud, if the distance between the three-dimensional point and each obtained plane is not less than a preset distance, the three-dimensional point is regarded as a to-be-processed three-dimensional point of the body that does not belong to the carriage.

8. The method according to claim 1, characterized in that Clustering the three-dimensional points to be processed to obtain at least one target three-dimensional point set includes: Take any unclustered 3D point to be processed as the current cluster center; From the currently unclustered 3D points to be processed, determine the 3D points to be processed that are located within the outer expansion space occupied by the candidate 3D point set to which the current cluster center belongs; Add the determined 3D points to be processed to the candidate 3D point set to which the current cluster center belongs, and return to the step of determining the 3D points to be processed that are located within the outer expansion space occupied by the candidate 3D point set to which the current cluster center belongs from the 3D points to be processed that are not currently clustered, until there are no 3D points to be processed that are located within the outer expansion space occupied by the candidate 3D point set to which the current cluster center belongs among the 3D points to be processed that are not currently clustered; and returning to execute the step of taking any unclustered three-dimensional point to be processed as the current cluster center until there are no unclustered three-dimensional points to be processed; From the candidate three-dimensional point sets to which each cluster center belongs, a candidate three-dimensional point set including three-dimensional points whose number is not less than a preset number threshold is determined as a target three-dimensional point set.

9. The method according to claim 1, characterized in that: The image area occupied by the obstacle in the detected image includes: The obtained image to be detected is input into the pre-trained target detection model with DenseNet121 as the backbone network to obtain the image area occupied by the obstacle; The activation function in the DenseNet 121 includes a Mish function, and / or the number of output channels of each network layer of the DenseNet 121 increases in the following manner: ； Where i=1, 2, ..., n; % represents the remainder operation. represents the number of output channels of the i-th network layer, and n represents the number of network layers included in the DenseNet121; and / or, The target detection model is built based on the YOLOv4 network.

10. An obstacle position detection system, characterized in that: The obstacle position detection system comprises a control device and a robot, wherein: The control device is used to execute the method described in any one of claims 1 to 9 above; The robot is used to travel under the control of the control device and stack goods in the carriage.

11. An obstacle position detection device, characterized in that: The device comprises: A first acquisition module is used to acquire an initial point cloud; wherein the initial point cloud is obtained by scanning the compartment outside the compartment through a laser radar in the robot when the target side of the compartment is closed and opened; the robot is also provided with a three-dimensional camera; A three-dimensional point determination module, used to determine three-dimensional points of the body of the carriage that do not belong to the carriage from the three-dimensional points included in the initial point cloud as three-dimensional points to be processed; A clustering module, used for clustering the three-dimensional points to be processed to obtain at least one target three-dimensional point set; A second acquisition module is used to control the robot to drive to a detection position in the compartment adjacent to the spatial position represented by the obtained target three-dimensional point set when the target side is open, and to scan the spatial position represented by the obtained target three-dimensional point set through the three-dimensional camera to obtain an image to be detected and a corresponding point cloud to be matched; A detection module, used to detect the image area occupied by obstacles in the obtained image to be detected; A position determination module, used to determine the spatial position represented by the three-dimensional point corresponding to the pixel point contained in the detected image area in the corresponding to-be-matched point cloud as the position of the obstacle in the vehicle compartment; The second acquisition module is specifically used to: determine the current spatial area to be detected from multiple spatial areas to be detected according to a specified direction; wherein the multiple spatial areas to be detected are: multiple rectangular spatial areas obtained by dividing the spatial area in the car in the specified direction; the specified direction is the direction in which the surface opposite to the target side faces the target side; when the current spatial area to be detected and the spatial position represented by any obtained target three-dimensional point set have an intersection, control the robot to drive to the detection position at the specified side of the current spatial area to be detected with the target side opened, and obtain an image containing the current spatial area to be detected and a point cloud of the current spatial area to be detected through the three-dimensional camera, as the current image to be detected and the corresponding point cloud to be matched respectively; wherein the specified side represents the side of the current spatial area to be detected close to the target side; The detection module is specifically used to: detect the image area occupied by the obstacle in the current image to be detected as the current image area to be processed; The position determination module is specifically used to: determine the spatial position represented by the three-dimensional point corresponding to the pixel point contained in the current image area to be processed in the point cloud to be matched corresponding to the current image to be detected as the position of the obstacle in the current spatial area to be detected; and return to execute the step of determining the current spatial area to be detected from multiple spatial areas to be detected in a specified direction until the position of the obstacle in the spatial area to be detected is determined, thereby obtaining the position of the obstacle in the vehicle compartment.

12. A control device, characterized in that: It includes a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory communicate with each other through the communication bus; Memory, used to store computer programs; A processor, for implementing any of the methods described in claims 1-9 when executing a program stored in a memory.

13. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method according to any one of claims 1 to 9 is implemented.

Images