Active construction method and active construction device of a map and construction system of a map

Abstract

The application provides an active construction method and device of a map and a construction system of the map. The method comprises: acquiring current state information of a robot and an initial three-dimensional map, wherein the current state information comprises an initial joint angle; taking the initial joint angle as a root node, constructing a search tree in a target sampling space, and determining a best node in the search tree, wherein the target sampling space is a robot-reachable space of the robot; determining an optimal path of the robot from the root node to the best node according to the best node; acquiring information of an environment in a process in which the robot moves from the initial joint angle to the best node, obtaining target point cloud information, and updating the initial three-dimensional map according to the target point cloud information. In the process of constructing the search tree, reachable constraints, obstacle avoidance constraints and posture constraints are considered, thereby avoiding the problem that a high-dimensional space constraint planning model needs to be solved in the process of constructing a robot map in the prior art.

Description

Technical Field

[0001] This application relates to the field of three-dimensional spatial map construction, and more specifically, to an active map construction method, an active construction device, a computer-readable storage medium, and a map construction system. Background Technology

[0002] Currently, mobile robot mapping and navigation systems primarily acquire a 2D occupancy grid map in 3D space for obstacle avoidance planning. Multi-functional robots, possessing both mobility and execution capabilities, not only require safe navigation within their environment but also need to perform operations in 3D space, thus necessitating a 3D map. 3D maps contain richer environmental information; therefore, the mapping process for multi-functional robots demands more precise camera observation postures to prevent mapping failure due to lost tracking features.

[0003] In existing technologies, to perform 3D mapping on a composite robot, the next target point to which the robot will move needs to be determined first. During the robot's movement from the current target point to the next, the 3D map is updated until the map of the target area is fully constructed. In practice, since the target point is in Cartesian space while the robot's depth camera is in joint space, it is necessary to inversely transform from Cartesian space to joint space, using the depth camera's motion pose as a constraint. This requires solving a high-dimensional constrained planning problem to determine whether the composite robot can move from the current target point to the next. However, solving high-dimensional constrained planning problems is prone to problems such as time-consuming solutions and low success rates.

[0004] Therefore, a forward solution method is urgently needed to avoid problems in solving constrained planning models in high-dimensional spaces.

[0005] The information disclosed above in the background section is only intended to enhance the understanding of the background art of the art described herein. Therefore, the background art may contain certain information that does not constitute prior art known to those skilled in the art in this country. Summary of the Invention

[0006] The main objective of this application is to provide an active map construction method, an active map construction device, a computer-readable storage medium, and a map construction system, so as to avoid the problem in the prior art that it is necessary to solve a high-dimensional spatially constrained planning model in the process of constructing a robot's map.

[0007] According to one aspect of the present invention, an active map construction method is provided, comprising: acquiring current state information of a robot and an initial three-dimensional map, wherein the current state information includes an initial joint angle, and the initial three-dimensional map includes information about the robot's environment in the current state; constructing a search tree in a target sampling space with the initial joint angle as the root node, and determining the optimal node in the search tree, wherein the target sampling space is a space reachable by the robot; determining an optimal path for the robot to move from the root node to the optimal node based on the optimal node, and controlling the robot to move from the initial joint angle to the optimal node according to the optimal path; acquiring environmental information during the robot's movement from the initial joint angle to the optimal node to obtain target point cloud information, and updating the initial three-dimensional map based on the target point cloud information.

[0008] Optionally, the initial joint angles are the angles of all joints of the robot in the current state; each node in the search tree corresponds to an information gain value, and the optimal node is the node with the largest information gain value in the search tree.

[0009] Optionally, constructing a search tree in the target sampling space with the initial joint angle as the root node includes: obtaining the maximum number of nodes in the search tree; collecting predetermined nodes in the target sampling space with the initial joint angle as the root node, and determining extended nodes based at least on the predetermined nodes, and adding the extended nodes to the current search tree; stopping the construction of the search tree until the total number of nodes in the current search tree reaches the maximum number of nodes.

[0010] Optionally, sampling a predetermined node in the target sampling space includes: determining a sampling interval of the robot and sampling within the sampling interval to obtain a sampling angle, wherein the sampling interval is the interval of joint angles sampled by the robot; obtaining the current joint angle of the robot; calculating the sum of the sampling angle and the current joint angle to obtain the target joint angle of the robot; determining whether the target joint angle exceeds the limit range of the robot's movement; if the target joint angle is within the limit range, determining the target joint angle as the predetermined node; if the target joint angle is not within the limit range, re-determining the sampling angle.

[0011] Optionally, determining the sampling interval of the robot includes: obtaining the maximum movement speed of the robot and obtaining the sampling period of the robot; and determining the upper limit and lower limit of the sampling interval based on the maximum movement speed and the sampling period.

[0012] Optionally, the robot further includes a depth camera, and determines extended nodes based at least on the predetermined node, including: calculating the pose information of the predetermined node in the joint space to obtain the pose information of the predetermined node in Cartesian space; determining whether the pose of the depth camera satisfies the constraints based on the pose information; if the pose of the depth camera satisfies the constraints, determining the nearest neighbor point based on the predetermined node, wherein the nearest neighbor point is the node with the smallest distance to the predetermined node in the current search tree; and determining the extended nodes based on the predetermined node and the nearest neighbor point, using a linear interpolation method.

[0013] Optionally, determining the nearest neighbor based on the predetermined node includes: calculating the Euclidean distance between the predetermined node and each node in the current search tree to obtain multiple target distances; determining the minimum target distance among the multiple target distances; and determining the node in the search tree corresponding to the minimum target distance as the nearest neighbor.

[0014] Optionally, calculating the pose information of the predetermined node in the joint space to obtain the pose information of the predetermined node in the Cartesian space includes: using positive kinematics to perform coordinate transformation on the predetermined node in the joint space to obtain the pose information of the predetermined node in the Cartesian space.

[0015] Optionally, the extended node is determined based on the predetermined node and the nearest neighbor node, using linear interpolation, including: using the formula: V e =V k +λ*(V s -V k ), calculate the extended node, where V e V represents the extended node. k V represents the nearest neighbor point. s The predetermined node is defined as 0 ≤ λ ≤ 1.

[0016] Optionally, after determining the extended node based at least on the predetermined node, before adding the extended node to the current search tree, the method further includes: using the FCL integration library to determine whether the extended node collides with the nearest neighbor; if the extended node does not collide with the nearest neighbor, adding the extended node to the current search tree; and if the extended node collides with the nearest neighbor, controlling the robot to re-collect the predetermined node.

[0017] Optionally, calculating the information gain value of each node in the search tree includes: using Calculate the information gain value of the extended node, where IG(n) k) represents the information gain value of the extended node, IG(n) k-1 ) represents the information gain value of the nearest neighbor. This indicates that the depth camera is currently within the visible region Visible(M,ξ). k Voxel m not observed i The quantity, I(m) i ) is an indicator function. β is the path cost from the nearest neighbor to the extended node in the current search tree, and β is the weighting coefficient between the information gain value of the nearest neighbor and the path cost.

[0018] Optionally, after updating the initial 3D map based on the target point cloud information, the method further includes: determining the root node of the search tree by identifying the best node where the robot is currently located, and sequentially executing the steps of constructing the search tree and determining the best node, determining the optimal path and controlling the robot's movement, and acquiring the target point cloud information and updating the map; until the information gain value of the best node in the reconstructed search tree is less than a predetermined value.

