A method and device for solving the pose of a space vehicle

Abstract

The application discloses a kind of solving method and device of spatial vehicle pose, and the parameter required for three-dimensional reconstruction is obtained by cooperative robot through vision sensor and calibration plate after the vehicle to be detected reaches specified parking area;Image of the region where the feature points on the surface of the vehicle body are located is collected, and the image is transmitted to host computer for processing;The two-dimensional pixel coordinates of the feature points on the surface of the vehicle body in the plan image are extracted by image processing algorithm;The three-dimensional coordinate information of the feature points on the surface of the vehicle body under the camera coordinate system is restored by three-dimensional reconstruction of feature points;A three-dimensional coordinate transformation model between the camera coordinate system and the vehicle coordinate system is established according to the three-dimensional model of the automobile;The point cloud data of the feature points on the surface of the vehicle body in the camera coordinate system and the vehicle coordinate system is brought in by using the coordinate transformation model based on Rodrigues matrix, and the pose of the vehicle in the indoor space is obtained by singular value decomposition method.The calculation between three-dimensional points is more reliable in the vehicle pose calculation result.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of vehicle detection detection, and particularly relates to a space vehicle pose calculation method and device. BACKGROUND

[0002] The installation angle and position of sensors on a vehicle directly determine whether the vehicle can normally run and give the vehicle driver correct and sufficient information, so the detection of functions of various sensors is an important part of the quality detection of a vehicle factory. However, in the existing vehicle sensor calibration equipment, most of them adopt a method that a vehicle to be detected is driven into a vehicle centering platform, the vehicle is pushed into a specified coordinate system and aligned through mechanical contact of a centering push block on the centering platform, so as to determine the position of the vehicle and the sensor, and then a specified target is moved to a corresponding position through a rigid target mounting platform (gantry) to calibrate the sensor. The equipment not only has a large floor space, poor scalability, but also has high cost and insufficient flexibility. Since the installation (position and angle) of the vehicle sensor is designed based on the vehicle coordinate system, in order to realize low-cost and high-flexibility automobile auxiliary driving sensor positioning, the original vehicle centering platform must be removed, and a low-cost, high-flexibility and small-space vehicle coordinate system calibration method is designed to obtain the pose of the vehicle.

[0003] With the popularity of intelligent cars, the technology of car sensors is also more mature, and its market size is also expanding. The functional configuration of many high-end luxury vehicles is extended to domestic independent brands of vehicles. Many mature sensor technologies are applied to ordinary household passenger cars, such as cameras, laser radars, millimeter wave radars, ultrasonic radars, and other sensors. Correctly assembling (position and angle) the sensors on the vehicle can ensure the normal operation of the vehicle and give the vehicle driver correct and sufficient information. Therefore, for the sensors installed on the vehicle, according to the assembly technical requirements, correctly checking whether the measured sensor function meets the design requirements is an important link of the vehicle final inspection of the vehicle factory, and the key technology is the solving method of the vehicle space pose. Due to the limitation of the rigid target mounting platform (gantry), the existing equipment platform needs to align the vehicle coordinate system with the work station coordinate system before the sensor calibration. The way taken is to drive the vehicle to be measured into the vehicle centering platform, and through the mechanical contact of the centering push block on the centering table, the vehicle is pushed into the specified coordinate system and aligned, so as to determine the position of the vehicle and the sensor. The equipment not only has large floor area, poor expandability, but also has high cost and insufficient flexibility. In addition, the mechanical contact type centering is difficult to ensure the absolute alignment of the vehicle coordinate system and the work station coordinate system, and the centering effect will be affected by many factors such as tire pressure, suspension, etc. In addition, the current platform also lacks effective verification means for the centering result, and there is random error which is difficult to control. Further, with the increase of the number of front-mounted sensors, the original rigid target mounting platform (gantry) will also lead to too large, more complex structure, and occupy more space, which is difficult to adapt to large-scale sensor calibration.

