Sensor array-based autonomous control method for non-contact physiotherapy robot posture

Abstract

The present application relates to the field of physiotherapy robots, and discloses a non-contact physiotherapy robot posture autonomous control method based on a sensor array. The method comprises the following steps: controlling a physiotherapy component of a robot to move above a treatment site; collecting distance information in real time by using a distance sensor array; constructing a 3D feature of the treatment site by using the distance information; calculating an expected treatment posture of the 3D feature of the treatment site; generating a movement path of the physiotherapy component from a current posture to the expected treatment posture, and controlling the physiotherapy component to move according to the generated movement path; judging whether the treatment site moves up and down; if the treatment site moves up and down, the distance information is reacquired; otherwise, a dynamic stability is formed between the physiotherapy component and the treatment site. The method can effectively improve the physiotherapy effect and applicability of the non-contact physiotherapy robot.

Description

Technical Field

[0001] This invention relates to the field of physiotherapy robots. More specifically, this invention relates to a method for autonomous posture control of a non-contact physiotherapy robot based on a sensor array. Background Technology

[0002] Non-contact physiotherapy robots are robots that utilize advanced technology and equipment to provide rehabilitation treatment without physical contact with the human body. They transmit therapeutic effects through physical principles such as sensing, optics, sound waves, or electromagnetism, thereby promoting physical recovery. Current non-contact physiotherapy robots treat the human body using methods including infrared therapy, phototherapy, ultrasound therapy, magnetic therapy, and electromagnetic pulse therapy. Non-contact physiotherapy robots not only reduce direct contact between patients and equipment, avoiding cross-infection, but also provide precise treatment plans, avoiding errors caused by human operation. They are typically equipped with intelligent systems that automatically adjust parameters such as treatment intensity and frequency based on the patient's symptoms and physical condition, improving treatment effectiveness. Non-contact physiotherapy robots treat specific parts of the body through physiotherapy components mounted on their robotic arms. Before treatment, the robotic arm's movement must be controlled to move the physiotherapy components to a fixed position along a preset trajectory. However, because patients have different body shapes and treatment areas, controlling the physiotherapy components to remain in the same fixed position during treatment can lead to poor treatment results. In addition, the patient's body may move during the treatment, which may change the distance between the physiotherapy components and the area to be treated, resulting in poor treatment results.

[0003] In summary, existing non-contact physiotherapy robots suffer from poor treatment efficacy and limited applicability. Summary of the Invention

[0004] To address the technical problems of poor therapeutic effects and limited applicability of existing non-contact physiotherapy robots, the present invention provides solutions in the following aspects.

[0005] In a first aspect, the present invention provides a non-contact physiotherapy robot posture autonomous control method based on a sensor array, wherein the physiotherapy robot has a physiotherapy component and a distance sensor array installed at the end of its robotic arm, and the distance sensor in the distance sensor array is used to measure the distance from the distance sensor to the treatment site, the method comprising: The robot's physiotherapy components are moved to a position above the area to be treated; distance information is collected in real time using a distance sensor array; the distance information is used to characterize the distance between each distance sensor and the area to be treated; The distance information is used to construct 3D features of the area to be treated; Calculate the desired treatment pose based on the 3D features of the treatment area; the desired treatment pose is used to characterize the ideal pose of the physiotherapy components. Generate the motion path of the physiotherapy component from its current posture to the desired treatment posture, and control the physiotherapy component (replaced with a robotic arm) to move according to the generated motion path; In response to the physiotherapy components reaching the desired treatment posture, it is determined whether the area to be treated has moved up or down. If the area to be treated moves up or down, the distance information is reacquired, and a new motion path is generated based on the distance information. The physiotherapy component is then controlled to move along the generated motion path. Otherwise, the physiotherapy component and the area to be treated are controlled to form a dynamic stability. The dynamic stability means that when the area to be treated moves, the physiotherapy component moves along with it and the relative position between the two remains unchanged.

[0006] Its beneficial effects are as follows: The method of this invention acquires the 3D features of the treatment area not based on distance information collected by a single sensor, but on distance information collected by multiple sensors. This makes the constructed 3D features of the treatment area more accurate, and consequently, the generated desired treatment posture more accurate, helping to improve the therapeutic effect of non-contact physiotherapy robots. By controlling the movement of the physiotherapy components to the desired treatment posture instead of a fixed position, the applicability of non-contact physiotherapy robots is improved. Furthermore, after the physiotherapy components reach the desired treatment posture, if the treatment area moves, the desired treatment posture will also change. By controlling the movement of the physiotherapy components to the new desired treatment posture, the optimal posture of the physiotherapy components during treatment is ensured. During treatment, by establishing dynamic stability between the physiotherapy components and the treatment area, changes in the relative position between the physiotherapy components and the treatment area due to human movement can be avoided, thus ensuring the therapeutic effect.

