CN113524205A - Throwing trajectory planning method, device and medium for redundant arm of humanoid robot - Google Patents

Abstract

The present application relates to the technical field of throwing trajectory planning, and discloses a throwing trajectory planning method, device and medium for a redundant arm of a humanoid robot, wherein the method includes: acquiring a preset acceleration change curve, according to the preset acceleration change curve, The acceleration phase trajectory is generated from the world starting point position, the world target point position and the world shot point position; the throwing trajectory is obtained according to the acceleration phase trajectory; a preset time interval is used to discretely process the throwing trajectory to obtain multiple discrete positions; Solve the joint angle at discrete positions to obtain multiple solution results; when there is a solution result that is no solution, adjust the position of the world shot point, and repeat the steps of obtaining the preset acceleration trajectory curve until each solution result is a solution, according to Each solution result is a set of target joint angles determined by each joint angle corresponding to the solution. The projecting trajectory planning of the humanoid robot is realized, the trajectory solving accuracy is improved, and the trajectory solving rate is improved.

Description

Throwing trajectory planning method, device and medium for redundant arm of humanoid robot

technical field

The present application relates to the technical field of throwing trajectory planning, and in particular, to a throwing trajectory planning method, device and medium for a redundant arm of a humanoid robot.

Background technique

Throwing is a basic and important form of human operation, and the robot can also expand its operating range by throwing, so as to achieve rapid sorting or transmission of operation targets. At present, most of the research on robot throwing relies on non-redundant industrial robots or redundant collaborative robots. Both types of robots have large workspaces, and various trajectory planning methods can be used to meet the output at specific throwing points. Requirements for specific casting speeds, such as joint space planning, splines, sampling searches, teach-in simulation, machine learning, etc. However, the redundant arm of the humanoid robot is limited by the torso block and joint range. It is not universal for methods such as spline curve or sampling search that may generate complex throwing paths, and teaching simulation and machine learning also have trajectory accuracy. And the problem of learning difficulty, although the joint space planning is simple and general, for the redundant arm of a humanoid robot with a floating base, its throwing planning process is not intuitive, and its applicability is not strong.

SUMMARY OF THE INVENTION

The main purpose of the present application is to provide a throwing trajectory planning method, device and medium for a redundant arm of a humanoid robot, which aims to solve the problem that the throwing trajectory planning method in the prior art relies on industrial robots or collaborative robots with relatively low performance. For a manipulator with a large workspace, there are technical problems in the application of the redundant arm of the humanoid robot, such as the practicability of the planning method, the trajectory solving rate and the trajectory solving accuracy.

In order to achieve the above purpose of the invention, the present application proposes a throwing trajectory planning method for a redundant arm of a humanoid robot, the method is suitable for the redundant arm of a humanoid robot, and the method includes:

Obtain the world starting point position, the world target point position and the world shooting point position in the world coordinate system;

Acquire a preset acceleration change curve, and generate an acceleration phase trajectory according to the preset acceleration change curve, the world starting point position, the world target point position, and the world starting point position;

According to the acceleration phase trajectory, the following phase trajectory acquisition, the deceleration phase trajectory acquisition, and the trajectory splicing are respectively performed to obtain the throwing trajectory;

Using a preset time interval, discrete processing is performed on the throwing trajectory to obtain a plurality of discrete positions;

The joint angle is solved for each of the discrete positions respectively, and a plurality of solution results are obtained;

When the solution result is no solution, adjust the position of the shot point in the world, repeat the steps of obtaining the preset acceleration trajectory curve, until each solution result is a solution, according to each solution The result is that each joint angle corresponding to the solution determines the target joint angle set.

The application also proposes a throwing trajectory planning device for a redundant arm of a humanoid robot, the device is suitable for the redundant arm of a humanoid robot, and the device includes:

The data acquisition module is used to acquire the position of the world starting point, the position of the world target point and the position of the world starting point in the world coordinate system;

an acceleration phase trajectory determination module, configured to obtain a preset acceleration change curve, and generate an acceleration phase trajectory according to the preset acceleration change curve, the world starting point position, the world target point position, and the world starting point position;

The throwing trajectory determination module is used for obtaining the trajectory of the follow-up phase, the acquisition of the trajectory of the deceleration phase, and the splicing of the trajectory according to the trajectory of the acceleration phase, to obtain the trajectory of the throwing throw;

a discrete position determination module, configured to perform discrete processing on the throwing trajectory using a preset time interval to obtain a plurality of discrete positions;

The solution result determination module is used to solve the joint angle of each discrete position respectively, and obtain a plurality of solution results;

The target joint angle set determination module is used to adjust the position of the world shot point when the solution result is no solution, and repeat the step of obtaining the preset acceleration trajectory curve until each of the solution results is no solution. In order to have a solution, the target joint angle set is determined according to each joint angle corresponding to each of the solution results.

The present application also proposes a computer device, including a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of any one of the above methods when the processor executes the computer program.

The present application also provides a computer-readable storage medium on which a computer program is stored, and when the computer program is executed by a processor, implements the steps of any one of the methods described above.

The throwing trajectory planning method, device and medium of the redundant arm of the humanoid robot of the present application, wherein the method generates an acceleration phase trajectory according to a preset acceleration change curve, the world starting point position, the world target point position and the world shooting point position, According to the trajectory of the acceleration phase, the trajectory of the follow-up phase, the trajectory of the deceleration phase, and the trajectory splicing are respectively obtained to obtain the throwing trajectory. Secondly, the throwing trajectory is discretely processed at a preset time interval to obtain a plurality of discrete positions, and then respectively The joint angle is solved for each discrete position, and multiple solution results are obtained. Finally, when the solution result is no solution, the position of the world shot point is adjusted, and the steps of generating the trajectory of the acceleration phase are repeated until each solution result is a solution. , according to each joint angle corresponding to each solution result, the target joint angle set is determined, and the three-stage throwing trajectory is obtained by splicing the acceleration phase trajectory, the following phase trajectory, and the deceleration phase trajectory. The three-stage throwing trajectory is suitable for It is used for the redundant arm of the humanoid robot, so as to realize the throwing trajectory planning of the redundant arm of the humanoid robot, which has strong versatility in the face of different throwing targets; The joint angle is solved by the position, and the step-by-step analytical inverse solution calculation of the throwing plan of the redundant arm of the humanoid robot is realized, which improves the trajectory solving accuracy and the trajectory solving rate.

Description of drawings

1 is a schematic flowchart of a method for planning a throwing trajectory of a redundant arm of a humanoid robot according to an embodiment of the application;

2 is a schematic diagram of seven degrees of freedom of a redundant arm of a humanoid robot according to an embodiment of the application;

3 is a schematic diagram of a parabolic trajectory of a redundant arm of a humanoid robot according to an embodiment of the application;

4 is a schematic diagram of adjusting the fifth joint of the redundant arm of the humanoid robot according to an embodiment of the application;

5 is a schematic block diagram of the structure of a throwing trajectory planning device for a redundant arm of a humanoid robot according to an embodiment of the application;

FIG. 6 is a schematic structural block diagram of a computer device according to an embodiment of the present application.

The realization, functional characteristics and advantages of the purpose of the present application will be further described with reference to the accompanying drawings in conjunction with the embodiments.

Detailed ways

In order to make the purpose, technical solutions and advantages of the present application more clearly understood, the present application will be described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application, but not to limit the present application.

Referring to FIG. 1, an embodiment of the present application provides a throwing trajectory planning method for a redundant arm of a humanoid robot, and the method is applicable to the redundant arm of a humanoid robot, and the method includes:

S1: Obtain the world starting point position, the world target point position and the world shooting point position in the world coordinate system;

S2: Obtain a preset acceleration change curve, and generate an acceleration phase trajectory according to the preset acceleration change curve, the world starting point position, the world target point position, and the world shooting point position;

S3: According to the acceleration phase trajectory, the acquisition of the follow-up phase trajectory, the acquisition of the deceleration phase trajectory, and the trajectory splicing are respectively performed to obtain the throwing trajectory;

S4: using a preset time interval to perform discrete processing on the throwing trajectory to obtain a plurality of discrete positions;

S5: Solve the joint angle for each of the discrete positions respectively, and obtain a plurality of solving results;

S6: When there is no solution in the solution result, adjust the position of the world shot point, and repeat the step of obtaining the preset acceleration trajectory curve until each solution result is a solution. The above solution results are the set of target joint angles determined by each joint angle corresponding to the solution.

In this embodiment, the acceleration phase trajectory is first generated according to the preset acceleration change curve, the world starting point position, the world target point position, and the world shooting point position, and the following phase trajectory acquisition, deceleration phase trajectory acquisition, and trajectory splicing are respectively performed according to the acceleration phase trajectory. , obtain the throwing trajectory, and then use the preset time interval to discretely process the throwing trajectory to obtain multiple discrete positions, and then solve the joint angle for each discrete position separately to obtain multiple solution results, and finally when there is a solution When the result is no solution, adjust the position of the starting point in the world, repeat the steps of generating the trajectory of the acceleration phase until each solution result is a solution, and determine the target joint angle set according to each joint angle corresponding to each solution result. The three-stage throwing trajectory is obtained by splicing the acceleration phase trajectory, the following phase trajectory, and the deceleration phase trajectory. The three-stage throwing trajectory is suitable for the redundant arm of the humanoid robot, so as to realize the throwing of the redundant arm of the humanoid robot. Throwing trajectory planning has strong versatility in the face of different throwing targets; and by solving the joint angle for each discrete position after the throwing trajectory is discrete, the throwing of the redundant arm of the humanoid robot is realized. The planned step-by-step analytical inverse solution calculation improves the trajectory solving accuracy and the trajectory solving rate.

It can be understood that the redundant arm of the humanoid robot of the present application is an arm with seven degrees of freedom.

是第一关节的关节角，

是第二关节的关节角，

是第三关节的关节角，

是第四关节的关节角，

是第五关节的关节角，

是第六关节的关节角，

是第七关节的关节角，其中，第一关节、第二关节和第三关节是位于肩部的关节，第四关节是位于肘部的关节，第五关节、第六关节和第七关节是位于手腕的关节。2, the seven degrees of freedom of the redundant arm of the humanoid robot includes 7 joints,

is the joint angle of the first joint,

is the joint angle of the second joint,

is the joint angle of the third joint,

is the joint angle of the fourth joint,

is the joint angle of the fifth joint,

is the joint angle of the sixth joint,

is the joint angle of the seventh joint, where the first, second and third joints are the joints at the shoulder, the fourth joint is the joint at the elbow, and the fifth, sixth and seventh joints are at the joint of the wrist.

It can be understood that the first joint, the second joint, the third joint and the fourth joint determine the position of the arm end of the redundant arm of the humanoid robot, and the fifth, sixth and seventh joints only affect the humanoid robot. The posture of the arm end of the redundant arm of the robot is more important during the throwing process is the position of the arm end and the orientation of the palm opening and closing. The end of the arm refers to the end of the two ends of the forearm that is close to the hand.

From the redundant arm to the hand, it includes: upper arm, elbow, forearm, wrist, and hand.

