Upper limb posture calculation method and system

Abstract

The invention provides an upper limb posture calculation method and system, and the method comprises the steps: calculating a shoulder joint coordinate according to an elbow joint coordinate and an elbow joint distance, and obtaining upper limb posture data; the shoulder joint coordinates, the elbow joint coordinates and the tail end center point coordinates are stored as a data set, and the data set is processed to obtain a first data set; whether the newly obtained elbow joint coordinates are in the first data set or not is judged, and if not, the newly obtained elbow joint coordinates are stored in a second data set; solving a first average value of all shoulder joint point positions in the first data set, and taking the first average value as a shoulder joint center point; solving a second average value of all shoulder joint point positions in the second data set, comparing the second average value with the shoulder joint center point, and if the deviation between the second average value and the shoulder joint center point is larger than a preset value, updating the shoulder joint center point. Whether the shoulder joint center deviates or not can be judged, the shoulder joint center point is updated, and therefore the position of the shoulder joint center point can be accurately determined.

Description

Technical Field

[0001] The present invention relates to the technical field of upper limb rehabilitation, and in particular to an upper limb posture calculation method and system. Background Art

[0002] An upper limb rehabilitation robot driven by a forearm, that is, the rehabilitation robot only drives the patient's forearm, and performs rehabilitation movements by driving the forearm through the mechanical arm, and the forearm drives the upper arm; the power part and power supply part of the mechanical structure of this type of rehabilitation robot are farther away from the user, making it safer and more comfortable.

[0003] In the existing technology, in actual use, upper limb rehabilitation robots need to evaluate the patient's rehabilitation effect and limit the upper limb movement space range. The effect evaluation and space limitation require the patient's upper limb posture. For an upper limb rehabilitation robot that only drives the forearm, it is relatively easy to obtain the elbow joint position, but it is difficult to obtain the shoulder joint center. The existing technology and conventional methods can only roughly confirm the shoulder joint center position, ignoring the shoulder joint center offset caused by the change of sitting posture during the patient's use. Summary of the invention

[0004] Based on this, the purpose of the present invention is to provide a method and system for calculating upper limb posture to solve the deficiencies in the above-mentioned prior art.

[0005] In a first aspect, the present invention provides an upper limb posture calculation method, which is applied to an upper limb rehabilitation robot, and the method comprises: Real-time acquisition of the forearm motion radius, the end center coordinates of the upper limb rehabilitation robot, and the elbow distance from the end center to the elbow joint; Calculate the elbow joint coordinates based on the end center point coordinates, the forearm movement radius, the normal vector perpendicular to the movement trajectory of the end center point, the elbow joint distance and the forearm direction vector; Calculating shoulder joint coordinates according to the elbow joint coordinates and the elbow joint distance, and obtaining upper limb posture data based on the elbow joint coordinates and the shoulder joint coordinates; Based on the upper limb posture data, the shoulder joint coordinates, the elbow joint coordinates, and the end center point coordinates are stored as a data group, the data group is deleted and supplemented according to the elbow joint coordinates, and the data group is processed to obtain a first data group; Determine whether the newly acquired elbow joint coordinates are in the first data group, and if not, store the newly acquired elbow joint coordinates in the second data group; Calculate a first average value of all shoulder joint point positions in the first data group, and use the first average value as the shoulder joint center point; Obtain a second average value of all shoulder joint point positions in the second data group, and compare the second average value with the shoulder joint center point; if a deviation between the second average value and the shoulder joint center point is greater than a preset value, update the shoulder joint center point.

[0006] Compared with the prior art, the beneficial effects of the present invention are: calculating the shoulder joint coordinates through the elbow joint coordinates and the elbow joint distance, obtaining the upper limb posture data through the elbow joint coordinates and the shoulder joint coordinates, obtaining a data group through the upper limb posture data, and processing the data group to obtain a first data group, and by comparing the second average value with the shoulder joint center point, it is possible to determine whether the shoulder joint center is offset, and update the shoulder joint center point, so that the position of the shoulder joint center point can be accurately confirmed.

