A multi-degree-of-freedom boom motion planning method, device, equipment and storage medium

Abstract

The present application provides a multi-degree-of-freedom boom motion planning method, apparatus, equipment and storage medium, including: determining the current end working range of the boom based on the current movable joints of the boom and the status information of each joint; determining the number N of synchronous motion joints in this cycle based on whether the current end working range exceeds the preset working range; determining the flexibility coefficient corresponding to each joint based on the current status information of each joint and the position information of the current obstacle, and selecting N joints as synchronous motion joints in this cycle based on the determined flexibility coefficients; determining the target angles of the synchronous motion joints in this cycle, and controlling the boom to move according to the target angles of the synchronous motion joints in this cycle; when planning the boom motion, considering reducing the number of moving joints and improving the flexibility of the boom at the same time, resolving the contradiction between the two, and improving the comprehensive optimization effect of the boom motion planning.

Description

Technical Field

[0001] The present application relates to the field of intelligent planning, and in particular to a multi-degree-of-freedom boom motion planning method, device, equipment and storage medium. Background Art

[0002] Each joint in an existing electro-hydraulic multi-joint boom shares a hydraulic flow source. During boom motion planning, the more joints selected for movement, the greater the pressure on the hydraulic valve group to distribute flow. This can cause a sudden drop in flow supply due to pressure cutoff, increasing the risk of exceeding joint trajectory tracking errors. Based on these reasons, the fewer joints that are simultaneously engaged, the lower the risk of exceeding joint trajectory tracking errors. The lower the risk during operation, the better. Furthermore, existing multi-joint booms are typical redundant degrees of freedom booms. The more redundant degrees of freedom (i.e., the more joints that are simultaneously engaged), the more flexible the multi-joint boom is, making it more conducive to adjusting the boom's local posture in conditions such as obstacle avoidance and confined space operations. The current boom motion planning algorithm optimizes the number of engaged joints and motion flexibility independently, resulting in poor overall optimization results. Summary of the Invention

[0003] The present application provides a multi-degree-of-freedom boom motion planning method, device, equipment and storage medium. When planning the boom motion, it simultaneously considers reducing the number of motion joints and improving the flexibility of the boom, resolves the contradiction between the two, and improves the comprehensive optimization effect of the boom motion planning.

[0004] In a first aspect, the present application provides a multi-degree-of-freedom boom motion planning method, comprising:

[0005] Determine the current end working range of the boom according to the current movable joints of the boom and the current status information of each joint of the boom;

[0006] Determine the number N of synchronous motion joints in this cycle according to whether the current end working range exceeds the preset working range, where N is a positive integer greater than 1;

[0007] Determine the flexibility coefficients corresponding to the joints according to the current state information of the joints and the current position information of the obstacles, and select N joints as synchronous motion joints in this cycle based on the determined flexibility coefficients;

[0008] The target angle of the synchronous motion joint in this cycle is determined, and the arm is controlled to move according to the target angle of the synchronous motion joint in this cycle.

[0009] In one or more possible embodiments, based on the determined flexibility coefficients, selecting N joints as synchronous motion joints in this cycle includes:

[0010] Determining the priority coefficient corresponding to each joint based on the flexibility coefficient corresponding to each joint, the preset weight, and the normal distribution function corresponding to the current joint angle of each joint; wherein the normal distribution function is constructed according to a preset mean and a preset standard deviation;

[0011] According to each priority coefficient, N joints are selected as the synchronous motion joints of this cycle.

[0012] In one or more possible embodiments, determining the flexibility coefficient corresponding to each joint according to the current state information of each joint and the current position information of the obstacle includes:

[0013] Determine the current position information of the arm segments associated with each joint according to the current state information of each joint;

[0014] Determine the location information of the obstacle according to the sensor;

[0015] Determining the distance between each boom section and the obstacle based on the current position information of each boom section and the position information of the obstacle;

[0016] The flexibility coefficients of the joints associated with the respective arm sections are determined based on the comparison results of the distances corresponding to the respective arm sections with the preset limit distances.

[0017] In one or more possible embodiments, determining the number N of synchronous motion joints in this cycle according to whether the current end working range exceeds a preset working range includes:

[0018] Comparing the current working range of the end of the boom with the preset working range;

[0019] When it is determined that the working range of the current end of the boom does not exceed the preset working range, the number of synchronous motion joints in the previous cycle is reduced by a preset step length to obtain a first number; if the first number is not less than the preset fixed value, the first number is used as the number N of synchronous motion joints in the current cycle; if the first number is less than the preset fixed value, the first number is increased by a preset step length to obtain a second number as the number N of synchronous motion joints in the current cycle;

