Dynamic security domain real-time constraint control method and system for rehabilitation robot

Abstract

The invention relates to the technical field of manipulator control, and discloses a dynamic safety domain real-time constraint control method and system for a rehabilitation robot, and the method comprises the steps: obtaining a real-time joint angular position, a tail end velocity vector and a motor braking torque limit; a joint space inertia matrix and a Jacobian matrix are calculated based on the angular position, and the inertia matrix is mapped to the tail end movement axial direction to determine direction conversion mass; a braking acceleration limit value is calculated according to the converted mass, and braking displacement is predicted; dynamically configuring a momentum envelope surface as a security domain boundary based on a prediction result; according to the method, a correlation mechanism of the tail end motion state and the safety boundary is established, the problem of inertia energy difference generated by configuration anisotropy is solved, safety risks caused by physical overtravel are prevented, and the braking efficiency of the manipulator is adaptively adjusted along with the motion posture.

Description

Technical Field

[0001] This invention relates to a dynamic safety domain real-time constraint control method and system for rehabilitation robots, belonging to the field of robotic arm control technology. Background Technology

[0002] Currently, rehabilitation robots use robotic arms to guide the patient's limbs to perform trajectory movements. Setting static geometric boundaries or virtual walls to restrict the position of the end effector within a safe space is the mainstream practice in the industry, in order to ensure that the patient's limbs do not physically exceed limits.

[0003] While existing technologies utilize mechanical compensation to optimize patient load, significant bottlenecks remain in the dynamic response at the control logic level. For example, Chinese invention patent CN108272602B discloses a suspension weight-reduction device for a lower limb rehabilitation robot, which uses the physical properties of springs to compensate for fluctuations in the patient's center of gravity. This type of solution focuses on load balance in the direction of gravity, neglecting the inertial characteristics of the end effector as its configuration and speed drastically change. This leads to a lack of prediction of braking displacement at high speeds, making it prone to slipping out of safety boundaries due to momentum overload. Dynamic traction and resistance training require the robotic arm to have high-speed trajectory response capabilities, which current position constraint methods... Ignoring the inertial momentum characteristics that change with configuration and speed, the increase in linear velocity leads to an increase in kinetic energy, which lengthens the minimum braking displacement required in sudden situations. This causes the end effector to slip out of the physical safety threshold due to momentum overload after the robot triggers a stop command at high speed. The preset large-area buffer area compresses the robot's stroke in a limited space, which cannot cover the range of physiological activities. Setting a fixed low speed limit results in insufficient training intensity. Affected by the inertial anisotropy of the multi-degree-of-freedom linkage mechanism under different poses, the single-parameter braking strategy has insufficient braking distance under high inertia poses and excessive constraints under low inertia poses.

[0004] Therefore, the technical problem to be solved by this invention is to construct a control mechanism that senses the motion state of the manipulator and synchronously corrects the braking envelope surface to address the contradiction between the real-time nature of safety constraints and the difference in inertia of the mechanism in dynamic tasks, so that the constraint boundary can expand and contract in real time with the dynamic characteristics of the mechanism. Summary of the Invention

[0005] To address the problems mentioned in the background art, the technical solution of the present invention is as follows: A real-time constraint control method for dynamic safety domains of rehabilitation robots, comprising the following steps: Step 101: Obtain the real-time joint angle position of the rehabilitation robot, the instantaneous velocity vector of the end effector, and the rated torque of each drive motor driving the rehabilitation robot, and collect the real-time winding temperature of the drive motor. Step 102: Correct the rated dynamic torque according to the thermal torque attenuation coefficient corresponding to the real-time winding temperature, determine the dynamic torque limit, and update the joint space inertia matrix and kinematic Jacobian matrix of the rehabilitation robot based on the real-time joint angle position. Step 103: Extract the unit direction vector of the instantaneous velocity vector of the end effector, calculate the configuration space mapping mass using the kinematic Jacobian matrix and the joint space inertia matrix, and perform normalized projection of the configuration space mapping mass on the unit direction vector to determine the directional equivalent mass of the end effector under the current motion direction; Step 104: Perform nonlinear mapping operation based on the directional equivalent mass and dynamic torque limit to calculate the braking acceleration limit value of the rehabilitation robot along the instantaneous velocity vector direction of the end effector in the current pose, and perform integral prediction on the instantaneous velocity vector of the end effector based on the braking acceleration limit value to determine the predicted braking displacement. Step 105: Calculate the real-time spatial distance between the end effector and the preset static geometric boundary, and dynamically update the radial scale of the momentum envelope surface based on the topological comparison results of the predicted braking displacement and the real-time spatial distance, and use the momentum envelope surface as the dynamic safety domain boundary of the rehabilitation robot. Step 106: When the real-time pose trajectory of the end effector enters the boundary of the dynamic safety domain, a torque cutoff command is generated and output to the current control loop of the drive motor to force the deceleration action.