[0007] Furthermore, the step of obtaining the forearm movement radius, the end center point coordinates of the upper limb rehabilitation robot, and the elbow joint distance from the end center point to the elbow joint in real time includes: Obtain the user's upper arm length, forearm length, forearm movement radius, end center point coordinates, and elbow joint distance from the end center point to the elbow joint; Based on the posture sensor conversion calculation of the upper limb rehabilitation robot, the normal vector of the end center point and the direction vector of the forearm are obtained.

[0008] Furthermore, the calculation expression of the elbow joint coordinates is: ; ; ; ······ ; In the formula, , , , They represent the elbow joint coordinates in the initial state, the elbow joint coordinates at the first time of adoption, the elbow joint coordinates at the second time of adoption, and the elbow joint coordinates at the tth time of adoption, respectively. , , , They represent the coordinates of the center point of the terminal in the initial state, the coordinates of the center point of the terminal at the first time of adoption, the coordinates of the center point of the terminal at the second time of adoption, and the coordinates of the center point of the terminal at the tth time of adoption, respectively. It represents the radius of motion of the forearm. , , , They represent the normal vector of the vertical terminal plane in the initial state, the normal vector of the vertical terminal plane at the first time of adoption, the normal vector of the vertical terminal plane at the second time of adoption, and the normal vector of the vertical terminal plane at the tth time of adoption, respectively. represents the elbow distance, , , , They respectively represent the direction vector parallel to the human forearm in the initial state, the direction vector parallel to the human forearm at the first adoption moment, the direction vector parallel to the human forearm at the second adoption moment, and the direction vector parallel to the human forearm at the tth adoption moment.

[0009] Furthermore, the shoulder joint coordinate calculation expression is: ; ; ······ ; In the formula, , , They represent the shoulder joint coordinates at the first time of adoption, the shoulder joint coordinates at the second time of adoption, and the shoulder joint coordinates at the tth time of adoption, respectively. , , , They represent the elbow joint coordinates in the initial state, the elbow joint coordinates at the first time of adoption, the elbow joint coordinates at the second time of adoption, and the elbow joint coordinates at the tth time of adoption, respectively. Indicates the distance the elbow joint moves.

[0010] Furthermore, before the step of storing the shoulder joint coordinates, the elbow joint coordinates, and the end center point coordinates as a data group based on the upper limb posture data, the method further includes: Solving the motion curve solution of the shoulder joint coordinates, and storing the shoulder joint coordinate points, elbow joint coordinate points and the end center point as an array; If the solution of the motion curve is discarded, no data will be recorded.

[0011] Furthermore, the step of processing the data group to obtain the first data group includes: Confirm whether only one set of elbow joint coordinates is retained in the data set, and if so, obtain the first data set.

[0012] Furthermore, if the deviation between the second average and the shoulder joint center point is greater than a preset value, the step includes: If the deviation between the second average value and the center point of the shoulder joint is greater than 20 mm, and the second data group is greater than 200 groups, it is determined that the center point of the shoulder joint is offset.

[0013] In a second aspect, the present invention further provides an upper limb posture calculation system for implementing the above-mentioned upper limb posture calculation method, the system comprising: A real-time acquisition module is used to acquire the forearm movement radius, the end center point coordinates of the upper limb rehabilitation robot, and the elbow joint distance from the end center point to the elbow joint in real time; A first calculation module, used for calculating the elbow joint coordinates based on the end center point coordinates, the forearm movement radius, the normal vector perpendicular to the movement trajectory of the end center point, the elbow joint distance and the forearm direction vector; A second calculation module, used for calculating shoulder joint coordinates according to the elbow joint coordinates and the elbow joint distance, and obtaining upper limb posture data based on the elbow joint coordinates and the shoulder joint coordinates; A storage processing module, for storing the shoulder joint coordinates, the elbow joint coordinates, and the end center point coordinates as a data group based on the upper limb posture data, removing and supplementing the data group according to the elbow joint coordinates, and processing the data group to obtain a first data group; a judgment storage module, used for judging whether the newly acquired elbow joint coordinates are in the first data group, and if not, storing the newly acquired elbow joint coordinates in the second data group; A first obtaining module, used for obtaining a first average value of the positions of all shoulder joint points in the first data group, and using the first average value as the shoulder joint center point; The second obtaining module is used to obtain the second average value of the positions of all shoulder joint points in the second data group, and compare the second average value with the shoulder joint center point. If the deviation between the second average value and the shoulder joint center point is greater than a preset value, the shoulder joint center point is updated.

