Navigation and positioning method and device for body intelligence

Abstract

The invention provides an intelligent navigation positioning method and device. The method comprises the following steps: acquiring a global picture of a static camera on a surgical robot; acquiring a dynamic video of a dynamic camera on the surgical robot; acquiring a positioning picture of an NDI camera on the surgical robot; navigating the robot to a preparation position based on the global picture, the positioning picture and the dynamic video; the relative position of the static camera and the NDI camera is fixed, and the relative position of the dynamic camera and the tail end tool of the robot is fixed. According to the method, the global picture and the dynamic video are acquired, so that autonomous navigation of the robot is realized on the basis.

Description

Technical Field

[0001] This application relates to the technical field of medical image processing, and more specifically, to a navigation and positioning method and device for embodied intelligence. Background Art

[0002] In recent years, with the continuous development of the fields of computer vision and artificial intelligence, embodied artificial intelligence (embodied AI) has received extensive attention from academia and industry at home and abroad. Embodied artificial intelligence emphasizes that an embodied intelligent agent actively obtains real feedback from the physical world through situational interaction with the environment, and makes the embodied intelligent agent more intelligent by learning from the feedback.

[0003] However, for a surgical robot, to achieve embodied intelligence, it is first necessary to achieve autonomous navigation to the surgical preparation position. Summary of the Invention

[0004] The problem solved by this application is the lack of an autonomous navigation solution for current surgical robots.

[0005] To solve the above problems, a first aspect of this application provides a navigation and positioning method for embodied intelligence, which includes:

[0006] Obtain a global picture of a static camera on the surgical robot;

[0007] Obtain a dynamic video of a dynamic camera on the surgical robot;

[0008] Obtain a positioning picture of an NDI camera on the surgical robot;

[0009] Based on the global picture, positioning picture, and dynamic video, navigate the robot to the preparation position;

[0010] The relative positions of the static camera and the NDI camera are fixed, and the relative position of the dynamic camera and the end effector of the robot is fixed.

[0011] A second aspect of this application provides a manufacturing system for the navigation and positioning method for embodied intelligence, which includes:

[0012] A first acquisition module, which is used to obtain a global picture of a static camera on the surgical robot;

[0013] A second acquisition module, which is used to obtain a dynamic video of a dynamic camera on the surgical robot;

[0014] A third acquisition module, which is used to obtain a positioning picture of an NDI camera on the surgical robot;

[0015] A preparation position navigation module, which is used to navigate the robot to the preparation position based on the global image, positioning image, and dynamic video; the relative positions of the static camera and the NDI camera are fixed, and the relative position of the dynamic camera and the end effector of the robot is fixed.

[0016] A third aspect of the present application provides an electronic device, including: a memory and a processor; the memory can be configured to store a program, and the processor is coupled to the memory and is used to execute the program in the memory for:

[0017] Obtain a global image of the static camera on the surgical robot;

[0018] Obtain a dynamic video of the dynamic camera on the surgical robot;

[0019] Obtain a positioning image of the NDI camera on the surgical robot;

[0020] Based on the global image, positioning image, and dynamic video, navigate the robot to the preparation position;

[0021] The relative positions of the static camera and the NDI camera are fixed, and the relative position of the dynamic camera and the end effector of the robot is fixed.

[0022] A fourth aspect of the present application provides a computer-readable storage medium, on which a computer program is stored, and the program is executed by a processor to implement the aforementioned navigation and positioning method for embodied intelligence.

[0023] In the present application, by obtaining the global image and dynamic video, the autonomous navigation of the robot is realized on this basis. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 It is a flowchart of the navigation and positioning method for embodied intelligence according to an embodiment of the present application;

[0025] Figure 2 It is a schematic diagram of the global image of the navigation and positioning method for embodied intelligence according to an embodiment of the present application;

[0026] Figure 3 It is an architecture diagram of the navigation and positioning device for embodied intelligence according to an embodiment of the present application;

[0027] Figure 4 It is an architecture diagram of the electronic device according to an embodiment of the present application. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0028] To make the above objects, features, and advantages of the present application more apparent and understandable, the following provides a detailed description of the specific embodiments of the present application with reference to the accompanying drawings. Although the accompanying drawings show exemplary embodiments of the present application, it should be understood that the present application can be implemented in various forms and should not be limited by the embodiments described herein. On the contrary, these embodiments are provided to enable a more thorough understanding of the present application and to fully convey the scope of the present application to those skilled in the art.

[0029] It should be noted that, unless otherwise specified, the technical terms or scientific terms used in the present application should have the ordinary meaning understood by those skilled in the art to which the present application belongs.

[0030] The embodiments of the present application provide the above-mentioned navigation and positioning method for embodied intelligence. The specific scheme of this method is Figure 1 - Figure 2 as shown. This method can be executed by a navigation and positioning device for embodied intelligence, and this navigation and positioning device for embodied intelligence can be integrated in electronic devices such as computers, servers, computers, server clusters, and data centers. Combining Figure 1 as shown, wherein, the navigation and positioning method for embodied intelligence includes:

[0031] S101, obtaining a global picture of a static camera on the surgical robot;

[0032] In the present application, a static camera is used to obtain a global view of the surgical environment, providing relatively complete spatial information. The global picture includes the surgical field, tool positions, patient anatomical structures, etc.

[0033] In the present application, it also includes preprocessing the global picture to correct image distortion, so as to improve the accuracy of the global picture.