[0006] Preferably, the directional equivalent mass determined in step 103 is used to define the difference in inertial energy of the rehabilitation robot in an unstructured training environment due to the anisotropy of the linkage configuration. Step 103 specifically includes: obtaining the inverse matrix of the joint space inertial matrix, performing sequential matrix multiplication on the kinematic Jacobian matrix, the inverse matrix, and the transpose matrix of the kinematic Jacobian matrix to generate the task space inertial mapping matrix, and performing a quadratic normalized projection operation on the task space inertial mapping matrix using the unit direction vector to obtain the directional equivalent mass characterizing the kinetic energy characteristics of the end effector in a specific axis.

[0007] Preferably, the directional equivalent mass follows the calculation rules below: ,in, For directional equivalent mass; It is a unit direction vector; The kinematic Jacobian matrix; Here is the joint space inertia matrix; The transpose matrix of the unit direction vector; Let be the transpose of the kinematic Jacobian matrix; It is the inverse of the joint space inertia matrix.

[0008] Preferably, when updating the kinematic Jacobian matrix in step 102, the determinant of the kinematic Jacobian matrix is ​​calculated in real time, and when the determinant is lower than a preset singular value threshold, a damping least squares correction factor is added to the diagonal elements of the kinematic Jacobian matrix to eliminate the sudden change in joint driving torque when the rehabilitation robot approaches a singular configuration.

[0009] Preferably, the method further includes step 107: real-time monitoring of the feedback current of the drive motor, and constructing an instantaneous interactive power consumption model based on the feedback current; step 107 is performed before step 105, wherein the physiological spasticity characteristics of the controlled limb of the rehabilitation robot are identified based on the instantaneous interactive power consumption model, and the virtual contact stiffness of the momentum envelope surface is linearly increased and adjusted according to the physiological spasticity characteristics.

[0010] Preferably, the radial scale of the momentum envelope in step 105 further includes: setting the predicted braking displacement as the minimum dynamic obstacle avoidance radius of the end effector, and using a unit direction vector to perform a spatial directional extension of the minimum dynamic obstacle avoidance radius, so that the boundary of the dynamic safety domain expands and contracts in real time with the end momentum state of the rehabilitation robot.

[0011] Preferably, the generation of torque cutoff command in step 106 further includes: calculating the coordinated deceleration weight of each drive joint based on the braking acceleration limit value, driving each motion axis of the rehabilitation robot to converge synchronously to zero speed state, so as to maintain the expected trajectory shape of the end effector during braking. When obtaining the real-time winding temperature in step 101, the real-time temperature data is obtained by using a temperature sensor installed inside the drive motor, and the thermal torque attenuation coefficient is queried using a preset thermal drift characteristic curve, so as to compensate the braking ability of the rehabilitation robot under continuous high load operation in real time.

[0012] Preferably, the calculation frequency in step 105 is not less than 1000Hz to ensure that the refresh cycle of the momentum envelope is synchronized with the underlying torque feedback control cycle of the rehabilitation robot at the millisecond level.

[0013] Preferably, after the output torque cutoff command in step 106, the method further includes: blocking the external position given signal during the deceleration action and switching the control law of the drive motor to the virtual impedance control mode, using a preset damping coefficient to absorb the structural impact load generated by the rehabilitation robot during emergency braking.

[0014] A dynamic safety domain real-time constraint control system for rehabilitation robots includes a multi-dimensional parameter acquisition unit, a dynamic model solving module, an equivalent inertia projection module, a braking displacement prediction module, a safety envelope dynamic configuration module, and a conflict constraint execution unit. The multi-dimensional parameter acquisition unit is used to acquire the real-time joint angle position, instantaneous velocity vector of the end effector, rated torque of the drive motor, and real-time winding temperature of the rehabilitation robot. The dynamic model solution module is used to adjust the dynamic torque according to the real-time winding temperature to determine the dynamic torque limit, and to update the joint space inertia matrix and kinematic Jacobian matrix of the rehabilitation robot based on the real-time joint angle position. The equivalent inertia projection module is used to extract the unit direction vector of the instantaneous velocity vector of the end effector, calculate the configuration space mapping mass using the kinematic Jacobian matrix and the joint space inertia matrix, and perform projection operation on the unit direction vector to determine the directional equivalent mass of the end effector under the current motion direction; The braking displacement prediction module is used to perform nonlinear mapping operations based on the directional equivalent mass and dynamic torque limit to solve the braking acceleration limit value, and to perform integral prediction on the instantaneous velocity vector of the end effector based on the braking acceleration limit value to determine the predicted braking displacement. The safety envelope dynamic configuration module is used to calculate the real-time spatial distance between the end effector and the preset static geometric boundary, and dynamically update the radial scale of the momentum envelope surface based on the topological comparison results of the predicted braking displacement and the real-time spatial distance, and use the momentum envelope surface as the boundary of the dynamic safety domain. The conflict constraint execution unit is used to generate a torque cutoff command and output it to the current control loop of the drive motor when the real-time pose trajectory of the end effector enters the boundary of the dynamic safety domain, so as to perform deceleration.

