Monocular vision relative pose measurement method for automatic docking of cabin sections and cabin section docking method

Abstract

The invention relates to a cabin posture measurement method, in particular to a monocular vision relative posture measurement method for cabin automatic butt joint and a cabin butt joint method, and aims to overcome the defects that in an existing automatic butt joint method, the universality of posture relation calibration is poor, and the operation process is tedious. According to the cabin butt joint method, L target tools are arranged on the butt joint faces of a fixed cabin and a movable cabin in a one-to-one correspondence mode, and L is larger than or equal to 2; according to the cabin butt joint method, the relative pose of the movable cabin is calculated through a relative pose measurement method, and automatic butt joint of the cabin is achieved specifically through the steps of collecting images of a to-be-butt-jointed state, calculating the relative pose of the movable cabin, guiding the pose adjusting mechanism to adjust the angle of the movable cabin, guiding the pose adjusting mechanism to adjust the displacement of the movable cabin and the like.

Description

Technical Field

[0001] The invention relates to a cabin posture measurement method, and in particular to a monocular vision relative posture measurement method for cabin automatic docking and a cabin docking method. Background Art

[0002] The production and manufacturing of missiles and spacecraft in the fields of aviation and aerospace, as well as underwater spacecraft in the field of ships, all involve cabin docking. Cabin docking is a key link in its production and manufacturing, and it is a complex project with high technical difficulty and involving many fields. The quality of cabin docking affects the overall performance of the product to a certain extent. Traditional cabin docking uses rigid tooling positioning and manual hole making. It is difficult to quickly adjust the coaxiality error between cabins and the alignment deviation of the locating pin and the locating groove in the circumferential direction to the required range by human eyes, resulting in high labor intensity, low work efficiency, and long assembly cycle for workers. Moreover, the assembly effect of this method depends on the experience and operation level of the workers. The overall docking accuracy is low, the docking speed is slow, and the operation stability is poor. Therefore, realizing the automatic docking of cabins is the goal and trend of the development of cabin assembly. The automatic docking of cabins requires high precision to ensure production quality, improve efficiency and reduce investment costs, and realize automation through non-contact automatic measurement of cabin posture. A Chinese patent with publication number CN112381881A discloses a method for automatic docking of large rigid components based on monocular vision. Through monocular vision measurement, positioning and guidance technology, the connected target on the surface of the large rigid component to be docked is quickly measured to guide the attitude adjustment platform for docking. This technical solution requires pre-calibration of the posture relationship between the mobile and fixed rigid components in the docking state, and this posture relationship is only applicable to the same type of products at present, with poor versatility and cumbersome operation process. Summary of the invention

[0003] The purpose of the present invention is to solve the shortcomings of the existing automatic docking methods, such as poor versatility of posture relationship calibration and complicated operation process, and to provide a monocular vision relative posture measurement method and a cabin docking method for automatic cabin docking.

[0004] Invention Concept

[0005] In view of the situation that the cabin section is large in size and it is inconvenient to hang a connected target on the docking surface, or there are too few positioning pin holes on the docking end surface to install multiple targets, and the cabin section surface has no obvious geometric features, the present invention arranges 2L small target toolings, L≥2, and L target toolings are arranged one by one on the docking surface of the fixed cabin section and the docking surface of the movable cabin section, and the target toolings are installed through the docking points of the docking surface; the image of the docking state is collected by a monocular vision system to ensure that each target tooling is included in the image, the relative posture of the movable cabin section is accurately calculated according to the image of the docking state, and the attitude adjustment mechanism is guided to adjust the yaw angle, pitch angle and roll angle of the movable cabin section to align with the angle of the fixed cabin section, and then the relative displacement is calculated according to the coordinates of the docking point where the target tooling is installed, and the attitude adjustment mechanism is guided to adjust the displacement of the movable cabin section in the horizontal direction, vertical direction and heading direction, so as to realize automatic docking of the cabin section.

[0006] To achieve the above objectives, the technical solutions provided by the present invention are as follows:

[0007] A monocular vision relative posture measurement method for automatic docking of cabin sections is special in that it comprises the following steps:

[0008] S1. Build a measuring device, which includes a posture adjustment mechanism, a monocular vision system and 2L target fixtures, where L≥2; the posture adjustment mechanism is arranged on the mobile vehicle body, the movable cabin is arranged on the posture adjustment mechanism, and the monocular vision system is arranged on the mobile vehicle body or the fixed platform; the 2L target fixtures are evenly divided into two groups, which are arranged at corresponding positions of the docking surface of the movable cabin section and the docking surface of the fixed cabin section respectively; the target fixture includes a fixture body, an assembly hole arranged on the fixture body and a plurality of target feature points located in the same plane, the assembly hole is coaxially connected with the docking point on the corresponding docking surface, and the plane where the target feature point is located is perpendicular to the corresponding docking surface;

