JP6734764B2 - Position estimation device, map information preparation device, moving body, position estimation method and program - Google Patents

Description

The present invention relates to a position estimation device, a map information preparation device, a moving body, a position estimation method and a program.

A moving body such as a traveling carriage or robot is provided with an arm capable of turning and moving directly, and a laser range sensor is mounted on the tip of the arm to measure the distance to obstacles existing in the surrounding environment and A technique for constructing an obstacle map from the positional relationship between the obstacle and the obstacle is known. The laser range sensor outputs the distance information to the obstacle based on the sensor origin, so in order to create a map of the obstacle while moving, the distance information output by the laser range sensor is constantly transformed. It is necessary to update the values in absolute coordinates (map coordinates). Therefore, in the map creation by the moving body, a means for constantly measuring the accurate position/orientation of the laser range sensor is required. In order to measure the position information of the laser range sensor, a configuration using a position sensor such as GPS can be considered.

For example, in Patent Document 1 and Patent Document 2, a survey is provided in which a traveling carriage is provided with an arm having an expansion/contraction mechanism, and a GPS and a laser range sensor are mounted on the tip of the arm so that position information of the arm tip can be measured. A device is disclosed. If this technology is used, it is difficult to move the traveling carriage to the surveying point due to rubble or topographical problems such as the site of civil engineering work or disaster site, or the survey target does not fall within the viewing angle of the laser ranging sensor. Even in high places, it is possible to survey by extending the arm. Further, since the position of the laser range sensor can be confirmed by the positioning information by the GPS at the arm end, the accuracy of the survey result can be improved by converting the coordinates of the laser range sensor using the positioning information by GPS. Can be kept.

JP, 7-128057, A JP-A-9-196672

However, the methods of Patent Documents 1 and 2 are based on the assumption that the sensor position is observed by GPS, and cannot be used in an environment such as indoors where GPS signals cannot be received. In addition, the position of the traveling vehicle itself can be geometrically inferred to some extent from the tire rotation speed and steering angle, but since it is a calculation method (odometry) that calculates the relative displacement from the initial position, the traveling distance increases. There is a problem that errors accumulate.

On the other hand, a technique called scan matching is known as an alternative technique of odometry. Scan matching is a technique for inversely estimating the amount of movement of a sensor by aligning the point cloud information acquired from a laser range sensor in time series. By using this scan matching, it is possible to estimate the position of the moving body without accumulating errors such as odometry. By the way, if the arm keeps a fixed posture while the moving body is running, the moving amount of the laser range sensor is equal to the moving amount of the moving body, and the moving amount of the moving body can be estimated by scan matching. Then, the position of the moving body can be estimated by adding up the estimated movement amounts. However, if the posture of the arm changes while the moving body is traveling, there is a problem that the moving amount of the sensor and the moving amount of the moving body are different from each other.

Therefore, an object of the present invention is to provide a position estimation device, a map information preparation device, a moving body, a position estimation method and a program that can solve the above-mentioned problems.

A first aspect of the present invention is to provide a range sensor that is provided on an arm capable of changing a posture and measures a distance to an object, and a position of the object indicated by distance information obtained by scanning the range sensor. Scan matching based on the point cloud information corresponding to, and a scan matching processing unit that estimates the amount of movement of the range sensor, and information about the movement of the moving body provided in the moving body having the arm is measured. The odometry processing unit that estimates the movement amount of the moving body based on the measurement value of the sensor, and the instruction information based on the presence or absence of the posture change of the arm are acquired, and the odometry processing unit estimates based on the instruction information. A position estimation device that includes one of a movement amount and a movement amount estimated by the scan matching processing unit, and a position information estimation unit that estimates position information of the moving body based on the selected movement amount. ..

The position information estimation unit in the second aspect of the present invention is one of the movement amount estimated by the odometry processing unit and the movement amount estimated by the scan matching processing unit, based on the instruction information input by the operator. Select.

The position estimation device in the third aspect of the present invention determines whether or not there is a posture change of the arm based on information measured by a sensor that measures a posture change of the arm, and based on the determination, the odometry. A switching instruction unit that outputs instruction information for instructing which one of the movement amount estimated by the processing unit and the movement amount estimated by the scan matching processing unit is selected is further included.

The switching instruction unit in the fourth aspect of the present invention instructs the selection of the movement amount estimated by the odometry processing unit when it is determined that the posture change of the arm has occurred.

The position estimation apparatus according to the fifth aspect of the present invention is an input that receives an input of instruction information as to which of the movement amount estimated by the odometry processing unit and the movement amount estimated by the scan matching processing unit is selected. An instruction to estimate the movement amount of the moving body based on the instruction information received by the input receiving unit, of the instruction information output by the switching instruction unit and the instruction information received by the input receiving unit. And an operator instruction priority processing unit for performing the operation.

In the scan matching calculation according to the sixth aspect of the present invention, the scan matching calculation unit uses, as an initial value, the movement amount of the moving body in the minute time period estimated by the odometry processing unit.

According to a seventh aspect of the present invention, the position estimation device described above, a sensor position/orientation calculation unit that calculates position information based on the moving body of the range sensor according to a change in the attitude of the arm, and The point cloud information obtained by scanning by the range sensor is corrected using the position information calculated by the sensor position/orientation calculation unit and the position information of the moving body estimated by the position estimation device, and map information is obtained. A map information creation device comprising: a map information creation unit to be created.

An eighth aspect of the present invention is a moving body including the position estimating device according to any one of the above, and an arm capable of changing a posture and provided with a range sensor included in the position estimating device.

A ninth aspect of the present invention is that a position estimation device, which is provided on an arm whose posture can be changed and includes a range sensor that measures a distance to an object, provides distance information obtained by scanning the range sensor. Scan matching is performed based on the point cloud information corresponding to the position of the object to be shown, the movement amount of the range sensor is estimated, and information regarding the movement of the moving body provided in the moving body having the arm is measured. The movement amount of the moving body is estimated based on the measurement value of the sensor, the instruction information based on the presence or absence of the posture change of the arm is acquired, and the movement amount of the moving body and the range sensor based on the instruction information. Of the movement amount, and a position estimation method for estimating position information of the moving body based on the selected movement amount.

A tenth aspect of the present invention is a distance obtained by scanning a computer of a position estimation device, which is provided on an arm capable of changing a posture and includes a range sensor for measuring a distance to an object, by the range sensor. Means for estimating the amount of movement of the range sensor by performing scan matching based on the point group information corresponding to the position of the object indicated by the information, and information on the movement of the moving body provided in the moving body having the arm. Means for estimating the movement amount of the moving body based on a measurement value of a sensor for measuring, and instruction information based on whether or not there is a change in the posture of the arm, and based on the instruction information, the movement amount of the moving body and It is a program for functioning as a means for selecting one of the movement amounts of the range sensor and estimating position information of the moving body based on the selected movement amount.

According to the present invention, with respect to the position information of a moving body provided with an arm (manipulator) whose posture can be changed, the position information of the moving body is calculated by scan matching based on the detection result of a range sensor attached to the arm. In this case, the position information of the moving body can be accurately calculated regardless of the change in the posture of the arm.

