A human body surface modeling and physiotherapy trajectory generation and safety control method

Abstract

This invention relates to a method for human body surface modeling, therapeutic trajectory generation, and safety control, belonging to the field of therapeutic control technology. Through depth fusion of end-effector monocular vision and ToF single-point ranging, it achieves low-cost, high-precision, and robust human body surface modeling and safe therapeutic trajectory control without the need for a 3D depth camera or additional lighting. Simultaneously, through dual-mode control and a multi-layered safety envelope mechanism, it addresses the safety requirements of both suspended constant-distance therapeutic therapy and contact therapeutic therapy scenarios, significantly improving the modeling robustness, environmental adaptability, and human-computer interaction safety of the therapeutic robot, providing an effective technical path for the practical application of intelligent therapeutic robots.

Description

Technical Field

[0001] This invention belongs to the field of physiotherapy control technology, specifically a method for human body surface modeling, physiotherapy trajectory generation and safety control. Background Technology

[0002] In the field of rehabilitation and physiotherapy, with the continuous development of robotics technology, collaborative robots are increasingly being applied to physiotherapy scenarios. These robots use end effectors on robotic arms to perform therapeutic operations such as pressing, rubbing, and tapping on the human body surface. Because the physiotherapy process usually needs to be performed within close range of the human body, and the human body surface has complex and irregular curved features, physiotherapy robots must have the ability to perceive the geometric shape of the human body surface and plan safe and effective physiotherapy trajectories accordingly.

[0003] Currently, the mainstream technologies for acquiring geometric information of the human body surface in physiotherapy robots mainly rely on 3D depth sensors such as structured light, binocular vision, or Time-of-Flight (ToF) depth cameras. These sensors can directly output point clouds or depth maps, facilitating subsequent modeling and trajectory planning. However, this approach has the following shortcomings in practical applications: (1) High cost and integration complexity: Three-dimensional depth sensors are usually expensive and have a large size and weight, which puts higher demands on the load capacity and structural compactness of the robotic arm end, increasing the overall cost and integration difficulty of the system.

[0004] (2) Limited environmental adaptability: Structured light and binocular vision are prone to depth loss or matching errors in strong ambient light, reflective surfaces or low-texture areas; ToF depth cameras also have unstable ranging under strong light or transparent / semi-transparent surface conditions. To overcome the above problems, additional lighting devices or light-shielding structures are often required, which further increases the complexity of the system.

[0005] (3) Monocular vision has cost advantages but lacks scale information: Monocular visible light cameras have the advantages of low cost, small size and easy integration, but their single-frame images lack depth information at the real scale, making it difficult to directly support geometric modeling of the human body surface and subsequent trajectory planning.

[0006] (4) Single-point ToF sensors are difficult to independently construct surface-level geometric models: ToF single-point ranging sensors can provide high-precision absolute distance information, but their measurement range is limited to a single point or a small area. They cannot independently construct surface-level geometric models covering the physiotherapy area, and are also difficult to use directly for trajectory planning and attitude control.

[0007] Furthermore, existing physiotherapy robots have relatively simple safety control mechanisms during the execution phase. During therapy, the human body may experience minute displacements due to factors such as breathing, slight body movements, and changes in position. Relying solely on a single sensor for feedback control can easily lead to safety risks such as collisions, pressure sores, or trajectory deviations. Especially in the two modes of suspended constant-distance therapy and close-contact therapy, the requirements for control strategies and safety redundancy differ significantly, and existing solutions often struggle to address both simultaneously.

[0008] In summary, current technologies lack a solution that can balance the accuracy of human body surface geometry modeling with the safety of the physiotherapy execution phase without requiring a 3D depth camera or additional lighting. Therefore, there is an urgent need to propose a modeling and control method that integrates end-effector monocular vision and Time-of-Flight (ToF) single-point ranging to improve the modeling robustness and operational safety of physiotherapy robots while controlling cost and system complexity. Summary of the Invention

