Sensor calibration method, mobile body, control device, and control program

Abstract

The sensor calibration method is a sensor calibration method for calibrating a target sensor that is attached to the mobile body 1 of an autonomously moving mobile body 100 and detects surrounding objects in the form of point cloud data, and includes obtaining a point cloud map around the mobile body 1, detecting objects around the mobile body 1 using the target sensor, determining the position of each point of the point cloud data detected by the target sensor in the reference coordinate system by coordinate converting the position of each point in the sensor coordinate system of the target sensor to a reference coordinate system, and calibrating the sensor coordinate system so that the positional deviation between corresponding points in the point cloud map and the point cloud data in the reference coordinate system is small.

Description

[Technical Field]

[0001] The technology disclosed herein relates to a sensor calibration method, a moving body, a control device, and a control program. [Background technology]

[0002] Patent Document 1 discloses a mobile body control system for controlling a mobile body. The mobile body is equipped with a ranging sensor that acquires point cloud data of objects around the mobile body. The mobile body moves autonomously while avoiding obstacles based on the point cloud data acquired by the ranging sensor. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Application Publication No. 2023-176361 Summary of the Invention

[0004] A mobile object may be equipped with multiple sensors that acquire point cloud data. Each sensor is attached to a predetermined position on the mobile object. The attachment position of each sensor may include an attachment error. Furthermore, the attachment position of each sensor may shift due to vibrations during the movement of the mobile object. In such a case, the relative positional relationship between the sensors may shift, which may result in inappropriate detection of obstacles and adversely affect the autonomous movement of the mobile object. For this reason, it is desirable to calibrate the sensors with high accuracy in order to correct the relative positional relationship between the sensors.

[0005] The technology disclosed herein has been made in view of the above points, and its purpose is to improve the accuracy of sensor calibration.

[0006] The sensor calibration method disclosed herein is a sensor calibration method for calibrating a target sensor that is attached to the mobile body of an autonomously moving mobile body and detects surrounding objects in the form of point cloud data, and includes acquiring a point cloud map around the mobile body, detecting objects around the mobile body using the target sensor, determining the position of each point of the point cloud data detected by the target sensor in the sensor coordinate system of the target sensor by coordinate transforming the position of each point of the point cloud data detected by the target sensor into a reference coordinate system, and calibrating the sensor coordinate system so that the positional deviation between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is small.

[0007] The mobile body disclosed herein is an autonomously moving mobile body that comprises a mobile body main body, a target sensor attached to the mobile body main body that detects surrounding objects in the form of point cloud data, and a control device that calibrates the sensor coordinate system of the target sensor, wherein the control device acquires a point cloud map of the surroundings of the mobile body main body, detects objects around the mobile body main body using the target sensor, and determines the position of each point of the point cloud data detected by the target sensor in the sensor coordinate system of the target sensor by coordinate transforming the position of each point of the point cloud data detected by the target sensor into a reference coordinate system, and calibrates the sensor coordinate system so that the positional deviation between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is small.

[0008] The control device disclosed herein is a control device that is attached to the mobile body of an autonomously moving mobile body and calibrates a target sensor that detects surrounding objects in the form of point cloud data, acquires a point cloud map around the mobile body, detects objects around the mobile body using the target sensor, determines the position of each point of the point cloud data detected by the target sensor in the sensor coordinate system of the target sensor by coordinate transforming the position of each point of the point cloud data detected by the target sensor into a reference coordinate system, and calibrates the sensor coordinate system so that the positional deviation between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is small.

[0009] The control program disclosed herein is a control program for calibrating a target sensor that is attached to the mobile body of an autonomously moving mobile body and detects surrounding objects in the form of point cloud data, and causes a computer to realize the following functions: a function for acquiring a point cloud map around the mobile body; a function for detecting objects around the mobile body using the target sensor; a function for determining the position of each point of the point cloud data detected by the target sensor in the sensor coordinate system of the target sensor by coordinate transforming the position of each point of the point cloud data in the reference coordinate system; and a function for calibrating the sensor coordinate system so that the positional deviation between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is reduced.

[0010] According to the sensor calibration method, the accuracy of sensor calibration can be improved.

[0011] According to the mobile body, the accuracy of calibration of the sensor can be improved.

[0012] According to the control device, the accuracy of calibration of the sensor can be improved.

[0013] According to the control program, the accuracy of calibration of the sensor can be improved. [Brief explanation of the drawings]

[0014] [Figure 1] FIG. 1 is a perspective view of a moving object. [Figure 2] FIG. 2 is a schematic diagram showing the detection range of the sensor. [Figure 3] FIG. 3 is a diagram illustrating a hardware configuration of the control device. [Figure 4] FIG. 4 is a functional block diagram showing the configuration of the control system of the processor. [Figure 5] FIG. 5 is a flowchart of the basic operation of a mobile unit. [Figure 6] FIG. 6 is a flowchart showing a method for calibrating a sensor. [Figure 7] FIG. 7 is a flowchart of a subroutine of the method for extracting points. [Figure 8] FIG. 8 is an explanatory diagram for explaining a method of extracting points. [Figure 9] FIG. 9 is a flowchart of a subroutine of the calibration method. [Figure 10] FIG. 10 is a flowchart of a subroutine of the cost function calculation method. [Figure 11] FIG. 11 is a functional block diagram showing the configuration of a control system of a processor according to a modified example. [Figure 12] FIG. 12 is a side view of the mobile body when the robot arm is in the running configuration. [Figure 13] FIG. 13 is a plan view of the mobile body when the robot arm is in the running configuration. [Figure 14] FIG. 14 is a side view of a moving body according to a modified example. DETAILED DESCRIPTION OF THE INVENTION

[0015] Exemplary embodiments will be described in detail below with reference to the drawings. FIG. 1 is a perspective view of a moving body 100. The moving body 100 moves autonomously. The moving body 100 includes a moving body main body 1 and a control device 6 that causes the moving body main body 1 to move autonomously. For example, the moving body 100 moves within a facility such as a store, hospital, or nursing home. In addition to moving, the moving body 100 may also perform tasks such as handing over an item or opening and closing a door.

[0016] For example, the mobile body 1 is a mobile robot that includes a robot arm 12. In detail, the mobile body 1 may have a carriage 10, a base 11 mounted on the carriage 10, and a robot arm 12 connected to the base 11.

[0017] The bogie 10 has a defined front-to-rear direction. In this example, the bogie 10 has a generally rectangular planar shape. For example, the longitudinal direction of the rectangle is the front-to-rear direction. The lateral direction of the rectangle is the left-to-right direction.

[0018] The dolly 10 includes a plurality of wheels 13 and is capable of traveling. In this example, the dolly 10 includes four wheels 13. The dolly 10 may be capable of traveling straight and turning. In this example, the dolly 10 is capable of moving forward, backward, left, right, and diagonally while maintaining its posture, i.e., moving in all directions. In other words, the dolly 10 may be capable of translational movement in directions other than the forward and backward direction. Furthermore, the dolly 10 may also be capable of rotating on the spot. For example, the four wheels 13 include a set of wheels 13 aligned in the left-right direction at the front of the bottom of the dolly 10 and another set of wheels 13 aligned in the left-right direction at the rear of the bottom of the dolly 10. The wheels 13 may be arranged to form a rectangle on the bottom of the dolly 10. More specifically, the four wheels 13 are arranged at the four corners of the bottom of the dolly 10.

[0019] Specifically, the wheel 13 may be an omnidirectional wheel. In this example, the wheel 13 is a Mecanum wheel. The wheel 13 has a plurality of barrel-shaped rollers arranged around the outer periphery of the wheel. For example, the rotation axis of each roller is inclined at 45 degrees relative to the axle of the wheel 13.

[0020] The mobile body 1 may have a motor 13a that drives the wheels 13 and an encoder 13b that detects the amount of rotation of the motor 13a (see FIG. 3). In this example, the mobile body 1 has four sets of motors 13a and encoders 13b corresponding to the four wheels 13. The four wheels 13 may be independently driven by the corresponding motors 13a.

[0021] The cart 10 may be able to move in any direction in two dimensions using these four wheels 13. For example, the cart 10 can translate or turn in any direction, including forward / backward, left / right, and diagonal. The cart 10 can also rotate on the spot.

[0022] The base 11 may be mounted on the cart 10. In this example, the base 11 has a shape that resembles the upper half of a human body. The base 11 may be fixed to the cart 10 so as not to be movable.

[0023] The moving body 1 has two robot arms 12. A hand 14 may be attached to the tip of each robot arm 12.

[0024] The two robot arms 12 are connected to different portions of the base 11. For example, the two robot arms 12 are connected to different portions of the base 11 in the width direction, which is one direction in a plan view. In other words, the width direction is the direction in which the connection portion of one robot arm 12 to the base 11 and the connection portion of the other robot arm 12 to the base 11 are aligned in a plan view. The base 11 may have a front and a back that face opposite each other in a plan view. For example, the front side of the base 11 is the front, and the back side is the rear, defining the front-to-rear direction. The width direction may be a horizontal direction that is perpendicular to the front-to-rear direction. In other words, the width direction is the left-to-right direction relative to the front-to-rear direction. For example, one robot arm 12 is connected to the left side of the base 11, and the other robot arm 12 is connected to the right side of the base 11.

[0025] When the planar shape of the carriage 10 is a substantially rectangular shape having a longitudinal direction and a lateral direction, the width direction substantially coincides with the lateral direction of the planar shape of the carriage 10.

[0026] For example, as shown in FIG. 1, the robot arm 12 has a plurality of links L and a plurality of joints J that connect the links L. The robot arm 12 is configured to operate in three dimensions. In this example, the robot arm 12 is a multi-joint robot arm. That is, the shape of the robot arm 12 may be freely changed by rotating the joints. The robot arm 12 is supported by a base 11.

[0027] For example, the multiple links L include a first link L1, a second link L2, a third link L3, a fourth link L4, a fifth link L5, a sixth link L6, and a seventh link L7, which are arranged in series from the base 11 side. The seventh link L7 is located at the tip of the robot arm 12. For example, the multiple joints J include a first joint J1, a second joint J2, a third joint J3, a fourth joint J4, a fifth joint J5, a sixth joint J6, and a seventh joint J7, which are arranged in series from the base 11 side. The position and orientation of the seventh link L7 have six degrees of freedom, including translational and rotational directions about three orthogonal axes. The robot arm 12 may be a so-called seven-axis robot having seven joints J. In other words, the robot arm 12 has redundancy. Redundancy is a characteristic in which the rotation angles of the multiple joints J corresponding to the position and orientation of the tip of the robot arm 12 are not uniquely determined.

[0028] The base 11 and the first link L1 are rotatably connected by a first joint J1. The first link L1 and the second link L2 are rotatably connected by a second joint J2. The second link L2 and the third link L3 are rotatably connected by a third joint J3. The third link L3 and the fourth link L4 are rotatably connected by a fourth joint J4. The fourth link L4 and the fifth link L5 are rotatably connected by a fifth joint J5. The fifth link L5 and the sixth link L6 are rotatably connected by a sixth joint J6. The sixth link L6 and the seventh link L7 are rotatably connected by a seventh joint J7.

[0029] A hand 14 may be connected to a seventh link L7 at the tip of the robot arm 12. In other words, the hand 14 is connected to the robot arm 12 so as to be rotatable around the rotation axis of the seventh joint J7. The hand 14 is an end effector attached to the robot arm 12.