[0019] According to another aspect of the present invention, an active map construction apparatus is also provided, comprising: an acquisition unit, configured to acquire current state information of a robot and an initial three-dimensional map, the current state information including an initial joint angle, and the initial three-dimensional map including information about the robot's environment in the current state; a construction unit, configured to construct a search tree in a target sampling space with the initial joint angle as the root node, and determine the optimal node in the search tree, the target sampling space being a space reachable by the robot; a first determination unit, configured to determine, based on the optimal node, the optimal path for the robot to move from the root node to the optimal node, and control the robot to move from the initial joint angle to the optimal node according to the optimal path; and an update unit, configured to acquire environmental information during the robot's movement from the initial joint angle to the optimal node, obtain target point cloud information, and update the initial three-dimensional map based on the target point cloud information.

[0020] According to another aspect of the present invention, a computer-readable storage medium is also provided, the computer-readable storage medium including a stored program, wherein the program executes any one of the methods described.

[0021] According to one aspect of the present invention, a map building system is also provided, comprising: a robot and an active building device, wherein the active building device is communicatively connected to the robot and is used to execute any of the methods described herein.

[0022] In this embodiment of the invention, the map construction method first obtains the robot's current state information and an initial 3D map. The robot's current state information includes the robot's initial joint angles, and the initial 3D map is the information of the robot's environment in its current state. Then, using the initial joint angles as the root node, sampling is performed in the target sampling space to construct a search tree, and the optimal node of the search tree is determined. After that, based on the optimal node, the optimal path for the robot to move from the root node to the optimal node is determined, and the robot is controlled to move from the initial joint angle to the optimal node according to the optimal path, that is, the robot is controlled to move from the root node to the optimal node according to the optimal path. Finally, the environmental information during the robot's movement from the initial joint angle to the optimal node is obtained to obtain target point cloud information, and the initial 3D map is updated based on the target point cloud information. In this scheme, the robot's initial joint angle is used as the root node. Sampling is performed in the target sampling space to construct a search tree, and the optimal node in the search tree is determined. Then, based on the optimal node, the optimal path for the robot to move is determined. Since the target sampling space is the space reachable by the robot, the robot is guaranteed to reach the optimal node by moving from the root node to the optimal node according to the optimal path. Compared with the existing technology, which determines the reachability of the path from the initial joint angle to the optimal node by inverse solution after determining the optimal node, i.e., compared with the inverse solution of the high-dimensional space constrained planning model from the optimal node to the initial joint angle, this scheme provides a method to determine the optimal node by forward search, without the need to establish a high-dimensional space constrained planning model. This avoids the problems of time-consuming and low success rate in the inverse solution process, and avoids the problem of needing to solve a high-dimensional space constrained planning model in the process of building the robot map in the existing technology. In addition, in this solution, the target point cloud information is obtained during the process of the robot moving to the optimal node from the initial joint angle, and the initial 3D map is updated based on the target point cloud information, which ensures that the initial 3D map can be updated relatively accurately and that the updated map is relatively complete. Attached Figure Description

[0023] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings:

[0024] Figure 1 A flowchart of an active map construction method according to an embodiment of this application is shown;

[0025] Figure 2 A schematic diagram of the structure of an active map building apparatus according to an embodiment of this application is shown;

[0026] Figure 3 A flowchart illustrating an active map construction method according to yet another embodiment of this application is shown;

[0027] Figure 4 A flowchart illustrating the construction of a search tree according to one embodiment of this application is shown;

[0028] Figure 5 A schematic diagram illustrating the determination of an extension node according to one embodiment of this application is shown.

[0029] The above figures include the following reference numerals:

[0030] 100. Current node; 200. Reserved node; 300. Extended node; 400. Nearest neighbor; 500. Target sampling space. Detailed Implementation

[0031] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0032] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present application, and not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative effort should fall within the scope of protection of the present application.

[0033] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this application are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate for the embodiments of this application described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0034] As mentioned in the background section, in the prior art, when constructing a robot map, it is necessary to solve the inverse solution of high-dimensional space. In order to solve the above problem, in a typical embodiment of this application, an active map construction method, an active map construction device, a computer-readable storage medium, and a map construction system are provided.

[0035] According to an embodiment of this application, an active map construction method is provided.

[0036] Figure 1 This is a flowchart of an active map construction method according to an embodiment of this application. Figure 1 As shown, this proactive construction method includes the following steps:

[0037] Step S101: Obtain the robot's current state information and initial 3D map. The current state information includes the initial joint angles, and the initial 3D map includes information about the robot's environment in the current state.

[0038] Step S102: Using the initial joint angle as the root node, construct a search tree in the target sampling space and determine the best node in the search tree. The target sampling space is the space that the robot can reach.

[0039] Step S103: Based on the above-mentioned optimal node, determine the optimal path for the robot to move from the root node to the optimal node, and control the robot to move from the initial joint angle to the optimal node according to the above-mentioned optimal path.

[0040] Step S104: Obtain environmental information during the process of the robot moving from the initial joint angle to the optimal node, obtain target point cloud information, and update the initial 3D map based on the target point cloud information.

[0041] In the above-described map construction method, firstly, the current state information of the robot and an initial 3D map are obtained. The current state information of the robot includes the initial joint angles of the robot, and the initial 3D map is the environmental information of the robot in its current state. Then, the initial joint angles are used as the root node, and sampling is performed in the target sampling space to construct a search tree and determine the optimal node. After that, based on the optimal node, the optimal path for the robot to move from the root node to the optimal node is determined, and the robot is controlled to move from the initial joint angle to the optimal node according to the optimal path, that is, the robot is controlled to move from the root node to the optimal node according to the optimal path. Finally, the environmental information during the process of the robot moving from the initial joint angle to the optimal node is obtained to obtain target point cloud information, and the initial 3D map is updated based on the target point cloud information. In this scheme, the robot's initial joint angle is used as the root node. Sampling is performed in the target sampling space to construct a search tree, and the optimal node in the search tree is determined. Then, based on the optimal node, the optimal path of the robot is determined. Since the target sampling space is the space reachable by the robot, the robot is guaranteed to reach the optimal node by moving from the initial joint angle according to the optimal path. Compared with the existing technology, which determines the reachability of the path from the initial joint node to the optimal node by inverse solution after determining the optimal node, i.e., compared with the inverse solution of the high-dimensional space constrained planning model from the optimal node to the initial joint angle, this scheme provides a method to determine the optimal node by forward search, without the need to establish a high-dimensional space constrained planning model. This avoids the problems of time-consuming and low success rate in the inverse solution process, and avoids the problem of needing to solve a high-dimensional space constrained planning model in the process of building the robot map in the existing technology. In addition, in this solution, the target point cloud information is obtained during the process of the robot moving to the optimal node from the initial joint angle, and the initial 3D map is updated based on the target point cloud information, which ensures that the initial 3D map can be updated relatively accurately and that the updated map is relatively complete.

[0042] Specifically, in practical applications, the search tree described above can be stored using a Kd-tree (K-dimensional Tree) data storage structure, and the constructed map can be a three-dimensional map.

[0043] Specifically, the process of determining the optimal path from the root node to the best node is as follows: After constructing the search tree in the target sampling space, i.e., after constructing the search tree with the maximum number of nodes Nmax, traverse all nodes in the entire search tree, find the node with the largest information gain value, and perform a backtracking operation to obtain the branch from the root node to the best node, which is the optimal path. The specific process of the backtracking operation is as follows: start from the best node, search for the parent node corresponding to the best node, add the parent node to the path, then treat the parent node as a child node, and continue backtracking forward until the root node is encountered, at which point the backtracking stops.

[0044] Specifically, there are two ways for the robot to move from the root node to the optimal node: the first is to move directly from the root node to the optimal node along the optimal path; the second is to execute only the first step or the first few steps on the optimal path each time, and discard the remaining points on the path.

