An anthropomorphic motion online planning method for humanoid robotic arms

Abstract

This invention provides a method for anthropomorphic online motion planning of humanoid robotic arms. The method includes: constructing a hierarchical humanoid robotic arm anthropomorphic online motion planning framework comprising a configuration layer, a path layer, and a trajectory layer; collecting natural human upper limb movement samples; constructing a configuration anthropomorphic generation model based on natural arm shape angle constraints of the shoulder, elbow, and wrist; constructing an online path planning method based on the configuration anthropomorphic generation model, mapping Cartesian space paths to joint space paths; constructing a real-time generation method for the anthropomorphic trajectory layer based on the rolling field of view and the minimum Jerk criterion; and outputting robotic arm control commands. This invention decomposes the anthropomorphic motion planning problem of humanoid robotic arms, introducing human configuration features, path features, and minimum Jerk time characteristics into the configuration layer, path layer, and trajectory layer, respectively, enabling the robotic arm to maintain human motion patterns, achieving reasonable joint postures, continuous and smooth paths, and natural and fluid trajectories, thus improving the anthropomorphism of the motion.

Description

Technical Field

[0001] This invention belongs to the field of motion planning for humanoid robotic arms, and relates to an anthropomorphic online motion planning method for humanoid robotic arms. Background Technology

[0002] With the development of human-robot collaborative manufacturing, service robots, and collaborative robot technologies, robotic arms are no longer confined to closed, isolated industrial production line environments, but are increasingly entering open environments shared with humans. However, most existing robotic arm motion planning methods revolve around shortest path, shortest time, collision avoidance, or precise end-effector pose tracking. While these methods can meet the functional requirements of traditional automation tasks, they typically treat the redundant degrees of freedom of the robotic arm merely as tools for obstacle avoidance, singularity avoidance, or joint constraint handling, lacking modeling of humanoid configuration selection mechanisms. For a seven-DOF humanoid robotic arm, multiple sets of inverse kinematic solutions often exist under the same end-effector pose conditions. Without effective humanoid constraints, the system is prone to generating abnormal elbow elevation, stiff wrist posture, or disjointed joint coordination movements, failing to reflect the structural advantages of the humanoid robotic arm and posing a threat to human safety.

[0003] On the other hand, existing anthropomorphic motion generation methods mostly employ offline teaching and reproduction, single-layer cost function optimization, or learning strategies targeting a specific type of action. These methods typically only reflect human-like characteristics at one level—the configuration layer, path layer, or trajectory layer—making it difficult to simultaneously achieve a comprehensive effect of "human-like posture, human-like path, and human-like speed changes" throughout the entire task execution process. Especially in dynamic target tracking and human-computer interaction scenarios, the target is constantly changing. If offline planning or full-segment replanning is still used, computational costs will be high, control commands will be prone to sudden changes, and motion interruptions or jitter will occur.

[0004] Therefore, how to establish an online motion planning method that integrates anthropomorphism at the configuration layer, path layer, and trajectory layer for humanoid robotic arms, and how to ensure that the method can maintain smooth continuity and real-time response as the target is constantly updated, remains a key technical problem that urgently needs to be solved in this field. Summary of the Invention

[0005] In view of this, the present invention provides an anthropomorphic motion online planning method for humanoid robotic arms. By constructing a hierarchical online planning framework including a configuration layer, a path layer, and a trajectory layer, the method enables the generation of natural, smooth, and online-updable motion of the humanoid robotic arm toward dynamic targets.

[0006] This invention provides an anthropomorphic motion online planning method for humanoid robotic arms, including:

[0007] Step S1: Construct a hierarchical humanoid robotic arm anthropomorphic online motion planning framework that includes a configuration layer, a path layer, and a trajectory layer;

[0008] Step S2: Based on the hierarchical humanoid robotic arm anthropomorphic online motion planning framework, collect natural motion samples of human upper limbs and construct an anthropomorphic generative model of configuration based on the natural arm shape angle constraints of shoulder, elbow, and wrist.

[0009] Step S3: Based on the anthropomorphic generation model of the configuration, construct an online path planning method to map the Cartesian space path to the joint space path;

[0010] Step S4: Based on the joint space path, construct a real-time generation method for anthropomorphic trajectory layers based on the rolling field of view and the minimum Jerk criterion, and output robotic arm control commands.

[0011] In the method, step S1 includes:

[0012] Step S1.1 Define the system state variables of the humanoid robotic arm. The system state variables include joint angle, joint angular velocity, joint angular acceleration, end pose, and target pose information;

[0013] Step S1.2 Based on the system state variables A hierarchical humanoid robotic arm anthropomorphic online motion planning framework is constructed, which decouples the online motion planning process of the humanoid robotic arm from the current end pose to the target end pose into a configuration layer, a path layer and a trajectory layer connected in sequence according to the anthropomorphic motion generation process.

[0014] Step S1.3 Establish an anthropomorphic configuration generation model at the configuration layer. Includes natural arm-shaped prediction unit and natural arm type inverse solution unit The natural arm-shaped prediction unit Based on the robotic arm's end-effector pose and historical end-effector pose information, the reference natural arm shape angle is output. The natural arm-shaped inverse kinematic unit Based on the end-effector pose and reference natural arm angle Solve for the set of candidate joint configurations that satisfy the end-effector pose conditions. ;

[0015] Step S1.4 Establish a joint path planning method at the path layer, based on the system state variables. A Cartesian space reference path is generated from the current end-effector pose and the target end-effector pose, and the pose path point sequence is discretized. Using the anthropomorphic configuration generation model of the configuration layer Solve the pose path point sequence Corresponding joint configuration path candidate sequence From joint configuration path candidate sequences Determine the executable joint space path sequence ;

[0016] Step S1.5 Establish a real-time joint trajectory generation method in the trajectory layer, based on the joint spatial path sequence. This involves solving the relationship between joint position and joint velocity changes over time during the motion execution of the humanoid robotic arm, and generating the current control cycle. Internal continuous joint trajectory ;

[0017] Step S1.6 Establish an online update mechanism for the hierarchical humanoid robotic arm's anthropomorphic online motion planning framework, when the system state variables... When the target pose changes, an update is triggered. The executed trajectory segments are retained, and the current execution state of the robotic arm is used as the new planning starting point to re-plan the configuration layer, path layer, and trajectory layer, thereby realizing the humanoid robotic arm's anthropomorphic online motion facing the target pose change.

[0018] In the method, step S2 includes:

[0019] Step S2.1 Based on the geometric relationship between the center of the shoulder joint, the center of the elbow joint, and the center of the wrist joint of the humanoid robotic arm, define the arm angle used to characterize the redundant configuration. The arm-shaped angle Used to describe the spatial orientation of the robotic arm elbow relative to the line connecting the shoulder and wrist, given the end-of-task pose determination condition;

[0020] Step S2.2 Obtain sample data to characterize the natural movement features of the human upper limb. The sample data includes the end-effector pose sequence and natural arm angle sequence of the humanoid robotic arm's motion trajectory, forming a training sample set. ,in Indicates the first The task end pose and historical end pose sequences in the group sample. Indicates the first The natural arm-shaped angles corresponding to the group of samples;

[0021] Step S2.3 Using the training sample set As training input, construct and train a natural arm shape prediction unit. The natural arm-shaped prediction unit This includes either a neural network model or a mapping model based on teaching data, used to establish a mapping relationship between the task end pose and the historical end pose sequence and the natural arm shape angle;

[0022] Step S2.4 In the current control cycle The task end-effector pose and historical end-effector pose sequences are input into the natural arm shape prediction unit. Output the current control cycle Corresponding reference natural arm angle ;

[0023] Step S2.5 Constructing the natural arm-shaped inverse kinematic unit The reference natural arm angle As a basis for redundancy degree of freedom allocation, a pool of candidate joint configurations that satisfy end-effector pose constraints and joint constraints is solved. ;

[0024] Step S2.6 Based on the trained natural arm-shaped angle prediction unit Inverse kinematic unit with natural arm Constructing anthropomorphic configuration generative model .

[0025] In the method, step S3 includes:

[0026] Step S3.1 Generate a reference path in Cartesian space based on the current end-effector pose and the target pose; the reference path is one of a straight line segment, a spline curve, or a Bézier curve.

[0027] Step S3.2 Discretize the reference path according to a preset step size or an adaptive step size to obtain a pose path point sequence. ;

[0028] Step S3.3 For the pose path point sequence Each discrete point in the model is generated according to the anthropomorphic configuration generation model of the configuration layer. Generate corresponding joint configuration path candidate sequences ;

[0029] Step S3.4 Construct a path layer optimization model using the rate of change in adjacent joint configurations as a constraint:

[0030]

