Hitch angle estimation device, hitch angle estimation method, and program

Abstract

To provide a hitch angle estimation device capable of properly estimating a trailer hitch angle in world coordinates.SOLUTION: A hitch angle estimation device 16 includes: a conversion unit 3D that performs a transformation from the view coordinate system to the world coordinate system for the sequence of points representing the bottom of the towbar and trailer on the image taken by camera 11 mounted on vehicle 1 towing the trailer via the towbar; a straight fitting unit 3E that performs a line fit to the sequence of points that represent the bottom of the towbar and trailer after the transformation has been performed based on the line indicating the tow bar in the world coordinate system obtained by straight line fitting and the line indicating the bottom end of the trailer in the world coordinate system; a calculation unit 3F that calculates the trailer hitch angle in the world coordinate system; and a processing unit 3G that performs Kalman filtering on the trailer hitch angle in the world coordinate system, calculated by calculation unit 3F.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present disclosure relates to a hitch angle estimation device, a hitch angle estimation method, and a program. [Background technology]

[0002] Patent Document 1 describes image-based trailer angle estimation. In the technology described in Patent Document 1, Kalman filtering is applied to each trailer angle measurement based on three-dimensional angle measurements. In the technology described in Patent Document 1, the path (trajectory) of each wheel and line detected in the image is correlated with the corresponding two-dimensional trailer angle using the Kalman filtering process. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Application Publication No. 2023-178952 Summary of the Invention [Problem to be solved by the invention]

[0004] However, Patent Document 1 does not disclose a technology for appropriately estimating the trailer hitch angle in the world coordinate system based on an image captured by a camera mounted on a vehicle towing a trailer via a tow bar when the image is unclear. Therefore, the technology disclosed in Patent Document 1 is unable to appropriately estimate the trailer hitch angle in the world coordinate system based on an image captured by a camera mounted on a vehicle towing a trailer via a tow bar when the image is unclear.

[0005] In view of the above, an object of the present disclosure is to provide a hitch angle estimation device, a hitch angle estimation method, and a program that can appropriately estimate the trailer hitch angle in a world coordinate system based on an image taken by a camera mounted on a vehicle towing a trailer via a tow bar, even when the image is unclear. is. [Means for solving the problem]

[0006] (1) One aspect of the present disclosure is a method for detecting a trailer towing position from a view coordinate system to a world coordinate system, the method comprising: a transformation unit that performs transformation from a view coordinate system to a world coordinate system on a sequence of points indicating a tow bar on an image captured by a camera mounted on a vehicle towing a trailer via a tow bar and a sequence of points indicating a bottom end of the trailer on the image; a straight line fitting unit that performs linear fitting of the sequence of points indicating the tow bar after the transformation has been performed by the transformation unit and linear fitting of the sequence of points indicating the bottom end of the trailer after the transformation has been performed by the transformation unit; and a straight line fitting unit that performs linear fitting of the sequence of points indicating the tow bar in the world coordinate system and the bottom end of the trailer obtained by the linear fitting. This hitch angle estimation device includes a calculation unit that calculates the trailer hitch angle in a world coordinate system, and a processing unit that performs Kalman filter processing on the trailer hitch angle in the world coordinate system calculated by the calculation unit, wherein the processing unit uses the trailer hitch angle in the world coordinate system calculated by the calculation unit as an observation function, and uses a motion model that represents the vehicle, the trailer, and the tow bar obtained by performing a transformation from the view coordinate system to the world coordinate system on the image as a state transition function, thereby outputting the trailer hitch angle in the world coordinate system after the Kalman filter processing has been performed.

[0007] (2) In the hitch angle estimation device of (1), the Kalman filter used in the Kalman filter processing may be an unscented Kalman filter (UKF).

[0008] (3) In the hitch angle estimation device of (1) or (2), the motion model representing the vehicle, the trailer, and the tow bar may include the trailer hitch angle in the world coordinate system, the vehicle steering angle in the world coordinate system, the vehicle speed in the world coordinate system, the vehicle wheelbase in the world coordinate system, the vehicle hitch length in the world coordinate system, and the trailer beam length in the world coordinate system.

