Position and attitude acquisition device

Abstract

To make it possible to accurately acquire the position and attitude of a charging inlet regardless of the influence of ambient light while suppressing an increase in cost.SOLUTION: A position and attitude acquisition device of the present disclosure acquires the position and attitude of a charging inlet of an electric vehicle, and includes an imaging device that irradiates the charging inlet with infrared rays to acquire an infrared image of the charging inlet, a pre-processing unit that expands the dynamic range of the infrared image acquired by the imaging device, an edge detection unit that detects an opening-side edge of a recess included in the charging inlet that is close to the imaging device and a bottom-side edge of the recess that is away from the imaging device, from the image processed by the pre-processing unit, and a position and attitude estimation unit that estimates the position and attitude of the charging inlet relative to the imaging device based on the opening-side edge and bottom-side edge detected by the edge detection unit.SELECTED DRAWING: Figure 4

Description

[Technical Field]

[0001] The present disclosure relates to a position and attitude acquisition device for acquiring the position and attitude of a charging inlet of an electric vehicle. [Background technology]

[0002] Conventionally, object recognition devices installed on production lines equipped with picking systems are known (see, for example, Patent Document 1). This object recognition device recognizes the position and orientation of a workpiece on a tray through template matching using 2D images and 3D data captured by a 3D sensor camera. Other known methods include a posture estimation device that uses a neural network to detect the type of charging inlet and key points (position and orientation) within the charging inlet area from an RGBD image (see, for example, Patent Document 2), and a method that uses two types of convolutional neural networks (CNNs) for the charging inlet discovery process and posture detection process to estimate the position of an object in an image relative to the camera's position and orientation (see, for example, Patent Document 3). Furthermore, conventionally, an automated system that identifies and positions charging inlets for electric vehicles based on 2D images is known (see, for example, Non-Patent Document 1). This automated system uses image contour information obtained by fusing a 2D original image captured with automatic exposure with a gradient image to acquire the 3D orientation of the charging port using a PnP algorithm. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Patent No. 6936974 [Patent Document 2] US Patent Application Publication No. 2023 / 102948 [Patent Document 3] US Patent Application Publication No. 2022 / 383543 [Non-Patent Document 1] Research on Fast Identification and Location of Contour Features of Electric Vehicle Charging Port in Complex Scenes, June 2021 IEEE Access PP(99):1-1 Summary of the Invention [Problem to be solved by the invention]

[0004] However, 3D sensors (3D cameras) for obtaining 3D data are expensive, and their use is limited to indoor environments where they are less susceptible to light. Furthermore, using a neural network to obtain the position and orientation of a charging inlet requires a large amount of training data and computational time, making it difficult to reduce the cost of building the neural network. Furthermore, the technology described in Non-Patent Document 1 makes it difficult to obtain image contour information from 2D images captured with automatic exposure, which have large differences in shading. The use of ellipses can lead to discrepancies in feature positions when obtaining orientation using the PnP algorithm.

[0005] Therefore, a main object of the present disclosure is to make it possible to accurately acquire the position and orientation of a charging inlet regardless of the influence of ambient light while suppressing an increase in costs. [Means for solving the problem]

[0006] The position and orientation acquisition device disclosed herein is a position and orientation acquisition device that acquires the position and orientation of a charging inlet of an electric vehicle, and includes an imaging device that irradiates the charging inlet with infrared rays to acquire an infrared image of the charging inlet, a pre-processing unit that expands the dynamic range of the infrared image acquired by the imaging device, an edge detection unit that detects an opening edge of a recess included in the charging inlet that is close to the imaging device and a bottom edge of the recess that is away from the imaging device from the image processed by the pre-processing unit, and a position and orientation estimation unit that estimates the position and orientation of the charging inlet relative to the imaging device based on the opening edge and the bottom edge detected by the edge detection unit.

[0007] The position and orientation acquisition device disclosed herein includes an imaging device, a preprocessing unit, an edge detection unit, and a position and orientation estimation unit. The imaging device irradiates a charging inlet with infrared light to acquire an infrared image of the charging inlet. This allows for stable acquisition of an infrared image with reduced influence from ambient light. The preprocessing unit also widens the dynamic range of the infrared image acquired by the imaging device. This allows for acquisition of an image without overexposure or underexposure at the edges (contours) of the charging inlet. Furthermore, the edge detection unit detects, from the image processed by the preprocessing unit, an opening-side edge of a recess included in the charging inlet that is close to the imaging device and a bottom-side edge of the recess that is distant from the imaging device. The position and orientation estimation unit then estimates the position and orientation of the charging inlet relative to the imaging device based on the opening-side edge and bottom-side edge detected by the edge detection unit. This allows the opening edge and the bottom edge to be detected with high accuracy without using an expensive 3D camera, ToF camera, etc., and the position and orientation of the charging inlet relative to the imaging device to be estimated with high accuracy from the size and degree of positional misalignment of the opening edge and the bottom edge using the previously known specifications (dimensions) of the charging inlet. As a result, it becomes possible to accurately acquire the position and orientation of the charging inlet regardless of the influence of ambient light, while suppressing an increase in costs.

