Three-dimensional calibration automatic system suitable for six-axis physiotherapy robot

Abstract

The invention discloses a three-dimensional calibration automatic system suitable for a six-axis physiotherapy robot, and relates to the technical field of robot vision calibration and automatic control, and the system comprises an equipment communication management module which automatically identifies and distributes connection equipment through an intelligent detection technology, and outputs an equipment control instruction; the data acquisition and processing module is used for identifying 13-point feature pixels through RGB information, performing deep matching, calculating 13-point theoretical feature three-dimensional information and outputting an optimized calibration matrix MB2R in combination with a transformation relation between the camera and the tail end of the mechanical arm and a base coordinate transformation relation; and the result verification and backup uploading module is used for converting the relative position of the three-dimensional feature platform and the robot, acquiring three-dimensional information of an evaluation point, controlling the mechanical arm to move to an optimization point position, and calculating a pixel difference value and a height difference value. The method has the advantages that full-automatic millimeter-level calibration precision is achieved through a collaborative structure of multi-protocol heterogeneous equipment intelligent integration, iterative optimization algorithm fitting and closed-loop double-index verification.

Description

Technical Field

[0001] The present invention relates to the technical field of robot vision calibration and automatic control, and in particular to an automatic three-dimensional calibration system suitable for a six-axis physiotherapy robot. Background Art

[0002] Calibration of a robot's 3D system primarily involves determining the spatial relationship between the robotic arm and the 3D camera. Six-axis robots offer two main 3D system layouts: hand-on-eye and hand-off-eye. These two layouts offer distinct 3D calibration schemes, both employing fixed and mobile 3D cameras or calibration plates to capture images at different spatial locations. Calibration accuracy is generally around 15mm. However, the 3D vision system calibration of six-axis therapy robots requires high accuracy and is complex. Automating high-precision 3D vision calibration is extremely difficult, and therefore, mass production of therapy robots requires significant time-consuming 3D vision system calibration, significantly impacting production efficiency. Summary of the Invention

[0003] In order to solve the above technical problems, an automatic three-dimensional calibration system for a six-axis physiotherapy robot is provided. This technical solution solves the problem raised in the above background technology that the calibration of the three-dimensional vision system takes up a lot of time and seriously affects production efficiency.

[0004] In order to achieve the above purpose, the technical solution adopted by the present invention is:

[0005] An automatic three-dimensional calibration system for a six-axis physiotherapy robot, comprising:

[0006] The device communication management module uses the MATLAB environment on the Windows platform and automatically identifies and assigns connected devices through intelligent detection technology based on a three-dimensional feature platform, outputting device control instructions. The connected devices include 3D cameras, laser rangefinders, infrared cameras, six-axis robotic arms, and physical therapy robot control terminals.

[0007] The data acquisition and processing module receives device control instructions from the device communication management module, drives the connected device to collect response information, identifies 13 feature pixels through RGB information and performs depth matching. Combining the transformation relationship between the camera and the end of the robotic arm and the base coordinate transformation relationship, it calculates the 13-point theoretical feature three-dimensional information, and uses the iterative linear least squares algorithm to fit and output the optimized calibration matrix M_B2R;

[0008] The result verification and backup upload module, after receiving the optimized calibration matrix M_B2R, transforms the relative position of the 3D feature platform and the robot, obtains the 3D information of the evaluation point, controls the robot arm to move to the optimized point, and calculates the pixel difference and height difference to evaluate the accuracy.

[0009] Preferably, the three-dimensional feature platform automatically identifies and assigns connected devices through intelligent detection technology, and outputs device control instructions specifically including:

[0010] The MATLAB environment is used to integrate multiple device communication protocols on the Windows platform. These include: 485 serial port protocol to control the laser rangefinder and infrared indicator logic module, USB driver-free protocol to connect to the infrared camera, dedicated SDK to drive the 3D camera, Ethernet TCP / IP protocol based on the default IP address of the same network segment to connect to the six-axis robotic arm, and ADB wired protocol to connect to the physical therapy robot control terminal.

[0011] The three-dimensional feature platform identifies the type of connected device and dynamically allocates control resources, enabling the laser rangefinder to return distance measurement data in real time. The infrared indicator logic module generates indicator light on and off combination instructions on demand. The infrared camera automatically adapts to collect infrared feature images. The 3D camera driver obtains RGB-D data from the three-dimensional feature platform. The six-axis robotic arm outputs motion control instructions based on the base coordinate system.

[0012] Preferably, receiving the device control instruction of the device communication management module and driving the connected device to collect response information specifically includes:

[0013] Control the laser rangefinder to measure the current distance, calculate and adjust the target height position of the robotic arm;

