Non-contact physiotherapy robot attitude autonomous control method based on sensor array

Abstract

The invention relates to the field of physiotherapy robots, in particular to a non-contact physiotherapy robot attitude autonomous control method based on a sensor array. The method comprises the following steps: controlling a physiotherapy assembly of the robot to move to a position above a to-be-treated part; distance information is collected in real time through a distance sensor array; constructing 3D features of the to-be-treated part by using the distance information; calculating an expected treatment posture of the 3D feature of the to-be-treated part; generating a motion path of the physiotherapy assembly from the current posture to the expected treatment posture, and controlling the physiotherapy assembly to move according to the generated motion path; judging whether the to-be-treated part moves up and down or not; if the to-be-treated part moves up and down, the distance information is acquired again; otherwise, controlling the physical therapy assembly and the to-be-treated part to form dynamic stability. By adopting the method provided by the invention, the physiotherapy effect and applicability of the non-contact physiotherapy robot can be effectively improved.

Description

Technical Field

[0001] The present invention relates to the field of physiotherapy robots. More specifically, the present invention relates to a non-contact physiotherapy robot attitude autonomous control method based on a sensor array. Background Art

[0002] A non-contact physiotherapy robot is a robot that uses advanced technologies and equipment to perform rehabilitation treatments without contacting the human body. It can transfer the physiotherapy effect through physical principles such as induction, optics, sound waves, or electromagnetism, thereby promoting the recovery of the body. The treatment methods of non-contact physiotherapy robots in the prior art for the human body include: infrared therapy, light therapy technology, ultrasonic therapy, magnetic therapy technology, and electromagnetic pulse therapy. Non-contact physiotherapy robots can not only reduce the direct contact between patients and equipment, avoid cross-infection, but also provide precise treatment plans to avoid errors in manual operations. They are usually equipped with intelligent systems that can automatically adjust parameters such as treatment intensity and frequency according to the symptoms and physical conditions of patients to improve the treatment effect. The non-contact physiotherapy robot treats a certain part of the human body through the physiotherapy components installed on its robotic arm. Before treatment, it is first necessary to control the movement of the robotic arm so that the physiotherapy components move along a preset motion trajectory to a certain fixed pose. However, due to the different body shapes of patients and different parts to be treated, if the physiotherapy components are controlled to be in the same fixed pose to treat patients, the treatment effect will be poor. In addition, during the treatment process, the patient's body may move, resulting in a change in the distance between the physiotherapy components and the part to be treated, which will also lead to poor treatment effects.

[0003] In summary, the non-contact physiotherapy robots in the prior art have technical problems of poor treatment effects and poor applicability. Summary of the Invention

[0004] To solve the technical problems of poor treatment effects and poor applicability existing in the non-contact physiotherapy robots in the prior art, the present invention provides solutions in the following aspects.

[0005] In a first aspect, the present invention provides a non-contact physiotherapy robot attitude autonomous control method based on a sensor array. A physiotherapy component and a distance sensor array are installed at the end of the robotic arm of the physiotherapy robot. The distance sensors in the distance sensor array are used to measure the distance from the distance sensors to the part to be treated. The method includes:

[0006] Controlling the physiotherapy component of the robot to move above the part to be treated; using the distance sensor array to collect distance information in real time; the distance information is used to represent the distance between each distance sensor and the part to be treated;

[0007] Constructing a 3D feature of the part to be treated using the distance information;

[0008] Calculate the desired treatment pose of the 3D features of the part to be treated; the desired treatment pose is used to characterize the ideal pose of the physiotherapy component;

[0009] Generate a motion path for the physiotherapy component from the current pose to the desired treatment pose, and control the physiotherapy component (changed to a robotic arm) to move according to the generated motion path;

[0010] In response to the physiotherapy component reaching the desired treatment pose, determine whether the part to be treated moves up and down;

[0011] If the part to be treated moves up and down, re-acquire the distance information, 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 a dynamic stability to be formed between the physiotherapy component and the part to be treated, where the dynamic stability means that when the part to be treated moves, the physiotherapy component moves along and the relative position between the two remains unchanged.

