Quadruped single-arm robot cooperative motion control method and system

Abstract

The invention provides a four-foot single-arm robot cooperative motion control method and system, and relates to the field of four-foot single-arm robot planning and control, and the method comprises the steps: building a multi-rigid-body dynamic model and a single-rigid-body reduced-order model as model constraints; screening out a feasible landing area; a safe landing area is generated; formulating a landing point cost function, and determining an optimal landing point; discretizing a state transition equation of the quadruped single-arm robot; taking steps capable of spanning as inequality constraints, taking the optimal landing point as elevation map equality constraints, and introducing the inequality constraints and the optimal landing point into a model prediction controller; based on a multi-rigid-body dynamic model, a multi-task planning method is optimized, control strategies are reasonably distributed, conflict tasks of four-foot movement and mechanical arm operation are dynamically coordinated, and cooperative control is conducted on multiple tasks. And fusing target data of a depth camera at the tail end of the mechanical arm, and autonomously planning a posture adjustment path of the robot body. The cooperative grabbing stability and accuracy of the robot are remarkably improved.

Description

Technical Field

[0001] The present invention relates to the field of planning and control of a quadruped single-arm robot, and in particular to a method and system for coordinated motion control of a quadruped single-arm robot. Background Art

[0002] The collaborative motion control method of a quadruped and single-arm robot refers to a control strategy that coordinates the motion of a quadruped robot and the action of a single-arm robot so that they can work together and complete complex tasks. A quadruped robot usually refers to a mobile robot with four leg support points that can simulate the walking posture of animals and has strong maneuverability and stability, while a single-arm robot usually refers to a robot with one mechanical arm that can perform tasks such as grasping and carrying.

[0003] By installing a robotic arm on a quadruped robot, not only can the working space of the robotic arm be expanded, but the robot can also be given the ability to perform operational tasks. However, with the introduction of the robotic arm, the overall dynamics and control complexity of the robot increase significantly. In addition, the lack of elevation map perception information greatly restricts the ability of the quadruped single-arm robot to perform dynamic operational tasks in complex terrain. Therefore, a collaborative motion control method for a quadruped single-arm robot is urgently needed.

[0004] However, traditional collaborative control methods rely on decoupling control strategies, which usually separate the control tasks of the quadruped robot and the robotic arm. When performing complex tasks, mutual interference may occur, and there is a lack of real-time perception and processing capabilities for complex terrain. It is impossible to fully utilize terrain information to dynamically adjust the foot position or optimize the grasping path, resulting in insufficient stability of the robot in complex terrain or dynamic tasks. Summary of the invention

[0005] In order to solve the technical problem that traditional collaborative control methods rely on decoupling control strategies, the control tasks of the quadruped robot and the robotic arm are usually performed separately, which may cause mutual interference when performing complex tasks, and lack the real-time perception and processing capabilities of complex terrains, and cannot fully utilize terrain information to dynamically adjust the foot position or optimize the grasping path, resulting in insufficient stability of the robot under complex terrain or dynamic tasks. The present invention provides a collaborative motion control method and system for a quadruped single-arm robot.

[0006] The technical solution provided by the embodiment of the present invention is as follows:

[0007] First aspect:

[0008] An embodiment of the present invention provides a method for controlling the coordinated motion of a quadruped single-arm robot, comprising:

[0009] S1: Establish the whole machine multi-rigid body dynamics model of the quadruped single-arm robot and the single rigid body reduced-order model as model constraints;

[0010] Wherein, the multi-rigid body dynamics model is used for whole body controller calculation, and the single rigid body reduced-order model is used for real-time predictive control;

[0011] S2: Generate a local elevation map by fusing the forward-looking depth camera with the IMU, rasterize the elevation map based on the slope threshold and curvature filter, and set terrain feasibility constraints to screen out feasible landing areas;

[0012] S3: Formulate a terrain cost function and corresponding constraints as visual constraints, combine the slope standard deviation and the height mean of the elevation map, and generate a safe landing area in the feasible landing area;

[0013] S4: formulating a foothold cost function, and determining an optimal foothold in the safe foothold area based on the elevation map;

[0014] S5: Based on the single rigid body reduced-order model, the state transfer equation of the quadruped single-arm robot is discretized;

[0015] S6: using the traversable steps selected in step S2 according to the current foothold and the foothold without prior terrain as inequality constraints, and using the optimal foothold selected in step S4 as the elevation map equality constraint, and introducing them into the model predictive controller to optimize the motion cycle including the support phase;

[0016] S7: Based on the multi-rigid body dynamics model, optimize the multi-task planning method based on the null space Jacobian matrix, reasonably allocate the control strategy, dynamically coordinate the conflicting tasks of quadruped motion and manipulator operation, and perform collaborative control of multiple tasks;

[0017] S8: The rasterized elevation map is used as the grasping constraint condition, and the target data of the depth camera at the end of the robotic arm is integrated to autonomously plan the posture adjustment path of the robot body.

[0018] Second aspect:

[0019] An embodiment of the present invention provides a quadruped single-arm robot collaborative motion control system, comprising:

[0020] processor;

[0021] A memory having computer-readable instructions stored thereon, wherein when the computer-readable instructions are executed by the processor, the method for controlling the coordinated motion of a quadruped single-arm robot as described in the first aspect is implemented.

