FABRIK-based rigid-flexible combined mechanical arm motion planning method and system and computer storage medium
Through the FABRIK-based combined robot arm motion planning method, the inverse kinematics problem of rigid-flexible tandem robot arm is simplified, the problems of large calculation volume and limited applicability of traditional methods are solved, and effective support for internal detection of nuclear reactors is achieved.
Patent Information
- Application Number
- CN202411512911.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-10-28
- Publication Date
- 2024-12-06
- Estimated Expiration
- Not applicable · inactive patent
AI Technical Summary
Traditional methods have large calculations and limited applicability when dealing with the inverse kinematics of rigid and flexible tandem robotic arms, making it difficult to effectively solve the needs of narrow spaces such as internal detection of nuclear reactors.
The rigid-flexible combined robotic arm motion planning method based on FABRIK is adopted to simplify the robotic arm by equivalently simplifying iteratively solving using the FABRIK algorithm, combining joint limit avoidance and obstacle avoidance to solve the inverse kinematic parameters.
The calculation difficulty in traditional methods is simplified, so that the rigid-flexible tandem robot arm can more effectively approach the target position and attitude, meeting the needs of internal detection of nuclear reactors.
Smart Images

Figure CN119077746A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of robot control, and in particular to a FABRIK-based rigid-flexible combined robot arm motion planning method and system, and a computer storage medium. Background Art
[0002] In recent years, the widespread use of robots has gradually replaced human labor in various jobs. However, traditional robotic arms face the problem of being too large in size when performing tasks such as internal inspection of nuclear power plants, pipeline maintenance, post-earthquake rescue missions, and medical surgery in small spaces, which hinders their ability to perform these tasks normally. This flexible mechanism has excellent bending properties and can flexibly change its shape. It has strong applicability in narrow spaces. Many research institutions have carried out research and clinical applications of flexible mechanisms. Bryan et al. designed a robot that connects the shape of the robot with tendons through a combined drive of cables and air pressure. Buckingham et al. designed a snake-like robot for internal pipeline inspection and maintenance of nuclear reactors. He et al. designed a snake-like robot with a cage-like structure. However, it is still difficult for robotic arms to inspect inside nuclear reactors. In order to inspect such a small space inside a nuclear reactor, a rigid-flexible serial robotic arm was designed. Inverse kinematics has always been a hot topic and difficulty in robotics research. At present, the general solution of the inverse solution of the kinematics of rigid mechanisms is based on solving the generalized Jacobian inverse solution. Yu et al. proposed a method for solving the inverse kinematics of a robot-manipulator based on quaternion motion equations. The constructed kinematic control theory was used to solve the inverse kinematics problem of the manipulator. Vorobiev proposed a method for constructing the kinematics of a dual-arm robot. This method not only solves the forward and inverse problems of the manipulator kinematics, but also considers additional conditions to eliminate the dangerous approach of the connecting rod. However, for flexible mechanisms with a large number of degrees of freedom, traditional methods have disadvantages such as large computational complexity and limited applicability. The principal curve method can effectively solve these problems. Chikjian proposed a mode analysis method for spine curves in the 1990s, using spine curves to summarize the overall geometric posture of the robot. This method makes the robot joints fit the spine curve as closely as possible. Fahimi introduced the concept of backbone curves and an effective fitting method to control the trajectory of the manipulator. Li et al. proposed a uniform bending scheme for a multi-segment cable-driven flexible mechanism, converting the inverse kinematic problem of multiple segments into a single-segment problem. Hu proposed a geometric method for simultaneously planning the end effector posture and manipulator structure. The synchronous planning of the posture configuration is achieved by modeling and solving the single-arc or double-arc joint angles in space. Mu proposed a dual arc method to simultaneously solve the end position, end orientation, and task configuration of a concentric linear drive manipulator. However, few studies have been conducted on the inverse kinematics of rigid-flexible serial manipulators. A kinematics solution method for rigid-flexible serial manipulators based on FABRIK is proposed. Summary of the invention
[0003] The present invention aims to solve at least one of the technical problems in the related art to a certain extent. To this end, one object of the present invention is a method and system for motion planning of a rigid-flexible combined manipulator based on FABRIK, and a computer storage medium.
[0004] The technical solution adopted by the present invention is to solve the motion planning method of the rigid-flexible combined manipulator based on FABRIK, which includes the following steps:
[0005] The rigid-flexible serial robot arm is simplified equivalently to obtain an equivalent simplified arm;
[0006] Use the FABRIK algorithm to iterate forward and backward on the equivalent simplified arm, and then iterate as many times as needed until the difference between the current and desired end position and posture converges sufficiently;
[0007] During the FABRIK iteration process, the joint limit avoidance and obstacle avoidance of the entire equivalent simplified arm are completed;
[0008] The equivalent simplified arm is fitted with the rigid-flexible serial robot arm, and the kinematic parameters of the rigid-flexible serial robot arm are solved.
[0009] Furthermore, according to the structural characteristics of the seven-DOF redundant robotic arm in the form of SRS adopted by the rigid mechanism, the first three joints are regarded as shoulder joints, the last three joints are regarded as wrist joints, and the fourth joint is regarded as elbow joints. The flexible mechanism part adopts a two-segment continuum robotic arm, which is equivalent to two arcs. The tangents on both sides of the arc coincide with the two segments of the flexible mechanism and intersect at nodes. These nodes can be connected to the shoulder, elbow, and wrist and are regarded as equivalent simplified arms.
[0010] Furthermore, before executing the FABRIK algorithm, it is necessary to calculate the total length of the equivalent robot arm and the distance between the base and the target to determine whether the target is within the reachable range. When the target is within the reachable range, forward iteration begins;
[0011] Execute the FABRIK iterative procedure. The algorithm moves from the end to the base to calculate the new coordinate position of each joint, then moves from the root joint to the end to ensure that the first joint is still in the original coordinate, and then iterates as many times as needed until the difference between the current and desired end position and posture converges sufficiently.
[0012] Furthermore, for each simplified arm, the joint angle avoidance limit can be confirmed based on the feasible domain of the equivalent simplified arm to update the previous joint. The forward and reverse approach iterations simultaneously avoid the angle limit of the joint segment and ultimately achieve the angle avoidance limit of the entire equivalent simplified arm.
[0013] When the Euclidean distance between the obstacle and the equivalent link is less than the safe distance, the obstacle is avoided. The obstacle should be avoided by making the equivalent simplified arm as far away as possible. Therefore, the new position of the equivalent simplified arm is on the line formed by the plane formed by the link to be adjusted and the previous link and the boundary position of the safety zone.
[0014] Another technical solution adopted by the present invention is: a motion planning method and system for a rigid-flexible combined manipulator based on FABRIK, comprising:
[0015] Equivalent simplified robotic arm unit: used to perform equivalent simplification on complex robotic arms;
[0016] FABRIK iteration unit: used to iterate the algorithm so that the difference between the current and expected end position and posture is sufficiently converged;
[0017] Joint limit and obstacle avoidance unit: used to calculate and reconfirm joint positions, perform joint angle avoidance limits, and detect and avoid obstacles;
[0018] Input unit: used to input parameters such as joint configuration, joint position and posture, arc angle, arm angle, etc. required for inverse solution;
[0019] Output unit: obtain the solved kinematic parameters of the rigid-flexible serial robot arm.
[0020] Another technical solution adopted by the present invention is: a computer storage medium having a computer program stored thereon, wherein the program, when executed by a processor, implements the following steps:
[0021] Establish the DH coordinate system and give the coordinate transformation matrix between the rigid mechanism and the flexible mechanism coordinate system respectively;
[0022] After equivalently simplifying the robot arm, perform FABRIK algorithm iteration. During the forward iteration, calculate the position of each joint, and during the backward iteration, repeat the iteration while ensuring that the position of the first joint remains unchanged.
[0023] Calculate the new unit vector of the joint of the equivalent simplified arm while avoiding the joint limit, and perform forward and reverse iterative calculations to complete the angle avoidance limit of the equivalent simplified arm;
[0024] The inverse kinematics is solved based on the data of each joint.
[0025] The beneficial effects of the present invention are:
[0026] The present invention discloses a FABRIK-based rigid-flexible combined manipulator motion planning method and system, and a computer storage medium. Aiming at the need for a manipulator to replace manual labor to detect the narrow internal space of a nuclear reactor, the method performs equivalent simplification on a rigid-flexible serial manipulator, uses the FABRIK iteration program to make the end closer to the target, and during the iteration process, it is necessary to consider the joint limit avoidance and obstacle avoidance of the equivalent simplified arm, and finally perform inverse kinematics solution. Therefore, the computational difficulty of solving inverse kinematics by traditional methods is simplified, so that the rigid-flexible serial manipulator can meet the detection requirements of nuclear reactors. BRIEF DESCRIPTION OF THE DRAWINGS
[0027] Figure 1 It is a schematic diagram of an equivalent simplified arm in the inverse kinematics solution method of the rigid-flexible serial robot arm in the present invention.
[0028] Figure 2 It is a schematic diagram of a first iteration process of the FABRIK algorithm in the motion planning of the rigid-flexible combined robot arm based on FABRIK in the present invention.
[0029] Figure 3 It is a schematic diagram of avoiding joint over-limit in the motion planning of the rigid-flexible combined manipulator based on FABRIK in the present invention.
[0030] Figure 4 It is a schematic diagram of obstacle avoidance in the FABRIK-based rigid-flexible combined robot motion planning method of the present invention.
[0031] Figure 5 It is a schematic diagram of double arc fitting in the FABRIK-based rigid-flexible combined robot motion planning method of the present invention.
[0032] Figure 6 It is a schematic diagram of the arm angle defined in the FABRIK-based rigid-flexible combined robot motion planning method of the present invention. DETAILED DESCRIPTION
[0033] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application may be combined with each other.
[0034] In response to the need for a robotic arm to replace manual labor to inspect the narrow internal space of a nuclear reactor, a rigid-flexible serial robotic arm was designed, and a method based on the FABRIK algorithm to solve the inverse kinematics problem of the rigid-flexible serial robotic arm was proposed, including the following steps:
[0035] refer to Figure 1The rigid mechanism part adopts a seven-degree-of-freedom redundant robotic arm structure in the form of SRS, which is the base of the robotic arm. The first three joints intersect at one point to form a ball joint, called the shoulder joint S; the axes of the last three joints also intersect at one point to form a ball joint, called the wrist joint W; the fourth joint is the elbow joint E. The flexible mechanism part adopts two sections of continuum robotic arms. The positions of these joint points are named N1, N2 and N3 respectively. The flexible mechanism adopts two sections of series-connected flexible robotic arms. Under the assumption of constant curvature, the flexible robotic arm is simplified into two tangent arcs. The tangents of these two arcs at the end points intersect at N4 and N5. The end of the rigid-flexible serial robot is recorded as N6. Connect these points in sequence to form equivalent line segments. L i is the length of the equivalent line segment. The length of the equivalent line segment simplified by the seven-degree-of-freedom rigid mechanism is determined by its own structure. The lengths of L4, L5, and L6 are determined by the bending angles θ8 and θ9 of the flexible mechanism. Figure 1 The double-line part in represents the equivalent line segment. Figure 1 In the triangle in , we can get:
[0036]
[0037] Where: λ1=N0N1, r1 is the radius of the first arc, θ1 is the angle of the first arc, and s1 is the length of the first arc in the assumption of equal curvature of the flexible mechanism. Then the equivalent simplified arm length of the flexible mechanism can be calculated as:
[0038]
[0039] From equations (1) and (2), we can know that for each state of the flexible mechanism, there is only one uniquely determined equivalent simplified arm corresponding to it. In addition, once the equivalent simplified articulated arm is determined, the flexible mechanism can also be uniquely determined and updated.
[0040] Equivalent simplified arm node N of rigid-flexible serial robot arm i The coordinates are:
[0041]
[0042] Start two iterations of the FABRIK algorithm for the equivalent simplified arm;
[0043] refer to Figure 2 ,FABRIK is an algorithm that minimizes the error between the target point and the end effector by controlling the joint angles. This method is divided into two stages, forward and backward. The method starts from the last joint of the link and adjusts each joint forward. Then it works backward in the same way to complete a full iteration. First, the distance between each joint is calculated. and base p1 and target p tIf this distance is less than the sum of all joint distances, dist<d, the target is within reach, otherwise it cannot be reached. If the target is within reach, the FABRIK iteration procedure can be executed. In the forward iteration phase, the algorithm starts from the end effector p n To begin, move inward to the base p1 and estimate the position of each joint as shown in the reference Figure 2 (a). Afterwards, the end effector is assigned to a target position p' n =p t , as referenced Figure 2 (b). Then, at p' n and the next joint p n-1 Draw a line between the joints p' n-1 The new position of is on this line, at a distance p i+1 -p i The distance corresponds to the length of the connecting rod. Similarly, the (n-2)th joint p' n-2 The position can also be determined by p' n-1 and the next joint p n-2 The algorithm continues until all new joints including the base p1 are calculated, as shown in the reference Figure 2 (c) as shown. p' n It can be expressed as:
[0044]
[0045] Considering that the new position of the base, p'1, must be different from its initial position, a backward iteration of the algorithm is required. A full iteration is completed when the same process is repeated, but this time starting from the root joint and moving outward to the end effector. Therefore, let the new position of the first joint, p"1, be its initial position, p1. Then, determine the new position of the joint, p"2, using the line passing through the points p"1 and p'2. Repeat this step for all the remaining joints including the end effector. With the base, p1, translated to the desired position, determine the end effector, p n The location, as referenced Figure 2 (d) as shown. p” i It can be expressed as:
[0046]
[0047] After one full iteration, the end effector is closer to the target. This process is then repeated for as many iterations as needed until the end effector is the same as or close enough to the desired target. FABRIK allows the end effector to converge on any given target position when the total length of the links is greater than the distance to the target. However, if the target is not within the reachable region, there is a termination condition that compares the previous position of the end effector to the current position, and if this distance is less than the indicated tolerance, FABRIK terminates its operation.
[0048] Start joint limit avoidance and obstacle avoidance for the equivalent simplified arm;
[0049] refer to Figure 3 In the FABRIK iteration process, the joint angle of the equivalent simplified arm of the rigid-flexible serial robot may exceed the joint limit of the rigid-flexible serial robot. The joint limit avoidance of the equivalent simplified arm of the rigid-flexible serial robot can also be completed in the forward and backward iteration process. Figure 3 A schematic diagram showing the FABRIK joint limit avoidance. Figure 3 In (a), when the joint angle θ4 of the equivalent simplified arm is greater than the joint limit angle θ 4max When moving, joint limits need to be avoided.
[0050] refer to Figure 3 (b) shows the feasible domain of the equivalent simplified arm L4. Avoiding joint limits should make the equivalent simplified arm as close to N'4N3 as possible. So the new position of the equivalent simplified arm L4 is at p t On the line formed by the plane formed by N'4 and N'4N3 and the limit position of the feasible region, as shown in reference Figure 3 The dashed line in (b). t The normal vector of the plane formed by N'4 and N'4N3 is:
[0051] v4=(p t N'4 / p t N'4)×(N'4N3 / N'4N3) (6)
[0052] The new unit vector of the equivalent simplified arm N'4N3 is:
[0053] e4=Rot(v4,θ 4max )·e5 (7)
[0054] In subsequent iterations The joint angle avoidance limit of the remaining joint segments is also the same. Similarly, the angle avoidance limit of the joint segment can also be applied to the reverse approach iteration. When the angle avoidance limit of the joint segment is applied to both the forward and reverse approach iterations, the angle avoidance limit of the entire equivalent simplified arm can be finally achieved.
[0055] According to the given task and environmental constraints, the safe path for the end of the rope-driven redundant manipulator to avoid obstacles is first determined and discretized, that is, the path is divided into N equal parts to obtain N+1 points. Then, according to the environmental constraints, for any point on the path, its corresponding end posture X is planned. e Secondly, for the obstacle avoidance of the robot arm, consider using geometric methods to adjust the equivalent simplified arm.
[0056] As reference Figure 4 As shown in the figure, when the Euclidean distance between the obstacle and the equivalent link L4 is less than the safety distance d safe When N is reached, it will cause a collision. To avoid this, obstacles need to be avoided. Avoiding obstacles should make the equivalent simplified arm as far away from N as possible. O So the new position of the equivalent simplified arm L4 is at p t On the line formed by the plane formed by N'4 and N'4N3 and the boundary position of the security zone, as shown in reference Figure 4 The dashed line in (b). t The normal vector of the plane formed by N'4 and N'4N3 is:
[0057] v4=(p t N'4 / p t N'4)×(N'4N3 / N'4N3) (8)
[0058] Equivalent simplified arm The new unit vector is:
[0059] e4=Rot(v4,θ safe )·e5 (9)
[0060] where θ safe The result is:
[0061] θ safe =asin(d safe / N O p t ) (10)
[0062] Start solving the kinematic parameters of the rigid-flexible serial robot;
[0063] The flexible mechanism uses two continuum manipulators. Equal curvature is usually used to simplify the kinematic calculation of the continuum manipulator. That is, the bending degree of each joint can be regarded as the same. In this way, the flexible mechanism is represented by the parameters of two tangent arcs. Figure 5 As shown, P s is the coordinate of the starting point, O C1 , O C2is the center of the two arcs. θ1, θ1 is the angle of the center, S1, S2 are the lengths of the two arcs. s N m N5 and ΔN m N5N6.
[0064] Through one FABRIK iteration, we can get p s , the positions of N5, N6, N7 and the directions of the equivalent simplified arms L4, L5, L6. It is easy to derive the size of θ as:
[0065]
[0066] Since the length of the flexible mechanism is a constant, we can get:
[0067]
[0068] In this way, the positions of the two space arcs can be obtained. Since the angle of the arc changes, the lengths of λ1 and λ2 need to be updated. According to the geometric relationship, we can get:
[0069]
[0070] The rigid mechanism uses a seven-degree-of-freedom redundant manipulator in the form of SRS, which introduces a redundant input variable to form the self-motion of the manipulator. When we solve the inverse kinematics, it causes uncertainty in the solution, and there will be countless sets of joint angles corresponding to a given end position. In order to obtain an inverse solution that meets our task needs and has practical significance, we use the arm angle parameter To describe the degrees of freedom of the robot's null space. Figure 6 As shown, the arm angle is the angle of the reference surface around the vector According to the right-hand screw rule, rotate to the angle when the current arm profile coincides.
[0071] The normal vector of the reference surface is defined as:
[0072]
[0073] Where V is an arbitrary unit vector parallel to z0.
[0074] The FABRIK method can be used to obtain N i The position of each point. In this way, we can get the values of point S, point W and vector SW in turn. Combined with the arm angle parameterization, the angles of each joint can be solved. For the equivalent simplified arm that has been determined, the angle of the elbow joint can be obtained by the following cosine formula:
[0075]
[0076] Among them, θ4 has two possibilities, positive and negative, which respectively indicate that the elbow joint is up or down.
[0077] After obtaining the elbow joint angle, the shoulder (S) joint angle can be solved. Since the shoulder joint angle solution depends on the angle of the c reference plane, it is necessary to derive The specific expression of . is the matrix transformation relationship between the No. 3 coordinate system and the base coordinate system when the No. 3 coordinate system is in the reference plane. It can be determined by the following formula:
[0078]
[0079] By reference Figure 6 It can be seen that The direction is direction. The direction can be determined by the vector Rotate around l by an angle α to get:
[0080]
[0081] y3 0 It can be determined by the following formula:
[0082]
[0083] The rotation matrix R is:
[0084] R(l,α)=I+sinα[l × ]+(1-cosα)[l × ] 2 (20)
[0085] Among them: × ] represents the cross multiplication factor of vector l after normalization. [l × The expression of ] is:
[0086]
[0087] According to the right-hand rule:
[0088]
[0089] 0 The expression is:
[0090]
[0091] In the case of other arm types, the transformation matrix between the third coordinate system and the base coordinate system is:
[0092]
[0093] From formula (24) (25), we can get:
[0094] 0 R3=A s sinψ+B s cosψ+C S (26)
[0095] in: From this we can get:
[0096]
[0097] θ2=acos(-(a s32 sinψ+b s32 cosψ+c s32 )) (28)
[0098]
[0099] where s ψ = sinψ, c ψ =cosψ. sxy Represents the matrix A s The x-th row and y-th column element of b sxy , c sxy Same reason.
[0100] The elbow joint angle is known, we can get:
[0101]
[0102] From this we can get:
[0103]
[0104] θ6=±acos(-(a s33 sinψ+b s33 cosψ+c s33 )) (32)
[0105]
[0106] Based on the above method, the present invention also provides a FABRIK-based rigid-flexible combined robot arm motion planning method and system, including:
[0107] Equivalent simplified robot arm unit, used to perform equivalent simplification on complex robot arms;
[0108] FABRIK iteration unit, used to iterate the algorithm so that the difference between the current and expected end position and posture is sufficiently converged;
[0109] Joint limit and obstacle avoidance unit, used to calculate and reconfirm joint positions, perform joint angle avoidance limits, and detect and avoid obstacles;
[0110] An input unit is used to input parameters such as joint configuration, joint position and posture, arc angle, arm angle, etc. required for inverse kinematics solution;
[0111] The output unit obtains the solved kinematic parameters of the rigid-flexible serial robot arm.
[0112] Based on the above method, the present invention further provides a computer storage medium, characterized in that a computer program is stored thereon, and when the program is executed by a processor, the following steps are implemented:
[0113] Establish the DH coordinate system and give the coordinate transformation matrix between the rigid mechanism and the flexible mechanism coordinate system respectively;
[0114] After equivalently simplifying the robot arm, perform FABRIK algorithm iteration. During the forward iteration, calculate the position of each joint, and during the backward iteration, repeat the iteration while ensuring that the position of the first joint remains unchanged.
[0115] Calculate the new unit vector of the joint of the equivalent simplified arm while avoiding the joint limit, and perform forward and reverse iterative calculations to complete the angle avoidance limit of the equivalent simplified arm;
[0116] The inverse kinematics is solved based on the data of each joint.
[0117] In the present invention, a method and system for motion planning of a rigid-flexible combined manipulator based on FABRIK, and a computer storage medium are provided. The method enables a rigid-flexible serial robot to synchronously plan the terminal posture and overall configuration while completing obstacle avoidance. First, an equivalent simplification method for simplifying a rigid-flexible serial robot into an equivalent line segment is proposed. Secondly, a terminal roll angle adjustment method is added during the equivalent line segment iteration process; at the same time, a method for avoiding joint overrun is proposed. Thirdly, a method for inverse kinematic parameter fitting of a rigid-flexible serial robot using "arm angle" and "double arc" is proposed. On this basis, the method adjusts the relative position of the equivalent line segment and the obstacle during the equivalent line segment iteration process to achieve obstacle avoidance of the equivalent line segment, thereby completing obstacle avoidance of the rigid-flexible serial robot.
[0118] The above is a specific description of the preferred implementation of the present invention, but the invention is not limited to the embodiments. Those skilled in the art can make various equivalent modifications or substitutions without violating the spirit of the present invention. These equivalent modifications or substitutions are all included in the scope defined by the claims of this application.
Claims
1. A motion planning method for a rigid-flexible combined robot arm based on FABRIK, characterized in that: The steps include: The rigid-flexible serial robot arm is simplified equivalently to obtain an equivalent simplified arm; The equivalent simplified arm is iterated using the FABRIK iterative algorithm in two stages, namely iteration toward the base and iteration toward the end effector, and then multiple iterations are performed as needed until the difference between the current and desired end position and posture converges sufficiently; During the FABRIK iteration process, the joint limit avoidance and obstacle avoidance of the entire equivalent simplified arm are completed; The equivalent simplified arm is fitted with the rigid-flexible serial robot, and the kinematic parameters of the rigid-flexible serial robot are solved.
2. According to the FABRIK-based rigid-flexible combined robot motion planning method of claim 1, the rigid-flexible serial robot is characterized by equivalent simplification: According to the structural characteristics of the seven-DOF redundant robotic arm in the form of SRS adopted by the rigid mechanism, the first three joints are regarded as shoulder joints, the last three joints are regarded as wrist joints, and the fourth joint is regarded as elbow joints. The flexible mechanism part adopts a two-segment continuum robotic arm. Under the assumption of constant curvature, the continuum robotic arm is equivalent to two arcs. The tangents on both sides of the arc coincide with the two segments of the flexible mechanism respectively and intersect at nodes. These nodes can be connected to the shoulder, elbow, and wrist joints and are regarded as equivalent simplified arms.
3. According to the FABRIK-based rigid-flexible combined robot motion planning method of claims 1 and 2, the FABRIK iterative algorithm is characterized by : Before executing the FABRIK algorithm, it is necessary to calculate the total length of the equivalent robotic arm and the distance between the base and the target to determine whether the target is within the reachable range. When the target is within the reachable range, iterate toward the base. Execute FABRIK iteration, the algorithm is executed from the end effector to the base, calculates the new coordinate position of each joint, and then moves from the root joint to the end to ensure that the first joint is still in the original coordinate, and then performs multiple iterations as needed until the difference between the current and desired end position and posture converges sufficiently.
4. According to any one of claims 1 to 3, a FABRIK-based rigid-flexible combined manipulator motion planning method, wherein the joint limit avoidance and obstacle avoidance of the equivalent simplified arm are characterized by: : For each simplified arm, the joint angle avoidance limit can be updated and confirmed based on the feasible domain of the equivalent simplified arm. The joint segments are simultaneously avoided by forward and reverse iterations, and the angle limit avoidance of the entire equivalent simplified arm can be achieved eventually. When the Euclidean distance between the obstacle and the equivalent link is less than the safe distance, the obstacle is avoided. To avoid the obstacle, the equivalent simplified arm should be as far away as possible. Therefore, the new position of the equivalent simplified arm is on the line formed by the plane formed by the link to be adjusted and the previous link and the boundary position of the safety zone.
5. According to the FABRIK-based rigid-flexible combined robot motion planning method of claim 1, the kinematic parameters of the rigid-flexible serial robot are solved, characterized by: For the flexible mechanism, the equivalent simplified arm is fitted to the flexible mechanism using the equal curvature assumption to solve the kinematic parameters; For rigid mechanisms, the arm angle is used to fit the equivalent simplified arm to the rigid mechanism and solve the kinematic parameters.
6. The FABRIK-based rigid-flexible combined robot arm motion planning system is characterized by: include: Equivalent simplified robot arm unit, used to perform equivalent simplification on complex robot arms; FABRIK iteration unit, used to iterate the algorithm so that the difference between the current and expected end position and posture is sufficiently converged; Joint limit and obstacle avoidance unit, used to calculate and reconfirm joint positions, perform joint angle avoidance limits, and detect and avoid obstacles; An input unit is used to input parameters such as joint configuration, joint position and posture, arc angle, arm angle, etc. required for inverse solution; The output unit outputs the solved kinematic parameters of the rigid-flexible serial robot arm.
7. A computer-readable storage medium storing a computer program, characterized in that: A computer program is stored thereon, and when the program is executed by a processor, the following steps are implemented: Establish the DH coordinate system and give the coordinate transformation matrix between the rigid mechanism and the flexible mechanism coordinate system respectively; After equivalently simplifying the robot arm, perform FABRIK algorithm iteration. During the forward iteration, calculate the position of each joint. During the backward iteration, make sure the position of the first joint remains unchanged and repeat the iteration. When the equivalent simplified arm avoids the joint limit, the new unit vector of the joint is calculated, and forward and reverse iterative calculations are performed to complete the angle avoidance limit of the equivalent simplified arm; The inverse kinematics is solved based on the data of each joint.
Citation Information
Patent Citations
Master-slave underwater robotic arm delay control method
CN106054599A
Simplified kinematics solving method of linkage flexible mechanical arm, device and system
CN110576438A
Collaborative planning method and system for solving inverse kinematics of rope-driven mechanical arm and computer storage medium
CN115533920A
Mechanical arm kinetic parameter identification method and parameter verification method
CN115781681A
Super-redundant serpentine arm bilateral offset following inverse solution algorithm
CN118260507A
Cited By
Dynamic model fuzzy adaptive control method fusing visual feedback
CN120972594A
Humanoid robot arm end mechanism path real-time planning method
CN121572314A