[0045] It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0046] In one embodiment of this application, the initial joint angles are the angles of all joints of the robot in its current state; each node in the search tree corresponds to an information gain value, and the optimal node is the node with the largest information gain value in the search tree. In this embodiment, during the construction of the search tree, each node in the search tree corresponds to an information gain value, and the node with the largest information gain value in the search tree is taken as the optimal node. This further ensures that the optimal path is more easily determined subsequently, further ensures that the target point cloud information obtained subsequently is more complete, and further ensures that a more complete 3D map can be obtained.

[0047] Specifically, the information gain value of each node in the search tree is the cumulative information gain, which is the cumulative information gain from moving from the root node to the current node in the search tree. Based on the cumulative information gain, it can be determined which path in the search tree the robot obtains the most information gain on. Subsequently, the node with the largest information gain value in the search tree is determined as the best node, and the path from the root node to the best node is determined as the optimal path. Since the best node has the largest information gain value, the target point cloud information obtained by the robot from the root node to the best node is more than the target point cloud information on other paths, which further ensures that the map can be updated relatively quickly and completely, and further ensures that the obtained map is more complete and accurate.

[0048] To ensure a relatively fast search tree construction and a strong search capability, in one embodiment of this application, the search tree is constructed in the target sampling space with the initial joint angle as the root node. This includes: obtaining the maximum number of nodes in the search tree; collecting predetermined nodes in the target sampling space with the initial joint angle as the root node, determining expansion nodes based at least on the predetermined nodes, and adding the expansion nodes to the current search tree; and stopping the construction of the search tree when the total number of nodes in the current search tree reaches the maximum number of nodes.

[0049] Specifically, in the process of building the search tree described above, after determining the expansion nodes based on at least the predetermined nodes, these expansion nodes are added to the search tree. The information gain value of each expansion node needs to be calculated; that is, for each expansion node determined (excluding the root node), its information gain value needs to be calculated. After the search tree is built, the information gain value of each node in the entire search tree is also calculated. Then, based on the information gain value corresponding to each node, the node with the largest information gain value is determined as the optimal node. The calculated information gain value of the expansion node is the cumulative effective observations obtained from the branch from the root node to the current expansion node.

[0050] In addition, for ease of calculation, the information gain value of the root node can be set to 0. Of course, the information gain value of the root node can also be non-zero. In this case, the information gain value of the root node is the information gain value obtained by the current node.

[0051] In another embodiment of this application, sampling a predetermined node in the target sampling space includes: determining a sampling interval of the robot and sampling within the sampling interval to obtain a sampling angle, wherein the sampling interval is the interval of joint angles sampled by the robot; obtaining the current joint angle of the robot; calculating the sum of the sampling angle and the current joint angle to obtain the target joint angle of the robot; determining whether the target joint angle exceeds the robot's movable limit interval; if the target joint angle is within the limit interval, determining the target joint angle as the predetermined node; if the target joint angle is not within the limit interval, re-determining the sampling angle. In this embodiment, sampling is performed within the sampling interval to obtain the sampling angle, and the target joint angle is determined based on the sampling angle and the robot's current joint angle. This ensures that the obtained target joint angle is relatively reasonable. If the target joint angle does not exceed the robot's limit interval, the target joint angle is determined as the predetermined node, meaning the determined predetermined node is the robot's movable angle. This avoids safety issues caused by exceeding the limit interval during subsequent robot movement.

[0052] Specifically, in the process of obtaining the sampling angle by sampling within the above-mentioned sampling interval, one sampling angle can be collected at a time, or multiple sampling angles can be collected at a time, which is not limited in this application.

[0053] To ensure that the determined sampling interval is reasonable, in another embodiment of this application, determining the sampling interval of the robot includes: obtaining the maximum movement speed of the robot and obtaining the sampling period of the robot; and determining the upper limit and lower limit of the sampling interval based on the maximum movement speed and the sampling period.

[0054] Specifically, the maximum speed of the robot described above is the speed set at the factory. In actual applications, the robot consists of a chassis and a robot body. Assuming the chassis has two motors and the robot body has six motors, and each motor has a motion vector, the maximum speed is the sum of these eight motion vectors. The sampling period described above is the period during which the robot samples within the sampling interval. After determining the sampling interval, the robot can collect sampling angles within the sampling interval according to the sampling period. The sampling angle is the angle obtained by sampling within the sampling interval.

[0055] In one specific embodiment of this application, the upper limit of the sampling interval is obtained by multiplying the robot's maximum speed by the sampling period, and the negative of the upper limit of the sampling interval is used as the lower limit of the sampling interval. For example, if the robot's maximum speed is Vmax and the sampling period is dt, the sampling interval is [-Vmax×dt, Vmax×dt].

[0056] In another embodiment of this application, the robot further includes a depth camera. Determining extended nodes, at least based on the predetermined nodes, includes: calculating the predetermined nodes in the joint space to obtain the pose information of the predetermined nodes in Cartesian space; determining whether the pose of the depth camera satisfies the constraints based on the pose information; if the pose of the depth camera satisfies the constraints, determining the nearest neighbor based on the predetermined nodes, where the nearest neighbor is the node with the smallest distance to the predetermined nodes in the current search tree; and determining the extended nodes based on the predetermined nodes and the nearest neighbor, using linear interpolation. In this embodiment, the pose information of the predetermined node in the joint space is calculated to obtain the pose information of the predetermined node in the Cartesian space. Based on the pose information, it is determined whether the pose of the depth camera satisfies the constraints. This makes it relatively easy to determine whether the pose of the depth camera satisfies the constraints. That is, in this scheme, the pose of the camera is used as the constraint, which can prevent the loss of features tracked by the depth camera and avoid map construction failure. If the pose of the depth camera satisfies the constraints, the nearest neighbor is determined based on the predetermined node, and then the extended node is determined based on the predetermined node and the nearest neighbor. This ensures that the obtained extended node is more reasonable.

[0057] Specifically, in practical applications, after determining the aforementioned extended nodes, the aforementioned nearest neighbor can be used as the parent node of the extended node, and the extended node can be inserted into the aforementioned search tree.

[0058] Specifically, the constraints on whether the depth camera's pose satisfies can be adjusted according to the actual situation. For example, in a certain scenario, if the depth camera is not required to acquire target point cloud information about the ground (i.e., the depth camera is not perpendicular to the ground), the angle between the depth camera's Z-axis direction and the ground's normal vector can be calculated in Cartesian space. When the normal vector angle is 180°, the depth camera faces the ground; when the normal vector angle is 90°, the depth camera is parallel to the ground. Since it is not desired to acquire target point cloud information about the ground, the normal vector angle should be less than 90°. Furthermore, to ensure that adjacent frames have a certain shared viewing area during mapping, the pose change between adjacent frames cannot be too large, and the depth camera's pose also needs to satisfy certain variation constraints.

[0059] In order to determine the nearest neighbor more accurately, in one embodiment of this application, determining the nearest neighbor based on the predetermined node includes: calculating the Euclidean distance between the predetermined node and each node in the current search tree to obtain multiple target distances; determining the minimum target distance among the multiple target distances; and determining the node in the search tree corresponding to the minimum target distance as the nearest neighbor.

[0060] In one specific embodiment of this application, if the search tree is stored using a Kd Tree (K-dimensional Tree) data storage structure, the nearest neighbor search can also be used to determine the nearest neighbor point. This ensures that the nearest neighbor point can be determined relatively quickly, and that the algorithm complexity for determining the nearest neighbor point is low.

