Method and system for positioning space target

Abstract

The invention provides a method and a system for positioning a space target. According to the method, three distance measuring sensors which are not arranged on the same straight line simultaneously obtain three observation distances for any target point on an observation object, and the three observation distances are combined with the spatial geometrical relationship of the three observation distances corresponding to the same target point and the position coordinates of each distance measuring sensor in a reference coordinate system; calculating the position coordinates of each target point in the reference coordinate system at the moment; and tracking and analyzing the position and attitude of the observation object based on the change of the position coordinates of the plurality of target points in the reference coordinate system along with time. According to the technical scheme, a space target point is positioned through three-point distance measurement, so that quantitative quick positioning and tracking of a moving point in a three-dimensional space are realized, and the displacement track of each mark point (a fluorescent identification mark is pasted in advance) of an observation object is measured, so that the FMS test process is more efficient and accurate.

Description

Technical Field

[0001] The present invention relates to the technical field of real-time positioning of space targets, and in particular to a method and system for positioning space targets that can be applied to sports posture measurement and analysis in sports medicine. Background Art

[0002] In the field of sports medicine, functional movement screening (FMS), a body function evaluation method consisting of 7 movements, is usually used to evaluate the basic motor functions of various groups of people. At present, the evaluation of FMS still relies on well-trained health and fitness experts to use visual observation, photography, rulers and protractors, and sufficient experience to make accurate judgments and analyses, and draw objective conclusions. In order to discover the different problems exposed by the subjects from different observation angles, the tester needs to observe the subjects continuously and take photos multiple times. Correspondingly, the subjects also need to repeat the same action many times, which inevitably leads to pain, interruption of the test, and makes it difficult for the subjects to complete the 7 movements. In particular, rehabilitation doctors are generally subject to more restrictions when evaluating the rehabilitation motor function of the elderly and stroke patients.

[0003] In summary, how to accurately, quickly and scientifically locate and track the movements and postures of subjects undergoing functional movement screening to avoid the situation where subjects need to repeat FMS test movements due to personnel observation omissions is a key technical problem that needs to be solved in the field of sports medicine (especially functional movement screening). Summary of the invention

[0004] In order to solve the above technical problems, the present invention provides a method and system for positioning space targets that can accurately locate and track the subject's movements and postures based on three-point ranging positioning technology and combined with spatial geometric relationships and the observation accuracy of ranging sensors.

[0005] A first aspect of the present invention provides a method for locating a space target. The method comprises the following steps:

[0006] S1, preset reference coordinate system;

[0007] S2, setting N distance measuring sensors that are not on the same straight line, and calibrating the position coordinates of each distance measuring sensor based on the reference system, where N is a natural number greater than or equal to 3;

[0008] S3, based on the observed distances of each of the plurality of target points on the observed object obtained by three ranging sensors that are not in a straight line among the N ranging sensors at any sampling time, and the position coordinates of each of the three ranging sensors in the reference coordinate system at any same sampling time, calculating the position coordinates of each target point in the reference coordinate system at any sampling time;

[0009] S4, based on a plurality of continuous sampling moments and the position coordinates of the plurality of target points in the reference coordinate system, tracking and analyzing the position and posture of the observed object.

[0010] Furthermore, in step S2, the position coordinates of each ranging sensor are calibrated based on the reference system, which is specifically implemented as follows: a number of marking points are set as scales on the X, Y, and Z axes of the reference coordinate system, and based on the observation distance of each ranging sensor to a marking point on the X, Y, and Z axes, and the coordinates of all marking points in the reference coordinate system, the position coordinates of each ranging sensor in the reference coordinate system are calculated and calibrated. Preferably, the ranging sensors are the same depth cameras.

[0011] Furthermore, the following processing is performed before step S3: step S31, determining the ranging sensor among the N ranging sensors that has moved relative to the last sampling moment; and recalibrating the position coordinates of the moving ranging sensor based on the reference system.