中的上标T表述的对向量G1的转置，G1可以是本申请中的任一向量。It is understood that in this application,

The transposition of the vector G1 expressed by the superscript T in , G1 may be any vector in this application.

For S1, the world starting point position, the world target point position and the world starting point position in the world coordinate system can be obtained from the storage space of the humanoid robot, and the world starting point position, the world starting point position and the world starting point position in the world coordinate system can also be obtained from the industrial computer. The position of the target point and the position of the world shot point can also be obtained from the third-party application system.

The starting position of the world is the starting position of the redundant arm of the humanoid robot in the world coordinate system when throwing.

The position of the world target point is the target position of the thrown object when the redundant arm of the humanoid robot throws in the world coordinate system.

Throwing objects are objects that need to be thrown to the position of the world target point. The throwing object is placed in the hands of the redundant arms of the humanoid robot for throwing.

The position of the world shot point is the position where the hand of the redundant arm is separated from the thrown object when the redundant arm of the humanoid robot in the world coordinate system throws.

It is understandable that the position of the starting point in the world and the position of the shooting point in the world are both the positions where the arm end of the redundant arm of the humanoid robot passes through when throwing.

For S2, a preset acceleration change curve can be obtained from the storage space of the humanoid robot, a preset acceleration change curve can also be obtained from an industrial computer, and a preset acceleration change curve can also be obtained from a third-party application system.

The preset acceleration change curve is the change curve of the acceleration in the acceleration phase.

Optionally, the preset acceleration change curve is set as a trapezoidal curve. In this way, the acceleration is firstly performed with a linearly increasing acceleration, then a fixed acceleration is used for acceleration, and finally a linearly decreasing acceleration is used for acceleration, which simplifies the control of the redundant arm.

It can be understood that, the preset acceleration change curve may also be set to other linearity, such as a normal distribution curve, which is not specifically limited herein.

Wherein, firstly construct the throwing plane and throwing coordinate system according to the world starting point position and the world target point position, and then construct the throwing plane and throwing coordinate system according to the throwing plane and throwing coordinate system, according to the preset acceleration change curve, the world starting point The position, the position of the world target point, and the position of the world shot point generate an acceleration phase trajectory.

For S3, the following phase trajectory is obtained according to the acceleration phase trajectory, then the deceleration phase trajectory is obtained according to the following phase trajectory, and finally the acceleration phase trajectory, the following phase trajectory, and the deceleration phase trajectory are sequentially spliced, and the spliced trajectory is used as the throwing trajectory. . Thereby, a three-stage throwing trajectory is obtained. The three-section throwing trajectory is suitable for the redundant arm of the humanoid robot, so as to realize the throwing trajectory planning of the redundant arm of the humanoid robot and face different throwing targets. Has strong generality.

The trajectory of the acceleration phase is the trajectory of the redundant arm in preparation for the throwing to accelerate the movement.

The trajectory of the follow-up phase is the trajectory of the redundant arm following the projectile object to perform a parabolic following motion after the projectile object is thrown.

The deceleration phase trajectory is the trajectory of the redundant arm performing deceleration motion after following the phase trajectory.

It can be understood that the throwing trajectory is a set of position data that the arm end of the redundant arm of the humanoid robot passes through when throwing.

It can be understood that at the end of the trajectory in the acceleration phase, the redundant arm throws the projectile object, then follows the projectile object to perform a parabolic follow movement, and finally decelerates according to the trajectory of the deceleration phase until it stops.

For S4, the preset time interval can be obtained from the storage space of the humanoid robot, the preset time interval can also be obtained from an industrial computer, and the preset time interval can also be obtained from a third-party application system. The preset time interval is a specific value. For example, the preset time interval is set to 0.01 seconds, which is not specifically limited in this example.

Wherein, the throwing trajectory is discretely processed to obtain a plurality of discrete points, the time difference between two adjacent discrete points is the same as the preset time interval, and the position of each discrete point is used as a plurality of discrete positions. This provides the basis for the step-by-step analytical inverse solution calculation of the throwing planning of the redundant arm of the humanoid robot.

It can be understood that the discrete position is the position data of the arm end of the redundant arm.

For S5, because the position of the end of the arm and the orientation of the opening and closing of the palm are more important in the throwing process, when the inverse solution calculation is used to solve the joint angle for each of the discrete positions, the first five joints ( The joint angles of the first joint, the second joint, the third joint, the fourth joint, the fifth joint) have higher priority than the joint angles of the latter two joints (the sixth joint, the seventh joint), and are subject to redundancy The structural space of the arm is limited. The motors of the sixth and seventh joints at the wrist are small in terms of torque and working space, and do not contribute much to the throwing speed, because the influence of the wrist joint on the posture of the end will increase the calculation of the inverse solution. The complexity and the unintuitiveness of the throwing planning process, so the joint angles of the last two joints (the sixth joint and the seventh joint) are fixed during the throwing process.

Therefore, for each discrete position, the fifth joint based on the redundant arm does not affect the position of the throwing object and the sixth and seventh joints of the redundant arm are fixed during the throwing process. The rule of , while keeping the joint angle of the fifth joint unchanged, the joint angles of the first four joints (the first joint, the second joint, the third joint, and the fourth joint) are solved, and then the joint angles obtained by the solution are obtained. The attitude calculation of the end of the arm is performed, and finally the Z-axis direction of the seventh coordinate system of the joint coordinate system set is used as the palm opening and closing direction, and the sixth joint and the seventh joint of the redundant arm are cast in the throw. According to the fixed rules during the casting process, the joint angle calculation is performed on each joint of the redundant arm, thereby completing a round of inverse solution calculation.

For S6, when the solution result is no solution, it means that the current position of the shot point in the world is not a suitable position where the redundant arm is separated from the throwing object. Therefore, the position of the shot point in the world needs to be adjusted. After adjustment The position of the world shot point is used for the next iterative calculation, and steps S2 to S6 are repeatedly executed until each of the solution results is a solution, and then all the joint angles corresponding to each discrete position calculated in the last iterative calculation are used as A collection of target joint angles. That is, the number of discrete positions multiplied by 7 equals the number of joint angles in the target joint angle set.

Optionally, the step of determining the target joint angle set according to each joint angle corresponding to each of the solution results, including: seven joint angles (the first one corresponding to each discrete position calculated in the last iteration) joint angle corresponding to the joint, the second joint, the third joint, the fourth joint, the fifth joint, the sixth joint and the seventh joint) as a joint angle vector, and take all the joint angle vectors as the target joint angle set.

Optionally, the step of adjusting the position of the world shot point includes: determining a solution-free trajectory stage according to the discrete position where the solution result is no solution, to obtain a solution-free trajectory stage set; obtaining a preset The shot point adjustment rule of , adjusts the position of the world shot point according to the unsolved trajectory stage set and the preset shot point adjustment rule.

The set of unsolved trajectory phases includes: one or more of acceleration phase trajectory, following phase trajectory, and deceleration phase trajectory.

Optionally, the preset shot point adjustment rule is randomly selected within a preset position range. It can be understood that the preset shot point adjustment rules can also be set to other rules. For example, when the unsolved trajectory stage set includes a deceleration stage trajectory, it means that the throw is too early, and the The position of the world shot point is adjusted to the direction close to the trajectory of the deceleration stage, which is not specifically limited in this example.

The target joint angle set is used to control the joint angle of the joints of the redundant arm of the humanoid robot, so as to realize the projecting of the projectile object to the world target point position.

In one embodiment, the above-mentioned step of generating an acceleration phase trajectory according to the preset acceleration change curve, the world starting point position, the world target point position and the world starting point position includes:

S21: Determine the throwing plane and throwing coordinate system according to the world starting point position and the world target point position;

S22: Converting the world starting point position, the world target point position and the world shooting point position to the coordinate system of casting, to obtain the starting point position of the casting, the position of the casting target point and the position of the throwing shooting point;

S23: Acquire a preset acceleration trajectory curve, and in the throwing plane, according to the preset acceleration change curve, the preset acceleration trajectory curve, the throwing starting point position, the throwing target point position and the The acceleration phase trajectory is generated from the position of the throwing hand point.

This embodiment realizes that the casting plane and the casting coordinate system are established first, and then the acceleration phase trajectory is generated according to the preset acceleration change curve and the preset acceleration trajectory curve based on the casting plane and the casting coordinate system, This provides the basis for the planning of the three-stage throwing trajectory.

For S21, the plane where the gravity direction, the world starting point position and the world target point position are located is used as the throwing plane; then the throwing coordinate system is determined according to the throwing plane, wherein the world starting point position is on the throwing plane As the origin of the throwing coordinate system, the X-axis of the throwing coordinate system is parallel to the throwing plane, the Y-axis of the throwing coordinate system is perpendicular to the throwing plane, and the Z-axis of the throwing coordinate system is parallel to the direction of gravity on the contrary.

，抛投坐标系的Y轴表述为

，抛投坐标系的Z轴表述为

，相互计算关系为：Among them, the X-axis of the throwing coordinate system is expressed as

, the Y-axis of the throwing coordinate system is expressed as

, the Z axis of the throwing coordinate system is expressed as

, the mutual calculation relationship is:

是所述世界目标点位置，

是所述世界起点位置，

是计算向量的

的模长，

为

，

中的T是转置。

is the world target location,

is the starting position of the world,

is the computed vector

the modulo length,

for

,

The T in is the transpose.

是抛投坐标系相对世界坐标系的齐次变换矩阵。in,

is the homogeneous transformation matrix of the projection coordinate system relative to the world coordinate system.

That is, a homogeneous transformation matrix includes a rotation matrix and position coordinates.

For S22, transform the world starting point position to the casting coordinate system to obtain the throwing starting point position; perform the casting coordinate system transformation on the world target point position to obtain the throwing target point position; Convert the coordinate system of the throwing to get the position of the throwing point. Since the position of the world starting point position on the throwing plane is taken as the origin of the throwing coordinate system, the throwing starting point position is (0, 0, 0).

For S23, the preset acceleration trajectory curve can be obtained from the storage space of the humanoid robot, the preset acceleration trajectory curve can also be obtained from an industrial computer, and the preset acceleration trajectory curve can also be obtained from a third-party application system.

Optionally, the preset acceleration trajectory curve is set as a straight line, thereby simplifying the control of the redundant arm. It can be understood that, the preset acceleration trajectory curve may also be set to other linearity, for example, a curve, which is not specifically limited herein.

Wherein, in the throwing plane, according to the preset acceleration change curve, the preset acceleration trajectory curve, the throwing starting point position, the throwing target point position and the throwing hand position respectively The calculation of the throwing angle, the calculation of the throwing speed and the generation of the trajectory of the acceleration phase are performed.

In one embodiment, in the throwing plane, according to the preset acceleration change curve, the preset acceleration trajectory curve, the throwing starting point position, the throwing target point position and the throwing The step of generating the trajectory of the acceleration phase from the position of the throwing hand point includes:

S231: In the throwing plane, according to the preset acceleration trajectory curve, the throwing target point position and the throwing hand point position, respectively perform throwing angle calculation and throwing speed calculation to obtain the target throwing angle and target throwing speed;