[0004] The most common vehicle sensor calibration equipment at present needs to align the vehicle coordinate system with the work station coordinate system before the sensor calibration due to the limitation of the rigid target mounting platform (gantry). The way taken is to drive the vehicle to be measured into the vehicle centering platform, and through the mechanical contact of the centering push block on the centering table, the vehicle is pushed into the specified coordinate system and aligned, so as to determine the position of the vehicle and the sensor, and then the sensor target is moved to the specified position through the gantry. Such equipment not only has large floor area, poor expandability, but also has high cost and insufficient flexibility. In addition, the mechanical contact type centering is difficult to ensure the absolute alignment of the vehicle coordinate system and the work station coordinate system, and the centering effect will be affected by many factors such as tire pressure, suspension, etc. In addition, the current platform also lacks direct verification means for the centering result, and only indirectly verifies the centering situation through the contact sensor, which has random error and is difficult to control. Further, with the increase of the number of front-mounted sensors, the original rigid target mounting platform (gantry) is difficult to adapt to large-scale sensor calibration due to the large occupied space and the complex structure of the equipment. SUMMARY

[0005] The present application aims to provide a precise, simple and low-cost spatial vehicle pose solving method and device for vehicle multi-sensor calibration task in space, so as to replace the original vehicle positioning centering platform and further determine the pose of various sensors on the vehicle, which can cooperate with the robot to calibrate the vehicle sensor in the future.

[0006] The technical scheme adopted by the present application is:

[0007] A spatial vehicle pose solving method, the specific steps are as follows:

[0008] Step 1, after the vehicle to be detected reaches the designated parking area, the collaborative robot leads the vision sensor and the calibration board to move, starts the vision sensor and executes the calibration program to obtain the parameters required for three-dimensional reconstruction.

[0009] Step 2, collect the image of the area where the feature points on the vehicle body are located, collect the image of the vehicle body area around the fuel cap, and transmit the image to the host computer for processing.

[0010] Step 3, vehicle body image feature point extraction: extract the two-dimensional pixel coordinates of the vehicle body surface feature points in the plane image through image processing algorithm.

[0011] Step 4, restore the three-dimensional coordinate information (X C , Y C , Z C ) of the vehicle body surface feature points in the camera coordinate system through three-dimensional reconstruction of the feature points.

[0012] Step 5, according to the three-dimensional model of the automobile, establish a three-dimensional coordinate transformation model between the camera coordinate system (X C , Y C , Z C ) and the vehicle coordinate system (X V , Y V , Z V ).

[0013] Step 6, use the coordinate transformation model based on the Rodrigues matrix to establish the relationship between the camera coordinate system (X C , Y C , Z C ) and the vehicle coordinate system (X V , Y V , Z V ), and input the point cloud data of the vehicle body feature points in the camera coordinate system and the vehicle coordinate system, and solve the scale factor λ, the rotation matrix R and the translation vector T by the singular value decomposition method, that is, the pose of the vehicle in the indoor space is solved.

[0014] Further, in step 2, other feature point information rich areas are collected according to different vehicle models.

[0015] Further, the camera coordinate system (X C , Y C , Z C ) in step 5, i.e. the vision sensor coordinate system, takes the vision sensor focus center as the coordinate origin, the Z C axis extends in the positive direction of the direction of the object, the X C axis is the row direction and points to the right of the origin, and the Y C axis is the column direction and points to the lower side of the origin.

[0016] Further, the vehicle coordinate system (X V , Y V , Z V ) in step 5 takes the vehicle center of mass as the coordinate origin, the X V axis is parallel to the ground and points to the forward direction of the vehicle, the Y V axis is parallel to the ground and points to the left side of the vehicle, and the Z V axis is perpendicular to the plane and points to the upper side of the vehicle.

[0017] Further, the three-dimensional coordinate transformation model between the camera coordinate system (X C , Y C , Z C ) and the vehicle coordinate system (X V , Y V , Z V ) in step 5 is shown in formula (1):

[0018]

[0019] In the formula, λ is a scale factor, R is a rotation matrix in coordinate transformation, and T is a translation parameter.

[0020] Further, the rotation matrix R in step 6 is regarded as a Rodrigues matrix composed of an anti-symmetric matrix, as shown in formula (2) and formula (3):

[0021]

[0022] R = (I + S)(I - S) -1 (3)

[0023] In the formula, I is a three-order unit matrix, S represents an anti-symmetric matrix, and the three parameters a, b and c in S are independent of each other.

[0024] The application adopts the above technical scheme, through the fixed visual sensor and the fixed parking space, the vehicle body area can be in the detection range of the visual sensor after the vehicle reaches the specified parking space and stops, the three-dimensional coordinates of the feature points are calculated, the three-dimensional model of the vehicle is combined, the three-dimensional relationship between the points is established, and then the relative pose of the vehicle and the camera is solved, so that the parking pose of the vehicle and the sensor pose are solved. Through the calculation between the three-dimensional points, the vehicle pose calculation result is more reliable. BRIEF DESCRIPTION OF DRAWINGS

[0025] The application will be further described in detail below in combination with the drawings and specific embodiments.

[0026] Fig. 1 It is a structural schematic diagram of a space vehicle pose acquisition device of the application;

[0027] Fig. 2 It is a camera coordinate system schematic diagram of the application;

[0028] Fig. 3 It is a vehicle coordinate system schematic diagram of the application. EMBODIMENT

[0029] To make the purpose, technical scheme and advantages of the embodiments of the application more clear, the technical scheme in the embodiments of the application will be clearly and completely described below in combination with the drawings in the embodiments of the application.

[0030] In vehicle multi-sensor calibration, the most important work is to solve the vehicle parking pose. In the current vehicle pose solving scheme, some are positioned by collecting the multi-view images of the calibration board and the vehicle body, which not only needs to add a calibration board in space, but also needs a device to drive the camera to collect multi-view images, which is contrary to the defect of simplifying the large occupation of traditional equipment. The application solves the problem by the fixed visual sensor and the fixed parking space, so that the vehicle body area can be in the detection range of the visual sensor after the vehicle reaches the specified parking space and stops, the three-dimensional coordinates of the feature points are calculated, the three-dimensional model of the vehicle is combined, the three-dimensional relationship between the points is established, and then the relative pose of the vehicle and the camera is solved, so that the parking pose of the vehicle and the sensor pose are solved. The scheme of the application solves the problem by the calculation between the three-dimensional points, and the vehicle pose calculation result is more reliable.

[0031] As shown in one of the drawings, Figs. 1-3 The application discloses a space vehicle pose solving method, and the specific steps are as follows:

[0032] Step 1, after the vehicle to be detected reaches the specified parking area, the collaborative robot leads the visual sensor and the calibration board to move, starts the visual sensor, executes the calibration program, and obtains the parameters required for three-dimensional reconstruction.

[0033] Step 2, collect the image of the area where the feature points of the vehicle body surface are located, the present application mainly collects the image of the vehicle body area near the fuel cap, of course, other areas with rich feature point information can also be collected according to different vehicle models, and the image is transmitted to the host computer for processing.

[0034] Step 3, vehicle body image feature point extraction, mainly through image processing algorithm to extract the two-dimensional pixel coordinates of the vehicle body surface feature points in the plane image.

[0035] Step 4, through the three-dimensional reconstruction of the feature points, the three-dimensional coordinate information (X C , Y C , Z C ) of the vehicle body surface feature points in the camera coordinate system is restored.

[0036] Step 5, according to the three-dimensional model of the vehicle, a three-dimensional coordinate transformation model between the camera coordinate system (X C , Y C , Z C ) and the vehicle coordinate system (X V , Y V , Z V ) is established.

[0037] Step 6, the relationship between the camera coordinate system (X C , Y C , Z C ) and the vehicle coordinate system (X V , Y V , Z V ) is established by using the coordinate transformation model based on the Lodrigues matrix, the point cloud data of the vehicle body feature points in the camera coordinate system and the vehicle coordinate system are brought in, and the scale factor λ, the rotation matrix R and the translation vector T are solved by the singular value decomposition method, that is, the pose of the vehicle in the indoor space is solved.

[0038] Further, the schematic diagrams of the camera coordinate system (X C , Y C , Z C ) and the vehicle coordinate system (X V , Y V , Z V ) in step 5 are shown in Fig. 2 , 3 , the camera coordinate system (i.e. the vision sensor coordinate system) takes the focus center of the vision sensor as the origin, the Z C axis extends in the positive direction of the shooting direction, the X C axis is the row direction and points to the right of the origin, and the X C axis is the column direction and points to the lower side of the origin; the vehicle coordinate system takes the vehicle mass center as the coordinate origin, the X V axis is parallel to the ground and points to the vehicle forward direction, the YV The axis is parallel to the ground and points to the left side of the vehicle, Z V The axis is perpendicular to the ground and points upwards.

[0039] Further, in step 5, the three-dimensional coordinate transformation model between the camera coordinate system (X C , Y C , Z C ) and the vehicle coordinate system (X V , Y V , Z V ) is shown as formula (1):

[0040]

[0041] In the formula, λ is the scale factor, R is the rotation matrix in the coordinate transformation, and T is the translation parameter.