[0012] The beneficial effects are as follows: The method of the present invention does not rely on the distance information collected by a single sensor when acquiring the 3D features of the part to be treated, but relies on the distance information collected by multiple sensors, so that the constructed 3D features of the part to be treated are more accurate, and further the generated desired treatment pose is more accurate, which helps to improve the physiotherapy effect of the non-contact physiotherapy robot. By controlling the physiotherapy component to move to the desired treatment pose instead of a fixed position, the applicability of the non-contact physiotherapy robot is improved; in addition, after the physiotherapy component reaches the desired treatment pose, if the part to be treated moves, the desired treatment pose will also change. By controlling the physiotherapy component to move to the new desired treatment pose, the best pose of the physiotherapy component during treatment is ensured; during the treatment process, by making a dynamic stability be formed between the physiotherapy component and the part to be treated, it is possible to prevent the relative position between the physiotherapy component and the part to be treated from changing due to the movement of the human body, thereby ensuring the treatment effect.

[0013] Preferably, the distance sensor array is composed of two layers of sensors. The sensors in the same layer are at the same height and located on the circumference of the same circle. Each sensor is installed on a sensor bracket, and the sensor bracket is fixed at the end of the robotic arm; constructing the 3D features of the part to be treated using the distance information includes:

[0014] Fuse the point cloud data of each distance sensor of the distance sensor array to obtain the fused point cloud data;

[0015] Calculate the first local curvature of the part to be treated based on the original sensor data, and generate a dense voxel grid based on the fused point cloud data; the calculation expression of the first local curvature is:

[0016]

[0017] k1 represents the first local curvature, h t and h b respectively represent the heights of the upper - layer sensor and the lower - layer sensor, Δx represents the mean value of the distance between the projection of each sensor on the treatment site in the x - axis direction and the reference point, and Δy represents the mean value of the distance between the projection of each sensor on the treatment site in the y - axis direction and the reference point. A certain point on the treatment site can be selected as the reference point;

[0018] Perform sparsity optimization on the dense voxel grid to obtain a sparse voxel map;

[0019] Detect the motion type of the treatment site, where the motion type includes no motion, small - amplitude motion, and large - amplitude motion;

[0020] In response to the motion type of the treatment site being no motion or small - amplitude motion, use the sparse voxel map to restore the curvature field;

[0021] Perform variational implicit surface optimization based on the restored curvature field to obtain a surface function;

[0022] Calculate the second local curvature of the treatment site using the surface function;

[0023] Check whether the deviation between the first local curvature and the second local curvature is less than the deviation threshold. If the deviation between the two is less than the deviation threshold, output the surface function and use the surface function as the 3D feature of the treatment site.

[0024] The beneficial effects are as follows: When constructing the 3D feature of the treatment site, the method of the present invention constructs its 3D feature only when there is no motion or small - amplitude motion in the treatment site, thereby ensuring that the constructed 3D feature coincides with the actual 3D feature and improving the physical therapy effect of the physical therapy component; if there is a large - amplitude movement in the treatment site, discard the current value and re - obtain the first local curvature and the sparse voxel map. In addition, if the first local curvature is inconsistent with the second local curvature, it indicates that the constructed surface function is inaccurate. The method of the present invention uses the surface function as the 3D feature of the treatment site only when the deviation between the two is very small, thereby further ensuring the accuracy of the 3D feature. Moreover, adopting the sensor array layout method of this embodiment can improve the depth measurement accuracy and can calculate and obtain curvature information accurately, efficiently, and in real - time.

[0025] Preferably, the 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 the encoder is connected to the input of the bottleneck layer, and the output of the bottleneck layer is connected to the input of the decoder; the encoder is used to compress and transform 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 only retain 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 to obtain a compressed sparse voxel map.

[0026] Preferably, the expression of the overall loss function used when training the sparsity optimization model is:

[0027] loss T = loss B + 0.5 × loss G + 0.1 × loss TV ;

[0028] In the formula, loss T represents the overall loss function, loss B , loss G and loss TV represent the binary cross-entropy loss function, 3D gradient difference loss, and total variation regularization loss, respectively.

[0029] 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, where 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.

[0030] Preferably, the size of the convolution kernel of the first sparse 3D convolutional layer is 3*3*3, and the stride of the convolution kernel sliding is 1; the activation function of the first sparse 3D convolutional layer uses the LeakyReLU function; the size of the pooling kernel of the first sparse max pooling layer is 2*2*2, and the stride of the pooling kernel sliding is 2.

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

[0032] Preferably, the decoder includes a first sparse 3D transposed convolution layer, a second sparse 3D convolution layer, a second sparse 3D transposed convolution layer, and an output layer. The input of the first sparse 3D transposed convolution 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 transposed convolution layer. The input of the second sparse 3D transposed convolution layer is also connected to the output of the second sparse max pooling layer. The output of the second sparse 3D transposed convolution layer is connected to the output layer.