It is a figure which shows an example of the traveling vehicle carrying the surveying device in 1st embodiment which concerns on this invention. It is a block diagram showing an example of a surveying device in a first embodiment concerning the present invention. It is a 1st figure explaining the preparation process of map information in the surveying device in 1st embodiment which concerns on this invention. It is a 2nd figure explaining the preparation process of map information in the surveying instrument in 1st embodiment which concerns on this invention. It is a 1st figure explaining the estimation method of the position information of the surveying instrument in 1st embodiment which concerns on this invention. It is a 2nd figure explaining the estimation method of the position information of the surveying instrument in 1st embodiment which concerns on this invention. It is a figure explaining the case where scan matching is not suitable. It is a figure explaining the estimation processing of the amount of movement of the surveying instrument in a first embodiment concerning the present invention. It is a flow chart which shows an example of estimating processing of the amount of movement of a surveying device in a first embodiment concerning the present invention. It is a block diagram showing an example of a surveying device in a second embodiment concerning the present invention. It is a 1st block diagram which shows an example of the surveying instrument in 3rd embodiment which concerns on this invention. It is a 2nd block diagram which shows an example of the surveying instrument in 3rd embodiment which concerns on this invention. It is a 1st block diagram which shows an example of the surveying instrument in 4th embodiment which concerns on this invention. It is a 2nd block diagram which shows an example of the surveying instrument in 4th embodiment which concerns on this invention.

<First embodiment>
Hereinafter, a surveying device according to an embodiment of the present invention will be described with reference to FIGS.
FIG. 1 is a diagram showing an example of a traveling vehicle equipped with a surveying device according to the first embodiment of the present invention.
As shown in FIG. 1, the traveling vehicle 1 includes an arm 2 (2a, 2b), a surveying device 100, and a control device 40. The arm 2 has a link mechanism including two joint portions 3a and 3b, and the arm 2a and the arm 2b can pivot in a direction (up and down) perpendicular to the ground. Alternatively, the arm 2 may be capable of horizontally (left and right) turning motion. The mechanism of the arm 2 is not limited to the illustrated mechanism, and may include, for example, more link portions, or may have, for example, a linear motion mechanism and be capable of extending motion.
One end of the arm 2 is pivotally connected to the traveling carriage 1, and a laser range sensor 101 is provided near the other end. The control device 40 controls the traveling/stopping of the traveling vehicle 1 and the turning operation (or extension operation) of the arm 2.

The laser range sensor 101 provided in the vicinity of the tip of the arm 2 serves as an object (for example, a wall, a fence, a building, a rock, etc., which are collectively referred to as an obstacle) existing around the traveling carriage 1. It is used to measure the distance and create map information around the laser range sensor 101 based on the distance information. The surveying device 100 creates the map information.
Since the arm 2 can swivel, for example, surveying at a high place is possible. In addition, the laser range sensor 101 can measure the distance even while the traveling vehicle 1 is traveling. Therefore, the surveying device 100 can create map information based on the distance information measured while the traveling vehicle 1 is traveling. In that case, since the measured distance information is a value measured in the coordinate system with the traveling carriage 1 as a reference, in order to create the map information, it must be converted into the coordinate information in the coordinate system as the reference of the map information. I have to. The surveying device 100 responds to the distance information in the coordinate system based on the sensor measured by the laser range sensor 101, according to the amount of movement of the laser range sensor 101 as the traveling vehicle 1 travels and the attitude of the arm 2. The positional relationship between the traveling carriage 1 and the laser range sensor 101 is corrected. The surveying instrument 100 performs these corrections to calculate the position information of the laser range sensor 101 in the map coordinate system. Further, the surveying device 100 corrects the position information of the obstacle indicated by the distance information measured by the laser range sensor 101 by the position of the laser range sensor 101 in the map coordinate system to obtain position information in the map coordinate system. Convert and create map information.

Here, each coordinate system of FIG. 1 will be described. Σ _m is a map coordinate system (fixed coordinates). Σ ₀ is a robot coordinate system. The robot coordinate system is a coordinate system based on the traveling carriage 1. In the robot coordinate system, for example, the center of gravity of the traveling carriage 1 is set as the origin. Σ ₁ is the link ₁ coordinate system. The link ₁ coordinate system is a coordinate system whose origin is the center of the joint portion 3a connecting the arm 2a and the traveling carriage 1. Σ ₂ is the link ₂ coordinate system. The link ₂ coordinate system is a coordinate system whose origin is the center of the joint portion 3b connecting the arms 2b and 2a. Σ _S is a laser coordinate system. The laser coordinate system is a coordinate system with the laser range sensor 101 as a reference. In the laser coordinate system, for example, the center of gravity of the laser range sensor 101 is the origin. Further, ^m T ₀ represents coordinate conversion from the map coordinate system (Σ _M ) to the robot coordinate system (Σ ₀ ). ⁰ T ₁ indicates coordinate conversion from the robot coordinate system (Σ ₀ ) to the link ₁ coordinate system (Σ ₁ ). ¹ T ₂ indicates coordinate conversion from the link ₁ coordinate system (Σ ₁ ) to the link ₂ coordinate system (Σ ₂ ). ² T _S indicates coordinate conversion from the link ₂ coordinate system (Σ ₂ ) to the laser coordinate system (Σ _S ). In the following description, it is assumed that the origin of the robot coordinate system (Σ ₀ ) coincides with the origin of the map coordinate system (Σ _m ) in the initial state (when the traveling carriage 1 starts to move).

Next, the surveying instrument 100 will be described in detail. FIG. 2 is a block diagram showing an example of the surveying device in the first embodiment according to the present invention.
The surveying instrument 100 is a computer including a CPU (Central Processing Unit). Further, as described later, the surveying device 100 is a position estimation device and a map information creation device. As shown in FIG. 1, the surveying device 100 includes a laser range sensor 101, a sensor (link mechanism) 102, a sensor (moving body) 103, an operator operation terminal 104, a sensor position/orientation calculation unit 105, and a scan. A matching processing unit 106, an odometry processing unit 107, a carriage movement amount estimation switching unit 108, and a coordinate conversion processing unit 109 are provided.

The laser range sensor 101 is, for example, a three-dimensional laser scanner. The laser range sensor 101 measures the distance to an obstacle while scanning with laser light. The set of obstacle positions indicated by the distance information obtained by scanning by the laser range sensor 101 is called point cloud information.

The sensor (link mechanism) 102 is one or more sensors that measure the posture information of the arm 2. The sensor (link mechanism) 102 is, for example, an angle measurement sensor that measures a turning angle formed by each of the arms 2a and 2b with respect to the horizontal direction (or the vertical direction). The vertical movement of the arm 2 is controlled by the controller 40. For example, the elevation angle measurement sensor is provided in each of the arms 2a and 2b, and measures the angle (θ _a ) formed by the arm 2a with the horizontal direction and the angle (θ _b ) formed by the arm 2b with the horizontal direction. When the arm 2 has a linear motion mechanism, the sensor (link mechanism) 102 may further have a stroke measurement sensor that measures the stroke (length) of the arm 2.