[0042] Further, in step 6, the rotation matrix R is regarded as a Rodrigues matrix composed of skew-symmetric matrices, as shown in formula (2) and formula (3):

[0043]

[0044] R = (I + S)(I - S) -1 (3)

[0045] In the formula, I is a three-order unit matrix, and the three parameters a, b, and c in S are independent of each other.

[0046] A spatial vehicle pose acquisition device includes a fixed support 1 and a collaborative robot 2. The fixed support 1 is arranged above a specified parking area 6 and has a crossbeam higher than the vehicle body of a vehicle 5. The collaborative robot 2 is slidingly arranged on the bottom surface of the crossbeam and has a workspace that satisfies a test function. One end of one mechanical arm is provided with a visual sensor 3, and the other mechanical arm is provided with a calibration plate 4. The visual sensor 3 acquires images of the feature area of the vehicle body. The calibration plate 4 is arranged within the acquisition range of the visual sensor 3 to facilitate parameter calibration of the visual sensor.

[0047] Further, the specified parking area 6 is planned by using a marking line.

[0048] Further, the fixed support 1 is fixedly installed above the specified parking area 6.

[0049] Further, the fixed support 1 is movably arranged above the specified parking area 6. The bottom of the fixed support 1 is provided with a moving device, and the moving device is provided with a locking and limiting structure.

[0050] Further, the moving device is an electric sliding rail.

[0051] Furthermore, the collaborative robot 2 is configured to be equipped with a calibration program.

[0052] Furthermore, it also includes a host computer, which connects to the collaborative robot 2 and controls the actions of the collaborative robot 2.

[0053] Furthermore, the host computer is equipped with an image processing program, which processes the images collected by the vision sensor 3 to obtain the vehicle's pose in the indoor space.

[0054] During the deployment of the calibration environment, a predetermined number of vision sensors 3 can be installed on the side areas of the calibration environment, such as the left and right sides, using devices such as fixed brackets 1. This allows for convenient installation of the vision sensors 3 even in open environments. Furthermore, the specific position and shooting angle of the vision sensors 3 in the side areas can be flexibly changed by the collaborative robot 2. The images captured by the vision sensors 3 are used to calculate the pose relationship between the vehicle body and the calibration environment (i.e., the pose relationship of the vehicle body in the world coordinate system). A calibration plate 4 is installed on another robotic arm of the collaborative robot 2 to ensure that the vision sensors 3 can capture enough usable images of the calibration plate 4 regardless of whether the vehicle is parked in a designated parking environment, thus facilitating the parameter calibration of the vision sensors.

[0055] Compared with existing technologies, this invention has the following technical features: 1. Considering the limitations and high cost of traditional vehicle sensor calibration equipment, this invention abandons the complex and inefficient traditional equipment calibration methods and cleverly applies machine vision to vehicle positioning in vehicle sensor calibration tasks. This method is simple in structure, highly flexible, highly scalable, and has low equipment cost. 2. This invention estimates the pose of the vehicle to be detected by the sensor using a machine vision-based method, replacing the traditional vehicle centering platform. This not only simplifies the equipment structure and allows for adaptation to more types of vehicle models, but also enables better guidance of collaborative robots to move sensor targets in the future, replacing the original rigid target mounting platform, thereby achieving highly flexible sensor detection. Furthermore, this method is non-contact. 3. This invention is applicable to the spatial pose estimation of vehicles off the production line in OEMs, significantly reducing the cost of sensor calibration for off-line vehicles in OEMs. 4. The vehicle pose estimation scheme adopted in this invention is simpler and lower in cost, and its application scenarios are not limited to OEMs. In the future, it can be extended to auto repair shops and 4S stores for sensor calibration after vehicle inspection and maintenance. Since auto repair shops and 4S stores do not have enough space and funds, it is more appropriate to use this type of low-cost, space-saving equipment.

[0056] It is apparent that the described embodiments are only some — but not all — of the embodiments of the present application. The embodiments described in this application and features in the embodiments can be combined with each other in cases without conflict. The components of the embodiments of the present application, which are generally described and shown in the accompanying drawings, can be arranged and designed in various different configurations. Therefore, the detailed description of the embodiments of the present application is not intended to limit the scope of the claimed application, but merely represents selected embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative work, shall fall within the scope of protection of the present application.

Claims