[0061] In another embodiment of this application, the predetermined node in the joint space is calculated to obtain its pose information in Cartesian space. This includes: using forward kinematics to perform coordinate transformation on the predetermined node in the joint space to obtain its pose information in Cartesian space. In this embodiment, forward kinematics is used to calculate the predetermined node in the joint space to obtain its pose information in Cartesian space. This not only avoids the problem of high dimensionality during inverse solving but also further ensures that the pose information can be determined relatively quickly and definitively.

[0062] To further ensure the accuracy of the calculated extended nodes and the good search capability of the subsequently constructed search tree, in another embodiment of this application, the extended nodes are determined based on the predetermined nodes and the nearest neighbors, using linear interpolation, including: using the formula: V e =V k +λ*(V s -V k ), calculate the above extended nodes, where V e V represents the aforementioned extended node. k V represents the nearest neighbor point mentioned above. s This indicates the aforementioned predetermined node, where 0 < λ < 1.

[0063] In another embodiment of this application, after determining the extended node based at least on the predetermined node, before adding the extended node to the current search tree, the method further includes: using the FCL integration library to determine whether a collision occurs during the robot's movement from the nearest neighbor to the extended node; if no collision occurs during the robot's movement from the nearest neighbor to the extended node, adding the extended node to the current search tree; and if a collision occurs during the robot's movement from the nearest neighbor to the extended node, controlling the robot to re-collect the predetermined node. In this embodiment, the FCL integrated library is used to determine whether a collision will occur during the robot's movement from the nearest neighbor to the extended node. This allows for a relatively quick determination of whether a collision will occur during the robot's movement from the nearest neighbor to the extended node. If no collision occurs during the robot's movement from the nearest neighbor to the extended node, the extended node is then added to the search tree. This further ensures that the optimal path for the robot to move from the root node to the best node is reachable. This also further avoids self-collisions or collisions with other environmental objects during the robot's movement, thus further preventing safety issues that may arise during the robot's movement.

[0064] In one embodiment of this application, calculating the information gain value of each node in the search tree includes: using... Calculate the information gain value of the above extended nodes, where IG(n) k ) represents the information gain value of the aforementioned extended node, IG(n) k-1 ) represents the information gain value of the nearest neighbor. This indicates that the depth camera currently operates within the visible region (Visible(M,ξ)). k Voxel m not observed i The quantity, I(m) i ) is an indicator function. β represents the path cost from the nearest neighbor to the extended node in the current search tree, and β is the weighting coefficient between the information gain value of the nearest neighbor and the path cost.

[0065] Specifically, the above I(m) i ) is an indicator function, when voxel m i When not observed, I(m) i When voxel m is 1, i When observed, I(m) i The value is 0.

[0066] To further ensure the completeness of the constructed map, in another embodiment of this application, after updating the initial 3D map based on the target point cloud information, the method further includes: determining the root node of the search tree by identifying the best node where the robot is currently located, and sequentially executing the steps of constructing the search tree and determining the best node, determining the optimal path and controlling the robot's movement, and acquiring the target point cloud information and updating the map; until the information gain value of the best node in the reconstructed search tree is less than a predetermined value.

[0067] This application also provides a map building apparatus. It should be noted that the map building apparatus of this application can be used to execute the map building method provided in this application. The map building apparatus provided in this application will be described below.

[0068] Figure 2 This is a schematic diagram of the structure of an active map construction device according to an embodiment of this application. Figure 2 As shown, the active construction device includes:

[0069] The acquisition unit 10 is used to acquire the robot's current state information and initial three-dimensional map. The current state information includes the initial joint angles, and the initial three-dimensional map includes information about the robot's environment in the current state.

[0070] The construction unit 20 is used to construct a search tree in the target sampling space with the initial joint angle as the root node, and to determine the best node in the search tree. The target sampling space is the space that the robot can reach.

[0071] The first determining unit 30 is used to determine the optimal path for the robot to move from the root node to the optimal node based on the optimal node, and to control the robot to move from the initial joint angle to the optimal node according to the optimal path.

[0072] The update unit 40 is used to acquire environmental information during the process of the robot moving from the initial joint angle to the optimal node, obtain target point cloud information, and update the initial three-dimensional map based on the target point cloud information.

[0073] In the aforementioned active map construction device, the acquisition unit is used to acquire the robot's current state information and an initial 3D map. The current state information includes the initial joint angle, and the initial 3D map includes information about the robot's environment in the current state. The construction unit is used to construct a search tree in the target sampling space with the initial joint angle as the root node, and determine the optimal node in the search tree. The target sampling space is the space that the robot can reach. The first determination unit is used to determine the optimal path for the robot to move from the root node to the optimal node based on the optimal node, and control the robot to move from the initial joint angle to the optimal node according to the optimal path. The update unit is used to acquire environmental information during the robot's movement from the initial joint angle to the optimal node, obtain target point cloud information, and update the initial 3D map based on the target point cloud information. In this scheme, the robot's initial joint angle is used as the root node. Sampling is performed in the target sampling space to construct a search tree, and the optimal node in the search tree is determined. Then, based on the optimal node, the optimal path of the robot is determined. Since the target sampling space is the space reachable by the robot, the robot is guaranteed to reach the optimal node by moving from the initial joint angle according to the optimal path. Compared with the existing technology, which determines the reachability of the path from the initial joint node to the optimal node by inverse solution after determining the optimal node, i.e., compared with the inverse solution of the high-dimensional space constrained planning model from the optimal node to the initial joint angle, this scheme provides a method to determine the optimal node by forward search, without the need to establish a high-dimensional space constrained planning model. This avoids the problems of time-consuming and low success rate in the inverse solution process, and avoids the problem of needing to solve a high-dimensional space constrained planning model in the process of building the robot map in the existing technology. In addition, in this solution, the target point cloud information is obtained during the process of the robot moving to the optimal node from the initial joint angle, and the initial 3D map is updated based on the target point cloud information, which ensures that the initial 3D map can be updated relatively accurately and that the updated map is relatively complete.

[0074] Specifically, in practical applications, the search tree described above can be stored using a Kd Tree (K-dimensional Tree) data storage structure, and the constructed map can be a three-dimensional map.

[0075] Specifically, the process of determining the optimal path from the root node to the best node is as follows: After constructing the search tree in the target sampling space, i.e., after constructing the search tree with the maximum number of nodes Nmax, traverse all nodes in the entire search tree, find the node with the largest information gain value, and perform a backtracking operation to obtain the branch from the root node to the best node, which is the optimal path. The specific process of the backtracking operation is as follows: start from the best node, search for the parent node corresponding to the best node, add the parent node to the path, then treat the parent node as a child node, and continue backtracking forward until the root node is encountered, at which point the backtracking stops.

[0076] Specifically, there are two ways for the robot to move from the root node to the optimal node: the first is to move directly from the root node to the optimal node along the optimal path; the second is to execute only the first step or the first few steps on the optimal path each time, and discard the remaining points on the path.

[0077] In one embodiment of this application, the initial joint angles are the angles of all joints of the robot in its current state; each node in the search tree corresponds to an information gain value, and the optimal node is the node with the largest information gain value in the search tree. In this embodiment, during the construction of the search tree, each node in the search tree corresponds to an information gain value, and the node with the largest information gain value in the search tree is taken as the optimal node. This further ensures that the optimal path is more easily determined subsequently, further ensures that the target point cloud information obtained subsequently is more complete, and further ensures that a more complete 3D map can be obtained.

[0078] Specifically, the information gain value of each node in the search tree is the cumulative information gain, which is the cumulative information gain from moving from the root node to the current node in the search tree. Based on the cumulative information gain, it can be determined which path in the search tree the robot obtains the most information gain on. Subsequently, the node with the largest information gain value in the search tree is determined as the best node, and the path from the root node to the best node is determined as the optimal path. Since the best node has the largest information gain value, the target point cloud information obtained by the robot from the root node to the best node is more than the target point cloud information on other paths, which further ensures that the map can be updated relatively quickly and completely, and further ensures that the obtained map is more complete and accurate.