[0012] Furthermore, in step S3, the observed distance of the specific target point is obtained at any sampling moment, and three ranging sensors that are not on the same straight line are used as the relevant ranging sensors of the specific target point at any sampling moment; the relevant ranging sensors of different target points at the same sampling moment are the same or different, and the relevant ranging sensors of the same target point at different sampling moments are the same or different.

[0013] Further, in step S3, the position coordinates of each target point in the reference coordinate system at any sampling moment are calculated, which is implemented as follows: at any sampling moment, after obtaining three observation distances corresponding to each target point through the relevant ranging sensor of each specific target point, the position coordinates of each target point in the reference coordinate system are calculated in real time; or it is implemented as follows: obtaining and recording the three observation distances of each target point and the corresponding identification of the relevant ranging sensor at each sampling moment, and after an observation cycle, calculating the position coordinates of each target point in the reference coordinate system at each sampling moment in the observation cycle in combination with the position coordinates of each ranging sensor at each sampling moment in the observation cycle.

[0014] Furthermore, in the process of calculating the position coordinates of each target point in the reference coordinate system, one of the three observation distances obtained by the relevant ranging sensor of the specific target point at any sampling moment is replaced by its geometric theoretical value; the geometric theoretical value of the observation distance is calculated according to the spatial geometric relationship between the three observation distances obtained by the relevant ranging sensor of the specific target point at any sampling moment, in combination with the position coordinates of the relevant ranging sensor in the reference coordinate system and the other two of the three observation distances except the replaced observation distance, using an incremental step size proportional to the measurement error of the ranging sensor, and performing cyclic iteration to calculate the target theoretical value of the replaced observation distance, and the target theoretical value calculated in each round is compared with the observation distance to obtain the target theoretical value.

[0015] The second aspect of the present invention provides a system for locating space targets. The system includes: at least N ranging sensors that are not on the same straight line, N is a natural number greater than or equal to 3; the computing and processing module is a computing and processing module that is connected to all ranging sensors for communication; the computing and processing module is used to execute the above-mentioned method for locating space targets.

[0016] The technical solution provided by the present invention realizes the rapid positioning and tracking of the target point on the observed object in three-dimensional space by using the method of locating the spatial target point through three-point ranging. When the system is applied to sports medicine, it can conveniently and accurately track the displacement trajectory of each bony marker point (pre-attached with fluorescent identification markers) of the observed object, thereby realizing the collection and analysis of the motion trajectory and posture of the moving object, making the FMS test process more efficient and accurate. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 A schematic diagram of a system for locating space targets provided by the present invention.

[0018] Figure 2 A schematic diagram of the spatial geometric relationship used to calculate the coordinate position of a target point in the method for locating a space target provided by the present invention.

[0019] Figure 3 The figure is a schematic diagram of the spatial geometric relationship used to calculate the geometric theoretical value of L3 in one embodiment of the present invention.

[0020] Figure 4 The figure is a schematic diagram of the spatial geometric relationship between relevant parameters in the process of iterative calculation of q2 in one embodiment of the present invention. DETAILED DESCRIPTION

[0021] In order to make the technical problems, technical solutions and beneficial effects solved by the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0022] The method for locating a space target provided by the present invention comprises the following steps:

[0023] S1, preset reference coordinate system;

[0024] S2, setting N distance measuring sensors that are not on the same straight line, and calibrating the position coordinates of each distance measuring sensor based on the reference system, where N is a natural number greater than or equal to 3;

[0025] S3, based on the observed distances of each of the plurality of target points on the observed object obtained by three ranging sensors that are not in a straight line among the N ranging sensors at any sampling time, and the position coordinates of each of the three ranging sensors in the reference coordinate system at any same sampling time, calculating the position coordinates of each target point in the reference coordinate system at any sampling time;

[0026] S4, based on a plurality of continuous sampling moments and the position coordinates of the plurality of target points in the reference coordinate system, tracking and analyzing the position and posture of the observed object.