[0008] A position and orientation acquisition method according to the present disclosure acquires the position and orientation of a charging inlet of an electric vehicle by irradiating the charging inlet with infrared light to acquire an infrared image of the charging inlet, expanding the dynamic range of the acquired infrared image, detecting an opening edge of a recess included in the charging inlet that is close to the imaging device and a bottom edge of the recess that is remote from the imaging device from the image with the expanded dynamic range, and estimating the position and orientation of the charging inlet relative to the imaging device based on the detected opening edge and bottom edge. This method makes it possible to accurately acquire the position and orientation of the charging inlet regardless of the influence of ambient light while suppressing increases in cost. [Brief explanation of the drawings]

[0009] [Figure 1] 1 is a schematic configuration diagram showing a parking facility to which a charging connector inserting / removing device including a position and attitude acquisition device according to the present disclosure is applied. [Figure 2] FIG. 2 is a plan view showing a charging inlet of the electric vehicle. [Figure 3] 1 is a schematic diagram illustrating a charging connector inserting / removing device including a position and orientation acquisition device according to an embodiment of the present disclosure. [Figure 4] FIG. 2 is a control block diagram illustrating the position and orientation acquisition device of the present disclosure. [Figure 5] 10 is a flowchart illustrating a routine executed by an image acquisition unit and a preprocessing unit of the position and orientation acquisition apparatus of the present disclosure. [Figure 6] 10 is a flowchart illustrating a routine executed by an object detection unit of the position and orientation acquisition device of the present disclosure. [Figure 7] 10 is a flowchart illustrating a routine executed by an edge search area setting unit of the position and orientation acquisition apparatus of the present disclosure. [Figure 8] 10 is a flowchart illustrating a routine executed by an edge detection unit of the position and orientation acquisition apparatus of the present disclosure. [Figure 9] 10 is a flowchart illustrating a routine executed by a position and orientation estimation unit of the position and orientation acquisition device of the present disclosure. [Figure 10] 10 is an explanatory diagram illustrating a procedure for estimating the position and orientation of a charging inlet by a position and orientation estimation unit of a position and orientation acquisition device according to the present disclosure. FIG. [Figure 11] 10 is an explanatory diagram illustrating a procedure for estimating the position and orientation of a charging inlet by a position and orientation estimation unit of a position and orientation acquisition device according to the present disclosure. FIG. [Figure 12] 10 is a diagram illustrating a procedure for estimating the position and orientation of a charging inlet by a position and orientation estimation unit of a position and orientation acquisition device according to the present disclosure. DETAILED DESCRIPTION OF THE INVENTION

[0010] Next, embodiments of the present disclosure will be described with reference to the drawings.

[0011] FIG. 1 is a schematic diagram showing a parking facility 100 to which a charging connector insertion / removal device 1 including a position and attitude acquisition device 10 of the present disclosure is applied. The parking facility 100 shown in the figure is an automated valet parking system that automatically drives a vehicle V between a vehicle occupant boarding and disembarking location (not shown) and multiple parking spaces to achieve automatic parking entry and exit. In this parking facility 100, the charging connector insertion / removal device 1 is used to insert and remove a charging connector 200 into a charging inlet (charging port) CI of a target vehicle (electric vehicle) V in the narrow space between adjacent vehicles V in order to automatically charge the battery (not shown) of an electric vehicle (battery electric vehicle or plug-in hybrid vehicle) included in the multiple vehicles V parked in the parking facility 100. The charging connector 200 is connected via a cable to a charging device 300 that outputs AC power or converts AC power from an AC power source such as a commercial power source into DC power and outputs the DC power.

[0012] As shown in FIG. 2, the charging inlet CI of the vehicle (electric vehicle) V includes a recess (receptacle) R, a positive power supply terminal Tp, a negative power supply terminal Tn, and a plurality of communication terminals Tc disposed within the recess R, and a lid L. The recess R is a bottomed circular hole having a circular cross-sectional shape, into which a fitting portion (housing) 201 (see FIG. 3) of the charging connector 200 is fitted. Note that the recess R may be a bottomed tapered hole and is not limited to a circular hole. The positive power supply terminal Tp and the negative power supply terminal Tn are formed in a bottomed tubular shape (cylindrical shape) and protrude from a bottom surface Sb of the recess R toward an opening O of the recess R in parallel with an axis Ar of the recess R. The positive power supply terminal Tp and the negative power supply terminal Tn face each other across the axis Ar on a first diameter of the bottom surface Sb. Furthermore, depending on the application, each communication terminal Tc is disposed in one of two cylinders that face each other across an axis Ar on a second diameter that is perpendicular to the first diameter of the bottom surface Sb. The lid L closes the opening O of the recess R and also opens the opening O to allow the fitting of the charging connector 200.