[0015] Compared with the prior art, the beneficial effects of the present invention are: 1. In the dynamic safety domain of the rehabilitation robot, an instantaneous velocity vector-related dynamic momentum envelope of the end effector is established. The predicted braking distance determined based on the physical limit deceleration is introduced into the safety boundary determination process, so that the robot's spatial constraint logic responds to the kinetic energy state of the mechanical structure in real time. This solution solves the risk of physical boundary overtravel caused by inertial impact during high-speed movement of the rehabilitation robot, avoids the lag of dynamic load in the traditional static geometric boundary treatment, and improves the continuity of the robot's motion trajectory by predicting the braking path while ensuring the safety of the patient's limbs.

[0016] 2. By utilizing the configuration space equivalent mass mapping mechanism, the joint space inertia matrix is ​​deeply coupled with the end effector motion direction, enabling the safety envelope to adaptively adjust according to the anisotropy of the mechanism configuration. To address the differences in end effector mass representation under different poses of a multi-degree-of-freedom manipulator, the dynamic momentum envelope surface is expanded and contracted in real time with the manipulator configuration inertial load by updating the real physical limit deceleration in real time. When the robot is in a high inertia configuration, the envelope surface extends to reserve a safe braking path; when in a low inertia configuration, the envelope surface tightens to improve interaction flexibility. This scheme eliminates the risk of control failure caused by the blind zone of mechanism inertia and ensures consistent braking performance throughout the entire workspace.

[0017] 3. The system collects feedback current from the driver to construct an instantaneous interactive power consumption model, establishes a mapping rule for the virtual boundary stiffness to change with the patient's interactive state, realizes endogenous immunity to abnormal stress conflict of physiological spasm, and when the interactive power consumption shows a reverse mutation and the rate of change exceeds the preset threshold, the system reduces the virtual boundary stiffness and adjusts the center coordinates of the safety domain to offset the superposition effect of mechanical constraints and patient spasm stress. Through interactive power consumption calculation at the logic level, the hard constraints are transformed into a physiologically sensitive flexible barrier to avoid secondary limb damage caused by mechanical impact and improve the interactive compliance of rehabilitation training. Attached Figure Description

[0018] Figure 1 This is a closed-loop principle block diagram of the real-time constraint control system for the dynamic safety domain of the rehabilitation robot of the present invention. Figure 2 This is a graph showing the dynamic change characteristics of instantaneous interactive power consumption and the identification curve of spasm features in this invention; Figure 3 This is a flowchart illustrating the logical composition and implementation of the dynamic safety domain real-time constraint control method of the present invention. Detailed Implementation

[0019] The present invention will be further described below with reference to the accompanying drawings and embodiments. The following embodiments are intended to explain the present invention and are not intended to limit the scope of protection of the present invention.

[0020] This invention provides a dynamic safety domain real-time constraint control method and system for rehabilitation robots, including a multi-dimensional parameter acquisition unit, a dynamic model solving module, an equivalent inertia projection module, a braking displacement prediction module, a safety envelope dynamic configuration module, and a conflict constraint execution unit; the multi-dimensional parameter acquisition unit acquires the real-time joint angle positions of the rehabilitation robot. Instantaneous velocity vector of end effector Rated torque of the drive motor and real-time winding temperature To address the issue of thermal attenuation of driving torque caused by temperature rise in rehabilitation robots under continuous high-load operation, the dynamic model solution module calculates the driving torque based on the real-time winding temperature. Corresponding thermal torque attenuation coefficient Corrected custom power torque Determine the dynamic torque limit In the formula This refers to the dynamic torque limit, expressed in Nm. Rated torque, in Nm. Let be the thermal torque attenuation coefficient, and let be the thermal torque attenuation coefficient. The thermal drift characteristic curve is obtained through a preset query; simultaneously, the dynamic model solution module is based on the real-time joint angle position. Update joint space inertia matrix and the kinematic Jacobian matrix The calibration process for the thermal torque attenuation characteristics of the drive motor includes: [the following steps are taken] in the motor test bench... To adjust the ambient temperature gradient, the stall torque output of the motor windings under different temperature gradients is obtained, and the real-time winding temperature is recorded. Compared with the measured output torque Calculate the measured output torque based on the corresponding values. With customized power torque The ratio determines the thermal torque attenuation coefficient. By fitting the data curves at the sampling points using the least squares method, the thermal torque attenuation coefficient is established. With real-time winding temperature The variable analytical function is used by the dynamic model solution module to correct the upper limit of the drive system torque output in real time based on temperature feedback during the continuous operation of the rehabilitation robot.