S232: Obtain the starting speed, the maximum acceleration, the total acceleration time and the maximum acceleration time ratio;

S233: In the throwing plane, according to the target throwing angle, the target throwing speed, the throwing starting point position, the starting point speed, the maximum acceleration, the total acceleration time and the The maximum acceleration time ratio generates the acceleration phase trajectory, wherein the throwing hand position is used as the end point of the trajectory path of the acceleration phase trajectory, and the target throwing speed is used as the acceleration phase trajectory. Trajectory velocity, the target throw angle as the angle of the tangent to the end point of the trajectory path of the acceleration phase trajectory.

In this embodiment, firstly, in the throwing plane, according to the preset acceleration trajectory curve, the position of the throwing target point and the position of the throwing hand point, the throwing angle calculation and throwing speed calculation are respectively performed, and then the throwing angle is calculated in the throwing plane. In the throwing plane, according to the target throwing angle, the target throwing speed, the throwing starting point position, the starting point speed, the maximum acceleration, the total acceleration time and the maximum acceleration time Proportional generation of the acceleration phase trajectory provides the basis for the planning of a three-stage throw trajectory.

For S231, in the throwing plane, because the preset acceleration trajectory curve is a straight line, the throwing angle is calculated according to the throwing point position, and the calculated throwing angle is used as the target throwing angle; Then in the throwing plane, the throwing speed is calculated according to the target throwing angle, the throwing target point position, the throwing hand position and the acceleration of gravity, and the calculated throwing velocity is used as the target throwing speed. speed.

是对

的转置计算。Understandably,

is true

transpose calculation.

的计算公式和目标抛出速度

的计算公式为：Wherein, the preset acceleration trajectory curve is a straight line, and the target throwing angle

Calculation formula and target throwing speed

The calculation formula is:

是四象限反正切函数，

是所述抛投出手点位置，

为

，

为

，

是所述抛投目标点位置，

是转置计算，cos是余弦函数，sin是正弦函数，g是重力加速度。

is the four-quadrant arctangent function,

is the release point position of the throw,

for

,

for

,

is the position of the target point of the cast,

is the transpose calculation, cos is the cosine function, sin is the sine function, and g is the acceleration of gravity.

That is to say, in the throwing plane, because the preset acceleration trajectory curve is a straight line, first extract the value in the Z-axis direction and the value in the X-axis direction from the position of the throwing hand point, and then extract The value in the Z-axis direction and the value in the X-axis direction are input into the four-quadrant arctangent function for arctangent calculation, and the calculated data is used as the target throwing angle; the position of the throwing target point is subtracted from the throwing hand. point position, the straight-line distance between the throwing target point and the shot point is obtained; in the throwing plane, the throwing speed is calculated according to the acceleration of gravity, the throwing angle of the target and the straight-line distance between the throwing target point and the shooting point, Use the calculated throwing velocity as the target throwing velocity.

For S232, the starting point speed, the maximum acceleration, the total acceleration time and the maximum acceleration time ratio can be obtained from the storage space of the humanoid robot, and the starting point speed, the maximum acceleration, the total acceleration time and the maximum acceleration can also be obtained from a third-party application system. For the time-consuming ratio, the starting point speed, the maximum acceleration, the total acceleration duration and the maximum acceleration time-consuming ratio can also be written into the program implementing the present application.

The starting speed is a specific value, which is the speed of the arm end of the redundant arm at the starting position of the throw.

Optionally, the starting point speed is set to 0. It can be understood that, the starting speed may also be other values, which are not specifically limited herein.

The maximum acceleration is the maximum acceleration of the arm end of the redundant arm in the acceleration phase (that is, the phase corresponding to the acceleration phase trajectory).

The maximum acceleration time ratio is the ratio of the time taken by the arm end of the redundant arm to accelerate from the throwing starting point to the maximum acceleration in the acceleration phase to the total acceleration time.

The total acceleration time is the total time that the arm end of the redundant arm does the acceleration movement in the acceleration phase.

For S233, because the preset acceleration trajectory curve is a straight line, and the arm end of the redundant arm is to throw the throwing object at the end of the acceleration phase trajectory, the acceleration phase trajectory is a straight trajectory, and the throwing The shot point position is the end point of the trajectory path of the acceleration phase trajectory, the target throwing velocity is the trajectory speed of the end point of the trajectory path of the acceleration phase trajectory, and the target throw angle is the trajectory of the acceleration phase trajectory The angle of the tangent to the end point of the path.

和所述轨迹速度

的计算公式为：where the trajectory path

and the trajectory velocity

The calculation formula is:

是所述目标抛出速度，

是所述最大加速度用时比例，

是所述加速总时长，

是所述最大加速度，

是所述起点速度，

是所述抛投起点位置。

is the target throw velocity,

is the ratio of the maximum acceleration time,

is the total duration of the acceleration,

is the maximum acceleration,

is the starting speed,

is the starting position of the throw.

是计算

的

，

中的

是所述目标抛出速度。Understandably,

is to calculate

of

,

middle

is the target throw velocity.

That is to say, multiply the modulo length of the straight-line distance between the throwing target point and the shooting point by 2 and divide by the target throwing speed in turn, and use the calculated data as the total acceleration time; use the maximum acceleration time The ratio is multiplied by the total acceleration duration to obtain the duration of the continuously increasing acceleration phase; the maximum acceleration is divided by the duration of the continuously increasing acceleration phase to obtain the average acceleration of the continuously increasing acceleration phase; the total acceleration duration is calculated Subtract the duration of the continuously increasing acceleration phase to get the end duration of the constant acceleration phase.

Because the preset acceleration change curve is set as a trapezoidal curve, for the acceleration phase trajectory that is less than the duration of the continuously increasing acceleration phase, the trajectory speed is calculated according to the starting point speed, the average acceleration of the continuously increasing acceleration phase and the running time, according to the Calculate the trajectory position of the throwing starting point position, the starting point speed, the average acceleration of the continuously increasing acceleration phase and the running time; for the duration greater than or equal to the continuously increasing acceleration phase, and less than or equal to the end of the constant acceleration phase The acceleration phase trajectory of the duration, the trajectory speed is calculated according to the duration of the continuously increasing acceleration phase, the starting point speed, the average acceleration of the continuously increasing acceleration phase and the running time, and the duration of the continuously increasing acceleration phase, the casting start point The position, the starting speed, the average acceleration of the continuously increasing acceleration phase, and the running time are used to calculate the trajectory position; for the acceleration phase trajectory that is greater than the end duration of the acceleration phase that remains unchanged, and is less than the total acceleration duration, according to the acceleration total duration. The duration, the duration of the continuously increasing acceleration phase, the starting point speed, the average acceleration of the continuously increasing acceleration phase, and the running time to calculate the trajectory velocity, according to the total acceleration duration, the duration of the continuously increasing acceleration phase, the throwing The position of the starting point, the speed of the starting point, the average acceleration of the continuously increasing acceleration phase and the running time calculate the trajectory position.

Wherein, the acceleration phase trajectory that is less than the duration of the continuously increasing acceleration phase is the trajectory of the acceleration continuously increasing; the trajectory that is greater than or equal to the duration of the continuously increasing acceleration phase, and is less than or equal to the end duration of the constant acceleration phase, is the acceleration phase trajectory of the trajectory where the acceleration remains unchanged; the acceleration phase trajectory that is greater than the end duration of the acceleration maintaining phase and less than the total acceleration duration is the acceleration reduction trajectory. That is to say, the trajectory of the acceleration phase is divided into three sub-phases: acceleration increasing, acceleration maintaining, and acceleration decreasing unchanged.

是抛物坐标系的原点，也就是所述抛投起点位置，

是所述抛投出手点位置和跟随阶段轨迹的开始点，

是跟随阶段轨迹的结束点和减速阶段轨迹的开始点，

是减速阶段轨迹的结束点，

是所述抛投目标点位置。Referring to FIG. 3, a schematic diagram of a parabolic trajectory is disclosed.

is the origin of the parabolic coordinate system, that is, the starting point of the throwing,

is the position of the throwing hand and the starting point of the trajectory of the follow-up phase,

is the end point of the trajectory of the following phase and the start point of the trajectory of the deceleration phase,

is the end point of the deceleration phase trajectory,

is the throwing target point position.

In one embodiment, the above-mentioned steps of separately performing joint angle solutions for each of the discrete positions to obtain multiple solution results include:

S51: Using the DH parameter rule, establish a local coordinate system for the arm base and each joint of the redundant arm, and obtain the arm base coordinate system and the set of joint coordinate systems;

S52: Obtain the length of the upper arm, the length of the forearm, and the length from the palm to the wrist of the redundant arm;

S53: Obtain the discrete positions from each of the discrete positions as a target discrete position;

S54: Determine the initial position of the center point of the center point of the thrown object under the arm base coordinate system according to the target discrete position and the length from the palm to the wrist;

S55: Obtain the initial value of the fifth joint angle, the initial value of the sixth joint angle, and the initial value of the seventh joint angle of the redundant arm, according to the arm base coordinate system, the joint coordinate system set, and the length of the upper arm , the length of the forearm, the length from the palm to the wrist, the initial position of the center point, the initial value of the fifth joint angle, the initial value of the sixth joint angle and the initial value of the seventh joint angle, for all The joint angle calculation is performed on each joint of the redundant arm, and the joint angle calculation result set is obtained;

S56: When the joint angle calculation result does not exist in the joint angle calculation result set and the result is no solution, repeat the step of obtaining the discrete position from each of the discrete positions as the target discrete position, until the joint angle There is no solution in the calculation result set of the joint angle or the acquisition of the discrete position is completed;

S57: When the joint angle calculation result is no solution in the joint angle calculation result set, it is determined that the joint angle calculation result corresponding to the joint angle calculation result set is no solution. untie;

S58: When the joint angle calculation result does not exist in the joint angle calculation result set and the joint angle calculation result is no solution, determine that there is no solution result corresponding to the joint angle calculation result set for which the joint angle calculation result is no solution for solution.

In this embodiment, a local coordinate system is first established for the arm base and each joint of the redundant arm, and secondly, the fifth joint of the redundant arm does not affect the position of the thrown object and the first joint of the redundant arm. The joint angles of the sixth joint and the seventh joint are calculated according to the fixed rules during the throwing process, and then the posture of the arm end in the arm base coordinate system is calculated according to the calculated joint angles. The Z-axis direction of the seventh coordinate system of the joint coordinate system set is used as the palm opening and closing direction, and the sixth and seventh joints of the redundant arm are fixed during the throwing process. The joint angle calculation is carried out for each joint of the redundant arm, so that the joint angle is solved for each discrete position using the inverse solution calculation, and the step-by-step analytical inverse of the throwing planning of the redundant arm of the humanoid robot is realized. The solution calculation improves the trajectory solving accuracy and the trajectory solving rate.

For S51, the DH parameter is the parameter of the robotic arm of the robot. The DH parameter generally uses four parameters: X-axis rotation, X-axis translation, Z-axis rotation and Z-axis translation; X-axis rotation, generally expressed by α; X-axis translation, generally expressed by a; Z-axis rotation, generally expressed by θ ; Z-axis translation, generally expressed by d. According to the order of axes, it is divided into standard DH parameters and improved DH parameters.