[0033] Preferably, the method for further compressing the sparse voxel features by the bottleneck layer includes: inputting the compact representation of the sparse voxel features into a convolutional compression channel with a size of 1*1*1 to generate low-dimensional features;

[0034] Applying L1 regularization to the generated low-dimensional features to obtain the compact representation of the sparse voxel features.

[0035] Preferably, detecting the motion type of the part to be treated includes:

[0036] Performing instantaneous change detection on the part to be treated based on the point cloud data collected by the distance sensor array to obtain the instantaneous change rate of the part to be treated;

[0037] Performing spatial consistency analysis on the part to be treated, including: performing a rigid body motion hypothesis test on the part to be treated based on the point cloud data of the distance sensor to obtain the change amount of the position of the part to be treated, and then calculating the residual energy E rigid , where the residual energy is used to quantify the sum of squared errors between the change amount of the position of the part to be treated and the predicted change amount;

[0038] Performing feature displacement analysis on the part to be treated, including: calculating the average change amount δ f of the compressed feature vector of the part to be treated and the spatial position offset δ p under the optimal match;

[0039] Fitting the trajectory of the part to be treated, and then calculating the moving speed vector of the part to be treated Calculating the credibility γ of the fitted trajectory; the moving speed vector Each element of which respectively represents the moving speed of the part to be treated in each direction;

[0040] In response to the instantaneous change rate of the part to be treated being less than the preset change rate threshold and the residual energy E rigid being less than the residual energy threshold, it is determined that the part to be treated has not moved; in response to the average change amount δ f of the compressed feature vector being greater than the average change amount threshold of the compressed feature vector and the spatial position offset δp Less than the spatial position offset threshold under the optimal match, it is determined that the treatment site has a small amplitude of movement; in response to the confidence γ of the trajectory being greater than the confidence threshold and the modulus of the movement speed vector being greater than the speed threshold, it is determined that the treatment site has a large amplitude of movement.

[0041] In summary, the beneficial effects of the present invention are as follows: The method of the present invention can improve the physical therapy effect and applicability of the non-contact physical therapy robot. Description of the Drawings

[0042] By reading the following detailed description with reference to the drawings, the above and other objects, features, and advantages of the exemplary embodiments of the present invention will become readily understood. In the drawings, several embodiments of the present invention are shown in an exemplary rather than restrictive manner, and the same or corresponding reference numerals represent the same or corresponding parts, wherein:

[0043] Figure 1 is a schematic flowchart of a method for autonomous attitude control of a non-contact physical therapy robot based on a sensor array according to an embodiment of the present invention;

[0044] Figure 2 is a schematic diagram of a sensor bracket according to an embodiment of the present invention.

[0045] Figure 3 is a schematic diagram of a sparsity optimization model structure according to an embodiment of the present invention. Detailed Embodiments

[0046] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0047] Next, the detailed embodiments of the present invention will be described in detail with reference to the drawings.

[0048] Embodiment of the method for autonomous attitude control of a non-contact physical therapy robot based on a sensor array:

[0049] As Figure 1 shown, the method for autonomous attitude control of a non-contact physical therapy robot based on a sensor array of the present invention includes:

[0050] A non-contact physical therapy robot attitude autonomous control method based on a sensor array, characterized in that a physical therapy component and a distance sensor array are installed at the end of the robotic arm of the physical therapy robot, the distance sensor is used to measure the distance from the distance sensor to the part to be treated, and the method includes:

[0051] S101. Control the physical therapy component to move to the initial position and collect distance information, specifically: control the physical therapy component of the robot to move above the part to be treated; use the 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 part to be treated;

[0052] S102. Use the distance information to construct the 3D feature of the part to be treated;

[0053] S103. Calculate the expected treatment attitude of the 3D feature of the part to be treated; the expected treatment attitude is used to characterize the ideal pose of the physical therapy component;

[0054] S104. Generate a motion path for the physical therapy component from the current attitude to the expected treatment attitude, and control the physical therapy component to move according to the generated motion path;

[0055] The method of generating the motion path of the physical therapy component from the current attitude to the expected treatment attitude can adopt a rasterized scanning algorithm, a full coverage path planning algorithm or an adaptive path planning algorithm. The full coverage path planning algorithm can adopt a Boustrophedon path algorithm, a spiral scanning algorithm or a heuristic algorithm based on wavefront propagation (Wavefront). The adaptive path planning algorithm can adopt a dynamic window method, an RRT* algorithm or a model predictive control (MPC) algorithm. In one embodiment, the path planning algorithm in the paper titled "Optimal Path Planning using RRT* based Approaches: A Survey and Future Directions" published by Iram Noreen, Amna Khan and Zulfiqar Habib can also be adopted.

