Search

CN-121989245-A - Motion planning method, system, equipment and storage medium for restraining gesture of mechanical arm

CN121989245ACN 121989245 ACN121989245 ACN 121989245ACN-121989245-A

Abstract

The invention provides a motion planning method, a system, equipment and a storage medium for restraining the gesture of a mechanical arm, wherein the method comprises the steps of establishing a kinematic model, resolving and solving inverse kinematics, and screening a unique optimal joint solution; the method comprises the steps of performing high-efficiency collision detection by adopting a hierarchical bounding body tree and convex decomposition technology, combining a coarse detection algorithm and a precise algorithm of a separation theorem, calculating a box body grabbing point pose according to visual information, deciding a single/double box grabbing strategy, adopting differential tail end pose constraint sampling based on grabbing heights, performing rotary sampling on a low-level box body by using four degrees of freedom and positions and around a fixed shaft, increasing a disturbed angle of a surrounding tool by a high-level box body to provide a compaction force, performing path planning on sampling points by using a random rapid expansion tree after inverse solution and collision verification, and performing smooth optimization on a successful path to generate a final track. The invention can carry out strict constraint on the terminal gesture at the planning level and integrates the motion planning method of the solving and detecting module so as to ensure the reliability and the safety.

Inventors

  • TAN LIMIN
  • Gu Rongqi
  • LU ZHENFEI

Assignees

  • 上海西井科技股份有限公司

Dates

Publication Date
20260508
Application Date
20260305

