Robot global attitude estimation method combining physical modeling and visual perception

Abstract

The invention discloses a robot global attitude estimation method combining physical modeling and visual perception, which combines a Cosseerat Rod theory, utilizes sparse internal sensor input to model internal strain distribution of a flexible body, and utilizes dual quaternion representation to carry out efficient global attitude coding. In order to realize comprehensive spatial registration, the method is further combined with attitude clues of monocular vision depth estimation, so that joint registration between the reconstructed internal shape and external visual feedback is realized. The fusion framework dynamically corrects a difference between a shape based on an internal physical model and an external visual perception attitude, thereby realizing accurate and stable control in a complex or limited environment. The method is particularly effective in applications such as surgical robots and soft continuum actuators, as these applications require minimal perception and strong global-local state perception.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of continuum robot state perception, and particularly relates to a robot global pose estimation method combining physical modeling and visual perception, which is suitable for precise state perception of continuum robots in complex environments such as surgical navigation and industrial detection. BACKGROUND

[0002] Continuum robots and soft manipulators have unique advantages in the navigation of complex or restricted environments, such as human body cavities or industrial pipelines. However, due to limited internal sensing resources and the highly nonlinear characteristics of soft deformation, it is still a major challenge to achieve accurate shape reconstruction and end-effector pose estimation in such robots.

[0003] The shape estimation method of continuum robots in the prior art relies on dense sensing (such as fiber Bragg grating arrays), which increases complexity, cost, and calibration requirements. Similarly, end-effector pose estimation techniques based solely on external vision, especially monocular cameras, also have problems of scale ambiguity and depth uncertainty. These methods face problems such as high cost, complex calibration, and interference from occlusions, and lack a joint optimization mechanism for internal mechanical physical models and external visual perception, resulting in insufficient accuracy of global state estimation. SUMMARY

[0004] The purpose of the present application is to provide a robot global pose estimation method combining physical modeling and visual perception, which combines Cosserat Rod modeling theory, dual quaternion pose representation, and monocular vision pose solving, and through the joint optimization of the three modules, it realizes high-precision shape reconstruction and end-effector pose estimation of flexible continuum robots. The present application is decoupled and integrated with path planning as a pre-state perception module for path planning; it is suitable for flexible structure state perception tasks such as surgical navigation and pipeline detection.

[0005] The technical solution to achieve the purpose of the present application is a robot global pose estimation method combining physical modeling and visual perception, characterized by the following specific steps:

[0006] Step 1, strain information acquisition; use the strain sensors sparsely arranged in the continuum robot to collect deformation data of different segments of the continuum robot; proceed to Step 2,

[0007] Step 2, based on the cosserat rod theory, reconstruct the internal shape of the continuum robot to obtain the shape skeleton point cloud of the physical model of the continuum robot, i.e. the three-dimensional centerline point cloud and the attitude rotation matrix; proceed to Step 3,

[0008] Step 3, based on dual quaternions, model and optimize the end-effector pose of the continuum robot; proceed to Step 4,

[0009] Step 4, define each coordinate system and establish a conversion relationship, complete the unification of the posture and position.

[0010] Compared with the prior art, the present application has the following advantages:

[0011] (1) The global pose estimation method disclosed in the present application combines cosserat rod modeling with dual quaternion pose encoding: based on the Cosserat Rod theory, combined with sparse sensor input (such as curvature and axial strain) for spatial integration, the three-dimensional shape and pose trajectory of the continuum robot is constructed. In order to improve the compactness and numerical stability of the pose encoding, dual quaternions are introduced to uniformly represent the pose, realizing the simultaneous expression of rotation and translation of 6-DoF. This method still maintains high computational stability under low hardware resource conditions.