[0014] Control the infrared camera to collect real-time infrared images of the laser ranging spot and the indicator light, identify the difference between the pixel centers of the two, and iteratively calculate the target point of the robot arm until the pixel difference meets the preset threshold;

[0015] Control the 3D camera to obtain the RGB_D data of the 3D feature platform, identify the 13 feature point pixels through the RGB information and perform depth matching, and calculate the 13-point theoretical feature 3D information by combining the transformation relationship M_C2T between the camera and the end of the robotic arm and the transformation relationship M_T2B between the base coordinates of the robotic arm and the end;

[0016] Control the six-axis robotic arm to move to the preset point and collect actual three-dimensional feature information;

[0017] Based on 13 sets of theoretical three-dimensional feature information and actual three-dimensional feature information, the iterative linear least squares algorithm is used to fit and optimize the calibration matrix M_B2R.

[0018] Preferably, identifying 13 feature pixels through RGB information and performing depth matching, combining the transformation relationship between the camera and the end of the robotic arm and the base coordinate transformation relationship, and calculating the 13-point theoretical feature three-dimensional information specifically includes:

[0019] The pixel coordinates of 13 feature points in the image plane are located using the color and texture information of the RGB image;

[0020] Combined with the depth data, the pixel coordinates are converted into three-dimensional space coordinates to obtain the actual three-dimensional feature information of the feature points in the actual scene;

[0021] According to the camera installation structure, calculate the transformation matrix M_C2T from the camera coordinate system to the robot arm tool end coordinate system;

[0022] Using the robot arm kinematic model, calculate the transformation matrix M_T2B from the tool end coordinate system to the robot arm base coordinate system;

[0023] The actual three-dimensional feature information is transformed through the transformation matrices M_C2T and M_T2B to obtain 13 theoretical features.

[0024] Preferably, outputting the optimized calibration matrix by iterative linear least squares algorithm fitting specifically includes:

[0025] Three-dimensional information P based on 13 groups of theoretical characteristics 理论 The three-dimensional information P of the actual features 实际 , build a data pair set

[0026] Define the homogeneous transformation equation P from the base coordinate system to the robot coordinate system 实际 =M B2R ·P 理论 , where M B2R is the 4×4 optimization calibration matrix to be solved;

[0027] Establishing the objective function The Levenberg-Marquardt iterative algorithm is used to linearize the nonlinear transformation and update the parameters through the Jacobian matrix until the objective function converges to the preset threshold;

[0028] Reconstruct the converged transformation parameters into a homogeneous transformation matrix M B2R , which is output to the result verification module as the optimized calibration matrix.

[0029] Preferably, after receiving the optimized calibration matrix M_B2R, transforming the relative position of the three-dimensional feature platform and the robot to obtain the three-dimensional information of the evaluation point specifically includes:

[0030] Control the six-axis robotic arm to move to the preset photo-taking point to change the relative position of the 3D feature platform and the robot;

[0031] Control the 3D camera to obtain the RGB_D data of the 3D feature platform at the current relative position;

[0032] The pixel coordinates of multiple preset evaluation points are identified through RGB information, and the actual three-dimensional information of the evaluation points is calculated in combination with the depth data.

[0033] Preferably, controlling the robot arm to move to the optimized point and calculating the pixel difference and height difference to evaluate the accuracy specifically includes:

[0034] The optimized calibration matrix M B2R The theoretical three-dimensional coordinates P applied to the current evaluation point 理论 , calculate and obtain the optimized coordinates in the robot coordinate system;

[0035] Control the six-axis robot arm to move to P 优化 Corresponding spatial position;

[0036] The actual three-dimensional coordinates P of the end of the robot arm at the evaluation point are collected in real time by the 3D camera 实际 ;

[0037] Calculate pixel difference and height difference for calibration.

[0038] Preferably, calculating pixel differences and height differences for calibration specifically includes:

[0039] Based on the RGB image, the theoretical projection position of the evaluation point on the image plane is compared with the actual projection position to generate the pixel offset where u 理论 and v 理论 is the theoretical projection position of the image plane, u 实际 and v 实际 is the actual projection position, △d is the pixel offset;

[0040] Extract the actual 3D coordinates P 实际 With the optimized coordinate P 优化 Height deviation in the Z-axis direction Δh=|Z 实际 -Z 优化 |, where △h is the height deviation;

[0041] If the pixel offset is ≤ the accuracy threshold and the height deviation is ≤ the accuracy threshold, the calibration accuracy is determined to be qualified; otherwise, the recalibration process is triggered, where the accuracy threshold is set by the target of the high-precision 3D calibration algorithm architecture.

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