[0013] As shown in FIG. 1 , the charging connector inserting / extracting device 1 includes a transport vehicle (mobile body) 2 equipped with a position and attitude acquisition device 10, a robot arm 3 serving as a connector moving device (insertion / extraction mechanism) that holds (supports) a charging connector 200 and is supported by the transport vehicle 2, and a robot control device 30 that controls the robot arm 3. The transport vehicle 2 is a so-called automated guided vehicle (AGV) or autonomous mobile transport robot (AMR) that is capable of self-propelled within the parking facility 100. In this embodiment, the robot arm 3 is a six-axis robot that includes multiple links and multiple actuators (electric actuators). Note that the charging connector inserting / extracting device 1 may include a mobile body, instead of the transport vehicle 2, that supports the robot arm 3 and travels along rails laid within the parking facility 100.

[0014] As shown in FIG. 3 , the robot arm 3 supports a connector holding unit 5 via a compliance unit 4. The compliance unit 4 absorbs positional and angular misalignment of the charging connector 200 with respect to the charging inlet CI so that the charging connector 200, which is brought close to the charging inlet CI by the robot arm 3, follows the charging inlet CI. The connector holding unit 5 can detachably hold the charging connector 200. However, the connector holding unit 5 may also be a fixed portion to which the charging connector 200 is fixed. The robot control device 30 includes a microcomputer or the like having a CPU, ROM, RAM, input / output devices, etc. (not shown), and controls the robot arm 3 according to command signals from the position and attitude acquisition device 10, a management device of the parking facility 100 (not shown), etc.

[0015] When charging the battery of a vehicle V in a parking facility 100, the transport vehicle 2 of the charging connector inserting / removing device 1 is caused to travel to the vicinity of the target vehicle (electric vehicle) V, and after the transport vehicle 2 stops, the robot control device 30 causes the robot arm 3 to enter the narrow space between adjacent vehicles V so that the charging connector 200 approaches the charging inlet CI of the target vehicle V. In addition, the position and attitude acquisition device 10 acquires the position and attitude of the charging inlet CI of the target vehicle V near the target vehicle V. Then, the robot control device 30 controls the robot arm 3 to insert the charging connector 200 held by the connector holding unit 5 into the charging inlet CI based on the position and attitude of the charging inlet CI acquired by the position and attitude acquisition device 10. In addition, after charging of the battery is completed, the robot control device 30 controls the robot arm 3 to remove the charging connector 200 held by the connector holding unit 5 from the charging inlet CI.

[0016] 4 is a control block diagram showing the position and orientation acquisition device 10 of the present disclosure. As shown in the figure, the position and orientation acquisition device 10 includes an imaging device 11 supported by a robot arm 3 together with a charging connector 200, and a computer 12. As shown in FIG. 3, the imaging device 11 includes a 2D camera 110, an illumination unit 111, and a band-pass filter 115.

[0017] The 2D camera 110 includes an imaging element that has sensitivity up to the infrared light range and converts the intensity of received light into an electrical signal for output. The illumination unit 111 is annular and disposed in front of the 2D camera 110, coaxial with the optical axis of the 2D camera 110. In this embodiment, the illumination unit 111 includes a plurality of infrared light-emitting diodes 112 arranged at intervals in the circumferential and radial directions around the optical axis, and can illuminate the charging inlet CI as the target with infrared light (e.g., infrared light with a peak wavelength of 940 nm). The bandpass filter 115 is disposed in a hollow portion of the illumination unit 111 and allows infrared light (e.g., infrared light with a dominant wavelength of 900-990 nm) reflected by the charging inlet CI as the target to enter the 2D camera 110. This enables the 2D camera 110 to capture an infrared image of the charging inlet CI. The imaging device 111 may be fixed to the connector holder 5 as shown in FIG. 3 or attached to the charging connector 200.

[0018] The computer 12 includes a CPU, ROM, RAM, a communication module, etc. (not shown), and exchanges information with the robot control device 30 via wired or wireless communication. The computer 12 may be installed near the charging device 300. As shown in Fig. 4, the computer 12 includes an image acquisition unit 13, a preprocessing unit 14, an object detection unit 15, an edge search range setting unit 16, an edge detection unit 17, and a position and orientation estimation unit 18 configured as functional blocks through cooperation between hardware such as a CPU and a plurality of programs installed in the ROM.

