Door opening method, robot, and computer-readable storage medium

Abstract

The present application discloses a door opening method, a robot, and a computer-readable storage medium. The door opening method includes: recognizing a color image of a target scene to obtain an image of a door and an image of a handle; determining a first point cloud corresponding to the image of the door and a second point cloud corresponding to the image of the handle according to a depth image of the target scene; obtaining the state of the door; when the state of the door is a closed state, determining a normal vector parallel to the ground of the door according to at least one of the first point cloud and the second point cloud; determining the pose of an actuator at least according to the normal vector and the second point cloud; driving the actuator to grasp the handle according to the pose; while the actuator grasps the handle, driving the actuator to move along a first circular arc trajectory to turn the handle, and driving the actuator to move along a second circular arc trajectory to open the door. The method provided by the present application enables the robot to accurately perform autonomous door opening.

Description

Technical Field

[0001] This application relates to the technical field of robotics, and particularly to a door opening method, a robot, and a computer-readable storage medium. Background Art

[0002] With the development of intelligent robot technology, its application requirements and scenarios are gradually becoming more complex. The autonomous door opening function has become one of the important skills of many robots. After visually recognizing and roughly positioning the door and the handle, more specific information about them needs to be analyzed to effectively plan the door opening action.

[0003] Currently, when a robot implements the autonomous door opening function, the algorithms used are complex and the accuracy rate also needs to be further improved. Summary of the Invention

[0004] This application provides a door opening method, a robot, and a computer-readable storage medium, which can enable the robot to accurately perform autonomous door opening.

[0005] In the first aspect of the embodiments of this application, a door opening method is provided. The method includes: recognizing a color image of a target scene to obtain an image of a door and an image of a handle; determining a first point cloud corresponding to the image of the door and a second point cloud corresponding to the image of the handle according to a depth image of the target scene; obtaining the state of the door; when the state of the door is a closed state, determining a normal vector parallel to the ground of the door according to at least one of the first point cloud and the second point cloud; determining the pose of an actuator at least according to the normal vector and the second point cloud; driving the actuator to grasp the handle according to the pose; while the actuator grasps the handle, driving the actuator to move along a first circular arc trajectory to turn the handle, and driving the actuator to move along a second circular arc trajectory to open the door.

[0006] In the second aspect of the embodiments of this application, a robot is provided. The robot includes a processor, a memory, and a communication circuit. The processor is respectively coupled to the memory and the communication circuit. Program data is stored in the memory. The processor realizes the steps in the above method by executing the program data in the memory.

[0007] In the third aspect of the embodiments of this application, a computer-readable storage medium is provided. The computer-readable storage medium stores a computer program, and the computer program can be executed by a processor to realize the steps in the above method.

[0008] The beneficial effects are as follows: The door opening method of this application includes: recognizing a color image of a target scene to obtain an image of a door and an image of a handle; determining a first point cloud corresponding to the image of the door and a second point cloud corresponding to the image of the handle according to a depth image of the target scene; obtaining the state of the door; when the state of the door is a closed state, determining a normal vector parallel to the ground of the door according to at least one of the first point cloud and the second point cloud; determining the pose of an actuator at least according to the normal vector and the second point cloud; driving the actuator to grab the handle according to the pose; while the actuator grabs the handle, driving the actuator to move along a first circular arc trajectory to turn the handle, and driving the actuator to move along a second circular arc trajectory to open the door. The method of this application accurately analyzes the position and state information of the door and the handle during the process of controlling a robot to open the door, without manual participation, and can improve the accuracy of the robot's autonomous door opening. Description of the Drawings

[0009] To more clearly illustrate the technical solutions in the embodiments of this application, the following will briefly introduce the drawings required for the description of the embodiments. Obviously, the drawings in the following description are only some embodiments of this application. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings. Among them:

[0010] Figure 1 is a schematic flowchart of an embodiment of the door opening method of this application;

[0011] Figure 2 is Figure 1 a partial flowchart of step S130 in