1. A method for solving the pose of a space vehicle, characterized in that: It comprises the following steps: Step 1, after the vehicle to be detected arrives at the designated parking area, the collaborative robot leads the vision sensor and the calibration board to move, starts the vision sensor and performs the calibration procedure to obtain the parameters required for three-dimensional reconstruction; Step 2, collect the image of the area where the feature points on the vehicle body are located, collect the image of the vehicle fuel cap and the vehicle body area near the vehicle fuel cap, and transmit the image to the host computer for processing; Step 3, vehicle body image feature point extraction: extract the two-dimensional pixel coordinates of the vehicle body surface feature points in the planar image through image processing algorithm; Step 4, restore the three-dimensional coordinate information (X C ,Y C ,Z C ) of the surface feature points of the vehicle body in the camera coordinate system through three-dimensional reconstruction of the feature points; Step 5, according to the three-dimensional model of the car, establish the three-dimensional coordinate transformation model between the camera coordinate system (X C , Y C , Z C ) and the vehicle coordinate system (X V , Y V , Z V ); Step 6, the relationship between the camera coordinate system (X C ,Y C ,Z C ) and the vehicle coordinate system (X V ,Y V ,Z V ) is established by using a coordinate transformation model based on the Lodrigues matrix, the point cloud data of the body feature points in the camera coordinate system and the vehicle coordinate system are brought in, and the scale factor λ, the rotation matrix R and the translation vector T are solved by the singular value decomposition method, that is, the pose of the vehicle in the indoor space is solved.

2. The method of claim 1, wherein: In step 2, collect feature point information in the area rich in different vehicle models.

3. The method of claim 1, wherein: Camera coordinate system (X C ,Y C ,Z C ) in Step 5 has the focus center of the vision sensor as the origin, the Z C axis extends in the positive direction of the direction in which the object is photographed, the X C axis is the row direction and points to the right of the origin, and the Y C axis is the column direction and points to the lower side of the origin.

4. The method of claim 1, wherein: The vehicle coordinate system (X V ,Y V ,Z V ) in step 5 has the vehicle center of mass as the coordinate origin, the X V axis is parallel to the ground and points in the direction of vehicle travel, the Y V axis is parallel to the ground and points to the left of the vehicle, and the Z V axis is perpendicular to the plane and points upward in the vehicle.

5. The method of claim 1, wherein: In step 5, the three-dimensional coordinate transformation model between the camera coordinate system (X C ,Y C ,Z C ) and the vehicle coordinate system (X V ,Y V ,Z V ) is shown as formula (1): In the formula, λ is the scale factor, R is the rotation matrix in the coordinate transformation, and T is the translation parameter.

6. The method of claim 5, wherein: In step 6, the rotation matrix R is regarded as a Rodrigues matrix composed of skew-symmetric matrices, as shown in formula (2) and formula (3): R = (I + S)(I - S) -1 (3) In the formula, I is a three-order unit matrix, S represents a skew-symmetric matrix, and the three parameters a, b and c in S are independent of each other.

7. A space vehicle pose acquisition device, employing the space vehicle pose solving method according to any one of claims 1 to 6, characterized in that: The device comprises a fixed support, a collaborative robot, the fixed support is arranged above the designated parking area and has a crossbeam higher than the vehicle body; the collaborative robot is slidingly arranged at the bottom surface of the crossbeam, the collaborative robot has a working space meeting the test function, the collaborative robot has at least two mechanical arms, one end of one of the mechanical arms is provided with a vision sensor, and the other mechanical arm is provided with a calibration board; the vision sensor collects images of the feature area of the vehicle body; the calibration board is used within the collection range of the vision sensor to facilitate parameter calibration of the vision sensor.

8. The spatial vehicle pose acquisition apparatus according to claim 7, wherein: The designated parking area uses a marking line to plan the parking area, the fixed support is movably arranged above the designated parking area, the bottom of the fixed support is provided with a moving device, the moving device is provided with a locking and limiting structure, and the moving device is an electric sliding rail.

9. The spatial vehicle pose acquisition apparatus according to claim 7, wherein: The collaborative robot is configured to carry a calibration program matched with the calibration board to provide calibration dimensions.

10. The spatial vehicle pose acquisition apparatus according to claim 7, wherein: It also comprises a host computer, the collaborative robot is connected to the host computer, the host computer controls the action of the collaborative robot; the host computer is provided with an image processing program, and the image collected by the vision sensor is processed by the image processing program to obtain the pose of the vehicle in the indoor space.

Images