[0009] (4) One aspect of the present disclosure is a hitch angle estimation device that performs a transformation from a view coordinate system to a world coordinate system with respect to a sequence of points indicating the tow bar on an image captured by a camera mounted on a vehicle towing a trailer via the tow bar and a sequence of points indicating a bottom end of the trailer on the image; a straight line fitting step in which the hitch angle estimation device performs straight line fitting of the sequence of points indicating the tow bar after the transformation has been performed in the transformation step and straight line fitting of the sequence of points indicating the bottom end of the trailer after the transformation has been performed in the transformation step; and a straight line fitting step in which the hitch angle estimation device performs a straight line fitting of the line indicating the tow bar in the world coordinate system obtained by the straight line fitting and the line indicating the bottom end of the trailer in the world coordinate system. and a processing step in which the hitch angle estimation device performs Kalman filter processing on the trailer hitch angle in the world coordinate system calculated in the calculation step, wherein the processing step uses the trailer hitch angle in the world coordinate system calculated in the calculation step as an observation function, and uses a motion model representing the vehicle, trailer, and tow bar obtained by performing a transformation from the view coordinate system to the world coordinate system on the image as a state transition function, thereby outputting the trailer hitch angle in the world coordinate system after the Kalman filter processing has been performed.

[0010] (5) One aspect of the present disclosure is a method for causing a processor to perform a transformation from a view coordinate system to a world coordinate system with respect to a sequence of points indicating the tow bar on an image captured by a camera mounted on a vehicle towing a trailer via a tow bar and a sequence of points indicating a bottom end of the trailer on the image; a linear fitting step of performing linear fitting of the sequence of points indicating the tow bar after the transformation has been performed in the transformation step and linear fitting of the sequence of points indicating the bottom end of the trailer after the transformation has been performed in the transformation step; and a linear fitting step of performing linear fitting of the sequence of points indicating the tow bar in the world coordinate system and a linear fitting of the bottom end of the trailer in the world coordinate system based on the line obtained by the linear fitting and the line indicating the bottom end of the trailer in the world coordinate system. and a processing step of performing Kalman filter processing on the trailer hitch angle in the world coordinate system calculated in the calculation step, wherein in the processing step, the trailer hitch angle in the world coordinate system calculated in the calculation step is used as an observation function, and a motion model representing the vehicle, trailer, and tow bar obtained by performing a transformation from the view coordinate system to the world coordinate system on the image is used as a state transition function, thereby outputting the trailer hitch angle in the world coordinate system after the Kalman filter processing has been performed. [Effects of the Invention]

[0011] According to the present disclosure, even when an image captured by a camera mounted on a vehicle towing a trailer via a tow bar is unclear, the trailer hitch angle in the world coordinate system can be appropriately estimated based on the image. [Brief explanation of the drawings]

[0012] [Figure 1] 1 is a diagram illustrating an example of a vehicle to which a hitch angle estimation device according to a first embodiment is applied. [Figure 2] FIG. 2 is a diagram showing an example of the relationship between the vehicle, trailer, and tow bar shown in FIG. [Figure 3] 1A and 1B are diagrams showing an example of an image including a trailer and a tow bar captured by a camera. [Figure 4] FIG. 10 is a diagram illustrating an example of a trailer hitch angle in a world coordinate system calculated by a calculation unit. [Figure 5] FIG. 2 is a diagram illustrating an example of a motion model showing a vehicle, a trailer, and a tow bar. [Figure 6] 4 is a flowchart illustrating an example of processing executed in the hitch angle estimation device of the first embodiment. DETAILED DESCRIPTION OF THE INVENTION

[0013] Hereinafter, embodiments of a hitch angle estimation device, a hitch angle estimation method, and a program according to the present disclosure will be described with reference to the drawings.

