Desktop mechanical arm pushing method and system for scene with obstacle

Abstract

The invention discloses a desktop mechanical arm pushing method and system for a scene with obstacles. The desktop mechanical arm pushing method comprises the steps that a mechanical arm pushing system software and hardware environment is built; using an RGBD camera to obtain desktop scene information; determining whether to introduce a global path planning algorithm according to the complexity of obstacle distribution in the desktop scene information; when the environment is complex, an intermediate point is obtained through a global path planning algorithm and serves as a current target point, the mechanical arm is controlled to push the object to move to the current target point through an MPPI algorithm, and after the mechanical arm reaches the intermediate point, the next intermediate point serves as the current target point, and so on until the object to be moved reaches the final target position; and when the environment is relatively simple, the MPPI algorithm is directly utilized to control the mechanical arm to push the to-be-moved object to move to the final target position. According to the invention, the problem that an object which is easy to deform or inconvenient to grab is moved to a target position in a pushing manner under the situation that disordered obstacles exist on a desktop can be solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot arm control, and mainly relates to a desktop robot arm pushing method and system for scenes with obstacles. Background Art

[0002] With the continuous advancement of robotic arm technology, desktop robotic arms are playing an increasingly important role in assisting people with disabilities in their daily lives. These robotic arms have greatly improved the autonomy and quality of life of people with disabilities by providing functions such as moving, grasping, and delivering daily necessities. However, in real environments, objects to be moved may be easily deformed and difficult to grasp, such as paper cups filled with water, large utensils, etc., which often become challenges for desktop robotic arms when performing mobile tasks. Existing robotic arm grasping and pushing systems often lack sufficient intelligence and flexibility to effectively identify and adapt to these characteristics, resulting in reduced service efficiency and even possible damage to the robotic arm or the environment. In addition, for desktop scenarios with other obstacles, desktop robotic arms need to have the ability to automatically identify and avoid obstacles.

[0003] In this context, it is particularly important to develop a desktop service robot arm system and method that can stably and efficiently perform pushing operations in scenarios with obstacles. Summary of the invention

[0004] Purpose of the invention: To address the problem that existing desktop mobile robotic arms cannot move easily deformed objects in complex desktop environments, a desktop robotic arm pushing system and method are provided that can stably and efficiently perform pushing operations in scenarios where obstacles exist.

[0005] Technical solution: The desktop mechanical arm pushing method for scenes with obstacles described in the present invention comprises the following steps:

[0006] Build the software and hardware environment of the robotic arm propulsion system;

[0007] Use RGBD camera to obtain desktop scene information;

[0008] Whether it is necessary to introduce a global path planning algorithm is determined according to the complexity of the obstacle distribution in the desktop scene information; when the object to be moved must bypass obstacles to reach the final target position from the initial position, global path planning is performed on the two-dimensional plane first, and the middle point is obtained through the global path planning algorithm. The middle point is used as the current target point, and the model prediction path integral control algorithm MPPI is used to control the robotic arm to push the object to the current target point. When the robotic arm reaches this middle point, the next middle point is used as the current target point, and so on until the object to be moved reaches the final target position; when the object to be moved can reach the final target position directly from the initial position along a straight line, the model prediction path integral control algorithm is directly used to control the robotic arm to push the object to be moved to the final target position.

[0009] Furthermore, the hardware and software environment of the robotic arm propulsion system is built, including:

[0010] S11, constructing the URDF model of the robot arm and the URDF model of the object to be moved;

[0011] S12, constructing the IsaacGym reinforcement learning environment, and importing the URDF model of the robot arm and the URDF model of the object to be moved into the IsaacGym reinforcement learning environment;

[0012] S13, build a physical platform driven by an RGBD camera and a robotic arm, and the field of view of the RGBD camera can completely cover the initial position of the object to be moved on the desktop;

[0013] S14, connecting the computer to the robotic arm and the RGBD camera for communication, and performing hand-eye calibration on the robotic arm and the RGBD camera; using the ROS robotic arm operating system to perform software connection and control of the computer, the robotic arm and the RGBD camera.

[0014] Furthermore, the RGBD camera and the robotic arm adopt an eyes-outside-hand approach.