Wherein, using the DH parameter rule, the specific steps of establishing a local coordinate system for the arm base and each joint of the redundant arm will not be repeated here.

Since the redundant arm of the humanoid robot of the present application is an arm with seven degrees of freedom, there are seven joint coordinate systems in the set of joint coordinate systems.

For S52, the length of the upper arm, the length of the forearm, and the length from the palm to the wrist of the redundant arm can be obtained from the storage space of the humanoid robot, or the length of the upper arm, the length of the forearm of the redundant arm can be obtained from the industrial computer length, palm-to-wrist length, the upper arm length, forearm length, and palm-to-wrist length of the redundant arm can also be obtained from a third-party application system, and the upper arm length, forearm length, The palm-to-wrist length is written into the program implementing this application.

The upper arm length is the length of the upper arm of the redundant arm.

The forearm length is the length of the forearm of the redundant arm.

The palm-to-wrist length is the length from the palm to the wrist of the hand of the redundant arm.

For S53, one discrete position is sequentially acquired from each of the discrete positions, and the acquired discrete position is used as a target discrete position.

For S54, since the target discrete position is the position under the throwing coordinate system, the target discrete position is converted into the position under the arm base coordinate system to obtain the base discrete position; the base discrete position is the redundant arm's position. The position data of the end of the arm, and the length from the palm to the wrist is a fixed value. Therefore, according to the discrete position of the base and the length from the palm to the wrist, the center point of the center point of the thrown object under the arm base coordinate system can be obtained. initial position.

In S55, the initial value of the fifth joint angle is the initial value of the joint angle of the fifth joint. The initial value of the sixth joint angle is the initial value of the joint angle of the sixth joint. The initial value of the seventh joint angle is the initial value of the joint angle of the seventh joint.

Wherein, according to the arm base coordinate system, the joint coordinate system set, the length of the upper arm, the length of the forearm, the length from the palm to the wrist, the initial position of the center point, the initial position of the fifth joint angle value, the initial value of the sixth joint angle and the initial value of the seventh joint angle, the joint angle calculation and matching of the first joint, the second joint, the third joint and the fourth joint of the redundant arm are performed in turn. The joint angle calculation is performed on each joint of the redundant arm, and each joint angle obtained by the calculation is used as the joint angle calculation result set

For S56, when there is no joint angle calculation result in the joint angle calculation result set and the result is no solution, it means that the joint angle corresponding to the first joint, the joint angle corresponding to the second joint, and the third joint can be calculated at the target discrete position. Values of the joint angle corresponding to the joint, the joint angle corresponding to the fourth joint, and the joint angle corresponding to the fifth joint, and repeat steps S53 to S56 until the joint angle calculation result exists in the joint angle calculation result set, and the calculation result of the joint angle is none. Solve or complete the acquisition of the discrete positions.

For S57, when there is no solution in the joint angle calculation result in the joint angle calculation result set, it means that the solution result corresponding to the discrete position corresponding to the joint angle calculation result set is no solution, Therefore, it is determined that the solution result corresponding to the discrete position corresponding to the joint angle calculation result set for which the joint angle calculation result is no solution is no solution.

For S8, when the joint angle calculation result does not exist in the joint angle calculation result set and the joint angle calculation result is no solution, it means that all the joint angle calculation results in the joint angle calculation result set have solutions, so it is determined that there is no solution. The solution result corresponding to the discrete position corresponding to the joint angle calculation result set for which the joint angle calculation result is no solution exists is a solution.

In one embodiment, the above is based on the arm base coordinate system, the joint coordinate system set, the upper arm length, the forearm length, the length from the palm to the wrist, the initial position of the center point, the The initial value of the fifth joint angle, the initial value of the sixth joint angle and the initial value of the seventh joint angle, the joint angle calculation is performed on each joint of the redundant arm, and the steps of obtaining a set of joint angle calculation results include:

S551: Based on the rule that the fifth joint of the redundant arm does not affect the position of the object to be thrown and that the sixth and seventh joints of the redundant arm are fixed during the throwing process, according to the rule of the arm The base coordinate system, the set of joint coordinate systems, the length of the upper arm, the length of the forearm, the length from the palm to the wrist, and the initial position of the center point, for the first joint and the second joint of the redundant arm Joint angle calculation is performed on the joint, the third joint and the fourth joint to obtain the to-be-adjusted value of the first joint angle, the to-be-adjusted value of the second joint angle, the to-be-adjusted value of the third joint angle and the to-be-adjusted value of the fourth joint angle;

S552: Wait according to the arm base coordinate system, the joint coordinate system set, the upper arm length, the forearm length, the length from the palm to the wrist, the initial position of the center point, and the first joint angle Adjustment value, to-be-adjusted value of the second joint angle, to-be-adjusted value of the third joint angle, to-be-adjusted value of the fourth joint angle, initial value of the fifth joint angle, initial value of the sixth joint angle and the initial value of the seventh joint angle, perform joint angle calculation on each joint of the redundant arm, and obtain the joint angle calculation result set.

In this embodiment, the joint angle is first determined based on the rule that the fifth joint of the redundant arm does not affect the position of the thrown object and that the sixth and seventh joints of the redundant arm are fixed during the throwing process Calculate, and then calculate the joint angle of each joint of the redundant arm, so as to realize the solution of the joint angle of each discrete position by using the inverse solution calculation, and realize the throwing of the redundant arm of the humanoid robot. The planned step-by-step analytical inverse solution calculation improves the trajectory solving accuracy and the trajectory solving rate.

For S551, based on the rule that the fifth joint of the redundant arm does not affect the position of the throwing object and the sixth and seventh joints of the redundant arm are fixed during the throwing process, that is, keep The joint angle corresponding to the fifth joint, the joint angle corresponding to the sixth joint, and the joint angle corresponding to the seventh joint do not change.

Wherein, the joint angle calculation is performed on the first joint, the second joint, the third joint and the fourth joint of the redundant arm, that is, when the arm end of the redundant arm is calculated to move to the target discrete position, the first joint angle is calculated. Values of the joint angle corresponding to one joint, the joint angle corresponding to the second joint, the joint angle corresponding to the third joint, and the joint angle corresponding to the fourth joint. The value calculated from the joint angle of the first joint is taken as the value to be adjusted for the first joint angle, the value calculated from the joint angle of the second joint is taken as the value to be adjusted for the second joint angle, and the joint angle of the third joint is taken as the value to be adjusted for the second joint. The calculated value is used as the to-be-adjusted value of the third joint angle, and the value calculated from the joint angle of the fourth joint is used as the to-be-adjusted value of the fourth joint angle.

For S552, according to the arm base coordinate system, the joint coordinate system set, the upper arm length, the forearm length, the length from the palm to the wrist, the initial position of the center point, and the first joint angle Value to be adjusted, value to be adjusted of the second joint angle, value to be adjusted of the third joint angle, value to be adjusted of the fourth joint angle, initial value of the fifth joint angle, initial value of the sixth joint angle value and the initial value of the seventh joint angle, the posture calculation of the arm end in the arm base coordinate system and the joint angle calculation of each joint of the redundant arm are performed in turn, and the calculated joint angles are calculated. As the joint angle calculation result set.

In one embodiment, the above is based on the arm base coordinate system, the joint coordinate system set, the upper arm length, the forearm length, the length from the palm to the wrist, the initial position of the center point, the The value to be adjusted of the first joint angle, the value to be adjusted of the second joint angle, the value to be adjusted of the third joint angle, the value to be adjusted of the fourth joint angle, the initial value of the fifth joint angle, the The initial value of the six joint angles and the initial value of the seventh joint angle, the joint angle calculation is performed on each joint of the redundant arm, and the steps of obtaining the joint angle calculation result set include:

S5521: According to the to-be-adjusted value of the first joint angle, the to-be-adjusted value of the second joint angle, the to-be-adjusted value of the third joint angle, the to-be-adjusted value of the fourth joint angle, and the initial value of the fifth joint angle value, the initial value of the sixth joint angle and the initial value of the seventh joint angle to calculate the posture of the arm end under the arm base coordinate system to obtain the first end posture;

S5522: Based on taking the Z-axis direction of the seventh coordinate system of the joint coordinate system set as the palm opening and closing direction and the sixth joint and the seventh joint of the redundant arm being fixed during the throwing process according to the arm base coordinate system, the set of joint coordinate systems, the length of the upper arm, the length of the forearm, the length from the palm to the wrist, the initial position of the center point and the first end posture , perform joint angle calculation on each joint of the redundant arm, and obtain the joint angle calculation result set.

This embodiment firstly calculates the posture of the arm end in the arm base coordinate system according to the calculated joint angles, and then uses the Z-axis direction of the seventh coordinate system of the joint coordinate system set as the palm opening and closing direction The sixth joint and the seventh joint of the redundant arm are fixed and unchanged during the throwing process to calculate the joint angle of each joint of the redundant arm, so as to realize the inverse solution calculation. The joint angle is solved at each discrete position, which realizes the step-by-step analytical inverse solution calculation for the throwing planning of the redundant arm of the humanoid robot, improves the trajectory solving accuracy and improves the trajectory solving rate.

For S5521, according to the positive kinematics solution of the robot arm, according to the to-be-adjusted value of the first joint angle, the to-be-adjusted value of the second joint angle, the to-be-adjusted value of the third joint angle, and the to-be-adjusted value of the fourth joint angle The adjustment value, the initial value of the fifth joint angle, the initial value of the sixth joint angle and the initial value of the seventh joint angle are used to calculate the posture of the arm end in the arm base coordinate system, and the calculated The end pose is used as the first end pose, thereby realizing the mapping of joint angles to the task space.

For S5522, the Z-axis direction of the seventh coordinate system of the joint coordinate system set (the local coordinate system corresponding to the seventh joint) is used as the palm opening and closing direction and the sixth joint of the redundant arm and the The rule that the seventh joint is fixed during the throwing process, that is, the joint angle corresponding to the sixth joint and the joint angle corresponding to the seventh joint are kept unchanged. When the arm end of the redundant arm is located at the target discrete position, Adjust the joint angle corresponding to the fifth joint and calculate the joint angle corresponding to the first joint, the joint angle corresponding to the second joint, the joint angle corresponding to the third joint and the joint angle corresponding to the fourth joint, so that the opening and closing direction of the palm is consistent with the throwing direction. The projection plane is parallel.

Wherein, the joint angle calculation is performed on each joint of the redundant arm, that is, when the arm end of the redundant arm moves to the target discrete position, the joint angle corresponding to the first joint and the joint corresponding to the second joint are calculated. angle, the joint angle corresponding to the third joint, the joint angle corresponding to the fourth joint, and the value of the joint angle corresponding to the fifth joint. When the value of the joint angle of the joint can be calculated, it means that the joint angle corresponding to the joint is calculated as There is a solution. When the value of the joint angle of the joint cannot be calculated, it means that the calculation result of the joint angle corresponding to the joint is no solution.