[0020] When it is determined that the working range of the current end of the boom exceeds the preset working range, the number of synchronous motion joints in the previous cycle is increased by a preset step length to obtain a third number, which is used as the number N of synchronous motion joints in this cycle.

[0021] In one or more possible embodiments, the method further includes:

[0022] When it is determined that the current period is the initial period, the flexibility coefficient of each joint is a preset flexibility coefficient.

[0023] In one or more possible embodiments, determining the current end working range of the boom according to the current movable joints of the boom and the current state information of each joint of the boom includes:

[0024] Determine a Jacobian matrix and a pseudo-inverse matrix of the Jacobian matrix based on the current movable joints of the boom and the state information of each joint of the boom, and determine a condition number K based on the Jacobian matrix and the pseudo-inverse matrix of the Jacobian matrix, wherein the condition number K represents the motion capability of the boom in each direction;

[0025] When it is determined that the condition number K is not less than a preset condition number, or when it is determined that an obstacle exists based on the obstacle information obtained in advance, the current end working range of the boom is determined based on the current movable joints of the boom and the current status information of each joint of the boom.

[0026] In one or more possible embodiments, determining the target angle of the synchronous motion joint in this cycle includes:

[0027] Update the Jacobian matrix and the pseudo-inverse matrix of the Jacobian matrix according to the synchronous motion joints of this cycle;

[0028] According to the updated Jacobian matrix and the pseudo-inverse matrix of the Jacobian matrix, the target angle of the synchronous motion joint in this cycle is determined in combination with the inverse kinematics rule.

[0029] In one or more possible embodiments, the method further includes:

[0030] When it is determined that the condition number K is less than the preset condition number and there is no obstacle, the target joint is selected according to the preset target selection rule and the target angle corresponding to the target joint is determined based on the inverse kinematics rule, and the arm is controlled to move according to the target angle corresponding to the target joint.

[0031] In a second aspect, the present application further provides a multi-degree-of-freedom boom motion planning device, the device comprising:

[0032] A working range determination module is used to determine the current end working range of the boom according to the current movable joints of the boom and the current status information of each joint of the boom;

[0033] A synchronous motion joint number determination module is used to determine the number N of synchronous motion joints in this cycle according to whether the current end working range exceeds the preset working range, where N is a positive integer greater than 1;

[0034] a synchronous motion joint determination module, configured to determine the flexibility coefficients corresponding to the joints according to the current state information of the joints and the current position information of the obstacles, and select N joints as synchronous motion joints in this cycle based on the determined flexibility coefficients;

[0035] The target angle determination module is used to determine the target angle of the synchronous motion joint in this cycle, and control the arm to move according to the target angle of the synchronous motion joint in this cycle.

[0036] In a third aspect, the present application provides a multi-degree-of-freedom boom motion planning device, the device comprising:

[0037] At least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to execute the multi-degree-of-freedom arm motion planning method as described in any one of the first aspects.

[0038] In a fourth aspect, the present application further provides a storage medium storing a computer program, wherein the computer program is used to enable a computer to execute the multi-degree-of-freedom boom motion planning method as described in any one of the first aspects.

[0039] According to a multi-degree-of-freedom boom motion planning method, device, equipment and storage medium provided in the present application, the number of synchronous motion joints in this cycle is first determined based on the current working range of the boom, and then the corresponding synchronous motion joints are selected based on the flexibility coefficient corresponding to each joint; at the same time, both the number of synchronous motion joints and the flexibility coefficient corresponding to each joint are taken into consideration, and the risk of exceeding the limit of joint trajectory tracking error and the flexibility of the boom during boom operation are taken into consideration at the same time, so as to ensure better comprehensiveness of the boom during movement. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] The drawings herein are incorporated into and constitute a part of the specification, illustrate embodiments consistent with the present application, and together with the specification are used to explain the principles of the present application, and do not constitute an improper limitation on the present application.

[0041] Figure 1 A flowchart of a multi-degree-of-freedom boom motion planning method provided according to an embodiment;

[0042] Figure 2 A flowchart of a method for selecting a synchronous motion joint according to an embodiment;

[0043] Figure 3 A flowchart of a method for determining a flexible coefficient according to an embodiment is provided;

[0044] Figure 4 A flowchart of a method for determining the number of synchronous motion joints provided according to an embodiment;

[0045] Figure 5A flowchart of a multi-degree-of-freedom boom motion planning method provided according to an embodiment;

[0046] Figure 6 Schematic diagram of a multi-degree-of-freedom boom motion planning device provided according to an embodiment;