The sensor position/posture calculation unit 105 calculates position information and posture information of the laser range sensor 101 in the robot coordinate system according to the posture of the arm 2. The sensor position/orientation calculation unit 105 calculates the position information and the attitude information of the laser range sensor 101 in the robot coordinate system by a geometric method using the angle (θ _a ) measured by the angle measurement sensor. For example, if the length of the arm 2a is L _a and the angle formed with the horizontal direction is θ _a , the x coordinate in the link ₁ coordinate system of the joint portion 3b is L _a ·cos θ _a , the z coordinate is L _a ·sin θ _a , and the posture is The information is represented by θ _a . The same applies to the laser range sensor 101. The sensor position/orientation calculation unit 105 calculates the position information and the attitude information of the laser range sensor 101 in the robot coordinate system by using the relationship of the coordinate transformation of ⁰ T ₁ , ¹ T ₂ , and ² T _S.

The scan matching processing unit 106 includes position information and posture information of the laser range sensor 101 in the robot coordinate system calculated by the sensor position/orientation calculation unit 105 and distance information (points) between the obstacle and the obstacle measured by the laser range sensor 101. Group information) and the amount of movement (moving distance, moving direction) of the laser range sensor 101 is estimated by scan matching. As the scan matching algorithm, for example, an ICP (Iterative Closest Point) algorithm can be used. The scan matching processing unit 106 outputs the estimated movement amount of the laser range finder sensor 101 to the trolley movement amount estimation switching unit 108. The moving distance is the distance to the current predetermined position of the traveling carriage 1 based on the position of the predetermined position (for example, the center of gravity position) of the traveling carriage 1 before a predetermined time. The moving direction is, for example, an angle formed by the traveling direction of the traveling vehicle 1 with respect to a predetermined direction.

The sensor (moving body) 103 is one or a plurality of sensors provided on the traveling carriage 1. The sensor (moving body) 103 measures information regarding the amount of movement of the traveling vehicle 1. The sensor (moving body) 103 is, for example, a wheel rotation speed measurement sensor that measures the rotation speed of a wheel that rotates the tire of the traveling carriage 1. The traveling distance of the traveling vehicle 1 can be calculated based on the rotational speed of the wheel measured by the wheel rotational speed measurement sensor. The sensor (moving body) 103 is, for example, a steering angle measurement sensor that detects the steering angle of the tire of the traveling vehicle 1. The traveling direction of the traveling carriage 1 can be calculated based on the steering angle measured by the steering angle measurement sensor.

The odometry processing unit 107 integrates the wheel rotation speed measured by the wheel rotation speed measurement sensor (sensor (moving body) 103) and the total steering angle measured by the steering angle measurement sensor (sensor (moving body) 103), The moving amount (moving distance, moving direction) of the traveling vehicle 1 is calculated. The odometry processing unit 107 outputs the calculated estimated value of the movement amount of the laser range finder sensor 101 to the dolly movement amount estimation switching unit 108.

The carriage movement amount estimation switching unit 108 acquires the movement amount of the laser range sensor 101 from the scan matching processing unit 106, and acquires the movement amount of the traveling carriage 1 from the odometry processing unit 107. Further, the dolly movement amount estimation switching unit 108 acquires the movement amount estimation switching instruction information from the operator operation terminal 104. The movement amount estimation switching instruction information includes information that indicates which of the movement amount estimated by the scan matching processing unit 106 and the movement amount calculated by the odometry processing unit 107 should be selected.
The dolly movement amount estimation switching unit 108 selects any one of the obtained two movement amounts based on the movement amount estimation switching instruction acquired from the operator operation terminal 104, and the selected movement amount is the coordinate conversion processing unit. Output to 109.

The coordinate conversion processing unit 109 acquires point cloud information from the laser range finder sensor 101, and acquires movement amount information for each predetermined time from the carriage movement amount estimation switching unit 108. First, the coordinate conversion processing unit 109 calculates the position information and the posture information of the traveling vehicle 1 in the map coordinate system by integrating the movement amounts acquired from the vehicle movement amount estimation switching unit 108. The position information is coordinate information of a predetermined position (for example, the center of gravity position) of the traveling vehicle 1 in the map coordinate system. The posture information is, for example, an angle formed by the traveling direction of the traveling vehicle 1 with respect to a predetermined direction. At this time, it is used that the movement amount of the laser range sensor 101 estimated by the scan matching processing unit 106 can be regarded as the same as the movement amount of the traveling vehicle 1 unless the posture of the arm 2 is changed, as will be described later in detail. To do. That is, if the posture of the arm 2 is not changed, the coordinate conversion processing unit 109 integrates the movement amount of the laser range sensor 101 estimated by the scan matching processing unit 106 with higher accuracy and no error accumulation, and thus the traveling carriage. The position information and the posture information of 1 are estimated. On the other hand, when the posture of the arm 2 is changed, the coordinate conversion processing unit 109 estimates the position information and the posture information of the traveling vehicle 1 by integrating the movement amounts calculated by the odometry processing unit 107. When the position information of the traveling carriage 1 is estimated, the coordinate conversion processing unit 109 corrects the point cloud information acquired from the laser range sensor 101 with the position information of the traveling carriage 1 and the like, and the point cloud information in the map coordinate system. Calculate coordinate information and create map information. Specifically, the coordinate conversion processing unit 109 performs coordinate conversion of the coordinate information of the point group information measured by the laser range sensor 101 in the laser coordinate system into a value based on the link ₂ coordinate system, and further, the link ₁ A value based on the coordinate system is converted into coordinate information in the robot coordinate system. Then, the position information and the posture information of the traveling vehicle 1 estimated previously are added to calculate the coordinate information in the map coordinate system of each point cloud information. The coordinate information of each point cloud information obtained by calculation indicates the position where some obstacle exists. That is, it is map information. The coordinate conversion processing unit 109 outputs the created map information to the viewer unit 200. The viewer unit 200 generates image information including an obstacle based on the acquired map information and causes the display device to display the image information.

Although the surveying instrument 100 has various functions for creating map information in addition to this, in the present embodiment, the positions of the traveling carriage 1 and the laser range sensor 101 required for creating map information. Only the configuration required for the information/posture information estimation process and the map information creation process will be described, and description of the other components will be omitted.

Next, with reference to FIGS. 3 to 4, an outline of a process of creating map information by the surveying device 100 will be described.
FIG. 3 is a first diagram illustrating a map information creation process in the surveying device according to the first embodiment of the present invention.
FIG. 3 shows how the surveying instrument 100 performs surveying for creating map information. The laser range sensor 101 irradiates a laser in a predetermined range and measures distance information to the surface shape of an obstacle existing in the surrounding environment. Specifically, when the obstacle 50 is present in the vicinity, the laser range sensor 101 measures the distance from the obstacle 50 by reflection from the surface of the obstacle 50. The dotted circles ^S P ₁ , ^S P _2, ... Displayed on the obstacle 50 indicate the point cloud information corresponding to the distance information measured by scanning by the laser range sensor 101. ^S P ₁ , ^S P _2, ... Are coordinate information of each point in the laser coordinate system acquired by the laser range finder sensor 101. The coordinate conversion processing unit 109 converts the coordinate information of each point into the map coordinate system by the following formula.
^M P _i = ^M T ₀ · ⁰ T ₁ · ¹ T ₂ · ² T _S · ^S P _i
P _i =[X _i , Y _i , Z _i ] ^T
P={P ₁ , P ₂ ,..., P _i ,..., P _n }
When the coordinate information in the map coordinate system can be calculated for each of the measured point groups, the set becomes the map information representing the position information of the surface of the obstacle existing in the surrounding environment. The coordinate conversion processing unit 109 outputs the created map information to the viewer unit 200.