[0022] The third aspect:

[0023] An embodiment of the present invention provides a computer-readable storage medium, on which a computer program is stored. When the program is executed by a processor, the method for controlling the coordinated motion of a quadruped single-arm robot as described in the first aspect is implemented.

[0024] The beneficial effects brought about by the technical solution provided by the embodiment of the present invention include at least:

[0025] In the embodiment of the present invention, by establishing a multi-rigid body dynamics model and a single rigid body reduced-order model, the robot's motion behavior in complex terrain can be accurately described, the response speed and accuracy of the control system are improved, and by generating a high-precision local elevation map and performing efficient rasterization processing according to the slope threshold and curvature, it can sense and adapt to terrain changes in real time, effectively identify feasible landing areas, thereby ensuring stability and efficient movement in complex terrain. By combining the slope standard deviation and height mean of the elevation map, a safe landing area is generated, which can improve the robot's stability in complex terrain and avoid falls or other operations caused by uneven ground or dangerous areas. In case of task failure, by determining the optimal foothold, the best foot position can be dynamically selected to improve the robot's gait stability. By introducing the elevation map equation and inequality constraints into the model predictive control, the robot's stability and task execution efficiency in a dynamic environment can be improved. By optimizing the multi-task planning method, the task priority can be reasonably allocated to ensure the task coordination between the quadruped robot and the robotic arm, so that the robot can maintain efficiency and stability when performing multiple tasks and avoid conflicts between tasks. By fusing the elevation map and the depth camera data at the end of the robotic arm, dynamic adjustment of the grasping path is realized, which significantly improves the grasping success rate in complex terrain. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0027] Figure 1 A schematic flow chart of a method for controlling the coordinated motion of a quadruped single-arm robot provided by an embodiment of the present invention;

[0028] Figure 2 A schematic diagram of a coordinate system of a local map provided by an embodiment of the present invention;

[0029] Figure 3 A schematic diagram of searching for the best landing point provided by an embodiment of the present invention;

[0030] Figure 4A diagram of a stair climbing simulation experiment based on a coupling architecture of elevation map constraints and model predictive control provided in an embodiment of the present invention;

[0031] Figure 5 A diagram of a "model prediction-whole body control" hierarchical collaborative motion simulation experiment provided by an embodiment of the present invention;

[0032] Figure 6 A schematic structural diagram of a quadruped single-arm robot collaborative motion control system provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0033] The technical solution of the present invention is described below in conjunction with the accompanying drawings.

[0034] In the embodiments of the present invention, words such as "exemplarily" and "for example" are used to indicate examples, illustrations or explanations. Any embodiment or design described as "example" in the present invention should not be interpreted as being more preferred or more advantageous than other embodiments or designs. Specifically, the use of the word "example" is intended to present the concept in a specific way. In addition, in the embodiments of the present invention, the meaning expressed by "and / or" can be both, or it can be either of the two.

[0035] In the embodiments of the present invention, "image" and "picture" can sometimes be used interchangeably. It should be noted that when the difference between them is not emphasized, the meanings they intend to express are the same. "of", "corresponding, relevant" and "corresponding" can sometimes be used interchangeably. It should be noted that when the difference between them is not emphasized, the meanings they intend to express are the same.

[0036] In the embodiments of the present invention, sometimes a subscript such as W1 may be written as a non-subscript such as W1. When the difference is not emphasized, the meanings to be expressed are consistent.

[0037] In order to make the technical problems, technical solutions and advantages to be solved by the present invention more clear, a detailed description will be given below with reference to the accompanying drawings and specific embodiments.

[0038] Reference Manual Attached Figure 1 , showing a flow chart of a method for collaborative motion control of a quadruped single-arm robot provided in an embodiment of the present invention.

[0039] The embodiment of the present invention provides a method for controlling the coordinated motion of a quadruped single-arm robot, which can be implemented by a device for controlling the coordinated motion of a quadruped single-arm robot, and the device for controlling the coordinated motion of a quadruped single-arm robot can be a terminal or a server. The processing flow of the method for controlling the coordinated motion of a quadruped single-arm robot can include the following steps:

[0040] S1: Establish the whole machine multi-rigid body dynamics model of the quadruped single-arm robot and the single rigid body reduced-order model as model constraints.

[0041] It should be noted that establishing the whole-machine multi-rigid body dynamics model and single rigid body reduced-order model of the quadruped single-arm robot can accurately describe the overall and local motion characteristics of the robot, realize comprehensive simulation of complex dynamics through the multi-rigid body model, and improve the accuracy of motion control, while the single rigid body reduced-order model improves the computational efficiency, making real-time control and prediction more efficient.

[0042] Among them, the multi-rigid body dynamics model is used for whole-body controller calculation, and the single rigid body reduced-order model is used for real-time predictive control.

[0043] The model constraints are as follows:

[0044] ;

[0045] Where M(q) represents the joint space mass matrix, q represents the joint space quantity, represents the second-order derivative of the joint space quantity q, represents the bias force vector, represents the first-order derivative of the joint space quantity q, S j represents the selection matrix of the j-th supporting leg, τ represents the generalized joint driving torque, J c represents the contact Jacobian matrix, f c represents the contact force vector, T represents the transposition operation, m represents the total mass of the whole machine, , n represents the total number of supporting legs, represents the inertia tensor, represents the angular velocity of the whole machine, represents the acceleration of the center of mass, g represents the acceleration due to gravity, Indicates the jth support leg in the world coordinate system The foot reaction below, Represents the position vector from the center of mass to the foot end of the jth supporting leg.