[0043] This paper proposes an automatic three-dimensional calibration system for a six-axis physiotherapy robot. Through iterative optimization of 13 data pairs, the calibration error is reduced. At the same time, the Levenberg-Marquardt algorithm processes noise and outliers, ensuring stable convergence under sensor data fluctuations (such as depth data errors), avoiding overfitting, and effectively eliminating manual parameter adjustments. This shortens the single calibration time and directly improves mass production efficiency.

[0044] The present invention proposes an automatic three-dimensional calibration system for a six-axis physiotherapy robot. Through closed-loop evaluation using dual indicators (pixel offset Δd and height deviation Δh), calibration deviations are detected and corrected, reducing reliance on manual spot checks and improving system reliability. At the same time, when equipment fluctuates (such as robotic arm positioning errors), recalibration is automatically triggered to avoid production interruptions and ensure mass production consistency. In addition, data is backed up and uploaded to the physiotherapy robot control terminal (via the ADB protocol) only after verification, reducing invalid data storage and improving system efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 It is a schematic diagram of the process of the present invention;

[0046] Figure 2 This is a flow chart of the present invention for automatically identifying and allocating connected devices through intelligent detection technology based on a three-dimensional feature platform and outputting device control instructions;

[0047] Figure 3 A schematic diagram of the process of receiving device control instructions from the device communication management module and driving the connected device to collect response information in the present invention;

[0048] Figure 4 This is a schematic diagram of the process of calculating the 13-point theoretical feature 3D information by identifying 13 feature pixels through RGB information and depth matching, combining the transformation relationship between the camera and the end of the robotic arm and the base coordinate transformation relationship;

[0049] Figure 5 Schematic diagram of the process of outputting an optimized calibration matrix by fitting through an iterative linear least squares algorithm in the present invention;

[0050] Figure 6 This is a schematic diagram of the process of transforming the relative position of the three-dimensional feature platform and the robot to obtain the three-dimensional information of the evaluation point after receiving the optimized calibration matrix M_B2R in the present invention;

[0051] Figure 7 Schematic diagram of the process of controlling the robot arm to move to the optimized point and calculating the pixel difference and height difference to evaluate the accuracy in the present invention;

[0052] Figure 8 Schematic diagram of the process of calculating pixel difference and height difference for calibration in the present invention. DETAILED DESCRIPTION

[0053] The following description is intended to disclose the present invention so that those skilled in the art can implement the present invention. The preferred embodiments described below are merely examples, and those skilled in the art may conceive of other obvious variations.

[0054] Reference Figure 1As shown, a three-dimensional automatic calibration system for a six-axis physiotherapy robot includes:

[0055] The device communication management module uses the MATLAB environment on the Windows platform and automatically identifies and assigns connected devices through intelligent detection technology based on a three-dimensional feature platform, outputting device control instructions. The connected devices include 3D cameras, laser rangefinders, infrared cameras, six-axis robotic arms, and physical therapy robot control terminals.

[0056] The data acquisition and processing module receives device control instructions from the device communication management module, drives the connected device to collect response information, identifies 13 feature pixels through RGB information and performs depth matching. Combining the transformation relationship between the camera and the end of the robotic arm and the base coordinate transformation relationship, it calculates the 13-point theoretical feature three-dimensional information, and uses the iterative linear least squares algorithm to fit and output the optimized calibration matrix M_B2R;

[0057] The result verification and backup upload module, after receiving the optimized calibration matrix M_B2R, transforms the relative position of the 3D feature platform and the robot, obtains the 3D information of the evaluation point, controls the robot arm to move to the optimized point, and calculates the pixel difference and height difference to evaluate the accuracy.

[0058] Among them, the three-dimensional feature platform is a calibration target with 13 built-in specific spatially distributed feature points, providing a spatial reference through RGB texture and depth information. The device communication management module is a multi-protocol integration center based on MATLAB, supporting dynamic identification and instruction allocation of five types of heterogeneous devices: 485 serial port (laser rangefinder / infrared indicator), USB driver-free (infrared camera), dedicated SDK (3D camera), TCP / IP (six-axis robotic arm), and ADB (robot control terminal). The 13-point theoretical feature three-dimensional information is the theoretical spatial coordinates of the feature points calculated through the coordinate transformation chain of the camera coordinate system, the robotic arm tool coordinate system (MC2T), and the robotic arm base coordinate system (MT2B). Pixel alignment is to use the infrared camera to identify the pixel center deviation of the laser spot and the indicator light, and iteratively control the movement of the robotic arm until they coincide.

[0059] The device communication management module builds a protocol conversion layer in the Windows / MATLAB environment: the laser rangefinder returns distance data in real time through the 485 serial port, and the infrared indicator module receives on-off combination commands; the 3D camera obtains RGB-D data streams through a dedicated SDK driver; the six-axis robotic arm receives base coordinate system motion commands through the TCP / IP protocol; after the system automatically identifies the device type, it dynamically allocates control threads (for example, driving laser ranging and robotic arm movement simultaneously).

[0060] The data acquisition and processing module performs hierarchical control: in height control, the Z-axis displacement of the robot arm is calculated through laser ranging data; in feature matching, the pixel coordinates (u, v) of 13 feature points are located through RGB images, and the depth data is converted into the three-dimensional coordinates P of the camera coordinate system. c and through P 理论 =MT2B*MC2T*P c Calculate the theoretical coordinates; finally, establish the objective function and iteratively update the Jacobian matrix until convergence.

[0061] The result verification module uses double evaluation to make the robot move to P 优化 =M B2R *P 理论 After that, the 3D camera measures the coordinates P 实际 , to evaluate the projection error and height tolerance.

[0062] Reference Figure 2 As shown, based on the 3D feature platform, intelligent detection technology is used to automatically identify and assign connected devices, and the output is device control instructions, including:

[0063] The MATLAB environment is used to integrate multiple device communication protocols on the Windows platform. These include: 485 serial port protocol to control the laser rangefinder and infrared indicator logic module, USB driver-free protocol to connect to the infrared camera, dedicated SDK to drive the 3D camera, Ethernet TCP / IP protocol based on the default IP address of the same network segment to connect to the six-axis robotic arm, and ADB wired protocol to connect to the physical therapy robot control terminal.

[0064] The three-dimensional feature platform identifies the type of connected device and dynamically allocates control resources, enabling the laser rangefinder to return distance measurement data in real time. The infrared indicator logic module generates indicator light on and off combination instructions on demand. The infrared camera automatically adapts to collect infrared feature images. The 3D camera driver obtains RGB-D data from the three-dimensional feature platform. The six-axis robotic arm outputs motion control instructions based on the base coordinate system.

[0065] The 85 serial port protocol is a common communication standard for industrial equipment, implementing half-duplex communication over a single twisted-pair cable. It is used here to control the laser rangefinder (which returns distance data) and the infrared indicator module (which receives on / off commands). The USB driver-free protocol is a plug-and-play communication specification that automatically loads a universal driver, enabling the infrared camera to transmit infrared signature images in real time. The dedicated SDK driver is a software development kit provided by 3D camera manufacturers that deeply integrates hardware functions and directly obtains RGB-D data streams.

[0066] Before use, the system scans port resources: detecting COM ports (485 devices), USB nodes (infrared cameras), SDK services (3D cameras), network devices (robotic arm IP), and ADB connections (Android terminals). Independent communication threads are created to allocate independent I / O channels to each device. This allows the laser rangefinder to send a 0x03 read command and parse the hexadecimal return value as a floating-point distance value. The infrared indicator module sends 8-bit binary commands (e.g., 10110011) to control the on / off combinations of eight indicator lights. The 3D camera calls the AcquireFrame() function to acquire RGB-D data packets and automatically decompresses them into an image and depth matrix. The six-axis robotic arm encapsulates the MoveJ(Base, Pose) function to convert pose data into TCP packets. The control terminal then uploads the calibration matrix via adbpushM_B2R.txt / sdcard / calib / . A MATLAB timer can then be used to monitor device status every 500ms.

[0067] Reference Figure 3 As shown, receiving the device control instruction of the device communication management module and driving the connected device to collect the response information specifically include:

[0068] Control the laser rangefinder to measure the current distance, calculate and adjust the target height position of the robotic arm;

[0069] Control the infrared camera to collect real-time infrared images of the laser ranging spot and the indicator light, identify the difference between the pixel centers of the two, and iteratively calculate the target point of the robot arm until the pixel difference meets the preset threshold;

[0070] Control the 3D camera to obtain the RGB_D data of the 3D feature platform, identify the 13 feature point pixels through the RGB information and perform depth matching, and calculate the 13-point theoretical feature 3D information by combining the transformation relationship M_C2T between the camera and the end of the robotic arm and the transformation relationship M_T2B between the base coordinates of the robotic arm and the end;

[0071] Control the six-axis robotic arm to move to the preset point and collect actual three-dimensional feature information;

[0072] Based on 13 sets of theoretical three-dimensional feature information and actual three-dimensional feature information, the iterative linear least squares algorithm is used to fit and optimize the calibration matrix M_B2R.