[0014] First Embodiment Fig. 1 is a diagram showing an example of a vehicle 1 to which a hitch angle estimation device 16 of the first embodiment is applied. Fig. 2 is a diagram showing an example of the relationship between the vehicle 1, trailer TR, and tow bar DB shown in Fig. 1. In detail, Fig. 2(A) is a diagram showing the vehicle 1, trailer TR, and tow bar DB as viewed from the side, and Fig. 2(B) is a diagram showing the vehicle 1, trailer TR, and tow bar DB as viewed from below (the lower side of Fig. 2(A)). In the example shown in FIGS. 1 and 2, a vehicle 1 tows a trailer TR via a tow bar DB. The vehicle 1 is equipped with a camera 11, an HMI (Human Machine Interface) 12, a vehicle speed sensor 13, a steering angle sensor 14, a vehicle control device 15, a steering actuator 15A, a braking actuator 15B, a driving actuator 15C, and a hitch angle estimation device 16. The camera 11 is disposed, for example, at the rear end 1R of the vehicle 1. The camera 11 captures an image of the rear of the vehicle 1 (the right side in FIG. 2(A)) and transmits an image (for example, a fisheye lens image) IM (see FIG. 3(A)) including the trailer TR and the tow bar DB to the hitch angle estimation device 16. As shown in FIG. 2, the tow bar DB is fixed to the trailer TR and is connected to the vehicle 1 so as to be rotatable about a hitch ball HB.

[0015] 3A and 3B are diagrams showing an example of an image IM including a trailer TR and a tow bar DB photographed by the camera 11. Specifically, FIG. 3A is a diagram showing an example of an image IM including a trailer TR and a tow bar DB photographed by the camera 11. FIG. 3B is a diagram showing an example of a sequence of points PTR1 indicating the bottom end TR1 of the trailer TR and a sequence of points PDB indicating the tow bar DB, which are extracted from the image IM shown in FIG. 3A by an extraction unit 3C (described later). FIG. 3C is a diagram showing an example of a line LTR1 indicating the bottom end TR1 of the trailer TR in the world coordinate system and a line LDB indicating the tow bar DB in the world coordinate system, which are obtained by performing a conversion from the view coordinate system to the world coordinate system by a conversion unit 3D (described later) on the sequence of points PTR1 and the sequence of points PDB shown in FIG. 3B and then performing line fitting by a line fitting unit 3E (described later).

[0016] 1 to 3, the HMI 12 has a function of receiving various operations by the driver of the vehicle 1, and transmits a signal indicating the operation by the driver of the vehicle 1 to the vehicle control device 15. The vehicle speed sensor 13 detects the speed (vehicle speed) V [m / s] of the vehicle 1 and transmits the detection result to the vehicle control device 15 and the hitch angle estimation device 16. The steering angle sensor 14 detects the steering angle Φ [deg] and transmits the detection result to the vehicle control device 15 and the hitch angle estimation device 16. The vehicle control device 15 controls the steering actuator 15A, the braking actuator 15B, and the drive actuator 15C based on the signals transmitted from the HMI 12, etc.

[0017] The hitch angle estimation device 16 is configured by a microcomputer including a communication interface (I / F) 161, a memory 162, and a processor 163. The communication interface 161 has an interface circuit for connecting the hitch angle estimation device 16 to the camera 11, the HMI 12, the vehicle speed sensor 13, the steering angle sensor 14, and the vehicle control device 15. The memory 162 stores programs and various data used in the processing executed by the processor 163. In detail, the memory 162 stores, for example, the wheelbase WB of the vehicle 1 (see FIG. 2(A)), the rear overhang OH of the vehicle 1 (see FIG. 2(A)), etc. The wheelbase WB of the vehicle 1, the rear overhang OH of the vehicle 1, etc. are written to the memory 162, for example, when the vehicle 1 is manufactured. The memory 162 also stores the trailer beam length TBL (the length from the hitch ball HB to the wheels of the trailer TR) (see FIG. 2(A)). The trailer beam length TBL is calculated, for example, based on the behavior of the trailer TR during calibration running of the trailer TR, and is written to the memory 162 after the calibration running of the trailer TR. The processor 163 has a function as an acquisition unit 3A, a function as an inference unit 3B, a function as an extraction unit 3C, a function as a conversion unit 3D, a function as a straight line fitting unit 3E, a function as a calculation unit 3F, and a function as a processing unit 3G.