[0034] S102, obtaining a dynamic video of a dynamic camera on the surgical robot;

[0035] In the present application, the dynamic camera dynamically captures the real-time changes in the surgical operation area and the movement of the trolley, providing support for local navigation and real-time adjustment. It can move with the end tool and provide a local high-resolution real-time video of the surgical area.

[0036] In the present application, the dynamic video includes the real-time position of the tool and the dynamic changes in the target area.

[0037] S103, obtaining a positioning picture of an NDI camera on the surgical robot;

[0038] In the present application, an NDI (Northern Digital Inc.) camera is used to obtain high-precision positioning information, providing a reference for navigation. It is mainly used to identify the spatial positions of marker points or sensors.

[0039] S104. Navigate the robot to the preparation position based on the global image, positioning image, and dynamic video.

[0040] The relative positions of the static camera and the NDI camera are fixed, and the relative position of the dynamic camera and the end effector of the robot is fixed.

[0041] In this application, the multimodal information of the static camera, dynamic camera, and NDI camera is combined to accurately navigate the robot to the "preparation position".

[0042] In this application, by obtaining the global image and dynamic video, autonomous navigation of the robot is realized on this basis.

[0043] In this application, the global image of the static camera is used to provide an overall environmental reference, the dynamic camera updates local information in real time to ensure that the navigation adapts to dynamic changes, and the NDI camera provides high-precision three-dimensional positioning data to ensure the accuracy of the target.

[0044] In this application, the relative positions of the static camera and the NDI camera are fixed, which is convenient for the registration of multimodal data; the static camera provides global environmental information, and the NDI camera provides accurate three-dimensional positioning.

[0045] In this application, the dynamic camera moves with the end effector and provides real-time updated local information of the surgical area. It is used for dynamic target tracking and local adjustment.

[0046] In a specific embodiment, S104, which navigates the robot to the preparation position based on the global image, positioning image, and dynamic video, includes:

[0047] Construct a three-dimensional map based on the global image, positioning image, and dynamic video.

[0048] On the three-dimensional map, plan the first path of the robot, where the first path is the movement path of the trolley.

[0049] On the basis of the completion of the first path planning, plan the second path of the robot, where the second path is the movement path of the robotic arm.

[0050] Navigate the trolley according to the first path, and navigate the robotic arm according to the second path.

[0051] In this application, for the construction of the three-dimensional map, the global image (static camera) can provide the spatial framework information of the overall environment, including the surgical scene, obstacle positions, target position information, etc.; the positioning image (NDI camera) can provide the three-dimensional spatial coordinates of key marker points for calibrating the accurate positioning of the global map; the dynamic video (dynamic camera) can capture real-time local scene changes (such as moving tools, dynamic obstacles).

[0052] In this application, during map construction, a three-dimensional point cloud map of the surgical environment can be generated using data from the global image and the NDI camera; the real-time information captured by the dynamic camera is overlaid onto the global point cloud map to update the position information of the dynamic obstacles.

[0053] In this application, in the constructed three-dimensional map, a voxel grid or a three-dimensional grid can be used to represent the three-dimensional map. Each voxel in the map records the occupancy status of the environment (such as obstacles, free space).

[0054] In this application, the starting point of the second path is the ending point of the first path.

[0055] In this application, the first path planning (the movement path of the trolley) moves the surgical robot trolley to the target position to ensure that the entire robot is aligned with the surgical site.

[0056] In this application, the second path planning (the movement path of the robotic arm) plans the movement path of the robotic arm after the trolley reaches the preparation position, so that the end effector accurately reaches the target operation position (the preparation position).

[0057] In this application, for the combined navigation of the trolley and the robotic arm, the trolley first moves to the target position according to the first path. After the trolley reaches the target position, the robotic arm starts to move according to the second path and reaches the preparation position.

[0058] In this application, a three-dimensional map is constructed through multi-modal data fusion, combined with the step-by-step planning of the trolley path and the robotic arm path, realizing the precise navigation of the surgical robot to the preparation position. It effectively integrates environmental perception, path optimization, and dynamic control technologies, providing an efficient and flexible solution for navigation in complex surgical environments.

[0059] In a specific embodiment, constructing the three-dimensional map based on the global image, the positioning image, and the dynamic video includes:

[0060] When the dynamic camera is within the global image, obtain the relative position relationship between the static camera and the NDI camera, and determine the coordinate system transformation matrix between the static camera and the NDI camera;

[0061] Identify the positioning bracket of the dynamic camera in the positioning image, and determine the coordinate system transformation matrix between the dynamic camera and the NDI camera;

[0062] Based on the coordinate system transformation matrix between the static camera and the NDI camera and the coordinate system transformation matrix between the dynamic camera and the NDI camera, determine the coordinate system transformation matrix between the static camera and the dynamic camera;

[0063] A real-time coordinate system transformation matrix based on a static camera and a dynamic camera is used to unify the global image and the dynamic video under the same coordinate system to construct a 3D map.

[0064] It should be noted that, as Figure 2 shown, it is the global image captured by the static camera. Due to surgical requirements, its specific installation position is relatively close to the surgical position, resulting in the fact that the range captured by this global image cannot cover the entire operating room. Therefore, during actual use, the position of the dynamic camera has two different states: being within the global image. In this case, the dynamic camera and its corresponding positioning frame can be visually observed within the global image; being outside the global image. In this case, the dynamic camera is outside the range of the global image or in an occluded area, making it impossible to visually observe the dynamic camera and its corresponding positioning frame within the global image. For different states, different methods are required to construct the 3D map.

