阅读说明：本技术 一种机械臂拼接路径规划方法及装置 (Method and device for planning splicing path of mechanical arm ) 是由 刘传凯 李剑 刘茜 陈钢 谢圆 崔金 袁春强 王晓雪 张济韬 于 2021-08-03 设计创作，主要内容包括：本申请公开了一种机械臂路径规划的方法及计算机设备,该方法包括：确定机械臂起始点参数和目标点参数,根据所述起始点参数和所述目标点参数基于A*算法从预设轨迹库中搜索出机械臂从起始点运动到目标点的多个第一路径,其中,第一路径是指从起始点运动到目标点过程中任意相邻两节点之间的路径；根据预设的拼接算法依次将所述第一路径进行有序拼接得到机械臂从起始点运动到目标点所对应的初始路径,验证所述初始路径是否满足碰撞检测；若不满足,则调整所述初始路径得到调整后的路径,将调整后的路径作为机械臂的路径,其中,调整后的路径满足碰撞检测。本申请解决了现有技术所规划的路径使得机械臂的运动效率较低的技术问题。(The application discloses a method and computer equipment for planning a path of a mechanical arm, wherein the method comprises the following steps: determining a starting point parameter and a target point parameter of the mechanical arm, and searching a plurality of first paths of the mechanical arm from the starting point to the target point from a preset path library based on an A-x algorithm according to the starting point parameter and the target point parameter, wherein the first paths refer to paths between any two adjacent nodes in the process of moving from the starting point to the target point; sequentially splicing the first paths according to a preset splicing algorithm to obtain an initial path corresponding to the movement of the mechanical arm from a starting point to a target point, and verifying whether the initial path meets collision detection; and if not, adjusting the initial path to obtain an adjusted path, and taking the adjusted path as the path of the mechanical arm, wherein the adjusted path meets the collision detection. The technical problem that the path planned by the prior art enables the motion efficiency of the mechanical arm to be lower is solved.)

1. A method for planning a path of a mechanical arm is characterized by comprising the following steps:

determining a starting point parameter and a target point parameter of the mechanical arm, and searching a plurality of first paths of the mechanical arm from the starting point to the target point from a preset path library based on an A-x algorithm according to the starting point parameter and the target point parameter, wherein the first paths refer to paths between any two adjacent nodes in the process of moving from the starting point to the target point;

sequentially splicing the first paths according to a preset splicing algorithm to obtain an initial path corresponding to the movement of the mechanical arm from a starting point to a target point, and verifying whether the initial path meets collision detection;

and if not, adjusting the initial path to obtain an adjusted path, and taking the adjusted path as the path of the mechanical arm, wherein the adjusted path meets the collision detection.

2. The method of claim 1, wherein sequentially splicing the first path according to a preset splicing algorithm to obtain an initial path of the mechanical arm comprises:

determining a starting point and an end point of each first path, and determining any adjacent second path and third path from the plurality of first paths according to the starting point and the end points, wherein the end point of the second path is the same as the starting point of the third path;

respectively determining the acceleration and the planning time length corresponding to the planning of the second path and the third path, and determining the splicing acceleration and the splicing time length according to the preset splicing algorithm, the acceleration and the planning time length;

splicing the second path and the third path according to the splicing acceleration and the splicing duration to obtain a spliced path, wherein the starting point of the planned path is the starting point of the second path, and the end point of the planned path is the end point of the third path;

and splicing the spliced path with the next adjacent first path by a preset splicing algorithm until the plurality of first paths are spliced together to obtain the initial path, wherein the starting point of the next adjacent first path is the same as the end point of the spliced path.

3. The method of claim 2, wherein determining a stitching acceleration and a stitching duration based on the preset stitching algorithm, the acceleration and the planned duration comprises:

calculating the maximum value of the acceleration corresponding to the second path and the third path, and taking the maximum value of the acceleration as the splicing acceleration;

calculating a first speed corresponding to the second path according to the acceleration and the planning time corresponding to the second path, and calculating a second speed corresponding to the third path according to the acceleration and the planning time corresponding to the third path;

and calculating the absolute value of the difference between the first speed and the second speed, and obtaining the splicing time length according to the absolute value and the splicing acceleration.

4. The method of claim 3, wherein the second path and the third path are paths planned by a spatial joint path planning method, a Cartesian path planning method, or a curvilinear path planning method.

5. The method of claim 4, wherein if the second path and the third path are paths planned by a spatial joint path planning method, stitching the second path and the third path according to the stitching acceleration and the stitching duration to obtain a stitched path, comprising:

obtaining the planned path by the following formula:

wherein θ (t) represents the planned path; theta₀Indicating an initial state of the second path;representing the acceleration of the second path; t is t_c1Representing a duration of an acceleration segment of the second path;t_c2representing the duration of the uniform speed segment of the second path; t is t_c3Representing a duration of a deceleration segment of the second path; t is t_f1Denotes the duration of the second path, t_f1＝t_c1+t_c2+t_c3；Representing the splice acceleration; t is t_bRepresenting the splicing time;represents the acceleration of the third path; t is t_c4Represents a duration of an acceleration segment of the third path; t is t_c5Representing the duration of the uniform speed segment of the third path; t is t_c6Represents a duration of a deceleration segment of the third path; t is t_f2Indicates the duration of the third path, t_f2＝t_c4+t_c5+t_c6；θ_f2A target state representing a third path; t is t_ΔAnd representing the difference between the total time length of the second path and the third path and the total time length of the planned path.

