CN110111356B - Method for calculating rotating shaft direction and rotating angular velocity of moving rotating object - Google Patents

Method for calculating rotating shaft direction and rotating angular velocity of moving rotating object Download PDF

Info

Publication number
CN110111356B
CN110111356B CN201910218677.5A CN201910218677A CN110111356B CN 110111356 B CN110111356 B CN 110111356B CN 201910218677 A CN201910218677 A CN 201910218677A CN 110111356 B CN110111356 B CN 110111356B
Authority
CN
China
Prior art keywords
point
point cloud
matrix
angular velocity
time
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910218677.5A
Other languages
Chinese (zh)
Other versions
CN110111356A (en
Inventor
赵玺
邹建华
李家钊
敖毅波
徐笑昂
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xian Jiaotong University
Original Assignee
Xian Jiaotong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Xian Jiaotong University filed Critical Xian Jiaotong University
Priority to CN201910218677.5A priority Critical patent/CN110111356B/en
Publication of CN110111356A publication Critical patent/CN110111356A/en
Application granted granted Critical
Publication of CN110111356B publication Critical patent/CN110111356B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/292—Multi-camera tracking
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Landscapes

  • Engineering & Computer Science (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)

Abstract

The invention provides a method for resolving the rotating shaft direction and the rotating angular speed of a moving rotating object, which comprises the steps of firstly registering adjacent point clouds at the moment by using a closest point iterative point cloud registration algorithm, and constructing a local track matrix by using a registration matrix; secondly, position information of the points is obtained according to the local track matrix, the point tracks are fitted, the speed of the points at a certain moment is obtained, and finally an equation is established according to the rigid body rotation physical process, and a rotation parameter calculation step is carried out; the method gets rid of dependence on characteristic points, and describes the motion state of the target at a certain moment by using a local track matrix; the method has good robustness on the local deletion of the point cloud caused by target self-occlusion, and avoids errors caused by incomplete tracks of the characteristic points in the sequence point cloud; the method can effectively solve the rotating shaft and the rotating angular speed, and particularly when the angular speed of the rotating object is in variable-speed motion, the solved result can better track the change of the set angular speed, the solving precision is higher, and the solving advantage is more obvious.

Description

Method for calculating rotating shaft direction and rotating angular velocity of moving rotating object
Technical Field
The invention belongs to the field of moving object rotation parameter calculation and point cloud scanning, and particularly relates to a method for calculating the rotating shaft direction and the rotating angular speed of a moving rotating object.
Background
In the aerospace field, motion parameter solution for space non-cooperative targets is a precondition for target tracking, detection and on-orbit approach, so that the motion parameter research for the space non-cooperative targets is an important research field. One commonly used technical means is to observe and scan the non-cooperative target by using a laser sensor or a camera sensor to obtain a sequence three-dimensional point cloud of the non-cooperative target. In general, motion parameters of the non-cooperative target, such as a motion speed, a rotation axis direction and a rotation angular speed, can be obtained according to the sequence point cloud.
At present, a plurality of algorithms for solving the rotating shaft direction and the rotating angular velocity are available, and the algorithms are roughly divided into two types according to the main principle. The first method is to solve through a characteristic point track, and comprises the following main steps: (1) selecting characteristic points or mark points in the point cloud; (2) finding out the position track of the characteristic points from the sequence point cloud; (3) and establishing a motion equation to solve the rotating shaft direction and the rotating angular speed. The method depends on the selection of the feature points and the quality of the track quality, and the algorithm has two limitations: the first limitation is that the feature points need to be manually selected according to experience design rules, so that the situation that some non-cooperative targets have fewer or missing feature points under a certain rule may occur; the second limitation is that when the sensor scans the non-cooperative target, the situation that the characteristic points disappear on continuous frames of point clouds in the sequence point cloud due to the self-shielding effect of the non-cooperative target can occur, and the situation that the track of the characteristic points is incomplete is caused. The second algorithm is to perform point cloud registration through point clouds of adjacent frames, establish an equation according to a registration matrix and a rodregs formula so as to solve the rotation axis direction and the rotation angular velocity, and the rotation angular velocity and the rotation axis direction of the non-cooperative target solved by using the registration matrix between the point clouds are actually an average rotation velocity and an average rotation axis direction of the non-cooperative target in the period of time and cannot reflect the real motion condition of a certain moment. When the rotation speed of the non-cooperative target is changed, the estimated speed of the algorithm generates a large error.
Disclosure of Invention
Aiming at the defects of the prior art, the invention provides a method for resolving the rotating shaft direction and the rotating angular speed by using sequence point cloud data. The method solves the rotating shaft direction and the rotating angular speed by constructing the local track matrix to describe the overall motion condition of the non-cooperative target at a certain moment, gets rid of the dependence of characteristic points and effectively improves the accuracy of the rotating angular speed solution.
The method for calculating the rotating shaft direction and the rotating angular speed of the moving rotating target provided by the invention comprises the following steps of:
step 1, when solving tiOf time of dayWhen the rotation axis and the rotation angular velocity are matched, corresponding tiPoint cloud of time, respectively corresponding to ti-1Time ti+1Obtaining a registration matrix M by utilizing a closest point iterative point cloud registration algorithmi-1And Mi+1(ii) a The registration matrix M is of the form:
Figure BDA0002002875160000021
in the formula, R is a rotation change matrix with the size of 3 multiplied by 3, and T is a translation vector with the size of 1 multiplied by 3;
will tiExpansion of time point cloud data to Di＝[xi，k yi，k zi，k 1]m×4，k∈[1，m]M is the total number of the points in the frame point cloud; from the registration equation, D can be obtainediMi-1＝Di-1(ii) a For sequence point clouds, when t is foundiTime ti+1When registering the matrix between the point clouds at the moment, if i is more than or equal to 1, M is usedi-1To obtain M'i+1As a solution to Mi+1Is determined using a closest point iterative point cloud registration algorithm (ICP) iteration, M'i+1The construction method is as follows:
Figure BDA0002002875160000022
for tiThe corresponding expansion vector of any point P (X, y, z) of the time point cloud is X (X, y, z,1), and the corresponding t is seti+1The extended vector of the points on the time point cloud is Xi+1(xi+1，yi+1，zi+11) which can be calculated in the following way:
Xi+1＝XMi+1
at tiThe local trajectory at a moment is obtained by multiplying a certain point by a registration matrix sequence, and the form of an extended matrix XR of a design X is as follows:
Figure BDA0002002875160000031
the local trajectory matrix is designed as follows:
Figure BDA0002002875160000032
and multiplying the matrix XR and the matrix TM to obtain the position locus PS:
Figure BDA0002002875160000033
the first column, the second column and the third column of the PS are respectively the position tracks of the x-axis, the y-axis and the z-axis of the point.
Respectively fitting F (F) (T) to the x-axis, y-axis and z-axis position tracks of the points by using a unary quadratic polynomial function with time as a variable and position as a function value, deriving the fitting function to obtain the change relation v (F' (T) of speed and time on the local track, and further calculating TiAt the moment the velocity v of the point in each directionx、vy、vz。
Let aiSelecting n points from the time point cloud as
Figure BDA0002002875160000034
Wherein k is 1. Respectively solving n points at t by using point cloud track matrixiVelocity of time of day
Figure BDA0002002875160000035
Where k 1.. n, resulting in n points at tiResolving rotation parameters after the time position and the speed in each direction;
and 3, solving the rotation parameter: firstly, according to a motion equation, a t moment (broadly referred to as t) which can be solved according to the steps is constructediTime of day) position and velocity information solves an equation for the rotational axis and angular velocity of rotation,
and (3) constructing a motion equation: according to the theory of physical motionThe motion of the rigid body can be decomposed into two parts of translation along with the mass center and rotation around a rotating shaft passing through the mass center, and the non-cooperative target is regarded as the rigid body; set time t (broadly denoted t)iTime of day) is O ═ Ox oy oz]TTranslation speed is Vo＝[vox voy voz]TAccording to the rigid body rotation theory, its rotation around the rotation axis can be decomposed into rotation around its centroid with rotation angular velocity of Ωt＝[ωt，x ωt，y ωt，z]TThen any point P on the rigid bodyi＝[pix piy piz]Velocity of movement V at time ti＝[vt，i，x vt，i，y vt，i，z]TCan be expressed in that its motion form can be expressed as follows:
Vi＝Vo+Ωt×OPi (1)
in the formula, OPiIs PiThe distance of the point to the centroid O;
expanding the formula (1) into a matrix form:
Figure BDA0002002875160000041
obtained by calculation of equation (2)
Figure BDA0002002875160000042
The result on the right side of equation (3) is only related to the target rotation angular velocity, the target centroid coordinate and the centroid translation velocity, and is not related to the point on the selected point cloud, that is, for the point cloud at a certain moment, the rightmost side of the equation is a constant vector, and the constant vector is defined as C ═ Cx cy cz]TIf this is the case, the above equation becomes:
Figure BDA0002002875160000043
finishing the step (4) to obtain:
Figure BDA0002002875160000044
in the above equation, there are 6 unknown parameters ωx、ωy、ωz、cx、cy、czWherein, ω ist，x，ωt，y，ωt，zI.e. the angular velocities of the x-axis, y-axis and z-axis components of the moving rotating object at time t, cx、cy、czAre all constants; the matrix equation can be established for any one feature point, and three equations can be obtained; the 6 unknowns can be solved by selecting two or more points in the same frame of point cloud, and the following equations can be obtained by simultaneously connecting n points of the same point cloud at the same moment:
Figure BDA0002002875160000051
and (3) solving the parameters by a least square method, wherein the least square solution is as follows:
Figure BDA0002002875160000052
the solution obtained by the solution
Figure BDA0002002875160000053
The form is as follows:
Figure BDA0002002875160000054
in the formula, ωt，x，ωt，y，ωt，zThe angular velocity of each axis at the moment t of moving the rotating object is obtained;
the position and speed obtained at time t from any n points obtained in step 2Substituting the degree into the equation (6) in the step 3 to obtain the target rotation angular velocity at the time t
Figure BDA0002002875160000055
Here, time t generally refers to t in step 11Time t2Time t3At any of the times, the axis of rotation is pointed at ωx、ωy、ωzDetermined according to the vector addition rule.
Compared with the prior art, the invention has at least the following beneficial effects:
1. the resolving method gets rid of dependence on characteristic points, and describes the motion condition of a target at a certain moment by using a local track matrix; the method has good robustness on the local deletion of the point cloud caused by target self-occlusion, and avoids errors caused by incomplete tracks of the characteristic points in the sequence point cloud;
2. the method comprehensively considers the integral registration condition of the point clouds, and because the local track matrix is obtained by integral registration of adjacent frame point clouds, the track of a certain point generated by using the local track matrix not only reflects the motion track of the certain point, but also comprises the motion condition of the target whole at the moment, thereby avoiding the result calculation error caused by the track error of a single point;
3. compared with the method using a single registration matrix for calculation, the method takes the motion situation of a point before and after a certain moment into account when describing the motion situation of the point at the certain moment, so that the motion speed of the point is more accurate.
Drawings
FIG. 1 is a schematic flow diagram of the present invention;
FIG. 2 is a block diagram of solving and resolving a local trajectory matrix according to the present invention;
FIG. 3 is a non-cooperative target model in an embodiment of the present invention;
FIG. 4 is a scanning point cloud of a sensor for a non-cooperative target in an embodiment of the present invention;
FIG. 5 is a comparison of the present invention and a solution of rotational angular velocity according to the Rodrigues equation set up in accordance with an embodiment of the present invention;
FIG. 6 is a comparison of the present invention and the calculation of the shaft direction according to the Rodrigues equation in accordance with the present invention.
Detailed Description
The invention is described in detail below with reference to the figures and the specific embodiments.
As shown in fig. 1, the method for calculating the direction of the rotating shaft and the rotating angular velocity of the moving rotating target of the present invention specifically comprises the following steps:
the obtained sequence point cloud is named as t according to the time sequence0Time point cloud, t1Time point cloud, t2Time point cloud, t3A point cloud of time. -; due to the fact that at t0The rotation information does not exist before the moment, so the rotation parameter at the moment can not be accurately estimated, and the method estimates the rotation parameter from t1And the rotating shaft direction and the rotating angular speed corresponding to the target at the moment and later.
Step 1, constructing tiLocal trajectory matrix at time:
step 11, obtaining a registration matrix Mi-1And Mi+1: t to be solvediThe rotation axis at the time and the rotation angular velocity are combined with the corresponding tiThe point cloud of the moment is respectively ti-1Time t andi+1corresponding point clouds at the moment, and obtaining a registration matrix M by utilizing a closest point iteration point cloud registration algorithmi-1And Mi+1(ii) a The registration matrix M is of the form:
Figure BDA0002002875160000071
in the formula, R is a rotation change matrix with the size of 3 multiplied by 3, and T is a translation vector with the size of 1 multiplied by 3;
will tiExpansion of time point cloud data to Di＝[xi，k yi，k zi，k 1]m×4，k∈[1，m]And m is the total number of the points in the frame point cloud. From the registration equation, D can be obtainediMi-1＝Di-1(ii) a For sequence point clouds, when t is foundiTime ti+1When the matrix is de-registered between the point clouds at the moment, if i is more than or equal to 1, M is usedi-1To obtain M'i+1As a solution to Mi+1Is iterated, M ', of the initial solution of the nearest point cloud registration algorithm't+1The construction method is as follows:
Figure BDA0002002875160000072
step 12, constructing a local track matrix: using a registration matrix Mi-1And Mi+1Constructing a local trace matrix TM, the matrix TM representing tiThe specific form of the motion information of the whole point cloud at any moment is as follows:
Figure BDA0002002875160000073
wherein, I4×4Is an identity matrix with the size of 4;
step 2, acquiring a track of a plurality of points, and performing the following operation on each point:
for tiThe corresponding expansion vector of any point P (X, y, z) of the time point cloud is X (X, y, z,1), and the corresponding t is seti+1The extended vector of the points on the time point cloud is Xi+1(xi+1，yi+1，zi+11) which can be calculated in the following way:
Xi+1＝XMi+1
at tiThe local trajectory at a moment is obtained by multiplying a certain point by a registration matrix sequence, and the form of an extended matrix XR of a design X is as follows:
Figure BDA0002002875160000074
step 21, select tiThe extended matrix of a certain point P (X, y, z) on the time point cloud is X ═ X, y, z,1, and the extended matrix XR of X is designed as follows
Figure BDA0002002875160000081
Then ti-1、ti、ti+1The solution process for this point position trajectory PS for three time instants is as follows:
Figure BDA0002002875160000082
the first, second and third columns of PS are the x-, y-and z-axis position changes of the point, respectively.
Step 22, respectively, the locus of the point pair in the three directions of the x axis, the y axis and the z axis is subjected to unary quadratic polynomial function fitting by using time as a variable and using the position as a function value to obtain a locus function F (T), the locus function is subjected to derivation to obtain a change relation v (F' (T) of speed and time on a local locus, and then T is obtainediAt the moment the velocity v of the point in each directionx、vyAnd vz。
Let aiSelecting n points from the time point cloud as
Figure BDA0002002875160000083
Wherein k is 1. Respectively solving n points at t by using point cloud track matrixiVelocity of time of day
Figure BDA0002002875160000084
Where k 1.. n, resulting in n points at tiResolving rotation parameters after the time position and the speed in each direction;
step 3, resolving the rotating shaft direction and the rotating angular speed:
and (3) constructing a motion equation: according to the physical motion theory, the motion of the rigid body can be decomposed into two parts of translation along with the mass center and rotation around a rotating shaft passing through the mass center, and the non-cooperative target is regarded as the rigid body; set time t (broadly denoted t)iTime of day) is O ═[ox oy oz]TTranslation speed is Vo＝[vox voy voz]TAccording to the rigid body rotation theory, the rotation around the rotation axis can be decomposed into rotation around the mass center, and the rotation angular speed is set to be omegat＝[ωt，x ωt，yωt，z]T. Any point P on the rigid bodyi＝[pix piy piz]Velocity of movement V at time ti＝[vt，i，x vt，i，yvt，i，z]TCan be expressed in such a way that the motion form thereof can be expressed as follows:
Vi＝Vo+Ωt×OPi (1)
in the formula, OPiIs PiThe distance of the point to the centroid O;
expanding the formula (1) into a matrix form:
Figure BDA0002002875160000091
by arranging the formula (2), the product can be obtained
Figure BDA0002002875160000092
The result on the right side of equation (3) is only related to the target rotation angular velocity, the target centroid coordinate and the centroid translation velocity, and is not related to the point on the selected point cloud, that is, for a point cloud at a certain moment, the right side of equation is a constant vector, and the constant vector is defined as C ═ Cx cy cz]TIf this is the case, the above equation becomes:
Figure BDA0002002875160000093
finishing the step (4) to obtain:
Figure BDA0002002875160000094
in the above equation, there are 6 unknown parameters ωx、ωy、ωz、cx、cy、czFor any one feature point, the matrix equation can be established, and three equations can be obtained; the 6 unknowns can be solved by selecting two or more points in the same frame of point cloud, and the following equations can be obtained by simultaneously connecting n points of the same point cloud at the same moment:
Figure BDA0002002875160000101
and (3) solving the parameters by a least square method, wherein the least square solution is as follows:
Figure BDA0002002875160000102
the solution obtained by the solution
Figure BDA0002002875160000103
The form is as follows:
Figure BDA0002002875160000104
in the formula, ωt，x，ωt，y，ωt，zThe angular velocity of each axis at the moment t of moving the rotating object is obtained;
substituting the position and speed determined at time t by any n points determined in step 2 into equation (6) in step 3 to obtain the target rotation angular speed at time t as
Figure BDA0002002875160000105
Here, time t generally refers to t in step 11Time t2Time t3Any of the times. The rotation axis is directed tox、ωy、ωzDetermined according to the vector addition rule.
From time t (here broadly denoted t)iTime) point cloud, selecting any n points with the position pk(xt，k，yt，k，zt，k) And repeating the step 2 to respectively obtain the speed v of n points at the time t by using the point cloud local track matrixt，k，x、vt，k，y、vt，k，z(ii) a In the present embodiment, n is 20, and a position-related information matrix H is constructed using n point position information, and a velocity information vector Y is constructed using n point velocity information.
The position-related information matrix H has the following specific form:
Figure BDA0002002875160000111
the velocity information vector Y is in the specific form:
Figure BDA0002002875160000112
step 32, substituting the position-related information matrix H and the constructed velocity information vector Y obtained in step 31 into a solution formula of the partial angular velocity to obtain the partial angular velocity, wherein the solution formula of the partial angular velocity specifically comprises:
Figure BDA0002002875160000113
solve to obtain a solution
Figure BDA0002002875160000114
The form is as follows:
Figure BDA0002002875160000115
in the formula, ωt，x，ωt，y，ωt，zThe angular velocity is the partial angular velocity of each axis at the moment t of the moving rotating target;angular velocity of rotation of
Figure BDA0002002875160000116
The direction of rotation of the rotating shaft is defined byt，x、ωt，y、ωt，zDetermined according to the vector addition rule.
Example (b):
comparing the method provided by the invention with a method for resolving rotation and rotation angular velocity by using a single registration matrix (hereinafter referred to as a ' Rodrigue method ') by using a Rodrigues's Formula, and verifying the effectiveness and superiority of the method provided by the invention; in order to better compare the two algorithms, the same point cloud sequence obtained by sampling the same model is used in the comparison process, and the two comparison algorithms use the same obtained registration matrix, and only the subsequent steps using the registration matrix are different.
Scanning and sampling the model shown in the figure 4 by using an analog sensor, wherein in the sampling process, the model performs variable-speed motion around a rotating shaft; the simulation sensor generates sequence point cloud, and the effect of the point cloud is shown in figure 4; and comparing the resolving effect of the obtained sequence point cloud in the rotating shaft direction and the rotating angular speed by using the method provided by the invention and a Roche method.
FIG. 5 is a comparison experiment result of the rotation angular velocity calculation effect obtained by the algorithm proposed by the present invention and the Rogowski algorithm, and the left registration matrix calculation method in FIG. 5 uses tiTime point cloud and ti-1The registration matrix obtained by registering the point clouds (the last frame of point clouds in the sequence point clouds) is solved by using a Roche method, a right-side registration matrix is solved by using a method and a method using tiTime point cloud and ti+1The registration matrix obtained by registering the point clouds (the next frame of point clouds in the sequence point clouds) is solved by using a Rogowski method, so that the Rogowski algorithm has a good effect when rotating at a constant speed, but when the speed changes, the Rogowski algorithm using the left registration matrix can generate a 'lagging' condition, and the Rogowski algorithm using the right registration matrix can generate an 'advancing' condition.
Fig. 6 shows the experimental effect of the algorithm proposed by the present invention and the rogowski algorithm (including the left registration matrix and the right registration matrix) on the error of the rotation axis direction. When the rotating shaft is fixed, the two algorithms can well estimate the direction of the rotating shaft. The algorithm error provided by the invention is stable, and the error of the left registration matrix or the right registration matrix can fluctuate greatly.
The above-described implementation method is used for illustrating the principle, and is convenient for the person skilled in the art to understand the present invention, it should be pointed out that the present invention is not limited only to this, and the person skilled in the art can make modifications to the present invention within the scope of the claims; algorithms not described in detail in the present invention are common general knowledge in the art.