[0065] It should be noted here that the dynamic camera itself does not have a positioning frame, but its relative position to the end effector is fixed, so the positioning frame of the end effector can be used as its corresponding positioning frame.

[0066] It should be noted here that in this application, the two different states of the position of the dynamic camera are actually distinguished by whether the NDI camera can capture the corresponding positioning frame. However, in this application, the range of the NDI camera is similar to that of the static camera, so it is directly set to the global image of the static camera for judgment to improve the intuitiveness and visibility of the entire global image analysis.

[0067] In this application, the relative position between the static camera and the NDI camera is fixed, which is convenient for obtaining the relative position relationship between the static camera and the NDI camera and unifying their coordinate systems.

[0068] In this application, the method for determining the coordinate system transformation matrix between the static camera and the NDI camera can be:

[0069] Set calibration objects (such as checkerboards or marked points with known sizes) within the field of view of the static camera and the NDI camera; extract the feature points of the calibration objects in the captured images of the two cameras; use the spatial positions of the calibration points to calculate the relative pose (rotation matrix and translation vector) between the two cameras; determine the transformation matrix based on the relative pose.

[0070] In this application, the specific method for determining the transformation matrix based on the relative pose is easily determined when the rotation matrix and the translation vector are known, and will not be elaborated in this application.

[0071] In this application, the dynamic camera is connected to the environment through a fixing device (positioning frame), and the marker points of the positioning frame can be recognized in the positioning image.

[0072] In this application, the calibration of the dynamic camera and the NDI camera:

[0073] Use the NDI camera to capture the marker points (usually optical positioning marker points) on the positioning frame; the fixed relationship between the coordinate system of the dynamic camera and the positioning frame can be solved through the positions of the marker points to obtain the corresponding rotation matrix and translation vector, and then determine the transformation matrix.

[0074] In this application, based on the known transformation matrix between the static camera and the NDI camera and the transformation matrix between the dynamic camera and the NDI camera, the transformation matrix between the static camera and the dynamic camera can be deduced.

[0075] In this application, the specific process of unifying the coordinate systems can be as follows: Global picture: The global picture captured by the static camera adopts the coordinate system of the static camera; Dynamic video: The video frames captured by the dynamic camera are transformed into the coordinate system of the static camera through the transformation matrix.

[0076] In this application, constructing a three-dimensional map can include:

[0077] Point cloud superposition: Use the three-dimensional point cloud generated from the global picture of the static camera as the basis; The video frame information of the dynamic camera is superimposed on the point cloud in real time.

[0078] Dynamic update: The dynamic camera captures the environmental changes in real time and updates the newly detected point cloud or obstacles to the three-dimensional map.

[0079] Fusion algorithm: Use voxel filtering or multi-view geometry optimization to remove redundant points and improve the map accuracy.

[0080] In a specific embodiment, constructing a three-dimensional map based on the global picture, the positioning picture, and the dynamic video includes:

[0081] When the dynamic camera is outside the global picture, extract the current image frame of the dynamic camera;

[0082] Extract the feature points of the global picture and the current image frame respectively through the feature extraction algorithm;

[0083] Match the common feature points of the global picture and the current image frame;

[0084] Based on the common feature points, determine the transformation matrix between the global picture and the current image frame;

[0085] Based on the transformation matrix, transform the current image frame to the global picture to construct a three-dimensional map.

[0086] In this application, when the dynamic camera is outside the global image, it is impossible to directly construct a coordinate system transformation matrix by fixing the relative position. Therefore, it is necessary to achieve the alignment of the image frame of the dynamic camera with the global image through image feature matching.

[0087] In this application, for the current image frame in the video stream captured in real time by the dynamic camera, a local scene image under the current field of view of the dynamic camera is obtained for matching with the global image.

[0088] In this application, feature point extraction: Key feature points and their descriptors are extracted from the global image and the current image frame of the dynamic camera respectively to facilitate subsequent matching.

[0089] In this application, feature extraction algorithm: Use SIFT (Scale-Invariant Feature Transform) or ORB (Oriented FAST and Rotated BRIEF). When selecting a suitable algorithm, it is necessary to balance feature matching accuracy and real-time performance: SIFT: Suitable for complex textures and lighting changes, with high feature stability; ORB: Higher computational efficiency, suitable for real-time applications.

[0090] In this application, feature point matching: Common feature points are found in the global image and the current image frame of the dynamic camera for calculating the transformation matrix. Feature point matching algorithm: Use the brute-force matching (Brute-Force Matcher) or FLANN (Fast Library for Approximate Nearest Neighbors) algorithm; for ORB features, use the Hamming distance; for SIFT features, use the Euclidean distance (L2 Norm).

[0091] In this application, the RANSAC (Random Sample Consensus) algorithm is used to remove mis-matched points and retain an accurate set of common feature points.

[0092] In this application, transformation matrix: Based on the common feature points, calculate the spatial transformation relationship (including rotation and translation) between the two images. Specifically: Fundamental matrix calculation: Use the common feature points to calculate the fundamental matrix; Essential matrix calculation: Calculate the essential matrix through the camera intrinsic matrix and the fundamental matrix; Camera pose estimation: Use the essential matrix decomposition to obtain the relative pose (rotation matrix and translation vector). Transformation matrix: Construct the transformation matrix between the global image and the current image frame.