[0019] The image acquisition unit 13 controls the imaging device 11 and captures infrared images acquired by the imaging device 11. The preprocessing unit 14 expands the dynamic range of the infrared images acquired by the imaging device 11. The object detection unit 15 detects the charging inlet CI in the image processed by the preprocessing unit 14 using YOLO as an object detection algorithm. The edge search range setting unit 16 sets a search range for the opening edge Eo and the bottom edge Eb of the charging inlet CI in the image processed by the preprocessing unit 14 and the like. As shown in FIG. 2 , the opening edge Eo is the inner peripheral edge of the opening O of the recess R of the charging inlet CI that is close to the imaging device 11, and the bottom edge Eb is the inner peripheral edge of the recess R on the bottom surface Sb side that is away from the imaging device 11, i.e., the outer peripheral edge of the bottom surface Sb. The edge detection unit 17 detects the opening edge Eo and the bottom edge Eb of the charging inlet CI within the search range set by the edge search range setting unit 16. The position and orientation estimation unit 18 estimates the position and orientation of the charging inlet CI relative to the imaging device 11 based on the opening-side edge Eo and the bottom-side edge Eb detected by the edge detection unit 17.

[0020] Next, the procedure for acquiring the position and attitude of the charging inlet CI by the position and attitude acquisition device 10 will be described in detail with reference to FIGS. 5 to 12 and the like.

[0021] 5 is a flowchart showing a routine executed by the image acquisition unit 13 and the preprocessing unit 14 of the position and orientation acquisition device 10 when the transport vehicle 2 of the charging connector insertion / removal device 1 stops near a target vehicle V and the robot arm 3 brings the charging connector 200 close to the charging inlet CI of the target vehicle V. When the robot arm 3 brings the charging connector 200 close to the charging inlet CI of the target vehicle V, the irradiation unit 111 of the imaging device 11 irradiates the charging inlet CI with infrared light. The image acquisition unit 13 also transmits a command signal to the imaging device 11 to capture multiple infrared images (at least three, for example, four in this embodiment) of the charging inlet CI with different exposure times, and acquires the multiple infrared images captured with different exposure times from the imaging device 11 (step S100). The timing of irradiating the infrared light may be constant or may be only for the exposure time.

[0022] The preprocessing unit 14 acquires a plurality of infrared images from the image acquisition unit 13 and synthesizes the acquired infrared images to generate an HDR image with an expanded dynamic range (step S110). Furthermore, the preprocessing unit 14 performs tone mapping processing on the generated HDR image (step S120). Upon completion of the tone mapping processing, the preprocessing unit 14 instructs the object detection unit 15 to detect the charging inlet CI in the HDR image (step S130) and ends the routine of FIG. 5.

[0023] 6 is a flowchart showing a routine executed by the object detection unit 15 after the routine of FIG. 5 ends. In response to an instruction from the preprocessing unit 14 to detect a charging inlet CI, the object detection unit 15 acquires an HDR image that has been tone-mapped by the preprocessing unit 14 (step S200). Furthermore, the preprocessing unit 14 detects the charging inlet CI in the HDR image from the preprocessing unit 14 using YOLO, which has been trained on the charging inlet CI (step S210). In step S210, the object detection unit 15 divides the HDR image into n×n squares (grid cells), sets a predetermined number of bounding boxes (2DBBoxes, squares in this embodiment) for the n×n squares, and determines whether the charging inlet CI is included in the HDR image from the preprocessing unit 14 based on the reliability of each bounding box.

[0024] When the object detection unit 15 determines in step S210 that the HDR image from the preprocessing unit 14 includes the charging inlet CI (step S220: YES), the object detection unit 15 determines whether the center of the bounding box that includes the charging inlet CI is included in a predetermined central portion of the HDR image (step S230).When the center of the bounding box that includes the charging inlet CI is included in the central portion of the HDR image (step S230: YES), the object detection unit 15 determines whether the size of the bounding box that includes the charging inlet CI, i.e., whether the length of one side of the bounding box is equal to or greater than a predetermined reference value (step S240).

[0025] If the length of one side of the bounding box that includes the charging inlet CI is equal to or greater than the reference value (step S240: YES), the object detection unit 15 extracts image data within the bounding box that includes the charging inlet CI and removes (smoothes) noise using, for example, a Gaussian filter (step S250). Furthermore, the object detection unit 15 performs a Canny transform on the noise-removed image data to generate an edge image in which edges (contour lines) are emphasized (step S260). After generating the edge image, the object detection unit 15 instructs the edge search range setting unit 16 to set an edge search range (step S270) and ends the routine of FIG. 6.

[0026] On the other hand, if the object detection unit 15 determines through the processing of step S210 that the charging inlet CI is not included in the HDR image from the preprocessing unit 14 (step S220: NO), it sends a robot operation command to the robot control device 30 so that the imaging device 11 is moved by the robot arm 3 serving as a connector moving device, for example, by the angle of view (step S280), and ends the routine of Fig. 6. Also, if the center of the bounding box that contains the charging inlet CI is not included in the center of the HDR image (step S230: NO), the object detection unit 15 sends a robot operation command to the robot control device 30 so that the imaging device 11 is moved by the robot arm 3 by an amount corresponding to the difference between the center of the bounding box and the center of the HDR image (step S280), and ends the routine of Fig. 6.