Claims (9)

1. The method for calculating the rotating shaft direction and the rotating angular speed of the moving rotating object is characterized by comprising the following steps of:
step 1, constructing a local track matrix: when solving for tiWhen the rotation axis direction and the rotation angular velocity are at the moment, t is setiIterative point cloud registration algorithm and t by utilizing closest point for point cloud of timei-1Obtaining a registration matrix M by time point cloud registrationi-1(ii) a Using tiPoint cloud sum of time ti-1Registration matrix M obtained by moment point cloudi-1The registration calculation of the point cloud at the next moment is initialized by the information of (a), and the registration calculation is further iterated to obtain tiTo ti+1Registration matrix M of time instantsi+1Using the registration matrix Mi-1、Mi+1Constructing a local track matrix TM;
step 2, solving the position and the speed of the point: for tiMultiplying any point P on the point cloud of the moment by the local track matrix of the point obtained in the step 1 and the expansion vector of the point to obtain a position track PS of the point, performing curve fitting on the position track PS, and solving t according to a fitting curveiAt the moment the velocity v of the point in each directionx、vy、vz；
Step 3, solving the rotation parameters: selecting tiAny n points in the point cloud of the moment are processed according to the method of step 2Method of finding at tiN points selected from the time point cloud are respectively at tiPosition trajectory PS and speed at time; constructing equation of motion of moving object, simultaneous tiThe positions and the speeds of n points in the moment point cloud are solved to obtain a moving rotating target tiThe angular velocity of each axis at the moment; further, the moving rotating object t is obtained according to the angular velocityiThe rotational speed and the direction of the axis of rotation.
2. The method for resolving the rotating shaft direction and the rotating angular velocity of a moving rotating object according to claim 1, wherein the point cloud in step 1 is obtained by scanning and sampling through a sensor.
3. The method of resolving rotational axis direction and rotational angular velocity of a moving rotating object of claim 1, wherein the registration matrix M constructed in step 1 is of the form:
Figure FDA0002929754720000011
in the formula, R is a rotation change matrix, and T is a translation vector; will tiExpansion of time point cloud data to Di＝[xi,k yi,k zi,k1]m×4，k∈[1,m]M is the total number of the points in the frame point cloud; from the registration equation, D can be obtainediMi-1＝Di-1(ii) a For the point cloud which rotates at a constant speed or is sampled at a high frequency, the point cloud is obtained by Mi-1Can obtain M'i+1M'i+1As a solution to Mi+1Is iterated, M ', of the initial solution of the nearest point cloud registration algorithm'i+1The construction method is as follows:
Figure FDA0002929754720000021
4. the rotating axis of a moving rotating object of claim 1 andthe method for calculating the rotational angular velocity is characterized in that a local track matrix TM representing t is constructed in the step 1iThe motion information of the whole point cloud at the moment and the local track matrix TM have the specific form:
Figure FDA0002929754720000022
in the formula I4×4Is an identity matrix of size 4.
5. The method for calculating the direction and the angular velocity of the rotating shaft of the moving rotating object according to claim 4, wherein the step 2 of point locus fitting and velocity calculation comprises the following specific steps:
first, select tiAn expansion vector of an arbitrary point P (X, y, z) on the time point cloud is (X, y, z,1), and corresponds to ti+1The expansion vector of the point in the time point cloud is Xi+1(xi+1,yi+1,zi+11) which can be calculated in the following way:
Xi+1＝XMi+1
at tiThe local trajectory at a time instant is obtained by multiplying the point by the sequence of registration matrices, the extended matrix XR for the point being of the form:
Figure FDA0002929754720000023
then ti-1、ti、ti+1The point position trajectory PS at three times is specifically as follows:
Figure FDA0002929754720000024
the first column, the second column and the third column in the PS correspond to the position change of the point in the x axis, the y axis and the z axis respectively;
then, the positions in the three directions of the x axis, the y axis and the z axis are respectively processed by taking the time as a variable and taking the position as a functionFitting a binary polynomial function to obtain a track function F ═ F (T), and deriving the fitted track function to obtain a change relation v ═ F' (T) of speed and time on the local track, and further calculating TiAt the moment the velocity v of the point in each directionx、vyAnd vz。
6. The method for calculating the direction of the rotating shaft and the rotating angular velocity of the moving rotating object according to claim 1, wherein in step 3, a motion equation constructed for any point in the point cloud is Y ═ H θ, where H is a position-related information matrix, Y is a velocity information vector, and θ is an unknown parameter to be solved, and the specific form is as follows:
Figure FDA0002929754720000031
wherein the content of the first and second substances,
Figure FDA0002929754720000032
i.e. moving rotating object tiAngular velocity components of time x, y and z axes, cx、cy、czAre all constants; any point P on the rigid bodyi＝[pix piy piz]。
7. The method for resolving the direction of the rotating shaft and the rotating angular velocity of a moving rotating object according to claim 6, wherein in step 3, the least square solution of the motion equation of Y ═ H θ is:
Figure FDA0002929754720000033
the form is as follows:
Figure FDA0002929754720000034
wherein the content of the first and second substances,
Figure FDA0002929754720000035
i.e. moving rotating object tiAngular velocity of the respective axis at time cx、cy、czAre all constants; angular velocity of rotation of
Figure FDA0002929754720000036
The rotating direction of the rotating shaft is controlled by
Figure FDA0002929754720000037
Solving according to the vector addition rule.
8. The method for resolving the direction of the rotation axis and the rotational angular velocity of a moving rotating object according to claim 1, wherein n in step 3 has a value in the range of [2, + ∞ ].
9. The method for resolving direction of rotation axis and angular velocity of moving rotating object of claim 8, wherein n-20.
CN201910218677.5A 2019-03-21 2019-03-21 Method for calculating rotating shaft direction and rotating angular velocity of moving rotating object Active CN110111356B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910218677.5A CN110111356B (en) 2019-03-21 2019-03-21 Method for calculating rotating shaft direction and rotating angular velocity of moving rotating object

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910218677.5A CN110111356B (en) 2019-03-21 2019-03-21 Method for calculating rotating shaft direction and rotating angular velocity of moving rotating object