It can be understood that each time the joint angle calculation is performed on each joint of the redundant arm to obtain the joint angle calculation result set, the joint angle calculation result set refers to the five joint angle calculation results, and the five joint angle calculation results correspond to The first joint, the first joint, the first joint, the first joint, the fifth joint.

In one embodiment, the above-mentioned rule based on the fifth joint of the redundant arm does not affect the position of the throwing object and the rule that the sixth joint and the seventh joint of the redundant arm are fixed during the throwing process , according to the arm base coordinate system, the set of joint coordinate systems, the length of the upper arm, the length of the forearm, the length from the palm to the wrist and the initial position of the center point, the first position of the redundant arm is determined. The joint angles of the first joint, the second joint, the third joint and the fourth joint are calculated to obtain the first joint angle to be adjusted, the second joint angle to be adjusted, the third joint angle to be adjusted and the fourth joint angle to be adjusted value steps, including:

S5511: Calculate the joint angle of the fourth joint of the redundant arm according to the initial position of the center point, the length of the upper arm, the length of the forearm, and the length from the palm to the wrist, to obtain the fourth joint angle value to be adjusted;

S5512: Based on the rule that the fifth joint of the redundant arm does not affect the position of the thrown object, the sixth joint and the seventh joint of the redundant arm are fixed during the throwing process, and the redundant The kinematic equation of the arm end of the arm, according to the arm base coordinate system, the joint coordinate system set, the length of the upper arm, the length of the forearm, the length from the palm to the wrist and the initial position of the center point, determining the initial rotation matrix of the third coordinate system in the set of joint coordinate systems relative to the coordinate system of the arm base;

S5513: According to the initial rotation matrix, perform joint angle calculation on the first joint, the second joint and the third joint of the redundant arm to obtain the to-be-adjusted value of the first joint angle and the to-be-adjusted value of the second joint angle. The adjustment value and the to-be-adjusted value of the third joint angle.

This embodiment implements the rule that the fifth joint of the redundant arm does not affect the position of the throwing object and the sixth and seventh joints of the redundant arm are fixed during the throwing process. The arm base coordinate system, the joint coordinate system set, the length of the upper arm, the length of the forearm, the length from the palm to the wrist, and the initial position of the center point, for the first joint of the redundant arm , the second joint, the third joint and the fourth joint are used to calculate the joint angle, which provides a basis for the attitude calculation of the end of the arm in the coordinate system of the arm base.

、所述第二关节角待调整值

、所述第三关节角待调整值

和所述第四关节角待调整值

通过如下公式计算得到：The first joint angle to be adjusted

, the value of the second joint angle to be adjusted

, the value of the third joint angle to be adjusted

and the value of the fourth joint angle to be adjusted

It is calculated by the following formula:

The kinematic equation of the arm end of the redundant arm is:

是所述中心点初始位置，

是所述关节坐标系集合的第三坐标系相对所述手臂基座坐标系的初始旋转矩阵，

是所述关节坐标系集合的第四坐标系相对所述关节坐标系集合的第三坐标系的所述旋转矩阵，

是在所述冗余手臂的第五关节不影响所述抛投物体的位置和所述冗余手臂的第六关节及第七关节在抛投过程中固定不变时所述关节坐标系集合的第七坐标系相对所述关节坐标系集合的第四坐标系的所述旋转矩阵，

是所述上臂长度，

是所述小臂长度，

是所述手心到手腕长度；in,

is the initial position of the center point,

is the initial rotation matrix of the third coordinate system of the joint coordinate system set relative to the arm base coordinate system,

is the rotation matrix of the fourth coordinate system of the joint coordinate system set relative to the third coordinate system of the joint coordinate system set,

It is the set of joint coordinate systems when the fifth joint of the redundant arm does not affect the position of the throwing object and the sixth and seventh joints of the redundant arm are fixed during the throwing process. the rotation matrix of the seventh coordinate system relative to the fourth coordinate system of the joint coordinate system set,

is the upper arm length,

is the forearm length,

is the palm-to-wrist length;

是所述冗余手臂的肩部与手部之间的相对位置关系，

是所述冗余手臂的肘部与手部之间的相对位置关系，

是所述冗余手臂的手腕与手部之间的相对位置关系，分别表述为：

is the relative positional relationship between the shoulder and the hand of the redundant arm,

is the relative positional relationship between the elbow and the hand of the redundant arm,

is the relative positional relationship between the wrist and the hand of the redundant arm, expressed as:

是所述上臂长度，

是所述小臂长度，

是所述手心到手腕长度；in,

is the upper arm length,

is the forearm length,

is the palm-to-wrist length;

的计算公式为：According to the position where the fifth joint does not affect the thrown object, determine the length from the palm to the elbow

The calculation formula is:

The kinematic equation of the arm end of the redundant arm is expressed as:

等效为：Will

Equivalent to:

是所述冗余手臂的肘部，

是所述冗余手臂的手腕，

是所述冗余手臂的所述抛投物体的中心点；Referring to Figure 4, wherein,

is the elbow of the redundant arm,

is the wrist of the redundant arm,

is the center point of the throwing object of the redundant arm;

平面与参考平面的夹角作为初始臂角，其中，所述参考平面是所述第三关节的关节角为0时的

平面，

是所述冗余手臂的肩部；Referring to Figure 4, the

The angle between the plane and the reference plane is used as the initial arm angle, wherein the reference plane is the joint angle of the third joint when the joint angle is 0.

flat,

is the shoulder of the redundant arm;

相对所述手臂基座坐标系的旋转矩阵

的计算公式为：According to the Rodrigues formula, the initial arm angle

Rotation matrix relative to the arm base coordinate system

The calculation formula is:

为

，

是

轴的单位矢量，

是

的反对称矩阵，

轴是所述冗余手臂的肩部与所述抛投物体的中心点的连线；in,

for

,

Yes

the unit vector of the axis,

Yes

The antisymmetric matrix of ,

The axis is the line connecting the shoulder of the redundant arm and the center point of the throwing object;

计算公式为：The initial rotation matrix of the third coordinate system relative to the arm base coordinate system

The calculation formula is:

是3*3的矩阵，

是3*3的矩阵，

是3*3的矩阵，

是

为0时所述第三坐标系相对所述手臂基座坐标系的初始旋转矩阵；in,

is a 3*3 matrix,

is a 3*3 matrix,

is a 3*3 matrix,

Yes

When it is 0, the initial rotation matrix of the third coordinate system relative to the arm base coordinate system;

的计算公式：According to the triangle set relationship, the to-be-adjusted value of the fourth joint angle is obtained

Calculation formula:

、第二关节角待调整值

、第三关节角待调整值

的计算公式：According to the equation relationship of the kinematic equation of the arm end, the to-be-adjusted value of the first joint angle is obtained

, the second joint angle to be adjusted

, the third joint angle to be adjusted

Calculation formula:

是

的第2行第2列的值，

是

的第2行第2列的值，

是

的第2行第2列的值，

是

的第1行第2列的值，

是

的第1行第2列的值，

是

的第1行第2列的值，

是

的第3行第2列的值，

是

的第3行第2列的值，

是

的第3行第2列的值，

是

的第3行第3列的值，

是

的第3行第3列的值，

是

的第3行第3列的值，

是

的第3行第1列的值，

是

的第3行第1列的值，

是

的第3行第1列的值。in,

Yes

The value of row 2 and column 2 of ,

Yes

The value of row 2 and column 2 of ,

Yes

The value of row 2 and column 2 of ,

Yes

The value of row 1 and column 2 of ,

Yes

The value of row 1 and column 2 of ,

Yes

The value of row 1 and column 2 of ,

Yes

The value of row 3 and column 2 of ,

Yes

The value of row 3 and column 2 of ,

Yes

The value of row 3 and column 2 of ,

Yes

The value of row 3 and column 3 of ,

Yes

The value of row 3 and column 3 of ,

Yes

The value of row 3 and column 3 of ,

Yes

The value of row 3, column 1 of ,

Yes

The value of row 3, column 1 of ,

Yes

The value of row 3, column 1.

The rotation matrix is a rotation matrix of a homogeneous transformation matrix.

、第二关节角待调整值

、第三关节角待调整值

的计算公式，也就是根据等效手臂的运动学方程等式关系得到所述第一关节角待调整值

、第二关节角待调整值

、第三关节角待调整值

的计算公式。Wherein, according to the equation relationship of the kinematic equation of the arm end, the to-be-adjusted value of the first joint angle is obtained

, the second joint angle to be adjusted

, the third joint angle to be adjusted

The calculation formula of , that is, the value to be adjusted of the first joint angle is obtained according to the equation relationship of the kinematic equation of the equivalent arm

, the second joint angle to be adjusted

, the third joint angle to be adjusted

calculation formula.

即可计算得到所述第四关节角待调整值

。That is, using the formula

The value to be adjusted of the fourth joint angle can be calculated

.

即可计算得到所述第一关节角待调整值

；采用公式

即可计算得到所述第二关节角待调整值

；采用公式

即可计算得到所述第三关节角待调整值

。using the formula

The value to be adjusted of the first joint angle can be calculated and obtained

; using the formula

The value to be adjusted of the second joint angle can be calculated

; using the formula

The value to be adjusted of the third joint angle can be calculated

.

In one embodiment, the above is based on taking the Z-axis direction of the seventh coordinate system of the joint coordinate system set as the palm opening and closing direction and the sixth joint and the seventh joint of the redundant arm when throwing The rules that are fixed in the process are based on the arm base coordinate system, the joint coordinate system set, the upper arm length, the forearm length, the length from the palm to the wrist, the initial position of the center point and all The first terminal posture, the joint angle calculation is performed on each joint of the redundant arm, and the steps of obtaining the joint angle calculation result set include:

S55221: Determine the adjustment angle of the fifth joint based on taking the Z-axis direction of the seventh coordinate system as the palm opening and closing direction and making the Z-axis direction of the seventh coordinate system parallel to the throwing plane;

S55222: Determine the joint angle calculation result corresponding to the fifth joint according to the adjustment angle and the initial value of the fifth joint angle;

S55223: According to the arm base coordinate system, the joint coordinate system set, the length of the upper arm, the length of the upper arm, and the length from the palm to the wrist, perform the calculation on the center point of the wrist of the redundant arm on the Calculate the position in the arm base coordinate system to obtain the center position of the base wrist;

S55224: Calculate the joint angle of the redundant arm according to the center position of the wrist of the base, the length of the upper arm and the length of the forearm, and obtain the calculation result of the joint angle corresponding to the fourth joint;

S55225: Based on the rule that the sixth joint and the seventh joint of the redundant arm are fixed during the throwing process and the Rodrigues formula, according to the adjustment angle, the arm base coordinate system , the joint coordinate system set and the first end posture, determine the target rotation matrix of the third coordinate system in the joint coordinate system set relative to the arm base coordinate system;

S55226: According to the target rotation matrix, perform joint angle calculation on the first joint, the second joint and the third joint of the redundant arm to obtain the joint angle calculation result corresponding to the first joint, the second joint The corresponding joint angle calculation result and the joint angle calculation result corresponding to the third joint;

