CN-121994228-A - Humanoid robot upper limb and waist cooperative motion planning method

CN121994228ACN 121994228 ACN121994228 ACN 121994228ACN-121994228-A

Abstract

The application provides a planning method for cooperative movement of upper limbs and waists of a humanoid robot, which comprises the steps of adjusting an elbow joint solution space range according to analysis results of geometric constraint degrees and joint angular velocity variation, positioning potential singular point position distribution in a solution space based on angular velocity gradient variation in the solution space range formed after adjustment, evaluating evasion capability of candidate path points in a waist forward track according to the singular point position distribution and the solution space range after adjustment to generate a cooperative planning path, evaluating evasion effect of singular point positions through simulation execution of the cooperative planning path, outputting a movement control scheme, and iteratively updating joint angular velocity variation according to the movement control scheme to confirm completion of cooperative movement planning.

Inventors

JIANG MING
SHAN JIE
YUAN JIAHUA
LI YAYONG
ZHANG WENBO

Assignees

深圳市长盈机器人有限公司
深圳市长盈精密技术股份有限公司

Dates

Publication Date: 20260508
Application Date: 20251229

Claims (10)

1. The method for planning the cooperative movement of the upper limbs and the waist of the humanoid robot is characterized by comprising the following steps of: Acquiring waist forward distance, elbow joint solution space range and joint angular velocity variation of the humanoid robot, establishing a mapping relation between the waist forward distance and the elbow joint multi-solution region boundary, identifying a critical conversion point from the multi-solution region to a single solution region through solution space dimension analysis, and quantifying the geometrical constraint degree of degree-of-freedom compression by combining the critical conversion point; According to the analysis results of the geometric constraint degree and the joint angular velocity variation, adjusting the elbow joint solution space range; locating potential singular point position distribution in a solution space based on the angular velocity gradient change in the solution space range formed after adjustment; According to the singular point position distribution and the adjusted solution space range, evaluating the evading capability of candidate path points in the waist forward track, and generating a collaborative planning path; And evaluating the avoidance effect of the singular point position by simulating and executing the collaborative planning path, and outputting a motion control scheme.
2. The method for planning the cooperative motion of the upper limbs and the waist of the humanoid robot according to claim 1, wherein the steps of acquiring the waist forward distance, the elbow joint solution space range and the joint angular velocity variation of the humanoid robot, establishing a mapping relation between the waist forward distance and the elbow joint multi-solution region boundary, identifying a critical conversion point from the multi-solution region to a single solution region through solution space dimension analysis, and quantifying the geometrical constraint degree of freedom compression by combining the critical conversion point comprise the following steps: Acquiring the waist forward movement distance through an encoder at a waist pivot, and extracting elbow joint angular velocity data from elbow joint servo motor feedback signals; calculating the reachable domain range of the elbow joint in the Cartesian space according to a preset parameter table to obtain an initial boundary coordinate set of a joint solution space; determining a critical conversion point from a multi-solution area to a single-solution area by scanning sampling points on a waist forward track; Calculating an elbow joint operability index according to the condition number of the jacobian matrix of the critical conversion point, and obtaining a preliminary geometric constraint metric value by utilizing the ratio of the operability index to a preset reference operability; And fitting a nonlinear mapping function by using the preliminary geometric constraint metric value and the waist advancing distance, and determining the geometric constraint degree under different advancing distances.
3. The method for planning cooperative motion of an upper limb and a waist of a humanoid robot according to claim 1, wherein the adjusting the elbow joint solution space range according to the analysis result of the geometrical constraint degree and the joint angular velocity variation comprises: calculating the angular velocity change rate from elbow joint servo driver feedback data; Normalizing the geometric constraint degree and the angular velocity change rate, and adding according to preset weights to obtain a coupling analysis index; if the coupling analysis index is larger than a preset activation threshold, selecting a motion vector in a jacobian matrix zero space, and expanding an elbow joint feasible solution set; Aiming at a feasible solution set obtained after the elbow joint feasible solution is expanded, calculating a new solution space boundary point set by adopting a convex hull algorithm, and determining new upper and lower limit constraints of elbow joint angles to obtain an adjusted solution space range; And if the coupling analysis index is smaller than or equal to a preset activation threshold, reserving the current elbow joint physical limit range as the adjusted solution space range.
4. The method for planning the collaborative motion of an upper limb and a waist of a humanoid robot according to claim 1, wherein locating the position distribution of potential singular points in the solution space based on the angular velocity gradient change in the solution space formed after adjustment comprises: Extracting joint angle sampling points from the adjusted solution space range, calculating a jacobian matrix at each sampling point, and obtaining an angular velocity gradient through determinant value difference of adjacent sampling points to obtain a gradient vector field; Calculating the continuity deviation value of each sampling point in the gradient vector field and other points in the neighborhood of the sampling point by a sliding window method; Performing second-order differential operation on the continuity deviation value, and marking the continuity deviation value as a local extreme point if the sign changes and the deviation value exceeds a preset threshold value; And carrying out spatial clustering on the local extremum points by adopting a clustering algorithm to obtain a singular point candidate region, calculating the clustering center coordinates and the influence radius of the singular point candidate region, and establishing a singular point position distribution map.
5. The method for planning cooperative motion of an upper limb and a waist of a humanoid robot according to claim 1, wherein the estimating the evasive ability of candidate path points in a waist forward trajectory according to the singular point position distribution and the adjusted solution space range, and generating a cooperative planning path comprise: Reading the singular point position distribution and the adjusted solution space range, and performing discretization sampling on the waist forward track to obtain a candidate path point sequence; Calculating Euclidean distance from each candidate path point to the nearest singular point, and determining evasion capability scores according to the ratio of the distance to the influence radius; if the evasion capability score is lower than a preset safety threshold, searching for a replacement point in the neighborhood of the candidate point, calculating the comprehensive cost value through a path planning algorithm, and selecting a replacement point updating path with the minimum cost; And carrying out spline interpolation on the updated path point sequence, and adjusting the number of interpolation points according to the average value of the evasion capacity scores between the adjacent path points to generate a smooth collaborative planning path.
6. The humanoid robot upper limb and waist cooperative motion planning method according to claim 1, wherein the estimating the avoidance effect of the singular point position by performing the cooperative planning path through simulation includes: Reading an elbow joint angle sequence from the collaborative planning path, calculating angle difference values between adjacent sampling points in the elbow joint angle sequence, accumulating forward angle change to obtain total change amplitude in the stretching process, and recording an angle change rate peak value; According to the total variation amplitude and the speed peak value in the stretching process, converting the total variation amplitude and the speed peak value into a target displacement and a rotating speed upper limit of a servo motor, and executing waist forward movement and elbow joint stretching synchronous movement; acquiring joint position deviation and tail end track data in the executing process in real time; And comparing the distance between the tail end track data and the position of the singular point, and if the minimum distance is larger than the preset safety distance and the position deviation is in the allowable range, judging that the avoidance effect of the position of the singular point meets the requirement, and outputting a motion control scheme.
7. The method for planning cooperative motion of an upper limb and a waist of a humanoid robot according to claim 1, wherein the method further comprises iteratively updating the joint angular velocity variation according to the motion control scheme, and confirming completion of cooperative motion planning.
8. The method for planning cooperative motion of an upper limb and a waist of a humanoid robot according to claim 7, wherein iteratively updating the joint angular velocity variation according to the motion control scheme comprises: Extracting joint angular velocity values at all moments from a velocity curve of the motion control scheme, and calculating velocity difference values at adjacent moments as angular velocity adjustment amounts; reading current joint angle acquisition information, and inhibiting noise by a filtering method to obtain a smooth angle sequence; And updating the joint angular velocity by adopting an iterative optimization method according to the angular velocity adjustment quantity and the smooth angle sequence, and overlapping the adjustment quantity to the current angular velocity according to a preset coefficient in each iteration to obtain the optimized joint angular velocity change quantity.
9. The humanoid robot upper limb and waist cooperative motion planning method according to claim 8, wherein after the optimized joint angular velocity variation is obtained, the optimized joint angular velocity variation is mapped to solution space grid points to form a velocity field, velocity field gradient module value distribution is calculated, and cross-correlation operation is carried out on the velocity field gradient module value distribution and a singular point position distribution distance field to obtain an optimized velocity distribution and singular point distribution matching degree value.
10. The humanoid robot upper limb and waist cooperative motion planning method of claim 9, wherein the confirming cooperative motion planning completion includes: Comparing the matching degree value of the optimized speed distribution and the singular point distribution with a preset matching critical value, and if the matching degree value of the optimized speed distribution and the singular point distribution is larger than or equal to the preset critical value, confirming that the collaborative motion planning is completed; if the matching degree value is lower than a preset critical value, readjusting a solution space boundary condition, and executing solution space reconstruction to generate a new candidate path point sequence; Repeating path planning and simulation execution processes aiming at the new candidate path point sequence, and collecting updated joint angular velocity variation and tail end track data; And recalculating the matching degree value according to the updated data until the matching degree value of the optimized speed distribution and the singular point distribution is larger than or equal to a preset critical value, and confirming that the collaborative motion planning is completed.