[0047] Figure 7 A schematic diagram of a multi-degree-of-freedom boom motion planning device provided according to an embodiment;

[0048] Figure 8 A schematic diagram of a multi-degree-of-freedom boom motion planning storage medium provided according to an embodiment. DETAILED DESCRIPTION

[0049] To make the objectives, technical solutions, and advantages of the present invention more apparent, the present invention will be further described in detail below with reference to the accompanying drawings. It is apparent that the embodiments described are only some, not all, of the present invention. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the present invention without creative effort are intended to fall within the scope of protection of the present invention.

[0050] When planning joint motion, existing intelligent control systems for electro-hydraulic multi-joint booms share a common hydraulic flow source. Consequently, the greater the number of moving joints in the boom's motion plan, the greater the pressure on the hydraulic valve block to distribute flow. This increases the risk of excessive joint tracking errors due to a sudden drop in flow supply caused by pressure cuts. Therefore, the fewer joints simultaneously engaged, the better. Multi-joint booms are typical of redundant degrees of freedom (DOFs). The greater the number of redundant degrees of freedom (DOFs), the more flexibility a boom offers, facilitating local posture adjustments for situations like obstacle avoidance and confined space operations. Current redundant DOF inverse kinematics optimization methods fail to simultaneously address the conflicting requirements of reducing the number of moving joints and improving boom flexibility, failing to resolve these two requirements.

[0051] Based on the above problems, the present application provides a multi-degree-of-freedom arm motion planning method, device, equipment and storage medium. First, the number of synchronous motion joints in this cycle is determined according to the current working range of the arm, and then the corresponding synchronous motion joints are selected according to the flexibility coefficient corresponding to each joint; at the same time, both the number of synchronous motion joints and the flexibility coefficient corresponding to each joint are considered, and the risk of exceeding the limit of joint trajectory tracking error and the flexibility of the arm during the operation of the arm are taken into account at the same time, so as to ensure better comprehensiveness of the arm during the movement process.

[0052] Example 1

[0053] This application provides a multi-degree-of-freedom boom motion planning method, such as Figure 1Shown, including:

[0054] S101, determining a current end working range of the boom according to current movable joints of the boom and current status information of each joint of the boom;

[0055] In one or more possible embodiments, the multi-DOF boom may be a concrete pump truck boom, an aerial work vehicle boom, a high-rise jet fire truck boom, or a robot boom, etc., which are not listed here one by one. The multi-DOF boom may have multiple arm sections connected by joints, wherein in some embodiments, the movable joints may default to all joints in the multi-DOF boom; while in other embodiments, the multi-DOF boom may provide a manual joint locking function, that is, a user may manually lock certain specific joints in the multi-DOF boom through a control device such as a remote control or a touch screen, in which case the movable joints may be joints in the multi-DOF boom other than the manually locked joints.

[0056] In one or more possible embodiments, the state information of the current joints of the arm includes the joint angles and the positions of the joints; the Jacobian matrix J and the pseudo-inverse matrix J of the Jacobian matrix are determined according to the current movable joints of the arm and the state information of the current joints of the arm. + , and according to the formula k=||J||||J + ||Determine the condition number; the physical meaning of the above-mentioned condition number K is the uniformity of the transformation of the Jacobian matrix in all directions. The smaller the condition number K, the closer the movement ability of the arm in all directions, the more uniform the speed, and the better the overall flexibility of the arm; according to whether the above-mentioned condition number K is less than the preset condition number K*, or whether there is an obstacle is determined based on the pre-acquired obstacle information, determine whether to determine the current working range of the arm; the above-mentioned preset condition number K* is an empirical value determined according to the structure of the above-mentioned arm, and the value is generally greater than 1; when it is determined that the condition number K is not less than the preset condition number, or it is determined that there is an obstacle based on the pre-acquired obstacle information, the current end working range of the above-mentioned arm is determined based on the current movable joints of the above-mentioned arm and the status information of each current joint of the arm.

[0057] S102, determining the number N of synchronous motion joints in this cycle based on whether the current end working range exceeds the preset working range, where N is a positive integer greater than 1;

[0058] In one or more possible embodiments, the current end working range is compared with a preset working range; when it is determined that the current end working range of the boom does not exceed the preset working range, the number of synchronous motion joints in the previous cycle is reduced by a preset step size to obtain a first number; if the first number is not less than the preset fixed value, the first number is used as the number N of synchronous motion joints in the current cycle; if the first number is less than the preset fixed value, the first number is increased by a preset step size to obtain a second number, which is used as the number N of synchronous motion joints in the current cycle; when it is determined that the current end working range of the boom exceeds the preset working range, the number of synchronous motion joints in the previous cycle is increased by a preset step size to obtain a third number, which is used as the number N of synchronous motion joints in the current cycle. In some example application scenarios, the multi-degree-of-freedom boom may be a concrete pump truck boom. If the boom end needs to move within the same horizontal plane, N can be set to an integer greater than or equal to 3. In other example application scenarios, the multi-degree-of-freedom boom may be a robot boom. If the requirements for the motion trajectory of the boom end are lower, N can also be set to an integer greater than or equal to 2.

[0059] S103, determining the flexibility coefficients corresponding to the joints according to the current state information of the joints and the current position information of the obstacle, and selecting N joints as synchronous motion joints in this cycle based on the determined flexibility coefficients;

[0060] In one or more possible embodiments, the synchronous motion joints of this cycle may be N joints selected from the above-mentioned movable joints. In other possible embodiments, the synchronous motion joints of this cycle may also be N joints selected from all the joints included in the multi-degree-of-freedom arm. For example, in combination with some specific application scenarios, although some joints are manually locked and do not belong to movable joints, in motion planning, temporarily unlocking these manually locked joints may be able to achieve efficient obstacle avoidance or obtain better trajectory tracking effects. Therefore, based on the above application scenarios, in the process of determining the synchronous motion joints of this cycle, the manual locking state of the joints can be temporarily ignored, and N synchronous motion joints can be determined from all the joints of the multi-degree-of-freedom arm.

[0061] In one or more possible embodiments, based on the current position information of each joint and the current obstacle, the flexibility coefficient corresponding to each of the above joints is determined, and based on the flexibility coefficient corresponding to each joint, N movable joints are selected as a set of synchronous motion joints in this cycle; wherein the current obstacle position information is the information obtained by obtaining the original data from the laser radar sensor and then calculating it into the position in the arm coordinate system through posture conversion.

[0062] S104, determining a target angle of the synchronous motion joint in this cycle, and controlling the arm to move according to the target angle of the synchronous motion joint in this cycle.

[0063] In one or more possible embodiments, the target angle of the synchronous motion joint in this cycle is determined in the following manner: the target angle of the synchronous motion joint in this cycle is determined based on the synchronous motion joint in this cycle and the planned position of the arm end that needs to reach in this cycle; the position of the arm end that needs to reach in this cycle is determined based on the synchronous motion joint in this cycle and the inverse kinematics rules.

[0064] According to a multi-degree-of-freedom boom motion planning method provided in this application, the number of synchronous motion joints in this cycle is first determined based on the current working range of the boom, and then the corresponding synchronous motion joints are selected based on the flexibility coefficient corresponding to each joint; at the same time, both the number of synchronous motion joints and the flexibility coefficient corresponding to each joint are taken into consideration, and the risk of exceeding the limit of joint trajectory tracking error and the flexibility of the boom during boom operation are taken into account at the same time, so as to ensure better comprehensiveness of the boom during the movement process.

[0065] In one or more possible embodiments, based on the determined flexibility coefficients, N of the above joints are selected as the synchronous motion joints of this cycle, such as Figure 2 Shown, including:

[0066] S201, determining a priority coefficient corresponding to each of the joints based on the flexibility coefficient corresponding to each of the joints, a preset weight, and a normal distribution function corresponding to the current joint angle of each joint; wherein the normal distribution function is constructed according to a preset mean and a preset standard deviation;

[0067] S202: Select N of the above joints as synchronous motion joints in this cycle according to the priority coefficients.

[0068] In one or more possible embodiments, the normal distribution function f(x) is constructed based on a preset mean and a preset standard deviation, and the specific formula is as follows:

[0069]

[0070] Among them, the mean M and the standard deviation σ are both empirical values, i represents the number of each joint, M(i) and σ(i) can be obtained by statistically analyzing the distribution of the historical working joint angles of each joint, and x represents the current joint angle of each joint. For example, the boom of a pump truck includes 6 joints. In the direction from the vehicle body to the end of the boom, each joint is numbered 1 to 6, then i = 1, 2, ..., 6. When calculating the f(x) value corresponding to the first joint, the mean M(1) and standard deviation σ(1) of the first joint can be determined based on the empirical value. The current joint angle x of the first joint is substituted into the above formula to obtain the function value of the normal distribution function corresponding to the first joint. The normal distribution functions and function values ​​corresponding to the remaining joints can be obtained in a similar way, which will not be repeated here. At the same time, the motion planning method for the degree of freedom boom provided in this application is not limited to the boom of a pump truck, but can also be applied to various scenarios such as aerial work vehicles, fire trucks, robots, etc., which will not be listed one by one here.

