Massage control method, device and system of mechanical arm and storage medium

Abstract

The invention provides a massage control method, device and system of a mechanical arm and a storage medium, a massage head is arranged at the tail end of the mechanical arm, and the massage head is provided with a six-dimensional sensor to collect contact force data in real time; the method comprises the following steps: according to a trapezoidal speed curve corresponding to a Cartesian space generated according to a preset massage track, obtaining a target pose and a target force at a next moment; combining the six-dimensional actual force and the actual pose at the current moment; adopting an admittance model to obtain an expected acceleration deviation at the next moment so as to obtain a pose correction amount, setting the rigidity of the admittance model in the Z-axis direction to be 0, and synchronizing a Z-axis dynamic behavior with the trapezoidal speed curve; and the target pose at the next moment is corrected according to the pose, the corrected expected pose is generated, the joint angle of the expected pose is solved through inverse kinematics, and the mechanical arm is driven. According to the invention, the vertical accurate force control and the horizontal adaptive compliance are realized by compensating the pose through a trapezoidal speed curve and combining a rigidity separation strategy.

Description

TECHNICAL FIELD

[0001] Embodiments of the present disclosure relate to the technical field of robotics, and in particular to a massage control method, device, system and computer readable storage medium of a robot arm. BACKGROUND

[0002] Current massage devices mostly use fixed trajectories or simple force feedback control, which has the following problems:

[0003] 1. Insufficient force control accuracy: traditional methods are difficult to adjust the massage force in real time, resulting in uneven force and affecting comfort;

[0004] 2. Poor flexibility: the mechanical arm is prone to impact when contacting the human body due to rigid control, which poses a safety hazard;

[0005] 3. Difficulty in achieving tremor effect: existing technologies achieve tremor through high-frequency vibration motors, but the response speed is slow and cannot be coordinated with the massage trajectory;

[0006] 4. Poor coordination of degrees of freedom: lack of dynamic adjustment strategy in multi-degree-of-freedom control, which cannot adapt to complex human body curves.

[0007] For example, Chinese invention patent CN202011474070.2 discloses a pelvic conditioning massage device and massage method, which uses a preset trajectory and constant force control for massage. The massage robot lacks real-time force feedback. Existing compliant control algorithms are mostly based on joint space, making it difficult to achieve accurate force tracking in Cartesian space. SUMMARY

[0008] Embodiments of the present disclosure aim to provide a massage control method, device, system and computer readable storage medium of a robot arm, thereby solving the aforementioned problems in the prior art.

[0009] To achieve the above-mentioned purpose, the technical solutions adopted by the embodiments of the present disclosure are as follows:

[0010] In one aspect, the embodiments of the present disclosure provide a massage control method of a robot arm, the robot arm is provided with a massage head at the end, the massage head is provided with a six-dimensional sensor to collect contact force data in real time; the method comprises:

[0011] According to the corresponding trapezoidal velocity curve of the Cartesian space generated by the preset massage trajectory of the target massage area, the next time t+1 target pose and target force are obtained;

[0012] Obtain the current time t six-dimensional actual force / torque data and actual pose of the massage head in the Cartesian space;

[0013] Adopting a mobility model, according to the target pose at the next moment t+1, the target force, the six-dimensional actual force / torque at the current moment t and the actual pose data, a desired acceleration deviation at the next moment t+1 is obtained, wherein the mobility model sets the stiffness in the Z-axis direction to 0, and the Z-axis dynamic behavior is synchronized with a trapezoidal velocity curve;

[0014] According to the desired acceleration deviation, a pose correction amount is obtained, the target pose at the next moment t+1 is adjusted, and a corrected desired pose is generated;

[0015] The joint angles of the desired pose are solved through inverse kinematics to drive the robot arm.

[0016] Optionally, the trapezoidal velocity curve includes a slow-fast-slow change process, and the Z-axis dynamic behavior includes a light-heavy-light change process.

[0017] Optionally, when the massage head moves to the lowest position of the trajectory along the Z-axis during the massage, the virtual impedance force F imp in the Cartesian space is calculated in real time through the mobility model, wherein the impedance force component F imp,xy in the horizontal direction, i.e., the X / Y-axis plane.

[0018] If F imp，xy ≥ R1, R1 is a first resistance threshold, then ΔF = K p · F imp，xy is used to dynamically increase the Z-axis target force, K p is a proportional gain.

[0019] If F imp，xy ≥ R2, R2 is a second resistance threshold, then ΔF' = K p '· F imp，xy is used to correct the Z-axis target force, wherein Kp' > Kp.

[0020] Optionally, after the six-dimensional actual force / torque data and the actual pose of the massage head in the Cartesian space at the next moment t+1 are obtained, the method further includes:

[0021] According to a predetermined rotation matrix, the six-dimensional actual force / torque matrix, the actual motion amount, the target force and the target motion amount are rotated to the force control coordinate system, i.e., the Z-axis is aligned with the target force direction, and the rotation matrix is calculated according to the target force direction, wherein the motion amount includes acceleration, velocity and pose.

[0022] Optionally, the mobility model is adopted to obtain the desired acceleration deviation at the next moment t+1 according to the target pose at the next moment t+1, the target force, the six-dimensional actual force / torque at the current moment t and the actual pose data, including:

[0023] According to the next moment t+1 target pose, target force, the current moment t six-dimensional actual force / torque and actual pose, respectively, get the current moment t actual force and the next moment t+1 target force difference, the current moment t actual velocity and the next moment t+1 target velocity difference and the current moment t actual pose and the next moment t+1 target pose difference;

[0024] Using the admittance model According to the current moment t actual force and the next moment t+1 target force difference F e t , the current moment t actual velocity and the next moment t+1 target velocity difference And the current moment t actual pose and the next moment t+1 target pose difference X e t , get the next moment t+1 expected acceleration deviation

[0025] Optionally, according to the expected acceleration deviation, the pose correction amount is obtained, the target pose of the next moment t+1 is adjusted, and the corrected expected pose is generated, including:

[0026] Using the formula: The pose correction amount is calculated To prevent the integral cumulative error of Calculation ; wherein, P a t Indicates the actual value of the current moment t point position, that is, the actual pose, P d t+1 Indicates the next moment t+1 target pose, V d t+1 Indicates the next moment t+1 target velocity, V a t Indicates the actual velocity of the current moment t.

[0027] According to the pose correction amount Adjust the next moment target pose P d t+1 , generate the corrected expected pose P d t+2 ,

[0028] Optionally, if the last moment six-dimensional actual force / torque matrix, actual motion amount and the current moment target force, target motion amount are rotated to the force control coordinate system according to the predetermined rotation matrix, the rotated last moment actual force and the current moment target force difference, actual velocity and target velocity, actual pose and target pose difference are obtained.

[0029] The obtained difference data is input into an admittance model to obtain a rotated next-time t+1 expected acceleration deviation, and then to obtain a pose correction amount;

[0030] The pose correction amount is inversely rotated to adjust the target pose at the next time to obtain a corrected expected pose.

[0031] In another aspect, the massage control device of the mechanical arm is provided, the massage head is arranged at the end of the mechanical arm, and the massage head is provided with a six-dimensional sensor to collect contact force data in real time.

[0032] The massage trajectory planning module is configured to generate a corresponding trapezoidal velocity curve in the Cartesian space according to a preset massage trajectory of the target massage area, to obtain a next-time t+1 target pose and a target force.

[0033] The six-dimensional force acquisition module is configured to acquire six-dimensional actual force / torque data and an actual pose of the massage head in the Cartesian space at the current time t.

[0034] The expected acceleration deviation calculation module is configured to use an admittance model to obtain a next-time t+1 expected acceleration deviation according to the next-time t+1 target pose, the target force, the six-dimensional actual force / torque at the current time t, and the actual pose data, wherein the admittance model sets the stiffness in the Z-axis direction to 0, and the Z-axis dynamic behavior is synchronized with the trapezoidal velocity curve.

[0035] The pose correction module is configured to obtain a pose correction amount according to the expected acceleration deviation, to adjust the target pose at the next-time t+1, and to generate a corrected expected pose.

[0036] The inverse kinematics module is configured to solve the joint angle of the expected pose through inverse kinematics to drive the mechanical arm.

[0037] In another aspect, the massage control system of the mechanical arm is provided, the system includes a mechanical arm, a massage head arranged at the end of the mechanical arm, and a controller applied to the mechanical arm, the massage head is provided with a six-dimensional sensor to collect contact force data in real time; and the controller is configured to execute the steps of the method as described above.

[0038] In another aspect, the computer readable storage medium is provided, and a computer program is stored on the computer readable storage medium, the computer program is executed by a processor to implement the steps of the method as described above.

[0039] The beneficial effects of the embodiments of the present disclosure are as follows:

[0040] The method of this disclosure achieves precise force control in the vertical direction and adaptive compliance in the horizontal direction through a stiffness separation strategy. Based on rapid force value adjustment rather than mechanical vibration, it improves response speed and controllability. The synchronous design of trapezoidal acceleration / deceleration and force value curve enhances the massage rhythm and comfort. The application of the Cartesian space admittance impedance algorithm solves the posture coupling problem in joint space control. Force direction control dynamically adjusts the force control direction through a rotation matrix to adapt to complex massage needs. Attached Figure Description

[0041] Figure 1 This is a schematic flowchart of a massage control method for a robotic arm according to an embodiment of the present disclosure;

[0042] Figure 2 This is a schematic diagram of the structure of a massage control device for a robotic arm according to an embodiment of this disclosure;

[0043] Figure 3 This is a schematic diagram of the collaborative process between components of a massage control device for a robotic arm according to an embodiment of this disclosure;

[0044] Figure 4 This is a schematic diagram of the architecture of a massage control system for a robotic arm according to an embodiment of this disclosure. Detailed Implementation

[0045] To make the objectives, technical solutions, and advantages of the embodiments of this disclosure clearer, the embodiments of this disclosure will be further described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are merely illustrative of the embodiments of this disclosure and are not intended to limit the embodiments of this disclosure.

[0046] Example 1, as Figure 1 As shown, this disclosure proposes a massage control method for a robotic arm, applied to a back massage scenario. The robotic arm's end effector is equipped with a massage head, which has a six-dimensional sensor to collect contact force data in real time. The method includes:

[0047] Step S100: Based on the Cartesian space corresponding trapezoidal velocity curve generated by the preset massage trajectory of the target massage area, obtain the target pose and target force at the next moment t+1.

[0048] The massage head at the end of the mechanical arm is provided with a six-dimensional force sensor (Force / Torque Sensor, F / T Sensor) to collect contact force data in real time; the mechanical arm is a six-axis collaborative mechanical arm with a load of 10 kg; the six-dimensional force sensor has a range of ±200 N. The trajectory planning module stores preset trajectories of each massage area, including a travel trajectory and a massage trajectory; a trapezoidal velocity curve is generated according to the massage trajectory in the trajectory planning module, and the massage trajectory is expressed in the Cartesian coordinates with the center of the robot base as the origin, that is, the base coordinates.

[0049] The massage trajectory is the trajectory of the end of the mechanical arm, and the target value P of the massage trajectory at the next time point t+1 is obtained from the trajectory planning module. d t+1 That is, the target pose P d t+1 =[x d t+1 y d t+1 z d t+1 r xd t+1 r yd t+1 r zd t+1 T , x d t+1 , y d t +1 , z d t+1 is the target relative position of the end of the mechanical arm to the base of the mechanical arm at the next time point t+1, that is, a position vector with a size of 3x1; r xd t+1 , r yd t+1 , r zd t+1 represents the target relative attitude of the end of the mechanical arm to the base of the mechanical arm, which is a 3x1 vector. The derivative of P d t+1 is the velocity, and the second derivative is the acceleration, V d t+1 represents the target velocity of the massage head at the next time point t+1, and A d t+1 represents the target acceleration of the massage head at the next time point t+1. The end of the mechanical arm can be regarded as the massage head.

[0050] R t+1 is the rotation matrix at the next time point t+1, which is consistent with the output direction of the force, when the direction of the force is perpendicular to the Z axis of the Cartesian coordinate system,​ When the direction of the force needs to be rotated, wherein, R t+1 is a rotation matrix of the end of the mechanical arm to the base coordinate of the mechanical arm, that is, the posture (orientation) of the end of the mechanical arm at the next time t+1 relative to the base coordinate system, and the size is 3x3. The rotation matrix R t+1 is calculated according to the target force direction, and the specific calculation method is not described here.

[0051] Taking a back massage scene as an example, the back has multiple acupoints, each acupoint is a certain distance apart, forming a massage head travel trajectory, and the massage head moves to the corresponding acupoint according to the travel trajectory and massages the acupoint according to the pre-set massage trajectory. In order to simulate the professional masseur's technique and optimize the user experience, the massage method is designed to change the massage speed and force value cooperatively, that is, for an acupoint, a massage effect with cooperative change of massage speed and force value is set. The trajectory of the massage head is converted into coordinates in the Cartesian space, and a trapezoidal velocity curve in the Cartesian space is generated according to the massage trajectory. According to the trapezoidal velocity curve, the target pose, target speed, target acceleration and target force F d t+1 at each time are obtained. d t+1 It can be set in trajectory planning or pre-set by an independent force control module.

[0052] Step S200: acquiring six-dimensional actual force / torque data and actual pose in the Cartesian space at the current time t according to the six-dimensional force sensor. The actual pose P a The joint angle can be obtained by the joint sensor of the mechanical arm, and the forward kinematics is used.

[0053] The six-dimensional contact force data collected by the embodiment of the present disclosure is converted into Cartesian space coordinates. The actual pose P a The velocity can be obtained by derivation, P a t = [x a t y a t z a t r xa t r ya t r za t ] T The acceleration can be obtained by twice derivation, V a t , V d t+1 , A a t, A d t+1 Dimension size is same as P a t Dimension size is same as V a t A a t A

[0054] F a = [f xa f ya f za m xa m ya m za ] T , F a A xa , f ya , f za m xa , m ya , m za F d t+1 F a t F e t F a t F d t+1 F e t , F d t +1 , F a t Dimension size is same as P

[0055] When the direction of force needs to be rotated, because the rotation matrix size is 3x3; Therefore, F e t , F a t , F d t+1 is split into two 3x1 column vectors, for example, F d t+1 = [0:3] = [fxd t+1 f yd t+1 f zd t+1 ] T , F d t+1 = [3:6] = [m xd t+1 m yd t +1 m zd t+1 ] T , F e t [0:3], F d t+1 [0:3] and F a t [0:3] have the same dimension size, [3:6] is the same, at this time, F e t [0:3] = R t+ 1 F a t [0:3] - R t+1 F d t+1 [0:3], F e t [3:6] = R t+1 F a t [3:6] - R t+1 F d t+1 [3:6]; when the direction of the force is perpendicular to the Z axis of the Cartesian coordinate system, at this time, R t+1 is a unit matrix, so F e t [0:3] = F a t [0:3] - F d t+1 [0:3], so, F e t [3:6] = F a t [3:6] - F d t+1 [3:6], so, F e t = F a t - F d t+1 .

[0056] The embodiment of the present disclosure adopts force control as single-axis force control (Z-axis), and after step S200, when the direction of force needs to be changed, a rotation matrix about the force direction is multiplied on the left of the six-dimensional force matrix, the six-dimensional position matrix, the velocity matrix and the acceleration matrix to change the direction of the force control Z-axis relative to the inertial space. The rotation matrix R t The target force direction is calculated. By combining the rotation matrices around different axes, force control adjustment in any direction can be achieved.

[0057] That is, according to the predetermined rotation matrix, the six-dimensional force / torque actual data is rotated to the force control coordinate system, that is, the Z-axis is aligned with the target force direction, and the rotation matrix is calculated according to the target force direction.

[0058] The target force F d t+1 is the expected value preset by the user, but its direction is usually defined in the original base coordinate system. When the force control coordinate system is rotated, it needs to be rotated to the new coordinate system synchronously. The target pose, the target velocity and the target acceleration are in the Cartesian coordinate system, so they need to be rotated to the new coordinate system.

[0059] In step S300, a mobility model is adopted to obtain the expected acceleration deviation at the next moment t+1 according to the target pose, the target force at the next moment t+1, the six-dimensional actual force / torque at the current moment t and the actual pose data, wherein the mobility model sets the stiffness in the Z-axis direction to 0, and only the virtual mass M d，z and the damping D d，z respond to the change of force to ensure that the dynamic behavior of the Z-axis is synchronized with the trapezoidal velocity curve.

[0060] The trapezoidal velocity curve includes a slow-fast-slow change process, that is, the trapezoidal acceleration / deceleration trajectory: the massage path speed changes in the mode of “slow-fast-slow”, the acceleration: 0.1 m / s 2 , the maximum speed: 0.5 m / s; the Z-axis dynamic behavior includes a light-heavy-light change process, and the Z-axis force value curve: the force value changes in the mode of “light-heavy-light” and is synchronized with the speed. The force response matches the speed stage, and the M d，z , D d，z parameters need to be designed to realize dynamic following. The time is strictly synchronized, and the force control frequency needs to be greater than or equal to the trajectory update frequency. The vibration effect is realized by quickly adjusting the Z-axis target force value, the frequency: 5-10 Hz, the amplitude: ±10% of the target force value. That is, the contact force increases in the acceleration stage, and there is a vibration effect when the Z-axis force value is quickly adjusted. The period of the slow-fast-slow of the massage path speed is 20 ms to 50 ms.

[0061] Z-axis independent force control: set the Z-axis stiffness to 0, and realize the accurate adjustment of the vertical massage force value through the force control mode, the range is 0.1N-150N; the rest five degrees of freedom compliant control: simulate a second-order system, mass-damping-spring model, so that it has the compliant property, the parameters are adjustable, the stiffness is 50-200N / m, and the damping ratio is 0.6-0.8. According to the massage demand, such as the human body back curve, the rotation matrix R is calculated, the force control direction is adjusted, and the force control direction is perpendicular to the tangent of the human body back curve.

[0062] The admittance model is used to obtain the next moment t+1 expected acceleration deviation according to the next moment t+1 target pose, target force, current moment t six-dimensional actual force / torque and actual pose data, including:

[0063] In step S310, the difference between the current moment t actual force and the next moment t+1 target force, the difference between the current moment t actual speed and the next moment t+1 target speed, and the difference between the current moment t actual pose and the next moment t+1 target pose are obtained respectively according to the next moment t+1 target pose, target force, current moment t six-dimensional actual force / torque and actual pose.

[0064] The difference between the current moment t actual force and the next moment t+1 target force F e t a t d t+1 The difference between the current moment t actual pose and the next moment t+1 target pose X e t e t a t d t+1 Therefore, the difference between the current moment t actual speed and the next moment t+1 target speed Therefore, the difference between the current moment t actual acceleration and the next moment t+1 target acceleration

[0065] In step S320, the admittance model is used According to the difference between the current moment t actual force and the next moment t+1 target force F e t The difference between the current moment t actual speed and the next moment t+1 target speed And the difference between the current moment t actual pose and the next moment t+1 target pose X e t The next moment t+1 expected acceleration deviation is obtained

[0066] ​​​​​Admittance model In the admittance model, X e t is the difference between the actual pose at the current time t and the target pose at the next time t+1, which is also the look-ahead pose error, so X e t = P a t -P d t+1 Therefore, X Therefore, X Substituting The target acceleration error at the next time t+1 can be obtained as is the difference between the expected actual acceleration of the next period and the target acceleration at the next time t+1. K is a diagonal matrix of stiffness coefficients, B is a diagonal matrix of damping coefficients, and M is a diagonal matrix of inertia coefficients. K, B, and M are all 6x6 diagonal matrices, and M, B, and k are pre-set parameters.

[0067] In step S400, the pose correction amount is obtained according to the expected acceleration deviation, the target pose at the next time t+1 is adjusted, and the corrected expected pose is generated.

[0068] According to Because X e t = P a t -P d t+1 Therefore, X e t+1 = P a t+1 -P d t+2 At this time, P d t+2 represents the target point position at the next time t+2 obtained by the admittance model, that is, the target pose, which is the value required, that is, the target point position of the massage head running to the next time of the next period.

[0069] Because P d t+1 represents the target value of the point position at the next time t+1, that is, the target pose, which is obtained by the trajectory planning module; P a t represents the actual value of the point position at the current time t, that is, the actual pose, which is obtained by forward kinematics; therefore, let P a t+1 = P d t+1 , calculate P d t+2 , and P dt+2 The value is output to the inverse kinematics module of the robotic arm. After inverse kinematics solution, the robotic arm moves to that point. Calculation At this time, to prevent the cumulative error of integration, it can be... The formula can be rewritten as follows: d represents the velocity deviation value at the next moment. t Indicates the control period. This represents the speed deviation value at the current moment, newly calculated within the control cycle. With X e t It is built upon the foundation.

[0070] After step S200, if the six-dimensional actual force / torque matrix, actual motion quantity, and target force and target motion quantity of the previous moment are rotated to the force control coordinate system according to the predetermined rotation matrix, the difference between the actual force of the previous moment and the target force of the current moment, the difference between the actual velocity and the target velocity, and the difference between the actual pose and the target pose are obtained after rotation.

[0071] The obtained difference data are input into the admittance model to obtain the expected acceleration deviation at the next time t+1 after rotation, and then the pose correction amount is obtained.

[0072] The pose correction amount is rotated inversely to adjust the target pose at the next moment, thus obtaining the corrected desired pose.

[0073] Specifically, the aforementioned admittance model and calculation When the direction of the force is perpendicular to the coordinate system of the robotic arm, the formula is rewritten as follows: P a t =[0:3]=[x a t y a t z a t ] T ;P a t =[3:6]=[r xa t r ya t r za t ] T ;P d t +1 Also with P a t Similarly, it is split into two 3×1 vectors: position and orientation; at this point,

[0074] X et = [0:3] = R t+1 P a t [0:3] - R t+1 P d t+1 [0:3], X e t = [3:6] = R t+1 P a t [3:6] - R t+1 P d t+1 [3:6];

[0075]

[0076] Substitute the above parameters into the admittance model to calculate the expected pose deviation value X of the next time t+1 after rotation e t+1 ,

[0077] Since P needs to be calculated in the base coordinate system of the robot arm d t+2 At this time, R t -1 is the inverse matrix.

[0078] Step S500, solve the joint angle of the expected pose through inverse kinematics, and drive the robot arm.

[0079] Z-axis force control, the last step only makes the end have a soft nature, when setting the stiffness parameter of the Z-axis to 0, At this time, Wherein, The second derivative is the deviation value of the Z direction acceleration after the admittance calculation; is the deviation value of the Z direction force; is the actual force in the Z direction; is the target force in the Z direction; The formula shows that when the parameter is changed, the actual force will tend to the target force.

[0080] Suppose the robot arm end contacts the environment, that is, the robot arm end contacts the back.

[0081] The environment model is F=k env x e , is the depth of intrusion into the environment.

[0082] Combined with the admittance control formula (k=0) Substitute F=k env xe , because Substituting, we can obtain Because the inertial term is much smaller than the damping term, and the end effector of the robotic arm moves at a low speed (acceleration). At this point, the formula degenerates into... Solving the equation, we get: As t→∞, the corresponding contact force is F=k env x e =F T .

[0083] In conclusion, with stiffness k=0, removing the position feedback term reveals that the control is dominated by the damping term B, ultimately resulting in a certain velocity. At that time, the contact force stabilizes at F t Therefore, adjust F. t The magnitude of the contact force can be adjusted (k = 0).

[0084] Using the robotic arm massage control method of this disclosure embodiment, a real massage test was conducted on a user. The test results were as follows: force control accuracy: ±1.5N (target force 20N); vibration frequency: user satisfaction increased by 30% at 8Hz; force direction adjustment response time: <100ms.

[0085] The robotic arm massage control method of this disclosure adopts a combination of independent Z-axis force control and compliant degree of freedom: through a stiffness separation strategy, it achieves precise force control in the vertical direction and adaptive compliance in the horizontal direction; a dynamic vibration generation mechanism: based on rapid force value adjustment rather than mechanical vibration, it improves response speed and controllability; the synchronous design of trapezoidal acceleration and deceleration and force value curve enhances the massage rhythm and comfort; the application of the Cartesian space admittance impedance algorithm solves the pose coupling problem in joint space control; and force direction control: by dynamically adjusting the force control direction through a rotation matrix, it adapts to complex massage needs.

[0086] The robotic arm massage control method of this disclosure has the following advantages over the prior art: High-precision force control: Through zeroing the Z-axis stiffness and real-time feedback, the force control error is ≤±2% (compared to ±10% in traditional methods); Compliance and safety: Second-order system simulation reduces contact force impact by 40%, adapting to complex human body surfaces; Natural vibration effect: Rapid force adjustment (response time <50ms) achieves human hand-like vibration; Flexible adjustment of force direction: Force control in any direction is achieved through a rotation matrix, improving massage adaptability; Optimized user experience: Coordinated changes in speed and force simulate the techniques of a professional massage therapist.

[0087] like Figure 2 As shown, another aspect of this disclosure provides a massage control device for a robotic arm, wherein a massage head is provided at the end of the robotic arm, and the massage head is equipped with a six-dimensional sensor to collect contact force data in real time;

[0088] The massage trajectory planning module 100 is configured to generate a corresponding trapezoidal velocity curve in the Cartesian space according to a preset massage trajectory generated according to a target massage area, to obtain a target pose at a next time t+1 and a target force.

[0089] The six-dimensional force acquisition module 200 is configured to acquire six-dimensional actual force / torque data and an actual pose of the massage head in the Cartesian space at a current time t.

[0090] The desired acceleration deviation calculation module 300 is configured to obtain a desired acceleration deviation at the next time t+1 by using an admittance model according to the target pose at the next time t+1, the target force, the six-dimensional actual force / torque at the current time t, and the actual pose data, wherein the admittance model is configured to set the stiffness in the Z-axis direction to 0 and synchronize the Z-axis dynamic behavior with the trapezoidal velocity curve.

[0091] The corrected pose module 400 is configured to obtain a pose correction amount according to the desired acceleration deviation, adjust the target pose at the next time t+1, and generate a corrected desired pose.

[0092] The inverse kinematics module 500 is configured to solve joint angles of the desired pose by inverse kinematics to drive the robot arm.

[0093] The massage control device of the robot arm in the embodiment of the present disclosure further comprises a control module, a trajectory planning module, and a force control module. The trajectory planning module stores a preset massage trajectory, and the force control module controls the Z-axis force value. The method in the embodiment of the present disclosure realizes the compliant massage control of the six-axis robot arm based on the end six-dimensional force sensor data, in combination with the admittance impedance algorithm and the trapezoidal acceleration and deceleration planning. The admittance impedance algorithm is applied in the Cartesian space to dynamically adjust the pose of the robot arm. The robot arm control module controls the movement of the robot arm and outputs the current pose, velocity, and acceleration of the robot arm to the force control module (corrected pose).

[0094] As shown in FIG. 1, Figure 3 The modules and components in the massage control device of the robot arm in the embodiment of the present disclosure work cooperatively to realize the massage operation of the robot arm. The control module receives sensor data, calculates admittance impedance output, and generates joint instructions; the trajectory planning module and the force control module work cooperatively to ensure the smooth movement and the force tracking accuracy.

[0095] In another aspect, the present disclosure provides a massage control system of a robot arm, which comprises a robot arm, a massage head arranged at the end of the robot arm, and a controller applied to the robot arm. The massage head is provided with a six-dimensional sensor to collect contact force data in real time. The controller is configured to execute the steps of the method as described above.

[0096] As shown in FIG. 1, Figure 4As shown, the system further comprises an industrial computer, a depth camera and a touch display screen; a mechanical arm controller controls the movement of the six-axis mechanical arm, the controller is connected to the industrial computer, the six-axis force sensor is connected to the industrial computer, the industrial computer is provided with a touch display screen for operation, and the mechanical arm is further provided with a depth camera, and the information collected by the depth camera is transmitted back to the industrial computer in real time.

[0097] In another aspect, the embodiment of the present disclosure provides a computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to implement the steps of the method.

[0098] In embodiment two, on the basis of embodiment one, an adaptive adjustment mechanism according to skin tension is arranged, when the massage head moves to the lowest position of the track along the Z axis during the massage, the virtual impedance force F imp in the Cartesian space is calculated in real time through the admittance model, the horizontal impedance force component is selected, and for the horizontal impedance force selection position, the massage position with horizontal movement can be selected according to the preset massage mode, wherein the horizontal impedance force component is the impedance force component F imp,xy in the X / Y axis plane.

[0099] If F imp，xy ≥ R1, R1 is a first resistance threshold, then ΔF = K p · F imp，xy is adjusted, K p is a proportional gain, and R1 > 0.

[0100] If F imp，xy ≥ R2, R2 is a second resistance threshold, then overload protection is triggered, and ΔF' = K p '· F imp，xy is adjusted, wherein Kp' > Kp and R2 > R1.

[0101] Taking the massage of the back as an example, the human back is divided into normal thickness, thicker back and particularly thick back, or the back muscle is stiff to cause an abnormally high resistance, when the massage is performed by using the method of the embodiment of the present disclosure, the Z axis force value can be automatically adjusted for the massage of the back with different thicknesses, so as to improve the user experience; the resistance value F imp is a virtual impedance force generated by the admittance model, and the dynamic adjustment logic is that the adjustment amplitude is graded and improved with the increase of the virtual resistance, and the highest grade triggers a safety limiting amplitude. The horizontal virtual impedance is mapped to the Z axis force adjustment amount through the proportional gain K p , and the coupling relationship between the models is embodied.

[0102] a) Before the massage starts, the initial thickness D of the back is measured by the Z axis displacement sensor of the mechanical arm;

[0103] b) The first resistance threshold R1 and the second resistance threshold R2 are dynamically set, and satisfy:

[0104] R1 = R 10 + γ · D, where R 10 ∈ [5, 10] N is a reference threshold, γ ∈ [0.5, 1.5] N / cm is a thickness coefficient;

[0105] R2 = 1.5 · R1 + δ · D, δ ∈ [0.8, 2.0] N / cm;

[0106] c) the proportional gain K p positively with thickness: Kp = Kp0 · (D / D0), D0 is a standard thickness 5 cm, K p0 p0 = 0.5, and K imp,xy ∈ [0.2, 0.5].

[0107] The overload protection includes:

[0108] When F max ≥ R2, limit the maximum correction force ΔF imp,xy ' on Z axis = η · D, η ∈ [3, 6] N / cm;

[0109] When the over-threshold time t > t0, trigger the emergency retraction command, t0 = 0.5 s.

[0110] The above only is the preferred implementation of the embodiments of the present disclosure, it should be pointed out that, for the ordinary skilled in the art, without departing from the principles of the embodiments of the present disclosure, can also make several improvements and refinements, these improvements and refinements should also be regarded as the protection scope of the embodiments of the present disclosure.

Claims

1. A massage control method of a robot arm, characterized by, The robotic arm is equipped with a massage head at its end, and the massage head is equipped with a six-dimensional sensor to collect contact force data in real time; the method includes: Based on the Cartesian space trapezoidal velocity curve generated by the preset massage trajectory of the target massage area, the target pose and target force at the next moment t+1 are obtained. Obtain the actual force / torque data and actual pose of the massage head in the Cartesian space at the current time t; Using the admittance model, the expected acceleration deviation at the next time t+1 is obtained based on the target pose, target force, six-dimensional actual force / torque at the current time t, and actual pose data. The admittance model has its stiffness set to 0 in the Z-axis direction, and its dynamic behavior in the Z-axis is synchronized with the trapezoidal velocity curve. Based on the desired acceleration deviation, the pose correction amount is obtained, the target pose is adjusted at the next time t+1, and the corrected desired pose is generated. The joint angles of the desired pose are solved by inverse kinematics to drive the robotic arm.

2. The method of claim 1, wherein, The trapezoidal speed curve includes a slow-fast-slow change process, and the Z-axis dynamic behavior includes a light-heavy-light change process.

3. The method of claim 2, wherein, When the massage head moves along the Z axis to the lowest position of the trajectory in the massage, the virtual impedance force F in the Cartesian space is calculated in real time through the admittance model imp , wherein the impedance force component F imp,xy in the horizontal direction, i.e. the X / Y axis plane If F imp，xy ≥ R1, R1 is the first resistance threshold, then ΔF = K p · F imp，xy Dynamic lifting Z-axis target force, K p is the proportional gain; If F imp，xy ≥ R2, R2 is a second resistance threshold, then ΔF' = K p '· F imp，xy corrects the Z-axis target force, where Kp' > Kp.

4. The method of claim 1, wherein, After obtaining the actual force / torque data and actual pose of the massage head in the next time step t+1 in Cartesian space, the method further includes: According to a predetermined rotation matrix, the six-dimensional actual force / torque matrix, actual motion quantity, target force, and target motion quantity are rotated to the force control coordinate system, that is, the Z-axis is aligned with the target force direction. The rotation matrix is ​​calculated based on the target force direction, and the motion quantity includes acceleration, velocity, and pose.

5. The method according to any one of claims 1 to 4, characterized in that, The admittance model is used to obtain the expected acceleration deviation at the next time t+1 based on the target pose at the next time t+1, the target force, the six-dimensional actual force / torque at the current time t, and the actual pose data. This includes: Based on the target pose at the next time t+1, the target force, the actual force / torque at the current time t, and the actual pose, the differences between the actual force at the current time t and the target force at the next time t+1, the differences between the actual velocity at the current time t and the target velocity at the next time t+1, and the differences between the actual pose at the current time t and the target pose at the next time t+1 are obtained respectively. Using an admittance model a difference between the actual force at the current time t and the target force at the next time t+1 a difference between the actual velocity at the current time t and the target velocity at the next time t+1 and a difference between the actual pose at the current time t and the target pose at the next time t+1 a desired acceleration deviation at the next time t+1 6. The method of claim 5, wherein, The step of obtaining the pose correction amount based on the desired acceleration deviation, adjusting the target pose at the next time t+1, and generating the corrected desired pose includes: Using the formula: The pose correction amount is calculated To prevent the adoption of Calculate The integral accumulated error; wherein, P a t The actual value at the current time t point, that is, the actual pose, P d t+1 The next time t+1 target pose, The next time t+1 target speed, Vat represents the actual speed at the current time t; According to the pose correction amount Adjust the target pose P of the next moment d t+1 , generate the corrected expected pose 7. The method of claim 5, wherein, If, based on a predetermined rotation matrix, the six-dimensional actual force / torque matrix and actual motion at the current time t are rotated to the force control coordinate system along with the target force and target motion at the next time t+1, the difference between the actual force at the current time and the target force at the next time t+1, the difference between the actual velocity and the target velocity, and the difference between the actual pose and the target pose can be obtained. The obtained difference data are input into the admittance model to obtain the expected acceleration deviation at the next time t+1 after rotation, and then the pose correction amount is obtained. The pose correction amount is rotated inversely to adjust the target pose at the next time t+1, thus obtaining the corrected desired pose.

8. A massage control device of a robot arm, characterized by, The robotic arm is equipped with a massage head at its end, and the massage head is equipped with a six-dimensional sensor to collect contact force data in real time. The planning massage track module is configured to generate a corresponding trapezoidal velocity curve in Cartesian space according to a preset massage track generated based on a target massage area, to obtain a target pose at a next time t+1 and a target force; The six-dimensional force acquisition module is configured to acquire six-dimensional actual force / torque data and an actual pose of the massage head in Cartesian space at a current time t; The desired acceleration deviation calculation module is configured to obtain a desired acceleration deviation at the next time t+1 by using a mobility model based on the target pose at the next time t+1, the target force, the six-dimensional actual force / torque at the current time t, and the actual pose data, wherein the mobility model sets a stiffness in a Z-axis direction to 0, and a Z-axis dynamic behavior is synchronized with the trapezoidal velocity curve; The pose correction module is configured to obtain a pose correction amount based on the desired acceleration deviation, to adjust the target pose at the next time t+1, and to generate a corrected desired pose; The inverse kinematics module is configured to solve joint angles of the desired pose by inverse kinematics to drive the robot arm.

9. A massage control system of a robot arm, characterized by, The system comprises a robot arm, a massage head arranged at an end of the robot arm, and a controller applied to the robot arm, the massage head is provided with a six-dimensional sensor to collect contact force data in real time, and the controller is configured to execute steps of the method according to any one of claims 1 to 7.

10. A computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by a processor to implement steps of the method according to any one of claims 1 to 7.

Images