S55227: According to the calculation result of the joint angle corresponding to the first joint, the calculation result of the joint angle corresponding to the second joint, the calculation result of the joint angle corresponding to the third joint, and the calculation result of the joint angle corresponding to the fourth joint The joint angle calculation result corresponding to the fifth joint is used to determine the joint angle calculation result set.

This embodiment realizes that the throwing process of the sixth joint and the seventh joint of the redundant arm is based on taking the Z-axis direction of the seventh coordinate system of the joint coordinate system set as the palm opening and closing direction and the sixth joint and the seventh joint of the redundant arm. The fixed and invariable rules are based on the arm base coordinate system, the joint coordinate system set, the upper arm length, the forearm length, the length from the palm to the wrist, the initial position of the center point and the first A terminal posture is used to calculate the joint angle of each joint of the redundant arm, so as to realize the solution of the joint angle for each discrete position after the throwing trajectory is discrete, and realize the throwing of the redundant arm of the humanoid robot. The step-by-step analytical inverse solution calculation of the investment planning improves the trajectory solving accuracy and the trajectory solving rate.

对所述第五关节对应的关节角进行调整；Based on taking the Z-axis direction of the seventh coordinate system as the palm opening and closing direction and making the Z-axis direction of the seventh coordinate system parallel to the throwing plane, the adjustment angle is adopted.

adjusting the joint angle corresponding to the fifth joint;

是所述手臂基座坐标系相对所述世界坐标系的旋转矩阵，

是围绕所述世界坐标系的X轴的旋转矩阵，

是所述冗余手臂的第五关节角初始值，

是所述抛投坐标系相对所述世界坐标系的旋转矩阵的第二列向量，

为

，

是所述第一末端姿态，在采用调整角度

对所述第五关节对应的关节角进行调整之后所述第一末端姿态更新为

；in,

is the rotation matrix of the arm base coordinate system relative to the world coordinate system,

is the rotation matrix around the X-axis of the world coordinate system,

is the initial value of the fifth joint angle of the redundant arm,

is the second column vector of the rotation matrix of the cast coordinate system relative to the world coordinate system,

for

,

is the first end attitude, in which the angle of adjustment is adopted

After adjusting the joint angle corresponding to the fifth joint, the posture of the first end is updated as

;

为：The position of the center point of the wrist of the redundant arm in the arm base coordinate system is used as the base wrist center position, and the calculation formula of the base wrist center position

for:

是在采用调整角度

对所述第五关节对应的关节角进行调整之后所述第三坐标系相对所述手臂基座坐标系的目标旋转矩阵，

是在采用调整角度

对所述第五关节对应的关节角进行调整之后所述第四坐标系相对所述第三坐标系的旋转矩阵；

is adjusting the angle

After adjusting the joint angle corresponding to the fifth joint, the target rotation matrix of the third coordinate system relative to the arm base coordinate system,

is adjusting the angle

a rotation matrix of the fourth coordinate system relative to the third coordinate system after adjusting the joint angle corresponding to the fifth joint;

对所述第五关节对应的关节角进行调整之后所述初始臂角

更新为调整后的臂角

，根据罗德里格斯公式，将所述调整后的目标臂角

相对所述手臂基座坐标系的旋转矩阵为

：adjusting the angle

The initial arm angle after adjusting the joint angle corresponding to the fifth joint

Updated to adjusted arm angle

, according to the Rodriguez formula, the adjusted target arm angle

The rotation matrix relative to the arm base coordinate system is

:

是3*3的矩阵，

是3*3的矩阵，

是3*3的矩阵，

是在采用调整角度

对所述第五关节对应的关节角进行调整之后

为0时所述第三坐标系相对所述手臂基座坐标系的旋转矩阵,

是

轴的单位矢量，

是

的反对称矩阵，

轴是所述冗余手臂的肩部与所述冗余手臂的手腕的连线；in,

is a 3*3 matrix,

is a 3*3 matrix,

is a 3*3 matrix,

is adjusting the angle

After adjusting the joint angle corresponding to the fifth joint

When it is 0, the rotation matrix of the third coordinate system relative to the arm base coordinate system,

Yes

the unit vector of the axis,

Yes

The antisymmetric matrix of ,

The axis is the line connecting the shoulder of the redundant arm and the wrist of the redundant arm;

According to the triangle set relationship, the calculation formula is obtained:

Among them, the superscript T is the transpose calculation;

、所述第二关节对应的关节角计算结果

、所述第三关节对应的关节角计算结果

的计算公式：According to the equation relationship of the kinematic equation of the arm end, the calculation result of the joint angle corresponding to the first joint is obtained

, the calculation result of the joint angle corresponding to the second joint

, the calculation result of the joint angle corresponding to the third joint

Calculation formula:

是

的第2行第2列的值，

是

的第2行第2列的值，

是

的第2行第2列的值，

是

的第1行第2列的值，

是

的第1行第2列的值，

是

的第1行第2列的值，

是

的第3行第2列的值，

是

的第3行第2列的值，

是

的第3行第2列的值，

是

的第3行第3列的值，

是

的第3行第3列的值，

是

的第3行第3列的值，

是

的第3行第1列的值，

是

的第3行第1列的值，

是

的第3行第1列的值。in,

Yes

The value of row 2 and column 2 of ,

Yes

The value of row 2 and column 2 of ,

Yes

The value of row 2 and column 2 of ,

Yes

The value of row 1 and column 2 of ,

Yes

The value of row 1 and column 2 of ,

Yes

The value of row 1 and column 2 of ,

Yes

The value of row 3 and column 2 of ,

Yes

The value of row 3 and column 2 of ,

Yes

The value of row 3 and column 2 of ,

Yes

The value of row 3 and column 3 of ,

Yes

The value of row 3 and column 3 of ,

Yes

The value of row 3 and column 3 of ,

Yes

The value of row 3, column 1 of ,

Yes

The value of row 3, column 1 of ,

Yes

The value of row 3, column 1.

对所述第五关节对应的关节角进行调整，以使手掌开合方向与抛投平面平行。Among them, the adjustment angle

The joint angle corresponding to the fifth joint is adjusted so that the opening and closing direction of the palm is parallel to the throwing plane.

即可计算得到所述第四关节对应的关节角计算结果

；采用公式

即可计算得到所述第一关节对应的关节角计算结果

；采用公式

即可计算得到所述第二关节对应的关节角计算结果

；采用公式

即可计算得到所述第三关节对应的关节角计算结果

。Among them, the formula

The calculation result of the joint angle corresponding to the fourth joint can be calculated.

; using the formula

The calculation result of the joint angle corresponding to the first joint can be calculated.

; using the formula

The calculation result of the joint angle corresponding to the second joint can be calculated.

; using the formula

The calculation result of the joint angle corresponding to the third joint can be calculated.

.

Wherein, since the sixth joint and the seventh joint of the redundant arm are fixed during the throwing process, the initial value of the sixth joint angle can be used as the joint angle corresponding to the sixth joint As a result of the calculation, the initial value of the seventh joint angle can be used as the calculation result of the joint angle corresponding to the seventh joint, and finally the calculation result of the joint angle corresponding to the first joint and the joint angle corresponding to the second joint are calculated. result, the calculation result of the joint angle corresponding to the third joint, the calculation result of the joint angle corresponding to the fourth joint, the calculation result of the joint angle corresponding to the fifth joint, the calculation result of the joint angle corresponding to the sixth joint and The joint angle calculation result corresponding to the seventh joint is used as the joint angle calculation result set.

In one embodiment, the above-mentioned steps of obtaining the trajectory of the follow-up phase, the acquisition of the trajectory of the deceleration phase, and the splicing of the trajectory according to the trajectory of the acceleration phase, to obtain the trajectory of the throwing phase, include:

S31: Obtain finger opening and closing delay data of the redundant arm;

S32: Acquire the following phase trajectory according to the finger opening and closing delay data and the acceleration phase trajectory;

S33: Obtain the maximum acceleration of the end of the redundant arm;

S34: Acquire the deceleration phase trajectory according to the terminal maximum acceleration and the following phase trajectory;

S35: Splicing the acceleration phase trajectory, the following phase trajectory, and the deceleration phase trajectory in sequence to obtain the throwing trajectory.

In this embodiment, the following phase trajectory is obtained according to the finger opening and closing delay data and the acceleration phase trajectory, the deceleration phase trajectory is obtained according to the terminal maximum acceleration and the following phase trajectory, and finally the acceleration phase trajectory is obtained. The trajectory, the trajectory of the following phase and the trajectory of the deceleration phase are sequentially spliced to realize a three-stage throwing trajectory. The three-stage throwing trajectory is suitable for the redundant arm of the humanoid robot, so as to realize the The throwing trajectory planning of redundant arms has strong versatility in the face of different throwing targets.

For S31, the finger opening and closing delay data of the redundant arm can be obtained from the storage space of the humanoid robot, or the finger opening and closing delay data of the redundant arm can be obtained from the industrial computer, or the data of the finger opening and closing delay of the redundant arm can be obtained from the industrial computer. The finger opening and closing delay data of the redundant arm is obtained in the third-party application system, and the finger opening and closing delay data of the redundant arm can also be written into the program for realizing the present application.

The finger opening and closing delay data is the time difference data from the finger opening and closing command issued by the redundant arm of the humanoid robot to the finger opening and closing performed by the redundant arm of the humanoid robot.

For S32, the end point data of the acceleration phase trajectory is used as the starting point data of the following phase trajectory, and the following phase trajectory is determined according to a relationship proportional to the finger opening and closing delay data.

For S33, the maximum acceleration of the end of the redundant arm can be obtained from the storage space of the humanoid robot, the maximum acceleration of the end of the redundant arm can also be obtained from the industrial computer, and the maximum acceleration of the end of the redundant arm can also be obtained from a third-party application system. The maximum acceleration of the end of the redundant arm can also be written into the program implementing the present application.

The maximum acceleration of the end of the redundant arm is the maximum acceleration that can be performed by the end of the arm of the redundant arm of the humanoid robot.

For S34, the end point data of the following phase trajectory is used as the starting point data of the deceleration phase trajectory, and the deceleration phase trajectory is determined according to the inverse relationship with the maximum acceleration of the terminal.

Optionally, the trajectory of the deceleration phase adopts a straight line, thereby simplifying the control operation.

Optionally, the straight line of the deceleration phase trajectory and the tangent to the end point of the following phase trajectory are located on the same straight line.

For S35, the acceleration phase trajectory, the following phase trajectory, and the deceleration phase trajectory are sequentially spliced, and the spliced trajectory is used as the throwing trajectory. Therefore, the throwing trajectory sequentially includes three trajectories, namely: the acceleration phase trajectory, the following phase trajectory, and the deceleration phase trajectory.

5, the present application also proposes a throwing trajectory planning device for a redundant arm of a humanoid robot, the device is suitable for the redundant arm of a humanoid robot, and the device includes:

The data acquisition module 100 is used to acquire the position of the world starting point, the position of the world target point and the position of the world shooting point in the world coordinate system;