[0015] Furthermore, the RGBD camera is used to obtain desktop scene information, including:

[0016] S21, RGBD camera senses and scans the scene and obstacle environment where the robot is located, and obtains the RGB image and depth point cloud image of the desktop plane information;

[0017] S22. For the RGB image, use the YOLOv7 target detection algorithm to identify the object to be moved, store the coordinates of the center point (x, y) of the object to be moved and the coordinates of each boundary point (x i ,y j );

[0018] S23. For the depth point cloud image, a statistical filter is used to denoise the point cloud, and the point cloud with a height higher than the desktop is regarded as the location of the obstacle. After removing the point cloud corresponding to the obstacle, the movable space of the target object is obtained;

[0019] S24. According to the two-dimensional coordinate information of the object to be moved obtained in step S22, extract the three-dimensional boundary data information of the object to be moved from the denoised depth point cloud data, and store the three-dimensional boundary data information.

[0020] Furthermore, the model prediction path integral control algorithm is used to control the robot arm to push the object to the current target point, including:

[0021] According to the three-dimensional boundary data information of the object to be moved, the end of the robot arm is controlled to move near the object to be moved;

[0022] In the IsaacGym reinforcement learning environment, the optimal control input sequence is obtained through the model-predictive path integral control algorithm with the help of the URDF model of the robot arm and the URDF model of the object to be moved;

[0023] The optimal control input sequence is sent to the robotic arm, which controls the robotic arm to push the object to be moved until it reaches the final target position.

[0024] Furthermore, the method for obtaining the optimal control input sequence is:

[0025] The state equation of the robotic arm system is:

[0026] x t+1 =f(x t ,u t )

[0027] Among them, x t+1 is the state variable of the robotic arm system at time t+1, that is, the angle vector of each joint of the robotic arm at time t+1, x t is the state variable of the robotic system at time t, u t is the input control quantity of each joint of the robot arm at time t, and f is the nonlinear state transfer function;

[0028] In the IsaacGym reinforcement learning environment, K virtual robotic arms are generated in parallel. The state of the virtual robotic arm at time t is read from the state x of the robotic arm system at time t. t , randomly generate the control quantity sequence [U1,…,U K ], where the control sequence of the kth virtual robot arm is U k :

[0029] U k =[u k,t,…,u k,t+T-1 ]

[0030] Among them, k = 1, ..., K, represents the serial number of the virtual robot arm, u k,t ,…,u k,t+T-1 are the control quantities of each joint of the kth robot from time t to t+T-1, and MPPI converts [U1,…,U K ] act on the K virtual robotic arms in IsaacGym respectively. IsaacGym simulates the motion of the K virtual robotic arms in parallel in the future T time, and converts the K virtual robotic arm state sequence [Q1,…,Q K ] The reading is handed over to MPPI for calculation. The K virtual robotic arms correspond to K state sequences respectively, and the state sequence of the kth virtual robotic arm is:

[0031] Q k =[x k,t ,f(x k,t ,u k,t ),…,f(x k,t+T-1 ,u k,t+T-1 )]

[0032] Among them, Q k is the state sequence of the kth virtual robot arm, x k,t is the state variable of the kth virtual robot arm at time t, x k,t ,…,x k,t+T-1 is the state variable of the kth virtual robot arm from time t to time t+T-1;

[0033] According to the state sequence and a cost function C that needs to be minimized, the cost of the kth virtual robot arm is:

[0034] S k =C(Q k )

[0035] Then use the sampling weight sequence [ω1,…,ω K ] for K control quantity sequences [U1,…,U K ] is weighted for importance; sampling weight ω k The calculation formula is:

[0036]

[0037] Among them, η is the normalization parameter, β is an adjustment parameter used to adjust the importance sampling weight in the MPPI algorithm; ρ is the minimum sampling cost;

[0038] Approximately optimal control input sequence U * The calculation formula is:

[0039]

[0040] Furthermore, the minimum sampling cost calculation formula is: ρ = minS k .

[0041] The system corresponding to the above method includes:

[0042] System environment building module, used to build the software and hardware environment of the robotic arm propulsion system;

[0043] The scene information acquisition module is used to acquire desktop scene information using an RGBD camera;