[0079] To ensure rapid establishment of the search tree and strong search capability, in one embodiment of this application, the construction unit includes an acquisition module, a collection module, and a construction module. The acquisition module acquires the maximum number of nodes in the search tree. The collection module collects predetermined nodes in the target sampling space, using the initial joint angle as the root node, and determines extended nodes based at least on the predetermined nodes, adding the extended nodes to the current search tree. The construction module stops building the search tree when the total number of nodes in the current search tree reaches the maximum number of nodes.

[0080] Specifically, in the process of building the search tree described above, after determining the expansion nodes based on at least the predetermined nodes, these expansion nodes are added to the search tree. The information gain value of each expansion node needs to be calculated; that is, for each expansion node determined (excluding the root node), its information gain value needs to be calculated. After the search tree is built, the information gain value of each node in the entire search tree is also calculated. Then, based on the information gain value corresponding to each node, the node with the largest information gain value is determined as the optimal node. The calculated information gain value of the expansion node is the cumulative effective observations obtained from the branch from the root node to the current expansion node.

[0081] In addition, for ease of calculation, the information gain value of the root node can be set to 0. Of course, the information gain value of the root node can also be non-zero. In this case, the information gain value of the root node is the information gain value obtained by the current node.

[0082] In another embodiment of this application, the acquisition module includes a first determining submodule, a first acquiring submodule, a first calculating submodule, and a second determining submodule. The first determining submodule is used to determine the sampling interval of the robot and perform sampling within the sampling interval to obtain a sampling angle. The sampling interval is the interval of joint angles sampled by the robot. The first acquiring submodule is used to acquire the current joint angle of the robot. The first calculating submodule is used to calculate the sum of the sampling angle and the current joint angle to obtain the target joint angle of the robot. The second determining submodule is used to determine whether the target joint angle exceeds the robot's movable limit interval. If the target joint angle is within the limit interval, the target joint angle is determined as the predetermined node. If the target joint angle is not within the limit interval, the sampling angle is re-determined. In this embodiment, sampling angles are obtained by sampling within the sampling interval. Based on the sampling angles and the robot's current joint angles, the target joint angle is determined. This ensures that the obtained target joint angle is reasonable. If the target joint angle does not exceed the robot's limit range, the target joint angle is determined as a predetermined node. That is, the determined predetermined node is the angle at which the robot can move. This avoids safety issues for the robot caused by exceeding the limit range during subsequent robot movement.

[0083] Specifically, during the sampling process within the aforementioned sampling interval, one sampling angle can be collected at a time, or multiple sampling angles can be collected at a time; this application does not impose any restrictions on this.

[0084] To ensure that the determined sampling interval is reasonable, in another embodiment of this application, the first determining submodule includes a second acquiring submodule and a third determining submodule. The second acquiring submodule is used to acquire the maximum movement speed of the robot and the sampling period of the robot. The third determining submodule is used to determine the upper limit and lower limit of the sampling interval based on the maximum movement speed and the sampling period.

[0085] Specifically, the maximum speed of the robot described above is the speed set at the factory. In actual applications, the robot consists of a chassis and a robot body. Assuming the chassis has two motors and the robot body has six motors, and each motor has a motion vector, the maximum speed is the sum of these eight motion vectors. The sampling period described above is the period during which the robot samples within the sampling interval. After determining the sampling interval, the robot can collect sampling angles within the sampling interval according to the sampling period. The sampling angle is the angle obtained by sampling within the sampling interval.

[0086] In one specific embodiment of this application, the upper limit of the sampling interval is obtained by multiplying the robot's maximum speed by the sampling period, and the negative of the upper limit of the sampling interval is used as the lower limit of the sampling interval. For example, if the robot's maximum speed is Vmax and the sampling period is dt, the sampling interval is [-Vmax×dt, Vmax×dt].

[0087] In another embodiment of this application, the robot further includes a depth camera, and the acquisition module further includes a first transformation submodule, a fourth determination submodule, a fifth determination submodule, and a sixth determination submodule. The first transformation submodule is used to calculate the pose information of the predetermined node in the joint space to obtain the pose information of the predetermined node in Cartesian space. The fourth determination submodule is used to determine whether the pose of the depth camera satisfies the constraints based on the pose information. The fifth determination submodule is used to determine the nearest neighbor point based on the predetermined node when the pose of the depth camera satisfies the constraints; the nearest neighbor point is the node with the smallest distance to the predetermined node in the current search tree. The sixth determination submodule is used to determine the extended node based on the predetermined node and the nearest neighbor point, using linear interpolation. In this embodiment, the pose information of the predetermined node in the joint space is calculated to obtain the pose information of the predetermined node in the Cartesian space. Based on the pose information, it is determined whether the pose of the depth camera satisfies the constraint conditions. This makes it relatively easy to determine whether the pose of the depth camera satisfies the constraint conditions. That is, in this scheme, the pose of the camera is used as the constraint condition, which can prevent the loss of features tracked by the depth camera and avoid map construction failure. If the pose of the depth camera satisfies the constraint conditions, the nearest neighbor is determined based on the predetermined node, and then the extended node is determined based on the predetermined node and the nearest neighbor. This ensures that the obtained extended node is more reasonable.

[0088] Specifically, in practical applications, after determining the aforementioned extended nodes, the aforementioned nearest neighbor can be used as the parent node of the extended node, and the extended node can be inserted into the aforementioned search tree.

[0089] Specifically, the constraints on whether the depth camera's pose satisfies can be adjusted according to the actual situation. For example, in a certain scenario, if the depth camera is not required to acquire target point cloud information about the ground (i.e., the depth camera is not perpendicular to the ground), the angle between the depth camera's Z-axis direction and the ground's normal vector can be calculated in Cartesian space. When the normal vector angle is 180°, the depth camera faces the ground; when the normal vector angle is 90°, the depth camera is parallel to the ground. Since it is not desired to acquire target point cloud information about the ground, the normal vector angle should be less than 90°. Furthermore, to ensure that adjacent frames have a certain shared viewing area during mapping, the pose change between adjacent frames cannot be too large, and the depth camera's pose also needs to satisfy certain variation constraints.

[0090] To more accurately determine the nearest neighbor, in one embodiment of this application, the fifth determining submodule includes a second calculation submodule, a seventh determining submodule, and an eighth determining submodule. The second calculation submodule is used to calculate the Euclidean distance between the predetermined node and each node in the current search tree to obtain multiple target distances. The seventh determining submodule is used to determine the minimum target distance among the multiple target distances. The eighth determining submodule is used to determine the node in the search tree corresponding to the minimum target distance as the nearest neighbor.

[0091] In one specific embodiment of this application, if the search tree is stored using a Kd Tree (K-dimensional Tree) data storage structure, the nearest neighbor search can also be used to determine the nearest neighbor point. This ensures that the nearest neighbor point can be determined relatively quickly, and that the algorithm complexity for determining the nearest neighbor point is low.

[0092] In another embodiment of this application, the first transformation submodule includes a second transformation submodule, used to calculate the predetermined node in the joint space to obtain the pose information of the predetermined node in Cartesian space. In this embodiment, forward kinematics is used to calculate the predetermined node in the joint space to obtain the pose information of the predetermined node in Cartesian space. This not only avoids the problem of high dimensionality during inverse solving, but also further ensures that the pose information can be determined relatively quickly and definitively.