[0012] (2) Multi-source joint loss optimization strategy: a joint optimization framework is proposed, which integrates the continuum robot ontology modeling, visual depth perception and sensor feedback. By jointly estimating the end dual quaternion pose, the environmental depth re-projection error under the camera view, and the model residual error between the physical modeling and the sensor observation, a dynamic robust compensation mechanism for the error between the modules is realized. This method first uses dual quaternions for the alignment of the physical modeling and visual perception estimation of the continuum robot end, as a bridge for unified expression of spatial pose, providing a consistent and differentiable optimization expression for high-dimensional flexible systems, and has good scalability, which is convenient for integration with visual PnP solving, SLAM positioning and other perception modules.

[0013] (3) Multi-coordinate system adaptive calibration: based on PnP estimation and multi-source sensor input, a unified transformation model between the robot base coordinate, visual coordinate and world coordinate is constructed, realizing adaptive calibration and real-time update between coordinate systems; so as to realize the consistency alignment of the visual perception estimation result and the physical model output of the robot, seamlessly mapping the physical driven continuum modeling to the rigid form task space pose output, providing consistent description for robot trajectory control and vision. This process is the core bridge to realize the "perception-modeling-control closed loop". BRIEF DESCRIPTION OF DRAWINGS

[0014] Figure 1 The logic flow chart of the global pose estimation method of the present application; DETAILED DESCRIPTION

[0015] The terms used in the present application are only for the purpose of illustrating the embodiments of the present application, and are not intended to limit the present application. The following will be described in detail in combination with the accompanying drawings. Figure 1 Some embodiments of the present application are described in detail.

[0016] A robot global pose estimation method combining physical modeling and visual perception, the specific steps are as follows:

[0017] Step 1, strain information collection:

[0018] The strain sensor unit is selected from a fiber Bragg grating (FBG) or a resistance strain gauge, and the number of sensor units is less than or equal to 5.

[0019] The deformation variable data includes curvature variable κ(s), elongation rate ∈(s), and arc length s∈[0,L] representing the physical length coordinate of the center line from the robot base (s=0) to the end (s=L).

[0020] Step 2, based on the cosserat rod theory, combined with the spatial integration of the sparse sensor input, the shape skeleton point cloud of the continuum robot physical model is constructed, i.e. the three-dimensional center line point cloud and the attitude rotation matrix.

[0021] The specific steps are as follows:

[0022] Step 2.1, define the robot body coordinate system and variables:

[0023] Quantify the continuum robot center line with arc length s, and add an orthogonal frame {e1, e2, e3} at each point, where e2, e2, e3 represent the orthogonal basis in the robot body coordinate system, and e3 corresponds to the axial direction of the continuum robot; the three-dimensional center line point cloud p(s)∈R 3 , R 3 is a three-dimensional Euclidean space; the attitude rotation matrix R(s)∈SO(3); SO(3) is a three-dimensional special orthogonal group.

[0024] Define the curvature variable κ(s)=[κ x ,κ y ,τ] T , and the axial elongation rate ∈(s).

[0025] κ x ,κ y respectively represent the bending degree of the center line in the local coordinate system e1, e2, which describes the bending rate in the plane formed by each (i.e. e1-e3 plane and e2-e3 plane); τ represents the torsion rate, which represents the torsion angular velocity (i.e. the change in angle per unit length) of the continuum robot around its own axis e3.

[0026] Step 2.2, construct the kinematics equation of the continuum robot:

[0027]

[0028] wherein, is the vector to skew-symmetric matrix mapping.

[0029] where the initial boundary conditions are p(0) = 0, R(0) = I; I is the identity matrix;

[0030] Step 2.3, numerical integration and result extraction: uniformly or adaptively divide [0, L] into nodes i is the node number, N is the number of nodes; and in each segment, the curvature variable κ(s i ), elongation ∈(s i ) is solved according to the linear / spline interpolation method.

[0031] Step 2.4, the pose update formula is:

[0032]

[0033] wherein, R i is the rotation matrix at the position s i of the current step, indicating the pose of the node s i ; R i+1 is the rotation matrix at the position s i+1 of the next step; κ eff is the weighted average curvature vector of the node i→i+1; h = s i+1 -s i , h is the current step length, indicating the length of the center line along the rod.

