Sensing system and method for humanoid robot joints

Abstract

The application discloses a kind of located in humanoid robot joint sensing system and method, belong to humanoid robot technical field, system includes humanoid robot limbs and joint, camera, processor and calibration board, the joint includes shoulder joint, elbow joint, wrist joint and end effector, and / or hip joint, knee joint, ankle joint and end effector;Camera is composed of a binocular camera module and a TOF module, camera and processor are located on the joint of humanoid robot, processor calibration position, process the data obtained by camera and output control instruction to control end effector.The application can realize more extensive range of environmental perception, the camera on the joint can be freely adjusted with the movement of joint Angle and direction, so as to realize more flexible environmental perception and operation;Integrated camera data processing and motor control, reduce the communication delay between sensor and controller, improve the response speed and processing efficiency of system.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of humanoid robots, and relates to a sensing system and method located at the joints of a humanoid robot. BACKGROUND

[0002] In recent years, humanoid robots have become one of the research hotspots, and the goal is to achieve flexible and efficient operation in a human environment. In order to achieve this goal, humanoid robots need to have strong sensing capabilities in order to safely, flexibly and efficiently interact with the environment.

[0003] Traditional humanoid robot sensing systems usually rely on sensors fixed on the head or body of the robot, such as binocular cameras or laser radars, etc. However, this arrangement has certain limitations when the robot joints achieve all-around sensing and operation. Traditional humanoid robot sensing methods also have certain limitations, such as: laser radars are limited by their working principle; cameras require complex image processing algorithms and may face challenges in the case of limited computing resources; the data obtained by the sensors is usually sent to the central control unit, such as the robot main controller, for processing, which has the problems of communication delay and low data processing efficiency. SUMMARY

[0004] The purpose of the present application is to provide a sensing system and method located at the joints of a humanoid robot, by installing a binocular camera and a TOF camera on the joints of a humanoid robot and processing camera data in real time on the joints, to solve the defects of local sensing of humanoid robots and the difficulties in data processing in the prior art. It should be emphasized that the purpose of the present application is achieved by the following technical solutions.

[0005] The primary aspect of the present application is to provide a sensing system located at the joints of a humanoid robot, comprising the limbs and joints of a humanoid robot, a camera, a processor and a calibration board, the joints comprising shoulder joints, elbow joints, wrist joints and end effectors, and / or hip joints, knee joints, ankle joints and end effectors; the camera consists of a binocular camera module and a TOF module, the camera and the processor being located on the joints of the humanoid robot, the processor calibrating the position, processing the data obtained by the camera and outputting control instructions to control the end effectors.

[0006] Further, the end effectors comprise robot hands and / or robot feet.

[0007] Further, the cameras are arranged in pairs, each pair being mounted on two upper limbs or lower limbs.

[0008] Another aspect of the present application is to provide a method of sensing by the above-mentioned sensing system, comprising the following steps:

[0009] S1 fuses the binocular disparity map and the TOF depth image based on the binocular disparity map and the TOF depth image acquired by the camera to acquire a fused depth image;

[0010] S2 performs position calibration based on the camera, the installation position of the limbs of the humanoid robot and the calibration board to acquire a coordinate conversion matrix from the camera coordinate system to the end effector of the robot arm;

[0011] S3 acquires a conversion matrix from the end effector to the joints of the limb where the end effector is located based on the forward kinematics of the robot;

[0012] S4 converts the coordinates in the camera coordinate system into coordinates in the end effector coordinate system and coordinates in the limb joint coordinate system based on the coordinate conversion matrix from the camera coordinate system to the end effector of the robot arm and the conversion matrix from the end effector to the joints of the limb where the end effector is located to acquire object distance information of the environment and realize the coarse positioning and focusing functions of the robot.

[0013] Further, step S1 fuses the binocular disparity map and the TOF depth image based on an optimization fusion algorithm based on energy minimization to acquire a target depth image energy function, and the target depth image energy function comprises:

[0014]

