Vision-force sense co-simulation method based on ROS and AppeliaSim

Abstract

The invention discloses a vision-force sense co-simulation method based on ROS and ConeliaSim, a co-simulator UR5sim is provided based on the ConeliaSim, the simulator takes a UR5 mechanical arm as a prototype, kinematics and dynamics parameters of the UR5 mechanical arm are accurately modeled, a ViSP (Visual Servo Platform) is integrated, a Visual Servo control module and abundant practical tools are provided through a ViSP code library, and the visual sense-force sense co-simulation method based on the Visual Servo Platform is realized. According to the UR5sim, the implementation process of the simulator and the deployment process of a user-defined UR5 controller are remarkably simplified. According to the simulator, efficient communication between the AppeliaSim and the simulation mechanical arm instance is achieved through the ROS. The algorithm instance provided by the invention is realized by adopting C + +, and the performance of the algorithm instance is comprehensively verified through a simulation experiment.

Description

Technical Field

[0001] The present invention relates to the field of robot simulation technology, and in particular to a vision-force collaborative simulation method based on ROS and CoppeliaSim. Background Art

[0002] The autonomy and intelligence of robotic systems are inherently tied to the availability of sensory information about their external environment. Among various sensory modalities, vision and force perception play crucial roles. Visual perception provides robotic systems with global information about their surroundings, which can be used for task planning and obstacle avoidance, while force perception enables them to adjust motion through contact forces to ensure compliance with local environmental constraints during interactions. However, they suffer from a significant drawback: the sensors operate independently and cannot correct for errors generated by each other.

[0003] CoppeliaSim enables multi-threaded collaborative control through the integration of multimodal interfaces (such as Lua scripts and ROS interfaces), effectively reducing system coupling. Its multi-physics engine fusion capabilities accurately model the kinematic and dynamic characteristics of the robotic arm and support the simulation of complex scenarios involving multi-sensor fusion (such as vision and force). This platform provides an efficient verification solution for scenarios with limited hardware resources or insufficient sensor accuracy, significantly reducing algorithm verification costs and improving development efficiency. Summary of the Invention

[0004] The present invention provides a vision-force collaborative simulation method based on ROS and CoppeliaSim, so as to realize robot vision-force collaborative simulation modeling based on the CoppeliaSim simulation platform.

[0005] In one embodiment of the present invention, a vision-force collaborative simulation method based on ROS and CoppeliaSim is provided. A simulation framework for hybrid vision and force control of a robotic arm is constructed using the virtual robot simulation experiment platform CoppeliaSim. The method includes:

[0006] S1, build a cross-platform CoppeliaSim-VISP real-time data interaction system based on ROS, where CoppeliaSim is used for simulation modeling of robot arm motion, and the visual servo control platform ViSP is integrated for visual servo control;

[0007] S2, establish an efficient kinematics solver for the robotic arm based on KDL;

[0008] S3, in the CoppeliaSim simulation environment, acquires force and visual information in real time to perform collaborative simulation modeling of the robot arm's hybrid vision and force control, and performs joint motion control based on the established multi-threaded synchronous PD position and speed control mechanism.

[0009] Furthermore, the S1 specifically includes:

[0010] The ROS publish-subscribe topic method is used to implement a standardized communication interface for data exchange. The data exchange content includes: the joint position and velocity information of the robot arm; the total mass, center of mass position and inertia tensor of the tool; the image data collected by the vision sensor; the camera parameters; the transformation relationship between the flange and the end effector; the transformation relationship between the end effector and the flange; the force and torque measurements collected by the force sensor; the camera hand-eye calibration data; the target velocity and torque information of the robot joint; and the control status of the robot.

[0011] A topic publish-subscribe relationship is established between the robot control program node and the ROS node of the CoppeliaSim simulation platform. / visp_ros serves as the robot control program node, and / sim_ros_interface serves as the ROS node of the CoppeliaSim simulation platform. Publish and subscribe communications are performed through multiple topics.

[0012] Furthermore, the S1 specifically includes:

[0013] A posture information message conversion mechanism is established to realize parameter transfer in posture conversion calculation.

[0014] Furthermore, the S2 specifically includes:

[0015] Kinematic model construction: Use KDL::Chain to build the kinematic model of the robot arm's links and joints;

[0016] Establish a forward kinematics solver: Use the KDL chain solver to achieve real-time mapping from Cartesian space to joint space;

[0017] Establish Jacobian matrix solver: Get the current joint angle Jacobian matrix by calling KDL::ChainJntToJacSolver();

[0018] Establish an inverse kinematics solver: implement position-level inverse kinematics solution through KDL::ChainIkSolverPos_NR_JL;