[0034] Step 2.5, the position update formula is:

[0035] p i+1 = p i +hR i e3(1+∈ eff ),

[0036] wherein, P i is the spatial position at the current point s i ; p i+1 is the position at the next point s i+1 ; R i e3 is the axis direction under the current pose; ∈eff is the axial strain of the current step.

[0037] Therefore, after the above integration, the shape skeleton point cloud of the reconstructed continuum robot physical model is obtained:

[0038] {p(s), R(s)} 0≤s≤L

[0039] wherein, p(s) is a three-dimensional center line point cloud, and R(s) is a pose rotation matrix; the shape skeleton point cloud is subsequently used for pose registration.

[0040] and the end 6-DoF pose of the physical model of the continuum robot, p end = p(L) and R end = R(L) ;

[0041] Step 3, modeling and optimizing the end pose of the continuum robot based on dual quaternions;

[0042] Step 3.1, uniformly encoding the 6-DoF pose of the end of the continuum robot (i.e., the physical model) into the form of a dual quaternion;

[0043] The specific formula is:

[0044]

[0045] wherein, is a quaternion defined quantity, representing the dual quaternion encoding result of the 6-DoF pose of the end of the continuum robot, used to simultaneously represent the position and translation of the end; r represents the rotation matrix converted into a unit quaternion, and r = rotm2quat(R end ); t is a position vector, t = p end , a 3*1 translation vector, representing the position; ε is a dual unit element, satisfying ε 2 = 0, ε ≠ 0;

[0046] The dual quaternion can simultaneously encode translation and rotation, and is suitable for describing the pose of a rigid body end. Its advantages are:

[0047] (1) compact and unified expression: can simultaneously describe the position and orientation of the end, avoiding the redundancy and inconsistency problems caused by using a position vector + Euler angle / rotation matrix hybrid expression;

[0048] (2) compatible with visual estimation: consistent with the form of the 6-DoF pose output by the external visual PnP algorithm, facilitating direct alignment or construction of joint optimization objectives;

[0049] (3) support continuous optimization: good differentiability and numerical stability, suitable for end-to-end optimization calculation in pose regression, trajectory fitting or task space control.

[0050] Step 3.2, based on the visual PnP algorithm, performing depth estimation on the visual information of the continuum robot obtained by the camera arranged at the end of the continuum robot, and then obtaining the dual quaternion pose of the end estimated by visual perception;

[0051] Step 3.3, pose registration objective function construction;

[0052] To align the end-effector pose calculated from the physical model (from the Cosserat rod method) and the end-effector dual quaternion pose estimated from the visual perception, a pose registration loss function is constructed:

[0053]

[0054] To calculate the end-effector dual quaternion pose from the physical modeling (e.g., deformation of a flexible continuum);

[0055] To estimate the end-effector dual quaternion pose from the visual perception (e.g., PnP);

[0056] t model is the end-effector position vector of the continuum robot calculated from the physical modeling, t vision is the end-effector position vector of the continuum robot estimated from the visual perception.

[0057] λ scale is the weight factor, used to adjust the proportion of position error and pose error in the total objective function.

[0058] is the pose registration loss function, which reflects the registration error of aligning the pose and position of the results calculated from the physical modeling and the results estimated from the visual observation.

[0059] Step 3.4, optimization algorithm and initial value selection

[0060] Optimization algorithm: L-BFGS (Limited-memory Broyden-Fletcher-Goldfarb-Shanno) is a quasi-Newton optimization method suitable for nonlinear optimization problems. Therefore, the L-BFGS optimization algorithm is selected to optimize the pose registration loss function, which has the advantages of small memory overhead, fast convergence speed, and suitability for high-dimensional nonlinear optimization problems;

[0061] Initial value source: the initial pose is estimated from the camera image data and depth data by the visual PnP algorithm (such as EPnP, OPnP), which is used to initialize the dual quaternion.

[0062] Step 3.5, construct a multi-source joint loss function;

[0063] The multi-source combined loss function of the application realizes a dynamic robust compensation mechanism of the errors among the modules by combining the continuum robot end pose registration target function, the physical model projection and image depth reconstruction error under the camera view, and the model residual error between the physical model and the strain sensor observation. The dual quaternion is used for the first time for the physical modeling and perception estimation alignment of the continuum robot end, as a bridge for unified expression of the spatial pose, providing a consistent, differentiable optimization expression for the high-dimensional flexible system, and having good scalability, facilitating integration with visual PnP algorithm solving, SLAM positioning and other perception modules.

[0064] To realize the consistency of the physical model precision and the visual perception estimation, a comprehensive multi-source loss function is constructed

[0065]

[0066] for the pose registration target function;

[0067] for the physical model projection and image depth reconstruction error; the Euclidean distance square between the physical model calculation result and the visual perception estimation;

[0068] Among them, the physical model calculation result specifically refers to the intermediate state quantity derived based on the physical model (cosserat rod), including the rotation matrix R(s) and position p(s) along the arc length s and the derived quantities such as curvature κ(s) and axial strain ∈(s). In , the projection point is derived from p(s); in , the end pose {R(L), p(L)} is taken; in , the model predicted curvature is used to align with the sensor measurement.

[0069] for the model residual loss function between the physical model curvature and the strain sensor measurement. The square integral of the difference between the curvature measured by the sparsely arranged strain sensor (such as FBG) along the center line of the continuum robot and the curvature calculated and solved in the continuum physical model;

[0070] α, β, γ are dynamic adjustment parameters, which dynamically adjust the parameter size of the three items according to the specific task.

[0071] Step 4, define each coordinate system and establish the conversion relationship, complete the unification of the pose and the position;

[0072] After the modeling and optimization of the continuum robot are completed, the end pose result is unified and output to the task space coordinate system through coordinate transformation, to support subsequent applications such as navigation, control or system integration.

[0073] Step 4.1, Define three coordinate systems;

[0074] F B : Robot body base coordinate system, as the starting boundary condition of the physical model (Cosserat model). Used to describe the input and output basis of the robot centerline deformation variable;

[0075] F C : Camera coordinate system output by the vision system, pose estimation result obtained by the vision PnP algorithm, i.e. representing the target pose in this coordinate system;

[0076] F W : Task space coordinate system (world coordinate system), usually defined in the task space, such as for navigation, path planning, etc.

[0077] Step 4.2, Coordinate transformation and fusion relationship modeling;

[0078] Joint optimization needs to build the transformation relationship chain between the task space coordinate system and the robot body coordinate system as:

[0079] p w = T B · T BC · p CW

[0080] Where:

[0081] p w Position vector of a point in space in the task space coordinate system;

[0082] p CW Expression of the same space point in the robot body coordinate system;

[0083] T BC Transformation matrix from camera coordinate to robot body coordinate, representing the installation pose of the camera on the robot. Usually fixed parameters, can be preset through calibration or CAD model;

[0084] T B Pose of the camera relative to the task space coordinate system, obtained by the vision PnP algorithm and depth estimation output;

[0085] Used to convert the vision perception estimation information from the camera coordinate system to the robot body coordinate system, so as to align with the pose output by the robot physical model.

[0086] A point p in the task space coordinate system is observed by the camera to obtain its representation in the camera coordinate system (T

[0001] ), and then the installation transformation from the camera to the robot body (TBC ), and finally converted into the position p in the robot body coordinate system B , so as to realize the consistency alignment of the visual perception estimation result and the physical model output of the robot, and this process is the core bridge for realizing the "perception-modeling-control closed loop". The final pose output can be used as the input basis of the path planning and motion control module, supporting the calling of the subsequent task space trajectory generation, obstacle avoidance navigation and other functional modules.