[0027] Furthermore, if the length of one side of the bounding box that includes the charging inlet CI is less than the reference value (step S240: NO), the object detection unit 15 sends a robot operation command to the robot control device 30 so that the robot arm 3 approaches the charging inlet CI by a predetermined distance while maintaining the posture of the imaging device 11 (and charging connector 200) (step S280), and ends the routine of Fig. 6. When the routine of Fig. 6 ends after the processing of step S260, the operation of the robot arm 3 in accordance with the robot operation command stops, and then the image acquisition unit 13 and the preprocessing unit 14 execute the routine of Fig. 5 again, and the object detection unit 15 executes the routine of Fig. 6 again.

[0028] 7 is a flowchart showing a routine executed by the edge search range setting unit 16 after the processing of step S250 is executed and the routine of FIG. 6 is completed. In response to an instruction to set an edge search range from the object detection unit 15, the edge search range setting unit 16 acquires an edge image generated by the object detection unit 15 (step S300) and sets a search range for the opening-side edge Eo based on a bounding box that defines the range (outer periphery) of the edge image (step S310). The search range for the opening-side edge Eo defines an area of ​​the edge image from the object detection unit 15 that is estimated to include the opening-side edge Eo. In step S310, the edge search range setting unit 16 sets the search range for the opening-side edge Eo to, for example, a ring-shaped area having the inscribed circle of the bounding box as the outer periphery, a circle with a radius that is a predetermined percentage (<1) of the radius of the inscribed circle as the inner periphery, and the center of the bounding box as the center.

[0029] Furthermore, the edge search range setting unit 16 sets a search range for the bottom-side edge Eb based on the search range for the opening-side edge Eo set in step S310 (step S320). The search range for the bottom-side edge Eb defines a region of the edge image from the object detection unit 15 that is estimated to include the bottom-side edge Eb. In step S320, the edge search range setting unit 16 sets the search range for the bottom-side edge Eb as a ring-shaped region whose outer periphery is a circle whose radius is calculated by multiplying the outer radius of the search range for the opening-side edge Eo by a coefficient (<1) determined based on the specifications (dimensions) of the recess R of the charging inlet CI, and whose inner periphery is a circle whose radius is calculated by multiplying the inner radius of the search range for the opening-side edge Eo by the coefficient, with the center being the center of the bounding box. Furthermore, the edge search range setting unit 16 instructs the edge detection unit 17 to detect the opening-side edge Eo and the bottom-side edge Eb (step S330) and ends the routine of FIG. 7.

[0030] 8 is a flowchart showing a routine executed by the edge detection unit 17 after the routine of FIG. 7 ends. In response to an edge detection instruction from the edge search range setting unit 16, the edge detection unit 17 acquires the edge image and the search ranges of the opening-side edge Eo and the bottom-side edge Eb set by the edge search range setting unit 16 (step S400). After the process of step S400, the edge detection unit 17 uses a Hough transform to search for an object having a circular element within the search range of the opening-side edge Eo in the edge image (step S410) and determines whether an object having a circular element has been detected within the search range of the opening-side edge Eo (step S420). If an object having a circular element has been detected within the search range of the opening-side edge Eo (step S420: YES), the edge detection unit 17 extracts the object as the opening-side edge Eo and stores information indicating the opening-side edge Eo (coordinates in the edge image) in RAM or the like (step S430).

[0031] Next, the edge detection unit 17 uses a Hough transform to search for an object having a circular element within the search range of the bottom edge Eb in the edge image (step S440) and determines whether an object having a circular element is detected within the search range of the bottom edge Eb (step S450). If an object having a circular element is detected within the search range of the bottom edge Eb (step S450: YES), the edge detection unit 17 extracts the object as the bottom edge Eb and stores information indicating the bottom edge Eb (coordinates in the edge image) in RAM or the like (step S460). Furthermore, the edge detection unit 17 instructs the position and orientation estimation unit 18 to estimate the position and orientation of the charging inlet CI (step S470) and ends the routine of FIG. 8. Through the processes of FIGS. 5 to 7, the opening edge Eo and the bottom edge Eb obtained in step S430 or S460 of FIG. 8 are both approximately perfect circles.

[0032] 9 is a flowchart showing a routine executed by the position and orientation estimation unit 18 after the routine of FIG. 8 is completed. In response to a position and orientation estimation instruction from the edge detection unit 17, the position and orientation estimation unit 18 acquires information indicating the opening-side edge Eo and information indicating the bottom-side edge Eb (step S500). Next, based on the information acquired in step S500, the position and orientation estimation unit 18 calculates the coordinates of the center Co of the opening-side edge Eo and the coordinates of the center Cb of the bottom-side edge Eb, and calculates the amount of deviation δ (unit: pixel) between the centers Co and Cb (step S510). The center Co of the opening-side edge Eo and the center Cb of the bottom-side edge Eb can be calculated, for example, by averaging the coordinates of multiple points forming the opening-side edge Eo or the bottom-side edge Eb. Furthermore, the position and orientation estimation unit 18 calculates the diameter (pixels) of the bottom edge Eb based on the information indicating the bottom edge Eb, and calculates a conversion coefficient k by dividing the previously known inner diameter (actual size, unit: mm) of the recess R of the charging inlet IC by the diameter of the bottom edge Eb (step S520). The diameter of the bottom edge Eb may be calculated by averaging the distance between two intersections between the line passing through the center Cb and the bottom edge Eb, or may be the maximum or minimum value of the distance between the two intersections.

[0033] Here, if the center of the bounding box that contains the charging inlet CI is located in the center of the HDR image (step S230: YES in FIG. 5 ), the amount of misalignment between the imaging device 11 (charging connector 200) and the charging inlet CI (recess R) in the Y-axis direction in the figure is substantially zero, as shown in FIG. 10 . Furthermore, if the amount of misalignment between the imaging device 11 and the charging inlet CI (recess R) in the Y-axis direction is substantially zero and the charging connector 200 and the charging inlet CI are not directly facing each other, then in the edge image (HDR image), the center Co of the opening-side edge Eo and the center Cb of the bottom-side edge Eb are misaligned in the Z-axis direction in the figure, as shown in FIG. 10 . In this case, the charging inlet CI is tilted around the Y-axis (pitch direction) in the figure with respect to the imaging device 11, as shown in FIG. 11 , and the tilt angle θ of the charging inlet CI with respect to the imaging device 11 is proportional to the amount of misalignment δ·k between the center Co of the opening-side edge Eo and the center Cb of the bottom-side edge Eb, as shown in FIG. 12 . Therefore, the tilt angle θ can be calculated from the previously determined depth D (unit: mm) of the recess R and the amount of deviation δ·k as θ = arcsin(δ·k / D). The tilt angle around the Z axis can also be calculated in the same way.

[0034] Based on these, the position and orientation estimation unit 18 calculates the arcsine value of the product of the displacement amount δ and the conversion coefficient k divided by the depth D of the recess R as the tilt angle θ (unit: rad) of the charging inlet CI with respect to the image capture device 11 (step S530). After the processing of step S530, the position and orientation estimation unit 18 determines whether the absolute value of the tilt angle θ is equal to or less than a predetermined threshold θref (step S540). The threshold θref is the upper limit of the tilt angle θ that can be absorbed by the compliance unit 4, and is set to a value corresponding to an angle of about 1°, for example. If the absolute value of the tilt angle θ is equal to or less than the threshold θref (step S540: YES), the position and orientation estimation unit 18 calculates the distance X between the image capture device 11 and the charging inlet CI based on the size of the opening-side edge Eo (step S550). In step S550, the position and orientation estimation unit 18 calculates the diameter (pixels) of the opening-side edge Eo based on the information indicating the opening-side edge Eo, and calculates the distance X between the imaging device 11 and the charging inlet CI as a value obtained by multiplying the diameter of the opening-side edge Eo by a conversion coefficient that is determined in advance based on the dimensions of the recess R, etc. Note that the diameter of the opening-side edge Eo may be calculated by averaging the distances between two intersections of a line passing through the center Co and the opening-side edge Eo, or may be the maximum or minimum value of the distances between the two intersections.

[0035] Then, position and orientation estimation unit 18 transmits the position and orientation information of charging inlet CI, i.e., the tilt angle θ calculated in step S530 and the distance X calculated in step S550, and a connector insertion command for instructing insertion of charging connector 200 into charging inlet CI to robot control device 30 (step S560), and ends the routine in Fig. 9. In response to receiving the position and orientation information and the connector insertion command, robot control device 30 sets a command value for robot arm 3 based on the position and orientation information, and controls robot arm 3 to insert charging connector 200 into charging inlet CI.

[0036] On the other hand, if the position and orientation estimation unit 18 determines that the absolute value of the tilt angle θ exceeds the threshold value θref (step S540: NO), it sends a robot operation command to the robot control device 30 so that the imaging device 11 is moved by the robot arm 3 serving as a connector moving device in a direction that makes the tilt angle θ zero, for example (step S570), and ends the routine of Fig. 9. When the routine of Fig. 9 ends after the processing of step S570, the operation of the robot arm 3 in accordance with the robot operation command stops, and then the routines of Figs. 5 to 9 are executed again.

[0037] As described above, the position and orientation acquisition device 10 includes the imaging device 11, the preprocessing unit 14, the edge detection unit 17, and the position and orientation estimation unit 18. The imaging device 11 is supported by the robot arm 3, which serves as a charging connector moving device, together with the charging connector 200 that is inserted into and removed from the charging inlet of the vehicle V, and irradiates the charging inlet CI with infrared light to acquire an infrared image of the charging inlet CI. This makes it possible to stably obtain an infrared image in which the influence of sunlight (ambient light) is reduced. Furthermore, the preprocessing unit 14 generates an HDR image based on the infrared image acquired by the imaging device 11, thereby widening the dynamic range of the infrared image. This makes it possible to obtain an image in which the edges (contour lines) of the charging inlet CI are free of overexposure or underexposure.

