Robot redundant attitude planning method and system based on position and obstacle constraint

Abstract

The invention discloses a robot redundant attitude planning method and system based on position and obstacle constraints, and the method comprises the following steps: generating a planning path of a robot based on a redundant attitude space and a known robot path position point, the planning path being represented by a position space and an attitude space; constructing an approximate key collision plane; calculating a feasible attitude area based on the obstacle information and the redundant attitude space on the approximate key collision plane; estimating a preferential sampling attitude area; continuously sampling in the position space and the attitude space, acquiring a pose node to expand the random search tree until a target node is reached, and acquiring a target path; and iterating the random search tree extension operation to obtain more target paths, and selecting a target path with the lowest total cost from all the obtained target paths. According to the method, the exploration efficiency of redundant attitude planning can be improved, the integrity and safety of the process are ensured, and the method has relatively high applicability.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot control and intelligent manufacturing, and particularly relates to a redundant posture planning method and system for a robot based on position and obstacle constraints. Background Art

[0002] Industrial robots are a key component of modern industrial automation and have been widely used in the field of machining due to their flexible operation, high efficiency, high precision, and low cost. Industrial robots often need to work in a dynamic and changing environment when performing tasks, which poses higher requirements for their obstacle avoidance ability.

[0003] The Chinese patent application with the publication number CN119188770A discloses a multi-degree-of-freedom robotic arm obstacle avoidance control method based on machine vision. The panoramic image data around the robotic arm is obtained through a multi-camera system, and the two-dimensional and three-dimensional information obtained by the RGB camera and the depth camera is combined to construct an environmental model. A pre-trained convolutional neural network model is used to identify fixed and moving obstacles in the image, and the motion trajectory of the moving obstacles is predicted by combining a Kalman filter. The improved rapidly-exploring random tree algorithm is used for path planning to ensure that the robotic arm maintains an efficient and accurate obstacle avoidance ability in a dynamic environment. The motion state of the robotic arm is monitored through real-time feedback control to avoid deviations caused by inertia or errors. The present invention can improve the dynamic adaptability and safe operation performance of the robotic arm in a complex environment and is widely applicable to robotic applications in fields such as industry and medicine.

[0004] In an actual robot working scenario, some tasks require the robot to strictly follow the constraints of six degrees of freedom (such as cutting and sewing, etc.), while other tasks only need to satisfy the constraints of five or fewer degrees of freedom (such as welding, gluing, grinding, etc.). For example, in arc welding operations, only three degrees of freedom are required to adjust the position of the end effector, two degrees of freedom are used to adjust the posture within a certain range, and the rotation around the axis of the end effector does not need to follow a specific path. After specifying the position and direction that the robot end effector needs to strictly follow, the remaining degrees of freedom related to the posture but not strictly constrained are called redundant posture degrees of freedom, and these degrees of freedom constitute the redundant posture space.

[0005] Therefore, in the problem of continuous motion path posture planning for a robot under precise position constraints, it is necessary to make full use of the redundant posture space to plan the posture required for the current path position point, efficiently avoid obstacles in the robot working environment, and at the same time ensure that the robot follows precise position constraints to ensure the integrity and safety of the process. Summary of the Invention

[0006] The present invention provides a robot redundant posture planning method and system based on position and obstacle constraints, aiming to solve the problems of low planning efficiency and applicability in the posture planning of the motion path of industrial robots in the prior art.

[0007] To solve the above technical problems, the robot redundant posture planning method based on position and obstacle constraints proposed by the present invention includes the following steps: Generate a planned path of the robot based on the redundant posture space and known robot path position points, and the planned path is represented by a position space and a posture space; Construct an approximate critical collision plane based on the path position points in the planned path and the obstacle information in the robot work space. The approximate critical collision plane is used to receive the projection of obstacles and transform the posture planning in three-dimensional space into a two-dimensional plane; Calculate a feasible posture region based on the obstacle information on the approximate critical collision plane and the redundant posture space; Estimate the central region of the feasible posture region as the priority sampling posture region according to the feasible posture region, the current path position point, the optimal posture point of the previous path position point, the subsequent path position point and the subsequent obstacles; Based on the guidance of the position space and the priority sampling posture region, continuously sample in the position space and the posture space to obtain pose nodes to expand the rapidly-exploring random tree until the target node is reached, and obtain a target path; Iteratively perform pose node sampling and rapidly-exploring random tree expansion operations to obtain more target paths, and select a target path with the lowest total cost among all the obtained target paths.