[0018] The acquisition unit 3A acquires an image IM including a trailer TR and a tow bar DB photographed by the camera 11. The acquisition unit 3A also acquires a vehicle speed V detected by a vehicle speed sensor 13 and a steering angle Φ detected by a steering angle sensor 14. 3(A), the inference unit 3B infers the bottom end TR1 of the trailer TR and the tow bar DB on the image IM based on the image IM acquired by the acquisition unit 3A. In detail, the inference unit 3B infers the bottom end TR1 of the trailer TR and the tow bar DB on the image IM based on the image IM acquired by the acquisition unit 3A by using a model obtained by learning using training data, which is a data set of training images taken by a training camera (not shown) mounted on a training vehicle (not shown) that tows a training trailer (not shown) via a training tow bar (not shown), and labels indicating the bottom end of the training trailer and the training tow bar on the training image. As shown in Figures 3(A) and 3(B), the extraction unit 3C extracts a sequence of points PTR1 indicating the bottom end TR1 of the trailer TR on the image IM and a sequence of points PDB indicating the tow bar DB on the image IM based on the bottom end TR1 of the trailer TR on the image IM and the tow bar DB inferred by the inference unit 3B.

[0019] The conversion unit 3D performs conversion from the view coordinate system to the world coordinate system for the image IM shown in Fig. 3(B) by using a known technique called, for example, coordinate conversion, etc. That is, the conversion unit 3D performs conversion from the view coordinate system to the world coordinate system for the sequence of points PTR1 indicating the bottom end portion TR1 of the trailer TR and the sequence of points PDB indicating the tow bar DB on the image IM shown in Fig. 3(B). The straight line fitting unit 3E performs straight line fitting of the sequence of points PTR1 indicating the bottom end TR1 of the trailer TR after the transformation has been performed by the transformation unit 3D, and generates a straight line LTR1 indicating the bottom end TR1 of the trailer TR in the world coordinate system as shown in Fig. 3(C). The straight line fitting unit 3E also performs straight line fitting of the sequence of points PDB indicating the tow bar DB after the transformation has been performed by the transformation unit 3D, and generates a straight line LDB indicating the tow bar DB in the world coordinate system as shown in Fig. 3(C). The calculation unit 3F calculates the hitch angle ψ [deg] of the trailer TR in the world coordinate system based on the straight line LTR1 indicating the lower end TR1 of the trailer TR in the world coordinate system obtained by straight line fitting shown in Figure 3 (C), the straight line LDB indicating the tow bar DB in the world coordinate system, etc.

[0020] Fig. 4 is a diagram showing an example of the hitch angle ψ [deg] of the trailer TR in the world coordinate system calculated by the calculation unit 3F. The vertical axis of Fig. 4 represents the hitch angle ψ of the trailer TR in the world coordinate system, and the horizontal axis of Fig. 4 represents time. In Figure 4, the time period when the hitch angle ψ of the trailer TR in the world coordinate system is zero corresponds to the time period when the steering angle Φ of the vehicle 1 is zero (the time period when the vehicle 1 is traveling straight). The time period when the hitch angle ψ of the trailer TR in the world coordinate system is greater than zero corresponds to the time period when the steering angle Φ of the vehicle 1 is greater than zero (the time period when the vehicle 1 is turning).

[0021] 1 to 4, the processing unit 3G performs Kalman filter processing on the hitch angle ψ of the trailer TR in the world coordinate system calculated by the calculation unit 3F. The processing unit 3G performs Kalman filter processing using an unscented Kalman filter (UKF). Specifically, the processing unit 3G uses the hitch angle ψ (see FIG. 4) of the trailer TR in the world coordinate system calculated by the calculation unit 3F as an observation function in the Kalman filter processing. Also, the processing unit 3G uses a motion model (see FIG. 5) representing the vehicle 1, trailer TR, and tow bar DB obtained by converting the image IM from the view coordinate system to the world coordinate system as a state transition function in the Kalman filter processing.

[0022] FIG. 5 is a diagram showing an example of a motion model showing a vehicle 1, a trailer TR, and a tow bar DB. 1 to 5, the motion model showing the vehicle 1, trailer TR, and tow bar DB includes the steering angle Φ (see FIG. 5) of the vehicle 1 in the world coordinate system. The steering angle Φ detected by the steering angle sensor 14 is used as the steering angle Φ of the vehicle 1 in the world coordinate system. The motion model representing the vehicle 1, trailer TR, and tow bar DB also includes the speed V of the vehicle 1 in the world coordinate system. The speed V of the vehicle 1 detected by the vehicle speed sensor 13 is used as the speed V of the vehicle 1 in the world coordinate system. Furthermore, the motion model representing the vehicle 1, trailer TR, and tow bar DB includes the wheelbase WB of the vehicle 1 in the world coordinate system (see FIG. 5). The wheelbase WB of the vehicle 1 written in memory 162 (see FIG. 2(A)) is used as the wheelbase WB of the vehicle 1 in the world coordinate system.