[0038] Furthermore, the edge detection unit 17 detects an opening-side edge Eo of the recess R included in the charging inlet CI that is close to the imaging device 11 and a bottom-side edge Eb of the recess R that is away from the imaging device 11 from the HDR image generated (processed) by the preprocessing unit 14, more specifically, from an edge image obtained by applying a Gaussian filter or a Canny transform to the HDR image (steps S400-S470 in FIG. 8). Then, the position and orientation estimation unit 18 estimates the position and orientation of the charging inlet CI with respect to the imaging device 11 based on the opening-side edge Eo and bottom-side edge Eb detected by the edge detection unit 17 (steps S500-S570 in FIG. 9).

[0039] This makes it possible to accurately detect the opening edge Eo and the bottom edge Eb without using an expensive 3D camera, ToF camera, or the like, and to accurately estimate the position and orientation of the charging inlet CI relative to the imaging device 11 from the sizes and degree of positional deviation of the opening edge Eo and the bottom edge Eb by using the specifications (dimensions) of the charging inlet CI that are known in advance. As a result, it becomes possible to accurately acquire the position and orientation of the charging inlet CI regardless of the influence of ambient light, while suppressing an increase in costs.

[0040] In addition, the position and orientation estimation unit 18 calculates the tilt angle θ of the charging inlet CI relative to the imaging device 11 from the amount of deviation δ between the center Co as a characteristic point of the opening-side edge Eo and the center Cb as a characteristic point of the bottom-side edge Eb (steps S500-S530 in FIG. 9), and calculates the distance X between the imaging device 11 and the charging inlet CI based on the size (diameter) of the opening-side edge Eo (step S550 in FIG. 9).

[0041] This makes it possible to appropriately move the robot arm 3 serving as a connector moving device based on the tilt angle θ and distance X calculated by the position and orientation estimation unit 18, and to smoothly insert the charging connector 200 into the charging inlet CI. Note that in step S550 of Fig. 9, the distance X between the imaging device 11 and the charging inlet CI may be calculated based on the size (diameter) of the bottom edge Eb. Furthermore, the characteristic points of the opening edge Eo and the bottom edge Eb are not necessarily limited to the centers Co and Cb, and may be any points that enable the amount of deviation δ to be appropriately acquired.

[0042] Furthermore, when the tilt angle θ of the charging inlet CI is equal to or smaller than a threshold value (predetermined angle) θref (step S540; YES), the position and orientation estimation unit 18 calculates the distance X between the image capture device 11 and the charging inlet CI (S550). On the other hand, when the tilt angle θ of the charging inlet CI is greater than the threshold value θref (step S540: NO), the position and orientation estimation unit 18 operates the robot arm 3 so that the image capture device 11 moves in a direction that makes the tilt angle θ zero (step S570 in FIG. 9).

[0043] This prevents the charging connector 200 from being pressed against the periphery of the recess R of the charging inlet CI by the robot arm 3 when the charging connector 200 cannot be smoothly inserted into the charging inlet CI, and also makes it possible to adjust the posture of the charging connector 200 so that it can be smoothly inserted into the charging inlet CI.

[0044] The position and orientation acquisition device 10 also includes an edge search range setting unit 16, which sets a search range for the opening-side edge Eo and a search range for the bottom-side edge Eb in the HDR image generated by the preprocessing unit 14, more specifically, in an edge image obtained by applying a Gaussian filter or Canny transform to the HDR image (steps S300-S330 in FIG. 7).The edge detection unit 17 then detects the opening-side edge Eo and the bottom-side edge Eb from their corresponding search ranges (steps S400-S470 in FIG. 8).

[0045] In this way, by narrowing the search range for the opening-side edge Eo and the bottom-side edge Eb prior to detecting them, it is possible to accurately detect the opening-side edge Eo and the bottom-side edge Eb while reducing the computational load.

[0046] Furthermore, the position and orientation acquisition device 10 includes an object detection unit 15 that uses YOLO as an object detection algorithm to determine whether the charging inlet CI is included in the HDR image generated by the preprocessing unit 14. When the charging inlet CI is not included in at least the HDR image generated by the preprocessing unit 14 (step S220: NO), the object detection unit 15 operates the robot arm 3 so that the charging inlet CI is included in the infrared image acquired by the imaging device 11 (step S280).

[0047] This allows the opening edge Eo and the bottom edge Eb to be detected with high accuracy, and the position and orientation of the charging inlet CI with respect to the imaging device 11 to be estimated with high accuracy.

[0048] The imaging device 11 also includes a 2D camera 110, an irradiation unit 111 that irradiates the charging inlet CI with infrared light, and a bandpass filter 115 that allows the infrared light reflected by the charging inlet CI to enter the 2D camera 110.