[0056] S105. In response to the physical therapy component reaching the expected treatment attitude, judge whether the part to be treated moves up and down;

[0057] S106. Control the physical therapy component according to the judgment result, specifically: if the part to be treated moves up and down, re-acquire the distance information, generate a new motion path based on the distance information, and control the physical therapy component to move according to the generated motion path; otherwise, control the physical therapy component and the part to be treated to form dynamic stability, and the dynamic stability means that when the part to be treated moves, the physical therapy component moves along and the relative position between the two remains unchanged.

[0058] There are various ways to form a dynamic stability between the control physiotherapy component and the part to be treated. For example, if the part to be treated is located in the user's abdomen or chest, the physiotherapy component can be controlled to reciprocate up and down at a preset amplitude and preset frequency, and the frequency of the reciprocating up and down movement is synchronized with the user's breathing frequency. That is, when the user inhales, the part to be treated moves downward, and when the user exhales, the part to be treated moves upward; the physiotherapy component moves up and down synchronously according to the user's breathing frequency.

[0059] The method of the present invention does not rely on the distance information collected by a single sensor when obtaining the 3D features of the part to be treated, but relies on the distance information collected by multiple sensors, so that the constructed 3D features of the part to be treated are more accurate, and then the generated desired treatment posture is more accurate, which helps to improve the physiotherapy effect of the non-contact physiotherapy robot. By controlling the physiotherapy component to move to the desired treatment posture instead of a fixed position, the applicability of the non-contact physiotherapy robot is improved; in addition, after the physiotherapy component reaches the desired treatment posture, if the part to be treated moves, the desired treatment posture will also change. By controlling the physiotherapy component to move to the new desired treatment posture, the best pose of the physiotherapy component during treatment is ensured; during the treatment process, by forming a dynamic stability between the physiotherapy component and the part to be treated, it is possible to avoid the relative position between the physiotherapy component and the part to be treated from changing due to the movement of the human body, thereby ensuring the treatment effect.

[0060] In one embodiment, the distance sensor array is composed of two layers of sensors, and the sensors in the same layer are at the same height and located on the circumference of the same circle. Each sensor is installed on a sensor bracket, and the sensor bracket is fixed at the end of the robotic arm. The sensor bracket is as Figure 2 shown. Three sensors can be installed on the upper layer, and six sensors can be installed on the lower layer; the sensor bracket includes a lower ring 1 and an upper ring 2. The lower ring 1 and the upper ring 2 are fixedly connected by a support portion 3. Three sensor installation positions are provided on the upper ring 2, and six sensor installation positions are provided on the lower ring 1; constructing the 3D features of the part to be treated by using the distance information includes:

[0061] S201. Fuse the point cloud data of each distance sensor in the distance sensor array to obtain the fused point cloud data;

[0062] S202. Calculate the first local curvature of the part to be treated according to the original data of the sensor, and generate a dense voxel grid according to the fused point cloud data; calculating the first local curvature of the part to be treated according to the fused point cloud data includes:

[0063]

[0064] k1 represents the first local curvature, ht and h b respectively represent the heights of the upper - layer sensor and the lower - layer sensor, Δx represents the mean value of the distance between the projection of each sensor on the treatment site in the x - axis direction and the reference point, and Δy represents the mean value of the distance between the projection of each sensor on the treatment site in the y - axis direction and the reference point. A certain point on the treatment site can be selected as the reference point.

[0065] When sensors at different heights observe the same point, a viewing - angle difference (parallax angle θ) will be formed.

[0066]

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

[0068] S203. Optimize the sparsity of the dense voxel grid to obtain a sparse voxel map;

[0069] S204. Detect the motion type of the treatment site, where the motion type includes no motion, small - amplitude motion, and large - amplitude motion;

[0070] S205. In response to the motion type of the treatment site being no motion or small - amplitude motion, use the sparse voxel map to restore the curvature field;

[0071] S206. Perform variational implicit - surface optimization based on the restored curvature field to obtain a surface function;

[0072] S207. Use the surface function to calculate the second local curvature of the treatment site;

[0073] S208. Check whether the deviation between the first local curvature and the second local curvature is less than the deviation threshold. If the deviation between the two is less than the deviation threshold, output the surface function and use the surface function as the 3D feature of the treatment site.

[0074] When the method of the present invention constructs the 3D feature of the treatment site, it constructs the 3D feature only when the treatment site has no motion or small - amplitude motion, so as to ensure that the constructed 3D feature coincides with the actual 3D feature and improve the physical - therapy effect of the physical - therapy component; if the treatment site moves significantly, discard the current value and re - obtain the first local curvature and the sparse voxel map. In addition, if the first local curvature and the second local curvature are inconsistent, it means that the constructed surface function is inaccurate. The method of the present invention uses the surface function as the 3D feature of the treatment site only when the deviation between the two is very small, thereby further ensuring the accuracy of the 3D feature.

[0075] Furthermore, by arranging the sensor array in two layers, stereo vision is achieved through the height difference, improving the measurement depth; the sensor array of the present invention can directly obtain curvature information, enhancing the surface constraint. The curvature information can be applied to the detection of the motion type of the treatment site and the curvature consistency analysis in this embodiment.

[0076] Adopting the sensor array layout method of this embodiment can improve the depth measurement accuracy and facilitate the real-time, accurate and efficient calculation of curvature information.

[0077] In one embodiment, the fusion of the point cloud data of each distance sensor includes:

[0078] S301. Extrinsic calibration: Determine the static pose relationship between each distance sensor;

[0079] S302. Time synchronization: Align the timestamps of the sensor data;

[0080] S303. Preprocess the point cloud data collected by the sensor, including: filtering, denoising, and feature extraction;

[0081] S304. Point cloud coordinate transformation: Batch-convert the point cloud data of each sensor to the global coordinate system;

[0082] S305. Point cloud fusion and registration: Align the point clouds of each sensor and perform dynamic target tracking.

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

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

[0085] In one embodiment, the sparsity optimization of the dense voxel grid includes: inputting the dense voxel grid into the sparsity optimization model to obtain a compressed sparse voxel map; the sparsity optimization model includes an encoder, the output of the encoder is connected to the input of the bottleneck layer, and the output of the bottleneck layer is connected to the input of the decoder; the encoder is used to compress and transform 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 only retain 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 a compressed sparse voxel map.

[0086] The bottleneck layer is the hub of encoding / decoding. In the application scenario of the present invention, it can achieve the function of filtering out noise points and retaining the structural skeleton.

[0087] The sparsity optimization model of this embodiment belongs to the compression voxel-based mapping of unsupervised learning, which can provide data-adaptive and non-linear compression, better capture complex structures and redundancies, and generally achieve higher compression ratios and better reconstruction qualities.

[0088] When training the sparsity optimization model, the expression of the overall loss function used is:

[0089] loss T = loss B + 0.5 × loss G + 0.1 × loss TV ;

[0090] In the formula, loss T represents the overall loss function, and loss B , loss G and loss TV represent the binary cross-entropy loss function, 3D gradient difference loss, and total variation regularization loss respectively.

[0091] Among them, the role of the binary cross-entropy loss function is: to measure the probability distribution difference between the reconstructed voxel grid and the original voxel grid at the voxel level. It forces the decoder output to be as close as possible to the original binary or probabilistic voxel values. The role of the 3D gradient difference loss is: to force the reconstruction result to be consistent with the original data in terms of spatial structure (especially edges and textures). It focuses on the similarity of local spatial variations. The role of the total variation regularization loss is: to impose a spatial smoothness constraint on the reconstructed voxel grid itself, suppressing unnecessary noise or small oscillations. It encourages the reconstruction result to be "piecewise constant" in space. Therefore, adopting the loss function of this embodiment can greatly improve the training effect of the sparsity optimization model.

[0092] As Figure 3 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, where 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.

[0093] In this embodiment, in order to reduce the computational amount, the size of the convolutional kernel of the first sparse 3D convolutional layer is set to 3*3*3, and in order to maintain the resolution, the stride of the convolutional kernel sliding is 1. The convolutional kernel of the first sparse 3D convolutional layer is used to learn local geometric patterns, and the local geometric patterns include plane relationships, surface continuity, and occlusion structures.

[0094] Set the output channels of the first sparse 3D convolutional layer to increase layer by layer, thereby gradually expanding the feature dimension.

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

[0096] The size of the pooling kernel of the first sparse max pooling layer is 2*2*2, the sliding step of the pooling kernel is 2, and the pooling layer only operates on the non-empty voxel region, thereby discarding 75% of the redundant voxels.

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

[0098] In one embodiment, the method for the bottleneck layer to further compress sparse voxel features includes:

[0099] S701: Input the compact representation of the sparse voxel features into a convolutional compression channel with a size of 1*1*1 to generate low-dimensional features;