[0030] In more detail, the multiple joints J may include a joint that functions as a shoulder joint. For example, the multiple joints J include a joint that has the functions of horizontal extension and horizontal flexion in the shoulder joint. The rotation axis of the joint that has the functions of horizontal extension and horizontal flexion in the shoulder joint extends in a substantially vertical direction. The multiple joints J may include a joint that has the functions of extension and flexion in the shoulder joint. The rotation axis of the joint that has the functions of extension and flexion in the shoulder joint extends in a substantially horizontal direction.

[0031] For example, the first joint J1 functions as a shoulder joint of the robot arm 12. The first joint J1 may have the functions of horizontal extension and horizontal flexion at the shoulder joint. The rotation axis of the first joint J1 extends in a substantially vertical direction.

[0032] For example, the second joint J2 functions as a shoulder joint of the robot arm 12. The second joint J2 may have the functions of extension and flexion at the shoulder joint. The rotation axis of the second joint J2 extends in a substantially horizontal direction.

[0033] For example, the third joint J3 functions as a shoulder joint of the robot arm 12. The third joint J3 may have the function of internal rotation and external rotation in a shoulder joint.

[0034] The multiple joints J may include a joint that functions as a wrist joint. For example, the multiple joints J include a joint that has the functions of internal rotation and external rotation or the functions of pronation and supination at the wrist joint. For example, the seventh joint J7 may have the functions of internal rotation and external rotation at the wrist joint. The sixth joint J6 may have the functions of pronation and supination at the wrist joint.

[0035] The multiple joints J may include an intermediate joint between a shoulder joint and a wrist joint. The intermediate joint may also be referred to as an elbow joint. The intermediate joint may have functions of extension and flexion at the intermediate joint, or functions of internal rotation and external rotation at the intermediate joint. The fourth joint J4 may have functions of extension and flexion at the intermediate joint. The fifth joint J5 may have functions of internal rotation and external rotation at the intermediate joint.

[0036] The robot arm 12 has a motor 12a (see FIG. 3) that rotates and drives each joint J. For example, the motor 12a is a servo motor. Each motor 12a has an encoder 12b (see FIG. 3).

[0037] The mobile body 100 may include a sensor 3 that detects objects around the mobile body 1 (hereinafter simply referred to as "peripheral objects") in the form of point cloud data. In this disclosure, "objects" includes both inanimate and animate objects. The sensor 3 is attached to the mobile body 1. For example, the sensor 3 is attached to a dolly 10. In this example, the sensor 3 is a distance measurement sensor that measures the distance from the sensor 3 to the peripheral objects. For example, the sensor 3 is a LiDAR (Light Detection and Ranging) sensor. The sensor 3 has, for example, a light-emitting unit that irradiates measurement light, specifically laser light, toward the periphery of the mobile body 1 and a light-receiving unit that receives the measurement light reflected off the surface of the peripheral object. The sensor 3 measures the flight time of the measurement light irradiated from the light-emitting unit, which hits the surface of the peripheral object and returns to the light-receiving unit. The sensor 3 measures the distance from the sensor 3 to the surface of the peripheral object based on the measured flight time. The sensor 3 may generate point cloud data based on the measured distance. The point cloud data is three-dimensional position information of the surface of the peripheral object. For example, the sensor 3 outputs the calculated point cloud data to the control device 6. The sensor 3 may repeatedly detect surrounding objects at a predetermined detection period while the mobile body 1 is moving. The sensor 3 may output the detection result of the sensor 3, i.e., the point cloud data, to the control device 6 every time the sensor 3 detects a surrounding object.

[0038] In this example, the moving body 100 is equipped with a plurality of sensors 3. Fig. 2 is a schematic diagram showing the detection ranges of the sensors 3. Fig. 2 is a plan view of the moving body 100, with the robot arm 12 and the like omitted. The moving body 100 may be equipped with a first sensor 3A, a second sensor 3B, and a third sensor 3C.

[0039] The first sensor 3A detects an object at least in front of the mobile body 1. The first sensor 3A is attached to the front of the carriage 10. For example, the first sensor 3A is attached to the carriage 10, forward of the base 11 and approximately in the center in the left-right direction. The first sensor 3A detects an object in the three-dimensional space around the mobile body 1. The first sensor 3A may be a 3D LiDAR. The first sensor 3A scans the measurement light in the horizontal and vertical directions. In this example, the first sensor 3A scans the measurement light 360 degrees in the horizontal direction, as shown by the two-dot chain line in FIG. 2. In the vertical direction, the first sensor 3A scans the measurement light within a predetermined range including an elevation angle and a depression angle. The scanning range of the measurement light is the object detection range of the first sensor 3A.

[0040] The second sensor 3B and the third sensor 3C detect objects at least behind the mobile body 1. The second sensor 3B and the third sensor 3C may be attached to the rear of the carriage 10. More specifically, the second sensor 3B and the third sensor 3C may be attached to a part of the carriage 10 behind the base 11. The second sensor 3B is attached to the left rear corner of the carriage 10, and the third sensor 3C is attached to the right rear corner of the carriage 10. The second sensor 3B and the third sensor 3C may detect objects in a horizontal two-dimensional space around the mobile body 1.

[0041] The third sensor 3C has a detection range that at least partially overlaps with the detection range of the second sensor 3B. That is, the object detection range of the second sensor 3B and the object detection range of the third sensor 3C at least partially overlap. For example, the second sensor 3B and the third sensor 3C are 2D LiDARs. The second sensor 3B and the third sensor 3C scan the measurement light in the horizontal direction. The second sensor 3B and the third sensor 3C detect objects in at least a range in the horizontal direction where the first sensor 3A cannot detect objects. The second sensor 3B scans the measurement light at least to the left rear of the dolly 10. The third sensor 3C scans the measurement light at least to the right rear of the dolly 10. The scanning range of the measurement light by the second sensor 3B and the scanning range of the measurement light by the third sensor 3C partially overlap behind the dolly 10. In this example, the second sensor 3B scans the measurement light horizontally by approximately 270 degrees from the front to the right, including the left area of ​​the mobile body 1, as shown by the dashed line in FIG. 2. The third sensor 3C scans the measurement light horizontally by approximately 270 degrees from the front to the left, including the right area of ​​the mobile body 1, as shown by the dashed line in FIG. 2. The second sensor 3B and the third sensor 3C detect objects at approximately the same height. That is, the scanning plane of the measurement light by the second sensor 3B and the scanning plane of the measurement light by the third sensor 3C are at approximately the same height. The scanning ranges of the measurement light of each of the second sensor 3B and the third sensor 3C are the object detection ranges of each of the second sensor 3B and the third sensor 3C.

[0042] 2, since the base 11 is disposed behind the first sensor 3A, the first sensor 3A cannot properly scan the measurement light into the range F that overlaps with the base 11. On the other hand, since the second sensor 3B and the third sensor 3C are disposed behind the base 11, the second sensor 3B and the third sensor 3C can also scan the measurement light into the range F.

[0043] Hereinafter, when the first sensor 3A, the second sensor 3B, and the third sensor 3C are not distinguished from one another, they will be simply referred to as "sensors 3." The first sensor 3A is an example of "another sensor." The second sensor 3B and the third sensor 3C are examples of "target sensors." That is, in this example, the second sensor 3B and the third sensor 3C are the sensors to be calibrated. In the following description, the second sensor 3B will also be referred to as the "first target sensor 3B." The third sensor 3C will also be referred to as the "second target sensor 3C." When the second sensor 3B and the third sensor 3C are not distinguished from one another, they will also be simply referred to as the "target sensors."

[0044] FIG. 3 is a diagram showing the hardware configuration of the control device 6. The control device 6 controls the entire mobile body 1. The control device 6 causes the mobile body 1 to move autonomously while estimating the self-position of the mobile body 1. The control device 6 operates the motor 13a of the wheel 13 to move the mobile body 1. Furthermore, the control device 6 controls the motor 12a of the robot arm 12 to cause the robot arm 12 to perform a predetermined task. The control device 6 has a processor 61, a storage device 62, and a memory 63.

[0045] The processor 61 performs various types of arithmetic processing. For example, the processor 61 is formed of a processor such as a CPU (Central Processing Unit). The processor 61 may be formed of an MCU (Micro Controller Unit), an MPU (Micro Processor Unit), an FPGA (Field Programmable Gate Array), a PLC (Programmable Logic Controller), a system LSI, or the like. The processor 61 operates the motor 13a, causing the mobile body 1 to drive autonomously.

[0046] The memory 62 stores programs and various data executed by the processor 61. For example, the memory 62 stores a control program. The memory 62 stores map information related to a map of the environment in which the mobile body 1 moves. For example, the map information includes a three-dimensional map and a two-dimensional map. The three-dimensional map is formed from three-dimensional point cloud data. For example, the three-dimensional map is a three-dimensional point cloud map. The three-dimensional point cloud map is an example of a point cloud map. In the following description, the three-dimensional point cloud map will be simply referred to as a "point cloud map." In a three-dimensional map, the three-dimensional shapes of obstacles in the environment, such as walls, ceilings, handrails, shelves, tables, or chairs, are represented by point cloud data. A two-dimensional map is a planar map. For example, a two-dimensional map is a two-dimensional occupancy grid map. In a two-dimensional map, the planar shapes of obstacles in the environment, such as walls, ceilings, handrails, shelves, tables, or chairs, are represented. For example, a two-dimensional map is formed by projecting a three-dimensional map onto a plane. The storage device 62 is formed of a non-volatile memory, a hard disk drive (HDD), a solid state drive (SSD), etc. The memory 63 temporarily stores data, etc. For example, the memory 63 is formed of a volatile memory.

[0047] Here, various coordinate systems will be explained. In this disclosure, the coordinate system of the point cloud map is referred to as the "map coordinate system." The map coordinate system is also referred to as the global coordinate system. A coordinate system defined based on the mobile body 1 is referred to as the "mobile body coordinate system." A coordinate system defined based on the sensor 3 is referred to as the "sensor coordinate system."

[0048] FIG. 4 is a functional block diagram showing the configuration of the control system of the processor 61. The processor 61 realizes various functions by reading a control program from the storage device 62 into the memory 63 and expanding it. Specifically, the processor 61 functions as a state estimator 64 that estimates the state of the mobile body 1, a map generator 65 that generates a map of the environment in which the mobile body 1 moves, a path generator 66 that plans a path for the mobile body 1, a trajectory generator 67 that generates a target trajectory according to the path, a movement controller 68 that moves the mobile body 1 according to the target trajectory, a calibrator 610 that calibrates the target sensors, and a corrector 611 that corrects the coordinates of each point in the point cloud data (hereinafter referred to as "sensing data") detected by the target sensors. The processor 61 also functions as an operation amount calculator 69 that calculates the operation amount of the motor 13a.

[0049] The state estimator 64 performs self-position estimation to estimate the position and attitude of the mobile body 100 in a map coordinate system. The state estimator 64 receives the detection results of the sensor 3, the detection results of the encoder 13b, and the map information in the memory 62. The map information is, for example, a three-dimensional map. The state estimator 64 compares the detection results of the sensor 3 with the map information to estimate the current position of the mobile body 1, i.e., its self-position. Here, the position of the mobile body 1 also includes the orientation of the mobile body 1, i.e., its attitude.

