Odometry parameter calibration device, method, and program

Abstract

To calibrate odometry parameters regarding differential two-wheeled vehicles by acquiring the minimum amount of measurement values required for each vehicle.SOLUTION: Along with the traveling of a vehicle, an execution control unit 70 causes an acquisition unit 62 to repeatedly acquire the measurement values of the translational speed of the vehicle, the angular speed of the vehicle, and the revolving speed of each wheel, and causes a parameter initial value calculation unit 64 to obtain the initial values of first and second parameters on the basis of the measurement values acquired in an initial value target time range. Thereafter, the execution control unit 70 causes a parameter update unit 66 to update the first and second parameters on the basis of newly acquired measurement values until an update termination condition for determining that the fluctuation of the first and second parameters are sufficiently reduced by updating is satisfied, and causes an odometry parameter calculation unit 68 to obtain an odometry parameter from the first and second parameters the updates of which have been completed.SELECTED DRAWING: Figure 5

Description

[Technical Field]

[0001] The present disclosure relates to an odometry parameter calibration device, an odometry parameter calibration method, and an odometry parameter calibration program. [Background technology]

[0002] Non-Patent Document 1 discloses a method for calibrating parameters (hereinafter referred to as "odometry parameters") used in odometry, which is one method for estimating a vehicle's self-position. When the vehicle is a two-wheel differential vehicle, the odometry parameters are the distance between the two driven wheels and the radius of each wheel. This calibration method involves setting the amount of measurement values ​​to be used for calibration in advance, running the vehicle to acquire the set amount of measurement values, and then performing calculations to calibrate the odometry parameters. [Prior art documents] [Non-patent literature]

[0003] [Non-Patent Document 1] Sousa, RB, Petry, MR, Costa, PG et al., “OptiOdom: a Generic Approach for Odometry Calibration of Wheeled Mobile Robots,” J Intel Robot Syst 105, 39 (2022). Summary of the Invention [Problem to be solved by the invention]

[0004] Since the values ​​of the odometry parameters differ for each vehicle due to variations in parts and assembly during the vehicle manufacturing process, calibration work must be performed on all vehicles during the manufacturing process. Therefore, the manufacturing cost due to the time required for calibration work accounts for a certain percentage of the cost of all vehicles.

[0005] Furthermore, due to variations between vehicles, the amount of measurement values ​​required for calibration also differs from vehicle to vehicle. Collecting a large number of these measurement values ​​increases the driving time, which in turn increases the calibration time. Therefore, it is necessary to shorten the calibration process and reduce manufacturing costs by collecting the minimum amount of measurement values ​​required for each vehicle and calibrating it.

[0006] In the calibration method of Non-Patent Document 1, the amount of measurement values ​​to be used for calibration is set in advance, so depending on the vehicle, the amount of measurement values ​​may be insufficient or excessive. If the amount of measurement values ​​is insufficient, the odometry parameters will not be calibrated correctly, so it is necessary to set a larger amount of measurement values ​​to be acquired and run the vehicle again. If the amount of measurement values ​​is excessive, it means that the running time was too long.

[0007] An object of the present disclosure is to provide a method for calibrating odometry parameters for a differential two-wheel vehicle by acquiring the minimum amount of measurements required for each vehicle. [Means for solving the problem]

[0008] In order to achieve the above object, an odometry parameter calibration device according to the present disclosure is an odometry parameter calibration device that calibrates odometry parameters including an inter-wheel distance and a radius of each wheel for both wheels that drive a differential two-wheel vehicle, wherein the vehicle is a vehicle for which a measurement reference position and a measurement reference direction that serve as references for measuring the position and attitude of the vehicle, and a running reference position and a running reference direction that serve as references for the position and attitude in running control of the vehicle, are set, and the odometry parameters are specified by the inter-wheel distance and the radius of each wheel, a first parameter that is a parameter that defines a relationship between the angular velocity of the vehicle and the rotational velocity of each of the wheels, and a second parameter that is specified by the relative position between the measurement reference position and the traveling reference position, the relative direction between the measurement reference direction and the traveling reference direction, and the wheel-to-wheel distance, and that defines a relationship among the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, the rotational velocity of each of the wheels, and the first parameter; and the odometry parameter calibration device an acquisition unit that acquires the measured values ​​of the vehicle attitude and the rotational speed of each of the wheels; a parameter initial value calculation unit that calculates an initial value of the first parameter based on the measured values ​​of the angular velocity of the vehicle and the rotational speed of each of the wheels within an initial value target time range, and calculates an initial value of the second parameter based on the initial value of the first parameter, and the measured values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the vehicle attitude, and the rotational speed of each of the wheels within the initial value target time range; a parameter update unit that updates the first parameter based on the latest first parameter and the additionally acquired measured values ​​of the angular velocity of the vehicle and the rotational speed of each of the wheels, and updates the second parameter based on the latest second parameter, the updated first parameter, a change amount of the first parameter due to the update, and the additionally acquired measured values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the vehicle attitude, and the rotational speed of each of the wheels; an odometry parameter calculation unit that calculates the odometry parameters from the first parameter and the second parameter;and an execution control unit that repeatedly acquires the measurement values ​​of the attitude of the vehicle and the rotational speed of each of the wheels, causes the parameter initial value calculation unit to determine the initial values ​​of the first parameter and the initial values ​​of the second parameter based on the measurement values ​​of the translational speed of the vehicle, the angular speed of the vehicle, the attitude of the vehicle, and the rotational speed of each of the wheels acquired during the initial value target time range, and then causes the parameter update unit to update the first parameter and the second parameter based on the newly acquired measurement values ​​of the translational speed of the vehicle, the angular speed of the vehicle, the attitude of the vehicle, and the rotational speed of each of the wheels, until an update termination condition is satisfied for determining that fluctuations in the first parameter and the second parameter due to the update have become sufficiently small, and causes the odometry parameter calculation unit to determine the odometry parameter from the first parameter and the second parameter whose update has been completed.