[0046] S2: Generate a local elevation map by fusing the forward-looking depth camera with the IMU, rasterize the elevation map based on the slope threshold and curvature filtering, and set terrain feasibility constraints to screen out feasible landing areas.

[0047] Among them, the forward-looking depth camera is a camera that uses depth sensing technology to measure the distance of objects in the scene. It can provide three-dimensional information of objects and is widely used in robot perception. The IMU (inertial measurement unit) is a sensor that contains an accelerometer, gyroscope, and magnetometer. It is used to measure the acceleration, angular velocity, and direction changes of the robot to help provide motion information. The local elevation map is an image generated based on the three-dimensional data obtained by the depth camera, in which each pixel value represents the height of the location and is used to describe the changes in the terrain. Slope threshold and curvature filtering use slope and curvature calculations to filter and process the data in the elevation map to ensure the accuracy and stability of the data.

[0048] It should be noted that by fusing the forward-looking depth camera with the IMU to generate a local elevation map and rasterize it, detailed information on complex terrain can be efficiently captured, providing the robot with real-time environmental perception. Setting terrain feasibility constraints can screen out safe landing areas, effectively preventing the robot from becoming unstable due to terrain intolerance, and improving the robot's autonomous navigation and adaptability, enabling it to stably perform tasks in complex environments.

[0049] In a possible implementation, the elevation map is rasterized as follows:

[0050] ;

[0051] Among them, G i represents the elevation map of the i-th grid, x i Indicates the x-direction position coordinate of the i-th grid, y i Represents the y-direction position coordinate of the i-th grid, Represents the estimated terrain height of the ith grid.

[0052] In a possible implementation, the screening conditions for feasible landing areas are specifically:

[0053] ;

[0054] ;

[0055] Among them, h obstacle represents the height of the obstacle point, p t represents the foothold at time t, p t-1,stance represents the foothold at time t-1, p t,z represents the z-axis height of the foothold at time t, h max represents the maximum step height, p t-1,stance,z represents the z-axis height of the landing point at time t-1, h represents the height of the elevation map, Indicates the average height of the elevation map. represents roughness, s represents slope, s(h) represents the slope of the elevation map, s(h)max Indicates the maximum slope threshold.

[0056] In a possible implementation, the maximum step height range is: ; The value range of the maximum slope threshold is: .

[0057] Reference Manual Attached Figure 2 , showing a schematic diagram of a coordinate system of a local map provided in an embodiment of the present invention.

[0058] like Figure 2 As shown in the figure, there are multiple coordinate systems, namely: robot base coordinate system {O0}, forward-looking depth camera coordinate system {O1}, and map coordinate system {M}. The forward-looking depth camera in the figure shows its observation range through the blue dotted line, and the distance from {O1} to the target point is shown by the dotted line. The purpose of this figure is to show how to obtain terrain data through the forward-looking depth camera and the unknown depth camera and use it to plan the movement of the quadruped robot.

[0059] S3: Formulate a terrain cost function and corresponding constraints as visual constraints, combine the slope standard deviation and height mean of the elevation map, and generate a safe landing area in the feasible landing area.

[0060] It should be noted that by formulating a terrain cost function and combining the slope standard deviation and height mean of the elevation map, it is possible to accurately analyze terrain features and identify stable areas as safe landing areas, allowing the robot to avoid unstable or dangerous terrain and ensure stability and efficient movement in complex environments. Through the guidance of visual constraints, the robot's autonomous navigation capability in dynamic environments is improved, ensuring the safety and reliability of task execution.

[0061] In a possible implementation, S3 specifically includes:

[0062] S301: Calculate the score of the foothold in the elevation map:

[0063] ;

[0064] Among them, s f represents the foothold score, s(h) represents the slope of the elevation map, σ represents the standard deviation calculation, represents the average value of the slope of the elevation map, h represents the height of the elevation map, It represents the average height of the elevation map, and λ1, λ2, and λ3 represent the weights of the edge term, slope term, and roughness, respectively.

[0065] S302: Determine a safe landing area based on the landing point score.

[0066] When the landing point score is close to 0, it corresponds to a flat area and the landing point is a safe landing point.

[0067] When the landing point score is close to 1, it corresponds to an edge, steep slope or rough area, and the landing point is a dangerous landing point.

[0068] In a possible implementation, the weights of the edge term, the slope term, and the roughness satisfy: ; The value range of λ1 is [0, 1]; the value range of λ2 is [0, 0.8]; the value range of λ3 is [0, 0.5].

[0069] Reference Manual Attached Figure 3 , showing a schematic diagram of the optimal landing point search provided by an embodiment of the present invention.

[0070] like Figure 3 As shown in the figure, the robot model is on the left and an obstacle environment with stairs is shown on the right. The robot selects the best landing point by analyzing the terrain. Several key points are marked in the figure, among which P hip is the current position of the robot's hip, p n Indicates no prior terrain foothold, S i represents the search space range, The dotted line represents the potential path of the robot from the current footstep to the target footstep. This process helps the robot choose the most appropriate footstep to ensure smooth progress by considering terrain obstacles and gait stability.