[0050] In this example, the state estimator 64 performs self-position estimation using the three-dimensional point cloud data of the first sensor 3 A. The state estimator 64 compares environmental information around the mobile body 1 obtained from the three-dimensional point cloud data of the first sensor 3 A with a three-dimensional map, and estimates the position of the mobile body 1 within the environment represented by the three-dimensional map, i.e., the self-position.

[0051] The map generator 65 generates a map based on the detection results of the sensor 3. Specifically, the map generator 65 generates or modifies a three-dimensional map based on the detection results of the sensor 3. In this example, before autonomous movement is performed, the three-dimensional map is generated using SLAM (Simultaneous Localization and Mapping) technology. Specifically, while the mobile body 1 is moving within the environment, the state estimator 64 and the map generator 65 acquire the detection results of the sensor 3 and perform self-position estimation and map generation in parallel. The generated map information, i.e., the three-dimensional map, is stored in the memory 62. When generating the map before autonomous movement is performed, the mobile body 1 is moved by manual operation by the user.

[0052] Furthermore, the map generator 65 updates the two-dimensional map. The two-dimensional map can also be updated during autonomous movement. The map generator 65 detects obstacles in the environment based on the detection results of the sensor 3 acquired while the mobile body 1 is moving, and updates the two-dimensional map.

[0053] The route generator 66 reads the destination and map information from the memory 62. The destination is set in advance in the memory 62. The map information at this time is, for example, a two-dimensional map. At this time, the route generator 66 may read intermediate points in addition to the destination. The state quantities (including the estimated position) of the mobile body 1 are input to the route generator 66 from the state estimator 64.

[0054] The path generator 66 generates a path from the current position of the mobile body 1 to the destination based on map information. The path generator 66 references the map information to generate a path that avoids interference with obstacles, etc. If a passage is set in the environment, the path generator 66 generates a path along the passage. For example, the path generator 66 generates a path using an A-star search algorithm, an RRT algorithm, a Dijkstra algorithm, or a geometric approach. The path generator 66 outputs an array of positions through which the mobile body 1 passes as a path to the trajectory generator 67. Each position includes the attitude of the mobile body 1 in addition to position information.

[0055] The trajectory generator 67 generates a target trajectory from the current position of the mobile body 1 according to the generated path. The trajectory generator 67 generates the target trajectory of the mobile body 1 using a predetermined method (for example, line-of-sight guidance law). The state quantities of the mobile body 1 are input to the trajectory generator 67 from the state estimator 64. The trajectory generator 67 calculates a command speed for the mobile body 1.

[0056] Alternatively, the trajectory generator 67 may calculate the command speed by model predictive control (MPC). Model predictive control determines a control input, i.e., a speed command, by sequentially solving an optimization problem based on a model of the mobile body 1. The trajectory generator 67 predicts future state quantities from the current state quantities of the mobile body 1 and obstacles, calculates an optimal path for the mobile body 1, and calculates a moving speed from the current position to the target position to follow that path as a command speed.

[0057] The command speed calculated by the trajectory generator 67 is input to the movement controller 68. The movement controller 68 outputs a command value according to the command speed to the operation amount calculator 69.

[0058] The movement controller 68 executes control to avoid interference between the mobile body 1 and an obstacle. The movement controller 68 monitors the approach of the mobile body 1 to an obstacle based on the detection results of the sensors 3. In this example, the movement controller 68 monitors the approach of the mobile body 1 to an obstacle using all of the detection results of the first sensor 3A, the second sensor 3B, and the third sensor 3C. For example, the movement controller 68 slows down or stops the mobile body 1 depending on the distance between the mobile body 1 and the obstacle.

[0059] The manipulated variable calculator 69 distributes the command value to the plurality of motors 13a and calculates the command manipulated variable for each of the plurality of motors 13a. For example, the manipulated variable is the rotation speed or torque of the motor.

[0060] Each motor 13a operates according to a command operation amount. In some cases, the motor 13a is provided with a dedicated controller for operating the motor 13a. For example, if the motor 13a is a servo motor, the motor 13a further includes a servo amplifier. In this case, the servo amplifier operates the motor 13a according to the command operation amount. As a result, the mobile body 1 moves.

[0061] The calibrator 610 calibrates the sensor coordinate system of the target sensor based on the point cloud map generated by the map generator 65 and the sensing data detected by the target sensor. Specifically, the calibrator 610 calculates a coordinate transformation matrix for calibrating the sensor coordinate system. The sensor coordinate system is defined based on the target sensor. The position of each point in the sensing data is basically represented based on the sensor coordinate system. The position of each point in the sensing data is transformed into various coordinate systems depending on the intended use of the sensing data. For example, the position of each point represented in the sensor coordinate system is transformed into a mobile body coordinate system. Since the target sensor is attached to the mobile body 1, the relationship of the sensor coordinate system with respect to the mobile body coordinate system is fixed. In other words, the position of each point represented in the sensor coordinate system is transformed into the mobile body coordinate system using the relationship of the sensor coordinate system with respect to the mobile body coordinate system. As a result, each point in the sensing data is represented in a coordinate system based on the mobile body 1. Furthermore, the position of each point represented in the mobile body coordinate system may be transformed into a map coordinate system. For example, the relationship of the mobile body coordinate system to the map coordinate system is determined by self-localization of the mobile body 1. The position of each point expressed in the mobile body coordinate system is transformed into the map coordinate system using the relationship of the mobile body coordinate system to the map coordinate system. As a result, each point of the sensing data is expressed in a coordinate system based on the map.

[0062] Here, if the position or orientation of the target sensor relative to the mobile body 1 is misaligned, or if the optical axis of the target sensor is misaligned, the positions of each point detected by the target sensor will not be properly coordinate-transformed into the mobile body coordinate system. In other words, the positions of each point will be transformed to positions that are misaligned from their actual positions based on the mobile body 1. The calibrator 610 calculates a coordinate transformation matrix for transforming the positions of each point into appropriate positions based on the mobile body 1. In other words, the coordinate transformation matrix is ​​a coordinate transformation matrix that calibrates the sensor coordinate system. When the positions of each point of the sensing data are transformed using the coordinate transformation matrix, the positions of each point are transformed into coordinates in which the misalignment has been calibrated.

[0063] The calibrator 610 compares points in the sensing data with corresponding points in the point cloud map in a common coordinate system, i.e., a reference coordinate system, to determine a coordinate transformation matrix that reduces the positional deviation between the two points. In this process, the calibrator 610 extracts points that are assumed to represent a specific object from the sensing data and the point cloud map, and then compares the extracted points in the sensing data with the extracted points in the point cloud map to determine a coordinate transformation matrix. Specifically, the calibrator 610 extracts points that form a plane from multiple sensing points, extracts points that form a plane from multiple map points, and compares the extracted points. In other words, the specific object is a plane. The reference coordinate system is a coordinate system that serves as a reference for determining the positional deviation between corresponding points. The reference coordinate system may be, for example, a mobile body coordinate system, a map coordinate system, or the like.

[0064] In detail, the calibrator 610 acquires a point cloud map and sensing data. In this example, the calibrator 610 acquires the point cloud map by reading the point cloud map from the memory 62. The calibrator 610 acquires sensing data by detecting objects around the mobile body 1 with the target sensors. That is, the calibrator 610 acquires sensing data by having the target sensors detect objects around the mobile body 1.

[0065] The calibrator 610 extracts points that form planes of an object in both the point cloud map and the sensing data. That is, the calibrator 610 removes points other than those that form planes from both the point cloud map and the sensing data. Points that form planes are points that represent the planes of an object that has a plane, such as a wall or a door. Points other than those that form planes are points that represent non-planar parts such as curved surfaces or edges.

[0066] The calibrator 610 determines the position of each point of the sensing data in the reference coordinate system by coordinate-transforming the position of each point of the sensing data in the sensor coordinate system to the reference coordinate system. In this example, the calibrator 610 determines the position of each point of the point cloud map in the map coordinate system to the reference coordinate system by coordinate-transforming the position of each point of the point cloud map in the map coordinate system to the reference coordinate system. Furthermore, the calibrator 610 determines a coordinate transformation matrix for coordinate-transforming the position of each point of the sensing data in the reference coordinate system so as to reduce the positional deviation between corresponding points between the point cloud map and the sensing data in the reference coordinate system. In this example, the calibrator 610 determines a coordinate transformation matrix for coordinate-transforming the position of each point of the sensing data in the reference coordinate system so as to reduce the positional deviation between corresponding points between the point cloud map and the sensing data in the reference coordinate system, as well as the positional deviation between corresponding points between the sensing data of the first target sensor 3B and the sensing data of the second target sensor 3C in the reference coordinate system. The calibrator 610 stores the determined coordinate transformation matrix in the memory 62.

[0067] More specifically, the calibrator 610 calculates a cost associated with the positional shift between corresponding points in the sensed data and the point cloud map in the reference coordinate system for each of the multiple sets of sensed data. In this example, the calibrator 610 further calculates a cost associated with the positional shift between corresponding points in the sensed data of the first target sensor 3B and the sensed data of the second target sensor 3C in the reference coordinate system. The calibrator 610 calculates a cost function by summing up multiple costs associated with the multiple sets of sensed data. The calibrator 610 calculates a coordinate transformation matrix for transforming the position of each point in the sensed data in the reference coordinate system so as to minimize the cost function.

[0068] The corrector 611 corrects the coordinates of each point of the sensing data using the coordinate transformation matrix obtained by the calibrator 610. In this example, the corrector 611 corrects the coordinates of each point of the sensing data when sensing data is input from a target sensor while the moving body 100 is autonomously moving. When the sensing data of the second sensor 3B and the third sensor 3C are used by the movement controller 68 or the like as described above, the sensing data is corrected using the coordinate transformation matrix stored in the memory 62. As a result, the sensing data is corrected to an appropriate position with the moving body main body 1 as the reference.

[0069] Next, a description will be given of the basic operation of the moving body 100. Fig. 5 is a flowchart of the autonomous movement of the moving body 100. In this example, the control device 6 performs the autonomous movement by repeatedly executing the processing of the flowchart in Fig. 5 at a predetermined cycle.

[0070] First, in step S1, the state estimator 64 acquires surrounding environment information. Specifically, the state estimator 64 acquires the detection signal of the sensor 3 and the detection signal of the encoder 13b. If the sensor 3 is the target sensor, the state estimator 64 acquires the detection signal of the sensor 3 corrected by the corrector 611.

[0071] In step S2, the state estimator 64 performs self-localization.

[0072] In step S3, the route generator 66 executes route planning, generating a route for the mobile body 1 based on map information, the estimated position of the mobile body 1, and the destination.

[0073] In step S4, the trajectory generator 67 calculates a command velocity from the estimated position of the mobile body 1 so as to follow the generated path.

[0074] In step S5, the movement controller 68 causes the mobile body 1 to operate in accordance with the command speed. At this time, the movement controller 68 monitors the approach of the mobile body 1 to an obstacle using all of the detection results of the first sensor 3A, the second sensor 3B, and the third sensor 3C. When the mobile body 1 approaches an obstacle, the movement controller 68 controls the mobile body 1 so that the mobile body 1 avoids the obstacle. At this time, as described above, the sensing data that are the detection results of the second sensor 3B and the third sensor 3C are corrected to an appropriate position with the mobile body 1 as the reference, so the position of the obstacle is accurately detected and interference between the mobile body 1 and the obstacle is suppressed.