[0009] S2. Use a monocular vision system to obtain a corresponding image of the docking state, ensuring that the image of the docking state includes a complete set of 2L target fixtures;

[0010] S3, extracting the coordinates of the target feature points and assembly holes of the 2L target tooling in the image of the docking state in the camera coordinate system, and converting them to obtain their corresponding coordinates in the coordinate system of the posture adjustment mechanism;

[0011] S4. In the coordinate system of the attitude adjustment mechanism, the plane equations of the target feature points of the 2L target fixtures are respectively fitted, and the corresponding normal vectors are respectively calculated to obtain the target plane normal vector. The normal vector of the active cabin docking surface and the normal vector of the fixed cabin docking surface are calculated from the target plane normal vector and the corresponding docking point coordinates of the target fixture;

[0012] S5. In the coordinate system of the attitude adjustment mechanism, the yaw angle α and pitch angle β of the movable cabin relative to the fixed cabin are calculated according to the normal vector of the docking surface of the movable cabin and the normal vector of the docking surface of the fixed cabin. The roll angle γ of the movable cabin relative to the fixed cabin is calculated according to the coordinates of the corresponding docking points of the two sets of target tooling. The relative displacement of the movable cabin relative to the fixed cabin is calculated according to the coordinates of the corresponding docking points of the two sets of target tooling to complete the relative attitude measurement; the coordinates of the docking points are the same as the coordinates of the corresponding target tooling assembly holes.

[0013] Further, in step S5, the yaw angle α and pitch angle β of the movable compartment relative to the fixed compartment are calculated based on the normal vector of the docking surface and are specifically:

[0014] According to the normal vector of the docking surface, the rotation matrix of the movable cabin relative to the fixed cabin is calculated based on the Rodriguez rotation formula, and then the yaw angle α and pitch angle β of the movable cabin relative to the fixed cabin are obtained;

[0015] The rolling angle γ of the movable cabin relative to the fixed cabin is calculated according to the coordinates of the corresponding docking points of the two sets of target fixtures, specifically:

[0016] The direction vector of the line connecting the two target fixtures on the fixed section is calculated according to the corresponding docking points, and the direction vector of the line connecting the two target fixtures on the movable section is calculated according to the corresponding docking points. Then the angle between the two direction vectors is calculated to obtain the rolling angle γ of the movable section relative to the fixed section.

[0017] Furthermore, step S3 is specifically as follows:

[0018] S3.1. Extract the image coordinates of the target feature points of the 2L target fixtures in the docking state image, combine the three-dimensional coordinates of the target feature points in the target coordinate system and the internal parameters of the pre-calibrated monocular vision system, and use the PNP algorithm to calculate the rotation matrix that converts the three-dimensional coordinates of the target feature points on each target fixture in the docking state in the target coordinate system to the camera coordinate system. and translation vector Among them, the value of i is 1, 2, ..., 2L;

[0019] The three-dimensional coordinates of the target feature points in the target coordinate system are obtained by pre-calibration or its processing model;

[0020] S3.2, according to the three-dimensional coordinates and rotation matrix of the target feature point in the target coordinate system and translation vector Calculate the coordinates of all target feature points in the camera coordinate system;

[0021] S3.3, according to the three-dimensional coordinates and rotation matrix of the target assembly hole in the target coordinate system and translation vector Calculate the coordinates of all target assembly holes in the camera coordinate system;

[0022] S3.4. According to the rigid body transformation relationship between the pre-calibrated camera coordinate system and the attitude adjustment mechanism coordinate system Calculate the coordinates of the target feature points and target assembly holes in the coordinate system of the posture adjustment mechanism; where: is the rotation matrix between the camera coordinate system and the attitude adjustment mechanism coordinate system, is the translation vector between the camera coordinate system and the attitude adjustment mechanism coordinate system.

[0023] Furthermore, in step S5, the calculation of the relative displacement of the movable compartment relative to the fixed compartment is specifically as follows: calculating the coordinate difference of the corresponding docking points of the two sets of target tooling, and then calculating the average value of the coordinate difference of the docking points, that is, obtaining the relative displacement of the movable compartment relative to the fixed compartment.

[0024] Further, in step S1, L=2;

[0025] In step S5, the coordinates of the assembly holes of the target fixture in the coordinate system of the posture adjustment mechanism are recorded as The relative displacement of the movable compartment relative to the fixed compartment is calculated according to the following formula:

[0026]

[0027] Among them, u, v, and w are the relative displacements of the movable compartment relative to the fixed compartment in the heading direction, horizontal direction, and vertical direction, respectively.

[0028] At the same time, the present invention also provides a compartment docking method, which is special in that it includes the following steps:

[0029] Step 1, constructing the measuring device as described in step S1;

[0030] Step 2, using the above steps S2 to S5 to calculate the yaw angle α and pitch angle β of the movable compartment relative to the fixed compartment;

[0031] Step 3, adjusting the attitude adjustment mechanism according to the yaw angle α and pitch angle β obtained in step 2;

[0032] Step 4, using the above steps S2 to S5 to calculate the roll angle γ of the movable compartment relative to the fixed compartment;

[0033] Step 5, adjusting the attitude adjustment mechanism according to the roll angle γ obtained in step 4;

[0034] Step 6, using the above steps S2 to S5 to calculate the relative displacement of the movable compartment with respect to the fixed compartment;

[0035] Step 7: Adjust the attitude adjustment mechanism according to the relative displacement obtained in step 4 to complete the docking of the movable cabin and the fixed cabin.

[0036] Furthermore, step 3 is specifically as follows:

[0037] Determine whether the yaw angle α and pitch angle β described in step 2 are within the set first convergence threshold range. If not, adjust the attitude adjustment mechanism according to the yaw angle α and pitch angle β described in step 2, and then return to step 2. If within the range, proceed to step 4.

[0038] Furthermore, step 5 is specifically as follows:

[0039] Determine whether the roll angle γ in step 4 is within the set second convergence threshold range. If not, adjust the attitude adjustment mechanism according to the roll angle γ in step 4, and then return to step 4. If within the range, proceed to step 6.

[0040] Furthermore, step 7 is specifically as follows:

[0041] 5.1. Determine whether the relative displacement in step 6 is within the set second convergence threshold range. If not, adjust the posture adjustment mechanism according to the relative displacement in step 6, and then return to step 6. If within the range, complete the adjustment.

[0042] 5.2. Remove the target tooling, control the attitude adjustment mechanism to move along the heading direction, and achieve docking between the movable compartment and the fixed compartment.

[0043] Beneficial effects of the present invention:

[0044] 1. The present invention installs L target fixtures on the docking surface of the movable cabin section and the docking surface of the fixed cabin section, L≥2, and at least two target fixtures are set on each docking surface to complete the docking posture measurement, without the need for coordinate information of other docking points of the docking surface. The plane where the target feature point of the present invention is located is perpendicular to the corresponding docking surface, so it is suitable for the situation where the installation position is not fixed due to the rotation around the axis of the docking point during the installation of the target fixture. It is also suitable for the situation where the cabin size is large or the number of docking points on the docking surface is small, thereby improving the flexibility and scope of application of relative posture measurement.

[0045] 2. The present invention collects and calculates the spatial posture of the target tooling through a monocular vision system, thereby solving the relative posture of the corresponding cabin section, and then calculates the relative translation value of the cabin section according to the coordinates of the docking points where the target tooling is installed. The operation is simple and reliable, with high measurement accuracy and low requirements on the number of docking points of the docking surface. There is no need to obtain the coordinates of the remaining docking points of the target tooling that are not set, and there is no need to calibrate the docking points of the docking surface in advance. Therefore, the calibration process of the present invention is simple, and only the camera intrinsic parameters, camera extrinsic parameters, and the positional relationship between the target feature points and the assembly holes on the target tooling need to be calibrated in advance. When the target feature points are arranged on the target tooling by mechanical processing, the positional relationship between the marker points and the assembly holes can be obtained through the processing model, and it may not even be calibrated in advance.

[0046] 3. The present invention uses a target tooling to lead out the coordinates of the docking points of the cabin sections, without pasting or printing targets on the surface of the cabin sections. The target tooling images suspended on the docking surfaces of the fixed cabin sections and the movable cabin sections are collected in real time through a monocular vision system, and the docking posture is obtained in combination with the three-dimensional coordinates of the target tooling in the target coordinate system, thereby guiding the posture adjustment mechanism to adjust the movable cabin sections to realize automatic docking. The measurement system has a simple structure, high stability, and is easy to operate and maintain. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 is a schematic structural diagram of an embodiment of the present invention;

[0048] Figure 2 Schematic diagram of target feature points in an embodiment of the present invention.

[0049] Description of reference numerals:

[0050] 1-attitude adjustment mechanism, 2-movable cabin, 3-fixed cabin, 4-docking point, 5-third target tooling, 6-fourth target tooling, 7-movable body, 8-first target tooling, 9-second target tooling, 10-monocular camera. DETAILED DESCRIPTION