[0071] S4: Develop a foothold cost function to determine the optimal foothold in the safe foothold area based on the elevation map.

[0072] It should be noted that by formulating a foothold cost function and combining it with an elevation map to select the optimal foothold, the robot can effectively avoid choosing unstable or unsuitable terrain for landing. The robot can dynamically adjust its pace according to the actual terrain, thereby improving its stability and adaptability in complex terrain.

[0073] In a possible implementation manner, S4 is specifically:

[0074] Select the foothold that meets the following conditions within the search space as the optimal foothold:

[0075] ;

[0076] in, represents the optimal foothold at the i-th moment, represents the distance term weight, p i represents the foothold at the ith moment, p n Indicates that there is no prior terrain foothold, is the score item weight, s f S represents the landing point score, i Indicates the search space range, which is within the safe landing area.

[0077] S5: Discretize the state transition equation of the quadruped single-arm robot based on the single rigid body reduced-order model.

[0078] It should be noted that the discretization of the state transfer equation based on the single rigid body reduced-order model effectively simplifies the complex dynamic system, enabling the robot to calculate and predict its motion state more quickly when performing real-time control tasks, reducing the computational burden of the system and improving the response speed and stability of the quadruped single-arm robot in complex environments.

[0079] The state transfer equation of the discretized quadruped single-arm robot is specifically:

[0080] ;

[0081] Among them, ΔT represents the control period, I represents the unit matrix, A represents the state coefficient matrix, B represents the control coefficient matrix, represents the control state sequence at time k, u(k) represents the control input sequence at time k, A k represents the discrete state transfer matrix, B k represents the discrete control matrix.

[0082] The discrete state transfer matrix is ​​specifically:

[0083] ;

[0084] Among them, R Z Represents the rotation matrix around the Z axis, represents the desired angle of the kth joint, d represents the gravity direction matrix, , I3 represents the 3×3 identity matrix.

[0085] The discrete control matrix is ​​specifically:

[0086] ;

[0087] Among them, r1 represents the vector from the right front leg foot to the center of mass, r2 represents the vector from the left front leg foot to the center of mass, r3 represents the vector from the right hind leg foot to the center of mass, and r4 represents the vector from the left front leg foot to the center of mass. Indicates the world coordinate system The inverse matrix of the moment of inertia, .

[0088] Reference Manual Attached Figure 4, showing a stair climbing simulation experiment diagram based on a coupling architecture of elevation map constraints and model predictive control provided by an embodiment of the present invention.

[0089] S6: The traversable steps selected in step S2 according to the current foothold and the foothold without prior terrain are used as inequality constraints, and the optimal foothold selected in step S4 is used as the elevation map equality constraint, which are introduced into the model predictive controller to optimize the motion cycle including the support phase.

[0090] It should be noted that by introducing inequality constraints and elevation map equality constraints into the model predictive controller, the motion cycle of the support phase can be dynamically optimized, more accurate foothold selection and motion planning can be achieved, the robot's terrain adaptability and real-time control accuracy are improved, and its efficient movement and task completion in different environments are ensured.

[0091] In a possible implementation, the optimization problem of the model predictive controller is specifically:

[0092] ;

[0093] Among them, min means minimization, J represents the optimization objective function, U represents the optimal control input sequence, A qp represents the state coefficient matrix, x(0) represents the robot's initial state matrix, B qp represents the control coefficient matrix, D represents the state trajectory sequence, Q represents the state weight, T represents the matrix transpose operation, and R represents the input weight.

[0094] The friction cone constraint conditions of the foot-end environment contact point of the model predictive controller are as follows:

[0095] ;

[0096] Among them, D k represents the expected state of the system at time k, u k represents the control input sequence at time k, represents the minimum moment at time k, C k represents the friction cone matrix at time k, represents the maximum torque at time k.

[0097] The elevation graph equality and inequality constraints of the model predictive controller are as follows:

[0098] ;

[0099] ;

[0100] in, represents the optimal foothold at the i-th moment, represents the distance term weight, p i represents the foothold at the ith moment, p n Indicates that there is no prior terrain foothold, is the score item weight, s f S represents the landing point score, i Represents the search space range, which is within the safe landing area. obstacle represents the height of the obstacle point, p t represents the foothold at time t, p t-1,stance represents the foothold at time t-1, p t,z represents the z-axis height of the foothold at time t, h max represents the maximum step height, p t-1,stance,z represents the z-axis height of the landing point at time t-1, h represents the height of the elevation map, Indicates the average height of the elevation map. represents roughness, s represents slope, and s(h) represents the slope of the elevation map.

[0101] Reference Manual Attached Figure 5 , showing a diagram of a “model prediction-whole body control” hierarchical collaborative motion simulation experiment provided by an embodiment of the present invention.

[0102] S7: Based on the multi-rigid body dynamics model, optimize the multi-task planning method based on the zero-space Jacobian matrix, reasonably allocate control strategies, dynamically coordinate the conflicting tasks of quadruped motion and robotic arm operation, and perform collaborative control of multiple tasks.