[0100] S702: Apply L1 regularization to the generated low-dimensional features to obtain the compact representation of the sparse voxel features.

[0101] In one embodiment, detecting the motion type of the part to be treated includes:

[0102] S801: Perform instantaneous change detection on the part to be treated based on the point cloud data collected by the distance sensor array to obtain the instantaneous change rate of the part to be treated;

[0103] In this embodiment, the instantaneous change rate measured by each sensor can be calculated first, and then summed to obtain the instantaneous change rate of the part to be treated.

[0104] The calculation expression for the instantaneous change rate measured by the i-th sensor is:

[0105]

[0106] In the formula, Δd i (t) is the derivative based on the point cloud data of the sensor, d i (t) represents the point cloud data of the i-th sensor, and Δt represents the unit time length.

[0107] S802. Perform spatial consistency analysis on the treatment site, including: performing a rigid body motion hypothesis test on the treatment site based on the point cloud data of the distance sensor, so as to obtain the change amount of the position of the treatment site, and then calculating the residual energy E rigid , where the residual energy is used to quantify the sum of squared errors between the change amount of the position of the treatment site and the predicted change amount;

[0108] In this embodiment, there are a total of 9 sensors in the distance sensor array, and the residual energy E rigid The calculation expression is:

[0109]

[0110] In the above two formulas, R and T respectively represent the rotation matrix and translation vector (true value) of the rigid body motion, and respectively represent the estimated rotation matrix and estimated translation vector of the rigid body motion, represents the displacement vector of the three-dimensional point corresponding to the i-th sensor between the current frame and the previous frame, H0 represents the rigid body motion hypothesis, represents the position vector of the three-dimensional point corresponding to the i-th sensor in the previous frame.

[0111] S803. Perform feature displacement analysis on the treatment site, including: calculating the average change amount δ f of the compressed feature vector of the treatment site and the spatial position offset δ p under the optimal match;

[0112] The spatial position offset under the optimal match refers to finding a spatial position offset amount that minimizes the matching error by comparing the similarities of two or more objects.

[0113] In this embodiment, the average change amount δ f of the compressed feature vector is calculated as follows:

[0114]

[0115] The spatial position offset δ p under the optimal match is calculated as follows:

[0116]

[0117] In the formula, α k (t) represents the feature encoding vector of the k-th compressed voxel at time t, α k (t - Δt) represents the feature encoding vector of the k-th compressed voxel at time t - Δt, K represents the total number of compressed voxels, ||||1 represents the L1 norm, ||||2 represents the L2 norm, ξk (t) and ξ k (t - Δt) respectively represent the position encoding vectors of the k-th compressed voxel at time t and time t - Δt, and Δt represents the unit time length.

[0118] In this embodiment, the average change amount δ of the compressed feature vector f is represented by the average change amount of the L1 norm of the compressed voxel features, and can reflect the motion intensity of the feature space.

[0119] S804. Fit the trajectory of the part to be treated, and then calculate the movement speed vector of the part to be treated Calculate the credibility γ of the fitted trajectory; the movement speed vector The respective elements of represent the movement speeds of the part to be treated in each direction;

[0120] The movement speed vector of the part to be treated The calculation expression is:

[0121]

[0122] The calculation expression of the credibility γ of the trajectory is:

[0123]

[0124] In the above two formulas, RMS fit represents the root mean square error of the trajectory fitting. represents the least squares estimate of the movement speed vector, argmin represents the value of the variable when the function is minimized, T represents the observation window length, τ represents the intermediate quantity for summation, represents the position vector of the part to be treated at time t - τ, represents the position vector of the part to be treated at time 0, σ p represents the spatial position offset under the optimal match; δ max is a constant, |||| represents the norm, and Δt represents the unit time length.

[0125] S805. In response to the instantaneous change rate of the part to be treated being less than the preset change rate threshold and the residual energy E rigid being less than the residual energy threshold, determine that the part to be treated has not moved; in response to the average change amount δ of the compressed feature vector f being greater than the average change amount threshold of the compressed feature vector and the spatial position offset δ p under the optimal match being less than the spatial position offset threshold under the optimal match, determine that the part to be treated has a small amplitude of movement; in response to the credibility γ of the trajectory being greater than the credibility threshold and the modulus of the movement speed vector being greater than the speed threshold, then determine that the part to be treated has a large amplitude of movement.

[0126] In this embodiment, the change rate threshold is 5, and other appropriate values can also be taken in other embodiments. The values of the residual energy threshold, the compressed voxel feature displacement threshold, the position consistency metric threshold, the confidence threshold, and the velocity threshold can all be determined based on experiments.