[0012] Figure 3 is Figure 1 a partial flowchart of step S130 in

[0013] Figure 4 is a schematic structural diagram of the behavior tree of this application;

[0014] Figure 5 is a schematic structural diagram of an embodiment of the robot of this application;

[0015] Figure 6 is a schematic structural diagram of an embodiment of the computer-readable storage medium of this application. Detailed Embodiments

[0016] The following will clearly and completely describe the technical solutions in the embodiments of this application with reference to the drawings in the embodiments of this application. Obviously, the described embodiments are only some embodiments of this application, rather than all embodiments. Based on the embodiments in this application, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of this application.

[0017] Refer to Figure 1 , Figure 1 which is a schematic flowchart of an embodiment of the door opening method of the present application. It should be noted that in this embodiment, the L-shaped handle is used to introduce the door opening process of the robot. The L-shaped handle is in an L shape, including a connecting section and a handle section. One end of the connecting section is connected to one end of the handle section, and at the same time, the other end of the connecting section is connected to the first surface of the door. At the same time, the length direction of the handle section is parallel to the first surface. The door opening process includes:

[0018] S110: Identify the color image of the target scene to obtain the image of the door and the image of the handle.

[0019] Specifically, when identifying the color image, a first rectangular frame of the door and a second rectangular frame of the handle are obtained. The first rectangular frame frames the door, and the second rectangular frame frames the handle.

[0020] In order to determine whether the door is in the closed state or the open state subsequently, in addition to framing the door, the first rectangular frame of the door also needs to include a part of the area around the door, that is to say, the first rectangular frame should be appropriately larger than the door. Among them, the process of determining whether the door is in the closed state or the open state can be specifically referred to below.

[0021] S120: Determine the first point cloud of the image of the door and the second point cloud of the image of the handle according to the depth image of the target scene.

[0022] Specifically, after identifying the door and the handle in step S110, according to the depth image, the first point cloud of the scene within the first rectangular frame, that is, the first point cloud of the door, and the second point cloud of the scene within the second rectangular frame, that is, the second point cloud of the handle, are obtained.

[0023] S130: Obtain the state of the door, and when the state of the door is the closed state, obtain the preset parameters based on the first point cloud and the second point cloud.

[0024] First, preprocess the first point cloud. The preprocessing includes: converting the first point cloud from the point cloud coordinate system to the world coordinate system. Then, in order to improve the calculation efficiency, downsample the obtained first point cloud, and remove the points with smaller z values in the first point cloud.

[0025] After the preprocessing is completed, determine the state of the door according to the first point cloud. Refer to Figure 2 , and this process includes:

[0026] S131: Screen out the points in the first point cloud whose normal vectors are parallel to the ground.

[0027] Among them, the normal vector of a point refers to the normal vector of the plane formed by the point and several surrounding points. Specifically, for each point in the first point cloud, it is determined whether the normal vector of the point is parallel to the ground. If it is parallel to the ground, it is screened out.

[0028] S132: Use the region growing algorithm based on the normal vector to cluster the screened points to obtain the first sub-point clouds of different categories.

[0029] Specifically, cluster the screened points according to the normal vectors of the points. After clustering, the normal vectors of all points in the first sub-point cloud of the same category are the same or the difference is within a predetermined range, and the normal vectors of the points in the first sub-point clouds of different categories are different. That is to say, the first sub-point clouds of the same category are in or approximately in the same plane, and the first sub-point clouds of different categories are in different planes.

[0030] S133: Determine the state of the door according to the first sub-point clouds of different categories.

[0031] When the number of the first sub-point clouds obtained in step S132 is one, it means that all the points screened out in step S131 are in or approximately in the same plane. At this time, it can be determined that the door is in the closed state.