[0015] wherein, D is a depth image, E data represents a data item, E smooth represents a smoothing item, λ 1 represents the weight of the data item, λ 2 represents the weight of the smoothing item, u , v represents a pixel point coordinate, ω u , v represents a pixel weight, D s u , v represents a binocular disparity map, D T u , v represents a TOF depth map, N represents the neighborhood of adjacent pixels, represents the gradient at the pixel in the depth image, u v .

[0016] All physical quantities in the present application have SI units.

[0017] ​​​​Further, the optimization fusion algorithm based on energy minimization includes:

[0018]

[0019] wherein, represents a fused depth image.

[0020] Further, the position calibration in step S2 is based on the camera, the installation position of the humanoid robot limbs, and the calibration board. The formula for obtaining the coordinate conversion matrix from the camera coordinate system to the end effector of the robot arm is:

[0021]

[0022] wherein, and are the coordinate transformation matrices of the camera and the end effector, respectively, from position 1 to position 2, is the required coordinate transformation matrix between the end effector and the camera.

[0023] Further, represents the transformation product of two rigid bodies based on the camera world coordinate system, represents the transformation product of two rigid bodies based on the robot base coordinate system:

[0024]

[0025] wherein, , and , are the poses of the camera relative to the world coordinate system and the pose of the end effector relative to the robot base coordinate system, respectively.

[0026] Further, according to the rotation and translation matrices between the camera, the end effector, and the base coordinate system, the following formula is obtained:

[0027]

[0028] wherein, R A , t A and R B , t B are the rotation and translation matrices of the camera and the end effector, respectively, from position 1 to position 2, R X , t X are the rotation and translation matrices between the camera and the end effector, Ris a 3 × 3 rotation matrix, while t is a 3 × 1 translation vector.

[0029] After expansion, the following equations are obtained:

[0030]

[0031] Further, the Tsai-Lenz algorithm is used to solve the coordinate transformation rotation matrix of the camera and the end effector R X :

[0032]

[0033] Thus, the coordinate transformation translation matrix of the camera and the end effector of the robot arm can be solved t X , and finally the coordinate transformation matrix of the camera and the end effector of the robot arm is obtained T X :

[0034]

[0035] Further, step S3 obtains the conversion matrix between the end effector and the three joints (shoulder, elbow, wrist or hip, knee, ankle) of the limb where the end effector is located based on the forward kinematics of the robot, including:

[0036]

[0037] wherein the numbers of the shoulder joint, the elbow joint and the wrist joint or the hip joint, the knee joint and the ankle joint are 0, 1 and 2 respectively, and the number of the end effector is 3, represents the transformation from joint i to joint i +1.

[0038] Further, step S4 converts the coordinates in the camera coordinate system into coordinates in the coordinate system of the end effector of the robot arm and coordinates in the coordinate systems of the shoulder joint, the elbow joint and the wrist joint based on the coordinate conversion matrix from the camera coordinate system to the end effector of the robot arm and the conversion matrix between the end effector of the robot arm and the three joints of the shoulder, the elbow and the wrist, obtains the object distance information of the environment, and realizes the coarse positioning and focusing functions of the robot, including:

[0039]

[0040] P c is the coordinate in the camera coordinate system, P r is the coordinate in the coordinate system of the end effector of the robot arm, represents a transformation from the camera to the end effector, P w is a coordinate in the wrist coordinate system, P e is a coordinate in the elbow coordinate system, P s is a coordinate in the shoulder coordinate system.

[0041] Further, according to the coordinates of the object in the camera coordinate system, when the object is found, the distance position information of the object in the camera coordinate system is obtained d′ , which is converted into distance position information in different joint coordinate systems d′ . d .

[0042] When an obstacle is detected, the robot's speed is reduced or stopped to avoid collision.

[0043] According to the position of the obstacle relative to the robot, the steering angle of the robot is adjusted to bypass the obstacle.

[0044] After the obstacle disappears or the robot successfully avoids the obstacle, the normal moving speed and direction of the robot are restored.

[0045] The perception system and method provided by the present application have the following advantages compared with the prior art:

[0046] (1) Installing the camera on the joint of the robot can achieve a wider range of environmental perception. With the movement of the robot, the camera on the joint can obtain environmental information in different directions and angles, thereby obtaining a more comprehensive perception field of view.

[0047] (2) Compared with the camera fixedly installed on the head of the robot, the camera on the joint can freely adjust the viewing angle and direction with the movement of the joint, thereby realizing more flexible environmental perception and operation. This flexibility enables the robot to adapt to more diverse tasks and working scenarios.

[0048] (3) Integrating camera data processing and motor control reduces the communication delay between sensors and controllers, improves the response speed and processing efficiency of the system. By processing camera data in real time, more accurate positioning and navigation can be achieved, enabling the robot to accurately identify obstacles and avoid them in complex environments. Timely obstacle avoidance can greatly improve the safety of humanoid robots, avoiding accidental collisions and injuries, and protecting the safety of operators and the surrounding environment. BRIEF DESCRIPTION OF DRAWINGS

[0049] Figure 1 is a structural schematic diagram of a perception system provided by the present application and located at the joint of a humanoid robot.

[0050] Figure 2 Figure 1 is a flowchart of a sensing method provided by the present application at a joint of a humanoid robot.

[0051] Figure 3 Figure 2 is a structural diagram of another sensing system provided by the present application at a joint of a humanoid robot.

[0052] In the drawings: 1-shoulder joint, 2-elbow joint, 3-wrist joint, 4-end effector, 5-camera, 6-calibration board. DETAILED DESCRIPTION

[0053] The technical solutions of the present application will be described clearly and completely below with reference to the accompanying drawings of the specification. Obviously, the described embodiments are only some of the embodiments of the present application, but not all the embodiments. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative work fall within the protection scope of the present application.

[0054] In the description of the present application, it should be understood that the terms "center", "longitudinal", "transverse", "upper", "lower", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer" and the like indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, and are only for the purpose of facilitating the description of the present application and simplifying the description, and do not indicate or imply that the device or element referred to must have a particular orientation, be constructed and operated in a particular orientation, and therefore cannot be understood as a limitation of the present application. In addition, the terms "first", "second" are only for the purpose of description and cannot be understood as indicating or implying relative importance or quantity or position.

[0055] In the description of the present application, it should be noted that unless otherwise explicitly specified and limited, the terms "mounting", "connecting", "connecting" should be understood broadly, for example, it can be fixedly connected, or it can be detachably connected, or integrally connected; it can be mechanically connected, or it can be electrically connected; it can be directly connected, or it can be indirectly connected through an intermediate medium, or it can be the communication inside two elements. For those skilled in the art, the specific meaning of the above terms in the present application can be understood according to the specific circumstances. Embodiment 1

[0056] A sensing system at a joint of a humanoid robot, as shown in Figure 1As shown, including humanoid robot mechanical arm, shoulder joint 1, elbow joint 2 and wrist joint 3 on the mechanical arm and end effector 4, camera 5 and processor are arranged on the end effector 4, and a calibration board 6 is further included. The camera 5 is composed of a binocular camera module and a TOF module, the processor calibrates the position, processes the data obtained by the camera 5 and outputs control instructions to control the end effector 4. Embodiment 2

[0057] The method for perceiving by the perception system of embodiment 1 is shown in the flow as follows Figure 2 As shown, it includes the following:

[0058] Step S1, install the binocular camera module and the TOF camera at the joint position of the robot, perform depth information fusion based on the binocular disparity map obtained by the binocular camera and the TOF depth image obtained based on the TOF, and obtain a fused depth image.

[0059] In this embodiment, the binocular camera module and the TOF camera are fixed on the end effector of the robot, and the binocular camera and the TOF camera image data are processed by the main control of the joint motor.

[0060] Specifically, when obtaining the binocular disparity map, the binocular camera needs to be calibrated first, including the calibration of the internal and external parameters, to ensure that the binocular camera can accurately obtain the depth information. Due to the process problems such as manufacturing and installation of the binocular camera lens, the image will be distorted, so it is necessary to introduce a non-linear distortion correction term in the ideal linear imaging model of the camera. In three-dimensional measurement, an important step is to calibrate the internal and external parameters related to the camera, including distortion parameters k 1, k 2 , k 3 , P 1, P 2 These parameters constitute the external parameters of the camera.

[0061] Specifically, since the TOF camera uses the time-of-flight principle to obtain depth information, the TOF depth image needs to be set according to the specific application scene, including laser power, integration time, frame rate and other parameters, in order to obtain clear and accurate depth images. In actual application, attention should be paid to the change of environmental light, and the depth image should be collected in a dark environment as much as possible, or other measures should be taken to reduce the influence of environmental light.

[0062] In this embodiment, the binocular disparity map and the TOF depth image are fused according to an optimization fusion algorithm based on energy minimization to obtain a target depth image energy function, and the target depth image energy function includes:

[0063]

[0064] in, D It is a depth image. E data Represents a data item. E smooth Indicates the smoothing term. λ 1 indicates the weight of the data item. λ 2 represents the weight of the smoothing term, ( u , v ) represents the pixel coordinates. ω ( u , v ) represents pixel weight, D s ( u , v () represents a binocular parallax map. D T ( u , v ) represents a TOF depth map. N Represents the neighborhood of adjacent pixels. Represents pixels in a depth image ( u , v The gradient at ().

[0065] The energy function of the target depth image is optimized according to an energy-minimization-based optimization fusion algorithm to obtain a fused depth image. The energy-minimization-based optimization fusion algorithm includes:

[0066]

[0067] in, This represents a fused depth image.

[0068] Step S2: Perform hand-eye calibration based on the camera, the installation position of the humanoid robot arm, and the calibration plate to obtain the coordinate transformation matrix from the camera coordinate system to the end effector of the robot arm;

[0069] In this embodiment, as Figure 1 As shown, the camera is located on the end effector of the humanoid robotic arm. The robot's base coordinate system is the shoulder joint coordinate system. During the calibration process, the relationship between the shoulder joint and the calibration plate remains unchanged. Solve for the positional relationship between the camera and the end effector.

[0070] In this embodiment, hand-eye calibration is performed based on the camera, the mounting position of the humanoid robot arm, and the calibration plate to obtain the coordinate transformation matrix from the camera coordinate system to the end effector of the robotic arm, including:

[0071]

[0072] In the formula, and are the coordinate transformation matrices of the relative motion between the camera and the end-effector from position 1 to position 2, is the coordinate transformation matrix required between the end-effector and the camera.

[0073] where, can be represented as the transformation product of two rigid bodies based on the camera world coordinate frame, can be represented as the transformation product of two rigid bodies based on the robot base coordinate frame:

[0074]

[0075] where, , and , are the pose of the camera with respect to the world coordinate frame and the pose of the end-effector of the robot arm with respect to the robot base coordinate frame, respectively.

[0076] From the rotation and translation matrices between the camera, the end-effector of the robot arm, and the base coordinate frame, the following equations can be obtained:

[0077]

[0078] where, R A , t A and R B , t B are the rotation and translation matrices of the relative motion between the camera and the end-effector from position 1 to position 2, R X , t X are the rotation and translation matrices between the camera and the end-effector, R is a 3 x 3 rotation matrix, and t is a 3 x 1 translation vector.

[0079] After expanding, the following equations are obtained:

[0080]

[0081] The Tsai-Lenz algorithm is used to solve the coordinate transformation rotation matrix R X :

[0082]

[0083] Thus, the coordinate transformation translation matrix of the camera and the end effector of the robot arm can be solved t X Finally, the coordinate transformation matrix of the camera and the end effector of the robot arm is obtained T X :

[0084]

[0085] In step S3, based on the forward kinematics of the robot, the conversion matrix of the end effector of the robot arm to the three joints of the shoulder, elbow and wrist of the robot arm is obtained, including:

[0086]

[0087] In this embodiment, the numbers of the shoulder joint, elbow joint and wrist joint or hip joint, knee joint and ankle joint are 0, 1 and 2 respectively, and the number of the end effector is 3, represents the transformation from joint i to joint i +1.

[0088] Based on the coordinate conversion matrix of the camera coordinate system to the end effector of the robot arm and the conversion matrix between the end effector of the robot arm and the shoulder, elbow and wrist joints of the robot arm, the coordinates in the camera coordinate system are converted into the coordinates in the end effector coordinate system of the robot arm and the coordinates in the three coordinate systems of the shoulder, elbow and wrist joints, the object distance information of the environment is obtained, and the coarse positioning and focusing functions of the robot are realized, including:

[0089]

[0090] P c For the coordinates in the camera coordinate system, P r For the coordinates in the end effector coordinate system of the robot arm, represents the transformation from the camera to the end effector, P w For the coordinates in the wrist joint coordinate system, P e For the coordinates in the elbow joint coordinate system, P s For the coordinates in the shoulder joint coordinate system.

[0091] In step S4, based on the coordinate conversion matrix of the camera coordinate system to the end effector of the robot arm and the conversion matrix between the end effector of the robot arm and the three joints of the shoulder, elbow and wrist of the robot arm, the coordinates in the camera coordinate system are converted into the coordinates in the end effector coordinate system of the robot arm and the coordinates in the three coordinate systems of the shoulder, elbow and wrist joints, the object distance information of the environment is obtained, and the coarse positioning and focusing functions of the robot are realized.

[0092] In the embodiment, the focusing function of the robot is realized based on the object distance information of the environment in the fused depth image, including:

[0093]

[0094] wherein, is the fused depth image matrix, d i,j represents the depth value of the pixel point in the i th row and the j th column of the image. m and n represent the height and width of the image, respectively.

[0095] In the fused depth image, a certain object is selected as the target, that is, in the fused depth image matrix it is embodied as a region with a depth value X × Y .

[0096] The depth information is converted into the corresponding focal length value using the intrinsic matrix of the binocular camera and a suitable optical formula. The depth value of the target object is Z , the camera intrinsic matrix is K , and the focal length f can be calculated by the following formula:

[0097]

[0098] wherein, α and β are the horizontal and vertical focal length parameters in the intrinsic matrix.

[0099] According to the calculated focal length value, the adjustment of the focal length is realized by controlling the motor or driver of the camera. Assuming that the motor or driver control parameter of the focal length adjustment is M , then the process of adjusting the focal length can be represented as:

[0100]

[0101] Specifically, the binocular camera will control the direction and angle of motor rotation according to the value of M , so as to realize the adjustment of the focal length.

[0102] In the embodiment, according to the coordinates of the object in the camera coordinate system, when the object is found, the distance position information of the object in the camera coordinate system d′ is obtained, and d′ is converted into distance position information in different joint coordinate systems d .

[0103] ​When an obstacle is detected, collision is avoided by reducing the speed of the robot or stopping movement;

[0104] According to the position of the obstacle relative to the robot, the turning angle of the robot is adjusted to bypass the obstacle;

[0105] After the obstacle disappears or the robot successfully avoids the obstacle, the normal moving speed and direction of the robot are restored. Embodiment 3

[0106] The perception system in this embodiment is located on the mechanical arm of the humanoid robot, including two mechanical arms of the humanoid robot, the right mechanical arm has a left shoulder joint 1, a left elbow joint 2, a left wrist joint 3, a left end effector 4 and a left camera 5, and the left mechanical arm has a shoulder joint 1, an elbow joint 2, a wrist joint 3, an end effector 4 and a right camera 5. In addition, the system also includes a calibration board 6 and a processor responsible for data processing.

[0107] In this embodiment, the camera 5 is used as a sensor of the humanoid robot, which can take advantage of its long detection distance and strong robustness to perform obstacle avoidance and positioning algorithms. The camera 5 is composed of a binocular camera module and a TOF module, and the processor calibrates the position, processes the data obtained by the camera 5 and outputs the control of the end effector 4. When installed, the camera and the mechanical hand point in the same direction. The mechanical arm hovers at a high altitude, so the camera and the mechanical hand point are used to detect the target in front and locate the obstacle target.

[0108] In this embodiment, a solid-state area array laser radar is used to detect obstacles within a horizontal angle of view of 54° and a vertical angle of view of 43°, and to provide accurate depth distance information. At the same time, the binocular camera module has a horizontal field of view of 80° and a focal length of 3.0mm, which is used to capture the position information of the obstacle. The camera modules on the left and right mechanical arms independently collect data, and the laser radar is connected to the joint module processor through a serial port. The joint module processor processes the point cloud information of the laser radar and the image information of the camera to realize comprehensive perception of the environment, and generates obstacle avoidance and positioning strategies accordingly.

[0109] In this embodiment, the joint module processor system uses an STM32 series processor as the main control chip, which can receive position information from the binocular camera module and the TOF module in real time. Through image registration and depth fusion algorithm, the processor generates the accurate position of the mechanical arm in 3D space. Based on these data, the processor plans the obstacle avoidance path of the robot in real time, and continuously updates the position information of the mechanical arm through matrix transformation algorithm to ensure that it will not collide with the obstacle. At the same time, the processor responds to the instructions from the robot main control system to adjust the speed and position of the mechanical arm in real time, ensuring the accuracy and safety of task execution.

[0110] In this embodiment, by combining the advantages of the binocular camera module and the TOF module, the binocular camera module can provide rich environmental position information, while the TOF module can provide accurate distance data through its efficient depth detection capability. The laser radar can scan a large range of areas in real time, further enhancing the obstacle avoidance accuracy, especially in complex or dynamic environments. This multi-sensor fusion technology greatly improves the adaptability and robustness of the robot in various scenarios.

[0111] In this embodiment, thanks to the high-speed computing capability of the joint module processor master chip STM32 processor, the robot can quickly respond to external obstacles and changes in a dynamic environment, and through real-time path planning and control algorithms, it can ensure efficient operation while avoiding obstacles. The multi-sensor data is fused through Kalman filtering to ensure that the robot can quickly and accurately respond in various scenarios. Especially during the movement of the robotic arm, the system can ensure the smoothness and reliability of the motion to prevent collisions with objects in the environment.

[0112] Although the embodiments of the present application have been shown and described above, it should be understood that the above embodiments are exemplary and should not be construed as limiting the present application, and those skilled in the art can make changes, modifications, replacements and variations to the above embodiments without departing from the principles and purposes of the present application within the scope of the present application. The scope of protection of the present application is defined by the claims and their equivalent technical solutions.

Claims

1. A method of sensing by a sensing system located at a joint of a humanoid robot, characterized in that, The perception system comprises humanoid robot limbs and joints, cameras, processors and calibration boards, the joints comprising shoulder joints, elbow joints, wrist joints and end effectors, and / or hip joints, knee joints, ankle joints and end effectors; The cameras are composed of a binocular camera module and a TOF module, the cameras and processors are located on the joints of the humanoid robot, the processors calibrate positions, process data obtained by the cameras and output control instructions to control the end effectors; The method comprises the following steps: S1, based on the binocular disparity map and the TOF depth image obtained by the cameras, depth information fusion is performed to obtain a fused depth image; S2, based on the cameras, the installation positions of the limbs of the humanoid robot and the calibration boards, position calibration is performed to obtain a coordinate conversion matrix from the camera coordinate system to the end effector of the robot arm; S3, based on the forward kinematics of the robot, a conversion matrix from the end effector to the joints of the limb where the end effector is located is obtained; S4, based on the coordinate conversion matrix from the camera coordinate system to the end effector of the robot arm and the conversion matrix from the end effector to the joints of the limb where the end effector is located, coordinates in the camera coordinate system are converted into coordinates in the end effector coordinate system and coordinates in the limb joint coordinate system, distance information of objects in the environment is obtained, and coarse positioning and focusing functions of the robot are realized; In step S1, depth information fusion is performed on the binocular disparity map and the TOF depth image based on an optimization fusion algorithm based on energy minimization to obtain a target depth image energy function, the target depth image energy function comprising: , wherein, D is a depth image, E data denotes a data term, E smooth denotes a smoothness term, λ 1 denotes a weight of the data term, λ 2 denotes a weight of the smoothness term, u , v ) denotes a pixel point coordinate, ω ( u , v ) denotes a pixel weight, D s ( u , v ) denotes a binocular disparity map, D T ( u , v ) denotes a TOF depth map, N denotes a neighborhood of neighboring pixels, denotes a gradient at a pixel ( u , v ) in the depth image, The optimization fusion algorithm based on energy minimization comprises: ， wherein, denotes a fused depth image; In step S2, based on the cameras, the installation positions of the limbs of the humanoid robot and the calibration boards, position calibration is performed to obtain a coordinate conversion matrix from the camera coordinate system to the end effector, and the formula is: ， wherein and are coordinate transformation matrices of the relative motion of the camera and the end effector between position 1 and position 2, respectively, is the coordinate transformation matrix required between the end effector and the camera.

2. The method of claim 1, wherein, The end effector comprises a robot hand and / or a robot foot.

3. The method of claim 1, wherein, The cameras are arranged in pairs, and each pair is installed on two upper limbs or lower limbs.

4. The method of claim 1, wherein, denotes the transformation product of two rigid bodies based on the camera world coordinate system, denotes the transformation product of two rigid bodies based on the robot base coordinate system: ， wherein, , and , are the poses of the camera with respect to the world coordinate frame and the pose of the end-effector with respect to the robot base coordinate frame, respectively.

5. The method of claim 1, wherein, According to the rotation and translation matrices between the cameras, the end effector and the base coordinate system, the following formula is obtained: ， wherein R A , t A and R B , t B are the rotation and translation matrices of the relative motion between the camera and the end-effector from position 1 to position 2, respectively, R X , t X are the rotation and translation matrices between the camera and the end-effector, R is a 3 x 3 rotation matrix, and t is a 3 x 1 translation vector; After expansion, the following equation is obtained: ， Solving the coordinate transformation rotation matrix of the camera and the end effector using Tsai-Lenz algorithm R X : ， from which the coordinate transformation translation matrix of the camera and the robot end effector is derived t X from which the coordinate transformation translation matrix of the camera and the robot end effector is derived T X : 。 6. The method of claim 1, wherein, In step S3, based on the forward kinematics of the robot, a conversion matrix from the end effector to the joints of the limb where the end effector is located is obtained, and the formula is: ， where the shoulder, elbow and wrist joints or the hip, knee and ankle joints are numbered 0, 1 and 2, respectively, and the end effector is numbered 3, represents the transformation from joint i to joint i + 1.

7. The method of claim 1, wherein, The calculation formula of step S4 is: ， P c is a coordinate in the camera coordinate system, P r is a coordinate in the end effector coordinate system of the robot arm, denotes the transformation from the camera to the end effector, P w is a coordinate in the wrist joint coordinate system, P e is a coordinate in the elbow joint coordinate system, P s is a coordinate in the shoulder joint coordinate system.

8. The method of claim 4, wherein, According to the coordinates of the object in the camera coordinate system, distance position information of the object in the camera coordinate system is acquired when the object is found d′ , the distance position information in the different joint coordinate systems is converted d′ d ;​ When an obstacle is detected, the robot's speed is reduced or stopped to avoid collision; According to the position of the obstacle relative to the robot, the steering angle of the robot is adjusted to make it bypass the obstacle; After the obstacle disappears or the robot successfully avoids the obstacle, the normal moving speed and direction of the robot are restored.

Images