[0093] 3D map construction: Based on the transformation matrix, align the current image frame of the dynamic camera to the global image coordinate system and construct a 3D map in the unified coordinate system. Image alignment: Transform the image frame of the dynamic camera to generate an image aligned with the global image. Point cloud overlay: Fuse the 3D point cloud generated from the global image with the point cloud corresponding to the image frame of the dynamic camera. Map update: The real-time changes captured by the dynamic camera (such as dynamic obstacles) are overlaid onto the global map to generate an updated 3D map.

[0094] In a specific embodiment, on the three-dimensional map, planning the first path of the robot includes:

[0095] Obtain the pose coordinates of the part to be operated on in the three-dimensional map;

[0096] Select a position within the reachable range of the pose coordinates as the target position;

[0097] Select the three-dimensional map within the height range of the trolley and project it into a two-dimensional map;

[0098] Based on the current position of the trolley and the template position, plan the trolley path on the two-dimensional map;

[0099] Determine whether the trolley path is feasible on the three-dimensional map;

[0100] If it is feasible, use the trolley path as the first path.

[0101] In this application, the pose coordinates of the part to be operated on include the three-dimensional spatial position of the part to be operated on and its posture (such as the rotation angle). Through the registration of the preoperative image data and the three-dimensional map, the precise position of the surgical site in the map is determined.

[0102] In this application, the method for obtaining the pose coordinates of the part to be operated on: positioning technology: using the NDI camera or other positioning devices on the surgical robot to obtain the coordinates of the marked points of the part to be operated on. Coordinate transformation: converting the local coordinates of the surgical site through the global coordinate system of the three-dimensional map to obtain the global pose coordinates of the part to be operated on

[0103] In this application, reachable range: the ranges of trolley movement and manipulator operation need to meet the following conditions:

[0104] Movement restrictions of the trolley: The maximum movement range and passing width of the trolley need to be considered.

[0105] Operation restrictions of the manipulator: The manipulator needs to be able to reach the part to be operated on from the trolley, and the degrees of freedom and arm span length of the manipulator determine the reachable range.

[0106] In this application, the reachable range of the pose coordinates is the range from which the end of the manipulator can reach the preparation position. In this application, the specific acquisition method of the reachable range can be obtained through individual simulations or by setting the active radius, and the specific acquisition method is not elaborated in this application.

[0107] In this application, target position selection: Select an optimal position as the target position of the trolley within the reachable range of the part to be operated on.

[0108] In this application, the target position is optimized by the following conditions: minimizing the moving distance between the trolley and the target position. Optimizing the angle and operating space of the robotic arm operation (avoiding extreme positions). Ensuring that there are no obstacles in the surrounding environment affecting the operation.

[0109] In this application, the selection of the trolley height range: taking the height of the trolley as a reference (such as 0.5 m to 1.5 m from the ground), intercepting the voxel grid or raster data within this height range in the three-dimensional map.

[0110] In this application, the two-dimensional map generation method: Voxel projection: Projecting the occupied voxels in the three-dimensional map onto a two-dimensional plane (such as the x-y plane). Marking the occupancy status: Occupancy status (obstacle): The area with obstacles in the projected voxel is marked as 1. Free status (passable): The obstacle-free area is marked as 0.

[0111] In this application, the path planning algorithm: Generating the first path through the A* algorithm, Dijkstra algorithm, or RRT (Rapidly-Exploring Random Tree).

[0112] In this application, judging whether the trolley path is feasible on the three-dimensional map: All the voxels in the three-dimensional map corresponding to the points on the path must be in the free status (no obstacles).

[0113] In this application, the feasibility verification method:

[0114] Path projection: Corresponding the two-dimensional path to the three-dimensional map to generate a three-dimensional path point sequence.

[0115] Voxel occupancy check: For each point in the path points, checking whether the corresponding three-dimensional voxel is in the free status.

[0116] Through judgment: If all points meet the obstacle avoidance conditions, the path is feasible; otherwise, re-planning is required.

[0117] In this application, combining the three-dimensional and two-dimensional maps for planning improves the efficiency of path planning and ensures the reliability of the planning results.

[0118] In a specific embodiment, the planning of the second path for the robot includes:

[0119] Based on the kinematic parameters of the robotic arm, constructing the kinematic equation of the robotic arm;

[0120] Obtaining the current position of the end effector of the robotic arm;

[0121] Based on the preparation position and the current position, planning the path nodes of the robotic arm;

[0122] Interpolating the adjacent path nodes of the robotic arm to obtain the second path.

[0123] In one embodiment, in the interpolation of adjacent path nodes of the robotic arm, polynomial interpolation algorithm is used to interpolate the adjacent path nodes of the robotic arm.

[0124] In one embodiment, interpolating the adjacent path nodes of the robotic arm to obtain the second path includes:

[0125] According to the kinematic equation of the robotic arm, determine the robotic arm joint angles corresponding to the path nodes of the robotic arm;

[0126] Through the particle swarm optimization algorithm, determine the interpolation time of each robotic arm joint among adjacent path nodes;

[0127] According to the interpolation time of each robotic arm joint among adjacent path nodes, and the joint angles at the starting and ending points, interpolate the adjacent path nodes to obtain the second path.

[0128] In this application, the particle swarm optimization algorithm is used to determine the interpolation time of the robotic arm joints. By determining the appropriate interpolation time, an optimal trajectory that meets the motion requirements can be obtained.