FIG. 4 is a second diagram illustrating a map information creation process in the surveying device according to the first embodiment of the present invention.
FIG. 4 shows an example of the map information displayed on the display device by the viewer unit 200. An image (obstacle image 50') imitating the shape of the obstacle 50 is displayed in the displayed map information. The surveying instrument 100 travels in various spaces, acquires the coordinate information (Σ _S ) of the surrounding environment via the laser range sensor 101 even during the traveling, and uses the information in the map coordinate system (Σ _m ). Convert to create map information of the space you have traveled. By the way, in order to convert the coordinate information (Σ _S ) of the surrounding environment into the map coordinate system (Σ _m ), it is converted into the coordinate system based on each of the link ₂ coordinate system, the link ₁ coordinate system and the robot coordinate system, Finally, we have to convert from the robot coordinate system to the map coordinate system. Here, conversion from the laser coordinate system to the link ₂ coordinate system, from the link ₂ coordinate system to the link ₁ coordinate system, and from the link ₁ coordinate system to the robot coordinate system is performed by the laser range sensor 101 and the arm 2b, and the arm 2b and the arm 2a, respectively. The coordinates can be converted from the positional relationship between the arm 2a and the traveling carriage 1 by calculation based on the geometrical relationship. However, the coordinate information from the robot coordinate system to the map coordinate system requires the position information and the posture information of the traveling vehicle 1. Next, the estimation of the position information of the traveling vehicle 1 will be described with reference to FIGS.

FIG. 5 is a first diagram illustrating a method for estimating position information of a surveying device according to the first embodiment of the present invention.
FIG. 5 shows a state in which the surveying device 100 travels while performing surveying (scanning by the laser range sensor 101). It shows that the traveling vehicle 1 existing at the position A at a certain time has moved to the position B after that. During this period, it is assumed that the posture of the arm 2 does not change. That is, the positional relationship among the laser range sensor 101, the arm 2a, the arm 2b, and the traveling carriage 1 is constant during traveling from the position A to the position B. Under this condition, the movement amount ΔL _S of the laser range finder sensor 101 and the movement amount ΔL _B of the traveling vehicle 1 are the same distance. Therefore, the movement amount [Delta] L _B of the traveling vehicle 1, scan matching processing unit 106, a laser measuring area sensor 101 acquires the point cloud data measured at predetermined time intervals, based on the acquired point cloud data of the time series was It can be estimated by scan matching. Incidentally, often moving amount [Delta] L _B of the traveling vehicle 1 can be calculated even with a moving amount of the calculation results by the odometry processing unit 107, the odometry, especially when traveling long distances, error increases Since it is known that a high accuracy may not be obtained, it is desirable to estimate the movement amount using scan matching. If calculating the amount of movement [Delta] L _B of the traveling vehicle 1, the coordinate conversion processing unit 109, a laser measuring area point group information sensor 101 is measured can be converted to the map coordinate system.

FIG. 6 is a second diagram illustrating the method for estimating the position information of the surveying device according to the first embodiment of the present invention.
FIG. 6A shows point cloud information measured by the laser range sensor 101 at position A (right figure) and point cloud information measured by the laser range sensor 101 at position B (left figure). Although each point cloud information indicates the surface shape of the obstacle 50, the scanning range is limited as the traveling vehicle 1 approaches the obstacle 50 and the distance between the laser range sensor 101 and the obstacle 50 becomes smaller. , The point cloud information measured at the position A is as shown in FIG. 6(a) in which the shape of the obstacle 50 is captured in a wider range, and the point cloud information measured at the position B is in a narrower range of the obstacle 50. It is as shown in the left figure of FIG.

FIG. 6B is a diagram illustrating the principle of scan matching. FIG. 6B shows a process in which each of the point groups shown in the left diagram of FIG. 6B is moved so as to be superimposed on the corresponding point in the right diagram. If all the points do not overlap, change the relationship between the overlapping points and try again. Then, the translation amount and the rotation amount when all the points are overlapped are obtained. The calculated translation amount and rotation amount are the movement amount ΔL _S of the laser range sensor 101. As an algorithm for superimposing two point groups, for example, the ICP algorithm can be used. This is the principle of scan matching. That is, the movement amount when one of the point cloud information measured at different times is moved and the distance between the two point cloud information becomes the shortest is set as the movement amount of the laser range sensor 101 during that time. As described above, when the posture of the arm 2 does not change, the movement amount ΔL _S becomes the movement amount ΔL _B of the traveling vehicle 1.

By the way, when the traveling vehicle 1 travels from the position A to the position B, it suffices to be able to always use highly accurate scan matching, but there are situations where scan matching cannot be applied. Next, a case where scan matching cannot be applied will be described with reference to FIG.
FIG. 7 is a diagram illustrating a case where scan matching is not suitable.
FIG. 7A shows a posture in which the arm 2a is swung upward to raise the laser range sensor 101 to a certain height, and the arm 2a becomes horizontal with the ground while the traveling carriage 1 is stopped on the spot. It is shown that the arm 2 is lowered to a straight posture. If the amount of movement before and after this posture change is estimated by the scan matching described in FIG. 6B, the amount of movement illustrated by the amount of movement ΔL7 can be obtained.

FIG. 7B shows a state in which the traveling vehicle 1 has moved in the diagonally lower right direction on the paper by an amount indicated by ΔL7 without changing the posture of the arm 2. If the amount of movement before and after this movement is estimated by scan matching, the amount of movement ΔL7 can be obtained. When the movement amount is estimated by scan matching in this way, it is not known whether the traveling carriage 1 has moved, the posture of the arm 2 has changed, or the traveling carriage 1 has also moved while the posture of the arm 2 changed. .. For example, if the amount of movement by scan matching is taken as the amount of movement of the traveling carriage 1, in the case of FIG. 7A, the traveling carriage 1 has moved by the movement amount ΔL7 even though the traveling carriage 1 has not moved. It will be estimated that. Such an inconvenience occurs if the movement amount of the traveling vehicle 1 is always estimated by scan matching. Next, the control method of this embodiment for solving this inconvenience will be described.

FIG. 8 is a diagram illustrating a movement amount estimation process of the surveying device according to the first embodiment of the present invention.
In the section 8A, the traveling carriage 1 moves with the posture of the arm 2 fixed. In the section 8B, the traveling carriage 1 moves while operating the posture of the arm 2. In the section 8C, the traveling vehicle 1 moves again with the posture of the arm 2 fixed.
The surveying instrument 100 of the present embodiment estimates the amount of movement of the traveling vehicle 1 in the section 8A by scan matching. That is, in the surveying device 100, the carriage movement amount estimation switching unit 108 selects the movement amount estimated by the scan matching processing unit 106 for the movement amount in the section 8A.
Moreover, the surveying apparatus 100 estimates the movement amount of the traveling vehicle 1 in the section 8B by odometry. That is, with respect to the movement amount of the section 8B, the cart movement amount estimation switching unit 108 selects the movement amount calculated by the odometry processing unit 107.
Further, the surveying device 100 estimates the movement amount of the traveling vehicle 1 in the section 8C by scan matching. That is, with respect to the movement amount of the section 8C, the cart movement amount estimation switching unit 108 selects the movement amount estimated by the scan matching processing unit 106.
The movement amount estimation method is switched in each section by the carriage movement amount estimation switching unit 108 based on the switching instruction information input by the operator from the operator operation terminal 104.
In the present embodiment, while the posture of the arm 2 is changing, the movement amount is calculated by odometry to solve the inconvenience described with reference to FIG. 7.

Next, the flow of the movement amount estimation processing of the surveying instrument 100 will be described.
FIG. 9 is a flowchart showing an example of the movement amount estimation processing of the surveying device according to the first embodiment of the present invention.
As a premise, it is assumed that the traveling vehicle 1 is traveling in the map information creation target range. In the meantime, the laser range sensor 101 scans with laser light at predetermined time intervals, and outputs the point cloud information measured each time to the scan matching processing unit 106 and the coordinate conversion processing unit 109. Further, the sensor (link mechanism) 102 outputs information on the posture of the arm 2 (angle of the joint portion) to the sensor position/posture calculation unit 105 at predetermined time intervals. Further, the sensor (moving body) 103 outputs information regarding the traveling state of the traveling vehicle 1 (tire rotation speed, steering angle) to the odometry processing unit 107 at predetermined time intervals.
Further, during traveling, the arm 2 changes its posture as necessary according to an instruction from the operator. Further, when changing the posture of the arm 2, the operator inputs instruction information to the operator operating terminal 104 so as to switch the method of calculating the movement amount to odometry. When the posture of the arm 2 is fixed, the operator inputs instruction information to the operator operation terminal 104 to switch the method of calculating the movement amount to scan matching.

(Movement estimation)
First, the operator determines whether to operate the link mechanism (step S11). That is, the operator determines whether to change the posture of the arm 2. When operating (step S11; Yes), the operator inputs information instructing the estimation of the movement amount by odometry to the operation terminal 104. The truck movement amount estimation switching unit 108 acquires this information and sets the movement amount estimation method to odometry. Next, the process proceeds to step S15.

On the other hand, when the operation is not performed (step S11; No), the operator inputs information instructing the estimation of the movement amount by scan matching to the operation terminal 104. The dolly movement amount estimation switching unit 108 acquires this information and sets the movement amount estimation method to scan matching. Next, the operator determines whether to end the movement of the traveling vehicle 1 (step S12). When the processing is to be ended (step S12; Yes), this processing flow is ended.

If not completed (step S12; No), the surveying instrument 100 estimates the movement amount by scan matching. First, the sensor position/orientation calculation unit 105 acquires sensor detection information regarding the attitude of the arm 2 such as the turning angles of the joint units 3a and 3b from the sensor (link mechanism) 102 (step S13). When the arm 2 has a linear motion mechanism, the sensor position/posture calculation unit 105 acquires stroke information of the arm 2. The sensor position/orientation calculation unit 105 uses the acquired sensor detection information to calculate coordinate information of the laser range finder sensor 101 in the robot coordinate system. The coordinate information of the laser range sensor 101 in the robot coordinate system is used, for example, when the point cloud information is coordinate-converted into map coordinates when map information is created. The sensor position/orientation calculation unit 105 outputs the calculated coordinate information to the scan matching processing unit 106. The scan matching processing unit 106 also acquires the point cloud information measured by the laser range sensor 101. The scan matching processing unit 106 starts scan matching (step S14). More specifically, the scan matching processing unit 106 performs a process of superimposing the point cloud information acquired at a predetermined time on the point cloud information acquired at every predetermined time, and the two point cloud information are best overlapped. At this time (when the distance between the two point group information is minimum), the translation amount and the rotation amount are calculated, and the movement amount for each minute time is obtained. The scan matching processing unit 106 estimates the movement amount of the traveling vehicle 1 by integrating the movement amount for each minute time that is obtained every moment. The dolly movement amount estimation switching unit 108 acquires the movement amount estimated by the scan matching processing unit 106 and outputs it to the coordinate conversion processing unit 109. This is the movement amount estimation by scan matching in this embodiment.
While the scan matching processing unit 106 estimates the movement amount, the processing from step S11 is repeated.

In step S11, when the operator inputs information instructing the estimation of the movement amount by odometry (step S11; Yes), the odometry processing unit 107 starts estimation of the movement amount by odometry (step S15). More specifically, the odometry processing unit 107 acquires information (tire rotation speed, steering angle) regarding the movement of the traveling carriage 1 from the sensor (moving body) 103, and calculates the movement amount of the traveling carriage 1 by odometry. .. The dolly movement amount estimation switching unit 108 acquires the movement amount calculated by the odometry processing unit 107 and outputs it to the coordinate conversion processing unit 109. This is the movement amount estimation by odometry in this embodiment.
In parallel with the odometry processing unit 107 calculating the movement amount, the operator determines whether to operate the link mechanism (step S16). When operating (step S16; Yes), the calculation of the movement amount by the odometry processing unit 107 is continued (step S17). The processing from step S16 is repeated.

On the other hand, when the link mechanism is not operated (step S16; No), the operator inputs information instructing the estimation of the movement amount by scan matching to the operation terminal 104. The cart movement amount estimation switching unit 108 acquires this information and switches the estimation of the movement amount to scan matching.
Next, the operator determines whether to end the movement of the traveling vehicle 1 (step S18). If the processing is to end (step S18; Yes), this processing flow ends.
If not completed (step S18; No), the scan matching processing unit 106 forgets (erases) the point cloud information stored last in the previous scan matching processing (step S19). This means that the amount of movement is not calculated between the previous point cloud information and the current point cloud information across the section where the movement amount calculation by odometry is performed. This is because when the section where the movement amount is calculated by odometry is straddled, the estimated movement amount becomes an inaccurate value due to the fact that there is too much space or the posture of the arm 2 is different.

Next, the sensor position/posture calculation unit 105 acquires sensor information regarding the posture of the arm 2 such as the angles of the joint units 3a and 3b from the sensor (link mechanism) 102 (step S20). The sensor position/orientation calculation unit 105 calculates coordinate information of the laser range sensor 101 in the robot coordinate system. The sensor position/orientation calculation unit 105 outputs the calculated coordinate information to the scan matching processing unit 106. The scan matching processing unit 106 also acquires the point cloud information measured by the laser range sensor 101. The scan matching processing unit 106 restarts the scan matching (step S21). The dolly movement amount estimation switching unit 108 acquires the movement amount estimated by the scan matching processing unit 106 and outputs it to the coordinate conversion processing unit 109. Then, the processing from step S11 is repeated.

The calculation outline of the movement amount estimation of the traveling vehicle 1 in the first embodiment will be described.
The moving amount of the traveling vehicle 1 at time t is x(t). Then, x(t) can be expressed by the following equation.
x(t)=x(t-1)+dx _est (t)
Here, for example, when estimating the two-dimensional position information/posture information, x(t) can be expressed using the XY coordinates and the posture angle θ.
x(t)=[X(t), Y(t), θ]
Note that dx _est (t) represents a movement amount in a minute time, and in the case of scan matching, dx _est (t)=dx _sm (t), and in the case of odometry, dx _est (t)=dx _odom (t). Is.

In addition, the movement amount estimation by scan matching is calculated from the point cloud P(t) acquired this time and the previously acquired P(t−1).
dx _sm (t)= _fsm (P(t), P(t-1))

The estimation of the amount of change by odometry is calculated from the vehicle speed v(t) (tire rotation speed) and the steering angle φ(t).
dx _odom (t)=f _odom (v(t), φ(t))

(Self-location estimation/map information creation)
The coordinate conversion processing unit 109 calculates the position information/posture information of the traveling vehicle 1 in the map coordinate system by integrating the traveling amounts of the traveling vehicle 1 acquired from the vehicle traveling amount estimation switching unit 108 (self-position estimation). For example, the position information and the posture information of the laser range sensor 101 calculated in step S20 and the like are added. Thereby, the position information/posture information of the laser range sensor 101 in the map coordinate system is calculated. Further, the coordinate conversion processing unit 109 adds the position information/posture information in the map coordinate system of the laser range sensor 101 to the point cloud information acquired by the laser range sensor 101, and the coordinate information in the map coordinate system of the obstacle surface. To calculate. Thereby, map information can be created.

According to the present embodiment, in a scene where the posture of the arm 2 is changed, the operator has an operation to stop the scan matching and switch to the estimation of the movement amount by the odometry. Further, when the posture of the arm 2 is fixed, a unit for restarting scan matching is provided by an operation of the operator. Therefore, when the posture of the arm 2 is changed, the movement amount is estimated by odometry, and in other situations, the movement amount is estimated using scan matching with relatively high precision, so that highly accurate self-position estimation is possible. Further, the surveying device 100 can create accurate map information by accurately measuring its own position.

In addition, since there is a means for switching between the scan matching and the odometry (the carriage movement amount estimation switching unit 108) as described above, it is not necessary to limit the operation of the surveying instrument 100 in order to prevent the scan matching from malfunctioning. Is a restriction that the arm 2 must be fixed). Further, according to the present embodiment, the position information/posture information of the traveling vehicle 1 can be accurately estimated even in an environment where GPS positioning is impossible, such as indoors, and accurate map information can be created. With this, according to the present embodiment, the operation range of the surveying instrument 100 can be improved.