[0031] in, This represents the optimized joint space path sequence. Indicates the first The joint configuration corresponding to each discrete path point, for ,Pick , The initial configuration of the path starting point for the humanoid robotic arm;

[0032] Step S3.5: Solve the path layer optimization model using one of the following methods: dynamic programming, graph search, or point-by-point rolling optimization. Determine the joint configuration combination that minimizes the change in adjacent joint configurations, and form a joint space path sequence. ;

[0033] Step S3.6 When the target pose changes, retain the executed path segments and use the current execution state as the new planning starting point to regenerate the pose path point sequence and the corresponding joint space path sequence for the unexecuted path segments, so as to ensure the configuration continuity and path continuity in the online planning process.

[0034] In the method, step S4 includes:

[0035] Step S4.1 Set the scroll view window length and rolling update cycle In each control cycle Within this process, the unexecuted segments of the spatial joint path are renumbered to form the executed joint path. ,from Select the joint space path segment within the currently scrolling view window. ;

[0036] Step S4.2 Scroll the view window length Using the start and end positions, start and end velocities, and start and end accelerations as boundary conditions, a minimum Jerk criterion smooth trajectory optimization model is constructed:

[0037]

[0038] in, This represents the optimized trajectory function. Indicates from the first Path points The movement reached the first Path points Local trajectory segments, Indicates the first Joint jerk of the segment trajectory;

[0039] Step S4.3: Using heuristic search or quadratic programming methods, solve the minimum Jerk criterion smooth trajectory optimization model to obtain... ;

[0040] Step S4.4 When the target pose changes, new boundary conditions are established at the splicing point at the end of the executed trajectory, the local joint trajectory in the subsequent window is re-solved, and the position, velocity and acceleration of the trajectory at the splicing point are ensured to be continuous.

[0041] Step S4.5 converts the local joint trajectory function generated by the rolling process into joint position commands and joint velocity commands, and sends them to the underlying controller to drive the humanoid robotic arm to complete online anthropomorphic motion. Attached Figure Description

[0042] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without any creative effort.

[0043] Figure 1 This is a flowchart illustrating an online motion planning method for a humanoid robotic arm provided in an embodiment of the present invention.

[0044] Figure 2 This is a schematic diagram of the seven-degree-of-freedom humanoid robotic arm structure used in the embodiments of the present invention;

[0045] Figure 3 This is a schematic diagram illustrating the definition of the arm-shaped angle used in an embodiment of the present invention;

[0046] Figure 4 This is a schematic diagram of online generation of joint space path based on natural arm shape constraint provided in an embodiment of the present invention;

[0047] Figure 5 This is a schematic diagram illustrating the joint path generation process in an example of the present invention;

[0048] Figure 6 This is a comparison chart of the trajectory curve generated by the example method of this invention and the trajectory curve generated by the toppra method. Specific Implementation

[0049] To better understand the technical solution of the present invention, the embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0050] It should be understood that the described embodiments are only some embodiments of the present invention, and not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0051] refer to Figure 1 This embodiment provides an online motion planning method for humanoid robotic arms. For example... Figure 2 As shown, the humanoid robotic arm includes shoulder joints J1, J2, and J3, elbow joint J4, and wrist joints J5, J6, and J7, and their joint states are represented as follows:

[0052]

[0053] in, This represents the joint state vector of the humanoid robotic arm. Indicates the first The joint angle of each joint.

[0054] A method for online anthropomorphic motion planning for humanoid robotic arms includes the following steps:

[0055] Step A1: Construct a hierarchical humanoid robotic arm online motion planning framework comprising a configuration layer, a path layer, and a trajectory layer.

[0056] Step A1.1 Obtain the current joint angles of the robotic arm Joint speed Joint acceleration , terminal pose and target pose This constitutes the current planning cycle. corresponding state vector ;

[0057] Step A1.2 Based on the system state variables The humanoid robotic arm will be moved from its current end-effector pose. Movement to the target end pose The online motion planning process is decoupled into a configuration layer, a path layer, and a trajectory layer, which are connected sequentially according to the anthropomorphic motion generation process.

[0058] Step A1.3 Configuration layer: Establish an anthropomorphic configuration generation model The anthropomorphic configuration generation model Includes natural arm-shaped prediction unit and natural arm type inverse solution unit The natural arm shape prediction unit outputs a reference natural arm shape angle based on the end-effector pose and historical pose information. The natural arm-shaped guided inverse kinematic unit Based on the end-effector pose and reference natural arm angle Solve for the set of candidate joint configurations that satisfy the end-effector pose conditions. ;