[0129] In one embodiment, the step of determining the interpolation time among adjacent path nodes through the particle swarm optimization algorithm includes:

[0130] Initialize the particle swarm, initialize the particle positions and velocities; each particle in the particle swarm represents a combination of robotic arm joint angles;

[0131] Solve the joint coefficients and joint stage velocities;

[0132] Judge whether the joint coefficients and joint stage velocities meet the speed limits;

[0133] When the joint stage velocities meet the speed limits, calculate the fitness of the particles;

[0134] Optimize the particle positions and velocities;

[0135] Judge whether the particle positions and velocities meet the boundary conditions;

[0136] When the particle positions and velocities meet the boundary conditions, obtain the individual optimal value and population optimal value of the particles;

[0137] Re-iterate the joint coefficients and joint stage velocities until the end condition is met;

[0138] According to the joint coefficients and joint stage velocities, determine the joint running time of each robotic arm joint;

[0139] Select the maximum value among the joint running times of each robotic arm joint as the interpolation time in the adjacent path nodes.

[0140] In one implementation, the path nodes include a path start point, path intermediate points, and a path end point; the interpolating the adjacent path nodes according to the interpolation time of each robotic arm joint in the adjacent path nodes and the joint angles of the start and end points to obtain the second path includes:

[0141] Obtain the stage path between the path start point and the first path intermediate point;

[0142] Interpolate the stage path between the path start point and the first path intermediate point using a cubic polynomial;

[0143] Obtain the stage path between the last path intermediate point and the path end point;

[0144] Interpolate the stage path between the last path intermediate point and the path end point using a cubic polynomial;

[0145] Obtain the stage path between adjacent path intermediate points;

[0146] Interpolate the stage path between adjacent path intermediate points using a quintic polynomial;

[0147] Traverse all the path intermediate points to obtain the second path.

[0148] In one implementation, the path nodes include a path start point, path intermediate points, and a path end point; the interpolating the adjacent path nodes according to the interpolation time of each robotic arm joint in the adjacent path nodes and the joint angles of the start and end points to obtain the second path includes:

[0149] Construct a minimum beat number trajectory, where the minimum beat number trajectory is a quintic polynomial;

[0150] Determine the boundary conditions for the start and end points of the adjacent path nodes; among them, the speeds and accelerations of the path start point and the path end point in the path nodes are both 0;

[0151] According to the minimum beat number trajectory and the boundary conditions, construct a system of equations and solve for the six coefficients of the quintic polynomial of the minimum beat number trajectory;

[0152] Substitute the six solved coefficients into the quintic polynomial equation to generate trajectory points within the time interval of the start and end points of the adjacent path nodes to obtain the interpolated stage path;

[0153] Traverse all the path intermediate points to obtain the second path.

[0154] In a specific embodiment, navigating the trolley along the first path includes:

[0155] Fix the relative pose of the dynamic camera and the trolley, and control the trolley to move along the first path;

[0156] Obtain the current dynamic video and identify the obstacle information in the dynamic video;

[0157] Project the obstacle information onto the three-dimensional map and confirm whether it is on the first path;

[0158] In the case of being on the first path, re-plan a new first path.

[0159] Fix the relative pose of the dynamic camera and the trolley so that the dynamic camera captures real-time information about the environment around the trolley, which can then be directly mapped for obstacle detection and path planning, improving the reaction efficiency and accuracy.

[0160] In this application, the path of the trolley can be dynamically updated according to the three-dimensional map to adapt to complex surgical scenarios.

[0161] In this application, utilize the precise obstacle avoidance ability of the three-dimensional map to ensure the feasibility and safety of the path.

[0162] In this application, control the trolley to move along the first path: Control algorithm: PID control: Adjust the linear velocity and angular velocity of the trolley to ensure that the trolley follows the path. Pure Pursuit algorithm: Select the next target point according to the path curve to guide the movement of the trolley. Control objective: Minimize the deviation between the current position of the trolley and the first path. Ensure smooth movement and avoid path deviation or jitter.

[0163] In this application, identify the obstacle information in the dynamic video: Use deep learning algorithms (such as YOLO, SSD) or traditional computer vision methods (such as color segmentation, edge detection) to detect obstacles from the video. Extract the key information of the obstacles: Position: Pixel coordinates of the obstacle within the camera's field of view. Size: Width, height or depth of the obstacle.

[0164] In this application, if the dynamic camera is an RGB-D camera or equipped with a lidar, the depth information of the obstacle can be directly obtained. If there is no depth information, the distance of the obstacle can be estimated through a binocular camera or multi-view geometry.

[0165] In this application, convert the pixel coordinates and depth information of the obstacle into three-dimensional coordinates in the camera coordinate system.

[0166] In this application, it is confirmed whether it is located on the first path: Obstacle projection: The obstacle is transformed from the dynamic camera coordinate system to the global coordinate system of the 3D map: 3D map update: The global coordinates of the obstacle are projected onto the 3D map to mark the voxels or grids where the obstacle is located. Path obstacle determination: Determine whether the obstacle is located on the current first path: Traverse each point on the path and check whether it overlaps with the voxels where the obstacle is located. If there is an overlap, it is determined that the path is blocked by the obstacle.

[0167] In this application, the obstacle information is monitored in real time by a dynamic camera, and combined with the 3D map update and path replanning technologies to ensure that the trolley can reach the target position safely and smoothly according to the planned path.