[0073] The pixel center difference is the coordinate offset between the laser spot and the indicator light center in the infrared image, which is used for closed-loop control of the robot arm posture. During its hierarchical control, the laser rangefinder first returns the distance d, and the height compensation Δz = k*(d 目标-d), and then send the Z-axis displacement command of the robot arm through the TCP / IP protocol. The infrared camera can capture a 1024×768 image, and Gaussian fitting locates the center of the light spot (x1, y1) and the center of the indicator light (x2, y2). When the difference between the pixel centers is greater than 3 pixel units, the displacement is calculated. The D camera obtains point cloud data, and the RGB channel is divided into 13 feature areas. The camera coordinates are calculated by depth matching, and the robot arm feedbacks the current M T2B Matrix calculation theory coordinate calculation, finally construct 13 groups (P 理论 , P 实际 ) data pairs and perform optimal fitting on the matrix.

[0074] Reference Figure 4 As shown in the figure, 13 feature pixels are identified through RGB information and depth matching is performed. Combined with the transformation relationship between the camera and the end of the robotic arm and the base coordinate transformation relationship, the 13 theoretical feature three-dimensional information is calculated, including:

[0075] The pixel coordinates of 13 feature points in the image plane are located using the color and texture information of the RGB image;

[0076] Combined with the depth data, the pixel coordinates are converted into three-dimensional space coordinates to obtain the actual three-dimensional feature information of the feature points in the actual scene;

[0077] According to the camera installation structure, calculate the transformation matrix M_C2T from the camera coordinate system to the robot arm tool end coordinate system;

[0078] Using the robot arm kinematic model, calculate the transformation matrix M_T2B from the tool end coordinate system to the robot arm base coordinate system;

[0079] The actual three-dimensional feature information is transformed through the transformation matrices M_C2T and M_T2B to obtain 13 theoretical features.

[0080] Among them, the transformation matrix M_C2T is a 4×4 homogeneous transformation matrix, which represents the transformation from the camera coordinate system to the tool end coordinate system, including rotation and translation parameters, which is calculated based on the physical installation structure of the camera (such as fixed to the end of the robotic arm); the transformation matrix M_T2B is a 4×4 homogeneous transformation matrix, which represents the transformation from the tool end coordinate system to the base coordinate system, which is calculated from the joint angle using the robotic arm kinematic model (such as DH parameters); the theoretical feature three-dimensional information is the predicted position of the feature point obtained through coordinate transformation, which is used for subsequent calibration optimization and comparison with the actual collected data.

[0081] First, use a 3D camera to collect an RGB image (e.g., 1024×768 resolution) of the three-dimensional feature platform. Through color segmentation (e.g., based on HSV threshold) and texture analysis (e.g., edge detection algorithm), the pixel coordinates (u, v) of 13 preset feature points are located. Each feature point has a unique color or texture (e.g., a specific distribution point built into the three-dimensional feature platform of the document) to ensure recognition accuracy. RGB image processing is performed in real time in the MATLAB environment, and a pixel coordinate data stream is output. Combined with the depth data (RGB-D data stream) synchronously collected by the 3D camera, the pixel coordinates are converted into three-dimensional coordinates (P 实际 ). Based on the camera installation structure (such as the camera is fixed to the end of the robot arm), the M_C2T matrix can be calculated. This involves measuring the offset (such as the translation vector T) and direction (such as the rotation matrix R) of the camera relative to the end of the tool. And using the robot arm kinematic model (such as the DH parameter model mentioned in the document), the M_T2B matrix is ​​calculated from the current joint angle. Finally, the actual three-dimensional feature information (P 实际 ) is transformed into the base coordinate system through M_C2T and M_T2B, and the 13-point theoretical feature three-dimensional information (P 理论 ).

[0082] Reference Figure 5 As shown, through the iterative linear least squares algorithm fitting, the output optimization calibration matrix specifically includes:

[0083] Three-dimensional information P based on 13 groups of theoretical characteristics 理论 The three-dimensional information P of the actual features 实际 , build a data pair set

[0084] Define the homogeneous transformation equation P from the base coordinate system to the robot coordinate system 实际 =M B2R ·P 理论 , where M B2R is the 4×4 optimization calibration matrix to be solved;

[0085] Establishing the objective function The Levenberg-Marquardt iterative algorithm is used to linearize the nonlinear transformation and the parameters are updated through the Jacobian matrix until the objective function converges to the preset threshold;

[0086] Reconstruct the converged transformation parameters into a homogeneous transformation matrix M B2R , which is output to the result verification module as the optimized calibration matrix.

[0087] The Jacobian matrix is ​​the partial derivative matrix of the objective function with respect to the transformation parameters, which is used to iteratively update the parameter step size; the convergence threshold is the preset error limit of the objective function (such as 10-6mm 2), and the iteration stops when the error is lower than this value.

