# Pose Determination for Malfunctioned Satellites Based on Depth Information

Authors: Feng Yu; Yi Zhao; Yanhua Zhang
Journal: International Journal of Aerospace Engineering
 (2019)

---

## Abstract

Autonomous on-orbit servicing is the future space activity which can be utilized to extend the satellite life. Relative pose estimation for a malfunctioned satellite is one of the key technologies to achieve robotic on-orbit servicing. In this paper, a relative pose determination method by using point cloud is presented for the final phase of the rendezvous and docking of malfunctioned satellites. The method consists of three parts: (1) planes are extracted from point cloud by utilizing the random sample consensus algorithm. (2) The eigenvector matrix and the diagonal eigenvalue matrix are calculated by decomposing the point cloud distribution matrix of the extracted plane. The eigenvalues are utilized to recognize rectangular planes, and the eigenvector matrix is the attitude rotation matrix from the sensor to the plane. The solution of multisolution problem is also presented. (3) An extended Kalman filter is designed to estimate the translational states, the rotational states, the location of mass center, and the moment-of-inertia ratios. Because the method only utilizes the local features without observing the whole satellite, it is suitable for the final phase of rendezvous and docking. The algorithm is validated by a series of mathematical simulations.


---

## Body

## 1. Introduction

The on-orbit servicing technology, which is a very challenging research topic, is commonly regarded to be utilized to extend the life of malfunctioned satellites. Although several on-orbit servicing demonstration cases were performed [1], autonomous on-orbit servicing operation under realistic conditions has not been carried out in space up to now. A series of technologies including autonomous navigation, control, and robotic arm are still immature and should be furtherly developed.Because the relative pose measurement for a malfunctioned satellite is the precondition of on-orbit servicing [2], many researchers have made many efforts in order to solve this important problem. The relative navigation of TECSAS/DEOS, being studied at the center for robotics and machinery at the German Aerospace Center (DLR), is realized by a stereo vision system. In the automatic mode, a number of images are taken and dumped to ground segment and then are processed to estimate the relative position and the target motion [3]. The Argon system utilizes two different view-size cameras and a flash LIDAR. Complex image processing techniques are developed to measure the relative position and relative attitude of noncooperative targets [4, 5]. Volpe et al. proposed an on-orbit relative pose and shape estimation with the use of a monocular camera and a distance sensor [6, 7]. In China, Caixiu proposed a method by using a single camera to measure the relative pose by utilizing a solar panel triangle structure and a circular docking ring [8]. Miao et al. utilized solar panel components as the identification objects for noncooperative spacecraft pose determination [9]. Zhang et al. presented the relative pose measurement algorithm by using a circle and a noncoplanar feature point [10]. Huang et al. proposed a monocular visual servoing control method using only the edge lines of satellite brackets [11]. According to the above research, visual navigation is the main relative pose determination method in the relative state estimation for malfunctioned satellites [12]. However, the visual system sometimes suffers poor imaging quality in the space due to the special optical environment, which leads to the difficulty in subsequent processing. What is worse, the surface of the malfunctioned satellite is often covered by multilayer insulation (MLI) materials for the thermal protection. Because of this, optical features such as texture change according to the direction of lighting and view; the image features may not be obviously identified [13].Therefore, other viable image processing method or sensors were utilized by some researchers. Terui et al. used binocular stereo vision to calculate the dense 3D points of target satellite [13], and relative pose was obtained by using Iterative Closest Point (ICP). LIDAR has been employed for full 6DOF pose estimation [14]. Gómez Martínez et al. proposed a pose estimation method for a rocket body in orbit using a time-of-flight (TOF) camera [15]. Hillenbrand and Lampariello realized the motion estimation and model identification of free-floating targets by utilizing range data measurements [16]. The aforementioned literatures intrinsically utilize the target geometrical shape by processing a point cloud of 3D coordinates mapping the sensed target surface. The ICP method is commonly employed to calculate target pose parameters.However, Lichter and Dubowsky proposed a method to estimate the state, shape, and inertia parameters of noncooperative targets based on a series of distance images [17]. This method includes three parts: firstly, the coarse target pose can be estimated by condensing the range data of the target; secondly, the full dynamic states and inertial parameters are estimated by using Kalman filters; and finally, the shape of the target is estimated by using the estimated pose information and the raw range data. This proposed method is effective only when the whole target is observed. But it is difficult to obtain the complete distance image of the target at the close distance. Therefore, the author advised that a team of range imagers should be distributed uniformly to capture the whole target surface and the relative pose knowledge between sensors mounted on different satellites is required to be known with high accuracy. Literature [15] identified the rocket’s geometrical properties including the cylindrical body, nozzle, side tanks, and fairing from the point cloud in the target pose initialization.Inspired by literatures [15, 17], this paper proposes a new method to estimate the pose of a malfunctioned satellite based on the rectangle feature from point cloud. The complete distance image of the target is not necessary for this method, and only the local rectangular features are extracted to measure the relative position and attitude. Simulation results show the effectiveness of the proposed method.


## 2. Rectangular Plane Extraction, Recognition, and Pose Determination

### 2.1. Plane Extraction from Point Cloud