[0103] Among them, the null space Jacobian matrix is ​​a mathematical tool that uses the robot's Jacobian matrix to analyze the degrees of freedom in the robot's motion and find the degrees of freedom that do not affect the existing tasks to perform other operations. It is usually used in multi-task coordination.

[0104] It should be noted that by optimizing multi-task planning based on a multi-rigid body dynamics model and the zero-space Jacobian matrix, different tasks can be efficiently coordinated while ensuring the stability of the robot's motion, avoiding control failures caused by task conflicts. By dynamically adjusting the control strategy, the gait control and robotic arm operation of the quadruped robot are effectively balanced, enabling the robot to perform tasks such as movement and grasping at the same time, thereby improving the overall efficiency and flexibility of the system and ensuring that the robot can complete multiple tasks in a complex environment and maintain an efficient and stable working state.

[0105] In the present invention, the task priorities of the whole body controller are allocated as follows: first priority: supporting leg trajectory tracking task to ensure the stability of the foot contact force; second priority: floating base height task; third priority: swing leg trajectory tracking task, according to the swing phase constrained by the elevation map, the trajectory deviation under the under-actuated degree of freedom can be suppressed; fourth priority: floating base rotation task to achieve fuselage posture tracking; fifth priority: robotic arm end linear motion task; sixth priority: robotic arm end rotation task, the whole body dynamics equation obtains the desired joint torque , the expected joint position , expected joint angular velocity and the expected joint acceleration .

[0106] To compensate for the uncertainty of the leg model, the joint torque is sent through the underlying feedforward torque and PD control in the torque applied to the system:

[0107] ;

[0108] in, represents the jth desired joint torque, τ j represents the jth current joint torque, represents the jth desired joint angle, q j represents the jth current joint angle, represents the expected joint angular velocity of the jth joint, represents the jth current joint angular velocity, k p and k d Both represent the proportional and derivative adjustment parameters of the underlying joint PD controller.

[0109] S8: The rasterized elevation map is used as the grasping constraint condition, and the target data of the depth camera at the end of the robotic arm is integrated to autonomously plan the posture adjustment path of the robot body.

[0110] It should be noted that by using the rasterized elevation map as the grasping constraint condition and combining it with the data from the depth camera at the end of the robotic arm, the posture and path of the robot body can be adjusted in real time, allowing the robot to flexibly respond to environmental changes and ensure that the grasping task can be successfully completed in complex terrain, thereby improving the robot's adaptability, accuracy and reliability of task completion.

[0111] In a possible implementation, S8 specifically includes:

[0112] S801: Trigger the grasping task through the handle and start the grasping state machine.

[0113] S802: Detect the target position based on YOLOv5, and exit the capture if the target is not in the field of view.

[0114] S803: First determine whether the vertical height meets the requirements , z t Indicates the vertical height of the target, z max Indicates the maximum vertical height; if so, call the elevation map data to perform terrain correction, identify the climbable terrain features around the target point through the elevation map, plan the robot body posture adjustment path, and re-detect the target vertical height after the posture adjustment is completed.

[0115] S804: Continue to determine whether the vertical height meets the requirement ; If so, reselect the target point, manually intervene or report the error; otherwise, proceed to the next step.

[0116] S805: Determine vertical height After that, determine whether the horizontal distance meets , x t Indicates the horizontal distance of the target, x max Indicates the maximum horizontal height; if so, approach the target point through walking gait movement; otherwise, enter standing grasping.

[0117] S806: During the walking gait movement approach process, the horizontal distance and vertical height are continuously detected until the conditions are met, and then the standing state is switched to perform grasping.

[0118] In the present invention, the path correction process of the quadruped robot when performing a dynamic grasping task is specifically as follows: the robot first sends a grasping task signal through the handle and confirms the target. During the grasping task, the robot identifies the target through the forward-looking depth camera and the YOLOv5 target detection model. If the target is not identified, the robot will exit the grasping task and report failure. After the target is identified, the robot performs terrain analysis based on the elevation map data, adjusts the posture and corrects the grasping path. If it is detected that the vertical height of the target does not meet the requirements, the robot will perform terrain correction and adjust the posture, and recheck the height of the target. Subsequently, the robot continues to determine whether the horizontal distance needs to be adjusted. If the conditions are met, it executes a gait approaching the target and enters the grasping state. Through this path correction method, the robot can dynamically adjust its path to adapt to environmental changes, thereby improving the success rate and adaptability of grasping tasks.

[0119] In general, the system starts with command input, first obtains environmental information based on target grasping information and elevation map data, and then calculates the expected trajectory through the model predictive control module, including the expected position, speed and state. The system plans the grasping task based on the expected position and target information, and corrects the robot's movement in real time. The robot coordinates the movements of the quadruped robot and the robotic arm through the whole body control and motor control modules. The whole body control module is responsible for coordinating the robot's gait control, while the motor control module accurately controls the robot's joints to ensure the robot's motion stability and task execution efficiency. Through efficient task planning, state feedback and model predictive control, this process can enable the robot to complete precise multi-task collaborative operations in complex terrain, improving the robot's adaptability in dynamic environments and the success rate of grasping tasks.

[0120] The beneficial effects brought about by the technical solution provided by the embodiment of the present invention include at least:

[0121] In the embodiment of the present invention, by establishing a multi-rigid body dynamics model and a single rigid body reduced-order model, the robot's motion behavior in complex terrain can be accurately described, the response speed and accuracy of the control system are improved, and by generating a high-precision local elevation map and performing efficient rasterization processing according to the slope threshold and curvature, it can sense and adapt to terrain changes in real time, effectively identify feasible landing areas, thereby ensuring stability and efficient movement in complex terrain. By combining the slope standard deviation and height mean of the elevation map, a safe landing area is generated, which can improve the robot's stability in complex terrain and avoid falls or other operations caused by uneven ground or dangerous areas. In case of task failure, by determining the optimal foothold, the best foot position can be dynamically selected to improve the robot's gait stability. By introducing the elevation map equation and inequality constraints into the model predictive control, the robot's stability and task execution efficiency in a dynamic environment can be improved. By optimizing the multi-task planning method, the task priority can be reasonably allocated to ensure the task coordination between the quadruped robot and the robotic arm, so that the robot can maintain efficiency and stability when performing multiple tasks and avoid conflicts between tasks. By fusing the elevation map and the depth camera data at the end of the robotic arm, dynamic adjustment of the grasping path is realized, which significantly improves the grasping success rate in complex terrain.

[0122] Reference Manual Attached Figure 6 , showing a structural schematic diagram of a quadruped single-arm robot collaborative motion control system provided by the present invention.

[0123] The present invention further provides a quadruped single-arm robot coordinated motion control system 20, which is applied to the above-mentioned quadruped single-arm robot coordinated motion control method, comprising:

[0124] Processor 201.

[0125] The memory 202 stores computer-readable instructions. When the computer-readable instructions are executed by the processor 201, the coordinated motion control method of the quadruped single-arm robot in the method embodiment is implemented.

[0126] The quadruped single-arm robot collaborative motion control system 20 provided by the present invention can execute the above-mentioned quadruped single-arm robot collaborative motion control method and achieve the same or similar technical effects. To avoid repetition, the present invention will not go into details.

[0127] The beneficial effects brought about by the technical solution provided by the embodiment of the present invention include at least:

[0128] In the embodiment of the present invention, by establishing a multi-rigid body dynamics model and a single rigid body reduced-order model, the robot's motion behavior in complex terrain can be accurately described, the response speed and accuracy of the control system are improved, and by generating a high-precision local elevation map and performing efficient rasterization processing according to the slope threshold and curvature, it can sense and adapt to terrain changes in real time, effectively identify feasible landing areas, thereby ensuring stability and efficient movement in complex terrain. By combining the slope standard deviation and height mean of the elevation map, a safe landing area is generated, which can improve the robot's stability in complex terrain and avoid falls or other operations caused by uneven ground or dangerous areas. In case of task failure, by determining the optimal foothold, the best foot position can be dynamically selected to improve the robot's gait stability. By introducing the elevation map equation and inequality constraints into the model predictive control, the robot's stability and task execution efficiency in a dynamic environment can be improved. By optimizing the multi-task planning method, the task priority can be reasonably allocated to ensure the task coordination between the quadruped robot and the robotic arm, so that the robot can maintain efficiency and stability when performing multiple tasks and avoid conflicts between tasks. By fusing the elevation map and the depth camera data at the end of the robotic arm, dynamic adjustment of the grasping path is realized, which significantly improves the grasping success rate in complex terrain.

[0129] It should be understood that the processor in the embodiment of the present invention may be a central processing unit (CPU), and the processor may also be other general-purpose processors, digital signal processors (DSP), application specific integrated circuits (ASIC), field programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0130] It should also be understood that the memory in the embodiments of the present invention may be a volatile memory or a non-volatile memory, or may include both volatile and non-volatile memories. Among them, the non-volatile memory may be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), or a flash memory. The volatile memory may be a random access memory (RAM), which is used as an external cache. By way of example and not limitation, many forms of random access memory (RAM) are available, such as static RAM (SRAM), dynamic random access memory (DRAM), synchronous DRAM (SDRAM), double data rate synchronous dynamic random access memory (DDR SDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous link DRAM (SLDRAM), and direct rambus RAM (DR RAM).

[0131] The above embodiments can be implemented in whole or in part by software, hardware (such as circuits), firmware or any other combination. When implemented by software, the above embodiments can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions or computer programs. When a computer instruction or computer program is loaded or executed on a computer, a process or function according to an embodiment of the present invention is generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. Computer instructions can be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, computer instructions can be transmitted from one website, computer, server or data center to another website, computer, server or data center by wired (such as infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server or data center that contains one or more available media sets. The available medium can be a magnetic medium (for example, a floppy disk, a hard disk, a tape), an optical medium (for example, a DVD), or a semiconductor medium. The semiconductor medium can be a solid-state hard disk.

[0132] It should be understood that the term "and / or" in this article is only a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. A and B can be singular or plural. In addition, the character " / " in this article generally indicates that the associated objects before and after are in an "or" relationship, but it may also indicate an "and / or" relationship. Please refer to the context for specific understanding.

[0133] In the present invention, "at least one" means one or more, and "plurality" means two or more. "At least one of the following" or similar expressions refers to any combination of these items, including any combination of single items or plural items. For example, at least one of a, b, or c can mean: a, b, c, ab, ac, bc, or abc, where a, b, and c can be single or multiple.

[0134] It should be understood that in various embodiments of the present invention, the size of the serial numbers of the above-mentioned processes does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0135] Those skilled in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0136] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the above-described equipment, devices and units can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0137] In the several embodiments provided by the present invention, it should be understood that the disclosed devices, apparatuses and methods can be implemented in other ways. For example, the device embodiments described above are only schematic, for example, the division of units is only a logical function division, and there may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another device, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0138] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0139] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit.