[0014] Furthermore, the real-time acquisition module includes: An acquisition unit, used to acquire the length of the upper arm, the length of the lower arm, the movement radius of the lower arm, the coordinates of the end center point, and the elbow distance from the end center point to the elbow joint of the user; The conversion calculation unit is used to obtain the normal vector of the end center point and the forearm direction vector based on the posture sensor conversion calculation of the upper limb rehabilitation robot.

[0015] Furthermore, the second obtaining module includes: A judgment unit is used to judge that the center point of the shoulder joint is offset if the deviation between the second average value and the center point of the shoulder joint is greater than 20 mm, and the second data group is greater than 200 groups. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 is a flow chart of the upper limb posture calculation method in the first embodiment of the present invention; Figure 2 It is a structural block diagram of the upper limb posture calculation system in the second embodiment of the present invention.

[0017] Description of main component symbols: 11. Real-time acquisition module; 12. First calculation module; 13. Second calculation module; 14. Storage processing module; 15. Judgment storage module; 16. First obtaining module; 17. Second obtaining module.

[0018] The following specific implementation manner will further illustrate the present invention in conjunction with the above-mentioned drawings. DETAILED DESCRIPTION

[0019] In order to facilitate the understanding of the present invention, the present invention will be described more fully below with reference to the relevant drawings. Several embodiments of the present invention are given in the drawings. However, the present invention can be implemented in many different forms and is not limited to the embodiments described herein. On the contrary, the purpose of providing these embodiments is to make the disclosure of the present invention more thorough and comprehensive.

[0020] It should be noted that when an element is referred to as being "fixed to" another element, it may be directly on the other element or there may be a central element. When an element is considered to be "connected to" another element, it may be directly connected to the other element or there may be a central element at the same time. The terms "vertical", "horizontal", "left", "right" and similar expressions used herein are for illustrative purposes only.

[0021] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art to which the present invention belongs. The terms used herein in the specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. The term "and / or" used herein includes any and all combinations of one or more of the related listed items.

[0022] Embodiment 1 See also Figure 1 , which is a method for calculating upper limb posture in a first embodiment of the present invention, and the method includes steps S1 to S7: S1, real-time acquisition of the forearm motion radius, the end center coordinates of the upper limb rehabilitation robot, and the elbow distance from the end center to the elbow joint; Specifically, the step S1 includes steps S11 to S12: S11, obtaining the length of the upper arm, the length of the forearm, the movement radius of the forearm, the coordinates of the end center point, and the elbow joint distance from the end center point to the elbow joint of the user; S12, obtaining a normal vector of the end center point and a forearm direction vector based on the posture sensor conversion calculation of the upper limb rehabilitation robot; It can be understood that the user's upper arm length, forearm length and forearm movement radius can be obtained through actual measurement. The user's upper arm length a, forearm length b, forearm radius r, and elbow distance from the center point of the rehabilitation robot end to the elbow joint are input. , the coordinates of the center point of the end of the rehabilitation robot P (x, y, z). In this embodiment, the forearm radius is the movement radius of the forearm, and the average approximate value of the forearm movement radius can be used. The coordinates of the center point of the end can be obtained by forward solving different upper limb rehabilitation robots. The end posture data is represented by vectors A (x1, y1, z1) and O (x2, y2, z2). A represents the normal vector perpendicular to the end plane, and O represents the direction vector parallel to the human forearm, which can be obtained by conversion and calculation of the posture sensor at the end. Upper arm length, forearm length, rehabilitation robot end center point-elbow joint distance It can be obtained through actual measurement.