[0071] Based on the flexibility coefficient ξ(i) corresponding to each joint, the preset weight W(i), and the normal distribution function f(x) corresponding to the current joint angle of each joint, the priority coefficient ψ(i) corresponding to each joint is determined using the following formula:

[0072] ψ(i)=W(i)*f(i)*ξ(i)

[0073] Among them, the preset weight W(i) is also determined by the set of movable joints and is an empirical value obtained based on experiments; f(i) is f(x) corresponding to the i-th joint; finally, according to each priority coefficient ψ(i), N joints are selected as the synchronous motion joints in this cycle.

[0074] In one or more possible embodiments, the flexibility coefficient corresponding to each joint is determined based on the current state information of each joint and the current position information of the obstacle, such as Figure 3 Shown, including:

[0075] S301, determining the current position information of the arm segments associated with each joint based on the current state information of each joint;

[0076] In one or more possible embodiments, the above-mentioned current state information of each joint is input into the forward kinematics model corresponding to the arm frame, and finally the current position information of each joint-related arm segment is obtained.

[0077] S302, determining the location information of the obstacle according to the sensor;

[0078] In one or more possible embodiments, the obstacle position information is obtained by obtaining raw data from a lidar sensor and then calculating the position in the boom coordinate system through posture conversion.

[0079] S303, determining the distance between each boom segment and the obstacle based on the current position information of each boom segment and the position information of the obstacle;

[0080] S304: Determine the flexibility coefficient of each joint associated with each arm segment based on a comparison result between the distance corresponding to each arm segment and a preset limit distance.

[0081] In one or more possible embodiments, the distance d(i) between each arm segment and the obstacle is calculated based on the current position information of each arm segment and the position information of the obstacle. The flexibility coefficient corresponding to each joint is calculated according to the formula ξ(i)=sgn(Ξ)*d(i), where d * (i) is the maximum safe distance between each arm segment and the obstacle, obtained by actual measurement; when the current cycle is determined to be the initial cycle, it is not necessary to determine the flexibility coefficient of each movable joint according to S301-S304, and the preset flexibility coefficient corresponding to each joint is directly used.

[0082] In one or more possible embodiments, when it is determined that the condition number K is not less than the preset condition number but there are no obstacles, the position information of the above-mentioned obstacles cannot be obtained, and the distance between the above-mentioned arm sections and the obstacles cannot be calculated. Therefore, when the above-mentioned condition number K is not less than the preset condition number but there are no obstacles, the corresponding flexibility coefficient of each joint is directly determined to be 1, and the priority coefficient corresponding to each joint is further calculated according to the above-mentioned formula ψ(i)=W(i)*f(i)*ξ(i).

[0083] In one or more possible embodiments, the number N of synchronous motion joints in this cycle is determined based on whether the current end working range exceeds the preset working range, such as Figure 4 Shown, including:

[0084] S401, comparing the current working range of the end of the boom with the preset working range;

[0085] S402: When it is determined that the working range of the current end of the boom does not exceed the preset working range, the number of synchronous motion joints in the previous cycle is reduced by a preset step length to obtain a first number; if the first number is not less than the preset fixed value, the first number is used as the number N of synchronous motion joints in the current cycle; if the first number is less than the preset fixed value, the first number is increased by a preset step length to obtain a second number as the number N of synchronous motion joints in the current cycle;

[0086] S403: When it is determined that the working range of the current end of the boom exceeds the preset working range, the number of synchronous motion joints in the previous cycle is increased by a preset step length to obtain a third number as the number N of synchronous motion joints in this cycle.

[0087] In one or more possible embodiments, comparing the current working range of the arm end with the preset working range means comparing the current working range of the arm end, which is composed of the horizontal motion range and the vertical motion range in which the current working range of the arm end can work, with the preset working range. When any one of the horizontal motion range and the vertical motion range in which the current working range of the arm end can work exceeds the preset working range, it is determined that the current working range of the arm end exceeds the above-mentioned preset working range; when it is determined that the current working range of the arm end does not exceed the above-mentioned preset working range, the number of synchronous motion joints in the previous cycle is subtracted by 1 to obtain a first number. If this cycle is the initial cycle, the number of synchronous motion joints preset in the initial cycle is subtracted by 1 to obtain the first number; if the first number If it is not less than the preset fixed value 3, the above-mentioned first number is used as the number N of synchronous motion joints in this cycle; if the above-mentioned first number is less than the above-mentioned preset fixed value 3, the first number is increased by the preset step length to obtain the second number, which is used as the number N of synchronous motion joints in this cycle; when it is determined that the current end working range of the arm exceeds the above-mentioned preset working range, the number of synchronous motion joints in the previous cycle is increased by 1 to obtain the third number, which is used as the number N of synchronous motion joints in this cycle; according to the multi-degree-of-freedom arm in this application, it is usually determined that the number of synchronous motion joints in the arm is greater than or equal to 3 to ensure that the end of the arm moves in the same horizontal plane. Therefore, in some examples of this application, the preset fixed value is 3, and the number of synchronous motion joints is determined on the basis of ensuring that the end of the arm moves in the same horizontal plane.