[0127] In one embodiment, it further includes:

[0128] S401. Compare the distance information collected this time with the distance information collected last time to determine whether the physiotherapy component is moving towards the treatment site;

[0129] S402. In response to the physiotherapy component moving towards the treatment site, reduce the movement speed of the physiotherapy component;

[0130] S403. In response to the distance between the physiotherapy component and the treatment site reaching the risk distance, control the physiotherapy component to perform an avoidance action to a safe area.

[0131] Using the method of this embodiment can avoid the collision between the physiotherapy component and the treatment site, thereby improving the safety of the entire physiotherapy process.

[0132] In one embodiment, it further includes: Before obtaining the distance information measured by the distance sensor array, the internal parameters of each sensor of the distance sensor array need to be individually calibrated, and the external parameters of each sensor need to be jointly calibrated.

[0133] The joint calibration includes static calibration and dynamic calibration. The static calibration includes:

[0134] S501. Place the polyhedron 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 polyhedron calibration frame;

[0135] S502. Extract the feature points of the point cloud plane from the point cloud data obtained from each distance sensor, and establish the global 3D coordinate correspondence relationship of the feature points detected by each distance sensor;

[0136] S503. For each distance sensor, input the scanned point cloud data of each into the Ceres Solver for optimization.

[0137] The dynamic calibration includes:

[0138] S601. Control the polyhedron calibration frame to move within the field of view of each distance sensor, and record the corresponding timing data of each distance sensor at the same time;

[0139] S602. Align the observation trajectories of each distance sensor, and solve for the external parameters and time offsets;

[0140] S603. Perform time synchronization calibration for each sensor.

Claims

1. A non-contact physiotherapy robot attitude autonomous control method based on a sensor array, characterized in that, A physiotherapy component and a distance sensor array are installed at the end of the robotic arm of the physiotherapy robot. The distance sensors in the distance sensor array are used to measure the distance from the distance sensors to the part to be treated. The method includes: Controlling the physiotherapy component of the robot to move above the part to be treated; using the distance sensor array to collect distance information in real time; the distance information is used to represent the distance between each distance sensor and the part to be treated; Using the distance information to construct the 3D feature of the part to be treated; Calculating the desired treatment pose of the 3D feature of the part to be treated; the desired treatment pose is used to represent the ideal pose of the physiotherapy component; Generating a motion path for the physiotherapy component from the current pose to the desired treatment pose, and controlling the physiotherapy component to move according to the generated motion path; In response to the physiotherapy component reaching the desired treatment pose, determining whether the part to be treated moves up and down; If the part to be treated moves up and down, re-acquiring the distance information, generating a new motion path based on the distance information, and controlling the physiotherapy component to move according to the generated motion path; otherwise, controlling a dynamic stability to be formed between the physiotherapy component and the part to be treated, where the dynamic stability means that when the part to be treated moves, the physiotherapy component moves along and the relative position between the two remains unchanged.

2. The non-contact physiotherapy robot attitude autonomous control method based on a sensor array according to claim 1, characterized in that, The distance sensor array is two layers of sensors. The sensors in the same layer are at the same height and located on the circumference of the same circle. Each sensor is installed on a sensor bracket, and the sensor bracket is fixed at the end of the robotic arm. Using the distance information to construct the 3D feature of the part to be treated includes: Fusing the point cloud data of each distance sensor in the distance sensor array to obtain the fused point cloud data; Calculating the first local curvature of the part to be treated based on the original data of the sensors, and generating a dense voxel grid based on the fused point cloud data; the calculation expression of the first local curvature is: k1 represents the first local curvature, h t and h b respectively represent the heights of the upper-layer sensor and the lower-layer sensor. Δx represents the mean value of the distance between the projection of each sensor on the treatment site and the reference point in the x-axis direction, and Δy represents the mean value of the distance between the projection of each sensor on the treatment site and the reference point in the y-axis direction. A certain point on the treatment site can be selected as the reference point; Performing sparsity optimization on the dense voxel grid to obtain a sparse voxel map; Detecting the motion type of the part to be treated, where the motion type includes no motion, small-amplitude motion, and large-amplitude motion; In response to the motion type of the part to be treated being no motion or small-amplitude motion, restoring the curvature field using the sparse voxel map; Performing variational implicit surface optimization based on the restored curvature field to obtain a surface function; Calculating the second local curvature of the part to be treated using the surface function; Checking whether the deviation between the first local curvature and the second local curvature is less than a deviation threshold. If the deviation between the two is less than the deviation threshold, outputting the surface function and using the surface function as the 3D feature of the part to be treated.