[0027] The position coordinates of each ranging sensor in step S2 are calibrated based on the reference system, including: respectively calibrating the position coordinates of each ranging sensor in the reference coordinate system of The axis sets several marking points as a scale, and each distance sensor is used to measure The observed distance of a marked point on the axis and the coordinates of all marked points in the reference coordinate system are used to calculate and calibrate the position coordinates of each distance measuring sensor in the reference coordinate system. Figure 1 As shown, in the pre-set reference coordinate system In The axis sets a number of marking points P1, P2, P3, P4, and P5 as a scale, and the coordinates of each marking point are known. For example, in one embodiment, the coordinates of the marking points P1, P2, P3, P4, and P5 are respectively shown in the following table.

[0028]

[0029]

[0030] a and b in the table are known constants.

[0031] exist Figure 1In the example, the distance measuring sensors A1, A2, A3, A4, and A5 are not arranged on the same straight line. Based on the coordinates of the set marking points, the position coordinates of each distance measuring sensor are calibrated according to the following algorithm. The orientation of the local coordinate system on the distance measuring sensor is consistent with the reference coordinate system. For any distance measuring sensor A j , (1≤j≤5, j is an integer) is represented by a vector:

[0032]

[0033]

[0034]

[0035] Among them, X Aj , Y Aj , Z Aj A is the distance sensor j In the reference coordinate system The position coordinates of The component on the axis. Assume that the distance sensor A j To the marked point P i The distance between them is l i :

[0036] l i 2 =(X Aj -X Pi ) 2 +(Y Aj -Y Pi ) 2 +(Z Aj -Z Pi ) 2

[0037] Theoretically, any three points can be used to solve the distance sensor A j The three coordinate components of the location point. For example, take three measurement points P0, P1, P3 and measure the distance values ​​as: l0, l1, l3:

[0038]

[0039] (1)-(2):

[0040] 10 2 -l1 2 =(X Aj ) 2 -(X Aj -a) 2 =a 2 -2X Aj a

[0041]

[0042] (1)-(3):

[0043]

[0044]

[0045] The above three measurement points can be selected arbitrarily in the "Feature Recognition Point" table. Repeating the above calibration process for each ranging sensor Aj (j>=3) in the working environment can obtain the position of the ranging sensor in the reference coordinate system. The position coordinate calibration of the ranging sensor is a self-learning process and is the premise of the moving point calculation of the core part of this article, the "three-point ranging positioning point method". Preferably, the ranging sensors are the same depth cameras.

[0046] Furthermore, the following processing is performed before step S3: step S31, determining the ranging sensor among the N ranging sensors that has moved relative to the last sampling moment; and recalibrating the position coordinates of the moving ranging sensor based on the reference system.

[0047] Furthermore, in step S3, the observed distance of the specific target point is obtained at any sampling time, and three ranging sensors that are not on the same straight line are used as the relevant ranging sensors of the specific target point at any sampling time. The relevant ranging sensors of different target points at the same sampling time are the same or different, and the relevant ranging sensors of the same target point at different sampling times are the same or different.

[0048] Furthermore, in step S3, the position coordinates of each target point in the reference coordinate system at any sampling moment are calculated, which is implemented as follows: at any sampling moment, after obtaining the three observation distances corresponding to each target point through the relevant distance measuring sensor of each specific target point, the position coordinates of each target point in the reference coordinate system are calculated in real time. The calculation of the position coordinates of each target point in the reference coordinate system at any sampling moment can also be implemented as follows: obtaining and recording the three observation distances of each target point and the corresponding identification of the relevant distance measuring sensor at each sampling moment; after an observation cycle, the position coordinates of each target point in the reference coordinate system at each sampling moment in the observation cycle are calculated in combination with the position coordinates of each distance measuring sensor at each sampling moment in the observation cycle.