The acceleration phase trajectory determination module 200 is configured to obtain a preset acceleration change curve, and generate an acceleration phase trajectory according to the preset acceleration change curve, the world starting point position, the world target point position and the world starting point position;

The throwing trajectory determination module 300 is configured to obtain the throwing trajectory by acquiring the following phase trajectory, acquiring the decelerating phase trajectory and splicing the trajectory according to the acceleration phase trajectory respectively;

A discrete position determination module 400, configured to perform discrete processing on the throwing trajectory using a preset time interval to obtain a plurality of discrete positions;

The solution result determination module 500 is used to solve the joint angle for each of the discrete positions respectively to obtain a plurality of solution results;

The target joint angle set determination module 600 is configured to adjust the position of the world shot point when the solution result is no solution, and repeat the step of obtaining the preset acceleration trajectory curve until each solution result is obtained. All have a solution, and the target joint angle set is determined according to each joint angle corresponding to each of the solution results.

In this embodiment, the acceleration phase trajectory is first generated according to the preset acceleration change curve, the world starting point position, the world target point position, and the world shooting point position, and the following phase trajectory acquisition, deceleration phase trajectory acquisition, and trajectory splicing are respectively performed according to the acceleration phase trajectory. , obtain the throwing trajectory, and then use the preset time interval to discretely process the throwing trajectory to obtain multiple discrete positions, and then solve the joint angle for each discrete position separately to obtain multiple solution results, and finally when there is a solution When the result is no solution, adjust the position of the starting point in the world, repeat the steps of generating the trajectory of the acceleration phase until each solution result is a solution, and determine the target joint angle set according to each joint angle corresponding to each solution result. The three-stage throwing trajectory is obtained by splicing the acceleration phase trajectory, the following phase trajectory, and the deceleration phase trajectory. The three-stage throwing trajectory is suitable for the redundant arm of the humanoid robot, so as to realize the throwing of the redundant arm of the humanoid robot. Throwing trajectory planning has strong versatility in the face of different throwing targets; and by solving the joint angle for each discrete position after the throwing trajectory is discrete, the throwing of the redundant arm of the humanoid robot is realized. The planned step-by-step analytical inverse solution calculation improves the trajectory solving accuracy and the trajectory solving rate.

Referring to FIG. 6 , an embodiment of the present application further provides a computer device, the computer device may be a server, and its internal structure may be as shown in FIG. 6 . The computer device includes a processor, memory, a network interface, and a database connected by a system bus. Among them, the processor of the computer design is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium, an internal memory. The nonvolatile storage medium stores an operating system, a computer program, and a database. The memory provides an environment for the execution of the operating system and computer programs in the non-volatile storage medium. The database of the computer equipment is used for storing data such as the throwing trajectory planning method of the redundant arm of the humanoid robot. The network interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a method for planning a throwing trajectory of a redundant arm of a humanoid robot is realized. The method for planning the throwing trajectory of the redundant arm of the humanoid robot, the method is applicable to the redundant arm of the humanoid robot, and the method includes: obtaining the world starting point position, the world target point position and the world position in the world coordinate system The position of the shot point; obtain a preset acceleration change curve, and generate an acceleration stage trajectory according to the preset acceleration change curve, the world starting point position, the world target point position, and the world shooting point position; according to the acceleration stage trajectory Obtaining the trajectory in the following phase, acquiring the trajectory in the deceleration phase, and splicing the trajectory respectively, to obtain the throwing trajectory; using a preset time interval, discretely processing the throwing trajectory to obtain a plurality of discrete positions; The joint angle is solved at discrete positions, and multiple solution results are obtained; when there is no solution in the solution result, the position of the world shot point is adjusted, and the steps of obtaining the preset acceleration trajectory curve are repeated until each The solution results are all solutions, and the target joint angle set is determined according to each joint angle corresponding to each solution result being a solution.

In this embodiment, the acceleration phase trajectory is first generated according to the preset acceleration change curve, the world starting point position, the world target point position, and the world shooting point position, and the following phase trajectory acquisition, deceleration phase trajectory acquisition, and trajectory splicing are respectively performed according to the acceleration phase trajectory. , obtain the throwing trajectory, and then use the preset time interval to discretely process the throwing trajectory to obtain multiple discrete positions, and then solve the joint angle for each discrete position separately to obtain multiple solution results, and finally when there is a solution When the result is no solution, adjust the position of the starting point in the world, repeat the steps of generating the trajectory of the acceleration phase until each solution result is a solution, and determine the target joint angle set according to each joint angle corresponding to each solution result. The three-stage throwing trajectory is obtained by splicing the acceleration phase trajectory, the following phase trajectory, and the deceleration phase trajectory. The three-stage throwing trajectory is suitable for the redundant arm of the humanoid robot, so as to realize the throwing of the redundant arm of the humanoid robot. Throwing trajectory planning has strong versatility in the face of different throwing targets; and by solving the joint angle for each discrete position after the throwing trajectory is discrete, the throwing of the redundant arm of the humanoid robot is realized. The planned step-by-step analytical inverse solution calculation improves the trajectory solving accuracy and the trajectory solving rate.

An embodiment of the present application also provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, a method for planning a throwing trajectory of a redundant arm of a humanoid robot is implemented, including the steps of: The method is applicable to the redundant arm of a humanoid robot, and the method includes: obtaining the world starting point position, the world target point position and the world shooting point position in the world coordinate system; obtaining a preset acceleration change curve, according to the preset acceleration The change curve, the position of the world starting point, the position of the world target point and the position of the world shot point generate an acceleration phase trajectory; according to the acceleration phase trajectory, the acquisition of the follow-up phase trajectory, the acquisition of the deceleration phase trajectory and the trajectory splicing are performed respectively, Obtaining a throwing trajectory; using a preset time interval to perform discrete processing on the throwing trajectory to obtain a plurality of discrete positions; respectively solving the joint angles for each of the discrete positions to obtain a plurality of solving results; When the solution result is no solution, adjust the position of the world shot point, and repeat the steps of obtaining the preset acceleration trajectory curve until each solution result is a solution, and according to each solution result is Each joint angle corresponding to the solution determines the target joint angle set.

The above-mentioned method for planning the throwing trajectory of the redundant arm of the humanoid robot, in this embodiment, the acceleration phase trajectory is first generated according to the preset acceleration change curve, the world starting point position, the world target point position and the world shooting point position, and the acceleration phase trajectory is generated according to the acceleration phase position. The trajectory is obtained by acquiring the trajectory in the following phase, acquiring the trajectory in the deceleration phase, and splicing the trajectory to obtain the throwing trajectory. Secondly, using a preset time interval, the throwing trajectory is discretely processed to obtain multiple discrete positions, and then each discrete position is obtained separately. The joint angle is solved at the position, and multiple solution results are obtained. Finally, when the solution result is no solution, the position of the world shot point is adjusted, and the steps of generating the trajectory of the acceleration stage are repeated until each solution result is a solution. The solution results are that each joint angle corresponding to the solution determines the target joint angle set, and the three-stage throwing trajectory is obtained by splicing the acceleration phase trajectory, the following phase trajectory, and the deceleration phase trajectory. The redundant arm of the robot realizes the throwing trajectory planning of the redundant arm of the humanoid robot, and has strong versatility in the face of different throwing targets; and after discretizing the throwing trajectory, joints are performed for each discrete position. The solution of the angle realizes the step-by-step analytical inverse solution calculation of the throwing planning of the redundant arm of the humanoid robot, which improves the trajectory solving accuracy and the trajectory solving rate.

Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be implemented by instructing relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage In the medium, when the computer program is executed, it may include the processes of the above-mentioned method embodiments. Wherein, any reference to memory, storage, database or other medium provided in this application and used in the embodiments may include non-volatile and/or volatile memory. Nonvolatile memory may include read only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double-rate SDRAM (SSRSDRAM), enhanced SDRAM (ESDRAM), synchronous Link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

It should be noted that, herein, the terms "comprising", "comprising" or any other variation thereof are intended to encompass non-exclusive inclusion, such that a process, device, article or method comprising a series of elements includes not only those elements, It also includes other elements not expressly listed or inherent to such a process, apparatus, article or method. Without further limitation, an element qualified by the phrase "comprising a..." does not preclude the presence of additional identical elements in the process, apparatus, article, or method that includes the element.

The above are only the preferred embodiments of the present application, and are not intended to limit the scope of the patent of the present application. Any equivalent structure or equivalent process transformation made by using the contents of the description and drawings of the present application, or directly or indirectly applied to other related The technical field is similarly included in the scope of patent protection of this application.

Claims (12)