[0140] If the function is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several instructions for a computer device (which can be a personal computer, server, or network device, etc.) to perform all or part of the steps of the methods of various embodiments of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM), random access memory (RAM), disk or optical disk, etc., various media that can store program codes.

[0141] An embodiment of the present invention provides a computer-readable storage medium having a computer program stored thereon. When the program is executed by a processor, the method for controlling the coordinated motion of a quadruped single-arm robot according to the method embodiment is implemented.

[0142] A computer-readable storage medium provided by the present invention can implement the steps and effects of the quadruped single-arm robot collaborative motion control method of the above method embodiment. To avoid repetition, the present invention will not go into details.

[0143] The beneficial effects brought about by the technical solution provided by the embodiment of the present invention include at least:

[0144] In the embodiment of the present invention, by establishing a multi-rigid body dynamics model and a single rigid body reduced-order model, the robot's motion behavior in complex terrain can be accurately described, the response speed and accuracy of the control system are improved, and by generating a high-precision local elevation map and performing efficient rasterization processing according to the slope threshold and curvature, it can sense and adapt to terrain changes in real time, effectively identify feasible landing areas, thereby ensuring stability and efficient movement in complex terrain. By combining the slope standard deviation and height mean of the elevation map, a safe landing area is generated, which can improve the robot's stability in complex terrain and avoid falls or other operations caused by uneven ground or dangerous areas. In case of task failure, by determining the optimal foothold, the best foot position can be dynamically selected to improve the robot's gait stability. By introducing the elevation map equation and inequality constraints into the model predictive control, the robot's stability and task execution efficiency in a dynamic environment can be improved. By optimizing the multi-task planning method, the task priority can be reasonably allocated to ensure the task coordination between the quadruped robot and the robotic arm, so that the robot can maintain efficiency and stability when performing multiple tasks and avoid conflicts between tasks. By fusing the elevation map and the depth camera data at the end of the robotic arm, dynamic adjustment of the grasping path is realized, which significantly improves the grasping success rate in complex terrain.

[0145] The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art who is familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

[0146] There are a few points to note:

[0147] (1) The drawings of the embodiments of the present invention only involve structures related to the embodiments of the present invention. Other structures may refer to conventional designs.

[0148] (2) For the sake of clarity, in the drawings used to describe the embodiments of the present invention, the thickness of layers or regions is exaggerated or reduced, that is, these drawings are not drawn according to the actual scale. It is understood that when an element such as a layer, film, region or substrate is referred to as being "on" or "under" another element, the element may be "directly" "on" or "under" the other element or there may be intermediate elements.

[0149] (3) In the absence of conflict, the embodiments of the present invention and the features therein may be combined with each other to obtain new embodiments.

[0150] The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. The protection scope of the present invention shall be based on the protection scope of the claims.

Claims

1. A method for controlling the coordinated motion of a quadruped single-arm robot, characterized in that: include: S1: Establish the whole machine multi-rigid body dynamics model of the quadruped single-arm robot and the single rigid body reduced-order model as model constraints; Wherein, the multi-rigid body dynamics model is used for whole body controller calculation, and the single rigid body reduced-order model is used for real-time predictive control; S2: Generate a local elevation map by fusing the forward-looking depth camera with the IMU, rasterize the elevation map based on the slope threshold and curvature filter, and set terrain feasibility constraints to screen out feasible landing areas; S3: Formulate a terrain cost function and corresponding constraints as visual constraints, combine the slope standard deviation and the height mean of the elevation map, and generate a safe landing area in the feasible landing area; S4: formulating a foothold cost function, and determining an optimal foothold in the safe foothold area based on the elevation map; S5: Based on the single rigid body reduced-order model, the state transfer equation of the quadruped single-arm robot is discretized; S6: using the traversable steps selected in step S2 according to the current foothold and the foothold without prior terrain as inequality constraints, and using the optimal foothold selected in step S4 as the elevation map equality constraint, and introducing them into the model predictive controller to optimize the motion cycle including the support phase; S7: Based on the multi-rigid body dynamics model, optimize the multi-task planning method based on the null space Jacobian matrix, reasonably allocate the control strategy, dynamically coordinate the conflicting tasks of quadruped motion and manipulator operation, and perform collaborative control of multiple tasks; S8: The rasterized elevation map is used as the grasping constraint condition, and the target data of the depth camera at the end of the robotic arm is integrated to autonomously plan the posture adjustment path of the robot body.

2. The method for controlling the coordinated motion of a quadruped single-arm robot according to claim 1, characterized in that: The elevation map is rasterized as follows: ； Among them, G i represents the elevation map of the i-th grid, x i Indicates the x-direction position coordinate of the i-th grid, y i Represents the y-direction position coordinate of the i-th grid, Represents the estimated terrain height of the ith grid.