Claims (12)

  1. 1. The motion planning method for restraining the gesture of the mechanical arm is applied to a container loading and unloading scene and is characterized by comprising the following steps of: S110, a kinematic modeling step, which comprises the steps of establishing a coordinate system based on a mechanical arm kinematic modeling rule according to mechanical arm model parameters, obtaining a grid model for collision detection and a homogeneous transformation matrix for describing relative transformation among connecting rods, and carrying out positive kinematic modeling on the mechanical arm model; S120, obtaining an optimal joint solution, wherein the optimal joint solution comprises the steps of decomposing joint variables and configuration space of the mechanical arm model into position sub-problems of the first three joints and posture sub-problems of the second three joints based on the positive kinematic modeling, resolving and solving the position sub-problems to obtain a plurality of candidate joint configurations, and screening out the unique optimal joint solution based on preset optimal configuration characteristics; S130, collision detection, namely constructing a three-dimensional grid of a mechanical arm model and an environment model corresponding to the unique optimal joint solution, organizing the three-dimensional grid by adopting a hierarchical bounding volume tree data structure, preprocessing a non-convex grid by utilizing a convex decomposition technology, and compressing original triangular patch leaf nodes into convex hull leaf nodes; S140, determining and deciding the target grabbing point pose, wherein the method comprises the steps of calculating grabbing point poses of the top surface or the side surface of each box body according to the global pose of the box body obtained by a visual perception module, and deciding to adopt a single-box grabbing or double-box synchronous grabbing strategy based on the consistency of the spatial adjacent relation and the poses among grabbing points; S150, terminal attitude constraint configuration and sampling comprise constructing terminal attitude constraint based on Liqun-Liqun algebra space mapping, and according to the height of a target grabbing point, differentiating two constraint sampling modes, namely adopting a four-degree-of-freedom constraint sampling mode for a box below a preset height threshold, wherein sampling vectors are [ x, y, z, θrel ], position components [ x, y, z ] are absolute coordinates, attitude components θrel are relative rotation angles around a fixed shaft relative to a starting attitude, adopting a five-degree-of-freedom constraint sampling mode for a box above the preset height threshold, adding a disturbance angle sampling component around a tool center point part on the basis of four degrees of freedom, wherein the sampling vectors are [ x, y, z, θrel, Pert ], wherein, Converting the relative gesture into a rotation matrix through index mapping, and combining the rotation matrix with the initial gesture to obtain terminal gesture sampling conforming to task constraint; S160, path planning comprises the steps of calling the corresponding optimal joint solution for each terminal space sampling point, performing collision verification through accurate collision detection, guiding planning by adopting the five-degree-of-freedom constraint sampling mode for an upper box, enabling an end effector to apply additional normal pressing force in a lifting stage, guiding planning by adopting the four-degree-of-freedom constraint sampling mode for a lower box, enabling the end effector to always keep a vertical downward posture, and smoothing an initial path by adopting a rear path shortcut optimization algorithm after the planning is successful, removing redundant turning points and generating a final executable motion track.
  2. 2. The method for planning motion for constraining a pose of a mechanical arm according to claim 1, wherein said step S110 comprises the steps of: S111, establishing a coordinate system for each joint under the mechanical arm kinematics modeling rule according to the number of joints and geometric parameters of the mechanical arm to form a complete kinematics chain; s112, calculating a transformation matrix Ai between adjacent connecting rod coordinate systems by using a homogeneous transformation formula based on the established coordinate systems and corresponding mechanical arm kinematics modeling parameter tables; S113, continuously multiplying the transformation matrixes Ai of all adjacent connecting rods, calculating a pose transformation matrix H of the mechanical arm end effector relative to a base coordinate system, and completing positive kinematic modeling; And S114, associating the mechanical arm model with a kinematic chain, and endowing a corresponding coordinate system for the grid model of each connecting rod.
  3. 3. The method of motion planning for constraining a pose of a robotic arm of claim 1, wherein said step S120 comprises the steps of: s121, giving the position of an end effector, and solving angles theta 1, theta 2 and theta 3 of the first three joints, wherein the theta 1 has two solutions about Xc-Yc plane projection, the theta 3 has two solutions corresponding to the upper or lower part of the elbow of the mechanical arm, and the theta 2 is uniquely determined according to the geometric relationship; S122, on the basis of obtaining the first three joint solutions, calculating a rotation matrix of a fourth connecting rod coordinate system relative to a third connecting rod coordinate system, decomposing the rotation matrix into Euler angles, and solving angles theta 4, theta 5 and theta 6 of the last three joints, wherein each of the theta 4 and the theta 6 has two solutions with a phase difference pi, and each of the theta 5 has one solution and a complementary angle solution thereof; S123, arranging and combining solutions of the position sub-problem and the attitude sub-problem to obtain eight groups of analysis solutions at most, screening a joint angle combination which is only in accordance with requirements from the multiple groups of solutions to be used as a unique optimal joint solution based on preset optimal configuration characteristics, wherein the optimal configuration characteristics at least comprise that a second connecting rod and a third connecting rod are in an upper attitude, and the joint angle solution corresponding to a fourth connecting rod is recorded as the unique optimal joint solution Selecting a solution among all candidate solutions Solutions that tend to be as 0 degrees as possible As a final solution for global optima.
  4. 4. The method of motion planning for constraining a pose of a robotic arm of claim 1, wherein said step S130 comprises the steps of: carrying out algorithm processing based on volume decomposition or surface convexity analysis on complex non-convex three-dimensional grids in the mechanical arm model and the environmental obstacle model, and dividing the complex non-convex three-dimensional grids into a plurality of convex parts; Each convex part is used as a leaf node to participate in the construction of the BVH tree, so that the depth of the BVH tree and the number of the leaf nodes are greatly reduced, the accurate detection which is originally needed to be carried out between a large number of triangular patch pairs is converted into the accurate detection which is carried out between a small number of convex hull pairs, and the accurate detection is efficiently completed through a GJK algorithm.
  5. 5. The method of motion planning for constraining a pose of a robotic arm as claimed in claim 1, wherein said step S140 comprises the steps of: s141, according to the pose of the box centroid provided by the visual perception module under the base coordinate system Calculating the position and the posture of the grabbing point according to the length L, the width W and the height H of the box body , wherein, The coordinate system represents the base coordinate system of the mechanical arm, the C coordinate system represents the central local coordinate system of the box, The coordinate system represents the coordinates of the gripping points of the box, for top gripping: = Transl (0, 0, H/2), for side grabs: = · Transl(0, ±W/2, H/2); S142, sorting all the boxes in the scene according to the central height of the boxes, and selecting the top K boxes with the highest height as candidate grabbing boxes; S143, for paired candidate boxes at similar heights, calculating Euclidean distances between grabbing points of the paired candidate boxes, and calculating an included angle theta between quaternions corresponding to the two grabbing gesture rotation matrixes; s144, if the distance is smaller than the set distance threshold and the included angle theta is smaller than the set angle threshold, judging that the pair of boxes meet the double-box synchronous grabbing condition, otherwise, adopting a single-box grabbing strategy.
  6. 6. The method for planning motion of constrained robot arm pose according to claim 1, wherein in step S150, the four-degree-of-freedom constrained sampling mode comprises the steps of: S151, inputting a start point end pose Rotstart, posstart and a target point end pose RotTarget, postarget; s152, calculating a relative rotation matrix from a starting posture to a target posture, wherein Rotdelta = Rottarget. Rotstart { -1}; S153, converting Rotdelta into a lie algebra space through logarithmic mapping to obtain a rotation vector, and extracting a rotation axis and a rotation angle, wherein the rotation angle is used as a target value of a gesture sampling component; S154, sampling the position component in absolute space, sampling the attitude component theta rel in a [0, angle ] interval or sampling in a Li algebraic space range near the value, and mapping the convolution torque array Rotsampled by a Rodrigas formula index in combination with a fixed rotation axis for the relative rotation angle theta sampled of the sampling; and S155, calculating absolute postures of sampling points, wherein Rotnew = Rotsampled. Rotstart, so that all sampling postures are located on or near the optimal constraint rotation axis paths from Rotstart to Rottarget, and posture constraints are strictly met.
  7. 7. The method for planning the motion of the constrained manipulator pose according to claim 6, wherein in step S150, the following steps are added on the basis of four-degree-of-freedom constraint in the five-degree-of-freedom constraint sampling mode: s156, defining a tool center point coordinate system, and taking a Y-axis [0,1,0] of the coordinate system as a disturbance axis axispert; s157, adding disturbance angle component in gesture sampling vector Pert, whose sampling range is within [0, maxPerturbation ], wherein MaxPerturbation is the maximum disturbance angle set empirically from the collision and ambient collision; S158, disturbance angle for sampling Sampled, combining the disturbance axes axispert, and obtaining a disturbance rotation matrix Rotpert through exponential mapping; S159, calculating absolute postures of sampling points, wherein Rotnew = Rotsampled. Rotstart. Rotpert, wherein Rotsampled is obtained by four-degree-of-freedom constraint sampling, and the operation is performed while meeting the constraint from the initial main posture to the target main posture, and introducing a controlled additional rotation around a local Y axis of a tool center point for providing additional normal pressing force during side grabbing.
  8. 8. A method of planning motion for constraining a pose of a manipulator according to claim 3, wherein in step S160, the path planning comprises the steps of: S161, initializing two random rapid expansion trees, wherein the initial state and the target state are taken as root nodes respectively; S162, iteratively executing the following processes of randomly sampling a new state xrand in an end constraint space, finding a node xnear closest to xrand in an existing tree, expanding one step from xnear to xrand to obtain new nodes xnew, verifying whether each xnew is feasible through inverse kinematics calculation and collision detection, and adding the feasible xnew into the tree; S163, after the xnew is inserted, performing rerouting optimization, namely searching whether a certain adjacent node xnearby exists in an adjacent node set of the xnew, so that the cost of reaching xnearby through the xnew is lower, reconnecting a parent node of xnearby to the xnew if the adjacent node is lower, checking whether other adjacent nodes can reach subtrees of the adjacent node or not by the xnew, and if the adjacent node can reach subtrees of the adjacent node lower, performing corresponding parent node replacement to optimize a tree structure; s164, when the two trees are expanded to be smaller than a set threshold value, the path searching is considered to be successful, and the two trees are connected at the position to form an initial path from the beginning to the target; S165, performing shortcut optimization on the obtained initial path, namely randomly selecting two non-adjacent nodes on the path, attempting to connect the two non-adjacent nodes by using linear interpolation, verifying the feasibility of the section of direct-connected path through collision detection, if the direct-connected path is feasible, replacing a broken line section between two points in the original path by using the direct-connected section, and repeating the process for a plurality of times to shorten the path length and reduce turning points so as to enable the final path to be smoother.
  9. 9. The motion planning method for constraining a pose of a mechanical arm according to claim 1, further comprising step S170, singular point avoidance for dealing with a wrist singular or joint space jump problem encountered in an inverse kinematics solving process, wherein the step S170 includes the steps of: s171, when the analytic inverse kinematics solution is free of solution or unstable due to the fact that the analytic inverse kinematics solution is close to a singular point, switching to use a damping least square method to carry out numerical iteration solution so as to obtain a continuous and stable joint angle solution; And S172, in path planning, when the optimal joint solution corresponding to the sampling points adjacent to the two task spaces is detected to generate configuration jump, and the calculated joint speed possibly exceeds the limit of the mechanical arm, the path section between the two points adopts joint space linear interpolation to carry out smooth transition.
  10. 10. A motion planning system for constraining a pose of a manipulator for implementing the motion planning method for constraining a pose of a manipulator according to claim 1, comprising: The motion modeling module is used for establishing a coordinate system based on a mechanical arm kinematics modeling rule according to mechanical arm model parameters, obtaining a grid model for collision detection and a homogeneous transformation matrix for describing relative transformation among all connecting rods, and performing positive kinematics modeling on the mechanical arm model; the optimal solving module is used for resolving and solving joint variables and configuration space of the mechanical arm model into position sub-problems of the first three joints and posture sub-problems of the second three joints based on the positive kinematic modeling to obtain a plurality of candidate joint configurations, and screening out unique optimal joint solutions based on preset optimal configuration characteristics; The system comprises a collision detection module, a convex decomposition technology, a node bounding box, a collision detection algorithm, a collision detection module and a collision detection module, wherein the three-dimensional grid of a mechanical arm model and an environment model corresponding to the unique optimal joint solution is constructed, and the three-dimensional grid is organized by adopting a hierarchical bounding volume tree data structure; The pose decision module is used for calculating the grabbing point pose of the top surface or the side surface of each box body according to the global pose of the box body obtained by the visual perception module, and deciding to adopt a single-box grabbing or double-box synchronous grabbing strategy based on the consistency of the spatial adjacent relation and the pose among grabbing points; The terminal gesture module constructs terminal gesture constraint based on Liqun-Liqun algebra space mapping, and distinguishes and adopts two constraint sampling modes according to the height of a target grabbing point, wherein for a box below a preset height threshold value, a four-degree-of-freedom constraint sampling mode is adopted, sampling vectors are [ x, Y, z, θrel ], wherein position components [ x, Y, z ] are absolute coordinates, gesture components θrel are relative rotation angles around a fixed shaft relative to a starting gesture, for a box above the preset height threshold value, a five-degree-of-freedom constraint sampling mode is adopted, a disturbance angle sampling component around a local Y axis of a tool center point is added on the basis of four degrees of freedom, sampling vectors are [ x, Y, z, θrel, Pert ], wherein, Converting the relative gesture into a rotation matrix through index mapping, and combining the rotation matrix with the initial gesture to obtain terminal gesture sampling conforming to task constraint; The path planning module is used for calling the corresponding optimal joint solution for each terminal space sampling point and carrying out collision verification through the accurate collision detection, adopting the five-degree-of-freedom constraint sampling mode to guide the planning for the upper box so that the end effector can apply additional normal pressing force in the lifting stage, adopting the four-degree-of-freedom constraint sampling mode to guide the planning for the lower box so that the end effector always keeps a vertical downward posture, and adopting a postposition path shortcut optimization algorithm to smooth an initial path after the planning is successful so as to remove redundant turning points and generate a final executable motion track.
  11. 11. A motion planning apparatus for constraining a pose of a robotic arm, comprising: A processor; A memory having stored therein executable instructions of the processor; wherein the processor is configured to perform the steps of the motion planning method of constraining a pose of a robotic arm of any of claims 1 to 9 via execution of the executable instructions.
  12. 12. A computer-readable storage medium storing a program, wherein the program when executed by a processor implements the steps of the motion planning method of constraining a pose of a manipulator according to any one of claims 1 to 9.