[0093] To further ensure the accuracy of the calculated extended nodes and the good search capability of the subsequently constructed search tree, in another embodiment of this application, the sixth determining submodule further includes a third calculation submodule, used to employ the formula: V e =V k +λ*(V s-V k ), calculate the above extended nodes, where V e V represents the aforementioned extended node. k V represents the nearest neighbor point mentioned above. s This indicates the aforementioned predetermined node, where 0 < λ < 1.

[0094] In another embodiment of this application, the apparatus further includes a second determining unit, configured to, after determining the extended node based at least on the predetermined node, and before adding the extended node to the current search tree, use the FCL integration library to determine whether a collision occurs during the robot's movement from the nearest neighbor to the extended node; if no collision occurs during the robot's movement from the nearest neighbor to the extended node, add the extended node to the current search tree; and if a collision occurs during the robot's movement from the nearest neighbor to the extended node, control the robot to re-collect the predetermined node. In this embodiment, the FCL integrated library is used to determine whether a collision will occur during the robot's movement from the nearest neighbor to the extended node. This allows for a relatively quick determination of whether a collision will occur during the robot's movement from the nearest neighbor to the extended node. If no collision occurs during the robot's movement from the nearest neighbor to the extended node, the extended node is then added to the search tree. This further ensures that the optimal path for the robot to move from the root node to the best node is reachable. This also further avoids self-collisions or collisions with other environmental objects during the robot's movement, thus further preventing safety issues that may arise during the robot's movement.

[0095] In one embodiment of this application, calculating the information gain value of each node in the search tree includes: using... Calculate the information gain value of the above extended nodes, where IG(n) k ) represents the information gain value of the aforementioned extended node, IG(n) k-1 ) represents the information gain value of the nearest neighbor. This indicates that the depth camera currently operates within the visible region (Visible(M,ξ)). k Voxel m not observed i The quantity, I(m) i ) is an indicator function. β represents the path cost from the nearest neighbor to the extended node in the current search tree, and β is the weighting coefficient between the information gain value of the nearest neighbor and the path cost.

[0096] Specifically, the above I(m) i ) is an indicator function, when voxel mi When not observed, I(m) i When voxel m is 1, i When observed, I(m) i The value is 0.

[0097] To further ensure the constructed map is relatively complete, in another embodiment of this application, the above-mentioned active construction device further includes an execution unit and a third determining unit. The execution unit is used to determine the root node of the search tree by the best node where the robot is currently located after updating the initial three-dimensional map based on the target point cloud information, and sequentially executes the following steps: constructing the search tree and determining the best node, determining the optimal path and controlling the robot's movement, and obtaining the target point cloud information and updating the map. The third determining unit is used until the information gain value of the best node in the reconstructed search tree is less than a predetermined value.

[0098] To enable those skilled in the art to clearly understand the technical solution of this application, a detailed description will be provided below with reference to specific embodiments:

[0099] Example 1

[0100] like Figure 3 The diagram shows a flowchart of a map construction method according to an embodiment of this application. First, the current state information of the robot and the initial 3D map of the robot in the current state are obtained. The current state information includes the initial joint angle of the robot in the current state. Then, a search tree is constructed in the target sampling space with the initial joint angle of the robot in the current state as the root node. Then, the node with the largest information gain value in the constructed search tree is taken as the optimal node. The optimal path is determined according to the initial joint angle and the optimal node. Then, the robot is controlled to move from the initial joint angle to the optimal node according to the optimal path. Then, the robot is controlled to obtain the target point cloud information during the process of moving from the initial joint angle to the optimal node. Then, the initial 3D map is updated according to the target point cloud information. Finally, the optimal node where the robot is located is taken as the initial joint angle, and the updated initial 3D map is taken as the initial 3D map corresponding to the optimal node. The search tree is constructed again with the initial joint angle as the root node. It is determined whether the information gain value of the optimal node in the constructed search tree is less than a predetermined value. If the information gain value of the optimal node is less than the predetermined value, the map construction is stopped. Otherwise, the above steps are continued to update the initial 3D map.

[0101] Example 2

[0102] like Figure 4The diagram illustrates the specific process of constructing the search tree. First, the maximum number of nodes, Nmax, is obtained. Then, predetermined nodes are collected in the target sampling space, and their pose information in Cartesian space is calculated using forward kinematics. Based on this pose information, it is determined whether the robot's depth camera pose satisfies the constraints. If the depth camera pose satisfies the constraints, the nearest neighbor is determined. Based on the nearest neighbor and the predetermined nodes, expanded nodes are determined. If the depth camera pose does not satisfy the constraints, the predetermined nodes are re-collected. Then, it is determined whether a collision occurs between the nearest neighbor and the expanded nodes. If the nearest neighbor does not collide with the expanded nodes, the nearest neighbor is used as the basis for selection. The node is used as the parent node of the expanded node. The expanded node is added to the current search tree, and the information gain value of the expanded node is calculated (the information gain value of the expanded node is calculated in this step so that the best node can be determined after the entire search tree is built). If the nearest neighbor node collides with the expanded node, the predetermined node is re-collected. Then, it is determined whether the total number of nodes in the current search tree has reached the maximum number of nodes Nmax. If the total number of nodes in the current search tree has reached the maximum number of nodes Nmax, the construction of the search tree is stopped. If the total number of nodes in the current search tree has not reached the maximum number of nodes Nmax, the predetermined node is collected in the target sampling space.

[0103] Example 3

[0104] like Figure 5 As shown, in a search tree, the current node 100 is the node already established in the current search tree. The robot collects the predetermined node 200 in the target sampling space 500. After collecting the predetermined node 200, it uses forward kinematics to calculate the predetermined node 200 and obtain the pose information of the predetermined node 200 in the Cartesian space. Based on the pose information, it determines whether the pose of the depth camera meets the constraints. If the pose of the depth camera meets the constraints, it determines the nearest neighbor 400 of the predetermined node 200. Based on the nearest neighbor 400 and the predetermined node 200, it determines the extended node 300. If the extended node 300 and the nearest neighbor 400 do not collide, the nearest neighbor 400 is used as the parent node of the extended node 300, and the extended node 300 is added to the search tree.

[0105] The aforementioned map construction apparatus includes a processor and a memory. The aforementioned acquisition unit, construction unit, first determination unit, and update unit are all stored in the memory as program units, and the processor executes the aforementioned program units stored in the memory to realize the corresponding functions.

[0106] The processor contains a kernel, which retrieves the corresponding program units from memory. One or more kernels can be configured, and by adjusting kernel parameters, the problem of solving a high-dimensional, constrained planning model can be avoided in the process of building a robot's map, as is present in existing technologies.

[0107] The memory may include non-permanent memory in computer-readable media, such as random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM, and the memory includes at least one memory chip.

[0108] This invention provides a computer-readable storage medium storing a program that, when executed by a processor, implements the map construction method described above.

[0109] This invention provides a processor for running a program, wherein the program executes the map construction method.

[0110] In a typical embodiment of this application, a map building system is also provided, which includes a robot and an active building device, wherein the active building device is communicatively connected to the robot and is used to execute any of the above-described active building methods.