[0019] Cartesian-joint space velocity conversion and limitation: Create a KDL::ChainIkSolverVel_pinv object and call the CartToJnt() interface to convert the end velocity to the joint velocity. Based on the set maximum joint velocity, use the vpRobot::saturateVelocities() function to saturate the velocity command.

[0020] Furthermore, the S3 specifically includes:

[0021] Multi-threaded synchronous PD position control: adopts a main thread-sub-thread dual-thread design. The main thread periodically publishes the robot control state as position control POSITION_CONTROL through the ROS topic, and the sub-thread independently runs the position control loop at a preset frequency.

[0022] Furthermore, the S3 specifically includes:

[0023] Multi-threaded synchronous speed control: A main-thread-sub-thread dual-thread design is adopted. The main thread periodically publishes the robot control state as speed control VELOCITY_CONTROL through the ROS topic, and the sub-thread independently runs the speed control loop at a preset frequency.

[0024] Furthermore, the child thread independently runs a position control loop at a preset frequency, specifically including:

[0025] a. Access shared data through mutex locks to avoid resource competition;

[0026] b. Publish the robot status to the ROS topic to ensure that the CoppeliaSim simulation environment receives the command synchronously;

[0027] c. Calculate the joint position error q error =q des -q current , and determine whether it is greater than a preset threshold;

[0028] d. If the joint error is greater than the preset threshold, then Calculate the PD control output and set the joint saturation velocity vel_max, where q des represents the joint expected value, q current Indicates the current value of the joint, PD controller parameter K p , K d is a constant. When the calculated joint speed exceeds the saturation speed value, the joint speed is set to be equal to the joint saturation speed vel_max, and the speed command is sent through the ROS topic;

[0029] e. If the joint position error is less than the preset threshold, unlock the mutex, send a zero velocity command and exit the loop.

[0030] Furthermore, the child thread independently runs a speed control loop at a preset frequency, specifically including:

[0031] a. Access shared data through mutex locks to avoid resource competition;

[0032] b. Publish the robot status to the ROS topic to ensure that the CoppeliaSim simulation environment receives the command synchronously;

[0033] c. Set the joint saturation speed vel_max. When the joint speed exceeds the saturation speed value, the joint speed is set to be equal to the joint saturation speed vel_max, and the speed command is sent through the ROS topic;

[0034] d. At the end of the cycle, unlock the mutex lock and force a zero speed command to achieve a safe shutdown.

[0035] Furthermore, the S3 specifically includes:

[0036] The virtual robot simulation experiment platform CoppeliaSim was used to build a simulation experiment scene containing a robotic arm and vision and force sensors, including:

[0037] Build a simulation environment in CoppeliaSim. First, create a ground as a base plane. Then load the robotic arm model and place it above the ground to check the positions of the joints and end effector.

[0038] Add a force / torque sensor to the end effector, ensuring it is properly connected and capable of measuring the torque of the end effector. Mount the camera on the end effector in an "eye-in-hand" manner and adjust the camera's position and orientation so that it can correctly capture images of the workspace. For visual positioning, add an AprilTag as a target marker, place it on the table, and adjust its position.

[0039] Place a cylindrical socket on the table as the target nesting location for the end-of-arm tooling; replace the end-of-arm tooling with a cylindrical shaft, adjusting the size and shape so that it can precisely fit into the cylindrical socket on the table.

[0040] Furthermore, the S3 specifically includes:

[0041] After the simulation is initialized, it is iterated in a loop. In each loop iteration, the robot control program node calculates the Cartesian space tool coordinate system velocity of the robot arm at the next moment according to the constructed visual servo control law based on the current Cartesian space tool coordinate system pose information of the robot arm in the simulation, the speed of the Cartesian space tool coordinate system of the robot arm, the force of the Cartesian space tool coordinate system of the robot arm, the current pose and expected pose information of the AprilTag identified in the Cartesian space camera coordinate system of the robot arm, and publishes it through the ROS topic.

[0042] The present invention provides a vision-force collaborative simulation method based on ROS and CoppeliaSim, which has the following beneficial effects:

[0043] (1) Overcoming Hardware Resource and Sensor Accuracy Issues: UR5sim provides an effective solution to the problems of limited hardware resources or insufficient sensor accuracy. By testing and verifying in a simulated experimental environment, it avoids high hardware resource requirements, reduces the impact of errors caused by insufficient sensor accuracy, and improves the overall practicality and applicability of the system.

[0044] (2) Accurate modeling and simplified implementation process: Using the UR5 robotic arm as a prototype, its kinematic and dynamic parameters were accurately modeled. This ensures that the simulator can accurately simulate the real-world motion behavior of the robotic arm, providing a reliable experimental platform for subsequent algorithm verification and controller design. At the same time, the integration of ViSP (Visual Servo Platform) significantly simplifies the implementation process of the simulator itself and the deployment process of user-defined UR5 controllers, reducing development difficulty and cost while improving development efficiency.