[0168] The embodiment of this application provides a navigation and positioning device for embodied intelligence, which is used to execute the navigation and positioning method for embodied intelligence described above in this application. The following provides a detailed description of the navigation and positioning device for embodied intelligence.

[0169] As Figure 3 shown, the navigation and positioning device for embodied intelligence includes:

[0170] The first acquisition module 101 is used to acquire the global picture of the static camera on the surgical robot;

[0171] The second acquisition module 102 is used to acquire the dynamic video of the dynamic camera on the surgical robot;

[0172] The third acquisition module 103 is used to acquire the positioning picture of the NDI camera on the surgical robot;

[0173] The preparation position navigation module 104 is used to navigate the robot to the preparation position based on the global picture, positioning picture and dynamic video; the relative positions of the static camera and the NDI camera are fixed, and the relative position of the dynamic camera and the end effector of the robot is fixed.

[0174] In one implementation, the preparation position navigation module 104 is further used for:

[0175] Based on the global picture, positioning picture and dynamic video, construct a 3D map; on the 3D map, plan the first path of the robot, where the first path is the movement path of the trolley; on the basis of the completion of the first path planning, plan the second path of the robot, where the second path is the movement path of the robotic arm; navigate the trolley according to the first path, and navigate the robotic arm according to the second path.

[0176] In one implementation, the preparation position navigation module 104 is further used for:

[0177] When the dynamic camera is located within the global image, obtain the relative position relationship between the static camera and the NDI camera, and determine the coordinate system transformation matrix between the static camera and the NDI camera; identify the positioning frame of the dynamic camera in the positioning image, and determine the coordinate system transformation matrix between the dynamic camera and the NDI camera; based on the coordinate system transformation matrix between the static camera and the NDI camera and the coordinate system transformation matrix between the dynamic camera and the NDI camera, determine the coordinate system transformation matrix between the static camera and the dynamic camera; based on the real-time coordinate system transformation matrix between the static camera and the dynamic camera, unify the global image and the dynamic video to the same coordinate system, and construct a three-dimensional map.

[0178] In one implementation, the preparation position navigation module 104 is further configured to:

[0179] When the dynamic camera is located outside the global image, extract the current image frame of the dynamic camera; respectively extract the feature points of the global image and the current image frame through a feature extraction algorithm; match the common feature points of the global image and the current image frame; based on the common feature points, determine the transformation matrix between the global image and the current image frame; based on the transformation matrix, transform the current image frame to the global image, and construct a three-dimensional map.

[0180] In one implementation, the preparation position navigation module 104 is further configured to:

[0181] Obtain the pose coordinates of the part to be operated on in the three-dimensional map; select a position as the target position within the reachable range of the pose coordinates; select the three-dimensional map within the height range of the trolley and project it into a two-dimensional map; based on the current position of the trolley and the template position, plan the trolley path on the two-dimensional map; determine whether the trolley path is feasible on the three-dimensional map; in the case of feasibility, use the trolley path as the first path.

[0182] In one implementation, the preparation position navigation module 104 is further configured to:

[0183] Based on the kinematic parameters of the robotic arm, construct the kinematic equation of the robotic arm; obtain the current position of the end tool of the robotic arm; based on the preparation position and the current position, plan the path nodes of the robotic arm; interpolate the adjacent path nodes of the robotic arm to obtain the second path.

[0184] In one implementation, the preparation position navigation module 104 is further configured to:

[0185] Fix the relative pose of the dynamic camera and the trolley, control the trolley to move according to the first path; obtain the current dynamic video, and identify the obstacle information in the dynamic video; project the obstacle information onto the three-dimensional map to confirm whether it is on the first path; in the case of being on the first path, re-plan a new first path.

[0186] The navigation and positioning device for embodied intelligence provided in the above embodiments of the present application has a corresponding relationship with the navigation and positioning method for embodied intelligence provided in the embodiments of the present application. Therefore, the specific content in this system has a corresponding relationship with the navigation and positioning method for embodied intelligence. The specific content can be referred to the records in the navigation and positioning method for embodied intelligence, and will not be elaborated herein again.

[0187] The navigation and positioning device for embodied intelligence provided in the above embodiments of the present application and the navigation and positioning method for embodied intelligence provided in the embodiments of the present application are based on the same inventive concept and have the same beneficial effects as the methods adopted, run or implemented by the application programs stored therein.

[0188] The internal functions and structures of the navigation and positioning device for embodied intelligence are described above. As Figure 4 shown, in practice, the navigation and positioning device for embodied intelligence can be implemented as an electronic device, including: a memory 301 and a processor 303.

[0189] The memory 301 can be configured to store programs.

[0190] In addition, the memory 301 can also be configured to store various other data to support operations on the electronic device. Examples of these data include instructions for any application program or method for operating on the electronic device, contact data, phone book data, messages, pictures, videos, etc.

[0191] The memory 301 can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic memory, flash memory, a magnetic disk or an optical disk. The processor 303, coupled to the memory 301, is configured to execute the programs in the memory 301 for:

[0192] Obtaining a global picture of a static camera on the surgical robot;

[0193] Obtaining a dynamic video of a dynamic camera on the surgical robot;

[0194] Obtaining a positioning picture of an NDI camera on the surgical robot;

[0195] Navigating the robot to the preparation position based on the global picture, the positioning picture, and the dynamic video;

[0196] The relative positions of the static camera and the NDI camera are fixed, and the relative position of the dynamic camera and the end effector of the robot is fixed.

[0197] In one embodiment, the processor 303 is further configured to:

[0198] Based on the global picture, the positioning picture, and the dynamic video, construct a three-dimensional map; on the three-dimensional map, plan a first path for the robot, where the first path is the movement path of the trolley; on the basis of the completion of the first path planning, plan a second path for the robot, where the second path is the movement path of the robotic arm; navigate the trolley according to the first path, and navigate the robotic arm according to the second path.

[0199] In one embodiment, the processor 303 is further configured to:

[0200] When the dynamic camera is within the global picture, obtain the relative position relationship between the static camera and the NDI camera, and determine the coordinate system transformation matrix between the static camera and the NDI camera; identify the positioning bracket of the dynamic camera in the positioning picture, and determine the coordinate system transformation matrix between the dynamic camera and the NDI camera; according to the coordinate system transformation matrix between the static camera and the NDI camera and the coordinate system transformation matrix between the dynamic camera and the NDI camera, determine the coordinate system transformation matrix between the static camera and the dynamic camera; based on the real-time coordinate system transformation matrix between the static camera and the dynamic camera, unify the global picture and the dynamic video under the same coordinate system to construct a three-dimensional map.

[0201] In one embodiment, the processor 303 is further configured to:

[0202] When the dynamic camera is outside the global picture, extract the current image frame of the dynamic camera; respectively extract the feature points of the global picture and the current image frame through a feature extraction algorithm; match the common feature points of the global picture and the current image frame; based on the common feature points, determine the transformation matrix between the global picture and the current image frame; based on the transformation matrix, transform the current image frame to the global picture to construct a three-dimensional map.

[0203] In one embodiment, the processor 303 is further configured to:

[0204] Obtain the pose coordinates of the part to be operated on in the three-dimensional map; within the reachable range of the pose coordinates, select a position as the target position; select the three-dimensional map within the height range of the trolley and project it into a two-dimensional map; based on the current position of the trolley and the template position, plan the trolley path on the two-dimensional map; determine whether the trolley path is feasible on the three-dimensional map; if it is feasible, use the trolley path as the first path.

[0205] In one embodiment, the processor 303 is further configured to:

[0206] Based on the kinematic parameters of the robotic arm, construct the kinematic equation of the robotic arm; obtain the current position of the end tool of the robotic arm; based on the preparation position and the current position, plan the path nodes of the robotic arm; interpolate the adjacent path nodes of the robotic arm to obtain the second path.

[0207] In one embodiment, the processor 303 is further configured to:

[0208] Fix the relative pose of the dynamic camera and the trolley, control the trolley to move along the first path; obtain the current dynamic video, and identify the obstacle information in the dynamic video; project the obstacle information onto the three-dimensional map to confirm whether it is on the first path; in the case of being on the first path, re-plan a new first path.

[0209] In this application, Figure 4 only some components are schematically shown, which does not mean that the electronic device only includes Figure 4 the components shown.

[0210] The electronic device provided in this embodiment and the navigation and positioning method for embodied intelligence provided in the embodiments of this application are based on the same inventive concept, and have the same beneficial effects as the methods adopted, run or implemented by the application programs stored therein.

[0211] Those skilled in the art should understand that the embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, this application can take the form of a computer program product implemented on one or more computer-readable storage media (including but not limited to disk memory, CD-ROM, optical memory, etc.) containing computer-usable program code.

[0212] This application is described with reference to the flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to the embodiments of this application. It should be understood that each process and / or block in the flowchart and / or block diagram, and the combination of processes and / or blocks in the flowchart and / or block diagram can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing devices generate for implementation in the process Figure 1 one process or multiple processes and / or blocks Figure 1means for the functions specified in one or more boxes. These computer program instructions can also be stored in a computer-readable memory capable of guiding a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory produce a manufactured article including an instruction means, and the instruction means implements the process Figure 1 one process or more processes and / or boxes Figure 1 means for the functions specified in one or more boxes.

[0213] These computer program instructions can also be loaded onto a computer or other programmable data processing device, so that a series of operation steps are executed on the computer or other programmable device to produce a computer-implemented process. Thus, the instructions executed on the computer or other programmable device provide for implementing the process Figure 1 one process or more processes and / or boxes Figure 1 steps for the functions specified in one or more boxes.

[0214] In a typical configuration, a computing device includes one or more processors (CPUs), an input / output interface, a network interface, and memory. The memory may include non-permanent memory in the form of computer-readable media, random access memory (RAM), and / or non-volatile memory, such as read-only memory (ROM) or flash memory (Flash RAM). Memory is an example of computer-readable media.

[0215] This application also provides a computer-readable storage medium corresponding to the navigation and positioning method for embodied intelligence provided in the foregoing embodiments. A computer program (i.e., program product) is stored thereon. When the computer program is run by a processor, it will execute the interactive image analysis assistance method for 3D aerial imaging provided in any of the foregoing embodiments.

[0216] Computer readable media include permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. Information can be computer readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disk read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassettes, magnetic tape magnetic disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer readable media does not include temporary computer readable media (transitory media), such as modulated data signals and carrier waves.

[0217] The computer-readable storage medium provided in the above-mentioned embodiment of the present application and the interactive image analysis auxiliary method for 3D aerial imaging provided in the embodiment of the present application are based on the same inventive concept and have the same beneficial effects as the methods adopted, run or implemented by the application programs stored therein.

