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

Abstract

The invention discloses a multi-joint mechanism control method, electronic equipment, an operation machine and a computer readable storage medium, and the method comprises the steps: responding to a condition that the tail end of a multi-joint mechanism moves towards a target position and meets an obstacle avoidance triggering condition; determining an obstacle avoidance middle path point according to the size data and the position data of the obstacle and the target position; based on a preset attitude constraint condition and an inverse motion iterative solution algorithm, a first target attitude of the multi-joint mechanism when the tail end of the multi-joint mechanism reaches the obstacle avoidance middle path point is determined, and the first target attitude comprises a first angle sequence composed of angles corresponding to all joints in the multi-joint mechanism. According to the technical scheme, the obstacle avoidance middle path point is determined according to the obstacle information and the target position, the posture of the multi-joint mechanism when the tail end of the multi-joint mechanism reaches the obstacle avoidance middle path point is solved, the purpose of automatic obstacle avoidance can be achieved, and meanwhile the processing efficiency and reliability are high.

Description

Technical Field

[0001] The present application relates to the technical field of working machinery, and in particular to a multi-joint mechanism control method, electronic equipment, working machinery and computer-readable storage medium. Background Art

[0002] For operating machinery with multi-joint booms, such as pump trucks, concrete placing machines, and folding-arm cranes, boom extension and retraction are currently mostly achieved through manual control based on the boom extension and retraction device. However, due to the limitations of the construction scene and the deviation of human observation, the boom may scratch objects outside the field of view, that is, the existing multi-joint mechanism control method has the problem of being unable to automatically avoid obstacles. Summary of the invention

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

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

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

[0006] In response to detecting that an obstacle avoidance trigger condition is satisfied during the process in which the end of the multi-joint mechanism moves toward the target position, determining an obstacle avoidance intermediate path point according to size data and position data of the obstacle and the target position;

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

[0008] As one implementation manner, the preset posture constraint condition includes an iteration end condition and an angle constraint condition corresponding to each of the joints;

[0009] The method of determining the first target posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the obstacle bypassing intermediate path point based on the preset posture constraint condition and the inverse motion iterative solution algorithm comprises:

[0010] Step S10, according to the position deviation between the expected position of the end of the multi-joint mechanism and the intermediate path point of the obstacle avoidance calculated in the previous iteration process, the angle increment corresponding to each joint in the current iteration process is obtained based on the inverse kinematic iterative solution algorithm;

[0011] Step S11, determining the calculated angles corresponding to the joints in the current iteration according to the angle increments corresponding to the joints and the expected angles corresponding to the joints calculated in the previous iteration;

[0012] Step S12, optimizing the calculated angle based on the angle restriction condition to obtain the expected angles corresponding to the joints in the current iteration process;

[0013] Step S13, determining the expected position of the end of the multi-joint mechanism in the current iteration process according to the expected angles corresponding to the joints in the current iteration process;

[0014] Step S14: Check whether the iteration end condition is met, if not, return to step 10, if yes, execute step S15;

[0015] Step S15, determining a first target posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the intermediate path point of the obstacle avoidance according to the expected angles corresponding to the joints in the current iteration process.

[0016] As one implementation manner, the angle restriction condition includes an angle range interval and a singular angle interval corresponding to each joint;

[0017] The iteration end condition includes that the number of inverse motion iterations is greater than a preset number and / or the position deviation is less than a preset threshold.

[0018] As one embodiment, in response to detecting that an obstacle avoidance trigger condition is satisfied during the process in which the end of the multi-joint mechanism moves toward the target position, before determining the obstacle avoidance intermediate path point according to the size data and position data of the obstacle and the target position, the method includes:

[0019] Determining a target position of an end of the multi-joint mechanism;

[0020] Based on the preset posture constraint condition and the inverse kinematic 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 angles corresponding to each of the joints;

[0021] Planning a first moving path of the end of the multi-joint mechanism from an initial position to the target position according to the initial posture of the multi-joint mechanism and the second target posture;