3. The method for controlling the coordinated motion of a quadruped single-arm robot according to claim 1, characterized in that: The screening conditions for the feasible landing area are specifically: ； ； Among them, h obstacle represents the height of the obstacle point, p t represents the foothold at time t, p t-1,stance represents the foothold at time t-1, p t,z represents the z-axis height of the foothold at time t, h max represents the maximum step height, p t-1,stance,z represents the z-axis height of the landing point at time t-1, h represents the height of the elevation map, Indicates the average height of the elevation map. represents roughness, s represents slope, s(h) represents the slope of the elevation map, s(h) max Indicates the maximum slope threshold.

4. The method for controlling the coordinated motion of a quadruped single-arm robot according to claim 3, characterized in that: The maximum step height has a value range of: ; The value range of the maximum slope threshold is: .

5. The method for controlling the coordinated motion of a quadruped single-arm robot according to claim 1, characterized in that: The S3 specifically includes: S301: Calculate the score of the foothold in the elevation map: ； Among them, s f represents the foothold score, s(h) represents the slope of the elevation map, σ represents the standard deviation calculation, represents the average value of the slope of the elevation map, h represents the height of the elevation map, represents the average height of the elevation map, λ1, λ2, and λ3 represent the weights of the edge term, slope term, and roughness, respectively; S302: Determine the safe landing area according to the landing point score; When the landing point score is close to 0, it corresponds to a flat area and the landing point is a safe landing point; When the landing point score is close to 1, corresponding to an edge, steep slope or rough area, the landing point is a dangerous landing point.

6. The method for controlling the coordinated motion of a quadruped single-arm robot according to claim 1, characterized in that: The weights of the edge term, slope term, and roughness satisfy: ; The value range of λ1 is [0, 1]; the value range of λ2 is [0, 0.8]; the value range of λ3 is [0, 0.5].

7. The method for controlling the coordinated motion of a quadruped single-arm robot according to claim 1, characterized in that: The S4 is specifically: Select a landing point that meets the following conditions within the search space as the optimal landing point: ； in, represents the optimal foothold at the i-th moment, represents the distance term weight, p i represents the foothold at the ith moment, p n Indicates that there is no prior terrain foothold, is the score item weight, s f S represents the landing point score, i Represents a search space range, and the search space range is within the safe landing area.

8. The method for controlling the coordinated motion of a quadruped single-arm robot according to claim 1, characterized in that: The optimization problem of the model predictive controller is specifically: ； Among them, min means minimization, J represents the optimization objective function, U represents the optimal control input sequence, A qp represents the state coefficient matrix, x(0) represents the robot's initial state matrix, B qp represents the control coefficient matrix, D represents the state trajectory sequence, Q represents the state weight, T represents the matrix transpose operation, and R represents the input weight; The friction cone constraint condition of the foot-end environment contact point of the model predictive controller is specifically: ； Among them, D k represents the expected state of the system at time k, u k represents the control input sequence at time k, represents the minimum moment at time k, C k represents the friction cone matrix at time k, represents the maximum moment at time k; The elevation graph equality and inequality constraints of the model predictive controller are specifically: ； ； in, represents the optimal foothold at the i-th moment, represents the distance term weight, p i represents the foothold at the ith moment, p n Indicates that there is no prior terrain foothold, is the score item weight, s f S represents the landing point score, i represents the search space range, and the search space range is within the safe landing area, h obstacle represents the height of the obstacle point, p t represents the foothold at time t, p t-1,stance represents the foothold at time t-1, p t,z represents the z-axis height of the foothold at time t, h max represents the maximum step height, p t-1,stance,z represents the z-axis height of the landing point at time t-1, h represents the height of the elevation map, Indicates the average height of the elevation map. represents roughness, s represents slope, and s(h) represents the slope of the elevation map.

9. The method for controlling the coordinated motion of a quadruped single-arm robot according to claim 1, characterized in that: The S8 specifically includes: S801: Trigger the grasping task through the handle and start the grasping state machine; S802: Detect the target position based on YOLOv5, and exit the capture if the target is not in the field of view; S803: First determine whether the vertical height meets the requirements , z t Indicates the vertical height of the target, z max Indicates the maximum vertical height; if so, call the elevation map data to perform terrain correction, identify the climbable terrain features around the target point through the elevation map, plan the robot body posture adjustment path, and re-detect the target vertical height after the posture adjustment is completed; S804: Continue to determine whether the vertical height meets the requirement ; If yes, reselect the target point, manually intervene or report the error; otherwise, proceed to the next step; S805: Determine vertical height After that, determine whether the horizontal distance meets , x t Indicates the horizontal distance of the target, x max Indicates the maximum horizontal height; if so, approach the target point through walking gait movement; otherwise, enter standing grasping; S806: During the walking gait movement approach process, the horizontal distance and vertical height are continuously detected until the conditions are met, and then the standing state is switched to perform grasping.

10. A quadruped single-arm robot cooperative motion control system, characterized in that: include: processor; A memory having computer-readable instructions stored thereon, wherein when the computer-readable instructions are executed by the processor, the method for controlling the coordinated motion of a quadruped single-arm robot as claimed in any one of claims 1 to 9 is implemented.

Images