[0049] Furthermore, in the process of calculating the position coordinates of each target point in the reference coordinate system, one of the three observation distances obtained by the relevant ranging sensor of the specific target point at any sampling moment is replaced by its geometric theoretical value; the geometric theoretical value of the observation distance is calculated according to the spatial geometric relationship between the three observation distances obtained by the relevant ranging sensor of the specific target point at any sampling moment, in combination with the position coordinates of the relevant ranging sensor in the reference coordinate system and the other two of the three observation distances except the replaced observation distance, using an incremental step size proportional to the measurement error of the ranging sensor, and performing cyclic iteration to calculate the target theoretical value of the replaced observation distance, and the target theoretical value calculated in each round is compared with the observation distance to obtain the target theoretical value.

[0050] For any target point Q in the reference coordinate system The spatial geometric relationship between the relevant distance measuring sensors and the measurement parameters is as follows: Figure 2 As shown. Figure 2 In the figure, A1, A2, and A3 are the locations of the three depth cameras. It is known that they are in the absolute coordinate system The coordinate value below. is the normal vector of the triangle plane [P] formed by A1A2A3. L1, L2, L3 are the distances (known quantities) from A1, A2, A3 to the target point Q measured by the depth camera respectively; L3′ is the ideal value of the analytical solution from A3 to the target point Q, ΔL3 = L3-L3 correction value, point B is the projection point of point Q on plane [P]. Draw a perpendicular line through point Q to A1A2 and A2A3, and the foot of the perpendicular is A 12 , A 23 ; For two origins, A 12 , A 23 The rectangular coordinate system, the coordinate axes are fixed relative to the absolute coordinate system, and the origin A 12 , A 23 The angle β1 is the angle between two vectors. The angle between the two vectors, angle β2 The angle between them. The variables to be solved are angles α1 and α2: Angle α1 is and rectangular coordinate system of The angle between the axes, angle α2 is and rectangular coordinate system of The angle of the axis. The directions of the three axes are all the directions indicated by the arrows on point Q.

[0051] Unit vector:

[0052] Unit vector:

[0053] r1=L1sinβ1; q1=L1cosβ1;

[0054] r2=L2sinβ2; q2=L2cosβ2;

[0055] L i (i=1..3): measurement value output by the ranging sensor.

[0056] Distance measuring sensors A1, A2, A3 in the reference coordinate system The position coordinates in are expressed as follows:

[0057]

[0058]

[0059]

[0060]

[0061] in,

[0062]

[0063]

[0064]

[0065] γ is a known quantity and The angle between them can be calculated as follows.

[0066]

[0067] Normal vector of the plane (Two vectors Unit vector after cross product):

[0068]

[0069]

[0070] In the reference coordinate system The component coefficients on each coordinate axis are n x , n y , n z as follows:

[0071] Cartesian coordinate system The origin of the coordinate system is in the reference coordinate system The coordinates in are:

[0072]

[0073]

[0074]

[0075]

[0076] coefficient

[0078] in,

[0079]

[0080]

[0081] coefficient

[0082] Cartesian coordinate system The origin of the coordinate system is in the reference coordinate system The coordinates in are:

[0083]

[0084] Through geometric relationships, we can get the rectangular coordinate system The coordinate axes of the reference coordinate system The direction vector is:

[0085]

[0086]

[0087]

[0088] The geometric parameters 1.1, 1.1, 1.1.1.L are calculated by the following formulas 12 , 1.1. is the distance between 1.1 and 1.1, which is a constant, and 1.1. is the distance between 1.1 and 1.1, which is a constant:

[0089]

[0090]

[0091] The cosine law of any triangle can be used to find the value of β1:

[0092]

[0093]

[0094] Geometric solution for the target point Q:

[0095] like Figure 3 As shown, through geometric analysis, we can get Q point with A 12 is a circle with a center and a radius of r1 (denoted as circle 1) and a 23 is the intersection of a circle with a radius of r2 (denoted as circle 2) and α1 and α2 are variables that determine the position of point Q. If only three measured distances L1, L2, and L3 are given as input conditions, there are two solutions with two symmetries and planes A1, A2, and A3, namely Figure 3 Q and Q′ shown.