[0087] The method can be used as a pre-state perception module of the path planning module and can be decoupled and integrated with the path planning.

[0088] The shape reconstruction and global pose estimation method of the continuum robot disclosed in the application is based on the Cosserat rod theory, and realizes the modeling of the internal strain and curvature of the continuum structure under sparse sensor data. The mechanical physical model (Cosserat rod model) of the continuum robot is fused with the dual quaternion to realize compact global pose encoding, and the external pose correction is realized by joint monocular vision depth estimation, so as to effectively solve the physical model-vision perception mismatch problem (such as dynamic correction of visual scale ambiguity and internal model error) in the application of the robot.

[0089] Embodiment 1:

[0090] The global pose estimation method of the continuum robot is used for global pose estimation of a flexible catheter robot, wherein the total length of the catheter robot is 300 mm, the diameter is 5 mm, four FBG strain sensor arrays are installed on different paragraphs, a camera is installed at the end of the catheter, and a 1080p monocular of 30 Hz is selected.

[0091] As shown in Figure 1 , the specific steps for pose estimation are as follows:

[0092] Step 1, real-time acquisition of FBG strain information, sent into the physical model solution to reconstruct the skeleton;

[0093] Step 2, monocular vision captures the target scene and matches the environmental features;

[0094] Step 3, unify the pose into a dual quaternion, and fuse it with the physical model end;

[0095] Step 4, output the corrected end position and shape trajectory for interpolation and trajectory control.

[0096] Although the embodiments of the application have been shown and described above, it should be understood that the above embodiments are exemplary and should not be construed as limiting the application, and those skilled in the art can make changes, modifications, replacements and modifications to the above embodiments without departing from the principles and purposes of the application within the scope of the application.

Claims

1. A robot global posture estimation method combining physical modeling and visual perception, characterized in that: The specific steps are: Step 1: Strain information collection: Use the sparsely arranged strain sensors in the continuum robot to collect deformation data of different segments of the continuum robot; go to step 2, Step 2: Based on the cosseratrod theory, reconstruct the internal shape of the continuum robot and obtain the shape skeleton point cloud of the continuum robot physical model, that is, the three-dimensional centerline point cloud and posture rotation matrix; then go to step 3. Step 3: Based on the dual quaternion, the end posture of the continuum robot is modeled and optimized; go to step 4. Step 4: Define each coordinate system and establish the transformation relationship to complete the unification of posture and position.

2. The robot global posture estimation method combining physical modeling and visual perception according to claim 1 is characterized in that: In step 1, the sparsely arranged strain sensors generally refer to ≤5 sensing units.

3. The robot global posture estimation method combining physical modeling and visual perception according to claim 1 is characterized in that: In step 2, define the robot body coordinate system and variables: The centerline of the continuum robot is quantified by arc length s, and an orthogonal frame {e1, e2, e3} is added to each point, where e1, e2, e3 represent the orthogonal basis in the robot body coordinate system, and e3 corresponds to the axis of the continuum robot; the three-dimensional centerline point cloud p(s)∈R 3 , R 3 is a three-dimensional Euclidean space; the posture rotation matrix R(s)∈SO(3); SO(3) is a three-dimensional special orthogonal group; Define the curvature variable κ(s) = [κ x ,κ y ,τ] T , axial elongation∈(s); κ x , κ y They respectively represent the degree of curvature of the center line in the local coordinate system e1 and e2, and describe the curvature of the center line in the planes they respectively constitute (i.e., the e1-e3 plane and the e2-e3 plane); the torsion rate τ represents the torsional angular velocity of the continuum robot around its own axis e3.