[0009] To solve the above problems, the technical solution provided by the present invention is as follows: The present invention provides a method for human body surface modeling, physiotherapy trajectory generation, and safety control, the method comprising the following steps: S100. Before the physiotherapy begins, drive the end effector of the robotic arm to move along a preset scanning trajectory, acquire multiple frames of monocular images, and record the pose of the end effector of the robotic arm corresponding to each frame of monocular image. S200. While acquiring the monocular image, simultaneously acquire single-point ranging distance data from the ToF distance sensor; S300: Based on the camera intrinsic parameters and the hand-eye calibration extrinsic parameters, the end-effector pose of the robotic arm is converted into a camera pose sequence; S400. Under the constraint of the known camera pose sequence, perform multi-view reconstruction on multiple frames of monocular images to generate a point cloud or height field of the human body surface, and calculate the surface normal or curvature information from the point cloud or height field. S500, Perform offset compensation on the ToF single-point ranging distance data to map the fixed spatial offset between the ToF ranging point and the physiotherapy point to the equivalent distance of the physiotherapy point; S600: Calculate the predicted distance in the ToF direction based on the point cloud or height field of the human body surface, and perform a consistency quality assessment between the predicted distance and the equivalent distance; trigger a backoff strategy when the consistency quality assessment does not meet a preset threshold or an abnormal ToF signal is detected. S700. When the consistency quality assessment meets the preset threshold, a physiotherapy trajectory is generated based on the human body surface geometry model and output to the robotic arm controller for execution. The physiotherapy trajectory includes at least position, posture and process parameters. S800, during the physiotherapy execution phase, select either suspended constant distance control or contact fit control based on the mode: During suspended constant distance control, the equivalent distance is used as a feedback quantity to perform closed-loop control on the displacement or velocity of the end along the surface normal to maintain the preset distance. During contact control, the force sensor output is used as feedback to perform compliant control to maintain a preset force range, and the equivalent distance is used as a safety boundary. In any mode, at least one or more of the following are used to form a safety envelope: ToF threshold, force threshold, and velocity / acceleration limit.

[0010] Preferably, there is a spatial offset between the ranging point of the ToF single-point ranging distance sensor and the action point of the physiotherapy head, and the offset distance is 2cm to 20cm.

[0011] Preferably, the preset scanning trajectory covers an area with a bed surface width of 50cm to 100cm; the trajectory is a grid reciprocating trajectory, a parallel line trajectory, a circular arc trajectory, or a combination thereof; For each sampling point Record: Image ToF distance Timestamp ; Calculate the camera pose sequence: ; Under known pose constraints, perform multi-view reconstruction to generate point clouds or height fields on the human body surface: at least one of the following can be used: feature matching and triangulation, multi-view stereo matching based on known pose, planar sweeping, voxel fusion / raster fusion; and calculate surface normals and curvature. Filter the ToF readings and according to and By performing bias compensation, the equivalent distance of the therapeutic point of action is obtained. .

[0012] Preferably, the consistency quality assessment includes: calculating the difference between the predicted distance obtained from the monocular modeling and the measured ToF distance after bias compensation; triggering a fallback strategy when the difference exceeds a preset threshold; the fallback strategy includes at least one of the following: rescanning, reducing speed, increasing the safe distance, pausing therapy, or retreating; specifically: The predicted distance is calculated based on a geometric model along the ToF optical axis. ; Calculate the consistency error: ; A credibility score S is generated by combining indicators such as the number of feature matches, reprojection error, and point cloud density / hole ratio. When any of the conditions are met or If the ToF signal is abnormal, a fallback strategy will be triggered. When the evaluation is successful, a geometric model M of the human body surface is output for trajectory generation.

[0013] Preferably, the surface normal vector is calculated from the point cloud or height field and used for the posture planning of the therapy head, so that the therapy head aligns with the surface normal at a preset angle or moves along the surface tangential direction.

[0014] Preferably, during physiotherapy, switching between suspended constant distance control and contact control is allowed, and ToF distance or force feedback is used to perform transition speed limiting control during the switching process.