[0111] In the aforementioned map construction system, the system includes a robot and an active construction device. The active construction device is communicatively connected to the robot and can execute the aforementioned active construction method. In this method, firstly, the robot's current state information and an initial 3D map are acquired. The robot's current state information includes its initial joint angles, and the initial 3D map represents the robot's environment information in its current state. Then, the initial joint angles are used as the root node, and sampling is performed in the target sampling space to construct a search tree and determine the optimal node. Next, based on the optimal node, the optimal path for the robot to move from the root node to the optimal node is determined, and the robot is controlled to move from the initial joint angle to the optimal node along the optimal path. Finally, environmental information during the robot's movement from the initial joint angle to the optimal node is acquired to obtain target point cloud information, and the initial 3D map is updated based on this target point cloud information. In this scheme, the robot's initial joint angle is used as the root node. Sampling is performed in the target sampling space to construct a search tree, and the optimal node in the search tree is determined. Then, based on the optimal node, the optimal path of the robot is determined. Since the target sampling space is the space reachable by the robot, the robot is guaranteed to reach the optimal node by moving from the initial joint angle according to the optimal path. Compared with the existing technology, which determines the reachability of the path from the initial joint node to the optimal node by inverse solution after determining the optimal node, i.e., compared with the inverse solution of the high-dimensional space constrained planning model from the optimal node to the initial joint angle, this scheme provides a method to determine the optimal node by forward search, without the need to establish a high-dimensional space constrained planning model. This avoids the problems of time-consuming and low success rate in the inverse solution process, and avoids the problem of needing to solve a high-dimensional space constrained planning model in the process of building the robot map in the existing technology. In addition, in this solution, the target point cloud information is obtained during the process of the robot moving to the optimal node from the initial joint angle, and the initial 3D map is updated based on the target point cloud information, which ensures that the initial 3D map can be updated relatively accurately and that the updated map is relatively complete.

[0112] This invention provides a device including a processor, a memory, and a program stored in the memory and executable on the processor. When the processor executes the program, it performs at least the following steps:

[0113] Step S101: Obtain the robot's current state information and initial 3D map. The current state information includes the initial joint angles, and the initial 3D map includes information about the robot's environment in the current state.

[0114] Step S102: Using the initial joint angle as the root node, construct a search tree in the target sampling space and determine the best node in the search tree. The target sampling space is the space that the robot can reach.

[0115] Step S103: Based on the above-mentioned optimal node, determine the optimal path for the robot to move from the root node to the optimal node, and control the robot to move from the initial joint angle to the optimal node according to the above-mentioned optimal path.

[0116] Step S104: Obtain environmental information during the process of the robot moving from the initial joint angle to the optimal node, obtain target point cloud information, and update the initial 3D map based on the target point cloud information.

[0117] The devices mentioned in this article can be servers, PCs, tablets, mobile phones, etc.

[0118] This application also provides a computer program product, which, when executed on a data processing device, is suitable for executing an initialization program having at least the following method steps:

[0119] Step S101: Obtain the robot's current state information and initial 3D map. The current state information includes the initial joint angles, and the initial 3D map includes information about the robot's environment in the current state.

[0120] Step S102: Using the initial joint angle as the root node, construct a search tree in the target sampling space and determine the best node in the search tree. The target sampling space is the space that the robot can reach.

[0121] Step S103: Based on the above-mentioned optimal node, determine the optimal path for the robot to move from the root node to the optimal node, and control the robot to move from the initial joint angle to the optimal node according to the above-mentioned optimal path.

[0122] Step S104: Obtain environmental information during the process of the robot moving from the initial joint angle to the optimal node, obtain target point cloud information, and update the initial 3D map based on the target point cloud information.

[0123] In the above embodiments of the present invention, the descriptions of each embodiment have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions of other embodiments.

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

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

[0126] Furthermore, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

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

[0128] As can be seen from the above description, the embodiments of this application achieve the following technical effects:

[0129] 1) In the map construction method of this application, firstly, the current state information of the robot and an initial 3D map are obtained. The current state information of the robot includes the initial joint angle of the robot, and the initial 3D map is the environmental information of the robot in the current state. Then, the initial joint angle is used as the root node, and sampling is performed in the target sampling space to construct a search tree and determine the best node. After that, based on the best node, the optimal path for the robot to move from the root node to the best node is determined, and the robot is controlled to move from the initial joint angle to the best node according to the optimal path, that is, the robot is controlled to move from the root node to the best node according to the optimal path. Finally, the environmental information during the process of the robot moving from the initial joint angle to the best node is obtained to obtain the target point cloud information, and the initial 3D map is updated based on the target point cloud information. In this scheme, the robot's initial joint angle is used as the root node. Sampling is performed in the target sampling space to construct a search tree, and the optimal node in the search tree is determined. Then, based on the optimal node, the optimal path of the robot is determined. Since the target sampling space is the space reachable by the robot, the robot is guaranteed to reach the optimal node by moving from the initial joint angle according to the optimal path. Compared with the existing technology, which determines the reachability of the path from the initial joint node to the optimal node by inverse solution after determining the optimal node, i.e., compared with the inverse solution of the high-dimensional space constrained planning model from the optimal node to the initial joint angle, this scheme provides a method to determine the optimal node by forward search, without the need to establish a high-dimensional space constrained planning model. This avoids the problems of time-consuming and low success rate in the inverse solution process, and avoids the problem of needing to solve a high-dimensional space constrained planning model in the process of building the robot map in the existing technology. In addition, in this solution, the target point cloud information is obtained during the process of the robot moving to the optimal node from the initial joint angle, and the initial 3D map is updated based on the target point cloud information, which ensures that the initial 3D map can be updated relatively accurately and that the updated map is relatively complete.

[0130] 2) In the active map construction device of this application, the acquisition unit is used to acquire the current state information of the robot and an initial three-dimensional map. The current state information includes the initial joint angle, and the initial three-dimensional map includes the environmental information of the robot in the current state. The construction unit is used to construct a search tree in the target sampling space with the initial joint angle as the root node, and determine the best node in the search tree. The target sampling space is the space that the robot can reach. The first determination unit is used to determine the optimal path for the robot to move from the root node to the optimal node according to the optimal node, and control the robot to move from the initial joint angle to the optimal node according to the optimal path. The update unit is used to acquire the environmental information of the robot during the process of moving from the initial joint angle to the optimal node, obtain target point cloud information, and update the initial three-dimensional map according to the target point cloud information. In this scheme, the robot's initial joint angle is used as the root node. Sampling is performed in the target sampling space to construct a search tree, and the optimal node in the search tree is determined. Then, based on the optimal node, the optimal path of the robot is determined. Since the target sampling space is the space reachable by the robot, the robot is guaranteed to reach the optimal node by moving from the initial joint angle according to the optimal path. Compared with the existing technology, which determines the reachability of the path from the initial joint node to the optimal node by inverse solution after determining the optimal node, i.e., compared with the inverse solution of the high-dimensional space constrained planning model from the optimal node to the initial joint angle, this scheme provides a method to determine the optimal node by forward search, without the need to establish a high-dimensional space constrained planning model. This avoids the problems of time-consuming and low success rate in the inverse solution process, and avoids the problem of needing to solve a high-dimensional space constrained planning model in the process of building the robot map in the existing technology. In addition, in this solution, the target point cloud information is obtained during the process of the robot moving to the optimal node from the initial joint angle, and the initial 3D map is updated based on the target point cloud information, which ensures that the initial 3D map can be updated relatively accurately and that the updated map is relatively complete.