The surveying by the surveying instrument 100 is performed as follows, for example. In other words, the arm 2 is moved while being fixed while moving to a place where detailed surveying is performed, and in a place where detailed surveying is required, the posture of the arm 2 is changed to change the laser range sensor 101 from various angles and positions. To obtain point cloud information. At this time, the traveling carriage 1 is also moved in parallel with the measurement to finely adjust the surveying position. With such a surveying method, the movement amount estimation by odometry is performed only for a small distance at the time of position adjustment, so that an estimation error can be suppressed and accurate position estimation can be performed.

Further, generally, in a surveying device for creating map information, a laser range sensor is often fixedly provided on the carriage, and in this case, there is a problem due to a change in posture of the arm 2 as in the present application (FIG. 7). Does not occur. On the other hand, the surveying instrument 100 of the present embodiment is provided with the laser range sensor 101 near the tip of the arm 2, so that it is possible to perform surveying of high places and create map information of high places. In addition, the laser range sensor 101 can be inserted into a narrow place where the traveling carriage 1 cannot enter, and surveying can be performed to create map information. Further, according to the position estimation method of the present embodiment, the problem explained in FIG. 7 caused by the laser range sensor 101 provided at the tip of the arm 2 capable of changing the posture is solved by the movement estimation by the scan matching and the movement by the odometry. This can be overcome by switching the quantity estimation, and the point cloud information measured by the laser range sensor 101 can be coordinate-converted with accurate position information of the traveling vehicle 1. Therefore, it is possible to obtain accurate map information of high places and narrow places that could not be obtained in the past.