[0023] The motion model representing the vehicle 1, trailer TR, and tow bar DB includes the hitch length HL of the vehicle 1 in the world coordinate system (see FIG. 5). As shown in FIG. 5, the hitch length HL of the vehicle 1 in the world coordinate system is the sum of the rear overhang OH of the vehicle 1 in the world coordinate system and the length ΔL from the rear end 1R of the vehicle 1 to the hitch ball HB in the world coordinate system. The rear overhang OH of the vehicle 1 written in memory 162 (see FIG. 2(A)) is used as the rear overhang OH of the vehicle 1 in the world coordinate system. The length ΔL from the rear end 1R of the vehicle 1 to the hitch ball HB in the world coordinate system is used as the length ΔL (see FIG. 2(A)) calculated based on the image IM (see FIG. 3(A)) including the trailer TR and tow bar DB captured by the camera 11. The motion model representing the vehicle 1, trailer TR, and tow bar DB also includes the trailer beam length TBL in the world coordinate system (see FIG. 5). As the trailer beam length TBL in the world coordinate system, the trailer beam length TBL (see FIG. 2(A)) calculated based on the behavior of the trailer TR during the calibration run of the trailer TR as described above and written to the memory 162 is used. Furthermore, the motion model representing the vehicle 1, trailer TR, and tow bar DB includes the hitch angle ψ of the trailer TR in the world coordinate system (see FIG. 5). The hitch angle ψ of the trailer TR in the world coordinate system included in the motion model representing the vehicle 1, trailer TR, and tow bar DB satisfies the following equation:

[0024] ψ=-(V / WB)(tanΦ+(WB / TBL)×sinψ+(HL / TBL)×tanΦ×cosψ)

[0025] The hitch angle ψ of the trailer TR in the world coordinate system in the above equation changes in accordance with changes in the steering angle Φ of the vehicle 1 in the world coordinate system, similar to the hitch angle ψ of the trailer TR in the world coordinate system shown in Figure 4 (i.e., the hitch angle ψ of the trailer TR in the world coordinate system calculated by the calculation unit 3F).

[0026] In the example shown in FIGS. 1 to 5, the processing unit 3G outputs the hitch angle ψ of the trailer TR in the world coordinate system after the above-mentioned Kalman filter processing has been performed. That is, in the examples shown in Figures 1 to 5, the processing unit 3G estimates the hitch angle ψ of the trailer TR in the world coordinate system by appropriately combining the observation function and the state transition function (more specifically, by reflecting the hitch angle ψ of the trailer TR in the world coordinate system that was calculated last time). Therefore, in the example shown in Figures 1 to 5, even if the image IM taken by the camera 11 mounted on the vehicle 1 towing the trailer TR via the towing bar DB is unclear, the hitch angle ψ of the trailer TR in the world coordinate system can be appropriately estimated based on the image IM. 1 to 5, if the image IM captured by the camera 11 mounted on the vehicle 1 towing the trailer TR via the tow bar DB is blurred, the hitch angle ψ of the trailer TR in the world coordinate system is not estimated based solely on the blurred image IM, but rather the hitch angle ψ of the trailer TR in the world coordinate system, etc., included in the motion model used when the hitch angle ψ of the trailer TR in the world coordinate system was previously estimated, is taken into consideration to estimate the hitch angle ψ of the trailer TR in the world coordinate system this time. This makes it possible to reduce the risk of the hitch angle ψ of the trailer TR in the world coordinate system being inappropriately estimated based on the blurred image IM.