[0009] The present disclosure also provides an odometry parameter calibration method for calibrating odometry parameters including a wheel-to-wheel distance and a radius of each wheel for both wheels driving a differential two-wheel vehicle, wherein the vehicle is a vehicle for which a measurement reference position and a measurement reference direction are set as references for measuring the position and attitude of the vehicle, and a running reference position and a running reference direction are set as references for the position and attitude in running control of the vehicle, and the odometry parameters are specified by the wheel-to-wheel distance and the radius of each wheel, and are determined by the angular velocity of the vehicle and the radius of each wheel. and a second parameter, which is a parameter that specifies a relationship between the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, the rotational speed of each wheel, and the first parameter, and is specified by the relative position between the measurement reference position and the traveling reference position, the relative direction between the measurement reference direction and the traveling reference direction, and the wheel-to-wheel distance, and is determined from the first parameter, which is a parameter that specifies a relationship between the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, the rotational speed of each wheel, and the first parameter, and the odometry parameter calibration method is an initial parameter calculation step of calculating an initial value of the first parameter based on the measurement values ​​of the angular velocity of the vehicle and the rotational speed of each of the wheels within an initial value target time range, and calculating an initial value of the second parameter based on the initial value of the first parameter and the measurement values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational speed of each of the wheels within the initial value target time range; a parameter update step of updating the first parameter based on the latest first parameter and additionally acquired measurement values ​​of the angular velocity of the vehicle and the rotational speed of each of the wheels, and updating the second parameter based on the latest second parameter, the updated first parameter, a change amount for the first parameter due to the update, and additionally acquired measurement values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational speed of each of the wheels; an odometry parameter calculation step of calculating the odometry parameters from the first parameter and the second parameter;and an execution control step of repeatedly acquiring the measurement values ​​of the attitude of the vehicle and the rotational speed of each of the wheels, causing the parameter initial value calculation step to determine initial values ​​of the first parameter and the second parameter based on the measurement values ​​of the translational speed of the vehicle, the angular speed of the vehicle, the attitude of the vehicle, and the rotational speed of each of the wheels acquired within the initial value target time range, and then causing the parameter update step to update the first parameter and the second parameter based on the newly acquired measurement values ​​of the translational speed of the vehicle, the angular speed of the vehicle, the attitude of the vehicle, and the rotational speed of each of the wheels, until an update termination condition is satisfied for determining that fluctuations in the first parameter and the second parameter due to the update have become sufficiently small, and causing the odometry parameter calculation step to determine the odometry parameter from the first parameter and the second parameter for which updating has been completed.

[0010] An odometry parameter calibration program according to the present disclosure is an odometry parameter calibration program for calibrating odometry parameters including an inter-wheel distance and a radius of each wheel for both wheels that drive a differential two-wheel vehicle, wherein the vehicle is a vehicle for which a measurement reference position and a measurement reference direction that serve as references in measuring the position and attitude of the vehicle, and a running reference position and a running reference direction that serve as references for the position and attitude in running control of the vehicle, are set, and the odometry parameters are parameters calculated from a first parameter that is specified by the inter-wheel distance and the radius of each wheel and is a parameter that defines a relationship between the angular velocity of the vehicle and the rotational speed of each wheel, and a second parameter that is specified by the relative position between the measurement reference position and the running reference position, the relative direction between the measurement reference direction and the running reference direction, and the inter-wheel distance and is a parameter that defines a relationship among the translational speed of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, the rotational speed of each wheel, and the first parameter; and an acquisition unit that acquires measured values ​​of the angular velocity of the vehicle and the rotational speed of each of the wheels, and the rotational speed of each of the wheels; a parameter initial value calculation unit that calculates an initial value of the first parameter based on the measured values ​​of the angular velocity of the vehicle and the rotational speed of each of the wheels within an initial value target time range, and calculates an initial value of the second parameter based on the initial value of the first parameter, and the measured values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational speed of each of the wheels within the initial value target time range; a parameter update unit that updates the first parameter based on the latest first parameter and the additionally acquired measured values ​​of the angular velocity of the vehicle and the rotational speed of each of the wheels, and updates the second parameter based on the latest second parameter, the updated first parameter, a change amount for the first parameter due to the update, and the additionally acquired measured values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational speed of each of the wheels; an odometry parameter calculation unit that calculates the odometry parameters from the first parameter and the second parameter; andand repeatedly acquiring the measurement values ​​of the attitude of the vehicle and the rotational speed of each of the wheels, and causing the parameter initial value calculation unit to determine the initial values ​​of the first parameter and the initial values ​​of the second parameter based on the measurement values ​​of the translational speed of the vehicle, the angular speed of the vehicle, the attitude of the vehicle, and the rotational speed of each of the wheels acquired during the initial value target time range, and then causing the parameter update unit to update the first parameter and the second parameter based on the newly acquired measurement values ​​of the translational speed of the vehicle, the angular speed of the vehicle, the attitude of the vehicle, and the rotational speed of each of the wheels, until an update termination condition is satisfied for determining that fluctuations in the first parameter and the second parameter due to the update have become sufficiently small, and causing the odometry parameter calculation unit to function as an execution control unit to determine the odometry parameters from the first parameter and the second parameter whose updates have been completed. [Effects of the Invention]

[0011] The odometry parameter calibration device, method, and program according to the present disclosure can provide a method for calibrating odometry parameters for a differential two-wheel vehicle by obtaining the minimum amount of measurement values ​​required for each vehicle. [Brief explanation of the drawings]

[0012] [Figure 1] 1 is a diagram showing a schematic configuration of an odometry parameter calibration system according to an embodiment of the present invention; [Figure 2] FIG. 2 is a diagram illustrating the hardware configuration of each component included in the odometry parameter calibration system. [Figure 3] FIG. 2 is a diagram for explaining odometry parameters. [Figure 4] FIG. 2 is a diagram for explaining odometry parameters. [Figure 5] FIG. 2 is a block diagram illustrating an example of a functional configuration of the odometry parameter calibration device. [Figure 6] 10 is a flowchart showing the flow of an odometry parameter calibration process. [Figure 7]10 is a flowchart showing the flow of a parameter calculation process. [Figure 8] FIG. 10 is a diagram for explaining the effect of the present embodiment. [Figure 9] FIG. 10 is a diagram for explaining Modification 2. DETAILED DESCRIPTION OF THE INVENTION

[0013] An example of an embodiment of the present disclosure will be described below with reference to the drawings. Note that the same or equivalent components and parts in each drawing are given the same reference numerals. Also, the dimensions and proportions of the drawings are exaggerated for the convenience of explanation and may differ from the actual proportions.