<Second embodiment>
Hereinafter, a surveying device according to a second embodiment of the present invention will be described with reference to FIG.
FIG. 10 is a block diagram showing an example of a surveying device according to the second embodiment of the present invention.
Among the configurations according to the second embodiment of the present invention, the same components as the functional units configuring the surveying instrument 100 according to the first embodiment of the present invention are denoted by the same reference numerals, and description thereof will be omitted. The surveying apparatus 100A according to the second embodiment includes a scan matching processing unit 106′ in place of the scan matching processing unit 106 of the first embodiment, and an odometry processing unit 107′ in place of the odometry processing unit 107.
The odometry processing unit 107′ outputs the movement amount calculated by the odometry to the scan matching processing unit 106′ as well as the carriage movement amount estimation switching unit 108.
The scan matching processing unit 106′ uses the movement amount calculated by the odometry processing unit 107′ as an initial value (initial displacement amount) in the scan matching process to estimate the movement amount. The processing of the scan matching processing unit 106' will be described below.

In the second embodiment, the scan matching processing unit 106' performs change amount estimation by scan matching by the following formula. That is, the calculation from the point group P(t) acquired this time and P(t-1) acquired the previous time is similar to the first embodiment, but dx _odom (t) is given as the initial value of scan matching.
dx _sm (t)=f _sm (P(t), P(t-1), dx _odom (t))

For example, when performing scan matching with the ICP algorithm, unless an appropriate initial value (initial value of translation amount and rotation amount when moving so as to overlap two point groups) is given, a local solution is likely to be formed, or calculation is not possible. It may be difficult to converge. Therefore, it is necessary to give an appropriate initial value in order to make the calculation process efficient and stable. Therefore, in the present embodiment, position information based on odometry when the point cloud information is measured at the immediately preceding timing (time t−1) and odometry when the point cloud information is measured at the current timing (time t). The difference with the position information (the amount of movement in a minute time due to odometry) is given as an initial value. That is, in the present embodiment, the scan matching processing unit 106′ causes the traveling vehicle 1 at the minute time (time t−1 to time t) calculated by the odometry processing unit 107′ in the processing of steps S14 and S21 of FIG. The amount of movement by odometry of is given as an initial value for scan matching. The error accumulates when the movement amount is calculated by odometry for a long time, but the movement amount can be calculated relatively accurately during a very short time. By giving this as an initial value (initial displacement), it is possible to suppress the amount of calculation and increase the possibility of quickly and reliably obtaining a solution for scan matching.