[0088] In one or more possible embodiments, the target angle of the synchronous motion joint in this cycle is determined, including: updating the above-mentioned Jacobian matrix and the pseudo-inverse matrix of the Jacobian matrix according to the synchronous motion joint in this cycle; determining the target angle of the synchronous motion joint in this cycle based on the updated Jacobian matrix and the pseudo-inverse matrix of the Jacobian matrix in combination with the inverse kinematics rule; wherein, the Jacobian matrix is ​​updated to determine the position that the end of the arm needs to reach in this cycle, and after determining the updated Jacobian matrix and the pseudo-inverse matrix of the Jacobian matrix, that is, after determining the position that the end of the arm needs to reach in this cycle, the target angle of the synchronous motion joint in this cycle is determined according to the current position of the end of the arm and one or more of the three methods of end-segment arm posture optimization, anti-joint optimization and anti-joint over-limit optimization. The above-mentioned end-segment arm posture optimization, anti-joint optimization and anti-joint over-limit optimization will not be repeated here.

[0089] In one or more possible embodiments, the above method also includes: when it is determined that the above condition number K is less than a preset condition number and there is no obstacle, selecting the target joint according to the preset target selection rule and determining the target angle corresponding to the above target joint based on the inverse kinematics rule, and controlling the above boom to move according to the target angle corresponding to the above target joint; selecting the target joint according to the preset target selection rule includes: selecting the target joint based on experience, for example, in the concrete pump truck boom, the 1st, 3rd, and 5th joints can be selected as target joints, or the 2nd, 4th, and 6th joints can be selected as target joints; or, the synchronous motion joints of the previous cycle can be used as target joints.

[0090] According to a multi-degree-of-freedom boom motion planning method provided in this application, Figure 5 Shown, including:

[0091] S501, obtaining the current movable joints of the boom, the status information of each joint of the boom, and obstacle information;

[0092] S502, calculating the Jacobian matrix and the pseudo-inverse matrix of the Jacobian matrix based on the current movable joints of the boom and the state information of each joint of the boom, and determining the condition number K;

[0093] S503, determining whether the condition number K is greater than or equal to a preset condition number or determining whether there is an obstacle based on the above obstacle information; if so, executing S504; otherwise, executing S5010;

[0094] S504, determining the current end working range of the boom based on the current movable joints of the boom and the current state information of each joint of the boom, and determining whether the current end working range exceeds the preset working range. If so, execute S505; otherwise, execute S506;

[0095] S505, increasing the number of synchronous motion joints in the previous cycle by 1 to obtain the number N of synchronous motion joints in this cycle, and executing step S507;

[0096] S506, subtract 1 from the number of synchronous motion joints in the previous cycle to obtain a first number, and determine whether the first number is less than 3. If so, add 1 to the first number to obtain a second number, and use the second number as the number N of synchronous motion joints in the current cycle; otherwise, use the first number as the number N of synchronous motion joints in the current cycle, and execute step S507;

[0097] S507, determining the flexibility coefficient corresponding to each joint according to the current state information of each joint and the current position information of the obstacle;

[0098] S508, determining a priority coefficient corresponding to each of the joints based on the flexibility coefficient corresponding to each of the joints, a preset weight, and a normal distribution function corresponding to the current joint angle of each joint, and selecting N of the joints as the synchronous motion joints for this cycle based on the priority coefficients;

[0099] S509, updating the Jacobian matrix and the pseudo-inverse matrix of the Jacobian matrix according to the synchronous motion joints of this cycle;

[0100] S5010: Determine the target angle of the synchronous motion joint in this cycle based on the inverse kinematics rule, and control the arm to move according to the target angle of the synchronous motion joint in this cycle.