[0014] Fig. 1 shows a schematic configuration of an odometry parameter calibration system 100 according to this embodiment. Fig. 2 shows the hardware configuration of each component included in the odometry parameter calibration system 100. As shown in Figs. 1 and 2, the odometry parameter calibration system 100 includes an odometry parameter calibration device 10, a motion capture system 30, and a vehicle 40.

[0015] The vehicle 40 is a differential two-wheel automobile, a mobile robot, or the like. A measurement reference position and a measurement reference direction are set for the vehicle 40, which serve as references for measuring the position and attitude of the vehicle 40. A running reference position and a running reference direction are also set for the vehicle 40, which serve as references for the position and attitude in running control of the vehicle 40. In this embodiment, a marker 42 is attached to the vehicle 40, which allows the position and attitude of the vehicle 40 to be measured externally by motion capture. The measurement reference position is the position of the marker in a planar view of the vehicle 40, and the measurement reference direction is a direction that represents the attitude of the marker 42 in a planar view of the vehicle 40. The running reference position is the center position of both wheels 44 in a planar view of the vehicle 40, and the running reference direction is the forward direction perpendicular to the rotation axes of both wheels 44 in a planar view of the vehicle 40.

[0016] 2 , the vehicle 40 includes wheels 44, a motor 46 that drives the wheels 44, an encoder 48 that measures the rotation angle of the wheels 44, and a microcomputer 50. The microcomputer 50 includes a CPU (Central Processing Unit), a memory, a storage device, a communication I / F (Interface), etc. The microcomputer 50 controls the motor 46 in response to an operation command from a controller (not shown) that controls the operation of the vehicle 40. The microcomputer 50 also acquires the measurement value of the rotation angle of the wheels 44 measured by the encoder 48 and transmits it to the odometry parameter calibration device 10.

[0017] As shown in Fig. 2, the motion capture system 30 includes a camera 32 and a microcomputer 34. Although Figs. 1 and 2 show only one camera 32, the motion capture system 30 includes multiple cameras 32 for capturing images of markers 42 attached to the vehicle 40 from multiple angles. The camera 32 may be a visible light camera, an infrared camera, or any other camera capable of capturing images of the markers 42. The camera 32 captures images of the vehicle 40 moving along a predetermined travel route.

[0018] The microcomputer 34 includes a CPU, a memory, a storage device, a communication I / F, etc. The microcomputer 34 detects the markers 42 from the image captured by the camera 32, calculates the position and attitude of the vehicle 40 to which the markers 42 are attached, and transmits the calculated values ​​to the odometry parameter calibration device 10.

[0019] The odometry parameter calibration device 10 calibrates odometry parameters including the wheel-to-wheel distance and the radius of each wheel for both wheels 44 that drive a differential two-wheel vehicle 40. As shown in Fig. 2, the odometry parameter calibration device 10 has a CPU 12, a memory 14, a storage device 16, an input device 18, an output device 20, a storage medium reader 22, and a communication I / F 24. Each component is connected to each other via a bus 26 so as to be able to communicate with each other.

[0020] The storage device 16 stores an odometry parameter calibration program for executing an odometry parameter calibration process. The CPU 12 is a central processing unit that executes various programs and controls each component. That is, the CPU 12 reads the program from the storage device 16 and executes the program using the memory 14 as a work area. The CPU 12 controls each component and performs various arithmetic processes in accordance with the program stored in the storage device 16.

[0021] The memory 14 is made up of RAM (Random Access Memory) and serves as a working area to temporarily store programs and data. The storage device 16 is made up of ROM (Read Only Memory), HDD (Hard Disk Drive), SSD (Solid State Drive), etc., and stores various programs including the operating system and various data.

[0022] The input device 18 is a device for performing various inputs, such as a keyboard or a mouse. The output device 20 is a device for outputting various information, such as a display or a printer. A touch panel display may be used as the output device 20 to function as the input device 18.

[0023] The storage medium reader 22 reads data stored in various storage media such as CD (Compact Disc)-ROMs, DVD (Digital Versatile Disc)-ROMs, Blu-ray discs, and USB (Universal Serial Bus) memories, and writes data to the storage media. The communication I / F 24 is an interface for communicating with other devices, and uses standards such as Ethernet (registered trademark), FDDI, and Wi-Fi (registered trademark).

[0024] Here, the odometry parameters to be calculated in this embodiment will be described. The odometry parameters are parameters calculated from a first parameter, which is specified by the inter-wheel distance and the radius of each wheel 44 and defines the relationship between the angular velocity of the vehicle 40 and the rotational speed of each wheel, and a second parameter, which is specified by the relative position between the measurement reference position and the traveling reference position, the relative direction between the measurement reference direction and the traveling reference direction, and the inter-wheel distance and defines the relationship among the translational speed of the vehicle 40, the angular velocity of the vehicle 40, the attitude of the vehicle 40, the rotational speed of each wheel 44, and the first parameter.

[0025] The odometry parameters will be specifically described with reference to Figures 3 and 4. The left diagram of Figure 3 is a schematic diagram of a vehicle 40 viewed from above, and the right diagram of Figure 3 is a schematic diagram showing the definition of the difference between coordinate systems.

[0026] The distance between the wheels is W, and the ratio of the radius of the left and right wheels 44 to the distance between the wheels is R. r and R l As shown in the right figure of Figure 3, the marker coordinate system is (x m ,y m ), and the odometry coordinate system is (x o ,y o ), and the correlation between the marker coordinate system and the odometry coordinate system (hereinafter referred to as "coordinate relationship") is (x p ,y p ,θ p )

[0027] Also, the measured values ​​of the position and attitude of the vehicle 40 at time t are expressed as (x m (t),y m (t),θ m (t)). In the following, we will not use this value directly, but will use the value converted into the amount of change (x(t), y(t), θ(t)) from time 0 (the start of measurement), as shown in Figure 4. (x m (t),y m (t),θ m (t)) to (x(t), y(t), θ(t)) are calculated as in the following equations (1) to (3).

[0028]

number

[0029] In this embodiment, the above (R r ,R l ,W) are the odometry parameters to be calculated, and the coordinate relationship (x p ,y p ,θ p ) are calculated together. These odometry parameters are related to the following measurement values ​​by the following equations (4) and (5). Note that measurement values ​​with Δ are relative values ​​from the previous measurement value. Measurements The angle (ΔL R ,ΔL L ) Displacement of vehicle 40 (Δx, Δy) Rotation angle Δθ of the vehicle 40 The angle θ(t)(,x(t),y(t)) of the vehicle 40

[0030]

number

[0031] The purpose of calibrating the odometry parameters is to calculate the odometry parameters that minimize the errors in the following equations (6) and (7) derived from the above equations (4) and (5), that is, the odometry parameters that are most consistent with the measured values.

[0032]

number

[0033] In the following, (R r / W,R l / W) is parameter p1, (x p ,y p ,Wcosθ p ,Wsinθ p) is called parameter p2. V(t) in equation (7) is calculated by the following equations (8) to (12) using the measured values ​​Δθ, θ(t), and ΔL R , ΔL L , and parameter p1. Because V(t) depends on parameter p1, when a new measurement value is acquired, both the part that changes depending on the measurement value and the part that changes due to a change in parameter p1 must be taken into consideration.

[0034]

number

[0035] In conventional odometry parameter calibration methods, measurement values ​​are accumulated and then the odometry parameters are calculated in a batch process. Therefore, if the accumulated measurement values ​​are insufficient, the vehicle must be driven again to acquire measurement values. If the accumulated measurement values ​​are excessive, the vehicle must have been driven for an excessively long time.

[0036] In this embodiment, the odometry parameters are calculated in real time while the measurement values ​​are being acquired, and by determining whether the calibration of the odometry parameters has been completed, the odometry parameters are calibrated by acquiring the minimum necessary measurement values.

[0037] Next, the functional configuration of the odometry parameter calibration device 10 according to this embodiment will be described.

[0038] Fig. 5 is a block diagram showing an example of the functional configuration of the odometry parameter calibration device 10. As shown in Fig. 5, the odometry parameter calibration device 10 includes, as its functional configuration, an acquisition unit 62, a parameter initial value calculation unit 64, a parameter update unit 66, an odometry parameter calculation unit 68, and an execution control unit 70. Each functional configuration is realized by the CPU 12 reading out an odometry parameter calibration program stored in the storage device 16, expanding it in the memory 14, and executing it.

