Attention-based B-mode teleoperation human-machine sharing control system and method

Abstract

This invention discloses a human-machine shared control system and method for B-mode ultrasound remote operation based on an attention mechanism. The system acquires 3D point clouds using a stereo camera; obtains the user's focus point position using VR glasses; compresses the point cloud data based on an attention mechanism, dividing the point cloud data into a high-precision display region A and a low-precision display region B. The center of region A is determined based on the user's focus point position, and the point clouds of regions A and B are compressed using different resolutions. The system remotely controls the movement of a robotic arm using a Touch X joystick; the robotic arm drives the stereo camera to perform B-mode ultrasound scanning; and it acquires the operator's position control commands and makes real-time adjustments based on the contact force feedback between the robotic arm's end effector and the human body. This invention achieves higher compression efficiency and improves the efficiency and safety of remote operation.

Description

Technical Field

[0001] This invention relates to the field of teleoperation control technology, specifically to a human-machine shared control system and method for B-mode teleoperation based on an attention mechanism. Background Technology

[0002] Currently, enabling robots to make accurate and efficient decisions in unstructured environments remains a significant challenge. In such environments, robots often face complex terrain, obstacles, and changing situations, requiring a high degree of intelligence and flexibility to cope. Teleoperation allows operators to remotely control robots to perform tasks without being directly present in complex or dangerous environments. This approach protects operator safety and improves task efficiency, as the robot can leverage the operator's intelligence to handle unexpected situations and execute complex tasks.

[0003] Currently, many remote operating systems do not effectively utilize the operator's intelligence to further reduce their workload and improve task completion efficiency and safety. In these systems, the operator often only operates the robot's control system and is unable to actively and effectively control the robot's sensing system.

[0004] For example, the robot teleoperation system and method based on electromagnetic force feedback and augmented reality, which has patent publication number CN110815258A, uses the operator's gestures and voice information to extract control commands through reasoning and guide the virtual robot to move. The real robot receives data through the Internet to replicate the virtual robot's movements. However, this system only allows the operator to complete the command control and does not make the teleoperation system more intelligent by utilizing the operator's attention.

[0005] For example, a control device and equipment for a bilateral teleoperation system, as disclosed in patent publication number CN110007601A, improves the accuracy of the bilateral teleoperation medical system robot in tracking the motion state of the target environment by sending the target force feedback value to the slave robot of the bilateral teleoperation system and then feeding it back to the operator. However, this device only provides one-way feedback of the robot's target force and does not utilize the operator's attention to provide feedback to the teleoperation system, resulting in low efficiency of the teleoperation system. Summary of the Invention

[0006] To overcome the shortcomings and deficiencies of existing technologies, this invention provides a human-machine shared control system and method for B-mode teleoperation based on an attention mechanism. This invention extracts the operator's attention information and then uses this information to dynamically adjust the 3D point cloud compression process to achieve higher compression efficiency, thereby increasing the frame rate of the environment presentation and making the teleoperation process safer and smoother. The intelligent control module intelligently adjusts the contact force and contact position based on the operator's position control commands and scans the human body surface. This invention effectively reduces the operator's burden while ensuring the efficiency and safety of task completion, and can be applied to robot teleoperation systems.

[0007] To achieve the above objectives, the present invention adopts the following technical solution:

[0008] This invention provides a B-mode teleoperation human-machine sharing control system based on an attention mechanism, comprising: VR glasses, Touch X joystick, robotic arm, stereo camera, point cloud compression module, and intelligent control module;

[0009] The stereo camera is used to scan the human body and acquire 3D point clouds;

[0010] The VR glasses are used to render 3D point clouds and extract the user's focus points.

[0011] The point cloud compression module is used to compress point clouds based on an attention mechanism, dividing the point cloud data into a high-precision display area A and a low-precision display area B. The center of area A is determined according to the user's point of interest, and the point clouds of area A and area B are compressed using different resolutions respectively.

[0012] The Touch X joystick is wirelessly connected to the robotic arm for remote control of the robotic arm's movement;

[0013] The robotic arm is connected to a stereo camera, and the robotic arm drives the stereo camera to perform a B-mode ultrasound scan.

[0014] The intelligent control module is used to acquire the operator's position control commands and make real-time adjustments based on the contact force feedback between the robotic arm end and the human body.

[0015] As a preferred technical solution, region A occupies a smaller portion of the point cloud data, while region B is the remaining portion of the point cloud data after region A is removed, and occupies the majority of the point cloud data. Region B adopts a linear gradient approach, and the point cloud data within region B is linearly adjusted according to the distance between region B and region A, gradually reducing its resolution.

[0016] As a preferred technical solution, the point cloud compression module is used for point cloud compression based on an attention mechanism, specifically including:

[0017] EyePose, which acquires eye-tracking data from VR glasses, and InCloud, which acquires point cloud data.

[0018] The EyesPose data is set as the center of region A. The user's attention point is obtained based on the EyesPose data. The position and size of region A are adjusted according to the user's attention point.

[0019] Iterate through all points in the input point cloud. For each point P, calculate its distance d from the center of the sphere in region A. Calculate the resolution at point P based on the distance d and convert it into the retention probability of point P. Determine whether to retain point P based on probability sampling. If point P is retained, encode it into the output point cloud OutCloud. Otherwise, continue iterating until the entire point cloud has been traversed.