[0015] Preferably, during the physiotherapy process, a slight lift is triggered at the end of the action segment interval, and at least one frame of image and ToF distance are acquired to correct the local geometric model or update the subsequent trajectory.

[0016] Preferably, the abnormal rollback strategy is triggered when the number of feature matches is insufficient, the reprojection error exceeds the threshold, the point cloud density is lower than the threshold, or the ToF signal is abnormal.

[0017] Preferably, the monocular image, ToF distance, and end-effector pose are aligned using a unified timestamp or by interpolation.

[0018] Compared with the prior art, the technical solution provided by this invention has the following advantages: This invention discloses a method for human body surface modeling, therapeutic trajectory generation, and safety control. Through depth fusion of end-effector monocular vision and Time-of-Flight (ToF) single-point ranging, it achieves low-cost, high-precision, and robust human body surface modeling and safe therapeutic trajectory control without the need for a 3D depth camera or additional lighting. Simultaneously, through dual-mode control and a multi-layered safety envelope mechanism, it addresses the safety requirements of both suspended constant-distance therapeutic therapy and contact therapeutic therapy scenarios, significantly improving the modeling robustness, environmental adaptability, and human-computer interaction safety of the therapeutic robot. This provides an effective technical path for the practical application of intelligent therapeutic robots. Attached Figure Description

[0019] Figure 1 This is a system block diagram used in Example 1; Figure 2 This is a schematic diagram of the scanning trajectory in Example 1; Figure 3 This is a schematic diagram of the data fusion process in Example 1; Figure 4 This is a control logic block diagram of the system in Example 1; Figure 5 This is a flowchart of the scanning modeling and physiotherapy execution in Example 1; Figure 6 This is the state machine diagram for the execution control of Example 1. Detailed Implementation

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

[0021] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this application are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate for the embodiments of this application described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0022] In this application, the terms "upper," "lower," "left," "right," "front," "rear," "top," "bottom," "inner," "outer," "middle," "vertical," "horizontal," "lateral," and "longitudinal" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. These terms are primarily for the purpose of better describing this application and its embodiments, and are not intended to limit the indicated device, element, or component to having a specific orientation, or to be constructed and operated in a specific orientation.

[0023] Furthermore, in addition to indicating location or positional relationship, some of the aforementioned terms may also have other meanings. For example, the term "above" may also be used in some cases to indicate a certain dependency or connection relationship. Those skilled in the art can understand the specific meaning of these terms in this application based on the specific circumstances.

[0024] Furthermore, the terms "installation," "setup," "equipped with," "connection," "linking," and "socketing" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral structure; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium, or an internal connection between two devices, components, or parts. Those skilled in the art can understand the specific meaning of these terms in this application based on the specific circumstances.

[0025] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0026] This embodiment provides a method for human body surface modeling, physiotherapy trajectory generation, and safety control, aiming to solve the problems of high cost, complex integration, and poor environmental adaptability caused by existing physiotherapy robots that rely on three-dimensional depth sensors. By fusing end-effector monocular vision with ToF single-point ranging, it achieves low-cost, highly robust human body surface modeling and safe physiotherapy trajectory control.

[0027] The system used in this embodiment includes: a collaborative robotic arm, a monocular camera mounted on the end of the robotic arm, a ToF single-point ranging sensor, a six-dimensional force sensor, and a processor and memory. There is a fixed spatial offset (e.g., 2cm to 20cm) between the ToF sensor ranging point and the point of action of the therapy head, which is compensated for in subsequent steps through coordinate transformation.