[0088] Get the theoretical coordinates P from 13 feature points 理论 (M T2B* M C2T conversion) and the actual coordinate P 实际 (3D camera measurement), forming 13 sets of data pairs. The least squares objective function can be established, and the nonlinear transformation is approximated as a linear problem. Each iteration solves the incremental equation (J T J+λI)δ=J T ε, where J is the Jacobian matrix (function of M B2R 6 degrees of freedom parameter partial derivative), ε is the residual vector, λ is the damping factor. Thus, the increment δ is calculated to update M B2R The algorithm is terminated when the rate of change of the objective function value is less than 0.001 or the residual norm is less than 0.1 mm. The converged rotation matrix R and translation vector T are reconstructed into a homogeneous transformation matrix and output to the result verification module for accuracy evaluation.

[0089] Reference Figure 6 As shown in the figure, after receiving the optimized calibration matrix M_B2R, the relative position of the 3D feature platform and the robot is transformed to obtain the 3D information of the evaluation point, specifically including:

[0090] Control the six-axis robotic arm to move to the preset photo-taking point to change the relative position of the 3D feature platform and the robot;

[0091] Control the 3D camera to obtain the RGB_D data of the 3D feature platform at the current relative position;

[0092] The pixel coordinates of multiple preset evaluation points are identified through RGB information, and the actual three-dimensional information of the evaluation points is calculated in combination with the depth data.

[0093] Among them, the photo point refers to the preset spatial position of the robotic arm (XYZ coordinates of the base coordinate system), which is used to change the relative viewing angle of the three-dimensional feature platform and the camera; the evaluation point refers to the preset verification point on the three-dimensional feature platform (different from the 13 feature points used for calibration).

[0094] Motion commands (TCP / IP protocol) are sent based on the base coordinate system to control the six-axis robotic arm to move to a preset position. The arm's Z-axis height is first adjusted to avoid collisions, followed by translation to the target XY coordinates (accuracy required: ±0.1mm). Once in position, the joints are locked to prevent vibration from affecting acquisition. This triggers the AcquireFrame() function in the 3D camera SDK to simultaneously acquire: a 1920×1080 resolution RGB image for color recognition evaluation points, and 16-bit grayscale data. This data is then cached in a MATLAB memory queue to ensure timestamp alignment. Finally, HSV threshold segmentation is performed on the color image (for example, for red evaluation points: H∈[0,10], S>0.8), locating the pixel coordinates (u, v). The Z value of the corresponding (u, v) position in the depth map is then taken (median filtering is used to eliminate outliers). The physical coordinates are then calculated using the camera's intrinsic parameters (factory calibration values), outputting the actual 3D coordinates Pactual for the four evaluation points.

[0095] Reference Figure 7 As shown, controlling the robot arm to move to the optimized point and calculating the pixel difference and height difference to evaluate the accuracy specifically include:

[0096] The optimized calibration matrix M B2R The theoretical three-dimensional coordinates P applied to the current evaluation point 理论 , calculate and obtain the optimized coordinates in the robot coordinate system;

[0097] Control the six-axis robot arm to move to P 优化 Corresponding spatial position;

[0098] The actual three-dimensional coordinates P of the end of the robot arm at the evaluation point are collected in real time by the 3D camera 实际 ;

[0099] Calculate pixel difference and height difference for calibration.

[0100] Among them, the optimized coordinates refer to the theoretical three-dimensional coordinates P of the evaluation point 理论 (Base coordinate system) by optimizing the calibration matrix M B2R The transformed coordinates, P 优化 =M B2R *P 理论 (robot coordinate system), represents the theoretical ideal position after calibration.

[0101] The theoretical coordinates P of each evaluation point 理论 (Base coordinate system) Apply the optimization matrix and extract only the first three-dimensional coordinates (X 优化 , Y 优化 , Z 优化 ) as the target of the robot arm. The motion command can be sent to the six-axis robot arm through the TCP / IP protocol, and the end moves to P with an accuracy of ±0.05mm. 优化The robot can detect the position of the robot arm and monitor the joint torque in real time during the movement. If the limit is exceeded, an emergency pause will be triggered. The 3D camera can be triggered to obtain RGB-D data. The acquisition is completed within 500ms after the robot arm is in place (to avoid vibration errors). When the pixel coordinates of the end marker of the robot arm (such as the red reflective ball) in the RGB image are identified, the depth map is combined to calculate the P 实际 (camera coordinate system), and then converted to the robot coordinate system. The camera model can be used to calculate the theoretical projection position (u 理论 , v 理论 ), and extract the actual projection position (u 实际 , v 实际 ), directly compare the Z coordinates, and pass the verification if the pixel difference is ≤ 2.0 pixels and the height difference is ≤ 0.5 mm.

[0102] Reference Figure 8 As shown, calculating pixel difference and height difference for calibration specifically includes:

[0103] Based on the RGB image, the theoretical projection position of the evaluation point on the image plane is compared with the actual projection position to generate the pixel offset where u 理论 and v 理论 is the theoretical projection position of the image plane, u 实际 and v 实际 is the actual projection position, △d is the pixel offset;