[0007] Preferably, the distance sensor array consists of two layers of sensors, with sensors in the same layer at the same height and located on the circumference of the same circle. Each sensor is mounted on a sensor bracket, which is fixed to the end of the robotic arm. Constructing 3D features of the treatment area using the distance information includes: The point cloud data of each distance sensor in the distance sensor array are fused to obtain the fused point cloud data; The first local curvature of the treatment area is calculated based on the raw sensor data, and a dense voxel mesh is generated based on the fused point cloud data; the expression for calculating the first local curvature is: ; Indicates the first local curvature. and These represent the heights of the upper and lower sensors, respectively. This represents the average distance between the projection of each sensor onto the treatment site along the x-axis and the reference point. This represents the average distance between the projections of each sensor onto the treatment site along the y-axis and the reference point. A specific point on the treatment site can be selected as the reference point. Sparsity optimization is performed on the dense voxel mesh to obtain a sparse voxel mapping; The movement type of the treatment area is detected, including no movement, small-amplitude movement, and large-amplitude movement. In response to the motion type of the treatment site being no motion or small-amplitude motion, the curvature field is restored using sparse voxel mapping. Variational implicit surface optimization is performed based on the recovered curvature field to obtain the surface function; The second local curvature of the treatment site is calculated using surface functions; Check whether the deviation between the first local curvature and the second local curvature is less than the deviation threshold. If the deviation is less than the deviation threshold, output the surface function and use the surface function as the 3D feature of the treatment area.

[0008] The beneficial effects are as follows: When constructing the 3D features of the treatment area, the method of the present invention only constructs the 3D features when the treatment area has no movement or only slight movement, thereby ensuring that the constructed 3D features match the actual 3D features and improving the therapeutic effect of the physiotherapy components. If the treatment area moves significantly, the current value is discarded, and the first local curvature and sparse voxel mapping are reacquired. Furthermore, if the first local curvature and the second local curvature are inconsistent, it indicates that the constructed surface function is inaccurate. The method of the present invention only uses the surface function as the 3D feature of the treatment area when the deviation between the two is very small, thereby further ensuring the accuracy of the 3D features. Moreover, the sensor array deployment method of this embodiment can improve the depth measurement accuracy and can calculate and acquire curvature information accurately and efficiently in real time.

[0009] Preferably, sparsity optimization of the dense voxel grid includes: inputting the dense voxel grid into a sparsity optimization model to obtain a compressed sparse voxel map; the sparsity optimization model includes an encoder, the output of which is connected to the input of a bottleneck layer, and the output of which is connected to the input of a decoder; the encoder is used to compress the dense voxel grid data into a low-dimensional, high-information-density sparse representation; the bottleneck layer is used to further compress the sparse representation to discard redundant information and retain only the minimum sufficient statistics required for reconstruction, thereby generating a highly compressed sparse representation; the decoder is used to reconstruct the highly compressed compact representation into the original voxel structure, thereby obtaining the compressed sparse voxel map.

[0010] Preferably, the overall loss function expression used when training the sparse optimization model is: ; In the formula Represents the overall loss function. , and These represent the binary cross-entropy loss function, the 3D gradient difference loss, and the total variation regularization loss, respectively.

[0011] Preferably, the encoder includes an input layer, a first sparse 3D convolutional layer, a first sparse max pooling layer, and a second sparse max pooling layer, wherein the input layer is connected to the input of the first sparse 3D convolutional layer, the output of the first sparse 3D convolutional layer is connected to the input of the first sparse max pooling layer, and the output of the first sparse max pooling layer is connected to the input of the second sparse max pooling layer through a residual block.

[0012] Preferably, the kernel size of the first sparse 3D convolutional layer is 3. 3 3. The stride of the convolutional kernel is 1; the activation function of the first sparse 3D convolutional layer is the LeakyReLU function; the size of the pooling kernel of the first sparse max pooling layer is 2. 2 2. The sliding step size of the pooling core is 2.

[0013] Preferably, the convolution kernel of the first sparse 3D convolutional layer is used to learn local geometric patterns, including planar relationships, surface continuity, and occlusion structures.

[0014] Preferably, the decoder includes a first sparse 3D deconvolution layer, a second sparse 3D convolution layer, a second sparse 3D deconvolution layer, and an output layer. The input of the first sparse 3D deconvolution layer is connected to the output of the bottleneck layer, and its output is connected to the input of the second sparse 3D convolution layer. The input of the second sparse 3D convolution layer is also connected to the residual block of the encoder. The output of the second sparse 3D convolution layer is connected to the input of the second sparse 3D deconvolution layer. The input of the second sparse 3D deconvolution layer is also connected to the output of the second sparse max pooling layer. The output of the second sparse 3D deconvolution layer is connected to the output layer.