[0023] It is worth noting that P, A, and O are updated in real time according to data sampling and calculation, and can be collected once every 100ms. P0, A0, and O0 represent the initial value of the coordinates of the center point of the end, the initial value of the normal vector perpendicular to the plane of the end, and the initial value of the vector parallel to the direction of the human forearm, respectively.

[0024] S2, calculating the elbow joint coordinates based on the end center point coordinates, the forearm movement radius, the normal vector perpendicular to the movement trajectory of the end center point, the elbow joint distance and the forearm direction vector; It can be understood that the upper limb posture can be expressed by the coordinates of the end center point plus the elbow joint position plus the shoulder joint position. The calculation expression of the elbow joint coordinates is: ; ; ; ······ ; In the formula, , , , They represent the elbow joint coordinates in the initial state, the elbow joint coordinates at the first time of adoption, the elbow joint coordinates at the second time of adoption, and the elbow joint coordinates at the tth time of adoption, respectively. , , , They represent the coordinates of the center point of the terminal in the initial state, the coordinates of the center point of the terminal at the first time of adoption, the coordinates of the center point of the terminal at the second time of adoption, and the coordinates of the center point of the terminal at the tth time of adoption, respectively. It represents the radius of motion of the forearm. , , , They represent the normal vector of the vertical terminal plane in the initial state, the normal vector of the vertical terminal plane at the first time of adoption, the normal vector of the vertical terminal plane at the second time of adoption, and the normal vector of the vertical terminal plane at the tth time of adoption, respectively. represents the elbow distance, , , , They respectively represent the direction vector parallel to the human forearm in the initial state, the direction vector parallel to the human forearm at the first adoption moment, the direction vector parallel to the human forearm at the second adoption moment, and the direction vector parallel to the human forearm at the tth adoption moment.

[0025] It is worth noting that the initial value of the elbow joint coordinates is ,and , , is the iterative update value of the elbow joint coordinates.

[0026] S3, calculating shoulder joint coordinates according to the elbow joint coordinates and the elbow joint distance, and obtaining upper limb posture data based on the elbow joint coordinates and the shoulder joint coordinates; It can be understood that the shoulder joint coordinate calculation expression is: ; ; ······ ; In the formula, , , They represent the shoulder joint coordinates at the first time of adoption, the shoulder joint coordinates at the second time of adoption, and the shoulder joint coordinates at the tth time of adoption, respectively. , , , They represent the elbow joint coordinates in the initial state, the elbow joint coordinates at the first time of adoption, the elbow joint coordinates at the second time of adoption, and the elbow joint coordinates at the tth time of adoption, respectively. Indicates the elbow joint movement distance; Understandably, , , The solution is a circular curve with the centers being , , , the radius is , through the formula It can be seen that the plane normal vector it represents is , passing through point , through the formula It can be seen that the plane normal vector it represents is , passing through point ; It should be noted that the combined formula And the formula The intersection equation of the plane can be obtained and the centers of the two circles can be calculated and The distance from the center of the two circles to the straight line is recorded as , ,judge , and relationship, if < or , then discard this group of solutions (the corresponding end data and elbow joint coordinate data are discarded at the same time), and continue to the next calculation. ≥ and , take the point closest to the intersection line as , The solution is The distance to the intersection line is , The distance to the intersection line is ; Combined with formula Can get , The only solution of , similarly, can be obtained at time t Therefore, according to the above content, the upper limb posture data can be obtained, which can be recorded as [ ].