[0075] The moving body 100 autonomously moves to the destination while estimating the self-position of the moving body main body 1 by repeating the processes from step S1 to step S5.

[0076] Next, a calibration method for the target sensor will be described. Calibration is performed at any timing. For example, calibration is performed before the mobile body 100 starts to be used. Alternatively, calibration is performed when a predetermined calibration condition is met while the mobile body 100 is being used. For example, the calibration condition may be that a predetermined period of time has elapsed since the mobile body 100 started to be used, or that a deviation in the position or attitude of the target sensor is detected. Here, the calibration method will be described using an example in which calibration is performed when generating a point cloud map before autonomous movement starts.

[0077] FIG. 6 is a flowchart showing a method for calibrating a target sensor.

[0078] First, in step S101, the map generator 65 creates a point cloud map. As described above, the map generator 65 creates the point cloud map using, for example, SLAM technology. The map generator 65 detects objects around the mobile body 1 using the first sensor 3A while the mobile body 1 is moving, and collects point cloud data of the objects. The map generator 65 creates a point cloud map from the collected point cloud data of the first sensor 3A. The map generator 65 stores the generated point cloud map in the memory 62.

[0079] In step S102, the calibrator 610 detects objects around the mobile body 1 using the target sensor. The calibrator 610 acquires sensing data by having the target sensor detect objects around the mobile body 1 within an area corresponding to the point cloud map. In this example, the calibrator 610 acquires multiple sets of sensing data using the target sensor in multiple situations in which at least one of the position and attitude of the mobile body 1 is different. The state estimator 64 estimates the self-position of the mobile body 1 at the time the sensing data was acquired by the target sensor. The calibrator 610 and the state estimator 64 associate the sensing data with the corresponding estimated position and store them in the memory 62. In this way, multiple sets of sensing data are acquired under multiple situations of the mobile body 1. The situation of the mobile body 1 is defined by the position and attitude of the mobile body 1.

[0080] In this example, the detection of the sensing data is performed in parallel with the generation of the point cloud map. Specifically, while the mobile body 1 moves within the environment, the first sensor 3A acquires point cloud data of surrounding objects to generate the point cloud map, and at the same time, the target sensor acquires point cloud data, i.e., sensing data.

[0081] In step S103, the calibrator 610 acquires the point cloud map from the memory 62.

[0082] In step S104, the calibrator 610 calculates the position of each point of the sensing data in the reference coordinate system by converting the position of each point of the sensing data in the sensor coordinate system of the target sensor into the reference coordinate system. In this example, the reference coordinate system is a mobile body coordinate system. The calibrator 610 converts the position of each point of the sensing data in the sensor coordinate system into the mobile body coordinate system based on the relationship of the sensor coordinate system with respect to the mobile body coordinate system. The relationship of the sensor coordinate system with respect to the mobile body coordinate system is stored in the memory 62. In this way, the position of each point of the sensing data is converted from the sensor coordinate system to the mobile body coordinate system. Furthermore, the calibrator 610 converts the position of each point of the point cloud map in the map coordinate system into the mobile body coordinate system for each of multiple situations of the mobile body 1. Specifically, the calibrator 610 converts the position of each point of the point cloud map in the map coordinate system into the mobile body coordinate system based on the estimated positions of the mobile body 1 associated with multiple sets of sensing data. In this way, a plurality of point cloud maps corresponding to each of a plurality of situations of the mobile body 1 are obtained in the mobile body coordinate system.

[0083] In step S105, the calibrator 610 extracts points that form a plane in each of the point cloud map and the sensing data. In other words, the calibrator 610 removes points other than the points that form a plane in each of the point cloud map and the sensing data. The point extraction method will be described in detail later.

[0084] Next, in step S106, the calibrator 610 calibrates the sensor coordinate system so as to reduce the positional deviation between corresponding points in the reference coordinate system between the point cloud map and the sensing data. Specifically, the calibrator 610 identifies points corresponding to the sensing data extraction points from the point cloud map, and calculates a coordinate transformation matrix for calibration so as to reduce the positional deviation between the two corresponding extraction points. In this example, the calibrator 610 calibrates the sensor coordinate system so as to reduce the positional deviation between corresponding points in the reference coordinate system between the sensing data of the first target sensor 3B and the sensing data of the second target sensor 3C, in addition to the positional deviation between corresponding points in the reference coordinate system between the point cloud map and the sensing data. In detail, the calibrator 610 identifies a point corresponding to an extraction point of the sensing data of the first target sensor 3B from the extraction points of the sensing data of the second target sensor 3C, and calculates a coordinate transformation matrix for calibration so as to reduce the positional deviation of these two corresponding extraction points in addition to the positional deviation between the two corresponding extraction points on the point cloud map and the sensing data. The method for calculating the coordinate transformation matrix for calibration will be described in detail later. The calibrator 610 completes calibration by calculating the coordinate transformation matrix for calibration.

[0085] After the calibration is completed, the position of each point in the point cloud detected by the target sensor is corrected by a coordinate transformation matrix for calibration.

[0086] Next, a method for extracting points from a point cloud map and sensing data will be described in detail. FIG. 7 is a flowchart of a subroutine of the point extraction method. FIG. 8 is an explanatory diagram for explaining the point extraction method. Point extraction is performed for each of a plurality of point cloud maps and a plurality of sensing data. The plurality of sensing data includes sensing data from the second sensor 3B and sensing data from the third sensor 3C. The sensing data from the second sensor 3B and the third sensor 3C each include a plurality of sensing data under different conditions of the position and attitude of the mobile body 1. Point extraction from the sensing data is performed for each type of target sensor and the condition of the mobile body 1.

[0087] Below, point extraction will be described using sensing data as an example. First, in step S201, the calibrator 610 estimates the normal of each point included in the data. In detail, the calibrator 610 obtains an infinitesimal plane defined by the target point (hereinafter referred to as the "target point") and at least two points existing within a predetermined range r1 around the target point for which normal estimation is to be performed, and estimates the normal n of the obtained infinitesimal plane as the normal of the target point. The calibrator 610 performs this estimation of normal n for all points included in the data. In FIG. 8, the normal of each point is represented by a dashed line.

[0088] Next, in step S202, the calibrator 610 determines whether each point included in the data constitutes a plane. Specifically, the calibrator 610 expands the infinitesimal plane determined in step S201 to a predetermined search range r2 for the target point. The calibrator 610 searches for another point located on the plane expanded to the search range r2 and having a normal whose angle with the target point's normal is within a predetermined angle threshold. The search conditions are referred to as "search conditions." The search range r2 is larger than the range r1. The calibrator 610 considers points whose distance from the expanded plane is within a predetermined distance threshold to be points located on the expanded plane. If a point satisfying the search conditions is found, the target point and the other point are presumed to be points on the same plane. In other words, if a point satisfying the search conditions is found, the target point is presumed to be a point on a plane having a predetermined size, i.e., a point that constitutes a plane.

[0089] If a point that satisfies the search conditions is found, calibrator 610 determines that the target point is a point that constitutes a plane. On the other hand, if a point that satisfies the search conditions is not found, calibrator 610 determines that the target point is not a point that constitutes a plane.

[0090] In the example shown in Figure 8, point p2 is located on a plane obtained by enlarging the infinitesimal plane of target point p1. The angle between the normal n1 of target point p1 and the normal n2 of point p2 is within the angle threshold. In this case, target point p1 is determined to be a point that constitutes the plane.

[0091] Thus, if the target point is a point that constitutes a plane, it is highly likely that points near the target point also constitute the same plane as the target point. Therefore, the normal of the infinitesimal plane is highly likely to be the normal formed by the target point. In addition, if the plane formed by the target point is a plane with a certain size, it is highly likely that a point with a normal at approximately the same angle as the target point exists on a plane obtained by expanding the infinitesimal plane to the search range r2. On the other hand, if the target point does not constitute a plane, the infinitesimal plane formed by the target point and its neighboring points is a plane unrelated to the shape of the part where the target point is located, and the normal of the infinitesimal plane may be significantly different from the actual normal of the target point. Even if such a infinitesimal plane is expanded to the search range r2, it is highly unlikely that another point exists on the expanded plane. Even if another point exists on the expanded plane, the target point and the other point are not actually located on the same plane, so the angles of their normals may be significantly different. Therefore, if the above-mentioned search conditions are met, it is highly likely that the target point is a point that constitutes a plane. On the other hand, if the search conditions are not met, it is highly likely that the target point is not a point that constitutes a plane.

[0092] If it is determined that the target point is a point that forms a plane, the calibrator 610 extracts the target point in step S203. The calibrator 610 temporarily stores the extracted target point in the memory 63.

[0093] If it is determined that the target point is not a point that constitutes a plane, then in step S204, calibrator 610 does not extract the target point.

[0094] Then, in step S205, calibrator 610 determines whether or not it has determined for all points included in the data whether or not the points constitute a plane. If the determination has not been completed, calibrator 610 returns to step S202, selects another point included in the data as the target point, and repeats the process from step S202.

[0095] If the determination has been completed for all points included in the data, the calibrator 610 ends point extraction.

[0096] The calibrator 610 performs this point extraction for all point cloud maps. When multiple pieces of sensing data are acquired as in this example, the calibrator 610 performs the same extraction for all of the sensing data.

[0097] Next, the method for calibrating the target sensor will be described in detail with reference to Fig. 9, which is a flowchart of a subroutine for the method for calibrating the target sensor.

[0098] First, in step S301, the calibrator 610 calculates a cost function relating to the positional deviation between corresponding points in the reference coordinate system (in this example, the moving body coordinate system) and the sensing data on the point cloud map. The calculation method of the cost function will be described in detail later.

[0099] In step S302, the calibrator 610 derives a coordinate transformation matrix. That is, the calibrator 610 obtains a coordinate transformation matrix for transforming the position of each point of the sensing data in the reference coordinate system so as to minimize the cost function. In other words, the calibrator 610 obtains a coordinate transformation matrix that minimizes the cost function.

[0100] In step S303, the calibrator 610 determines whether the optimization of the cost function has converged. Specifically, the calibrator 610 determines whether a convergence condition has been satisfied. For example, the convergence condition may be that the ratio of the total cost using the current coordinate transformation matrix to the total cost before calibration (hereinafter referred to as the "cost reduction rate") is equal to or less than a predetermined reduction rate threshold. The convergence condition may be that the difference between the previous cost reduction rate and the current cost reduction rate, i.e., the amount of change, is equal to or less than a predetermined difference threshold. The convergence condition may be that the number of iterations of deriving the coordinate transformation matrix reaches a predetermined threshold. The convergence condition may be a combination of at least two of the three conditions described above, and may be that at least one of the multiple conditions is satisfied.

[0101] The total cost is a value obtained by substituting the derived coordinate transformation matrix into the cost function. In other words, the total cost is a value that comprehensively represents the positional deviation between corresponding points when each point of the sensing data is actually coordinate-transformed using the coordinate transformation matrix. The total cost before calibration is a value obtained using the cost function without transforming the sensing data using coordinate matrix transformation. In other words, the total cost before calibration is a value that comprehensively represents the positional deviation between corresponding points when each point of the sensing data is not coordinate-transformed. In other words, the cost reduction rate represents the overall reduction in the positional deviation between corresponding points when no calibration is performed and when calibration is performed using the current coordinate transformation matrix. If the overall reduction in the positional deviation between corresponding points is equal to or greater than a certain level, the convergence condition for the cost reduction rate is satisfied.