[0051] The present invention provides a monocular vision relative posture measurement method for automatic cabin docking, which is used to measure the posture of a movable cabin 2 relative to a fixed cabin 3. The cabin docking method adjusts the movable cabin 2 through a posture adjustment mechanism 1 based on the measured posture. In this embodiment, four steps are sequentially performed: monocular vision system internal parameter calibration, monocular vision system external parameter calibration, target tooling calibration, docking posture solution, and movable cabin 2 posture adjustment to achieve automatic cabin docking. The monocular vision system of the present invention is provided with a monocular camera 10, and the monocular vision system internal parameter calibration and its external parameter calibration are the internal parameter calibration and external parameter calibration of the monocular camera 10. The cabin docking method of this embodiment specifically includes the following steps:

[0052] Step 1: Monocular camera 10 internal parameter calibration

[0053] The internal parameters of the monocular camera 10 are calibrated using a high-precision internal calibration board to obtain the internal parameters of the monocular camera 10. The internal calibration board is provided with a plurality of feature points. The internal parameters of the monocular camera 10 include the focal length of the lens, the coordinates of the intersection of the camera optical axis and the imaging physical plane in the image coordinate system, and the distortion coefficient. The following steps are included:

[0054] 1) Within the field of view of the monocular camera 10, move the intrinsic calibration plate 8-12 times in a non-parallel manner, and try to cover the entire field of view. The camera takes an image of the intrinsic calibration plate every time it moves to a certain position. The intrinsic calibration plate image should include all the feature points on the intrinsic calibration plate, extract the center of the feature points on the intrinsic calibration plate, and correspond them to their world coordinates one by one.

[0055] 2) Using the image coordinates and world coordinates of the landmark points, the camera perspective projection model as well as the camera's internal parameters and distortion coefficients are obtained.

[0056] The specific calibration method refers to Zhang Zhengyou's paper "A flexible new technique for camera calibration. IEEE Transactions on Pattern Analysis and Machine Intelligence [JJ. 2000, 22 (11): 1330-1334". Definition: The coordinates of the center of any feature point P on the intrinsic calibration plate in the world coordinate system are (X w ,Y w ,Z w ), the coordinates in the pixel coordinate system are (u, v), then the camera perspective projection model is as follows;

[0057]

[0058] Where:

[0059] λ is an arbitrary non-zero scale factor;

[0060] A is the camera internal parameter matrix;

[0061] f x 、f y is the effective focal length of the camera in the X and Y directions, that is, the ratio of the physical size of the unit pixel in the X and Y axis directions in the image coordinate system to the focal length;

[0062] (u0, v0) is the coordinate of the intersection of the camera optical axis and the image plane in the pixel coordinate system;

[0063] (R, T) is the camera external parameter, which represents the conversion relationship between the world coordinate system and the camera coordinate system.

[0064] Step 2: Monocular camera 10 external parameter calibration

[0065] That is, the conversion relationship between the monocular camera 10 and the coordinate system of the attitude adjustment mechanism is calibrated. In the present embodiment, the attitude adjustment mechanism 1 adopts a six-degree-of-freedom attitude adjustment mechanism 1, and the movable cabin 2 is arranged on the attitude adjustment mechanism 1. In order to guide the six-degree-of-freedom attitude adjustment mechanism 1 to achieve precise docking of large cabins, it is necessary to use hand-eye calibration to solve the conversion relationship between the monocular camera 10 and the coordinate system of the attitude adjustment mechanism, and finally convert the three-dimensional coordinate docking posture data of the target mark point and the assembly hole in the target coordinate system to the coordinate system of the attitude adjustment mechanism. The attitude adjustment mechanism 1 of the present invention is arranged on the mobile body 7, and the monocular camera 10 can be installed on the mobile body 7 or the fixed platform, and the external parameter calibration step can be applied. The specific steps are as follows:

[0066] 1) Design the hand-eye calibration mechanical fixture according to the installation method of the monocular camera 10, determine its installation position, and stick the external parameter calibration plate to the hand-eye calibration mechanical fixture. In this embodiment, the monocular camera 10 is fixedly connected to the mobile vehicle body 7, and the hand-eye calibration mechanical fixture is installed on the posture adjustment mechanism 1 and is within the field of view of the monocular camera 10;

[0067] In other embodiments of the present invention, the monocular camera 10 is installed on a fixed platform, and the hand-eye calibration mechanical tooling is installed on the posture adjustment mechanism 1.

[0068] 2) During calibration, the posture adjustment mechanism 1 is manually or automatically controlled to continuously move three positions along one of the X, Y, and Z directions. During the movement, the external reference calibration plate is always within the field of view of the monocular camera 10, and the current external reference calibration plate image is captured each time it moves to a certain position;

[0069] 3) Repeat step 2), control the posture adjustment mechanism 1 to move in another direction, and take an image of the external reference calibration plate;