[0008] Preferably, the calculation method of the priority sampling posture region is as follows: For each posture point in the feasible posture region, use the Gaussian kernel density function to estimate its probability density in the feasible posture region. The calculation method is as follows:

[0009]

[0010] In the formula, is the feasible posture region is the discretized redundant posture point in is the posture point in the feasible posture region is the probability density in is the feasible posture region is the sampling point in is the number of discretized redundant posture points, is the normalized comprehensive weight, which is used to assign different weight influence factors to each pose point and can be adjusted according to specific requirements. is the Gaussian kernel function with a bandwidth of . Select the points in a part of the region with the largest probability density value to form a set, which is used as the preferred sampling pose region corresponding to the current path position point. The point with the highest probability density in the preferred sampling pose region is the optimal pose point.

[0011] Preferably, the calculation method of the normalized comprehensive weight is as follows:

[0012] where is the optimal pose point corresponding to the path position point at the previous moment to the influence of each pose point in the feasible pose region , is the normalized , and the calculation method is as follows:

[0013]

[0014] In the formula, is an adjustable parameter used to control the influence degree of on ; are the normalized values of the influence weights of the path position points at the current moment , the future path position points , the future path position points , and the path position point closest to the next obstacle to each pose point in the feasible pose region , and are calculated according to the following formula:

[0015] In the formula, is an adjustable parameter used to control the influence degree of the path position point on the set points in , and the corresponding is obtained by adjusting the value of .

[0016] Preferably, the value rule of the adjustable parameter is as follows: For the normalized weight of the path position point at the current moment, ; For the future path position points the normalized weight , ; For the future path position points the normalized weight , ; For the normalized weight of the path position point closest to the next obstacle the normalized weight , .

[0017] Preferably, the method for obtaining the target path is specifically as follows: Randomly sample in the position space and the pose space to generate collision-free pose nodes as sampling nodes; Take the minimum joint displacement between two pose nodes of the robot as the cost function, and select the valid node closest to the sampling node in the random search tree; Based on the sampling node and the closest valid node, expand the nodes in the position space and the pose space respectively to generate new nodes; Backtrack the position sequence of the new node in the path, select the low-cost nodes as the candidate range of the new connection method, perform cost evaluation and collision-free verification on the nodes within the candidate range, and obtain the collision-free and lowest-cost node as the final parent node; Generate a path connection according to the new node and the final parent node; Extend from the position sequence corresponding to the new node towards the path end point, screen the low-cost candidate child nodes, adjust the parent node of the candidate child nodes to the new node, and update the path through cost evaluation and collision-free verification; When the expanded node approaches the target node, complete a pose planning process to generate a target path.

[0018] Preferably, the random sampling algorithm uses the RRT* algorithm.

[0019] Preferably, the sampling method of the RRT* algorithm includes the following steps: Perform initial preprocessing on the redundant pose space and the priority sampling pose region, combine the position space and the pose space, and establish an initial representation of the path to be optimized; Use the Gaussian sampling method to sample the discrete position space to generate position nodes, and guide the path to expand towards the end point by increasing the mean value of the sampling distribution during each iteration; Under the constraints of the redundant pose space and the priority sampling region corresponding to each position node, obtain the pose nodes by combining the target biasing strategy; Combine the pose nodes with the corresponding position nodes to generate complete path nodes.

[0020] Preferably, the target biasing strategy is expressed as follows:

[0021] wherein is the target biasing threshold, is randomly selected from a uniform distribution from 0 to 1, and is expressed by the formula: When , attitude nodes are sampled in the preferred sampling attitude region through the function; When , attitude nodes are randomly sampled in the redundant attitude space through the function; If the node sampled through the function collides, the function is used to resample in the redundant attitude space .

[0022] Preferably, node expansion is respectively performed in the position space and the attitude space to generate new nodes, and the new nodes include new position nodes and new attitude nodes; The method for generating the new position nodes is: starting from the position sequence where the nearest position node is located, taking one step forward along the direction of the end point of the path to obtain new position nodes; The method for generating the new attitude nodes is: node expansion is performed using an interpolation algorithm between the nearest attitude node and the sampled attitude node in the attitude space, and the new attitude nodes are expressed as follows:

[0023] wherein represents spherical linear interpolation of quaternion attitudes, is a random number selected in , indicating that the new attitude node is located between the nearest attitude node and the sampled attitude node ; Combining the new position nodes and the new attitude nodes to generate a temporary node, performing collision detection. If the temporary node collides and interferes with the environment, new position nodes and new attitude nodes are re-obtained to generate a temporary node until the temporary node does not collide and interfere with the environment, and then it is used as the final new node.

[0024] On the other hand, the present invention also proposes a robot redundant attitude planning system based on position and obstacle constraints. The system is used to implement the above-mentioned robot redundant attitude planning method, and includes: A path planning module, based on a redundant pose space and known robot path position points, generates a planned path for the robot and represents it using a position space and a pose space; A critical collision plane construction module, based on the path position points in the planned path and the obstacle information in the robot workspace, constructs an approximate critical collision plane for converting three-dimensional pose planning into a two-dimensional plane and receiving the projection of obstacles; A feasible pose region calculation module, based on the obstacle information on the approximate critical collision plane and the redundant pose space, calculates the feasible pose region; A priority sampling region estimation module, according to the feasible pose region, the current path position point, the optimal pose point of the previous moment path position point, the subsequent path position point and the subsequent obstacles, estimates the central region of the feasible pose region as the priority sampling pose region; A pose optimization module, guided by the position space and the priority sampling pose region, continuously samples in the position space and the pose space to obtain pose nodes to expand the random search tree until the target node is reached to obtain a target path; iterates the pose node sampling and random search tree expansion operations to obtain more target paths, and selects the target path with the lowest total cost among all the obtained target paths.

[0025] Compared with the prior art, the present invention has the following technical effects: 1. The redundant pose planning method proposed by the present invention simplifies the pose planning problem in three-dimensional space to a two-dimensional plane by analyzing the obstacle information and constructing an approximate critical collision plane for identifying potential collisions, and then preprocesses the redundant pose space based on Gaussian kernel density estimation, calculates the feasible pose region in the approximate critical collision plane, and on this basis, determines the priority sampling pose region by considering the current path position point, the future path position point and the nearby obstacles using the Gaussian kernel density estimation function, so as to guide the exploration direction of the sampling-based motion planning algorithm through the priority sampling pose region and improve its exploration efficiency.

[0026] 2. The redundant pose planning method proposed by the present invention is suitable for pose planning under the precise continuous motion path constraint of industrial robots, can make full use of the redundant pose space to plan the pose required by the current path position point, avoid obstacles in the robot working environment, and ensure the integrity and safety of the process.

[0027] 3. The redundant pose planning method proposed by the present invention combines the position space and the pose space, is applicable to the robot path planning problem under various complex scenarios, including high redundancy robots, multi-joint manipulators, etc., and has strong applicability. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 is a schematic flow chart of the planning method described in the present invention; Figure 2 It is a schematic diagram of the robot coordinate system and redundant attitude space described in the embodiments of the present invention; Figure 3 It is a schematic diagram of constructing an approximate critical collision plane described in the embodiments of the present invention; Figure 4 It is the feasible region described in the embodiments of the present invention Schematic diagram; Figure 5 It is the schematic diagram of the maximum length range where the approach vector is projected onto the plane ; Figure 6 It is a schematic diagram of calculating the weight of path position points described in the embodiments of the present invention; Figure 7 It is a schematic diagram of the node optimization method described in the embodiments of the present invention; Figure 8 It is a schematic diagram of the node sampling method of the attitude optimization model described in the embodiments of the present invention. Detailed implementation manners

[0029] To make the objectives, technical solutions, and advantages of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below in conjunction with specific embodiments of the present application and with reference to the accompanying drawings.

[0030] Redundant attitude refers to the situation where, when an industrial robot completes a certain task, since its degrees of freedom (DOF) are more than the degrees of freedom required by the task, there are multiple possible joint configurations or attitudes to achieve the position and orientation of the same end effector. For example, a 6-degree-of-freedom robot completes a 5-degree-of-freedom task, then the robot is redundant in the extra degree of freedom. Taking a welding robot as an example, during the welding process, the end effector of the robot arm can reach the welding point in multiple ways without affecting the welding accuracy.

[0031] Redundant attitude space refers to the set of all redundant attitudes that meet the task requirements, and is usually used to describe the possible motion range of the robot in the redundant degrees of freedom.

[0032] Embodiment 1 This embodiment is a method for planning the redundant attitudes of a robot based on position and obstacle constraints. As Figure 1 shown, it includes the following steps 1 to 6: Step 1, based on the redundant attitude space and the known path position points of the robot, generate the planned path of the robot, and the planned path is represented using the position space and the attitude space.

[0033] For the specific acquisition method, please refer to Figure 2, which is a schematic diagram of the robot coordinate system and the redundant attitude space in this embodiment. In robotics, a coordinate system is usually fixedly connected to the end effector of the robot , and the pointing direction of the end effector (the axis of the end effector) is set as axis, and its corresponding direction vector is named the approach vector . The range of the redundant angle is defined using the approach vector. The approach vector and the coordinate system of the current path position point axis has an included angle of . The approach vector is projected onto the plane of the coordinate system of the current path position point, and the included angle with the axis is , as shown in the following formula:

[0034]

[0035] are respectively the included angle between the approach vector and the axis of the coordinate system of the current path position point, and the included angle between the approach vector and the axis of the coordinate system of the current path position point.

[0036] . Specifically, according to the known robot path position points and the redundant attitude space, a redundant attitude space planning model is constructed. Given the redundant attitude space range, the planned path of the robot can be expressed as:

[0037] In the formula, represents the upper and lower limits of the redundant attitude space, is the known path position point, is the number of known robot path position points. The robot path attitude planning task can be expressed as planning an attitude within the redundant attitude space range for each known path position point under the conditions of the known path position points and generating a path with small robot joint displacements and no collisions:

[0038] In the formula is the path with small robot joint displacements and no collisions, is the attitude representation corresponding to . ​

[0039] Step 2: Based on the path position points in the planned path and the obstacle information in the robot workspace, construct an approximate critical collision plane, which is used to receive the projection of obstacles and transform the attitude planning in three-dimensional space into a two-dimensional plane. By using the critical collision plane, the three-dimensional attitude planning problem is transformed into a two-dimensional problem, which simplifies the calculation and ensures the effectiveness of obstacle constraints at the same time.

[0040] Please refer to Figure 3 , which is a schematic diagram for constructing an approximate critical collision plane. In this embodiment, an appropriate bounding box is selected for simplification according to the shape of the obstacle. Taking a spherical bounding box as an example, the horizontal plane where its center point is located represents the largest cross-section of the spherical bounding box in the vertical direction, and collisions mostly occur near this plane. Therefore, an approximate critical collision plane is constructed based on this central plane. Assume that the center coordinates of the spherical obstacle bounding box are , and the radius of the sphere is . The approximate critical collision plane established at the height of can be expressed as . On this plane, the projected coordinates of the obstacle center are , and the boundary of the obstacle can be expressed as a circle with as the center and as the radius.