[0218] It should be noted that in the description provided herein, a large number of specific details are described. However, it is understood that the embodiments of the present application can be practiced without these specific details. In some instances, well-known structures and technologies are not shown in detail so as not to obscure the understanding of this description.

[0219] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0220] The above is only an embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various changes and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the scope of the claims of the present application.

Claims

1. A navigation and positioning method for embodied intelligence, characterized in that: include: Get a global picture of the static camera on the surgical robot; Obtain dynamic video from the dynamic camera on the surgical robot; Get the positioning image of the NDI camera on the surgical robot; Based on the global image, the positioning image and the dynamic video, the robot is navigated to a preparation position; The relative positions of the static camera and the NDI camera are fixed, and the relative positions of the dynamic camera and the end tool of the robot are fixed.

2. The navigation and positioning method for embodied intelligence according to claim 1, characterized in that: The method of navigating the robot to a preparation position based on the global image, the positioning image and the dynamic video comprises: Constructing a three-dimensional map based on the global image, positioning image and dynamic video; Planning a first path of the robot on the three-dimensional map, where the first path is a movement path of the trolley; On the basis of the completion of the first path planning, a second path of the robot is planned, where the second path is a motion path of the robot arm; The trolley is navigated according to a first path, and the robotic arm is navigated according to a second path.

3. The navigation and positioning method for embodied intelligence according to claim 2, characterized in that: The constructing of a three-dimensional map based on the global image, the positioning image and the dynamic video includes: When the dynamic camera is located in the global picture, the relative position relationship between the static camera and the NDI camera is obtained, and the coordinate system conversion matrix of the static camera and the NDI camera is determined; Identify the positioning frame of the dynamic camera in the positioning picture, and determine the coordinate system conversion matrix of the dynamic camera and the NDI camera; Determine the coordinate system conversion matrices of the static camera and the dynamic camera according to the coordinate system conversion matrices of the static camera and the NDI camera, and the coordinate system conversion matrices of the dynamic camera and the NDI camera; Based on the real-time coordinate system conversion matrix of static cameras and dynamic cameras, the global image and dynamic video are unified into the same coordinate system to construct a three-dimensional map.

4. The navigation and positioning method for embodied intelligence according to claim 2, characterized in that: The constructing of a three-dimensional map based on the global image, the positioning image and the dynamic video includes: When the dynamic camera is outside the global picture, extracting a current image frame of the dynamic camera; The feature points of the global image and the current image frame are extracted respectively by a feature extraction algorithm; Match the common feature points of the global image and the current image frame; Based on the common feature points, determining a transformation matrix of the global image and the current image frame; Based on the transformation matrix, the current image frame is transformed into a global picture to construct a three-dimensional map.

5. The navigation and positioning method for embodied intelligence according to any one of claims 2 to 4, characterized in that: Planning a first path of the robot on the three-dimensional map includes: Obtaining the position coordinates of the surgical site in the three-dimensional map; Select a position within the reachable range of the pose coordinates as the target position; Select the three-dimensional map within the height range of the trolley and project it into a two-dimensional map; Planning a trolley path on the two-dimensional map based on the current position of the trolley and the template position; Determining whether the trolley path is feasible on the three-dimensional map; When feasible, the trolley path is used as the first path.

6. The navigation and positioning method for embodied intelligence according to any one of claims 2 to 4, characterized in that: The planning of the second path of the robot includes: Based on the kinematic parameters of the robot arm, the kinematic equation of the robot arm is constructed; Get the current position of the tool at the end of the robot arm; Based on the preparation position and the current position, planning the path nodes of the robot arm; Adjacent path nodes of the robot arm are interpolated to obtain the second path.

7. The navigation and positioning method for embodied intelligence according to any one of claims 2 to 4, characterized in that: The navigating the trolley according to the first path includes: Fixing the relative position of the dynamic camera and the trolley, and controlling the trolley to move along a first path; Get the current dynamic video and identify the obstacle information in the dynamic video; Projecting the obstacle information onto the three-dimensional map to confirm whether the obstacle is located on the first path; In the case of being on the first path, a new first path is replanned.

8. A navigation and positioning device for embodied intelligence, characterized in that: include: A first acquisition module, which is used to acquire a global image of a static camera on the surgical robot; A second acquisition module, which is used to acquire dynamic video of a dynamic camera on the surgical robot; A third acquisition module is used to obtain a positioning image of the NDI camera on the surgical robot; A preparation position navigation module is used to navigate the robot to the preparation position based on the global image, positioning image and dynamic video; the relative positions of the static camera and the NDI camera are fixed, and the relative positions of the dynamic camera and the end tool of the robot are fixed.

9. An electronic device, characterized in that: include: Memory and processor; The memory is used to store programs; The processor, coupled to the memory, is configured to execute the program to: Get a global picture of the static camera on the surgical robot; Obtain dynamic video from the dynamic camera on the surgical robot; Get the positioning image of the NDI camera on the surgical robot; Based on the global image, the positioning image and the dynamic video, the robot is navigated to a preparation position; The relative positions of the static camera and the NDI camera are fixed, and the relative positions of the dynamic camera and the end tool of the robot are fixed.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: The program is executed by a processor to implement the navigation and positioning method for embodied intelligence as described in any one of claims 1-6.

Images