[0015] Preferably, the method for further compressing sparse voxel features in the bottleneck layer includes: inputting a compact representation of the sparse voxel features into a grid with a size of 1. 1 Convolution with 1 channel compresses the channel, thereby generating low-dimensional features; L1 regularization is applied to the generated low-dimensional features to obtain a compact representation of the sparse voxel features.

[0016] Preferably, the type of movement detected at the site of treatment includes: The instantaneous change rate of the treatment area is obtained by detecting the instantaneous changes of the treatment area based on the point cloud data collected by the distance sensor array. Spatial consistency analysis of the treatment area is performed, including: testing the rigid body motion hypothesis of the treatment area based on point cloud data from distance sensors to obtain the change in the position of the treatment area, and then calculating the residual energy. The residual energy is used to quantify the sum of squared errors between the change in the location of the treatment site and the predicted change. Perform characteristic displacement analysis on the treatment site, including: calculating the average change in the compression eigenvector of the treatment site. Spatial position offset under optimal matching ; The trajectory of the treatment area is fitted, and then the movement velocity vector of the treatment area is calculated. Calculate the reliability of the fitted trajectory. Movement speed vector Each element represents the speed of movement of the treatment site in each direction; The response is that the instantaneous rate of change at the treatment site is less than a preset rate of change threshold and the residual energy If the residual energy is less than the residual energy threshold, it is determined that no movement has occurred at the treatment site; this is in response to the average change in the compressed feature vector. Spatial position offset greater than the average change threshold of the compressed feature vector and the optimal matching If the spatial position offset is less than the threshold for optimal matching, it is determined that the treatment site has experienced a small-amplitude movement; this is in response to the reliability of the trajectory. The movement speed vector is greater than the confidence threshold. If the magnitude is greater than the velocity threshold, it is determined that a large-amplitude movement has occurred at the treatment site.

[0017] In summary, the beneficial effects of the present invention are that the method of the present invention can improve the therapeutic effect and applicability of non-contact physiotherapy robots. Attached Figure Description

[0018] The above and other objects, features, and advantages of exemplary embodiments of the present invention will become readily apparent upon reading the following detailed description with reference to the accompanying drawings. In the drawings, several embodiments of the invention are illustrated by way of example and not limitation, and like or corresponding reference numerals denote like or corresponding parts, wherein: Figure 1 This is a schematic flowchart illustrating an autonomous posture control method for a non-contact physiotherapy robot based on a sensor array according to an embodiment of the present invention; Figure 2 This is a schematic diagram illustrating a sensor bracket according to an embodiment of the present invention.

[0019] Figure 3 This is a schematic diagram illustrating the structure of a sparsity optimization model according to an embodiment of the present invention. Detailed Implementation

[0020] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0021] The specific embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0022] Example of an autonomous posture control method for a non-contact physiotherapy robot based on sensor array: like Figure 1 As shown, the non-contact physiotherapy robot posture autonomous control method based on sensor array of the present invention includes: A non-contact physiotherapy robot posture autonomous control method based on a sensor array, characterized in that the end effector of the physiotherapy robot's robotic arm is equipped with a physiotherapy component and a distance sensor array, wherein the distance sensor is used to measure the distance from the distance sensor to the treatment area, and the method includes: S101. Control the physiotherapy component to move to the initial position and collect distance information. Specifically, control the physiotherapy component of the robot to move above the area to be treated; use a distance sensor array to collect distance information in real time; the distance information is used to characterize the distance between each distance sensor and the area to be treated. S102. Construct 3D features of the treatment area using the distance information; S103. Calculate the desired treatment pose of the 3D features of the treatment area; the desired treatment pose is used to characterize the ideal pose of the physiotherapy components. S104. Generate the motion path of the physiotherapy component from its current posture to the desired treatment posture, and control the physiotherapy component to move according to the generated motion path; The motion path of the physiotherapy component from its current posture to the desired treatment posture can be generated using a rasterized scanning algorithm, a full-coverage path planning algorithm, or an adaptive path planning algorithm. Full-coverage path planning algorithms can employ the Boustrophedon path algorithm, the spiral scanning algorithm, or a wavefront-based heuristic algorithm. Adaptive path planning algorithms can use dynamic windowing or RRT (Real-Time Therapy). Algorithms or Model Predictive Control (MPC) algorithms. In one embodiment, the algorithm titled "Optimal Path Planning using RRT" by Iram Noreen, Amna Khan, and Zulfiqar Habib can also be used. The path planning algorithm in the paper "Based Approaches: A Survey and Future Directions".