[0020] As a preferred technical solution, for each point P, the distance d between it and the center of the sphere in region A is calculated. Based on the distance d, the resolution at point P is calculated and converted into the retention probability of point P, specifically including:

[0021] The point cloud resolution Res of region A A For Res max ;

[0022] For any point P(x, y, z) in region B, the distance between point P and the center of the sphere in region A is calculated using the following formula, expressed as:

[0023]

[0024] The temporary resolution is:

[0025] Res tmp =Res max -(Res max -Res min )*δ*d

[0026] The resolution at point P is:

[0027] Res B =max(Res min Res tmp )

[0028] The probability of retaining point P is:

[0029]

[0030] Among them, P keep Represents the probability of point P being retained, δ represents the rate of change of resolution, and Res min Res max These represent the minimum and maximum values ​​of the point cloud resolution range, respectively.

[0031] As a preferred technical solution, the intelligent control module is used to acquire the operator's position control commands and make real-time adjustments based on the contact force feedback between the robotic arm's end effector and the human body, specifically including:

[0032] Mapping the displacement increment of the Touch X joystick to the coordinate system of the robotic arm's end effector is expressed as:

[0033]

[0034] Where, ΔX T ΔX represents the displacement increment of the Touch X joystick. TER This represents the displacement increment in the coordinate system of the robotic arm's end effector. Let be the rotation matrix from the robot's base coordinate system to the coordinate system of the robotic arm's end effector. This is the rotation matrix from the Touch X joystick base coordinate system to the robot base coordinate system;

[0035] The interaction force between the robotic arm and the environment is calculated as follows:

[0036]

[0037]

[0038] Among them, F s F is the force detected by the force sensor at the end effector of the robotic arm. TC F represents the force generated by the tool's weight in the tool's center-of-mass coordinate system. RE0 This represents the interaction force between the robotic arm and its environment. Let be the rotation matrix of the tool coordinate system relative to the force sensor coordinate system. This is the rotation matrix from the robot arm's end-effector coordinate system to the force sensor coordinate system;

[0039] The force control algorithm is constructed as follows:

[0040]

[0041] Among them, X d , These represent the desired position, velocity, and acceleration of the robotic arm's end effector, X0, ... These represent the actual position, velocity, and acceleration of the robotic arm's end effector, F. REd Let M, D, and K be the desired interaction force, and let them be force controller parameters, which are diagonal matrices with positive constants as diagonal elements.

[0042] The final control value is obtained by adding the control value of the Touch X joystick to the control value of the force controller, and is expressed as:

[0043] ΔX=ΔX TERS′+ΔX FR S

[0044]

[0045]

[0046]

[0047]

[0048] Where dt is the control time interval;

[0049] Control is achieved by transforming the control quantity ΔX based on the robot's end-effector coordinate system to the robot's base coordinate system, specifically as follows:

[0050]

[0051] Among them, X cur This is the current position of the robotic arm. This is the rotation matrix from the end effector of the robotic arm to the base coordinate system of the robotic arm;

[0052] Let X be the desired position of the robotic arm's next move. d Transforming to the camera coordinate system yields the coordinates of that point in the camera coordinate system:

[0053]

[0054] By finding X C The normal vector N corresponding to the point whose Euclidean distance to all points in the 3D point cloud model is the minimum. C As the normal vector of the desired point, normal vector N C Transform to the robot's base coordinate system to obtain the desired normal vector:

[0055]

[0056] in, This is the rotation matrix from the robot's base coordinate system to the camera coordinate system;

[0057] The desired robotic arm end effector posture is obtained based on the desired normal vector, specifically expressed as:

[0058] R = arctan(-N) R [1], -N R [2])

[0059] P = -arcsin(-N) R [0])

[0060] γ = 0

[0061] Where, NR Let R, P, and Y be the normal vector of the object to be manipulated corresponding to the desired position in the robot's end effector coordinate system under the robot's base coordinate system, and let R, P, and Y be the desired values ​​of the Euler angles of the robot's end effector.

[0062] By the desired position X of the end d The desired angles of each joint are obtained by inverse kinematics of the desired end-effector postures R, P, and Y, which control the movement of the robotic arm.

[0063] This invention also provides a human-machine shared control method for B-mode teleoperation based on an attention mechanism, comprising the following steps:

[0064] 3D point cloud acquisition based on stereo camera;

[0065] Based on VR glasses, the location of the user's focus point can be obtained;

[0066] Point cloud compression is performed based on an attention mechanism. The point cloud data is divided into a high-precision display area A and a low-precision display area B. The center of area A is determined according to the user's point of interest. The point clouds of area A and area B are compressed using different resolutions respectively.

[0067] Remote control of robotic arm movement based on Touch X joystick;

[0068] A robotic arm drives a stereo camera to perform ultrasound scanning tasks.

[0069] It acquires the operator's position control commands and makes real-time adjustments based on the contact force feedback between the robotic arm's end effector and the human body.

[0070] As a preferred technical solution, region A occupies a smaller portion of the point cloud data, while region B is the remaining portion of the point cloud data after region A is removed, and occupies the majority of the point cloud data. Region B adopts a linear gradient approach, and the point cloud data within region B is linearly adjusted according to the distance between region B and region A, gradually reducing its resolution.

[0071] As a preferred technical solution, the point cloud compression based on the attention mechanism specifically includes:

[0072] EyePose, which acquires eye-tracking data from VR glasses, and InCloud, which acquires point cloud data.