[0059] Step A1.4 Path Layer: Establish a joint path planning method based on the system state variables. Current end pose and target end pose Generate a Cartesian space reference path and discretize it to obtain a sequence of pose path points. Using the anthropomorphic configuration generation model of the configuration layer Solve the pose path point sequence Corresponding joint configuration path candidate sequence From joint configuration path candidate sequences Determine the executable joint space path sequence ;

[0060] Step A1.5 Trajectory Layer: Establish a real-time joint trajectory generation method based on the system state variables. Current joint velocity Joint acceleration and the anthropomorphic path sequence in joint space output by the path layer Generate the current control cycle Internal continuous joint trajectory The joint trajectory It is used to describe the relationship between the joint position and joint speed of a humanoid robotic arm and time during the motion execution process.

[0061] Step A1.6 Establish an online update mechanism when system status variables... When the target pose changes, an update is triggered. The executed trajectory segment is retained, and the current execution state of the robotic arm is used as the new planning starting point to re-plan the configuration, path, and trajectory, thereby realizing humanoid online motion planning of the robotic arm in response to target changes.

[0062] Step A2: Based on the aforementioned hierarchical anthropomorphic online motion planning framework, construct a configuration anthropomorphic generative model based on natural arm shape constraints, which specifically includes the following steps:

[0063] Step A2.1 as follows Figure 3 As shown, the coordinate system of the humanoid robotic arm is defined as B, and the center of the shoulder joint S, the center of the elbow joint E, and the center of the wrist joint W are located at the intersection of the axes of joints J2, J4, and J6, respectively.

[0064] Step A2.2 Definition As the reference vector, The shoulder-elbow vector. The elbow and wrist vectors are... For shoulder and wrist vectors, and The plane formed is called the reference plane. and The plane formed is called the arm plane, and the angle between the reference plane and the arm plane is defined as the arm angle. Its mathematical meaning is that the reference plane is around The angle of rotation to the arm plane, the direction according to The right-hand rule determines this;

[0065] Step A2.3 Based on the humanoid robotic arm dragging teaching, and based on the arm angle... The definition refers to collecting sample data to characterize the natural movement features of the human upper limbs, wherein the sample data includes the task end pose and historical end pose sequences. and its corresponding natural arm angle To form a training sample set ;

[0066] Step A2.4 Constructing an arm-shaped angle prediction unit based on a Long Short-Term Memory (LSTM) network The input is the length. Task end pose and historical end pose sequence The output is the corresponding reference natural arm angle. By introducing an LSTM (Long Short-Term Memory) network to predict the reference natural arm angle, the redundant degrees of freedom of the humanoid robotic arm are no longer allocated based solely on arbitrary results of numerical calculations, but are instead allocated towards an elbow posture distribution that conforms to the teaching patterns of human upper limbs.

[0067] Step A2.5 Constructing the inverse kinematic unit of the humanoid robotic arm's natural arm shape The input is the end-effector pose. Compared with the reference natural arm angle The output is the corresponding joint angle candidate pool. The specific steps are as follows:

[0068] Step A2.5.1 Set the end-effector pose. Transform into matrix form:

[0069]

[0070] in, It is in 4×4 matrix form. It is a 3×3 end attitude matrix. It is a 3×1 position vector;

[0071] Step A2.5.2 Calculate the target vector from the shoulder to the wrist based on the robotic arm's geometric parameters:

[0072]

[0073] in, This is the offset vector from the base to the shoulder in the base coordinate system. This is the offset vector from the wrist to the tool end in the 7th joint coordinate system.

[0074] Calculate the shoulder-wrist distance:

[0075]

[0076] in, Representing vectors Representation in the base coordinate system.

[0077] Step A2.5.3 Solve for the elbow joint J4. The value is calculated using the Law of Cosines:

[0078]

[0079] This results in two sets of elbow configurations: , The two sets of solutions correspond to different elbow flexion directions.

[0080] Step A2.5.4 Solve for the reference plane normal vector, following these steps:

[0081] Calculate the unit direction vector from shoulder to wrist:

[0082]

[0083] And construct the normal vector of the reference arm plane based on the base-to-shoulder vector and the shoulder-to-wrist vector:

[0084]

[0085] Normalization yields: .

[0086] Step A2.5.5 Solve for the shoulder joints J1, J2, and J3. The value is calculated according to the following steps:

[0087] Determine the J4 joint of the elbow. Then, for each elbow configuration, construct the shoulder posture matrix when the reference arm angle is zero. ,in The calculation is as follows:

[0088]

[0089] Further define the coefficient matrix:

[0090]

[0091] in, It is by The constructed antisymmetric matrix;

[0092] Then refer to the natural arm angle as The shoulder posture matrix at that time is:

[0093]

[0094] in, The shoulder joint posture matrix represents the position of the shoulder joint when the natural arm angle is determined. ;

[0095] Since the matrix elements on both sides of the equation are the same, the joint J2 can be solved. value:

[0096]

[0097] For any Calculate joints J1 and J3. value:

[0098]

[0099] in, Representation matrix The Middle Line number Column element values; Represents the shoulder joint angle branch;

[0100] Therefore, for each elbow joint angle branch, two sets of candidate shoulder joint angle values ​​can be obtained.

[0101] Step A2.5.6 Solve for the wrist joints J5, J6, and J7. The value is calculated according to the following steps:

[0102] elbow joint J4 The value has been obtained. Based on the kinematic chain of the fourth joint of the robotic arm, calculate the rotation matrix from the third to the fourth coordinate system. Then calculate the wrist pose matrix:

[0103]

[0104] Then expand to linear form:

[0105]

[0106] in:

[0107]

[0108] Since the matrix elements on both sides of the equation are the same, joint J6 can be solved. value:

[0109]

[0110] For any Calculate joints J5 and J7. value:

[0111]

[0112] in, Representation matrix The Middle Line number Column element values; Represents the wrist joint angle branch;

[0113] Therefore, for each elbow joint angle branch, two sets of candidate wrist joint angle values ​​can be obtained.

[0114] Step A2.5.7 combines the elbow joint angle branch, shoulder joint angle branch, and wrist joint angle branch to generate a candidate pool of joint angles for the humanoid robotic arm. Under non-singular conditions, this embodiment can generate up to 8 sets of joint pools. .

[0115] Step A2.6 Prediction unit based on trained natural arm-shaped angle Inverse kinematic unit with natural arm Constructing anthropomorphic configuration generative model .

[0116] Step A3: Based on the aforementioned anthropomorphic generative model of the configuration, construct an online anthropomorphic path planning method, which specifically includes the following steps:

[0117] Step A3.1 as follows Figure 4 As shown, a Cartesian reference path is generated based on the current end-effector pose and the target pose. In this embodiment, in an unobstructed environment, a point-to-point straight line is used to generate the Cartesian reference path. .

[0118] Step A3.2 Adjust the Cartesian reference path according to the adaptive step size. Discretize to obtain the end-effector pose path point sequence. , This represents a single pose point that includes both position and orientation. The number of pose points;

[0119] Step A3.3 Based on the end-effector pose path point sequence To form an anthropomorphic configuration generation model Input set ,for Its contained historical data of end pose sequence points is less than Therefore, the forward push method is used to obtain the number of units, i.e.:

[0120]

[0121] For the rest have:

[0122]

[0123] Step A3.4 Configuration Generation Model Output a sequence of reference joint angle values ​​in joint space. ,in It contains up to 8 sets of joint angle values ​​for reference.

[0124] Step A3.5 Using the rate of change of adjacent joint configurations as a constraint, construct the following path layer optimization model:

[0125]

[0126] in, This represents the optimized joint space path sequence. Indicates the first The joint configuration corresponding to each discrete path point, for ,Pick , The initial configuration of the path starting point for the humanoid robotic arm;

[0127] Step A3.6: Solve the path layer optimization model using one of the point-by-point rolling optimization methods to determine the joint configuration combination that minimizes the change in adjacent joint configurations, and form a joint space path sequence. , This represents a single spatial path point containing 7 joint angles. The number of pose points;

[0128] Step A3.7 When the target pose changes, retain the executed joint path segments. Using the current execution state as the new planning starting point, the pose path point sequence and the corresponding joint space anthropomorphic path sequence are regenerated for the unexecuted path segments, thus generating new joint path segments. ,in It is the last executed point of the old trajectory to ensure configuration continuity and path continuity during the online planning process.

[0129] Step A4: Based on the anthropomorphic path planned online, construct a real-time anthropomorphic trajectory layer generation method based on the rolling view and the minimum Jerk criterion, and output robotic arm control commands. This specifically includes the following steps:

[0130] Step A4.1 employs a rolling view method to perform local optimization selection on the anthropomorphic path in the joint space output by the path layer. The specific steps are as follows:

[0131] Step A4.1.1 Set the robotic arm control cycle ,like Figure 5 As shown, the dashed line segment This indicates the joint paths that the humanoid robotic arm has already traversed, and the joint paths that have not yet been executed. Renumber the nodes to form the execution path:

[0132]

[0133] in, Indicates the number after renumbering Each joint path point This indicates the number of path points in the joint space path currently to be executed.

[0134] Step A4.1.2 Check the execution joint path Does a joint steering point exist?

[0135]

[0136] in, Indicates the first The first path point in the nth path point If the above formula holds true, then the path point is determined to have a turning point and is identified as a joint turning point. If multiple joint turning points exist, the joint turning point closest to the current starting joint configuration is selected as the priority endpoint within the current scrolling view, denoted as... subscript Indicates the number of the first joint turning point to appear;

[0137] Step A4.1.3 Determine the local joint path endpoint within the current scrolling view window. If a joint turning point exists, use that point as the local joint path endpoint; otherwise, use the target joint configuration or the scrolling view window boundary point in the joint space path to be executed as the local joint path endpoint.

[0138]

[0139] Step A4.1.4 Set the scroll view window length From the execution joint path Selecting locally optimized joint path points:

[0140]

[0141] This local joint path includes constraints on the current robotic arm state, recent joint path constraints, and local endpoint constraints, which are used for real-time generation of subsequent trajectory layers.

[0142] Step A4.2 involves solving the trajectory of the joint path segments within the current scrolling view window based on the minimum Jerk criterion. The specific steps are as follows:

[0143] Step A4.2.1 is the local joint path. Allocate time nodes:

[0144]

[0145] in, This indicates the start time of the current control cycle. Indicates the end time of the current scrolling view window. Indicates the first The passage time corresponding to each local joint path point. In this specific implementation example, the time interval is allocated based on the joint spatial distance between adjacent path points:

[0146]

[0147] in, For time adjustment factor, Indicates the upper limit of joint velocity;

[0148] Step A4.2.2 Construct a smooth trajectory optimization model that passes through intermediate path points using the minimum Jerk criterion:

[0149]

[0150] in, This represents the optimized trajectory function. Indicates from the first Path points The movement reached the first Path points Local trajectory segments, Indicates the first Joint jerk of the segment trajectory.

[0151] Step A4.2.3 Establish path point traversal constraints:

[0152]

[0153] The path points are constrained to ensure that the generated joint trajectory passes through each local joint path point within the current scrolling view window in sequence.

[0154] Step A4.2.4 Establish continuity constraints at intermediate path points. For any intermediate path point... To satisfy:

[0155]

[0156] This constraint ensures that the position, velocity, and acceleration of the robotic arm are continuous when it passes through intermediate waypoints;

[0157] Step A4.2.5 Establish the initial and final boundary conditions, using the current actual joint state of the robotic arm as the initial boundary condition:

[0158]

[0159] in, , These represent the actual joint angular velocity and actual joint angular acceleration of the robotic arm at the current moment, respectively.

[0160] Establish termination boundary conditions:

[0161]

[0162] Step A4.2.6 Establish joint physical constraints:

[0163]

[0164] in, , These represent the lower and upper limits of the joint angle, respectively. Indicates the upper limit of joint velocity. Indicates the upper limit of joint acceleration;

[0165] Step A4.2.7 Use quadratic programming to solve the minimum Jerk trajectory optimization model to obtain the segmented local optimal joint trajectories within the current scrolling view window. , It can represent anthropomorphic smooth joint trajectories generated within the currently scrolling view window;

[0166] Step A4.2.8 Based on the robotic arm control cycle The sampling joint control commands are as follows:

[0167]

[0168] Then, control commands are sent to the underlying controller.

[0169] According to the method described in this invention, a simulation experiment was conducted on the seven-DOF humanoid robotic arm, and its standard DH parameters are shown in the figure.

[0170] Table 1 Standard DH Parameter Table for Humanoid Robotic Arms

[0171] Components / Coordinate System 1 0 -90 0.2405 2 0 90 0 3 0 -90 0.2560 4 0 90 0 5 0 -90 0.2100 6 0 90 0 7 0 0 0.1725