Because satellites are man-made objects, there are some regular components on satellites, such as the main body, solar panels, and docking ring. Although these components are not designed specifically to measure the relative pose of malfunctioned satellites, they have obvious geometric features for recognition and can be utilized to realize relative navigation. The rectangular feature is one of the typical geometric shapes on malfunctioned satellites. For example, the main body of most satellites and solar panels is composed of a variety of rectangles. Therefore, this paper proposes to select the rectangular planes as the identification object.The random sample consensus (RANSAC) algorithm is utilized to extract planes from the point cloud dataset. The algorithm can be described as follows: firstly, select a mathematical model; secondly, find the so-called minimum set from the original point cloud sample, and then estimate the parameters of the model by using the minimum set of points; thirdly, check the consistency of the remaining points with the estimated model, and add these qualified points into the point set; and finally, the parameters of the model can be reestimated using all the points that conform to the model.According to the aforementioned RANSAC algorithm, the specific steps for extracting the plane from the point cloud are proposed as follows:(1)
Select three pointsP1x1,y1,z1, P2x2,y2,z2, and P3x3,y3,z3 randomly in the point cloud dataset
(2)
Calculate the parameters of the planeS according to P1, P2, and P3
(3)
Suppose the plane thickness asε, and calculate the distance from any point to the plane in the point cloud:
(1)di=Axi+Byi+Czi+DA2+B2+C2
(4)
Find out how many points are satisfyingdi<ε
(5)
Repeat step 1~step 4K times, and select the plane that includes the most points, where K can be calculated by the following formula:
(2)K=log1−Φlog1−1−τ3,where τ is the proportion of the outer point in the point cloud and Φ is the probability that the optimal plane can be found after K calculation


### 2.2. Rectangular Plane Recognition

Some components of a malfunctioned satellite are composed of rectangular planes. The dimensions of these rectangular planes are known for a specific malfunctioned satellite. After the plane is extracted from the point cloud, the point cloud distribution matrix is utilized to identify if it is a rectangle plane and determine its dimensions.Before calculating the point cloud distribution matrix, the location of the geometric centroid of the extracted plane can be determined as(3)rm=1n∑i=1nri,where n is the point number in the point cloud, ri is the position of the nth point in the sensor coordinate system, and rm is the centroid location of the extracted plane.In literature [17], the attitude is determined based on the second moment information of point cloud, which is called as geometric moment matrix. However, the point cloud distribution matrix is proposed in this paper, and then, the point cloud distribution matrix is decomposed to calculate the attitude and identify the shape. In essence, both the geometric moment matrix and point cloud distribution matrix reflect the shape information of the point cloud, but the latter requires less processing power. The point cloud distribution matrix is computed as
(4)J=1n∑i=1nri−rm⋅ri−rmT.The eigenvector matrix and the diagonal eigenvalue matrix can be obtained by utilizing a matrix decomposition operation as(5)J=RΛRT,where Λ is the diagonal eigenvalue matrix of J and R is the eigenvector matrix.The eigenvalue matrix reflects the point cloud distribution in the extracted plane body coordinates, which is described in Figure1. The origin of the coordinate system is the center of the extracted plane; the X-axis, Y-axis and Z-axis are parallel to the principal geometric axes. If the extracted plane is a rectangle, the X-axis and Y-axis are parallel to the short and long sides of the rectangle plane, respectively, and the corresponding side length can be denoted as b and a, respectively. The eigenvalue matrix reflects the dimension parameters of the extracted plane. Therefore, Λ is invariable for a specific plane. That is to say, it does not change with the relative pose.Figure 1
The body coordinate system.Similarly, in the body coordinate system, it is obvious that the eigenvalues matrix can be defined as(6)Λ=1n∑i=1ndi−dm⋅di−dmT,where n is the point number in the point cloud, di is the position of the nth point in the body coordinate system, and dm is the origin of the body coordinate system, which also can be defined as
(7)dm=1n∑i=1ndi.It is supposed that the feature points obey a uniform distribution in the feature plane coordinate system and the measurement noise of point cloud is the white Gaussian noise. Therefore,di can be rewritten as
(8)di=d¯i+εi,where d¯i is the true position of the nth point in the body coordinate system and εi is the noise.Therefore, equation (6) can also be rewritten as
(9)Λ=1n∑i=1ndix−dmx2dix−dmxdiy−dmydix−dmxdiz−dmzdix−dmxdiy−dmydiy−dmy2diy−dmydiz−dmzdix−dmxdiz−dmzdiy−dmydiz−dmzdiz−dmz2,where dixdiydiz are the three components of di and dmxdmydmz are the three components of dm.SupposeIy=1/n∑i=1ndiy−dmy2 and then
(10)EIy=E1n∑i=1ndiy2+dmy2−2⋅diydmy,where EIy is the mathematical expectation of Iy.Substitute equation (8) into (10), and then, equation (10) can be expressed as
(11)EIy=E1n∑i=1nd¯iy2+E1n∑i=1nεiy2+2E1n∑i=1nd¯iyεiy−2E1n∑i=1ndmyd¯iy+εiy+E1n∑i=1ndmy2.If it is supposed that the extracted plane is a rectangular plane and the number of pointn is large enough, the following equations can hold:
(12)E1n∑i=1ndmy2≈0,(13)E1n∑i=1ndmyd¯iy+εiy≈0,(14)E1n∑i=1nd¯iyεiy≈0,(15)E1n∑i=1nεiy2=σy2,(16)E1n∑i=1nd¯iy2=b212,where σy is the mean square deviation of noise εiy and b is the side length of the edge which is parallel to the Y-axis.Equations (12), (13), (14), (15), and (16) are substituted into equation (11) and then
(17)EIy=b212+σy2.Similarly,EIx=a2/12+σx2 and EIz=σz2.SupposeIxy=1/n∑i=1ndix−dmxdiy−dmy and then
(18)EIxy=E1n∑i=1ndixdiy−E1n∑i=1ndixdmy−E1n∑i=1ndiydmx+E1n∑i=1ndmxdmy.It is regarded thatdix and diy are independent of each other. Therefore, the following equation holds:
(19)EIxy=0.Similarly,EIxz=0 and EIyz=0.It can be seen from the above derivation that the eigenvalue matrixΛ of the rectangular point cloud plane is only related to the size of the rectangular plane and the noise of the sensor. If the mean square deviation of the sensor noise is known, the parameters a and b can be estimated. Therefore, the extracted rectangular surface of the satellite can be identified by calculating the eigenvalue matrix Λ.


### 2.3. Coarse Pose Determination

The matrix decomposition operation presented in equation (5) can provide the attitude rotation matrix. The eigenvector matrix R is the attitude rotation matrix from the sensor coordinate system to the body coordinate system of the extracted rectangular plane. It is assumed that the sensor coordinate system is the same as the chaser body coordinate system in this paper.Thus, the quaternion of the body coordinate system with respect to the chaser body coordinate system, which is denoted asqcr, can be calculated as
(20)qc0r=121+Rxx+Ryy+Rzz1/2,qc1r=14qc0rRyz−Rzy,qc2r=14qc0rRzx−Rxz,qc3r=14qc0rRxy−Ryx,where Rxx, Ryy, Rzz, Ryz, Rzy, Rzx, Rxz, Rxy, and Ryx are the elements of the attitude rotation matrix R.If the eigenvalues ofJ are arranged as the middle, the largest, and the smallest in the diagonal line of Λ, there are 4 cases for establishing the body coordinate system depending on the axial direction due to the symmetrical structure of the extracted rectangle plane. The 4 cases are shown in Figure 2.Figure 2
Description of the multisolution problem.It is supposed that case (1) is the coordinate system of the present time (see Figure2). Any one of 4 cases of the body coordinate system may appear during the next matrix decomposition operation. The relationship between the target attitude qit and the attitude quaternion qcr can be described as
(21)qit=qicqcrqrt,where qic is the attitude quaternion of the chaser satellite and qrt is the quaternion from the body coordinate system of the extracted rectangular plane to the target body coordinate system, which is known.Because the target attitude quaternionq^it can be predicted in the filter which is designed in Section 3, the predicted q^cr can be derived as
(22)q^cr=qciq^itqtr.The predictedq^cr can be utilized to solve the multisolution problem presented in Figure 2. Define the difference quaternion between the predicted q^cr and the measured qcr as
(23)δqcr=q^cr−1qcr.If the scalar part ofδqcr is close to 1, case (1) occurs. If the scalar part of δqcr is close to 0, any one of cases (2)~(4) occurs, and the body coordinate system of the extracted plane needs to be changed to case (1) as
(24)q~cr=qcrq′,where q~cr is the adjusted quaternion and q′ is the quaternion which is utilized to adjust the attitude quaternion qcr, satisfying
(25)q′=u⋅sinθ2+cosθ2,where u is the direction of the Euler axis and θ is equal to 180 degrees.Cases (2), (3), and (4) need to be changed to case (1); the coordinate system should be rotated 180 degrees around theX-, Y-, and Z-axes, respectively. The corresponding quaternions are q′=0010T, q′=1000T, and q′=0100T, respectively.If there are 2 or more extracted rectangular planes in a single measurement, the similar problem exists. It is supposed that there are 2 extracted rectangular planes. The 2 attitude quaternions are denoted asqc1r and qc2r, respectively. The corresponding target attitudes qi1t and qi2t can be derived according equation (21). The difference quaternion between qi1t and qi2t can be obtained by using equation (23). If the scalar part of the difference quaternion is close to 0, the similar operation presented in equations (24) and (25) should be performed.


## 2.1. Plane Extraction from Point Cloud

Because satellites are man-made objects, there are some regular components on satellites, such as the main body, solar panels, and docking ring. Although these components are not designed specifically to measure the relative pose of malfunctioned satellites, they have obvious geometric features for recognition and can be utilized to realize relative navigation. The rectangular feature is one of the typical geometric shapes on malfunctioned satellites. For example, the main body of most satellites and solar panels is composed of a variety of rectangles. Therefore, this paper proposes to select the rectangular planes as the identification object.The random sample consensus (RANSAC) algorithm is utilized to extract planes from the point cloud dataset. The algorithm can be described as follows: firstly, select a mathematical model; secondly, find the so-called minimum set from the original point cloud sample, and then estimate the parameters of the model by using the minimum set of points; thirdly, check the consistency of the remaining points with the estimated model, and add these qualified points into the point set; and finally, the parameters of the model can be reestimated using all the points that conform to the model.According to the aforementioned RANSAC algorithm, the specific steps for extracting the plane from the point cloud are proposed as follows:(1)
Select three pointsP1x1,y1,z1, P2x2,y2,z2, and P3x3,y3,z3 randomly in the point cloud dataset
(2)
Calculate the parameters of the planeS according to P1, P2, and P3
(3)
Suppose the plane thickness asε, and calculate the distance from any point to the plane in the point cloud:
(1)di=Axi+Byi+Czi+DA2+B2+C2
(4)
Find out how many points are satisfyingdi<ε
(5)
Repeat step 1~step 4K times, and select the plane that includes the most points, where K can be calculated by the following formula:
(2)K=log1−Φlog1−1−τ3,where τ is the proportion of the outer point in the point cloud and Φ is the probability that the optimal plane can be found after K calculation


## 2.2. Rectangular Plane Recognition

Some components of a malfunctioned satellite are composed of rectangular planes. The dimensions of these rectangular planes are known for a specific malfunctioned satellite. After the plane is extracted from the point cloud, the point cloud distribution matrix is utilized to identify if it is a rectangle plane and determine its dimensions.Before calculating the point cloud distribution matrix, the location of the geometric centroid of the extracted plane can be determined as(3)rm=1n∑i=1nri,where n is the point number in the point cloud, ri is the position of the nth point in the sensor coordinate system, and rm is the centroid location of the extracted plane.In literature [17], the attitude is determined based on the second moment information of point cloud, which is called as geometric moment matrix. However, the point cloud distribution matrix is proposed in this paper, and then, the point cloud distribution matrix is decomposed to calculate the attitude and identify the shape. In essence, both the geometric moment matrix and point cloud distribution matrix reflect the shape information of the point cloud, but the latter requires less processing power. The point cloud distribution matrix is computed as
(4)J=1n∑i=1nri−rm⋅ri−rmT.The eigenvector matrix and the diagonal eigenvalue matrix can be obtained by utilizing a matrix decomposition operation as(5)J=RΛRT,where Λ is the diagonal eigenvalue matrix of J and R is the eigenvector matrix.The eigenvalue matrix reflects the point cloud distribution in the extracted plane body coordinates, which is described in Figure1. The origin of the coordinate system is the center of the extracted plane; the X-axis, Y-axis and Z-axis are parallel to the principal geometric axes. If the extracted plane is a rectangle, the X-axis and Y-axis are parallel to the short and long sides of the rectangle plane, respectively, and the corresponding side length can be denoted as b and a, respectively. The eigenvalue matrix reflects the dimension parameters of the extracted plane. Therefore, Λ is invariable for a specific plane. That is to say, it does not change with the relative pose.Figure 1
The body coordinate system.Similarly, in the body coordinate system, it is obvious that the eigenvalues matrix can be defined as(6)Λ=1n∑i=1ndi−dm⋅di−dmT,where n is the point number in the point cloud, di is the position of the nth point in the body coordinate system, and dm is the origin of the body coordinate system, which also can be defined as
(7)dm=1n∑i=1ndi.It is supposed that the feature points obey a uniform distribution in the feature plane coordinate system and the measurement noise of point cloud is the white Gaussian noise. Therefore,di can be rewritten as
(8)di=d¯i+εi,where d¯i is the true position of the nth point in the body coordinate system and εi is the noise.Therefore, equation (6) can also be rewritten as
(9)Λ=1n∑i=1ndix−dmx2dix−dmxdiy−dmydix−dmxdiz−dmzdix−dmxdiy−dmydiy−dmy2diy−dmydiz−dmzdix−dmxdiz−dmzdiy−dmydiz−dmzdiz−dmz2,where dixdiydiz are the three components of di and dmxdmydmz are the three components of dm.SupposeIy=1/n∑i=1ndiy−dmy2 and then
(10)EIy=E1n∑i=1ndiy2+dmy2−2⋅diydmy,where EIy is the mathematical expectation of Iy.Substitute equation (8) into (10), and then, equation (10) can be expressed as
(11)EIy=E1n∑i=1nd¯iy2+E1n∑i=1nεiy2+2E1n∑i=1nd¯iyεiy−2E1n∑i=1ndmyd¯iy+εiy+E1n∑i=1ndmy2.If it is supposed that the extracted plane is a rectangular plane and the number of pointn is large enough, the following equations can hold:
(12)E1n∑i=1ndmy2≈0,(13)E1n∑i=1ndmyd¯iy+εiy≈0,(14)E1n∑i=1nd¯iyεiy≈0,(15)E1n∑i=1nεiy2=σy2,(16)E1n∑i=1nd¯iy2=b212,where σy is the mean square deviation of noise εiy and b is the side length of the edge which is parallel to the Y-axis.Equations (12), (13), (14), (15), and (16) are substituted into equation (11) and then
(17)EIy=b212+σy2.Similarly,EIx=a2/12+σx2 and EIz=σz2.SupposeIxy=1/n∑i=1ndix−dmxdiy−dmy and then
(18)EIxy=E1n∑i=1ndixdiy−E1n∑i=1ndixdmy−E1n∑i=1ndiydmx+E1n∑i=1ndmxdmy.It is regarded thatdix and diy are independent of each other. Therefore, the following equation holds:
(19)EIxy=0.Similarly,EIxz=0 and EIyz=0.It can be seen from the above derivation that the eigenvalue matrixΛ of the rectangular point cloud plane is only related to the size of the rectangular plane and the noise of the sensor. If the mean square deviation of the sensor noise is known, the parameters a and b can be estimated. Therefore, the extracted rectangular surface of the satellite can be identified by calculating the eigenvalue matrix Λ.


## 2.3. Coarse Pose Determination

The matrix decomposition operation presented in equation (5) can provide the attitude rotation matrix. The eigenvector matrix R is the attitude rotation matrix from the sensor coordinate system to the body coordinate system of the extracted rectangular plane. It is assumed that the sensor coordinate system is the same as the chaser body coordinate system in this paper.Thus, the quaternion of the body coordinate system with respect to the chaser body coordinate system, which is denoted asqcr, can be calculated as
(20)qc0r=121+Rxx+Ryy+Rzz1/2,qc1r=14qc0rRyz−Rzy,qc2r=14qc0rRzx−Rxz,qc3r=14qc0rRxy−Ryx,where Rxx, Ryy, Rzz, Ryz, Rzy, Rzx, Rxz, Rxy, and Ryx are the elements of the attitude rotation matrix R.If the eigenvalues ofJ are arranged as the middle, the largest, and the smallest in the diagonal line of Λ, there are 4 cases for establishing the body coordinate system depending on the axial direction due to the symmetrical structure of the extracted rectangle plane. The 4 cases are shown in Figure 2.Figure 2
Description of the multisolution problem.It is supposed that case (1) is the coordinate system of the present time (see Figure2). Any one of 4 cases of the body coordinate system may appear during the next matrix decomposition operation. The relationship between the target attitude qit and the attitude quaternion qcr can be described as
(21)qit=qicqcrqrt,where qic is the attitude quaternion of the chaser satellite and qrt is the quaternion from the body coordinate system of the extracted rectangular plane to the target body coordinate system, which is known.Because the target attitude quaternionq^it can be predicted in the filter which is designed in Section 3, the predicted q^cr can be derived as
(22)q^cr=qciq^itqtr.The predictedq^cr can be utilized to solve the multisolution problem presented in Figure 2. Define the difference quaternion between the predicted q^cr and the measured qcr as
(23)δqcr=q^cr−1qcr.If the scalar part ofδqcr is close to 1, case (1) occurs. If the scalar part of δqcr is close to 0, any one of cases (2)~(4) occurs, and the body coordinate system of the extracted plane needs to be changed to case (1) as
(24)q~cr=qcrq′,where q~cr is the adjusted quaternion and q′ is the quaternion which is utilized to adjust the attitude quaternion qcr, satisfying
(25)q′=u⋅sinθ2+cosθ2,where u is the direction of the Euler axis and θ is equal to 180 degrees.Cases (2), (3), and (4) need to be changed to case (1); the coordinate system should be rotated 180 degrees around theX-, Y-, and Z-axes, respectively. The corresponding quaternions are q′=0010T, q′=1000T, and q′=0100T, respectively.If there are 2 or more extracted rectangular planes in a single measurement, the similar problem exists. It is supposed that there are 2 extracted rectangular planes. The 2 attitude quaternions are denoted asqc1r and qc2r, respectively. The corresponding target attitudes qi1t and qi2t can be derived according equation (21). The difference quaternion between qi1t and qi2t can be obtained by using equation (23). If the scalar part of the difference quaternion is close to 0, the similar operation presented in equations (24) and (25) should be performed.


## 3. Estimation of the Rotational and Translational Parameters

### 3.1. State Equations

When two satellites are nearly in the same circular orbit with a close distance, the Hill equations can perfectly describe the translational motion dynamics as(26)ẋυ̇=03×3I3×3F1F2xυ+03×3I3×3w1,where
(27)F1=0000−n20003n2,F2=002n000−2n00,n is the average orbital angular rate of the malfunctioned satellite and x and υ are the relative position and velocity between the target and the chaser, which is described in the target orbital frame. w1 is the disturbance force on an unit mass which is modeled as Gaussian white noise.As the target satellite is a malfunctioned satellite, the angular velocity cannot be obtained directly. The attitude kinematics and dynamics can be written as(28)q̇t=12qt⊗ω¯,ω̇t=It−1−ωt×Itωt+It−1w2,where qt is the target attitude quaternion, ω¯=0ωtTT, ω is the target angular velocity, It is the moment-of-inertia ratio matrix which can be described as
(29)It=1IxyIxxIxzIxxIxyIxxIyyIxxIyzIxxIxzIxxIyzIxxIzzIxx=1I~xyI~xzI~xyI~yyI~yzI~xzI~yzI~zz,and w2 is the ratio of the disturbing torque and Ixx, which is modeled as Gaussian white noise.


### 3.2. Measurement Equations

The geometrical relationship between the two satellites is described in Figure3. Ocxcyczc is the chaser body coordinate system. rmk is the location of the geometric centroid of an extracted rectangular plane, and it is expressed in the chaser body coordinate system, where k denotes the sequence number of the extracted rectangular plane. Orxryrzr is the target nominal body coordinate system. rbk is the location of the geometric centroid of the extracted rectangular plane in the frame Orxryrzr. Because the target is a malfunctioned satellite, the satellite structure model is known. Therefore, the relative pose between the extracted rectangular plane and frame Orxryrzr is known. So rbk is a known value. Because the mass center changes due to the fuel consumption, the location error of the mass center is denoted as bk. Otxtytzt is the target body coordinate system, whose origin Ot is at the true mass center, and its axes xt, yt, and zt are parallel to the axes xr, yr, and zr, respectively. x is the displacement vector from the target mass center to the chaser mass center and it is expressed in the target orbital frame. The origin of the orbital frame lies in the mass center of service satellite, the xo axis is in the flight direction, the zo axis is in the direction of the Earth center, and the yo axis which completes the right-handed orthogonal coordinate system is in the opposite direction of the angular momentum of the orbit.Figure 3
Geometrical relationship between two satellites.According to the geometrical relationship presented in Figure3, the displacement x can be described as
(30)x=Rtotrbk+bk−RocotRcocrmk+υk,where Rtot is the attitude rotation matrix from the frame Otxtytzt to the target orbital frame, Rocot is the rotation matrix from the chaser orbital frame to the target orbital frame, Rcoc is the rotation matrix from the frame Ocxcyczc to the chaser orbital frame and it can be determined by the chaser attitude determination system, and υk is the observation noise of the geometric centroid of the extracted rectangular plane.Because the two satellites are nearly in same circular orbit with a close distance, the rotation matrixROcOt can be regarded as the unit matrix [18].
(31)ROcOt≈I.Therefore, equation (30) can be rewritten as
(32)RocotRcocrmk≈Rcocrmk≈−x+Rtocrbk+Rtocbk−Rcocυk=−x+RiocRtirbk+RiocRtibk+υ~k,where Rioc is the rotation matrix from the inertial frame to the chaser orbital frame and it can be determined according to the orbit information and Rit is target attitude matrix.It is obvious thatbk is a constant vector and can be modeled as
(33)ḃk=0.As the target is a malfunctioned satellite which is noncooperative, no information including the attitude parameters can be provided to the service spacecraft. An available way to measure the target attitude is combining the relative attitude and the chaser attitude. According to equation (21), the attitude measurement is
(34)qit=qicqcrηkqrt,where ηk is the observation noise.A set of feature rectangles is tracked by the observer. As the target tumbling in orbit, new rectangular features are observed and must be added to the state vector, and some features disappear and can be only propagated in the filter. This strategy has been proven effective in literatures [6, 7, 19].


### 3.3. EKF Filter Design

Define state variables as(35)X=xTυTITqtTωtTbTT,where IT=I~yy,I~zz,I~xy,I~xz,I~yz and b denotes location errors of all the tracked feature rectangles.The dynamic equations are made up of equations (25), (33), and (28). Due to the nonlinearity of attitude kinematics and dynamic equations, a linearization operation can be performed. Then, the new state vector is described as
(36)X~=δxTδυTδITδqt,13TδωtTδbTT,where δx=x−x^, δυ=υ−υ^, δb=b−b^, δI=I−I^, δωt=ωt−ω^t, δqtT=q^t−1⊗qt, and δqt=1δqt,13TT. Because δq0 is not an independent variable and it has the second-order variations, δq0 is ignored in the linearization procedure.Define the state transfer matrixΦk/k−1.
(37)Φk/k−1=I+AT,where T is the discrete time, A is the Jacobian matrix of partial derivatives of the dynamic equations with respect to state variables X^, that is,
(38)A=03×3I3×303×503×303×303×3mF1F203×503×303×303×3m05×305×305×505×305×305×3m03×303×303×5−ω^t×0.5I3×303×3m03×303×3F403×3F303×3m03m×303m×303m×503m×303m×303m×3m,where F1 and F2 are defined in Section 3.1, F3=I^t−1⋅Ι^tω^t×−ω^t×⋅Ι^t,
(39)F4=I^t−1⋅00m2m30m20m10m30m30m1m2−ω^t×⋅00ω^t2ω^t30ω^t20ω^t10ω^t30ω^t30ω^t1ω^t2,m1m2m3T=I^t−1ω^t×Ι^tω^t, and m is the number of the tracked features.Similarly, in equations (32) and (34), a linearization operation is also performed as
(40)Z1,k=Rcocrmk+x^−RiocR^tirbk−RiocR^tib^k=−δx+RiocR^tiδbk−RiocR^tirbk×+RiocR^tibk×δqt,13+υ~k,Z2,k=q^t−1qicqcrηkqrt13=δqt,13+η~k.Therefore,Z=Z1,1TZ2,1T⋯Z1,kTZ2,kT⋯T and ε=υ~1Tη~1T⋯υ~kTη~kT⋯T, and the observation matrix can be defined as
(41)H=−I3×303×303×5−RiocR^tirb1×+RiocR^tib1×03×3RiocR^ti03×3m−203×303×303×303×5I3×303×303×303×3m−203×3⋮⋮⋮⋮⋮⋮⋮⋮−I3×303×303×5−RiocR^tirbk×+RiocR^tibk×03×303×3k−1RiocR^ti03×3m−k03×303×303×5I3×303×303×3k−103×303×3m−k⋮⋮⋮⋮⋮⋮⋮⋮.The detailed information of the EKF can be concluded as(1)
Initialization:
(42)X^0=EX0,P0=EX0−EX0X0−EX0T
(2)
Time update:
(43)Ẋk−1=fXk−1,wk−1,Pk/k−1=Φk/k−1⋅Pk−1⋅Φk/k−1T+Qk−1
(3)
Measurement update:
(44)Kk=Pk/k−1⋅HkT⋅Hk⋅Pk/k−1⋅HkT+Rk−1,δX^k=KkZ,Pk=I−Kk⋅Hk⋅Pk/k−1The estimated statesx^, υ^, b^, q^k, ω^k, and I^k can be innovated right after measurement update.


## 3.1. State Equations

When two satellites are nearly in the same circular orbit with a close distance, the Hill equations can perfectly describe the translational motion dynamics as(26)ẋυ̇=03×3I3×3F1F2xυ+03×3I3×3w1,where
(27)F1=0000−n20003n2,F2=002n000−2n00,n is the average orbital angular rate of the malfunctioned satellite and x and υ are the relative position and velocity between the target and the chaser, which is described in the target orbital frame. w1 is the disturbance force on an unit mass which is modeled as Gaussian white noise.As the target satellite is a malfunctioned satellite, the angular velocity cannot be obtained directly. The attitude kinematics and dynamics can be written as(28)q̇t=12qt⊗ω¯,ω̇t=It−1−ωt×Itωt+It−1w2,where qt is the target attitude quaternion, ω¯=0ωtTT, ω is the target angular velocity, It is the moment-of-inertia ratio matrix which can be described as
(29)It=1IxyIxxIxzIxxIxyIxxIyyIxxIyzIxxIxzIxxIyzIxxIzzIxx=1I~xyI~xzI~xyI~yyI~yzI~xzI~yzI~zz,and w2 is the ratio of the disturbing torque and Ixx, which is modeled as Gaussian white noise.


## 3.2. Measurement Equations

The geometrical relationship between the two satellites is described in Figure3. Ocxcyczc is the chaser body coordinate system. rmk is the location of the geometric centroid of an extracted rectangular plane, and it is expressed in the chaser body coordinate system, where k denotes the sequence number of the extracted rectangular plane. Orxryrzr is the target nominal body coordinate system. rbk is the location of the geometric centroid of the extracted rectangular plane in the frame Orxryrzr. Because the target is a malfunctioned satellite, the satellite structure model is known. Therefore, the relative pose between the extracted rectangular plane and frame Orxryrzr is known. So rbk is a known value. Because the mass center changes due to the fuel consumption, the location error of the mass center is denoted as bk. Otxtytzt is the target body coordinate system, whose origin Ot is at the true mass center, and its axes xt, yt, and zt are parallel to the axes xr, yr, and zr, respectively. x is the displacement vector from the target mass center to the chaser mass center and it is expressed in the target orbital frame. The origin of the orbital frame lies in the mass center of service satellite, the xo axis is in the flight direction, the zo axis is in the direction of the Earth center, and the yo axis which completes the right-handed orthogonal coordinate system is in the opposite direction of the angular momentum of the orbit.Figure 3
Geometrical relationship between two satellites.According to the geometrical relationship presented in Figure3, the displacement x can be described as
(30)x=Rtotrbk+bk−RocotRcocrmk+υk,where Rtot is the attitude rotation matrix from the frame Otxtytzt to the target orbital frame, Rocot is the rotation matrix from the chaser orbital frame to the target orbital frame, Rcoc is the rotation matrix from the frame Ocxcyczc to the chaser orbital frame and it can be determined by the chaser attitude determination system, and υk is the observation noise of the geometric centroid of the extracted rectangular plane.Because the two satellites are nearly in same circular orbit with a close distance, the rotation matrixROcOt can be regarded as the unit matrix [18].
(31)ROcOt≈I.Therefore, equation (30) can be rewritten as
(32)RocotRcocrmk≈Rcocrmk≈−x+Rtocrbk+Rtocbk−Rcocυk=−x+RiocRtirbk+RiocRtibk+υ~k,where Rioc is the rotation matrix from the inertial frame to the chaser orbital frame and it can be determined according to the orbit information and Rit is target attitude matrix.It is obvious thatbk is a constant vector and can be modeled as
(33)ḃk=0.As the target is a malfunctioned satellite which is noncooperative, no information including the attitude parameters can be provided to the service spacecraft. An available way to measure the target attitude is combining the relative attitude and the chaser attitude. According to equation (21), the attitude measurement is
(34)qit=qicqcrηkqrt,where ηk is the observation noise.A set of feature rectangles is tracked by the observer. As the target tumbling in orbit, new rectangular features are observed and must be added to the state vector, and some features disappear and can be only propagated in the filter. This strategy has been proven effective in literatures [6, 7, 19].


## 3.3. EKF Filter Design

Define state variables as(35)X=xTυTITqtTωtTbTT,where IT=I~yy,I~zz,I~xy,I~xz,I~yz and b denotes location errors of all the tracked feature rectangles.The dynamic equations are made up of equations (25), (33), and (28). Due to the nonlinearity of attitude kinematics and dynamic equations, a linearization operation can be performed. Then, the new state vector is described as
(36)X~=δxTδυTδITδqt,13TδωtTδbTT,where δx=x−x^, δυ=υ−υ^, δb=b−b^, δI=I−I^, δωt=ωt−ω^t, δqtT=q^t−1⊗qt, and δqt=1δqt,13TT. Because δq0 is not an independent variable and it has the second-order variations, δq0 is ignored in the linearization procedure.Define the state transfer matrixΦk/k−1.
(37)Φk/k−1=I+AT,where T is the discrete time, A is the Jacobian matrix of partial derivatives of the dynamic equations with respect to state variables X^, that is,
(38)A=03×3I3×303×503×303×303×3mF1F203×503×303×303×3m05×305×305×505×305×305×3m03×303×303×5−ω^t×0.5I3×303×3m03×303×3F403×3F303×3m03m×303m×303m×503m×303m×303m×3m,where F1 and F2 are defined in Section 3.1, F3=I^t−1⋅Ι^tω^t×−ω^t×⋅Ι^t,
(39)F4=I^t−1⋅00m2m30m20m10m30m30m1m2−ω^t×⋅00ω^t2ω^t30ω^t20ω^t10ω^t30ω^t30ω^t1ω^t2,m1m2m3T=I^t−1ω^t×Ι^tω^t, and m is the number of the tracked features.Similarly, in equations (32) and (34), a linearization operation is also performed as
(40)Z1,k=Rcocrmk+x^−RiocR^tirbk−RiocR^tib^k=−δx+RiocR^tiδbk−RiocR^tirbk×+RiocR^tibk×δqt,13+υ~k,Z2,k=q^t−1qicqcrηkqrt13=δqt,13+η~k.Therefore,Z=Z1,1TZ2,1T⋯Z1,kTZ2,kT⋯T and ε=υ~1Tη~1T⋯υ~kTη~kT⋯T, and the observation matrix can be defined as
(41)H=−I3×303×303×5−RiocR^tirb1×+RiocR^tib1×03×3RiocR^ti03×3m−203×303×303×303×5I3×303×303×303×3m−203×3⋮⋮⋮⋮⋮⋮⋮⋮−I3×303×303×5−RiocR^tirbk×+RiocR^tibk×03×303×3k−1RiocR^ti03×3m−k03×303×303×5I3×303×303×3k−103×303×3m−k⋮⋮⋮⋮⋮⋮⋮⋮.The detailed information of the EKF can be concluded as(1)
Initialization:
(42)X^0=EX0,P0=EX0−EX0X0−EX0T
(2)
Time update:
(43)Ẋk−1=fXk−1,wk−1,Pk/k−1=Φk/k−1⋅Pk−1⋅Φk/k−1T+Qk−1
(3)
Measurement update:
(44)Kk=Pk/k−1⋅HkT⋅Hk⋅Pk/k−1⋅HkT+Rk−1,δX^k=KkZ,Pk=I−Kk⋅Hk⋅Pk/k−1The estimated statesx^, υ^, b^, q^k, ω^k, and I^k can be innovated right after measurement update.


## 4. Simulation and Evaluation

In order to evaluate the effectiveness of extracting a point cloud plane, a point cloud rectangular plane is simulated by the simulation software. The point cloud is randomly simulated obeying a uniform distribution in a rectangular plane, which ranges from -5 cm to 5 cm in theX-axis, ranges from -9 cm to 9 cm in the Y-axis, and fixes at 6.5 cm in the Z-axis. It is also simulated that there are 2500 points including 100, 500, and 1250 outliers, respectively, in the rectangular plane. The results are shown in Figures 4–7.Figure 4
The original point cloud.Figure 5
Point cloud with 100 outliers.Figure 6
Point cloud with 500 outliers.Figure 7
Point cloud with 1250 outliers.In this simulation, the repeat number is set to be 300 times, and the distance threshold is 0.005. The extraction results are shown in Table1.Table 1
Plane extraction from the point cloud.
Outliers
The number of extracted points
Time (ms)
0
2500
20
100
2400
20
500
2000
30
1250
1252
70It can be seen in Table1 that the plane can be extracted exactly from the point cloud with outliers. However, the processing speed of the plane extraction is slowed down with the increase of outliers.The point cloud declines in number as the distance between the target and the sensor increases. In order to validate the effectiveness of a rectangular feature recognition formula as the number of points varies, the number of points in the rectangular plane is set to be 10000, 5000, 2500, 1250, and 625, respectively. The geometric centroid and the point cloud distribution matrix of the extracted plane are calculated, and the three eigenvalues are obtained by using the matrix decomposition operation. The theoretical value of the geometric centroid is (0, 0, 6.5), and the eigenvalues are 8.3333, 27, and 0, respectively. The geometric centroid and the 3 eigenvalues of the rectangular point cloud are calculated and presented in Table2. Therefore, it can be seen that the geometric centroid and the eigenvalues are almost not obviously affected by the number of points.Table 2
The geometric centroid and eigenvalues.
Number (points)
The center of gravity
Eigenvalue distribution
10000
(0.0154, -0.0390, 6.5000)
(8.4102, 26.7072, 0)
5000
(-0.0079, -0.0090, 6.5000)
(8.3106, 27.3320, 0)
2500
(-0.0801, -0.0033, 6.5000)
(8.5595, 26.7923, 0)
1250
(0.1542, -0.0438, 6.5000)
(8.4415, 26.4924, 0)
625
(0.0906, -0.3373, 6.5000)
(8.2372, 27.7920, 0)And then, the random noise of 0.01 cm, 0.1 cm, and 1 cm is added to the rectangular point cloud plane which contains 2500 points. It can be seen in Table3 that the location error does not obviously increase as the added random noise increases, because the number of points is large enough.Table 3
The location error of the geometric centroid.
Error (cm)
The geometric centroid (cm)
The location error (cm)
0
(-0.0801, -0.0033, 6.5000)
(-0.0801, -0.0033, 0)
0.01
(0.0817, -0.0833, 6.4999)
(0.0817, -0.0833, 0.0001)
0.1
(0.0541, 0.0661, 6.4984)
(0.0541, 0.0661, 0.0016)
1
(-0.0261, 0.0210, 6.5254)
(-0.0261, 0.0210, 0.0254)The theoretical eigenvalues and the eigenvalues computed by using the matrix decomposition operation are presented in Table4. The computed eigenvalues are compared with the theoretical eigenvalues, and both the values are almost the same. Therefore, the effectiveness of the proposed recognition algorithm is validated according to Tables 3 and 4.Table 4
The eigenvalues.
Error (cm)
Eigenvalues (simulation)
Eigenvalues (theory)
0
(8.5595, 26.7923, 0)
(8.3333, 27, 0)
0.01
(8.5898, 26.9769, 0.0001)
(8,3334, 27.0001, 0.0001)
0.1
(8.3124, 26.9526, 0.0103)
(8.3433, 27.01, 0.01)
1
(9.4664, 27.7494, 0.9765)
(9.3333, 28, 1)In order to validate the effectiveness of the relative navigation method, digital simulation software has been written in VC++. In this simulation, it is assumed that the target and the chaser are in the same orbit with different orbital phases. The chaser locates at the position which is about 25 m behind the target satellite. The inertia matrix of the target satellite is set asIt=diag30,15,20kg⋅m2. The initial angular velocity of the target is 001°/s. The point cloud data is randomly simulated by using uniform distribution on the target surface with Gaussian noise. The number of the points is 1800, and the mean square deviation of the Gaussian error is 0.10.10.1 m. The true location error b is set as 0.10.10.1 m.The PCL (Point Cloud Library) is employed to extract planes. And then, the proposed method is utilized to recognize the feature rectangular plane and determine the relative position and attitude between the feature rectangular plane and the chaser. The relative position and attitude errors are shown in Figures8–10.Figure 8
Position errors.Figure 9
Attitude errors with multisolution.Figure 10
Attitude errors removing multisolution.It can be seen from Figure8 that the accuracy of the relative position is better than 0.01 m, and the triaxial maximum errors are about 0.01 m, 0.01 m, and 0.01 m, respectively. The attitude error with a multisolution problem is shown in Figure 9, and the attitude error after removing the multisolution problem is shown in Figure 10. The multisolution problem occurs, and then the error is about 180°. There is no such phenomenon in Figure 10. Therefore, the method proposed in this paper has solved the attitude multisolution problem caused by the symmetrical rectangular plane. The attitude error presented in Figure 10 is better than 0.2°, and the triaxial maximum error is about 0.13°, 0.12°, and 0.16°.In the relative navigation filter,x0 is set as the first observation of the relative position, and ẋ0 is set as 0. qt0 is set as the first observation of the target attitude, and ωt0 is set to be the target angular velocity corrupted by initial errors. The covariance matrix P0=diagσx2,συ2,σb2,σqt,132,σωt2,σI2, where σx=121212m2, σẋ2=0.120.120.12m/s2, σb2=0.120.120.12m2, σqt,132=0.120.120.12rad, σωt2=10−4×121212rad/s2, and σI2=0.120.120.120.120.12. The system noise is Gaussian noise with zero mean, and the system noise variance matrix is Q=10−12I6×6.It can be seen from Figures11–13 that not only the relative position and velocity can be estimated but also the mass center location error of the malfunctioned satellite can be estimated. According to the simulation curves, the relative position and velocity errors converge in a very short time, and the fluctuation is very small after convergence. The accuracy of the three axis positions is better than 1.4 mm, 3.2 mm, and 1.4 mm. The triaxial velocity error is better than 0.01 mm/s, 0.01 mm/s, and 0.01 mm/s, respectively. The estimation error of the mass center location is less than 1 mm, 1 mm, and 1 mm, respectively.Figure 11
Relative position errors.Figure 12
Relative velocity errors.Figure 13
Location errors of the mass center.The estimation errors of the attitude, the angular velocity, and the moment-of-inertia ratios of the target satellite are presented in Figures14–16. There are some fluctuations of the malfunctioned satellite attitude, but the accuracy is better than 0.1°, and the angular velocity error is better than 0.01°/s. The moment-of-inertia ratios also converge to the true values as time goes by.Figure 14
Attitude errors of the target.Figure 15
Angular velocity errors of the target.Figure 16
Moment-of-inertia ratio errors.


## 5. Conclusion

A new method is proposed in this paper to estimate target pose by utilizing local feature rectangular planes when the whole satellite cannot be observed. The plane extraction method, the rectangular feature recognition method, and the solution of multisolution problem are presented. Finally, the Kalman filter is designed to estimate the relative position and the attitude of the target satellite. Therefore, this paper provides a feasible algorithm for relative navigation during rendezvous and docking to malfunctioned satellites.



---

*Source: 6895628-2019-06-11.xml*