6. The method of any of claims 1-5, wherein verifying whether the initial path satisfies collision detection comprises:

constructing an obstacle model and an outer envelope model of the mechanical arm moving along the initial path, and judging whether the outer envelope model and the obstacle model meet preset conditions or not;

if so, determining that the initial path meets the collision detection; otherwise, determining that the initial path does not satisfy collision detection.

7. The method of claim 6, wherein the predetermined conditions include:

there is no intersection between the outer envelope space and the obstacle model; and/or

The shortest distance between the outer envelope space and the obstacle model is greater than a preset threshold.

8. The method of claim 7, wherein adjusting the initial path results in an adjusted path comprising:

determining at least one point with intersection or shortest distance smaller than a preset threshold between the outer envelope space and the obstacle model, and determining a first path to which each point belongs and first paths to which the front section and the rear section of the first path belong on the initial path;

and respectively connecting the starting point of the first path with the terminal of the first path to which the previous section belongs, and connecting the end point of the first path with the starting point of the first path to which the next section belongs to obtain the adjusted path.

9. A computer device, the computer device comprising:

a memory for storing instructions for execution by at least one processor;

a processor for executing instructions stored in a memory to perform the method of any one of claims 1-8.

10. A computer-readable storage medium having stored thereon computer instructions which, when executed on a computer, cause the computer to perform the method of any one of claims 1-8.

Technical Field

The application relates to the technical field of mechanical arm path planning, in particular to a method and computer equipment for mechanical arm path planning.

Background

The mechanical arm of the space station is a seven-degree-of-freedom redundant mechanical arm, the operation flexibility is high, and the goals of high efficiency, high speed, energy loss reduction and base disturbance reduction can be achieved while various complex operations can be realized through the flexibility optimization. In order to control the robot arm to perform various complicated operations, the path of the robot arm needs to be planned.

The existence of the redundant characteristic of path planning of the mechanical arm also makes the planning and control of the mechanical arm more complicated. At present, a path planning scheme of a mechanical arm does not relate to a scheme that motion of a tail end in a Cartesian space is mapped to a joint space, so that the motion of the tail end in the Cartesian space cannot be mapped to the joint space for analysis and solution, and search and optimization need to be carried out in an infinite space, so that the mechanical arm operation path planning not only needs to consider the problem of a kinematics layer, but also needs to be combined with a dynamics model to establish a motion planning system, analyze dynamics characteristics of the mechanical arm under different types of tasks and operations, establish a mechanical arm motion path planning criterion, and provide a motion planning method combining the kinematics model and the dynamics model. In addition, the motion planning of the spatial station robot arm performing complex operations generally cannot reach the target point through one path, and needs to reach the target point from the starting point through a plurality of intermediate points. The existing method generally adopts a mode of firstly decelerating to an intermediate point and then accelerating from the intermediate point to splice the front section and the rear section of the path of the intermediate point, so that the efficiency of the space station mechanical arm for executing complex operation is reduced. Further consideration is needed to be given to optimizing the splicing mode of the front path and the rear path of the intermediate point so as to improve the movement efficiency of the mechanical arm of the space station.

Disclosure of Invention

The technical problem that this application was solved is: the planned path for the prior art makes the motion efficiency of the mechanical arm low. The application provides a method and computer equipment for planning a path of a mechanical arm^＊The algorithm searches a plurality of first paths of the mechanical arm from the starting point to the target point from a preset path library, sequentially splices the first paths according to a preset splicing algorithm to obtain an initial path corresponding to the mechanical arm from the starting point to the target point, verifies whether the initial path meets the collision detection, if not, adjusts the initial path to obtain an adjusted path meeting the collision detection, and provides a splicing mode for optimizing front and rear paths of an intermediate point so as to improve the motion efficiency of the mechanical arm.

In a first aspect, an embodiment of the present application provides a method for planning a path of a robot arm, where the method includes:

determining a starting point parameter and a target point parameter of the mechanical arm, and basing on A according to the starting point parameter and the target point parameter^＊Searching a plurality of first paths of the mechanical arm from the starting point to the target point from a preset track library by using an algorithm, wherein the first paths refer to paths between any two adjacent nodes in the process of moving from the starting point to the target point;

sequentially splicing the first paths according to a preset splicing algorithm to obtain an initial path corresponding to the movement of the mechanical arm from a starting point to a target point, and verifying whether the initial path meets collision detection;

and if not, adjusting the initial path to obtain an adjusted path, and taking the adjusted path as the path of the mechanical arm, wherein the adjusted path meets the collision detection.

In the scheme provided in the examples of this application, according to A^＊Searching a plurality of first paths of the mechanical arm from the starting point to the target point from a preset path library by an algorithm, sequentially splicing the first paths according to a preset splicing algorithm to obtain an initial path corresponding to the mechanical arm from the starting point to the target point, and verifyingAnd whether the initial path meets the collision detection or not is judged, and if not, the initial path is adjusted to obtain an adjusted path meeting the collision detection.

Optionally, sequentially splicing the first path according to a preset splicing algorithm to obtain an initial path of the mechanical arm, including:

determining a starting point and an end point of each first path, and determining any adjacent second path and third path from the plurality of first paths according to the starting point and the end points, wherein the end point of the second path is the same as the starting point of the third path;

respectively determining the acceleration and the planning time length corresponding to the planning of the second path and the third path, and determining the splicing acceleration and the splicing time length according to the preset splicing algorithm, the acceleration and the planning time length;

