A mobile robot arm human-machine safety path planning method based on a danger index

Abstract

The application provides a mobile manipulator human-machine safety path planning method based on a danger index; firstly, an external information sensing system of the mobile manipulator is established, including a two-dimensional laser radar system for detecting the distance of surrounding obstacles and a vision system for identifying the human body contour; then, a global map of the working range of the manipulator and the start and end positions are given, and a global path reachable by the mobile manipulator is planned by an algorithm; during the movement process, if the vision system does not detect human body information, the mobile manipulator will move along the global path to the target position; if the vision system detects human body information, the two-dimensional laser radar system is used to scan and obtain the human body contour coordinates, the mobile manipulator and the human body contour are surrounded by plane minimum enclosing circles respectively, the human-machine safety distance is calculated and obtained, and the human-machine danger evaluation index is evaluated; the path planning algorithm is improved, and the mobile manipulator is guided to move away from the personnel in the scene; the application can guarantee the safety of human-machine integration and lay a foundation for the development of the human-machine interaction field.

Description

Technical Field

[0001] This invention relates to the field of robotics, and in particular to a method for human-machine safety path planning for a mobile robotic arm based on a hazard index. Background Technology

[0002] A mobile robotic arm is a robot system constructed by mounting a maneuverable robotic arm on a mobile platform. It can reach almost any point in the workspace via the mobile platform and transport objects to designated locations as needed. It has been widely used in manufacturing, agriculture, and military industries. As the application scenarios of mobile robotic arms become increasingly complex, tasks requiring collaboration between humans and the mobile robotic arm are becoming more common. Ensuring the safety of human-robot collaboration is therefore extremely important. Most existing mobile robotic arm safety technologies focus on obstacle avoidance planning and motion control of the robotic arm itself, generally treating all objects in the robot's surrounding environment as obstacles, with little consideration given to the safety of personnel within the work area. This significantly hinders the widespread adoption of mobile robotic arms for human-robot collaboration. Summary of the Invention

[0003] This invention proposes a human-machine safety path planning method for mobile robotic arms based on a hazard index, which can solve the problems related to safety path planning and control of mobile robotic arms and mobile chassis coupling systems in existing technologies in human-machine collaborative environments.

[0004] The present invention adopts the following technical solution.

[0005] A method for human-machine safety path planning for a mobile robotic arm based on a hazard index includes the following steps;

[0006] Step S1: Establish an external information perception system for the mobile robotic arm. This system includes a two-dimensional lidar system for detecting the distance to obstacles around the mobile robotic arm's motion system and a vision system for recognizing human body contours.

[0007] Step S2: Given the global map of the working area of ​​the mobile robotic arm and its starting and ending positions, plan the global path that the mobile robotic arm can reach.

[0008] Step S3: During the actual joint motion control process, if the vision system does not detect human information, the mobile robotic arm will move along the global path to the target position; if the vision system detects human information, the human contour coordinates are obtained by scanning with a two-dimensional lidar system, and the mobile robotic arm and the human contour are surrounded by the smallest enclosing circle in the plane respectively. The distance between the centers of the two enclosing circles is calculated to obtain the human-machine safety distance.

[0009] Step S4: Assess the human-machine hazard assessment index;

[0010] Step S5: Given a human-machine collision hazard index threshold, improve the path planning algorithm based on the calculated hazard index to guide the mobile robotic arm away from people in the scene.

[0011] In step S1, the mobile robotic arm motion system includes a six-axis flexible collaborative robotic arm placed on a four-wheel omnidirectional moving chassis, with an electric gripper installed at the end of the robotic arm.

[0012] In step S1, the operator can remotely operate the mobile robotic arm via a control handle or PC.

[0013] In step S2, a global path that the mobile robotic arm can reach is planned according to Dijkstra's algorithm.

[0014] In step S2, a global map of the work area is input into the external information perception system of the mobile robotic arm. This global map of the work area is created by scanning the work scene with a two-dimensional LiDAR mounted on the chassis and using the Gmapping method to record the positions of static obstacles in the scene.

[0015] In step S3, the inverse kinematics of the mobile robotic arm is calculated based on the planned global path points in Cartesian coordinates to obtain joint control values, and joint motion control is performed accordingly.

[0016] In step S3, when the vision system detects human information and obtains the human contour coordinates through scanning by the two-dimensional LiDAR system, it encloses the moving robotic arm and the human contour with the smallest enclosing circle in the plane, and calculates the distance d between the centers of the two enclosing circles. t Thus, the safe distance D between humans and machines is obtained. t =d t -R1-R2, where R1 and R2 are the minimum envelope circle radii of the human body and the mobile robotic arm contours, respectively.

[0017] The mobile robotic arm is a robotic arm positioned on a mobile chassis;

[0018] In step S4, the human-machine hazard assessment index f(f) is calculated. d ,f v The specific method is as follows:

[0019]

[0020] In the formula, K DV f is the non-negative hazard index assessment coefficient. d For distance influence factor, f v f is the speed influence factor. dc This is the critical value for the distance influence factor;