[0045] (3) Efficient Communication and Comprehensive Verification: The simulator achieves efficient communication between CoppeliaSim and the simulated robotic arm instance through ROS, ensuring real-time data transmission and interaction, enabling the system to work more efficiently. Furthermore, the proposed algorithm instance is implemented in C++, and its performance is fully verified through simulation experiments, ensuring the effectiveness and reliability of the algorithm, providing strong support and guarantee for practical application. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 A flowchart of a vision-force collaborative simulation method based on ROS and CoppeliaSim is provided in accordance with one embodiment of the present invention;

[0047] Figure 2 A topic publish-subscribe relationship between a robot control program node and a ROS node of a CoppeliaSim simulation platform in a vision-force collaborative simulation method based on ROS and CoppeliaSim provided by one embodiment of the present invention;

[0048] Figure 3 A UR5 robotic arm link coordinate system in a visual-force collaborative simulation method based on ROS and CoppeliaSim provided in one embodiment of the present invention;

[0049] Figure 4 The DH parameter definition of the UR5 robot arm in a visual-force collaborative simulation method based on ROS and CoppeliaSim provided in one embodiment of the present invention;

[0050] Figure 5A geometric model of a pinhole camera in a vision-force collaborative simulation method based on ROS and CoppeliaSim provided in one embodiment of the present invention;

[0051] Figure 6 A diagram of a multi-threaded synchronous PD position and speed control mechanism in a vision-force collaborative simulation method based on ROS and CoppeliaSim, provided in one embodiment of the present invention;

[0052] Figure 7 An embodiment of the present invention provides a simulation experiment scenario in a vision-force collaborative simulation method based on ROS and CoppeliaSim. DETAILED DESCRIPTION

[0053] The present invention will be further described in detail below by means of specific embodiments in conjunction with the accompanying drawings. Similar elements in different embodiments are numbered with associated similar elements. In the following embodiments, many detailed descriptions are provided to enable the present invention to be better understood. However, those skilled in the art will readily appreciate that some of the features may be omitted under different circumstances, or may be replaced by other elements, materials, or methods. In some cases, some operations related to the present invention are not shown or described in the specification. This is to avoid the core of the present invention being overwhelmed by excessive descriptions, and for those skilled in the art, it is not necessary to describe these related operations in detail. They can fully understand the related operations based on the description in the specification and the general technical knowledge in the art.

[0054] In addition, the features, operations, or characteristics described in the specification may be combined in any appropriate manner to form various embodiments. Furthermore, the steps or actions in the method description may be reordered or adjusted in a manner readily apparent to those skilled in the art. Therefore, the various sequences in the specification and drawings are provided solely for the purpose of clearly describing a particular embodiment and are not intended to be mandatory, unless otherwise specified.

[0055] The first embodiment of the present invention provides a visual-force collaborative simulation method based on ROS and CoppeliaSim. A UR5Sim simulator is developed using CoppeliaSim and the Visual Servo platform. The simulator simulates the kinematic and dynamic parameters of a real UR5 robot and combines visual servoing functions. A key innovation of this method is to couple vision and force in the feature space, so that the task can utilize the interactive information from the two sensors. In addition, the introduction of BLF allows dynamic constraints to be imposed on visual errors. The proposed control algorithm improves the dynamic performance, flexibility and stability of the visual servo system. Figure 1 Provide detailed explanation.

[0056] S1, build a cross-platform CoppeliaSim-VISP real-time data interaction system based on ROS, where CoppeliaSim is used for simulation modeling of robotic arm motion, and the visual servo control platform ViSP is integrated for visual servo control.

[0057] The specific steps are as follows:

[0058] S11. Define the ROS standardized communication interface for data interaction required by the UR5 visual-force collaborative simulation system. The topic name, ROS message type, and data content are shown in the following table:

[0059]

[0060]

[0061] S12, build the topic publish-subscribe relationship between the robot control program node and the ROS node of the CoppeliaSim simulation platform as follows Figure 2 As shown in the figure, / visp_ros is the robot control program node, and / sim_ros_interface is the ROS node of the CoppeliaSim simulation platform. They publish and subscribe to multiple topics. The following is the main topic publishing and subscription topology:

[0062] S121, Joint state information: / CoppeliaSim / ur5 / joint_state publishes the joint position and velocity information of the robot arm, and / visp_ros subscribes to this topic to obtain real-time joint states.

[0063] S122. Tool inertia information: / CoppeliaSim / ur5 / tool / inertia publishes the inertia information of the tool, and / visp_ros subscribes to this topic to understand the physical characteristics of the tool.

[0064] S123, Image data: / CoppeliaSim / ur5 / camera / image publishes the image data of the visual sensor, and / visp_ros subscribes to this topic to obtain visual information.

[0065] S124, Camera information: / CoppeliaSim / ur5 / camera / camera_info publishes the camera parameter information, and / visp_ros subscribes to this topic to learn about the camera configuration.

[0066] S125, Transformation relationship between flange and end effector: / CoppeliaSim / ur5 / g0 and / CoppeliaSim / ur5 / flMe publish the transformation relationship between flange and end effector, and / visp_ros subscribes to these topics to perform coordinate transformation and motion planning.

[0067] S126, Camera hand-eye calibration data: / CoppeliaSim / ur5 / eMc publishes camera hand-eye calibration data, and / visp_ros subscribes to this topic to obtain the position relationship between the camera and the end effector.

[0068] S127, Force sensor data: / CoppeliaSim / ur5 / eMc publishes force and torque measurements from force sensors, and / visp_ros subscribes to this topic to obtain force feedback information.

[0069] S128, control state and target information: / ucl / joint_state publishes the target speed and torque information of the robot joint, and / visp_ros subscribes to this topic to obtain control instructions.

[0070] S129, / ucl / robot_state publishes the control status of the robot, and / visp_ros subscribes to this topic to understand the current control system status.

[0071] also, Figure 2 The topics that do not appear are the topics that come with the CoppeliaSim simulation platform and are responsible for handling the simulation pause, continue, start, and stop control commands: / pauseSimulation: used to pause the simulation, / triggerNextStep: used to trigger the next simulation, / enableSyncMode: used to enable the synchronization mode, / startSimulation: used to start the simulation, / stopSimulation: used to stop the simulation.

[0072] Through these topics, the robot control program ( / visp_ros node) can obtain various real-time data from the CoppeliaSim simulation environment and make control decisions and motion planning based on this data. The CoppeliaSim simulation platform ( / sim_ros_interface node) is responsible for exchanging simulation status and control instructions between CoppeliaSim and ROS, ensuring seamless interaction between the simulation environment and the ROS system.

[0073] S13, pose information message conversion mechanism: realize the bidirectional conversion between ROS geometry_msgs / Pose and ViSPvpHomogeneousMatrix, with conversion error ≤ 1×10-6 rad.

[0074] ROS geometry_msgs / Pose has actual physical meaning, while VISP vpHomogeneousMatrix is ​​a mathematical matrix without specific physical meaning. It is constructed only for subsequent calculations. The innovation of this embodiment is to build a physical simulation simulator UR5sim, which can obtain UR5's Pose information in the simulator and convert it into VISP vpHomogeneousMatrix matrix parameters for visual servo control law calculation using VISP. Finally, the calculation results are output into Pose posture through this conversion method and act on UR5, ultimately realizing UR5 servo control Pose calculation in the simulation platform.

[0075] S131. The specific steps for converting from geometry_msgs / Pose to vpHomogeneousMatrix are as follows:

[0076] S1311. Extract position information: Extract the position field from geometry_msgs / Pose, obtain the x, y, z coordinates, and construct the translation vector t = (x, y, z) T , where t is the three-dimensional coordinate value in the Cartesian coordinate system;

[0077] S1312. Extracting posture information: Extracting the orientation field from geometry_msgs / Pose to obtain the quaternion q = (qx, qy, qz, qw) to describe the rotation posture of the object.

[0078] S1313. Convert the quaternion to a rotation matrix: Use the quaternion to rotation matrix conversion formula to convert q to a 3x3 rotation matrix R;

[0079] Quaternion to rotation matrix:

[0080]

[0081] Rotation matrix R:

[0082]

[0083] S1314. Construct a homogeneous transformation matrix: combine the rotation matrix R and the translation vector t into a 4x4 homogeneous transformation matrix H;

[0084]

[0085] S132. The specific steps for converting from vpHomogeneousMatrix to geometry_msgs / Pose are as follows:

[0086] S1321. Extract rotation matrix: extract the 3x3 rotation matrix R from vpHomogeneousMatrix;

[0087] S1322. Extract translation vector: Extract the 3x1 translation vector t = (x, y, z) from vpHomogeneousMatrix T ;

[0088] S1323, converting the rotation matrix into a quaternion: using a rotation matrix to quaternion conversion algorithm, convert R into a quaternion q = (qx, qy, qz, qw);

[0089] S1324. Construct a geometry_msgs / Pose message: fill the translation vector t into the position field, fill the quaternion q into the orientation field, and construct a geometry_msgs / Pose message.

[0090] S2, establish an efficient kinematics solver for the robotic arm based on KDL.

[0091] The specific steps are as follows:

[0092] S21. Kinematic model construction: Based on Figure 3 and Figure 4 , define the link coordinate system of the UR5 six-degree-of-freedom robot arm with joint limit protection (±3.1415rad) according to the UR5 DH parameters, and use KDL::Chain to define the links and joints of the UR5 robot arm based on the parameters.

[0093] Figure 4 Physical meaning of the parameters:

[0094] a i-1 : The torsion angle of connecting rod i-1 (around x i-1 Axis rotation)

[0095] a i-1 : The torsion angle of connecting rod i-1 (around x i-1 Axis rotation)

[0096] d i : The torsion angle of connecting rod i-1 (around z i Axis rotation)

[0097] θ i : The torsion angle of connecting rod i-1 (around z i Axis rotation)

[0098] a2=0:4250m, a3=0:3922m, d1=0:0892m, d4=0:1091m, d5=0:0946m, d6=0:0823m, df=0:0805m.

[0099] S22. Implement the UR5 forward kinematics solver: Calculate the homogeneous transformation matrix of each link in turn based on the UR5 robot arm created in S21 Update the rotation and translation of the current link according to the joint angle q The transformation matrix of the next link Multiply them together to get the homogeneous transformation matrix of the UR5 robot arm relative to the end effector.

[0100] in express:

[0101]

[0102] S23, Jacobian Matrix Solver: Uses numerical differentiation to calculate the terminal velocity Jacobian matrix J(q) in real time. Calls KDL::ChainJntToJacSolver() to obtain the current joint angle Jacobian matrix. Accuracy ≤ 1×10-6rad / m;

[0103] S24. Implement the UR5 inverse kinematics solver: For a specific set of joint angles q = [θ1, θ2, θ3, θ4, θ5, θ6], use the forward kinematics function f(q) to represent the function mapping the joint angles to the end effector pose. Given the end effector target pose T of the six-axis manipulator target , solve the joint angle so that the forward kinematics function f(q) satisfies: f(q)=T target , the specific steps are as follows:

[0104] S241. Initialize the parameters and set the initial joint angle q of the robot arm. init and target end pose T target , calculate the initial residual r(q) = T target -f(q), where r(q) is a 6-dimensional vector containing 3-dimensional position error and 3-dimensional attitude error.

[0105] S242. The iterative formula using the LM (Levenberg-Marquardt) method is:

[0106] (J(q) T J(q)+λI)δ=-J(q) T r(q)

[0107] in, is the Jacobian matrix of the robot arm, λ is the damping factor, and δ is the joint angle update step size.

[0108] S243. Obtain the current robot arm joint angle Jacobian matrix J(q) through S23, and perform SVD decomposition on J(q):

[0109] J(q)=UΣ -1 V T

[0110] S244, update step size through SVD decomposition

[0111] δ=VΣ -1 U T r(q)

[0112] S245, get the update step size δ, and then update the joint angle

[0113] q k =q k-1 +δ

[0114] S246, damping factor λ is adjusted based on the change of residual, and the residual change is calculated

[0115]

[0116] S247, if ρ>0, it means the step size is valid, Otherwise, λ = 2λ. The loop is repeated until the maximum number of iterations is 100. If convergence occurs, that is, r(q) < ε, the result is returned; otherwise, an error is returned.

[0117] S25, Cartesian-joint space velocity conversion module:

[0118] S251, according to S22, obtain the current robot arm end position fMe,

[0119] R: 3×3 rotation matrix, representing the orientation of the end effector;

[0120] p=(p x ,p x ,p y ) T : A 3×1 translation vector representing the position of the end effector relative to the base.

[0121] S252, combine the linear velocity and angular velocity into a 6×6 velocity transformation matrix

[0122] Where R: rotation matrix of the end effector coordinate system;

[0123] 0: 3×3 zero matrix;

[0124] p* : Antisymmetric matrix of the end effector position:

[0125]

[0126] S253, the velocity V of the end effector coordinate system cart-end Velocity V converted to the base coordinate system cart-base =fV e ·V cart-end .

[0127] S254. Convert the base coordinate system velocity to joint velocity:

[0128] S254, set the maximum speed, the maximum joint speed is 2.61rad / s.

[0129] S3, in the CoppeliaSim simulation environment, acquires force and visual information in real time to perform collaborative simulation modeling of the robot arm's hybrid vision and force control, and performs joint motion control based on the established multi-threaded synchronous PD position and speed control mechanism.

[0130] This embodiment implements a multi-source data synchronization control architecture based on the cross-platform CoppeliaSim-VISP of ROS. The specific steps are as follows:

[0131] S31, scene rendering, and physics processing are based on CoppeliaSim. The algorithm verification instance is based on ROS topic communication. CoppeliaSim implements data interaction with the algorithm verification instance (robotic arm control program) through the simROS interface and topic communication. Data interaction specifically includes the following four modules:

[0132] S311, Image acquisition module: Real-time acquisition of RGB image data through ROS topic / CoppeliaSim / ur5 / camera / image, with a resolution of ≥100×100 pixels; Figure 5 As shown, the RGB image data is u and v in the image plane. u and v are related to the control law error e and serve as the input of the control law.

[0133] S312, force data acquisition module: acquires six-dimensional force / torque sensor data in real time through the ROS topic / CoppeliaSim / ur5 / ft_sensor, with a range of ±80N / ±20N·m and an accuracy of 0.1N / 0.05N·m;

[0134] S313, multi-threaded synchronous PD position control module: adopts a main thread-sub thread dual-thread design. The main thread periodically publishes the robot control state as POSITION_CONTROL through the ROS topic / ucl / robot_state, with a frequency of ≥10Hz;

[0135] like Figure 6 As shown, the child thread independently runs the control loop at a frequency of 500Hz, which includes the following steps:

[0136] S3131, access shared data through std::mutex mutual exclusion lock to avoid resource competition (robot arm joint position q, speed dq);

[0137] S3132. Publish the robot status to the topic / ucl / robot_state to ensure that the CoppeliaSim simulation environment receives the command synchronously.

[0138] S3133, calculate joint position error q error =q des -q current , and determine whether it is greater than a preset threshold;

[0139] If the joint error is greater than the preset threshold, Calculate the PD control output and set the joint saturation velocity vel_max, where q des represents the joint expected value, q current Indicates the current value of the joint, PD controller parameter K p , K d is a constant. When the calculated joint speed exceeds the saturation speed value, the joint speed is set to be equal to the joint saturation speed vel_max, and the speed command is sent through the ROS topic;

[0140] If the joint position error is less than the preset threshold, the mutex is unlocked, a zero velocity command is sent, and the loop exits.

[0141] S3134. At the end of the cycle, unlock the mutex lock and force a zero speed command to achieve a safe shutdown.

[0142] S314 multi-threaded synchronous speed control module: adopts a main thread-sub thread dual-thread design. The main thread periodically publishes the robot control state as VELOCITY_CONTROL through the ROS topic / ucl / robot_state, with a frequency of ≥10Hz;

[0143] like Figure 6 As shown, the child thread independently runs the control loop at a frequency of 500Hz, which includes the following steps:

[0144] S3141, publish the robot status to the / ucl / robot_state topic to ensure that the CoppeliaSim simulation environment receives the command synchronously;

[0145] S3142, access shared data through std::mutex mutual exclusion lock to avoid resource competition (robot arm joint position q, speed dq);

[0146] S3143. Set the joint saturation speed vel_max to {2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100}. When the given speed exceeds this value, it is equal to the joint saturation speed vel_max. Send the speed command through the / ucl / joint_state topic.

[0147] S3144. At the end of the cycle, unlock the mutex lock and force a zero speed command to achieve a safe shutdown.

[0148] S32. Use the virtual robot simulation experiment platform CoppeliaSim to build an experimental scene including a UR5 robotic arm and vision and force sensors. The specific steps are as follows:

[0149] S321. Set up a simulation environment in CoppeliaSim. First, create a ground plane as a base plane. Then load the UR5 robot model, place it above the ground, and check the positions of its joints and end effector.

[0150] S322. Attach the FT300 force / torque sensor to the UR5's end effector, ensuring it is properly connected and capable of measuring the end effector's torque. Mount the RealSense D435i camera on the UR5's end effector in an "eye-on-hand" configuration, adjusting the camera's position and orientation to accurately capture the workspace. For visual positioning, add a 36h11 series AprilTag as a target marker, place it on the table, and adjust its position.

[0151] S323. Place a cylindrical socket on the table as the target nesting position for the end tool of the robot arm. Replace the end tool of the UR5 with a cylindrical shaft and adjust its size and shape so that it can fit accurately into the cylindrical socket on the table. Figure 7 shown.

[0152] S324. Create a LUA sub-script in the simulation scene and use the sim.getObjectHandle() function built into the CoppeliaSim system to obtain the handle of the sensor object, thereby realizing the collection and processing of the visual sensor and force sensor data. The sampling frequency of the visual sensor is 30Hz, and the sampling frequency of the force sensor is 125Hz.

[0153] S325 and / sim_ros_interface are the ROS nodes of the simulation environment. The simROS.advertise() function built into the CoppeliaSim system is used to create multiple ROS topics for publishing simulation status information.