[0044] The mobile control module is used to determine whether it is necessary to introduce a global path planning algorithm according to the complexity of the obstacle distribution in the desktop scene information; when the object to be moved must bypass obstacles to reach the final target position from the initial position, a global path planning of the two-dimensional plane is first performed, and the middle point is obtained through the global path planning algorithm. The middle point is used as the current target point, and the model prediction path integral control algorithm MPPI is used to control the robotic arm to push the object to move to the current target point. When the robotic arm reaches this middle point, the next middle point is used as the current target point, and so on until the object to be moved reaches the final target position; when the object to be moved can reach the final target position directly from the initial position along a straight line, the model prediction path integral control algorithm is directly used to control the robotic arm to push the object to be moved to the final target position.

[0045] An electronic device for storing and executing the above method, the device comprising:

[0046] A memory storing executable program code;

[0047] a processor coupled to the memory;

[0048] The processor calls the executable program code stored in the memory to execute the steps of the desktop robotic arm pushing method for scenes with obstacles.

[0049] A computer-readable storage medium for storing and executing the above method, wherein the computer-readable storage medium stores computer instructions, and when the computer instructions are called, they are used to execute the steps of the desktop robotic arm pushing method for scenes with obstacles.

[0050] Beneficial effects: Compared with the prior art, the significant technical effects of the present invention are: by combining the model prediction path integral control algorithm with the IsaacGym reinforcement learning environment to apply to the pushing operation of the desktop robotic arm, the task of pushing objects that are easily deformed or inconvenient to grasp to the target position in the scenario where there are cluttered obstacles on the desktop is solved; it has the ability to perceive the environment, can detect and analyze the surrounding environment in real time, and has intelligent decision-making capabilities to plan the optimal pushing path; in addition, the system has relatively high stability and adaptability, which can ensure that the robotic arm can complete tasks safely and accurately in various complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 It is a flow chart of a desktop mechanical arm pushing method for a scene with obstacles in the present invention;

[0052] Figure 2 It is a schematic diagram of the desktop robotic arm and obstacle scene;

[0053] Figure 3 This is a flow chart of the robotic arm performing a push operation using the IsaacGym reinforcement learning environment and the model-predictive path integral control algorithm. DETAILED DESCRIPTION

[0054] The present invention will be further explained below in conjunction with the accompanying drawings and specific implementations. It should be understood that the following specific implementations are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0055] The desktop mechanical arm pushing method for scenes with obstacles described in the present invention is as follows: Figure 1 As shown, the following steps are included: building the hardware and software environment of the robotic arm pushing system; using the RGBD camera to obtain desktop scene information; and using the model prediction path integral control algorithm to control the robotic arm to push the object to the target position.

[0056] Build the software and hardware environment for robotic arm promotion: including building the URDF model of the robotic arm and the URDF model of the object to be moved. Most robotic arm manufacturers on the market will provide URDF model files corresponding to their own robotic arms. For self-developed robotic arms, you can use 3D modeling software such as SolidWorks to design and build your own model, and export the URDF file. Build the IsaacGym reinforcement learning environment on the computer, and import the URDF model of the robotic arm and the URDF model of the object to be moved into the IsaacGym reinforcement learning environment. Build a physical platform for the camera and robotic arm promotion system, using the eye-outside-hand method. The camera is directly above the desktop, and the field of view can completely cover the initial position of the object to be moved on the desktop, such as Figure 2As shown. The computer is connected to the robot arm and camera through USB communication, and the hand-eye calibration of the robot arm and camera is performed. The ROS robot arm operating system is used to connect and control the computer, robot arm and camera through software.

[0057] The use of RGBD cameras to obtain desktop scene information specifically includes: the RGBD camera senses and scans the scene and obstacle environment where the robot is located, and obtains the RGB image and depth point cloud image of the desktop plane information. For the RGB image, the YOLOv7 target detection algorithm is used to identify the object to be moved, and the two-dimensional coordinate information of the object to be moved is stored, including the coordinates of the center point (x, y) of the object to be moved and the coordinates of each boundary point (x i ,y j ). For the depth point cloud image, a statistical filter is used to denoise the point cloud, and the point cloud with a height higher than the desktop is regarded as an obstacle. After removing the point cloud corresponding to the obstacle, the movable space of the target object is obtained. According to the previously obtained two-dimensional coordinate information of the object to be moved, the three-dimensional boundary data information of the object to be moved is extracted from the depth point cloud data and the three-dimensional boundary data information is stored.

[0058] Using the model prediction path integral control algorithm to control the robot arm to push the object to the target position specifically includes: determining whether it is necessary to introduce a global path planning algorithm based on the complexity of the obstacle distribution in the scene. Make a judgment based on the information in the RGB image. When the object to be moved can reach the target position directly from the initial position along a straight line, the scene is considered simple; when the object to be moved must bypass obstacles to reach the target position from the initial position, the scene is considered complex. When the scene is complex, first perform global path planning on the two-dimensional plane, and use the global path planning algorithm to obtain the middle point (short-term target point). The middle point is used as the current target point. When the robot arm reaches this middle point, the next middle point is used as the current target point, and so on until the paper cup reaches the final target position; when the scene is simple, directly use the final position as the current target point.

[0059] Execute between the current position and the current target point, and control the end of the robot arm to move near the obstacle to be moved according to the three-dimensional boundary data information of the object to be moved. Through the model prediction path integral control algorithm, the robot arm is controlled to push the object to be moved toward the target point. The state equation of the robot arm system is:

[0060] x t+1 =f(x t ,u t ) (1)

[0061] Among them, x t is the state variable of the robotic arm system at time t, that is, the angle vector of each joint of the robotic arm at time t, x t+1is the state variable of the robotic arm system at time t+1, that is, the angle vector of each joint of the robotic arm at time t+1, u t is the input control value of each joint of the robot arm at time t. For a six-degree-of-freedom robot arm, x t and u t are all six-dimensional vectors. f is the nonlinear state transfer function, which describes the state x of the robot system. t How to control the input quantity u t Changes with time t. K virtual robotic arms are generated in parallel in IsaacGym, and the state of the virtual robotic arm at time t is read from the state x of the robotic arm system at time t. t , randomly generate the control amount of each virtual robotic arm in the future T time domains, and the control amount sequence of the kth virtual robotic arm is:

[0062] U k =[u k,t ,…,u k,t+T-1 ] (2)

[0063] Among them, k = 1, ..., K, represents the serial number of the virtual robot arm, u k,t ,…,u k,t+T-1 They are the control values ​​of each joint of the kth virtual robot arm from time t to t+T-1. For a six-degree-of-freedom robot arm, u k,t ,…,u k,t+T-1 They are all 6-dimensional vectors, that is, the control sequence U of the kth six-degree-of-freedom manipulator k is T 6-dimensional vectors. MPPI converts [U1,…,U K ] act on the K virtual robotic arms in IsaacGym respectively. IsaacGym simulates the motion of the K virtual robotic arms in parallel in the future T time, and converts the K virtual robotic arm state sequence [Q1,…,Q K ] Reading is handed over to MPPI for calculation, the process is as follows Figure 3 As shown. K virtual robotic arms correspond to K state sequences respectively, where the state sequence of the kth virtual robotic arm is:

[0064] Q k =[x k,t ,f(x k,t ,u k,t ),…,f(x k,t+T-1 ,u k,t+T-1 )] (3)

[0065] Among them, Q k is the state sequence of the kth virtual robot arm, x k,t is the state variable of the kth virtual robot arm at time t, x k,t ,…,xk,t+T-1 is the state variable of the kth virtual robot arm from time t to time t+T-1;

[0066] According to the state sequence and a cost function C that needs to be minimized, the cost of the kth virtual robot arm is:

[0067] S k =C(Q k ) (4)

[0068] Then use the sampling weight sequence [ω1,…,ω K ] for K control quantity sequences [U1,…,U K ] is weighted by importance. Sampling weight ω k By S with tuning parameter β k The inverse exponential calculation of β is used to adjust the importance sampling weight in the MPPI algorithm. At the same time, for numerical stability, S k Will subtract the minimum cost ρ = minS k , and normalize it with the normalization parameter η.

[0069]

