Multi-joint mechanism control method, electronic device, work machine, and storage medium

Abstract

The application discloses a multi-joint mechanism control method, an electronic device, a working machine and a computer readable storage medium. The method comprises the following steps: in response to detecting that a round-obstacle triggering condition is met during movement of a multi-joint mechanism end to a target position, determining a round-obstacle intermediate path point according to size data and position data of an obstacle and the target position; determining a first target posture of the multi-joint mechanism when the multi-joint mechanism end reaches the round-obstacle intermediate path point based on a preset posture constraint condition and an inverse kinematics iteration solving algorithm, wherein the first target posture comprises a first angle sequence composed of angles corresponding to each joint in the multi-joint mechanism. In the technical scheme of the application, the round-obstacle intermediate path point is determined according to the obstacle information and the target position, and the posture of the multi-joint mechanism when the multi-joint mechanism end reaches the round-obstacle intermediate path point is solved, so that the automatic obstacle avoidance purpose can be achieved, and the processing efficiency and reliability are high.

Description

Technical Field

[0001] This application relates to the field of construction machinery technology, and in particular to a control method for a multi-joint mechanism, electronic equipment, construction machinery, and computer-readable storage medium. Background Technology

[0002] For construction machinery with multi-joint booms, such as concrete pump trucks, concrete placing booms, and articulated boom cranes, the current methods mostly rely on boom extension and retraction devices to achieve boom extension and retraction through manual operation. However, due to the limitations of the construction scene and the deviation of human observation, the boom may scrape against objects outside the field of view. In other words, the existing control methods for multi-joint mechanisms have the problem of not being able to automatically avoid obstacles. Summary of the Invention

[0003] The purpose of this application is to provide a multi-joint mechanism control method, electronic device, operating machinery and computer-readable storage medium that can achieve automatic obstacle avoidance while having high processing efficiency and reliability.

[0004] To achieve the above objectives, the technical solution of this application is implemented as follows:

[0005] In a first aspect, this application provides a control method for a multi-joint mechanism, the method comprising:

[0006] In response to detecting that the obstacle avoidance triggering condition is met during the movement of the end of the multi-joint mechanism toward the target position, the intermediate path point for obstacle avoidance is determined based on the size data and position data of the obstacle and the target position.

[0007] Based on preset attitude constraints and inverse motion iterative solution algorithm, the first target attitude of the multi-joint mechanism is determined when the end of the multi-joint mechanism reaches the intermediate path point of the obstacle bypass. The first target attitude includes a first angle sequence composed of the angles corresponding to each joint in the multi-joint mechanism.

[0008] As one implementation, the preset posture constraints include an iteration termination condition and angle limitation conditions corresponding to each of the joints.

[0009] The determination of the first target posture of the multi-joint mechanism when its end reaches the obstacle-avoidance intermediate path point, based on preset posture constraints and an inverse motion iterative solution algorithm, includes:

[0010] Step S10: Based on the positional deviation between the expected position of the end of the multi-joint mechanism and the intermediate path point of the obstacle bypass calculated in the previous iteration, obtain the angle increment corresponding to each joint in the current iteration based on the inverse motion iterative solution algorithm;

[0011] Step S11: Determine the calculated angles of each joint in the current iteration based on the angle increments corresponding to each joint and the expected angles corresponding to each joint calculated in the previous iteration.

[0012] Step S12: Optimize the calculated angle based on the angle constraints to obtain the expected angles corresponding to each joint in the current iteration.

[0013] Step S13: Determine the expected position of the end of the multi-joint mechanism in the current iteration based on the expected angles corresponding to each joint in the current iteration.

[0014] Step S14: Check whether the iteration termination condition is met. If not, return to step 10. If yes, proceed to step S15.

[0015] Step S15: Based on the expected angles corresponding to each joint in the current iteration, determine the first target posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the obstacle bypass intermediate path point.

[0016] As one implementation, the angle limiting conditions include the angle range intervals and singular angle intervals corresponding to each joint;

[0017] The iteration termination conditions include the number of reverse motion iterations being greater than a preset number and / or the position deviation being less than a preset threshold.

[0018] In one implementation, the response to detecting that an obstacle avoidance triggering condition is met during the movement of the end of the multi-joint mechanism toward the target position, before determining the intermediate path point for obstacle avoidance based on the obstacle's size data, position data, and the target position, includes:

[0019] Determine the target position at the end of the multi-joint mechanism;