[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] As one embodiment, in response to detecting that an obstacle avoidance trigger condition is satisfied during the process in which the end of the multi-joint mechanism moves toward the target position, before determining the obstacle avoidance intermediate path point according to the size data and position data of the obstacle and the target position, the method includes:

[0024] Acquire a control mode of the multi-joint mechanism, wherein the control mode includes any one of the following: an arm-retracting mode and an arm-extending mode;

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

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

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

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

[0029] The path length is the shortest;

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

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

[0032] In a second aspect, the present application provides an electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the multi-joint mechanism control method as described in the first aspect is implemented.

[0033] In the third aspect, the present application provides a working machine, including a controller, a multi-joint mechanism and a laser radar arranged on the multi-joint mechanism, wherein the laser radar is used to collect size data and position data of obstacles in the process of the end of the multi-joint mechanism moving toward the target position, and when the controller executes the computer program, it implements the multi-joint mechanism control method described in the first aspect.

[0034] In a fourth aspect, the present application provides a computer-readable storage medium, in which a computer program is stored. When the computer program is executed by a processor, the multi-joint mechanism control method as described in the first aspect is implemented.

[0035] The present application provides a multi-joint mechanism control method, electronic device, working machine and computer-readable storage medium, the method comprising: in response to detecting that an obstacle bypassing trigger condition is met during the movement of the end of the multi-joint mechanism toward the target position, determining the obstacle bypassing intermediate path point according to the size data and position data of the obstacle and the target position; based on the preset posture constraint condition and the inverse motion iterative solution algorithm, determining the first target posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the obstacle bypassing intermediate path point, the first target posture including the first angle sequence composed of the angles corresponding to each joint in the multi-joint mechanism. The technical solution of the present application first determines the obstacle bypassing intermediate path point according to the acquired size data and position data of the obstacle and the target position when the obstacle bypassing trigger condition is detected, and then determines the first target posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the obstacle bypassing intermediate path point based on the preset posture constraint condition and the inverse motion iterative solution algorithm, so as to bypass the obstacle by controlling the posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the obstacle bypassing intermediate path point, so as to achieve the purpose of automatic obstacle avoidance, and at the same time, the processing efficiency and reliability are high. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for use in the embodiments will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.

[0037] Figure 1 A schematic flow chart of a multi-joint mechanism control method provided in an embodiment of the present application;

[0038] Figure 2 A schematic diagram of a process for obtaining a second target posture of a multi-joint mechanism in an embodiment of the present application;

[0039] Figure 3 A schematic diagram of the specific process of a multi-joint mechanism control method provided in an embodiment of the present application. DETAILED DESCRIPTION

[0040] The specific embodiments of the present application will be described in detail below in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all of the embodiments. Based on the description of the present application, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the present application.

[0041] It should be noted that, in this article, the terms "include", "comprises" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also includes other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "includes a ..." does not exclude the existence of other identical elements in the process, method, article or device including the element. In addition, components, features, and elements with the same name in different embodiments of the present application may have the same meaning or different meanings, and their specific meanings need to be determined by their explanation in the specific embodiment or further combined with the context of the specific embodiment.

[0042] It should be understood that, although the terms first, second, third, etc. may be used to describe various information in this article, these information should not be limited to these terms. These terms are only used to distinguish the same type of information from each other. For example, without departing from the scope of this article, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Depending on the context, the word "if" as used herein can be interpreted as "at the time of" or "when" or "in response to determination". Furthermore, as used in this article, the singular forms "one", "one" and "the" are intended to also include plural forms, unless there is an opposite indication in the context. It should be further understood that the terms "comprising" and "including" indicate the presence of the described features, steps, operations, elements, components, projects, kinds, and / or groups, but do not exclude the presence, occurrence or addition of one or more other features, steps, operations, elements, components, projects, kinds, and / or groups. The terms "or" and "and / or" used herein are interpreted as inclusive, or mean any one or any combination. Thus, “A, B, or C” or “A, B and / or C” means “any of the following: A; B; C; A and B; A and C; B and C; A, B, and C.” An exception to this definition will occur only when a combination of elements, functions, steps, or operations are inherently mutually exclusive in some manner.