[0027] S4, based on the upper limb posture data, the shoulder joint coordinates, the elbow joint coordinates, and the end center point coordinates are stored as a data group, the data group is deleted and supplemented according to the elbow joint coordinates, and the data group is processed to obtain a first data group; It should be noted that the shoulder joint center point coordinate output and the user's sitting posture deviation monitoring, the real-time shoulder joint center will change with the movement of the scapula, but the user's sitting posture has not changed at this time, that is, the upper limb motion limit of the upper limb rehabilitation robot should not change. The upper limb motion limit origin of the upper limb rehabilitation robot is confirmed by the shoulder joint position, so it is necessary to output an average value of the shoulder joint position to represent the center point of the shoulder joint. When the user's sitting posture does not change, the shoulder joint center point should remain stable. When the sitting posture shifts, the shoulder joint center point should change according to the offset.

[0028] Specifically, the step S4 includes step S41 to step S42: S41, solving the motion curve solution of the shoulder joint coordinates, and storing the shoulder joint coordinate points, elbow joint coordinate points and the end center point as an array; S42, if the solution of the motion curve is discarded, no data is recorded; Understandably, the , , After solving the problem, save the shoulder joint coordinate point, elbow joint coordinate point, and end center point as an array [ ],[ ],[ ]If the shoulder joint coordinate solution is abandoned, this group of data will not be recorded; Specifically, the step S4 further includes step S43: S43, confirming whether only one set of elbow joint coordinates is retained in the data set, and if so, obtaining the first data set; It should be explained that after the data is stored, the data group is deleted and re-recorded. The data is deleted and re-recorded according to the elbow joint coordinates. For the current 1000 groups of data, it is confirmed that only one group of data is retained in the 30mm*30mm*30mm space of each elbow joint coordinate, that is, for For example, remove all Coordinate values ​​between Between, y coordinate value between between, Coordinate values ​​between The other elbow joint coordinate data and the data in the group to which they belong, the processed data group is recorded as the first data group.

[0029] S5, determining whether the newly acquired elbow joint coordinates are in the first data group, and if not, storing the newly acquired elbow joint coordinates in the second data group; It can be understood that when the new elbow joint coordinate data is within the space of the first data group, the new data is stored in the space of the second data group. If the new elbow joint coordinate is not within the space of the first data group, it is added to the first data group. If the new elbow joint coordinate is within the space of the first data group and the space of the second data group, the data in the second data group is replaced. This process is repeated.

[0030] S6, obtaining a first average value of all shoulder joint point positions in the first data group, and using the first average value as the shoulder joint center point; It can be understood that the average value of all shoulder joint point positions in the first data group is obtained, and the average value of the shoulder joint positions in the first data group is recorded as the shoulder joint center point.

[0031] S7, obtaining a second average value of all shoulder joint point positions in the second data group, and comparing the second average value with the shoulder joint center point, and if a deviation between the second average value and the shoulder joint center point is greater than a preset value, updating the shoulder joint center point; Specifically, the step S7 includes step S71: S71, if the deviation between the second average value and the center point of the shoulder joint is greater than 20 mm, and the number of the second data groups is greater than 200, then it is determined that the center point of the shoulder joint is offset; It should be explained that the average value of the shoulder joint point position of the second data group is obtained. When the deviation between the average value of the shoulder joint point position of the second data group and the shoulder joint center point position is greater than 20 mm and the data of the second data group is greater than 200 groups, all the data of the first data group are discarded, the data of the second data group are transferred to the first data group, and the second data group is cleared. That is, it is considered that the shoulder joint center has shifted at this time, and the shoulder joint center point is updated.

[0032] In summary, the upper limb posture calculation method in the above-mentioned embodiment of the present invention calculates the shoulder joint coordinates through the elbow joint coordinates and the elbow joint distance, obtains the upper limb posture data through the elbow joint coordinates and the shoulder joint coordinates, obtains a data group through the upper limb posture data, and processes the data group to obtain a first data group, and by comparing the second average value with the shoulder joint center point, it can be determined whether the shoulder joint center is offset, and the shoulder joint center point is updated, so that the position of the shoulder joint center point can be accurately confirmed, and the posture of the upper limb can be obtained in real time, and posture monitoring and data correction can be performed for the offset caused by changes in sitting posture during rehabilitation exercises.