[0023] S105. In response to the physiotherapy component reaching the desired treatment posture, determine whether the area to be treated has moved up or down. S106. Control the physiotherapy component according to the judgment result, specifically: if the treatment area moves up and down, reacquire the distance information and generate a new motion path based on the distance information, and control the physiotherapy component to move according to the generated motion path; otherwise, control the physiotherapy component to form a dynamic stability between the treatment area and the treatment area, wherein the dynamic stability means that when the treatment area moves, the physiotherapy component moves along with it and the relative position between the two remains unchanged.

[0024] There are several ways to control the dynamic stability between the physiotherapy component and the treatment area. For example, if the treatment area is located on the user's abdomen or chest, the physiotherapy component can be controlled to move up and down at a preset amplitude and frequency, with the frequency of the up-and-down movement synchronized with the user's breathing rate. That is, when the user inhales, the treatment area moves downward, and when the user exhales, the treatment area moves upward; the physiotherapy component moves up and down synchronously with the user's breathing rate.

[0025] The method of this invention acquires 3D features of the treatment area not based on distance information collected by a single sensor, but on distance information collected by multiple sensors. This makes the constructed 3D features of the treatment area more accurate, and consequently, the generated desired treatment posture more accurate. This helps improve the therapeutic effect of non-contact physiotherapy robots. By controlling the movement of the physiotherapy components to the desired treatment posture instead of a fixed position, the applicability of non-contact physiotherapy robots is improved. Furthermore, if the treatment area moves after the physiotherapy components reach the desired treatment posture, the desired treatment posture will also change. By controlling the movement of the physiotherapy components to the new desired treatment posture, the optimal posture of the physiotherapy components during treatment is ensured. During the treatment process, by establishing dynamic stability between the physiotherapy components and the treatment area, changes in the relative position between the physiotherapy components and the treatment area due to human movement can be avoided, thus ensuring the therapeutic effect.

[0026] In one embodiment, the distance sensor array consists of two layers of sensors, with sensors in the same layer at the same height and located on the same circumference. Each sensor is mounted on a sensor bracket, which is fixed to the end effector of the robotic arm. The sensor bracket is as follows: Figure 2 As shown, the upper layer can accommodate 3 sensors, and the lower layer can accommodate 6 sensors; the sensor support includes a lower ring 1 and an upper ring 2, which are fixedly connected by a support 3. The upper ring 2 has three sensor mounting positions, and the lower ring 1 has 6 sensor mounting positions; constructing 3D features of the treatment area using the distance information includes: S201. The point cloud data of each distance sensor in the distance sensor array are fused to obtain the fused point cloud data. S202. Calculate the first local curvature of the treatment area based on the raw data from the sensors, and generate a dense voxel mesh based on the fused point cloud data; calculating the first local curvature of the treatment area based on the fused point cloud data includes: ; Indicates the first local curvature. and These represent the heights of the upper and lower sensors, respectively. This represents the average distance between the projection of each sensor onto the treatment site along the x-axis and the reference point. This represents the average distance between the projections of each sensor onto the treatment site along the y-axis and the reference point. A specific point on the treatment site can be selected as the reference point.

[0027] When sensors at different heights observe the same point, they will create a difference in viewing angle (parallax angle θ).

[0028] ; d represents parallax, h represents the height difference between sensor layers; α and β represent the incident angles of the upper and lower sensors, respectively.

[0029] S203. Perform sparsity optimization on the dense voxel mesh to obtain a sparse voxel mapping; S204. Detect the movement type of the treatment area, wherein the movement type includes no movement, small-amplitude movement, and large-amplitude movement; S205. In response to the motion type of the treatment site being no motion or small-amplitude motion, the curvature field is restored using sparse voxel mapping. S206. Variational implicit surface optimization is performed based on the recovered curvature field to obtain the surface function; S207. Calculate the second local curvature of the treatment site using surface functions; S208. Check whether the deviation between the first local curvature and the second local curvature is less than the deviation threshold. If the deviation is less than the deviation threshold, output the surface function and use the surface function as the 3D feature of the treatment area.

[0030] The method of this invention constructs 3D features of the treatment area only when the area is stationary or experiences minimal movement. This ensures that the constructed 3D features match the actual 3D features, improving the therapeutic effect of the physiotherapy components. If the treatment area moves significantly, the current value is discarded, and the first local curvature and sparse voxel mapping are re-acquired. Furthermore, if the first local curvature and the second local curvature are inconsistent, it indicates that the constructed surface function is inaccurate. The method of this invention uses the surface function as the 3D feature of the treatment area only when the deviation between the two is very small, further ensuring the accuracy of the 3D features.

[0031] Furthermore, by setting the sensor array in two layers, stereoscopic vision is achieved through the height difference, thereby improving the measurement depth. The sensor array of this invention can directly acquire curvature information, enhancing surface constraints. This curvature information can be applied to the detection of motion types in the treatment area and the curvature consistency analysis in this embodiment.

[0032] The sensor array deployment method adopted in this embodiment can improve the depth measurement accuracy and facilitate the real-time, accurate and efficient calculation and acquisition of curvature information.

[0033] In one embodiment, the point cloud data from various distance sensors are fused, including: S301, External parameter calibration: Determine the static pose relationship between each distance sensor; S302, Time Synchronization: Aligning the timestamps of sensor data; S303. Preprocess the point cloud data acquired by the sensor, including filtering, noise reduction and feature extraction. S304, Point Cloud Coordinate Transformation: Batch transformation of point cloud data from various sensors to the global coordinate system; S305, Point Cloud Fusion and Registration: Align the point clouds of each sensor and perform dynamic target tracking.

[0034] In this embodiment, the point cloud registration method can be adopted from the paper titled "Automatic Point Cloud Registration Based on Voxel Downsampling and Key Point Extraction" by Zhang Bin and Xiong Chuanbing.

[0035] S306. Output unified point cloud: Generate fusion result.

[0036] In one embodiment, sparsity optimization of a dense voxel grid includes: inputting the dense voxel grid into a sparsity optimization model to obtain a compressed sparse voxel map; the sparsity optimization model includes an encoder, the output of which is connected to the input of a bottleneck layer, and the output of which is connected to the input of a decoder; the encoder is used to compress the dense voxel grid data into a low-dimensional, high-information-density sparse representation; the bottleneck layer is used to further compress the sparse representation to discard redundant information and retain only the minimum sufficient statistics required for reconstruction, thereby generating a highly compressed sparse representation; the decoder is used to reconstruct the highly compressed compact representation back into the original voxel structure, thereby obtaining the compressed sparse voxel map.

[0037] The bottleneck layer is the hub of encoding / decoding. In the application scenario of this invention, it can achieve the function of filtering noise and preserving the structural skeleton.

[0038] The sparse optimization model in this embodiment belongs to unsupervised learning based on compressed voxel mapping, which can provide adaptive and nonlinear data compression, better capture complex structures and redundancy, and usually achieve higher compression ratios and better reconstruction quality.

[0039] The overall loss function used when training the sparse optimization model is expressed as follows: ; In the formula Represents the overall loss function. , and These represent the binary cross-entropy loss function, the 3D gradient difference loss, and the total variation regularization loss, respectively.

[0040] The binary cross-entropy loss function measures the difference in probability distribution between the reconstructed voxel mesh and the original voxel mesh at the voxel level. It forces the decoder output to be as close as possible to the original binary or probabilistic voxel values. The 3D gradient difference loss forces the reconstruction result to maintain consistency with the original data in terms of spatial structure (especially edges and textures). It focuses on the similarity of local spatial variations. The total variation regularization loss imposes spatial smoothness constraints on the reconstructed voxel mesh itself, suppressing unwanted noise or small oscillations. It encourages the reconstruction result to be spatially "piecewise constant". Therefore, using the loss functions in this embodiment can significantly improve the training effect of the sparsity optimization model.

[0041] like Figure 3As shown, in this embodiment, the encoder includes an input layer, a first sparse 3D convolutional layer, a first sparse max pooling layer, and a second sparse max pooling layer. The input layer is connected to the input of the first sparse 3D convolutional layer, the output of the first sparse 3D convolutional layer is connected to the input of the first sparse max pooling layer, and the output of the first sparse max pooling layer is connected to the input of the second sparse max pooling layer through a residual block.

[0042] In this embodiment, to reduce computational load, the kernel size of the first sparse 3D convolutional layer is set to 3. 3 3. To maintain resolution, the stride of the convolutional kernel is set to 1. The convolutional kernel of the first sparse 3D convolutional layer is used to learn local geometric patterns, including planar relationships, surface continuity, and occlusion structures.

[0043] The output channels of the first sparse 3D convolutional layer are set to increase layer by layer, thereby gradually expanding the feature dimension.

[0044] To balance the gradient flow of sparse features, the activation function of the first sparse 3D convolutional layer is the LeakyReLU function.

[0045] The size of the pooling kernel in the first sparse max-pooling layer is 2. 2 2. The sliding step size of the pooling kernel is 2. The pooling layer only operates on non-empty voxel regions, thus discarding 75% of redundant voxels.

