A method for constructing a robot teleoperation force sense of visual information fusion

Abstract

The application discloses a kind of robot teleoperation force sense construction methods of fusion visual information.Method includes through three depth cameras, based on point cloud and target recognition, the visual information acquisition method of work environment, obtains task target position and from end obstacle model;Through force sensor, task target position and from end obstacle model respectively obtain interactive work force, virtual guiding force and virtual environment force, then according to interactive work force, virtual guiding force and virtual environment force construction fusion force;Finally, fusion force is transmitted to force feedback device, forms the final teleoperation force sense.The application introduces visual information into the construction process of virtual force sense, realizes the enhancement of force sense, and through the fusion force of fusion visual information, can help operator to guide robot to avoid obstacles, approach task target, reduce the work burden of operator.

Description

Technical Field

[0001] This invention belongs to the field of teleoperation presence construction, and in particular relates to a method for constructing robot teleoperation force perception presence by integrating visual information. Background Technology

[0002] With the development of robotics technology, teleoperation technology has been widely applied in many fields, including industrial operations, deep space exploration, and deep-sea development. Teleoperation presence is one of the important indicators for evaluating the performance of a teleoperation system; it refers to the ability of an operator to truly feel involved and participate in the process remotely through technological means.

[0003] Teleoperation presence primarily comprises two aspects: force presence and visual presence. Current research largely explores these as independent implementations, lacking effective methods to utilize visual information to enhance force presence. Existing technologies lack a method for constructing robot teleoperation force presence that integrates visual information. This method should incorporate visual information into the virtual force perception construction process to enhance force presence. Summary of the Invention

[0004] This invention addresses the problem of insufficient visual information guidance in existing research on force perception presence by proposing a method for constructing force perception presence in robot teleoperation that integrates visual information. This method can provide operators with force perception presence that integrates visual information, helping them guide the robot to avoid obstacles and approach the task target, thereby reducing the operator's workload.

[0005] The technical solution adopted in this invention is as follows, including the following steps:

[0006] Step 1: Using three depth cameras, acquire the location of the task target and the obstacle model from the end using a method for acquiring visual information of the work environment based on point cloud and target recognition.

[0007] Step 2: First, obtain the interactive operation force through the force sensor, obtain the target virtual guidance force through the task target position, and obtain the virtual environment force through the obstacle model at the slave end. Then, construct a fused force based on the fused visual information according to the interactive operation force, the target virtual guidance force, and the virtual environment force.

[0008] Step 3: Transmit the fusion force to the force feedback device to form the final sense of presence of the teleoperation force.

[0009] In step one, the method for acquiring visual information of the work environment based on point cloud and target recognition includes three steps: generation of the point cloud model of the environment from the end, segmentation and reconstruction of the work environment from the end, and identification of the location of the task target. Specifically:

[0010] Step S1: Generating the point cloud model of the end environment:

[0011] First, depth images F1, F2…F1 are obtained from depth camera 1 and depth camera 2, respectively, for each of the corresponding N frames. i …F N For all depth images F i Perform downsampling to obtain the downsampled depth image F. i Next, the downsampled depth image F i Spatial and temporal filtering processes are performed to establish a point cloud model of the environment at the slave end.

[0012] Step S2: Segmentation and Reconstruction of the End-User Operating Environment

[0013] Step S2.1: Search for the maximum plane based on the point cloud model of the slave environment.

[0014] First, a small subset of samples is randomly selected from the edge environment point cloud model to fit the work plane, forming the work plane to be detected; then, the average error distance of the work plane to be detected is obtained; finally, an optimal work plane is selected from multiple work planes to be detected.

[0015] Step S2.2: Based on the optimal working plane, remove unnecessary data from the point cloud model of the end environment to obtain the point cloud set P. R ;

[0016] Step S2.3: Based on the point cloud set P R Perform point cloud clustering and segmentation on the edge environment to obtain multiple target neighborhood point sets;

[0017] Step S2.4: Surface Reconstruction: Using the Poisson surface reconstruction algorithm, each target neighborhood point set is converted into a corresponding surface-continuous object model, and each object model is used as a potential end obstacle model;

[0018] Step S3: Target Location Identification

[0019] The corresponding RGB image is obtained using depth camera 3. This RGB image is then input into the target detection algorithm to obtain the pixel position of the target in the RGB image. The target position is then obtained through the intrinsic parameter matrix and pose matrix. Finally, the potential obstacle model is classified based on the target position.

[0020] If the target location is located within a potential slave obstacle model, then that potential slave obstacle model is used as the target object model.

[0021] If the target location is not in the potential slave obstacle model, then the potential slave obstacle model will be used as the slave obstacle model.

[0022] Step two, the method for constructing a sense of presence through the integration of visual information, comprises four parts: interactive operational force, virtual guidance force, virtual environment force, and force feedback fusion mechanism. Specifically:

[0023] Step S1: Construct interactive operation force F based on the force sensor m ;

[0024] Step S2: Construct a virtual guiding force F based on the target location. gvf ;

[0025] Step S3: Obtain the virtual environmental force F based on the obstacle model at the slave end. e ;

[0026] Step S4, Force Feedback Fusion Mechanism

[0027] By combining the interactive operational force obtained from the force sensor, the virtual guidance force obtained from the task target location, and the virtual environmental force obtained from the obstacle model at the end, a fused force F is obtained. mix :

[0028] F mix =k m F m +k gvf F gvf +k e F e

[0029] Where, k m ,k gvf ,k e These are the weighting coefficients for interactive operation force, virtual guidance force, and virtual environment force, respectively. The weighting coefficients are obtained according to the following formula:

[0030]

[0031] Where k1, k2, k3, k4 are constants.

[0032] In step S1 of step one, the specific steps for establishing the end-user environment point cloud model based on the downsampled depth image are as follows:

[0033] Step S1.1: Set a rectangular sliding window with a length and width of 2m and 2n respectively, based on the downsampled depth image F i For each pixel depth value, obtain the local mean m centered at point (a,b) within the window. ab :

[0034]

[0035] Among them, D cd F represents the downsampled depth image iThe depth value corresponding to the middle pixel (c, d);

[0036] Step S1.2: Obtain the local variance v centered at point (a,b). ab :

[0037]

[0038] Step S1.3: Based on the local mean m ab and local variance v ab For the downsampled depth image F i Perform depth value updates; the updated depth image F i The depth value corresponding to pixel (a, b) The following formula is used to obtain the result:

[0039]

[0040] k ab =v ab / (v ab +σ)

[0041] Among them, D ab F represents the depth image after downsampling. i The depth value corresponding to pixel (a, b) before updating; σ is a preset constant;

[0042] Step S1.4: Based on the updated depth image F... i Move the rectangular sliding window with a step size of 1, and repeat steps S1.1 to 1.3 until the downsampled depth image F is obtained. i Pixels at all locations are traversed to obtain the depth image F of that frame. i The corresponding spatially filtered image F i,L ;

[0043] Step S1.5: For N frames of depth images F1, F2…F… i …F N To obtain their respective spatial filtered images F 1，L ,F 2，L …F i，L …F N，L Then, the position of each pixel in the spatially filtered image is traversed to obtain a temporally filtered depth image corresponding to each depth camera. The depth value D of each pixel (x,y) in the temporally filtered depth image is... N (x, y) is obtained by processing it according to the following formula:

[0044] D N (x,y)=med(D1(x,y),D2(x,y),D3(x,y)...Di (x,y)...D N (x,y))