An inertial measurement unit (IMU) such as a gyro sensor may be further added as the sensor (moving body) 103 of the second embodiment. Then, the odometry processing unit 107 ′ calculates the movement amount in a minute time based on the three-axis acceleration and the angular velocity measured by the IMU in addition to the odometry, and the scan matching processing unit 106 ′ uses this movement amount as an initial value. You may use. Alternatively, only the movement amount in a minute time calculated based on the measurement result by the IMU may be used as the initial value of scan matching.
The scan matching processing unit 106' performs scan matching using an initial value based on at least one of odometry and IMU. Thereby, the position information and the posture information of the traveling vehicle 1 can be estimated more accurately and at high speed. For example, even if the posture of the arm 2 is fixed, the posture of the laser range finder sensor 101 may change greatly, for example, when the angle of the scaffolding of the traveling carriage 1 changes rapidly. In such a case, if scan matching is performed without giving an appropriate initial value, it may be difficult to search for corresponding points, and scan matching may fail. In this embodiment, the initial displacement can be given by using the data measured by the odometry or IMU, and the scan matching process can be made efficient and stable.

According to the second embodiment, in addition to the effects of the first embodiment, it is possible to improve the accuracy, speed, and robustness of the scan matching processing by the scan matching processing unit 106', and improve the operability.

<Third embodiment>
Hereinafter, a surveying device according to a third embodiment of the present invention will be described with reference to FIGS. 11 to 12.
FIG. 11: is a 1st block diagram which shows an example of the surveying instrument in 3rd embodiment which concerns on this invention.
Of the configurations according to the third embodiment of the present invention, the same parts as the functional units configuring the surveying instrument 100 according to the first embodiment of the present invention are denoted by the same reference numerals, and description thereof will be omitted. The surveying instrument 100B according to the third embodiment includes a switching instruction unit 110 in place of the operator operation terminal 104 of the first embodiment.
The switching instruction unit 110 manages whether to estimate the movement amount of the traveling vehicle 1 by odometry or scan matching. Specifically, the switching instruction unit 110 acquires information regarding the posture change of the arm 2 from the sensor (link mechanism) 102. For example, the switching instruction unit 110 acquires the angle information of the joint units 3a and 3b from the sensor (link mechanism) 102. Further, the switching instruction unit 110 determines a posture change of the arm 2 from the acquired information. The switching instruction unit 110 determines whether to estimate the movement amount of the traveling vehicle 1 by odometry or by scan matching based on the determination of the posture change of the arm 2. More specifically, when it is determined that the movement amount is currently estimated by scan matching, and it is determined that the posture of the arm 2 has changed based on the information acquired from the sensor (link mechanism) 102, the switching instruction unit 110 instructs the cart movement amount estimation switching unit 108 to switch to the movement amount estimation by odometry. Further, when the movement amount is estimated by odometry at present, if it is determined that there is no change in the posture of the arm 2 for a predetermined time based on the information acquired from the sensor (link mechanism) 102, the switching instruction unit 110 causes the carriage to move. The movement amount estimation switching unit 108 is instructed to switch to the movement amount estimation by scan matching. Note that, for example, a current movement amount estimation method is recorded in a storage unit (not shown), and the switching instruction unit 110 reads out this information and determines which method, scan matching or odometry, is used to estimate the movement amount. Identify.

Next, a movement amount estimation process by the surveying device 100B of the third embodiment will be described. In the movement amount estimation processing of the present embodiment, in Steps S11 and S16 of the flowchart of FIG. 9, the switching instruction unit 110 determines whether or not the arm 2 (link mechanism) is operating, instead of the operator, and the determination result is used. A movement amount estimation method is instructed to the trolley movement amount estimation switching unit 108 based on this (whether scan matching or odometry). Other processes are the same as those in the first embodiment.

The surveying instrument 100B according to the third embodiment has means for automating scan matching and odometry switching in accordance with a change in the posture of the arm 2. Therefore, according to the third embodiment, in addition to the effects of the first embodiment, the work load of the operator performing the scan matching switching process is eliminated, and erroneous operation can be prevented.

It should be noted that although FIG. 11 illustrates an example of combining with the first embodiment, it is also possible to combine with the second embodiment. FIG. 12 shows a configuration example when combined with the second embodiment.
FIG. 12 is a second block diagram showing an example of the surveying device in the third embodiment according to the present invention. As illustrated, the surveying device 100B′ includes a switching instruction unit 110 in place of the operator operation terminal 104 in the surveying device 100A of the second embodiment.

<Fourth Embodiment>
Hereinafter, a surveying device according to the fourth embodiment of the present invention will be described with reference to FIGS. 13 to 14.
FIG. 13 is a first block diagram showing an example of a surveying device in the fourth embodiment according to the present invention.
Of the configurations according to the fourth embodiment of the present invention, the same components as the functional units configuring the surveying instrument 100 according to the first embodiment of the present invention are denoted by the same reference numerals, and description thereof will be omitted. The surveying device 100C according to the fourth embodiment includes a switching instruction unit 110 and an operator instruction priority processing unit 111 in addition to the configuration of the first embodiment.
The switching instruction unit 110 determines whether to estimate the movement amount of the traveling vehicle 1 by odometry or scan matching. The function of the switching instruction unit 110 itself is the same as that described in the third embodiment. The switching instruction unit 110 of the present embodiment outputs the determined movement amount estimation method (trolley movement amount estimation switching instruction information) to the operator instruction priority processing unit 111.

The operator instruction priority processing unit 111 acquires the cart movement amount estimation switching instruction information from both the switching instruction unit 110 and the operator operation terminal 104. When the instruction information acquired from the operator operation terminal 104 exists, the operator instruction priority processing unit 111 gives priority to the instruction information acquired from the switching instruction unit 110 and estimates the instruction information acquired from the operator operation terminal 104 by the carriage movement amount. Output to the switching unit 108.

For example, even when the arm 2 is traveling with the posture fixed, there is an environment in which the estimation of the movement amount by scan matching is not suitable. The environment in which the scan matching is not suitable is, for example, a case where the surroundings are surrounded by an obstacle having a low reflectance of laser light, or a case where the road surface has a large unevenness. When the reflectance of the laser light is low, the point cloud information cannot be sufficiently acquired, which is not suitable for scan matching. Moreover, when the change rate of the attitude of the laser range finder sensor 101 is large, scan matching is not suitable. In such a case, the operator inputs information instructing the movement amount estimation by odometry to the operator operation terminal 104. The operator operation terminal 104 outputs this instruction information to the operator instruction priority processing unit 111. For example, immediately after the operator instruction priority processing unit 111 acquires instruction information for switching to the movement amount estimation by scan matching from the switching instruction unit 110 a short time ago and outputs the instruction information to the dolly movement amount estimation switching unit 108. Also, information for instructing the movement amount estimation by the odometry acquired from the operator operation terminal 104 is output to the carriage movement amount estimation switching unit 108. The dolly movement amount estimation switching unit 108 selects the movement amount estimated value by the odometry processing unit 107 and outputs it to the coordinate conversion processing unit 109.

Further, the operator instruction priority processing unit 111 ignores the instruction information acquired from the switching instruction unit 110 until explicitly inputting the instruction information for instructing the cancellation of the instruction information previously acquired from the operator operation terminal 104. It may be controlled to. For example, when the operator inputs the movement amount estimation by odometry in the operator operation terminal 104 in a space surrounded by an obstacle having a low reflectance, the posture of the arm 2 is changed or fixed in the space. Then, even when the switching instruction unit 110 outputs the information instructing the switching of the movement amount estimation method to the operator instruction priority processing unit 111 each time, the operator instruction priority processing unit 111 uses the switching instruction information. Is not output to the cart movement amount estimation switching unit 108. For example, by performing such control, the intention of the operator can be surely reflected.