[0104] Extract the actual 3D coordinates P 实际 With the optimized coordinate P 优化 Height deviation in the Z-axis direction Δh=|Z 实际 -Z 优化 |, where △h is the height deviation;

[0105] If the pixel offset is ≤ the accuracy threshold and the height deviation is ≤ the accuracy threshold, the calibration accuracy is determined to be qualified; otherwise, the recalibration process is triggered, where the accuracy threshold is set by the target of the high-precision 3D calibration algorithm architecture.

[0106] Among them, the theoretical projection position is obtained by optimizing the coordinate P through the camera model. 优化 The two-dimensional coordinates projected onto the image plane; the actual projected position is the actual pixel coordinate of the end of the robotic arm in the RGB image, extracted through feature recognition (such as the red reflective ball); the accuracy threshold is pre-set by the high-precision three-dimensional calibration algorithm architecture.

[0107] During its use, it can be based on the optimized coordinates P 优化 and camera intrinsic parameters (f x ,f y ,c x ,c y ), real-time calculation of theoretical projection position (u理论 ,v 理论 ), then perform HSV segmentation (H∈[0,10] red interval) and morphological filtering on the RGB image to locate the center of the end marker of the robotic arm (u 实际 ,v 实际 ), with sub-pixel accuracy of 0.1 pixel. Thus, the MATLAB vector operation function is called to directly output the result, and the result is directly obtained from P 实际 and P 优化 The absolute value of the Z component is taken (ignoring the XY plane error), so as to perform dual condition judgment and display the judgment result in real time on the interactive interface.

[0108] In summary, the advantages of the present invention are: through the collaborative structure of intelligent integration of multi-protocol heterogeneous equipment, iterative optimization algorithm fitting and closed-loop dual-index verification, fully automated millimeter-level calibration accuracy is achieved.

[0109] The above shows and describes the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The above embodiments and descriptions merely illustrate the principles of the present invention. Various changes and modifications may be made to the present invention without departing from the spirit and scope of the present invention. Such changes and modifications are intended to fall within the scope of the present invention. The scope of protection claimed by the present invention is defined by the appended claims and their equivalents.

Claims

1. An automatic three-dimensional calibration system for a six-axis physiotherapy robot, characterized in that: include: The device communication management module uses the MATLAB environment on the Windows platform and automatically identifies and assigns connected devices through intelligent detection technology based on a three-dimensional feature platform, outputting device control instructions. The connected devices include 3D cameras, laser rangefinders, infrared cameras, six-axis robotic arms, and physical therapy robot control terminals. The data acquisition and processing module receives device control instructions from the device communication management module, drives the connected device to collect response information, identifies 13 feature pixels through RGB information and performs depth matching. Combining the transformation relationship between the camera and the end of the robotic arm and the base coordinate transformation relationship, it calculates the 13-point theoretical feature three-dimensional information, and uses the iterative linear least squares algorithm to fit and output the optimized calibration matrix M_B2R; The result verification and backup upload module, after receiving the optimized calibration matrix M_B2R, transforms the relative position of the 3D feature platform and the robot, obtains the 3D information of the evaluation point, controls the robot arm to move to the optimized point, and calculates the pixel difference and height difference to evaluate the accuracy.

2. The automatic three-dimensional calibration system for a six-axis physiotherapy robot according to claim 1, characterized in that: The automatic identification and allocation of connected devices by intelligent detection technology based on the three-dimensional feature platform and the output of device control instructions specifically include: The MATLAB environment was used to integrate multiple device communication protocols on the Windows platform. These protocols included: 485 serial port protocol to control the laser rangefinder and infrared indicator logic module, USB driver-free protocol to connect to the infrared camera, dedicated SDK to drive the 3D camera, Ethernet TCP / IP protocol based on the default IP address of the same network segment to connect to the six-axis robotic arm, and ADB wired protocol to connect to the physical therapy robot control terminal. The three-dimensional feature platform identifies the type of connected device and dynamically allocates control resources, enabling the laser rangefinder to return distance measurement data in real time. The infrared indicator logic module generates indicator light on and off combination instructions on demand. The infrared camera automatically adapts to collect infrared feature images. The 3D camera driver obtains RGB-D data from the three-dimensional feature platform. The six-axis robotic arm outputs motion control instructions based on the base coordinate system.