1. the throwing trajectory planning method of the redundant arm of a humanoid robot, is characterized in that, described method is applicable to the redundant arm of humanoid robot, and described method comprises: Obtain the world starting point position, the world target point position and the world shooting point position in the world coordinate system; Acquire a preset acceleration change curve, and generate an acceleration phase trajectory according to the preset acceleration change curve, the world starting point position, the world target point position, and the world starting point position; According to the acceleration phase trajectory, the following phase trajectory acquisition, the deceleration phase trajectory acquisition, and the trajectory splicing are respectively performed to obtain the throwing trajectory; Using a preset time interval, discrete processing is performed on the throwing trajectory to obtain a plurality of discrete positions; The joint angle is solved for each of the discrete positions respectively, and a plurality of solution results are obtained; When the solution result is no solution, adjust the position of the shot point in the world, repeat the steps of obtaining the preset acceleration trajectory curve, until each solution result is a solution, according to each solution The result is that each joint angle corresponding to the solution determines the target joint angle set. 2. the throwing trajectory planning method of the redundant arm of the humanoid robot according to claim 1, is characterized in that, described according to described preset acceleration change curve, described world starting point position, described world target point position and the steps of generating the acceleration phase trajectory from the position of the world shot point, including: Determine the throwing plane and throwing coordinate system according to the world starting point position and the world target point position; Converting the world starting point position, the world target point position and the world shooting point position to the coordinate system of the throwing to obtain the starting point position of the throwing, the position of the throwing target point and the position of the throwing shooting point; Obtain a preset acceleration trajectory curve, in the throwing plane, according to the preset acceleration change curve, the preset acceleration trajectory curve, the throwing starting point position, the throwing target point position and the throwing The release hand position generates the acceleration phase trajectory. 3. the throwing trajectory planning method of the redundant arm of the humanoid robot according to claim 2, is characterized in that, described in described throwing plane, according to described preset acceleration change curve, described preset The step of generating the acceleration phase trajectory from the acceleration trajectory curve, the throwing starting point position, the throwing target point position and the throwing hand point position, including: In the throwing plane, according to the preset acceleration trajectory curve, the throwing target point position and the throwing hand position, the throwing angle calculation and throwing speed calculation are respectively performed to obtain the target throwing angle and target throwing speed; Get the starting point speed, maximum acceleration, total acceleration time and the ratio of maximum acceleration time; In the throwing plane, according to the target throwing angle, the target throwing speed, the throwing starting point position, the starting point speed, the maximum acceleration, the total acceleration time and the maximum acceleration The acceleration phase trajectory is generated by the time ratio, wherein the position of the throwing hand point is used as the end point of the trajectory path of the acceleration phase trajectory, and the target throwing speed is used as the trajectory speed of the end point of the trajectory path of the acceleration phase trajectory , the target throwing angle is taken as the angle of the tangent to the end point of the trajectory path of the acceleration phase trajectory. 4. the throwing trajectory planning method of the redundant arm of the humanoid robot according to claim 3, is characterized in that, described separately carries out the solution of joint angle to each described discrete position, obtains the step of a plurality of solution results ,include: Using the DH parameter rule, a local coordinate system is established for the arm base and each joint of the redundant arm, and the arm base coordinate system and the joint coordinate system set are obtained; Obtain the length of the upper arm, the length of the forearm, and the length from the palm to the wrist of the redundant arm; obtaining the discrete position from each of the discrete positions as a target discrete position; Determine the initial position of the center point of the center point of the thrown object under the arm base coordinate system according to the target discrete position and the length from the palm to the wrist; Obtain the initial value of the fifth joint angle, the initial value of the sixth joint angle and the initial value of the seventh joint angle of the redundant arm, according to the arm base coordinate system, the joint coordinate system set, the upper arm length, the The length of the forearm, the length from the palm to the wrist, the initial position of the center point, the initial value of the fifth joint angle, the initial value of the sixth joint angle and the initial value of the seventh joint angle, for the redundant The joint angle calculation is performed on each joint of the remaining arm to obtain a set of joint angle calculation results; When there is no joint angle calculation result in the joint angle calculation result set and the result is no solution, repeat the step of obtaining the discrete position from each of the discrete positions as the target discrete position until the joint angle calculation result In the set, the joint angle calculation result is no solution or the acquisition of the discrete position is completed; When the joint angle calculation result has no solution in the joint angle calculation result set, it is determined that the solution result corresponding to the joint angle calculation result set for which the joint angle calculation result is no solution is no solution; When the joint angle calculation result does not exist in the joint angle calculation result set and the joint angle calculation result is no solution, it is determined that there is no joint angle calculation result corresponding to the joint angle calculation result set corresponding to the joint angle calculation result is yes. untie. 5. the throwing trajectory planning method of the redundant arm of the humanoid robot according to claim 4, described according to the arm base coordinate system, the joint coordinate system set, the length of the upper arm, the forearm Length, the length from the palm to the wrist, the initial position of the center point, the initial value of the fifth joint angle, the initial value of the sixth joint angle and the initial value of the seventh joint angle, the The steps of calculating the joint angle for each joint to obtain a set of joint angle calculation results include: Based on the rule that the fifth joint of the redundant arm does not affect the position of the thrown object and the sixth and seventh joints of the redundant arm are fixed during the throwing process, according to the arm base The coordinate system, the set of joint coordinate systems, the length of the upper arm, the length of the forearm, the length from the palm to the wrist, and the initial position of the center point, for the first joint, second joint, The third joint and the fourth joint perform joint angle calculation to obtain the to-be-adjusted value of the first joint angle, the to-be-adjusted value of the second joint angle, the to-be-adjusted value of the third joint angle and the to-be-adjusted value of the fourth joint angle; According to the arm base coordinate system, the set of joint coordinate systems, the length of the upper arm, the length of the forearm, the length from the palm to the wrist, the initial position of the center point, and the value to be adjusted for the first joint angle , the to-be-adjusted value of the second joint angle, the to-be-adjusted value of the third joint angle, the to-be-adjusted value of the fourth joint angle, the initial value of the fifth joint angle, the initial value of the sixth joint angle, and the The seventh initial value of the joint angle is used, and joint angle calculation is performed on each joint of the redundant arm to obtain the joint angle calculation result set. 6. the throwing trajectory planning method of the redundant arm of the humanoid robot according to claim 5, described according to the arm base coordinate system, the joint coordinate system set, the upper arm length, the forearm length, the length from the palm to the wrist, the initial position of the center point, the to-be-adjusted value of the first joint angle, the to-be-adjusted value of the second joint angle, the to-be-adjusted value of the third joint angle, the fourth The joint angle to be adjusted, the initial value of the fifth joint angle, the initial value of the sixth joint angle, and the initial value of the seventh joint angle, and calculating the joint angle of each joint of the redundant arm to obtain the The steps of joint angle calculation result set, including: According to the to-be-adjusted value of the first joint angle, the to-be-adjusted value of the second joint angle, the to-be-adjusted value of the third joint angle, the to-be-adjusted value of the fourth joint angle, the initial value of the fifth joint angle, The initial value of the sixth joint angle and the initial value of the seventh joint angle are calculated for the posture of the arm end under the arm base coordinate system to obtain the first end posture; Based on the rule that the Z-axis direction of the seventh coordinate system of the joint coordinate system set is used as the palm opening and closing direction, and the sixth joint and the seventh joint of the redundant arm are fixed during the throwing process. , according to the arm base coordinate system, the joint coordinate system set, the upper arm length, the forearm length, the length from the palm to the wrist, the initial position of the center point and the first end posture, to Each joint of the redundant arm performs joint angle calculation to obtain the joint angle calculation result set. 7. The throwing trajectory planning method of the redundant arm of the humanoid robot according to claim 6, wherein the fifth joint based on the redundant arm does not affect the position and the position of the throwing object. The sixth and seventh joints of the redundant arm are fixed in the casting process, according to the arm base coordinate system, the joint coordinate system set, the length of the upper arm, the length of the forearm, The length from the palm to the wrist and the initial position of the center point, calculate the joint angle of the first joint, the second joint, the third joint and the fourth joint of the redundant arm, and obtain the first joint angle to be adjusted value, The steps of the to-be-adjusted value of the second joint angle, the to-be-adjusted value of the third joint angle, and the to-be-adjusted value of the fourth joint angle include: According to the initial position of the center point, the length of the upper arm, the length of the forearm, and the length from the palm to the wrist, the joint angle of the fourth joint of the redundant arm is calculated, and the fourth joint angle to be adjusted is obtained. value; Based on the rule that the fifth joint of the redundant arm does not affect the position of the thrown object, the sixth and seventh joints of the redundant arm are fixed during the throwing process, and the The kinematic equation of the arm end is determined according to the arm base coordinate system, the joint coordinate system set, the length of the upper arm, the length of the forearm, the length from the palm to the wrist and the initial position of the center point. the initial rotation matrix of the third coordinate system in the set of joint coordinate systems relative to the coordinate system of the arm base; According to the initial rotation matrix, the joint angle calculation is performed on the first joint, the second joint and the third joint of the redundant arm to obtain the to-be-adjusted value of the first joint angle and the to-be-adjusted value of the second joint angle and the third joint angle to be adjusted. 8. the throwing trajectory planning method of the redundant arm of the humanoid robot according to claim 7, is characterized in that, described based on the Z-axis direction of the seventh coordinate system of described joint coordinate system set as palm opening and closing The direction and the rule that the sixth joint and the seventh joint of the redundant arm are fixed during the throwing process, according to the arm base coordinate system, the joint coordinate system set, the length of the upper arm , the length of the forearm, the length from the palm of the hand to the wrist, the initial position of the center point and the attitude of the first end, perform joint angle calculation on each joint of the redundant arm, and obtain the joint angle calculation result set steps, including: The adjustment angle of the fifth joint is determined based on taking the Z-axis direction of the seventh coordinate system as the palm opening and closing direction and making the Z-axis direction of the seventh coordinate system parallel to the throwing plane; Determine the joint angle calculation result corresponding to the fifth joint according to the adjustment angle and the initial value of the fifth joint angle; According to the arm base coordinate system, the joint coordinate system set, the upper arm length, the upper arm length, and the length from the palm to the wrist, the center point of the wrist of the redundant arm is calculated on the arm base Calculate the position in the coordinate system to obtain the center position of the wrist of the base; Calculate the joint angle of the redundant arm according to the center position of the wrist of the base, the length of the upper arm and the length of the forearm, and obtain the calculation result of the joint angle corresponding to the fourth joint; Based on the rule that the sixth joint and the seventh joint of the redundant arm are fixed during the throwing process and the Rodrigues formula, according to the adjustment angle, the coordinate system of the arm base, the the joint coordinate system set and the first end posture, and determine the target rotation matrix of the third coordinate system in the joint coordinate system set relative to the arm base coordinate system; According to the target rotation matrix, the joint angle calculation is performed on the first joint, the second joint and the third joint of the redundant arm to obtain the joint angle calculation result corresponding to the first joint and the joint angle corresponding to the second joint. The joint angle calculation result and the joint angle calculation result corresponding to the third joint; According to the calculation result of the joint angle corresponding to the first joint, the calculation result of the joint angle corresponding to the second joint, the calculation result of the joint angle corresponding to the third joint, the calculation result of the joint angle corresponding to the fourth joint and the The joint angle calculation result corresponding to the fifth joint is determined, and the joint angle calculation result set is determined. 9. the throwing trajectory planning method of the redundant arm of the humanoid robot according to claim 1, is characterized in that, described according to described acceleration phase trajectory, respectively carry out the acquisition of following phase trajectory, the acquisition of deceleration phase trajectory and the trajectory The steps of splicing to get the throwing trajectory include: Acquiring finger opening and closing delay data of the redundant arm; Obtain the following phase trajectory according to the finger opening and closing delay data and the acceleration phase trajectory; Obtain the maximum acceleration of the end of the redundant arm; Obtain the deceleration phase trajectory according to the terminal maximum acceleration and the following phase trajectory; The trajectory of the acceleration phase, the trajectory of the following phase, and the trajectory of the deceleration phase are sequentially spliced to obtain the throwing trajectory. 10. A throwing trajectory planning device for a redundant arm of a humanoid robot, wherein the device is suitable for the redundant arm of a humanoid robot, and the device comprises: The data acquisition module is used to acquire the position of the world starting point, the position of the world target point and the position of the world starting point in the world coordinate system; an acceleration phase trajectory determination module, configured to obtain a preset acceleration change curve, and generate an acceleration phase trajectory according to the preset acceleration change curve, the world starting point position, the world target point position, and the world starting point position; The throwing trajectory determination module is used for obtaining the trajectory of the follow-up phase, the acquisition of the trajectory of the deceleration phase, and the splicing of the trajectory according to the trajectory of the acceleration phase, to obtain the trajectory of the throwing throw; a discrete position determination module, configured to perform discrete processing on the throwing trajectory using a preset time interval to obtain a plurality of discrete positions; The solution result determination module is used to solve the joint angle of each discrete position respectively, and obtain a plurality of solution results; The target joint angle set determination module is used to adjust the position of the world shot point when the solution result is no solution, and repeat the step of obtaining the preset acceleration trajectory curve until each of the solution results is no solution. In order to have a solution, the target joint angle set is determined according to each joint angle corresponding to each of the solution results. 11. A computer device, comprising a memory and a processor, wherein the memory stores a computer program, wherein the processor implements the steps of the method according to any one of claims 1 to 9 when the processor executes the computer program . 12. A computer-readable storage medium on which a computer program is stored, characterized in that, when the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 9 are implemented.

Images