[0049] This allows for stable infrared images to be obtained with sufficient illuminance for sunlight and the like, while suppressing the influence of ambient light by using light in a wavelength range that is not contained in large amounts in sunlight and that ensures sufficient spectral sensitivity.

[0050] Furthermore, the preprocessing unit 14 generates an HDR image by combining a plurality of infrared images captured by the imaging device 11 with different exposure times, and performs tone mapping on the generated HDR image (steps S100-S120 in FIG. 5).

[0051] This makes it possible to obtain an HDR image with clear edges (contours) of the charging inlet CI and no loss of information.

[0052] The object detection algorithm used in the object detection unit 15 of the position and orientation acquisition device 10 is not limited to the above-mentioned YOLO, and may be, for example, SSD (Single Shot Multibox Detector) or CNN (Convolutional Neural Network). Furthermore, the edge search range setting unit 16 is not limited to setting the search ranges for the opening-side edge Eo and the bottom-side edge Eb based on the inscribed circle of a bounding box, and may set the search ranges for the opening-side edge Eo and the bottom-side edge Eb using, for example, monocular depth estimation. Furthermore, the edge detection unit 17 may detect the opening-side edge Eo and the bottom-side edge Eb using an algorithm other than the Hough transform. In addition, the position and orientation acquisition device 10 may be modified to estimate the position and orientation of the charging inlet CI relative to the imaging device 11 based on the opening edge and bottom edge of the recess Rt of at least one of the bottomed cylindrical positive power supply terminal Tp and negative power supply terminal Tn, instead of the opening edge Eo and bottom edge Eb of the recess R into which the fitting portion 201 of the charging connector 200 is fitted.

[0053] The present invention is not limited to the above-described embodiment, and various modifications can be made within the scope of the present disclosure. Furthermore, the above-described embodiment is merely a specific form of the invention described in the Summary of the Invention, and does not limit the elements of the invention described in the Summary of the Invention. [Industrial Applicability]

[0054] The invention of the present disclosure is useful in equipment that can automatically insert and remove a charging connector into and from a charging inlet of an electric vehicle. [Explanation of symbols]

[0055] 1 charging connector insertion / removal device, 3 robot arm (charging connector moving device), 10 position and orientation acquisition device, 11 imaging device, 110 2D camera, 111 irradiation unit, 115 bandpass filter, 13 image acquisition unit, 14 preprocessing unit, 15 object detection unit, 16 edge search range setting unit, 17 edge detection unit, 18 position and orientation estimation unit, 30 robot control device, 200 charging connector, CI charging inlet. Eb bottom edge, Eo opening edge, R, Rt recess.

Claims

1. A position and attitude acquisition device for acquiring a position and attitude of a charging inlet of an electric vehicle, an imaging device that irradiates the charging inlet with infrared rays to capture an infrared image of the charging inlet; a pre-processing unit that widens the dynamic range of the infrared image acquired by the imaging device; an edge detection unit that detects an opening edge of a recess included in the charging inlet that is close to the imaging device and a bottom edge of the recess that is away from the imaging device from the image processed by the preprocessing unit; a position and orientation estimation unit that estimates a position and orientation of the charging inlet relative to the imaging device based on the opening-side edge and the bottom-side edge detected by the edge detection unit; A position and orientation acquisition device comprising:

2. 2. The position and orientation acquisition device according to claim 1, The position and orientation estimation unit calculates the tilt angle of the charging inlet relative to the imaging device from the amount of deviation between a feature point of the opening-side edge and a feature point of the bottom-side edge, and calculates the distance between the imaging device and the charging inlet based on the size of the opening-side edge or the bottom-side edge.

3. 3. The position and orientation acquisition device according to claim 2, an object detection unit that determines whether the charging inlet is included in the image processed by the preprocessing unit using an object detection algorithm; an edge search range setting unit that sets a search range for the opening-side edge and a search range for the bottom-side edge in the image processed by the preprocessing unit, the imaging device is supported by a charging connector moving device together with the charging connector that is inserted into or removed from the charging inlet; the object detection unit, when the charging inlet is not included in the image processed by the preprocessing unit, operates the charging connector moving device so that the charging inlet is included in the infrared image acquired by the imaging device; the edge detection unit detects the opening-side edge and the bottom-side edge from the corresponding search ranges, The position and attitude estimation unit calculates the distance between the imaging device and the charging inlet when the tilt angle of the charging inlet is equal to or less than a predetermined angle, and operates the charging connector moving device so that the imaging device moves in a direction that makes the tilt angle zero when the tilt angle of the charging inlet is greater than the predetermined angle.

4. 4. The position and orientation acquisition device according to claim 1, the imaging device includes a 2D camera, an irradiation unit that irradiates the charging inlet with infrared light, and a bandpass filter that allows the infrared light reflected by the charging inlet to be incident on the 2D camera; The preprocessing unit generates an HDR image by synthesizing a plurality of infrared images captured by the imaging device with different exposure times, and performs tone mapping on the generated HDR image.

Images