[0073] The EyesPose data is set as the center of region A. The user's attention point is obtained based on the EyesPose data. The position and size of region A are adjusted according to the user's attention point.

[0074] Iterate through all points in the input point cloud. For each point P, calculate its distance d from the center of the sphere in region A. Calculate the resolution at point P based on the distance d and convert it into the retention probability of point P. Determine whether to retain point P based on probability sampling. If point P is retained, encode it into the output point cloud OutCloud. Otherwise, continue iterating until the entire point cloud has been traversed.

[0075] As a preferred technical solution, for each point P, the distance d between it and the center of the sphere in region A is calculated. Based on the distance d, the resolution at point P is calculated and converted into the retention probability of point P, specifically including:

[0076] The point cloud resolution Res of region A A For Res max ;

[0077] For any point P(x, y, z) in region B, the distance between point P and the center of the sphere in region A is calculated using the following formula, expressed as:

[0078]

[0079] The temporary resolution is:

[0080] Res tmp =Res max -(Res max -Res min )*δ*d

[0081] The resolution at point P is:

[0082] Res B =max(Res min Res tmp )

[0083] The probability of retaining point P is:

[0084]

[0085] Among them, P keep Represents the probability of point P being retained, δ represents the rate of change of resolution, and Res min Res max These represent the minimum and maximum values ​​of the point cloud resolution range, respectively.

[0086] As a preferred technical solution, the operator's position control commands are obtained, and real-time adjustments are made based on the contact force feedback between the robotic arm's end effector and the human body. Specifically, this includes:

[0087] Mapping the displacement increment of the Touch X joystick to the coordinate system of the robotic arm's end effector is expressed as:

[0088]

[0089] Where, ΔX T ΔX represents the displacement increment of the Touch X joystick. TER This represents the displacement increment in the coordinate system of the robotic arm's end effector. Let be the rotation matrix from the robot's base coordinate system to the coordinate system of the robotic arm's end effector. This is the rotation matrix from the Touch X joystick base coordinate system to the robot base coordinate system;

[0090] The interaction force between the robotic arm and the environment is calculated as follows:

[0091]

[0092]

[0093] Among them, F s F is the force detected by the force sensor at the end effector of the robotic arm. TC F represents the force generated by the tool's weight in the tool's center-of-mass coordinate system. RE0 This represents the interaction force between the robotic arm and its environment. Let be the rotation matrix of the tool coordinate system relative to the force sensor coordinate system. This is the rotation matrix from the robot arm's end-effector coordinate system to the force sensor coordinate system;

[0094] The force control algorithm is constructed as follows:

[0095]

[0096] Among them, X d , These represent the desired position, velocity, and acceleration of the robotic arm's end effector, X0, ... These represent the actual position, velocity, and acceleration of the robotic arm's end effector, F. REd Let M, D, and K be the desired interaction force, and let them be force controller parameters, which are diagonal matrices with positive constants as diagonal elements.

[0097] The final control value is obtained by adding the control value of the Touch X joystick to the control value of the force controller, and is expressed as:

[0098] ΔX=ΔX TER S′+ΔX FR S

[0099]

[0100]

[0101]

[0102]

[0103] Where dt is the control time interval;

[0104] Control is achieved by transforming the control quantity ΔX based on the robot's end-effector coordinate system to the robot's base coordinate system, specifically as follows:

[0105]

[0106] Among them, X cur This is the current position of the robotic arm. This is the rotation matrix from the end effector of the robotic arm to the base coordinate system of the robotic arm;

[0107] Let X be the desired position of the robotic arm's next move. d Transforming to the camera coordinate system yields the coordinates of that point in the camera coordinate system:

[0108]

[0109] By finding X C The normal vector N corresponding to the point whose Euclidean distance to all points in the 3D point cloud model is the minimum. C As the normal vector of the desired point, normal vector N C Transform to the robot's base coordinate system to obtain the desired normal vector:

[0110]

[0111] in, This is the rotation matrix from the robot's base coordinate system to the camera coordinate system;

[0112] The desired robotic arm end effector posture is obtained based on the desired normal vector, specifically expressed as:

[0113] R = arctan(-N) R [1], -N R [2])

[0114] P = -arcsin(-N) R [0])

[0115] Y = 0

[0116] Where, N R Let R, P, and Y be the normal vector of the object to be manipulated corresponding to the desired position in the robot's end effector coordinate system under the robot's base coordinate system, and let R, P, and Y be the desired values ​​of the Euler angles of the robot's end effector.

[0117] By the desired position X of the end d The desired angles of each joint are obtained by inverse kinematics of the desired end-effector postures R, P, and Y, which control the movement of the robotic arm.

[0118] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0119] (1) This invention dynamically adjusts the robot’s perception behavior by extracting the operator’s attention information, which can respond to the operator’s needs more intelligently and improve the efficiency and safety of remote operation.

[0120] (2) Based on the human-machine shared control method, this invention combines the high precision advantage of the robot with the wisdom advantage of the operator, which can give full play to the operator's subjective awareness and decision-making ability, making the system more flexible and intelligent when performing tasks.

[0121] (3) The present invention utilizes the operator’s attention information to dynamically adjust the compression process of 3D point cloud, thereby achieving higher compression efficiency and improving the frame rate of remote display. Attached Figure Description

[0122] Figure 1 This is a schematic diagram of the architecture of the B-mode teleoperation human-machine shared control system based on the attention mechanism of the present invention;