[0154] S33. The robot control program is considered as a ROS node. The ROS node can be regarded as a main() function. The specific steps of synchronous control of the node and the multi-source data of the simulation scene are as follows:

[0155] S331, initialize the simulation to establish the connection between the robot and the visual and force sensors and initialize the position, and then enter an infinite loop;

[0156] S332. In each loop iteration, the ROS node calculates the Cartesian space tool coordinate system velocity of the UR5 robot at the next moment according to the visual servo control law algorithm through the current Cartesian space tool coordinate system pose information of the UR5 robot in the simulation, the Cartesian space tool coordinate system velocity of the UR5 robot, the Cartesian space tool coordinate system force of the UR5 robot, the current pose and expected pose information of the AprilTag identified in the Cartesian space camera coordinate system of the UR5 robot, and performs ROS topic communication according to the format and content of S11 and the publish-subscribe relationship of S12.

[0157] S333 and CoppeliaSim subscribe to topics through the simROS interface, and use the four types of data synchronization mechanism modules of S31 (image acquisition module, force data acquisition module, multi-threaded synchronous PD position control module, and multi-threaded synchronous speed control module) to update the robot control program and synchronize the simulation scene rendering.

[0158] Furthermore, in this embodiment, a visual servo control law for a robotic arm is constructed based on an asymmetric time-varying tan-type obstacle Lyapunov function, specifically including:

[0159] S41. Define the visual feature error as:

[0160] e s =s w -s

[0161]

[0162] in, is the expected value of the feature point on the u-axis, is the expected value of the feature point on the v-axis, and ι is the time.

[0163] S42. Determine the camera's field of view constraint based on the camera resolution:

[0164]

[0165] Among them, γ is the parameter for adjusting the error convergence rate, Δ ∞ is a parameter that affects the size of the convergence error. and Is a positive number. ∞ Can be set to meet For any small value of and

[0166] S43. In order to stabilize the visual servo system, the visual servo control law of the robotic arm is constructed:

[0167]

[0168] Among them, v c is the camera's movement speed, is the speed of the characteristic point error, β is the controller gain, is the pseudo-inverse of the image interaction matrix.

[0169] The tan-type BLF used in this embodiment dynamically constrains visual errors, improving the dynamic response and stability of the visual servo system. Furthermore, this method overcomes the key limitation of symmetric time-invariant BLFs, which prevents arbitrary error tracking, thereby enhancing the system's applicability and performance.

[0170] The above examples are used to illustrate the present invention, which are only used to help understand the present invention and are not intended to limit the present invention. Those skilled in the art can make several simple deductions, modifications or substitutions based on the concept of the present invention.

Claims

1. A visual-force collaborative simulation method based on ROS and CoppeliaSim, characterized in that: A simulation framework for hybrid vision and force control of a robotic arm is constructed using the virtual robot simulation experiment platform CoppeliaSim. The method includes: S1, build a cross-platform CoppeliaSim-VISP real-time data interaction system based on ROS, where CoppeliaSim is used for simulation modeling of robot arm motion, and the visual servo control platform ViSP is integrated for visual servo control; S2, establish an efficient kinematics solver for the robotic arm based on KDL; S3, in the CoppeliaSim simulation environment, acquires force and visual information in real time to perform collaborative simulation modeling of the robot arm's hybrid vision and force control, and performs joint motion control based on the established multi-threaded synchronous PD position and speed control mechanism.

2. A visual-force collaborative simulation method based on ROS and CoppeliaSim according to claim 1, characterized in that: Said S1 specifically includes: The ROS publish-subscribe topic method is used to implement a standardized communication interface for data exchange. The data exchange content includes: the joint position and velocity information of the robot arm; the total mass, center of mass position and inertia tensor of the tool; the image data collected by the vision sensor; the camera parameters; the transformation relationship between the flange and the end effector; the transformation relationship between the end effector and the flange; the force and torque measurements collected by the force sensor; the camera hand-eye calibration data; the target velocity and torque information of the robot joint; and the control status of the robot. A topic publish-subscribe relationship is established between the robot control program node and the ROS node of the CoppeliaSim simulation platform. / visp_ros serves as the robot control program node, and / sim_ros_interface serves as the ROS node of the CoppeliaSim simulation platform. Publish and subscribe communications are performed through multiple topics.

3. A visual-force collaborative simulation method based on ROS and CoppeliaSim according to claim 2, characterized in that: Said S1 specifically further includes: A posture information message conversion mechanism is established to realize parameter transfer in posture conversion calculation.