[0020] Based on the preset posture constraints and the inverse motion iterative solution algorithm, a second target posture of the multi-joint mechanism is obtained when the end of the multi-joint mechanism reaches the target position; the second target posture includes a second angle sequence composed of the angles corresponding to each joint.

[0021] Based on the initial posture of the multi-joint mechanism and the second target posture, a first movement path is planned for the end effector of the multi-joint mechanism from the initial position to the target position;

[0022] The multi-joint mechanism is controlled to move based on the first movement path, so that the end of the multi-joint mechanism moves toward the target position.

[0023] In one implementation, the response to detecting that an obstacle avoidance triggering condition is met during the movement of the end of the multi-joint mechanism toward the target position, before determining the intermediate path point for obstacle avoidance based on the obstacle's size data, position data, and the target position, includes:

[0024] Obtain the control mode for the multi-joint mechanism, the control mode including any one of the following: arm retraction mode and arm extension mode;

[0025] The target position of the end of the multi-joint mechanism is determined according to the control mode.

[0026] As one implementation, the method further includes:

[0027] Based on the current posture of the multi-joint mechanism, the first target posture, and the second target posture, and combined with the target cost function and multi-objective constraints, a second movement path is planned for the end effector of the multi-joint mechanism from its current position, passing through the obstacle bypass intermediate path point to the target position; the target cost function includes the shortest path cost function and the non-collision cost function.

[0028] As one implementation, the multi-objective constraint includes at least one of the following:

[0029] The path length is the shortest;

[0030] The path's minimum height is greater than or equal to the height threshold;

[0031] The furthest distance between the path and obstacles is less than or equal to the distance threshold.

[0032] In a second aspect, this application provides an electronic device including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the multi-joint mechanism control method as described in the first aspect.

[0033] Thirdly, this application provides a working machine, including a controller, a multi-joint mechanism, and a laser radar disposed on the multi-joint mechanism. The laser radar is used to collect size data and position data of obstacles during the process of the end of the multi-joint mechanism moving towards a target position. When the controller executes a computer program, it implements the multi-joint mechanism control method as described in the first aspect.

[0034] Fourthly, this application provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the multi-joint mechanism control method as described in the first aspect.

[0035] This application provides a control method, electronic device, operating machinery, and computer-readable storage medium for a multi-joint mechanism. The method includes: responding to the detection that an obstacle avoidance trigger condition is met during the movement of the end of the multi-joint mechanism toward a target position; determining an intermediate path point for obstacle avoidance based on the size and position data of the obstacle and the target position; and determining a first target posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the intermediate path point for obstacle avoidance based on preset posture constraints and an inverse motion iterative solution algorithm. The first target posture includes a first angle sequence composed of the angles corresponding to each joint in the multi-joint mechanism. The technical solution of this application firstly determines an intermediate path point for obstacle avoidance based on the acquired size and position data of the obstacle and the target position when the obstacle avoidance trigger condition is detected. Then, based on preset posture constraints and an inverse motion iterative solution algorithm, it determines the first target posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the intermediate path point for obstacle avoidance. By controlling the posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the intermediate path point for obstacle avoidance, automatic obstacle avoidance can be achieved, while maintaining high processing efficiency and reliability. Attached Figure Description

[0036] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0037] Figure 1 A flowchart illustrating a multi-joint mechanism control method provided in an embodiment of this application;

[0038] Figure 2 This is a schematic diagram illustrating the process of obtaining the second target posture of the multi-joint mechanism in an embodiment of this application;

[0039] Figure 3 This is a schematic diagram illustrating the specific process of a multi-joint mechanism control method provided in an embodiment of this application. Detailed Implementation

[0040] The specific embodiments of this application will now be described in detail with reference to the accompanying drawings. Obviously, the described embodiments are merely a part of the embodiments of this application, and not all of them. Based on the description of this application, all other embodiments obtained by those skilled in the art without inventive effort are within the scope of protection of this application.

[0041] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element. Furthermore, components, features, and elements with the same names in different embodiments of this application may have the same meaning or different meanings, the specific meaning of which must be determined by its interpretation in that specific embodiment or further in conjunction with the context of that specific embodiment.