[0070] 4) The movement of step 2) and step 3) is represented by rotation transformation B (including rotation matrix and translation vector) in the coordinate system of the posture adjustment mechanism, and by rotation transformation A in the camera coordinate system. It is defined that the transformation relationship from the camera coordinate system to the coordinate system of the posture adjustment mechanism is E. At this time, a typical hand-eye calibration equation can be constructed: AE=EB. After solving E, the rotation matrix and translation vector from the camera coordinate system to the coordinate system of the posture adjustment mechanism can be obtained.

[0071] Step 3: Target tooling calibration

[0072] The target fixture used in the present invention includes a fixture body, a plurality of target feature points arranged on the fixture body and located in the same plane, and an assembly hole arranged on the fixture body. The fixture body is connected to the docking point 4 of the docking surface through the assembly hole. The docking point 4 includes a pin hole and a pin shaft respectively arranged on the fixed compartment 3 and the movable compartment 2. Each target fixture is calibrated separately using a high-precision three-dimensional measuring device, the purpose of which is to establish a corresponding target coordinate system and record the three-dimensional coordinates of the target fixture in the target coordinate system, that is, the three-dimensional coordinates of the target feature points and the assembly holes in the target coordinate system. In this embodiment, a total of 4 target fixtures are provided, i=1, 2, 3, 4.

[0073] In other embodiments of the present invention, the target feature points can be set on their corresponding target tooling through mechanical processing, and the three-dimensional coordinates of the target feature points and assembly holes in the target coordinate system can be directly obtained through the physical model of mechanical processing without prior calibration.

[0074] Steps 1 to 3 belong to system calibration work. Generally, calibration is only required once, and the calibration results can be reused later.

[0075] Step 4: Solving the docking posture and adjusting the posture of the movable module 2

[0076] 1) The monocular camera 10 is fixedly mounted on the mobile vehicle 7, and the four target fixtures are evenly divided into two groups, and are installed one by one on the docking points 4 of the docking surface of the movable cabin 2 and the docking surface of the fixed cabin 3. When the target fixture is installed, the assembly hole is coaxially connected with the docking point 4 of the corresponding docking surface, and the plane where the target feature point is located is perpendicular to the corresponding docking surface, which is used to calculate the posture of the corresponding cabin. Two target fixtures are installed on each docking surface. The target fixtures on the fixed cabin 3 are respectively recorded as the first target fixture 8 and the second target fixture 9, and the target fixtures on the movable cabin 2 are respectively recorded as the third target fixture 5 and the fourth target fixture 6.

[0077] 2) Use the monocular camera 10 to capture the image of the docking state. Ensure that the image of the docking state includes the complete first target fixture 8, the second target fixture 9, the third target fixture 5 and the fourth target fixture 6.

[0078] 3) Process the collected image of the docking state to extract the coordinates of the target feature points and the assembly holes in the camera coordinate system; specifically:

[0079] 3.1) The three-dimensional coordinates of the target feature points in the first target fixture 8, the second target fixture 9, the third target fixture 5 and the fourth target fixture 6 measured by the high-precision three-dimensional measuring equipment in step 3 in the target coordinate system are recorded as The corresponding assembly hole coordinates are recorded as: The rigid body transformation relationship between the camera coordinate system and the attitude adjustment mechanism coordinate system in step 2 is recorded as is the corresponding rotation matrix, is the corresponding translation vector;

[0080] 3.2) Extract and identify the image features of the docking state image to obtain the image coordinates of the target feature points on each target tooling, combine the pre-calibrated three-dimensional coordinates of the target feature points in the target coordinate system and the internal parameters of the monocular camera 10 obtained in step 1, and use the PNP algorithm to calculate the rigid body transformation relationship of the three-dimensional coordinates of the target feature points in the docking state in the target coordinate system to the camera coordinate system. Where i = 1, 2, 3, 4, is the corresponding rotation matrix, is the corresponding translation vector.

[0081] 3.3) The coordinates of all target feature points in the camera coordinate system are calculated according to the following formula:

[0082]

[0083] Among them, P1 t P2 is the coordinate of the target feature point on the first target fixture 8 in the camera coordinate system; t P3 is the coordinate of the target feature point on the second target fixture 9 in the camera coordinate system; t P4 is the coordinate of the target feature point on the third target fixture 5 in the camera coordinate system; t is the coordinate of the target feature point on the fourth target fixture 6 in the camera coordinate system; They are respectively the rotation matrix and translation matrix of the first target fixture 8 in the target coordinate system for converting the three-dimensional coordinates to the camera coordinate system. They are respectively the rotation matrix and translation matrix of the second target fixture 9 in the target coordinate system for converting the three-dimensional coordinates to the camera coordinate system. They are respectively the rotation matrix and translation matrix of the third target fixture 5 in the target coordinate system converted to the camera coordinate system. They are respectively the rotation matrix and the translation matrix for converting the three-dimensional coordinates of the fourth target tooling 6 in the target coordinate system to the camera coordinate system.