splicing the second path and the third path according to the splicing acceleration and the splicing duration to obtain a spliced path, wherein the starting point of the planned path is the starting point of the second path, and the end point of the planned path is the end point of the third path;

and splicing the spliced path with the next adjacent first path by a preset splicing algorithm until the plurality of first paths are spliced together to obtain the initial path, wherein the starting point of the next adjacent first path is the same as the end point of the spliced path.

Optionally, determining a splicing acceleration and a splicing time length according to the preset splicing algorithm, the acceleration and the planning time length includes:

calculating the maximum value of the acceleration corresponding to the second path and the third path, and taking the maximum value of the acceleration as the splicing acceleration;

calculating a first speed corresponding to the second path according to the acceleration and the planning time corresponding to the second path, and calculating a second speed corresponding to the third path according to the acceleration and the planning time corresponding to the third path;

and calculating the absolute value of the difference between the first speed and the second speed, and obtaining the splicing time length according to the absolute value and the splicing acceleration.

Optionally, the second path and the third path are paths planned by a spatial joint path planning method, a cartesian path planning method, or a curved path planning method.

Optionally, if the second path and the third path are paths obtained by planning in a space joint path planning method, splicing the second path and the third path according to the splicing acceleration and the splicing duration to obtain a spliced path, including:

obtaining the planned path by the following formula:

wherein θ (t) represents the planned path; theta₀Indicating an initial state of the second path;representing the acceleration of the second path; t is t_c1Representing a duration of an acceleration segment of the second path; t is t_c2Representing the duration of the uniform speed segment of the second path; t is t_c3Representing a duration of a deceleration segment of the second path; t is t_f1Denotes the duration of the second path, t_f1＝t_c1+t_c2+t_c3；Representing the splice acceleration; t is t_bRepresenting the splicing time;represents the acceleration of the third path; t is t_c4Represents a duration of an acceleration segment of the third path; t is t_c5Represents the thirdThe duration of the uniform speed segment of the path; t is t_c6Represents a duration of a deceleration segment of the third path; t is t_f2Indicates the duration of the third path, t_f2＝t_c4+t_c5+t_c6；θ_f2A target state representing a third path; t is t_ΔAnd representing the difference between the total time length of the second path and the third path and the total time length of the planned path.

Optionally, verifying whether the initial path satisfies collision detection comprises:

constructing an obstacle model and an outer envelope model of the mechanical arm moving along the initial path, and judging whether the outer envelope model and the obstacle model meet preset conditions or not;

if so, determining that the initial path meets the collision detection; otherwise, determining that the initial path does not satisfy collision detection.

Optionally, the preset conditions include:

there is no intersection between the outer envelope space and the obstacle model; and/or

The shortest distance between the outer envelope space and the obstacle model is greater than a preset threshold.

Optionally, adjusting the initial path to obtain an adjusted path includes:

determining at least one point with intersection or shortest distance smaller than a preset threshold between the outer envelope space and the obstacle model, and determining a first path to which each point belongs and first paths to which the front section and the rear section of the first path belong on the initial path;

and respectively connecting the starting point of the first path with the terminal of the first path to which the previous section belongs, and connecting the end point of the first path with the starting point of the first path to which the next section belongs to obtain the adjusted path.

In a second aspect, the present application provides a computer device comprising:

a memory for storing instructions for execution by at least one processor;

a processor for executing instructions stored in a memory to perform the method of the first aspect.

In a third aspect, the present application provides a computer readable storage medium having stored thereon computer instructions which, when run on a computer, cause the computer to perform the method of the first aspect.

Drawings

Fig. 1 is a schematic flowchart of a method for planning a path of a robot arm according to an embodiment of the present disclosure;

fig. 2A is a schematic diagram of a joint path obtained by trapezoidal velocity interpolation according to an embodiment of the present application;

fig. 2B is a schematic diagram of a joint path obtained by another trapezoidal velocity interpolation provided in the embodiment of the present application;

FIG. 3A is a velocity diagram of a joint path according to an embodiment of the present disclosure;

FIG. 3B is a velocity diagram of yet another joint path provided by an embodiment of the present application;

FIG. 4A is a schematic view of an acceleration of a joint path according to an embodiment of the present disclosure;

FIG. 4B is a schematic acceleration diagram of another joint path provided by an embodiment of the present application;

fig. 5 is a schematic flowchart of joint space planning of a robot arm according to an embodiment of the present disclosure;

fig. 6 is a schematic diagram of a linear path planning motion of a robot provided in an embodiment of the present disclosure;

fig. 7 is a schematic flow chart of cartesian space linear path planning of a robot according to an embodiment of the present disclosure;

FIG. 8 is a diagram illustrating a minimum norm solution provided by an embodiment of the present application;

fig. 9A is a schematic diagram illustrating a planned path obtained by planning a first trajectory and a second trajectory according to a preset stitching algorithm according to an embodiment of the present application;

FIG. 9B is a velocity diagram of a planned path according to an embodiment of the present disclosure;

FIG. 9C is a schematic diagram illustrating acceleration of a planned path according to an embodiment of the present disclosure;

FIG. 10A is a diagram illustrating a spliced path directly connecting a first trace and a second trace in order according to an embodiment of the present disclosure;

fig. 10B is a schematic diagram of a spliced path obtained by sequentially connecting a first track and a second track by a preset algorithm according to an embodiment of the present application;

FIG. 10C shows a schematic diagram of a comparison of two splice paths provided by embodiments of the present application;