[0123] Figure 2 This is a schematic diagram illustrating the application scenario of the B-mode teleoperation human-machine shared control system based on the attention mechanism of the present invention;

[0124] Figure 3 This is a schematic diagram of the method for point cloud compression based on the attention mechanism of the present invention. Detailed Implementation

[0125] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0126] Example 1

[0127] like Figure 1 , Figure 2 As shown, this embodiment provides a B-mode telescopic human-machine sharing control system based on an attention mechanism. The system includes: VR glasses, Touch X joystick, robotic arm, stereo camera, and intelligent control module.

[0128] VR glasses (such as HoloLens glasses) are used to render 3D point clouds, view 3D point clouds captured and compressed by stereo cameras, and extract the user's focus points. Touch X joysticks are used to remotely and wirelessly operate robotic arms. The robotic arms (such as Elite robotic arms) are connected to stereo cameras (such as COMATRIX stereo cameras). Touch X joysticks remotely and wirelessly control the robot's movement and perform ultrasound scanning tasks through precise control of the robotic arm's end effector. The stereo camera scans the human body and captures 3D point clouds.

[0129] In this embodiment, the VR glasses used are HoloLens glasses. HoloLens glasses acquire the operator's attention by capturing the operator's eye information. The gaze tracking module of HoloLens glasses can effectively fulfill the requirement of acquiring the user's focused area, and has gaze tracking function. It uses its built-in infrared depth sensor and infrared camera to accurately track the user's eye position and focus. When the user gazes at an object on the screen, HoloLens glasses capture the position of the user's eyes in real time through the infrared camera and calculate the user's gaze focus through the built-in algorithm, thereby accurately determining the user's focus point. This allows the system to perceive the user's attention in real time, thereby responding more effectively to the user's commands or providing corresponding auxiliary information, improving the efficiency and accuracy of remote operation.

[0130] The Hololens gaze tracking module outputs the user's spatial gaze relative to the surrounding environment, including the origin (Origin(x,y,z)) and the ray direction (Direction(x,y,z)). In the point cloud compression system, only Origin(x,y,z) is used as the center of the user's currently focused area (area A). The portion of the point cloud data excluding area A is called area B, which is the low-precision display area. This method dynamically adjusts the position of the focused area based on the user's point of interest, ensuring high-precision display of the area of ​​interest and thus improving the performance of the point cloud compression system and the user experience.

[0131] A stereo camera captures 3D point clouds of the robot's surroundings and the robot's human body, providing rich and accurate environmental information. Based on the operator's attention captured by the teleoperation module, the point cloud is compressed. Specifically, when the operator's attention is focused on a specific area or object, the compression algorithm prioritizes retaining detailed information about that area or object. For parts not of interest to the operator, greater compression is applied, thus saving bandwidth and storage space. This intelligent point cloud compression method not only effectively reduces data transmission overhead but also preserves crucial information, enabling the operator to more accurately understand the robot's environment and more efficiently guide the robot in performing tasks.

[0132] In this embodiment, a high-definition stereo camera, such as a COMATRIX high-definition camera, is selected to ensure accurate reconstruction of the human body point cloud. This selection ensures that the system can capture high-resolution environmental information, thereby improving the accuracy and completeness of the human body point cloud. By accurately reconstructing the human body point cloud, the operator can have a clearer understanding of the robot's surrounding environment, thus enabling more accurate teleoperation and ensuring operational safety.

[0133] During point cloud compression, different resolutions are used for regions A and B. Region A is compressed at a high resolution to allow the operator to better observe details at the focus point, while region B is compressed at a lower resolution to improve the point cloud compression ratio. Furthermore, the resolution of point cloud data farther away from region A decreases linearly. This gradual approach ensures that point cloud data far from the user's area of ​​interest has appropriate accuracy when displayed, while reducing potential visual discomfort during remote operation.

[0134] like Figure 3 As shown, the point cloud compression process based on the attention mechanism is as follows: The algorithm receives EyesPose and point cloud data InCloud from HoloLens as input, setting EyesPose as the center C of region A. The algorithm iterates through all points in the input point cloud. For each point P, the distance d between it and the center of region A is calculated, and the resolution at point P is calculated accordingly. This resolution is then converted into a retention probability for point P. Based on probability sampling, a decision is made whether to retain point P. If point P is retained, it is encoded into the output point cloud OutCloud; otherwise, the iteration continues until the entire point cloud has been traversed.

[0135] In this embodiment, the resolutions of region A and region B can be calculated using the following method:

[0136] The point cloud resolution Res of region A A For Res max ;

[0137] For any point P(x, y, z) in region B, the distance between point P and the center of the sphere in region A is calculated using the following formula, where the distance along the z-axis is ignored, and is expressed as:

[0138]

[0139] The temporary resolution is:

[0140] Res tmp =Res max -(Res max -Res min )*δ*d

[0141] The resolution at point P is

[0142] Res B =max(Res min Res tmp )

[0143] To facilitate downsampling of region B by the computer, probability sampling can be used to determine whether point P should be retained. The probability of retaining point P can be calculated using the following formula:

[0144]

[0145] Among them, P keep This represents the probability that point P is retained.

[0146] In this embodiment, the intelligent control module intelligently adjusts in real time based on the operator's position control commands and the contact force feedback between the robotic arm's end effector and the human body. This intelligent adjustment allows the robotic arm's end effector to flexibly adjust the contact force and contact position during the human body scanning process, ensuring the safety of the human body. By combining position control and force feedback control, this module can minimize pressure and discomfort on the human body while maintaining scanning accuracy, thereby improving the safety and comfort of the scanning process.

