Sensing system and method for joints of humanoid robot

Abstract

The invention discloses a sensing system and method for humanoid robot joints, and belongs to the technical field of humanoid robots, the system comprises humanoid robot four limbs and joints, a camera, a processor and a calibration board, the joints comprise shoulder joints, elbow joints, wrist joints and end actuators, and / or hip joints, knee joints, ankle joints and end actuators; the camera is composed of a binocular camera module and a TOF module, the camera and the processor are located on joints of the humanoid robot, and the processor calibrates the position, processes data obtained by the camera and outputs a control instruction to control the end effector. According to the invention, environment perception in a wider range can be realized, and the camera on the joint can freely adjust the visual angle and direction along with the movement of the joint, so that more flexible environment perception and operation are realized; camera data processing and motor control are integrated, the communication delay between the sensor and the controller is reduced, and the response speed and the processing efficiency of the system are improved.

Description

Technical Field

[0001] The present invention 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 Art

[0002] In recent years, humanoid robots have become one of the hot topics of research, with the goal of achieving flexible and efficient operation in human environments. To achieve this goal, humanoid robots need to have strong perception capabilities in order to interact with the environment safely, flexibly and efficiently.

[0003] Traditional humanoid robot perception systems usually rely on sensors fixed to the robot's head or body, such as binocular cameras or lidar. However, this arrangement has certain limitations when it comes to achieving all-round perception and operation of robot joints. Traditional humanoid robot perception methods also have certain limitations. For example, lidar is limited by its working principle; cameras require complex image processing algorithms and may face challenges when computing resources are limited; data obtained by sensors is usually sent to the robot's main controller such as the central control unit for processing, which has problems with communication delays and low data processing efficiency. Summary of the invention

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

[0005] The primary aspect of the present invention is to provide a perception system located at the joints of a humanoid robot, including the limbs and joints of the humanoid robot, a camera, a processor and a calibration board, the joints including shoulder joints, elbow joints, wrist joints and end effectors, and / or hip joints, knee joints, ankle joints and end effectors; the camera is composed of a binocular camera module and a TOF module, the camera and the processor are located on the joints of the humanoid robot, the processor calibrates the position, processes the data obtained by the camera and outputs control instructions to control the end effector.

[0006] Furthermore, the end effector includes a robot arm and / or a robot foot.

[0007] Furthermore, the cameras are arranged in pairs, and each pair is installed on two upper limbs or lower limbs respectively.

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

[0009] S1 performs depth information fusion based on the binocular disparity map and TOF depth image obtained by the camera to obtain a fused depth image;

[0010] S2 performs position calibration based on the camera, the installation positions of the limbs of the humanoid robot and the calibration plate, and obtains the coordinate transformation matrix from the camera coordinate system to the robot end effector;

[0011] S3 obtains the transformation matrix between the end effector and the limb joint where the end effector is located based on the forward kinematics of the robot;

[0012] Based on the coordinate transformation matrix from the camera coordinate system to the end effector of the robot's mechanical arm, and the transformation matrix from the end effector to the limb joint where the end effector is located, S4 transforms the coordinates in the camera coordinate system into the coordinates in the end effector coordinate system and the coordinates in the limb joint coordinate system, obtains the distance information of objects in the environment, and realizes the coarse positioning and focusing functions of the robot.

[0013] Furthermore, step S1 performs depth information fusion on the binocular disparity map and the TOF depth image according to an optimization fusion algorithm based on energy minimization to obtain a target depth image energy function, wherein the target depth image energy function includes:

[0014] E(D)=λ 1 E data (D)+λ 2 E smooth (D)

[0015]

[0016] Where D is the depth image, E data Represents a data item, E smooth represents the smoothness term, λ 1 represents the weight of the data item, λ 2 represents the weight of the smoothing term, (u, v) represents the pixel coordinates, ω(u, v) represents the pixel weight, D s (u,v) represents the binocular disparity map, D T (u, v) represents the TOF depth map, N represents the neighborhood of adjacent pixels, and |▽D(u, v)| represents the gradient at the pixel (u, v) in the depth image.

[0017] All physical quantities in the present invention, if any, are expressed in SI units.

[0018] Furthermore, the energy minimization-based optimization fusion algorithm includes:

[0019]

[0020] Among them, D *Represents the fused depth image.

[0021] Furthermore, step S2 performs position calibration based on the camera, the installation positions of the limbs of the humanoid robot and the calibration plate, and the formula for obtaining the coordinate transformation matrix from the camera coordinate system to the robot end effector is:

[0022]

[0023] In the formula, and are the coordinate transformation matrices for the relative motion of 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.

[0024] Furthermore, 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:

[0025]

[0026]

[0027] In the formula, and are the position of the camera relative to the world coordinate system, and the position of the end effector relative to the robot base coordinate system.

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

[0029]

[0030] Among them, 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×3 rotation matrix, and t is a 3×1 translation vector.

[0031] After expansion, we get the following equation:

[0032] R A R X =R X R B

[0033] R A tX +t A =R X t B +t X

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

[0035]

[0036] From this, the coordinate transformation translation matrix t of the camera and the end effector of the robot can be obtained: X , and finally obtain the coordinate transformation matrix T of the camera and the end effector of the robot X :

[0037]

[0038] Furthermore, step S3 obtains the transformation 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:

[0039]

[0040] The shoulder joint, elbow joint and wrist joint or hip joint, knee joint and ankle joint are numbered 0, 1 and 2 respectively, the end effector is numbered 3, and T i i+1 represents the transformation from joint i to joint i+1.

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

[0042]

[0043] P e =T 1 2 P w

[0044]

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

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

[0047] When an obstacle is detected, the robot is prevented from collision by reducing its speed or stopping its movement.

[0048]

[0049] Depending on the position of the obstacle relative to the robot, adjust the robot's steering angle to make it go around the obstacle.

[0050]

[0051] After the obstacle disappears or the robot successfully avoids the obstacle, the robot resumes its normal moving speed and direction.

[0052] Compared with the prior art, the perception system and method at the joints of a humanoid robot provided by the present invention have the following advantages:

[0053] (1) Mounting cameras on the joints of the robot can achieve a wider range of environmental perception. As the robot moves, the cameras on the joints can obtain environmental information in different directions and angles, thereby obtaining a more comprehensive perception field of view.

[0054] (2) Compared with cameras fixed on the robot head, cameras on joints can freely adjust their viewing angles and directions as the joints move, thus achieving more flexible environmental perception and operation. This flexibility allows the robot to adapt to a wider variety of tasks and work scenarios.

[0055] (3) Integrating camera data processing with motor control reduces the communication delay between the sensor and the controller, improving the system’s response speed and processing efficiency. 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, avoid accidental collisions and injuries, and protect the safety of the operator and the surrounding environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 It is a structural schematic diagram of a perception system located at the joints of a humanoid robot provided by the present invention.

[0057] Figure 2 It is a flow chart of a sensing method at a joint of a humanoid robot provided by the present invention.

[0058] Figure 3 It is a schematic structural diagram of another sensing system located at the joints of a humanoid robot provided by the present invention.

[0059] Figure numerals: 1-shoulder joint, 2-elbow joint, 3-wrist joint, 4-end effector, 5-camera, 6-calibration plate. DETAILED DESCRIPTION

[0060] The technical solution of the present invention is described clearly and completely below in conjunction with the accompanying drawings of the specification. Obviously, the described implementation is only a part of the implementation of the present invention, not all of the implementations. Based on the implementation of the present invention, all other implementations obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0061] In the description of the present invention, it should be understood that the terms "center", "longitudinal", "lateral", "upper", "lower", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside" and the like indicate positions or positional relationships based on the positions or positional relationships shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present invention. In addition, the terms "first" and "second" are used for descriptive purposes only, and cannot be understood as indicating or implying relative importance, quantity or position.

[0062] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0063] Example 1

[0064] A sensing system located at the joints of a humanoid robot, such as Figure 1As shown, it includes a humanoid robot arm, a shoulder joint 1, an elbow joint 2, a wrist joint 3 and an end effector 4 on the robot arm, a camera 5 and a processor are provided on the end effector 4, and also includes a calibration board 6. 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 a control instruction to control the end effector 4.

[0065] Example 2

[0066] The sensing method of the sensing system of Example 1 is adopted, and the process is as follows Figure 2 As shown, including the following:

[0067] Step S1, installing a binocular camera module and a TOF camera at the joint position of the robot, performing depth information fusion based on a binocular disparity map obtained by the binocular camera and a TOF depth image obtained based on TOF, and obtaining a fused depth image.

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

[0069] Specifically, when obtaining the binocular disparity map, the binocular camera needs to be calibrated first, including the calibration of internal and external parameters, to ensure that the binocular camera can accurately obtain depth information. Due to the production and installation process of the binocular camera lens, the image is distorted. Therefore, it is necessary to introduce nonlinear distortion correction terms into 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 the distortion parameter k. 1 , k 2 , k 3 , p 1 , p 2 , these parameters constitute the extrinsic parameters of the camera.

[0070] Specifically, since the TOF camera uses the time-of-flight principle to obtain depth information, the acquisition of TOF depth images requires parameter settings according to the specific application scenario, including laser power, integration time, frame rate and other parameters, in order to obtain clear and accurate depth images. In actual applications, it is necessary to pay attention to changes in ambient lighting, try to collect depth images in a darker environment, or take other measures to reduce the impact of ambient light.

[0071] In this embodiment, the binocular disparity map and the TOF depth image are fused with depth information 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:

[0072] E(D)=λ 1 Edata (D)+λ 2 E smooth (D)

[0073]

[0074] Where D is the depth image, E data Represents a data item, E smooth represents the smoothness term, λ 1 represents the weight of the data item, λ 2 represents the weight of the smoothing term, (u, v) represents the pixel coordinates, ω(u, v) represents the pixel weight, D s (u,v) represents the binocular disparity map, D T (u, v) represents the TOF depth map, N represents the neighborhood of adjacent pixels, and |▽D(u, v)| represents the gradient at the pixel (u, v) in the depth image.

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

[0076]

[0077] Among them, D * Represents the fused depth image.

[0078] Step S2, performing hand-eye calibration based on the camera, the installation position of the humanoid robot arm and the calibration plate, and obtaining a coordinate transformation matrix from the camera coordinate system to the robot arm end effector;

[0079] In this embodiment, if Figure 1 As shown, the camera is located on the end effector of the humanoid robot, and the robot 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, and the positional relationship between the camera and the end effector is solved.

[0080] In this embodiment, hand-eye calibration is performed 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 robot arm end effector, including:

[0081]

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

[0083] in, It can be expressed as the transformation product of two rigid bodies based on the camera world coordinate system. It can be expressed as the product of the transformations of two rigid bodies based on the robot base coordinate system:

[0084]

[0085] In the formula, and They are the position of the camera relative to the world coordinate system and the position of the end effector of the robotic arm relative to the robot base coordinate system.

[0086] According to the rotation and translation matrices between the camera, the end effector of the robotic arm, and the base coordinate system, the following formula can be obtained:

[0087]

[0088] Among them, R A ,t A and R B ,t B The camera and the end effector of the robot arm are Figure 2 The rotation and translation matrix of the relative motion between position 1 and position 2, R X ,t X are the rotation and translation matrices between the camera and the end effector of the robotic arm, R is a 3×3 rotation matrix, and t is a 3×1 translation vector.

[0089] After expansion, we get the following equation:

[0090] R A R X =R X R B

[0091] R A t X +t A =R X t B +t X

[0092] Use the Tsai-Lenz algorithm to solve the coordinate transformation rotation matrix R of the camera and the end effector of the robotic arm X :

[0093]

[0094]

[0095] From this, the coordinate transformation translation matrix t of the camera and the end effector of the robot can be obtained: X, and finally obtain the coordinate transformation matrix T of the camera and the end effector of the robot X :

[0096]

[0097] Step S3, based on the forward kinematics of the robot, obtain the transformation matrix between the end effector of the robot arm and the three joints of the shoulder, elbow and wrist of the robot arm, including:

[0098]

[0099] In this embodiment, the shoulder joint, elbow joint and wrist joint are numbered 0, 1 and 2 respectively, the end effector is numbered 3, and T i i+1 represents the transformation from joint i to joint i+1.

[0100] Based on the coordinate conversion matrix from the camera coordinate system to the robot's end effector and the conversion matrix between the end effector and the shoulder, elbow and wrist joint of the robot, the coordinates in the camera coordinate system are converted into the coordinates in the robot's end effector coordinate system and the shoulder, elbow and wrist joint coordinate system, and the distance information of the objects in the environment is obtained to realize the coarse positioning and focusing functions of the robot, including:

[0101]

[0102] P e =T 1 2 P w

[0103]

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

[0105] Step S4, based on the coordinate transformation matrix from the camera coordinate system to the end effector of the robot's mechanical arm and the transformation matrix from the end effector of the mechanical arm to the three joints of the mechanical arm shoulder, elbow, and wrist, transform the coordinates in the camera coordinate system into the coordinates in the end effector coordinate system of the robot's mechanical arm and the coordinates in the three joints of the shoulder, elbow, and wrist, obtain the distance information of objects in the environment, and realize the coarse positioning and focusing functions of the robot.

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