[0027] FIG. 6 is a flowchart illustrating an example of processing executed in the hitch angle estimation device 16 of the first embodiment. 6, in step S10, the acquisition unit 3A acquires an image IM including a trailer TR and a tow bar DB captured by the camera 11. The acquisition unit 3A also acquires the vehicle speed V detected by the vehicle speed sensor 13 and the steering angle Φ detected by the steering angle sensor 14. The acquisition unit 3A also acquires the wheelbase WB of the vehicle 1, the rear overhang OH of the vehicle 1, the trailer beam length TBL, etc., which are written in the memory 162. In step S11, the inference unit 3B infers the bottom end portion TR1 of the trailer TR and the tow bar DB on the image IM based on the image IM acquired in step S10. In step S12, the extraction unit 3C extracts a sequence of points PTR1 indicating the bottom end TR1 of the trailer TR on the image IM and a sequence of points PDB indicating the tow bar DB on the image IM based on the bottom end TR1 of the trailer TR on the image IM inferred in step S11. In step S13, the conversion unit 3D converts the sequence of points PTR1 indicating the bottom end TR1 of the trailer TR and the sequence of points PDB indicating the tow bar DB on the image IM from the view coordinate system to the world coordinate system.

[0028] In step S14, the straight line fitting unit 3E performs straight line fitting of the sequence of points PTR1 indicating the bottom end TR1 of the trailer TR after step S13 has been executed, and generates a straight line LTR1 indicating the bottom end TR1 of the trailer TR in the world coordinate system. The straight line fitting unit 3E also performs straight line fitting of the sequence of points PDB indicating the tow bar DB after step S13 has been executed, and generates a straight line LDB indicating the tow bar DB in the world coordinate system. do. In step S15, the calculation unit 3F calculates the hitch angle ψ [deg] of the trailer TR in the world coordinate system based on the straight line LTR1 indicating the lower end TR1 of the trailer TR in the world coordinate system obtained by straight line fitting shown in Figure 3 (C), the straight line LDB indicating the tow bar DB in the world coordinate system, etc. In step S16, the processing unit 3G executes Kalman filter processing on the hitch angle ψ of the trailer TR in the world coordinate system calculated in step S15.

[0029] Second Embodiment The vehicle 1 to which the hitch angle estimation device 16 of the second embodiment is applied is configured in the same manner as the vehicle 1 to which the hitch angle estimation device 16 of the first embodiment described above is applied, except for the points described below.

[0030] As described above, in the vehicle 1 to which the hitch angle estimation device 16 of the first embodiment is applied, the conversion unit 3D performs conversion from the view coordinate system to the world coordinate system for the image IM shown in Figure 3(B) by using a known technique. On the other hand, in the vehicle 1 to which the hitch angle estimation device 16 of the second embodiment is applied, the conversion unit 3D performs a conversion from the view coordinate system to the world coordinate system for the image IM shown in Figure 3(B) by using a technology other than publicly known technology (for example, a technology specific to the manufacturer of the vehicle 1).

[0031] As described above, embodiments of the hitch angle estimation device, hitch angle estimation method, and program of the present disclosure have been described with reference to the drawings. However, the hitch angle estimation device, hitch angle estimation method, and program of the present disclosure are not limited to the above-described embodiments and may be modified as appropriate without departing from the spirit and scope of the present disclosure. The configurations of the above-described embodiments may be combined as appropriate. In the above-described embodiments, the processing performed by the hitch angle estimation device 16 has been described as software processing performed by executing a program. However, the processing performed by the hitch angle estimation device 16 may also be hardware processing. Alternatively, the processing performed by the hitch angle estimation device 16 may be a combination of both software and hardware. Furthermore, the program stored in the memory 162 of the hitch angle estimation device 16 (the program that realizes the functions of the processor 163 of the hitch angle estimation device 16) may be provided, distributed, etc., by being recorded on a computer-readable storage medium such as a semiconductor memory, a magnetic recording medium, an optical recording medium, etc. [Explanation of symbols]

[0032] 1...vehicle, 1R...rear end, 11...camera, 12...HMI, 13...vehicle speed sensor, 14...steering angle sensor, 15...vehicle control device, 15A...steering actuator, 15B...braking actuator, 15C...driving actuator, 16...hitch angle estimation device, 161...communication interface, 162...memory, 163...processor, 3A...acquisition unit, 3B...inference unit, 3C...extraction unit, 3D...conversion unit, 3E...straight line fitting unit, 3F...calculation unit, 3G...processing unit