[0147] In this embodiment, the robotic arm needs to be selected with end-effector force feedback. End-effector force feedback allows the robotic arm to sense the contact force between its end effector and the environment and transmit this information to the intelligent control module. This selection ensures that the robotic arm can intelligently adjust the contact force and position during task execution to meet the operator's needs and ensure operational safety.

[0148] In order to control both the position of the robotic arm's end effector and the interaction force between the end effector and the environment, the two control quantities must be decoupled. Specifically, force control and position control are converted to the end effector coordinate system of the robotic arm. The force controller controls the movement of the robotic arm in the z-axis direction of the end effector coordinate system, while TouchX controls the movement of the robotic arm in the X and Y axes of the end effector coordinate system.

[0149] First, map the TouchX displacement increments to the coordinate system of the robotic arm's end effector:

[0150]

[0151] Where, ΔX T Let ΔX be the displacement increment of Touch X. TER This represents the displacement increment in the coordinate system of the robotic arm's end effector. Let be the rotation matrix from the robot's base coordinate system to the coordinate system of the robotic arm's end effector. This is the rotation matrix from the Touch X base coordinate system to the robot base coordinate system.

[0152] Assume the force detected by the force sensor at the end effector of the robotic arm is F. s The force measured by the force sensor includes the tool's weight and the interaction force with the environment. Let's assume the force generated by the tool's weight in the tool's center-of-mass coordinate system is F. TC The interaction force between the robotic arm and the environment is F. RE0 Then we have:

[0153]

[0154] in, Let be the rotation matrix of the tool coordinate system relative to the force sensor coordinate system. Let be the rotation matrix from the robot arm's end effector coordinate system to the force sensor coordinate system. Therefore, the interaction force between the robot arm and the environment can be obtained as:

[0155]

[0156] Since the robotic arm's end effector is always perpendicular to the human body, it is only necessary to control the force along the Z-axis in the robotic arm's end effector coordinate system to the desired value. The force control algorithm used here is:

[0157]

[0158] Among them, X d , These represent the desired position, velocity, and acceleration of the robotic arm's end effector, X0, ... These represent the actual position, velocity, and acceleration of the robotic arm's end effector, F. REd Let M, D, and K be the desired interaction force. M, D, and K are force controller parameters, and are diagonal matrices with positive constants at their diagonal elements. For simplicity and without loss of generality, the gravity term is omitted here.

[0159]

[0160] Let the position error ΔX FR =(X d -X0), we can get:

[0161]

[0162] Simplifying, we get:

[0163]

[0164]

[0165] Where dt is the control time interval, the final control value can be obtained by adding the control value of Touch X to the control value of the force controller:

[0166] ΔX=ΔX TER S′+ΔX FR S

[0167] in, Since ΔX is based on the robot's end-effector coordinate system, it needs to be transformed into the robot's base coordinate system to achieve control.

[0168]

[0169] Where X cur This is the current position of the robotic arm. This is the rotation matrix from the end effector of the robotic arm to the base coordinate system of the robotic arm.

[0170] After obtaining the desired position of the robotic arm's next step, it is necessary to acquire the robotic arm's pose at that desired position. First, let the desired position of the robotic arm's next step be X. d Transforming to the camera coordinate system yields the coordinates of that point in the camera coordinate system:

[0171]

[0172] By finding X C The normal vector N corresponding to the point whose Euclidean distance to all points in the 3D point cloud model is the minimum. C The normal vector of the desired point. Since the normal vector N... C This is in the camera coordinate system; transforming it to the robot's base coordinate system yields:

[0173]

[0174] in, This is the rotation matrix from the robot's base coordinate system to the camera coordinate system. After obtaining the desired normal vector, to make the robot arm's end effector perpendicular to the object being manipulated, the end effector needs to be aligned in the opposite direction of the desired normal vector. Therefore, the desired robot arm end effector pose can be obtained using the normal vector.

[0175] R = arctan(-N) R [1], -N R [2])

[0176] P = -arcsin(-N) R [0])

[0177] Y = 0

[0178] Where, N R Let X be the normal vector of the object to be manipulated, corresponding to the desired position in the robot's end effector coordinate system, within the robot's base coordinate system. R, P, and Y are the expected values ​​of the Euler angles at the end effector. Then, by considering the desired position X of the end effector... dThe desired kinematics of the end effector's desired poses R, P, and Y are used to obtain the desired angles of each joint, which are then transmitted to the robot to control its motion.

[0179] Example 2

[0180] This embodiment provides a human-machine shared control method for B-mode teleoperation based on an attention mechanism, including the following steps:

[0181] 3D point cloud acquisition based on stereo camera;

[0182] Based on VR glasses, the location of the user's focus point can be obtained;

[0183] Point cloud compression is performed based on an attention mechanism. The point cloud data is divided into a high-precision display area A and a low-precision display area B. The center of area A is determined according to the user's point of interest. The point clouds of area A and area B are compressed using different resolutions respectively.

[0184] Remote control of robotic arm movement based on Touch X joystick;

[0185] A robotic arm drives a stereo camera to perform ultrasound scanning tasks.

[0186] It acquires the operator's position control commands and makes real-time adjustments based on the contact force feedback between the robotic arm's end effector and the human body.