FIG. 10D shows a schematic diagram of a comparison of two splice paths provided by embodiments of the present application;

fig. 11 is a schematic structural diagram of a computer device according to an embodiment of the present application.

Detailed Description

In the solutions provided in the embodiments of the present application, the described embodiments are only a part of the embodiments of the present application, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.

In order to better understand the technical solutions, the technical solutions of the present application are described in detail below with reference to the drawings and specific embodiments, and it should be understood that the specific features in the embodiments and examples of the present application are detailed descriptions of the technical solutions of the present application, and are not limitations of the technical solutions of the present application, and the technical features in the embodiments and examples of the present application may be combined with each other without conflict.

The method for planning a path of a robot arm provided in the embodiments of the present application is described in further detail below with reference to the drawings in the specification, and a specific implementation manner of the method may include the following steps (a flow of the method is shown in fig. 1):

step 101, determining a starting point parameter and a target point parameter of the mechanical arm, and basing on A according to the starting point parameter and the target point parameter^＊Algorithm slave to preAnd searching a plurality of first paths of the mechanical arm from the starting point to the target point in the track library, wherein the first paths refer to paths between any two adjacent nodes in the process of moving from the starting point to the target point.

Specifically, in the solution provided in the embodiment of the present application, when planning a path of a mechanical arm, if an input planning task is a complex task, where the complex task is a task that the mechanical arm cannot directly execute, the complex task is described as: the node comprises a starting node, an intermediate node 1, an intermediate node 2, an intermediate node 3, …, an intermediate node n and a target node, wherein n is a positive integer not less than 1.

Further, when planning according to the complex task, a path between any two points needs to be planned, that is, the planned path includes n +1 paths, which are respectively: path 1 is from the start node to intermediate node 1; path 2 is intermediate node 1 to intermediate node 2; path 3 is intermediate node 2 to intermediate node 3; …, respectively; path n is intermediate node n-1 to intermediate node n; path n +1 is the intermediate node n to the target node.

Specifically, in the solution provided in the embodiment of the present application, the path planning refers to planning of basic motion of the mechanical arm, so that a joint or a tail end of the mechanical arm moves along a predetermined path, and the problems of collision with the surrounding environment and collision with the mechanical arm do not occur during the motion process. The method mainly comprises joint space path planning (planning mechanical arm joint motion), Cartesian space path planning (planning mechanical arm tail end motion, obtaining joint motion by inverse solution) and the like, wherein a path end point is realized by using the joint space path planning according to a task appointed by a mechanical arm joint angle; the path end point is realized by using Cartesian space linear path planning according to the task appointed by the terminal pose; and small-range fine operation tasks such as tool operation, end adapter butt joint and the like are realized by using Cartesian space linear path planning, and then a path 1, a path 2 and a path 3 …, namely a path n +1, are obtained by the path planning mode. For ease of understanding, the above path planning modes are briefly described below.

First, joint space planning

The joint space trajectory planning method describes the path of the space manipulator by using a function of joint angles to plan the path. The joint space planning does not need to describe the path shape between two path points in a rectangular coordinate system, and the calculation is simple and easy. The joint space path planning implementation method comprises the steps of giving a starting point position form and a stopping point position form of the space mechanical arm, and interpolating a joint to obtain a joint path.

Specifically, trajectory planning is performed in a joint space, and a starting point position and a terminating point position of the space manipulator need to be given, and interpolation is performed on the joint to obtain a joint path. Common interpolation methods include trapezoidal velocity interpolation, polynomial interpolation, and the like. The trapezoidal speed interpolation function divides the joint motion into three stages of acceleration, constant speed and deceleration for description, and the method has a more intuitive principle and is more widely applied; the polynomial interpolation function can determine the running track of the mechanical arm joint according to the constraint condition of the mechanical arm joint.

For example, if the joint angle θ of the starting node of the planned path₀Planning the joint angle theta of the target node of the path_fAnd the time t corresponding to the track_fThe following trapezoidal velocity interpolation can be used:

wherein θ (t) represents a planned path;representing the acceleration of the acceleration section and the deceleration section of the path; t is t_c1Represents a duration of an acceleration segment of the path; t is t_c2Representing the duration of a uniform speed segment of the path; t is t_fIndicating the duration of the path.

Fig. 2A is a schematic diagram of a joint path obtained by trapezoidal velocity interpolation according to an embodiment of the present application; fig. 2B is a schematic diagram of a joint path obtained by another trapezoidal velocity interpolation provided in the embodiment of the present application; FIG. 3A is a velocity diagram of a joint path according to an embodiment of the present disclosure; FIG. 3B is a velocity diagram of yet another joint path provided by an embodiment of the present application; FIG. 4A is a schematic view of an acceleration diagram of a joint path according to an embodiment of the present disclosure; FIG. 4B is a schematic view of an acceleration diagram of another joint path provided by an embodiment of the present application; wherein, fig. 3A and 4A are velocity and acceleration corresponding to the joint path shown in fig. 2A; fig. 3B and 4B illustrate the velocity and acceleration associated with the joint path shown in fig. 2B.

Referring to fig. 5, a schematic flowchart of joint space planning for a robot arm according to an embodiment of the present disclosure is provided. Specifically, the spatial joint planning comprises the following specific steps:

1. firstly, obtaining an initial joint angle q of the mechanical arm according to the current configuration of the mechanical arm_ini＝[θ₁ θ₂ … θ_n]_iniAnd determining the desired joint angle q according to task requirements_des＝[θ₁ θ₂ … θ_n]_des；

