A positioning and attitude determination method for an aerial work platform based on RTK

Abstract

The present invention relates to a method for positioning and orientation of an aerial work platform based on RTK. According to the working environment and requirements of the aerial work platform, the present invention provides a method for determining the position and orientation of the base of the robotic arm of the aerial work platform in a factory building. A factory building coordinate system is established in combination with the 3D model of the factory building to be sprayed, the position coordinates of the steel structure components to be sprayed are clarified, and the movement path of the end of the spraying robotic arm is determined according to the known coordinates, so as to facilitate the automated spraying operation of the aerial work platform, enable real-time positioning and orientation of the work platform, ensure the spraying operation efficiency. At the same time, the positioning and orientation error is small, which can ensure full coverage of the spraying operation and prevent omission of the spraying surface.

Description

Technical Field

[0001] The present invention relates to the technical field of work vehicle positioning, and specifically to a positioning and attitude determination method for an aerial work vehicle based on RTK. Background Art

[0002] A patent with publication number CN114798253A, an equipment for aerial work and its control method, provides an aerial work equipment that can automatically spray steel structures based on the 3D model of a steel structure workshop.

[0003] Since the 3D model of the workshop is known, a rectangular coordinate system called the workshop coordinate system can be established according to the 3D model of the workshop. Thus, the position and attitude of the steel structure components to be sprayed in the workshop coordinate system are known, and the path corresponding to the movement of the end of the spraying robotic arm in the workshop coordinate system is known. Therefore, a necessary condition for realizing the movement of the spraying robotic arm along the pre-set spraying trajectory is to determine the position and attitude where the base of the robotic arm is parked in the workshop coordinate system. In addition, to control the boom of the aerial work vehicle to send the robotic arm to a certain position in the spraying workshop coordinate system, it is necessary to determine the position and attitude where the chassis of the aerial work vehicle docks in the workshop coordinate system. To ensure the spraying operation efficiency, the positioning and attitude determination time must be real-time, and the manual measurement method is not feasible; due to the existence of small components such as purlins in the steel structure workshop, the positioning and attitude determination error must be small enough, otherwise the spraying surface cannot be completely covered. Therefore, a positioning and attitude determination method is needed. Summary of the Invention

[0004] In view of the problems existing in the prior art, the present invention discloses a positioning and attitude determination method for an aerial work vehicle based on RTK, including the following steps:

[0005] Step 1: Convert the measurement values of RTK into workshop rectangular coordinates:

[0006] The realization of this conversion can be divided into the following two steps:

[0007] Step 1: Convert geodetic coordinates into Earth-centered Earth-fixed rectangular coordinates

[0008] In the WGS-84 coordinate system, the following geodetic constants are used to calculate the rectangular coordinates. The semi-major axis of the Earth a = 6378137 meters, and the square of the eccentricity of the Earth Through the following formula, the latitude, longitude and elevation are converted into the Earth-centered Earth-fixed rectangular coordinate system (X, Y, Z)

[0009]

[0010]

[0011]

[0012] Step 2: Determine the rigid transformation between the Earth-Centered Earth-Fixed (ECEF) coordinate system and the factory building coordinate system

[0013] Since both the ECEF coordinate system and the factory building coordinate system are rectangular coordinate systems and the length unit is meter for both, there exists a definite rigid transformation \(R, t\) such that \(\mathbf{p}\) A =\(R\cdot\) E \(\mathbf{p}\) A +\(t\), where \(\mathbf{p}\) A is the coordinate of point \(A\) in the factory building coordinate system, E \(\mathbf{p}\) A is the coordinate of point \(A\) in the ECEF coordinate system, \(R\in SO(3)\) is the rotation matrix, and \(t\) is the translation vector;

[0014] Let \(R, t\) be the parameters to determine the rigid transformation. Select several fixed points \(q_1, q_2,\cdots, q\) n in the factory building. At each point, use RTK to measure its coordinate \(\mathbf{p}_1, \mathbf{p}_2,\cdots, \mathbf{p}\) n in the ECEF coordinate system. Then the selection of parameters \(R, t\) should satisfy the minimum sum of the squares of the error distances

[0015]

[0016] The parameters satisfying the above conditions can be solved through the following steps:

[0017] (1) Calculate the centroids of the two sets of points

[0018] (2) Calculate the two sets of centralized vectors

[0019] (3) Calculate the \(3\times3\) covariance matrix \(S = XY\) T , where \(X = [x_1\cdots x\) n , \(Y = [y_1\cdots y\) n

[0020] (4) Calculate the singular value decomposition of \(S\), \(S = U\sum V\) T , then the optimal rotation matrix is

[0021]

[0022] (5) The optimal translation vector is

[0023] Therefore, based on the above two steps, the measurement values of RTK can be directly transformed into the factory building coordinate system, and thus it is assumed in the following part that RTK directly outputs the \(x\), \(y\), \(z\) coordinates in the factory building coordinate system; ​

[0024] Step 2. Determine the position and attitude of the aerial work platform in the factory building coordinate system

[0025] To measure the position and attitude of the aerial work platform in the factory building coordinate system, two RTK measurement points are installed on the vehicle chassis. When the factory building floor is relatively flat, it can be assumed that the vehicle chassis is parallel to the factory building floor. Then, the position and attitude of the aerial work platform can be determined only by using the two RTK measurement points. The method is as follows:

[0026] Assume that the installation positions of the two RTKs on the aerial work platform are respectively C p A and C p B It is known that

[0027]

[0028] At the same time, according to the measurement values of the RTKs, the positions of the two points in the factory building, p A and p B

[0029]

[0030] Determine the position p c and attitude R C ;

[0031] First, there are the following relationships

[0032] p A = R C · C p A + p C (1)

[0033] p B = R C · C p B + p C (2)

[0034] If the vehicle chassis is parallel to the ground, that is, the z-axis of the aerial work platform is the same as the z-axis of the factory building, then the column vectors of the rotation matrix R C can be expressed as: R c = [r1 r2 e3]

[0035] From the orthogonality of the column vectors, <r1, e3> = <r2, e3> = 0, so

[0036]

[0037] Subtracting equations (1) and (2) gives: p B - pA = R C · ( C p B - C p A )

[0038] Written in coordinate form:

[0039]

[0040] Considering only the first two terms, we get:

[0041]

[0042] The matrix on the left side of the equation is a 2D rotation matrix, so it can be written as:

[0043]

[0044] Thus, we obtain a system of equations in the following form:

[0045] C1cosθ + C2sinθ + C3 = 0

[0046] C1cosθ - C2sinθ + C4 = 0

[0047] There is a unique solution θ = arctan2(-C1C4 - C2C3, C2C4 - C1C3);

[0048] After calculating the attitude of the aerial work platform, adding equations (1) and (2) gives the position of the aerial work platform

[0049]

[0050] If the vehicle chassis is not parallel to the ground, to determine the accurate pose of the aerial work platform, it is not possible to assume that the z-axis direction of the vehicle is the same as the z-axis direction of the factory building. An inclination sensor is installed on the vehicle chassis, and the coordinates of the RTK measurement points are combined to determine the accurate pose of the aerial work platform;

[0051] The inclination sensor measures the angles between the x and y axes of the sensor and the sea level, that is, the complementary angles of the angles with the gravity direction; if the x and y axes of the sensor are made to coincide with the x and y axes of the aerial work platform during installation of the inclination sensor, then the angles between the x and y axes of the aerial work platform and the gravity direction can be measured; since the gravity direction is approximately the same as the opposite direction of the z-axis of the factory building, therefore, the angles between the x and y axes of the aerial work platform and the z-axis of the factory building can actually be measured through the inclination sensor;

[0052] Assume that the readings of the inclination sensor in the x and y axis directions are α x , α y , so the unit vector in the gravity direction in the coordinate system of the aerial work platform can be expressed as:

[0053]

[0054] The coordinates of the unit vector in the direction of gravity in the plant coordinate system are:

[0055]

[0056] Therefore, assuming the position of the aerial work platform in the plant is p C and the attitude is R C , combining the installation positions of the two RTKs on the aerial work platform C p A and C p B , and the measured positions p A and p B in the plant, a total of three sets of corresponding relationships are obtained:

[0057] p g = R C · C p g + p C (1)

[0058] p A = R C · C p A + p C (2)

[0059] p B = R C · C p B + p C (3)

[0060] (2) and (3) are subtracted from (1) to obtain

[0061] p A - p g = R C ·( C p A - C p g )

[0062] p B - p g = R C ·( C p B - C p g )

[0063] Denote and Let Unit orthonormalization:

[0064]

[0065]

[0066] Thus, unit orthonormal vectors are obtained For Do the same linear combination to get:

[0067]

[0068]

[0069] Therefore

[0070]

[0071]

[0072] Since R C is a rotation matrix, so are also unit orthonormal vectors, thus obtaining:

[0073]

[0074] Thus

[0075] After calculating the attitude of the aerial work platform, add equations (1), (2), and (3) to obtain the position of the aerial work platform:

[0076]

[0077] According to the above derivation, it is easy to see that the readings of the inclinometers essentially determine the corresponding relationship of a three-dimensional vector in the coordinate system of the aerial work platform and the coordinate system of the factory building, that is, the C p g and p g corresponding relationship. Therefore, if the inclinometers are not used, the RTK measurement points on the end platform of the aerial work platform can also be used to determine the corresponding relationship of a three-dimensional vector (i.e., the coordinates of the point) in the coordinate system of the aerial work platform and the coordinate system of the factory building. Since the position of the end platform of the aerial work platform is at the zero position before starting the operation, the coordinates of the RTK on the platform in the coordinate system of the aerial work platform are fixed. If the coordinates of this position in the aerial work platform can be measured, combined with the measurement points of the two RTKs on the chassis of the aerial work platform, the pose of the aerial work platform can also be determined.

[0078] Thus, a method for determining the pose of the aerial work platform in the factory building using only three RTK measurement points is given.

[0079] Based on the above discussions, there are three methods to determine the position and pose of the aerial work platform in the workshop, which are summarized as follows:

[0080] (1). For the scenario where the workshop floor is flat, the position and pose of the aerial work platform in the workshop can be determined only by using the two RTK measurement points installed on the vehicle chassis;

[0081] (2). For the scenario where the workshop floor is uneven, there are two improvement methods:

[0082] 1). Install an inclination sensor on the vehicle chassis, and determine the position and pose of the aerial work platform in the workshop according to the readings of the two RTK measurement points and the inclination sensor;

[0083] 2). Without installing an inclination sensor, use the RTK measurement point installed on the end platform. As long as the boom is kept in the zero position and fixed before operation, the position and pose of the aerial work platform in the workshop can also be determined according to the three RTK measurement points;

[0084] Step 3: Determine the position and orientation of the end platform of the boom in the workshop coordinate system

[0085] The position of the end platform in the workshop can be directly measured according to the RTK installed on the platform. To calculate the orientation of the platform, the kinematic model of the aerial work platform and the position and pose of the aerial work platform in the workshop that have been calculated can be used. Since the end platform always remains almost parallel to the vehicle chassis, the boom of the aerial work platform only has three degrees of freedom: rotation, pitch, and extension. According to the position of a certain point in the end platform in the aerial work platform, the joint values of the boom can be inversely solved, so that the position and pose of the platform in the aerial work platform can be calculated by forward kinematics. Finally, the position and pose of the end platform in the workshop can be calculated using the position and pose of the aerial work platform in the workshop.

[0086] Model the boom kinematics using the standard D-H parameters as follows:

[0087]

[0088] The position and pose of the end platform in the aerial work platform are uniquely determined by the three joint values θ1, θ2, and d3, and the remaining parameters a i , d i are obtained by actually measuring the dimensions of the boom;

[0089] Now, calculate the three joint values θ1, θ2, and d3 according to the measurement values of the end platform RTK; assume that the point measured by the end platform RTK in the workshop coordinate system is p P , and the position and pose of the aerial work platform in the workshop are represented by a 4×4 homogeneous matrix as follows:

[0090]

[0091] The position coordinates of the end platform RTK in the aerial work vehicle can then be calculated:

[0092]

[0093] According to the D-H parameters of the boom, the transformation matrices between the various linkages can be known:

[0094]

[0095]

[0096]

[0097]

[0098] Thus, the transformation matrix of the end platform relative to the base coordinate system of the aerial work vehicle is: T = A1A2A3A4

[0099] Assume that the installation position of the RTK on the end platform is fixed and the coordinates are [x4 y4 z4] T , and the coordinates of the RTK in the base coordinate system of the aerial work vehicle calculated previously are C p p = [x0 y0 z0] T , then there is:

[0100]

[0101] The above equation is equivalent to:

[0102]

[0103] Calculate the inverses of the transformations A1 and A2 as follows:

[0104]

[0105] The left-hand side of the equal sign is:

[0106]

[0107] The right-hand side of the equal sign is:

[0108]

[0109] First, the second equation is in the form of the equation C1cosθ1 + C2sinθ1 + C3 = 0, and the value of θ1 can be solved. Substituting the value of θ1 into the first equation gives an equation in the form of C4cosθ2 + C5sinθ2 + C6 = 0, so that the value of θ2 can be solved. Finally, the value of d3 is obtained according to the third equation;

[0110] Based on the measurement values of RTK, the three joint values of the boom can be obtained, thereby calculating the pose of the end platform relative to the aerial work platform. Then, using the pose of the aerial work platform relative to the factory building calculated previously, the pose of the end platform in the factory building can be obtained.

[0111] Advantages of the present invention: According to the working environment and requirements of the aerial work platform, the present invention provides a method for determining the position and attitude of the base of the robotic arm of the aerial work platform in the factory building. By combining the 3D model of the factory building to be sprayed to establish a factory building coordinate system, clarifying the position coordinates of the steel structure components to be sprayed, and determining the movement path of the end of the spraying robotic arm according to the known coordinates, it is convenient for the automated spraying operation of the aerial work platform, can position and orient the work platform in real time, ensures the spraying operation efficiency. At the same time, the positioning and orientation error is small, which can ensure full coverage of the spraying operation and no spraying surface will be missed. Detailed implementation manners

[0112] Embodiment 1

[0113] A method for positioning and orienting an aerial work platform based on RTK of the present invention includes the following steps:

[0114] Step 1: Convert the measurement values of RTK into the rectangular coordinates of the factory building

[0115] RTK is the abbreviation of "Real-Time Kinematic", which is a technology that provides real-time, centimeter-level precision positioning. Since the RTK measurement values are longitude, latitude, and elevation values in the geodetic coordinate system and cannot be directly applied to the factory building coordinate system, it is necessary to first convert the RTK measurement values into the factory building coordinate system. The realization of this conversion can be divided into the following two steps:

[0116] Step 1: Convert the geodetic coordinates into the Earth-Centered Earth-Fixed (ECEF) rectangular coordinates

[0117] In the WGS-84 coordinate system, the following geodetic constants are used to calculate the rectangular coordinates. The semi-major axis of the Earth a = 6378137 meters, and the square of the eccentricity of the Earth Through the following formula, the latitude, longitude, and elevation are converted into the Earth-Centered Earth-Fixed (ECEF) rectangular coordinate system (X, Y, Z)

[0118]

[0119]

[0120]

[0121] Step 2: Determine the rigid transformation between the Earth-Centered Earth-Fixed (ECEF) coordinate system and the factory building coordinate system

[0122] Since both the Earth-centered Earth-fixed coordinate system and the factory building coordinate system are rectangular coordinate systems and the length unit is meter for both, there exists a definite rigid transformation \(R, t\) between the two coordinate systems such that \(\mathbf{p}\) A = \(R\cdot\) E \(\mathbf{p}\) A + \(t\), where \(\mathbf{p}\) A is the coordinate of point \(A\) in the factory building coordinate system, E \(\mathbf{p}\) A is the coordinate of point \(A\) in the Earth-centered Earth-fixed coordinate system, \(R\in SO(3)\) is the rotation matrix, is the translation vector;

[0123] For the parameters \(R, t\) that determine the rigid transformation, select several fixed points \(q_1, q_2, \cdots, q\) n in the factory building, and use RTK to measure the coordinates \(\mathbf{p}_1, \mathbf{p}_2, \cdots, \mathbf{p}\) n of this point in the Earth-centered Earth-fixed coordinate system at each point. Then the selection of the parameters \(R, t\) should satisfy that the sum of the squares of the error distances is minimized

[0124]

[0125] The parameters satisfying the above conditions can be solved through the following steps:

[0126] (1) Calculate the centroids of the two sets of points

[0127] (2) Calculate the two sets of centralized vectors

[0128] (3) Calculate the \(3\times3\) covariance matrix \(S = XY\) T , where \(X = [x_1\cdots x\) n , \(Y = [y_1\cdots y\) n

[0129] (4) Calculate the singular value decomposition of \(S\), \(S = U\sum V\) T , then the optimal rotation matrix is

[0130]

[0131] (5) The optimal translation vector is

[0132] Complete the direct transformation of the RTK measurement values to the factory building coordinate system, so that in the following part, it is assumed that RTK directly outputs the \(x\), \(y\), \(z\) coordinates in the factory building coordinate system;

[0133] Step Two: Determine the position and attitude of the aerial work vehicle in the factory building coordinate system

[0134] ​Install two RTK measurement points on the vehicle chassis, and use the two RTK measurement points to determine the position and attitude of the aerial work vehicle. The method is as follows:

[0135] Assume that the installation positions of the two RTKs in the aerial work vehicle are respectively C p A and C p B It is known that

[0136]

[0137] At the same time, according to the measurement values of the RTKs, the positions of the two points in the workshop can be known as p A and p B

[0138]

[0139] Determine the position p c and attitude R C ;

[0140] First, there are the following relationships

[0141] p A = R C · C p A + p C (1)

[0142] p B = R C · C p B + p C (2)

[0143] If the vehicle chassis is parallel to the ground, that is, the z-axis of the aerial work vehicle is consistent with the z-axis of the workshop, then the column vectors of the rotation matrix R C can be expressed as: R C = [r1 r2 e3]

[0144] From the orthogonality of the column vectors, <r1, e3> = <r2, e3> = 0, thus

[0145]

[0146] Subtract equations (1) and (2) to get: p B - p A = R C ·( C p B - C p A )

[0147] Written in coordinate form:

[0148]

[0149] Considering only the first two terms, we get:

[0150]

[0151] The matrix on the left side of the equation is a 2D rotation matrix, so it can be written as:

[0152]

[0153] Thus, we obtain a system of equations in the following form:

[0154] C1cosθ + C2sinθ + C3 = 0

[0155] C1cosθ - C2sinθ + C4 = 0

[0156] There is a unique solution θ = arctan2(-C1C4 - C2C3, C2C4 - C1C3);

[0157] After calculating the attitude of the aerial work platform, adding equations (1) and (2) gives the position of the aerial work platform

[0158]

[0159] If the vehicle chassis is not parallel to the ground, to determine the accurate pose of the aerial work platform, it cannot be assumed that the z-axis direction of the vehicle is the same as that of the factory building. An inclinometer is installed on the vehicle chassis, and the coordinates of the RTK measurement points are combined to determine the precise pose of the aerial work platform;

[0160] The inclinometer measures the angles between the x and y axes of the sensor and the sea level, that is, the complementary angles to the angles with the gravity direction; if the x and y axes of the sensor are made to coincide with the x and y axes of the aerial work platform during installation, then the angles between the x and y axes of the aerial work platform and the gravity direction can be measured; since the gravity direction is approximately the same as the opposite direction of the z-axis of the factory building, therefore, the angles between the x and y axes of the aerial work platform and the z-axis of the factory building can actually be measured through the inclinometer;

[0161] Assume that the readings of the inclinometer in the x and y axis directions are α x , α y , so the unit vector in the gravity direction in the coordinate system of the aerial work platform can be expressed as:

[0162]

[0163] And the coordinates of the unit vector in the gravity direction in the coordinate system of the factory building are:

[0164]

[0165] Therefore, assume the position of the aerial work platform in the workshop is p C and the attitude is R C , combining the installation positions of two RTKs in the aerial work platform C p A and C p B , and the measured positions p A and p B in the workshop, a total of three sets of corresponding relationships are obtained:

[0166] p g = R C · C p g + p C (1)

[0167] p A = R C · C p A + p C (2)

[0168] p B = R C · C p B + p C (3)

[0169] (2) and (3) are subtracted from (1) to obtain

[0170] p A - p g = R C ·( C p A - C p g )

[0171] p B - p g = R C ·( C p B - C p g )

[0172] Denote and Orthogonalize the unit vectors:

[0173]

[0174] ​

[0175] Thus, unit orthogonal vectors are obtained. For Performing the same linear combination gives:

[0176]

[0177]

[0178] Therefore

[0179]

[0180]

[0181] Since R C is a rotation matrix, thus are also unit orthogonal vectors, and thus we obtain:

[0182]

[0183] Thus

[0184] After calculating the attitude of the aerial work platform, adding equations (1), (2), and (3) gives the position of the aerial work platform:

[0185]

[0186] From the above derivation, it is easy to see that the readings of the inclination sensors essentially determine the correspondence of a three-dimensional vector in the coordinate systems of the aerial work platform and the factory building, i.e., the C p g and p g correspondence in the previous section. Therefore, if the inclination sensors are not used, the RTK measurement points on the end platform of the aerial work platform can also be used to determine the correspondence of a three-dimensional vector (i.e., the coordinates of a point) in the coordinate systems of the aerial work platform and the factory building. Since the position of the end platform of the aerial work platform is at the zero position before starting the operation, the coordinates of the RTK on the platform in the coordinate system of the aerial work platform are fixed. If the coordinates of this position in the aerial work platform can be measured and combined with the RTK measurement points on the two chassis of the aerial work platform, the pose of the aerial work platform can also be determined.

[0187] Thus, a method for determining the pose of the aerial work platform in the factory building using only three RTK measurement points is given.

[0188] Based on the above discussion, we have given three methods for determining the pose of the aerial work platform in the factory building, which can be summarized as follows:

[0189] (1) For the scenario of a flat factory floor, the pose of the aerial work platform in the factory can be determined using only the two RTK measurement points installed on the vehicle chassis.

[0190] (2) For the scenario of an uneven factory floor, there are two improvement methods:

[0191] 1) Install an inclinometer on the vehicle chassis, and determine the pose of the aerial work platform in the factory based on the readings of the two RTK measurement points and the inclinometer.

[0192] 2) Without installing an inclinometer, use the RTK measurement point installed on the end platform. As long as the boom is kept in the zero position and fixed before operation, the pose of the aerial work platform in the factory can also be determined based on the three RTK measurement points.

[0193] Step 3: Determine the position and orientation of the end platform of the boom in the factory coordinate system

[0194] The position of the end platform in the factory can be directly measured using the RTK installed on the platform. To calculate the orientation of the platform, the kinematic model of the aerial work platform and the pose of the aerial work platform in the factory that has been calculated can be used. Since the end platform is always nearly parallel to the vehicle chassis, the boom of the aerial work platform has only three degrees of freedom: rotation, pitch, and extension. Based on the position of a certain point in the end platform in the aerial work platform, the joint values of the boom can be inversely solved, so that the pose of the platform in the aerial work platform can be calculated using forward kinematics. Finally, the pose of the end platform in the factory can be calculated using the pose of the aerial work platform in the factory.

[0195] Model the kinematics of the boom using the standard D-H parameters as follows:

[0196]

[0197] The pose of the end platform in the aerial work platform is uniquely determined by the three joint values θ1, θ2, and d3, and the remaining parameters a i , d i are obtained by actually measuring the dimensions of the boom;

[0198] Now, calculate the three joint values θ1, θ2, and d3 based on the measured values of the end platform RTK; assume that the point measured by the end platform RTK in the factory coordinate system is p P , and the pose of the aerial work platform in the factory is represented by a 4×4 homogeneous matrix as follows:

[0199]

[0200] Then, the position coordinates of the end platform RTK in the aerial work platform can be calculated:

[0201]

[0202] According to the D-H parameters of the boom, the transformation matrix between each link can be obtained:

[0203]

[0204]

[0205]

[0206]

[0207] Therefore, the transformation matrix of the end platform relative to the base coordinate system of the aerial work platform is: T = A1A2A3A4

[0208] Assume that the installation position of the RTK on the end platform is fixed and the coordinates are [x4 y4 z4] T , and the coordinates of the RTK in the base coordinate system of the aerial work platform calculated previously are C p P = [x0 y0 z0] T , then there is:

[0209]

[0210] The above equation is equivalent to:

[0211]

[0212] Calculate the inverses of the transformations A1 and A2 as follows:

[0213]

[0214] The left-hand side of the equal sign is:

[0215]

[0216] The right-hand side of the equal sign is:

[0217]

[0218] First, the second equation is in the form of C1cosθ1 + C2sinθ1 + C3 = 0, and the value of θ1 can be solved. Substituting the value of θ1 into the first equation gives an equation in the form of C4cosθ2 + C5sinθ2 + C6 = 0, and then the value of θ2 can be solved. Finally, the value of d3 can be obtained according to the third equation;

[0219] According to the measurement values of the RTK, the three joint values of the boom can be obtained, and then the pose of the end platform relative to the aerial work platform can be calculated. Using the pose of the aerial work platform relative to the factory building calculated previously, the pose of the end platform in the factory building can be obtained.

[0220] The components not described in detail in this article are prior art.

[0221] Although the specific embodiments of the present invention have been described in detail above, the present invention is not limited to the above embodiments. Within the scope of knowledge possessed by those of ordinary skill in the art, various changes can be made without departing from the gist of the present invention, and modifications or variations that do not involve creative labor are still within the protection scope of the present invention.

Claims