[0107]

[0108] Among them, D d * is the fused depth image matrix, d i,j Represents the depth value of the pixel at the i-th row and j-th column in the image. m and n represent the height and width of the image respectively.

[0109] In the fused depth image, select an object as the target, that is, in the fused depth image matrix D d * It is represented as an area X×Y with depth values.

[0110] Use the intrinsic parameter matrix of the stereo camera and the appropriate optical formula to convert the depth information into the corresponding focal length value. The depth value of the target object is Z, the camera intrinsic parameter matrix is ​​K, and the focal length f can be calculated by the following formula:

[0111]

[0112] Among them, α and β are the horizontal and vertical focal length parameters in the intrinsic parameter matrix.

[0113] According to the calculated focal length value, the focal length is adjusted by controlling the motor or driver of the camera. Assuming that the motor or driver control parameter for focal length adjustment is M, the process of adjusting the focal length can be expressed as:

[0114] M=f desired -f current

[0115] Specifically, the binocular camera will control the direction and angle of the motor rotation according to the value of M, thereby adjusting the focal length.

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

[0117] When an obstacle is detected, the robot is prevented from collision by reducing its speed or stopping its movement;

[0118]

[0119] According to the position of the obstacle relative to the robot, adjust the robot's steering angle to make it bypass the obstacle;

[0120]

[0121] After the obstacle disappears or the robot successfully avoids the obstacle, the robot resumes its normal moving speed and direction.

[0122] Example 3

[0123] The perception system in this embodiment is located on the robotic arm of the humanoid robot, including two robotic arms of the humanoid robot. The right robotic 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 robotic 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.

[0124] In this embodiment, the camera 5 is used as a sensor of the humanoid robot, and its long detection distance and strong robustness can be used to execute obstacle avoidance and positioning algorithms. The camera 5 consists of a binocular camera module and a TOF module. The processor calibrates the position, processes the data obtained by the camera 5, and outputs the control end effector 4. When installed, the camera and the manipulator point in the same direction. The manipulator hovers at a higher place, so the camera and the manipulator point to detect the target in front and locate the obstacle target.

[0125] In this embodiment, a solid-state array laser radar is used to detect obstacles within a horizontal viewing angle of 54° and a vertical viewing angle of 43°, and 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 location information of obstacles. The camera modules on the left and right robotic arms collect data independently, and the laser radar is connected to the joint module processor through a serial port. The joint module processor realizes a comprehensive perception of the environment by processing the point cloud information of the laser radar and the image information of the camera, and generates obstacle avoidance and positioning strategies accordingly.

[0126] In this embodiment, the joint module processor system main control chip adopts the STM32 series processor, which can receive the position information from the binocular camera module and the TOF module in real time. Through image registration and deep fusion algorithm, the processor generates the precise position of the robot 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 robot arm through the matrix transformation algorithm to ensure that it does not collide with obstacles. At the same time, the processor responds to the instructions from the robot main control system, adjusts the speed and position of the robot arm in real time, and ensures the accuracy and safety of task execution.

[0127] In this embodiment, by combining the advantages of the binocular camera module and the TOF module, the binocular camera module can provide rich environmental location information, while the TOF module provides accurate distance data through its efficient depth detection capability. The laser radar can scan a large area 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 a variety of scenarios.

[0128] In this embodiment, thanks to the high-speed computing power of the joint module processor main control 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, ensure efficient operation while avoiding obstacles. Multi-sensor data is fused through Kalman filtering to ensure that the robot can respond quickly and accurately in various scenarios. Especially during the movement of the robotic arm, the system can ensure the stability and reliability of the movement and prevent collisions with objects in the environment.

[0129] Although the embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and cannot be understood as limiting the present invention. A person skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention without departing from the principles and purpose of the present invention. The protection scope of the present invention is defined by the claims and their equivalent technical solutions.

Claims

1. A perception system located at the joints of a humanoid robot, It is characterized in that The robot comprises limbs and joints of a humanoid robot, a camera, a processor and a calibration plate, wherein the joints comprise 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 processor are located on the joints of the humanoid robot. The processor calibrates the position, processes the data obtained by the camera, and outputs control instructions to control the end effector.

2. The sensing system according to claim 1, It is characterized in that The end effector includes a robot arm and / or a robot foot.