2. Calculate the total motion angle dist | | | q_des-q_intSelecting a speed trapezoidal planning mode, and calculating the total track planning time t according to the limit requirements on the maximum speed and the maximum acceleration of the joint in the task_fAnd an acceleration period time t_c1At the same time, setting the time step t₀Calculating the total planning step number n as t_f/t₀；

3. Calculating the step length of each interpolation step: k (k is 0 to n) represents the number of interpolations, and t represents the number of interpolations_k＝k·t₀Indicates the time of the kth interpolation, then t_kAngular displacement theta of time_kThe angular displacement function which can be expressed according to the trapezoidal speed interpolation value is calculated and given;

4. judging whether the current planning step number reaches the total number, if so, finishing the planning; otherwise, returning to the step3 to carry out solving circularly until the planning task is completed.

Two, Cartesian space path planning

The Cartesian space path planning refers to a process of realizing movement of a mechanical arm from an initial node to a target node by planning movement of the tail end of the mechanical arm on the basis of giving an initial configuration and a target tail end pose of the mechanical arm. The complete path planning of the Cartesian space does not complete the whole path planning work, and the final path planning of the joint space is converted. The Cartesian space path planning implementation method comprises the following steps: firstly, describing an expected path in a Cartesian space by a mathematical method, calculating the pose information of each interpolation point at the tail end, then solving through inverse kinematics to obtain corresponding parameters of a joint, establishing a corresponding motion equation through path planning of the joint space according to coordinates of the points, and then carrying out interpolation to obtain a target track.

Specifically, assume pose X of the starting node of the planned path_e0＝[P_e0，ψ_e0]And position and posture X of planning path target node_ef＝[P_ef，ψ_ef]. During path planning, an end effector requiring a robotic arm starts from a starting node X_e0To the end point X_efThe motion track of the linear motion is shown in fig. 6.

Referring to fig. 7, a schematic flow chart of a linear path provided in the embodiment of the present application is shown. Specifically, the straight-line path planning step is as follows:

1. knowing the initial pose X of the end of the robot arm with respect to the inertial frame_e0And expected pose X_efThereby obtaining a position vector P of the start point and the end point_e0And P_efAnd attitude matrix R_e0And R_ef；

2. From P_e0And P_efCalculating the movement displacement d_f＝||P_e0-P_ef| l, using Obtaining the direction cosine of a straight line pointing to the end point from the starting point;

3. calculating the attitude change matrix Rot of the end point relative to the starting point, and inversely calculating the change alpha of the corresponding ZYX Euler angle by the attitude change matrix Rot_n、β_nAnd gamma_nNamely the variation of the Euler angle of the attitude of the tail end in the motion process;

4. calculating the total time t of the trajectory planning according to the limit requirement on the terminal speed in the task_fAnd an acceleration period time t_sAt the same time, setting the time step t₀Calculating the total planning step number n as t_f/t₀；

5. Calculating the step length of each interpolation step: k (k is 0 to n) represents the number of interpolations, and t represents the number of interpolations_k＝k·t₀Indicates the time of the kth interpolation, then t_kDisplacement of time s_kCan be calculated and given according to the displacement function in the speed trapezoidal method, and can obtain the interpolation step length deltas_k；

6. Determining the position differential d of each step_x,d_y,d_z；

7. Solving the attitude matrix of the k step;

8. by dR_k＝R_k+1-R_kObtaining the attitude differential operator of the k stepRe-operatorObtaining the attitude differential variable delta_x,δ_y,δ_z；

9.D＝[d_x,d_y,d_z,δ_x,δ_y,δ_z]^TI.e. the differential motion vector in each interpolation step, is calculated by Obtaining the motion velocity vector of the object in the interpolation step;

10. on the basis of finishing the planning of the speed and the angular speed of the kth step at the tail end, solving based on an inverse kinematics equation of a speed level to obtain the rotation angular speed of the kth step of each joint;

11. after the rotation angular velocity of each joint is obtained, the rotation angular acceleration and the rotation angular displacement of each joint in the kth step are respectively obtained through derivation and integral operation of the rotation angular velocity of each joint, and the track planning of the kth step is completed;

12. judging whether the current planning step number reaches the total number, if so, finishing the planning; and if not, returning to the step 5 for solving circularly until the planning task is finished.

Further, in order to facilitate understanding of the process of solving the inverse kinematics equation based on the velocity class according to the above step 10 to obtain the rotation angular velocity of the kth step of each joint, the inverse kinematics process in the mechanical arm is briefly described below.

Specifically, inverse kinematics of a mechanical arm refers to a process of mapping operation of an end effector of the mechanical arm to a joint, and aims to convert the terminal pose of the mechanical arm in work into a corresponding joint variable of the mechanical arm. The pose control of the mechanical arm at the tail end operation can be realized through the inverse kinematics solution, and the pose control method has important application in motion analysis, off-line programming and track control of the mechanical arm.

For the mechanical arm, a kinematic inverse solution model is established according to the mapping relation between the terminal velocity and the joint angular velocity. The following analysis was performed for the inverse kinematics of the robotic arm in the fully controlled mode, in the following specific procedure:

from the kinematic equation, the motion of the end of the mechanical arm can be described by the motion of the base of the mechanical arm and the motion of the joint, namely:

the mapping relation of the tail end of the mechanical arm, the base and the joint angle is described by adopting a matrix transformation mode as follows:

for the seven-degree-of-freedom large mechanical arm system, due to the fact that the spacecraft base and the robot body are in motion coupling, when the robot moves, the pose of the spacecraft base can move along with the motion of the seven-degree-of-freedom large mechanical arm. According to whether the base of the seven-degree-of-freedom large mechanical arm is controlled or not, the control mode of the seven-degree-of-freedom large mechanical arm can be divided into three working modes, namely fixed base, free floating (base is not controlled) and free flying (base is controlled). For the seven-degree-of-freedom large-scale mechanical arm with different working modes, the constraint relations are the same, namely for the seven-degree-of-freedom large-scale mechanical arm, mapping relations of the tail end speed, the joint angle and the base speed exist.

In general, the position and the posture are controlled, namely v₀、ω₀All are 0, the differential equation of motion in this mode can be expressed as:

wherein J_mIs a transformation matrix of the joint motion mapped to the terminal cartesian motion.Andthe motion of the Cartesian and the joint is respectively expressed and is described by using the first derivative of the pose and the angle. For a large-scale mechanical arm with seven degrees of freedom,indicating tip speed Indicating joint velocityThe following can be established:

in the formula J_mThe method is a common Jacobian matrix of the ground mechanical arm, and the inverse kinematics of the speed level of the seven-degree-of-freedom large mechanical arm in the fixed base mode can be obtained according to the formula:

for the redundant manipulator, the above formula is actually a special solution, also called a minimum two-norm solution, and the vector formed by the angular velocities of the joints, which is solved by the solution, has the minimum norm and represents the condition of minimum overall motion amount. And when other performance indexes need to be optimized, performing inverse kinematics solution by adopting a gradient projection method.

The minimum two-norm method needs to calculate the generalized inverse of the Jacobian matrix, the following method can avoid the calculation of the generalized inverse, and the real-time performance of calculation can be greatly improved.

The solution of the above formula is composed of a special solution and a homogeneous solution, and can be expressed as

Wherein r is an arbitrary constant,the solution is any special solution, and the solution is,is a homogeneous solution. Satisfies the following conditions:

from the above equation, the square of the norm of the angular velocity of the joint can be obtained as

To seekThe right side of the above formula can be made to have a bias to r of 0, i.e.

According to the above formula, the constant r corresponding to the minimum norm solution can be solved as

Substituting the above formula into the minimum two norm solution

Order and vectorThe vector in the same direction is u_kI.e. by

Then the process of the first step is carried out,

according to the geometric meaning of dot-product, u_kRepresenting a vectorProjected onto a vectorLength of (c), therefore, the above formula indicates that the minimum norm solution is uniqueSolution (II)Minus its projection ontoThe resulting vectors for the part in the vector direction. Referring to fig. 8, a schematic diagram of a minimum norm solution provided in the embodiment of the present application is shown.

Further, in the solution provided in the embodiment of the present application, after the planning of the path between any two nodes is completed, the path may be stored in a preset path library, and when the path of the mechanical arm is planned, multiple segments of paths from the start node to the target node may be selected from the preset path library, and the multiple segments of paths are sequentially connected to obtain the path from the start node to the target node.

By way of example, can be represented by A^＊The algorithm selects a multi-segment path from the starting node to the target node from a preset track library.

For ease of understanding, pair A below^*The algorithm principle is briefly introduced.

A^＊The algorithm is a typical heuristic search method, is widely applied to solving the optimal path, and has the core of designing the valuation function. Introducing a valuation function f (x) when selecting the next evaluation node of the current node

f(x)＝g(x)+h(x)

Where g (x) is the actual cost of the minimum cost path from the starting point to node x; h (x) is the estimated cost of the path from node x to the destination point.

To facilitate the utilization of A^＊The algorithm defines 7 joint angles of the space manipulator as a seven-dimensional array, and the initial joint angle isThe current joint angle isThe optimal joint angle in the ith step isThe joint angle search step length isThe joint angle of the successor node in the step i is(determined by the joint angle search step size) the desired joint angle is

Defining valuation functionsWherein q is_imIs the joint angle of the mth joint in the joint angles of the mechanical arm in the ith step, q_{des m}The joint angle of the mth joint among the joint angles is desired for the robot arm. Therefore, f in the joint angle of the successor node of the step i can be obtained_iAnd (q) taking the node with the minimum value as the optimal joint angle of the step, generating the joint angle of the subsequent node of the step (i + 1) according to the step length, and repeating the process to ensure that the optimal joint angle can be obtained in each step, so that the configuration space optimal path planning of the space manipulator from the initial joint angle to the target joint angle can be realized through the evaluation function.

For the purpose of analysis, 3 tables, i.e., an OPEN table, a CLOSE table, and a successor node table, are set. Starting point q_iniPut into an OPEN table and initialize f₀(q)＝h₀And (q), setting the CLOSE table as an empty table.

The following process is repeated until the target point is found. If the OPEN table is an empty table, the path planning of the space manipulator fails; otherwise, the following loop begins.

Step1 selects the node with the minimum f value which is not set in the OPEN table as the optimal node q_{best_i}And put it into the CLOSE table, and delete it from the OPEN table at the same time;

step2 judging q_{best_i}Whether it is the target point q_desIf, ifIf yes, the solution is successful, and the cycle is ended; otherwise, carrying out the next step;

step3 according to the set Step length l_stepValue, calculate successor node q_{suc_i}；

Step4 follows the following procedure for each successive node q_{suc_i}And (3) processing:

a. judging q_{suc_i}Whether the robot arm is in the free motion space of the space robot arm, if so, establishing a slave q_{suc_i}Return q_{best_i}The pointer of (2); otherwise, go to Step 3;

b. computing

c. Judging q_{suc_i}If the current position is in the OPEN table, turning to the step d; otherwise, turning to e;

d. defining an array q_{old_i}＝q_{suc_i}Q is prepared by_{old_i}Fill to q_{best_i}And then comparing the new and old track costs in the subsequent node table: if g (q)_{old_i})>g(q_{suc_i}) Update q_{old_i}The parent node of is q_{best_i}And update g (q)_{old_i})＝g(q_{suc_i}) Further, f (q) is corrected_{old_i}) Values, while reordering the OPEN table; otherwise, turning to g;

e. judging q_{suc_i}If yes, go to d; otherwise, turning to f;

f. q is to be_{suc_i}Put in OPEN table and add to q_{best_i}In the successor table of (1), go to g;

g. calculating f (q)_{suc_i}) And go to a.

And 102, sequentially splicing the first paths according to a preset splicing algorithm to obtain an initial path corresponding to the movement of the mechanical arm from a starting point to a target point, and verifying whether the initial path meets the collision detection.

In a possible implementation manner, sequentially and sequentially splicing the first path according to a preset splicing algorithm to obtain an initial path of the mechanical arm, the method includes: determining a starting point and an end point of each first path, and determining any adjacent second path and third path from the plurality of first paths according to the starting point and the end points, wherein the end point of the second path is the same as the starting point of the third path; respectively determining the acceleration and the planning time length corresponding to the planning of the second path and the third path, and determining the splicing acceleration and the splicing time length according to the preset splicing algorithm, the acceleration and the planning time length; splicing the second path and the third path according to the splicing acceleration and the splicing duration to obtain a spliced path, wherein the starting point of the planned path is the starting point of the second path, and the end point of the planned path is the end point of the third path; and splicing the spliced path with the next adjacent first path by a preset splicing algorithm until the plurality of first paths are spliced together to obtain the initial path, wherein the starting point of the next adjacent first path is the same as the end point of the spliced path.

Further, in a possible implementation manner, determining a splicing acceleration and a splicing time length according to the preset splicing algorithm, the acceleration and the planning time length includes: calculating the maximum value of the acceleration corresponding to the second path and the third path, and taking the maximum value of the acceleration as the splicing acceleration; calculating a first speed corresponding to the second path according to the acceleration and the planning time corresponding to the second path, and calculating a second speed corresponding to the third path according to the acceleration and the planning time corresponding to the third path; and calculating the absolute value of the difference between the first speed and the second speed, and obtaining the splicing time length according to the absolute value and the splicing acceleration.

Further, in a possible implementation manner, the second path and the third path are paths planned by a space joint path planning method, a cartesian path planning method, or a curved path planning method.

For the convenience of understanding, the process of stitching between adjacent paths is briefly described below by taking the path planned in the joint space as an example.

As an example, the preset splicing algorithm is adopted to plan the joint spaceWhen splicing is carried out, the splicing acceleration is usedStitching a first trajectory and a second trajectory, wherein the first trajectory and the second trajectory are both derived from joint space planning,respectively representing the acceleration corresponding to the acceleration section and the deceleration section of the first track and the acceleration corresponding to the acceleration section and the deceleration section of the second track. Specifically, the first track and the second track are respectively represented by the following formula:

wherein, theta₁(t) and θ₂(t) representing a first track and a second track, respectively; t is t_c1、t_c4Respectively showing the corresponding time lengths of the first track and the second track acceleration section; t is t_c2、t_c5Respectively showing the corresponding time lengths of the first track and the second track constant speed section; t is t_f1、t_f2Respectively showing the corresponding time length of the first track and the second track.

Further, the first track and the second track are spliced to obtain a planned path according to the following formula:

wherein θ (t) represents the planned path; theta₀Representing a second pathAn initial state;representing the acceleration of the second path; t is t_c1Representing a duration of an acceleration segment of the second path; t is t_c2Representing the duration of the uniform speed segment of the second path; t is t_c3Representing a duration of a deceleration segment of the second path; t is t_f1Denotes the duration of the second path, t_f1＝t_c1+t_c2+t_c3；Representing the splice acceleration; t is t_bRepresenting the splicing time;represents the acceleration of the third path; t is t_c4Represents a duration of an acceleration segment of the third path; t is t_c5Representing the duration of the uniform speed segment of the third path; t is t_c6Represents a duration of a deceleration segment of the third path; t is t_f2Indicates the duration of the third path, t_f2＝t_c4+t_c5+t_c6；θ_f2A target state representing a third path; t is t_ΔAnd representing the difference between the total time length of the second path and the third path and the total time length of the planned path.

In particular, t_bThe time of the splicing is indicated, indicating splice acceleration t_ΔThe time difference of the spliced track from the original track is shown. The original path and the splicing path reach the same end point, so that the splicing track is better than the original trackTrace, and t_ΔIn the time range of Fig. 9A, 9B, and 9C show the relevant information of the planned path obtained by planning the first trajectory and the second trajectory according to the preset stitching algorithm, where fig. 9A shows a schematic diagram of the planned path obtained by planning the first trajectory and the second trajectory according to the preset stitching algorithm provided in the embodiment of the present application; FIG. 9B is a velocity diagram of a planned path according to an embodiment of the present disclosure; fig. 9C is a schematic diagram illustrating an acceleration of a planned path according to an embodiment of the present application.