[0084] 3.4) The position relationship between the target feature point and the assembly hole on the target fixture is fixed, so the coordinates of the assembly hole of the target fixture in the camera coordinate system are calculated by the following formula:

[0085]

[0086] 3.5) The coordinates of the target feature points in the camera coordinate system are converted to the coordinate system of the attitude adjustment mechanism according to the following formula;

[0087]

[0088] in, is the coordinate of the target feature point in the coordinate system of the posture adjustment mechanism, is the coordinate of the target feature point in the camera coordinate system, i=1,2,3,4;

[0089] Similarly, the coordinates of the target fixture's assembly hole in the attitude adjustment mechanism coordinate system can be obtained: i=1,2,3,4, the assembly hole of the target fixture is coaxial with the corresponding docking point 4, so the coordinates of the assembly hole in the posture adjustment mechanism coordinate system can represent the coordinates of the docking point 4 of the corresponding docking surface, and the coordinates of the docking points 4 corresponding to the first target fixture 8, the second target fixture 9, the third target fixture 5 and the fourth target fixture 6 in the posture adjustment mechanism coordinate system are respectively

[0090] 4) Calculate the angle of movable compartment 2 relative to fixed compartment 3

[0091] 4.1) According to the coordinates of the target feature points of each target fixture in the coordinate system of the posture adjustment mechanism, the target plane equations of the first target fixture 8, the second target fixture 9, the third target fixture 5 and the fourth target fixture 6 are respectively fitted as follows:

[0092] A1x+B1y+C1z+D1=0

[0093] A2x+B2y+C2z+D2=0

[0094] A3x+B3y+C3z+D3=0

[0095] A4x+B4y+C4z+D4=0

[0096] 4.2) According to the above target plane equation, the corresponding target plane normal vector is calculated. Combined with the vertical relationship between the target plane and its corresponding docking surface, as well as the coordinates of the docking point 4 corresponding to the target tooling, the plane equations of the docking surface of the fixed cabin 3 and the docking surface of the movable cabin 2 are fitted respectively, and then their normal vectors are obtained.

[0097] Using the normal vectors of the docking surfaces of the fixed cabin 3 and the movable cabin 2, the rotation matrix of the movable cabin 2 relative to the fixed cabin 3 is calculated based on the Rodriguez rotation formula, and then the yaw angle α and pitch angle β of the movable cabin 2 relative to the fixed cabin 3 are obtained.

[0098] 5) Determine whether the yaw angle α and the pitch angle β are within the set first convergence threshold range. If not within the range, guide the attitude adjustment mechanism 1 to adjust the movable cabin 2 according to the yaw angle α and the pitch angle β in step 4.2), and then return to step 4.2) for iterative calculation; if within the range, it means that the yaw angle α and the pitch angle β are adjusted in place to achieve parallelism of the two docking surfaces, and proceed to the next step. The first convergence threshold can be determined according to the actual situation to ensure the accuracy of the angle adjustment.

[0099] 6) In the coordinate system of the attitude adjustment mechanism, the line equation is calculated according to the coordinates of the docking point 4 corresponding to the first target fixture 8 and the second target fixture 9, and the line equation is calculated according to the coordinates of the docking point 4 corresponding to the third target fixture 5 and the fourth target fixture 6, and then the direction vectors of the two line equations are obtained respectively, and the angle between the two direction vectors is calculated to obtain the rolling angle γ of the docking surface of the movable cabin 2 relative to the docking surface of the fixed cabin 3.

[0100] 7) Determine whether the roll angle γ is within the set second convergence threshold range. If not, guide the attitude adjustment mechanism 1 to adjust the movable cabin 2 according to the value, and then return to step 4, step 2) for iterative calculation. Since the yaw angle α and pitch angle β have been adjusted in step 4, step 4, step 4) and step 5) do not need to be repeated after returning to step 4, step 4) and step 5) are not required. If it is within the range, it means that the roll angle γ is adjusted in place, and the next step is entered. The second convergence threshold can be determined according to actual conditions.

[0101] 8) Calculate translation parameters

[0102] After the angle is adjusted, there is only a translation change between the movable cabin 2 and the fixed cabin 3. The translation parameter is calculated based on the average value of the coordinate difference between the docking surface and the corresponding contact point 4 in the attitude adjustment mechanism coordinate system. Specifically, according to the formula shown below, the relative displacement of the movable cabin 2 relative to the fixed cabin 3 is calculated respectively. u, v, and w are the relative displacements of the movable cabin 2 along the heading direction, horizontal direction, and vertical direction, respectively, which are also the relative displacements along the three directions of the attitude adjustment mechanism coordinate system.

[0103]