[0033] Embodiment 2 See also Figure 2 , shown is an upper limb posture calculation system in a second embodiment of the present invention, which is used to implement the upper limb posture calculation method in the first embodiment, and the system includes: A real-time acquisition module 11 is used to acquire the forearm movement radius, the end center point coordinates of the upper limb rehabilitation robot, and the elbow joint distance from the end center point to the elbow joint in real time; The first calculation module 12 is used to calculate the elbow joint coordinates based on the end center point coordinates, the forearm movement radius, the normal vector perpendicular to the movement trajectory of the end center point, the elbow joint distance and the forearm direction vector. The calculation expression of the elbow joint coordinates is: ; ; ; ······ ; In the formula, , , , They represent the elbow joint coordinates in the initial state, the elbow joint coordinates at the first time of adoption, the elbow joint coordinates at the second time of adoption, and the elbow joint coordinates at the tth time of adoption, respectively. , , , They represent the coordinates of the center point of the terminal in the initial state, the coordinates of the center point of the terminal at the first time of adoption, the coordinates of the center point of the terminal at the second time of adoption, and the coordinates of the center point of the terminal at the tth time of adoption, respectively. Indicates the radius of motion of the forearm, , , , They represent the normal vector of the vertical terminal plane in the initial state, the normal vector of the vertical terminal plane at the first time of adoption, the normal vector of the vertical terminal plane at the second time of adoption, and the normal vector of the vertical terminal plane at the tth time of adoption, respectively. represents the elbow distance, , , , They respectively represent the direction vector parallel to the human forearm in the initial state, the direction vector parallel to the human forearm at the first time of adoption, the direction vector parallel to the human forearm at the second time of adoption, and the direction vector parallel to the human forearm at the t-th time of adoption; The second calculation module 13 is used to calculate the shoulder joint coordinates according to the elbow joint coordinates and the elbow joint distance, and obtain the upper limb posture data based on the elbow joint coordinates and the shoulder joint coordinates. The shoulder joint coordinate calculation expression is: ; ; ······ ; In the formula, , , They represent the shoulder joint coordinates at the first time of adoption, the shoulder joint coordinates at the second time of adoption, and the shoulder joint coordinates at the tth time of adoption, respectively. , , , They represent the elbow joint coordinates in the initial state, the elbow joint coordinates at the first time of adoption, the elbow joint coordinates at the second time of adoption, and the elbow joint coordinates at the tth time of adoption, respectively. Indicates the elbow joint movement distance; A storage processing module 14 is used to store the shoulder joint coordinates, the elbow joint coordinates, and the end center point coordinates as a data group based on the upper limb posture data, remove and supplement the data group according to the elbow joint coordinates, and process the data group to obtain a first data group; A judgment storage module 15, used for judging whether the newly acquired elbow joint coordinates are in the first data group, and if not, storing the newly acquired elbow joint coordinates in the second data group; A first obtaining module 16 is used to obtain a first average value of the positions of all shoulder joint points in the first data group, and use the first average value as the shoulder joint center point; The second obtaining module 17 is used to obtain the second average value of all shoulder joint point positions in the second data group, and compare the second average value with the shoulder joint center point. If the deviation between the second average value and the shoulder joint center point is greater than a preset value, the shoulder joint center point is updated.

[0034] In some optional embodiments, the real-time acquisition module 11 includes: An acquisition unit, used to acquire the length of the upper arm, the length of the lower arm, the movement radius of the lower arm, the coordinates of the end center point, and the elbow distance from the end center point to the elbow joint of the user; The conversion calculation unit is used to obtain the normal vector of the end center point and the forearm direction vector based on the posture sensor conversion calculation of the upper limb rehabilitation robot.

[0035] In some optional embodiments, the storage processing module 14 includes: A solving unit, used for solving the motion curve solution of the shoulder joint coordinates, and storing the shoulder joint coordinate points, elbow joint coordinate points and the end center point as an array; A first judgment unit, if the solution of the motion curve is discarded, then no data is recorded; A confirmation unit is used to confirm whether only one set of elbow joint coordinates is retained in the data set, and if so, obtain the first data set.