Further, after all the paths from the starting point to the target point are spliced together to obtain the initial path, it is also required to verify whether the initial path collides with a preset obstacle. In the solution provided in the embodiment of the present application, there are various ways to verify whether the initial path satisfies the collision detection, and one of the ways is taken as an example for description below.

In one possible implementation, verifying whether the initial path satisfies collision detection includes:

constructing an obstacle model and an outer envelope model of the mechanical arm moving along the initial path, and judging whether the outer envelope model and the obstacle model meet preset conditions or not;

if so, determining that the initial path meets the collision detection; otherwise, determining that the initial path does not satisfy collision detection.

Further, in a possible implementation manner, the preset condition includes: there is no intersection between the outer envelope space and the obstacle model; and/or the shortest distance between the outer envelope space and the obstacle model is greater than a preset threshold.

Specifically, define χ_obs(t) is the obstacle space at time t; chi shape_arm(θ (t)) is along the stitching trajectoryAnd (5) enveloping the outside of the mechanical arm at the moment t in the motion process. In the process that the mechanical arm moves along the splicing track, one of the following two equivalent conditions needs to be met:

(1) the mechanical arm does not collide with the obstacle space, i.e.

(2) Space of obstacle χ_obs(t) and the outer envelope space of the manipulator χ_arm(θ (t)) is greater than 0, namely dis (χ)_arm(θ(t))，χ_obs(t))>0。

And 103, if the initial path does not meet the collision detection, adjusting the initial path to obtain an adjusted path, and taking the adjusted path as the path of the mechanical arm, wherein the adjusted path meets the collision detection.

In one possible implementation manner, adjusting the initial path to obtain an adjusted path includes: determining at least one point with intersection or shortest distance smaller than a preset threshold between the outer envelope space and the obstacle model, and determining a first path to which each point belongs and first paths to which the front section and the rear section of the first path belong on the initial path; and respectively connecting the starting point of the first path with the terminal of the first path to which the previous section belongs, and connecting the end point of the first path with the starting point of the first path to which the next section belongs to obtain the adjusted path.

Further, if the initial path satisfies the collision detection, the process ends.

In order to facilitate understanding of technical effects of the solutions provided in the embodiments of the present application, the following description is made by way of example. Referring to FIGS. 10A and 10B, FIG. 10A shows a spliced path directly connecting a first trace and a second trace in order, where S₁₁Representing the distance of the mechanical arm moving along the first track; s₁₂Representing the distance of the mechanical arm moving along the second track; the distance (the area of the shaded portion in fig. 10A) by which the robot arm moves along the spliced path obtained by sequentially connecting the first trajectory and the second trajectory is S₁＝S₁₁+S₁₂。

FIG. 10B shows a spliced path obtained by sequentially connecting a first track and a second track according to a predetermined algorithm provided in an embodiment of the present application, wherein the distance (the area of the shaded portion in FIG. 10B) that the mechanical arm moves along the spliced path is S₂＝S₂₁+S₂₂+S₂₃。

Since the distance of the mechanical arm movement is the same before and after splicing, S1 is required to be S2. Since S11 is known to be S21, S12 is simply required to be satisfied as S22+ S23 (1). In this case, as shown in fig. 10C below, S12 is S23+ S14(2) in comparison with the area before and after splicing. Comparing the above equation (1) and equation (2), if the condition S22 is satisfied as S14, the robot arm movement distance before and after splicing is the same.

Further, as shown in FIG. 10D, S₂₂＝1/2*h(t_c1-t_b+t_c2)，S₁₄＝h*t_ΔThen there is t_ΔRepresenting the time difference, t, of the spliced trajectory from the original trajectory_Δ＝1/2*(t_c1-t_b+t_c2)。

When in useIs provided withThen t_ΔTaking the maximum value, i.e. t_Δ＝1/2(t_c1-t_b+t_c2)＝1/2(t_c1+t_c2)。

When in useIs provided withThen t_ΔTaking the minimum value, i.e. t_Δ＝1/2(t_c1-t_b+t_c2)＝1/2[t_c1-(t_c1+t_c2)+t_c2]＝0。

In summary, the original path and the splicing path are connectedThe obtained end points are the same, the obtained splicing track is superior to the original track, and t_ΔIn the time range of

In the scheme provided in the examples of this application, according to A^＊The algorithm searches a plurality of first paths of the mechanical arm from the starting point to the target point from a preset path library, sequentially splices the first paths according to a preset splicing algorithm to obtain an initial path corresponding to the mechanical arm from the starting point to the target point, verifies whether the initial path meets the collision detection, if not, adjusts the initial path to obtain an adjusted path meeting the collision detection, and provides a splicing mode for optimizing front and rear paths of an intermediate point so as to improve the motion efficiency of the mechanical arm.

Referring to fig. 11, the present application provides a computer device comprising:

a memory 1101 for storing instructions for execution by at least one processor;

a processor 1102 for executing instructions stored in memory to perform the method described in fig. 1.

A computer-readable storage medium having stored thereon computer instructions which, when executed on a computer, cause the computer to perform the method of fig. 1.

As will be appreciated by one skilled in the art, embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, optical storage, and the like) having computer-usable program code embodied therein.

The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.

These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.

These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.

It will be apparent to those skilled in the art that various changes and modifications may be made in the present application without departing from the spirit and scope of the application. Thus, if such modifications and variations of the present application fall within the scope of the claims of the present application and their equivalents, the present application is intended to include such modifications and variations as well.