[0032] When the number of the first sub-point clouds obtained in step S132 is multiple, it can be directly determined that the door is in the open state, or considering that when some doors are in the closed state, the door and the wall will not be completely in the same plane. At this time, in order to improve the accuracy of the judgment, the multiple first sub-point clouds are sorted in descending order according to the number of points they contain, and then the first included angle between the plane where the first first sub-point cloud is located and the plane where the second first sub-point cloud is located, and the second included angle between the plane where the first first sub-point cloud is located and the plane where the third first sub-point cloud is located are determined. If both the first included angle and the second included angle are less than the included angle threshold, it is determined that the door is in the closed state, otherwise it is determined that the door is in the open state, or as long as the first angle is less than the included angle threshold, it is determined that the door is in the closed state, otherwise it is determined that the door is in the open state.

[0033] Among them, when the door is in the open state, the robot can directly pass through the door without performing the door-opening action. Only when the door is in the closed state does the robot need to perform the door-opening action. Therefore, only when it is determined that the state of the door is the closed state, the subsequent steps are executed.

[0034] After determining that the state of the door is the closed state, according to the first sub-point cloud including the most points, determine the door plane, where the door plane refers to the surface of the door perpendicular to the ground. Refer to Figure 3 , the process of determining the door plane includes:

[0035] S134: Cluster the first sub-point cloud with the most points using the RGB region growing algorithm to obtain second sub-point clouds of different categories.

[0036] S135: Determine the region defined by the circumscribed rectangle of the second sub-point cloud with the most points as the door plane.

[0037] Considering that the first sub-point cloud with the most points may still include wall points, in order to ensure accuracy and considering the different colors of the door and the wall, cluster the first sub-point cloud with the most points using the RGB region growing algorithm, that is, classify the first sub-point cloud with the most points according to the color of the points to obtain second sub-point clouds of different categories, and then determine the region defined by the circumscribed rectangle of the second sub-point cloud with the most points as the door plane.

[0038] Of course, it is also possible to directly determine the region defined by the circumscribed rectangle of the first sub-point cloud with the most points as the door plane.

[0039] After determining the door plane, determine the normal vector of the door plane as the normal vector of the door parallel to the ground, and determine two door frames according to the door plane. It can be understood that the door plane determined by the above method is rectangular, so the two sides perpendicular to the ground in the door plane are the door frames.

[0040] The above is the processing process for the first point cloud. The following introduces the processing process for the second point cloud:

[0041] Similarly, preprocess the second point cloud first. The preprocessing includes: converting the second point cloud from the point cloud coordinate system to the world coordinate system.

[0042] After the preprocessing is completed, considering that the background of the handle is the door, so the second point cloud includes part of the point cloud of the door. Therefore, the RGB region growing algorithm can be used to cluster the second point cloud, that is, classify the second point cloud according to the color of the points to obtain third sub-point clouds of two categories. Among them, the third sub-point cloud where all points are in or approximately in the same plane is the point cloud of the door, and the other third sub-point cloud is the point cloud of the handle.

[0043] At this time, the normal vector of the door parallel to the ground can also be determined according to the point cloud of the door. The specific method includes: calculating the normal vector of the region defined by the circumscribed rectangle of the point cloud of the door.

[0044] Among them, in order to improve the accuracy, the handle point cloud can be clustered again. Specifically, at this time, cluster the handle point cloud according to the distance between points to obtain fourth sub-point clouds of different categories. Among them, the fourth sub-point cloud with the most points can be used as the final handle point cloud, or the fourth sub-point cloud where the center point of the second rectangle is located can also be used as the final handle point cloud.

[0045] After obtaining the handle point cloud, perform PCA (Principal Component Analysis) decomposition on the handle point cloud to obtain the long axis of the L-shaped handle. Among them, the long axis of the L-shaped handle refers to the axis passing through both ends of the handle segment in the L-shaped handle.

[0046] After processing the first point cloud and the second point cloud as described above, the normal vector of the door parallel to the ground, the two door frames, and the long axis of the L-shaped handle are obtained. It can be understood that the width of the door can be determined according to the distance between the two door frames.

[0047] The following introduces the determination process of the door hinge in the two door frames, as well as the determination process of the head and tail of the L-shaped handle:

[0048] Consider the door frame farther from the handle as the door hinge. Therefore, calculate the distances between the two door frames and the handle respectively, and then determine the door frame with the larger distance from the handle as the door hinge.

[0049] Among them, the head of the L-shaped handle refers to the end of the handle segment far from the connecting segment, and the tail refers to the end of the handle segment connected to the connecting segment.

[0050] Among them, if the sizes of the head and tail of the L-shaped handle are known, the head and tail of the handle can be determined according to the number of points included at both ends of the handle segment; if the sizes of the head and tail of the L-shaped handle are unknown, considering that usually, the distance between the head and the door hinge is greater than the distance between the tail and the door hinge, the head and tail of the handle can also be determined according to the distances from both ends of the handle segment to the door hinge.

[0051] S140: Determine the pose according to the normal vector and the extending direction of the long axis; determine the first arc trajectory according to the head and tail of the handle; determine the second arc trajectory according to the width of the door and the door hinge.

[0052] S150: Drive the actuator to grasp the handle according to the pose, and while the actuator grasps the handle, drive the actuator to move along the first arc trajectory to turn the handle, and finally drive the actuator to move along the second arc trajectory to open the door.

[0053] Among them, the trajectory planning during the process of driving the actuator to move along the first arc trajectory or the second arc trajectory can adopt the trajectory planning algorithm in the existing technology, and no more introduction will be made here.

[0054] Specifically, the extending direction of the long axis of the handle is perpendicular to the normal vector of the door. According to the extending direction of the long axis of the handle and the normal vector of the door, the pose when the actuator grasps the handle can be determined. Among them, the actuator can be installed at any position of the robot, such as the end of the robot, and the actuator can specifically be any tool that can open the door.

[0055] During the process of turning the handle, the head of the handle rotates around the tail. Therefore, according to the head and tail of the handle, the first arc trajectory passed by the actuator when turning the handle can be determined. In one application scenario, the distance for driving the actuator to move along the first arc trajectory can be one-fourth of the circumference of a circle with the length of the handle segment as the radius. In another application scenario, the distance for driving the actuator to move along the first arc trajectory can be a fixed value preset by the designer.

[0056] During the process of pushing and pulling the door, the door rotates around the door hinge. Therefore, according to the width of the door and the door hinge, the second arc trajectory passed by the actuator during the process of pushing and pulling the door can be determined. In one application scenario, the distance for driving the actuator to move along the second arc trajectory can be one-fourth of the circumference of a circle with the door width as the radius. In another application scenario, the distance for driving the actuator to move along the second arc trajectory is a fixed value preset by the designer.

[0057] Among them, after the actuator moves along the second arc trajectory and finishes, the actuator releases the handle. Finally, the entire robot passes through the door. That is to say, during the previous whole process, only the robotic arm of the robot is moving, and the base of the robot is in a stationary state. After the actuator moves a predetermined distance along the second arc trajectory, the door is in an open state, and only then can the entire robot pass through the door.

[0058] Among them, during the process of opening the door, it is possible to open the door by pulling the door or by pushing the door. In this embodiment, after driving the actuator to move along the first arc trajectory, the actuator can be driven to perform one of the actions of pushing the door or pulling the door along the second arc trajectory first. If the robot cannot move the door, then the actuator is driven to perform the other action of pushing the door or pulling the door along the second arc trajectory. That is to say, this embodiment does not judge the way of opening the door in advance.

[0059] Although the above specifically introduces the process of the robot opening the door with an L-shaped handle, this application is not limited thereto. When the handle is other types of handles, the above method or a method similar to the above can also be used to drive the robot to open the door.

[0060] For example, when the handle is a spherical handle, compared with the L-shaped handle, there is no long axis. Therefore, at this time, only based on the position of the handle and the normal vector of the door parallel to the ground, the pose of the robot end when grasping the handle can be determined. And when driving the actuator to grasp the handle, the end is directly driven to rotate around the handle to turn the handle, and finally the actuator is driven to move along the second arc trajectory to open the door.