[0028] See attached document Figure 1-6 The method in this embodiment includes the following steps: S100. Before the physiotherapy begins, drive the end effector of the robotic arm to move along a preset scanning trajectory, acquire multiple frames of monocular images, and record the pose of the end effector of the robotic arm corresponding to each frame of monocular image. S200. While acquiring the monocular image, simultaneously acquire single-point ranging distance data from the ToF distance sensor; S300: Based on the camera intrinsic parameters and the hand-eye calibration extrinsic parameters, the end-effector pose of the robotic arm is converted into a camera pose sequence; S400. Under the constraint of the known camera pose sequence, perform multi-view reconstruction on multiple frames of monocular images to generate a point cloud or height field of the human body surface, and calculate the surface normal or curvature information from the point cloud or height field. S500, Perform offset compensation on the ToF single-point ranging distance data to map the fixed spatial offset between the ToF ranging point and the physiotherapy point to the equivalent distance of the physiotherapy point; S600: Calculate the predicted distance in the ToF direction based on the point cloud or height field of the human body surface, and perform a consistency quality assessment between the predicted distance and the equivalent distance; trigger a backoff strategy when the consistency quality assessment does not meet a preset threshold or an abnormal ToF signal is detected. S700. When the consistency quality assessment meets the preset threshold, a physiotherapy trajectory is generated based on the human body surface geometry model and output to the robotic arm controller for execution. The physiotherapy trajectory includes at least position, posture and process parameters. S800, during the physiotherapy execution phase, select either suspended constant distance control or contact fit control based on the mode: During suspended constant distance control, the equivalent distance is used as a feedback quantity to perform closed-loop control on the displacement or velocity of the end along the surface normal to maintain the preset distance. During contact control, the force sensor output is used as feedback to perform compliant control to maintain a preset force range, and the equivalent distance is used as a safety boundary. In any mode, at least one or more of the following are used to form a safety envelope: ToF threshold, force threshold, and velocity / acceleration limit.

[0029] Specifically, including Mode A: Suspended Constant Distance Mode (ToF Closed Loop) Execute based on the tangential motion provided by the trajectory; Normal direction based on distance deviation Adjust the end displacement or velocity to form a closed-loop control; when If a Time-of-Flight (ToF) anomaly occurs, immediately decelerate / stop / retreat.

[0030] Mode B: Fitting Contact Mode (Force-Controlled Smoothness) Execute tangential motion based on the trajectory; In the normal direction, compliance control (impedance / admittance) is performed based on feedback from the force sensor to keep the contact force within a preset range; The Time-of-Flight (ToF) equivalent distance serves as a safety boundary: triggering retreat / pause when abnormal proximity, sudden distance changes, or distances below a safety threshold are detected.

[0031] Mode switching transition Allows switching between mode A and mode B; The switching process must go through a transition section: simultaneously constrain distance and force thresholds, and limit speed / acceleration to avoid instantaneous impact or false triggering.

[0032] Before the physical therapy begins, the robotic arm needs to scan the human body surface to obtain geometric information. By driving the end effector of the robotic arm to move along a preset trajectory, the system can obtain images and corresponding pose information from multiple perspectives covering the physical therapy area, providing a data foundation for subsequent multi-view reconstruction.

[0033] The method is performed using the following system: The system includes a robotic arm, an end-effector monocular camera, a ToF single-point ranging distance sensor, a force sensor, a processor, and a memory.

[0034] The coordinate calibration and offset parameters are as follows: Calibrate the camera's intrinsic parameter K; Hand-eye calibration obtains the extrinsic parameters of the camera and tool coordinate systems. ; Calibrate the extrinsic parameters of the ToF coordinate system and the tool coordinate system ; Fixed spatial offset between the ToF ranging point and the point of application of physical therapy. The offset is preferably about 5cm, and can be set in the range of 2cm–20cm.

[0035] The preset scanning trajectory covers an area with a bed surface width of 50cm to 100cm, and the trajectory is a grid reciprocating trajectory, a parallel line trajectory, a circular arc trajectory, or a combination thereof; The end effector is raised to the scanning height so that the ToF reading falls within its range. In a preferred embodiment, the bed height is approximately 60 cm, the bed width is approximately 70 cm, and the scanning coverage width is preferably approximately 70 cm.

[0036] Sampling is performed along a preset scanning trajectory, which can be a grid reciprocating trajectory, a parallel line trajectory, a circular arc trajectory, or a combination thereof; for each sampling point... Record: Image ToF distance Timestamp ; Calculate the camera pose sequence: ; Perform multi-view reconstruction to generate point clouds or height fields on the human body surface under known pose constraints: at least one of feature matching and triangulation, multi-view stereo matching based on known pose, planar sweeping, voxel fusion / grid fusion can be used; and the surface normal and curvature can be calculated (through local plane fitting, PCA or other methods).