The surveying apparatus 100C according to the fourth embodiment includes a switching instruction unit 110 that automatically switches between scan matching and odometry switching depending on whether or not the posture of the arm 2 has changed, and an operator regardless of an instruction from the switching instruction unit 110. The operator instruction priority processing unit 111 forcibly switches and maintains the movement amount estimation method in response to the instruction. According to the present embodiment, in addition to the effects of the first embodiment, the work load of the operator performing the scan matching switching process is eliminated, and erroneous operation can be prevented. Furthermore, when the operator determines that the scan matching process is not stable due to topographical conditions or the like, it is possible to switch to the estimation of the movement amount by odometry, so that the estimation error of the movement amount can be reduced.

Note that, although FIG. 13 illustrates an example of combining with the first embodiment, it is also possible to combine with the second embodiment. FIG. 14 shows a configuration example when combined with the second embodiment.
FIG. 14 is a second block diagram showing an example of a surveying device according to the fourth embodiment of the present invention. As illustrated, the surveying device 100C′ includes a switching instruction unit 110 and an operator instruction priority processing unit 111 in addition to the configuration of the surveying device 100A of the second embodiment.

The surveying devices 100, 100A, 100B, 100B', 100C, and 100C' described above have a computer system inside. The process of each process in the surveying instrument 100 and the like described above is stored in a computer-readable recording medium in the form of a program, and the above process is performed by the computer reading and executing the program. Here, the computer-readable recording medium refers to a magnetic disk, a magneto-optical disk, a CD-ROM, a DVD-ROM, a semiconductor memory, or the like. Further, the computer program may be distributed to the computer via a communication line, and the computer that receives the distribution may execute the program.

Further, the program may be for realizing a part of the functions described above. Further, it may be a so-called difference file (difference program) that can realize the above-mentioned functions in combination with a program already recorded in the computer system.

In addition, it is possible to appropriately replace the constituent elements in the above-described embodiments with known constituent elements without departing from the spirit of the present invention. Further, the technical scope of the present invention is not limited to the above-described embodiment, and various modifications can be made without departing from the spirit of the present invention.
For example, although the operator operation terminal 104 is included in the surveying apparatus 100 in FIG. 2 and the like, the operator operation terminal 104 is not included in the surveying apparatus 100, and the surveying apparatus 100 does not include the information received by the operator operation terminal 104. It may be configured to include a function unit for acquiring.
The traveling carriage 1 is an example of a moving body. The cart movement amount estimation switching unit 108 and the coordinate conversion processing unit 109 are an example of a position information estimation unit. The coordinate conversion processing unit 109 is an example of a map information creation unit. The operator operation terminal 104 is an example of an input reception unit.

1... Traveling vehicle 2, 2a, 2b... Arm 40... Control device 50... Obstacle 50'... Obstacle image 100, 100A, 100B, 100B', 100C, 100C'...・Surveying device 101... Laser range sensor 102... Sensor (link mechanism)
103... Sensor (moving body)
104... Operator operation terminal 105... Sensor position/orientation calculation unit 106, 106'... Scan matching processing unit 107, 107'... Odometry processing unit 108... Cart movement amount estimation switching unit 109... ..Coordinate conversion processing unit 110... Switching instruction unit 111... Operator instruction priority processing unit

Claims (10)

Provided on an arm capable of changing its posture, performing range matching on the basis of point group information corresponding to the position of the object indicated by the distance information obtained by scanning the range sensor for measuring the distance to the object, A scan matching processing unit that estimates the movement amount of the range sensor,
An odometry processing unit that estimates the amount of movement of the moving body based on a measurement value of a sensor that measures information about the movement of the moving body provided in the moving body having the arm,
Obtaining instruction information based on the presence or absence of a change in the posture of the arm, based on the instruction information, select one of the movement amount estimated by the odometry processing unit and the movement amount estimated by the scan matching processing unit, A position information estimation unit that estimates the position information of the moving body based on the selected amount of movement,
A position estimation device including. The position information estimation unit selects one of the movement amount estimated by the odometry processing unit and the movement amount estimated by the scan matching processing unit based on the instruction information input by the operator,
The position estimation device according to claim 1. Based on the information measured by the sensor that measures the posture change of the arm, the presence or absence of the posture change of the arm is determined, and based on the determination, the movement amount estimated by the odometry processing unit and the scan matching processing unit are A switching instruction unit that outputs instruction information instructing which one of the estimated movement amounts is selected,
The position estimation device according to claim 1, further comprising: The switching instruction unit gives an instruction to select the movement amount estimated by the odometry processing unit when it is determined that the posture change of the arm has occurred,
The position estimation device according to claim 3. An input receiving unit that receives an input of instruction information as to which of the movement amount estimated by the odometry processing unit and the movement amount estimated by the scan matching processing unit is selected,
Of the instruction information output by the switching instruction unit and the instruction information received by the input receiving unit, operator instruction priority for instructing estimation of the movement amount of the moving body based on the instruction information received by the input receiving unit A processing unit,
The position estimation device according to claim 3, further comprising: In the scan matching calculation, the scan matching processing unit uses an amount of movement of the moving body estimated by the odometry processing unit in a minute time as an initial value.
The position estimation device according to any one of claims 1 to 5. A position estimation device according to any one of claims 1 to 6,
A sensor position/orientation calculation unit that calculates position information based on the moving body of the range sensor according to a change in the attitude of the arm;
The point cloud information obtained by scanning by the range sensor is corrected using the position information calculated by the sensor position/orientation calculation unit and the position information of the moving body estimated by the position estimation device, and map information is obtained. A map information creation unit that creates
A map information creating device. A position estimation device according to any one of claims 1 to 6,
An arm provided with a range sensor that can change its posture and that is provided in the position estimation device,
A mobile body equipped with. The position estimation device
Provided on an arm capable of changing its posture, performing range matching on the basis of point group information corresponding to the position of the object indicated by the distance information obtained by scanning the range sensor for measuring the distance to the object, Estimate the amount of movement of the range sensor,
Estimating the amount of movement of the moving body based on a measurement value of a sensor that measures information about the movement of the moving body provided in the moving body having the arm,
Acquiring instruction information based on whether or not the posture of the arm has changed, based on the instruction information, one of the moving amount of the moving body and the moving amount of the range sensor is selected, and based on the selected moving amount. Estimate the position information of the moving body,
Location estimation method. The computer of the position estimation device
Provided on an arm capable of changing its posture, performing range matching on the basis of point group information corresponding to the position of the object indicated by the distance information obtained by scanning the range sensor for measuring the distance to the object, Means for estimating the amount of movement of the range sensor,
Means for estimating the amount of movement of the moving body based on a measurement value of a sensor that is provided in the moving body having the arm and measures information about the movement of the moving body;
Acquiring instruction information based on whether or not the posture of the arm has changed, based on the instruction information, one of the moving amount of the moving body and the moving amount of the range sensor is selected, and based on the selected moving amount. Means for estimating the position information of the moving body,
Program to function as.

Images