Publications (2)

Publication Number Publication Date
CN110111356A CN110111356A (en) 2019-08-09
CN110111356B true CN110111356B (en) 2021-05-28

Family

ID=67484384

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910218677.5A Active CN110111356B (en) 2019-03-21 2019-03-21 Method for calculating rotating shaft direction and rotating angular velocity of moving rotating object

Country Status (1)

Country Link
CN (1) CN110111356B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110654381B (en) * 2019-10-09 2021-08-31 北京百度网讯科技有限公司 Method and device for controlling a vehicle
CN110850662B (en) * 2019-11-01 2022-06-24 上海航天控制技术研究所 Multi-degree-of-freedom optical search system
CN112800594B (en) * 2021-01-08 2022-12-13 天津中环领先材料技术有限公司 Grinding disc loss algorithm based on silicon wafer grinding equipment

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102607534A (en) * 2012-03-13 2012-07-25 上海交通大学 Satellite relative attitude measuring method based on structure from motion
CN105628026A (en) * 2016-03-04 2016-06-01 深圳大学 Positioning and posture determining method and system of mobile object
CN107655473A (en) * 2017-09-20 2018-02-02 南京航空航天大学 Spacecraft based on SLAM technologies is with respect to autonomous navigation system
CN108534784A (en) * 2018-03-13 2018-09-14 北京控制工程研究所 A kind of non-cooperative Spacecraft spin angle velocity method of estimation based on space Circular test

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9355462B2 (en) * 2013-05-08 2016-05-31 Caterpillar Inc. Motion estimation system utilizing point cloud registration
CN105953795B (en) * 2016-04-28 2019-02-26 南京航空航天大学 A kind of navigation device and method for the tour of spacecraft surface
US10371818B2 (en) * 2017-04-18 2019-08-06 Raytheon Company Motion compensation for dynamic imaging
CN107665496B (en) * 2017-08-25 2020-04-10 北京控制工程研究所 Three-dimensional attitude registration method
CN109143205B (en) * 2018-08-27 2020-07-24 深圳一清创新科技有限公司 External parameter calibration method and device for integrated sensor

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102607534A (en) * 2012-03-13 2012-07-25 上海交通大学 Satellite relative attitude measuring method based on structure from motion
CN105628026A (en) * 2016-03-04 2016-06-01 深圳大学 Positioning and posture determining method and system of mobile object
CN107655473A (en) * 2017-09-20 2018-02-02 南京航空航天大学 Spacecraft based on SLAM technologies is with respect to autonomous navigation system
CN108534784A (en) * 2018-03-13 2018-09-14 北京控制工程研究所 A kind of non-cooperative Spacecraft spin angle velocity method of estimation based on space Circular test

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Point cloud based relative pose estimation of a satellite in close range;lujiang liu et al.;《sensors》;20160604;第1-18页 *
基于点云矩形面特征的故障航天器位姿测量;郁丰等;《中国惯性技术学报》;20180430;第26卷(第2期);第255-260页 *

