A segmented dynamic obstacle avoidance method for a super redundant robotic arm
Through the segmented dynamic obstacle avoidance method, a dynamic obstacle warning area and collision risk assessment model is established, obstacle avoidance areas are divided and collision avoidance joint groups are allocated, which solves the problem of low dynamic obstacle avoidance efficiency of traditional robotic arms under complex working conditions, and realizes the realization of The real-time obstacle avoidance capability of super redundant robotic arms in multiple obstacle scenarios.
Patent Information
- Application Number
- CN202211381036.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-11-05
- Publication Date
- 2024-11-26
- Estimated Expiration
- 2042-11-05
AI Technical Summary
Traditional robotic arms are difficult to meet the dynamic obstacle avoidance needs of super redundant robotic arms under complex working conditions. The existing obstacle avoidance algorithms have problems such as low planning decision-making efficiency and slow response to the environment. It is especially difficult to achieve real-time in multiple obstacle scenarios. Avoid obstacles.
The dynamic obstacle avoidance method is adopted to realize the dynamic arm dynamic by establishing a dynamic obstacle warning area, building a collision risk assessment model, dividing obstacle avoidance areas, assigning collision avoidance joint groups and updating the position of the robot arm through iterative inverse kinematics algorithms. Avoid obstacles.
It effectively improves the obstacle avoidance ability of the super redundant robot arm in dynamic obstacle scenarios, realizes real-time obstacle avoidance and adaptability to multiple obstacles, and improves the dynamic obstacle avoidance efficiency of the robot arm.
Smart Images

Figure BDA0003928304850000036 
Figure BDA0003928304850000058 
Figure BDA0003928304850000063
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of mechanical arms, and in particular to a super-redundant mechanical arm segmented dynamic obstacle avoidance method. Background Art
[0002] Facing robot operations in confined spaces, dangerous environments and complex tasks, traditional industrial robotic arms are difficult to meet the application requirements under complex working conditions due to the limitations of size, drive configuration and degree of freedom. The super-redundant robotic arm, relying on the bionic snake mechanism, integrates the structure and movement characteristics of the snake spine, has the characteristics of large aspect ratio, high redundancy, strong flexibility, wide reach, and superior continuous bending and obstacle avoidance capabilities. It can play an irreplaceable role in deep cavity detection operations of large-scale precision equipment such as aerospace, nuclear power, etc.
[0003] The flexible control of the super-redundant manipulator requires a more complete and efficient motion and obstacle avoidance strategy. However, the redundant degrees of freedom make the control difficulty of the manipulator increase exponentially. In addition, the obstacle avoidance algorithm of the traditional manipulator has defects such as low planning and decision-making efficiency and slow dynamic response to the environment, which makes it difficult to directly apply it to the super-redundant manipulator. In addition, the coordination relationship between the joints of the manipulator is strongly coupled. The change of a single link will cause the posture adjustment of the remaining multiple links, which greatly restricts the obstacle avoidance ability of the super-redundant snake-like manipulator.
[0004] In recent years, a series of obstacle avoidance methods for super-redundant manipulators have been proposed, which can be mainly divided into the following categories: ① The method based on artificial potential field method, by introducing the target gravitational field and the repulsive field generated by the obstacle, drives the manipulator to avoid obstacles, but it is easy to fall into a local force balance state during calculation, especially when multiple obstacles act at the same time, the algorithm will fail; ② The method based on C space planning, first establishes all feasible C spaces of the manipulator according to the state characteristics of the manipulator, then maps the obstacles into the C space, and further plans the collision-free trajectory of the C space, but with the increase of the redundancy of the manipulator, the solution efficiency of this method is low, and it is difficult to achieve real-time obstacle avoidance requirements; ③ The method based on end-following, plans a collision-free trajectory according to the established scene, and avoids obstacles by taking the initiative at the end and the remaining joint nodes being driven, but this method is mostly applied to known static obstacles and lacks the ability to adapt to dynamic obstacles. The above obstacle avoidance strategies cannot effectively solve the dynamic obstacle avoidance problem of super-redundant manipulators while ensuring the solution efficiency.
[0005] Based on this, the present invention provides a super-redundant robotic arm segmented dynamic obstacle avoidance method. Summary of the invention
[0006] The following is a general summary of one or more aspects to provide a basic understanding of these aspects. This summary cannot describe all concepts in detail, and its only purpose is to present some concepts of one or more aspects in a simplified form as a prelude to the more detailed description that will be given later.
[0007] The present invention provides a super redundant mechanical arm segmented dynamic obstacle avoidance method, comprising the following steps:
[0008] Step 1: Based on the robot arm status information and observed obstacle information in the scene, a dynamic obstacle warning area centered on the robot arm is established.
[0009] Step 1.1: Obtain the state information of the robot arm from the 3D capture system and the gyroscope inside the robot arm joint: geometric configuration, robot arm posture, and the speed v of the i-th robot arm link Li , and obstacle information: geometric configuration, obstacle current speed v o , where i=1, 2, …, n, and n represents the number of links of the super-redundant robot arm.
[0010] Step 1.2: Use a closed ellipsoid to cover each link of the robot arm. The closed ellipsoid covering the i-th link of the robot arm is According to the geometric configuration of the obstacle, the obstacle is enclosed by a closed convex geometric body. The closed convex geometric body enclosing the obstacle is denoted as
[0011] Step 1.3: Simplify the connecting rods and construct the closed Minkowski sum between each connecting rod and the obstacle with the obstacle mass center as the center, which is recorded as:
[0012]
[0013] In the formula, use represents the Minkowski sum operation, Indicates taking For any point x in the region, Indicates taking Any point y in the region.
[0014] Step 1.4: Compute the closed-form Minkowski sum Minimum Euclidean distance d to the center of the corresponding connecting rod i , let the length of the robot arm connecting rod be L a , the dynamic obstacle warning distance is set as d with the connecting rod as the center warn =n*L a , when d i <d warn When the obstacle is detected, it is considered to be a potential risk.
[0015] Step 2: When an obstacle enters the monitoring range of the area, a collision risk assessment model is constructed based on the speed obstacle strategy to evaluate the possibility of collision between the robotic arm and the obstacle in real time.
[0016] Step 2.1: Convert the closed Minkowski and Projection along the base coordinate system XOY and XOZ planes is and The spatial coordinate of the i-th connecting rod is P Li , the relative speed between the i-th connecting rod and the obstacle is v rel =v Li -v o , P Li With v rel Projection along the XOY and XOZ planes of the base coordinate system is xoy P Li , xoz P Li and xoy v rel , xoz v rel .
[0017] Step 2.2: In the XOY plane, xoy P Li As the observation center, Draw tangents on both sides to form the obstacle cone area, which is recorded as xoy D, in the XOZ plane, xoz P Li As the observation center, Draw tangents on both sides to form the obstacle cone area, which is recorded as xoz D, then the speed obstacle area and It can be expressed as:
[0018]
[0019]
[0020] In the formula, τ is the time window.
[0021] Step 2.3: When the relative velocity vector xoy v rel Falling Within the region or xoz v rel Falling When inside the region, it is considered and There is a collision risk within the time range [0, τ], and the collision risk assessment model is constructed as follows:
[0022]
[0023] In the formula, ∩ means taking the intersection, and ∪ means taking the union.
[0024] Step 3: If there is a collision risk, divide the robot arm into several obstacle avoidance areas based on the obstacle information, construct local obstacle avoidance constraints, joint limit constraints and link length constraints, and solve the safe speed set of each link of the robot arm.
[0025] Step 3.1: When there is a risk of collision, Minimum distance d to the center of the corresponding connecting rod i Dynamically assign collision avoidance joint groups for hyper-redundant manipulators.
[0026] Step 3.2: Let the position of the i-th joint of the super redundant manipulator be P Ji , construct the link length constraint as:
[0027] ||P Ji -P Ji-1 ||2=L a ;
[0028] In the formula, ||…||2 represents the calculation of the vector's bi-norm.
[0029] Step 3.3: Let the super redundant robot arm joint limit angle be θ max , then P Ji is the vertex, L a is the generatrix, vector P Ji -P Ji-1 is the axis, 2θ max Construct a spherical cone area D for the cone angle joint , construct the joint limit constraint as:
[0030] P Ji+1 ∈D joint .
[0031] Step 3.4: Remember u i v rel To the speed barrier The vector of the minimum distance to the boundary is recorded as:
[0032] u i =arg min||vv rel ||2-v rel ;
[0033] Remember i for u i The unit direction vector is: Then the optimal collision avoidance plane (OCA) of link i can be expressed as:
[0034]
[0035] Let the speed of the connecting rod at the next moment be v Li_new , then the local obstacle avoidance speed constraint can be expressed as:
[0036]
[0037] Step 3.5: When updating the robot arm link speed, it is necessary to satisfy the local obstacle avoidance speed constraint, joint limit constraint and link length constraint at the same time, and the updated speed should be as close as possible to the original link speed v Li , then the joint obstacle avoidance problem can be described as:
[0038] v Li_new =arg min||vv Li ||2
[0039]
[0040] Step 3.6: Solving the function in step 3.5 can obtain the safe speed of the robot arm link at the next moment.
[0041] Step 4: Update the robot arm posture in real time through the forward and backward iterative inverse kinematics solution algorithm, thereby realizing segmented obstacle avoidance of the super-redundant robot arm in dynamic obstacle scenarios.
[0042] Step 4.1: For the assigned super-redundant robot arm collision avoidance joint group, prioritize updating the link velocity closest to the obstacle, and update the positions of the remaining joint nodes through the forward and backward iterative method.
[0043] Step 4.2: Convert the desired posture of the robot arm into joint angle input, and then update the robot arm posture and obstacle status.
[0044] Step 4.3: Repeat the above steps until all robot arm links are free from collision risks, thus achieving segmented dynamic obstacle avoidance of the super-redundant robot arm. BRIEF DESCRIPTION OF THE DRAWINGS
[0045] The drawings in the specification, which constitute a part of this application, are used to provide further understanding of this application. The schematic implementation of this application and its description are used to explain this application and do not constitute improper limitation of this application.
[0046] Figure 1 It is a flow chart of the super redundant manipulator segmented dynamic obstacle avoidance method in the present invention.
[0047] Figure 2 It is a schematic diagram of establishing a dynamic obstacle warning area in the present invention.
[0048] Figure 3 It is a schematic diagram of the position relationship between the spatial connecting rod and the obstacle in the present invention.
[0049] Figure 4 It is a schematic diagram of constructing the speed obstacle area of the xoy projection plane in the present invention.
[0050] Figure 5 It is a schematic diagram of the dynamic allocation of collision avoidance joint groups of the robot arm in the present invention.
[0051] Figure 6 It is a schematic diagram of dynamically updating the joint positions of the mechanical arm in the present invention.
[0052] Figure 7 It is an implementation effect diagram of segmented dynamic obstacle avoidance in the first simulation experiment of the present invention.
[0053] Figure 8 It is an implementation effect diagram of the segmented dynamic obstacle avoidance in the second simulation experiment of the present invention. DETAILED DESCRIPTION
[0054] In order to better understand the technical solution of the present invention, the following further introduces the implementation of the present invention in combination with the drawings and specific examples. Note that the aspects described below in combination with the drawings and specific embodiments are only exemplary and should not be understood as limiting the scope of protection of the present invention.
[0055] A flow chart of a super redundant robotic arm segmented dynamic obstacle avoidance method is as follows: Figure 1 As shown, the details are as follows:
[0056] Step 1: Based on the robot arm status information and the observed obstacle information in the scene, a dynamic obstacle warning area centered on the robot arm is established, such as Figure 2 shown.
[0057] First, the state information of the robot arm is obtained from the 3D capture system and the gyroscope inside the robot arm joint: geometric configuration, robot arm posture, and the speed v of the i-th robot arm link Li , and obstacle information: geometric configuration, obstacle current speed v o , where i = 1, 2, ..., n, n represents the number of super redundant robot arm links; each robot arm link is wrapped with a closed ellipsoid, and the closed ellipsoid covering the i-th robot arm link is denoted as According to the geometric configuration of the obstacle, a closed convex geometric body is used to enclose the obstacle, and the closed convex geometric body enclosing the obstacle is denoted as O. Secondly, the connecting rod is simplified, and the closed Minkowski sum between each connecting rod and the obstacle is constructed with the obstacle mass center as the center, which is denoted as:
[0058]
[0059] In the formula, use represents the Minkowski sum operation, Indicates taking For any point x in the region, Indicates taking Any point y in the region.
[0060] Finally, the closed-form Minkowski sum is calculated The minimum Euclidean distance di to the center of the corresponding connecting rod, and the length of the robot connecting rod is L a , the dynamic obstacle warning distance is set as d with the connecting rod as the center warn =n*L a , when d i <d warn When the obstacle is detected, it is considered to be a potential risk.
[0061] Step 2: When an obstacle enters the monitoring range of the area, a collision risk assessment model is constructed based on the speed obstacle strategy to evaluate the possibility of collision between the robot arm and the obstacle in real time, such as Figure 3 shown.
[0062] It is difficult to construct the speed barrier area directly with irregular geometric shapes, so the closed Minkowski and Projection along the base coordinate system XOY and XOZ planes is and The spatial coordinate of the i-th connecting rod is P Li , the relative speed between the i-th connecting rod and the obstacle is v rel =v Li -v o , P Li With v rel Projection along the XOY and XOZ planes of the base coordinate system is xoy P Li , xoz P Li and xoy v rel , xoz v rel .
[0063] like Figure 4 As shown, in the XOY plane, xoy P Li As the observation center, Draw tangents on both sides to form the obstacle cone area, which is recorded as xoy D, in the XOZ plane, xoz P Li As the observation center, Draw tangents on both sides to form the obstacle cone area, which is recorded as xoz D, then the speed obstacle area and It can be expressed as:
[0064]
[0065]
[0066] In the formula, τ is the time window.
[0067] Step 2.3: When the relative velocity vector xoy v rel Falling Within the region or xoz v rel Falling When inside the region, it is considered and There is a collision risk within the time range [0, τ], and the collision risk assessment model is constructed as follows:
[0068]
[0069] In the formula, ∩ means taking the intersection, Represents a set union.
[0070] Step 3: If there is a risk of collision, divide the robot arm into several obstacle avoidance areas based on the obstacle information
[0071] ,like Figure 5 As shown, according to the obstacle size and Minimum distance d to the center of the corresponding connecting rod i Dynamically assign collision avoidance joint groups for hyper-redundant manipulators.
[0072] The position of the i-th joint of the super redundant manipulator is P Ji , construct the link length constraint as:
[0073] ||P Ji -P Ji-1 ||2=L a ;
[0074] In the formula, ||…||2 represents the calculation of the vector's bi-norm
[0075] The joint limit angle of the super redundant manipulator is θ max , then P Ji is the vertex, L a is the generatrix, vector P Ji -P Ji-1 is the axis, 2θ max Construct a spherical cone area D for the cone angle joint , construct the joint limit constraint as:
[0076] P Ji+1 ∈D joint .
[0077] Define u i v rel To the speed barrier The vector of the minimum distance to the boundary is recorded as:
[0078] u i =argmin||vv rel ||2-v rel ;
[0079] Remember i for u i The unit direction vector is: Then the optimal collision avoidance plane (OCA) of link i can be expressed as:
[0080]
[0081] Let the speed of the connecting rod at the next moment be v Linew , then the local obstacle avoidance speed constraint can be expressed as:
[0082]
[0083] When updating the robot arm link speed, the local obstacle avoidance speed constraint, joint limit constraint and link length constraint need to be satisfied at the same time, and the updated speed should be as close to the original link speed v as possible. Li , then the joint obstacle avoidance problem can be described as:
[0084] v Li_new =arg min||vv Li ||2
[0085]
[0086] Step 4: Figure 6 As shown, for the assigned super-redundant robot arm collision avoidance joint group, the link speed closest to the obstacle is updated first, and the positions of the remaining joint nodes are updated through the forward and backward iterative method, thereby updating the robot arm posture and obstacle status.
[0087] Repeat the above steps until all robot arm links are free from collision risks, thus achieving segmented dynamic obstacle avoidance of the super-redundant robot arm.
[0088] In order to further illustrate the present invention, relevant experiments were designed and the algorithm integration and simulation verification of the proposed super-redundant robotic arm segmented dynamic obstacle avoidance method were carried out in Matlab R2020a.
[0089] Experiment 1: Super redundant manipulator segmented obstacle avoidance experiment under static obstacle conditions. Figure 7As shown in the figure, the experimental object is a 16-joint super redundant manipulator, which consists of an arm body and a base. The manipulator connecting rod is of equal length and is set to 128.5mm. There are two static obstacles in the scene, both of which are covered by a spherical area with a radius of 80mm. The coordinates of position one are (1600, 100, 400) and the coordinates of position two are (1400, -50, 200). The manipulator will move along the pre-planned terminal trajectory. If the obstacle avoidance movement is not performed, the manipulator will collide with the obstacle. The results show that through the super redundant manipulator segmented dynamic obstacle avoidance method proposed by the present invention, the manipulator can realize tracking the terminal trajectory point while changing its posture in time at obstacle one and obstacle two, effectively avoiding obstacles.
[0090] Experiment 2: Super redundant manipulator segmented obstacle avoidance experiment under dynamic obstacle conditions. Figure 8 As shown in the figure, the experimental object is a 16-joint super redundant manipulator, which consists of an arm body and a base. The manipulator connecting rod is of equal length and is set to 128.5mm. There are two dynamic obstacles in the scene, both of which are covered by a spherical area with a radius of 80mm. The initial position coordinates are (1400, -400, 400), the velocity vector is (0, 5, 0), and the initial position coordinates are (800, 400, 50), and the velocity vector is (0, -5, 0). The manipulator keeps the end fixed, and the obstacle moves toward the manipulator from both sides at the same time. If the obstacle is not avoided, the manipulator will collide with the obstacle. The results show that through the super redundant manipulator segmented dynamic obstacle avoidance method proposed by the present invention, the manipulator can dynamically allocate collision avoidance joint groups for multiple dynamic obstacles and adopt obstacle avoidance strategies at the same time when the end position is fixed, so as to effectively avoid obstacles.
[0091] In order to simplify the explanation of the present method, the above-mentioned diagrams and texts are described as a series of steps, but it should be understood and appreciated that these methods are not limited by the order of operations. Because they are implemented in one or more steps, some actions may occur in different orders, but those skilled in the art can understand the principles of their actions.
[0092] Although the illustrative specific embodiments of the present invention have been gradually described so that those skilled in the art can understand them, the present invention is not limited to the scope of the specific embodiments. Those skilled in the art can make various changes or modifications within the scope of the claims, as long as the various changes are within the spirit and scope of the present invention as defined and determined by the appended claims.
Claims
1. A super redundant robotic arm segmented dynamic obstacle avoidance method, It is characterized in that include: Step 1: Based on the robot arm status information and the observed obstacle information in the scene, a dynamic obstacle warning area centered on the robot arm is established; Step 2: When an obstacle enters the monitoring range of the area, a collision risk assessment model is constructed based on the speed obstacle strategy to evaluate the possibility of collision between the robot arm and the obstacle in real time; Step 3: If there is a collision risk, divide the robot arm into several obstacle avoidance areas according to the obstacle information, construct local obstacle avoidance constraints, joint limit constraints and link length constraints, and solve the safe speed set of each joint of the robot arm; Step 4: Update the robot arm posture in real time through the forward and backward iterative inverse kinematics solution algorithm, thereby realizing the super-redundant robot arm segmented obstacle avoidance in dynamic obstacle scenarios; In step 1, the following steps are specifically included: Step 1.1: Obtain the state information of the robot arm from the 3D capture system and the gyroscope inside the robot arm joint: geometric configuration, robot arm posture, and the speed v of the i-th robot arm link Li , and obstacle information: geometric configuration, obstacle current speed v o , where i = 1, 2, ..., n, and n represents the number of super-redundant robot arm links; Step 1.2: Use a closed ellipsoid to cover each link of the robot arm. The closed ellipsoid covering the i-th link of the robot arm is According to the geometric configuration of the obstacle, the obstacle is enclosed by a closed convex geometric body. The closed convex geometric body enclosing the obstacle is denoted as Step 1.3: Simplify the connecting rods and construct the closed Minkowski sum between each connecting rod and the obstacle with the obstacle mass center as the center, which is recorded as: In the formula, use represents the Minkowski sum operation, Indicates taking For any point x in the region, Indicates taking Any point y in the region; Step 1.4: Compute the closed-form Minkowski sum Minimum Euclidean distance d to the center of the corresponding connecting rod i , let the length of the robot arm connecting rod be L a , the dynamic obstacle warning distance is set as d with the connecting rod as the center warn =n*L a , when d i <d warn When the obstacle is detected, it is considered to be a potential risk.
2. A super redundant robotic arm segmented dynamic obstacle avoidance method according to claim 1, Features: In step 2, the following steps are specifically included: Step 2.1: Convert the closed Minkowski and Projection along the base coordinate system XOY and XOZ planes is and The spatial coordinate of the i-th connecting rod is P Li , the relative speed between the i-th connecting rod and the obstacle is v rel =v Li -v o , P Li With v rel Projection along the XOY and XOZ planes of the base coordinate system is xoy P Li , xoz P Li and xoy v rel , xoz v rel ; Step 2.2: In the XOY plane, xoy P Li As the observation center, Draw tangents on both sides to form the obstacle cone area, which is recorded as xoy D, in the XOZ plane, xoz P Li As the observation center, Draw tangents on both sides to form the obstacle cone area, which is recorded as xoz D, then the speed obstacle area and It is expressed as: In the formula, τ is the time window; Step 2.3: When the relative velocity vector xoy v rel Falling Within the region or xoz v rel Falling When inside the region, it is considered and There is a collision risk within the time range [0,τ], and the collision risk assessment model is constructed as follows: In the formula, ∩ means taking the intersection, and ∪ means taking the union.
3. A super redundant robotic arm segmented dynamic obstacle avoidance method according to claim 2, Features: In step 3, the following steps are specifically included: Step 3.1: When there is a risk of collision, Minimum distance d to the center of the corresponding connecting rod i Dynamically assign collision avoidance joint groups to super redundant manipulators; Step 3.2, record the position of the i-th joint of the super redundant manipulator as P Ji , construct the link length constraint as: ||P Ji -P Ji-1 || 2 =L a ; In the formula, ||…|| 2 Represents the calculation of the second norm of a vector; Step 3.3, record the super redundant manipulator joint limit angle as θ max , then P Ji is the vertex, L a is the generatrix, vector P Ji -P Ji-1 is the axis, 2θ max Construct a spherical cone area D for the cone angle joint , construct the joint limit constraint as: P Ji+1 ∈D joint ; Step 3.4, record u i v rel To the speed barrier The vector of the minimum distance to the boundary is recorded as: u i =argmin||vv rel || 2 -v rel ; Remember i for u i The unit direction vector is: Then the optimal collision avoidance plane OCA of link i is expressed as: Let the speed of the connecting rod at the next moment be v Li_new , then the local obstacle avoidance speed constraint is expressed as: Step 3.5: When updating the robot arm link speed, the local obstacle avoidance speed constraint, joint limit constraint and link length constraint need to be satisfied at the same time, and the updated speed should be as close as possible to the original link speed v Li , then the joint obstacle avoidance problem is described as: In the formula, v Li_new Indicates the updated link speed of the robot arm section i; Step 3.6, solve the function in step 3.5 to obtain the safe speed of the robot arm link at the next moment.
4. A super redundant manipulator segmented dynamic obstacle avoidance method according to claim 1, Features: In step 4, the following steps are specifically included: Step 4.1: For the assigned super-redundant robot arm collision avoidance joint group, the link speed closest to the obstacle is updated first, and the positions of the remaining joint nodes are updated through the forward and backward iterative method; Step 4.2, convert the desired posture of the robot arm into joint angle input, and then update the robot arm posture and obstacle status; Step 4.3, repeat the above steps until all robot arm links are free from collision risks, thus achieving segmented dynamic obstacle avoidance of the super-redundant robot arm.