[0102] The difference between the previous cost reduction rate and the current cost reduction rate indicates how much the positional deviation between corresponding points has been reduced overall by re-deriving the coordinate transformation matrix. If the positional deviation between corresponding points does not decrease significantly overall even after re-deriving the coordinate transformation matrix, the convergence condition for the difference in cost reduction rate is met.

[0103] The convergence condition regarding the number of iterations of deriving the coordinate transformation matrix is ​​independent of the cost reduction rate. The convergence condition regarding the number of iterations of deriving the coordinate transformation matrix is ​​satisfied when the derivation of the coordinate transformation matrix is ​​performed a certain number of times or more.

[0104] If the convergence condition is not satisfied, in step S304, the calibrator 610 performs coordinate transformation on each point in each of the multiple sets of sensing data using the derived coordinate transformation matrix. The calibrator 610 then returns to step S301 and repeats the process from the derivation of the cost function. That is, the derivation of the cost function and the coordinate transformation matrix is ​​performed again using sensing data that has been coordinate-transformed using the derived coordinate transformation matrix, so the next coordinate transformation matrix may change from the previous coordinate transformation matrix. In addition, the next coordinate transformation matrix is ​​derived using sensing data in which the positional deviation between corresponding points has been reduced by the coordinate transformation, so the next coordinate transformation matrix may be able to further reduce the positional deviation between corresponding points. The accuracy of the coordinate transformation matrix is ​​improved by repeatedly deriving the coordinate transformation matrix until the convergence condition is satisfied.

[0105] If the convergence condition is satisfied, in step S305, the calibrator 610 stores the coordinate transformation matrix for calibration in the memory 62 and ends this process. For example, the calibrator 610 derives the coordinate transformation matrix for calibration by multiplying all of the coordinate transformation matrices derived by repeating the process of step S302. Each point of the sensing data is subjected to coordinate transformation using the coordinate transformation matrix for calibration.

[0106] Next, the cost function calculation method will be explained in detail. Fig. 10 is a flowchart of a subroutine for the cost function calculation method. The cost function is calculated using points extracted from the point cloud map and sensing data, i.e., points that constitute a plane.

[0107] First, in step S401, the calibrator 610 creates a point cloud map and a data set of sensing data from the first target sensor 3B (second sensor 3B) and sensing data from the second target sensor 3C (third sensor 3C) for each acquisition situation of the mobile body 1. The point cloud map and sensing data are data in which points constituting a plane are extracted.

[0108] Next, in step S402, the calibrator 610 extracts one data set from the multiple data sets and creates two combinations of data from the multiple data included in the extracted data set. Specifically, since the data set includes three types of data: a point cloud map, sensing data from the first target sensor 3B, and sensing data from the second target sensor 3C, the calibrator 610 creates three combinations: a combination of the point cloud map and sensing data from the first target sensor 3B, a combination of the point cloud map and sensing data from the second target sensor 3C, and a combination of sensing data from the first target sensor 3B and sensing data from the second target sensor 3C.

[0109] In step S403, the calibrator 610 extracts one combination from the three created combinations. In step S404, the calibrator 610 searches for corresponding points between the two data sets included in one combination. For example, when the calibrator 610 extracts a combination of a point cloud map and sensing data from the first target sensor 3B, the calibrator 610 searches for corresponding points between the point cloud map and sensing data from the first target sensor 3B. As an example of searching for corresponding points, the calibrator 610 searches for a point in one data set that is located closest to a point in the other data set. The calibrator 610 determines these two closest points as corresponding points. In other words, the calibrator 610 searches for a point in the other data set that detects the same part of the same object as a point in one data set. Note that if there is no point in the other data set within a predetermined distance from a point in one data set, it may be determined that there is no corresponding point. The calibrator 610 searches for corresponding points for all points included in the data set with the fewer points. Note that calibrator 610 may search for corresponding points for a number of points that is less than the total number of points in the data with the smaller number of points.

[0110] In step S405, the calibrator 610 calculates a cost related to the positional deviation between corresponding points in the reference coordinate system. The cost is set based on the distance between a corresponding point of one data (i.e., a corresponding point to be aligned, hereinafter referred to as a "reference corresponding point") and a point of the other data obtained by coordinate transformation using a coordinate transformation matrix (i.e., a corresponding point to be aligned, hereinafter referred to as a "target corresponding point"). In more detail, the cost is set based on the distance d between a plane formed by the reference corresponding points and the target corresponding point. The cost is expressed, for example, by the following equation (1).

[0111] Cost = d 2 ={n1·(p1-p2)} 2 ···(1) In the formula, "·" represents an inner product, n1 represents the unit vector of the normal to the plane formed by the reference corresponding points, p1 represents the coordinates of the reference corresponding points, and p2 represents the coordinates of the target corresponding points.

[0112] The coordinate transformation matrix is ​​a matrix for bringing corresponding points of one data closer to corresponding points of the other data. If the coordinate transformation matrix is ​​appropriate, the distance between the two corresponding points will be small, resulting in a small cost. The coordinate transformation matrix is ​​a variable at the time of calculating this cost. The constants of the coordinate transformation matrix are calculated in step S302 described above. Calibrator 610 calculates the cost for all corresponding points found in step S403.

[0113] In step S406, the calibrator 610 weights the cost of each corresponding point. The weight indicates the importance of the cost. For example, the calibrator 610 may weight the corresponding point according to the angle of incidence of the measurement light from the target sensor onto the corresponding point. The larger the angle of incidence of the measurement light, the greater the distortion of the spot diameter on the object surface. A large distortion of the spot diameter reduces the accuracy of position detection. Therefore, the calibrator 610 may reduce the weight of the cost of the corresponding point as the angle of incidence of the measurement light onto the corresponding point increases. For example, the calibrator 610 may weight the corresponding point according to the reflection intensity of the measurement light at the corresponding point. The smaller the reflection intensity, the lower the accuracy of position detection of the corresponding point. Therefore, the calibrator 610 may reduce the weight of the cost of the corresponding point as the reflection intensity at the corresponding point decreases.

[0114] In step S407, the calibrator 610 adds the calculated cost to the cost function. If the cost is weighted, the calibrator 610 adds the weighted cost to the cost function. The initial value of the cost function is zero. The calibrator 610 adds all the calculated costs to the cost function. In other words, the cost function is expressed by the following equation (2).

[0115] Cost function = Σ(cost × weighting coefficient) (2) In step S408, the calibrator 610 determines whether the addition of costs to the cost function has been completed for all combinations of data included in the dataset. If the addition of costs has not been completed for all combinations, the calibrator 610 returns to step S403 and extracts another combination for which the addition of costs has not been completed. The calibrator 610 then executes the processes from step S404 onwards again.

[0116] The calibrator 610 repeats the processes of steps S403 to S408 to complete the addition of costs to the cost function for all combinations of data included in one data set. That is, for one data set, the cost of corresponding points between the point cloud map and the sensing data of the first target sensor 3B, the cost of corresponding points between the point cloud map and the sensing data of the second target sensor 3C, and the cost of corresponding points between the sensing data of the first target sensor 3B and the sensing data of the second target sensor 3C are added to the cost function.

[0117] If the addition of costs has been completed for all combinations, in step S409, the calibrator 610 determines whether the addition of costs to the cost function has been completed for all data sets. If the addition of costs has not been completed for all data sets, the calibrator 610 returns to step S402 and extracts another data set for which the addition of costs has not been completed. The calibrator 610 performs the processes of steps S403 to S408 for the extracted data set. The calibrator 610 repeats the processes of steps S402 to S408 to complete the addition of costs to the cost function for all data sets. In other words, the addition of costs to the cost function has been completed for data sets of all acquisition situations of the mobile body 1. This completes the calculation of the cost function.

[0118] In such a mobile body 100, the sensor coordinate system of the target sensor is calibrated so as to reduce the positional deviation between corresponding points in the point cloud map and the sensing data in the reference coordinate system. In this example, a point cloud map is generated based on the detection results of the first sensor 3A, and then self-localization is performed. Therefore, the sensor coordinate system of the target sensor is calibrated so as to reduce the positional deviation between corresponding points in the point cloud map and the sensing data in the reference coordinate system, thereby correcting the relative positional relationship between the first sensor 3A and the target sensor to an apparently appropriate positional relationship. As a result, if the mounting accuracy of the first sensor 3A on the bogie 10 is high (i.e., if the first sensor 3A is mounted in an appropriate position on the bogie 10), the positions of each point in the sensing data detected by the target sensor will also accurately reflect the actual position of the obstacle. Furthermore, in this example, the sensor coordinate system is calibrated so that the positional deviation between corresponding points in the reference coordinate system between the point cloud map and the point cloud data, as well as the positional deviation between corresponding points in the reference coordinate system between the sensing data of the first target sensor 3B and the sensing data of the second target sensor 3C, is reduced, thereby accurately grasping the position of the obstacle and suppressing interference between the mobile body 1 and the obstacle.

[0119] The mobile body 100 uses a point cloud map as the point cloud data that serves as the reference for evaluating the positional deviation of each point in the sensing data, thereby improving the accuracy of calibration. If point cloud data obtained by one-shot measurement using one sensor 3 were used as the reference, the number of points included in this reference point cloud data would be relatively small, and the number of points corresponding to the sensing data would also be relatively small. This could result in a decrease in calibration accuracy. On the other hand, the mobile body 100 uses a point cloud map as the reference, allowing for a relatively large number of points corresponding to the sensing data. This improves calibration accuracy. In particular, in this example, multiple sets of sensing data are used as sensing data for determining the positional deviation from points on the point cloud map, allowing for a larger number of corresponding points than when a single piece of sensing data is used. As a result, calibration accuracy can be further improved.

[0120] Furthermore, in this example, the mobile body 100 calculates a cost function that sums multiple costs for multiple sets of sensing data, and calculates a coordinate transformation matrix for coordinate-transforming the position of each point of the sensing data in the reference coordinate system so as to minimize this cost function. That is, the mobile body 100 calculates a cost function using all points included in the multiple sets of sensing data, and optimizes this cost function all at once. If a cost function is calculated for each set of sensing data and optimized, the cost function may be optimized for each set of sensing data, but may not be optimized for all of the multiple sets of sensing data. That is, even after calibration, there is a possibility that the positional deviation of each point may be relatively large across all of the multiple sets of sensing data. Since the mobile body 100 optimizes the cost function all at once, the cost function is optimized across all of the multiple sets of sensing data. As a result, the accuracy of calibration can be further improved.

[0121] In addition, the mobile body 100 extracts points constituting a plane from both the point cloud map and the sensing data. That is, the mobile body 100 removes points other than those constituting the plane from both the point cloud map and the sensing data. The points other than those constituting the plane are measured by detecting moving objects such as people. The mobile body 100 calibrates the sensor coordinate system with respect to the extracted points constituting the plane so as to minimize the positional deviation between corresponding points in the reference coordinate system between the point cloud map and the sensing data. By using the points constituting the plane in this way for calibration, the use of measurement points of moving objects such as people, which reduces the accuracy of the calibration, is suppressed. Furthermore, the points constituting the plane are arranged in a linear fashion, making it relatively easy to search for corresponding points between the points constituting the plane. As a result, the accuracy of the calibration can be further improved.

[0122] In this example, the target sensor detects objects in parallel with the detection of objects by the first sensor 3A for creating a point cloud map, thereby completing the calibration of the target sensor before executing autonomous movement.

[0123] Furthermore, in this example, the reference coordinate system is a mobile body coordinate system, and the positional deviation of each point of the sensing data is corrected in the mobile body coordinate system. Since the mobile body coordinate system is often used in operations, it makes it easier to handle the sensing data after calibration.

[0124] The control device 6 may control the robot arm 12 when performing autonomous movement. Fig. 11 is a functional block diagram showing the configuration of a control system of the processor 61 according to a modified example. The processor 61 may function as an arm controller 612 that controls the robot arm 12.

[0125] The arm controller 612 operates the robot arm 12. For example, the arm controller 612 deforms the robot arm 12 into a target shape. The arm controller 612 may maintain the robot arm 12 in the target shape. The arm controller 612 may operate the robot arm 12 by continuously changing the shape of the robot arm 12.

[0126] The arm controller 612 generates command values ​​according to the target shape of the robot arm 12. Based on the command values, the arm controller 612 calculates command operation amounts for each of the multiple motors 12a. For example, the operation amounts are the rotational speed or torque of the motors.

[0127] The arm controller 612 may maintain the robot arm 12 in a certain shape when the moving body 100 is moving, and may operate the robot arm 12 when the robot arm 12 is performing a task.

[0128] For example, when the moving body 100 is moving, the arm controller 612 maintains the robot arm 12 in a moving shape. In other words, when the moving body 100 is moving, the arm controller 612 fixes the shape of the robot arm 12 and prohibits the robot arm 12 from moving.

[0129] Fig. 12 is a side view of the mobile body 1 when the robot arm 12 is in the traveling shape. Fig. 13 is a plan view of the mobile body 1 when the robot arm 12 is in the traveling shape.

[0130] For example, the robot arm 12 in the running configuration is positioned at a relatively high position. For example, the robot arm 12 in the running configuration is bent at an intermediate joint between the shoulder joint and the wrist joint, for example, at the fourth joint J4, with the portion between the base 11 and the intermediate joint extending diagonally downward and rearward from the base 11, and the portion between the intermediate joint and the wrist joint extending forward from the intermediate joint. That is, the robot arm 12 in the running configuration has the intermediate joint pulled rearward and bent at the intermediate joint. As a result, the portion of the robot arm 12 closer to the hand than the intermediate joint is positioned at a relatively high position. Furthermore, the hand of the robot arm 12 is positioned relatively rearward.

[0131] The robot arm 12 in the traveling configuration is positioned higher than the first sensor 3A of the mobile body 1. The detection range of the first sensor 3A extends three-dimensionally from the first sensor 3A. The space above the first sensor 3A is included in the detection range of the first sensor 3A. Because the robot arm 12 is positioned above the first sensor 3A, it may block part of the detection range of the first sensor 3A. The detection results of the first sensor 3A that correspond to the robot arm 12 are treated as invalid. The higher the position of the robot arm 12, the farther the robot arm 12 is from the first sensor 3A. The farther the robot arm 12 is from the first sensor 3A, the smaller the area of ​​the detection range of the first sensor 3A that is blocked by the robot arm 12 tends to be. Therefore, in the traveling configuration, the detection range of the first sensor 3A is relatively large.

[0132] Furthermore, the robot arm 12 in the traveling configuration has a relatively small forward protrusion amount from the base 11. As the forward protrusion amount of the robot arm 12 decreases, the detection range of the first sensor 3A is expanded diagonally upward and forward from the first sensor 3A.

[0133] The width direction size of the overall shape of the running-shaped robot arm 12 in a plan view is relatively small. For example, in the running shape, the second link L2 is located at the outermost position in the width direction. Of the multiple links L, the links other than the second link L2 are located more inward in the width direction than the second link L2. By making the width direction size of the overall shape of the running-shaped robot arm 12 in a plan view relatively small, the possibility of interference between the robot arm 12 and other objects located in the width direction during running can be reduced. Note that the running-shaped robot arm 12 may be configured such that the first link L1 and the second link L2 are located forward of the rotation axis of the first joint J1 by rotating the first link L1 forward about the rotation axis of the first joint J1. This further reduces the width direction size of the second links L2 of the two robot arms, i.e., the width direction size of the overall shape of the robot arm 12 in a plan view.

[0134] The overall shape of the robot arm 12 in the traveling configuration in a plan view is contained within the inside of the carriage 10 in the front-to-rear direction. This reduces the possibility of interference between the robot arm 12 and other objects located in the front-to-rear direction when traveling.

[0135] In addition, the shapes of the two robot arms 12 in terms of their running shapes do not have to be completely identical. That is, the shapes of the two robot arms 12 may be slightly different. For example, the height of the tip of one robot arm 12 may be different from the height of the tip of the other robot arm 12. The rotation angle of the seventh joint J7 of one robot arm 12 may be different from the rotation angle of the seventh joint J7 of the other robot arm 12.

[0136] For example, when the robot arm 12 is performing work, the arm controller 612 operates the robot arm 12. In other words, when the robot arm 12 is performing work, the arm controller 612 permits the operation of the robot arm 12 and allows the robot arm 12 to freely operate. For example, after the mobile body 1 has reached its destination, the arm controller 612 operates the robot arm 12 to perform work by the robot arm 12.

[0137] 14 is a side view of a moving body 100 according to a modified example. The multiple sensors 3 provided in the moving body 100 include a fourth sensor 3D in addition to a first sensor 3A. The moving body 100 does not necessarily have to include at least one of the second sensor 3B and the third sensor 3C. The fourth sensor 3D is an example of a target sensor. In other words, the fourth sensor 3D is a sensor to be calibrated.

[0138] The target sensor may be arranged closer to the tip of the robot arm than a sensor other than the target sensor. For example, the fourth sensor 3D is arranged closer to the tip of the robot arm 12 than the first sensor 3A. That is, the distance from the fourth sensor 3D to the tip of the robot arm 12 is shorter than the distance from the first sensor 3A to the tip of the robot arm 12. The tip of the robot arm 12 is, for example, the seventh link L7.

[0139] The fourth sensor 3D may be disposed on the robot arm 12. For example, the fourth sensor 3D is disposed on one of the robot arms 12. The fourth sensor 3D is arbitrarily disposed on one link L of the multiple links L. For example, the fourth sensor 3D is disposed on the first link L1, the second link L2, the sixth link L6, or the seventh link L7.

[0140] Here, the distance to the tip of the robot arm 12 means the distance via a structure. For example, the distance from the first sensor 3A to the tip of the robot arm 12 is the distance from the first sensor 3A to the joint 11 of the cart 10, the joint 11 of the robot arm 12, passing through each joint J of the robot arm 12 in this order, and then to the tip of the robot arm 12. The distance from the fourth sensor 3D to the tip of the robot arm 12 is the distance from the fourth sensor 3D to the joint J of the robot arm 12 that is located closer to the tip of the robot arm 12 than the fourth sensor 3D, in this order, and then to the tip of the robot arm 12.

[0141] The fourth sensor 3D may detect an object in a three-dimensional space around the mobile body 1. For example, the fourth sensor 3D may be a 3D LiDAR. The fourth sensor 3D may scan the measurement light in the horizontal and vertical directions.

[0142] The control device 6 may calibrate the sensor coordinate system of the fourth sensor 3D based on the sensing data detected by the fourth sensor 3D and the point cloud map. Furthermore, the control device 6 may switch the sensor 3 used for self-location estimation depending on the distance to the destination. For example, the control device 6 may perform self-location estimation based on the detection result of the first sensor 3A in a first section where the distance to the destination is outside a predetermined switching range, and perform self-location estimation based on the detection result of the fourth sensor 3D in a second section where the distance to the destination is within the switching range.

[0143] The fourth sensor 3D is relatively close to the tip of the robot arm 12. Therefore, by performing self-localization using the fourth sensor 3D, the estimation accuracy of the position of the tip of the robot arm 12 is improved. Furthermore, since the fourth sensor 3D is calibrated with high accuracy by performing calibration based on the point cloud map, the estimation accuracy of the position of the tip of the robot arm 12 is further improved. The second section in which self-localization is performed using the fourth sensor 3D includes the destination. Therefore, when the robot arm 12 performs work at the destination, the position of the tip of the robot arm 12 is estimated with high accuracy. As a result, the accuracy of the work of the robot arm 12 is improved.

[0144] The first sensor 3A is relatively far from the tip of the robot arm 12. The position accuracy of the tip of the robot arm 12 obtained by self-localization estimation using the first sensor 3A may be inferior to that obtained by self-localization estimation using the fourth sensor 3D.

[0145] However, the detection range of the first sensor 3A is relatively wide. The first sensor 3A detects objects in the surrounding three-dimensional space. For example, the first sensor 3A is positioned so that its three-dimensional detection range is not obstructed by the base 11 and the robot arm 12 as much as possible.

[0146] The detection range of the fourth sensor 3D may be narrower than the detection range of the first sensor 3A. When the fourth sensor 3D is disposed on the robot arm 12, the detection range of the fourth sensor 3D depends on the traveling shape of the robot arm 12. A part of the detection range of the fourth sensor 3D may be blocked by the base 11 or the like. The area of ​​the detection range of the fourth sensor 3D that is blocked by the base 11 or the like may be larger than the area of ​​the detection range of the first sensor 3A that is blocked by the base 11 or the like.

[0147] The first section is relatively far from the destination. Therefore, in the first section, the self-position estimation may be performed using the first sensor 3A, prioritizing the detection of objects in a wide range over the positional accuracy of the tip of the robot arm 12.

[0148] Other Embodiments As described above, the above embodiment has been described as an example of the technology disclosed in this application. However, the technology of the present disclosure is not limited to this and can be applied to embodiments in which modifications, substitutions, additions, omissions, etc. are made as appropriate. Furthermore, the components described in the above embodiment can be combined to create new embodiments. Furthermore, the components described in the accompanying drawings and detailed description may include not only components essential for solving the problem, but also components that are not essential for solving the problem in order to exemplify the technology. Therefore, the fact that these non-essential components are described in the accompanying drawings or detailed description should not be interpreted as immediately determining that these non-essential components are essential.

[0149] The mobile body 1 may be a robot that does not include the robot arm 12. The mobile body 1 is not limited to a robot, but may be a mobile device such as a drone, a ship, or a vehicle. The movement path of the mobile body 1 is not limited to a passageway, but may be a road or a seaway.

[0150] The first sensor 3A is not limited to a 3D LiDAR as long as it can acquire point cloud data. The target sensors (in this example, the second sensor 3B and the third sensor 3C) are not limited to a 2D LiDAR as long as they can acquire point cloud data. The number of target sensors is also not limited, and may be one or three or more.

[0151] The calibrator 610 may acquire a point cloud map from outside the mobile body 100. For example, the calibrator 610 may acquire a point cloud map generated by a sensor attached to a mobile body other than the mobile body 100.

[0152] The reference coordinate system may be a map coordinate system. In this case, the calibrator 610 may convert the position of each point of the sensing data in the sensor coordinate system of the target sensor into the mobile body coordinate system, and then further convert the position into the map coordinate system based on the relationship of the mobile body coordinate system with respect to the map coordinate system. The relationship of the mobile body coordinate system with respect to the map coordinate system is determined by self-localization of the mobile body 1. In this case, the coordinate conversion of each point of the point cloud map into the reference coordinate system is omitted. In calculating the cost function shown in FIG. 10, the point cloud map may be common to all acquisition situations of the mobile body 1.

[0153] The calibrator 610 does not need to extract points that form a plane in each of the point cloud map and the sensing data. For example, the calibrator 610 may directly use the acquired point cloud map and sensing data for calibration.

[0154] The calibrator 610 does not need to calibrate the sensor coordinate system so as to reduce the positional deviation between corresponding points in the reference coordinate system between the sensing data of the first target sensor 3B and the sensing data of the second target sensor 3C. In other words, the calibrator 610 may calibrate the sensor coordinate system so as to reduce only the positional deviation between corresponding points in the reference coordinate system between the point cloud map and the sensing data.

[0155] The multiple sets of sensing data are not limited to the configuration of the above embodiment. The multiple sets of sensing data may be acquired by the target sensor in a situation where only the position of the mobile body 1 is different, or may be acquired by the target sensor in a situation where only the attitude of the mobile body 1 is different, or may be acquired by the target sensor in a situation where both the position and attitude of the mobile body 1 are different. The number of sets of sensing data may be two, four or more. The sensing data may be one set. In other words, the sensing data may be acquired by the target sensor in a situation where the mobile body 1 has a single position and attitude.

[0156] The calibrator 610 does not have to calculate the cost function by summing multiple costs for multiple sets of sensing data. For example, the calibrator 610 may calculate a cost function for each set of sensing data and optimize the cost function for each set of sensing data.

[0157] The timing when the calibrator 610 acquires the sensing data is not limited to when the first sensor 3A collects point cloud data for creating a point cloud map, but may be when the mobile body 1 is moving normally autonomously.

[0158] The convergence conditions of the cost function are not limited to the configurations in the above-described embodiments, and can be set arbitrarily.

[0159] The flowchart is merely an example. Steps in the flowchart may be changed, replaced, added, omitted, etc. as appropriate. The order of steps in the flowchart may also be changed, and serial processing may be performed in parallel. For example, in the flowchart shown in FIG. 6, step S101 and step S102 may be processed serially. For example, in the flowchart shown in FIG. 10, step S406 may be omitted.

[0160] The functionality of the elements disclosed herein may be implemented using one or more circuits or processing circuits, including general-purpose processors, special-purpose processors, integrated circuits, ASICs (Application Specific Integrated Circuits), FPGAs (Field Programmable Gate Arrays), and / or conventional circuitry. The functionality of the elements disclosed herein may be implemented using one or more circuits or processing circuits, including combinations of general-purpose processors, special-purpose processors, integrated circuits, ASICs, FPGAs, and conventional circuitry. The one or more circuits or processing circuits may be programmed using one or more programs stored together or separately in one or more memories or otherwise configured to perform the disclosed functions. A processor is considered a processing circuit or circuitry because it includes transistors and other circuits. A processor may also be a programmed processor that executes a program stored in a memory. In this disclosure, a circuit, unit, or means is hardware that performs the recited functions alone or in combination with each other, or hardware that is programmed to perform the recited functions alone or in combination with each other. The hardware may be any hardware disclosed herein that is programmed or configured to perform the recited functions.

[0161] A computer program containing computer instructions is stored in memory. The computer instructions provide logic and routines that enable hardware to perform the methods disclosed herein. The hardware includes, for example, processing circuits or circuitry. The computer program may be implemented in a known format in a computer-readable storage medium, a computer program product, a memory device, a recording medium such as a CD-ROM or DVD, and / or the memory of FPGAs or ASICs.

[0162] [Aspect] The above embodiments are specific examples of the following aspects.

[0163] (Aspect 1) The sensor calibration method is a method for calibrating a sensor 3 that is attached to the mobile body 1 of an autonomously moving mobile body 100 and that calibrates a target sensor (in this example, the second sensor 3B and the third sensor 3C) that detects surrounding objects in the form of point cloud data, and includes acquiring a point cloud map around the mobile body 1, detecting objects around the mobile body 1 using the target sensor, determining the position of each point of the point cloud data (sensing data) detected by the target sensor in the sensor coordinate system of the target sensor by coordinate transforming the position of each point of the point cloud data in the reference coordinate system, and calibrating the sensor coordinate system so that the positional deviation between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is small.

[0164] According to this configuration, the point cloud map is used as the point cloud data that serves as the reference for evaluating the positional deviation of each point in the sensing data, thereby improving the accuracy of the calibration. If point cloud data obtained by one-shot measurement with one sensor were used as the reference, the number of points contained in this reference point cloud data would be relatively small, and the number of points corresponding to the sensing data would also be relatively small. This could result in a decrease in the accuracy of the calibration. On the other hand, according to this configuration, the point cloud map is used as the reference, so the number of points corresponding to the sensing data can be relatively increased. This improves the accuracy of the calibration of the target sensor.

[0165] (Aspect 2) The sensor calibration method described in aspect 1 further includes creating the point cloud map, wherein a sensor (first sensor 3A) separate from the target sensor is attached to the mobile body 1, which detects surrounding objects in the form of point cloud data; creating the point cloud map involves detecting objects around the mobile body 1 using the separate sensor while the mobile body 1 is moving, and creating the point cloud map from the point cloud data detected by the separate sensor; detecting objects around the mobile body 1 involves detecting objects using the target sensor in parallel with the detection of objects by the separate sensor for creating the point cloud map.

[0166] According to this configuration, the calibration of the target sensor can be completed before the moving body 100 starts autonomous movement.

[0167] (Aspect 3) The sensor calibration method described in aspect 1 or aspect 2 further includes performing self-location estimation to estimate the position and attitude of the mobile body in a map coordinate system of the point cloud map, and determining the position of each point of the point cloud map in the reference coordinate system by coordinate transforming the position of each point of the point cloud map in the map coordinate system to the reference coordinate system based on the position and attitude of the mobile body estimated by the self-location estimation, wherein the reference coordinate system is a mobile body coordinate system defined based on the mobile body.

[0168] According to this configuration, the reference coordinate system is a mobile body coordinate system, and the positional deviation of each point of the sensing data is corrected in the mobile body coordinate system. Since the mobile body coordinate system is often used in operations, it becomes easier to handle the sensing data after calibration.

[0169] (Aspect 4) The sensor calibration method according to any one of aspects 1 to 3 further includes extracting points constituting a plane of the object in each of the point cloud map and the point cloud data, and in the calibrating, the sensor coordinate system is calibrated so that the positional deviation between corresponding points in the reference coordinate system between the point cloud map and the point cloud data is reduced for the extracted points constituting the plane of the object.

[0170] With this configuration, points other than the points constituting the plane are removed from both the point cloud map and the sensing data. The points other than the points constituting the plane are measured by detecting, for example, moving objects such as people. The sensor coordinate system is calibrated so that the positional deviation between corresponding points in the reference coordinate system between the point cloud map and the sensing data is reduced for the extracted points constituting the plane. By using the points constituting the plane for calibration in this manner, the use of measurement points of moving objects such as people, which reduces the accuracy of the calibration, is suppressed. Furthermore, the points constituting the plane are arranged linearly, making it relatively easy to search for corresponding points between the points constituting the plane. As a result, the accuracy of the calibration of the target sensor can be further improved.

[0171] (Aspect 5) In the sensor calibration method described in any one of aspects 1 to 4, detecting objects around the mobile body 1 involves acquiring multiple sets of point cloud data using the target sensor in situations where at least one of the position and attitude of the mobile body 1 is different, and calibrating involves calibrating the sensor coordinate system so that the positional deviation between corresponding points between the point cloud map in the reference coordinate system and the multiple sets of point cloud data is small.

[0172] This configuration makes it possible to increase the number of corresponding points compared to when a single piece of sensing data is used, thereby further improving the accuracy of calibration of the target sensor.

[0173] (Aspect 6) In the sensor calibration method described in any one of aspects 1 to 5, the calibrating includes: determining a cost related to the positional deviation between corresponding points between the point cloud map and the point cloud data in the reference coordinate system for each set of point cloud data of the multiple sets of point cloud data; calculating a cost function by summing up the multiple costs related to the multiple sets of point cloud data; and determining a coordinate transformation matrix for coordinate transforming the position of each point of the point cloud data in the reference coordinate system so as to minimize the cost function.

[0174] According to this configuration, a cost function is calculated using all points included in multiple sets of sensing data, and this cost function is optimized all at once. If a cost function is calculated for each set of sensing data and optimized, the cost function may be optimized for each set of sensing data, but may not be optimized for all of the multiple sets of sensing data. In other words, even after calibration, there is a possibility that the positional deviation of each point may be relatively large across all of the multiple sets of sensing data. According to this configuration, the cost function is optimized all at once, and therefore the cost function is optimized across all of the multiple sets of sensing data. As a result, the accuracy of calibration of the target sensor can be further improved.

[0175] (Aspect 7) In the sensor calibration method described in any one of claims 1 to 6, the target sensors include a first target sensor 3B and a second target sensor 3C having a detection range that at least partially overlaps with the detection range of the first target sensor 3B, and by calibrating the sensor coordinate system, the sensor coordinate system is calibrated so that in addition to the positional deviation between corresponding points between the point cloud map and the point cloud data in the reference coordinate system, the positional deviation between corresponding points between the point cloud data of the first target sensor 3B and the point cloud data of the second target sensor 3C in the reference coordinate system is reduced.

[0176] According to this configuration, the position of the obstacle can be accurately determined, and interference between the moving body 1 and the obstacle can be suppressed.

[0177] (Aspect 8) A sensor calibration method according to any one of claims 1 to 7, further comprising creating the point cloud map, wherein the mobile body 1 includes a robot arm 12 and is a mobile robot, and the mobile body 1 is equipped with a sensor (first sensor 3A) other than the target sensor (fourth sensor 3D) that detects surrounding objects in the form of point cloud data, and the target sensor is positioned closer to the tip of the robot arm 12 than the other sensor, and by creating the point cloud map, the other sensor detects objects around the mobile body 1 while the mobile body 1 is moving, and the point cloud map is created from the point cloud data detected by the other sensor.

[0178] According to this configuration, the target sensor is relatively close to the tip of the robot arm 12. Therefore, by performing self-localization using the target sensor, the estimation accuracy of the position of the tip of the robot arm 12 is improved. Furthermore, since the target sensor is calibrated with high accuracy by performing calibration based on the point cloud map, the estimation accuracy of the position of the tip of the robot arm 12 is further improved.

[0179] (Aspect 9) The mobile body 100 is an autonomously moving mobile body 100 and comprises a mobile body body 1, a target sensor attached to the mobile body body 1 and detecting surrounding objects in the form of point cloud data, and a control device 6 that calibrates the sensor coordinate system of the target sensor. The control device 6 acquires a point cloud map of the surroundings of the mobile body body 1, detects objects around the mobile body body 1 using the target sensor, and determines the position of each point of the point cloud data detected by the target sensor in the sensor coordinate system of the target sensor by coordinate transforming the position of each point of the point cloud data detected by the target sensor into a reference coordinate system, and calibrates the sensor coordinate system so that the positional deviation between corresponding points in the point cloud map and the point cloud data in the reference coordinate system is small.