[0021] Because the inertia of the multi-link mechanism of the robotic arm exhibits anisotropic behavior under different poses, the equivalent inertia projection module extracts the instantaneous velocity vector of the end effector. unit direction vector Using the kinematic Jacobian matrix With joint space inertia matrix Calculate the configuration space mapping quality and assign the configuration space mapping quality to the unit direction vector. Perform projection calculations to determine the directional equivalent mass of the end effector in the current motion direction. ,in The directional equivalent mass is expressed in kg. Unit direction vector, Let Jacobian matrix be the kinematic matrix. Here is the joint space inertia matrix. The transpose matrix of the unit direction vector. Let be the transpose of the kinematic Jacobian matrix. This is the inverse of the joint space inertia matrix; the braking displacement prediction module is based on the directional equivalent mass. With dynamic torque limit Perform nonlinear mapping operations to calculate the instantaneous velocity vector of the rehabilitation robot along the end effector in its current pose. Braking acceleration limit in direction And based on the braking acceleration limit value instantaneous velocity vector of the end effector Perform integral prediction to determine the predicted braking displacement. ,in To predict braking displacement, the unit is meters. This is the instantaneous velocity vector of the end effector, in m / s. This represents the braking acceleration limit, in m / s²; the equivalent inertia projection module receives the dynamic torque limit. The joint space torque constraints are converted into end-effector task space driving force vectors using the kinematic Jacobian matrix transpose matrix pseudo-inverse matrix. , ,in, This represents the task space driving force vector of the end effector in its current pose. The pseudo-inverse matrix is ​​the transpose of the kinematic Jacobian matrix. For dynamic torque limits; the equivalent driving force vector With unit direction vector Dot product operation, the result is equivalent to the direction in terms of mass. Determine the braking acceleration limit value by the quotient , ,in, This represents the physical deceleration limit of the end effector along the current direction of motion. The driving force is the component along the motion axis. To determine the directional equivalent mass, we define the boundary of the anisotropic braking performance of the manipulator under different configurations, which is limited by the driving capability.

[0022] The safety envelope dynamic configuration module calculates the real-time spatial distance between the end effector and the preset static geometric boundary. According to the predicted braking displacement With real-time spatial spacing The topology comparison results dynamically update the radial scale of the momentum envelope surface, using the momentum envelope surface as the boundary of the dynamic safety domain of the rehabilitation robot; the safety envelope dynamic configuration module predicts the braking displacement. Set as the minimum dynamic obstacle avoidance radius for the end effector, using unit direction vector The minimum dynamic obstacle avoidance radius is extended spatially, allowing the dynamic safety domain boundary to expand and contract in real time according to the momentum state of the manipulator's end effector. When the real-time pose trajectory of the end effector enters the dynamic safety domain boundary, the conflict constraint execution unit generates a torque cutoff command and outputs it to the current control loop of the drive motor, forcibly executing a deceleration and shutdown action. During this shutdown process, the system switches the control law of the drive motor to a virtual impedance control mode, using a preset damping coefficient to absorb the structural impact load generated by emergency braking. To mitigate torque abrupt changes in the rehabilitation robot near singular configurations, the dynamic model solution module calculates the kinematic Jacobian matrix in real time. The determinant, when below a preset singular value threshold, is in the kinematic Jacobian matrix. A damping least-squares correction factor is added to the diagonal elements; the system monitors the feedback current of the drive motor in real time, constructs an instantaneous interactive power consumption model based on the feedback current, identifies the physiological spasticity characteristics of the controlled limb through this model, and performs linear incremental adjustment of the virtual contact stiffness of the momentum envelope surface according to the physiological spasticity characteristics; the system combines the feedback current of the drive motor and the motion feedback signal to construct an instantaneous interactive power consumption model, calculates the dot product of the end-effector interaction torque and the real-time angular velocity vector to obtain the instantaneous interactive power consumption. When instantaneous interactive power consumption The value remained negative for five consecutive sampling periods, and the rate of change over time... The absolute value exceeds the spastic activation threshold. When the controlled limb is determined to enter a state of physiological spasticity, the virtual contact stiffness adjustment in the safety domain is triggered. The virtual contact stiffness increases linearly with the rate of change of power consumption to compensate for the loss, and the instantaneous interaction power consumption is monitored. The value returned to the positive range and the duration exceeded Subsequently, the virtual contact stiffness is smoothly converged to the initial reference value using a first-order low-pass filter curve, achieving automatic reset and dynamic unloading of the mechanical constraint strength. Furthermore, the system is based on the braking acceleration limit value. Calculate the coordinated deceleration weights of each drive joint, drive each motion axis to converge synchronously to zero speed, and during braking, obtain the gravity component of the end effector in the current trajectory tangential direction to predict the braking displacement. Asymmetric corrections are performed to compensate for the effect of gravity on braking displacement.

[0023] Example 1: When the temperature of the drive motor windings It is 50.0 At that time, the multi-dimensional parameter acquisition unit queries the thermal drift characteristic curve to determine the thermal torque attenuation coefficient. It is 0.94, combined with the rated torque. 150.0 Nm Calculation of dynamic torque limit The torque is 141.0 Nm. At this point, the end effector is in its fully extended position, and the real-time joint angle position is... Determine the joint space inertia matrix With the kinematic Jacobian matrix The equivalent inertia projection module is based on the instantaneous velocity vector of the end effector. unit direction vector and formula Calculate the equivalent mass in the direction downwards of the current motion. It weighs 12.5 kg. The directional equivalent mass is expressed in kg. Unit direction vector, Let Jacobian matrix be the kinematic matrix. Here is the joint space inertia matrix. The transpose matrix of the unit direction vector. Let be the transpose of the kinematic Jacobian matrix. The inverse of the joint space inertia matrix is ​​used by the braking displacement prediction module to fuse the equivalent mass in this direction. With dynamic torque limit Calculate the braking acceleration limit value under the current pose. It is 11.28 m / s².

[0024] When the instantaneous velocity vector of the end effector The modulus is 1.2 m / s and it moves towards the static geometric boundary to the real-time spatial spacing. When the value is 0.0650m, the braking displacement prediction module is based on the formula Calculate and predict braking displacement It is 0.0638m. To predict braking displacement, the unit is meters. This is the instantaneous velocity vector of the end effector, in m / s. The braking acceleration limit is expressed in m / s². The safety envelope dynamic configuration module compares and predicts the braking displacement. With real-time spatial spacing Due to the predicted braking displacement With real-time spatial spacing The safety margin was reduced to 0.0012m, causing interference between the momentum envelope and the static geometric boundary. The conflict constraint execution unit output a torque cutoff command to the drive motor. By associating the spatial dynamic characteristics of the mechanism configuration with the direction of the motion vector in real time, the system reduces the static geometric redundancy requirement while maintaining the safety redundancy of the braking displacement. This allows the trajectory of the robot end to extend to the physical limit boundary under high-speed conditions, achieving a synchronous match between the utilization rate of the working space and the safety of the dynamic constraint.

[0025] Example 2: This experiment verifies the directional equivalent mass. With dynamic torque limit The accuracy of cooperative braking prediction under multi-configuration and high-temperature conditions was assessed to objectively characterize the space utilization and safety of rehabilitation robots under complex physical constraints. The test platform was built based on a six-degree-of-freedom manipulator dynamics simulation environment, simulating the physical response of the manipulation device by solving the Lagrange dynamics equations. The core computing unit supports a sampling refresh rate of no less than 1000Hz. To verify the stability of the scheme under real sensor noise conditions, the experiment was conducted at the joint angle position. Gaussian white noise with a signal-to-noise ratio of 25dB is superimposed, and high-frequency jitter of the control loop at a frequency of 50Hz is simulated; in the parameter setting logic, the prediction period is... The value of the vector balances the data update frequency and computational resource consumption, when the instantaneous velocity vector of the end effector... When the modulus is not less than 0.5 m / s, in order to ensure the stability of the braking closed loop, the predicted period will be... The time was set to 1ms; the experiment established the sample group, control group 1, and control group 2 of this invention, wherein control group 1 adopted the static safety domain constraint method, and control group 2 ignored the real-time winding temperature when performing directional equivalent mass projection. Correction: The sample group of this invention implements a dynamic constraint method including temperature compensation and mass projection, see Table 1.

[0026] Table 1: Data Recording of Deviations Between End-of-Line Stop Position and Preset Safety Boundary under Different Test Conditions

[0027] Analysis of the data in Table 1 shows that in control group one, insufficient braking torque after motor temperature rise led to an increase in boundary intrusion to 0.065m. In control group two, neglecting thermal decay under high-temperature conditions resulted in a decrease in predicted braking displacement. The small value resulted in a 0.0333m overtravel conflict. The present invention utilizes the thermal torque attenuation coefficient... Real-time correction of dynamic torque limit This results in a boundary intrusion of 20.0. Up to 80.0 Within the range, the value is no greater than 0.001m; to further verify the directional equivalent mass. To assess the optimization effect under nonlinear configuration transformation, a motion intensity gradient system was introduced in the experiment. The braking performance of the manipulator in contracted, transitional, and fully extended poses was tested respectively, as shown in Table 2.

[0028] Table 2: Limits of Braking Acceleration Due to Configuration Anisotropy Table of measurement results of impact

[0029] Orientational equivalent mass The directional equivalent mass exhibits non-linear growth with configuration changes; as the manipulator evolves from a retracted pose to a fully extended pose, the directional equivalent mass... The braking acceleration limit increases from 8.24 kg to 25.32 kg. The velocity decreases from 16.98 m / s² to 5.53 m / s². The system adjusts the radial scale of the momentum envelope in real time according to the inflection point of the dynamic parameters, so as to maintain the workspace utilization rate of the contraction pose at 94.2% while maintaining safety redundancy.

[0030] Example 3: This example combines Figures 1 to 3 This document describes a real-time constraint control method and system for dynamic safety domains in rehabilitation robots. Figure 1 As shown, the patient or controlled limb, as a physiological interaction object, physically interacts with the rehabilitation robot body, which includes motors and sensors. The rehabilitation robot body uploads feedback signals including position, velocity, and temperature to the multi-dimensional state perception and calculation module, which performs operations such as collecting joint angles and end-effector velocities, monitoring the real-time temperature of the motor windings, and updating the inertia matrix and Jacobian matrix. The generated state data stream is then transmitted to the dynamic mass and displacement prediction module. The dynamic mass and displacement prediction module performs operations such as extracting the equivalent mass of the motion direction, correcting the torque limit after thermal decay, and integrally calculating the ultimate braking displacement. The generated prediction result stream is then transmitted to the safety domain construction and constraint module, which performs operations such as dynamically updating the momentum envelope surface scale, comparing the real-time spatial safety distance, and generating torque truncation commands. At the same time, it compares the distance with the static geometric boundary, which acts as a physical wall, and finally feeds back the torque truncation or impedance control signal to the rehabilitation robot body through the control command sending channel.

[0031] like Figure 2 As shown, the chart establishes a coordinate system with time (ms) on the horizontal axis, instantaneous interactive power consumption (W) on the left vertical axis, and power consumption change rate (W / s) on the right vertical axis. It plots the curves of instantaneous interactive power consumption (W), the curves of power consumption change rate (W / s), and the straight line representing the spasmodic activation threshold (W / s), demonstrating the dynamic process on the time axis where the interactive power consumption value decreases while the power consumption change rate suddenly changes and reaches the spasmodic activation threshold; for example... Figure 3As shown in the figure, this diagram illustrates the logical branches for achieving real-time constraints on the dynamic safety domain of the rehabilitation robot. These include a multi-dimensional parameter sensing module that includes real-time joint angle position, motor winding temperature, and end-effector velocity vector; a dynamic calculation module that includes dynamic torque limits, joint space inertia matrix, and thermal torque decay correction; an equivalent inertia projection module that includes unit direction vector extraction, configuration space mapping quality, and configuration anisotropy analysis; a braking displacement prediction module that includes integral prediction of displacement, braking acceleration limit value, and nonlinear mapping calculation; a dynamic envelope configuration module that includes momentum envelope surface generation, real-time spatial spacing comparison, and radial scale update; and a conflict constraint execution module that includes forced deceleration or impedance control, current control loop, and torque truncation commands.

[0032] Example 4: The system performs stall torque calibration to determine the thermal torque attenuation coefficient. With real-time winding temperature The mapping relationship is achieved through the output torque fed back by the external dynamometer and the real-time winding temperature fed back by the internal temperature sensor. Construct a mapping matrix at real-time winding temperature From 20.0 Increment to 100.0 During the process, with 5.0 To record the actual output torque of the motor under rated current and compare it with the rated torque, we need to record the step length. Calculate the ratio to obtain the thermal torque attenuation coefficient. The original sampling points are used to perform cubic polynomial fitting to determine the thermal drift characteristic curve, thereby enabling the dynamic model solution module to calculate the thermal drift characteristic curve based on the real-time winding temperature. Accurately compensate for torque loss caused by changes in motor winding resistance; the system executes instantaneous interactive power consumption model construction, and reads the feedback current of the drive motor with a sampling period of 1ms. This is then converted into total electromagnetic torque, which is achieved by subtracting the gravity term from the joint space dynamics equations in real time. Friction term With inertial torque term Extracting end interaction torque angular acceleration By measuring the joint angle position The system calculates the terminal interaction torque by performing a second numerical derivative and passing the result through a low-pass filter with a cutoff frequency of 50Hz. With real-time angular velocity vector The dot product is used to determine the instantaneous interactive power consumption. The instantaneous interactive power consumption of 5 consecutive sampling points All are negative and the rate of change of power consumption over time When the absolute value exceeds the preset spasm activation threshold of 150W / s, physiological spasm is determined to occur and the virtual contact stiffness of the momentum envelope surface is linearly increased.

[0033] The system executes the singular constraint procedure, and the dynamic model solution module calculates the kinematic Jacobian matrix. Perform singular value decomposition to obtain the minimum singular value. Minimum singular value Compared with the preset singularity threshold Real-time comparison is performed, including the outlier threshold. The value is the kinematic Jacobian matrix. 0.05 times the maximum singular value, at the minimum singular value Less than the singular value threshold At that time, the system is directed to the kinematic Jacobian matrix. Diagonal element injection damping coefficient To construct the damping Jacobian matrix, damping coefficient With minimum singular value Approaching zero, it monotonically increases exponentially until reaching the maximum damping limit. Furthermore, the Jacobian matrix null projection operator is used to apply a nonlinear bias to the velocity vector, guiding the robot to evolve towards a configuration with increased operability while meeting safety constraints.

[0034] Example 5: In deployment scenarios targeting unstructured rehabilitation environments, the system performs homogeneous transformation matrix calibration of static geometric boundaries relative to the manipulator's base coordinate system, and obtains the extreme value edges of the training space by touching them with the end effector. Group location sampling points The pose matrix of the static geometric boundary is solved using the least squares plane fitting method. Determine the spatial topological mapping reference of the physical security area in the base coordinate system, where For the first The coordinate vector of each sampling point. Let be the homogeneous transformation matrix describing the pose of the static geometric boundary; this pose matrix As real-time spatial spacing The calculation reference system compensates for spatial measurement deviations caused by the randomness of the robot's installation posture, so that the radial expansion range of the momentum envelope is aligned in real time with the actual spatial occupancy state of the physical boundary.

[0035] When the rehabilitation robot encounters individual subjects with different limb physical parameters, the system executes a limb dynamics identification procedure, driving the robotic arm to move the limb at a preset angular velocity of 0.2 rad / s. The passive reciprocating motion is executed. The multi-dimensional parameter acquisition unit reads the feedback torque and eliminates the gravitational torque component and frictional torque component of the manipulator body according to the mechanism dynamics model. An identification equation including the limb mass attribute is then established. ,in, Angular acceleration, Given angular velocity, calculate the equivalent moment of inertia of the limb. Compared with the reference static damping ; The equivalent rotational inertia of the limbs is expressed in kg·m². The reference static damping is expressed in N·m·s / rad. For limb interaction torque, The inertial and damping parameters, identified and determined, are incorporated into the instantaneous interactive power consumption model as dynamic zero-point compensation quantities, thereby reducing the impact of torque disturbances caused by uneven limb mass distribution on the accuracy of physiological spasticity feature discrimination.

[0036] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the present invention can be implemented in other specific forms without departing from the spirit or essential characteristics of the present invention.

[0037] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention.

Claims

1. A real-time constraint control method for dynamic safety domains in rehabilitation robots, characterized in that, Includes the following steps: Step 101: Obtain the real-time joint angle position of the rehabilitation robot, the instantaneous velocity vector of the end effector, and the rated torque of each drive motor driving the rehabilitation robot, and collect the real-time winding temperature of the drive motor. Step 102: Correct the rated dynamic torque according to the thermal torque attenuation coefficient corresponding to the real-time winding temperature, determine the dynamic torque limit, and update the joint space inertia matrix and kinematic Jacobian matrix of the rehabilitation robot based on the real-time joint angle position. Step 103: Extract the unit direction vector of the instantaneous velocity vector of the end effector, calculate the configuration space mapping mass using the kinematic Jacobian matrix and the joint space inertia matrix, and perform normalized projection of the configuration space mapping mass on the unit direction vector to determine the directional equivalent mass of the end effector under the current motion direction; Step 104: Perform nonlinear mapping operation based on the directional equivalent mass and dynamic torque limit to calculate the braking acceleration limit value of the rehabilitation robot along the instantaneous velocity vector direction of the end effector in the current pose, and perform integral prediction on the instantaneous velocity vector of the end effector based on the braking acceleration limit value to determine the predicted braking displacement. Step 105: Calculate the real-time spatial distance between the end effector and the preset static geometric boundary, and dynamically update the radial scale of the momentum envelope surface based on the topological comparison results of the predicted braking displacement and the real-time spatial distance, and use the momentum envelope surface as the dynamic safety domain boundary of the rehabilitation robot. Step 106: When the real-time pose trajectory of the end effector enters the boundary of the dynamic safety domain, a torque cutoff command is generated and output to the current control loop of the drive motor to force the deceleration action.

2. The method for real-time constraint control of dynamic safety domain for rehabilitation robots according to claim 1, characterized in that, The directional equivalent mass determined in step 103 is used to define the difference in inertial energy of the rehabilitation robot in an unstructured training environment due to the anisotropy of the link configuration. Step 103 specifically includes: obtaining the inverse matrix of the joint space inertial matrix, performing sequential matrix multiplication of the kinematic Jacobian matrix, the inverse matrix, and the transpose of the kinematic Jacobian matrix to generate the task space inertial mapping matrix, and performing a quadratic normalized projection operation on the task space inertial mapping matrix using the unit direction vector to obtain the directional equivalent mass characterizing the kinetic energy characteristics of the end effector in a specific axis.

3. The method for real-time constraint control of dynamic safety domain for rehabilitation robots according to claim 2, characterized in that, The directional equivalent mass follows the calculation rules below: ,in, For directional equivalent mass; It is a unit direction vector; The kinematic Jacobian matrix; Here is the joint space inertia matrix; The transpose matrix of the unit direction vector; Let be the transpose of the kinematic Jacobian matrix; It is the inverse of the joint space inertia matrix.