1. A positioning and attitude determination method for an aerial work platform based on RTK, characterized in that, The steps are as follows: Step 1: Convert the measurement values of RTK to the rectangular coordinates of the factory building Step 1: Convert the geodetic coordinates to the Earth-centered Earth-fixed rectangular coordinates In the WGS-84 coordinate system, the following geodetic constants are used to calculate rectangular coordinates. The semi-major axis of the Earth, a = 6,378,137 meters, and the square of the eccentricity of the Earth Convert latitude, longitude, and elevation to the Earth-centered, Earth-fixed rectangular coordinate system (X, Y, Z) Step 2: Determine the rigid transformation between the Earth-centered Earth-fixed coordinate system and the factory building coordinate system Since both the Earth-centered Earth-fixed coordinate system and the plant coordinate system are rectangular coordinate systems and the length unit is meters in both, there exists a definite rigid transformation \(R, t\) between the two coordinate systems such that \(\mathbf{p}\) A = \(R\cdot\) E \(\mathbf{p}\) A + \(t\), where \(\mathbf{p}\) A is the coordinate of point \(A\) in the plant coordinate system, E \(\mathbf{p}\) A is the coordinate of point \(A\) in the Earth-centered Earth-fixed coordinate system, \(R\in SO(3)\) is the rotation matrix, is the translation vector; To determine the parameters R and t of the rigid transformation, several fixed points q1, q2, ..., q are selected in the factory building n , and at each point, the coordinates p1, p2, ..., p of the point in the Earth-centered Earth-fixed coordinate system are measured using RTK n , then the selection of the parameters R and t should minimize the sum of the squares of the error distances Parameters that meet the above conditions can be solved through the following steps: (1) Calculate the centroids of two sets of points (2) Calculate two sets of centralized vectors (3), Calculate the 3×3 covariance matrix S = XY T , where X = [x1…x n , Y = [y1…y n ​ (4), Calculate the singular value decomposition of S, S = U∑V T , then the optimal rotation matrix is (5), the optimal translation vector is Complete the direct transformation of the measurement values of RTK to the factory building coordinate system, so that in the following parts, it is assumed that RTK directly outputs the x, y, and z coordinates in the factory building coordinate system; Step 2: Determine the position and attitude of the aerial work platform in the factory building coordinate system Install two RTK measurement points on the vehicle chassis, and use the two RTK measurement points to determine the position and attitude of the aerial work platform. The method is as follows: Suppose the installation positions of two RTKs in the aerial work vehicle are respectively C p A and c p B It is known that Meanwhile, based on the measurement values of RTK, the positions p of two points in the workshop can be known. A and p B Determine the position p of the aerial work platform in the factory building based on these two pairs of measurement points C and the attitude R C ; First, there are the following relationships p A = R C · C p A + p C (1) p B = R C · C p B + p C (2) If the vehicle chassis is parallel to the ground, that is, the z-axis of the aerial work vehicle is the same as the z-axis of the factory building, then the column vectors of the rotation matrix R C can be expressed as: R C = [r1 r2 e3] From the orthogonality of the column vectors, we can get <r1, e3> = <r2, e3> = 0, so Subtracting equation (2) from equation (1) gives: p B -p A = R C ·( C p B - C p A ) Written in coordinate form: Only considering the first two terms, we get: The matrix on the left side of the equation is a 2D rotation matrix, so it can be written as: Thus, a system of equations in the following form is obtained: C1cosθ + C2sinθ + C3 = 0 C1cosθ - C2sinθ + C4 = 0 There is a unique solution θ = arctan2(-C1C4 - C2C3, C2C4 - C1C3); After calculating the attitude of the aerial work platform, add equations (1) and (2) to obtain the position of the aerial work platform If the vehicle chassis is not parallel to the ground, in order to determine the accurate pose of the aerial work platform, it cannot be assumed that the z-axis direction of the vehicle is the same as the z-axis direction of the factory building. Install an inclination sensor on the vehicle chassis and combine the coordinates of the RTK measurement points to determine the accurate pose of the aerial work platform; Assume that the readings of the inclination sensor in the x and y axis directions are α x , α y , so the unit vector in the direction of gravity can be expressed in the coordinate system of the aerial work platform as: And the coordinates of the unit vector in the direction of gravity in the factory building coordinate system are: Therefore, assume that the position of the aerial work platform in the factory building is p C and the attitude is R C , combined with the installation positions of the two RTKs on the aerial work platform C p A and C p B , and the measured positions p A and p B in the factory building, a total of three sets of corresponding relationships are obtained: p g = R C · C p g + p C (1) p A = R C · C p A + p C (2) p B = R C · C p B + p C (3) (2), (3) minus (1) to get p A -p g = R C ·( C p A -cp g ) p B -p g = R C ·( C p B - C p g ) Record and will Unit orthonormalization: Thus, an orthonormal vector is obtained. For performing the same linear combination, we get: Therefore Since R C is a rotation matrix, so are also unit orthogonal vectors, thus we get: Thus After calculating the attitude of the aerial work platform, add equations (1), (2), and (3) to obtain the position of the aerial work platform: Step 3: Determine the position and attitude of the end platform of the boom in the factory building coordinate system Model the kinematics of the boom using the standard D-H parameters as follows: The pose of the end platform in the aerial work vehicle is uniquely determined by three joint values θ1, θ2, and d3, and the remaining parameters a i , d i are obtained by actually measuring the dimensions of the boom; Now, calculate three joint values θ1, θ2, and d3 based on the measurement values of the end - effector platform RTK; assume that the point measured by the end - effector platform RTK in the factory coordinate system is p P , and the pose of the aerial work platform in the factory is represented by the following 4×4 homogeneous matrix: Then the position coordinates of the end platform RTK in the aerial work platform can be calculated: According to the D-H parameters of the boom, the transformation matrices between each link can be known: Thus, the transformation matrix of the end platform relative to the base coordinate system of the aerial work platform is: T = A1A2A3A4 Assume that the installation position of the RTK on the end platform is fixed and the coordinates are [x4 y4 z4] T , and the coordinates of the RTK in the base coordinate system of the aerial work vehicle calculated previously are C p P = [x0 y0 z0] T , then there is: The above equation is equivalent to: Calculate the inverses of the transformations A1 and A2 as follows: The left side of the equal sign is: The right side of the equal sign is: First, the second equation is in the form of C1cosθ1 + C2sinθ1 + C3 = 0, and the value of θ1 can be solved. Substitute the value of θ1 into the first equation to get an equation in the form of C4cosθ2 + C5sinθ2 + C6 = 0, so that the value of θ2 can be solved. Finally, the value of d3 can be obtained according to the third equation; According to the measurement values of RTK, the three joint values of the boom can be obtained, so as to calculate the pose of the end platform relative to the aerial work platform. Then, using the pose of the aerial work platform relative to the factory building calculated before, the pose of the end platform in the factory building can be obtained.

Images