[0042] It should be understood that although the terms first, second, third, etc., may be used herein to describe various information, this information should not be limited to these terms. These terms are used only to distinguish information of the same type from one another. For example, without departing from the scope of this document, first information may also be referred to as second information, and similarly, second information may also be referred to as first information. Depending on the context, the word “if” as used herein may be interpreted as “when…” or “in response to determination”. Furthermore, as used herein, the singular forms “a,” “an,” and “the” are intended to also include the plural forms unless the context indicates otherwise. It should be further understood that the terms “comprising,” “including,” indicate the presence of the stated feature, step, operation, element, component, item, kind, and / or group, but do not exclude the presence, occurrence, or addition of one or more other features, steps, operations, elements, components, items, kinds, and / or groups. The terms “or” and “and / or” as used herein are to be interpreted as inclusive, or mean any one or any combination thereof. Therefore, "A, B, or C" or "A, B, and / or C" means "any one of the following: A; B; C; A and B; A and C; B and C; A, B, and C". Exceptions to this definition will only occur if the combination of elements, functions, steps, or operations is inherently mutually exclusive in some way.

[0043] It should be understood that although the steps in the flowcharts of this application's embodiments are shown sequentially according to the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless explicitly stated herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least some of the steps in the figures may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times, and their execution order is not necessarily sequential, but can be performed alternately or in turn with other steps or at least a portion of the sub-steps or stages of other steps.

[0044] It should be noted that step designations such as S1 and S2 are used in this document for the purpose of more clearly and concisely describing the corresponding content, and do not constitute a substantial limitation on the order. In specific implementation, those skilled in the art may execute S2 first and then S1, etc., but these should all be within the protection scope of this application.

[0045] It should be understood that the specific embodiments described herein are merely illustrative of this application and are not intended to limit this application.

[0046] See Figure 1 This is a flowchart illustrating a multi-joint mechanism control method provided in an embodiment of this application. This multi-joint mechanism control method can be executed by a multi-joint mechanism control device provided in this embodiment. The multi-joint mechanism control device can be implemented using software and / or hardware, such as a processor, controller, or other electronic device. The multi-joint mechanism control method provided in this embodiment includes the following steps:

[0047] Step S1: In response to the detection that the obstacle avoidance triggering condition is met during the movement of the end of the multi-joint mechanism toward the target position, determine the obstacle avoidance intermediate path point based on the obstacle size data, position data and target position.

[0048] The multi-joint mechanism includes multiple structures and multiple joints, with adjacent structures connected by joints. For example, a multi-joint mechanism can be the boom of a concrete pump truck, a robotic arm, etc., where the boom includes a segmented boom structure. In this embodiment, the multi-joint mechanism can specifically be the boom of a concrete pump truck, and its posture is a set of postures of each joint of the boom. During the control of the multi-joint mechanism's end effector moving towards the target position, the surrounding environment of the multi-joint mechanism can be monitored in real time using detection devices such as lidar installed on the mechanism. When an obstacle is detected, the position, size, and other information of the detected obstacle are fed back to the electronic equipment in real time. In one embodiment, the obstacle avoidance trigger condition can be that an obstacle is detected and the distance between the obstacle boundary and the multi-joint mechanism is less than a preset safety threshold. The preset safety threshold can be set by combining the sensitivity of the lever and the length of the multi-joint mechanism. For example, the preset safety threshold can be the product of γ and δ, where γ is the minimum safety threshold determined by the length of the multi-joint mechanism, and δ is a coefficient determined by the sensitivity of the lever. The target position is the position that the end of the multi-joint mechanism needs to reach. It can be represented by three-dimensional coordinates. For example, the target position can be the position of the end of the multi-joint mechanism when it is in a folded state, or it can be any specified position.