[0187] In this embodiment, region A occupies a small portion of the point cloud data, while region B is the remaining portion of the point cloud data after region A is removed, and occupies the majority of the point cloud data. Region B adopts a linear gradient approach, and the point cloud data within region B is linearly adjusted according to the distance between region B and region A, gradually reducing its resolution.

[0188] In this embodiment, point cloud compression based on an attention mechanism specifically includes:

[0189] EyePose, which acquires eye-tracking data from VR glasses, and InCloud, which acquires point cloud data.

[0190] The EyesPose data is set as the center of region A. The user's attention point is obtained based on the EyesPose data. The position and size of region A are adjusted according to the user's attention point.

[0191] Iterate through all points in the input point cloud. For each point P, calculate its distance d from the center of the sphere in region A. Calculate the resolution at point P based on the distance d and convert it into the retention probability of point P. Determine whether to retain point P based on probability sampling. If point P is retained, encode it into the output point cloud OutCloud. Otherwise, continue iterating until the entire point cloud has been traversed.

[0192] In this embodiment, for each point P, the distance d between it and the center of the sphere in region A is calculated. Based on the distance d, the resolution at point P is calculated and converted into the retention probability of point P. Specifically, this includes:

[0193] The point cloud resolution Res of region A A For Res max ;

[0194] For any point P(x, y, z) in region B, the distance between point P and the center of the sphere in region A is calculated using the following formula, expressed as:

[0195]

[0196] The temporary resolution is:

[0197] Res tmp =Res max -(Res max -Res min )*δ*d

[0198] The resolution at point P is:

[0199] Res B =max(Res min Res tmp )

[0200] The probability of retaining point P is:

[0201]

[0202] Among them, P keep Represents the probability of point P being retained, δ represents the rate of change of resolution, and Res min Res max These represent the minimum and maximum values ​​of the point cloud resolution range, respectively.

[0203] In this embodiment, the operator's position control commands are obtained, and real-time adjustments are made based on the contact force feedback between the robotic arm's end effector and the human body. Specifically, this includes:

[0204] Mapping the displacement increment of the Touch X joystick to the coordinate system of the robotic arm's end effector is expressed as:

[0205]

[0206] Where, ΔX T ΔX represents the displacement increment of the Touch X joystick. TER This represents the displacement increment in the coordinate system of the robotic arm's end effector. Let be the rotation matrix from the robot's base coordinate system to the coordinate system of the robotic arm's end effector. This is the rotation matrix from the Touch X joystick base coordinate system to the robot base coordinate system;

[0207] The interaction force between the robotic arm and the environment is calculated as follows:

[0208]

[0209]

[0210] Among them, F s F is the force detected by the force sensor at the end effector of the robotic arm. TC F represents the force generated by the tool's weight in the tool's center-of-mass coordinate system. RE0 This represents the interaction force between the robotic arm and its environment. Let be the rotation matrix of the tool coordinate system relative to the force sensor coordinate system. This is the rotation matrix from the robot arm's end-effector coordinate system to the force sensor coordinate system;

[0211] The force control algorithm is constructed as follows:

[0212]

[0213] Among them, X d , These represent the desired position, velocity, and acceleration of the robotic arm's end effector, X0, ... These represent the actual position, velocity, and acceleration of the robotic arm's end effector, F. REd Let M, D, and K be the desired interaction force, and let them be force controller parameters, which are diagonal matrices with positive constants as diagonal elements.

[0214] The final control value is obtained by adding the control value of the Touch X joystick to the control value of the force controller, and is expressed as:

[0215] ΔX=ΔX TER S′+ΔX FR S

[0216]

[0217]

[0218]

[0219]

[0220] Where dt is the control time interval;

[0221] Control is achieved by transforming the control quantity ΔX based on the robot's end-effector coordinate system to the robot's base coordinate system, specifically as follows:

[0222]

[0223] Among them, X cur This is the current position of the robotic arm. This is the rotation matrix from the end effector of the robotic arm to the base coordinate system of the robotic arm;

[0224] Let X be the desired position of the robotic arm's next move. d Transforming to the camera coordinate system yields the coordinates of that point in the camera coordinate system:

[0225]

[0226] By finding X C The normal vector N corresponding to the point whose Euclidean distance to all points in the 3D point cloud model is the minimum. C As the normal vector of the desired point, normal vector N C Transform to the robot's base coordinate system to obtain the desired normal vector:

[0227]

[0228] in, This is the rotation matrix from the robot's base coordinate system to the camera coordinate system;

[0229] The desired robotic arm end effector posture is obtained based on the desired normal vector, specifically expressed as:

[0230] R = arctan(-N) R [1], -N R [2])

[0231] P = -arcsin(-N) R [0])

[0232] Y = 0

[0233] Where, N R Let R, P, and Y be the normal vector of the object to be manipulated corresponding to the desired position in the robot's end effector coordinate system under the robot's base coordinate system, and let R, P, and Y be the desired values ​​of the Euler angles of the robot's end effector.

[0234] By the desired position X of the end d The desired angles of each joint are obtained by inverse kinematics of the desired end-effector postures R, P, and Y, which control the movement of the robotic arm.

