A robot arm active mapping method and device
By generating boundary voxel maps using visual sensors and combining them with the RRT-Connect algorithm, the problem of decoupling maps and paths in unknown environments for robotic arms was solved, enabling safe and efficient task execution.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- NANKAI UNIV
- Filing Date
- 2023-06-16
- Publication Date
- 2026-05-19
AI Technical Summary
In existing technologies, the mapping and path planning of robotic arms in unknown and complex environments are decoupled, leading to potential collision risks and making it impossible to complete tasks safely and efficiently.
The environment map is scanned by a visual sensor, a boundary voxel map is generated and stored in an octree map, and the space is divided in combination with the task scenario of the robotic arm. The RRT-Connect motion planning algorithm is used to calculate the interference-free path, so as to realize the active mapping of the robotic arm.
It enables synchronous planning of maps and paths in unknown environments, avoiding collision risks and improving the efficiency and safety of robotic arm task execution in complex environments.
Smart Images
Figure CN117182891B_ABST
Abstract
Description
Technical Field
[0001] This application relates to the field of image processing, and more particularly to a method for active mapping with a robotic arm, and also to a device for active mapping with a robotic arm. Background Technology
[0002] Currently, the intelligent manufacturing and automation equipment industry is attracting increasing attention, and traditional production lines are gradually shifting towards flexible manufacturing. Combined with spatial vision systems, robotic arms have garnered widespread attention in areas such as human-machine collaboration, home services, medical robots, and flexible manufacturing. This also presents new application requirements for robotic arms: how can they fully perceive their environment, avoid potential collision risks, and safely and efficiently complete designated tasks in unknown and complex environments?
[0003] In existing technologies, the active motion planning of robotic arms in unknown and complex environments mostly involves first achieving environmental perception through human-assisted modeling or by fixing one or more visual sensors around the robot. Then, the motion planning of the robotic arm is carried out under the premise that the environment is completely known. Therefore, the map and motion planning need to be decoupled separately.
[0004] Therefore, the problem that needs to be solved is the separate decoupling of the map and the path. Summary of the Invention
[0005] The purpose of this application is to overcome the shortcomings of existing technologies where the creation of maps and the mapping paths of robotic arms are decoupled separately, leading to collisions. This application provides a method for active mapping of robotic arms and also relates to an active mapping device for robotic arms.
[0006] This application provides a method for active mapping of a robotic arm, including:
[0007] S1 scans and updates the environmental map using a visual sensor;
[0008] S2 performs boundary voxel detection in the environment map to generate a boundary voxel map, stores the boundary voxel information of the boundary voxel map in an octree map, and keeps the minimum resolution of the reference coordinate system of the boundary voxel map consistent with the resolution of the environment octree map to generate a three-dimensional boundary map.
[0009] Optionally, the vision sensor is positioned at the end of the robotic arm for continuous active mapping, including:
[0010] Before scanning and updating the environment map:
[0011] S0 performs hand-eye calibration of the coordinate systems of the end-effector vision sensor and the actuator of the robotic arm;
[0012] After generating the 3D boundary map:
[0013] S3 divides the space based on the robotic arm's task scenario to determine the scale of the sub-regions for spatial exploration.
[0014] S4 determines the next sub-region to be explored based on the three-dimensional boundary map, the environmental octree map, and the regional historical exploration information.
[0015] S5 generates an observation sphere based on the positional relationship between the exploration subdomain and the robotic arm and the depth range of the visual sensor;
[0016] S6 generates sampling points uniformly on the observation sphere, and obtains the rigid body transformation matrix based on the rotational relationship between the spatial position of the sampling points and the optical axis of the visual sensor, thereby obtaining the viewpoint set;
[0017] S7 combines hand-eye calibration with the robotic arm kinematics model to perform inverse kinematics solution based on operability optimization, obtains the robotic arm observation configuration set corresponding to the viewpoint set, and sorts the elements in the configuration set;
[0018] S8 uses the RRT-Connect motion planning algorithm to calculate the interference-free path from the current configuration of the robotic arm to the optimal feasible configuration in the configuration set. Based on the interference-free path, the robotic arm is moved and S1 to S8 are repeated to form an environment map.
[0019] Optionally, the generation of the boundary voxel map further includes:
[0020] S2.1 Identify the set of voxels in the environment map whose occupancy state has changed relative to the previous environment map voxel occupancy state;
[0021] S2.2 Based on the occupancy status of the original boundary voxels in the current environment map, query the voxels in the voxel set, delete or retain the voxels, and generate a boundary voxel map;
[0022] Optionally, determining the exploration domain further includes:
[0023] S5.1 Query the information of the exploration domain in space through a multi-dimensional spatial point index to obtain the exploration domain information;
[0024] S5.2 Introduces map information entropy into the exploration information, using the cosine of the angle between the visual sensor's optical axis vector and the vector from the optical center to the center of the exploration domain as the motion cost, to evaluate and select the exploration domain, including:
[0025] The formula for calculating the information entropy of a single voxel x is as follows:
[0026]
[0027] Where H(x) is the information entropy of a single voxel x, and P o(x) represents the occupancy probability of voxel x, and ln denotes the natural logarithm.
[0028] Optionally, it also includes: generating a specified number of sampling points on the observation sphere using Fibonacci grid sampling.
[0029] Optionally, it also includes:
[0030] After obtaining the viewpoint set, the configuration sequence is obtained by comprehensively sorting the joint space Euclidean distance between the current configuration of the robotic arm and the corresponding candidate configuration based on the information gain corresponding to the candidate viewpoint, the operability index of the corresponding candidate configuration, and the candidate configuration.
[0031] This application also provides an active mapping device for a robotic arm, comprising:
[0032] The calibration module is used to perform hand-eye calibration of the coordinate systems of the end effector vision sensor and the actuator of the robotic arm; the acquisition module is used to scan and update the environmental map through the vision sensor.
[0033] The detection module is used to perform boundary voxel detection in the environment map and generate a boundary voxel map;
[0034] The boundary module is used to delete or retain the voxels based on the occupancy status of the original boundary voxels in the current environment map, and generate a boundary voxel octree map.
[0035] The region module is used to determine the sub-regions to be explored based on the three-dimensional boundary map, environment map, and historical exploration information.
[0036] The generation module is used to generate an observation sphere based on the positional relationship between the exploration domain and the robotic arm and the depth range of the visual sensor;
[0037] The viewpoint module is used to uniformly generate sampling points on the observation sphere, and obtain a rigid body transformation matrix based on the rotational relationship between the spatial position of the sampling points and the optical axis of the vision sensor, thereby obtaining a viewpoint set.
[0038] The configuration module is used to combine hand-eye calibration and robotic arm kinematics model to perform inverse kinematics solution based on operability optimization, obtain robotic arm observation configuration set corresponding to viewpoint set, and sort the elements in the configuration set;
[0039] The planning module is used to calculate the non-interference path from the current configuration of the robotic arm to the optimal feasible configuration in the configuration set using the RRT-Connect motion planning algorithm, move the robotic arm based on the non-interference path and repeat the functions of the above modules to form an environment map.
[0040] Optionally, generating the boundary voxel map includes:
[0041] The detection module identifies sets of voxels in the environment map whose voxel occupancy states have changed relative to the previous voxel occupancy states in the environment map.
[0042] The boundary module is used to delete or retain the voxels based on the occupancy status of the original boundary voxels in the current environment map, and generate a boundary voxel map.
[0043] Optionally, the generation module, in determining the exploration domain, further includes:
[0044] In the three-dimensional boundary map, information about the exploration domain is retrieved by querying the exploration domain in space through a multi-dimensional spatial point index.
[0045] Map information entropy is introduced into the exploration information, and the cosine of the angle between the optical axis vector of the visual sensor and the vector from the optical center to the center of the exploration domain is used as the motion cost to evaluate and select the exploration domain, including:
[0046] The formula for calculating the information entropy of a single voxel x is as follows:
[0047]
[0048] Where H(x) is the information entropy of a single voxel x, and P o (x) represents the occupancy probability of voxel x, and ln denotes the natural logarithm.
[0049] The exploration domain is determined by determining the error during scanning of the exploration domain based on the information entropy of multiple individual voxels x.
[0050] Optionally, the viewpoint module further includes:
[0051] After obtaining the viewpoint set, the viewpoint sequence is obtained by sorting them from near to far according to the Euclidean distance between the current position of the visual sensor and the candidate viewpoint positions;
[0052] The interference-free path is calculated based on the viewpoint sequence.
[0053] The advantages and beneficial effects of this application are as follows:
[0054] This application provides a method for active mapping by a robotic arm, comprising: S1 scanning and updating an environmental map using a vision sensor; S2 performing boundary voxel detection in the environmental map, storing the boundary voxel information in an octree map, wherein the minimum resolution of the reference coordinate system of the boundary voxel map is consistent with the resolution of the environmental octree map, thereby generating a three-dimensional boundary map. This application, by combining the boundary voxel map and the environmental octree map, can simultaneously query the occupancy status and boundary information of three-dimensional voxels, avoiding the decoupling of the map and path planning processes. Attached Figure Description
[0055] Figure 1 This is a schematic diagram of the active path planning method for robotic arms in this application.
[0056] Figure 2 This is a schematic diagram of the active path planning logic of the robotic arm in this application.
[0057] Figure 3 This is a schematic diagram of the working area of the robotic arm in this application.
[0058] Figure 4 This is a schematic diagram of the coverage observation viewpoint in this application.
[0059] Figure 5 This is a schematic diagram of the active mapping device in this application. Detailed Implementation
[0060] The present application will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand and implement the present application.
[0061] The following are examples of specific implementation processes provided to illustrate the technical solutions to be protected in this application. However, this application may also be implemented in other ways different from those described herein. Those skilled in the art can implement this application by different technical means under the guidance of the concept of this application. Therefore, this application is not limited to the specific embodiments below.
[0062] This application provides a method for active mapping of a robotic arm, and also relates to an active mapping device for a robotic arm. Figure 1 and Figure 5 The method and apparatus described in this application are illustrated respectively.
[0063] This application provides a method for active mapping by a robotic arm, comprising: S1 scanning and updating an environmental map using a vision sensor; S2 performing boundary voxel detection in the environmental map, storing the boundary voxel information in an octree map, wherein the minimum resolution of the reference coordinate system of the boundary voxel map is consistent with the resolution of the environmental octree map, thereby generating a three-dimensional boundary map. This application, by combining the boundary voxel map and the environmental octree map, can simultaneously query the occupancy status and boundary information of three-dimensional voxels, avoiding the decoupling of the map and path planning processes.
[0064] The following is combined Figure 1 and Figure 2 The embodiments of active path planning for robotic arms in this application are further described in detail.
[0065] S0 performs hand-eye calibration of the coordinate systems of the end-effector vision sensor and the actuator of the robotic arm.
[0066] Configure a visual perception system for the robotic arm, such as a vision sensor. Fix the robotic arm to the base, install the vision sensor on the end effector of the robotic arm, and then perform hand-eye calibration between the coordinate system of the vision sensor and the coordinate system of the end effector of the robotic arm.
[0067] The hand-eye calibration described in this application involves rotating the end effector around the x, y, and z axes of the robot arm's base coordinate system, calculating the corresponding pose matrix based on the rotation matrix, and then calculating the transformation matrix from the vision sensor coordinate system to the robot arm's base coordinate system based on the pose matrix. The robot arm's movement is based on the robot's base coordinate system, while the pose information acquired by the vision sensor is relative to the vision sensor's coordinate system. To ensure that the robot arm can accurately move to the workpiece pose identified by the laser line scanning vision sensor, the transformation relationship between the vision sensor coordinate system and the robot coordinate system is determined, i.e., the transformation matrix from the vision sensor coordinate system to the robot's base coordinate system, thus completing the hand-eye calibration.
[0068] The aforementioned visual perception system can also be other visual sensors capable of acquiring graphic data, such as small radars, laser scanners, etc.
[0069] S1 scans and updates the environmental map using a visual sensor.
[0070] By configuring a robotic arm vision perception system, the robotic arm is fixed to a base, and vision sensors are installed on the end effector of the robotic arm. Environmental map data is acquired based on the vision sensors and used to update previous map data.
[0071] S2 performs boundary voxel detection in the environment map to generate a boundary voxel map, stores the boundary voxel information of the boundary voxel map in an octree map, and keeps the minimum resolution of the reference coordinate system of the boundary voxel map consistent with the resolution of the environment octree map to generate a three-dimensional boundary map.
[0072] S2.1 Identify the set of voxels in the environment map whose voxel occupancy states have changed relative to the previous environment map voxel occupancy states.
[0073] In this application, map boundary voxels are defined as free voxels adjacent to unknown voxels. After each environment map update, boundary voxel detection is not performed on the entire map, but only on the set of voxels whose occupancy state has changed relative to the previous frame.
[0074] S2.2 Based on the voxel set query, query the voxel adjacent to the original boundary voxel in the current environment map, delete or retain the voxel, and combine it with the newly detected boundary voxel to generate a boundary voxel set.
[0075] The boundary voxel map is updated based on the current environment representation and the latest changes in the environment map. The data stored in each node represents its probability of being a map boundary.
[0076] In this way, the occupancy status and boundary information of discrete three-dimensional voxels within any sub-region of the working area can be queried simultaneously.
[0077] Then, by combining multi-dimensional spatial point indexes, an exploration information system can be created to update the distribution of environmental map boundaries and spatial occupancy status in real time, record historical exploration areas and observation viewpoint information, and provide efficient query of spatial sub-region exploration information.
[0078] The above-mentioned combination of boundary voxel map and environment octree map allows for simultaneous querying of the occupancy status and boundary information of three-dimensional voxels within a spatial sub-region, providing a basis for the next step of region selection. The following section provides a more detailed description of the active mapping implementation of the robotic arm path.
[0079] like Figure 1 As shown, the active mapping method for robotic arms provided in this application avoids the separate decoupling of environmental modeling and planning in unknown environments, effectively avoiding potential collision risks.
[0080] S3 divides the space based on the robotic arm's task scenario to determine the scale of the sub-regions for spatial exploration.
[0081] Based on the aforementioned 3D boundary map, environmental octree map, and regional historical exploration information, S4 determines the next sub-region to be explored.
[0082] The work area division described in this application is to divide the scene according to actual needs, such as dividing the robotic arm work area by combining scene requirements and the robotic arm's working range.
[0083] like Figure 4 As shown, since the end-effector of the robotic arm cannot observe the interference information between its own joints and the environment, an initial unobstructed area 301 is set for the robotic arm before the exploration begins, while the remaining space is completely unknown, namely the area to be explored 302.
[0084] The first step is to perform boundary voxel detection on the environmental map.
[0085] In this application, the map boundary voxel is defined as: the free voxel adjacent to the unknown voxel.
[0086] In this application, in order to improve computational efficiency, boundary detection is not performed on the entire map after each environment map update. Instead, it is performed on the set of voxels whose occupancy status has changed relative to the previous frame. The deletion or retention operation is determined by querying the occupancy status of the original boundary voxels in the current environment map.
[0087] The detailed process is as follows:
[0088] This application employs a multi-resolution candidate exploration domain generation method. After extracting the boundaries of the environment map, it uses the idea of spatial recursive partitioning to create a three-dimensional boundary map based on an octree structure.
[0089] Specifically, unlike existing environmental octree maps, which use observation data corresponding to known poses of visual sensors as input for fusion and updating, this application updates the boundary voxel map based on the current environmental representation and the latest changes in the environmental map. The data stored in each node represents its probability of being a map boundary.
[0090] A new boundary voxel map is generated by keeping the root nodes and minimum resolution of the boundary voxel map and the environment octree map consistent. This allows for simultaneous querying of the occupancy status and boundary information of any discrete 3D voxel in space.
[0091] Finally, by combining a multi-dimensional spatial point index, an exploration information system is created to update the boundary distribution and spatial occupancy status of the environmental map in real time, and to record historical exploration areas and observation viewpoint information. This effectively improves the speed of querying exploration information in spatial sub-regions.
[0092] S5 generates an observation sphere based on the positional relationship between the exploration subdomain and the robotic arm and the depth range of the visual sensor.
[0093] Specifically, taking the center of the exploration domain as the center of the sphere, and combining the depth range of the visual sensor and the distance function from the center of the sphere to the z-axis of the robot arm's base coordinate system, the radius is determined, and the observation sphere corresponding to the exploration domain is obtained.
[0094] When evaluating the value of candidate exploration domains, in addition to boundary density, this application also incorporates parameter information regarding the impact of observation processes such as visual sensor errors, environmental noise, and / or moving object interference on mapping accuracy. For example:
[0095] S5.1 Query the information of the exploration domain in space through a multi-dimensional spatial point index to obtain the exploration domain information;
[0096] S5.2 Introduces map information entropy into the exploration information, using the cosine of the angle between the visual sensor's optical axis vector and the vector from the optical center to the center of the exploration domain as the motion cost, to evaluate and select the exploration domain, including:
[0097] The formula for calculating the information entropy of a single voxel x is as follows:
[0098] The formula for calculating the information entropy of a single voxel x is as follows:
[0099]
[0100] Where H(x) is the information entropy of a single voxel x, and P o (x) represents the occupancy probability of voxel x, and ln denotes the natural logarithm.
[0101] S6 generates sampling points uniformly on the observation sphere, and obtains the rigid body transformation matrix based on the rotational relationship between the spatial position of the sampling points and the optical axis of the vision sensor, thereby obtaining the viewpoint set.
[0102] like Figure 5 As shown, a specified number of sampling points 304 are generated on the observation sphere 303 using Fibonacci grid sampling. The spatial coordinates of the sampling points 304 are used as candidate viewpoint positions, and the vector pointing from the sampling points 304 to the center of the sphere is used as the optical axis vector of the visual sensor at that position. The rotation vector between the current optical axis vector of the visual sensor and the optical axis vector at the sampling position is calculated, and the rotation matrix is obtained according to the Rodriguez rotation formula.
[0103] Finally, the corresponding visual sensor observation pose is calculated by combining coordinate transformation, and its validity is checked (there is an inverse kinematic solution without collision risk) to generate a candidate viewpoint set.
[0104] S7 combines hand-eye calibration with the robotic arm kinematics model to perform inverse kinematics solution based on operability optimization, obtains the robotic arm observation configuration set corresponding to the viewpoint set, and sorts the elements in the configuration set.
[0105] Specifically, after obtaining the viewpoint set, the configuration sequence is obtained by comprehensively sorting the joint space Euclidean distance between the current configuration of the robotic arm and the corresponding candidate configuration based on the information gain corresponding to the candidate viewpoint, the operability index of the corresponding candidate configuration, and the candidate configuration.
[0106] S8 uses the RRT-Connect motion planning algorithm to calculate the interference-free path from the current configuration of the robotic arm to the optimal feasible configuration in the configuration set. Based on the interference-free path, the robotic arm is moved and S1 to S8 are repeated to form an environment map.
[0107] After obtaining the candidate viewpoint set, the optimal viewpoint sequence is obtained by sorting the candidate viewpoints from near to far according to the Euclidean distance between the current position of the visual sensor and the candidate viewpoint positions. If it is detected that the exploration domain has been observed during the exploration process, the Euclidean distance between the viewpoint position and the previous observation position will also be considered in the evaluation of the candidate viewpoints, so as to achieve multi-directional observation of the historical exploration domain. Based on the interference-free path, the robotic arm is moved and S1 to S8 are repeated to form an environmental map.
[0108] In this application, the RRT-Connect motion planning algorithm is used to calculate an interference-free path from the current pose of the robotic arm to the optimal observation pose.
[0109] This application also provides a robotic arm active mapping device, which includes an environmental map creation device for creating maps to achieve path planning.
[0110] The following is combined Figure 5 The embodiments of active path planning for robotic arms in this application are further described in detail.
[0111] The calibration module 501 is used to perform hand-eye calibration of the coordinate systems of the end-effector vision sensor and the actuator of the robotic arm.
[0112] Configure a visual perception system for the robotic arm, such as a vision sensor. Fix the robotic arm to the base, install the vision sensor on the end effector of the robotic arm, and then perform hand-eye calibration between the coordinate system of the vision sensor and the coordinate system of the end effector of the robotic arm.
[0113] The hand-eye calibration described in this application involves rotating the end effector around the x, y, and z axes of the robot arm's base coordinate system, calculating the corresponding pose matrix based on the rotation matrix, and then calculating the transformation matrix from the vision sensor coordinate system to the robot arm's base coordinate system based on the pose matrix. The robot arm's movement is based on the robot's base coordinate system, while the pose information acquired by the vision sensor is relative to the vision sensor's coordinate system. To ensure that the robot arm can accurately move to the workpiece pose identified by the laser line scanning vision sensor, the transformation relationship between the vision sensor coordinate system and the robot coordinate system is determined, i.e., the transformation matrix from the vision sensor coordinate system to the robot's base coordinate system, thus completing the hand-eye calibration.
[0114] The aforementioned visual perception system can also be other visual sensors capable of acquiring graphic data, such as small radars, laser scanners, etc.
[0115] The acquisition module 502 is used to scan and update the environment map using a visual sensor.
[0116] By configuring a robotic arm vision perception system, the robotic arm is fixed to a base, and vision sensors are installed on the end effector of the robotic arm. Environmental map data is acquired based on the vision sensors and used to update previous map data.
[0117] The detection module 503 is used to perform boundary voxel detection in the environment map and generate a boundary voxel map, including: identifying a set of voxels in the environment map whose occupancy state has changed relative to the previous environment map voxel occupancy state.
[0118] Identify the set of voxels in the environment map whose occupancy state has changed relative to the previous environment map voxel occupancy state.
[0119] In this application, map boundary voxels are defined as free voxels adjacent to unknown voxels. After each environment map update, boundary voxel detection is not performed on the entire map, but only on the set of voxels whose occupancy state has changed relative to the previous frame.
[0120] Boundary module 504 is used to delete or retain the voxels based on the occupancy status of the original boundary voxels in the current environment map, and generate a boundary voxel octree map.
[0121] Specifically, the deletion or retention operation can be determined by querying the occupancy status of the original boundary voxel in the current environment map. For example, if the occupancy status changes and the occupancy status in the current environment map is actually retained, otherwise the updated map or voxel is deleted.
[0122] The minimum resolution of the boundary voxel map is fused into the root node of a preset environment octree map to generate a 3D boundary map.
[0123] The boundary voxel map is updated based on the current environment representation and the latest changes in the environment map. The data stored in each node represents its probability of being a map boundary.
[0124] A new 3D boundary map is generated by keeping the root nodes of the boundary voxel map and the environment octree map consistent at the minimum resolution.
[0125] In this way, the occupancy status and boundary information of any discrete three-dimensional voxel in space can be queried simultaneously.
[0126] Then, by combining multi-dimensional spatial point indexes, an exploration information system can be created to update the distribution of environmental map boundaries and spatial occupancy status in real time, record historical exploration areas and observation viewpoint information, and provide efficient query of spatial sub-region exploration information.
[0127] Region module 505 is used to determine the sub-region to be explored based on the three-dimensional boundary map, environment map, and historical exploration information;
[0128] The work area division described in this application is to divide the scene according to actual needs, such as dividing the robotic arm work area by combining scene requirements and the robotic arm's working range.
[0129] like Figure 4 As shown, since the end-effector of the robotic arm cannot observe the interference information between its own joints and the environment, an initial unobstructed area 301 is set for the robotic arm before the exploration begins, while the remaining space is completely unknown, namely the area to be explored 302.
[0130] The first step is to perform boundary voxel detection on the environmental map.
[0131] In this application, the map boundary voxel is defined as: the free voxel adjacent to the unknown voxel.
[0132] In this application, in order to improve computational efficiency, boundary detection is not performed on the entire map after each environment map update. Instead, it is performed on the set of voxels whose occupancy status has changed relative to the previous frame. The deletion or retention operation is determined by querying the occupancy status of voxels adjacent to the original boundary voxels in the current environment map.
[0133] The detailed process is as follows:
[0134] This application employs a multi-resolution candidate exploration domain generation method. After extracting the boundaries of the environment map, it uses the idea of spatial recursive partitioning to create a three-dimensional boundary map based on an octree structure.
[0135] Specifically, unlike existing environmental octree maps, which use observation data corresponding to known poses of visual sensors as input for fusion and updating, this application updates the boundary voxel map based on the current environmental representation and the latest changes in the environmental map. The data stored in each node represents its probability of being a map boundary.
[0136] By keeping the root nodes and minimum resolution of the boundary voxel map and the environment octree map consistent, it is possible to simultaneously query the occupancy status and boundary information of any discrete 3D voxel in space.
[0137] Finally, by combining a multi-dimensional spatial point index, an exploration information system is created to update the boundary distribution and spatial occupancy status of the environmental map in real time, and to record historical exploration areas and observation viewpoint information. This effectively improves the speed of querying exploration information in spatial sub-regions.
[0138] The generation module 506 is used to determine the region where the voxel set is located in the region to be explored as the exploration domain, and generate an observation sphere based on the positional relationship between the exploration domain and the robotic arm and the depth range of the visual sensor.
[0139] Specifically, taking the center of the exploration domain as the center of the sphere, and combining the depth range of the visual sensor and the distance function from the center of the sphere to the z-axis of the robot arm's base coordinate system, the radius is determined, and the observation sphere corresponding to the exploration domain is obtained.
[0140] When evaluating the value of candidate exploration domains, in addition to boundary density, this application also incorporates parameter information regarding the impact of observation processes such as visual sensor errors, environmental noise, and / or moving object interference on mapping accuracy. For example:
[0141] S5.1 Query the information of the exploration domain in space through a multi-dimensional spatial point index to obtain the exploration domain information;
[0142] S5.2 Introduces map information entropy into the exploration information, using the cosine of the angle between the visual sensor's optical axis vector and the vector from the optical center to the center of the exploration domain as the motion cost, to evaluate and select the exploration domain, including:
[0143] The formula for calculating the information entropy of a single voxel x is as follows:
[0144]
[0145] Where H(x) is the information entropy of a single voxel x, and P o (x) represents the occupancy probability of voxel x, and ln denotes the natural logarithm.
[0146] S6.3 Determine the error during scanning of the exploration domain based on the information entropy of the multiple individual voxels x, and determine the exploration domain.
[0147] The viewpoint module 507 is used to set multiple sampling points pointing to the center of the observation sphere on the observation sphere, generate a rotation matrix according to the rotation relationship between the sampling points and the optical axis of the visual sensor, and obtain a viewpoint set in combination with the hand-eye calibration.
[0148] After obtaining the viewpoint set, the configuration sequence is obtained by comprehensively sorting the joint space Euclidean distance between the current configuration of the robotic arm and the corresponding candidate configuration based on the information gain corresponding to the candidate viewpoint, the operability index of the corresponding candidate configuration, and the candidate configuration.
[0149] like Figure 5 As shown, a specified number of sampling points 304 are generated on the observation sphere 303 using Fibonacci grid sampling. The spatial coordinates of the sampling points 304 are used as candidate viewpoint positions, and the vector pointing from the sampling points 304 to the center of the sphere is used as the optical axis vector of the visual sensor at that position. The rotation vector between the current optical axis vector of the visual sensor and the optical axis vector at the sampling position is calculated, and the rotation matrix is obtained according to the Rodriguez rotation formula.
[0150] Finally, the corresponding visual sensor observation pose is calculated by combining coordinate transformation, and its validity is checked (there is an inverse kinematic solution without collision risk) to generate a candidate viewpoint set.
[0151] Configuration module 508 is used to combine hand-eye calibration and robotic arm kinematics model to perform inverse kinematics solution based on operability optimization, obtain robotic arm observation configuration set corresponding to viewpoint set, and sort the elements in the configuration set;
[0152] The planning module 509 is used to calculate the non-interference path from the current configuration of the robotic arm to the optimal feasible configuration in the configuration set using the RRT-Connect motion planning algorithm, move the robotic arm based on the non-interference path and repeat the functions of the above modules to form an environment map.
[0153] After obtaining the candidate viewpoint set, the optimal viewpoint sequence is obtained by sorting the candidate viewpoints from near to far according to the Euclidean distance between the current position of the visual sensor and the candidate viewpoint positions. If it is detected that the exploration domain has been observed during the exploration process, the Euclidean distance between the viewpoint position and the previous observation position will also be considered in the evaluation of the candidate viewpoints, so as to achieve multi-directional observation of the historical exploration domain. Based on the interference-free path, the robotic arm moves and repeats the functions of the above modules to form an environmental map.
[0154] In this application, the RRT-Connect motion planning algorithm is used to calculate an interference-free path from the current pose of the robotic arm to the optimal observation pose.
[0155] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application.
Claims
1. A method for active mapping with a robotic arm, characterized in that, include: S1 scans and updates the environmental map using a visual sensor; S2 performs boundary voxel detection in the environment map to generate a boundary voxel map, stores the boundary voxel information of the boundary voxel map in an octree map, and keeps the minimum resolution of the reference coordinate system of the boundary voxel map consistent with the resolution of the environment octree map to generate a three-dimensional boundary map. Before scanning and updating the environment map: S0 performs hand-eye calibration of the coordinate systems of the end-effector vision sensor and the actuator of the robotic arm; After generating the 3D boundary map: S3 divides the space based on the robotic arm's task scenario to determine the scale of the sub-regions for spatial exploration. S4 determines the next sub-region to be explored based on the three-dimensional boundary map, the environmental octree map, and the regional historical exploration information. S5 generates an observation sphere based on the positional relationship between the sub-region to be explored and the robotic arm, and the depth range of the visual sensor. S6 generates sampling points uniformly on the observation sphere, and obtains the rigid body transformation matrix based on the rotational relationship between the spatial position of the sampling points and the optical axis of the visual sensor, thereby obtaining the viewpoint set; S7 combines hand-eye calibration with the robotic arm kinematics model to perform inverse kinematics solution based on operability optimization, obtains the robotic arm observation configuration set corresponding to the viewpoint set, and sorts the elements in the configuration set; S8 uses the RRT-Connect motion planning algorithm to calculate the interference-free path from the current configuration of the robotic arm to the optimal feasible configuration in the configuration set. Based on the interference-free path, the robotic arm is moved and S1 to S8 are repeated to form an environment map.
2. The active mapping method for robotic arms according to claim 1, characterized in that, The generation of the boundary voxel map also includes: S2.1 Identify the set of voxels in the environment map whose occupancy state has changed relative to the previous environment map voxel occupancy state; S2.2 Based on the occupancy status of the original boundary voxels in the current environment map, query the voxels in the voxel set, delete or retain the voxels, and generate a boundary voxel map.
3. The active mapping method for robotic arms according to claim 1, characterized in that, S5 also includes: S5.1 Query the information of the exploration domain in space through a multi-dimensional spatial point index to obtain the exploration domain information; S5.2 Introduces map information entropy into the exploration information, using the cosine of the angle between the optical axis vector of the visual sensor and the vector from the optical center to the center of the exploration domain as the motion cost, to evaluate and select the exploration domain, including: Single voxel The formula for calculating information entropy is as follows: in, It is a single voxel Information entropy voxels The probability of occupancy, Represents the natural logarithm. .
4. The active mapping method according to claim 1, characterized in that, Also includes: A specified number of sampling points are generated on the observation sphere using Fibonacci grid sampling.
5. The active mapping method for a robotic arm according to claim 1, characterized in that, Also includes: After obtaining the viewpoint set, the configuration sequence is obtained by comprehensively sorting the joint space Euclidean distance between the current configuration of the robotic arm and the corresponding candidate configuration based on the information gain corresponding to the candidate viewpoint, the operability index of the corresponding candidate configuration, and the candidate configuration.
6. A robotic arm active mapping device, characterized in that, include: The calibration module is used to perform hand-eye calibration of the coordinate systems of the end effector vision sensor and the actuator of the robotic arm; The acquisition module is used to scan and update the environmental map using a visual sensor; The detection module is used to perform boundary voxel detection in the environment map and generate a boundary voxel map; The boundary module is used to delete or retain the voxels based on the occupancy status of the original boundary voxels in the current environment map, and generate a boundary voxel octree map. The region module is used to determine the sub-regions to be explored based on the 3D boundary map, environment map, and historical exploration information. The generation module is used to generate an observation sphere based on the positional relationship between the sub-region to be explored and the robotic arm and the depth range of the visual sensor; The viewpoint module is used to uniformly generate sampling points on the observation sphere, and obtain a rigid body transformation matrix based on the rotational relationship between the spatial position of the sampling points and the optical axis of the vision sensor, thereby obtaining a viewpoint set. The configuration module is used to combine hand-eye calibration and robotic arm kinematics model to perform inverse kinematics solution based on operability optimization, obtain robotic arm observation configuration set corresponding to viewpoint set, and sort the elements in the configuration set; The planning module is used to calculate the non-interference path from the current configuration of the robotic arm to the optimal feasible configuration in the configuration set using the RRT-Connect motion planning algorithm, move the robotic arm based on the non-interference path and repeat the functions of the above modules to form an environment map.
7. The robotic arm active mapping device according to claim 6, characterized in that, The generation of the boundary voxel map includes: The detection module identifies sets of voxels in the environment map whose voxel occupancy states have changed relative to the previous voxel occupancy states in the environment map. The boundary module is used to delete or retain the voxels based on the occupancy status of the original boundary voxels in the current environment map, and generate a boundary voxel map.
8. The robotic arm active mapping device according to claim 6, characterized in that, The generation module is also used for: In the three-dimensional boundary map, information about the exploration domain is retrieved by querying the exploration domain in space through a multi-dimensional spatial point index. Map information entropy is introduced into the exploration information, and the cosine of the angle between the optical axis vector of the visual sensor and the vector from the optical center to the center of the exploration domain is used as the motion cost to evaluate and select the exploration domain, including: Single voxel The formula for calculating information entropy is as follows: in, It is a single voxel Information entropy voxels The probability of occupancy, Represents the natural logarithm. ; According to multiple individual voxels The information entropy is used to determine the error during scanning of the exploration domain, and the exploration domain is determined.
9. The robotic arm active mapping device according to claim 6, characterized in that, The viewpoint module also includes: After obtaining the viewpoint set, the viewpoint sequence is obtained by sorting them from near to far according to the Euclidean distance between the current position of the visual sensor and the candidate viewpoint positions; The interference-free path is calculated based on the viewpoint sequence.