4. The method for real-time constraint control of dynamic safety domain for rehabilitation robots according to claim 1, characterized in that, When updating the kinematic Jacobian matrix in step 102, the determinant of the kinematic Jacobian matrix is ​​calculated in real time. When the determinant is lower than the preset singular value threshold, a damping least squares correction factor is added to the diagonal elements of the kinematic Jacobian matrix to eliminate the sudden change in joint driving torque when the rehabilitation robot approaches the singular configuration.

5. A real-time constraint control method for dynamic safety domains of rehabilitation robots according to claim 1, characterized in that, It also includes step 107: real-time monitoring of the feedback current of the drive motor, and constructing an instantaneous interactive power consumption model based on the feedback current; step 107 is executed before step 105, wherein the physiological spasticity characteristics of the controlled limb of the rehabilitation robot are identified based on the instantaneous interactive power consumption model, and the virtual contact stiffness of the momentum envelope surface is linearly increased and adjusted according to the physiological spasticity characteristics.

6. The method for real-time constraint control of dynamic safety domain for rehabilitation robots according to claim 5, characterized in that, Step 105, which dynamically updates the radial scale of the momentum envelope, also includes: setting the predicted braking displacement as the minimum dynamic obstacle avoidance radius of the end effector, and using the unit direction vector to perform spatial directional extension of the minimum dynamic obstacle avoidance radius, so that the boundary of the dynamic safety domain expands and contracts in real time with the end momentum state of the rehabilitation robot.

7. The method for real-time constraint control of dynamic safety domain for rehabilitation robots according to claim 1, characterized in that, The generation of torque cutoff command in step 106 also includes: calculating the coordinated deceleration weight of each drive joint based on the braking acceleration limit value, driving each motion axis of the rehabilitation robot to converge synchronously to zero speed state, so as to maintain the expected trajectory shape of the end effector during braking. When obtaining the real-time winding temperature in step 101, the temperature sensor installed inside the drive motor is used to obtain the real-time temperature data, and the thermal torque attenuation coefficient is queried using the preset thermal drift characteristic curve, so as to compensate the braking ability of the rehabilitation robot under continuous high load operation in real time.

8. The method for real-time constraint control of dynamic safety domain for rehabilitation robots according to claim 1, characterized in that, The calculation frequency in step 105 is no less than 1000Hz to ensure that the refresh cycle of the momentum envelope is synchronized with the underlying torque feedback control cycle of the rehabilitation robot at the millisecond level.

9. A real-time constraint control method for dynamic safety domains of rehabilitation robots according to claim 1, characterized in that, Step 106, after the output torque cutoff command, also includes: during the deceleration action, blocking the external position given signal and switching the control law of the drive motor to virtual impedance control mode, using the preset damping coefficient to absorb the structural impact load generated by the rehabilitation robot during emergency braking.

10. A dynamic safety domain real-time constraint control system for rehabilitation robots, used to implement the dynamic safety domain real-time constraint control method for rehabilitation robots as described in claim 1, characterized in that, The system includes a multi-dimensional parameter acquisition unit, a dynamic model solution module, an equivalent inertia projection module, a braking displacement prediction module, a safety envelope dynamic configuration module, and a conflict constraint execution unit. The multi-dimensional parameter acquisition unit is used to acquire the real-time joint angle position, instantaneous velocity vector of the end effector, rated torque of the drive motor, and real-time winding temperature of the rehabilitation robot. The dynamic model solution module is used to adjust the dynamic torque according to the real-time winding temperature to determine the dynamic torque limit, and to update the joint space inertia matrix and kinematic Jacobian matrix of the rehabilitation robot based on the real-time joint angle position. The equivalent inertia projection module is used to extract the unit direction vector of the instantaneous velocity vector of the end effector, calculate the configuration space mapping mass using the kinematic Jacobian matrix and the joint space inertia matrix, and perform projection operation on the unit direction vector to determine the directional equivalent mass of the end effector under the current motion direction; The braking displacement prediction module is used to perform nonlinear mapping operations based on the directional equivalent mass and dynamic torque limit to solve the braking acceleration limit value, and to perform integral prediction on the instantaneous velocity vector of the end effector based on the braking acceleration limit value to determine the predicted braking displacement. The safety envelope dynamic configuration module is used to calculate the real-time spatial distance between the end effector and the preset static geometric boundary, and dynamically update the radial scale of the momentum envelope surface based on the topological comparison results of the predicted braking displacement and the real-time spatial distance, and use the momentum envelope surface as the boundary of the dynamic safety domain. The conflict constraint execution unit is used to generate a torque cutoff command and output it to the current control loop of the drive motor when the real-time pose trajectory of the end effector enters the boundary of the dynamic safety domain, so as to perform deceleration.

Images