[0061] In this application, the above door-opening method can also be planned using a behavior tree, specifically as Figure 4As shown in the figure, taking the handle as an L-shaped handle, the process of planning to open the door using a behavior tree is introduced as follows:

[0062] Among them, the nodes with "A" are sub-action nodes, which are implemented in the form of ROS action (that is, the implementation progress of the task can be continuously fed back, and it can be terminated during the implementation of the task); the sub-action nodes of the feedback node are executed from left to right, and when the sub-action nodes of the feedback node are successful, the feedback node exits. The sub-nodes of the sequence node are executed from left to right. When a certain sub-node fails, the sequence node exits. The process of opening the door based on the behavior tree is as follows:

[0063] (1) First, continuously detect the door until successful.

[0064] (2) Then, when the door is detected to be in the open state, directly pass through the door. When the door is detected to be in the closed state, enter the door-opening sequence node.

[0065] (3) After entering the door-opening sequence node, the following steps are executed in sequence: detect the handle, drive the robot to move towards the door, grab the handle, turn the handle, and then try to push the door. If the door-pushing is successful, release the handle and pass through the door; if the door-pushing fails, then try to pull the door. After the door-pulling is successful, release the handle and pass through the door.

[0066] It can be seen from the above content that this application can enable the robot to achieve automatic door opening through simple method steps without increasing costs, and the entire process can exclude environmental interference, analyze the position and status information of the door and handle in the environment in real time and accurately, without manual intervention, has a certain robustness in complex environments, and the entire method runs fast and requires fewer parameters to be adjusted.

[0067] In addition, this application also uses a behavior tree to plan the entire door-opening process. Each sub-action is implemented by relatively independent modules, and the invocation of sub-actions is coordinated by the behavior tree. The method of the behavior tree is intuitive and easy to expand and debug. There is no need to directly modify the program source code, and only the structure of the behavior tree needs to be adjusted to achieve the adjustment of the planning process. At the same time, the execution situation of the behavior tree can be dynamically monitored during the execution process. Using these advantages of the behavior tree, the robot can achieve fully autonomous door opening, and it is convenient to define the handling of abnormal situations.

[0068] Refer to Figure 5 , Figure 5It is a schematic structural diagram of an embodiment of the robot in this application. The robot 200 includes a processor 210, a memory 220, and a communication circuit 230. The processor 210 is respectively coupled to the memory 220 and the communication circuit 230. Program data is stored in the memory 220. The processor 210 executes the program data in the memory 220 to implement the steps in the method of any of the above embodiments. For the detailed steps, reference can be made to the above embodiments and will not be elaborated here.

[0069] Among them, the robot 200 can be a robot of any type such as a three-axis robot or a six-axis robot, and there is no limitation here.

[0070] Refer to Figure 6 , Figure 6 It is a schematic structural diagram of an embodiment of the computer-readable storage medium in this application. The computer-readable storage medium 300 stores a computer program 310, and the computer program 310 can be executed by a processor to implement the steps in any of the above methods.

[0071] Among them, the computer-readable storage medium 300 can specifically be a device such as a USB flash drive, a mobile hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a magnetic disk, or an optical disc that can store the computer program 310, or it can also be a server storing the computer program 310. The server can send the stored computer program 310 to other devices for running, or it can also run the stored computer program 310 by itself.

[0072] The above are only the embodiments of this application, and do not limit the patent scope of this application accordingly. Any equivalent structural or equivalent process transformation made by using the content of the specification and drawings of this application, or directly or indirectly applied in other related technical fields, shall be equally included in the patent protection scope of this application.

Claims