[0096] Due to the measurement error of L3, circle 1 and circle 2 overlap in space, and it is impossible to ensure that they intersect in space and the position of point Q cannot be solved. Given the three points A1, A2, A3 and the three distance measurement values ​​L1, L2, L3, there is an unnecessary constraint condition, which can be solved by introducing a correction calculation method. Given the measurement values ​​L1, L2, Q is in a circle with A 12 The center is on a circle with radius r1.

[0097] The changing variable q2 can calculate the distance L3(q2) from point Q to A3 as q2 changes. ΔL3 is the difference between the measured value L3 and the theoretical value L3′: minimize ΔL3=L3-L3′, calculate the corrected L3′, and substitute L3′ into the formula to calculate the coordinate value of point Q.

[0098]

[0099]

[0100] Similarly:

[0101]

[0102] Formula 12 and Formula 13 are The two are equal to three scalar equations:

[0103]

[0104] By solving equation group 14, we can get the functional relationship between q2 and α1:

[0105] q2(α1)=r1sinα1sinγ-(L 12-q1)cosγ (15)

[0106] With the help of (14) and geometric relations You can get:

[0107]

[0108]

[0109] according to Figure 4 The geometric relationship can be obtained:

[0110] If q2≠0:

[0111]

[0112] L'3 theoretical value:

[0113]

[0114] If q2=0:

[0115] According to the measured L3 value, L3 is determined by the iterative comparison method. The specific calculation method includes:

[0116] Assume that the accuracy of the depth camera is d (for example, d = 0.005*L (L measurement distance));

[0117] The preset number of cycles is n, where n is a positive integer. The q2 calculated for the i-th cycle is marked as q2 (i) , the initial value of i is 0. q2 (i) Given an initial value q2 (0)

[0118]

[0119] By calculating the intermediate variable α1 (i＝0) (q2), α2 (i＝0) (q2) Then calculate β′ 2(i＝0) , when we get β′2 (i＝0) Substitute into formula 19 to calculate L3′ (i＝0) , and calculate the deviation ΔL3 (i＝0) =L3-L3′ (i＝0) . Give q2 a loop increment Δq2 and perform n loop calculations of q2, with i = i + 1 in each loop (e.g. Δq2 = (0.01-0.02)*d). In each loop, the intermediate variable α1 is calculated in turn. (i) (q2), α2 (i) (q2), β′2 (i) Then calculate ΔL3 (i) ;Judge ΔL3(i) Relative to ΔL3 (i-1) Whether a sign change occurs. When i=k, ΔL3 (i＝k) Relative to ΔL3 (i＝k-1) The sign changes and the loop ends. At this time, the converged result value L3′ (k) As the geometric theoretical value of L3, L3′:

[0120] L3′=L3′ (k) (20)

[0121] Calculate the coordinate value of point Q in the reference coordinate system:

[0122]

[0123]

[0124] The coordinate value of point Q in the reference coordinate system:

[0125]

[0126] Among them, r1=L1sinβ1;q1=L1cosβ1;n x , n y 、n z By calculating equation (4), the coefficients z1x, z 1y , z1z is calculated by equation (6), y 1x ,y 1y ,y 1z The variable angles α1 and α2 to be solved are calculated by equation (7). The corresponding q2 is calculated by combining equations 16 and 17 according to the L3′ loop iteration result (20). The coordinate solution of the spatial target point Q is now completed.

[0127] Corresponding to the above-mentioned method for locating a space target, the present invention also provides a system for locating a space target. The system comprises: at least N ranging sensors that are not on the same straight line, N being a natural number greater than or equal to 3; a computing and processing module that is connected to all ranging sensors for communication; and the computing and processing module is used to execute the above-mentioned method for locating a space target.

[0128] The above is only the preferred embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various changes and variations. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present application should be included in the protection scope of the present application. The above is only a specific implementation method of the present application, but the protection scope of the present application is not limited thereto. Any technician familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the protection scope of the claims.

