Autonomous mobile

Abstract

To make the first self-location / position presumed by an autonomous moving body coincident with a predetermined location / position.SOLUTION: An autonomous moving body includes an initial-particle generating section 7 that makes a location of a plurality of initial particles as one added with a random number to a predetermined location and sets the location of the plurality of initial particles such that an arithmetic average in location of the plurality of initial particles is coincident with the predetermined location. The initial-particle generating section 7 also generates a plurality of initial-particle pairs from the plurality of initial particles, makes, for each initial-particle pair, the position of two initial particles belonging to the initial-particle pair as one added with a random number to the predetermined position and sets the position of the two initial particles such that an arithmetic average in position of the two initial particles is coincident with a predetermined position. According to the above configuration, the first self-location / position presumed by the autonomous moving body can be made coincident with the predetermined location / position.SELECTED DRAWING: Figure 2

Description

Technical Field

[0004] , ,

[0006] , , ,

[0005] , , ,

[0001] The present invention relates to an autonomous mobile body.

Background Art

[0002] Patent Document 1 discloses a technique for estimating its own position using a particle filter. Specifically, it is stated that the estimation error of the own position is reduced by actively removing particles outside the infrared irradiation range.

Prior Art Documents

Patent Documents

[0003]

Patent Document 1

Summary of the Invention

Problems to be Solved by the Invention

[0004] By the way, when the operator starts the movement of the autonomous mobile body, the operator adjusts the position and orientation of the autonomous mobile body by moving or rotating the autonomous mobile body so that the position and orientation of the autonomous mobile body become a predetermined position and orientation. Here, as the adjustment accuracy, typically, in terms of position, it is less than 1 centimeter, and in terms of orientation, it is less than 3 degrees.

[0005] On the other hand, when using a particle filter for estimating the self-position and orientation of an autonomous mobile body, the autonomous mobile body estimates its self-position and orientation while repeating prediction of particles in the next step, likelihood calculation for each predicted particle, and resampling based on the likelihood in this order. Here, the autonomous mobile body needs to generate a plurality of initial particles for the first step. For the position and orientation of the plurality of initial particles, those with random noise added to the above-mentioned predetermined position and orientation are adopted. Then, the autonomous mobile body estimates the first self-position and orientation by calculating the arithmetic mean of the position and orientation of the plurality of initial particles.

[0006] Therefore, even if the operator adjusts the position and orientation of the autonomous mobile object with high precision, the initial self-position and orientation estimated by the autonomous mobile object will deviate slightly from the actual position and orientation after the operator's adjustments. This deviation is particularly noticeable when the variance is set high when the random noise follows a normal distribution, and when the number of particles is small.

[0007] The object of the present invention is to provide a technique for matching the initial self-position and orientation estimated by an autonomous mobile body to a predetermined position and orientation. [Means for solving the problem]

[0008] According to this disclosure, A self-position and orientation estimation unit that estimates its own position and orientation using a particle filter, An initial particle generation unit that generates multiple initial particles, Includes, The position and posture were adjusted so that they were in a predetermined position and posture. It is an autonomous mobile vehicle, The initial particle generation unit is, The positions of the plurality of initial particles are set to have noise added to the predetermined position, and the positions of the plurality of initial particles are set such that the arithmetic mean of the positions of the plurality of initial particles coincides with the predetermined position. Multiple pairs of initial particles are generated from the aforementioned multiple initial particles, and for each pair of initial particles, the poses of the two initial particles belonging to that pair are set to have noise added to a predetermined pose, and the poses of the two initial particles are set so that the arithmetic mean of the poses of the two initial particles matches the predetermined pose. An autonomous mobile device will be provided. [Effects of the Invention]

[0009] According to this disclosure, the initial self-position and orientation estimated by the autonomous mobile body can be made to match a predetermined position and orientation. [Brief explanation of the drawing]

[0010] [Figure 1] This is a perspective view of an autonomous mobile robot. [Figure 2] This is the initial particle generation flow. [Figure 3] This is an explanatory diagram of particle orientation. [Modes for carrying out the invention]

[0011] Figure 1 shows a perspective view of the autonomous mobile robot 1. As shown in Figure 1, the autonomous mobile robot 1 includes a robot body 2, two drive wheels 3, two driven wheels 4, a two-dimensional LiDAR device 5, and a control unit 6.

[0012] The two drive wheels 3 and two driven wheels 4 are rotatably positioned on the underside of the robot body 2. The control unit 6 controls a motor (not shown) to rotate the two drive wheels 3 individually. This allows the autonomous mobile robot 1 to move forward, backward, and turn.

[0013] The two-dimensional LiDAR device 5 is mounted on the side of the robot body 2 and generates point cloud data of the surrounding environment of the autonomous mobile robot 1 by emitting laser light in a horizontal plane and receiving the reflected light.

[0014] The control unit 6 includes a CPU 6a (Central Processing Unit) as a central processing unit, read-write RAM 6b (Random Access Memory), read-only ROM 6c (Read Only Memory), and read-write SSD 6d (Solid State Drive). The CPU 6a reads and executes the control program stored in ROM 6c and SSD 6d, and the control program causes the hardware, including the CPU 6a, to function as the initial particle generation unit 7, the self-position and orientation estimation unit 8, and the autonomous movement control unit 9.

[0015] The self-position and orientation estimation unit 8 estimates the self-position and orientation using a particle filter. The estimation of the self-position and orientation using the particle filter performs self-position and orientation estimation while repeating the prediction of particles in the next step, the likelihood calculation for each predicted particle, and resampling based on the likelihood in this order of description.

[0016] The particles are position and orientation data including position data and orientation data. The position data is XY coordinate data in the service environment of the autonomously moving robot 1. The position data includes X coordinate data and Y coordinate data. The orientation data is the azimuth data of the autonomously moving robot 1. The self-position and orientation estimation unit 8 estimates the position of the autonomously moving robot 1 by arithmetically averaging the position data of a plurality of particles for each step. Similarly, the self-position and orientation estimation unit 8 estimates the orientation of the autonomously moving robot 1 by arithmetically averaging the orientation data of a plurality of particles for each step.

[0017] The initial particle generation unit 7 generates a plurality of initial particles for the first step.

[0018] The autonomous movement control unit 9 controls the autonomous movement of the autonomously moving robot 1 based on the self-position and orientation estimated by the self-position and orientation estimation unit 8 and the map data stored in the SSD6d.

[0019] Next, the initial particle generation unit 7 will be described in detail while referring to FIGS. 2 and 3. FIG. 2 is an initial particle generation flow. FIG. 3 is an explanatory diagram of the orientation. For convenience of explanation, the number of a plurality of initial particles generated by the initial particle generation unit 7 is set to N.

[0020] As described above, when the autonomous mobile robot 1 starts moving, the operator adjusts the position and orientation of the autonomous mobile robot 1 by moving or rotating the autonomous mobile robot 1 so that the position and orientation of the autonomous mobile robot 1 become a predetermined position and orientation. Here, as the adjustment accuracy, typically, with respect to the position, it is less than 1 centimeter, and with respect to the orientation, it is less than 3 degrees. The predetermined position and orientation include a predetermined position and a predetermined orientation. The predetermined position consists of a predetermined X coordinate and a predetermined Y coordinate. The above-described initial particle generation flow may be executed after the operator adjusts the position and orientation of the autonomous mobile robot 1, or may be executed before the operator adjusts the position and orientation of the autonomous mobile robot 1 if the predetermined position and orientation are predetermined in advance.

[0021] First, the initial particle generation unit 7 sets the positions of a plurality of initial particles (S100). Specifically, the initial particle generation unit 7 generates a random number, and sets the X coordinate and Y coordinate of the first initial particle as the sum of the predetermined X coordinate and the predetermined Y coordinate and the random number. Next, the initial particle generation unit 7 newly generates a random number, and sets the X coordinate and Y coordinate of the second initial particle as the sum of the predetermined X coordinate and the predetermined Y coordinate and the random number. The initial particle generation unit 7 further repeats the above-described position setting for the third to (N - 1)th initial particles. Next, the initial particle generation unit 7 sets the X coordinate of the Nth initial particle based on the following formula (1).

[0022]

Equation

[0023] In the above formula (1), X(N) is the X coordinate of the Nth initial particle, X(i) is the X coordinate of the i-th initial particle, and X0 is the predetermined X coordinate. According to the above formula (1), as shown in the following formula (2), the arithmetic mean of the X coordinates of a plurality of initial particles will coincide with the predetermined X coordinate.

[0024]

Equation

[0025] Set the Y coordinates of multiple initial particles in the same way.

[0026] Next, the initial particle generation unit 7 sets the orientation of multiple initial particles (S100-S220).

[0027] Please refer to Figure 3. In Figure 3, t represents the orientation. All t values ​​are normalized to be within the range of -π to +π. For example, if t4 is 160 and t6 is -160, the simple arithmetic mean of t4 and t6 is (t4+t6) / 2, which is zero. However, visually speaking, the average of t4 and t6 should ideally be t5. Therefore, when arithmetic meaning the orientations of multiple initial particles, the orientations of each initial particle are converted into unit vectors [sin(t), cos(t)] on the unit circle, and the sum of the multiple unit vectors after the changes is inversely transformed to perform the arithmetic mean of the orientations as shown in equation (3) below. In equation (3) below, ti represents the orientation of the i-th initial particle, and tave represents the arithmetic mean orientation. In this embodiment, the initial particle generation unit 7 sets the orientation of multiple initial particles such that tave in equation (3) below matches a predetermined orientation t0.

[0028]

number

[0029] In the following steps, the initial particle generation unit 7 sets the orientation of multiple initial particles by generating multiple pairs of initial particles from the multiple initial particles. For each pair of initial particles, it sets the orientation of the two initial particles belonging to that pair by adding noise to a predetermined orientation, and sets the orientation of the two initial particles so that the arithmetic mean of the orientations of the two initial particles matches the predetermined orientation. In other words, the initial particle generation unit 7 sets the orientation for each pair of initial particles in steps S140 to S210 in Figure 2.

[0030] First, the initial particle generation unit 7 determines whether the number N of the multiple initial particles is odd (S110). If the determination result is YES (S110:YES), the Nth pose is set to a predetermined pose (S120). If the determination result is NO (S110:NO), the process proceeds to step S130. Next, the initial particle generation unit 7 sets the variable i to 1 (S130).

[0031] Next, the initial particle generation unit 7 generates random numbers within the range of -π to +π (S140). The random numbers may be uniform random numbers or normally distributed random numbers.

[0032] Next, the initial particle generation unit 7 determines whether the absolute value of the random number generated in step S140 is less than π / 2 (S150). If the determination result is YES (S150:YES), the process proceeds to S160; otherwise, the process proceeds to S190.

[0033] In step S160, the initial particle generation unit 7 assumes that the pose of the i-th initial particle is the predetermined pose t0 plus the random number generated in step S140. In step S170, the initial particle generation unit 7 assumes that the pose of the (i+1)-th initial particle is the predetermined pose t0 minus the random number generated in step S140. Refer to Figure 3 here. t0 is the predetermined pose, t1 is the pose of the i-th initial particle, and t9 is the pose of the (i+1)-th initial particle. According to steps S160 and S170, when the arithmetic mean of the pose of the i-th initial particle and the pose of the (i+1)-th initial particle is taken, the added random numbers cancel each other out, so the arithmetic mean can be made to match the predetermined pose t0. Returning to Figure 2, the initial particle generation unit 7 increments the variable i twice (S180) and proceeds to S220.

[0034] If the absolute value of the random number generated in step S140 is π / 2 or greater, then the following problem occurs when steps S160 and S170 are executed. That is, in Figure 3, the orientation of the i-th initial particle is t3, and the orientation of the (i+1)-th initial particle is t7. In this case, the arithmetic mean of t3 and t7 should ideally be t0, but according to equation (3) above, it becomes t5. Therefore, in step S190, the initial particle generation unit 7 sets the orientation of the i-th initial particle by adding the random number generated in step S140 to the predetermined orientation t0, while in step S200, the initial particle generation unit 7 sets the orientation of the (i+1)-th initial particle by adding π to the orientation of the i-th initial particle. In Figure 3, the orientation of the i-th initial particle is t3, and the orientation of the (i+1)-th initial particle is t8. According to steps S190 and S200, the orientation of the i-th initial particle and the orientation of the (i+1)-th initial particle are assumed not to cancel each other out on the right-hand side of equation (3) above. Returning to Figure 2, the initial particle generation unit 7 increments the variable i twice (S210) and proceeds to S220.

[0035] In step S220, the initial particle generation unit 7 determines whether the orientation settings for all initial particles have been completed (S220). If the determination result is YES (S220:YES), the process ends. If the determination result is NO (S220:NO), the process returns to step S140.

[0036] The above embodiment has the following features. The autonomous mobile robot 1 (autonomous mobile unit) includes a self-position and orientation estimation unit 8 that estimates its own position and orientation using a particle filter, and an initial particle generation unit 7 that generates a plurality of initial particles. The position and orientation of the autonomous mobile robot 1 are adjusted to reach a predetermined position and orientation. The initial particle generation unit 7 adds random numbers (noise) to the predetermined positions of multiple initial particles, and sets the positions of the multiple initial particles so that the arithmetic mean of the positions of the multiple initial particles coincides with the predetermined position. The initial particle generation unit 7 generates multiple pairs of initial particles from multiple initial particles, and for each pair of initial particles, it sets the poses of the two initial particles belonging to that pair to a predetermined pose plus random numbers (noise), and sets the poses of the two initial particles so that the arithmetic mean of the poses of the two initial particles matches the predetermined pose. With this configuration, the initial self-position pose estimated by the autonomous moving object can be made to match the predetermined position pose.

[0037] In this embodiment, the initial particle generation unit 7 generates multiple pairs of initial particles from multiple initial particles and sets the orientations of two initial particles for each pair. This is because the orientations are normalized to be within the range of -π to +π. For example, if there are three initial particles, the predetermined orientation is 0, and the first and second orientations are 179, no matter what value is set for the third orientation, the arithmetic mean of the orientations of these three initial particles cannot be made to match the predetermined orientation. This is because the third orientation is also normalized to be within the range of -π to +π and cannot be set to -358. In contrast, in this embodiment, the orientations of two initial particles are set for each pair of initial particles. By setting the orientations of two initial particles in this way, the arithmetic mean of the orientations of all the initial particles can be reliably set to the predetermined orientation. [Explanation of symbols]

[0038] 1. Autonomous Mobile Robot 7. Initial Particle Generation Unit 8 Self-position and orientation estimation unit

Claims

[Claim 1] A self-position and orientation estimation unit that estimates its own position and orientation using a particle filter, An initial particle generation unit that generates multiple initial particles, Includes, The position and posture were adjusted so that they were in a predetermined position and posture. It is an autonomous mobile vehicle, The initial particle generation unit is, The positions of the plurality of initial particles are set to have noise added to a predetermined position, and the positions of the plurality of initial particles are set such that the arithmetic mean of the positions of the plurality of initial particles coincides with the predetermined position. Multiple pairs of initial particles are generated from the aforementioned multiple initial particles, and for each pair of initial particles, the poses of the two initial particles belonging to that pair are set to have noise added to a predetermined pose, and the poses of the two initial particles are set so that the arithmetic mean of the poses of the two initial particles matches the predetermined pose. Autonomous mobile device.

Images