[0037] Filter the ToF readings (e.g., moving average / +median filtering), and based on... and By performing bias compensation, the equivalent distance of the therapeutic point of action is obtained. .

[0038] The consistency quality assessment includes: calculating the difference between the predicted distance obtained from the monocular modeling and the measured ToF distance after bias compensation; when the difference exceeds a preset threshold, a fallback strategy is triggered. The fallback strategy includes at least one of the following: rescanning, reducing speed, increasing the safe distance, pausing therapy, or retreating. Specifically: Predicted distance calculated along the ToF optical axis based on a geometric model. ; Calculate the consistency error: ; A credibility score S is generated by combining indicators such as the number of feature matches, reprojection error, and point cloud density / hole ratio. When any of the conditions are met or If the ToF signal is abnormal, a fallback strategy will be triggered. When the evaluation is successful, a geometric model M of the human body surface is output for trajectory generation.

[0039] The therapeutic trajectory is generated based on the geometric model M, and the trajectory includes: Position trajectory: Distributed along the surface tangentially / preset path; Posture trajectory: Preferably, the treatment head is aligned with the surface normal or maintains a preset angle; Process parameters: Target distance in suspended mode Target force range of the fitting mode Velocity / acceleration limit, ToF safety threshold, and force safety threshold, etc.

[0040] The surface normal vector is calculated from the point cloud or height field and used for the posture planning of the therapy head, so that the therapy head aligns with the surface normal at a preset angle or moves along the surface tangential.

[0041] During physiotherapy, switching between suspended constant distance control and contact control is allowed, and ToF distance or force feedback is used to perform transition speed limiting control during the switching process.

[0042] During physiotherapy, a slight lift is triggered at the end of the action segment interval, and at least one frame of image and ToF distance are acquired to correct the local geometric model or update the subsequent trajectory.

[0043] The abnormal rollback strategy is triggered when the number of feature matches is insufficient, the reprojection error exceeds the threshold, the point cloud density is lower than the threshold, or the ToF signal is abnormal.

[0044] The monocular image, ToF distance, and end-effector pose are aligned using a unified timestamp or by interpolation.

[0045] This invention achieves low-cost, high-precision, and robust human body surface modeling and safe therapeutic trajectory control through deep fusion of end-effector monocular vision and Time-of-Flight (ToF) single-point ranging, without requiring a 3D depth camera or additional lighting. Simultaneously, through dual-mode control and a multi-layered safety envelope mechanism, it addresses the safety requirements of both suspended constant-distance therapy and contact therapy scenarios, significantly improving the modeling robustness, environmental adaptability, and human-computer interaction safety of the therapeutic robot, providing an effective technical path for the practical application of intelligent therapeutic robots.

[0046] The above-described embodiments are merely illustrative of certain implementations of the present invention, and are described in a relatively specific and detailed manner. However, they should not be construed as limiting the scope of the present invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements are all within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the appended claims.

Claims