Description

Motion planning method, system, equipment and storage medium for restraining gesture of mechanical arm Technical Field The invention relates to the field of mechanical arm control, in particular to a motion planning method, a system, equipment and a storage medium for restraining the gesture of a mechanical arm. Background With the development of logistics automation, the automatic loading and unloading of the container body in the container by adopting a mechanical arm and an end effector (a vacuum chuck) has become an important application direction. However, such work environments often have high constraint characteristics such as narrow space, dense obstacles, irregular stacking of boxes, etc., which pose serious challenges to the motion planning of the robotic arm. Currently, an open source robotic arm motion planning framework (e.g., moveIt |) generally employs a joint space sampling-based planning method. The method comprises the typical processes of randomly sampling a configuration in a joint space of a mechanical arm, solving or calculating the pose of the tail end through numerical Inverse Kinematics (IK) or positive kinematics, filtering out infeasible samples through collision detection, and finally searching out a collision-free path from a starting point to an end point by utilizing sampling algorithms such as RRT, PRM and the like. This approach suffers from the following significant drawbacks: The end pose is out of control, and there is no direct correlation between the joint space sampling and the end Cartesian pose required for the task. The randomly sampled joint angles are calculated by positive kinematics, and the obtained terminal gestures are random and unpredictable. The planner focuses more on the connectivity and collision-free nature of the joint space than the specific orientation of the end effector. This results in the possibility that the pose of the robot arm tip (suction cup) may be constantly rotated in the generated trajectory. The effective sampling rate is low-when the task has stringent requirements on the tip pose (e.g., the suction cup must be vertically down or held at a certain tilt angle), the sampling points need to be filtered by a posterior check. Because the joint configuration meeting the strict posture constraint has extremely low proportion in the whole configuration space, the mode of sampling before filtering can lead to a large number of samples to be discarded, the planning efficiency is low, and even the planning fails because a feasible solution can not be found for a long time. Inverse kinematics solves the bottleneck that for each sampling point that needs to be mapped to joint space by inverse kinematics, the numerical inverse kinematics solver may fall into local optima, generate a wrist-turning configuration, or fail near singular points. The joint space sampling can not consider the multi-resolution and continuity of inverse kinematics solution, and is easy to cause configuration jump between adjacent path points, and severe shaking or overspeed in the motion process is caused. The collision detection efficiency is low, a large number of polygons need to be traversed in the traditional collision detection based on the triangular mesh, and the calculation cost is high. In high density obstacle environments, frequent collision queries become a major time bottleneck for motion planning. In the case of container handling, the above problems are further magnified. The sucking disc relies on vacuum negative pressure to adsorb the box, and the effective utilization of its adsorption affinity relies on sucking disc and the abundant laminating of box surface. If the tail end gesture is out of control, and the suction cup normal vector is not coincident with the box surface normal vector (namely gesture skew), tangential component force is introduced, so that effective adsorption force is weakened, and the box sliding risk is increased. The tangential component force weakens the effective normal adsorption force, so that the box body is easy to slide and swing in the carrying process, and even falls off directly in the acceleration or steering process, thereby causing operation failure or safety accidents. Particularly, when the upper box body is grasped laterally, gravity can generate a remarkable peeling tangential force, and if the posture is controlled improperly, the peeling risk is extremely high. In view of the above, the invention provides a motion planning method, a system, a device and a storage medium for restraining the gesture of a mechanical arm. Disclosure of Invention Aiming at the problems in the prior art, the invention aims to provide a motion planning method, a system, equipment and a storage medium for restraining the gesture of a mechanical arm, which overcome the difficulty in the prior art, can directly carry out strict restraint on the gesture of the tail end at a planning layer, and integrate a motion planning method of a high-e