[0070] Approximate optimal control sequence U * The calculation formula is:

[0071]

[0072] Get sequence U * Then, the first control quantity u of the sequence * Sent to the robotic arm system for execution. After the robotic arm system executes, the RGBD camera is used again to determine whether the target object has reached the current target point. If not, the model prediction path integral control algorithm is used to control the robotic arm to push the object toward the current target point. The above process is repeated over time until the object to be moved reaches the current target point. Next, it is determined whether the current target point is the final target point. If not, the next intermediate point is used as the current target point. The above process is repeated over time until the object to be moved reaches the current target point, that is, the robotic arm system has completed the pushing task. The process is as follows: Figure 1 shown.

[0073] In summary, this method solves the task of moving deformable objects or objects that are difficult to grasp to the target position by pushing in the presence of cluttered obstacles on the desktop by applying the model prediction path integral control algorithm combined with the IsaacGym reinforcement learning environment to the pushing operation of the desktop robotic arm.

[0074] It should be noted that the above content only illustrates the technical idea of ​​the present invention and cannot be used to limit the protection scope of the present invention. For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications all fall within the protection scope of the claims of the present invention.

Claims

1. A desktop robotic arm pushing method for scenes with obstacles, characterized in that: The following steps are involved: Build the software and hardware environment of the robotic arm propulsion system; Use RGBD camera to obtain desktop scene information; Whether it is necessary to introduce a global path planning algorithm is determined based on the complexity of the obstacle distribution in the desktop scene information; when the object to be moved must bypass obstacles to reach the final target position from the initial position, a global path planning of the two-dimensional plane is first performed, and the middle point is obtained through the global path planning algorithm. The middle point is used as the current target point, and the model prediction path integral control algorithm MPPI is used to control the robot arm to push the object to the current target point. When the robot arm reaches this middle point, the next middle point is used as the current target point, and so on until the object to be moved reaches the final target position; When the object to be moved can reach the final target position directly from the initial position along a straight line, the model prediction path integral control algorithm is directly used to control the robotic arm to push the object to be moved to the final target position.

2. The desktop robotic arm pushing method for scenes with obstacles according to claim 1, characterized in that: Build the software and hardware environment of the robotic arm propulsion system, including: S11, constructing the URDF model of the robot arm and the URDF model of the object to be moved; S12, constructing the IsaacGym reinforcement learning environment, and importing the URDF model of the robot arm and the URDF model of the object to be moved into the IsaacGym reinforcement learning environment; S13, build a physical platform driven by an RGBD camera and a robotic arm, and the field of view of the RGBD camera can completely cover the initial position of the object to be moved on the desktop; S14, connecting the computer to the robotic arm and the RGBD camera for communication, and performing hand-eye calibration on the robotic arm and the RGBD camera; using the ROS robotic arm operating system to perform software connection and control on the computer, the robotic arm and the RGBD camera.

3. The desktop robotic arm pushing method for scenes with obstacles according to claim 2, characterized in that: The RGBD camera and robotic arm adopt an eyes-outside-hand approach.

4. The desktop robotic arm pushing method for scenes with obstacles according to claim 1, characterized in that: Use the RGBD camera to obtain desktop scene information, including: S21, RGBD camera senses and scans the scene and obstacle environment where the robot is located, and obtains the RGB image and depth point cloud image of the desktop plane information; S22. For the RGB image, use the YOLOv7 target detection algorithm to identify the object to be moved, store the coordinates of the center point (x, y) of the object to be moved and the coordinates of each boundary point (x i ,y j ); S23. For the depth point cloud image, a statistical filter is used to denoise the point cloud, and the point cloud with a height higher than the desktop is regarded as the location of the obstacle. After removing the point cloud corresponding to the obstacle, the movable space of the target object is obtained; S24. According to the two-dimensional coordinate information of the object to be moved obtained in step S22, extract the three-dimensional boundary data information of the object to be moved from the denoised depth point cloud data, and store the three-dimensional boundary data information.

