Robot control methods
By using RGB camera calibration and stereo vision matching of Gray code coded points, a human body model is established to calculate the minimum distance between the human and the robot. A safe and compliant control strategy is selected, which solves the problems of physical fences occupying space and having poor flexibility, and enables the robot to effectively avoid collisions with pedestrians and cooperate safely.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- SUZHOU LINGSHI VISION TECH CO LTD
- Filing Date
- 2024-01-19
- Publication Date
- 2026-06-30
AI Technical Summary
In existing technologies, physical fences occupy space in factory production workshops and have poor flexibility, failing to meet the need for safe collaboration between humans and machines in the same space.
By using RGB camera calibration and Gray code points, a human body model is established through stereo vision matching. The minimum distance between the human and the robot is calculated, and a safe and compliant control strategy is selected to enable the robot to actively avoid obstacles.
It enables robots to effectively avoid collisions with pedestrians in the work area, improving the safety and flexibility of human-robot collaboration and making it suitable for different work environments.
Smart Images
Figure CN117644518B_ABST
Abstract
Description
Technical Field
[0001] This disclosure relates to a robot control method, belonging to the field of human-machine collaboration safety protection technology. Background Technology
[0002] Currently, physical protection within factory production workshops remains the primary method for preventing human-robot interference and collisions. This involves defining the robot's working area based on the actual site conditions and the robot's movement trajectory, and installing fences to isolate it from the outside environment.
[0003] However, the installation of such physical fences requires a certain amount of space, and the flexibility for secondary modifications is poor, and it cannot meet the needs of humans and machines working in the same space.
[0004] To overcome the drawbacks of physical fences, efforts are being made to transform passive protection into active avoidance by robots. Therefore, researching a reliable and flexible human-robot collaborative safety method has become a current research hotspot. Summary of the Invention
[0005] To address one of the aforementioned technical problems, this disclosure provides a robot control method.
[0006] According to one aspect of this disclosure, a robot control method is provided, comprising:
[0007] Multiple RGB cameras are fixed and placed in the area around the robot; the RGB cameras are calibrated to obtain the coordinate transformation matrix between the camera coordinate system and the world coordinate system;
[0008] Gray code points are pasted on each joint of the robot, and the spatial position information of the Gray code points is obtained through an RGB camera, thereby obtaining the robot's pose and joint position in the world coordinate system.
[0009] The system uses a camera to capture image sequences of the human-computer collaborative environment in real time, processes the images in the sequence, detects and obtains key points of the human body and their semantic information, and performs stereo vision matching on the semantic information to obtain the human body's pose information.
[0010] A human body model based on a capsule-shaped enclosure is established, and the minimum human-machine distance is calculated using this model; a human-machine safety index is obtained based on this minimum distance; and...
[0011] Based on the human-machine safety index, a safe and compliant control strategy for the robot is selected, and this strategy is used to control the robot.
[0012] According to at least one embodiment of the robot control method of this disclosure, Gray code coded points are pasted on each joint position of the robot, and the spatial position information of the Gray code coded points is obtained, thereby obtaining robot pose and joint position estimation, including:
[0013] Gray code points are affixed to each joint of the robot;
[0014] The image of the Gray code coded point is acquired using an RGB camera, and the position of the Gray code coded point in the camera coordinate system is obtained.
[0015] The position of the Gray code point in the world coordinate system is obtained based on the coordinate transformation matrix;
[0016] The robot's pose and joint positions in the world coordinate system are obtained through 3D reconstruction.
[0017] According to at least one embodiment of the robot control method of this disclosure, obtaining the robot's pose and joint positions in the world coordinate system through three-dimensional reconstruction includes:
[0018] In the disordered images of the robot captured by the RGB camera, pairwise matching is performed to obtain the corresponding matching points of the Gray code points;
[0019] Read the focal length f from the given file, and obtain the transformation relationship R,t between pairs of cameras in space based on epipolar geometry from the corresponding matching points;
[0020] Given the intrinsic and extrinsic parameter matrices of the camera and the corresponding matching points, the 3D coordinates of the corresponding matching points can be obtained through triangulation.
[0021] Nonlinear optimization is performed on the 3D point coordinates and camera parameters; the specific calculation formula is as follows: Among them, z ij In camera pose T i Observation marker p j The generated data; and
[0022] Based on the optimized camera parameters and the 3D point coordinates of the Gray code-encoded points, the robot's pose is modeled and the position information of each joint is estimated using a capsule bounding box model.
[0023] According to at least one embodiment of the robot control method of this disclosure, calibrating the RGB camera includes:
[0024] Given a number of Gray code points A1, A2, A3, ..., A n Arranged in the aforementioned area to display as many encoded points as possible within the frame of each RGB camera; several encoded points A1, A2, A3, ..., A... are recorded. nThe coordinates p1, p2, p3, ..., p in the RGB camera n , where p i =[u i v i ,1] T ;
[0025] Several coding points A1, A2, A3, ..., A n The coordinates in the world coordinate system are: P i =[X i Y i Z i ,1] T ,
[0026] The transformation relationship between the three-dimensional coordinates of the encoded point in the RGB camera coordinate system and its coordinates in the world coordinate system is as follows:
[0027] p i =sA[R,t]P i ,
[0028] Where s is the scaling factor, and [R,t] is the rotation and translation matrix for transforming the world coordinate system to the RGB camera coordinate system;
[0029] y is the internal coefficient matrix of the RGB camera; (u0, v0) is the image reference point of the RGB camera; (α, β) is the aspect ratio factor from the image plane coordinates to the pixel coordinates in the frame buffer; γ is the coefficient of the radial distortion of the RGB camera.
[0030] According to at least one embodiment of the robot control method of this disclosure, the safety index is the ratio of the current minimum human-machine distance to the current minimum safe human-machine distance, and the safety index is calculated by the following formula:
[0031] Among them, L hr S is the current minimum human-machine distance. p This represents the current minimum safe distance between humans and machines.
[0032] According to at least one embodiment of the robot control method of this disclosure, the current minimum human-machine safe distance S p (t0)=S h +S r +S s +C+Z d +Z r ;
[0033] Where t0 is the current time; S h This is due to the contribution of changes in the operator's position to the protection spacing; Sr The impact of the robot system's reaction time on the protection distance; S s This is due to the impact of the robot system's stopping distance on the protection spacing; C is the intrusion distance, which is the distance a part of the body can intrude into the sensing field before being detected; Z d Z is the uncertainty of the operator's position in the collaborative workspace; r It is the positional uncertainty of the robot system. Attached Figure Description
[0034] The accompanying drawings illustrate exemplary embodiments of the present disclosure and, together with the description thereof, serve to explain the principles of the present disclosure. These drawings are included to provide a further understanding of the present disclosure and are incorporated in and constitute a part of this specification.
[0035] Figure 1 This is a flowchart of a robot control method according to one embodiment of the present disclosure.
[0036] Figure 2 This is a schematic diagram showing the arrangement of a robot and an RGB camera according to one embodiment of the present disclosure.
[0037] Figure 3 This is a schematic diagram of RGB camera calibration according to one embodiment of the present disclosure.
[0038] Figure 4 This is a schematic diagram of the impedance model of a robot according to one embodiment of the present disclosure. Detailed Implementation
[0039] The present disclosure will now be described in further detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are for illustrative purposes only and are not intended to limit the scope of the disclosure. Furthermore, it should be noted that, for ease of description, only the parts relevant to the present disclosure are shown in the accompanying drawings.
[0040] It should be noted that, where there is no conflict, the embodiments and features described in this disclosure can be combined with each other. The technical solutions of this disclosure will now be described in detail with reference to the accompanying drawings and embodiments.
[0041] Unless otherwise stated, the exemplary implementations / embodiments shown are to be understood as providing exemplary features of various details that provide ways in which the technical concepts of this disclosure can be implemented in practice. Therefore, unless otherwise stated, the features of various implementations / embodiments may be additionally combined, separated, interchanged and / or rearranged without departing from the technical concepts of this disclosure.
[0042] The use of crosshairs and / or shading in the accompanying drawings is generally used to clarify the boundaries between adjacent components. Thus, unless otherwise stated, the presence or absence of crosshairs or shading does not convey or indicate any preference or requirement for the specific material, material properties, dimensions, proportions, commonalities between the illustrated components, or any other characteristics, properties, etc., of the components. Furthermore, in the accompanying drawings, the dimensions and relative dimensions of components may be exaggerated for clarity and / or descriptive purposes. When exemplary embodiments can be implemented differently, a specific process sequence may be performed in a different order than that described. For example, two consecutively described processes may be performed substantially simultaneously or in the reverse order of their description. Furthermore, the same reference numerals denote the same components.
[0043] When a component is referred to as being "on" or "above" another component, "connected to," or "joined to" another component, the component may be directly on, directly connected to, or directly joined to the other component, or there may be intermediate components. However, when a component is referred to as being "directly on" another component, "directly connected to," or "directly joined to" another component, there are no intermediate components. Therefore, the term "connection" can refer to a physical connection, an electrical connection, etc., and may or may not have intermediate components.
[0044] For descriptive purposes, this disclosure may use spatial relative terms such as “below,” “under,” “below,” “down,” “above,” “above,” “higher,” and “side (e.g., in a “sidewall”)” to describe the relationship between one component and another component as shown in the accompanying drawings. In addition to the orientations depicted in the drawings, the spatial relative terms are also intended to encompass different orientations of the device during use, operation, and / or manufacture. For example, if the device in the drawings is flipped, a component described as “below” or “under” another component or feature would subsequently be positioned “above” said other component or feature. Thus, the exemplary term “below” can encompass both “above” and “below” orientations. Furthermore, the device may be otherwise positioned (e.g., rotated 90 degrees or in other orientations), thus interpreting the spatial relative descriptive terms used herein accordingly.
[0045] The terminology used herein is for the purpose of describing particular embodiments and is not intended to be limiting. As used herein, unless the context clearly indicates otherwise, the singular forms “a” and “the” are intended to include the plural forms as well. Furthermore, when the terms “comprising” and / or “including” and variations thereof are used in this specification, it indicates the presence of the stated features, integrals, steps, operations, parts, components, and / or groups thereof, but does not exclude the presence or addition of one or more other features, integrals, steps, operations, parts, components, and / or groups thereof. It should also be noted that, as used herein, the terms “substantially,” “about,” and other similar terms are used as approximate terms rather than as terms of degree, thus explaining the inherent biases in measurements, calculated values, and / or provided values that would be recognized by one of ordinary skill in the art.
[0046] Figure 1 This is a flowchart of a robot control method according to one embodiment of the present disclosure.
[0047] like Figure 1 As shown, the robot control method disclosed herein may include:
[0048] S10. Fix and place multiple RGB cameras in the robot and the surrounding area; calibrate the RGB cameras to obtain the coordinate transformation matrix between the camera coordinate system and the world coordinate system;
[0049] S20. Gray code points are pasted on each joint of the robot, and the spatial position information of the Gray code points is obtained through an RGB camera, thereby obtaining the robot's pose and joint position in the world coordinate system.
[0050] S30. Real-time acquisition of image sequences of the human-computer collaborative environment using a camera; processing of the images in the image sequence; detection of key points of the human body and semantic information of the key points; stereo vision matching of the semantic information to obtain the pose information of the human body; the key points of the human body include 21 parts such as the top of the head, left ear, right ear, left eye, right eye, nose, left corner of mouth, right corner of mouth, neck, left shoulder, right shoulder, left elbow, right elbow, left wrist, right wrist, left hip, right hip, left knee, right knee, left ankle, and right ankle; and output of the coordinate information and number of the human body.
[0051] S40. Establish a human body model based on a capsule-shaped enclosure, and calculate the minimum human-machine distance using the human body model; obtain the human-machine safety index based on the minimum human-machine distance; and
[0052] S50. Based on the human-machine safety index, select a safe and compliant control strategy for the robot, and use this safe and compliant control strategy to control the robot.
[0053] In this disclosure, the robot can be a collaborative robot, which can be a robotic arm fixed to the ground, a workbench, or a mobile platform.
[0054] Therefore, the robot control method disclosed herein can effectively avoid collisions between the robot and pedestrians in the work area by real-time monitoring of the overall work area of the collaborative robot, and the method can be applied to different types of robot working environments.
[0055] The robot control method disclosed herein will be described in detail below with reference to specific implementation methods.
[0056] In some embodiments, S10 can be performed using Gray code coded points to calibrate the RGB camera. Specifically, several Gray code coded points A1, A2, A3, ..., A... are used. n Arranged in the aforementioned area, such that each RGB camera can display as many Gray code points as possible within its image; record several Gray code points A1, A2, A3, ..., A... n The coordinates p1, p2, p3, ..., p in the RGB camera n , where p i =[u i v i ,1] T Several Gray code points A1, A2, A3, ..., A n The coordinates in the world coordinate system are: P i =[X i Y i Z i ,1] T The transformation relationship between the three-dimensional coordinates of the Gray code-encoded point in the camera coordinate system of the RGB camera and its coordinates in the world coordinate system is as follows:
[0057] p i =sA[R,t]P i ;
[0058] Where s is the scaling factor, and [R,t] is the rotation and translation matrix for transforming the world coordinate system to the camera coordinate system of the RGB camera;
[0059] It is represented as the internal coefficient matrix of the RGB camera; (u0, v0) is the image reference point of the RGB camera; (α, β) is the aspect ratio factor from the image plane coordinates to the pixel coordinates in the frame buffer; γ is the coefficient of the radial distortion of the RGB camera.
[0060] In some embodiments, S20 specifically includes: S201, attaching Gray code points to each joint position of the robot; S202, acquiring images of the Gray code points using an RGB camera; and obtaining the position of the Gray code point in the camera coordinate system; S203, obtaining the position of the Gray code point in the world coordinate system according to the coordinate transformation matrix; and S204, obtaining the pose and joint position of the robot in the world coordinate system through three-dimensional reconstruction.
[0061] More specifically, the Gray code encoded points are identified by ID using an RGB camera, and the encoded points are identified by position using a circle recognition algorithm based on Arc-support LineSegments Revisited. Based on the Gray code encoded point information (ID, ux, uy) from different RGB camera perspectives, the position of the Gray code encoded point in the world coordinate system is obtained using the aforementioned coordinate transformation matrix, thus obtaining the three-dimensional point coordinates of the Gray code encoded point.
[0062] In a preferred embodiment, step S204, obtaining the robot's pose and joint position in the world coordinate system through 3D reconstruction, includes: S2041, performing pairwise matching in the unordered images of the robot acquired by the RGB camera to obtain the corresponding matching points for Gray code encoding points; S2042, reading the focal length f from a given file, and obtaining the transformation relationship R,t between the pairs of cameras in space based on epipolar geometry from the corresponding matching points; S2043, given the intrinsic and extrinsic parameter matrices of the camera and the corresponding matching points, obtaining the 3D point coordinates of the Gray code encoding points corresponding to the corresponding matching points through triangulation; S2044, performing nonlinear optimization on the 3D point coordinates and camera parameters; the specific calculation formula is as follows: Among them, z ij In camera pose T i Observation marker p j The generated data; and S2045, based on the optimized camera parameters and the three-dimensional point coordinates of the Gray code-encoded points, model the robot's pose based on the capsule bounding box model, and estimate the position information of each joint based on the robot model obtained from modeling the robot.
[0063] In some embodiments, S30 includes: processing the initial frame of the image sequence using the Backbone part of the OpenPose network model to obtain image features; performing initial heatmap estimation using the confidence map of the OpenPose network model; matching key points with different human bodies using the PAF part of the OpenPose network model; tracking key points using the CoTracker algorithm for processing subsequent image sequences; and performing 3D human pose estimation using the MultiView_Pose algorithm based on key point information (e.g., semantic information of key points) obtained from different camera perspectives, and obtaining human pose information.
[0064] In this disclosure, S40, a human body model based on a capsule enclosure is established, and the minimum human-machine distance is calculated using the human body model; the human-machine safety index is obtained based on the minimum human-machine distance.
[0065] Specifically, when building the human body model based on the capsule bounding box, the 3D human pose estimation obtained above can be used. Then, the minimum human-machine distance can be obtained based on the human body model and the robot model.
[0066] In one specific embodiment, the formula for calculating the minimum human-machine distance is as follows:
[0067] D(c t ,o t )=min||d i,j (t)||i=1~6j=1~15
[0068] Among them, C t It's the robot's position, O t Let t be the person's position, t be the time step, and d be the distance norm. i,j (t)|| is the minimum distance between the j-th obstacle capsule (e.g., human model) and the i-th robotic arm capsule (e.g., robotic arm model) at time step t.
[0069] On the other hand, the formula for calculating the minimum safe distance between humans and machines is as follows:
[0070] S p (t0)=S h +S r +S s +C+Z d +Z r
[0071] Where t0 is the current time; S h This is due to the contribution of changes in the operator's position to the protection spacing; S r The impact of the robot's reaction time on the protection distance; Ss This is due to the impact of the robot's stopping distance on the protection spacing; C is the intrusion distance, which represents the distance a part of the human body can intrude into the sensing field before being detected; Z d Z is the uncertainty of the operator's position in the collaborative workspace; r It is the robot's positional uncertainty.
[0072] Accordingly, the safety index is the ratio of the current minimum human-machine distance to the current minimum safe human-machine distance, and this safety index is calculated using the following formula:
[0073] Among them, L hr S is the current minimum human-machine distance. p This represents the current minimum safe distance between humans and machines.
[0074] In some embodiments, S50 includes: selecting a safe and compliant control strategy for the robot based on the safety index assessment results.
[0075] Specifically, when a human body enters the cooperative safety area, the robot works normally; when a human body enters the cooperative work area, the robot's pose is acquired, and the robot's path is replanned to avoid obstacles; when a human body and the robot are about to collide or have already collided, the robot acquires the semantic information of the human body's key points and uses power and force limiting methods for control.
[0076] The power and force limiting method utilizes semantic information of key points in the human body to limit different powers based on the maximum damage that different key points can withstand, and uses impedance control to maximize the simple harm to the human body.
[0077] Therefore, the robot control method disclosed herein can effectively avoid collisions between the robot and pedestrians in the work area through real-time monitoring of the robot's entire work area. Furthermore, this method is applicable to various robot working environments.
[0078] More preferably, when impedance control is applied to the robot, the impedance model can be expressed as:
[0079]
[0080] Where E = Xd - X, E is the position deviation, that is, the difference between the desired position Xd and the actual position X. Z is the desired impedance of the robot, F is the contact force between the robot and the external environment, and M, B and K are the inertia matrix, damping matrix and stiffness matrix, respectively.
[0081] Based on the above, the robot control method disclosed herein can solve the problems of low efficiency and poor real-time performance in human-robot collaborative safety early warning in existing technologies. Through this invention, personnel safety in human-robot collaboration can be fundamentally improved.
[0082] In the description of this specification, the references to terms such as "one embodiment / mode," "some embodiments / modes," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment / mode or example is included in at least one embodiment / mode or example of this application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment / mode or example. Moreover, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments / modes or examples. Furthermore, without contradiction, those skilled in the art can combine and integrate the different embodiments / modes or examples described in this specification, as well as the features of different embodiments / modes or examples.
[0083] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this application, "multiple" means at least two, such as two, three, etc., unless otherwise explicitly specified.
[0084] Those skilled in the art should understand that the above embodiments are merely for illustrating the present disclosure and are not intended to limit the scope of the disclosure. Those skilled in the art can make other changes or modifications based on the above disclosure, and these changes or modifications still fall within the scope of the present disclosure.
Claims
1. A robot control method, characterized in that, include: Multiple RGB cameras were fixed and placed in the area around the robot; The RGB camera is calibrated to obtain the coordinate transformation matrix between the camera coordinate system and the world coordinate system; Gray code points are pasted on each joint of the robot, and the spatial position information of the Gray code points is obtained through an RGB camera, thereby obtaining the robot's pose and joint position in the world coordinate system. The system uses a camera to capture image sequences of the human-computer collaborative environment in real time, processes the images in the sequence, detects and obtains key points of the human body and their semantic information, and performs stereo vision matching on the semantic information to obtain the human body's pose information. A human body model based on a capsule-shaped enclosure is established, and the minimum human-machine distance is calculated using the human body model; the human-machine safety index is obtained based on the minimum human-machine distance. as well as Based on the human-machine safety index, select a safe and compliant control strategy for the robot, and use this strategy to control the robot. The safety index is the ratio of the current minimum human-machine distance to the current minimum safe human-machine distance, and it is calculated using the following formula: ; Among them, L hr S is the current minimum human-machine distance. p The current minimum safe distance between humans and machines; the current minimum safe distance between humans and machines. ; in, It is the current time; This is due to the contribution of changes in the operator's position to the protection spacing; The impact of the robot system's reaction time on the protection distance; This is due to the effect of the robot system's stopping distance on the protection spacing; C is the intrusion distance, which is the distance a part of the body can intrude into the sensing field before being detected; It is the uncertainty of the operator's position in the collaborative workspace; It is the positional uncertainty of the robot system; Based on the human-machine safety index, a safe and compliant control strategy is selected for the robot, and this strategy is used to control the robot. This includes: when a human just enters the cooperative safety area, the robot operates normally; when a human enters the cooperative work area, the human's pose is acquired, and the robot's path is replanned for obstacle avoidance; when a human and the robot are about to collide or have already collided, semantic information of the human's key points is acquired, and power and force limiting methods are used for control; specifically, based on the power and force limiting method, the semantic information of the human's key points is used to limit different powers according to the maximum damage that different key points can withstand, and impedance control is used to minimize the harm to the human.
2. The robot control method according to claim 1, characterized in that, Gray code points are pasted onto each joint of the robot, and the spatial position information of the Gray code points is obtained, thereby obtaining the robot pose and joint position estimation, including: Gray code points are affixed to each joint of the robot; The image of the Gray code coded point is acquired using an RGB camera, and the position of the Gray code coded point in the camera coordinate system is obtained. The position of the Gray code point in the world coordinate system is obtained based on the coordinate transformation matrix; The robot's pose and joint positions in the world coordinate system are obtained through 3D reconstruction.
3. The robot control method according to claim 2, characterized in that, Obtaining the robot's pose and joint positions in the world coordinate system through 3D reconstruction includes: In the disordered images of the robot captured by the RGB camera, pairwise matching is performed to obtain the corresponding matching points of the Gray code points; Read the focal length f from the given file, and obtain the transformation relationship R,t between pairs of cameras in space based on epipolar geometry from the corresponding matching points; Given the intrinsic and extrinsic parameter matrices of the camera and the corresponding matching points, the 3D coordinates of the corresponding matching points can be obtained through triangulation. Nonlinear optimization is performed on the 3D point coordinates and camera parameters; the specific calculation formula is as follows: ;in, In camera pose Observation road sign The generated data; and Based on the optimized camera parameters and the 3D point coordinates of the Gray code-encoded points, the robot's pose is modeled and the position information of each joint is estimated using a capsule bounding box model.
4. The robot control method according to claim 1, characterized in that, The calibration of the RGB camera includes: Several Gray code points Arranged in the aforementioned area, such that each RGB camera can display as many encoded points as possible within its frame; several of these encoded points are recorded. Coordinates in the RGB camera ,in ; Several of the coding points The coordinates in the world coordinate system are: , The transformation relationship between the three-dimensional coordinates of the encoded point in the RGB camera coordinate system and its coordinates in the world coordinate system is as follows: ， Where s is the scaling factor, and [R,t] is the rotation and translation matrix for transforming the world coordinate system to the RGB camera coordinate system; This refers to the internal coefficient matrix of the RGB camera; α is the image reference point of the RGB camera; (α, β) is the aspect ratio factor from the image plane coordinates to the pixel coordinates in the frame buffer; γ is the coefficient of radial distortion of the RGB camera.