Also Published As

Publication number Publication date
CN110111356A (en) 2019-08-09

Similar Documents

Publication Publication Date Title
CN110111356B (en) Method for calculating rotating shaft direction and rotating angular velocity of moving rotating object
Wu et al. Hand-eye calibration: 4-D procrustes analysis approach
Huang et al. Observability-based consistent EKF estimators for multi-robot cooperative localization
CN108645416B (en) Non-cooperative target relative navigation simulation verification method based on vision measurement system
CN105096341B (en) Mobile robot position and orientation estimation method based on trifocal tensor and key frame strategy
CN114399528B (en) Three-dimensional space moving target tracking method and related device based on two-dimensional image
CN108710125A (en) For target following apart from method of bearing filtering
CN112652003B (en) Three-dimensional point cloud registration method based on RANSAC measure optimization
CN108152812B (en) Improved AGIMM tracking method for adjusting grid spacing
White et al. An iterative pose estimation algorithm based on epipolar geometry with application to multi-target tracking
CN113030940B (en) Multi-star convex type extended target tracking method under turning maneuver
CN116224320B (en) Radar target tracking method for processing Doppler measurement under polar coordinate system
CN111426322B (en) Adaptive target tracking filtering method and system for simultaneously estimating state and input
Frencl et al. Tracking with range rate measurements: turn rate estimation and particle filtering
CN112986977B (en) Method for overcoming radar extended Kalman track filtering divergence
CN113628254A (en) Target track determination method based on mobile platform and related equipment
CN113689501A (en) Double-machine cooperative target machine positioning and tracking control method based on convergence point
CN109474892B (en) Strong robust sensor network target tracking method based on information form
CN109754412B (en) Target tracking method, target tracking apparatus, and computer-readable storage medium
CN111796271B (en) Target tracking method and device under constraint of proportional guidance destination
Comport et al. Efficient model-based tracking for robot vision
Nie et al. Adaptive tracking algorithm based on 3D variable turn model
Liu et al. Adaptive MCMC particle filter for tracking maneuvering target
Gong Kalman Filter-based Signal Processing for Robot Target Tracking
CN108692731B (en) Non-equal standard error and zero-mean sphere probability error representation method

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant