Autonomous moving body

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]

[0001] The present invention relates to an autonomous moving body. [Background technology]

[0002] Patent Document 1 discloses a technology for estimating a self-location using a particle filter. Specifically, it claims that the estimation error of the self-location is reduced by actively removing particles that are outside the irradiation range of infrared rays. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] JP 2009-176031 A Summary of the Invention [Problem to be solved by the invention]

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

[0005] On the other hand, when a particle filter is used for estimating the self-position and orientation of an autonomous mobile body, the autonomous mobile body estimates its self-position and orientation by repeating the following steps in the order listed: prediction of particles for the next step, likelihood calculation for each predicted particle, and resampling based on the likelihood. Here, the autonomous mobile body needs to generate multiple initial particles for the first step. The positions and orientations of the multiple initial particles are the above-mentioned predetermined positions and orientations with random noise added thereto. Then, the autonomous mobile body estimates the initial self-position and orientation by arithmetically averaging the positions and orientations of the multiple initial particles.

[0006] Therefore, even if an operator adjusts the position and orientation of an autonomous moving body with high accuracy, the initial self-position and orientation estimated by the autonomous moving body will deviate slightly from the actual position and orientation after adjustment by the operator. The amount of deviation at this time becomes particularly noticeable when the variance is set large in the case where random noise follows a normal distribution, or when the number of particles is small.

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

[0008] According to the present disclosure, a self-position and orientation estimation unit that estimates a self-position and orientation using a particle filter; an initial particle generating unit that generates a plurality of initial particles; Including, The position and the attitude are adjusted so that the predetermined position and the predetermined attitude are achieved. An autonomous moving body, The initial particle generation unit includes: setting the positions of the plurality of initial particles to the predetermined positions with noise added thereto, and setting the positions of the plurality of initial particles such that an arithmetic average of the positions of the plurality of initial particles coincides with the predetermined position; generating a plurality of initial particle pairs from the plurality of initial particles, and for each initial particle pair, setting the orientation of two initial particles belonging to the initial particle pair to a predetermined orientation plus noise, and setting the orientations of the two initial particles such that an arithmetic mean of the orientations of the two initial particles coincides with the predetermined orientation; An autonomous vehicle is provided. Effect of the Invention

[0009] According to the present disclosure, the initial self-position and orientation estimated by an autonomous moving body can be made to coincide with a predetermined position and orientation. [Brief description of the drawings]

[0010] [Figure 1] FIG. 1 is a perspective view of an autonomous mobile robot. [Diagram 2] This is the initial particle generation flow. [Diagram 3] FIG. 13 is an explanatory diagram of the attitude of a particle. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

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

[0012] The two drive wheels 3 and the two driven wheels 4 are rotatably arranged 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 freely.

[0013] The two-dimensional LiDAR device 5 is provided 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, a read / write RAM 6b (Random Access Memory), a read-only ROM 6c (Read Only Memory), and a read / write SSD 6d (Solid State Drive). The CPU 6a reads and executes a control program stored in the ROM 6c or SSD 6d, and the control program causes hardware such as the CPU 6a to function as an initial particle generation unit 7, a self-position and orientation estimation unit 8, and an 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 self-position and orientation estimation using the particle filter estimates the self-position and orientation by repeating the following steps in the order listed: prediction of the particle in the next step, likelihood calculation for each predicted particle, and resampling based on the likelihood.