5. The desktop robotic arm pushing method for scenes with obstacles according to claim 1, characterized in that: The model predictive path integral control algorithm is used to control the robot arm to push the object to the current target point, including: According to the three-dimensional boundary data information of the object to be moved, the end of the robot arm is controlled to move near the object to be moved; In the IsaacGym reinforcement learning environment, the optimal control input sequence is obtained through the model-predictive path integral control algorithm with the help of the URDF model of the robot arm and the URDF model of the object to be moved; The optimal control input sequence is sent to the robotic arm, which controls the robotic arm to push the object to be moved until it reaches the final target position.

6. The desktop robotic arm pushing method for scenes with obstacles according to claim 5, characterized in that: The method to obtain the optimal control input sequence is: The state equation of the robotic arm system is: x t+1 =f(x t ,u t ) Among them, x t+1 is the state variable of the robotic arm system at time t+1, that is, the angle vector of each joint of the robotic arm at time t+1, x t is the state variable of the robotic system at time t, u t is the input control quantity of each joint of the robot arm at time t, and f is the nonlinear state transfer function; In the IsaacGym reinforcement learning environment, K virtual robotic arms are generated in parallel. The state of the virtual robotic arm at time t is read from the state x of the robotic arm system at time t. t , randomly generate the control quantity sequence [U1,…,U K ], where the control sequence of the kth virtual robot arm is U k : IN k =[in k,t ,…,in k,t+T-1 ] Where k = 1, ..., K, represents the serial number of the virtual robot arm, u k,t ,…,u k,t+T-1 are the control quantities of each joint of the kth robot from time t to t+T-1, and MPPI converts [U1,…,U K ] act on the K virtual robotic arms in IsaacGym respectively. IsaacGym simulates the motion of the K virtual robotic arms in parallel in the future T time, and converts the K virtual robotic arm state sequence [Q1,…,Q K ] The reading is handed over to MPPI for calculation. The K virtual robotic arms correspond to K state sequences respectively, and the state sequence of the kth virtual robotic arm is: Q k =[x k,t ,f(x k,t ,u k,t ),…,f(x k,t+T-1 ,u k,t+T-1 )] Among them, Q k is the state sequence of the kth virtual robot arm, x k,t is the state variable of the kth virtual robot arm at time t, x k,t ,…,x k,t+T-1 is the state variable of the kth virtual robot arm from time t to time t+T-1; According to the state sequence and a cost function C that needs to be minimized, the cost of the kth virtual robot arm is: S k =C(Q k ) Then use the sampling weight sequence [ω1,…,ω K ] for K control quantity sequences [U1,…,U K ] is weighted for importance; sampling weight ω k The calculation formula is: Among them, η is the normalization parameter, β is an adjustment parameter used to adjust the importance sampling weight in the MPPI algorithm; ρ is the minimum sampling cost; Approximate optimal control input sequence U * The calculation formula is:

7. The desktop robotic arm pushing method for scenes with obstacles according to claim 6, characterized in that: The minimum sampling cost calculation formula is: ρ = minS k .

8. A desktop robotic arm propulsion system for use in scenes with obstacles, characterized in that: include: System environment building module, used to build the software and hardware environment of the robotic arm propulsion system; The scene information acquisition module is used to acquire desktop scene information using an RGBD camera; The mobile control module is used to determine whether it is necessary to introduce a global path planning algorithm according to the complexity of the obstacle distribution in the desktop scene information; when the object to be moved must bypass obstacles to reach the final target position from the initial position, a global path planning of the two-dimensional plane is first performed, and the middle point is obtained through the global path planning algorithm. The middle point is used as the current target point, and the model prediction path integral control algorithm MPPI is used to control the robot arm to push the object to the current target point. When the robot arm reaches this middle point, the next middle point is used as the current target point, and so on until the object to be moved reaches the final target position; When the object to be moved can reach the final target position directly from the initial position along a straight line, the model prediction path integral control algorithm is directly used to control the robotic arm to push the object to be moved to the final target position.

9. An electronic device, characterized in that: The device comprises: A memory storing executable program code; a processor coupled to the memory; The processor calls the executable program code stored in the memory to execute the steps of the desktop robotic arm pushing method for scenes with obstacles as described in any one of claims 1-7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, which, when called, are used to execute the steps of the desktop robotic arm pushing method for scenes with obstacles as described in any one of claims 1-7.

Images