Pose mapping method based on drift function and synchronous control system

Abstract

The invention discloses a pose mapping method based on a drift function and a synchronous control system, and belongs to the technical field of robot teleoperation and intelligent control. According to the method, a heterogeneous coordinate system mapping relation between a near-end VR head-mounted display and a far-end observation robot is established, a drift function activated based on normalized operability is introduced, and an unreachable area and a kinematics singular configuration are dynamically avoided; the system adopts a double-layer architecture of outer ring mapping and inner ring PID control, and high-continuity and high-precision pose synchronization is realized. According to the invention, accurate pose control of the far-end observation robot by a near-end operator can be realized, and teleoperation safety and telepresence are improved.

Description

Technical Field

[0001] This invention belongs to the field of robot teleoperation and intelligent control technology, and more specifically relates to a pose mapping method and synchronous control system based on a drift function, which is used to realize precise pose control of a remote observation robot by a near-end operator. Background Technology

[0002] Currently, while virtual reality can construct immersive remote scenes, traditional pose mapping methods do not fully consider the physical constraints of remote observation robots. On the one hand, robots have limited reachability, and some mapped poses exceed their range of motion, leading to unresponsiveness. On the other hand, robots exhibit kinematic singularities in specific poses, where even minor pose changes can trigger abrupt changes in joint angles, resulting in mapping discontinuities. If mapping is discontinuous or the robot enters a singularity region, it may cause the observation robot to lose control and fail to accurately acquire environmental images. Furthermore, existing mixed reality technologies require operators to be physically present, failing to meet the "remote isolation" requirements of teleoperation. Summary of the Invention

[0003] In view of this, the present invention provides a pose mapping method and a synchronization control system based on a drift function, which aims to solve the problems of poor spatial adaptation, inability to avoid singular regions and low synchronization response accuracy in traditional pose mapping, and facilitates efficient pose synchronization between near-end and far-end devices.

[0004] To achieve the above objectives, the technical solution adopted by the present invention is as follows: In a first aspect, the present invention provides a pose mapping method based on a drift function, the method comprising the following steps: S1: Establish a unified coordinate system, obtain the operator's head pose and the robot's end-effector pose respectively, and establish a proportional mapping between position and pose based on the coordinate system transformation relationship; S2: Design a drift function and use the robot Jacobian matrix to calculate the normalized operability. When the robot's desired pose has an inverse kinematic solution and the normalized operability is higher than a preset singular threshold, perform proportional mapping. When the robot's desired pose does not have an inverse kinematic solution or the normalized operability is not higher than the preset singular threshold, activate the drift function to keep the robot in the previous pose. S3: Using the discretization traversal method, the positions and poses within the operator's reachable space are traversed. The proportion of poses with valid inverse kinematic solutions and normalized operability higher than a preset singularity threshold is statistically analyzed and used as the mapping continuity, ensuring that it is not lower than a preset proportion value.

[0005] In one specific implementation, in step S1, a left-handed coordinate system is established using a SteamVR positioning base station to record the operator's head position posture. :

[0006] in, The Cartesian coordinates of the operator's head. The quaternion for the operator's head posture; A right-handed coordinate system was established using the robot's base to record the robot's end-effector pose. :

[0007] in, Let Cartesian coordinates be the coordinates of the robot's end effector. Let be the quaternion of the robot's end effector; Based on coordinate system transformation relationships, a proportional mapping between position and attitude is established to realize the transformation of quaternions in heterogeneous coordinate systems, wherein: Location mapping:

[0008] Attitude mapping:

[0009] in, These represent the initial positions of the operator's head and the robot's end effector, respectively.

[0010] In one specific implementation, the formula for calculating the normalized operability in step S2 is:

[0011]

[0012] in, Indicates normalized operability. This represents the maneuverability value of the robot in a certain form. This represents the maximum maneuverability of the robot within its workspace. Let Jacobian matrix be used for the robot.

[0013] In one specific implementation, in step S2, when the robot's desired pose does not have an inverse kinematic solution or the normalized operability is not greater than a preset singularity threshold, the drift function is:

[0014] in, Represents the drift function, for The robot's end-effector pose at any given moment. for The robot's end-effector pose at any given moment.

[0015] In one specific implementation, in step S3, the preset ratio of the mapping continuity is... .

[0016] Secondly, this invention also provides a pose mapping synchronization control system based on a drift function. This system employs a two-layer outer-inner loop architecture to achieve real-time stable tracking of the desired pose by the observation robot, wherein: The outer ring collects the operator's head pose in real time based on the positioning base station, applies the above-mentioned pose mapping method to obtain the robot's expected pose, and outputs it to the inner ring; The inner loop uses an analytical method to solve the inverse kinematics of the observed robot based on the desired pose to obtain the desired joint angles. It then establishes closed-loop control for the desired joint angles, dynamically adjusts the joint angular velocity using real-time joint angle errors, and sends the velocity command to the robot control cabinet to achieve closed-loop control of the joint angles.