[0101] According to a multi-degree-of-freedom boom motion planning method provided in this application, the number of synchronous motion joints in this cycle is first determined based on the current working range of the boom, and then the corresponding synchronous motion joints are selected based on the flexibility coefficient corresponding to each joint; at the same time, both the number of synchronous motion joints and the flexibility coefficient corresponding to each joint are taken into consideration, and the risk of exceeding the limit of joint trajectory tracking error and the flexibility of the boom during boom operation are taken into account at the same time, so as to ensure better comprehensiveness of the boom during the movement process.

[0102] Example 2

[0103] This application also provides a multi-degree-of-freedom arm motion planning device, such as Figure 6 As shown, the above device includes:

[0104] A working range determination module 601 is used to determine the current end working range of the boom according to the current movable joints of the boom and the current status information of each joint of the boom;

[0105] Synchronous motion joint number determination module 602, for determining the number N of synchronous motion joints in this cycle according to whether the current end working range exceeds the preset working range, where N is a positive integer greater than 1;

[0106] Synchronous motion joint determination module 603, for determining the flexibility coefficients corresponding to the above-mentioned joints according to the current state information of each joint and the current position information of the obstacle, and selecting N joints as synchronous motion joints in this cycle based on the determined flexibility coefficients;

[0107] The target angle determination module 604 is used to determine the target angle of the synchronous motion joint in this cycle, and control the arm to move according to the target angle of the synchronous motion joint in this cycle.

[0108] Example 3

[0109] The present application also provides a multi-degree-of-freedom arm motion planning device, comprising at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the above-mentioned multi-degree-of-freedom arm motion planning method.

[0110] like Figure 7 As shown, the device includes a processor 701 , a memory 702 , a communication interface 703 and a bus 704 . The processor 701 , the memory 702 and the communication interface 703 are interconnected via the bus 704 .

[0111] The processor 701 is configured to read and execute instructions in the memory 702 , so that at least one processor can execute the multi-degree-of-freedom boom motion planning method provided in the above embodiment.

[0112] The memory 702 is used to store various instructions and programs of the multi-degree-of-freedom boom motion planning method provided by the above embodiment.

[0113] The bus 704 may be a peripheral component interconnect (PCI) bus or an extended industry standard architecture (EISA) bus. The bus may be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 7 Only one thick line is used in the diagram, but this does not mean that there is only one bus or one type of bus.

[0114] The processor 701 may be a central processing unit (CPU), a network processor (NP), a graphics processing unit (GPU), or any combination of a CPU, NP, and GPU. It may also be a hardware chip. The hardware chip may be an application-specific integrated circuit (ASIC), a programmable logic device (PLD), or a combination thereof. The PLD may be a complex programmable logic device (CPLD), a field-programmable gate array (FPGA), a generic array logic (GAL), or any combination thereof.

[0115] Example 4

[0116] This application also provides a storage medium, such as Figure 8 As shown, the medium stores a computer program, and the computer program is used to enable a computer to execute any one of the methods in the above embodiments.

[0117] The memory may include readable media in the form of volatile memory, such as random access memory (RAM) 801 and / or cache memory 802 , and may further include read-only memory (ROM) 803 .

[0118] The memory may also include a program / utility 805 having a set (at least one) of program modules 804, such program modules 804 including but not limited to: an operating system, one or more application programs, other program modules, and program data, each of which or some combination may include an implementation of a network environment.

[0119] According to a multi-degree-of-freedom arm motion planning method, device, equipment and storage medium provided in the present application, the number of synchronous motion joints in this cycle is first determined according to the current working range of the arm, and then the corresponding synchronous motion joints are selected according to the flexibility coefficient corresponding to each movable joint; by taking into account both the number of synchronous motion joints and the flexibility coefficient corresponding to each movable joint, the risk of exceeding the limit of joint trajectory tracking error can be reduced and the flexibility of the arm can be ensured.

[0120] Those skilled in the art will appreciate that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

[0121] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the present application. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0122] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0123] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0124] Obviously, those skilled in the art may make various changes and modifications to this application without departing from the spirit and scope of this application. Thus, if these modifications and variations of this application fall within the scope of the claims of this application and their equivalents, this application is intended to include these modifications and variations.

Claims

1. A multi-degree-of-freedom boom motion planning method, characterized in that: include: Determine the current end working range of the boom according to the current movable joints of the boom and the current status information of each joint of the boom; Determine the number N of synchronous motion joints in this cycle according to whether the current end working range exceeds the preset working range, where N is a positive integer greater than 1; Determine the flexibility coefficients corresponding to the joints according to the current state information of the joints and the current position information of the obstacles, and select N joints as synchronous motion joints in this cycle based on the determined flexibility coefficients; The target angle of the synchronous motion joint in this cycle is determined, and the arm is controlled to move according to the target angle of the synchronous motion joint in this cycle.