[0041] If the coordinates of the current path position point are , its projected coordinates on the plane are ( . The intersection point of the approach vector and the approximate critical collision plane can be expressed as the redundant attitude point on , and can be calculated using the following formula:

[0042]

[0043]

[0044] In the formula, is the projection of the approach vector on the approximate critical collision plane, and is the height difference between the plane and the path position point.

[0045] Step 3: Based on the obstacle information and the redundant attitude space on the approximate critical collision plane, calculate the feasible attitude region .

[0046] Please refer to Figure 4 , which is the feasible region Schematic diagram. To avoid collisions between the end effector of the robot and obstacles, when calculating the feasible region, the approaching vector Redundant attitude points on the approximate critical collision plane is restricted outside the boundary of the obstacle bounding box. For a spherical bounding box, The attitude points within the range must be located outside the circular boundary. For convenience of calculation, The redundant attitude points in the plane are discretized into grid points, and the distances between each grid point within and the path position point and the center point of the obstacle are calculated as shown in the following formula:

[0047]

[0048] In the formula, is the distance between each grid point within and the path position point, is the distance between each grid point within and the center point of the obstacle.

[0049] The final plane The feasible attitude region on can be expressed as:

[0050]

[0051] In the formula, is the radius of the end effector, is the maximum length range of the projection of the approaching vector within the redundant attitude space onto the plane as shown in Figure 5 ; Figure 4 then is the feasible attitude region of the redundant attitude space represented by on the plane .

[0052] Step 4. To make the optimal attitude points corresponding to adjacent path position points smoother, according to the feasible attitude region , considering the influence of multiple factors such as the current path position point, the optimal attitude points of the previous path position point, the subsequent path position point, and the subsequent obstacles, higher weights are assigned to the attitude points near the above region within the feasible attitude region , and the central region of the feasible attitude region is estimated as the priority sampling attitude region.

[0053] By introducing a priority sampling pose region, the invalid sampling range in the redundant pose space is reduced, thereby improving the sampling efficiency and search speed.

[0054] When calculating the priority sampling pose region, the probability density is calculated by superimposing a weight function on the basis of Gaussian kernel density estimation of the probability density, and a partial region with the highest probability density is selected as the priority sampling pose region. Different weight functions are set according to the importance of different factors to adjust the influence of each pose point within, integrating information from multiple dimensions such as pose point continuity and obstacle avoidance requirements into a unified framework. Gaussian kernel density estimation is used to analyze the probability density of the feasible pose region to ensure the accuracy of the priority sampling region and improve the global optimality of the generated path.

[0055] Specifically, the calculation method of the priority sampling pose region is as follows: S41: For each pose point within the range of the feasible pose region , use the Gaussian kernel density function to estimate its probability density in the feasible pose region , and the calculation method is as follows:

[0056]

[0057] In the formula, is the redundant pose point after discretization in the feasible pose region , is the probability density of the pose point in the feasible pose region , is the sampling point in the feasible pose region , is the number of redundant pose points after discretization, is the normalized comprehensive weight, used to assign different weight influence factors to each pose point, and is adjusted according to specific requirements, is the Gaussian kernel function with a bandwidth of ; S42: Select the points in a partial region with the largest probability density value to form a set as the priority sampling pose region corresponding to the current path position point, and the point with the highest probability density in the priority sampling pose region is the optimal pose point.

[0058] The calculation method of the normalized comprehensive weight is:

[0059] Among them, The optimal attitude point corresponding to the path position point at the previous moment The influence on each attitude point in the feasible attitude region To make the distribution of weights at different sampling points more reasonable, the calculated weights need to be normalized. To maintain the continuity between the optimal attitude point (approximate) corresponding to the current path position point and the optimal attitude point (approximate) corresponding to the position point at the previous moment is the normalized , when calculating the probability density of , the influence of on each attitude point in needs to be considered ([[]] ), which is calculated through the Gaussian kernel function:

[0060]

[0061] In the formula, is an adjustable parameter used to control the influence degree of on ; are the normalized values of the weights of the path position point at the current moment , the future path position point , the future path position point , and the path position point closest to the next obstacle , respectively, which are calculated according to the following formula:

[0062] In the formula, is an adjustable parameter used to control the influence degree of the path position point on the set points in . The farther the future path position point is from the current path position point, the lower its influence should be, which can be achieved by increasing the value of . By adjusting the value of , the corresponding can be obtained.

[0063] Please refer to Figure 6 , which is a schematic diagram of the calculation of the path position point weight. When selecting the optimal attitude point (approximate), it is necessary to consider avoiding obstacles near the future path position point to prevent mutations of the attitude point. For this reason, the weights of different path position points are calculated respectively, that is, the value-taking rule of the adjustable parameter is as follows: For the normalized weight of the path position point at the current moment, ; For the future path position points of the normalized weight , ; For the future path position points of the normalized weight , ; For the path position point closest to the next obstacle of the normalized weight , .

[0064] After performing Gaussian kernel density estimation on each point in , select the points in a part of the region with the largest probability density value to form a set as the priority sampling pose region corresponding to the current path position point , The point with the highest probability density in is the optimal pose point

[0065] Step 5: Based on the guidance of the position space and the priority sampling pose region, continuously sample in the position space and the pose space to obtain pose nodes to expand the random search tree until the target node is reached, and obtain a target path. Adopt the random search tree expansion strategy for the position space and the target bias strategy for the redundant pose space to guide the pose nodes to quickly expand towards the target region, significantly reducing the time cost of path planning.

[0066] Please refer to Figure 7 for the node optimization method, that is, the method for obtaining the target path. The method for obtaining the target path is specifically as follows: S51: Randomly sample in the position space and the pose space to generate collision-free pose nodes as sampling nodes .

[0067] In this embodiment, the random sampling algorithm adopts the RRT* algorithm. The sampling method of the RRT* algorithm includes the following steps: S511: Perform initial preprocessing on the redundant pose space and the priority sampling pose region, combine the position space and the pose space, and establish an initial representation of the path to be optimized, that is, the path to be optimized for the robot end effector:

[0068] Among them, represents the priority sampling pose region corresponding to the path position point .

[0069] S512: Sample the discrete position space using the Gaussian sampling method to generate position nodes. At each iteration, guide the path to expand towards the end point by increasing the mean of the sampling distribution. As Figure 8 shown, it is a schematic diagram of the node sampling method of the above-mentioned attitude optimization model. In the position space, the exploration direction of the RRT* algorithm needs to start from the starting position and purposefully search towards the end position . Use the following formula for Gaussian sampling in the discrete position space to obtain position nodes :

[0070] In the formula, represents the sequence value of the position nodes obtained through Gaussian sampling. According to these sequence values, the corresponding position nodes can be found. The random variable is drawn from the standard normal distribution . The mean of the Gaussian distribution, representing the center of the distribution, is set here as the target position of the current random search tree, that is, the sequence value of the position nodes to be sampled. is the standard deviation of the Gaussian distribution, controlling the distribution degree of the sampling. The larger the standard deviation, the wider the distribution of the sampling results, and vice versa, the more concentrated.

[0071] At each iteration, increase the value to guide the expansion of the position nodes in the direction of the end point of the random search tree path position. When the RRT* algorithm calculates a new collision-free trajectory, will be reset to zero, and at the same time, increase the value to expand the high-probability sampling area within the position space.

[0072] S513: Under the constraints of the redundant attitude space and the priority sampling area corresponding to each position node , combine the target bias strategy to obtain attitude nodes. .

[0073] The above-mentioned target bias strategy is expressed as follows:

[0074] In the formula, is the target bias threshold, is randomly selected from the uniform distribution from 0 to 1, and the formula is expressed as follows: When , sample attitude nodes in the priority sampling attitude area through the function; When , sample attitude nodes through Function in the redundant pose space Randomly sample pose nodes; If the node sampled by the function collides, then use the function to resample in the redundant pose space again.

[0075] S514: Combine the pose node with the corresponding position node to generate a complete path node.

[0076] S52: Use the minimum joint displacement between two pose nodes of the robot as the cost function, and select the valid node closest to the sampled node in the random search tree.

[0077] Specifically, use the minimum joint displacement between two pose nodes of the robot as the cost function, and select the node closest to this point in the random search tree. Set to represent the currently sampled pose node, as the current position node, and its sequence value in the path position points is . To ensure the coherence of the path, start from the position sequence in the position space, backtrack to the path start point, and detect the position nodes to one by one to check whether there are expanded pose nodes in the pose space. Once a pose expansion node is detected, stop the backtracking process and set this node as the valid closest position point . Calculate the cost function between all the random search tree nodes under this position node and , and select the node with the minimum cost function as the closest node , and use as the temporary parent node to expand new nodes.

[0078] S53: Based on the sampled node and the closest valid node, expand nodes in the position space and the pose space respectively to generate new nodes. The new nodes include new position nodes and new pose nodes.

[0079] As Figure 8 shown, the method for generating the new position node is: starting from the position sequence where the closest position node is located, take one step forward along the end direction of the path to obtain a new position node, denoted as .

[0080] The method for generating the new pose node is: use the interpolation algorithm to expand nodes between the closest pose node and the sampled pose node in the pose space, and the new pose node​ It is expressed as follows:

[0081] Wherein, represents quaternion attitude spherical linear interpolation, is a random number selected at , indicating that the new attitude node is located between the nearest attitude node and the sampled attitude node ; Combining the new position node and the new attitude node, a temporary node is generated, and collision detection is performed. If the temporary node collides with the environment, new position nodes and new attitude nodes are re-obtained to generate a temporary node until the temporary node does not collide with the environment, and then it will be used as the final new node , and it is added to the random search tree.

[0082] S54: Trace back the position sequence of the new node in the path, select the low-cost nodes as the candidate range for the new connection method, perform cost evaluation and collision-free verification on the nodes within the candidate range, and obtain the collision-free and lowest-cost node as the final parent node. Specifically, in the position space, trace back n position points in the direction of the path starting point with the position sequence, and within the area of this position node, select the nodes with a cost function lower than the search threshold as the search range for readjusting the node connection method.

[0083] S55: Generate a path connection according to the new node and the final parent node. Specifically, for each node within the adjacent nodes , perform the following operations: S551: List as the parent node of , calculate the minimum path cost , is the initial node.

[0084] S552: If the minimum cost function is lower than the original cost and the path has no collision interference, then update the parent node of to .

[0085] S553: After determining the new parent node , establish a new connection starting from the root node . To meet the constraint of the position sequence for continuous motion, when are not adjacent in the position node sequence, it is necessary to establish the position and corresponding attitude interpolation points between them. The interpolation point can be calculated by the following formula:

[0086] Wherein, is The number of path position points to be interpolated between is determined by the position indices of the two. A linear interpolation algorithm is adopted to further reduce the path connection cost and improve the smoothness and operability of the planned path.

[0087] S56: Extend from the position sequence corresponding to the new node towards the path end point, screen candidate child nodes with low cost, adjust the parent node of the candidate child nodes to the new node, and update the path through cost evaluation and collision-free verification.

[0088] Specifically, pruning is performed on the random search tree after the position node space where is located. As shown in Figure 7 , starting from the position sequence of the node in the position space, advance n position points towards the position end point, and select a set of nodes within this position area whose cost function is lower than the search threshold as the search range for readjusting the node connection method . The pruning process is similar to the method of reselecting the parent node for , and the specific method is as follows: S561: For each node within , try to replace its original parent node with ; S562: Calculate the minimum cost function of ; S563: If the minimum cost of the new path is lower than the cost of the original path and there is no collision interference, determine the node with the minimum cost, and list the node as the parent node of this node. When the sequences of the position nodes are not adjacent to each other, pose interpolation is performed.

[0089] S57: When the expanded node is close to the target node, complete an attitude planning process to generate a target path.

[0090] Step Six, iterate the attitude node sampling and random search tree expansion operations to obtain more target paths, and select the target path with the lowest total cost among all the obtained target paths.

[0091] Embodiment Two This embodiment is a robot redundant attitude planning system based on position and obstacle constraints. The system is used to implement the robot redundant attitude planning method as described in Embodiment One, including: The path planning module generates a planned path for the robot based on the redundant pose space and the known path position points of the robot, and represents it using the position space and the pose space; The critical collision plane construction module constructs an approximate critical collision plane based on the path position points in the planned path and the obstacle information in the robot workspace, which is used to transform the three-dimensional pose planning into a two-dimensional plane and receive the projection of the obstacles; The feasible pose region calculation module calculates the feasible pose region based on the obstacle information on the approximate critical collision plane and the redundant pose space; The priority sampling region estimation module estimates the central region of the feasible pose region based on the feasible pose region, the current path position point, the optimal pose point of the previous path position point, the subsequent path position point and the subsequent obstacles, and uses it as the priority sampling pose region; The pose optimization module continuously samples in the position space and the pose space under the guidance of the position space and the priority sampling pose region to obtain pose nodes to expand the random search tree until the target node is reached, and a target path is obtained; iteratively perform pose node sampling and random search tree expansion operations to obtain more target paths, and select the target path with the lowest total cost among all the obtained target paths.

[0092] The above are only the preferred embodiments of the present invention. It should be pointed out that for those of ordinary skill in the art, without departing from the creative concept of the present invention, several modifications and improvements can still be made, and these all belong to the protection scope of the present invention.

Claims

1. A robot redundant posture planning method based on position and obstacle constraints, characterized in that: The following steps are involved: Generate a planned path for the robot based on the redundant posture space and known robot path position points, wherein the planned path is represented by the position space and the posture space; Based on the path position points in the planned path and the obstacle information in the robot workspace, an approximate key collision plane is constructed, where the approximate key collision plane is used to receive the projection of the obstacle and transform the posture planning in the three-dimensional space into a two-dimensional plane; Calculating a feasible posture area based on the obstacle information and redundant posture space on the approximate critical collision plane; According to the feasible posture area, the current path position point, the optimal posture point of the path position point at the previous moment, the subsequent path position point and the subsequent obstacles, the central area of ​​the feasible posture area is estimated as the priority sampling posture area; Based on the guidance of the position space and the priority sampling posture area, continuous sampling is performed in the position space and the posture space to obtain posture nodes to expand the random search tree until the target node is reached to obtain a target path; Iterate the pose node sampling and random search tree expansion operations to obtain more target paths, and select the target path with the lowest total cost among all the obtained target paths.

2. The robot redundant posture planning method based on position and obstacle constraints according to claim 1 is characterized in that: The calculation method of the priority sampling posture area is: For each posture point in the feasible posture area, the Gaussian kernel density function is used to estimate its probability density in the feasible posture area. The calculation method is as follows: In the formula, The feasible posture area The redundant posture points after discretization in For posture point In the feasible posture area The probability density in The feasible posture area The sampling points in is the number of redundant posture points after discretization, It is the normalized comprehensive weight, which is used to give each posture point a different weight influence factor and is adjusted according to specific needs. The bandwidth is Gaussian kernel function; A set of points in a part of the area with the largest probability density value is selected as the priority sampling posture area corresponding to the current path position point. The point with the highest probability density in the priority sampling posture area is the optimal posture point.

3. The robot redundant posture planning method based on position and obstacle constraints according to claim 2 is characterized in that: The normalized comprehensive weight calculation method is: in, is the optimal posture point corresponding to the path position point at the previous moment For the feasible posture area The influence of each posture point in After normalization , the calculation method is as follows: In the formula, is an adjustable parameter used to control right the extent of the impact; They are the path location points at the current moment. 、Future path location point 、Future path location point , the closest path position to the next obstacle For the feasible posture area The normalized value of the influence weight of each posture point in is calculated according to the following formula: In the formula, is an adjustable parameter used to control the path location point right The influence of the central point is adjusted by Get the corresponding value of .

4. The robot redundant posture planning method based on position and obstacle constraints according to claim 3 is characterized in that: The adjustable parameters The value rules are as follows: For the current path location point The normalized weight of , ; For future path locations The normalized weight of , ; For future path locations The normalized weight of , ; For the path point closest to the next obstacle The normalized weight of , .

5. The robot redundant posture planning method based on position and obstacle constraints according to claim 1 is characterized in that: The method for obtaining the target path is specifically as follows: Randomly sample in the position space and attitude space to generate collision-free attitude nodes as sampling nodes; Taking the minimum joint displacement between two pose nodes of the robot as the cost function, select the valid node closest to the sampled node in the random search tree; Based on the sampled nodes and the nearest valid nodes, node expansion is performed in the position space and attitude space to generate new nodes; Backtrack the position sequence of the new node in the path, select low-cost nodes as the new connection method candidate range, evaluate the cost and verify the collision-free nodes in the candidate range, and obtain the collision-free and lowest-cost node as the final parent node; Generate path connections based on the new node and the final parent node; Extend from the position sequence corresponding to the new node toward the end of the path, screen low-cost candidate child nodes, adjust the parent node of the candidate child node to the new node, and update the path through cost evaluation and collision-free verification; When the extended node approaches the target node, a posture planning process is completed and a target path is generated.

6. The robot redundant posture planning method based on position and obstacle constraints according to claim 5 is characterized in that: The random sampling algorithm adopts the RRT* algorithm.

7. The robot redundant posture planning method based on position and obstacle constraints according to claim 6 is characterized in that: The sampling method of the RRT* algorithm comprises the following steps: Pre-process the redundant posture space and the priority sampling posture area, combine the position space and posture space, and establish the initial representation of the path to be optimized; The discrete position space is sampled using Gaussian sampling method to generate position nodes. In each iteration, the path is guided to expand toward the end point by increasing the mean of the sampling distribution. Under the constraints of the redundant posture space and the priority sampling area corresponding to each position node, the posture node is obtained by combining the target bias strategy; The posture node is combined with the corresponding position node to generate a complete path node.

8. The robot redundant posture planning method based on position and obstacle constraints according to claim 7 is characterized in that: The target bias strategy is expressed as follows: In the formula, is the target bias threshold, Randomly select from a uniform distribution between 0 and 1, the formula is as follows: when When, through Function in the priority sampling posture area The sampling posture node; when When, through Function in redundant pose space Randomly sample pose nodes; If passed If the nodes sampled by the function collide, use Function in redundant pose space Resample in .

9. The robot redundant posture planning method based on position and obstacle constraints according to claim 5, characterized in that: The node expansion is performed in the position space and the posture space respectively to generate new nodes, wherein the new nodes include new position nodes and new posture nodes; The new location node generation method is: starting from the location sequence where the nearest location node is located, moving one step along the end direction of the path to obtain a new location node; The new posture node generation method is: using an interpolation algorithm to perform node expansion between the nearest posture node and the sampled posture node in the posture space, and the new posture node is represented as follows: In the formula, Represents the spherical linear difference of the quaternion attitude, For The selected random number indicates that the new posture node is located at the nearest posture node With the sampling pose node between; Combine the new position node and the new posture node to generate a temporary node and perform collision detection. If the temporary node collides with the environment, re-acquire the new position node and the new posture node to generate a temporary node. If the temporary node does not collide with the environment, it will be used as the final new node.

10. A robot redundant posture planning system based on position and obstacle constraints, characterized in that: The system is used to implement the method according to any one of claims 1 to 9, comprising: The path planning module generates the robot's planned path based on the redundant posture space and the known robot path position points, and uses the position space and posture space to represent it; The critical collision plane construction module constructs an approximate critical collision plane based on the path position points in the planned path and the obstacle information in the robot workspace, which is used to convert the three-dimensional posture planning into a two-dimensional plane and receive the projection of obstacles; A feasible posture area calculation module, which calculates the feasible posture area based on the obstacle information and redundant posture space on the approximate key collision plane; The priority sampling area estimation module estimates the central area of ​​the feasible posture area as the priority sampling posture area according to the feasible posture area, the current path position point, the optimal posture point of the path position point at the previous moment, the subsequent path position point and the subsequent obstacles; The posture optimization module, based on the guidance of the position space and the priority sampling posture area, continuously samples in the position space and posture space, obtains posture nodes to expand the random search tree until reaching the target node and obtaining a target path; it iterates the posture node sampling and random search tree expansion operations to obtain more target paths, and selects the target path with the lowest total cost among all the obtained target paths.

Images