[0046] The decoder includes a first sparse 3D deconvolution layer, a second sparse 3D convolution layer, a second sparse 3D deconvolution layer, and an output layer. The input of the first sparse 3D deconvolution layer is connected to the output of the bottleneck layer, and its output is connected to the input of the second sparse 3D convolution layer. The input of the second sparse 3D convolution layer is also connected to the residual block of the encoder. The output of the second sparse 3D convolution layer is connected to the input of the second sparse 3D deconvolution layer. The input of the second sparse 3D deconvolution layer is also connected to the output of the second sparse max pooling layer. The output of the second sparse 3D deconvolution layer is connected to the output layer.

[0047] In one embodiment, the method for further compressing sparse voxel features in the bottleneck layer includes: S701. Input the compact representation of sparse voxel features into a size of 1. 1 Convolution with 1 channel compresses the channel, thereby generating low-dimensional features; S702. Apply L1 regularization to the generated low-dimensional features to obtain a compact representation of the sparse voxel features.

[0048] In one embodiment, detecting the type of movement at the treatment site includes: S801. Based on the point cloud data collected by the distance sensor array, instantaneous change detection is performed on the treatment area to obtain the instantaneous change rate of the treatment area. In this embodiment, the instantaneous rate of change measured by each sensor can be calculated first, and then the instantaneous rate of change of the treatment site can be obtained by summing them.

[0049] The expression for calculating the instantaneous rate of change measured by the i-th sensor is: ; In the formula, Derivatives of sensor-based point cloud data, This represents the point cloud data of the i-th sensor. Indicates the length of a unit of time.

[0050] S802. Perform spatial consistency analysis on the treatment area, including: testing the rigid body motion hypothesis of the treatment area based on the point cloud data from the distance sensor, thereby obtaining the change in the position of the treatment area, and then calculating the residual energy. The residual energy is used to quantify the sum of squared errors between the change in the location of the treatment site and the predicted change. In this embodiment, the distance sensor array has a total of 9 sensors, and the residual energy... The calculation expression is: ; ; In the above two equations, R and T represent the rotation matrix and translation vector (true value) of the rigid body motion, respectively. and Let these represent the estimated rotation matrix and estimated translation vector for the rigid body motion, respectively. This represents the displacement vector of the 3D point corresponding to the i-th sensor between the current frame and the previous frame. This represents the rigid body motion assumption. This represents the position vector of the 3D point corresponding to the i-th sensor in the previous frame.

[0051] S803. Perform characteristic displacement analysis on the treatment area, including: calculating the average change in the compression feature vector of the treatment area. Spatial position offset under optimal matching ; The spatial offset under optimal matching refers to finding a spatial offset that minimizes the matching error by comparing the similarity of two or more objects.

[0052] In this embodiment, the average change of the compressed feature vector The calculation expression is: ; Spatial position offset under optimal matching The calculation expression is: ; In the formula, This represents the feature encoding vector of the k-th compressed voxel at time t. Indicates the time of the k-th compressed voxel The feature encoding vector, where K represents the total number of compressed voxels. Describing the L1 norm, Describing the L2 norm, and These represent the time intervals of the k-th compressed voxel and time interval t, respectively. Position encoding vector, Indicates the length of a unit of time.

[0053] In this embodiment, the average change of the compressed feature vector The average change in the L1 norm of the compressed voxel features can be used to represent the motion intensity in the feature space.

[0054] S804. Fit the trajectory of the area to be treated, and then calculate the movement velocity vector of the area to be treated. Calculate the reliability of the fitted trajectory. Movement speed vector Each element represents the speed of movement of the treatment site in each direction; The velocity vector of the treatment site The calculation expression is: ; Reliability of the trajectory The calculation expression is: ; In the above two formulas, This represents the root mean square error of the trajectory fitting. Let represent the least squares estimate of the moving velocity vector, argmin represent the value of the variable that minimizes the function, and T represent the observation window length. This represents an intermediate quantity in the summation. express The position vector of the area to be treated at any given time. This represents the position vector of the treatment site at time 0. This indicates the spatial position offset under the optimal matching. It is a constant. Represents the norm, Indicates the length of a unit of time.

[0055] S805, responding to the instantaneous rate of change of the treatment site being less than a preset rate of change threshold and the residual energy If the residual energy is less than the residual energy threshold, it is determined that no movement has occurred at the treatment site; this is in response to the average change in the compressed feature vector. Spatial position offset greater than the average change threshold of the compressed feature vector and the optimal matching If the spatial position offset is less than the threshold for optimal matching, it is determined that the treatment site has experienced a small-amplitude movement; this is in response to the reliability of the trajectory. The movement speed vector is greater than the confidence threshold. If the magnitude is greater than the velocity threshold, it is determined that a large-amplitude movement has occurred at the treatment site.

[0056] In this embodiment, the rate of change threshold is 5, but other suitable values ​​may be used in other embodiments. The values ​​of the residual energy threshold, the compressed voxel characteristic displacement threshold, the position consistency metric threshold, the confidence threshold, and the velocity threshold can all be determined experimentally.