4. The robot global posture estimation method combining physical modeling and visual perception according to claim 3 is characterized in that: In step 2, the kinematic equations of the continuum robot are constructed: Where, is a vector to antisymmetric matrix mapping; and the initial boundary conditions are: p(0) = 0, R(0) = I; I is the identity matrix; Divide the centerline of the continuum robot into nodes i is the node number, N is the number of nodes; and the curvature variable κ(s i ), elongation∈(s i ); After posture and position updates, the shape skeleton point cloud of the physical model of the reconstructed continuum robot is obtained: {p(s),R(s)} 0≤s≤L ; Where p(s) is the 3D centerline point cloud and R(s) is the posture rotation matrix; the shape skeleton point cloud is subsequently used for posture registration.

5. The robot global posture estimation method combining physical modeling and visual perception according to claim 4 is characterized in that: In step 2, the posture update formula is: Where R i The position s of the current step i The rotation matrix at s represents the node i Posture; R i+1 The next position s i+1 The rotation matrix at κ eff is the weighted mean curvature vector of node i→i+1; h=s i+1 -s i , h is the current step length, which represents the length along the center line of the pole.

6. The robot global posture estimation method combining physical modeling and visual perception according to claim 4 is characterized in that: In step 2, the position update formula is: p i+1 =p i +hR i e3(1+∈ eff ), Where, P i is the current point s i The spatial position of p i+1 For the next point i+1 Position at R i e3 is the axis direction in the current posture; ∈eff is the axial strain in the current step.

7. The robot global posture estimation method combining physical modeling and visual perception according to claim 1 is characterized in that: The specific steps of step 3 are: Step 3.1, uniformly encode the end pose of the continuum robot physical model into the form of dual quaternion; Step 3.2: The end-point pose estimation technology based on external vision uses the PnP algorithm to perform depth estimation on the visual information of the continuum robot and convert it into the form of dual quaternion to obtain the end-point dual quaternion pose estimated by visual perception; Step 3.3, construct the pose registration objective function; Step 3.4: Optimization algorithm and initial value selection: The L-BFGS optimization algorithm is used to optimize the pose registration objective function; the PnP algorithm is used to estimate the initial pose from the image data and depth data collected by the camera, which is used to initialize the dual quaternion. Step 3.5: Construct a multi-source joint loss function.

8. The robot global posture estimation method combining physical modeling and visual perception according to claim 4 is characterized in that: In step 3.3, the pose registration objective function is constructed as: To calculate the dual quaternion pose of the end from physical modeling (such as the deformation of a flexible continuum); is the terminal dual quaternion pose estimated from visual perception (such as PnP); t model The end position vector of the continuum robot calculated by physical modeling, t vision The end position vector of the continuum robot estimated by visual perception; λ scale is the weight factor, which is used to adjust the proportion of position error and attitude error in the overall objective function; It is the pose registration objective function, which reflects the registration error of the pose and position alignment between the physical modeling calculation results and the visual observation estimation results.

9. The robot global posture estimation method combining physical modeling and visual perception according to claim 5, characterized in that: In step 3.5, the multi-source loss function constructed is: is the pose registration objective function; is the error between the physical model projection and the image depth reconstruction; the square of the Euclidean distance between the physical model calculation result and the visual perception estimation; is the model residual loss function between the model curvature and the strain sensor measurement, which is the square integral of the difference between the curvature measured by the sparsely set strain sensors along the center line of the continuum robot and the curvature calculated in the continuum physical model; α, β, and γ are dynamically adjusted parameters, and the parameter sizes of the three items are dynamically adjusted according to the specific tasks.

10. The robot global posture estimation method combining physical modeling and visual perception according to claim 1, characterized in that: In step 4, the transformation relationship chain between the task space coordinate system and the robot body coordinate system is: p B =T BC ·T CW ·p w in: p w The position vector of a point in space in the task space coordinate system; p B The expression of the same spatial point in the robot body coordinate system; T BC The transformation matrix from camera coordinates to robot body coordinates represents the installation pose of the camera on the robot; T CW The camera's position relative to the task space coordinate system; Used to transform visual perception estimation information from the camera coordinate system to the robot body coordinate system.

Images