[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 autonomous mobile robot 1. The position data includes X coordinate data and Y coordinate data. The orientation data is azimuth angle data of the autonomous mobile robot 1. The self-position and orientation estimation unit 8 estimates the position of the autonomous mobile robot 1 by taking the arithmetic average of the position data of the multiple particles for each step. Similarly, the self-position and orientation estimation unit 8 estimates the orientation of the autonomous mobile robot 1 by taking the arithmetic average of the orientation data of the multiple particles for each step.

[0017] The initial particle generating 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 autonomous mobile robot 1 based on the self-position and attitude estimated by the self-position and attitude estimation unit 8 and the map data stored in the SSD 6d.

[0019] Next, the initial particle generation unit 7 will be described in detail with reference to Fig. 2 and Fig. 3. Fig. 2 shows an initial particle generation flow. Fig. 3 is an explanatory diagram of the posture. For convenience of explanation, the number of multiple 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, the adjustment accuracy is typically less than 1 centimeter in terms of position and less than 3 degrees in terms of orientation. The predetermined position and orientation include a predetermined position and a predetermined orientation. The predetermined position is composed of a predetermined X coordinate and a predetermined Y coordinate. The above 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 determined 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 random numbers, and sets the X and Y coordinates of the first initial particle to the predetermined X and Y coordinates plus the random numbers. Next, the initial particle generation unit 7 generates new random numbers, and sets the X and Y coordinates of the second initial particle to the predetermined X and Y coordinates plus the random numbers. The initial particle generation unit 7 further repeats the above 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]

number

[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 ith initial particle, and X0 is the specified X coordinate. According to the above formula (1), the arithmetic average of the X coordinates of multiple initial particles coincides with the specified X coordinate, as shown in the following formula (2).

[0024]

number

[0025] The Y coordinates of multiple initial particles are set in the same way.

[0026] Next, the initial particle generating unit 7 sets the orientations of a plurality of initial particles (S100-S220).

[0027] Please refer to FIG. 3. In FIG. 3, t indicates orientation. All ts are normalized to be in the range of minus π to plus π. 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, in reality, the average of t4 and t6 should be t5, only visually. Therefore, when arithmetically averaging the orientations of multiple initial particles, each of the orientations of multiple initial particles is converted to a unit vector [sin(t),cos(t)] on a unit circle, and the sum of the multiple unit vectors after the change is inversely converted to perform the arithmetic mean of the orientations, as shown in the following formula (3). In the following formula (3), ti indicates the orientation of the i-th initial particle, and tave indicates the arithmetically averaged orientation. The initial particle generating unit 7 of this embodiment sets the orientations of a plurality of initial particles so that tave in the following formula (3) coincides with a predetermined orientation t0.

[0028]

number

[0029] Hereinafter, when setting the orientations of a plurality of initial particles, the initial particle generating unit 7 generates a plurality of initial particle pairs from a plurality of initial particles, and for each initial particle pair, sets the orientations of the two initial particles belonging to that initial particle pair to a predetermined orientation plus noise, and sets the orientations of the two initial particles so that the arithmetic mean of the orientations of the two initial particles coincides with the predetermined orientation. That is, the initial particle generating unit 7 sets the orientation for each initial particle pair in steps S140 to S210 in FIG. 2.

[0030] First, the initial particle generation unit 7 judges whether the number N of the multiple initial particles is an odd number (S110), and if the judgment result is YES (S110: YES), it sets the Nth orientation to a predetermined orientation (S120), and if the judgment result is NO (S110: NO), the process proceeds to step S130. Next, the initial particle generation unit 7 sets a variable i to 1 (S130).

[0031] Next, the initial particle generation unit 7 generates random numbers within the range of minus π to plus π (S140). The random numbers may be uniform random numbers or normal 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), and if the determination result is YES (S150: YES), the process proceeds to S160, and if the determination result is NO (S150: NO), the process proceeds to S190.

[0033] In step S160, the initial particle generating unit 7 sets the posture of the i-th initial particle to the predetermined posture t0 plus the random number generated in step S140. In step S170, the initial particle generating unit 7 sets the posture of the (i+1)-th initial particle to the predetermined posture t0 minus the random number generated in step S140. Please refer to FIG. 3. t0 is the predetermined posture, t1 is the posture of the i-th initial particle, and t9 is the posture of the (i+1)-th initial particle. According to steps S160 and S170, when the posture of the i-th initial particle and the posture of the (i+1)-th initial particle are arithmetically averaged, the added random numbers cancel each other out, so that the arithmetic average can be made to coincide with t0, which is the predetermined posture. Returning to FIG. 2, the initial particle generating unit 7 increments the variable i twice (S180) and proceeds to the process of S220.