[0180] This configuration can improve the accuracy of calibration of the target sensor.

[0181] (Aspect 10) The control device 6 is attached to the mobile body 1 of the autonomously moving mobile body 100 and calibrates a target sensor that detects surrounding objects in the form of point cloud data.The control device 6 acquires a point cloud map around the mobile body 1, detects objects around the mobile body 1 using the target sensor, and determines the position of each point of the point cloud data detected by the target sensor in the sensor coordinate system of the target sensor by coordinate transforming the position of each point of the point cloud data into a reference coordinate system.The control device 6 calibrates the sensor coordinate system so that the positional deviation between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is small.

[0182] This configuration can improve the accuracy of calibration of the target sensor.

[0183] (Aspect 11) The control program is a control program for calibrating a target sensor that is attached to the mobile body 1 of an autonomously moving mobile body 100 and detects surrounding objects in the form of point cloud data, and causes a computer to realize the following functions: a function for acquiring a point cloud map around the mobile body 1; a function for detecting objects around the mobile body 1 using the target sensor; a function for determining the position of each point of the point cloud data detected by the target sensor in the reference coordinate system by coordinate converting the position of each point in the sensor coordinate system of the target sensor to a reference coordinate system; and a function for calibrating the sensor coordinate system so that the positional deviation between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is reduced.

[0184] This configuration can improve the accuracy of calibration of the target sensor. [Explanation of symbols]

[0185] 100 Mobile 1 Mobile body 12 Robotic Arm 3A 1st sensor (another sensor) 3B Second sensor (first target sensor) 3C Third sensor (second target sensor) 3D 4th sensor (target sensor) 6. Control device

Claims

1. A sensor calibration method for calibrating a target sensor attached to a mobile body of an autonomously moving mobile body and detecting surrounding objects in the form of point cloud data, comprising: Acquiring a point cloud map of the surroundings of the mobile body; performing self-localization to estimate the position and orientation of the mobile body based on point cloud data detected by another sensor attached to the mobile body that detects surrounding objects in the form of point cloud data and the point cloud map; Detecting an object around the moving body by the object sensor; determining a position of each point of the point cloud data detected by the target sensor in the reference coordinate system by coordinate transforming the position of each point of the point cloud data in the sensor coordinate system of the target sensor into the reference coordinate system; determining positions in the reference coordinate system of corresponding points in the point cloud map that correspond to the point cloud data; calibrating the sensor coordinate system so that a positional deviation between corresponding points in the reference coordinate system between the point cloud map and the point cloud data is reduced; A sensor calibration method that uses the position and attitude of the mobile body estimated by the self-position estimation in at least one of determining the position of each point of the point cloud data in the reference coordinate system and determining the position of a corresponding point in the point cloud map in the reference coordinate system.

2. The method for calibrating a sensor according to claim 1, generating the point cloud map; a sensor separate from the target sensor attached to the main body of the moving body, the sensor detecting surrounding objects in the form of point cloud data; In creating the point cloud map, the other sensor detects objects around the mobile body while the mobile body is moving, and the point cloud map is created from point cloud data detected by the other sensor; The method for calibrating a sensor in which the detection of objects around the mobile body is performed by the target sensor in parallel with the detection of objects by the other sensor for creating the point cloud map.

3. A sensor calibration method for calibrating a target sensor attached to a mobile body of an autonomously moving mobile body and detecting surrounding objects in the form of point cloud data, comprising: Acquiring a point cloud map of the surroundings of the mobile body; Detecting an object around the moving body by the object sensor; determining a position of each point of the point cloud data detected by the target sensor in the reference coordinate system by coordinate transforming the position of each point of the point cloud data in the sensor coordinate system of the target sensor into the reference coordinate system; performing self-location estimation to estimate the position and orientation of the mobile body in a map coordinate system of the point cloud map; determining a position of each point of the point cloud map in the reference coordinate system by coordinate transforming the position of each point of the point cloud map in the map coordinate system into the reference coordinate system based on the position and attitude of the mobile body estimated by the self-location estimation; calibrating the sensor coordinate system so that a positional deviation between corresponding points in the reference coordinate system between the point cloud map and the point cloud data is reduced; A sensor calibration method, wherein the reference coordinate system is a moving body coordinate system defined based on the moving body itself.

4. The method for calibrating a sensor according to claim 1, extracting points constituting a plane of an object from each of the point cloud map and the point cloud data; A sensor calibration method in which the sensor coordinate system is calibrated so that the positional deviation between corresponding points in the reference coordinate system between the point cloud map and the point cloud data is reduced with respect to the extracted points that constitute the plane of the object.

5. The method for calibrating a sensor according to claim 1, In detecting an object around the mobile body, a plurality of sets of point cloud data are acquired by the target sensor under conditions where at least one of the position and the posture of the mobile body is different; A sensor calibration method in which the sensor coordinate system is calibrated so that the positional deviation between corresponding points between the point cloud map and the multiple sets of point cloud data in the reference coordinate system is reduced.

6. A sensor calibration method for calibrating a target sensor attached to a mobile body of an autonomously moving mobile body and detecting surrounding objects in the form of point cloud data, comprising: Acquiring a point cloud map of the surroundings of the mobile body; Detecting an object around the moving body by the object sensor; determining a position of each point of the point cloud data detected by the target sensor in the reference coordinate system by coordinate transforming the position of each point of the point cloud data in the sensor coordinate system of the target sensor into the reference coordinate system; calibrating the sensor coordinate system so that a positional deviation between corresponding points in the reference coordinate system between the point cloud map and the point cloud data is reduced; In detecting an object around the mobile body, a plurality of sets of point cloud data are acquired by the target sensor under conditions where at least one of the position and the posture of the mobile body is different; the calibration includes calibrating the sensor coordinate system so that a positional deviation between corresponding points in the reference coordinate system between the point cloud map and the plurality of sets of point cloud data is reduced; The calibrating step includes: calculating a cost function by calculating a cost function for each of the plurality of sets of point cloud data, the cost function being related to a positional deviation between corresponding points in the reference coordinate system between the point cloud map and the point cloud data, and the cost function being related to each of the plurality of sets of point cloud data; and determining a coordinate transformation matrix for transforming the coordinates of the positions of each point of the point cloud data in the reference coordinate system so as to minimize the cost function.

7. The method for calibrating a sensor according to claim 1, the target sensors include a first target sensor and a second target sensor having a detection range that at least partially overlaps with the detection range of the first target sensor; A sensor calibration method in which the sensor coordinate system is calibrated so that, in addition to the positional deviation between corresponding points between the point cloud map and the point cloud data in the reference coordinate system, the positional deviation between corresponding points between the point cloud data of the first target sensor and the point cloud data of the second target sensor in the reference coordinate system is reduced.

8. A sensor calibration method for calibrating a target sensor attached to a mobile body of an autonomously moving mobile body and detecting surrounding objects in the form of point cloud data, comprising: Acquiring a point cloud map of the surroundings of the mobile body; Detecting an object around the moving body by the object sensor; determining a position of each point of the point cloud data detected by the target sensor in the reference coordinate system by coordinate transforming the position of each point of the point cloud data in the sensor coordinate system of the target sensor into the reference coordinate system; calibrating the sensor coordinate system so that a positional deviation between corresponding points in the reference coordinate system between the point cloud map and the point cloud data is reduced; the mobile body is a mobile robot including a robot arm, and a sensor separate from the target sensor is attached to the mobile body to detect surrounding objects in the form of point cloud data; the target sensor is disposed at a position closer to the tip of the robot arm than the other sensor; A sensor calibration method for acquiring the point cloud map by detecting objects around the mobile body using the other sensor while the mobile body is moving, and creating the point cloud map from the point cloud data detected by the other sensor.

9. An autonomously moving vehicle, A mobile body; an object sensor attached to the mobile body and configured to detect surrounding objects in the form of point cloud data; Another sensor attached to the mobile body detects surrounding objects in the form of point cloud data; a control device that calibrates a sensor coordinate system of the target sensor; The control device Acquire a point cloud map of the surroundings of the mobile body; performing self-location estimation to estimate the position and attitude of the mobile body based on the point cloud data detected by the other sensor and the point cloud map; Detecting an object around the mobile body by the target sensor; determining a position of each point of the point cloud data detected by the target sensor in the reference coordinate system by coordinate transforming the position of each point of the point cloud data in the sensor coordinate system of the target sensor into a reference coordinate system; determining positions in the reference coordinate system of corresponding points in the point cloud map that correspond to the point cloud data; calibrating the sensor coordinate system so that a positional deviation between corresponding points in the reference coordinate system between the point cloud map and the point cloud data is reduced; A mobile body that uses the position and attitude of the mobile body itself estimated by the self-position estimation in at least one of determining the position of each point of the point cloud data in the reference coordinate system and determining the position of a corresponding point in the point cloud map in the reference coordinate system.

10. A control device that calibrates a target sensor attached to a mobile body of an autonomously moving mobile body and that detects surrounding objects in the form of point cloud data, Acquire a point cloud map of the surroundings of the mobile body; performing self-localization to estimate the position and orientation of the mobile body based on point cloud data detected by another sensor attached to the mobile body that detects surrounding objects in the form of point cloud data and the point cloud map; Detecting an object around the mobile body by the target sensor; determining a position of each point of the point cloud data detected by the target sensor in the reference coordinate system by coordinate transforming the position of each point of the point cloud data in the sensor coordinate system of the target sensor into a reference coordinate system; determining positions in the reference coordinate system of corresponding points in the point cloud map that correspond to the point cloud data; calibrating the sensor coordinate system so that a positional deviation between corresponding points in the reference coordinate system between the point cloud map and the point cloud data is reduced; A control device that uses the position and attitude of the mobile body estimated by the self-position estimation in at least one of determining the position of each point of the point cloud data in the reference coordinate system and determining the position of a corresponding point in the point cloud map in the reference coordinate system.

11. A control program for calibrating a target sensor attached to a main body of an autonomously moving vehicle, the target sensor detecting surrounding objects in the form of point cloud data, the control program comprising: A function of acquiring a point cloud map of the surroundings of the mobile body; a function of performing self-location estimation to estimate the position and attitude of the mobile body based on point cloud data detected by another sensor attached to the mobile body that detects surrounding objects in the form of point cloud data and the point cloud map; a function of detecting an object around the mobile body by the target sensor; a function of calculating the position of each point of the point cloud data detected by the target sensor in the reference coordinate system by coordinate transformation of the position of each point of the point cloud data in the sensor coordinate system of the target sensor into the reference coordinate system; a function of calculating positions in the reference coordinate system of corresponding points in the point cloud map that correspond to the point cloud data; a function of calibrating the sensor coordinate system so that a positional deviation between corresponding points in the reference coordinate system between the point cloud map and the point cloud data is reduced; A control program that uses the position and attitude of the mobile body estimated by the self-position estimation to at least one of determine the position of each point of the point cloud data in the reference coordinate system and determine the position of a corresponding point in the point cloud map in the reference coordinate system.

Images