4. The visual-force collaborative simulation method based on ROS and CoppeliaSim according to claim 1, characterized in that: Said S2 specifically includes: Kinematic model construction: Use KDL::Chain to build the kinematic model of the robot arm's links and joints; Establish a forward kinematics solver: Use the KDL chain solver to achieve real-time mapping from Cartesian space to joint space; Establish Jacobian matrix solver: Get the current joint angle Jacobian matrix by calling KDL::ChainJntToJacSolver(); Establish an inverse kinematics solver: implement position-level inverse kinematics solution through KDL::ChainIkSolverPos_NR_JL; Cartesian-joint space velocity conversion and limitation: Create a KDL::ChainIkSolverVel_pinv object and call the CartToJnt() interface to convert the end velocity to the joint velocity. Based on the set maximum joint velocity, use the vpRobot::saturateVelocities() function to saturate the velocity command.

5. The visual-force collaborative simulation method based on ROS and CoppeliaSim according to claim 1, characterized in that: Said S3 specifically includes: Multi-threaded synchronous PD position control: adopts a main thread-sub-thread dual-thread design. The main thread periodically publishes the robot control state as position control POSITION_CONTROL through the ROS topic, and the sub-thread independently runs the position control loop at a preset frequency.

6. The visual-force collaborative simulation method based on ROS and CoppeliaSim according to claim 1, characterized in that: Said S3 specifically includes: Multi-threaded synchronous speed control: A main-thread-sub-thread dual-thread design is adopted. The main thread periodically publishes the robot control state as speed control VELOCITY_CONTROL through the ROS topic, and the sub-thread independently runs the speed control loop at a preset frequency.

7. The visual-force collaborative simulation method based on ROS and CoppeliaSim according to claim 5, characterized in that: The child thread independently runs the position control loop at a preset frequency, including: a. Access shared data through mutex locks to avoid resource competition; b. Publish the robot status to the ROS topic to ensure that the CoppeliaSim simulation environment receives the command synchronously; c. Calculate the joint position error q error =q des -q current , and determine whether it is greater than a preset threshold; d. If the joint error is greater than the preset threshold, then Calculate the PD control output and set the joint saturation velocity vel_max, where q des represents the joint expected value, q current Indicates the current value of the joint, PD controller parameter K p , K d is a constant. When the calculated joint speed exceeds the saturation speed value, the joint speed is set to be equal to the joint saturation speed vel_max, and the speed command is sent through the ROS topic; e. If the joint position error is less than the preset threshold, unlock the mutex, send a zero velocity command and exit the loop.

8. The visual-force collaborative simulation method based on ROS and CoppeliaSim according to claim 6, characterized in that: The child thread independently runs a speed control loop at a preset frequency, specifically including: a. Access shared data through mutex locks to avoid resource competition; b. Publish the robot status to the ROS topic to ensure that the CoppeliaSim simulation environment receives the command synchronously; c. Set the joint saturation speed vel_max. When the joint speed exceeds the saturation speed value, the joint speed is set to be equal to the joint saturation speed vel_max, and the speed command is sent through the ROS topic; d. At the end of the cycle, unlock the mutex lock and force a zero speed command to achieve a safe shutdown.

9. The visual-force collaborative simulation method based on ROS and CoppeliaSim according to claim 1, characterized in that: Said S3 specifically further includes: The virtual robot simulation experiment platform CoppeliaSim was used to build a simulation experiment scene containing a robotic arm and vision and force sensors, including: Build a simulation environment in CoppeliaSim. First, create a ground as a base plane. Then load the robotic arm model and place it above the ground to check the positions of the joints and end effector. Add a force / torque sensor to the end effector, ensuring it is properly connected and capable of measuring the end effector's torque. Mount the camera on the end effector in an "eye-in-hand" manner, adjusting the camera's position and orientation so that it can correctly capture images of the workspace. For visual positioning, add an AprilTag as a target marker, place it on the table, and adjust its position. Place a cylindrical socket on the table as the target nesting location for the end-of-arm tooling; replace the end-of-arm tooling with a cylindrical shaft, adjusting the size and shape so that it can precisely fit into the cylindrical socket on the table.

10. The visual-force collaborative simulation method based on ROS and CoppeliaSim according to claim 1, characterized in that: Said S3 specifically further includes: After the simulation is initialized, it is iterated in a loop. In each loop iteration, the robot control program node calculates the Cartesian space tool coordinate system velocity of the robot arm at the next moment according to the constructed visual servo control law based on the current Cartesian space tool coordinate system pose information of the robot arm in the simulation, the speed of the Cartesian space tool coordinate system of the robot arm, the force of the Cartesian space tool coordinate system of the robot arm, the current pose and expected pose information of the AprilTag identified in the Cartesian space camera coordinate system of the robot arm, and publishes it through the ROS topic.

Images