[0049] Among these, given the size and location data of the obstacle and the target location, the formula (X) can be used. temp ,Y temp Z temp )=(X ob ,Y ob Z ob )+(X coe ,Y coe Z coe )×(X ob_M ,Y ob_M Z ob_M Calculate the coordinates of the intermediate path point around the obstacle. The subscript temp represents the intermediate path point around the obstacle, the subscript ob represents the obstacle position, the subscript ob_M represents the obstacle size, and the subscript coe represents the extension coefficient. The coordinates can be calculated using the formula (X... coe ,Y coe Z coe ) = D sec *(α, β, 0) are determined, and D sec The safety distance value is preset based on the length of the joint mechanism, and α and β are fixed values ​​set based on experience. The obstacle avoidance intermediate path point can be regarded as the intermediate path point planned for the end of the multi-joint mechanism, that is, the position point that the end of the multi-joint mechanism needs to pass through before moving to the target position.

[0050] Step S2: Based on the preset attitude constraints and the inverse motion iterative solution algorithm, determine the first target attitude of the multi-joint mechanism when the end of the multi-joint mechanism reaches the intermediate path point of the obstacle bypass. The first target attitude includes the first angle sequence composed of the angles corresponding to each joint in the multi-joint mechanism.

[0051] To enable the multi-joint mechanism to avoid collisions with obstacles, the first target posture of the multi-joint mechanism when its end reaches the intermediate path point around the obstacle can be determined based on preset posture constraints and an inverse motion iterative solution algorithm. The preset posture constraints describe the iterative solution limitations and the angle limitations of each joint.

[0052] The technical solution of this application firstly determines the intermediate path point of obstacle avoidance based on the size and position data of the obstacle and the target position when the obstacle avoidance trigger condition is detected. Then, based on the preset attitude constraints and the inverse motion iterative solution algorithm, the first target attitude of the multi-joint mechanism is determined when the end of the multi-joint mechanism reaches the intermediate path point of obstacle avoidance. By controlling the attitude of the multi-joint mechanism when the end of the multi-joint mechanism reaches the intermediate path point of obstacle avoidance, the purpose of automatic obstacle avoidance can be achieved, while the processing efficiency and reliability are high.

[0053] In one embodiment, the preset attitude constraints include an iteration termination condition and angle constraints corresponding to each joint; based on the preset attitude constraints and the inverse motion iterative solution algorithm, the first target attitude of the multi-joint mechanism is determined when the end of the multi-joint mechanism reaches the intermediate path point around the obstacle, including:

[0054] Step S10: Based on the position deviation between the expected position of the end of the multi-joint mechanism and the intermediate path point around the obstacle calculated in the previous iteration, obtain the angle increment corresponding to each joint in the current iteration based on the inverse motion iterative solution algorithm.

[0055] Step S11: Determine the calculated angles for each joint in the current iteration based on the angle increments corresponding to each joint and the expected angles corresponding to each joint calculated in the previous iteration.

[0056] Step S12: Optimize the calculated angle based on the angle constraints to obtain the expected angles for each joint in the current iteration.

[0057] Step S13: Determine the expected position of the end of the multi-joint mechanism in the current iteration based on the expected angles corresponding to each joint in the current iteration.

[0058] Step S14: Check if the iteration termination condition is met. If not, return to step 10. If yes, proceed to step S15.

[0059] Step S15: Based on the expected angles corresponding to each joint during the current iteration, determine the first target posture of the multi-joint mechanism when it reaches the intermediate path point around the obstacle at the end of the multi-joint mechanism.

[0060] In the first iteration, the expected position of the end effector of the multi-joint mechanism calculated in the previous iteration is taken as the current position of the end effector, i.e., the position of the end effector when the obstacle avoidance trigger condition is met. Simultaneously, in the first iteration, the expected angles corresponding to each joint calculated in the previous iteration are taken as the current angles corresponding to each joint, i.e., the angles corresponding to each joint when the obstacle avoidance trigger condition is met.

[0061] The method of obtaining the angle increments of each joint during the current iteration based on the inverse motion iterative solution algorithm can include calculating the Jacobian matrix, performing SVD decomposition and pseudo-inverse calculation on the Jacobian matrix to obtain the pseudo-inverse matrix, and obtaining the angle increments of each joint during the current iteration based on the pseudo-inverse matrix. The specific implementation principle of the inverse motion iterative solution algorithm can be found in existing technologies and will not be elaborated here. Thus, by using an inverse motion iterative solution algorithm with attitude constraints, the target attitude of the multi-joint mechanism when its end effector reaches the intermediate path point around the obstacle is solved, improving the solution speed and convergence speed. At the same time, it ensures the feasibility of the results, making the results more closely aligned with actual needs. This improves upon the problems of current one-click extension and retraction technologies, such as the inability to guarantee the final attitude solution result or the use of preset methods, leading to poor system environmental adaptability.

[0062] The angle constraints may include the angle range intervals and singular angle intervals corresponding to each joint; the iteration termination conditions include the number of inverse motion iterations being greater than a preset number and / or the position deviation being less than a preset threshold. The angle range interval indicates the range of possible angle values ​​for a joint, including an upper and lower limit; the angle range interval may differ for each joint. The singular angle interval indicates the optimized angle set for the corresponding joint for a preset tendency posture that is difficult or impossible to plan. For example, when the preset tendency posture is an arched posture, the angles of several specified joints in a multi-joint mechanism need to be optimized based on the singular angle interval. Here, the angle ang is calculated based on the angle constraints. i Optimize to obtain the expected angle Ang for each joint during the current iteration. new , can be represented as:

[0063]

[0064] Among them, ang i(max) ang i(min)Let ang represent the upper and lower limits of the angle of the i-th joint, respectively. i(setting) This represents the singular angle range of the i-th joint. The singular angle range is generally preset according to the specific equipment.

[0065] In one embodiment, in response to detecting that an obstacle avoidance triggering condition is met during the movement of the end of the multi-joint mechanism toward the target position, before determining the obstacle avoidance intermediate path point based on the obstacle's size data and position data and the target position, the method includes:

[0066] Determine the target position of the end effector of the multi-joint mechanism;

[0067] Based on preset attitude constraints and inverse motion iterative solution algorithm, the second target attitude of the multi-joint mechanism is obtained when the end of the multi-joint mechanism reaches the target position; the second target attitude includes a second angle sequence composed of the angles corresponding to each joint.

[0068] Based on the initial posture and the second target posture of the multi-joint mechanism, plan the first movement path of the end effector of the multi-joint mechanism from the initial position to the target position;

[0069] The multi-joint mechanism is moved based on the first moving path, so that the end of the multi-joint mechanism moves toward the target position.

[0070] The target position of the end effector of the multi-joint mechanism can be manually input by the user or automatically determined based on the control mode of the multi-joint mechanism. After determining the target position of the end effector, a second target posture of the multi-joint mechanism can be obtained based on preset posture constraints and an inverse motion iterative solution algorithm when the end effector reaches the target position. The specific process can refer to the aforementioned process for obtaining the first target posture. The specific process of planning the first movement path of the end effector from the initial position to the target position based on the initial posture and the second target posture of the multi-joint mechanism can refer to existing technologies and will not be repeated here. Here, the first movement path includes the angle change of each joint in each control cycle, that is, the angle information that each joint needs to change in each control cycle can be obtained based on the first movement path. The initial position of the end effector of the multi-joint mechanism refers to the position corresponding to the multi-joint mechanism before it starts moving. After determining the first movement path of the end effector from the initial position to the target position, the multi-joint mechanism can be controlled to move based on the first movement path, thereby moving the end effector of the multi-joint mechanism towards the target position. It should be noted that when controlling the multi-joint mechanism to move along the first moving path, if no obstacle is detected during the process, it will move until the end of the multi-joint mechanism reaches the target position. At this time, the posture of the multi-joint mechanism is the first target posture.

[0071] For example, the process of obtaining the second target pose of a multi-joint mechanism is described in detail below, see reference. Figure 2 This includes the following steps:

[0072] Step S101: Initialize boom parameters, initial angles of each joint, and Iter max d target .

[0073] Where, d target Iter represents the preset position error threshold. max This represents the maximum number of iterations for the inverse motion.

[0074] Step S102: Obtain the initial position of the end.

[0075] Step S103: Obtain the target position of the end and calculate the position error d between the initial position and the target position of the end. err .

[0076] Step S104, Detect d err ≥d target And Iter≤Iter max If the condition is true, proceed to step S105; otherwise, proceed to step S110.

[0077] Step S105: Calculate the Jacobian matrix J based on the position error, and obtain the pseudo-inverse matrix J through SVD decomposition and pseudo-inverse calculation. + .

[0078] Step S106: Obtain the angle increment of each joint based on the pseudo-inverse matrix.

[0079] Step S107: Update the joint angle Ang according to the angle increment of each joint. tmp .

[0080] Among them, Ang tmp This represents the joint angle calculated during the current iteration.

[0081] Step S108: Optimize joint angles based on preset posture constraints to obtain the optimized joint angle value Ang. new .

[0082] Step S109: Optimize the joint angle value Ang. new Update the position error d at the end err The number of iterations in the reverse motion is Iter.

[0083] Step S110: Output the latest joint angle sequence Ang target .

[0084] Here, the output joint angle sequence is the second target pose of the multi-joint mechanism.

[0085] In one embodiment, in response to detecting that an obstacle avoidance triggering condition is met during the movement of the end of the multi-joint mechanism toward the target position, before determining the obstacle avoidance intermediate path point based on the obstacle's size data and position data and the target position, the method includes:

[0086] Acquire the control mode for the multi-joint mechanism, which includes any one of the following: retracting arm mode and extending arm mode;

[0087] The target position of the end of the multi-joint mechanism is determined based on the control mode.

[0088] The boom retraction mode moves the end of the multi-joint mechanism closer to the turntable it's connected to, while the boom extension mode moves it further away from the turntable. Users can set the control mode of the multi-joint mechanism according to their needs. For example, if the multi-joint mechanism is a concrete pump truck boom in a folded state, and the boom end needs to reach a distant position, the control mode can be set to boom extension mode. After determining the control mode, a target position setting interface can be displayed, allowing the user to input the target position. Alternatively, the target position can be automatically determined after the control mode is set. For instance, if the multi-joint mechanism is a concrete pump truck boom in an extended state, and concrete pouring needs to be completed and the boom needs to be folded, selecting the boom retraction mode allows the target position to be determined based on historical records of the boom end's position in the folded state. This enables rapid determination of the target position of the multi-joint mechanism's end, further improving processing efficiency.

[0089] It should be noted that existing conventional boom end-effector planning methods continuously search for trajectory points of the boom end-effector obstacle avoidance path in three-dimensional space. While this facilitates obstacle avoidance and ensures a smooth end-effector trajectory, it cannot guarantee obstacle avoidance for the entire boom. In contrast, the boom retraction and extension modes of this application require a trajectory point search within the entire boom's working space to achieve obstacle avoidance for the entire boom. Therefore, a path point around the obstacle is first determined in three-dimensional space, and then control is performed in the attitude (angle sequence) space.

[0090] In one embodiment, the method further includes:

[0091] Based on the current posture, the first target posture, and the second target posture of the multi-joint mechanism, and combined with the target cost function and multi-objective constraints, a second movement path is planned for the end effector of the multi-joint mechanism, starting from the current position and passing through the intermediate path point around the obstacle to the target position; the target cost function includes the shortest path cost function and the non-collision cost function.

[0092] Here, the current posture of the multi-joint mechanism is taken as the initial posture, the first target posture as the intermediate target posture, and the second target posture as the final target posture. Combining the target cost function and multi-objective constraints, a path planning algorithm is used to plan the second movement path of the multi-joint mechanism's end effector from its current position, passing through the intermediate path point around the obstacle to the target position. The second movement path is the shortest path and the path in which the multi-joint mechanism will not collide with obstacles during movement. The shortest path cost function is used to minimize the path length traversed by each joint of the multi-joint mechanism's end effector from the initial position to the target position, while the non-collision cost function is used to ensure that the path does not collide with obstacles in the working environment.

[0093] Specifically, based on the current attitude of the multi-joint mechanism and the first target attitude, a first sub-movement path is planned from the current position of the multi-joint mechanism end-effector to the obstacle bypass intermediate path point; based on the first target attitude and the second target attitude, a second sub-movement path is planned from the obstacle bypass intermediate path point to the target position; the first sub-movement path and the second sub-movement path are spliced ​​together to determine the target movement path generated after splicing; the target movement path is optimized based on the target cost function and multi-objective constraints to obtain the second movement path of the multi-joint mechanism end-effector from the current position, passing through the obstacle bypass intermediate path point to the target position.

[0094] The specific processes for planning the first sub-path of the multi-joint mechanism's end effector from its current position to the obstacle avoidance midpoint, based on the current attitude and the first target attitude, and for planning the second sub-path of the multi-joint mechanism's end effector from the obstacle avoidance midpoint to the target position, based on the first and second target attitudes, can be found in existing technologies and will not be elaborated here. By concatenating the first and second sub-paths, the generated target path can be obtained. However, since the target path may not be optimal, further optimization is required.

[0095] The multi-objective constraints include at least one of the following: shortest path length; minimum path height greater than or equal to a height threshold; maximum distance between the path and obstacles less than or equal to a distance threshold. Furthermore, multi-objective constraints may also include an iteration count less than or equal to an iteration count threshold to avoid the iteration process getting stuck.

[0096] In this embodiment, the optimization of the target movement path based on the objective cost function and multi-objective constraints can be expressed as the following multi-constraint optimization problem:

[0097] J(x) = w1J PATH (x)+w2J obstacle (x)

[0098] st:Iter'≤Iter' max

[0099] h min ≥h t

[0100] max_distance≤Dis

[0101] Among them, J PATH (x) represents the shortest path cost function. The goal of this part is to minimize the path length traversed by each joint at the end of the multi-joint mechanism from the initial position to the target position, i.e., the angle of motion of each joint, in order to reduce the motion time. Meanwhile, x i Let N be the position of each joint at time step i, and N be the total number of time steps in the path.

[0102] Among them, J obstacle (x) represents the non-collision cost function, which is used to ensure that the path does not collide with obstacles in the working environment.

[0103]

[0104] ω1 and ω2 are weighting factors used to adjust the relative importance of different components in the objective cost function. These weights can be adjusted according to the performance requirements of specific application scenarios. Multi-objective constraints mainly include constraints on path height and the maximum distance between the path and obstacles. Iter' represents the number of iterations. max h represents the threshold for the number of iterations. min h represents the minimum height of the entire path. tThe minimum height threshold of the path is defined by `max_distance`, which represents the farthest distance between the entire path and the obstacle to ensure that the obstacle avoidance path does not deviate too far from the original path, thus preventing the introduction of other interference factors. `k` represents the obstacle-related cost value, which is positively correlated with the shortest distance between the entire path and the obstacle. `Dis` represents the distance threshold between the entire path and the obstacle. By constructing the above multi-constraint optimization problem, the feasibility and speed of the segmented obstacle avoidance path result can be guaranteed. Finally, through iterative solution, a segmented path satisfying multiple objective constraints is obtained, namely, the path from the end of the multi-joint mechanism from its current position to the intermediate path point around the obstacle, and the path from the intermediate path point around the obstacle to the target position. It should be noted that the second movement path can be considered as a segmented obstacle avoidance path, which includes the second angle change of each joint in each control cycle. After the end of the multi-joint mechanism moves to the target position according to the second movement path, the attitude of the multi-joint mechanism is the second target attitude. It is understandable that when the shortest second movement path from the current position of the multi-joint mechanism's end-effector to the target position, bypassing intermediate obstacle points, is achieved, the number of actions required by the multi-joint mechanism will be reduced, resulting in lower energy consumption and improved energy efficiency. Simultaneously, by ensuring the minimum path height is greater than or equal to a height threshold, the end-effector of the multi-joint mechanism can be kept a certain distance from the ground, preventing the end-effector or its associated equipment from touching the ground. Furthermore, by ensuring the maximum distance between the path and obstacles is less than or equal to a distance threshold, the multi-joint mechanism does not need to significantly bypass obstacles, minimizing energy consumption. Thus, optimizing the target movement path based on the objective cost function and multi-objective constraints can satisfy the requirements for path length, path height, collision information, and other path characteristics while ensuring the accuracy of the solution path. This ensures the practical feasibility and usability of the obtained movement path and guarantees the feasibility of the segmented obstacle avoidance path.

[0105] For example, a multi-joint mechanism is used as the boom of a concrete pump truck, see [reference]. Figure 3 The following is a detailed explanation of the control process for multi-joint mechanisms, including the following steps:

[0106] Step S201: Obtain the enable command for automatic arm extension mode or automatic arm retraction mode.

[0107] Step S202: Determine the target attitude based on the obtained target position at the end of the boom.

[0108] Step S203: Plan the movement path based on the target posture, and control the boom movement according to the movement path.

[0109] Step S204: Detect obstacle information.

[0110] Here, obstacle information may include the obstacle's center coordinates, size, and the nearest arm segment, etc.

[0111] Step S205: Determine whether the obstacle avoidance condition is met based on the distance between the boom and the obstacle. If yes, proceed to step S206; otherwise, proceed to step S210.

[0112] Here, the obstacle avoidance condition can be met if the distance between the boom and the obstacle is less than a safety threshold.

[0113] Step S206: Automatically generate intermediate path points for bypassing obstacles based on obstacle information.

[0114] Step S207: Plan the obstacle avoidance trajectory in joint space for the intermediate path points and target positions.

[0115] Step S208: Determine whether the planned segmented path meets the obstacle avoidance path requirements. If yes, proceed to step S209; otherwise, proceed to step S207.

[0116] Here, the obstacle avoidance path requirements can be specifically referred to in the multi-objective constraint conditions of the aforementioned embodiments.

[0117] Step S209: Complete the obstacle avoidance according to the planned segmented path until the end of the boom reaches the target position.

[0118] Step S210: Continue to control the boom movement according to the movement path until the end of the boom reaches the target position.

[0119] Based on the same inventive concept as the foregoing embodiments, this application provides an electronic device, including a processor, a memory, and a computer program stored in the memory and executable on the processor. When the computer program is executed by the processor, it implements the multi-joint mechanism control method as described above.

[0120] Based on the same inventive concept as the foregoing embodiments, this embodiment also provides a working machine, including a controller, a multi-joint mechanism, and a laser radar disposed on the multi-joint mechanism. The laser radar is used to collect size data and position data of obstacles during the process of the end of the multi-joint mechanism moving towards the target position. When the controller executes a computer program, it implements the multi-joint mechanism control method as described above.

[0121] Based on the same inventive concept as the foregoing embodiments, this embodiment also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the multi-joint mechanism control method as described above.

[0122] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0123] In this document, the terms “comprising,” “including,” or any other variations thereof are intended to cover non-exclusive inclusion, which includes not only the elements listed but also other elements not expressly listed.