[0017] In one specific implementation, the inner loop uses an incremental PID controller, with the control law as follows:

[0018] In the formula, For the first Incremental control signal at each sampling time; , and These are the sampling errors for the current time, the previous time, and the two time steps prior, respectively. The system sampling period; , , These are the proportional, integral, and differential gain coefficients, respectively.

[0019] Compared with the prior art, the present invention has at least the following beneficial effects: 1. This invention enables precise pose control of a remote observation robot by a near-end operator; it solves the problems of poor spatial adaptation, inability to avoid singular regions, and low synchronization response accuracy in traditional pose mapping, and achieves efficient pose synchronization between near-end and remote devices, thereby improving the safety and presence of remote robot operation.

[0020] 2. This invention combines proportional mapping with a drift function to adapt to heterogeneous spaces at the near and far ends, achieving high mapping continuity, which helps avoid discontinuities in pose mapping and significantly improves spatial adaptability.

[0021] 3. Based on the operability threshold and drift function, this invention can detect and avoid robot singular configurations in real time, prevent abrupt changes in joint angles, and achieve precise avoidance of singular regions.

[0022] 4. The inner-loop PID control of this invention reduces the robot's end-effector trajectory tracking error and joint response delay, and significantly improves the synchronization response accuracy, which helps to ensure high-quality acquisition of remote environmental images by the binocular camera and enhances the operator's sense of presence.

[0023] Other features and advantages of the invention will be set forth in the description which follows, and will be apparent in part from the description, or may be learned by practicing the invention. The objects and other advantages of the invention may be realized and obtained by means of the structures particularly pointed out in the written description and the accompanying drawings.

[0024] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0025] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used together with the embodiments of the invention to explain the invention and do not constitute a limitation thereof.

[0026] Figure 1 This is a schematic diagram of the spatial pose mapping process from the near-end operator to the far-end observation robot provided by the present invention.

[0027] Figure 2 The schematic diagram shows the pose mapping system configuration provided by this invention.

[0028] Figure 3 This is a schematic diagram of the real-time closed-loop control system for the pose of the observation robot provided by the present invention.

[0029] Figure 4 This is a schematic diagram of the PID control process for the inner ring joint angle provided by the present invention. Detailed Implementation

[0030] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are some embodiments of the present invention, but not all embodiments.

[0031] In the description of this invention, it should be noted that some processes described in this application specification and accompanying drawings include multiple operations that appear in a specific order. However, it should be clearly understood that these operations may be performed out of order or in parallel. Therefore, the following detailed description of the embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without inventive effort are within the scope of protection of this invention.

[0032] This invention specifically includes a pose mapping method based on a drift function and a pose mapping synchronization control system adapted to it. The method mainly includes the following steps: Step 1: Establish a unified coordinate system and pose representation.

[0033] Step 1-1: Use SteamVR positioning base stations to establish a left-handed coordinate system at the near end and record the operator's head position posture. ,in Cartesian coordinates, It is a quaternion of attitude.

[0034] Steps 1-2: Establish a right-handed coordinate system at the remote end using the robot's base and define the robot's end-effector pose. Based on coordinate system transformation relationships, a proportional mapping between position and attitude is established to realize the transformation of quaternions in heterogeneous coordinate systems: Location mapping:

[0035] Attitude mapping:

[0036] in These are the initial positions of the near-end head and the far-end robot, respectively.

[0037] Step 2: Design the drift function.

[0038] Define the drift function It is used to compensate for the reachability space constraints and singular region constraints of the observation robot.

[0039] When the robot's desired pose has an inverse kinematics solution and normalized operability ( When the singular threshold is set to 0.1, =0, execute proportional mapping; When the robot has no inverse solution or hour, This allows the robot to maintain its previous pose and avoid unreachable areas and strange configurations. In this invention, normalized operability The calculation formula is:

[0040] in, This represents the maneuverability value of the robot in a certain form. , For the robot's Jacobian matrix; This represents the maximum maneuverability of the robot within its workspace.

[0041] Step 3: Verify the continuity of the mapping.

[0042] Using a discretized traversal method, the positions and orientations within the operator's reachable space are traversed, and statistically valid inverse solutions are found. The pose percentage is defined as the mapping continuity. ,make sure This ensures the continuity of the mapping.

[0043] Furthermore, in this invention, the pose mapping synchronization control system based on the drift function is specifically as follows: The pose mapping synchronization control system adopts a two-layer architecture of "outer loop-inner loop" to achieve real-time stable tracking of the desired pose by the observation robot. Specifically: The outer ring is responsible for pose mapping calculation: Based on real-time acquisition of the operator's head pose from the positioning base station, the robot's desired pose is calculated using the aforementioned pose space mapping method based on the drift function, and then output to the inner loop controller.