[0235] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A B-mode teleoperation human-machine shared control system based on an attention mechanism, characterized in that, include: VR glasses, Touch X joystick, robotic arm, stereo camera, point cloud compression module and intelligent control module; The stereo camera is used to scan the human body and acquire 3D point clouds; The VR glasses are used to render 3D point clouds and extract the user's focus points. The point cloud compression module is used to compress point clouds based on an attention mechanism, dividing the point cloud data into a high-precision display area A and a low-precision display area B. The center of area A is determined according to the user's point of interest, and the point clouds of area A and area B are compressed using different resolutions respectively. The point cloud compression module is used for point cloud compression based on an attention mechanism, specifically including: EyePose, which acquires eye-tracking data from VR glasses, and InCloud, which acquires point cloud data. The EyesPose data is set as the center of region A. The user's attention point is obtained based on the EyesPose data. The position and size of region A are adjusted according to the user's attention point. Iterate through all points in the input point cloud. For each point P, calculate its distance d from the center of the sphere in region A. Calculate the resolution at point P based on the distance d and convert it into the retention probability of point P. Determine whether to retain point P based on probability sampling. If point P is retained, encode it into the output point cloud OutCloud. Otherwise, continue iterating until the entire point cloud has been traversed. For each point P, calculate its distance d from the center of the sphere in region A, calculate the resolution at point P based on the distance d, and convert it into the retention probability of point P, specifically including: The point cloud resolution Res of region A A For Res max ; For any point P(x,y,z) in region B, the distance between point P and the center of the sphere in region A is calculated using the following formula, expressed as: The temporary resolution is: Res tmp *Res max -(Res max -Res min )*δ*d The resolution at point P is: Res B =max(Res min ,Res tmp ) The probability of retaining point P is: Among them, P keep Represents the probability of point P being retained, δ represents the rate of change of resolution, and Res min Res max These represent the minimum and maximum values ​​of the point cloud resolution range, respectively; The Touch X joystick is wirelessly connected to the robotic arm for remote control of the robotic arm's movement; The robotic arm is connected to a stereo camera, and the robotic arm drives the stereo camera to perform a B-mode ultrasound scan. The intelligent control module is used to acquire the operator's position control commands and make real-time adjustments based on the contact force feedback between the robotic arm end and the human body.

2. The B-mode teleoperation human-machine shared control system based on attention mechanism according to claim 1, characterized in that, Region A contains a small amount of data in the point cloud, while Region B is the remaining part of the point cloud data after removing Region A, and contains the majority of the point cloud data. Region B adopts a linear gradient method, and the point cloud data in Region B is linearly adjusted according to the distance between Region B and Region A, gradually reducing its resolution.

3. The B-mode teleoperation human-machine shared control system based on attention mechanism according to claim 1, characterized in that, The intelligent control module is used to acquire the operator's position control commands and make real-time adjustments based on the contact force feedback between the robotic arm's end effector and the human body. Specifically, it includes: Mapping the displacement increment of the Touch X joystick to the coordinate system of the robotic arm's end effector is expressed as: Where, ΔX T ΔX represents the displacement increment of the Touch X joystick. TER This represents the displacement increment in the coordinate system of the robotic arm's end effector. Let be the rotation matrix from the robot's base coordinate system to the coordinate system of the robotic arm's end effector. This is the rotation matrix from the Touch X joystick base coordinate system to the robot base coordinate system; The interaction force between the robotic arm and the environment is calculated as follows: Among them, F s F is the force detected by the force sensor at the end effector of the robotic arm. TC F represents the force generated by the tool's weight in the tool's center-of-mass coordinate system. RE0 This represents the interaction force between the robotic arm and its environment. Let be the rotation matrix of the tool coordinate system relative to the force sensor coordinate system. This is the rotation matrix from the robot arm's end-effector coordinate system to the force sensor coordinate system; The force control algorithm is constructed as follows: Among them, X d , These represent the desired position, velocity, and acceleration of the robotic arm's end effector, X0, ... These represent the actual position, velocity, and acceleration of the robotic arm's end effector, F. REd Let M, D, and K be the desired interaction force, and let them be force controller parameters, which are diagonal matrices with positive constants as diagonal elements. The final control value is obtained by adding the control value of the Touch X joystick to the control value of the force controller, and is expressed as: ΔX=ΔX TER S′+ΔX FR S Where dt is the control time interval; Control is achieved by transforming the control quantity ΔX based on the robot's end-effector coordinate system to the robot's base coordinate system, specifically as follows: Among them, X cur This is the current position of the robotic arm. This is the rotation matrix from the end effector of the robotic arm to the base coordinate system of the robotic arm; Let X be the desired position of the robotic arm's next move. d Transforming to the camera coordinate system yields the coordinates of that point in the camera coordinate system: By finding X C The normal vector N corresponding to the point whose Euclidean distance to all points in the 3D point cloud model is the minimum. C As the normal vector of the desired point, normal vector N C Transform to the robot's base coordinate system to obtain the desired normal vector: in, This is the rotation matrix from the robot's base coordinate system to the camera coordinate system; The desired robotic arm end effector posture is obtained based on the desired normal vector, specifically expressed as: R=arctan(-N R [1],-N R [2]) P=-arcsin(-N R [0]) Y＝0 Where, N R Let R, P, and Y be the normal vector of the object to be manipulated corresponding to the desired position in the robot's end effector coordinate system under the robot's base coordinate system, and let R, P, and Y be the desired values ​​of the Euler angles of the robot's end effector. By the desired position X of the end d The desired angles of each joint are obtained by inverse kinematics of the desired end-effector postures R, P, and Y, which control the movement of the robotic arm.