[0036] In some optional embodiments, the second obtaining module 17 includes: The second judgment unit is used to judge that the center point of the shoulder joint is offset if the deviation between the second average value and the center point of the shoulder joint is greater than 20 mm, and the second data group is greater than 200 groups.

[0037] The functions or operation steps implemented when the above modules and units are executed are generally the same as those in the above method embodiments, and will not be repeated here.

[0038] The upper limb posture calculation system provided in the embodiment of the present invention has the same implementation principle and technical effects as those of the aforementioned method embodiment. For the sake of brief description, for matters not mentioned in the system embodiment, reference can be made to the corresponding contents in the aforementioned method embodiment.

[0039] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "examples", "specific examples", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0040] The above-mentioned embodiments only express several implementation methods of the present invention, and the description thereof is relatively specific and detailed, but it cannot be understood as limiting the scope of the patent of the present invention. It should be pointed out that, for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention shall be subject to the attached claims.

Claims

1. A method for calculating upper limb posture, applied to an upper limb rehabilitation robot, characterized in that: The method comprises: Real-time acquisition of the forearm motion radius, the end center coordinates of the upper limb rehabilitation robot, and the elbow distance from the end center to the elbow joint; Calculate the elbow joint coordinates based on the end center point coordinates, the forearm movement radius, the normal vector perpendicular to the movement trajectory of the end center point, the elbow joint distance and the forearm direction vector; Calculating shoulder joint coordinates according to the elbow joint coordinates and the elbow joint distance, and obtaining upper limb posture data based on the elbow joint coordinates and the shoulder joint coordinates; Based on the upper limb posture data, the shoulder joint coordinates, the elbow joint coordinates, and the end center point coordinates are stored as a data group, the data group is deleted and supplemented according to the elbow joint coordinates, and the data group is processed to obtain a first data group; Determine whether the newly acquired elbow joint coordinates are in the first data group, and if not, store the newly acquired elbow joint coordinates in the second data group; Calculate a first average value of all shoulder joint point positions in the first data group, and use the first average value as the shoulder joint center point; Obtain a second average value of all shoulder joint point positions in the second data group, and compare the second average value with the shoulder joint center point; if a deviation between the second average value and the shoulder joint center point is greater than a preset value, update the shoulder joint center point.

2. The upper limb posture calculation method according to claim 1, characterized in that: The step of obtaining the forearm movement radius, the coordinates of the end center point of the upper limb rehabilitation robot, and the elbow joint distance from the end center point to the elbow joint in real time comprises: Obtain the user's upper arm length, forearm length, forearm movement radius, end center point coordinates, and elbow joint distance from the end center point to the elbow joint; Based on the posture sensor conversion calculation of the upper limb rehabilitation robot, the normal vector of the end center point and the direction vector of the forearm are obtained.

3. The upper limb posture calculation method according to claim 1, characterized in that: The calculation expression of the elbow joint coordinates is: ； ； ； ······ ； In the formula, , , , They represent the elbow joint coordinates in the initial state, the elbow joint coordinates at the first time of adoption, the elbow joint coordinates at the second time of adoption, and the elbow joint coordinates at the tth time of adoption, respectively. , , , They represent the coordinates of the center point of the terminal in the initial state, the coordinates of the center point of the terminal at the first time of adoption, the coordinates of the center point of the terminal at the second time of adoption, and the coordinates of the center point of the terminal at the tth time of adoption, respectively. Indicates the radius of motion of the forearm, , , , They represent the normal vector of the vertical terminal plane in the initial state, the normal vector of the vertical terminal plane at the first time of adoption, the normal vector of the vertical terminal plane at the second time of adoption, and the normal vector of the vertical terminal plane at the tth time of adoption, respectively. represents the elbow distance, , , , They respectively represent the direction vector parallel to the human forearm in the initial state, the direction vector parallel to the human forearm at the first adoption moment, the direction vector parallel to the human forearm at the second adoption moment, and the direction vector parallel to the human forearm at the tth adoption moment.

4. The upper limb posture calculation method according to claim 1, characterized in that: The shoulder joint coordinate calculation expression is: ； ； ······ ； In the formula, , , They represent the shoulder joint coordinates at the first time of adoption, the shoulder joint coordinates at the second time of adoption, and the shoulder joint coordinates at the tth time of adoption, respectively. , , , They represent the elbow joint coordinates in the initial state, the elbow joint coordinates at the first time of adoption, the elbow joint coordinates at the second time of adoption, and the elbow joint coordinates at the tth time of adoption, respectively. Indicates the distance the elbow joint moves.

5. The upper limb posture calculation method according to claim 1, characterized in that: Before the step of storing the shoulder joint coordinates, the elbow joint coordinates, and the end center point coordinates as a data group based on the upper limb posture data, the method further includes: Solving the motion curve solution of the shoulder joint coordinates, and storing the shoulder joint coordinate points, elbow joint coordinate points and the end center point as an array; If the solution of the motion curve is discarded, no data will be recorded.

6. The upper limb posture calculation method according to claim 1, characterized in that: The step of processing the data group to obtain a first data group comprises: Confirm whether only one set of elbow joint coordinates is retained in the data set, and if so, obtain the first data set.

7. The upper limb posture calculation method according to claim 1, characterized in that: The step of if the deviation between the second average and the shoulder joint center point is greater than a preset value comprises: If the deviation between the second average value and the center point of the shoulder joint is greater than 20 mm, and the second data group is greater than 200 groups, it is determined that the center point of the shoulder joint is offset.

8. An upper limb posture calculation system, used to implement the upper limb posture calculation method according to any one of claims 1 to 7, characterized in that: The system comprises: A real-time acquisition module is used to acquire the forearm movement radius, the end center point coordinates of the upper limb rehabilitation robot, and the elbow joint distance from the end center point to the elbow joint in real time; A first calculation module, used for calculating the elbow joint coordinates based on the end center point coordinates, the forearm movement radius, the normal vector perpendicular to the movement trajectory of the end center point, the elbow joint distance and the forearm direction vector; A second calculation module, used for calculating shoulder joint coordinates according to the elbow joint coordinates and the elbow joint distance, and obtaining upper limb posture data based on the elbow joint coordinates and the shoulder joint coordinates; A storage processing module, for storing the shoulder joint coordinates, the elbow joint coordinates, and the end center point coordinates as a data group based on the upper limb posture data, removing and supplementing the data group according to the elbow joint coordinates, and processing the data group to obtain a first data group; a judgment storage module, used for judging whether the newly acquired elbow joint coordinates are in the first data group, and if not, storing the newly acquired elbow joint coordinates in the second data group; A first obtaining module, used for obtaining a first average value of the positions of all shoulder joint points in the first data group, and using the first average value as the shoulder joint center point; The second obtaining module is used to obtain the second average value of the positions of all shoulder joint points in the second data group, and compare the second average value with the shoulder joint center point. If the deviation between the second average value and the shoulder joint center point is greater than a preset value, the shoulder joint center point is updated.

9. The upper limb posture calculation system according to claim 8, characterized in that: The real-time acquisition module comprises: An acquisition unit, used to acquire the length of the upper arm, the length of the lower arm, the movement radius of the lower arm, the coordinates of the end center point, and the elbow distance from the end center point to the elbow joint of the user; The conversion calculation unit is used to obtain the normal vector of the end center point and the forearm direction vector based on the posture sensor conversion calculation of the upper limb rehabilitation robot.

10. The upper limb posture calculation system according to claim 8, characterized in that: The second obtaining module comprises: A judgment unit is used to judge that the center point of the shoulder joint is offset if the deviation between the second average value and the center point of the shoulder joint is greater than 20 mm, and the second data group is greater than 200 groups.

Images