2. The method according to claim 1, characterized in that Based on the determined flexibility coefficients, N joints are selected as synchronous motion joints in this cycle, including: Determining the priority coefficient corresponding to each joint based on the flexibility coefficient corresponding to each joint, the preset weight, and the normal distribution function corresponding to the current joint angle of each joint; wherein the normal distribution function is constructed according to a preset mean and a preset standard deviation; According to each priority coefficient, N joints are selected as the synchronous motion joints of this cycle.

3. The method according to claim 1, characterized in that Determine the flexibility coefficient corresponding to each joint based on the current state information of each joint and the current position information of the obstacle, including: Determine the current position information of the arm segments associated with each joint according to the current state information of each joint; Determine the location information of the obstacle according to the sensor; Determining the distance between each boom section and the obstacle based on the current position information of each boom section and the position information of the obstacle; The flexibility coefficients of the joints associated with the respective arm sections are determined based on the comparison results of the distances corresponding to the respective arm sections with the preset limit distances.

4. The method according to claim 1, wherein The number N of synchronous motion joints in this cycle is determined based on whether the current end working range exceeds the preset working range, including: Comparing the current working range of the end of the boom with the preset working range; When it is determined that the working range of the current end of the boom does not exceed the preset working range, the number of synchronous motion joints in the previous cycle is reduced by a preset step length to obtain a first number; if the first number is not less than a preset fixed value, the first number is used as the number N of synchronous motion joints in the current cycle; if the first number is less than the preset fixed value, the first number is increased by a preset step length to obtain a second number as the number N of synchronous motion joints in the current cycle; When it is determined that the working range of the current end of the boom exceeds the preset working range, the number of synchronous motion joints in the previous cycle is increased by a preset step length to obtain a third number, which is used as the number N of synchronous motion joints in this cycle.

5. The method according to claim 3, characterized in that The method further comprises: When it is determined that the current period is the initial period, the flexibility coefficient of each joint is a preset flexibility coefficient.

6. The method according to claim 1, characterized in that Determine the current end working range of the boom based on the current movable joints of the boom and the current status information of each joint of the boom, including: Determine a Jacobian matrix and a pseudo-inverse matrix of the Jacobian matrix based on the current movable joints of the boom and the state information of each joint of the boom, and determine a condition number K based on the Jacobian matrix and the pseudo-inverse matrix of the Jacobian matrix, wherein the condition number K represents the motion capability of the boom in each direction; When it is determined that the condition number K is not less than a preset condition number, or when it is determined that an obstacle exists based on the obstacle information obtained in advance, the current end working range of the boom is determined based on the current movable joints of the boom and the current status information of each joint of the boom.

7. The method according to claim 6, characterized in that Determine the target angle of the synchronous motion joint in this cycle, including: Update the Jacobian matrix and the pseudo-inverse matrix of the Jacobian matrix according to the synchronous motion joints of this cycle; According to the updated Jacobian matrix and the pseudo-inverse matrix of the Jacobian matrix, the target angle of the synchronous motion joint in this cycle is determined in combination with the inverse kinematics rule.

8. The method according to claim 6, characterized in that The method further comprises: When it is determined that the condition number K is less than the preset condition number and there is no obstacle, the target joint is selected according to the preset target selection rule and the target angle corresponding to the target joint is determined based on the inverse kinematics rule, and the arm is controlled to move according to the target angle corresponding to the target joint.

9. A multi-degree-of-freedom boom motion planning device, characterized in that: The device comprises: A working range determination module is used to determine the current end working range of the boom according to the current movable joints of the boom and the current status information of each joint of the boom; A synchronous motion joint number determination module is used to determine the number N of synchronous motion joints in this cycle according to whether the current end working range exceeds the preset working range, where N is a positive integer greater than 1; a synchronous motion joint determination module, configured to determine the flexibility coefficients corresponding to the joints according to the current state information of the joints and the current position information of the obstacles, and select N joints as synchronous motion joints in this cycle based on the determined flexibility coefficients; The target angle determination module is used to determine the target angle of the synchronous motion joint in this cycle, and control the arm to move according to the target angle of the synchronous motion joint in this cycle.

10. A multi-degree-of-freedom boom motion planning device, characterized in that: The device comprises: At least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to execute the multi-degree-of-freedom arm motion planning method as described in any one of claims 1 to 8.

11. A storage medium, characterized in that: The storage medium stores a computer program, and the computer program is used to enable a computer to execute the multi-degree-of-freedom boom motion planning method according to any one of claims 1 to 8.

Images