[0104] 9) Determine whether the relative displacements in the heading direction, horizontal direction, and vertical direction meet the set third convergence threshold. If not, guide the attitude adjustment mechanism 1 to adjust the movable cabin 2 according to the relative displacements in the heading direction, horizontal direction, and vertical direction calculated in step 6) of step 4, and return to step 2) of step 4 to perform iterative calculation. Since the angle adjustment has been completed in step 5) of step 4, it is unnecessary to repeat steps 4) to 7) of step 4 after returning to step 2) of step 4 at this time. If the second convergence threshold is met, complete this adjustment and proceed to the next step.

[0105] When setting the second convergence threshold, it is necessary to reserve space for disassembly of the target tooling in the heading direction.

[0106] 10) Remove the target tooling, control the attitude adjustment mechanism 1 to move along the heading direction, and achieve docking of the movable cabin 2 with the fixed cabin 3.

[0107] In other embodiments of the present invention, the monocular vision system may also include multiple monocular cameras 10, and the specific number can be adjusted according to actual docking needs. When multiple monocular cameras 10 are set, internal and external calibration are performed on the multiple monocular cameras 10 respectively, and the images of the docking state are obtained by the multiple monocular cameras 10 respectively to obtain the corresponding measurement postures, and then the measurement postures obtained by the multiple monocular cameras 10 are fused, and then the attitude adjustment mechanism 1 is guided to adjust the movable cabin 2 according to the fused measurement posture. The above-mentioned fusion is specifically to obtain the average value of the measurement postures obtained by the multiple monocular cameras 10, or correction is performed by other methods.

Claims

1. A monocular vision relative posture measurement method for automatic docking of cabin sections, characterized in that: The following steps are involved: S1. Build a measuring device, the measuring device comprising a posture adjustment mechanism (1), a monocular vision system and 2L target fixtures, wherein L≥2; the posture adjustment mechanism (1) is arranged on a mobile vehicle body (7), the movable cabin (2) is arranged on the posture adjustment mechanism (1), and the monocular vision system is arranged on the mobile vehicle body (7) or a fixed platform; the 2L target fixtures are evenly divided into two groups, and are respectively arranged at corresponding positions of the docking surface of the movable cabin (2) and the docking surface of the fixed cabin (3); the target fixture comprises a fixture body, an assembly hole arranged on the fixture body, and a plurality of target feature points located in the same plane, the assembly hole is coaxially connected with the docking point (4) on the corresponding docking surface, and the plane where the target feature point is located is perpendicular to the corresponding docking surface; S2. Use a monocular vision system to obtain a corresponding image of the docking state, ensuring that the image of the docking state includes a complete set of 2L target fixtures; S3, extracting the coordinates of the target feature points and assembly holes of the 2L target tooling in the image of the docking state in the camera coordinate system, and converting them to obtain their corresponding coordinates in the coordinate system of the posture adjustment mechanism; S4. In the coordinate system of the attitude adjustment mechanism, the plane equations of the target feature points of the 2L target fixtures are respectively fitted, and the corresponding normal vectors are respectively calculated to obtain the target plane normal vector. The normal vector of the docking surface of the movable cabin (2) and the normal vector of the docking surface of the fixed cabin (3) are calculated from the target plane normal vector and the corresponding docking point coordinates of the target fixture; S5. In the coordinate system of the attitude adjustment mechanism, the yaw angle α and the pitch angle β of the movable cabin (2) relative to the fixed cabin (3) are calculated based on the normal vector of the docking surface of the movable cabin (2) and the normal vector of the docking surface of the fixed cabin (3); the roll angle γ of the movable cabin (2) relative to the fixed cabin (3) is calculated based on the coordinates of the docking points (4) corresponding to the two sets of target tooling; the relative displacement of the movable cabin (2) relative to the fixed cabin (3) is calculated based on the coordinates of the docking points (4) corresponding to the two sets of target tooling, and the relative attitude measurement is completed; the coordinates of the docking points (4) are the same as the coordinates of the corresponding target tooling assembly holes.

2. The monocular vision relative posture measurement method for automatic docking of cabin sections according to claim 1 is characterized in that: In step S5, the yaw angle α and pitch angle β of the movable compartment (2) relative to the fixed compartment (3) are calculated based on the normal vector of the docking surface and are specifically: According to the normal vector of the docking surface, the rotation matrix of the movable cabin (2) relative to the fixed cabin (3) is calculated based on the Rodriguez rotation formula, and then the yaw angle α and pitch angle β of the movable cabin (2) relative to the fixed cabin (3) are obtained; The rolling angle γ of the movable compartment (2) relative to the fixed compartment (3) is calculated based on the coordinates of the corresponding docking points (4) of the two sets of target fixtures, specifically: The direction vector of the line connecting the two target fixtures corresponding to the docking points (4) on the fixed compartment (3) is calculated, the direction vector of the line connecting the two target fixtures corresponding to the docking points (4) on the movable compartment (2) is calculated, and then the angle between the two direction vectors is calculated, thereby obtaining the rolling angle γ of the movable compartment (2) relative to the fixed compartment (3).