Claims

1. A method for locating a space target, characterized in that: The method comprises the following steps: S1, preset reference coordinate system; S2, setting N distance measuring sensors that are not on the same straight line, and calibrating the position coordinates of each distance measuring sensor based on the reference system, where N is a natural number greater than or equal to 3; S3, based on the observed distances of each of the plurality of target points on the observed object obtained by three ranging sensors that are not in a straight line among the N ranging sensors at any sampling time, and the position coordinates of each of the three ranging sensors in the reference coordinate system at any same sampling time, calculating the position coordinates of each target point in the reference coordinate system at any sampling time; S4, based on a plurality of continuous sampling moments and the position coordinates of the plurality of target points in the reference coordinate system, tracking and analyzing the position and posture of the observed object.

2. The method according to claim 1, characterized in that In step S2, the position coordinates of each ranging sensor are calibrated based on the reference system, which is specifically implemented as follows: a number of marking points are set as scales on the X, Y, and Z axes of the reference coordinate system, and based on the observation distance of each ranging sensor to a marking point on the X, Y, and Z axes, and the coordinates of all marking points in the reference coordinate system, the position coordinates of each ranging sensor in the reference coordinate system are calculated and calibrated.

3. The method according to claim 1 or 2, characterized in that The following processing is also performed before step S3: step S31, determining the distance measuring sensor that has moved among the N distance measuring sensors relative to the previous sampling moment; and recalibrating the position coordinates of the distance measuring sensor that has moved based on the reference system.

4. The method according to claim 3, characterized in that In step S3, the observed distance of the specific target point is obtained at any sampling time, and three ranging sensors that are not on the same straight line are used as the relevant ranging sensors of the specific target point at any sampling time; the relevant ranging sensors of different target points at the same sampling time are the same or different, and the relevant ranging sensors of the same target point at different sampling times are the same or different.

5. The method according to claim 4, characterized in that In step S3, the position coordinates of each target point in the reference coordinate system at any sampling moment are calculated, which is implemented as follows: at any sampling moment, after obtaining the three observation distances corresponding to each target point through the relevant ranging sensor of each specific target point, the position coordinates of each target point in the reference coordinate system are calculated in real time.

6. The method according to claim 4, characterized in that In step S3, the position coordinates of each target point in the reference coordinate system at any sampling moment are calculated, which is implemented as follows: three observation distances of each target point and the corresponding identification of the relevant ranging sensor are obtained and recorded at each sampling moment; after an observation cycle, the position coordinates of each ranging sensor at each sampling moment in the observation cycle are combined to calculate the position coordinates of each target point in the reference coordinate system at each sampling moment in the observation cycle.

7. The method according to claim 5 or 6, characterized in that In the process of calculating the position coordinates of each target point in the reference coordinate system, one of the three observation distances obtained by the relevant ranging sensor of the specific target point at any sampling moment is replaced by its geometric theoretical value; the geometric theoretical value of the observation distance is calculated according to the spatial geometric relationship between the three observation distances obtained by the relevant ranging sensor of the specific target point at any sampling moment, in combination with the position coordinates of the relevant ranging sensor in the reference coordinate system and the other two of the three observation distances except the replaced observation distance, using an incremental step proportional to the measurement error of the ranging sensor, and performing cyclic iteration to calculate the target theoretical value of the replaced observation distance, and the target theoretical value calculated in each round is compared with the observation distance to obtain the geometric theoretical value.

8. A system for locating a space target, characterized in that: The system includes: at least N ranging sensors that are not on the same straight line, N being a natural number greater than or equal to 3; a computing and processing module that is communicatively connected to all the ranging sensors; and the computing and processing module is used to execute the method for locating a space target as described in any one of claims 1 to 7.

9. The system according to claim 8, characterized in that The distance measuring sensor is a depth camera, and each depth camera is the same.

Images