[0044] The inner loop is responsible for PID control of the joint angle: The observation robot satisfies the Piper criterion. The inverse kinematics of the observation robot are solved analytically to obtain the desired joint angles.

[0045] Design an incremental PID controller with the following control law:

[0046] In the formula, For the first Incremental control signal at each sampling time; The current joint angle; , and These are the sampling errors for the current time, the previous time, and the two time steps prior, respectively. This is the system sampling period.

[0047] Joint speed commands are sent to the robot control cabinet via the EtherCAT bus to achieve closed-loop control of joint angles.

[0048] The relevant principles and specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings: Figure 1 The process of spatial pose mapping from near-end operator to far-end observation robot is demonstrated: First, the operator's head pose is obtained through virtual reality positioning base station. If it exceeds the allowable range of pose space, the pose is marked as abnormal information and removed. If it does not exceed the allowable range, the desired pose of the far-end observation robot is solved through pose space mapping algorithm.

[0049] Figure 2 The present invention demonstrates the configuration of the pose mapping system proposed in this invention: wherein the head-mounted virtual reality device is located at the near end, in the left-handed coordinate system established by the base station; and the observation robot is located at the far end, in the right-handed coordinate system established by its base.

[0050] Figure 3 The demonstration showcased a real-time closed-loop pose control system for an observation robot. This system dynamically adjusts the controller's strategy by acquiring real-time feedback data on the robot's position, velocity, and other status, ensuring the robot can respond quickly and accurately complete its intended tasks. The control system employs a two-layer structure. In the outer layer, the desired pose of the robot is calculated through pose space mapping. In the inner layer, an inverse kinematics model of the robot is established to solve for the desired joint angles. A closed-loop control system is then established for these desired joint angles. This system dynamically adjusts the joint angular velocity using real-time joint angle errors and sends velocity commands to the robot control cabinet via a robot joint angular velocity control protocol. Upon receiving the joint velocity adjustment commands, the robot control cabinet uses internal servos to complete the joint velocity control.

[0051] Figure 4 The specific process of inner-loop joint angle PID control is demonstrated: First, the joint angle PID controller synchronously receives two key signals: the desired joint angle generated by pose space mapping. and the current joint angle collected in real time by joint sensors. And through the formula Calculated joint angle error Next, the joint angle PID controller measures the angle error. Performing proportional, integral, and differential operations: the proportional element responds quickly. To reduce instantaneous deviations, the integral stage accumulates... To eliminate steady-state error, the differential element predicts... The trend is changed to suppress overshoot, and the final output joint desired angular velocity is obtained. Subsequently, the joint speed controller receives... This is converted into a drive signal that the joint motor can execute, driving the motor according to... The corresponding rotational speed causes the joint to rotate at its current angle. From the perspective of expectations Approaching; finally, the joint sensors collect the adjusted data in real time. And feed back to the joint angle PID controller, and re-engage. The calculations form a continuously iterative closed-loop control to ensure... Stable and accurate tracking This provides a stable angle control basis for the synchronization of the robot's end-effector pose.

[0052] In a specific scenario implementation, Step 1: System Calibration: The binocular camera was calibrated using Zhang's calibration method to obtain intrinsic and extrinsic parameters. The calibration results show that the focal length of the left eye is... , The principal point is (648.22, 348.35). Initialize the initial poses of the proximal head and the distal robot. Establish coordinate system transformation relationships.

[0053] Step 2: Pose mapping and control program development: Develop a pose mapping module in Unity3D, and use C# to read the VR headset pose and perform proportional mapping calculations. Develop a PID control module in Visual Studio, establish TCP communication with the robot control cabinet via C++, and send joint speed commands.

[0054] Step 3: Experimental verification: The operator wears a VR headset and moves their head, while the system calculates the robot's desired pose in real time and controls its movement. Collect robot end-effector trajectory and joint response data to verify that the trajectory tracking error is ≤0.03m, the joint delay is ≤0.5s, and the mapping continuity is ≥90%.

[0055] The PID parameters of each joint are tuned using an empirical method. Based on the tuned control parameters, the joint angle control algorithm of the robot is designed on the Visual Studio platform, and communication is established with the robot's virtual machine to form a complete real-time robot pose control simulation system. The overall performance of the proposed control system is analyzed through control simulation.

[0056] In the simulation of the control system, the robot is made to move at a constant angular velocity along a circular trajectory in the workspace. The desired radius of the circular trajectory is 0.1m, the desired angular velocity of the circular motion is 0.33rad / s, the update frequency of the desired trajectory is 3Hz, and the sampling frequency of the robot joint controller is 50Hz.