1. An opening method, characterized in that, The method includes: Identifying a color image of a target scene to obtain an image of a door and an image of a handle; Determining a first point cloud corresponding to the image of the door and a second point cloud corresponding to the image of the handle according to the depth image of the target scene; Obtaining the state of the door; When the state of the door is a closed state, determining a normal vector parallel to the ground of the door according to at least one of the first point cloud and the second point cloud; Determining the pose of the actuator according to at least the normal vector and the second point cloud; Driving the actuator to grasp the handle according to the pose; While the actuator grasps the handle, driving the actuator to move along a first circular arc trajectory to twist the handle, and driving the actuator to move along a second circular arc trajectory to open the door; Among them, the step of obtaining the state of the door includes: Filtering out the points in the first point cloud whose normal vectors are parallel to the ground; Performing clustering processing on the filtered points by using a region growing algorithm of the normal vector to obtain first sub-point clouds of different categories; Determining the state of the door according to the first sub-point clouds of different categories; Among them, the step of determining the state of the door according to the first sub-point clouds of different categories includes: In response to the number of the first sub-point clouds being one, determining that the state of the door is a closed state; In response to the number of the first sub-point clouds being multiple, sorting the multiple first sub-point clouds in descending order according to the number of points included; If a first included angle between a plane where the first sub-point cloud ranked first is located and a plane where the second sub-point cloud ranked second is located, and a second included angle between the plane where the first sub-point cloud ranked first is located and a plane where the third sub-point cloud ranked third is located are both less than an included angle threshold, then determining that the state of the door is a closed state, otherwise determining that the state of the door is an open state.

2. The method according to claim 1, characterized in that, The step of determining a normal vector parallel to the ground of the door according to at least one of the first point cloud and the second point cloud includes: Determining a door plane according to the first sub-point cloud including the most points, where the door plane is a surface of the door perpendicular to the ground; Determining the normal vector of the door plane as the normal vector parallel to the ground of the door.

3. The method according to claim 2, characterized in that, The step of determining a door plane according to the first sub-point cloud including the most points includes: Performing clustering processing on the first sub-point cloud including the most points by using a region growing algorithm of RGB to obtain second sub-point clouds of different categories; Determining the region defined by the circumscribed rectangular frame of the second sub-point cloud including the most points as the door plane.

4. The method according to claim 2, wherein The handle is an L-shaped handle, the handle includes a connecting section and a handle section, one end of the connecting section is connected to a first surface of the door, the other end of the connecting section is connected to the handle section, and the length direction of the handle section is parallel to the first surface; The step of determining the pose of the actuator according to at least the normal vector and the second point cloud includes: Performing clustering processing on the second point cloud by using a region growing algorithm of RGB to obtain a handle point cloud; Determine the major axis of the handle according to the handle point cloud, where the major axis of the handle is the axis passing through both ends of the handle segment; Determine the pose according to the normal vector and the extension direction of the major axis.

5. The method according to claim 4, wherein The step of driving the actuator to move along a first circular arc trajectory to turn the handle includes: Determine the head and tail of the handle, where the head is the end of the handle segment far from the connection segment, and the tail is the end of the handle segment connected to the connection segment; Determine the first circular arc trajectory according to the head and tail of the handle; Drive the actuator to move along the first circular arc trajectory to turn the handle.

6. The method according to claim 5, wherein The step of determining the head and tail of the handle includes: Determine the two door frames of the door according to the door plane; Determine the door axis according to the two door frames; Determine the head and tail of the handle according to the distances from both ends of the handle segment to the door axis.

7. The method according to claim 4, wherein The step of driving the actuator to move along a second circular arc trajectory to open the door includes: Determine the two door frames of the door according to the door plane; Determine the width of the door according to the distance between the two door frames; Determine the door axis in the two door frames according to the distances between the two door frames and the handle; Determine the second circular arc trajectory according to the width of the door and the door axis; Drive the actuator to move along the second circular arc trajectory to open the door.

8. The method according to any one of claims 1 to 7, characterized in that Plan the door opening method using a behavior tree.

9. A robot, characterized in that, The robot includes a processor, a memory, and a communication circuit. The processor is respectively coupled to the memory and the communication circuit. Program data is stored in the memory. The processor executes the program data in the memory to implement the steps in the method according to any one of claims 1-8.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, and the computer program can be executed by a processor to implement the steps in the method according to any one of claims 1-8.

Images