[0021] Furthermore, the distance influence factor f d The calculation formula is

[0022]

[0023] Among them, L max ,L min These represent the maximum and minimum detection ranges of the lidar, D. max D min These represent the maximum and minimum safe distances allowed between humans and machines, respectively.

[0024] Furthermore, the speed influence factor f v The calculation formula is

[0025]

[0026] in, The factor affecting the joint rotation speed of the robotic arm. For factors affecting the rotational speed of the mobile chassis, As a factor affecting movement speed, Let be the rotational speed of the i-th joint of the robotic arm. ω represents the minimum and maximum rotational speeds of the i-th joint of the robotic arm, respectively. z The rotational speed of the mobile chassis is calculated using the following factors:

[0027]

[0028]

[0029] Where, δ i μ i i = 1, 2 are adjustment factors, and the movement speed influence factor is calculated as follows:

[0030]

[0031] Among them, v bmax v bmin These are the maximum and minimum moving speeds of the mobile chassis, respectively.

[0032] In step S5, the method for giving the human-machine collision hazard index threshold F is as follows: when f(f d f v When f(f) ≥ F, the mobile robotic arm undergoes improved artificial potential field method dynamic path planning and inverse kinematics calculation to obtain new joint control values ​​and perform joint motion control; when f(f) ≥ F, the mobile robotic arm undergoes improved artificial potential field method dynamic path planning and inverse kinematics calculation to obtain new joint control values ​​and perform joint motion control; d f v When F < F, take the current position of the moving robotic arm as the starting position and repeat steps S2 to S5.

[0033] The gravitational field calculation formula for the improved dynamic artificial potential field method described in step S5 is as follows:

[0034] gravitational field:

[0035] Among them, q, q obs q goal These represent the current position of the moving robotic arm, the position of the obstacle, and the position of the target, respectively, μ att d is the gravitational coefficient. att The range of gravitational influence;

[0036] The repulsive field is formed by the combined action of a static repulsive field and a dynamic repulsive field, and the calculation formulas are as follows:

[0037] Static repulsive field:

[0038] Dynamic repulsive field:

[0039] Where ρ(q,q) goal ρ(q,q) represents the Euclidean distance between the current position of the moving robotic arm and the target position. obs d represents the Euclidean distance between the current position of the moving robotic arm and the position of the obstacle. rep ξ represents the range of influence of the repulsive force. ξ is the repulsive force coefficient, f(f d ,f v ) represents the current danger index value, k DI k is the risk index conversion coefficient. ro v is the relative velocity coefficient. ro Let α be the relative velocity between the human and the machine, and α be the angle between their relative velocities.

[0040] When the mobile robotic arm motion system includes a six-axis flexible collaborative robotic arm mounted on a four-wheel omnidirectional chassis, and the end effector of the robotic arm is provided, the planning method is specifically as follows:

[0041] A kinematic model of the mobile robotic arm is established based on the DH method, and the rotation transformation matrix of the robotic arm is calculated.

[0042]

[0043] in

[0044] Calculate the Jacobian matrix of the mobile robotic arm based on the rotation transformation matrix. and its pseudo-inverse matrix

[0045] The Jacobian matrix of the mobile robotic arm is composed of the chassis Jacobian matrix. Jacobian matrix of robotic arm It consists of two parts. Based on the formula above, the Jacobian matrix of the mobile robotic arm is first solved using the vector product method. The steps are as follows: first, calculate the action vector of a single joint on the end effector of the moving robotic arm as the Jacobian matrix. A separate column, then the effects of all joints on the end effector of the moving robotic arm are superimposed;

[0046] Jacobian matrix column i for:

[0047]

[0048] In the formula, z i p is the third column of the i-th rotation transformation matrix. E The homogeneous transformation matrix The column vector consisting of the first three elements of the fourth column, p i for The column vector consisting of the first 3 elements of the 4th column;

[0049] Jacobian matrix J b and As shown in the following formula:

[0050]

[0051]

[0052]

[0053] In the formula, Let P(P) be the rotation transformation matrix from the world coordinate system to the robot arm base coordinate system. x ,P y Let P be the two-dimensional coordinates. OP is the distance from the center of the mobile chassis base to the center of the robotic arm mounting base. For the mobile base, such as... Figure 1 As shown in (b), its kinematic equations are:

[0054]

[0055] In the formula, The angle at which the chassis is oriented, and the angular velocity of the chassis.

[0056] l x ,l y As attached Figure 1 As shown in (b). Based on the above formula, the chassis movement speed is: The rotation transformation matrix of the mobile robotic arm is: The rotation transformation matrix is:

[0057]

[0058]

[0059] In the formula d E Let be the distance from the end effector to the origin of the robot arm's coordinate system. Since the position and motion of the end effector are determined jointly by the moving chassis and the six-degree-of-freedom robot arm, its inverse kinematics equation is:

[0060]

[0061] in, q m For [θ1 θ2 θ3 θ4 θ5 θ6] T . q b q m These represent the pose of the moving chassis and the six-axis joint angles of the robotic arm, respectively, K = [K P ,K O [ ] is the gain matrix of the position and attitude errors of the mobile robotic arm. Let I be the velocity vector of the end effector of the mobile robotic arm. n It is the identity matrix. With zero spatial velocity, optimize the pose q of the mobile robotic arm without affecting the position and orientation of the end effector of the mobile robotic arm;

[0062] Furthermore, In the formula, k0>0, ω(q) is the objective function for optimizing the pose variables of the mobile robotic arm, so that it moves along the target gradient direction;

[0063] The mechanical joint limit distance function is used as a secondary objective, i.e. In the formula The midpoint of the joint range value is determined by maximizing the distance to make it as close as possible to the midpoint of the range.

[0064] Given the initial and final poses of the mobile robotic arm's end effector, the poses are discretized using quaternion spherical linear interpolation. The interpolation formula is as follows:

[0065]

[0066] In the formula, θ is Q begin and Q end included angle;

[0067] In a human-machine collaborative environment, human-machine safety involves aspects such as position, speed, and robot geometry. Quantitative indicators from safety planning are used to measure the current level of safety between humans and machines. The hazard index is calculated as follows:

[0068] In the formula, K DV f is the non-negative hazard index assessment coefficient. d For distance influence factor, f v f is the speed influence factor. dcThis is the critical value for the distance influence factor;

[0069] Furthermore, the moving robotic arm and the human body contours are respectively enclosed by the smallest enclosing circle in the plane, and the distance d between the centers of the two enclosing circles is calculated. t According to the environment, pedestrian P i To the mobile robotic arm joint M i Distance D between the chassis O-shaped enclosure surface and the chassis O-shaped enclosure surface t =d t -R1-R2, where R1 and R2 are the minimum envelope circle radii of the human body and the mobile robotic arm contours, respectively;

[0070] Furthermore, the velocity influence factor f mentioned in step S4 v The calculation formula is:

[0071]

[0072] in, f is the factor affecting the joint rotation speed of the robotic arm. v (ω z ) represents the factor affecting the rotational speed of the mobile chassis, f v (v b The moving speed influence factor is calculated as follows:

[0073]

[0074]

[0075] In the formula, Let be the rotational speed of the i-th joint of the robotic arm. ω represents the minimum and maximum rotational speeds of the i-th joint of the robotic arm, respectively. z To adjust the rotational speed of the robotic arm joints and chassis, δ i μ i i = 1, 2 are adjustment factors, and the movement speed influence factor is calculated as follows:

[0076]

[0077] In the formula, v bmax v bmin These are the maximum and minimum moving speeds of the mobile chassis, respectively.

[0078] Furthermore, path planning is performed using the artificial potential field method based on the hazard index model; the resultant potential field function is as follows:

[0079] U sum =U att (q)+U rep (q)+U vreq (v ro )

[0080] In the formula, the combined potential field is composed of the superposition of the gravitational field, the repulsive field, and the dynamic repulsive field, as detailed below:

[0081] gravitational field

[0082] Static repulsive field

[0083] Dynamic repulsive field

[0084] Where ρ(q,q) goal ρ(q,q) represents the Euclidean distance between the current position of the moving robotic arm and the target position. obs d represents the Euclidean distance between the current position of the moving robotic arm and the position of the obstacle. rep ξ represents the range of influence of the repulsive force. ξ is the repulsive force coefficient, f(f d ,f v ) represents the current danger index value, k DI k is the risk index conversion coefficient. ro v is the relative velocity coefficient. ro Let be the relative velocity between the human and the machine, and α be the angle between their relative velocities. The virtual force calculated from the potential field is as follows:

[0085] gravitational

[0086] Static repulsion

[0087]

[0088] Dynamic repulsion When the calculated risk index between the human and the mobile robotic arm is greater than the given human-machine collision risk index threshold F, the dynamic repulsive field gradually increases, and the robot moves away from the danger source under the action of the combined repulsive field. When the risk index between the human and the robotic arm decreases to a safe level, the current pose is used as the new starting pose, and the above steps are repeated until the robot moves to the target position.

[0089] The present invention has the following advantages:

[0090] (1) In view of the shortcomings of the mobile robotic arm system widely used in current industrial production, which separates the control of the robotic arm and the mobile chassis, this invention establishes an overall inverse kinematic model of the robotic arm and the mobile chassis and establishes a corresponding risk index assessment model, which can effectively increase the safety of the mobile robotic arm in the overall operation process.

[0091] (2) In view of the shortcomings of the dynamic obstacle avoidance capability of the mobile robotic arm during the movement, this invention improves the dynamic artificial potential field method by establishing a hazard index assessment model and adjusting the dynamic repulsive field according to the hazard index to improve the dynamic obstacle avoidance performance of the mobile robotic arm, and ensures the operation efficiency and human-machine safety of the mobile robotic arm in a human-machine integrated environment. Attached Figure Description

[0092] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments:

[0093] Appendix Figure 1 This is a schematic diagram of the mobile robotic arm system of the present invention;

[0094] Appendix Figure 2 This is a simplified schematic diagram of the mobile robotic arm control system of the present invention;

[0095] Appendix Figure 3 This is a schematic diagram of the overall control flow of the present invention;

[0096] Appendix Figure 4 This is a schematic diagram of the system path planning of the present invention;

[0097] Appendix Figure 5 This is a schematic diagram of the collision bounding box model of the present invention;

[0098] Appendix Figure 6 This is a schematic diagram of the human-machine collaborative environment planning of the present invention;

[0099] Appendix Figure 7 This is a schematic diagram of the joint angles and wheel speeds of the mobile robotic arm of the present invention. Detailed Implementation

[0100] To more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are merely exemplary, and those skilled in the art can derive other embodiments based on the provided drawings without creative effort.

[0101] The structures, proportions, sizes, etc. illustrated in this specification are only for the purpose of assisting those skilled in the art in understanding and reading the content disclosed herein, and are not intended to limit the conditions under which the present invention can be implemented. Therefore, they have no substantial technical significance. Any modifications to the structure, changes in the proportions, or adjustments to the size, without affecting the effects and objectives that the present invention can produce, should still fall within the scope of the technical content disclosed in the present invention.

[0102] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. The specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0103] As shown in the figure, a method for human-machine safety path planning for a mobile robotic arm based on a risk index includes the following steps;

[0104] Step S1: Establish an external information perception system for the mobile robotic arm. This system includes a two-dimensional lidar system for detecting the distance to obstacles around the mobile robotic arm's motion system and a vision system for recognizing human body contours.

[0105] Step S2: Given the global map of the working area of ​​the mobile robotic arm and its starting and ending positions, plan the global path that the mobile robotic arm can reach.

[0106] Step S3: During the actual joint motion control process, if the vision system does not detect human information, the mobile robotic arm will move along the global path to the target position; if the vision system detects human information, the human contour coordinates are obtained by scanning with a two-dimensional lidar system, and the mobile robotic arm and the human contour are surrounded by the smallest enclosing circle in the plane respectively. The distance between the centers of the two enclosing circles is calculated to obtain the human-machine safety distance.

[0107] Step S4: Assess the human-machine hazard assessment index;

[0108] Step S5: Given a human-machine collision hazard index threshold, improve the path planning algorithm based on the calculated hazard index to guide the mobile robotic arm away from people in the scene.

[0109] Figure 1 This is a diagram illustrating the composition of the mobile robotic arm system of the present invention. Figure 1 As shown: the mobile chassis is a large Ridgeback; the robotic arm is a Rokax xMateCR7 six-axis flexible collaborative robotic arm; the robotic arm base is fixedly mounted on the mobile platform; a two-dimensional LiDAR is mounted on the front of the mobile chassis to sense obstacle information in the environment. The mobile robotic arm can be remotely controlled via PC or handheld controller to achieve human-machine interactive collaborative operation.

[0110] In step S1, the mobile robotic arm motion system includes a six-axis flexible collaborative robotic arm placed on a four-wheel omnidirectional moving chassis, with an electric gripper installed at the end of the robotic arm.

[0111] In step S1, the operator can remotely operate the mobile robotic arm via a control handle or PC.

[0112] In step S2, a global path that the mobile robotic arm can reach is planned according to Dijkstra's algorithm.

[0113] In step S2, a global map of the work area is input into the external information perception system of the mobile robotic arm. This global map of the work area is created by scanning the work scene with a two-dimensional LiDAR mounted on the chassis and using the Gmapping method to record the positions of static obstacles in the scene.

[0114] In step S3, the inverse kinematics of the mobile robotic arm is calculated based on the planned global path points in Cartesian coordinates to obtain joint control values, and joint motion control is performed accordingly.

[0115] In step S3, when the vision system detects human information and obtains the human contour coordinates through scanning by the two-dimensional LiDAR system, it encloses the moving robotic arm and the human contour with the smallest enclosing circle in the plane, and calculates the distance d between the centers of the two enclosing circles. t Thus, the safe distance D between humans and machines is obtained. t =d t -R1-R2, where R1 and R2 are the minimum envelope circle radii of the human body and the mobile robotic arm contours, respectively.

[0116] The mobile robotic arm is a robotic arm positioned on a mobile chassis;

[0117] In step S4, the human-machine hazard assessment index f(f) is calculated. d ,f v The specific method is as follows:

[0118]

[0119] In the formula, K DV f is the non-negative hazard index assessment coefficient. d For distance influence factor, f v f is the speed influence factor. dc This is the critical value for the distance influence factor;

[0120] Furthermore, the distance influence factor f d The calculation formula is

[0121]

[0122] Among them, L max ,L min These represent the maximum and minimum detection ranges of the lidar, D. max D min These represent the maximum and minimum safe distances allowed between humans and machines, respectively.

[0123] Furthermore, the speed influence factor f v The calculation formula is

[0124]

[0125] in, The factor affecting the joint rotation speed of the robotic arm. For factors affecting the rotational speed of the mobile chassis, As a factor affecting movement speed, Let be the rotational speed of the i-th joint of the robotic arm. ω represents the minimum and maximum rotational speeds of the i-th joint of the robotic arm, respectively. z The rotational speed of the mobile chassis is calculated using the following factors:

[0126]

[0127]

[0128] Where, δ i μ i i = 1, 2 are adjustment factors, and the movement speed influence factor is calculated as follows:

[0129]

[0130] Among them, v bmax v bmin These are the maximum and minimum moving speeds of the mobile chassis, respectively.

[0131] In step S5, the method for giving the human-machine collision hazard index threshold F is as follows: when f(f d f v When f(f) ≥ F, the mobile robotic arm undergoes improved artificial potential field method dynamic path planning and inverse kinematics calculation to obtain new joint control values ​​and perform joint motion control; when f(f) ≥ F, the mobile robotic arm undergoes improved artificial potential field method dynamic path planning and inverse kinematics calculation to obtain new joint control values ​​and perform joint motion control; d f v When F < F, take the current position of the moving robotic arm as the starting position and repeat steps S2 to S5.

[0132] The gravitational field calculation formula for the improved dynamic artificial potential field method described in step S5 is as follows:

[0133] gravitational field:

[0134] Among them, q, q obs q goal These represent the current position of the moving robotic arm, the position of the obstacle, and the position of the target, respectively, μ att d is the gravitational coefficient. att The range of gravitational influence;

[0135] The repulsive field is formed by the combined action of a static repulsive field and a dynamic repulsive field, and the calculation formulas are as follows:

[0136] Static repulsive field:

[0137] Dynamic repulsive field:

[0138] Where ρ(q,q) goal ρ(q,q) represents the Euclidean distance between the current position of the moving robotic arm and the target position. obs d represents the Euclidean distance between the current position of the moving robotic arm and the position of the obstacle. rep ξ represents the range of influence of the repulsive force. ξ is the repulsive force coefficient, f(f d ,f v ) represents the current danger index value, k DI k is the risk index conversion coefficient. ro v is the relative velocity coefficient. ro Let α be the relative velocity between the human and the machine, and α be the angle between their relative velocities.

[0139] When the mobile robotic arm motion system includes a six-axis flexible collaborative robotic arm mounted on a four-wheel omnidirectional chassis, and the end effector of the robotic arm is provided, the planning method is specifically as follows:

[0140] Figure 2 This is a simplified diagram of the mobile robotic arm control system of the present invention. Figure 3 This is the overall control flowchart of the present invention. Figure 2 As shown, a kinematic model of the mobile robotic arm is established based on the DH method, and the rotation transformation matrix of the robotic arm is calculated. in Calculate the Jacobian matrix of the mobile robotic arm based on the rotation transformation matrix. and its pseudo-inverse matrix

[0141] The Jacobian matrix of the mobile robotic arm is composed of the chassis Jacobian matrix. Jacobian matrix of robotic arm It consists of two parts. Based on the formula above, the Jacobian matrix of the mobile robotic arm is first solved using the vector product method. The steps are as follows: first, calculate the action vector of a single joint on the end effector of the moving robotic arm as the Jacobian matrix. A separate column, then the effects of all joints on the end effector of the moving robotic arm are superimposed;

[0142] Jacobian matrix column i for:

[0143]

[0144] In the formula, z i p is the third column of the i-th rotation transformation matrix.E The homogeneous transformation matrix The column vector consisting of the first three elements of the fourth column, p i for The column vector consisting of the first 3 elements of the 4th column;

[0145] Jacobian matrix J b and As shown in the following formula:

[0146]

[0147]

[0148]

[0149] In the formula, Let P(P) be the rotation transformation matrix from the world coordinate system to the robot arm base coordinate system. x ,P y Let P be the two-dimensional coordinates. OP is the distance from the center of the mobile chassis base to the center of the robotic arm mounting base. For the mobile base, such as... Figure 1 As shown in (b), its kinematic equations are:

[0150]

[0151] In the formula, The angle at which the chassis is oriented, and the angular velocity of the chassis.

[0152] As attached Figure 1 As shown in (b). Based on the above formula, the chassis movement speed is: The rotation transformation matrix of the mobile robotic arm is: The rotation transformation matrix is:

[0153]

[0154]

[0155] In the formula d E Let be the distance from the end effector to the origin of the robot arm's coordinate system. Since the position and motion of the end effector are determined jointly by the moving chassis and the six-degree-of-freedom robot arm, its inverse kinematics equation is:

[0156]

[0157] in, q m For [θ1 θ2 θ3 θ4 θ5 θ6] T . q b q mThese represent the pose of the moving chassis and the six-axis joint angles of the robotic arm, respectively, K = [K P ,K O [ ] is the gain matrix of the position and attitude errors of the mobile robotic arm. Let I be the velocity vector of the end effector of the mobile robotic arm. n It is the identity matrix. With zero spatial velocity, optimize the pose q of the mobile robotic arm without affecting the position and orientation of the end effector of the mobile robotic arm;

[0158] Furthermore, In the formula, k0>0, ω(q) is the objective function for optimizing the pose variables of the mobile robotic arm, so that it moves along the target gradient direction;

[0159] The mechanical joint limit distance function is used as a secondary objective, i.e. In the formula The midpoint of the joint range value is determined by maximizing the distance to make it as close as possible to the midpoint of the range.

[0160] Given the initial and final poses of the mobile robotic arm's end effector, the poses are discretized using quaternion spherical linear interpolation. The interpolation formula is as follows:

[0161]

[0162] In the formula, θ is Q begin and Q end included angle;

[0163] In a human-machine collaborative environment, human-machine safety involves aspects such as position, speed, and robot geometry. Quantitative indicators from safety planning are used to measure the current level of safety between humans and machines. The hazard index is calculated as follows:

[0164]

[0165] In the formula, K DV f is the non-negative hazard index assessment coefficient. d For distance influence factor, f v f is the speed influence factor. dc This is the critical value for the distance influence factor;

[0166] Furthermore, the moving robotic arm and the human body contours are respectively enclosed by the smallest enclosing circle in the plane, and the distance d between the centers of the two enclosing circles is calculated. t According to the environment, pedestrian P i To the mobile robotic arm joint M i Distance D between the chassis O-shaped enclosure surface and the chassis O-shaped enclosure surface t =d t -R1-R2, where R1 and R2 are the minimum envelope circle radii of the human body and the mobile robotic arm contours, respectively;

[0167] Furthermore, the velocity influence factor f mentioned in step S4 v The calculation formula is:

[0168]

[0169] in, f is the factor affecting the joint rotation speed of the robotic arm. v (ω z ) represents the factor affecting the rotational speed of the mobile chassis, f v (v b The moving speed influence factor is calculated as follows:

[0170]

[0171]

[0172] In the formula, Let be the rotational speed of the i-th joint of the robotic arm. ω represents the minimum and maximum rotational speeds of the i-th joint of the robotic arm, respectively. z To adjust the rotational speed of the robotic arm joints and chassis, δ i μ i i = 1, 2 are adjustment factors, and the movement speed influence factor is calculated as follows:

[0173]

[0174] In the formula, v bmax v bmin These are the maximum and minimum moving speeds of the mobile chassis, respectively.

[0175] Furthermore, path planning is performed using the artificial potential field method based on the hazard index model; the resultant potential field function is as follows:

[0176] U sum =U att (q)+U rep (q)+U vreq (v ro )

[0177] In the formula, the combined potential field is composed of the superposition of the gravitational field, the repulsive field, and the dynamic repulsive field, as detailed below:

[0178] gravitational field

[0179] Static repulsive field

[0180] Dynamic repulsive field

[0181] Where ρ(q,q)goal ρ(q,q) represents the Euclidean distance between the current position of the moving robotic arm and the target position. obs d represents the Euclidean distance between the current position of the moving robotic arm and the position of the obstacle. rep ξ represents the range of influence of the repulsive force. ξ is the repulsive force coefficient, f(f d ,f v ) represents the current danger index value, k DI k is the risk index conversion coefficient. ro v is the relative velocity coefficient. ro Let be the relative velocity between the human and the machine, and α be the angle between their relative velocities. The virtual force calculated from the potential field is as follows:

[0182] gravitational

[0183] Static repulsion

[0184]

[0185] Dynamic repulsion When the calculated risk index between the human and the mobile robotic arm is greater than the given human-machine collision risk index threshold F, the dynamic repulsive field gradually increases, and the robot moves away from the danger source under the action of the combined repulsive field. When the risk index between the human and the robotic arm decreases to a safe level, the current pose is used as the new starting pose, and the above steps are repeated until the robot moves to the target position.

Claims

1. A method for human-machine safety path planning for a mobile robotic arm based on a hazard index, characterized in that: Includes the following steps; Step S1: Establish an external information perception system for the mobile robotic arm. This system includes a two-dimensional lidar system for detecting the distance to obstacles around the mobile robotic arm's motion system and a vision system for recognizing human body contours. Step S2: Given the global map of the working area of ​​the mobile robotic arm and its starting and ending positions, plan the global path that the mobile robotic arm can reach. Step S3: During the actual joint motion control process, if the vision system does not detect human information, the mobile robotic arm will move along the global path to the target position; if the vision system detects human information, the human contour coordinates are obtained by scanning with a two-dimensional lidar system, and the mobile robotic arm and the human contour are surrounded by the smallest enclosing circle in the plane respectively. The distance between the centers of the two enclosing circles is calculated to obtain the human-machine safety distance. Step S4: Assess the human-machine hazard assessment index; Step S5: Given a human-machine collision hazard index threshold, improve the path planning algorithm based on the calculated hazard index to guide the mobile robotic arm away from people in the scene; In step S4, the human-machine hazard assessment index f(f) is calculated. d ,f v The specific method is as follows: In the formula, K DV f is the non-negative hazard index assessment coefficient. d For distance influence factor, f v f is the speed influence factor. dc This is the critical value for the distance influence factor; Furthermore, the distance influence factor f d The calculation formula is Among them, L max ,L min These represent the maximum and minimum detection ranges of the lidar, D. max D min These represent the maximum and minimum safe distances allowed between humans and machines, respectively. Furthermore, the speed influence factor f v The calculation formula is in, The factor affecting the joint rotation speed of the robotic arm. For factors affecting the rotational speed of the mobile chassis, As a factor affecting movement speed, Let be the rotational speed of the i-th joint of the robotic arm. ω represents the minimum and maximum rotational speeds of the i-th joint of the robotic arm, respectively. z The rotational speed of the mobile chassis is calculated using the following factors: Where, δ i μ i i = 1, 2 are adjustment factors, and the movement speed influence factor is calculated as follows: Among them, v bmax v bmin These are the maximum and minimum moving speeds of the mobile chassis, respectively.

2. The method for human-machine safety path planning of a mobile robotic arm based on a hazard index according to claim 1, characterized in that: In step S1, the mobile robotic arm motion system includes a six-axis flexible collaborative robotic arm placed on a four-wheel omnidirectional moving chassis, with an electric gripper installed at the end of the robotic arm.

3. The method for human-machine safety path planning of a mobile robotic arm based on a hazard index according to claim 2, characterized in that: In step S1, the operator can remotely operate the mobile robotic arm via a control handle or PC.

4. The method for human-machine safety path planning of a mobile robotic arm based on a hazard index according to claim 1, characterized in that: In step S2, a global path that the mobile robotic arm can reach is planned according to Dijkstra's algorithm.

5. The method for human-machine safety path planning of a mobile robotic arm based on a hazard index according to claim 1, characterized in that: In step S2, a global map of the work area is input into the external information perception system of the mobile robotic arm. This global map of the work area is created by scanning the work scene with a two-dimensional LiDAR mounted on the chassis and using the Gmapping method to record the positions of static obstacles in the scene.

6. The method for human-machine safety path planning of a mobile robotic arm based on a hazard index according to claim 1, characterized in that: In step S3, the inverse kinematics of the mobile robotic arm is calculated based on the planned global path points in Cartesian coordinates to obtain joint control values, and joint motion control is performed accordingly.

7. The method for human-machine safety path planning of a mobile robotic arm based on a hazard index according to claim 6, characterized in that: In step S3, when the vision system detects human information and obtains the human contour coordinates through scanning by the two-dimensional LiDAR system, it encloses the moving robotic arm and the human contour with the smallest enclosing circle in the plane, and calculates the distance d between the centers of the two enclosing circles. t Thus, the safe distance D between humans and machines is obtained. t =d t -R1-R2, where R1 and R2 are the minimum envelope circle radii of the human body and the mobile robotic arm contours, respectively.

8. The method for human-machine safety path planning of a mobile robotic arm based on a hazard index according to claim 7, characterized in that: The mobile robotic arm is a robotic arm placed on a mobile chassis.

9. The method for human-machine safety path planning of a mobile robotic arm based on a hazard index according to claim 8, characterized in that: In step S5, the method for setting the threshold F of the human-machine collision risk index is as follows: when f(f d , f v ) ≥ F, perform improved artificial potential field method dynamic path planning and inverse kinematics calculation on the mobile manipulator to obtain new joint control values and perform joint motion control; when f(f d , f v ) < F, take the current position of the mobile manipulator as the starting position and repeat steps S2 to S5; The gravitational field calculation formula of the improved artificial potential field method described in step S5 is as follows: Among them, q, q obs q goal These represent the current position of the moving robotic arm, the position of the obstacle, and the position of the target, respectively, μ att d is the gravitational coefficient. att The range of gravitational influence; The repulsive field is formed by the combined action of a static repulsive field and a dynamic repulsive field. The calculation formulas are as follows: Static repulsive field: Dynamic repulsive field: Where ρ(q,q) goal ρ(q,q) represents the Euclidean distance between the current position of the moving robotic arm and the target position. obs d represents the Euclidean distance between the current position of the moving robotic arm and the position of the obstacle. rep ξ is the range of influence of the repulsive force; f(f) is the repulsive force coefficient. d ,f v ) represents the current danger index value, k DI k is the risk index conversion coefficient. ro v is the relative velocity coefficient. ro Let α be the relative velocity between the human and the machine, and α be the angle between their relative velocities.

10. The method for human-machine safety path planning of a mobile robotic arm based on a hazard index according to claim 8, characterized in that: When the mobile robotic arm motion system includes a six-axis flexible collaborative robotic arm mounted on a four-wheel omnidirectional chassis, and the end effector of the robotic arm is provided, the planning method is specifically as follows: A kinematic model of the mobile robotic arm is established based on the DH method, and the rotation transformation matrix of the robotic arm is calculated. in Calculate the Jacobian matrix of the mobile robotic arm based on the rotation transformation matrix. and its pseudo-inverse matrix The Jacobian matrix of the mobile robotic arm is composed of the chassis Jacobian matrix. Jacobian matrix of robotic arm It consists of two parts. Based on the formula above, the Jacobian matrix of the mobile robotic arm is first solved using the vector product method. The steps are as follows: first, calculate the action vector of a single joint on the end effector of the moving robotic arm as the Jacobian matrix. A separate column, then the effects of all joints on the end effector of the moving robotic arm are superimposed; Jacobian matrix column i for: In the formula, z i p is the third column of the i-th rotation transformation matrix. E The homogeneous transformation matrix The column vector consisting of the first three elements of the fourth column, p i for The column vector consisting of the first 3 elements of the 4th column; Jacobian matrix J b and As shown in the following formula: In the formula, Let P(P) be the rotation transformation matrix from the world coordinate system to the robot arm base coordinate system. x ,P y Let P be the two-dimensional coordinates; OP be the distance from the center of the mobile chassis base to the center of the robotic arm mounting base; for the mobile base, its kinematic equation is: In the formula, The angle at which the chassis is oriented, and the angular velocity of the chassis. Based on the chassis movement speed in the above formula: The rotation transformation matrix of the mobile robotic arm is: The rotation transformation matrix is: In the formula d E Let be the distance from the end effector to the origin of the robot arm's coordinate system; since the position and motion of the end effector gripper are jointly determined by the moving chassis and the six-degree-of-freedom robot arm, its inverse kinematics equation is: in, q m [θ1θ2θ3θ4θ5θ6] T q b q m These represent the pose of the moving chassis and the six-axis joint angles of the robotic arm, respectively, K = [K P ,K O [ ] is the gain matrix of the position and attitude errors of the mobile robotic arm. Let I be the velocity vector of the end effector of the mobile robotic arm. n It is the identity matrix. With zero spatial velocity, optimize the pose q of the mobile robotic arm without affecting the position and orientation of the end effector of the mobile robotic arm; Furthermore, In the formula, k0>0, ω(q) is the objective function for optimizing the pose variables of the mobile robotic arm, so that it moves along the target gradient direction; The mechanical joint limit distance function is used as a secondary objective, i.e. In the formula The midpoint of the joint range value is used to maximize the distance to make it as close as possible to the midpoint of the range. Given the initial and final poses of the mobile robotic arm's end effector, the poses are discretized using quaternion spherical linear interpolation. The interpolation formula is as follows: In the formula, θ is Q begin and Q end included angle; In a human-machine collaborative environment, human-machine safety involves aspects such as position, speed, and robot geometry. Quantitative indicators from safety planning are used to measure the current level of safety between humans and machines. The hazard index is calculated as follows: In the formula, K DV f is the non-negative hazard index assessment coefficient. d For distance influence factor, f v f is the speed influence factor. dc This is the critical value for the distance influence factor; Furthermore, the moving robotic arm and the human body contours are respectively enclosed by the smallest enclosing circle in the plane, and the distance d between the centers of the two enclosing circles is calculated. t According to the environment, pedestrian P i To the mobile robotic arm joint M i Distance D between the chassis O-shaped enclosure surface and the chassis O-shaped enclosure surface t =d t -R1-R2, where R1 and R2 are the minimum envelope circle radii of the human body and the mobile robotic arm contours, respectively; Furthermore, the velocity influence factor f mentioned in step S4 v The calculation formula is: in, f is the factor affecting the joint rotation speed of the robotic arm. v (ω z ) represents the factor affecting the rotational speed of the mobile chassis, f v (v b The moving speed influence factor is calculated as follows: In the formula, Let be the rotational speed of the i-th joint of the robotic arm. ω represents the minimum and maximum rotational speeds of the i-th joint of the robotic arm, respectively. z To adjust the rotational speed of the robotic arm joints and chassis, δ i μ i i = 1, 2 are adjustment factors, and the movement speed influence factor is calculated as follows: In the formula, v bmax v bmin These are the maximum and minimum moving speeds of the mobile chassis, respectively. Furthermore, path planning is performed using the artificial potential field method based on the hazard index model; the resultant potential field function is as follows: U sum =U att (q)+U rep (q)+U vreq (v ro ) In the formula, the combined potential field is composed of the superposition of the gravitational field, the repulsive field, and the dynamic repulsive field, as detailed below: gravitational field Static repulsive field Dynamic repulsive field Where ρ(q,q) goal ρ(q,q) represents the Euclidean distance between the current position of the moving robotic arm and the target position. obs d represents the Euclidean distance between the current position of the moving robotic arm and the position of the obstacle. rep ξ is the range of influence of the repulsive force; f(f) is the repulsive force coefficient. d ,f v ) represents the current danger index value, k DI k is the risk index conversion coefficient. ro v is the relative velocity coefficient. ro Let be the relative velocity between the human and the machine, and α be the angle between their relative velocities. The virtual force calculated from the potential field is as follows: gravitational Static repulsion Dynamic repulsion When the calculated risk index between the human and the mobile robotic arm is greater than the given human-machine collision risk index threshold F, the dynamic repulsive field gradually increases, and the robot moves away from the danger source under the action of the combined repulsive field. When the risk index between the human and the robotic arm decreases to a safe level, the current pose is used as the new starting pose, and the above steps are repeated until the robot moves to the target position.

Images