[0057] In one embodiment, it also includes: S401. Compare the distance information collected in the current session with the distance information collected in the previous session to determine whether the physiotherapy component is moving toward the area to be treated. S402, In response to the movement of the physiotherapy component toward the area to be treated, reduce the movement speed of the physiotherapy component; S403. In response to the distance between the physiotherapy component and the treatment area reaching a risk distance, the physiotherapy component is controlled to perform an avoidance action to a safe area.

[0058] The method described in this embodiment can avoid collisions between the physiotherapy components and the area to be treated, thereby improving the safety of the entire physiotherapy process.

[0059] In one embodiment, the method further includes: before acquiring the distance information measured by the distance sensor array, the intrinsic parameters of each sensor in the distance sensor array need to be individually calibrated, and the extrinsic parameters of each sensor need to be jointly calibrated.

[0060] Joint calibration includes static calibration and dynamic calibration. Static calibration includes: S501. Place the polyhedral calibration frame in the common field of view of each distance sensor; and use each distance sensor to scan the point cloud plane of the polyhedral calibration frame. S502. Extract feature points of the point cloud plane from the point cloud data acquired by each distance sensor, and establish a global 3D coordinate correspondence between the feature points detected by each distance sensor. S503. For each distance sensor, input the point cloud data it scans into Ceres Solver for optimization.

[0061] Dynamic calibration includes: S601. Control the polyhedral calibration frame to move within the field of view of each distance sensor, and simultaneously record the timing data corresponding to each distance sensor. S602. Align the observation trajectories of each distance sensor and solve for the extrinsic parameters and time offset; S603. Perform time synchronization calibration on each sensor.

Claims

1. A method for autonomous posture control of a non-contact physiotherapy robot based on a sensor array, characterized in that, The end effector of the physiotherapy robot's robotic arm is equipped with physiotherapy components and a distance sensor array. The distance sensors in the array are used to measure the distance from the distance sensor to the treatment area. The method includes: The robot's physiotherapy components are moved to a position above the area to be treated; distance information is collected in real time using a distance sensor array; the distance information is used to characterize the distance between each distance sensor and the area to be treated; The distance information is used to construct 3D features of the area to be treated; Calculate the desired treatment pose based on the 3D features of the treatment area; the desired treatment pose is used to characterize the ideal pose of the physiotherapy components. Generate the motion path of the physiotherapy component from its current posture to the desired treatment posture, and control the physiotherapy component to move according to the generated motion path; In response to the physiotherapy components reaching the desired treatment posture, it is determined whether the area to be treated has moved up or down. If the treatment area moves vertically, the distance information is reacquired, and a new motion path is generated based on the distance information. The physiotherapy component is then controlled to move along the generated motion path. Otherwise, dynamic stability is achieved between the physiotherapy component and the treatment area. Dynamic stability means that when the treatment area moves, the physiotherapy component moves along with it, and the relative position between the two remains unchanged. The distance sensor array consists of two layers of sensors, with sensors in the same layer at the same height and located on the same circumference. Each sensor is mounted on a sensor bracket, which is fixed to the end of the robotic arm. Constructing 3D features of the treatment area using the distance information includes: The point cloud data of each distance sensor in the distance sensor array are fused to obtain the fused point cloud data; The first local curvature of the treatment area is calculated based on the raw data from the sensors, and a dense voxel mesh is generated based on the fused point cloud data; the expression for calculating the first local curvature is: ; Indicates the first local curvature. and These represent the heights of the upper and lower sensors, respectively. This represents the average distance between the projection of each sensor onto the treatment site along the x-axis and the reference point. This represents the average distance between the projection of each sensor onto the treatment site along the y-axis and the reference point. A specific point on the treatment site can be selected as the reference point. Sparsity optimization is performed on the dense voxel mesh to obtain a sparse voxel mapping; The movement type of the treatment area is detected, including no movement, small-amplitude movement, and large-amplitude movement. In response to the motion type of the treatment site being no motion or small-amplitude motion, the curvature field is restored using sparse voxel mapping. Variational implicit surface optimization is performed based on the recovered curvature field to obtain the surface function; The second local curvature of the treatment site is calculated using surface functions; Check whether the deviation between the first local curvature and the second local curvature is less than the deviation threshold. If the deviation is less than the deviation threshold, output the surface function and use the surface function as the 3D feature of the treatment area.