[0124] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the appended claims.

Claims

1. A control method for a multi-joint mechanism, characterized in that, The method includes: In response to detecting that the obstacle avoidance triggering condition is met during the movement of the end of the multi-joint mechanism toward the target position, the intermediate path point for obstacle avoidance is determined based on the size data and position data of the obstacle and the target position. Based on preset attitude constraints and inverse motion iterative solution algorithm, the first target attitude of the multi-joint mechanism is determined when the end of the multi-joint mechanism reaches the intermediate path point of the obstacle bypass. The first target attitude includes a first angle sequence composed of the angles corresponding to each joint in the multi-joint mechanism. The preset posture constraints include an iteration termination condition and angle limitation conditions corresponding to each joint; determining the first target posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the obstacle-avoidance intermediate path point based on the preset posture constraints and the inverse motion iterative solution algorithm includes: Step S10: Based on the positional deviation between the expected position of the end of the multi-joint mechanism and the intermediate path point of the obstacle bypass calculated in the previous iteration, obtain the angle increment corresponding to each joint in the current iteration based on the inverse motion iterative solution algorithm; Step S11: Determine the calculated angles of each joint in the current iteration based on the angle increments corresponding to each joint and the expected angles corresponding to each joint calculated in the previous iteration. Step S12: Optimize the calculated angle based on the angle constraints to obtain the expected angles corresponding to each joint in the current iteration. Step S13: Determine the expected position of the end of the multi-joint mechanism in the current iteration based on the expected angles corresponding to each joint in the current iteration. Step S14: Check whether the iteration termination condition is met. If not, return to step 10. If yes, proceed to step S15. Step S15: Based on the expected angles corresponding to each joint in the current iteration, determine the first target posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the obstacle bypass intermediate path point.