[0172] With the same initial joint states, target end-effector pose, and constraints on joint position, velocity, and acceleration, the motion trajectories of the seven joints of the robotic arm are generated using both the anthropomorphic online motion planning method described in this invention and the traditional Toppra method. The joint trajectories generated by the two methods are sampled to obtain curves showing the changes in joint position, joint velocity, and joint acceleration over time during the motion process, and these curves are plotted as shown below. Figure 6 The trajectory comparison diagram is shown. Among them, Figure 6 The upper part shows the trajectory curve generated by the method of this invention. Figure 6 The bottom side shows the trajectory curve generated by the traditional toppra method.

[0173] As can be seen, under the same motion task conditions, the joint position curves generated by the method of this invention change continuously, with each joint position maintaining a relatively smooth transition in the initial, middle, and final segments; the joint velocity curves change smoothly overall, without obvious sharp abrupt changes; and the joint acceleration curves, except for necessary adjustments in the start and end phases, exhibit relatively small fluctuations, maintaining good continuity and smoothness. In contrast, the trajectory generated by the traditional Toppra method, while satisfying joint motion constraints, is more biased towards time parameterization results. Its joint velocity curves exhibit obvious segmented changes, with some joint velocity peaks being relatively high; and the joint acceleration curves show obvious abrupt changes and broken line changes at trajectory switching positions.

[0174] The comparative results show that the present invention, through hierarchical collaborative planning of configuration layer, path layer and trajectory layer, and combined with the rolling field of view and minimum Jerk criterion to generate joint trajectories, can enable the humanoid robotic arm to achieve a smoother, more natural and human-machine collaborative motion process while completing the target motion task.

[0175] Compared with the prior art, the present invention has at least the following beneficial effects:

[0176] This invention decomposes the anthropomorphic motion planning problem of humanoid robotic arms, and introduces human body configuration features, path features and minimum Jerk time characteristics into the configuration layer, path layer and trajectory layer respectively, so that the robotic arm can maintain the motion law of human body, achieve reasonable joint posture, continuous and smooth path and natural and smooth trajectory, and improve the anthropomorphism of motion.

[0177] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

[0178] The contents not described in detail in this specification are common knowledge to those skilled in the art.

Claims

1. A method for online anthropomorphic motion planning for humanoid robotic arms, characterized in that, include: Step S1: Construct a hierarchical humanoid robotic arm anthropomorphic online motion planning framework that includes a configuration layer, a path layer, and a trajectory layer; Step S2: Based on the hierarchical humanoid robotic arm anthropomorphic online motion planning framework, collect natural motion samples of human upper limbs and construct an anthropomorphic generative model of configuration based on the natural arm shape angle constraints of shoulder, elbow, and wrist. Step S3: Based on the anthropomorphic generation model of the configuration, construct an online path planning method to map the Cartesian space path to the joint space path; Step S4: Based on the joint space path, construct a real-time generation method for anthropomorphic trajectory layers based on the rolling field of view and the minimum Jerk criterion, and output robotic arm control commands.

2. The method according to claim 1, characterized in that, Step S1 includes: Define the system state variables of the humanoid robotic arm The system state variables include joint angle, joint angular velocity, joint angular acceleration, end pose, and target pose information; Based on the system state variables A hierarchical humanoid robotic arm anthropomorphic online motion planning framework is constructed, which decouples the online motion planning process of the humanoid robotic arm from the current end pose to the target end pose into a configuration layer, a path layer and a trajectory layer connected in sequence according to the anthropomorphic motion generation process. An anthropomorphic configuration generation model is established at the configuration layer. Includes natural arm-shaped prediction unit and natural arm type inverse solution unit The natural arm-shaped prediction unit Based on the robotic arm's end-effector pose and historical end-effector pose information, the reference natural arm shape angle is output. The natural arm-shaped inverse kinematic unit Based on the end-effector pose and reference natural arm angle Solve for the set of candidate joint configurations that satisfy the end-effector pose conditions. ; A joint path planning method is established at the path layer, based on the system state variables. A Cartesian space reference path is generated from the current end-effector pose and the target end-effector pose, and the pose path point sequence is discretized. Using the anthropomorphic configuration generation model of the configuration layer Solve the pose path point sequence Corresponding joint configuration path candidate sequence From joint configuration path candidate sequences Determine the executable joint space path sequence ; A method for real-time joint trajectory generation is established at the trajectory layer, based on the joint spatial path sequence. This involves solving the relationship between joint position and joint velocity changes over time during the motion execution of the humanoid robotic arm, and generating the current control cycle. Internal continuous joint trajectory ; An online update mechanism is established for the hierarchical humanoid robotic arm's anthropomorphic online motion planning framework. When the system state variables... When the target pose changes, an update is triggered. The executed trajectory segments are retained, and the current execution state of the robotic arm is used as the new planning starting point to re-plan the configuration layer, path layer, and trajectory layer, thereby realizing the humanoid robotic arm's anthropomorphic online motion facing the target pose change.