[0039] The acquisition unit 62 acquires measurement values ​​of the translational velocity of the vehicle 40, the angular velocity of the vehicle 40, the attitude of the vehicle 40, and the rotational velocity of each wheel 44. Specifically, the acquisition unit 62 acquires the position and attitude (x(t), y(t), θ(t)) of the marker 42 at time t, which are measurement values ​​of the motion capture system 30. The acquisition unit 62 calculates the difference between the position and attitude (x(t-1), y(t-1), θ(t-1)) of the marker 42 at time t-1, which were previously acquired, and (x(t), y(t), θ(t)). The difference calculated here becomes the movement amount (Δx, Δy) of the vehicle 40 and the rotation angle Δθ of the vehicle 40. The acquisition unit 62 also acquires the rotation angle (L R (t),L L The acquisition unit 62 acquires the rotation angle (L R (t-1),L L The difference calculated here is the angle (ΔL R ,ΔL L )

[0040] The parameter initial value calculation unit 64 calculates an initial value for parameter p1 based on the measured values ​​of the angular velocity of the vehicle 40 and the rotational speed of each wheel 44 during the initial value target time range. The parameter initial value calculation unit 64 also calculates an initial value for parameter p2 based on the initial value of parameter p1 and the measured values ​​of the translational velocity of the vehicle 40, the angular velocity of the vehicle 40, the attitude of the vehicle 40, and the rotational speed of each wheel 44 during the initial value target time range.

[0041] The initial value target time range is a time range from the start of measurement for accumulating sufficient measurement values ​​for calculating the initial values ​​of the parameters p1 and p2. The initial value target time range may be determined based on the number of measurement values ​​acquired within that time range. For example, a time range for acquiring the number of measurement values ​​necessary for calculating the initial values ​​of the parameters p1 and p2 may be determined as the initial value target time range. The number necessary for calculating the initial values ​​is a number smaller than the number of measurement values ​​necessary for completing the calibration of the odometry parameters.

[0042] The initial value target time range may be determined based on the quality of the measurement values ​​acquired during that time. For example, the initial value target time range A(t) calculated in the process of calculating the initial values ​​of parameters p1 and p2 (to be described later) may be determined based on the quality of the measurement values ​​acquired during that time range. T A(t) and D(t) T The determinant of D(t) may be calculated, and the period until the absolute value of the determinant reaches a predetermined threshold value may be set as the initial value target time range. The threshold value may be determined empirically, for example, 0.001.

[0043] Specifically, as shown in the following equation (13), (ΔL R ,ΔL L ) is defined as A(t), and Δθ is defined as b(t), and the parameter initial value calculation unit 64 calculates the parameter p1 using the following equation (14).

[0044]

number

[0045] Furthermore, as shown in the following equation (15), V(t) in equation (7) is accumulated from time 0 to time t as D(t), and (Δx, Δy) is accumulated as E(t), and the parameter initial value calculation unit 64 calculates the parameter p2 using the following equation (16).

[0046]

number

[0047] Furthermore, the parameter initial value calculation unit 64 calculates the odometry parameter (R r ,R l ,W) and coordinate relationship (x p ,y p ,θ p ) is calculated using the following equations (17) to (21).

[0048]

number

[0049] Furthermore, the parameter initial value calculation unit 64 calculates the parameter update matrices P(t) and Q(t) according to the following equations (22) and (23).

[0050]

number

[0051] The parameter update unit 66 updates the parameter p1 based on the latest parameter p1 and additionally acquired measurement values ​​of the angular velocity of the vehicle 40 and the rotational speed of each wheel 44. Specifically, the parameter update unit 66 obtains a predicted value of the angular velocity of the vehicle 40 based on the latest parameter p1 and the additionally acquired measurement values ​​of the rotational speed of each wheel 44, obtains an error between the predicted value and the additionally acquired measurement value of the angular velocity of the vehicle 40, and updates the parameter p1 by obtaining an updated parameter p1 based on the error.

[0052] Furthermore, the parameter update unit 66 updates the parameter p2 based on the latest parameter p2, the updated parameter p1, the amount of change in parameter p1 due to the update, and additionally acquired measurement values ​​of the translational velocity of the vehicle 40, the angular velocity of the vehicle 40, the attitude of the vehicle 40, and the rotational speed of each wheel 44. Specifically, the parameter p2 is updated by determining a predicted value of the translational speed of the vehicle 40 based on the updated parameter p1, the latest parameter p2, and additionally acquired measurement values ​​of the angular velocity of the vehicle 40, the attitude of the vehicle 40, and the rotational speed of each wheel 44, determining an error between the predicted value and the additionally acquired measurement value of the translational speed of the vehicle 40, and determining an updated parameter p2 based on the error and the amount of change in parameter p1 due to the update.

[0053] The update of the parameter p1 will be explained in more detail. The current measurement time is t+1 and the previous measurement time is t, and the parameter p1 at time t+1 is calculated. The value measured at time t+1 is a relative value from the previous time, and the angle a(t+1)=(ΔLR ,ΔL L ), and the rotation angle Δθ(t+1) of the vehicle 40. As a premise, the parameter p1 and the parameter correction matrix P(t) at the previous time t have been calculated.

[0054] The parameter update unit 66 updates the parameter correction matrix P(t) according to the following equation (24), and updates the parameter p1 according to the following equation (25).

[0055]

number

[0056] In equation (25), p1(t) is the parameter p1 before updating, a(t+1) T p1(t) is the predicted value using the previous parameters, (Δθ(t+1)-a(t+1) T p1(t)) is the prediction error, and P(t+1)a(t+1) is the part that converts the prediction error into parameter changes.

[0057] The update of the parameter p2 will be explained in more detail. The parameter p2 at time t+1 is calculated. The value measured at time t+1 is the angle a(t+1)=(ΔL R ,ΔL L ), the rotation angle Δθ(t+1) of the vehicle 40, the movement amount Δp(t+1)=(Δx, Δy) of the vehicle 40, and the angle θ(t+1) of the vehicle 40. As a premise, the parameter p2 and the parameter correction matrix Q(t) at the previous time t have been calculated.

[0058] The parameter update unit 66 updates the parameter correction matrix Q(t) according to the following equation (26), and updates the parameter p2 according to the following equation (27).

[0059]

number

[0060] In equation (26), ΔQ(t) is the change due to the change in parameter p1, and V(t+1)V(t+1) T is the change due to the new measurement. Also, p2(t) in equation (27) is the parameter p2 before updating, V(t+1) T p2(t) is the predicted value using the previous parameters, (Δp(t+1)-V(t+1) T p2(t)) is the prediction error, Q(t+1)V(t+1) is the part that converts the prediction error into a change in the parameter, and Q(t+1)ΔQ(t)p2(t) is the part that converts the change in the motion model of the vehicle 40 due to a change in parameter p1 in equation (26) into a change in the parameter. ΔQ is expressed by the following equation (28).

[0061]

number

[0062] The odometry parameter calculation unit 68 calculates the odometry parameter (R r ,R l ,W) and coordinate relationship (x p ,y p ,θ p Specifically, the odometry parameter calculation unit 68 calculates (R r ,R l ,W) and (x p ,y p ,θ p ) Calculate.

[0063] As the vehicle 40 travels, the execution control unit 70 repeatedly causes the acquisition unit 62 to acquire measured values ​​of the translational velocity of the vehicle 40, the angular velocity of the vehicle 40, the attitude of the vehicle 40, and the rotational speed of each wheel 44. The execution control unit 70 also causes the parameter initial value calculation unit 64 to calculate initial values ​​of the parameters p1 and p2 based on the measured values ​​of the translational velocity of the vehicle 40, the angular velocity of the vehicle 40, the attitude of the vehicle 40, and the rotational speed of each wheel 44 acquired during the initial value target time range. Thereafter, the execution control unit 70 causes the parameter update unit 66 to update the parameters p1 and p2 based on the newly acquired measured values ​​of the translational velocity of the vehicle 40, the angular velocity of the vehicle 40, the attitude of the vehicle 40, and the rotational speed of each wheel 44, until an update termination condition is met, which determines whether fluctuations in the parameters p1 and p2 due to the update have become sufficiently small. The execution control unit 70 also causes the odometry parameter calculation unit 68 to calculate odometry parameters from the updated parameters p1 and p2.

[0064] The update termination condition may be, for example, when the fluctuation of the parameter over a predetermined time period in the past (e.g., 5 seconds) is within a predetermined percentage (e.g., 1%) of the absolute value of the parameter. Alternatively, the update termination condition may be when the trajectory of the vehicle 40 over a predetermined time period in the past (e.g., 5 seconds) is calculated using the current parameters and the difference from the actual measured value is within a predetermined range (e.g., within 5 cm).

[0065] The execution control unit 70 calculates the odometry parameter (R r ,R l ,W) and coordinate relationship (x p ,y p ,θ p ) is output. The execution control unit 70 may display changes in the parameters and the state of adaptation of the data on a display while the calibration process of the odometry parameters is being executed. This allows the changes in the parameters to be confirmed in real time. The execution control unit 70 may also notify the user by voice when the calibration of the odometry parameters is completed.

[0066] Next, the operation of the odometry parameter calibration system 100 according to this embodiment will be described.

[0067] The vehicle 40 starts moving along a predetermined movement path, and the motion capture system 30 starts capturing the motion of the markers 42 attached to the vehicle 40. Then, the odometry parameter calibration device 10 starts executing an odometry parameter calibration process. FIG. 6 is a flowchart showing the flow of the odometry parameter calibration process executed by the CPU 12 of the odometry parameter calibration device 10. The CPU 12 reads out an odometry parameter calibration program from the storage device 16, loads it into the memory 14, and executes it, whereby the CPU 12 functions as each functional component of the odometry parameter calibration device 10, and the odometry parameter calibration process shown in FIG. 6 is executed.

[0068] In step S10, the acquisition unit 62 acquires the position and orientation of the marker 42 at time t, which are measurement values ​​of the motion capture system 30. Then, the acquisition unit 62 acquires the difference between the previously acquired position and orientation of the vehicle 40 at time t−1 and the currently acquired position and orientation of the vehicle 40 at time t as the movement amount (Δx, Δy) of the vehicle 40 and the rotation angle Δθ of the vehicle 40.

[0069] Next, in step S20, the acquisition unit 62 acquires the rotation angle of the wheels 44 at time t, which is a measurement value of the encoder 48 of the vehicle 40. Then, the acquisition unit 62 calculates the difference between the previously acquired rotation angle of the wheels 44 at time t−1 and the currently acquired rotation angle of the wheels 44 at time t as the rotation angle (ΔL R ,ΔL L ) is obtained.

[0070] Next, in step S30, a parameter calculation process is executed. Here, the parameter calculation process will be described with reference to FIG.

[0071] In step S32, the parameter initial value calculation unit 64 determines whether the initial values ​​of the parameters have been calculated. If they have been calculated, the process proceeds to step S40, and if they have not been calculated, the process proceeds to step S34. In step S34, the parameter initial value calculation unit 64 determines whether sufficient measurement values ​​have been accumulated to calculate the initial values ​​of the parameters. If they have been accumulated, the process proceeds to step S38, and if they have not been accumulated, the process proceeds to step S36.

[0072] In step S36, the parameter initial value calculation unit 64 accumulates the measurement values ​​acquired in steps S10 and S20 in a predetermined storage area and returns to the odometry parameter calibration process (FIG. 6). In step S38, the parameter initial value calculation unit 64 calculates the initial values ​​of the parameters using equations (13) to (23) and returns to the odometry parameter calibration process (FIG. 6).

[0073] In step S40, the parameter update unit 66 updates the parameter correction matrix P(t) using equation (24) and updates the parameter p1 using equation (25). Next, in step S42, the parameter update unit 66 updates the parameter correction matrix Q(t) using equation (26) and updates the parameter p2 using equation (27). Next, in step S44, the odometry parameter calculation unit 68 calculates the odometry parameter (R r ,R l ,W) and coordinate relationship (x p ,y p ,θ p ) and returns to the odometry parameter calibration process (FIG. 6). Note that step S44 may not be performed every time steps S40 and S42 are performed, but may be performed only when an update termination condition is met in step S50, which will be described later.

[0074] Next, in step S50, the execution control unit 70 determines whether or not an update termination condition is met for determining whether the fluctuations in the parameters p1 and p2 due to the update have become sufficiently small. If the update termination condition is met, the process proceeds to step S60, and if the update termination condition is not met, the process returns to step S10. In step S60, the execution control unit 70 determines whether or not the calculated odometry parameters (R r ,R l ,W) and coordinate relationship (x p ,y p ,θ p ) and the odometry parameter calibration process is completed.

[0075] The output odometry parameters and coordinate relationships are used to estimate the self-position of the vehicle 40. Specifically, the odometry parameters (R r ,R l ,W), coordinate relationship (x p ,y p ,θ p ), the rotation angle of the left and right wheels 44 at a past time (ΔL R (t),ΔL L (t)), the position and orientation (x m (0),y m (0),θ m (0)), and the measurement period (time interval between data) Δt of the measurement value, the position and attitude (x m (t),y m (t),θ m Calculate (t).

[0076] More specifically, Δθ(t) is expressed by the following equation (29), and the movement amount (x o (t),y o (t),θ o (t)) (t=1, 2, ) are calculated in order according to the recurrence formulas shown in the following formulas (30) to (32). o (0),y o (0),θ o (0)) are all 0.

[0077]

number

[0078] And the estimated trajectory (x m (t),y m (t),θ m (t) (t=0, 1, 2, . . . ) is calculated using the following equations (33) and (34).

[0079]

number

[0080] The second term on the right side of equation (33) is the part where the movement changes due to the deviation between the marker coordinate system and the odometry coordinate system, and the third term is the part that converts the movement amount of the vehicle 40 from the odometry coordinate system to the marker coordinate system.

[0081] As described above, in the odometry parameter calibration system according to this embodiment, the odometry parameter calibration device repeatedly causes the acquisition unit to acquire measurement values ​​of the vehicle translational velocity, vehicle angular velocity, vehicle attitude, and rotational speed of each wheel as the vehicle travels, causes the parameter initial value calculation unit to calculate initial values ​​of the first parameter and the second parameter based on the measurement values ​​of the vehicle translational velocity, vehicle angular velocity, vehicle attitude, and rotational speed of each wheel acquired within the initial value target time range, and then causes the parameter update unit to update the first parameter and the second parameter based on the newly acquired measurement values ​​of the vehicle translational velocity, vehicle angular velocity, vehicle attitude, and rotational speed of each wheel, until an update termination condition is met for determining that fluctuations in the first parameter and the second parameter due to the update have become sufficiently small, and causes the odometry parameter calculation unit to calculate odometry parameters from the updated first parameter and the second parameter. This makes it possible to calibrate odometry parameters for a differential two-wheel vehicle by acquiring the minimum number of measurement values ​​required for each vehicle.

[0082] In a conventional odometry parameter calibration method, a vehicle is driven, measurement values ​​are acquired, and stored in storage, and the experiment (driving and collection of measurement values) is stopped at an appropriate point. Then, the measurement values ​​stored in storage are used to calculate odometry parameters. Then, a determination is made as to whether the update has ended. If the end has not been determined, another experiment must be conducted to collect measurement values. On the other hand, in this embodiment, as shown in FIG. 8, the odometry parameters are calculated while acquiring measurement values ​​until the parameters converge. This makes it possible to minimize the number of measurement values ​​acquired to calibrate the odometry parameters so that the estimated trajectory of the vehicle estimated using the odometry parameters matches the measured trajectory.

[0083] <Variation 1> In the above embodiment, the coordinate relationship (x p ,y p ,θ p ) is unknown, but in variant 1, the coordinate relationship (x p ,y p ,θ p For example, if the marker 42 can be attached to the vehicle 40 as designed, the coordinate relationship (x p ,y p ,θ p ) is known.

[0084] In the first modification, the right side of the equation (5) is assigned a known value (x p ,y p ,θ p ) and move everything except W to the left side. In Modification 1, equation (5) becomes equation (35) below. K(t) is given by equation (36) below. In Modification 1, W is the parameter p2 to be calculated in the above embodiment.

[0085]

number

[0086] In equation (35), the accumulation of K(t) from time 0 to time t is Ks (t), d(t) are stacked together to form d s If (t) is set, the initial value of W is calculated using the following equation (37).

[0087]

number

[0088] Moreover, in the first modification, the parameter correction matrix Q(t) is updated by the following equation (38), and the parameter W(t) is updated by the following equation (39).

[0089]

number

[0090] In equation (38), ΔQ(t) is the change due to the change in parameter p1, and K(t+1) T K(t+1) -1 is the change due to the new measurement. In addition, in equation (39), W(t) is the parameter W before the update, K(t+1)W(t) is the predicted value using the previous parameters, d(t+1)-K(t+1)W(t) is the prediction error, and Q(t+1)K(t+1) T is the part that converts the prediction error into a change in the parameter, and Q(t+1)ΔQ(t)W(t) is the part that converts the change in the motion model of the vehicle 40 due to a change in parameter p1 in equation (38) into a change in the parameter. Note that ΔQ(t) is expressed by the following equation (40). The initial values ​​F(0) and G(0) are set to 0.

[0091]

number

[0092] <Variation 2> In the above embodiment, a case was described in which measured values ​​of the position and attitude of the vehicle 40 are obtained by motion capture, but in variant example 2, a sensor is fixed to the vehicle 40 to detect the situation regarding the placement of objects present around the vehicle 40, placement information indicating the placement of the objects, and a configuration is provided for determining the position and attitude of the sensor from the detection results and placement information of the sensor.

[0093] 9, the vehicle 40 is equipped with a camera 52 as a sensor, and detects an AR marker 43 by photographing it with the camera 52, and calculates the relative position and orientation between the AR marker 43 and the camera 52. Then, the position and orientation of the camera 52 are acquired based on the calculated relative position and relative orientation and the placement information indicating the position and orientation of the AR marker 43. In this case, the measurement reference position is the position of the sensor (camera 52) in a planar view of the vehicle 40, and the measurement reference direction is a direction representing the orientation of the sensor in a planar view of the vehicle 40.

[0094] Furthermore, the processing performed by the CPU after reading the software (program) in the above-described embodiments may be performed by various processors other than the CPU. Examples of such processors include programmable logic devices (PLDs) whose circuit configuration can be changed after fabrication, such as field-programmable gate arrays (FPGAs), and dedicated electrical circuits, such as application-specific integrated circuits (ASICs), which are processors with circuit configurations specifically designed to perform specific processing. The processing may be performed by one of these various processors, or by a combination of two or more processors of the same or different types (e.g., multiple FPGAs, or a combination of a CPU and an FPGA). The hardware structure of these various processors is, more specifically, an electrical circuit that combines circuit elements such as semiconductor devices.

[0095] In the above embodiment, the program is pre-stored (installed) in a storage device, but the present invention is not limited to this. The program may be provided in a form stored in a storage medium such as a CD-ROM, a DVD-ROM, a Blu-ray disc, or a USB memory. The program may also be downloaded from an external device via a network. [Explanation of symbols]

[0096] 10 Odometry parameter calibration device 12 CPU 14 Memory 16 Storage device 18 Input Devices 20 Output Devices 22 Storage medium reader 24 Communication I / F 26 Bus 30 Motion Capture System 32 Camera 34 Microcomputer 40 vehicles 42 Marker 43 AR Marker 44 wheels 46 Motor 48 Encoder 50 microcomputers 52 Camera 62 Acquisition Department 64 Parameter initial value calculation section 66 Parameter Update Section 68 Odometry parameter calculation unit 70 Execution control section 100 Odometry Parameter Calibration System

Claims

1. An odometry parameter calibration device for calibrating odometry parameters including a wheel-to-wheel distance and a wheel radius for both wheels driving a differential two-wheel vehicle, the vehicle is a vehicle for which a measurement reference position and a measurement reference direction serving as references for measuring the position and attitude of the vehicle, and a running reference position and a running reference direction serving as references for the position and attitude in running control of the vehicle, are set; the odometry parameters are parameters calculated from a first parameter, which is specified by the inter-wheel distance and the radius of each wheel and defines a relationship between the angular velocity of the vehicle and the rotational speed of each wheel, and a second parameter, which is specified by the relative position between the measurement reference position and the traveling reference position, the relative direction between the measurement reference direction and the traveling reference direction, and the inter-wheel distance, and defines a relationship among the translational speed of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, the rotational speed of each wheel, and the first parameter; The odometry parameter calibration device an acquisition unit that acquires measurement values ​​of a translational velocity of the vehicle, an angular velocity of the vehicle, an attitude of the vehicle, and a rotational velocity of each of the wheels; a parameter initial value calculation unit that calculates an initial value of the first parameter based on measurement values ​​of the angular velocity of the vehicle and the rotational velocity of each of the wheels within an initial value target time range, and calculates an initial value of the second parameter based on the initial value of the first parameter and measurement values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational velocity of each of the wheels within the initial value target time range; a parameter update unit that updates the first parameter based on the latest first parameter and additionally acquired measurement values ​​of the angular velocity of the vehicle and the rotational speed of each of the wheels, and updates the second parameter based on the latest second parameter, the updated first parameter, a change amount of the first parameter due to the update, and additionally acquired measurement values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational speed of each of the wheels; an odometry parameter calculation unit that calculates the odometry parameter from the first parameter and the second parameter; an execution control unit that repeatedly causes the acquisition unit to acquire measurement values ​​of a translational velocity of the vehicle, an angular velocity of the vehicle, an attitude of the vehicle, and a rotational velocity of each of the wheels as the vehicle travels, causes the parameter initial value calculation unit to determine initial values ​​of the first parameter and the second parameter based on the measurement values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational velocity of each of the wheels acquired in the initial value target time range, and then causes the parameter update unit to update the first parameter and the second parameter based on the newly acquired measurement values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational velocity of each of the wheels, until an update termination condition is satisfied for determining that fluctuations in the first parameter and the second parameter due to the update have become sufficiently small, and causes the odometry parameter calculation unit to determine the odometry parameter from the first parameter and the second parameter whose update has been completed. Odometry parameter calibration device.

2. the acquisition unit includes a marker fixed to the vehicle and whose position and orientation can be measured externally, and a means for measuring the position and orientation of the marker; the measurement reference position is a position of the marker in a plan view of the vehicle, the measurement reference direction is a direction representing the attitude of the marker in a planar view of the vehicle, the traveling reference position is a center position between the two wheels in a plan view of the vehicle, The traveling reference direction is a forward direction perpendicular to the rotation axes of the two wheels in a plan view of the vehicle. The odometry parameter calibration device of claim 1 .

3. the acquisition unit includes a sensor fixed to the vehicle that detects a situation related to the placement of objects present around the vehicle, placement information that indicates the placement of the objects, and means for determining a position and attitude of the sensor from the detection result of the sensor and the placement information; the measurement reference position is a position of the sensor in a plan view of the vehicle, the measurement reference direction is a direction that represents the attitude of the sensor in a plan view of the vehicle, the traveling reference position is a center position between the two wheels in a plan view of the vehicle, The traveling reference direction is a forward direction perpendicular to the rotation axes of the two wheels in a plan view of the vehicle. The odometry parameter calibration device of claim 1 .

4. The parameter update unit updates the first parameter by calculating a predicted value of the angular velocity of the vehicle based on the latest first parameter and an additionally acquired measurement value of the rotational speed of each wheel, calculating an error between the predicted value and the additionally acquired measurement value of the angular velocity of the vehicle, and calculating an updated first parameter based on the error. The odometry parameter calibration device according to any one of claims 1 to 3.

5. The parameter update unit updates the second parameter by calculating a predicted value of the translational velocity of the vehicle based on the updated first parameter, the latest second parameter, and additionally acquired measured values ​​of the angular velocity of the vehicle, the attitude of the vehicle, and the rotational velocity of each wheel, calculating an error between the predicted value and the additionally acquired measured value of the translational velocity of the vehicle, and calculating an updated second parameter based on the error and an amount of change in the first parameter due to the update. The odometry parameter calibration device according to claim 4 .

6. An odometry parameter calibration method for calibrating odometry parameters including a wheel-to-wheel distance and a radius of each wheel for both wheels driving a differential two-wheel vehicle, comprising: the vehicle is a vehicle for which a measurement reference position and a measurement reference direction serving as references for measuring the position and attitude of the vehicle, and a running reference position and a running reference direction serving as references for the position and attitude in running control of the vehicle, are set; the odometry parameters are parameters calculated from a first parameter, which is specified by the inter-wheel distance and the radius of each wheel and defines a relationship between the angular velocity of the vehicle and the rotational speed of each wheel, and a second parameter, which is specified by the relative position between the measurement reference position and the traveling reference position, the relative direction between the measurement reference direction and the traveling reference direction, and the inter-wheel distance, and defines a relationship among the translational speed of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, the rotational speed of each wheel, and the first parameter; The odometry parameter calibration method includes: an acquisition step of acquiring measurement values ​​of a translational velocity of the vehicle, an angular velocity of the vehicle, an attitude of the vehicle, and a rotational velocity of each of the wheels; a parameter initial value calculation step of calculating an initial value of the first parameter based on measured values ​​of the angular velocity of the vehicle and the rotational speed of each of the wheels within an initial value target time range, and calculating an initial value of the second parameter based on the initial value of the first parameter and measured values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational speed of each of the wheels within the initial value target time range; a parameter updating step of updating the first parameter based on the latest first parameter and additionally acquired measurement values ​​of the angular velocity of the vehicle and the rotational speed of each of the wheels, and updating the second parameter based on the latest second parameter, the updated first parameter, a change amount of the first parameter due to the update, and additionally acquired measurement values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational speed of each of the wheels; an odometry parameter calculation step of calculating the odometry parameter from the first parameter and the second parameter; and an execution control step of repeatedly causing the acquisition step to acquire measurement values ​​of a translational velocity of the vehicle, an angular velocity of the vehicle, an attitude of the vehicle, and a rotational velocity of each of the wheels as the vehicle travels, causing the parameter initial value calculation step to determine initial values ​​of the first parameter and the second parameter based on the measurement values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational velocity of each of the wheels acquired in the initial value target time range, and then causing the parameter update step to update the first parameter and the second parameter based on the newly acquired measurement values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational velocity of each of the wheels, until an update termination condition is satisfied for determining that fluctuations in the first parameter and the second parameter due to the update have become sufficiently small, and causing the odometry parameter calculation step to determine the odometry parameter from the first parameter and the second parameter for which updating has been completed. Odometry parameter calibration method.

7. An odometry parameter calibration program for calibrating odometry parameters including a wheel-to-wheel distance and a radius of each wheel for both wheels driving a differential two-wheel vehicle, the vehicle is a vehicle for which a measurement reference position and a measurement reference direction serving as references for measuring the position and attitude of the vehicle, and a running reference position and a running reference direction serving as references for the position and attitude in running control of the vehicle, are set; the odometry parameters are parameters calculated from a first parameter, which is specified by the inter-wheel distance and the radius of each wheel and defines a relationship between the angular velocity of the vehicle and the rotational speed of each wheel, and a second parameter, which is specified by the relative position between the measurement reference position and the traveling reference position, the relative direction between the measurement reference direction and the traveling reference direction, and the inter-wheel distance, and defines a relationship among the translational speed of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, the rotational speed of each wheel, and the first parameter; Computer, an acquisition unit that acquires measurement values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational velocity of each of the wheels; a parameter initial value calculation unit that calculates an initial value of the first parameter based on measurement values ​​of the angular velocity of the vehicle and the rotational speed of each of the wheels within an initial value target time range, and calculates an initial value of the second parameter based on the initial value of the first parameter, and measurement values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational speed of each of the wheels within the initial value target time range; a parameter update unit that updates the first parameter based on the latest first parameter and additionally acquired measurement values ​​of the angular velocity of the vehicle and the rotational speed of each of the wheels, and updates the second parameter based on the latest second parameter, the updated first parameter, a change amount of the first parameter due to the update, and additionally acquired measurement values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational speed of each of the wheels; an odometry parameter calculation unit that calculates the odometry parameter from the first parameter and the second parameter; and and causing the acquisition unit to repeatedly acquire measurement values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational velocity of each of the wheels as the vehicle travels, causing the parameter initial value calculation unit to determine initial values ​​of the first parameter and the second parameter based on the measurement values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational velocity of each of the wheels acquired within the initial value target time range, and then causing the parameter update unit to update the first parameter and the second parameter based on the newly acquired measurement values ​​of the translational velocity of the vehicle, the angular velocity of the vehicle, the attitude of the vehicle, and the rotational velocity of each of the wheels, until an update termination condition is satisfied for determining that fluctuations in the first parameter and the second parameter due to the update have become sufficiently small, and causing the odometry parameter calculation unit to function as an execution control unit that causes the odometry parameter calculation unit to determine the odometry parameters from the first parameter and the second parameter whose updates have been completed. Odometry parameter calibration program.

Images