[0131] 3) In the map construction system of this application, the construction system includes a robot and an active construction device. The active construction device is communicatively connected to the robot and can execute the active construction method. In the active construction method, firstly, the current state information of the robot and an initial 3D map are obtained. The current state information of the robot includes the initial joint angle of the robot, and the initial 3D map is the environmental information of the robot in the current state. Then, the initial joint angle is used as the root node, and sampling is performed in the target sampling space to construct a search tree and determine the optimal node. After that, based on the optimal node, the optimal path for the robot to move from the root node to the optimal node is determined, and the robot is controlled to move from the initial joint angle to the optimal node according to the optimal path, that is, the robot is controlled to move from the root node to the optimal node according to the optimal path. Finally, the environmental information during the process of the robot moving from the initial joint angle to the optimal node is obtained to obtain target point cloud information, and the initial 3D map is updated based on the target point cloud information. In this scheme, the robot's initial joint angle is used as the root node. Sampling is performed in the target sampling space to construct a search tree, and the optimal node in the search tree is determined. Then, based on the optimal node, the optimal path of the robot is determined. Since the target sampling space is the space reachable by the robot, the robot is guaranteed to reach the optimal node by moving from the initial joint angle according to the optimal path. Compared with the existing technology, which determines the reachability of the path from the initial joint node to the optimal node by inverse solution after determining the optimal node, i.e., compared with the inverse solution of the high-dimensional space constrained planning model from the optimal node to the initial joint angle, this scheme provides a method to determine the optimal node by forward search, without the need to establish a high-dimensional space constrained planning model. This avoids the problems of time-consuming and low success rate in the inverse solution process, and avoids the problem of needing to solve a high-dimensional space constrained planning model in the process of building the robot map in the existing technology. In addition, in this solution, the target point cloud information is obtained during the process of the robot moving to the optimal node from the initial joint angle, and the initial 3D map is updated based on the target point cloud information, which ensures that the initial 3D map can be updated relatively accurately and that the updated map is relatively complete.

[0132] The above description is merely a preferred embodiment of this application and is not intended to limit this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A method for actively constructing a map, characterized in that, include: The robot's current state information and initial 3D map are obtained. The current state information includes initial joint angles, and the initial 3D map includes information about the robot's environment in the current state. Using the initial joint angle as the root node, a search tree is constructed in the target sampling space, and the best node in the search tree is determined. The target sampling space is the space that the robot can reach. Based on the optimal node, determine the optimal path for the robot to move from the root node to the optimal node, and control the robot to move from the initial joint angle to the optimal node according to the optimal path; Information about the environment during the process of the robot moving from the initial joint angle to the optimal node is obtained to obtain target point cloud information. The initial 3D map is then updated based on the target point cloud information. Using the initial joint angle as the root node, a search tree is constructed in the target sampling space, including: obtaining the maximum number of nodes in the search tree; using the initial joint angle as the root node, sampling predetermined nodes in the target sampling space, and determining extended nodes based at least on the predetermined nodes, and adding the extended nodes to the current search tree; until the total number of nodes in the current search tree reaches the maximum number of nodes, the construction of the search tree is stopped. The robot also includes a depth camera. At least based on the predetermined node, the robot determines an extended node, including: calculating the pose information of the predetermined node in the joint space to obtain the pose information of the predetermined node in Cartesian space; determining whether the pose of the depth camera satisfies the constraints based on the pose information; if the pose of the depth camera satisfies the constraints, determining the nearest neighbor point based on the predetermined node, where the nearest neighbor point is the node with the smallest distance to the predetermined node in the current search tree; and determining the extended node based on the predetermined node and the nearest neighbor point, using linear interpolation.

2. The active construction method according to claim 1, characterized in that, The initial joint angles are the angles of all joints of the robot in the current state; Each node in the search tree corresponds to an information gain value, and the optimal node is the node in the search tree with the largest information gain value.

3. The active construction method according to claim 1, characterized in that, In the target sampling space, sampling predetermined nodes includes: The sampling interval of the robot is determined, and sampling is performed within the sampling interval to obtain the sampling angle. The sampling interval is the range of joint angles sampled by the robot. Obtain the current joint angles of the robot; The target joint angle of the robot is obtained by calculating the sum of the sampled angle and the current joint angle. Determine whether the target joint angle exceeds the limit range of the robot's movement. If the target joint angle is within the limit range, determine the target joint angle as the predetermined node. If the target joint angle is not within the limit range, redetermine the sampling angle.

4. The active construction method according to claim 3, characterized in that, Determining the sampling range of the robot includes: Obtain the robot's maximum speed and the robot's sampling period; The upper and lower limits of the sampling interval are determined based on the maximum motion speed and the sampling period.

5. The active construction method according to claim 1, characterized in that, Determining the nearest neighbor based on the predetermined node includes: Calculate the Euclidean distance between the predetermined node and each node in the current search tree to obtain multiple target distances; Among the multiple target distances, determine the minimum target distance; The node in the search tree corresponding to the minimum target distance is determined as the nearest neighbor.

6. The active construction method according to claim 1, characterized in that, The pose information of the predetermined node in Cartesian space is calculated by performing calculations on the predetermined node in joint space, including: Using positive kinematics, the predetermined node in the joint space is calculated to obtain the pose information of the predetermined node in Cartesian space.

7. The active construction method according to claim 1, characterized in that, The extended node is determined based on the predetermined node and the nearest neighbor node, using linear interpolation, including: Formula used: Calculate the extended node, where, This refers to the extended node. Indicates the nearest neighbor point, This refers to the predetermined node. .

8. The active construction method according to claim 1, characterized in that, After determining the expansion node based at least on the predetermined node, and before adding the expansion node to the current search tree, the active construction method further includes: Using the FCL integrated library, it is determined whether a collision occurs during the robot's movement from the nearest neighbor to the extended node. If no collision occurs during the robot's movement from the nearest neighbor to the extended node, the extended node is added to the current search tree. If a collision occurs during the robot's movement from the nearest neighbor to the extended node, the robot is controlled to re-collect the predetermined node.

9. The active construction method according to any one of claims 1 to 8, characterized in that, After updating the initial 3D map based on the target point cloud information, the active construction method further includes: The optimal node where the robot is currently located is determined as the root node of the search tree, and the following steps are executed in sequence: constructing the search tree and determining the optimal node, determining the optimal path and controlling the robot's movement, and obtaining the target point cloud information and updating the map. This continues until the information gain value of the best node in the reconstructed search tree is less than a predetermined value.

10. An active map-building device, characterized in that, include: The acquisition unit is used to acquire the robot's current state information and initial 3D map. The current state information includes initial joint angles, and the initial 3D map includes information about the robot's environment in the current state. A construction unit is used to construct a search tree in the target sampling space with the initial joint angle as the root node, and to determine the best node in the search tree, wherein the target sampling space is the space that the robot can reach; The first determining unit is configured to determine the optimal path for the robot to move from the root node to the optimal node based on the optimal node, and control the robot to move from the initial joint angle to the optimal node according to the optimal path; The update unit is used to acquire environmental information during the process of the robot moving from the initial joint angle to the optimal node, obtain target point cloud information, and update the initial three-dimensional map based on the target point cloud information. Using the initial joint angle as the root node, a search tree is constructed in the target sampling space, including: obtaining the maximum number of nodes in the search tree; using the initial joint angle as the root node, sampling predetermined nodes in the target sampling space, and determining extended nodes based at least on the predetermined nodes, and adding the extended nodes to the current search tree; until the total number of nodes in the current search tree reaches the maximum number of nodes, the construction of the search tree is stopped. The robot also includes a depth camera. At least based on the predetermined node, the robot determines an extended node, including: calculating the pose information of the predetermined node in the joint space to obtain the pose information of the predetermined node in Cartesian space; determining whether the pose of the depth camera satisfies the constraints based on the pose information; if the pose of the depth camera satisfies the constraints, determining the nearest neighbor point based on the predetermined node, where the nearest neighbor point is the node with the smallest distance to the predetermined node in the current search tree; and determining the extended node based on the predetermined node and the nearest neighbor point, using linear interpolation.

11. A computer-readable storage medium, characterized in that, The computer-readable storage medium includes a stored program, wherein the program executes the active construction method according to any one of claims 1 to 9.

12. A map construction system, characterized in that, include: A robot and an active construction device, wherein the active construction device is communicatively connected to the robot and is used to execute the active construction method according to any one of claims 1 to 9.

Images