Description

Humanoid robot upper limb and waist cooperative motion planning method Technical Field The invention relates to the technical field of information, in particular to a method for planning cooperative movement of upper limbs and waists of a humanoid robot. Background The upper limb movement control of the humanoid robot is one of the core directions of the robotics study, and the technical level directly determines the practical application capability of the robot in the scenes of medical rehabilitation, industrial assembly, precise operation and the like. In upper limb movement planning, dynamic coordination among parts of a robot body is important, and particularly, the coordination relation between a waist and upper limb joints directly influences flexibility and stability of movement control. In the current research, the influence of the lumbar advancement distance on the elbow joint movement freedom is a key difficulty to be solved urgently. According to the robot kinematics theory, the solution space range of the elbow joint is determined by the end position constraint and the geometric relationship of the connecting rod. In the normal working range, the elbow joint is in a multi-solution area, namely, a plurality of optional bending angle configurations exist, and the system can select the optimal posture according to task requirements so as to avoid obstacles or optimize moment distribution. However, as the lumbar advancement distance increases and exceeds a threshold, the solution space boundary of the elbow joint is crossed, the multiple solution regions merge and shrink into a single solution region, where the joint has only a unique fixed angular configuration, a phenomenon known in robotics as solution space boundary crossing. The influence brought by the single solution state is reflected in a plurality of layers, namely, the system loses the flexibility of configuration selection in the motion planning layer, the efficiency is reduced or the collision risk is increased due to the fact that a suboptimal path is possibly forced to be adopted, when the forward movement distance is accidentally increased in the control stability layer, the system can be suddenly switched from the multiple solutions to the single solution, the angular velocity mutation of joints is caused, the motion is jittered or out of control, and in the actual operation layer, such as industrial assembly, the precision is reduced due to the fact that the mechanical arm cannot finely adjust the grabbing angle due to the limitation of the single solution, or the natural bending adjustment of a patient in the rehabilitation robot cannot be completed due to the fact that the joint configuration is fixed. Therefore, how to monitor the solution space change in real time in the lumbar forward movement process, maintain the multi-solution state of the elbow joint, effectively avoid singular points, ensure the continuity and coordination of the whole action, and become the core problem of the motion planning of the upper limbs of the humanoid robot. Disclosure of Invention The invention provides a method for planning cooperative movement of upper limbs and waists of a humanoid robot, which mainly comprises the following steps: Acquiring waist forward distance, elbow joint solution space range and joint angular velocity variation of the humanoid robot, establishing a mapping relation between the waist forward distance and the elbow joint multi-solution region boundary, identifying a critical conversion point from the multi-solution region to a single solution region through solution space dimension analysis, and quantifying the geometrical constraint degree of degree-of-freedom compression by combining the critical conversion point; According to the analysis results of the geometric constraint degree and the joint angular velocity variation, adjusting the elbow joint solution space range; locating potential singular point position distribution in a solution space based on the angular velocity gradient change in the solution space range formed after adjustment; According to the singular point position distribution and the adjusted solution space range, evaluating the evading capability of candidate path points in the waist forward track, and generating a collaborative planning path; Evaluating the avoidance effect of the singular point position by simulating and executing the collaborative planning path, and outputting a motion control scheme; And iteratively updating the joint angular velocity variation according to the motion control scheme, and confirming that the collaborative motion planning is completed. Further, the acquiring the waist forward distance, the elbow joint solution space range and the joint angular velocity variation of the humanoid robot, establishing a mapping relation between the waist forward distance and the elbow joint multi-solution region boundary, identifying a critical conversion point from the multi-solution