3. The non-contact physiotherapy robot attitude autonomous control method based on a sensor array according to claim 2, characterized in that, 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 the encoder is connected to the input of the bottleneck layer, and the output of the bottleneck layer is connected to the input of the decoder; the encoder is used to compress and transform 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 only retain 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 to obtain a compressed sparse voxel map.

4. The non-contact physiotherapy robot attitude autonomous control method based on a sensor array according to claim 3, characterized in that, The expression of the overall loss function used when training the sparsity optimization model is: loss T = loss B + 0.5 × loss G + 0.1 × loss TV ; where loss T represents the overall loss function, loss B , loss G and loss TV represent the binary cross-entropy loss function, 3D gradient difference loss, and total variation regularization loss, respectively.

5. The non-contact physiotherapy robot attitude autonomous control method based on a sensor array according to claim 3, 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, where 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.

6. The non-contact physiotherapy robot attitude autonomous decision-making method based on a sensor array according to claim 5, characterized in that, The size of the convolution kernel of the first sparse 3D convolutional layer is 3*3*3, and the stride of the convolution kernel sliding is 1; the activation function of the first sparse 3D convolutional layer uses the LeakyReLU function; the size of the pooling kernel of the first sparse max pooling layer is 2*2*2, and the stride of the pooling kernel sliding is 2.

7. The non-contact physiotherapy robot attitude autonomous decision-making method based on a sensor array according to claim 6, wherein The convolution kernel of the first sparse 3D convolutional layer is used to learn local geometric patterns, and the local geometric patterns include plane relationships, surface continuity, and occlusion structures.

8. The non-contact physiotherapy robot attitude autonomous control method based on a sensor array according to claim 3, characterized in that, The decoder includes a first sparse 3D transposed convolutional layer, a second sparse 3D convolutional layer, a second sparse 3D transposed convolutional layer, and an output layer, where the input of the first sparse 3D transposed convolutional layer is connected to the output of the bottleneck layer, its output is connected to the input of the second sparse 3D convolutional layer, the input of the second sparse 3D convolutional layer is also connected to the residual block of the encoder, the output of the second sparse 3D convolutional layer is connected to the input of the second sparse 3D transposed convolutional layer, the input of the second sparse 3D transposed convolutional layer is also connected to the output of the second sparse max pooling layer, and the output of the second sparse 3D transposed convolutional layer is connected to the output layer.

9. The non-contact physiotherapy robot attitude autonomous control method based on a sensor array according to claim 3, characterized in that, The method for the bottleneck layer to further compress the sparse voxel features includes: Inputting the compact representation of the sparse voxel features into a convolutional compression channel with a size of 1*1*1 to generate low-dimensional features; Applying L1 regularization to the generated low-dimensional features to obtain the compact representation of the sparse voxel features.

10. The non-contact physiotherapy robot attitude autonomous control method based on a sensor array according to claim 2, characterized in that, Detecting the motion type of the part to be treated includes: Performing instantaneous change detection on the part to be treated based on the point cloud data collected by the distance sensor array to obtain the instantaneous change rate of the part to be treated; Perform a spatial consistency analysis on the treatment site, including: performing a rigid body motion hypothesis test on the treatment site based on the point cloud data of the distance sensor to obtain the change amount of the position of the treatment site, and then calculating the residual energy E rigid , where the residual energy is used to quantify the sum of squared errors between the change amount of the position of the treatment site and the predicted change amount; Performing characteristic displacement analysis on the treatment site, including: calculating the average change amount δ of the compression characteristic vector of the treatment site to be treated f and the spatial position offset δ under the optimal match p ; Fit the trajectory of the treatment site, and then calculate the movement speed vector of the treatment site to be treated Calculate the credibility γ of the fitted trajectory; the movement speed vector Each element of represents the movement speed of the treatment site to be treated in each direction; In response to the instantaneous change rate of the part to be treated being less than a preset change rate threshold and the residual energy E rigid being less than the residual energy threshold, it is determined that the part to be treated has not moved; in response to the average change amount δ f of the compression feature vector being greater than the average change amount threshold of the compression feature vector and the spatial position offset δ p under the optimal match being less than the spatial position offset threshold under the optimal match, it is determined that the part to be treated has undergone a small-amplitude movement; in response to the credibility γ of the trajectory being greater than the credibility threshold and the modulus of the movement speed vector being greater than the speed threshold, it is determined that the part to be treated has undergone a large-amplitude movement.

Images