3. The sensing system according to claim 1, It is characterized in that The cameras are arranged in pairs, and each pair is installed on two upper limbs or lower limbs respectively.

4. A method for sensing by a sensing system according to any one of claims 1 to 3, It is characterized in that The following steps are involved: S1 performs depth information fusion based on the binocular disparity map and TOF depth image obtained by the camera to obtain a fused depth image; S2 performs position calibration based on the camera, the installation positions of the limbs of the humanoid robot and the calibration plate, and obtains the coordinate transformation matrix from the camera coordinate system to the robot end effector; S3 obtains the transformation matrix between the end effector and the limb joint where the end effector is located based on the forward kinematics of the robot; Based on the coordinate transformation matrix from the camera coordinate system to the end effector of the robot's mechanical arm, and the transformation matrix from the end effector to the limb joint where the end effector is located, S4 transforms the coordinates in the camera coordinate system into the coordinates in the end effector coordinate system and the coordinates in the limb joint coordinate system, obtains the distance information of objects in the environment, and realizes the coarse positioning and focusing functions of the robot.

5. The method according to claim 4, It is characterized in that Step S1 performs depth information fusion on the binocular disparity map and the TOF depth image according to an optimization fusion algorithm based on energy minimization to obtain a target depth image energy function, wherein the target depth image energy function includes: E(D)=λ 1 AND data (D)+λ 2 AND smooth (D) Where D is the depth image, E data Represents a data item, E smooth represents the smoothness term, λ 1 represents the weight of the data item, λ 2 represents the weight of the smoothing term, (u, v) represents the pixel coordinates, ω(u, v) represents the pixel weight, D s (u,v) represents the binocular disparity map, D T (u,v) represents the TOF depth map, N represents the neighborhood of adjacent pixels, Represents the gradient at pixel (u,v) in the depth image.

6. The method according to claim 5, It is characterized in that The optimization fusion algorithm based on energy minimization includes: Among them, D * represents the fused depth image; Step S2 performs position calibration based on the camera, the installation positions of the humanoid robot's limbs and the calibration plate, and obtains the formula for the coordinate transformation matrix from the camera coordinate system to the robot's end effector: In the formula, and are the coordinate transformation matrices for the relative motion of 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.

7. The method according to claim 6, It is characterized in that 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: In the formula, and are the position of the camera relative to the world coordinate system, and the position of the end effector relative to the robot base coordinate system.

8. The method according to claim 4, It is characterized in that According to the rotation and translation matrices between the camera, end effector, and base coordinate system, the following formula is obtained: Among them, 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 is the rotation and translation matrix between the camera and the end effector, R is a 3×3 rotation matrix, and t is a 3×1 translation vector; After expansion, we get the following equation: R A R X =R X R B R A t X +t A =R X t B +t X ; Use the Tsai-Lenz algorithm to solve the coordinate transformation rotation matrix R of the camera and the end effector X : From this, we can calculate the coordinate transformation translation matrix t of the camera and the end effector of the robot arm. X , and finally obtain the coordinate transformation matrix T of the camera and the end effector of the robot X :

9. The method according to claim 4, It is characterized in that Step S3 obtains the transformation matrix between the end effector and the three joints of the limb where the end effector is located, namely, the shoulder, elbow, wrist or the hip, knee, and ankle, based on the forward kinematics of the robot, including: The shoulder joint, elbow joint and wrist joint or hip joint, knee joint and ankle joint are numbered 0, 1 and 2 respectively, the end effector is numbered 3, and T i i+1 represents the transformation from joint i to joint i+1.

10. The method according to claim 4, It is characterized in that The calculation formula of step S4 is: P c is the coordinate in the camera coordinate system, P r is the coordinate of the robot arm end effector coordinate system, represents the transformation from the camera to the end effector, P w is the coordinate in the wrist joint coordinate system, P e is the coordinate in the elbow joint coordinate system, P s is the coordinate in the shoulder joint coordinate system.

11. The method according to claim 4, It is characterized in that According to the coordinates of the object in the camera coordinate system, when the object is found, the distance position information d′ of the object in the camera coordinate system is obtained, and d is converted into the distance position information d in different joint coordinate systems; When an obstacle is detected, the robot is prevented from collision by reducing its speed or stopping its movement; According to the position of the obstacle relative to the robot, adjust the robot's steering angle to make it bypass the obstacle; After the obstacle disappears or the robot successfully avoids the obstacle, the robot resumes its normal moving speed and direction.

Images