3. The monocular vision relative posture measurement method for automatic docking of cabin sections according to claim 2 is characterized in that: Step S3 is specifically as follows: S3.

1. Extract the image coordinates of the target feature points of the 2L target fixtures in the docking state image, combine the three-dimensional coordinates of the target feature points in the target coordinate system and the internal parameters of the pre-calibrated monocular vision system, and use the PNP algorithm to calculate the rotation matrix that converts the three-dimensional coordinates of the target feature points on each target fixture in the docking state in the target coordinate system to the camera coordinate system. and translation vector Among them, the value of i is 1, 2, ..., 2L; The three-dimensional coordinates of the target feature points in the target coordinate system are obtained by pre-calibration or its processing model; S3.2, according to the three-dimensional coordinates and rotation matrix of the target feature point in the target coordinate system and translation vector Calculate the coordinates of all target feature points in the camera coordinate system; S3.3, according to the three-dimensional coordinates and rotation matrix of the target assembly hole in the target coordinate system and translation vector Calculate the coordinates of all target assembly holes in the camera coordinate system; S3.

4. According to the rigid body transformation relationship between the pre-calibrated camera coordinate system and the attitude adjustment mechanism coordinate system Calculate the coordinates of the target feature points and target assembly holes in the coordinate system of the posture adjustment mechanism; where: is the rotation matrix between the camera coordinate system and the attitude adjustment mechanism coordinate system, is the translation vector between the camera coordinate system and the attitude adjustment mechanism coordinate system.

4. The monocular vision relative posture measurement method for automatic docking of cabin sections according to claim 2 or 3 is characterized in that: In step S5, the calculation of the relative displacement of the movable compartment (2) relative to the fixed compartment (3) is specifically as follows: the coordinate difference between the corresponding docking points (4) of the two sets of target tooling is calculated, and then the average value of the coordinate difference of the docking points (4) is calculated, so as to obtain the relative displacement of the movable compartment (2) relative to the fixed compartment (3).

5. The monocular vision relative posture measurement method for automatic docking of cabin sections according to claim 4 is characterized in that: In step S1, L=2; In step S5, the coordinates of the assembly holes of the target fixture in the coordinate system of the posture adjustment mechanism are recorded as The relative displacement of the movable compartment (2) relative to the fixed compartment (3) is calculated according to the following formula: Among them, u, v, and w are the relative displacements of the movable compartment (2) relative to the fixed compartment (3) in the heading direction, horizontal direction, and vertical direction, respectively.

6. A cabin docking method, characterized in that: The following steps are involved: Step 1, constructing the measuring device as described in step S1 of claim 1; Step 2, using steps S2 to S5 in claim 1 to calculate the yaw angle α and pitch angle β of the movable cabin (2) relative to the fixed cabin (3); Step 3, adjusting the attitude adjustment mechanism (1) according to the yaw angle α and the pitch angle β obtained in step 2; Step 4, using steps S2 to S5 in claim 1 to calculate the roll angle γ of the movable cabin (2) relative to the fixed cabin (3); Step 5, adjusting the attitude adjustment mechanism (1) according to the roll angle γ obtained in step 4; Step 6, using steps S2 to S5 in claim 1 to calculate the relative displacement of the movable compartment (2) relative to the fixed compartment (3); Step 7: According to the relative displacement obtained in step 4, the attitude adjustment mechanism (1) is adjusted to complete the docking of the movable cabin (2) and the fixed cabin (3).

7. The cabin docking method according to claim 6, characterized in that: Step 3 is as follows: Determine whether the yaw angle α and the pitch angle β in step 2 are within the set first convergence threshold range. If not, adjust the attitude adjustment mechanism (1) according to the yaw angle α and the pitch angle β in step 2, and then return to step 2. If they are within the range, proceed to step 4.

8. The compartment docking method according to claim 7, characterized in that: Step 5 is as follows: Determine whether the roll angle γ in step 4 is within the set second convergence threshold range. If not, adjust the attitude adjustment mechanism (1) according to the roll angle γ in step 4, and then return to step 4. If within the range, proceed to step 6.

9. The cabin docking method according to any one of claims 6 to 8, characterized in that: Step 7 is as follows: 5.

1. Determine whether the relative displacement in step 6 is within the set second convergence threshold range. If not, adjust the posture adjustment mechanism (1) according to the relative displacement in step 6, and then return to step 6. If within the range, complete the adjustment. 5.

2. Remove the target tooling, control the attitude adjustment mechanism (1) to move along the heading direction, and achieve docking of the movable cabin (2) with the fixed cabin (3).

Images