[0043] It should be understood that, although the various steps in the flowchart in the embodiment of the present application are displayed in sequence according to the indication of the arrows, these steps are not necessarily performed in sequence according to the order indicated by the arrows. Unless there is a clear explanation in this article, the execution of these steps does not have a strict order restriction, and it can be performed in other orders. Moreover, at least a portion of the steps in the figure may include a plurality of sub-steps or a plurality of stages, and these sub-steps or stages are not necessarily performed at the same time, but can be performed at different times, and their execution order is not necessarily performed in sequence, but can be performed in turn or alternately with other steps or at least a portion of the sub-steps or stages of other steps.

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

[0045] It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0046] See also Figure 1 , is a flow chart of a multi-joint mechanism control method provided in an embodiment of the present application. The multi-joint mechanism control method can be executed by a multi-joint mechanism control device provided in an embodiment of the present application. The multi-joint mechanism control device can be implemented in software and / or hardware, for example, it can be specifically an electronic device such as a processor or a controller. The multi-joint mechanism control method provided in this embodiment includes the following steps:

[0047] Step S1: In response to detecting that an obstacle avoidance trigger condition is satisfied during the process of the end of the multi-joint mechanism moving toward a target position, an obstacle avoidance intermediate path point is determined according to the size data and position data of the obstacle and the target position.

[0048] Among them, the multi-joint mechanism includes multiple structures and multiple joints, and adjacent structures are connected by joints, that is, adjacent structures are connected by joints. For example, the multi-joint mechanism can be a boom, a mechanical arm, etc. of a pump truck, and the structure included in the boom is a joint arm. In this embodiment, the multi-joint mechanism can be a concrete pump truck boom, and the posture of the multi-joint mechanism is a set of postures of the joints of the pump truck boom. In the process of controlling the end of the multi-joint mechanism to move toward the target position, the surrounding environment of the multi-joint mechanism can be monitored in real time by a detection device such as a laser radar installed on the multi-joint mechanism, so that when an obstacle is detected, the position, size and other information of the detected obstacle can be fed back to the electronic device 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 in combination with 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, and can be represented by three-dimensional coordinates. For example, the target position can be the position where the end of the multi-joint mechanism is located when the multi-joint mechanism is in a folded state, or it can be any specified position.

[0049] Wherein, when the size data and position data of the obstacle and the target position are obtained, the formula (X 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 ) calculates the coordinates of the middle path point of the obstacle avoidance, the subscript temp represents the middle path point of the obstacle avoidance, the subscript ob represents the obstacle position, the subscript ob_M represents the obstacle size, and the subscript coe represents the extension coefficient, and can be calculated according to the formula (X coe ,Y coe ,Z coe )=D sec *(α, β, 0) to determine, D sec is the safety distance value preset according to 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 before moving to the target position.

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

[0051] In order to enable the multi-joint mechanism to bypass obstacles and avoid collision with obstacles, the first target posture of the multi-joint mechanism can be determined when the end of the multi-joint mechanism reaches the middle path point of bypassing obstacles based on the preset posture constraint conditions and the inverse kinematics iterative solution algorithm. The preset posture constraint conditions are used to describe the iterative solution restriction information and the angle restriction information of each joint.

[0052] The technical solution of the present application first determines the intermediate path point of obstacle avoidance based on the acquired size data and position data of the obstacle and the target position when it is detected that the obstacle avoidance trigger condition is met, and then determines the first target posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the intermediate path point of obstacle avoidance based on preset posture constraints and an inverse motion iterative solution algorithm. By controlling the posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the intermediate path point of obstacle avoidance, the obstacle can be bypassed, thereby achieving the purpose of automatic obstacle avoidance with high processing efficiency and reliability.

[0053] In one embodiment, the preset posture constraint condition includes an iteration end condition and an angle constraint condition corresponding to each joint; based on the preset posture constraint condition and the inverse kinematic iterative solution algorithm, determining the first target posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the intermediate path point of the obstacle avoidance includes:

[0054] Step S10, according to the position deviation between the expected position of the end of the multi-joint mechanism and the intermediate path point of the obstacle avoidance calculated in the previous iteration process, the angle increment corresponding to each joint in the current iteration process is obtained based on the inverse kinematic iterative solution algorithm;

[0055] Step S11, determining the calculated angles corresponding to the joints in the current iteration process according to the angle increments corresponding to the joints and the expected angles corresponding to the joints calculated in the previous iteration process;

[0056] Step S12, optimizing the calculated angle based on the angle restriction condition to obtain the expected angle corresponding to each joint in the current iteration process;

[0057] Step S13, determining the expected position of the end of the multi-joint mechanism in the current iteration process according to the expected angles corresponding to the joints in the current iteration process;

[0058] Step S14: Check whether the iteration end condition is met. If not, return to step 10. If yes, execute step S15.

[0059] Step S15: Determine the first target posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the middle path point of the obstacle avoidance according to the expected angles corresponding to the joints in the current iteration process.

[0060] Among them, in the first iterative solution process, the expected position of the end of the multi-joint mechanism calculated in the previous iterative process is the current position of the end of the multi-joint mechanism, that is, the position of the end of the multi-joint mechanism when the obstacle avoidance trigger condition is detected. At the same time, in the first iterative solution process, the expected angles corresponding to each joint calculated in the previous iterative process are the current angles corresponding to each joint, that is, the angles corresponding to each joint when the obstacle avoidance trigger condition is detected.

[0061] Among them, obtaining the angle increments corresponding to each joint in the current iteration process 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, obtaining the pseudo-inverse matrix, and obtaining the angle increments corresponding to each joint in the current iteration process based on the pseudo-inverse matrix. The specific implementation principle of the inverse motion iterative solution algorithm can refer to the existing technology and will not be repeated here. In this way, the target posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the middle path point of the obstacle bypassing is solved by the inverse motion iterative solution algorithm with posture constraints, which improves the solution speed and convergence speed. At the same time, it ensures the feasibility of the results, makes the results closer to actual needs, and improves the current one-key extension and retraction technology. The final posture solution result cannot be guaranteed or is preset in a preset way, resulting in poor system environmental adaptability and other problems.

[0062] Among them, the angle restriction conditions may include the angle range intervals and singular angle intervals corresponding to each joint respectively; the iteration end conditions include that the number of inverse motion iterations is greater than the preset number and / or the position deviation is less than the preset threshold. Among them, the angle range interval is used to indicate the range of possible angle values ​​corresponding to the joint, including the upper limit value and the lower limit value of the angle, and the angle range interval corresponding to each joint may be different. The singular angle interval is used to indicate the optimized angle set for the corresponding joint for the preset inclination posture that cannot be planned or is difficult to plan. For example, when the preset inclination posture is an arch posture, it is necessary to optimize the angles of several joints specified in the multi-joint mechanism based on the singular angle interval. Here, the angle ang is calculated based on the angle restriction condition. i Optimize and obtain the expected angle Ang corresponding to each joint in the current iteration process new , which can be expressed as:

[0063]

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

[0065] In one embodiment, in response to detecting that an obstacle avoidance trigger condition is satisfied during the process of the end of the multi-joint mechanism moving toward the target position, before determining the obstacle avoidance intermediate path point according to the size data and position data of the obstacle and the target position, the method includes:

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

[0067] Based on preset posture constraints and an inverse kinematic 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 angles corresponding to each joint;

[0068] Planning a first moving path of the end of the multi-joint mechanism from the initial position to the target position according to the initial posture and the second target posture of the multi-joint mechanism;

[0069] 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.

[0070] Wherein, the target position of the end of the multi-joint mechanism can be manually input by the user, or can be automatically determined based on the control mode of the multi-joint mechanism. After determining the target position of the end of the multi-joint mechanism, the second target posture of the multi-joint mechanism can be obtained when the end of the multi-joint mechanism reaches the target position based on the preset posture constraint condition and the inverse motion iterative solution algorithm. The specific process can refer to the aforementioned acquisition process of the first target posture. Wherein, according to the initial posture and the second target posture of the multi-joint mechanism, the specific process of planning the first moving path of the end of the multi-joint mechanism from the initial position to the target position can refer to the prior art, which will not be repeated here. Here, the first moving path includes the angle change of each joint in each control cycle, that is, according to the first moving path, the angle information that each joint needs to change in each control cycle can be obtained. The initial position of the end of the multi-joint mechanism refers to the corresponding position of the multi-joint mechanism before it starts to move. After determining the first moving path of the end of the multi-joint mechanism from the initial position to the target position, the multi-joint mechanism can be controlled to move based on the first moving path, so that the end of the multi-joint mechanism moves toward the target position. It should be noted that when the multi-joint mechanism is controlled to move according to 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, and the posture of the multi-joint mechanism is the first target posture.

[0071] Exemplarily, the process of obtaining the second target posture of the multi-joint mechanism is described in detail below. Figure 2 , including the following steps:

[0072] Step S101, initialize the arm parameters, initial angles of each joint, Iter max d target .

[0073] Among them, d target Represents the preset position error threshold, Iter max Indicates the maximum number of iterations for the inverse kinematics.

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

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

[0076] Step S104: Detection d err ≥d target And Iter≤Iter max Is it established? If so, execute step S105; otherwise, execute step S110.

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

[0078] Step S106: Obtain the angle increment of each joint according to 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 Represents the joint angle calculated during the current iteration.

[0081] Step S108: Optimize the joint angle based on the preset posture constraint conditions to obtain the joint angle optimization value Ang new .

[0082] Step S109: Optimize the value Ang according to the joint angle new Update the position error d at the end err The number of inverse kinematic iterations Iter.

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

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

[0085] In one embodiment, in response to detecting that an obstacle avoidance trigger condition is satisfied during the process of the end of the multi-joint mechanism moving toward the target position, before determining the obstacle avoidance intermediate path point according to the size data and position data of the obstacle and the target position, the method includes:

[0086] Obtaining a control mode of the multi-joint mechanism, the control mode including any one of the following: arm retracting mode and arm extending mode;

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

[0088] Among them, the arm retracting mode refers to a mode in which the end of the multi-joint mechanism is moved closer to the turntable to which the multi-joint mechanism is connected; the arm extension mode refers to a mode in which the end of the multi-joint mechanism is moved away from the turntable to which the multi-joint mechanism is connected. The user can set the control mode of the multi-joint mechanism based on the needs. For example, when the multi-joint mechanism is a concrete pump truck arm and the arm is in a folded state, if the end of the arm is required to reach a farther position, the control mode of the multi-joint mechanism can be set to the arm extension mode. Among them, after determining the control mode, the setting interface of the target position can be output to obtain the target position entered by the user in the setting interface. In addition, after determining the control mode, the target position can also be automatically determined. For example, when the multi-joint mechanism is a concrete pump truck arm and the arm is in an extended state, if the concrete pouring is ended at this time and the arm needs to be controlled in a folded state, after selecting the arm retracting mode, the historical position of the end of the arm when the arm is in a folded state can be determined as the target position based on the historical records. In this way, the target position of the end of the multi-joint mechanism can be quickly determined, further improving the processing efficiency.

[0089] It should be noted that the existing boom end planning method in the conventional working mode is to continuously search for the trajectory points of the obstacle avoidance trajectory of the boom end in three-dimensional space, which ensures the smoothness of the end trajectory while facilitating obstacle avoidance, but cannot guarantee obstacle avoidance for the entire boom. In the arm retracting and arm extension modes of the present application, it is necessary to search for trajectory points in the working space of the entire boom to achieve obstacle avoidance for the entire boom. Therefore, an intermediate path point for avoiding obstacles is first determined in three-dimensional space, and then controlled in the attitude (angle sequence) space.

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

[0091] Based on the current posture, the first target posture and the second target posture of the multi-joint mechanism, combined with the target cost function and multi-target constraints, a second moving path is planned for the end of the multi-joint mechanism, starting from the current position, 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 is taken as the intermediate target posture, and the second target posture is taken as the final target posture. In combination with the target cost function and the multi-target constraint conditions, a path planning algorithm is used to plan the second moving path of the multi-joint mechanism end starting from the current position, passing through the obstacle-circumventing intermediate path point to the target position, and the second moving path is the shortest path in which the multi-joint mechanism will not collide with obstacles when moving. The shortest path cost function is used to minimize the path length of each joint when the multi-joint mechanism end moves from the initial position to the target position, and the collision-free cost function is used to ensure that the path does not collide with obstacles in the working environment.

[0093] Specifically, according to the current posture and the first target posture of the multi-joint mechanism, a first sub-movement path of the multi-joint mechanism terminal from the current position to the intermediate path point of the obstacle circumvention is planned; according to the first target posture and the second target posture, a second sub-movement path of the multi-joint mechanism terminal from the intermediate path point of the obstacle circumvention to the target position is planned; the first sub-movement path and the second sub-movement path are spliced ​​to determine the target moving path generated after splicing; based on the target cost function and multi-target constraints, the target moving path is optimized to obtain the second moving path of the multi-joint mechanism terminal starting from the current position, passing through the intermediate path point of the obstacle circumvention to the target position.

[0094] Among them, according to the current posture and the first target posture of the multi-joint mechanism, the first sub-movement path of the end of the multi-joint mechanism from the current position to the middle path point of the obstacle bypassing is planned, and according to the first target posture and the second target posture, the second sub-movement path of the end of the multi-joint mechanism from the middle path point of the obstacle bypassing to the target position is planned. The specific process can refer to the prior art and will not be repeated here. By splicing the first sub-movement path and the second sub-movement path, the target movement path generated after splicing can be obtained, and because the target movement path may not be optimal, the target movement path needs to be further optimized.

[0095] The multi-objective constraint condition includes at least one of the following: the path length is the shortest; the lowest height of the path is greater than or equal to the height threshold; the farthest distance between the path and the obstacle is less than or equal to the distance threshold. In addition, the multi-objective constraint condition may also include that the number of iteration calculations is less than or equal to the iteration calculation number threshold to avoid the iterative calculation process being stuck.

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

[0097] J(x)=w 1 J PATH (x)+w 2 J 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 of each joint when the end of the multi-joint mechanism moves from the initial position to the target position, that is, the angle of movement of each joint, so as to reduce the movement time. At the same time, x i is the position of each joint at time step i, and N is the total number of time steps in the path.

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

[0103]

[0104] ω 1 and ω 2 is a weight factor used to adjust the relative importance of different components in the target cost function. These weights can be adjusted according to the performance requirements of specific application scenarios. The multi-objective constraints mainly include the constraints on the path height, the constraints on the maximum distance between the path and the obstacle, etc. Iter' represents the number of iterative calculations, Iter' max It represents the threshold of iterative calculation times, h min Indicates the minimum height of the entire path, h tRepresents the minimum height threshold of the path, max_distance 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, thereby introducing other interference factors, k represents the obstacle-related cost value, which is positively correlated with the closest distance of the entire path to the obstacle, and Dis represents the distance threshold between the entire path and the obstacle. By constructing the above-mentioned multi-constraint optimization problem, the feasibility and rapidity of the segmented obstacle avoidance path results can be guaranteed, and finally, through iterative solution, a segmented path that meets the multi-objective constraints is obtained, that is, the path from the current position of the end of the multi-joint mechanism to the middle path point of the obstacle avoidance and the path from the middle path point of the obstacle avoidance to the target position. It should be noted that the second moving 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 is controlled to move to the target position according to the second moving path, the posture of the multi-joint mechanism is the second target posture. It can be understood that when the second moving path from the current position of the end of the multi-joint mechanism to the target position through the middle path point of the obstacle bypassing is the shortest, the number of actions required to be performed by the multi-joint mechanism will be less, and accordingly, the energy consumption required will be less, that is, the energy efficiency is improved. At the same time, by ensuring that the minimum height of the path is greater than or equal to the height threshold, the end of the multi-joint mechanism can be kept at a certain distance from the ground, avoiding the end of the multi-joint mechanism or the equipment set at the end of the multi-joint mechanism from touching the ground, etc. In addition, by ensuring that the farthest distance between the path and the obstacle is less than or equal to the distance threshold, the multi-joint mechanism does not need to bypass the obstacle by a large margin, minimizing the energy consumption required. In this way, the target moving path is optimized based on the target cost function and multi-objective constraints, and the requirements for path characteristics such as path length, path height, collision information, etc. can be met under the premise of ensuring the accuracy of the solution path, ensuring the actual feasibility and availability of the obtained moving path, and ensuring the feasibility of the segmented obstacle avoidance path.

[0105] For example, the multi-joint mechanism is used as a concrete pump truck boom, see Figure 3 , the process of controlling the multi-joint mechanism is described in detail below, including the following steps:

[0106] Step S201: Obtain an enabling instruction for an automatic arm extension mode or an automatic arm retraction mode.

[0107] Step S202: Determine the target posture according to the acquired target position of the boom end.

[0108] Step S203: planning a moving path based on the target posture, and controlling the arm motion according to the moving path.

[0109] Step S204: Detect obstacle information.

[0110] Here, the obstacle information may include the center coordinates, size, and the closest arm segment of the obstacle.

[0111] Step S205: determine whether the obstacle avoidance condition is met based on the distance between the boom and the obstacle. If so, execute step S206; otherwise, execute step S210.

[0112] Here, the obstacle avoidance condition may be satisfied when the distance between the boom and the obstacle is less than a safety threshold.

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

[0114] Step S207: planning the joint space obstacle avoidance trajectory for the intermediate path points and the target position.

[0115] Step S208, determine whether the planned segmented path meets the obstacle avoidance path requirements, if so, execute step S209, otherwise execute step S207.

[0116] Here, the obstacle avoidance path requirements may specifically refer to the multi-objective constraint conditions in the aforementioned embodiment.

[0117] Step S209: Complete 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 moving path until the end of the boom reaches the target position.

[0119] Based on the same inventive concept as the aforementioned embodiments, an embodiment of the present application provides an electronic device, comprising a processor, a memory, and a computer program stored in the memory and executable on the processor, wherein the computer program, when executed by the processor, implements the multi-joint mechanism control method as described above.

[0120] Based on the same inventive concept as the aforementioned embodiment, this embodiment also provides an operating machine, including a controller, a multi-joint mechanism, and a laser radar arranged on the multi-joint mechanism, wherein the laser radar is used to collect size data and position data of obstacles in the process of the end of the multi-joint mechanism moving toward the target position, and the controller implements the multi-joint mechanism control method as described above when executing a computer program.

[0121] Based on the same inventive concept as the aforementioned embodiment, this embodiment further provides a computer-readable storage medium, in which a computer program is stored. When the computer program is executed by a processor, the multi-joint mechanism control method as described above is implemented.

[0122] The technical features of the above-described embodiments may be arbitrarily combined. To make the description concise, not all possible combinations of the technical features in the above-described 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 "comprises," "comprising," or any other variations thereof, are intended to cover a non-exclusive inclusion of elements other than those listed and may also include additional elements not expressly listed.

[0124] The above is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art who is familiar with the present technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the attached claims.

Claims

1. A multi-joint mechanism control method, characterized in that: The method comprises: In response to detecting that an obstacle avoidance trigger condition is satisfied during the process in which the end of the multi-joint mechanism moves toward the target position, determining an obstacle avoidance intermediate path point according to size data and position data of the obstacle and the target position; Based on preset posture constraints and an inverse kinematic iterative solution algorithm, a first target posture of the multi-joint mechanism is determined when the end of the multi-joint mechanism reaches the intermediate path point of the obstacle avoidance. The first target posture includes a first angle sequence composed of the angles corresponding to the joints in the multi-joint mechanism.

2. The multi-joint mechanism control method according to claim 1, characterized in that: The preset posture constraint conditions include iteration end conditions and angle constraint conditions corresponding to each of the joints; The method of determining the first target posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the obstacle bypassing intermediate path point based on the preset posture constraint condition and the inverse motion iterative solution algorithm comprises: Step S10, according to the position deviation between the expected position of the end of the multi-joint mechanism and the intermediate path point of the obstacle avoidance calculated in the previous iteration process, the angle increment corresponding to each joint in the current iteration process is obtained based on the inverse kinematic iterative solution algorithm; Step S11, determining the calculated angles corresponding to the joints in the current iteration according to the angle increments corresponding to the joints and the expected angles corresponding to the joints calculated in the previous iteration; Step S12, optimizing the calculated angle based on the angle restriction condition to obtain the expected angles corresponding to the joints in the current iteration process; Step S13, determining the expected position of the end of the multi-joint mechanism in the current iteration process according to the expected angles corresponding to the joints in the current iteration process; Step S14: Check whether the iteration end condition is met, if not, return to step 10, if yes, execute step S15; Step S15, determining a first target posture of the multi-joint mechanism when the end of the multi-joint mechanism reaches the intermediate path point of the obstacle avoidance according to the expected angles corresponding to the joints in the current iteration process.

3. The multi-joint mechanism control method according to claim 2, characterized in that: The angle restriction conditions include angle range intervals and singular angle intervals corresponding to each joint; The iteration end condition includes that the number of inverse motion iterations is greater than a preset number and / or the position deviation is less than a preset threshold.

4. The multi-joint mechanism control method according to claim 1, characterized in that: In response to detecting that an obstacle avoidance trigger condition is satisfied during the process of the end of the multi-joint mechanism moving toward the target position, before determining the obstacle avoidance intermediate path point according to the size data and position data of the obstacle and the target position, the method includes: Determining a target position of an end of the multi-joint mechanism; Based on the preset posture constraint condition and the inverse kinematic 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 angles corresponding to each of the joints; Planning a first moving path of the end of the multi-joint mechanism from an initial position to the target position according to the initial posture of the multi-joint mechanism and the second target posture; 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.

5. The multi-joint mechanism control method according to any one of claims 1 to 4, characterized in that: In response to detecting that an obstacle avoidance trigger condition is satisfied during the process of the end of the multi-joint mechanism moving toward the target position, before determining the obstacle avoidance intermediate path point according to the size data and position data of the obstacle and the target position, the method includes: Acquire a control mode of the multi-joint mechanism, wherein the control mode includes any one of the following: an arm-retracting mode and an arm-extending mode; The target position of the end of the multi-joint mechanism is determined according to the control mode.

6. The multi-joint mechanism control method according to claim 4, characterized in that: The method further comprises: Based on the current posture of the multi-joint mechanism, the first target posture and the second target posture, combined with the target cost function and multi-target constraints, a second moving path of the end of the multi-joint mechanism starting from the current position, passing through the obstacle-avoiding intermediate path point to the target position is planned; the target cost function includes a shortest path cost function and a non-collision cost function.

7. The multi-joint mechanism control method according to claim 6, characterized in that: The multi-objective constraint condition includes at least one of the following: The path length is the shortest; The minimum height of the path is greater than or equal to the height threshold; The maximum distance between the path and the obstacle is less than or equal to the distance threshold.

8. 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, the multi-joint mechanism control method as described in any one of claims 1 to 7 is implemented.

9. An operating machine, comprising a controller, a multi-joint mechanism and a laser radar arranged on the multi-joint mechanism, wherein 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 toward a target position, and when the controller executes a computer program, it implements the multi-joint mechanism control method as described in any one of claims 1 to 7.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the multi-joint mechanism control method as claimed in any one of claims 1 to 7 is implemented.

Images