[0045] Where med() represents the median operation, D i (x,y) represents the spatially filtered image F of the i-th frame at pixel position (x,y). i,L The depth value in the middle;

[0046] Step S1.6; Finally, obtain a corresponding RGB image from depth camera 1 and depth camera 2 respectively, and register the temporally filtered depth image to the corresponding RGB image using the intrinsic parameter matrix to obtain the registered image. Use the registered images corresponding to depth camera 1 and depth camera 2 as point cloud model 1 and point cloud model 2 respectively, and use the camera pose matrix to stitch point cloud model 1 and point cloud model 2 into a slave environment point cloud model.

[0047] The specific steps in step one, S2.1, of searching for the maximum plane based on the point cloud model of the slave environment are as follows:

[0048] Step S2.1.1: First, set the initial parameters: set the iteration count Num. R Maximum error threshold dis threshold And the minimum number of points n R ;

[0049] Step S2.1.2: Next, three non-collinear points p1, p2, and p3 are randomly selected from the point cloud model of the slave environment to generate the detection work plane. The detection work plane composed of the three points is shown in the following formula:

[0050] a R x+b R y+c R z+d R =0

[0051] Where x, y, z are the coordinates of each point on the working plane to be inspected, and a R ,b R ,c R ,d R The parameters are those of the working plane to be inspected;

[0052] Step S2.1.3: First, obtain all other points p besides the three points p1, p2, and p3 mentioned above. i The Euclidean distance dis to the working plane to be inspected R (p i ):

[0053]

[0054] Where, x i ,yi ,z i Point p i The coordinates;

[0055] Then, determine point p. i Does it belong to the work area to be inspected?

[0056] If dis R (p i )≤dis threshold Then we consider point p to be... i This point p belongs to the working plane to be inspected. i Place it into the target point set;

[0057] Otherwise, point p is considered... i This point p does not belong to the working plane to be inspected. i Points outside the target point set;

[0058] Next, all points p i After the Euclidean distance data collection is completed, the number of target point clusters is obtained:

[0059] If the number of points in the target point set exceeds the minimum number of points in the point set, n R Then, the working plane to be tested is taken as the qualified fitting working plane, and all points p in the target point set are... i The average Euclidean distance to the working plane to be inspected is taken as the average error distance e of the working plane to be inspected. R ;

[0060] If the number of points in the target point set does not exceed the minimum number of points in the point set, n R If the result is negative, the working plane to be tested is considered an unqualified fitted working plane.

[0061] Step S2.1.4: Repeat steps S2.1.2 to S2.1.3 multiple times to obtain multiple qualified fitting working planes, and select the average error distance e. R The working plane corresponding to the minimum value is taken as the optimal working plane.

[0062] The specific operation of obtaining the point cloud set based on the optimal working plane in step one, S2.2, is as follows:

[0063] Step S2.2.1: Select point cloud data points p from the point cloud model of the slave environment. k (x k ,y k ,z k Obtain the normal vector of the optimal working plane. Obtain the point cloud data p pointing to any point in the optimal working plane. k vector vector sum vector Dot product, to get the dot product result:

[0064] If the dot product is positive, then the point cloud data p is considered to be positive. k To ensure the data is valid, retain this point cloud data p. k ;

[0065] Otherwise, the point cloud data p is considered... k As this is not necessary data, delete the point cloud data p from the point cloud model of the slave environment. k ;

[0066] Step S2.2.2: Repeat step S2.2.1 multiple times until no new point cloud data points are added, completing the removal of all unnecessary data and retaining all the remaining point cloud data p. k The point cloud set P R .

[0067] The step S2.3 in step one, which involves clustering and segmenting the point cloud of the end environment to obtain the target neighborhood point set, is as follows:

[0068] Step S2.3.1: Set initial parameters: set the neighborhood radius eps and the minimum number of neighborhood points min_sampels;

[0069] Step S2.3.2: First, select the point cloud set P. R Any point in the array can be used as the initial base point P. D In the point cloud set P R The relationship between the base point P and the base point is obtained through a Kdtree. D The adjacent points are used as the base point P. D Find the corresponding adjacent points and determine whether the adjacent points are the base point P. D neighborhood point P E :

[0070] If the base point P D If the distance between a point and its neighboring point is less than or equal to eps, then that neighboring point is the base point P. D neighborhood point P E To the neighborhood point P E Add initial base point P D The corresponding point set N Pd middle;

[0071] Otherwise, the adjacent point is considered not to be the base point P. D Discard the neighboring points;

[0072] Then, iterate through all adjacent points until there are no new adjacent points;

[0073] Step S2.3.3: Select point set N Pd The neighborhood point P inE As the new base point P D Repeat step S2.3.2 to obtain the new base point P. D The corresponding neighboring points, and the new base point P D The corresponding neighboring points are also added to the initial base point P. D The corresponding point set N Pd middle;

[0074] Traversing point set N Pd From all points in the set N, until no new neighboring points are found, the current point set N is changed. Pd As the target neighborhood point set;

[0075] Step S2.3.4: Reselect the point cloud set P R Another point in the matrix is ​​used as the next initial base point P. D And establish a new point set N Pd Repeat steps S2.3.2 to S2.3.3 to obtain another new target neighborhood point set;

[0076] Traversing the point cloud set P R By analyzing all points in the target neighborhood until no new points are found, the set of all target neighborhood points is obtained.

[0077] The specific steps in step two, S1, to construct the interactive working force based on the force sensor are as follows:

[0078] First, the optimal working force estimation parameters of the radial basis function neural network environment estimator are obtained by processing according to the following formula.

[0079]

[0080] Where arg min[X] represents ω when X reaches its minimum value. s sup(Y) represents the p value when Y reaches its maximum value. sd Ω s and Both are bounded sets, ω s These are the characteristic parameters of the working force in the working environment. For the radial basis function neural network matrix, p sd The input matrix;

[0081] Then, based on the optimal working capacity estimation parameters Obtain interactive operation force F m :

[0082]

[0083] Where t represents time, and T(t) represents delay. This is the matrix of a radial basis function neural network.

[0084] The specific steps for constructing the target virtual guiding force in step two, S2, are as follows:

[0085] Step S2.1: Take the current position of the robot's end effector as the starting point p. s The target location is used as the path endpoint p. e ;

[0086] Step S2.2: Constructing a cylindrical guiding force

[0087] First, obtain the path endpoint p. e Directly above and a distance p from the end of the path e For d min point p t (x t ,y t ,z t As the end of the cylindrical guiding force, based on the starting point p s (x s ,y s ,z s ) and point p t (x t ,y t ,z t Construct the desired path f(p), and build a cylindrical guiding force field with f(p) as the axis and r as the radius. Then, process the desired path length according to the following formula.

[0088]

[0089] Then, the robot end-effector position p is acquired in real time. f (x f ,y f ,z f ), obtain the path f(p) that corresponds to the robot's end-effector position p. f The nearest point

[0090]

[0091] Next, the deviation σ between the robot's end effector and the desired path f(p) is obtained. min (p f ), and obtain the distance between the robot's end effector and the cylindrical guiding force end effector.

[0092]

[0093] Finally, a virtual guiding force F is constructed for the robot's end effector under a cylindrical guiding force field. gvfci :

[0094]

[0095] Where, k gvf1 k gvf2 These are two positive gain parameters; F1 is the direction determined by p. f Point to p min The unit component of force; F2 is the direction from p f Point to p t The unit component of force;

[0096] Step S2.3: Constructing a conical guiding force

[0097] First, take the end point p of the cylindrical guiding force as an example. t Starting point, the target location point p e A desired path is constructed for the endpoint, and a conical guiding force field is constructed with this desired path as the axis. The cone opening angle of the guiding force field is θ. f The unit vector α of the central axis of the cone f Represented as:

[0098]

[0099] Next, the robot's end-effector position p is acquired in real time. f (x f ,y f ,z f ), p f relative to the target location point p e The line connecting the cones can be decomposed into components n along the axis of the cone. I (p f and the components n in the orthogonal direction R (p f And obtain the robot's end-effector position p. f The distance from the conical guiding force boundary to the cone axis is cone(p) f ):

[0100]

[0101] Where I is a 3×3 identity matrix;

[0102] When ||n R (p f )||≤cone(p f When ), it indicates that the robot's end point p f Within the conical guiding force field, the robot's end point p is obtained. f Radial deviation from the desired path e(p) f )=||n R (p f )||;

[0103] When ||n R (p f )||>cone(p f When ), it indicates that the robot's end point p f Outside the conical guiding force field, a new conical guiding force field is established until the robot's end point p. f Inside the cone-shaped guiding force field;

[0104] Finally, a virtual guiding force F is constructed for the robot's end effector under a conical guiding force field. gvfco :

[0105]

[0106] Where, k gvf3 k gvf4 There are two gain parameters;

[0107] Step S2.4: Construct the target virtual guiding force F gvf

[0108] If the robot's end effector position p s Distance between the target location and the target location The virtual guiding force F under the conical guiding force field gvfco As the target virtual guiding force F gvf ;

[0109] Otherwise, the virtual guiding force F under the cylindrical guiding force field will be... gvfci As the target virtual guiding force F gvf .

[0110] The specific steps for constructing the virtual environment force in step two, S3, are as follows:

[0111] Step S3.1: First, assemble all the slave obstacle models into a slave obstacle model set M. c The bounding box algorithm is used to obtain a bounding box corresponding to each slave obstacle model, and the relative position of each slave obstacle model and the size of each bounding box are obtained.

[0112] Next, in the set of obstacle models M at the end. c Select any one of the end obstacle models m a Obtain the obstacle model m from the end. a The center position p of the corresponding bounding box a (x a ,y a ,z a ) and the side length l of each side of the bounding box ax , l ay , l az; In the set of obstacle models M at the end c Select the obstacle model m from the end. a Any external obstacle model m b Obtain the obstacle model m from the end. b The center position p of the corresponding bounding box b (x b ,y b ,z b and the side length l of each side of the enclosure. bx , l by , l bz The shortest distance L between the two bounding boxes is obtained through processing. ab :

[0113]

[0114] Where, k j Let j be the coefficient, and k be the index j ∈ (x, y, z). j The following formula is used to obtain:

[0115]

[0116] Then, in the set of obstacle models M at the end. c Select the obstacle model m from the end. a For any external obstacle model, repeat the above steps in the set of external obstacle models M. c Select the obstacle model m from the middle. a Minimum distance for the end obstacle model m c And the end obstacle model m c As a slave obstacle model m a The corresponding nearest model is then processed according to the following formula to obtain the end obstacle model m. a Model magnification factor L a :

[0117]

[0118] Where, k min L is the weighting coefficient. ac For the obstacle model m a and its corresponding end obstacle model m c The distance between; l cx , l by , l cz For the obstacle model m c The side lengths of the corresponding bounding box;

[0119] Finally, the end obstacle model m a Each vertex expands outward along the normal direction by La The length is then remodeled and updated, and the updated model is used as the slave obstacle model m. a A virtual repulsive field model;

[0120] In the set of obstacle models M at the end c Select a new slave obstacle model and repeat the above steps to obtain the virtual repulsion field models corresponding to all slave obstacle models;

[0121] Step S3.2: When the force feedback point contacts the virtual repulsion field model and moves within it, a virtual contact point is set so that it slides on the surface of the repulsion field model. When the distance between the virtual contact point and the force feedback point is minimized, this virtual contact point is taken as the target virtual contact point corresponding to the force feedback point. The force feedback point is the position of the robot's end effector, which moves in real time. The virtual environmental force F is obtained according to the following formula. e :

[0122]

[0123] Where ε is the distance between the force feedback point and the target virtual contact point; k is the rate of increase of the distance between the force feedback point and the virtual contact point of the target. s k is the elastic coefficient. d is the damping coefficient.

[0124] The method of this invention employs a master-slave robot teleoperation system. The system mainly consists of a master module and a slave module. The master module includes an operator, a force feedback device, and a master host. The slave module includes a robot, force sensors, three depth cameras, a work plane, a task target, obstacles, and the slave host. The force sensors are mounted on the robot's end effector, along with the first depth camera 1, the second depth camera 2, and the third depth camera 3.

[0125] This invention employs three depth cameras and designs a method for acquiring visual information about the operational environment based on point cloud and target recognition. It obtains a remote obstacle model by generating a point cloud model of the remote environment and reconstructing the remote operational environment, and then uses a task target detection algorithm to obtain the position of the task target. Furthermore, it designs a method for constructing a force-feeling presence by fusing visual information, integrating interactive operational forces, virtual guidance forces, and virtual environmental forces to ultimately obtain a fused force. Specifically, the interactive operational forces are obtained from remote operational forces based on radial basis function neural networks and force sensing; the virtual guidance forces are divided into two guidance schemes—cylindrical guidance forces and conical guidance forces—depending on the operational stage; and the virtual environmental forces are obtained through an axis-aligned bounding box algorithm and a point-based force feedback model. Finally, a designed force feedback fusion mechanism is used to fuse the interactive operational forces, virtual guidance forces, and virtual environmental forces to obtain the fused force. The fused force is then transmitted to a force feedback device to form a remote-operation force-feeling presence.

[0126] Compared with the prior art, the present invention has the following beneficial effects:

[0127] This invention designs a method for constructing a sense of force presence by integrating visual information. It can effectively integrate end-user interactive operation force, virtual guidance force, and virtual environment force, and introduce visual information into the construction process of virtual force presence to enhance the sense of force presence. Through the fusion of visual information, it can help operators guide robots to avoid obstacles and approach task objectives, reducing the operator's workload. Attached Figure Description

[0128] Figure 1 This is a block diagram of the robot teleoperation force perception presence construction method proposed in this invention, which integrates visual information;

[0129] Figure 2 This is a schematic diagram of the virtual guiding force construction proposed in this invention;

[0130] Figure 3 This is a schematic diagram of the columnar guiding force construction proposed in this invention;

[0131] Figure 4 This is a schematic diagram of the cone-shaped guiding force construction proposed in this invention;

[0132] Figure 5 This is a schematic diagram showing the relationship between the force feedback point and the virtual contact point proposed in this invention. Detailed Implementation

[0133] 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. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0134] The inventive method includes the following steps, such as Figure 1 As shown:

[0135] Step 1: Using three depth cameras, acquire the location of the task target and the obstacle model from the end using a method for acquiring visual information of the work environment based on point cloud and target recognition.

[0136] Step 2: First, obtain the interactive operation force through the force sensor, obtain the virtual guidance force through the task target position, and obtain the virtual environment force through the obstacle model at the end. Then, construct a force perception presence based on the fused visual information according to the interactive operation force, virtual guidance force, and virtual environment force, and obtain the fused force.

[0137] Step 3: Transmit the fusion force to the force feedback device to form the final sense of presence of the teleoperation force.

[0138] In step one, the method for acquiring visual information of the work environment based on point cloud and target recognition includes three steps: generation of the point cloud model of the environment from the end, segmentation and reconstruction of the work environment from the end, and identification of the location of the task target. Specifically:

[0139] Step S1: Generating the point cloud model of the end environment:

[0140] First, depth images F1, F2…F1 are obtained from depth camera 1 and depth camera 2, respectively, for each of the corresponding N frames. i …F N For all depth images F i Perform downsampling to obtain the downsampled depth image F. i Next, the downsampled depth image F i Spatial and temporal filtering processes are performed to establish a point cloud model of the environment at the slave end.

[0141] In practice, the depth image is obtained by capturing images with depth cameras. Each depth camera 1 and 2 captures N=10 initial depth images. The specific steps of the downsampling operation are as follows: a decimation filter is used to extract each 2×2 image pixel block in the depth image, and the non-zero median of the image pixel block is used to downsample the initial depth image, reducing the depth image resolution from 848×480 to 424×240, thus obtaining the downsampled depth image F. i ;

[0142] In step S1 of step one, the specific steps for establishing the point cloud model of the slave environment are as follows:

[0143] Step S1.1: Set a rectangular sliding window with a length and width of 2m and 2n respectively, based on the downsampled depth image F iFor each pixel depth value, obtain the local mean m centered at point (a,b) within the window. ab :

[0144]

[0145] Among them, D cd F represents the downsampled depth image i The depth value corresponding to the pixel (c,d), where a and b are the coordinates of the point (a,b), the subscript c represents the distance from am to a+m, and the subscript d represents the distance from bn to b+n;

[0146] Step S1.2: Obtain the local variance v centered at point (a,b). ab :

[0147]

[0148] Step S1.3: Based on the local mean m ab and local variance v ab For the downsampled depth image F i Perform depth value updates; the updated depth image F i The depth value corresponding to pixel (a, b) The following formula is used to obtain the result:

[0149]

[0150] k ab =v ab / (v ab +σ)

[0151] Among them, D ab F represents the depth image after downsampling. i The depth value corresponding to pixel (a, b) before updating; σ is a preset constant, in specific implementation σ = 3, k ab This represents the depth value adjustment factor;

[0152] Step S1.4: Based on the depth image F formed after updating the depth values i Move the rectangular sliding window with a step size of 1, and repeat steps S1.1 to 1.3 until the downsampled depth image F is obtained. i Pixels at all locations are traversed to obtain the depth image F of that frame. i The corresponding spatially filtered image F i,L ;

[0153] Step S1.5: For N frames of depth images F1, F2…F… i …F NThe corresponding spatially filtered images F are obtained through steps S1.1 to S1.4. 1，L ,F 2，L …F i，L …F N，L Then, the position of each pixel in the spatially filtered image is traversed to obtain a temporally filtered depth image corresponding to each depth camera. The depth value D of each pixel (x,y) in the temporally filtered depth image is... N (x, y) is obtained by processing it according to the following formula:

[0154] D N (x,y)=med(D1(x,y),D2(x,y),D3(x,y)...D i (x,y)...D N (x,y))

[0155] Where med() represents the median operation, D i (x,y) represents the spatially filtered image F of the i-th frame at pixel position (x,y). i,L The depth value in the middle;

[0156] Step S1.6; Finally, obtain a corresponding RGB image from depth camera 1 and depth camera 2 respectively, and register the temporally filtered depth image to the corresponding RGB image using the known intrinsic parameter matrix to obtain the registered image. The registered image has both color and depth information. Use the registered images corresponding to depth camera 1 and depth camera 2 as point cloud model 1 and point cloud model 2 respectively, and use the known camera pose matrix to stitch point cloud model 1 and point cloud model 2 together to form a slave environment point cloud model containing all point cloud data.

[0157] Step S2, Segmentation and Reconstruction of the End-User Environment: Perform the following four steps in sequence: searching the maximum plane, removing invalid data, clustering segmentation, and surface reconstruction;

[0158] Step S2.1: Search for the maximum plane based on the point cloud model of the slave environment.

[0159] First, a small subset of samples is randomly selected from the end-user environmental point cloud model to fit the work plane, forming the work plane to be detected. Then, based on the work plane to be detected, other points in the end-user environmental point cloud model are tested to see if they belong to the work plane to be detected, until no new data points can be added to the work plane to be detected, or the predetermined number of iterations is reached. Next, the average error distance of the work plane to be detected is obtained, and finally, an optimal work plane is selected from multiple work planes to be detected.

[0160] The specific steps for searching the maximum plane in step S2.1 of step one are as follows:

[0161] Step S2.1.1: First, set the initial parameters: set the iteration count Num. R =1000, maximum error threshold dis threshold =0.02 and minimum number of points = 200n R ;

[0162] Step S2.1.2: Next, three non-collinear points p1, p2, and p3 are randomly selected from the point cloud model of the slave environment to generate the detection work plane. The detection work plane composed of the three points is shown in the following formula:

[0163] a R x+b R y+c R z+d R =0

[0164] Where x, y, z are the coordinates of each point on the working plane to be inspected, and a R ,b R ,c R ,d R The parameters are those of the working plane to be inspected;

[0165] Step S2.1.3: First, obtain all other points p besides the three points p1, p2, and p3 mentioned above. i The Euclidean distance dis to the working plane to be inspected R (p i ):

[0166]

[0167] Where, p i x represents all points in the point cloud model of the end environment except for points p1, p2, and p3. i ,y i ,z i Point p i The coordinates;

[0168] Then, determine point p. i Does it belong to the work area to be inspected?

[0169] If dis R (p i )≤dis threshold Then we consider point p to be... i This point p belongs to the working plane to be inspected. i Place it into the target point set;

[0170] Otherwise, point p is considered... i This point p does not belong to the working plane to be inspected. i Points outside the target point set;

[0171] Next, all points p i After the Euclidean distance data collection is completed, the number of target point clusters is obtained:

[0172] If the number of points in the target point set exceeds the minimum number of points in the point set, n R Then, the working plane to be tested is taken as the qualified fitting working plane, and all points p in the target point set are... i The average Euclidean distance to the working plane to be inspected is taken as the average error distance e of the working plane to be inspected. R ;

[0173] If the number of points in the target point set does not exceed the minimum number of points in the point set, n R If the result is not met, the working plane to be tested is considered an unqualified fitted working plane and is discarded.

[0174] Step S2.1.4: Select three more non-collinear points and repeat steps S2.1.2 to S2.1.3 multiple times to obtain multiple qualified fitting planes. Select the average error distance e. R The detection plane corresponding to the minimum value is taken as the optimal working plane; when the number of iterations in step S2.1.4 reaches Num R If no points are available, output the optimal planar model.

[0175] Step S2.2: Based on the optimal working plane, remove unnecessary data from the point cloud model of the end environment to obtain the point cloud set P. R ;

[0176] The specific operation for obtaining the point cloud set in step S2.2 of step one is as follows:

[0177] Step S2.2.1: Select point cloud data points p from the point cloud model of the slave environment. k (x k ,y k ,z k Obtain the normal vector of the optimal working plane facing the positive z-axis. Obtain the point cloud data p pointing to any point in the optimal working plane. k vector vector sum vector Dot product, to get the dot product result:

[0178] If the dot product of two vectors is positive, then the point cloud data p is considered to be positive. k To ensure the data is valid, retain this point cloud data p. k ;

[0179] Otherwise, the point cloud data p is considered... kAs this is not necessary data, delete the point cloud data p from the point cloud model of the slave environment. k ;

[0180] Step S2.2.2: Select other point cloud data p from the slave environment point cloud model. k (x k ,y k ,z k Repeat step S2.2.1 multiple times until no new point cloud data points are added, completing the removal of all unnecessary data and retaining all the remaining point cloud data p. k The point cloud set P R .

[0181] The above point cloud data constitutes a point cloud set P. R Used for point cloud clustering and segmentation in the lower part.

[0182] Step S2.3: Based on the point cloud set P R Perform point cloud clustering and segmentation on the edge environment to obtain multiple target neighborhood point sets;

[0183] The specific steps in step S2.3 of step one to obtain the target neighborhood point set are as follows:

[0184] Step S2.3.1: Set initial parameters: set the neighborhood radius eps = 0.02 and the minimum number of neighborhood points min_sampels = 50;

[0185] Kdtree can be used to find the neighboring points of a given point, but two adjacent points are not necessarily neighbors. Therefore, the neighborhood radius eps can be used to define the size of the neighborhood. If the distance between two points is less than or equal to eps, they are considered to be neighbors of each other. The minimum number of neighborhood points min_sampels is used to define the core object. If the neighborhood of a point contains more than or equal to min_sampels, then the point is a core object.

[0186] Step S2.3.2: First, select the point cloud set P. R Any point in the matrix can be used as the initial base point P. D In the point cloud set P R The Kdtree method is used to obtain the relationship with the base point P. D The adjacent points are used as the base point P. D For the corresponding adjacent points, obtain the distance from the adjacent points to the base point P. D The distance is calculated, and it is determined whether the adjacent points are the base point P. D neighborhood point P E :

[0187] If the base point P D If the distance between a point and its neighboring point is less than or equal to eps, then that neighboring point is the base point P.D neighborhood point P E To the neighborhood point P E Add initial base point P D The corresponding point set N Pd middle;

[0188] Otherwise, the adjacent point is considered not to be the base point P. D Discard the neighboring points;

[0189] Then, iterate through all adjacent points until there are no new adjacent points;

[0190] Step S2.3.3: Select point set N Pd The neighborhood point P in E As the new base point P D Repeat step S2.3.2 to obtain the new base point P. D The corresponding neighboring points, and the new base point P D The corresponding neighboring points are also added to the initial base point P. D The corresponding point set N Pd middle;

[0191] Traversing point set N Pd From all points in the set N, until no new neighboring points are found, the current point set N is changed. Pd As the target neighborhood point set;

[0192] In step S2.3.3, the point set N Pd The number of elements in the point set N gradually increases, and the number of elements in the point set N gradually increases. Pd The region represented by N gradually increases until the point set N is exhausted. Pd All points up to (i.e., point set N) Pd (There are no points in the neighborhood of any point whose distance is less than or equal to eps).

[0193] Step S2.3.4: Reselect the point cloud set P R Another point in the matrix is ​​used as the next initial base point P. D And establish a new point set N Pd The next initial base point P D Not belonging to any of the preceding point sets N Pd Then, repeat steps S2.3.2 to S2.3.3 to obtain another new target neighborhood point set;

[0194] Traversing the point cloud set P R Repeat the process until no new points are added, obtaining the complete target neighborhood point set, and proceed to step S2.4.

[0195] Step S2.4: Surface Reconstruction: Using the Poisson surface reconstruction algorithm, each target neighborhood point set is converted into a corresponding surface-continuous object model, and each object model is used as a potential end obstacle model;

[0196] Potential slave obstacle models can be either task target object models or slave obstacle models. Based on the task target location in step S3, and combined with the potential slave obstacle models, task target object models can be further eliminated, ultimately selecting all slave obstacle models.

[0197] Step S3: Target Location Identification

[0198] The corresponding RGB image is obtained using depth camera 3. This RGB image is then input into the target detection algorithm to obtain the pixel position of the target in the RGB image. The target position is then obtained through the intrinsic parameter matrix and pose matrix. Finally, the potential obstacle model is classified based on the target position.

[0199] If the target location is located within a potential slave obstacle model, then that potential slave obstacle model is used as the target object model.

[0200] If the target location is not in the potential slave obstacle model, then the potential slave obstacle model will be used as the slave obstacle model.

[0201] In practice, the YOLOv5 algorithm is used as the target detection algorithm for the task, and the specific method is as follows:

[0202] First, a dataset is created for commonly used task objectives: task objective images containing commonly used task objectives are obtained; the task objective images are expanded using data augmentation and other methods to increase the number of task objective images to more than 1,000; then all task objective images are labeled to complete the dataset creation.

[0203] Next, the network model is trained: the neural network model provided by YOLOv5 for model pre-training is used to train the above dataset to obtain a loss function that converges quickly, and then the pixel position of the task target in the RGB image is obtained.

[0204] In practice, after obtaining the pixel position of the target in the RGB image, the position of the target relative to the robot can be obtained through the known depth camera intrinsic parameter matrix and pose matrix, thus realizing the acquisition of the target position.

[0205] Step two, the method for constructing a sense of presence through the integration of visual information, comprises four parts: interactive operational force, virtual guidance force, virtual environment force, and force feedback fusion mechanism. Specifically:

[0206] Step S1: Construct interactive operation force F based on the force sensor m

[0207] The specific steps for S1 to construct the interactive working force based on the force sensor in step two are as follows:

[0208] First, the working force F at the slave end is obtained through a force sensor. s From the end working force F s It is described in the form of a radial basis function neural network system as follows:

[0209]

[0210] Where, ω s These are the characteristic parameters of the working force in the working environment; p is the radial basis function neural network matrix; sd Let p be the input matrix. sd Mainly composed of the robot's end-effector pose matrix P sd Velocity matrix Acceleration matrix composition.

[0211] Next, the optimal working force estimation parameters of the radial basis function neural network environment estimator are obtained according to the following formula.

[0212]

[0213] Where arg min[X] represents ω when X reaches its minimum value. s sup(Y) represents the value of p when Y reaches its maximum value. sd Ω s and Both are bounded sets;

[0214] by The adaptive rate is updated online to ensure better estimation of operational force parameters. s The adaptive rate matrix, This is the estimation error of the working force parameters; k s A constant greater than 0; the end-operating force F obtained from the end-effector force sensor. s As the output of the work capacity estimator, p s As input to the work capacity estimator, the optimal slave-end work capacity estimation parameters can be obtained based on the radial basis function neural network.

[0215] Finally, the optimal working capacity estimation parameters are obtained from the end. The data is transmitted to the master end, and the optimal slave end working capacity estimation parameters with a delay of T(t) are obtained. It also inputs a working force reconstructor based on radial basis function neural network, which can realize the reconstruction of real slave-end interactive working force at the master end;

[0216] Based on the optimal working capacity estimation parameters Obtain interactive operation force F m :

[0217]

[0218] Where t represents time, and T(t) represents delay. For radial basis function neural network matrices;

[0219] p ms Let p be the input matrix. ms The end-effector pose of the master force feedback device is mapped to matrix P via master-slave mapping. ms Velocity matrix Acceleration matrix composition.

[0220] Step S2: Construct a virtual guiding force F based on the target location. gvf ;

[0221] The specific steps for constructing the virtual guiding force in step two are as follows:

[0222] Virtual guiding force is divided into two guiding schemes depending on the operation stage: cylindrical guiding force (stage I) and conical guiding force (stage II).

[0223] Step S2.1: Take the current position of the robot's end effector as the starting point p. s The target location is used as the path endpoint p. e ;like Figure 2 As shown, the minimum distance d for constructing the conical guiding force is... min The starting point p is obtained. s to the end point p of the path e Distance between when If the current condition is met, directly create a cone-shaped guiding force (i.e., this is stage II); otherwise, first create a cylindrical guiding force (i.e., this is stage I), until... Create a cone-shaped guiding force;

[0224] Step S2.2: Constructing a cylindrical guiding force

[0225] like Figure 3 As shown, first, obtain the path endpoint p. e Directly above and a distance p from the end of the path e For d min point p t (x t ,y t,z t As the end of the cylindrical guiding force, based on the starting point p s (x s ,y s ,z s ) and point p t (x t ,y t ,z t Construct the desired path f(p), and build a cylindrical guiding force field with f(p) as the axis and r as the radius. Then, process the desired path length according to the following formula.

[0226]

[0227] In practice, the starting point p s (x s ,y s ,z s ) and point p t (x t ,y t ,z t Let f(p) be the points at both ends of the desired path f(p);

[0228] Then, the robot end-effector position p is acquired in real time. f (x f ,y f ,z f ), that is, the robot's real-time position, to obtain the position of the robot's end effector p in the desired path f(p). f The nearest point

[0229]

[0230] Next, the deviation σ between the robot's end effector and the desired path f(p) is obtained. min (p f And obtain the distance d between the robot end effector and the cylindrical guiding force end effector. pf,pt :

[0231]

[0232] Finally, a virtual guiding force F is constructed for the robot's end effector under a cylindrical guiding force field. gvfci :

[0233]

[0234] Where, k gvf1 =1.5, k gvf2 =1 represents two positive gain parameters used to adjust the magnitude of the force components in the two directions; F1 is the direction determined by p f Point to pmin The unit component of force; F2 is the direction from p f Point to p t The unit component of force;

[0235] Step S2.3: Constructing a conical guiding force

[0236] like Figure 4 As shown, firstly, the end point p of the cylindrical guiding force... t Starting point, the target location point p e A desired path is constructed for the endpoint, and a conical guiding force field is constructed with this desired path as the axis. The cone opening angle of the guiding force field is θ. f The unit vector α of the central axis of the cone f Represented as:

[0237]

[0238] Where |||| denotes the 2-norm symbol;

[0239] Next, the robot's end-effector position p is acquired in real time. f (x f ,y f ,z f ), that is, the robot's real-time position, p f relative to the target location point p e The line connecting the cones can be decomposed into components n along the axis of the cone. I (p f and the components n in the orthogonal direction R (p f And obtain the robot's end-effector position p. f The distance from the conical guiding force boundary to the cone axis is cone(p) f ):

[0240]

[0241] Where I is a 3×3 identity matrix;

[0242] When ||n R (p f )||≤cone(p f When ), it indicates that the robot's end point p f Within the conical guiding force field, the robot's end point p is obtained. f Radial deviation from the desired path e(p) f )=||n R (p f )||;

[0243] When ||n R (p f)||>cone(p f When ), it indicates that the robot's end point p f Outside the conical guiding force field, a new conical guiding force field is established until the robot's end point p. f Inside the cone-shaped guiding force field;

[0244] Finally, a virtual guiding force F is constructed for the robot's end effector under a conical guiding force field. gvfco :

[0245]

[0246] Where, k gvf3 =1.5, k gvf4 =1 represents two gain parameters used to adjust the magnitude of the force components in the two directions;

[0247] Step S2.4: Construct the target virtual guiding force F gvf

[0248] If the robot's end effector position p s Distance between the target location and the target location The virtual guiding force F under the conical guiding force field gvfco As the target virtual guiding force F gvf ;

[0249] Otherwise, the virtual guiding force F under the cylindrical guiding force field will be... gvfci As the target virtual guiding force F gvf .

[0250] Step S3: Obtain the virtual environmental force F based on the obstacle model at the slave end. e ;

[0251] The specific steps for S3 to construct virtual environment forces in step two are as follows:

[0252] Step S3.1: First, assemble all the slave obstacle models into a slave obstacle model set M. c The bounding box algorithm is used to obtain a bounding box corresponding to each slave obstacle model, and the relative position of each slave obstacle model and the size of each bounding box are obtained.

[0253] Next, in the set of obstacle models M at the end. c Select any one of the end obstacle models m a Obtain the obstacle model m from the end. a The center position p of the corresponding bounding box a (x a ,y a ,z a ) and the side length l of each side of the bounding box ax, l ay , l az ; In the set of obstacle models M at the end c Select the obstacle model m from the end. a Any external obstacle model m b Obtain the obstacle model m from the end. b The center position p of the corresponding bounding box b (x b ,y b ,z b and the side length l of each side of the enclosure. bx , l by , l bz The shortest distance L between the two bounding boxes is obtained through processing. ab :

[0254]

[0255] Where, k j Let j be the coefficient, and k be the index j ∈ (x, y, z). j The following formula is used to obtain:

[0256]

[0257] Then, in the set of obstacle models M at the end. c Select the obstacle model m from the end. a For any external obstacle model, repeat the above steps in the set of external obstacle models M. c Select the obstacle model m from the middle. a Minimum distance for the end obstacle model m c And the end obstacle model m c As a slave obstacle model m a The corresponding nearest model is then processed according to the following formula to obtain the end obstacle model m. a Model magnification factor L a :

[0258]

[0259] Where, k min =0.2 is the weighting coefficient, L ac For the obstacle model m a and its corresponding end obstacle model m c (i.e., the obstacle model m at the end) a The distance between the corresponding nearest models; cx , l by , l cz For the obstacle model m c The side lengths of the corresponding bounding box;

[0260] Finally, the end obstacle model m a Each vertex expands outward along the normal direction by L a The length is then remodeled and updated, and the updated model is used as the slave obstacle model m. a A virtual repulsive field model;

[0261] In the set of obstacle models M at the end c Select a new slave obstacle model and repeat the above steps to obtain the virtual repulsion field models corresponding to all slave obstacle models;

[0262] Step S3.2: Based on the contact relationship between the force feedback point and the virtual repulsive field model, obtain the magnitude and direction of the virtual environmental force. Using a point-based force feedback model, construct the virtual contact point projected onto the virtual repulsive field model by the force feedback point. The specific method is as follows:

[0263] like Figure 5 As shown, when the force feedback point comes into contact with the virtual repulsion field model and moves inside the virtual repulsion field model, a virtual contact point is set so that the virtual contact point slides on the surface of the repulsion field model. When the distance between the virtual contact point and the force feedback point is minimized, the virtual contact point is taken as the target virtual contact point corresponding to the force feedback point. The force feedback point adopts the position of the robot end effector that moves in real time.

[0264] When the force feedback point comes into contact with the virtual repulsive field model, a displacement occurs between the force feedback point and the target virtual contact point. The virtual environmental force F is obtained by processing this displacement using the Kelvin-Voigt model according to the following formula. e :

[0265]

[0266] Where ε is the distance between the force feedback point and the target virtual contact point; k is the rate of increase of the distance between the force feedback point and the virtual contact point of the target. s k is the elastic coefficient. d Let be the damping coefficient, defined as when At that time, k d For a constant, when At that time, k d =0, virtual environment force F e The force feedback point points to the target virtual contact point.

[0267] Step S4, Force Feedback Fusion Mechanism

[0268] Combining the interactive operational force obtained from the force sensor, the virtual guidance force obtained from the task target location, and the virtual environmental force obtained from the obstacle model at the end, the following force feedback fusion mechanism is designed to obtain the fused force F. mix :

[0269] F mix =k m F m +k gvf F gvf +k e F e

[0270] Where, k m ,k gvf ,k e These are the weighting coefficients for interactive operation force, virtual guidance force, and virtual environment force, respectively. The weighting coefficients are obtained according to the following formula:

[0271]

[0272] Where k1, k2, k3, k4 are constants.

[0273] In the specific implementation, k1 = 1, k2 = 1.5, k3 = 1, k4 = 1.

[0274] The above content is merely a technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. Any modifications made to the technical solution based on the technical concept proposed in this invention shall fall within the scope of protection of the claims of this invention.

Claims

1. A method for constructing a sense of presence in robot teleoperation by integrating visual information, characterized in that, Includes the following steps: Step 1: Using three depth cameras, acquire the location of the task target and the obstacle model at the end of the task environment based on the point cloud and target recognition visual information acquisition method; Step 2: First, construct interactive operation force, virtual guidance force, and virtual environment force respectively using force sensors, task target position, and slave obstacle model. Then, construct a fused force based on fused visual information based on the interactive operation force, virtual guidance force, and virtual environment force. Step 3: Transmit the fusion force to the force feedback device to form the final teleoperation force perception presence; The specific steps of step one are as follows: Step S1: Generating the point cloud model of the end environment: First, depth cameras 1 and 2 respectively obtain their corresponding data. N Frame depth images F1, F2… F i …F N For all depth images respectively F i Perform downsampling to obtain the downsampled depth image. F i Next, the downsampled depth image F i Spatial and temporal filtering processes are performed to establish a point cloud model of the environment at the slave end. Step S2: Segmentation and Reconstruction of the End-User Operating Environment Step S2.1: Search for the maximum plane based on the point cloud model of the slave environment. First, a small subset of samples is randomly selected from the edge environment point cloud model to fit the work plane, forming the work plane to be detected; then, the average error distance of the work plane to be detected is obtained; finally, an optimal work plane is selected from multiple work planes to be detected. Step S2.2: Based on the optimal working plane, remove unnecessary data from the point cloud model of the end environment to obtain a point cloud set. ; Step S2.3: Based on the point cloud set Perform point cloud clustering and segmentation on the edge environment to obtain multiple target neighborhood point sets; Step S2.4: Surface Reconstruction: Using the Poisson surface reconstruction algorithm, each target neighborhood point set is converted into a corresponding surface-continuous object model, and each object model is used as a potential end obstacle model; Step S3: Target Location Identification The corresponding RGB image is obtained using depth camera 3. This RGB image is then input into the target detection algorithm to obtain the pixel position of the target in the RGB image. The target position is then obtained through the intrinsic parameter matrix and pose matrix. Finally, the potential obstacle model is classified based on the target position. If the target location is located within a potential slave obstacle model, then that potential slave obstacle model is used as the target object model. If the target location is not in the potential slave obstacle model, then the potential slave obstacle model will be used as the slave obstacle model. The specific steps of step two are as follows: Step S1: Construct interactive working force based on force sensor ; Step S2: Construct a virtual guiding force for the target based on the location of the mission objective. Step S3: Construct virtual environment forces based on the obstacle model at the slave end. Step S4, Force Feedback Fusion Mechanism By combining the interactive operational force obtained from force sensors, the virtual guidance force obtained from the task target location, and the virtual environmental force obtained from the obstacle model at the end, a fused force is obtained. ： in, These are the weighting coefficients for interactive operation force, virtual guidance force, and virtual environment force, respectively. The weighting coefficients are obtained according to the following formula: in, It is a constant. The distance between the force feedback point and the target virtual contact point. This is the component of the line connecting the robot's end effector position and the target position along the axis of the cone.

2. The method for constructing a robot teleoperation force perception presence by fusing visual information according to claim 1, characterized in that: The steps for generating the point cloud model of the end environment are as follows: Step S1.1: Set a rectangular sliding window with a length and width of 2m and 2n respectively, based on the downsampled depth image. F i For each pixel depth value, obtain the local mean with point (a, b) as the center of the window. : in, Represents the depth image after downsampling F i Medium pixel ( c , d The corresponding depth value; Step S1.2: Obtain the local variance centered at point (a,b). : Step S1.3: Based on local mean and local variance For the downsampled depth image F i Perform depth value updates; updated depth image F i Medium pixel ( a , b The corresponding depth value The following formula is used to obtain it: in, Represents the depth image after downsampling F i Before the update, the pixels were ( a , b The corresponding depth value; This is a preset constant; Step S1.4: Based on the updated depth image F i Move the rectangular sliding window with a step size of 1, and repeat steps S1.1 to 1.3 until the downsampled depth image is obtained. F i Pixels at all locations are traversed to obtain the depth image of that frame. F i Corresponding spatially filtered image F i,L ; Step S1.5: For N Frame depth images F1, F2… F i …F N To obtain their respective spatial filtered images F 1，L ,F 2，L … F i，L …F N，L Then, the position of each pixel in the spatially filtered image is traversed to obtain a temporally filtered depth image corresponding to each depth camera. Each pixel in the temporally filtered depth image ( x , y Depth value The following formula is used to obtain it: Where med() represents the median operation, Indicates at pixel point ( x , y At position ) i Frame space filtered image F i,L The depth value in the middle; Step S1.6; Finally, obtain a corresponding RGB image from depth camera 1 and depth camera 2 respectively, and register the temporally filtered depth image to the corresponding RGB image using the intrinsic parameter matrix to obtain the registered image. Use the registered images corresponding to depth camera 1 and depth camera 2 as point cloud model 1 and point cloud model 2 respectively, and use the camera pose matrix to stitch point cloud model 1 and point cloud model 2 into a slave environment point cloud model.

3. The method for constructing a robot teleoperation force perception presence by fusing visual information according to claim 1, characterized in that: The specific steps for searching the maximum plane based on the end-user environment point cloud model are as follows: Step S2.1.1: First, set the initial parameters: set the number of iterations. Maximum error threshold and the minimum number of point sets ; Step S2.1.2: Next, three non-collinear points are randomly selected from the point cloud model of the slave environment to generate the detection work plane. The detection work plane composed of the three points is shown in the following formula: in, The coordinates of each point on the working plane to be inspected are: The parameters are those of the working plane to be inspected; Step S2.1.3: First, obtain all other points besides the three points mentioned above. p i Euclidean distance to the working plane to be inspected : in, Point p i The coordinates; Then, determine the point. p i Does it belong to the work plane to be inspected? If Then the point is considered p i This point belongs to the working plane to be inspected. p i Add it to the target point set; otherwise, consider the point as... p i This point does not belong to the working plane to be inspected. p i Points outside the target point set; then, all points p i After collecting the Euclidean distance data, obtain the number of points in the target point set: if the number of points in the target point set exceeds the minimum number of points in the set... Then, the working plane to be tested is taken as the qualified fitting working plane, and all points in the target point set are... p i The average Euclidean distance to the working plane to be inspected is taken as the average error distance of the working plane to be inspected. ; If the number of points in the target point set does not exceed the minimum number of points in the point set. If the result is negative, the working plane to be tested is considered an unqualified fitted working plane. Step S2.1.4: Repeat steps S2.1.2 to S2.1.3 multiple times to obtain multiple qualified fitting working planes, and select the average error distance. The working plane corresponding to the minimum value is taken as the optimal working plane.

4. The method for constructing a robot teleoperation force perception presence by fusing visual information according to claim 1, characterized in that: The specific operation for obtaining the point cloud set based on the optimal working plane is as follows: Step S2.2.1: Select point cloud data points from the point cloud model of the slave environment. Obtain the normal vector of the optimal working plane. Obtain point cloud data pointing to any point in the optimal working plane. vector , will vector sum vector Dot product, to get the dot product result: If the dot product is positive, then the point cloud data is considered to be... To ensure the data is valid, retain this point cloud data. ; Otherwise, the point cloud data is considered... This point cloud data is not required; delete it from the point cloud model of the endpoint environment. ; Step S2.2.2: Repeat step S2.2.1 multiple times until no new point cloud data points are added, completing the removal of all unnecessary data and retaining all the remaining point cloud data. Constructing a point cloud set .

5. The method for constructing a robot teleoperation force perception presence by fusing visual information according to claim 1, characterized in that: The specific steps for obtaining the target neighborhood point set by performing clustering and segmentation of the end-user environmental point cloud are as follows: Step S2.3.1: Perform initial parameter settings: Set the domain radius And the minimum number of neighborhood points, min_sampels; Step S2.3.2: First, select the point cloud set. Any point in the array can be used as the initial base point. In point cloud collection The base point is obtained through Kdtree. The adjacent points are used as the base points. The corresponding adjacent points, and determine whether the adjacent points are base points. neighborhood points If the base point The distance between the points is less than or equal to the distance between the points and the adjacent points. Then the adjacent point is the base point. neighborhood points To the neighboring point Add initial base point The corresponding point set N Pd middle; Otherwise, the adjacent point is considered not to be a base point. If a point is found to be a neighboring point, discard that point; then, iterate through all neighboring points until there are no new neighboring points. Step S2.3.3: Select point set N Pd Neighborhood points As a new starting point Repeat step S2.3.2 to obtain a new base point. The corresponding neighboring points, and the new base point The corresponding neighboring points are also added to the initial base point. The corresponding point set N Pd middle; Traversing point set N Pd From all points in the set N, until no new neighboring points are found, the current point set N is changed. Pd As the target neighborhood point set; Step S2.3.4: Reselect the point cloud set Another point in the matrix is ​​used as the next initial base point. And establish a new point set N Pd Repeat steps S2.3.2 to S2.3.3 to obtain another new target neighborhood point set; traverse the point cloud set. By analyzing all points in the target neighborhood until no new points are found, the complete set of target neighborhood points is obtained.

6. The method for constructing a robot teleoperation force perception presence by fusing visual information according to claim 1, characterized in that: The specific steps for building interactive operational capabilities are as follows: First, the optimal working force estimation parameters of the radial basis function neural network environment estimator are obtained by processing according to the following formula. : Where arg min[X] represents the value when X reaches its minimum. sup(Y) represents the value when Y reaches its maximum value. , Both are bounded sets. These are the characteristic parameters of the working force in the working environment. For radial basis function neural network matrices, The input matrix is ​​used; then, based on the optimal working capacity estimation parameters... Gain interactive operation capabilities : Where t represents time. Indicates a delay. This is the matrix of a radial basis function neural network.

7. The method for constructing a robot teleoperation force perception presence by fusing visual information according to claim 1, characterized in that: The specific steps for constructing the target virtual guiding force are as follows: Step S2.1: Take the current position of the robot's end effector as the starting point. Use the mission objective location as the path endpoint. ; Step S2.2: Constructing a cylindrical guiding force First, obtain the path endpoint. Directly above and from the end of the path for point As the end of the cylindrical guiding force, based on the starting point With point Construct the desired path ,by As axis, A cylindrical guiding force field is constructed for the radius, and the desired path length is obtained by processing it according to the following formula. : Then, the robot's end-effector position is acquired in real time. Obtain the desired path Middle and robot end position The nearest point : Next, obtain the robot's end effector and the desired path. Deviation between And obtain the distance between the robot's end effector and the cylindrical guiding force end effector. : Finally, a virtual guiding force is constructed for the robot's end effector under a cylindrical guiding force field. : in, These are two positive gain parameters; For direction by point to The unit component of force; For direction by point to The unit component of force; Step S2.3: Constructing a conical guiding force First, using the end point of the cylindrical guiding force The starting point is the location of the mission objective. A desired path is constructed for the endpoint, and a conical guiding force field is constructed with this desired path as the axis. The cone opening angle of the guiding force field is... The unit vector of the central axis of the cone Represented as: Next, the position of the robot's end effector is acquired in real time. ,Will relative to the mission target location The line connecting the cones can be decomposed into components along the axis of the cone. and components in orthogonal directions And obtain the position of the robot's end effector. The distance from the conical guiding force boundary to the axis of the cone. : in, It is a 3×3 identity matrix; when When, it indicates the robot's end point. Within the cone-shaped guiding force field, the robot's end point is obtained. Radial deviation from the desired path ; when When, it indicates the robot's end point. Outside the conical guiding force field, a new conical guiding force field is re-established until the robot's end point. Inside the cone-shaped guiding force field; Finally, a virtual guiding force is constructed for the robot's end effector under a conical guiding force field. : in, There are two gain parameters; Step S2.4: Construct the target virtual guiding force If the robot's end effector position Distance between the target location and the target location The virtual guiding force under the cone-shaped guiding force field As a target virtual guiding force ; Otherwise, the virtual guiding force under the cylindrical guiding force field As a target virtual guiding force .

8. The method for constructing a robot teleoperation force perception presence by fusing visual information according to claim 1, characterized in that: The specific steps for constructing the virtual environment force are as follows: Step S3.1: First, combine all the slave obstacle models into a slave obstacle model set. The bounding box algorithm is used to obtain a bounding box corresponding to each slave obstacle model, and the relative position of each slave obstacle model and the size of each bounding box are obtained. Next, in the set of obstacle models at the end. Select any one of the end obstacle models Obtain the obstacle model from the end. The center position of the corresponding bounding box and the side lengths of each side of the bounding box In the set of obstacle models at the end Selecting from the obstacle model at the end Any external obstacle model Obtain the obstacle model from the end. The center position of the corresponding bounding box and the side lengths of each side of the enclosure. The shortest distance between the two bounding boxes is obtained through processing. : in, For coefficients, subscript ,coefficient The following formula is used to obtain: Then, in the set of obstacle models at the end. Selecting from the obstacle model at the end For any external obstacle model, repeat the above steps in the set of external obstacle models. Select the obstacle model from the middle and the end. Minimum distance end obstacle model And the end obstacle model As a model of end-obstacles The corresponding nearest model is then processed according to the following formula to obtain the end obstacle model. Model magnification factor : in, These are the weighting coefficients. For the obstacle model at the end and its corresponding end-obstacle model The distance between them; For the end obstacle model The side lengths of each side of the bounding box are corresponding to the bounding box; finally, the obstacle model from the end will be... Each vertex expands outward along the normal direction The length is then remodeled and updated, and the updated model is used as the slave obstacle model. A virtual repulsive field model; In the set of obstacle models at the end Select a new slave obstacle model and repeat the above steps to obtain the virtual repulsion field models corresponding to all slave obstacle models; Step S3.2: When the force feedback point contacts the virtual repulsion field model and moves within it, a virtual contact point is set so that it slides on the surface of the repulsion field model. When the distance between the virtual contact point and the force feedback point is minimized, this virtual contact point is taken as the target virtual contact point corresponding to the force feedback point. The force feedback point is the position of the robot's end effector, which moves in real time. The virtual environmental force is obtained according to the following formula. : in, The distance between the force feedback point and the target virtual contact point; The rate of increase of the distance between the force feedback point and the virtual contact point of the target. The elastic coefficient, is the damping coefficient.

Images