[0057] As will be apparent to those skilled in the art from the above description of the embodiments, the present invention proposes a pose mapping method and a synchronization control system based on a drift function, which has the following advantages: By combining proportional mapping with drift functions, it adapts to heterogeneous spaces at the near and far ends, achieving a mapping continuity of over 90%, avoiding discontinuities in pose mapping, and significantly improving spatial adaptability.

[0058] Based on the operability threshold and drift function, the robot can detect and avoid singular configurations in real time, prevent abrupt changes in joint angles, and achieve precise avoidance of singular regions.

[0059] Inner-loop PID control ensures that the robot's end-effector trajectory tracking error is ≤0.03m, joint response delay is ≤0.5s, and synchronization response accuracy is significantly improved. This guarantees high-quality acquisition of distant environmental images by the binocular camera, enhancing the operator's sense of presence.

[0060] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, synchronization control systems, devices, or computer program products, etc. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media containing computer-usable program code.

[0061] It should be noted that the word "comprising" does not exclude the presence of components or steps not listed in the claims. The words "a" or "an" preceding a component do not exclude the presence of a plurality of such components. This invention can be implemented by means of hardware comprising several different components and by means of a suitably programmed computer.

[0062] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.

[0063] The above description of the disclosed embodiments enables those skilled in the art to implement or use the present invention. The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A pose mapping method based on a drift function, characterized in that, The method includes the following steps: S1: Establish a unified coordinate system, obtain the operator's head pose and the robot's end-effector pose respectively, and establish a proportional mapping between position and pose based on the coordinate system transformation relationship; S2: Design a drift function and use the robot Jacobian matrix to calculate the normalized operability. When the robot's desired pose has an inverse kinematic solution and the normalized operability is higher than a preset singular threshold, perform proportional mapping. When the robot's desired pose does not have an inverse kinematic solution or the normalized operability is not higher than the preset singular threshold, activate the drift function to keep the robot in the previous pose. S3: Using the discretization traversal method, the positions and poses within the operator's reachable space are traversed. The proportion of poses with valid inverse kinematic solutions and normalized operability higher than a preset singularity threshold is statistically analyzed and used as the mapping continuity, ensuring that it is not lower than a preset proportion value.

2. The pose mapping method based on a drift function according to claim 1, characterized in that, In step S1, a SteamVR positioning base station is used to establish a left-handed coordinate system and record the operator's head position posture. : in, The Cartesian coordinates of the operator's head. The quaternion for the operator's head posture; A right-handed coordinate system was established using the robot's base to record the robot's end-effector pose. : in, Let Cartesian coordinates be the coordinates of the robot's end effector. Let be the quaternion of the robot's end effector; Based on coordinate system transformation relationships, a proportional mapping between position and attitude is established to realize the transformation of quaternions in heterogeneous coordinate systems, wherein: Location mapping: Attitude mapping: in, These represent the initial positions of the operator's head and the robot's end effector, respectively.

3. The pose mapping method based on a drift function according to claim 2, characterized in that, In S2, the formula for calculating the normalized operability is: in, Indicates normalized operability. This represents the maneuverability value of the robot in a certain form. This represents the maximum maneuverability of the robot within its workspace. Let Jacobian matrix be used for the robot.

4. The pose mapping method based on a drift function according to claim 3, characterized in that, In step S2, when the desired robot pose does not have an inverse kinematic solution or the normalized operability is not greater than a preset singularity threshold, the drift function is: in, Represents the drift function, for The robot's end-effector pose at any given moment. for The robot's end-effector pose at any given moment.

5. The pose mapping method based on a drift function according to claim 1, characterized in that, In step S3, the preset ratio of the mapping continuity is... .

6. A pose mapping synchronization control system based on a drift function, characterized in that, The system employs a two-layer architecture of outer ring and inner ring to achieve real-time stable tracking of the desired pose by the observation robot, wherein: The outer ring collects the operator's head pose in real time based on the positioning base station, applies the pose mapping method as described in any one of claims 1-5 to obtain the robot's desired pose, and outputs it to the inner ring; The inner loop uses an analytical method to solve the inverse kinematics of the observed robot based on the desired pose to obtain the desired joint angles. It then establishes closed-loop control for the desired joint angles, dynamically adjusts the joint angular velocity using real-time joint angle errors, and sends the velocity command to the robot control cabinet to achieve closed-loop control of the joint angles.

7. A pose mapping synchronization control system based on a drift function according to claim 6, characterized in that, The inner loop uses an incremental PID controller, and the control law is: In the formula, For the first Incremental control signal at each sampling time; , and These are the sampling errors for the current time, the previous time, and the two time steps prior, respectively. The system sampling period; , , These are the proportional, integral, and differential gain coefficients, respectively.

Images