1. A method for human body surface modeling, physiotherapy trajectory generation, and safety control, characterized in that: The method includes the following steps: S100. Before the physiotherapy begins, drive the end effector of the robotic arm to move along a preset scanning trajectory, acquire multiple frames of monocular images, and record the pose of the end effector of the robotic arm corresponding to each frame of monocular image. S200. While acquiring the monocular image, simultaneously acquire single-point ranging distance data from the ToF distance sensor; S300: Based on the camera intrinsic parameters and the hand-eye calibration extrinsic parameters, the end-effector pose of the robotic arm is converted into a camera pose sequence; S400. Under the constraint of the known camera pose sequence, perform multi-view reconstruction on multiple frames of monocular images to generate a point cloud or height field of the human body surface, and calculate the surface normal or curvature information from the point cloud or height field. S500, Perform offset compensation on the ToF single-point ranging distance data to map the fixed spatial offset between the ToF ranging point and the physiotherapy point to the equivalent distance of the physiotherapy point; S600: Calculate the predicted distance in the ToF direction based on the point cloud or height field of the human body surface, and perform a consistency quality assessment between the predicted distance and the equivalent distance; trigger a backoff strategy when the consistency quality assessment does not meet a preset threshold or an abnormal ToF signal is detected. S700. When the consistency quality assessment meets the preset threshold, a physiotherapy trajectory is generated based on the human body surface geometry model and output to the robotic arm controller for execution. The physiotherapy trajectory includes at least position, posture and process parameters. S800, during the physiotherapy execution phase, select either suspended constant distance control or contact fit control based on the mode: During suspended constant distance control, the equivalent distance is used as a feedback quantity to perform closed-loop control on the displacement or velocity of the end along the surface normal to maintain the preset distance. During contact control, the force sensor output is used as feedback to perform compliant control to maintain a preset force range, and the equivalent distance is used as a safety boundary. In any mode, at least one or more of the following are used to form a safety envelope: ToF threshold, force threshold, and velocity / acceleration limit.

2. The method according to claim 1, characterized in that, The method is performed using the following system: The system includes a robotic arm, an end-effector monocular camera, a ToF single-point ranging distance sensor, a force sensor, a processor, and a memory.

3. The method according to claim 1, characterized in that, There is a spatial offset between the measuring point of the ToF single-point ranging distance sensor and the action point of the physiotherapy head, and the offset distance is 2cm to 20cm.

4. The method according to claim 1, characterized in that, The preset scanning trajectory covers an area with a bed surface width of 50cm to 100cm, and the trajectory is a grid reciprocating trajectory, a parallel line trajectory, a circular arc trajectory, or a combination thereof; For each sampling point Record: Image ToF distance Timestamp ; Calculate the camera pose sequence: ； Under known pose constraints, perform multi-view reconstruction to generate point clouds or height fields on the human body surface: at least one of the following can be used: feature matching and triangulation, multi-view stereo matching based on known pose, planar sweeping, voxel fusion / raster fusion; and calculate surface normals and curvature. Filter the ToF readings and according to and By performing bias compensation, the equivalent distance of the therapeutic point of action is obtained. .

5. The method according to claim 1, characterized in that, The consistency quality assessment includes: calculating the difference between the predicted distance obtained from the monocular modeling and the measured ToF distance after bias compensation; when the difference exceeds a preset threshold, a fallback strategy is triggered. The fallback strategy includes at least one of the following: rescanning, reducing speed, increasing the safe distance, pausing therapy, or retreating. Specifically: Predicted distance calculated along the ToF optical axis based on a geometric model. ; Calculate the consistency error: ； A credibility score S is generated by combining indicators such as the number of feature matches, reprojection error, and point cloud density / hole ratio. When any of the conditions are met or If the ToF signal is abnormal, a fallback strategy will be triggered. When the evaluation is successful, a geometric model M of the human body surface is output for trajectory generation.

6. The method according to claim 1, characterized in that, The surface normal vector is calculated from the point cloud or height field and used for the posture planning of the therapy head, so that the therapy head aligns with the surface normal at a preset angle or moves along the surface tangential.

7. The method according to claim 1, characterized in that, During physiotherapy, switching between suspended constant distance control and contact control is allowed, and ToF distance or force feedback is used to perform transition speed limiting control during the switching process.

8. The method according to claim 1, characterized in that, During physiotherapy, a slight lift is triggered at the end of the action segment interval, and at least one frame of image and ToF distance are acquired to correct the local geometric model or update the subsequent trajectory.

9. The method according to claim 1, characterized in that, The abnormal rollback strategy is triggered when the number of feature matches is insufficient, the reprojection error exceeds the threshold, the point cloud density is lower than the threshold, or the ToF signal is abnormal.

10. The method according to claim 1, characterized in that, The monocular image, ToF distance, and end-effector pose are aligned using a unified timestamp or by interpolation.

Images