3. The automatic three-dimensional calibration system for a six-axis physiotherapy robot according to claim 2, characterized in that: The receiving device control instruction of the device communication management module and driving the connected device to collect response information specifically include: Control the laser rangefinder to measure the current distance, calculate and adjust the target height position of the robotic arm; Control the infrared camera to collect real-time infrared images of the laser ranging spot and the indicator light, identify the difference between the pixel centers of the two, and iteratively calculate the target point of the robot arm until the pixel difference meets the preset threshold; Control the 3D camera to obtain the RGB_D data of the 3D feature platform, identify the 13 feature point pixels through the RGB information and perform depth matching, and calculate the 13-point theoretical feature 3D information by combining the transformation relationship M_C2T between the camera and the end of the robotic arm and the transformation relationship M_T2B between the base coordinates of the robotic arm and the end; Control the six-axis robotic arm to move to the preset point and collect actual three-dimensional feature information; Based on 13 sets of theoretical three-dimensional feature information and actual three-dimensional feature information, the iterative linear least squares algorithm is used to fit and optimize the calibration matrix M_B2R.

4. The automatic three-dimensional calibration system for a six-axis physiotherapy robot according to claim 3, characterized in that: The identification of 13 feature pixels through RGB information and depth matching, combined with the transformation relationship between the camera and the end of the robotic arm and the base coordinate transformation relationship, and the calculation of the 13-point theoretical feature three-dimensional information specifically include: The pixel coordinates of 13 feature points in the image plane are located using the color and texture information of the RGB image; Combined with the depth data, the pixel coordinates are converted into three-dimensional space coordinates to obtain the actual three-dimensional feature information of the feature points in the actual scene; According to the camera installation structure, calculate the transformation matrix M_C2T from the camera coordinate system to the robot arm tool end coordinate system; Using the robot arm kinematic model, calculate the transformation matrix M_T2B from the tool end coordinate system to the robot arm base coordinate system; The actual three-dimensional feature information is transformed through the transformation matrices M_C2T and M_T2B to obtain 13 theoretical features.

5. The automatic three-dimensional calibration system for a six-axis physiotherapy robot according to claim 4, characterized in that: The method of fitting through iterative linear least squares algorithm and outputting the optimized calibration matrix specifically includes: Three-dimensional information P based on 13 groups of theoretical characteristics 理论 The three-dimensional information P of the actual features 实际 , build a data pair set Define the homogeneous transformation equation P from the base coordinate system to the robot coordinate system 实际 =M B2R ·P 理论 , where M B2R is the 4×4 optimization calibration matrix to be solved; Establishing the objective function The Levenberg-Marquardt iterative algorithm is used to linearize the nonlinear transformation and update the parameters through the Jacobian matrix until the objective function converges to the preset threshold; Reconstruct the converged transformation parameters into a homogeneous transformation matrix M B2R , which is output to the result verification module as the optimized calibration matrix.

6. The automatic three-dimensional calibration system for a six-axis physiotherapy robot according to claim 5, characterized in that: After receiving the optimized calibration matrix M_B2R, the relative positions of the three-dimensional feature platform and the robot are transformed to obtain the three-dimensional information of the evaluation point. Specifically, the following steps are performed: Control the six-axis robotic arm to move to the preset photo-taking point to change the relative position of the 3D feature platform and the robot; Control the 3D camera to obtain the RGB_D data of the 3D feature platform at the current relative position; The pixel coordinates of multiple preset evaluation points are identified through RGB information, and the actual three-dimensional information of the evaluation points is calculated in combination with the depth data.

7. The automatic three-dimensional calibration system for a six-axis physiotherapy robot according to claim 6, characterized in that: The controlling of the robot arm to move to the optimized point and calculating the pixel difference and height difference to evaluate the accuracy specifically include: The optimized calibration matrix M B2R The theoretical three-dimensional coordinates P applied to the current evaluation point 理论 , calculate and obtain the optimized coordinates in the robot coordinate system; Control the six-axis robot arm to move to P 优化 Corresponding spatial position; The actual three-dimensional coordinates P of the end of the robot arm at the evaluation point are collected in real time by the 3D camera 实际 ; Calculate pixel difference and height difference for calibration.

8. The automatic three-dimensional calibration system for a six-axis physiotherapy robot according to claim 7, characterized in that: The calculation of pixel difference and height difference for calibration specifically includes: Based on the RGB image, the theoretical projection position of the evaluation point on the image plane is compared with the actual projection position to generate the pixel offset where u 理论 and v 理论 is the theoretical projection position of the image plane, u 实际 and v 实际 is the actual projection position, △d is the pixel offset; Extract the actual 3D coordinates P 实际 With the optimized coordinate P 优化 Height deviation in the Z-axis direction Δh=|Z 实际 -Z 优化 |, where △h is the height deviation; If the pixel offset is ≤ the accuracy threshold and the height deviation is ≤ the accuracy threshold, the calibration accuracy is determined to be qualified; otherwise, the recalibration process is triggered, where the accuracy threshold is set by the target of the high-precision 3D calibration algorithm architecture.

Images