2. The non-contact physiotherapy robot posture autonomous control method based on sensor array as described in claim 1, characterized in that, Sparsity optimization of dense voxel grids includes: inputting the dense voxel grid into a sparsity optimization model to obtain a compressed sparse voxel map; the sparsity optimization model includes an encoder, the output of which is connected to the input of a bottleneck layer, and the output of which is connected to the input of a decoder; the encoder is used to compress the dense voxel grid data into a low-dimensional, high-information-density sparse representation; the bottleneck layer is used to further compress the sparse representation to discard redundant information and retain only the minimum sufficient statistics required for reconstruction, thereby generating a highly compressed sparse representation; the decoder is used to reconstruct the highly compressed compact representation back into the original voxel structure, thereby obtaining the compressed sparse voxel map.

3. The non-contact physiotherapy robot posture autonomous control method based on sensor array as described in claim 2, characterized in that, The overall loss function used when training the sparse optimization model is expressed as follows: ； In the formula Represents the overall loss function. , and These represent the binary cross-entropy loss function, the 3D gradient difference loss, and the total variation regularization loss, respectively.

4. The non-contact physiotherapy robot posture autonomous control method based on sensor array as described in claim 2, characterized in that, The encoder includes an input layer, a first sparse 3D convolutional layer, a first sparse max pooling layer, and a second sparse max pooling layer. The input layer is connected to the input of the first sparse 3D convolutional layer, the output of the first sparse 3D convolutional layer is connected to the input of the first sparse max pooling layer, and the output of the first sparse max pooling layer is connected to the input of the second sparse max pooling layer through a residual block.

5. The non-contact physiotherapy robot posture autonomous decision-making method based on sensor array as described in claim 4, characterized in that, The kernel size of the first sparse 3D convolutional layer is 3. 3 3. The stride of the convolutional kernel is 1; the activation function of the first sparse 3D convolutional layer is the LeakyReLU function; the size of the pooling kernel of the first sparse max pooling layer is 2. 2 2. The sliding step size of the pooling core is 2.

6. The non-contact physiotherapy robot posture autonomous decision-making method based on sensor array as described in claim 5, characterized in that, The convolutional kernel of the first sparse 3D convolutional layer is used to learn local geometric patterns, including planar relationships, surface continuity, and occlusion structures.

7. The non-contact physiotherapy robot posture autonomous control method based on sensor array as described in claim 2, characterized in that, The decoder includes a first sparse 3D deconvolution layer, a second sparse 3D convolution layer, a second sparse 3D deconvolution layer, and an output layer. The input of the first sparse 3D deconvolution layer is connected to the output of the bottleneck layer, and its output is connected to the input of the second sparse 3D convolution layer. The input of the second sparse 3D convolution layer is also connected to the residual block of the encoder. The output of the second sparse 3D convolution layer is connected to the input of the second sparse 3D deconvolution layer. The input of the second sparse 3D deconvolution layer is also connected to the output of the second sparse max pooling layer. The output of the second sparse 3D deconvolution layer is connected to the output layer.

8. The non-contact physiotherapy robot posture autonomous control method based on sensor array as described in claim 2, characterized in that, Methods for further compression of sparse voxel features in the bottleneck layer include: Input a compact representation of sparse voxel features into a size of 1 1 Convolution with 1 channel compresses the channel, thereby generating low-dimensional features; L1 regularization is applied to the generated low-dimensional features to obtain a compact representation of the sparse voxel features.

9. The method for autonomous posture control of a non-contact physiotherapy robot based on a sensor array as described in claim 1, characterized in that, The types of movement detected in the area to be treated include: The instantaneous change rate of the treatment area is obtained by detecting the instantaneous changes of the treatment area based on the point cloud data collected by the distance sensor array. Spatial consistency analysis of the treatment area is performed, including: testing the rigid body motion hypothesis of the treatment area based on point cloud data from distance sensors to obtain the change in the position of the treatment area, and then calculating the residual energy. The residual energy is used to quantify the sum of squared errors between the change in the location of the treatment site and the predicted change. Perform characteristic displacement analysis on the treatment site, including: calculating the average change in the compression eigenvector of the treatment site. Spatial position offset under optimal matching ; The trajectory of the treatment area is fitted, and then the movement velocity vector of the treatment area is calculated. Calculate the reliability of the fitted trajectory. Movement speed vector Each element represents the speed of movement of the treatment site in each direction; The response is that the instantaneous rate of change at the treatment site is less than a preset rate of change threshold and the residual energy If the residual energy is less than the residual energy threshold, it is determined that no movement has occurred at the treatment site; this is in response to the average change in the compressed feature vector. Spatial position offset greater than the average change threshold of the compressed feature vector and the optimal matching If the spatial position offset is less than the threshold for optimal matching, it is determined that the treatment site has experienced a small-amplitude movement; this is in response to the reliability of the trajectory. The movement speed vector is greater than the confidence threshold. If the magnitude is greater than the velocity threshold, it is determined that a large-amplitude movement has occurred at the treatment site.

Images