3. The method according to claim 1, characterized in that, Step S2 includes: Based on the geometric relationship between the center of the shoulder joint, the center of the elbow joint, and the center of the wrist joint of the humanoid robotic arm, an arm-shaped angle is defined to characterize the redundant configuration. The arm-shaped angle Used to describe the spatial orientation of the robotic arm elbow relative to the line connecting the shoulder and wrist, given the end-of-task pose determination condition; Acquire sample data to characterize the natural movement features of the human upper limb. The sample data includes the end-effector pose sequence and natural arm angle sequence of the humanoid robotic arm's motion trajectory, forming a training sample set. ,in Indicates the first The task end pose and historical end pose sequences in the group sample. Indicates the first The natural arm-shaped angles corresponding to the group of samples; With the training sample set As training input, construct and train a natural arm shape prediction unit. The natural arm-shaped prediction unit This includes either a neural network model or a mapping model based on teaching data, used to establish a mapping relationship between the task end pose and the historical end pose sequence and the natural arm shape angle; In the current control cycle The task end-effector pose and historical end-effector pose sequences are input into the natural arm shape prediction unit. Output the current control cycle Corresponding reference natural arm angle ; Constructing natural arm-shaped inverse kinematic unit The reference natural arm angle As a basis for redundancy degree of freedom allocation, a pool of candidate joint configurations that satisfy end-effector pose constraints and joint constraints is solved. ; Based on the trained natural arm-shaped angle prediction unit Inverse kinematic unit with natural arm Constructing anthropomorphic configuration generative model .

4. The method according to claim 1, characterized in that, Step S3 includes: Based on the current end-effector pose and the target pose, a reference path is generated in Cartesian space; the reference path is one of a straight line segment, a spline curve, or a Bézier curve. Discretize the reference path according to a preset step size or an adaptive step size to obtain a pose path point sequence. ; For pose path point sequences Each discrete point in the model is generated according to the anthropomorphic configuration generation model of the configuration layer. Generate corresponding joint configuration path candidate sequences ; A path layer optimization model is constructed using the rate of change in adjacent joint configurations as a constraint term: in, This represents the optimized joint space path sequence. Indicates the first The joint configuration corresponding to each discrete path point, for ,Pick , The initial configuration of the path starting point for the humanoid robotic arm; The path layer optimization model is solved using one of the following methods: dynamic programming, graph search, or point-by-point rolling optimization. This determines the joint configuration combination that minimizes the changes in adjacent joint configurations and forms a joint space path sequence. ; When the target pose changes, the executed path segments are retained, and the current execution state is used as the new planning starting point. The pose path point sequence and the corresponding joint space path sequence are regenerated for the unexecuted path segments to ensure configuration continuity and path continuity in the online planning process.

5. The method according to claim 1, characterized in that, Step S4 includes: Set the length of the scroll view window. and rolling update cycle In each control cycle Within this process, the unexecuted segments of the spatial joint path are renumbered to form the executed joint path. ,from Select the joint space path segment within the currently scrolling view window. ; Scroll the view window length Using the start and end positions, start and end velocities, and start and end accelerations as boundary conditions, a minimum Jerk criterion smooth trajectory optimization model is constructed: in, This represents the optimized trajectory function. Indicates from the first Path points The movement reached the first Path points Local trajectory segments, Indicates the first Joint jerk of the segment trajectory; Heuristic search or quadratic programming methods are used to solve the optimization model of the smooth trajectory based on the minimum Jerk criterion, and the results are obtained. ; When the target pose changes, new boundary conditions are established at the splicing point at the end of the executed trajectory, the local joint trajectory in the subsequent window is resolved, and the position, velocity and acceleration of the trajectory at the splicing point are ensured to be continuous. The local joint trajectory function generated by the scrolling is converted into joint position commands and joint velocity commands, which are then sent to the underlying controller to drive the humanoid robotic arm to complete online anthropomorphic motion.

Images