[0034] If the absolute value of the random number generated in step S140 is π / 2 or more, the following problem occurs when steps S160 and S170 are executed. That is, in FIG. 3, the posture of the i-th initial particle is t3, and the posture of the (i+1)-th initial particle is t7. In this case, the arithmetic mean of t3 and t7 is originally set to t0, but according to the above formula (3), it becomes t5. Therefore, in step S190, the initial particle generation unit 7 sets the posture of the i-th initial particle to a predetermined posture t0 plus the random number generated in step S140, while in step S200, the initial particle generation unit 7 sets the posture of the (i+1)-th initial particle to a posture of the i-th initial particle plus π. In FIG. 3, the posture of the i-th initial particle is t3, and the posture 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 not offset by each other on the right side of the above formula (3). Returning to Fig. 2, the initial particle generating unit 7 increments the variable i twice (S210) and advances the process to S220.

[0035] In step S220, the initial particle generation unit 7 determines whether the setting of the postures of all the initial particles has been completed (S220), and if the determination result is YES (S220: YES), ends the processing, and if the determination result is NO (S220: NO), returns the processing to step S140.

[0036] The above embodiment has the following features. The autonomous mobile robot 1 (autonomous mobile body) 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 so that it is in a predetermined position and in a predetermined orientation. The initial particle generating unit 7 sets the positions of the multiple initial particles by adding random numbers (noise) to predetermined positions, 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 generating unit 7 generates a plurality of initial particle pairs from a plurality of initial particles, and for each initial particle pair, sets the orientation of the two initial particles belonging to the initial particle pair to a predetermined orientation plus a random number (noise), and sets the orientations of the two initial particles so that the arithmetic mean of the orientations of the two initial particles coincides with the predetermined orientation. With the above configuration, the initial self-position and orientation estimated by the autonomous moving body can be made to coincide with the predetermined position and orientation.

[0037] In this embodiment, the initial particle generating unit 7 generates a plurality of initial particle pairs from a plurality of initial particles, and sets the orientations of two initial particles for each initial particle pair. This is because the orientations are normalized to be in the range of minus π to plus π. If the number of the plurality of initial particles is three, the predetermined orientation is 0, and the first and second orientations are 179, the arithmetic average of the orientations of these three initial particles cannot be made to coincide with the predetermined orientation no matter what value the third orientation is set to. This is because the third orientation is also normalized to be in the range of minus π to plus π, and cannot be set to -358. In contrast, in this embodiment, the orientations of two initial particles are set for each initial particle pair. By setting the orientations of two initial particles at a time in this manner, the arithmetic average of the orientations of the entire plurality of initial particles can be reliably made to be the predetermined orientation. [Explanation of symbols]

[0038] 1. Autonomous mobile robot 7 Initial particle generation section 8 Self-position and orientation estimation unit

Claims

[Claim 1] a self-position and orientation estimation unit that estimates a self-position and orientation using a particle filter; an initial particle generating unit that generates a plurality of initial particles; Including, The position and the attitude are adjusted so that the predetermined position and the predetermined attitude are achieved. An autonomous moving body, The initial particle generation unit includes: setting the positions of the plurality of initial particles to be predetermined positions with noise added thereto, and setting the positions of the plurality of initial particles such that an arithmetic average of the positions of the plurality of initial particles coincides with the predetermined position; generating a plurality of initial particle pairs from the plurality of initial particles, and for each initial particle pair, setting the orientation of two initial particles belonging to the initial particle pair to a predetermined orientation plus noise, and setting the orientations of the two initial particles so that an arithmetic average of the orientations of the two initial particles coincides with the predetermined orientation; Autonomous mobile body.

Images