2. The multi-joint mechanism control method according to claim 1, characterized in that, The angle constraints include the angle range intervals and singular angle intervals corresponding to each joint; The iteration termination conditions include the number of reverse motion iterations being greater than a preset number and / or the position deviation being less than a preset threshold.

3. The multi-joint mechanism control method according to claim 1, characterized in that, The response to detecting that an obstacle avoidance trigger condition is met during the movement of the end of the multi-joint mechanism toward the target position, before determining the obstacle avoidance intermediate path point based on the obstacle's size and position data and the target position, includes: Determine the target position at the end of the multi-joint mechanism; Based on the preset posture constraints and the inverse motion iterative solution algorithm, a second target posture of the multi-joint mechanism is obtained when the end of the multi-joint mechanism reaches the target position; the second target posture includes a second angle sequence composed of the angles corresponding to each joint. Based on the initial posture of the multi-joint mechanism and the second target posture, a first movement path is planned for the end effector of the multi-joint mechanism from the initial position to the target position; The multi-joint mechanism is controlled to move based on the first movement path, so that the end of the multi-joint mechanism moves toward the target position.

4. The multi-joint mechanism control method according to any one of claims 1 to 3, characterized in that, The response to detecting that an obstacle avoidance trigger condition is met during the movement of the end of the multi-joint mechanism toward the target position, before determining the obstacle avoidance intermediate path point based on the obstacle's size and position data and the target position, includes: Obtain the control mode for the multi-joint mechanism, the control mode including any one of the following: arm retraction mode and arm extension mode; The target position of the end of the multi-joint mechanism is determined according to the control mode.

5. The multi-joint mechanism control method according to claim 3, characterized in that, The method further includes: Based on the current posture of the multi-joint mechanism, the first target posture, and the second target posture, and combined with the target cost function and multi-objective constraints, a second movement path is planned for the end effector of the multi-joint mechanism from its current position, passing through the obstacle bypass intermediate path point to the target position; the target cost function includes the shortest path cost function and the non-collision cost function.

6. The multi-joint mechanism control method according to claim 5, characterized in that, The multi-objective constraints include at least one of the following: The path length is the shortest; The path's minimum height is greater than or equal to the height threshold; The furthest distance between the path and obstacles is less than or equal to the distance threshold.

7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the multi-joint mechanism control method as described in any one of claims 1 to 6.

8. A working machine, comprising a controller, a multi-joint mechanism, and a laser radar disposed on the multi-joint mechanism, wherein the laser radar is used to collect size data and position data of an obstacle as the end of the multi-joint mechanism moves toward a target position, and the controller executes a computer program to implement the multi-joint mechanism control method as described in any one of claims 1 to 6.

9. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the multi-joint mechanism control method as described in any one of claims 1 to 6.

Images