Claims

1. a transformation unit that performs transformation from a view coordinate system to a world coordinate system for a sequence of points representing the tow bar on an image captured by a camera mounted on a vehicle towing a trailer via the tow bar and a sequence of points representing the bottom end of the trailer on the image; a straight line fitting unit that performs straight line fitting of the sequence of points representing the tow bar after the transformation performed by the transformation unit and straight line fitting of the sequence of points representing the bottom end of the trailer after the transformation performed by the transformation unit; a calculation unit that calculates a hitch angle of the trailer in the world coordinate system based on a straight line that indicates the tow bar in the world coordinate system obtained by the straight line fitting and a straight line that indicates a bottom end of the trailer in the world coordinate system; a processing unit that performs Kalman filtering on the trailer hitch angle in the world coordinate system calculated by the calculation unit, The processing unit using the trailer hitch angle in the world coordinate system calculated by the calculation unit as an observation function; by using, as a state transition function, a motion model representing the vehicle, the trailer, and the towbar obtained by performing a transformation from the view coordinate system to the world coordinate system on the image; outputting the trailer hitch angle in the world coordinate system after the Kalman filtering process has been performed; Hitch angle estimator.

2. 2. The hitch angle estimation device according to claim 1, wherein the Kalman filter used in the Kalman filter processing is an unscented Kalman filter.

3. 3. The hitch angle estimation device according to claim 1, wherein the motion model representing the vehicle, the trailer, and the tow bar includes a hitch angle of the trailer in the world coordinate system, a steering angle of the vehicle in the world coordinate system, a speed of the vehicle in the world coordinate system, a wheelbase of the vehicle in the world coordinate system, a hitch length of the vehicle in the world coordinate system, and a trailer beam length in the world coordinate system.

4. a transformation step in which the hitch angle estimation device performs transformation from a view coordinate system to a world coordinate system for a sequence of points representing the tow bar on an image captured by a camera mounted on a vehicle towing a trailer via the tow bar and a sequence of points representing the bottom end of the trailer on the image; a straight line fitting step in which the hitch angle estimation device performs straight line fitting of a sequence of points representing the tow bar after the transformation has been performed in the transformation step, and a straight line fitting of a sequence of points representing the bottom end of the trailer after the transformation has been performed in the transformation step; a calculation step in which the hitch angle estimation device calculates a hitch angle of the trailer in the world coordinate system based on a straight line indicating the tow bar in the world coordinate system obtained by the straight line fitting and a straight line indicating a bottom end of the trailer in the world coordinate system; a processing step in which the hitch angle estimation device performs Kalman filtering on the trailer hitch angle in the world coordinate system calculated in the calculation step, In the processing step, the hitch angle of the trailer in the world coordinate system calculated in the calculation step is used as an observation function; A motion model representing the vehicle, the trailer, and the tow bar obtained by performing a transformation from the view coordinate system to the world coordinate system on the image is used as a state transition function, The trailer hitch angle in the world coordinate system after the Kalman filter processing is performed is output. Hitch angle estimation method.

5. The processor a transformation step of transforming a sequence of points representing the tow bar on an image captured by a camera mounted on a vehicle towing a trailer via the tow bar and a sequence of points representing the bottom end of the trailer on the image from a view coordinate system to a world coordinate system; a straight line fitting step of performing straight line fitting of the sequence of points representing the tow bar after the transformation has been performed in the transformation step and straight line fitting of the sequence of points representing the bottom end of the trailer after the transformation has been performed in the transformation step; a calculation step of calculating a hitch angle of the trailer in the world coordinate system based on a straight line indicating the tow bar in the world coordinate system obtained by the straight line fitting and a straight line indicating a bottom end of the trailer in the world coordinate system; a processing step of performing Kalman filter processing on the trailer hitch angle in the world coordinate system calculated in the calculation step, In the processing step, the hitch angle of the trailer in the world coordinate system calculated in the calculation step is used as an observation function; A motion model representing the vehicle, the trailer, and the tow bar obtained by performing a transformation from the view coordinate system to the world coordinate system on the image is used as a state transition function, The trailer hitch angle in the world coordinate system after the Kalman filter processing is performed is output. program.

Images