4. A human-machine shared control method for B-mode teleoperation based on an attention mechanism, characterized in that, Includes the following steps: 3D point cloud acquisition based on stereo camera; Based on VR glasses, the location of the user's focus point can be obtained; Point cloud compression is performed based on an attention mechanism. The point cloud data is divided into a high-precision display area A and a low-precision display area B. The center of area A is determined according to the user's point of interest. The point clouds of area A and area B are compressed using different resolutions respectively. The point cloud compression based on the attention mechanism specifically includes: EyePose, which acquires eye-tracking data from VR glasses, and InCloud, which acquires point cloud data. The EyesPose data is set as the center of region A. The user's attention point is obtained based on the EyesPose data. The position and size of region A are adjusted according to the user's attention point. Iterate through all points in the input point cloud. For each point P, calculate its distance d from the center of the sphere in region A. Calculate the resolution at point P based on the distance d and convert it into the retention probability of point P. Determine whether to retain point P based on probability sampling. If point P is retained, encode it into the output point cloud OutCloud. Otherwise, continue iterating until the entire point cloud has been traversed. For each point P, calculate its distance d from the center of the sphere in region A, calculate the resolution at point P based on the distance d, and convert it into the retention probability of point P, specifically including: The point cloud resolution Res of region A A For Res max ; For any point P(x,y,z) in region B, the distance between point P and the center of the sphere in region A is calculated using the following formula, expressed as: The temporary resolution is: Res tmp *Res max -(Res max -Res min )*δ*d The resolution at point P is: Res B =max(Res min ,Res tmp ) The probability of retaining point P is: Among them, P keep Represents the probability of point P being retained, δ represents the rate of change of resolution, and Res min Res max These represent the minimum and maximum values ​​of the point cloud resolution range, respectively; Remote control of robotic arm movement based on Touch X joystick; A robotic arm drives a stereo camera to perform ultrasound scanning tasks. It acquires the operator's position control commands and makes real-time adjustments based on the contact force feedback between the robotic arm's end effector and the human body.

5. The B-mode teleoperation human-machine shared control method based on attention mechanism according to claim 4, characterized in that, Region A contains a small amount of data in the point cloud, while Region B is the remaining part of the point cloud data after removing Region A, and contains the majority of the point cloud data. Region B adopts a linear gradient method, and the point cloud data in Region B is linearly adjusted according to the distance between Region B and Region A, gradually reducing its resolution.

6. The B-mode teleoperation human-machine shared control method based on attention mechanism according to claim 4, characterized in that, Obtain the operator's position control commands and make real-time adjustments based on the contact force feedback between the robotic arm's end effector and the human body. Specifically, this includes: Mapping the displacement increment of the Touch X joystick to the coordinate system of the robotic arm's end effector is expressed as: Where, ΔX T ΔX represents the displacement increment of the Touch X joystick. TER This represents the displacement increment in the coordinate system of the robotic arm's end effector. Let be the rotation matrix from the robot's base coordinate system to the coordinate system of the robotic arm's end effector. This is the rotation matrix from the Touch X joystick base coordinate system to the robot base coordinate system; The interaction force between the robotic arm and the environment is calculated as follows: Among them, F s F is the force detected by the force sensor at the end effector of the robotic arm. TC F represents the force generated by the tool's weight in the tool's center-of-mass coordinate system. RE0 This represents the interaction force between the robotic arm and its environment. Let be the rotation matrix of the tool coordinate system relative to the force sensor coordinate system. This is the rotation matrix from the robot arm's end-effector coordinate system to the force sensor coordinate system; The force control algorithm is constructed as follows: Among them, X d , These represent the desired position, velocity, and acceleration of the robotic arm's end effector, X0, ... These represent the actual position, velocity, and acceleration of the robotic arm's end effector, F. REd Let M, D, and K be the desired interaction force, and let them be force controller parameters, which are diagonal matrices with positive constants as diagonal elements. The final control value is obtained by adding the control value of the Touch X joystick to the control value of the force controller, and is expressed as: ΔX=ΔX TER S′+ΔX FR S Where dt is the control time interval; Control is achieved by transforming the control quantity ΔX based on the robot's end-effector coordinate system to the robot's base coordinate system, specifically as follows: Among them, X cur This is the current position of the robotic arm. This is the rotation matrix from the end effector of the robotic arm to the base coordinate system of the robotic arm; Let X be the desired position of the robotic arm's next move. d Transforming to the camera coordinate system yields the coordinates of that point in the camera coordinate system: By finding X C The normal vector N corresponding to the point whose Euclidean distance to all points in the 3D point cloud model is the minimum. C As the normal vector of the desired point, normal vector N C Transform to the robot's base coordinate system to obtain the desired normal vector: in, This is the rotation matrix from the robot's base coordinate system to the camera coordinate system; The desired robotic arm end effector posture is obtained based on the desired normal vector, specifically expressed as: R=arctan(-N R [1],-N R [2]) P=-arcsin(-N R [0]) Y＝0 Where, N R Let R, P, and Y be the normal vector of the object to be manipulated corresponding to the desired position in the robot's end effector coordinate system under the robot's base coordinate system, and let R, P, and Y be the desired values ​​of the Euler angles of the robot's end effector. By the desired position X of the end d The desired angles of each joint are obtained by inverse kinematics of the desired end-effector postures R, P, and Y, which control the movement of the robotic arm.

Images