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

Abstract

This sensor calibration method is for calibrating a target sensor that is attached to a mobile object body (1) of an autonomously moving mobile object (100) and that detects surrounding objects in the form of point-cloud data. The sensor calibration method includes: acquiring a point-cloud map of the surroundings of the mobile object body (1); using the target sensor to detect objects surrounding the mobile object body (1); converting the position of each of the point-cloud data points detected by the target sensor from a sensor coordinate system of the target sensor to a reference coordinate system, thereby obtaining the position of each point-cloud data point in the reference coordinate system; and calibrating the sensor coordinate system so as to reduce the positional deviation between corresponding points between the point-cloud map and the point-cloud data in the reference coordinate system.

Description

Sensor Calibration Method, Moving Body, Control Device, and Control Program

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

[0002] Patent Document 1 discloses a mobile body control system for controlling a moving body. The moving body includes a distance measuring sensor that acquires point cloud data of an object around the moving body. The moving body autonomously moves while avoiding obstacles based on the point cloud data acquired by the distance measuring sensor.

[0003] Japanese Patent Application Laid-Open No. 2023-176361

[0004] By the way, the moving body may include a plurality of sensors that acquire point cloud data. Each sensor is attached to a predetermined position of the moving body. Here, the attachment position of each sensor may include an attachment error. Further, due to vibrations during the movement of the moving body, the attachment position of each sensor may shift. In such a case, the relative positional relationship between the sensors may shift, resulting in inappropriate detection of obstacles, which may adversely affect the autonomous movement of the moving body. Therefore, in order to correct the relative positional relationship between the sensors, it is desired to calibrate the sensors with high accuracy.

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

[0006] The sensor calibration method disclosed herein is a method for calibrating a target sensor that is attached to the 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 of the area around the body of the mobile body; detecting objects around the body of the mobile body with the target sensor; determining the position of each point in the point cloud data in the reference coordinate system by transforming the position of each point in the point cloud data detected by the target sensor in the sensor coordinate system of the target sensor to a reference coordinate system; and calibrating the sensor coordinate system such that the positional discrepancy between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is reduced.

[0007] The mobile body disclosed herein is an autonomously moving mobile body comprising: a mobile body body; a target sensor attached to the mobile body body for detecting surrounding objects in the form of point cloud data; and a control device for calibrating the sensor coordinate system of the target sensor. The control device acquires a point cloud map of the area around the mobile body body, detects objects around the mobile body body using the target sensor, determines the position of each point in the point cloud data in the reference coordinate system by transforming the position of each point in the point cloud data detected by the target sensor in the sensor coordinate system of the target sensor to a reference coordinate system, and calibrates the sensor coordinate system so that the positional discrepancy between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is reduced.

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

[0009] The control program disclosed herein is a control program for calibrating a target sensor attached to the body of an autonomously moving mobile body, which detects surrounding objects in the form of point cloud data, and enables a computer to perform the following functions: acquire a point cloud map of the area around the mobile body; allow the target sensor to detect objects around the mobile body; determine the position of each point in the point cloud data in the reference coordinate system by transforming the position of each point in the point cloud data detected by the target sensor in the sensor coordinate system to a reference coordinate system; and calibrate the sensor coordinate system so that the positional discrepancy between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is reduced.

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

[0011] The aforementioned mobile body can improve the calibration accuracy of the sensor.

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

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

[0014] Figure 1 is a perspective view of the mobile body. Figure 2 is a schematic diagram showing the detection range of the sensor. Figure 3 is a diagram showing the hardware configuration of the control device. Figure 4 is a functional block diagram showing the configuration of the control system of the processor. Figure 5 is a flowchart of the basic operation of the mobile body. Figure 6 is a flowchart of the sensor calibration method. Figure 7 is a flowchart of the subroutine for the point extraction method. Figure 8 is an explanatory diagram for explaining the point extraction method. Figure 9 is a flowchart of the subroutine for the calibration method. Figure 10 is a flowchart of the subroutine for the cost function calculation method. Figure 11 is a functional block diagram showing the configuration of the control system of the processor according to a modified example. Figure 12 is a side view of the mobile body when the robot arm is in a traveling shape. Figure 13 is a top view of the mobile body when the robot arm is in a traveling shape. Figure 14 is a side view of the mobile body according to a modified example.

[0015] The following describes exemplary embodiments in detail with reference to the drawings. Figure 1 is a perspective view of the mobile body 100. The mobile body 100 performs autonomous movement. The mobile body 100 comprises a mobile body body 1 and a control device 6 that causes the mobile body body 1 to perform autonomous movement. For example, the mobile body 100 moves within a facility such as a store, hospital, or nursing home. In addition to movement, the mobile body 100 may perform tasks such as handing over goods or opening and closing doors.

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

[0017] The trolley 10 has a defined front-to-back direction. In this example, the trolley 10 has a roughly rectangular planar shape. For example, the long side of the rectangle is the front-to-back direction, and the short side of the rectangle is the left-to-right direction.

[0018] The trolley 10 includes multiple wheels 13 and is capable of movement. In this example, the trolley 10 includes four wheels 13. The trolley 10 may be capable of moving in a straight line and turning. In this example, the trolley 10 is capable of moving forward and backward, left and right, and diagonally while maintaining its posture, i.e., it is capable of movement in all directions. In other words, the trolley 10 may be capable of parallel movement in directions other than the forward and backward direction. Furthermore, the trolley 10 may also be capable of rotating in place. For example, the four wheels 13 include a pair of wheels 13 arranged in the left-right direction at the front of the bottom of the trolley 10 and a pair of wheels 13 arranged in the left-right direction at the rear of the bottom of the trolley 10. They may be arranged to form a rectangle at the bottom of the trolley 10. More specifically, the four wheels 13 are arranged at the four corners of the bottom of the trolley 10.

[0019] More specifically, wheel 13 may be an omnidirectional wheel. In this example, wheel 13 is a Mecanum wheel. Wheel 13 has a plurality of barrel-shaped rollers arranged around its outer circumference. For example, the axis of rotation of each roller is inclined at 45 degrees with respect to the axle of wheel 13.

[0020] The mobile body 1 may have a motor 13a for driving the wheels 13 and an encoder 13b for detecting the amount of rotation of the motor 13a (see Figure 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 driven independently by the corresponding motors 13a.

[0021] The trolley 10 may be able to move in any direction in two dimensions using these four wheels 13. For example, the trolley 10 can move or rotate in any direction, such as forward, backward, left, right, or diagonally. The trolley 10 can also rotate in place.

[0022] The base 11 may be mounted on the trolley 10. In this example, the base 11 has a shape that mimics the upper body of a person. The base 11 may be fixed to the trolley 10 so as not to move.

[0023] The mobile unit 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 each connected to different parts of the base 11. For example, the two robot arms 12 are each connected to different parts 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 points of one robot arm 12 to the base 11 and the connection points 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, in the base 11, the front direction is defined as the side facing the front and the back direction as the rear. The width direction may be horizontal and perpendicular to the front-to-back direction. That is, the width direction is the left-to-right direction with respect to the front-to-back 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] Furthermore, if the planar shape of the trolley 10 is a roughly rectangular shape with a longitudinal direction and a transverse direction, the width direction will substantially coincide with the transverse direction of the planar shape of the trolley 10.

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

[0027] For example, the multiple links L include a first link L1, second link L2, third link L3, fourth link L4, fifth link L5, sixth link L6, and seventh link L7, which are arranged in series from the base 11. 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, second joint J2, third joint J3, fourth joint J4, fifth joint J5, sixth joint J6, and seventh joint J7, which are arranged in series from the base 11. The position and orientation of the seventh link L7 have six degrees of freedom, combining the translational and rotational directions for each of the three orthogonal axes. The robot arm 12 may also 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 the first joint J1. The first link L1 and the second link L2 are rotatably connected by the second joint J2. The second link L2 and the third link L3 are rotatably connected by the third joint J3. The third link L3 and the fourth link L4 are rotatably connected by the fourth joint J4. The fourth link L4 and the fifth link L5 are rotatably connected by the fifth joint J5. The fifth link L5 and the sixth link L6 are rotatably connected by the sixth joint J6. The sixth link L6 and the seventh link L7 are rotatably connected by the seventh joint J7.

[0029] A hand 14 may be connected to the 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] More specifically, multiple joints J may include joints that function as a shoulder joint. For example, multiple joints J may include joints that have the functions of horizontal extension and horizontal flexion in the shoulder joint. The axis of rotation of joints that have the functions of horizontal extension and horizontal flexion in the shoulder joint extends in a substantially vertical direction. Multiple joints J may include joints that have the functions of extension and flexion in the shoulder joint. The axis of rotation of joints that have 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 the shoulder joint of the robot arm 12. The first joint J1 may also have the functions of horizontal extension and horizontal flexion in the shoulder joint. The axis of rotation of the first joint J1 extends in a substantially vertical direction.

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

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

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

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

[0036] The robot arm 12 has motors 12a (see Figure 3) that rotate each joint J. For example, the motors 12a are servo motors. Each motor 12a has an encoder 12b (see Figure 3).

[0037] The mobile body 100 may be equipped with a sensor 3 that detects objects around the mobile body 1 (hereinafter simply referred to as "surrounding objects") in the form of point cloud data. In this disclosure, "object" includes both inanimate and living things. The sensor 3 is attached to the mobile body 1. For example, the sensor 3 is attached to a trolley 10. The sensor 3 in this example is a distance measuring sensor that measures the distance from the sensor 3 to the surrounding 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, towards the surroundings of the mobile body 1, and a light-receiving unit that receives the measurement light that strikes the surface of the surrounding objects and is reflected. The sensor 3 measures the flight time from the light-emitting unit until the measurement light irradiated from the light-emitting unit strikes the surface of the surrounding objects and returns to the light-receiving unit. Based on the measured flight time, the sensor 3 measures the distance from the sensor 3 to the surface of the surrounding objects. The sensor 3 may generate point cloud data based on the measured distance. Point cloud data is three-dimensional positional information of the surfaces of surrounding objects. For example, sensor 3 outputs the calculated point cloud data to control device 6. Sensor 3 may repeatedly detect surrounding objects at a predetermined detection cycle when the mobile body 1 is moving. Sensor 3 may output the detection result, i.e., point cloud data, to control device 6 each time a surrounding object is detected.

[0038] In this example, the mobile body 100 is equipped with multiple sensors 3. Figure 2 is a schematic diagram showing the detection range of the sensors 3. Figure 2 is a plan view of the mobile body 100, and the robot arm 12 and the like are omitted. The mobile 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 objects at least in front of the mobile body 1. The first sensor 3A is mounted on the front of the trolley 10. For example, the first sensor 3A is mounted on the trolley 10 in front of the base 11 and approximately in the center in the left-right direction. The first sensor 3A detects objects 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 horizontally, as shown by the dashed line in Figure 2. In the vertical direction, the first sensor 3A scans the measurement light within a predetermined range including the elevation and depression angles. 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 mounted on the rear of the trolley 10. More specifically, the second sensor 3B and the third sensor 3C may be mounted on the trolley 10 behind the base 11. The second sensor 3B is mounted on the left rear corner of the trolley 10, and the third sensor 3C is mounted on the right rear corner of the trolley 10. The second sensor 3B and the third sensor 3C may detect objects in the 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 overlap at least partially. 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 horizontally. The second sensor 3B and the third sensor 3C detect objects in the horizontal range that cannot be detected by the first sensor 3A. The second sensor 3B scans the measurement light at least to the left rear of the trolley 10. The third sensor 3C scans the measurement light at least to the right rear of the trolley 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 at the rear of the trolley 10. In this example, the second sensor 3B scans the measurement light horizontally for approximately 270 degrees from the front to the right, including the area to the left of the mobile body 1, as shown by the dashed line in Figure 2. The third sensor 3C scans the measurement light horizontally for approximately 270 degrees from the front to the left, including the area to the right of the mobile body 1, as shown by the dashed line in Figure 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 range of the measurement light of the second sensor 3B and the third sensor 3C are the respective object detection ranges of the second sensor 3B and the third sensor 3C.

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

[0043] Hereinafter, when not distinguishing each of the first sensor 3A, the second sensor 3B, and the third sensor 3C, they are simply referred to as "sensor 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 sensors to be calibrated. In the following description, the second sensor 3B is also referred to as "first target sensor 3B". The third sensor 3C is also referred to as "second target sensor 3C". When not distinguishing between the second sensor 3B and the third sensor 3C, they are simply referred to as "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 main body 1. The control device 6 causes the mobile body main body 1 to perform autonomous movement while estimating the self-position of the mobile body main body 1. The control device 6 operates the motor 13a of the wheel 13 to move the mobile body main body 1. Further, the control device 6 controls the motor 12a of the robot arm 12 to cause the robot arm 12 to perform a predetermined operation. The control device 6 includes a processor 61, a storage device 62, and a memory 63.

[0045] The processor 61 performs various arithmetic processes. 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. By the processor 61 operating the motor 13a, the mobile body main body 1 performs autonomous driving.

[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 regarding the map of the environment in which the mobile body main body 1 moves. For example, the map information includes a three-dimensional map and a two-dimensional map. The three-dimensional map is formed of 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 is simply referred to as a "point cloud map". In the three-dimensional map, the three-dimensional shapes of obstacles in the environment such as walls, ceilings, handrails, shelves, tables, or chairs are represented by the point cloud data. The two-dimensional map is a planar map. The two-dimensional map is, for example, a two-dimensional occupancy grid map. In the two-dimensional map, the planar shapes of obstacles in the environment such as walls, ceilings, handrails, shelves, tables, or chairs are represented. For example, the two-dimensional map is formed by projecting the three-dimensional map onto a plane. The memory 62 is formed of a non-volatile memory, a HDD (Hard Disc Drive), an SSD (Solid State Drive), or the like. The memory 63 temporarily stores data and the like. For example, the memory 63 is formed of a volatile memory.

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

[0048] Figure 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 control programs from the memory 62 into the memory 63 and expanding them. Specifically, the processor 61 functions as a state estimater 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 the path of 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 sensor, 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 sensor. The processor 61 also functions as an operation variable calculator 69 that calculates the operation variable of the motor 13a.

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

[0050] In this example, the state estimator 64 performs self-position estimation using the three-dimensional point cloud data from the first sensor 3A. The state estimator 64 compares the environmental information surrounding the mobile body 1 obtained from the three-dimensional point cloud data of the first sensor 3A with a three-dimensional map to estimate the position of the mobile body 1 within the environment represented by the three-dimensional map, i.e., its own 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, a three-dimensional map is generated using SLAM (Simultaneous Localization and Mapping) technology before autonomous movement is performed. More specifically, while the mobile body 1 is moving through 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 map generation is performed before autonomous movement is performed, the movement of the mobile body 1 is performed by manual control by the user.

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

[0053] The route generator 66 reads destination and map information from the memory 62. The destination is pre-set 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 also read waypoints 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 route generator 66 generates a route from the current position of the mobile body 1 to the destination based on map information. The route generator 66 generates a route that avoids interference with obstacles, etc., by referring to the map information. If a path is established in the environment, the route generator 66 generates a route along the path. For example, the route generator 66 generates a route using the A-star search algorithm, RRT algorithm, Dijkstra's algorithm, or a geometric approach. The route generator 66 outputs an array of positions that the mobile body 1 will pass through as a route 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 for the mobile body 1 from its current position, following the generated path. The trajectory generator 67 generates the target trajectory for the mobile body 1 using a predetermined method (for example, the 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 the command velocity of the mobile body 1.

[0056] Alternatively, the trajectory generator 67 may calculate the command velocity by model predictive control (MPC). Model predictive control obtains the control input, i.e., the velocity 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 any obstacles, calculates the optimal path for the mobile body 1, and calculates the command velocity as the speed at which it will travel from its current position to its target position along that path.

[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 corresponding to the command speed to the manipulated variable calculator 69.

[0058] The mobile controller 68 performs controls to avoid interference between the mobile body 1 and the obstacle. The mobile controller 68 monitors the proximity of the mobile body 1 to the obstacle based on the detection results of the sensors 3. In this example, the mobile controller 68 uses all the detection results from the first sensor 3A, the second sensor 3B, and the third sensor 3C to monitor the proximity of the mobile body 1 to the obstacle. For example, the mobile 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 command values ​​to the multiple motors 13a and calculates the commanded manipulated variable for each of the multiple motors 13a. For example, the manipulated variable may be the rotational speed or torque of the motor.

[0060] Each motor 13a operates according to the commanded input. A motor 13a may be equipped with its own controller for operation. For example, if motor 13a is a servo motor, it further includes a servo amplifier. In this case, the servo amplifier operates motor 13a according to the commanded input. 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 with respect to the target sensor. The position of each point in the sensing data is basically expressed with respect to the sensor coordinate system. The position of each point in the sensing data is transformed to various coordinate systems depending on the purpose of using the sensing data. For example, the position of each point expressed in the sensor coordinate system is transformed to the mobile body coordinate system. Since the target sensor is attached to the mobile body 1, the relationship between the sensor coordinate system and the mobile body coordinate system is fixed. In other words, the position of each point expressed in the sensor coordinate system is transformed to the mobile body coordinate system using the relationship between the sensor coordinate system and the mobile body coordinate system. As a result, each point in the sensing data is expressed in a coordinate system with respect to the mobile body 1. Furthermore, the position of each point expressed in the mobile body coordinate system may also be transformed to the map coordinate system. For example, the relationship between the mobile object coordinate system and the map coordinate system is determined by the self-position estimation of the mobile object 1. The position of each point expressed in the mobile object coordinate system is transformed into the map coordinate system using the relationship between the mobile object coordinate system and the map coordinate system. As a result, each point in the sensing data is expressed in a coordinate system based on the map.

[0062] Here, if the position or orientation of the target sensor is misaligned with respect to the mobile body 1, or if the optical axis of the target sensor is misaligned, the position of each point detected by the target sensor will not be properly transformed into the mobile body coordinate system. In other words, the position of each point will be transformed to a position that is misaligned from its actual position relative to the mobile body 1. The calibrator 610 calculates a coordinate transformation matrix to transform the position of each point to an appropriate position relative to the mobile body 1. In other words, the coordinate transformation matrix is ​​a coordinate transformation matrix that calibrates the sensor coordinate system. When the position of each point in the sensing data is transformed using the coordinate transformation matrix, the position of each point is transformed into a coordinate system that has been corrected for the misalignment.

[0063] The calibrator 610 compares a point in the sensing data with the corresponding point in the point cloud map in a common coordinate system, i.e., a reference coordinate system, to find a coordinate transformation matrix that minimizes the positional misalignment between the two points. At this time, the calibrator 610 extracts points from the sensing data and the point cloud map that are assumed to represent a specific object, and compares the extracted points from the sensing data with the extracted points from the point cloud map to find a coordinate transformation matrix. Specifically, the calibrator 610 extracts points that constitute a plane from among multiple sensing points, extracts points that constitute a plane from among multiple map points, and compares the extracted points with each other. In other words, a specific object is a plane. The reference coordinate system is a coordinate system that serves as a reference for determining the positional misalignment between corresponding points. The reference coordinate system can be, for example, a moving object coordinate system or a map coordinate system.

[0064] More specifically, the calibrator 610 acquires point cloud maps and sensing data. In this example, the calibrator 610 acquires a point cloud map by reading it from the memory 62. The calibrator 610 acquires sensing data by detecting objects around the mobile body 1 using the target sensor. That is, the calibrator 610 acquires sensing data by causing the target sensor to detect objects around the mobile body 1.

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

[0066] The calibrator 610 determines the position of each point in the sensing data in the reference coordinate system by transforming the position of each point in 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 in the point cloud map in the reference coordinate system by transforming the position of each point in the point cloud map in the map coordinate system to the reference coordinate system. Furthermore, the calibrator 610 calculates a coordinate transformation matrix to transform the position of each point in the sensing data in the reference coordinate system so that the positional discrepancy between corresponding points between the point cloud map and the sensing data in the reference coordinate system is minimized. In this example, the calibrator 610 calculates a coordinate transformation matrix to transform the position of each point in the sensing data in the reference coordinate system so that, in addition to minimizing the positional discrepancy between corresponding points between the point cloud map and the sensing data in the reference coordinate system, the positional discrepancy 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 is minimized. The calibrator 610 stores the calculated coordinate transformation matrix in the memory 62.

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

[0068] The corrector 611 corrects the coordinates of each point in 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 in the sensing data when sensing data is input from the target sensor while the mobile body 100 is performing autonomous movement. As described above, when the sensing data of the second sensor 3B and the third sensor 3C is used by the movement controller 68, etc., 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 respect to the mobile body 1.

[0069] Next, the basic operation of the mobile unit 100 will be explained. Figure 5 is a flowchart of the autonomous movement of the mobile unit 100. In this example, the control device 6 performs autonomous movement by repeatedly executing the processes in the flowchart of Figure 5 at a predetermined cycle.

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

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

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

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

[0074] In step S5, the movement controller 68 causes the mobile body 1 to perform an action according to the commanded speed. At this time, the movement controller 68 monitors the proximity of the mobile body 1 to the obstacle using all the detection results from the first sensor 3A, the second sensor 3B, and the third sensor 3C. If the mobile body 1 approaches the obstacle, the movement controller 68 controls the mobile body 1 to avoid the obstacle. At this time, as described above, the sensing data, which is the detection result of the second sensor 3B and the third sensor 3C, is corrected to an appropriate position with respect to the mobile body 1, so the location of the obstacle is accurately detected and interference between the mobile body 1 and the obstacle is suppressed.

[0075] The mobile body 100 autonomously moves to its destination while estimating its own position by repeating the processes from step S1 to step S5.

[0076] Next, the calibration method for the target sensor will be described. Calibration can be performed at any time. For example, calibration can be performed before the mobile body 100 is put into use. Alternatively, calibration can be performed when predetermined calibration conditions are met while the mobile body 100 is in use. For example, calibration conditions may include a predetermined period of time elapsed since the start of use of the mobile body 100, or the detection of a deviation in the position or orientation of the target sensor. Here, the calibration method will be described using the case where calibration is performed when the point cloud map is generated before autonomous movement begins as an example.

[0077] Figure 6 is a flowchart showing the calibration method for the target sensor.

[0078] First, in step S101, the map generator 65 creates a point cloud map. As described above, the map generator 65 creates a point cloud map using, for example, SLAM technology. While the mobile body 1 is moving, the map generator 65 detects objects around the mobile body 1 using the first sensor 3A and collects point cloud data of the objects. The map generator 65 creates a point cloud map from the point cloud data collected by 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 causing the target sensor to perform object detection 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 where at least one of the position and orientation of the mobile body 1 is different. The state estimator 64 estimates the self-position of the mobile body 1 at the time of acquisition of sensing data by the target sensor. The calibrator 610 and the state estimator 64 associate the sensing data with the corresponding estimated position and store it in the memory 62. In this way, multiple sets of sensing data are acquired under multiple conditions of the mobile body 1. The condition of the mobile body 1 is defined by the position and orientation of the mobile body 1.

[0080] In this example, sensing data detection is performed in parallel with point cloud map generation. Specifically, while the mobile body 1 moves through the environment to generate a point cloud map, the first sensor 3A acquires point cloud data of surrounding objects, 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 determines the position of each point in the sensing data in the reference coordinate system by transforming the position of each point in the sensing data in the sensor coordinate system of the target sensor to the reference coordinate system. In this example, the reference coordinate system is the mobile body coordinate system. The calibrator 610 transforms the position of each point in the sensing data in the sensor coordinate system to the mobile body coordinate system based on the relationship between the sensor coordinate system and the mobile body coordinate system. The relationship between the sensor coordinate system and the mobile body coordinate system is stored in the memory 62. In this way, the position of each point in the sensing data is transformed from the sensor coordinate system to the mobile body coordinate system. Furthermore, the calibrator 610 transforms the position of each point in the point cloud map in the map coordinate system to the mobile body coordinate system for each of the multiple situations of the mobile body 1. Specifically, the calibrator 610 transforms the position of each point in the point cloud map in the map coordinate system to the mobile body coordinate system based on the estimated position of the mobile body 1 associated with multiple sets of sensing data. In this way, multiple point cloud maps corresponding to each of the multiple situations of the mobile body 1 are obtained in the mobile body coordinate system.

[0083] In step S105, the calibrator 610 extracts points that constitute a plane in both the point cloud map and the sensing data. In other words, the calibrator 610 removes points that do not constitute a plane in both the point cloud map and the sensing data. Details of the point extraction method will be described later.

[0084] Next, in step S106, the calibrator 610 calibrates the sensor coordinate system so that the positional discrepancies between corresponding points in the point cloud map and the sensing data in the reference coordinate system are reduced. Specifically, the calibrator 610 identifies points corresponding to the extracted points of the sensing data from among the extracted points of the point cloud map, and calculates a coordinate transformation matrix for calibration so that the positional discrepancies between the two corresponding extracted points are reduced. In this example, in addition to reducing the positional discrepancies between corresponding points in the point cloud map and the sensing data in the reference coordinate system, the calibrator 610 also calibrates the sensor coordinate system so that the positional discrepancies between corresponding points in the sensing data of the first target sensor 3B and the sensing data of the second target sensor 3C in the reference coordinate system are reduced. More specifically, the calibrator 610, in addition to the positional discrepancies between the two corresponding extracted points in the point cloud map and sensing data, identifies points corresponding to the extracted points in the sensing data of the first target sensor 3B from among the extracted points in the sensing data of the second target sensor 3C, and calculates a calibration coordinate transformation matrix so that the positional discrepancies between these two corresponding extracted points are also reduced. Details of the method for calculating the calibration coordinate transformation matrix will be described later. The calibrator 610 completes the calibration by calculating the calibration coordinate transformation matrix.

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

[0086] Next, the method for extracting points from point cloud maps and sensing data will be explained in detail. Figure 7 is a flowchart of the subroutine for the point extraction method. Figure 8 is an explanatory diagram for explaining the point extraction method. Point extraction is performed for each of the multiple point cloud maps and multiple sensing data. The multiple 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 multiple sensing data under different position and orientation conditions for 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] The following describes point extraction using sensing data as an example. First, in step S201, the calibrator 610 estimates the normal vector of each point included in the data. More specifically, the calibrator 610 finds a small plane defined by the target point (hereinafter referred to as the "target point") and at least two points existing in a predetermined range r1 around the target point, and estimates the normal vector n of the obtained small plane as the normal vector of the target point. The calibrator 610 performs this estimation of normal vector n for all points included in the data. In Figure 8, the normal vector 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, with respect to the target point, the calibrator 610 expands the minute plane obtained in step S201 to a predetermined search range r2. The calibrator 610 searches for another point located on the expanded plane to the search range r2 and having a normal whose angle with the target point's normal is within a predetermined angle threshold. The conditions for this search are called the "search conditions." The search range r2 is larger than the range r1. The calibrator 610 considers a point located on the expanded plane if its distance from the expanded plane is within a predetermined distance threshold. If a point that satisfies the search conditions is found, it is presumed that the target point and the other point are on the same plane. In other words, the finding of a point that satisfies the search conditions means that the target point is presumed to be a point on a plane of a predetermined size, i.e., a point that constitutes a plane.

[0089] If a point that satisfies the search conditions is found, the calibrator 610 determines that the target point is a point that constitutes a plane. On the other hand, if no point that satisfies the search conditions is found, the 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 lies on a plane that is an enlargement of the infinitesimal plane of point p1. The angle between the normal n1 of point p1 and the normal n2 of point p2 is within the angle threshold. In this case, 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, then it is highly likely that points in its vicinity also constitute the same plane. Therefore, the normal of the infinitesimal plane is highly likely to be the normal of the target point. In addition, if the plane that the target point constitutes is a plane of a certain size, then it is highly likely that there is a point on the plane obtained by expanding the infinitesimal plane to the search range r2 that has a normal at approximately the same angle as the target point. On the other hand, if the target point does not constitute a plane, then 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 differ significantly from the actual normal of the target point. Even if such an infinitesimal plane is expanded to the search range r2, it is not highly likely that another point exists on the expanded plane. Even if another point exists on the expanded plane, the target point and the other point do not actually lie on the same plane, so the angles of their normals may differ significantly. Therefore, if the aforementioned search conditions are met, the target point is highly likely to be a point that constitutes a plane. On the other hand, if the search conditions are not met, the target point is highly likely not to be a point that constitutes a plane.

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

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

[0094] Subsequently, in step S205, the calibrator 610 determines whether or not it has performed a determination on all points included in the data as points that constitute a plane. If the determination is not complete, the 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 judgment has been completed for all points included in the data, the calibrator 610 will terminate point extraction.

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

[0097] Next, we will explain in detail the calibration method for the target sensor. Figure 9 is a flowchart of the subroutine for the calibration method of the target sensor.

[0098] First, in step S301, the calibrator 610 calculates a cost function relating to the positional discrepancies between corresponding points in the point cloud map and sensing data in the reference coordinate system (in this example, the moving coordinate system). Details of how the cost function is calculated will be described later.

[0099] In step S302, the calibrator 610 derives a coordinate transformation matrix. That is, the calibrator 610 finds a coordinate transformation matrix to transform the position of each point in the sensing data in the reference coordinate system so as to minimize the cost function. In short, the calibrator 610 finds the 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 the convergence conditions have been met. 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 less than or equal to a predetermined reduction rate threshold. The convergence condition may also be that the difference between the previous cost reduction rate and the current cost reduction rate, i.e., the amount of change, is less than or equal to a predetermined difference threshold. The convergence condition may also be that the number of iterations of deriving the coordinate transformation matrix reaches a predetermined threshold. The convergence condition is a combination of at least two of the three conditions described above, and may also be that at least one of multiple conditions is met.

[0101] The total cost is the 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 shift between corresponding points when each point in the sensing data is actually transformed using the coordinate transformation matrix. The total cost before calibration is the value obtained by the cost function without transforming the sensing data using the coordinate matrix transformation. In other words, the total cost before calibration is a value that comprehensively represents the positional shift between corresponding points when each point in the sensing data is not transformed. In short, the cost reduction rate represents how much the positional shift between corresponding points has decreased overall when calibration is performed using the coordinate transformation matrix compared to when no calibration is performed. If the positional shift between corresponding points decreases overall to a certain level or more, 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 represents how much the overall positional displacement between corresponding points has decreased by re-deriving the coordinate transformation matrix. If the overall positional displacement between corresponding points does not decrease significantly even after re-deriving the coordinate transformation matrix, the convergence condition for the difference in cost reduction rates is satisfied.

[0103] The convergence condition regarding the number of iterations in the derivation of the coordinate transformation matrix is ​​independent of the cost reduction rate. The convergence condition regarding the number of iterations in the derivation of 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 met, the calibrator 610, in step S304, transforms the coordinates of each point in each of the multiple sets of sensing data using the derived coordinate transformation matrix. Then, the calibrator 610 returns to step S301 and repeats the process starting from the derivation of the cost function. In other words, the derivation of the cost function and the coordinate transformation matrix is ​​performed again using the sensing data that has been transformed by the derived coordinate transformation matrix, so the next coordinate transformation matrix may differ from the previous coordinate transformation matrix. In addition, since the next coordinate transformation matrix is ​​derived using sensing data in which the positional misalignment between corresponding points has been reduced by the coordinate transformation, the next coordinate transformation matrix may be able to further reduce the positional misalignment between corresponding points. The accuracy of the coordinate transformation matrix is ​​improved by repeating the derivation of the coordinate transformation matrix until the convergence condition is met.

[0105] If the convergence condition is met, the calibrator 610 stores the calibration coordinate transformation matrix in the memory 62 in step S305 and terminates the process. For example, the calibrator 610 derives the calibration coordinate transformation matrix by multiplying all the coordinate transformation matrices derived by repeating the process in step S302. Each point in the sensing data is transformed using the calibration coordinate transformation matrix.

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

[0107] First, in step S401, the calibrator 610 creates a point cloud map, 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 status of the mobile body 1. The point cloud map and sensing data are data from which points constituting a plane have been extracted.

[0108] Next, in step S402, the calibrator 610 selects one dataset from among multiple datasets and creates a combination of two data from the multiple data contained in the selected dataset. Specifically, since the dataset contains 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 the sensing data from the first target sensor 3B, a combination of the point cloud map and the sensing data from the second target sensor 3C, and a combination of the sensing data from the first target sensor 3B and the sensing data from the second target sensor 3C.

[0109] In step S403, the calibrator 610 selects one combination from the three combinations created. In step S404, the calibrator 610 searches for corresponding points between the two data sets included in the selected combination. For example, if the calibrator 610 selects a combination of a point cloud map and sensing data from the first target sensor 3B, it searches for corresponding points between the point cloud map and the sensing data from the first target sensor 3B. As an example of searching for corresponding points, the calibrator 610 searches for a point in the other data set that is closest to a point in the other data set. The calibrator 610 considers these two closest points to be 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 the other data set. If there is no point in the other data set within a predetermined distance from a point in the one data set, it may be considered that no corresponding points exist. The calibrator 610 searches for corresponding points for all points included in the data set with the smaller number of points. Furthermore, the calibrator 610 may search for corresponding points with a number of points less than the total number of points in the data set with fewer points.

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

[0111] Cost = d ２ = {n1・(p1-p2)} ２ ... (1) However, the "•" in the formula represents the dot product, n1 represents the unit vector of the normal to the plane formed by the reference point, p1 represents the coordinates of the reference point, and p2 represents the coordinates of the symmetric point.

[0112] The coordinate transformation matrix is ​​a matrix used to bring the corresponding points of one set of data closer to the corresponding points of the other set of data. If the coordinate transformation matrix is ​​appropriate, the distance between the two corresponding points decreases, and as a result the cost decreases. The coordinate transformation matrix is ​​a variable at the time of this cost calculation. The constants of the coordinate transformation matrix are determined in step S302 described above. The calibrator 610 calculates the cost for all the corresponding points explored in step S403.

[0113] In step S406, the calibrator 610 weights the cost of each corresponding point. The weight represents the importance of the cost. For example, the calibrator 610 may weight the cost according to the angle of incidence of the measurement light from the target sensor to the corresponding point. The larger the angle of incidence of the measurement light, the greater the distortion of the spot diameter on the surface of the object. Since a large distortion of the spot diameter reduces the position detection accuracy, the calibrator 610 may reduce the weight of the cost of the corresponding point as the angle of incidence of the measurement light to the corresponding point increases. For example, the calibrator 610 may weight the cost according to the reflection intensity of the measurement light at the corresponding point. The smaller the reflection intensity, the lower the position detection accuracy 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 cost has been added to the cost function for all combinations of data included in the dataset. If the cost has not been added for all combinations, the calibrator 610 returns to step S403 and selects another combination for which the cost has not been added. After that, the calibrator 610 executes the process from step S404 onwards again.

[0116] The calibrator 610 completes the addition of costs to the cost function for all combinations of data included in a single dataset by repeating the process from steps S403 to S408. That is, for a single dataset, the costs of corresponding points between the point cloud map and the sensing data of the first target sensor 3B, the costs of corresponding points between the point cloud map and the sensing data of the second target sensor 3C, and the costs 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 cost addition has been completed for all combinations, in step S409, the calibrator 610 determines whether cost addition to the cost function has been completed for all datasets. If cost addition has not been completed for all datasets, the calibrator 610 returns to step S402 and retrieves another dataset for which cost addition has not been completed. The calibrator 610 performs the processes from steps S403 to S408 for the retrieved dataset. The calibrator 610 completes cost addition to the cost function for all datasets by repeating the processes from steps S402 to S408. In other words, cost addition to the cost function is completed for all datasets representing all acquisition statuses of the mobile body 1. This completes the calculation of the cost function.

[0118] In such a mobile device 100, the sensor coordinate system of the target sensor is calibrated so that the positional discrepancy between corresponding points in the point cloud map and sensing data in the reference coordinate system is reduced. In this example, a point cloud map is generated based on the detection results of the first sensor 3A, and then self-position estimation is performed. Therefore, by calibrating the sensor coordinate system of the target sensor so that the positional discrepancy between corresponding points in the point cloud map and sensing data in the reference coordinate system is reduced, the relative positional relationship between the first sensor 3A and the target sensor is corrected to an apparent appropriate positional relationship. As a result, if the mounting accuracy of the first sensor 3A to the trolley 10 is high (i.e., the first sensor 3A is mounted in an appropriate position on the trolley 10), the position of each point in the sensing data detected by the target sensor will accurately reflect the actual location of the obstacle. Furthermore, in this example, the sensor coordinate system is calibrated so that, in addition to the positional discrepancies between corresponding points in the point cloud map and point cloud data in the reference coordinate system, the positional discrepancies between corresponding points in the sensing data of the first target sensor 3B and the sensing data of the second target sensor 3C in the reference coordinate system are reduced. As a result, the location of the obstacle is accurately determined, and interference between the mobile body 1 and the obstacle is suppressed.

[0119] Furthermore, in the mobile device 100, a point cloud map is used as reference point cloud data for evaluating the positional deviation of each point in the sensing data, thereby improving the accuracy of calibration. If point cloud data obtained from a single-shot measurement of one sensor 3 were used as the reference, the number of points included in this reference point cloud data would be relatively small, resulting in a relatively small number of corresponding points with the sensing data. This could lead to a decrease in calibration accuracy. On the other hand, in the mobile device 100, since a point cloud map is used as the reference, the number of corresponding points with the sensing data can be increased relatively. This improves the accuracy of calibration. In particular, in this example, multiple sets of sensing data are used as sensing data to determine the positional deviation from the points on the point cloud map, so the number of corresponding points can be increased compared to when a single set of sensing data is used. As a result, the accuracy of calibration can be further improved.

[0120] Furthermore, in this example, the mobile device 100 calculates a cost function by summing up multiple costs related to multiple sets of sensing data, and then finds a coordinate transformation matrix to transform the position of each point in the sensing data in the reference coordinate system so as to minimize this cost function. In other words, the mobile device 100 calculates the cost function using all points included in the multiple sets of sensing data and optimizes this cost function all at once. If the cost function were calculated and optimized for each set of sensing data, the cost function might be optimized for each set of sensing data, but it might not be optimized for the entire set of sensing data. That is, even after calibration, there might be a relatively large positional shift for each point in the entire set of sensing data. In the mobile device 100, the cost function is optimized all at once, resulting in an overall optimization of the entire set of sensing data. As a result, the accuracy of the calibration can be further improved.

[0121] In addition, the mobile device 100 extracts points that constitute a plane from both the point cloud map and the sensing data. That is, the mobile device 100 removes points other than those that constitute a plane from both the point cloud map and the sensing data. Points other than those that constitute a plane are measured by detecting moving objects such as people. The mobile device 100 calibrates the sensor coordinate system with respect to the extracted points that constitute a plane so that the positional discrepancy between corresponding points in the point cloud map and the sensing data in the reference coordinate system is reduced. By using points that constitute a plane for calibration in this way, the use of measurement points of moving objects such as people, which would reduce the accuracy of the calibration, is suppressed. Furthermore, since the points that constitute a plane are arranged in a straight line, it is relatively easy to search for corresponding points between points that constitute a 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. This allows the calibration of the target sensor to be completed before autonomous movement is performed.

[0123] Furthermore, in this example, the reference coordinate system is a mobile coordinate system, and the positional shift of each point in the sensing data is corrected within the mobile coordinate system. Since mobile coordinate systems are often used in operational settings, the sensed data after calibration becomes easier to handle.

[0124] The control device 6 may also control the robot arm 12 when performing autonomous movement. Figure 11 is a functional block diagram showing the configuration of the control system of the processor 61 according to a modified example. The processor 61 may also 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 ​​corresponding to the target shape of the robot arm 12. Based on the command values, the arm controller 612 calculates the command operation amount for each of the multiple motors 12a. For example, the operation amount is the rotational speed or torque of the motor.

[0127] The arm controller 612 may maintain the robot arm 12 in a constant shape while the mobile body 100 is moving, and may operate the robot arm 12 when it is performing work.

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

[0129] Figure 12 is a side view of the mobile body 1 when the robot arm 12 is in a traveling configuration. Figure 13 is a top view of the mobile body 1 when the robot arm 12 is in a traveling configuration.

[0130] For example, the robot arm 12 in a mobile configuration is positioned relatively high. For instance, the mobile robot arm 12 bends at an intermediate joint between the shoulder joint and the wrist joint, for example, the fourth joint J4. The portion between the base 11 and the intermediate joint extends diagonally downward and backward from the base 11, while the portion between the intermediate joint and the wrist joint extends forward from the intermediate joint. In other words, the mobile robot arm 12 has a shape in which the intermediate joint is pulled backward and bent at the intermediate joint. As a result, the portion of the robot arm 12 closer to the end effector than the intermediate joint is positioned relatively high. Furthermore, the end effector of the robot arm 12 is positioned relatively far back.

[0131] In its mobile configuration, the robot arm 12 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. Since the robot arm 12 is positioned above the first sensor 3A, it may obstruct a portion of the detection range of the first sensor 3A. The detection results of the first sensor 3A corresponding to the robot arm 12 are treated as invalid. The higher the position of the robot arm 12, the further away the robot arm 12 is from the first sensor 3A. The further the robot arm 12 is from the first sensor 3A, the smaller the area of ​​the detection range of the first sensor 3A tends to be obstructed by the robot arm 12. Therefore, in its mobile configuration, the detection range of the first sensor 3A is relatively large.

[0132] Furthermore, the amount of forward protrusion of the robot arm 12 in its travel shape from the base 11 is relatively small. By reducing the amount of forward protrusion of the robot arm 12, the detection range of the first sensor 3A, specifically the diagonally upward forward area from the first sensor 3A, is expanded.

[0133] The overall width of the robot arm 12 in its mobile configuration, as viewed from above, is relatively small. For example, in the mobile configuration, the second link L2 is located on the outermost side in the width direction. Of the multiple links L, all links other than the second link L2 are positioned further inward in the width direction than the second link L2. By making the overall width of the robot arm 12 in its mobile configuration, as viewed from above, relatively small, the possibility of interference between the robot arm 12 and other objects located in the width direction during movement can be reduced. In addition, the robot arm 12 in its mobile configuration may be positioned in front of the rotation axis of the first joint J1 in the front-rear direction by rotating the first link L1 forward around the rotation axis of the first joint J1. This further reduces the width of the second link L2 of the two robot arms, i.e., the overall width of the robot arm 12 as viewed from above.

[0134] The overall shape of the robot arm 12 in its travel configuration, as seen from a plan view, is contained within the carriage 10 in the front-to-back direction. This reduces the possibility of interference between the robot arm 12 and other objects located in the front-to-back direction during travel.

[0135] Furthermore, the shapes of the two robot arms 12 do not have to be exactly the same in terms of their movement. In other words, the shapes of the two robot arms 12 may be slightly different. For example, the tip of one robot arm 12 may be at a different height than the tip of the other robot arm 12. The seventh joint J7 of one robot arm 12 may be at a different rotation angle than 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 robot arm 12 to move and allows the robot arm 12 to move freely. For example, the arm controller 612 operates the robot arm 12 after the mobile body 1 has reached its destination and performs work with the robot arm 12.

[0137] Figure 14 is a side view of a modified mobile body 100. The mobile body 100 has multiple sensors 3, including a first sensor 3A and a fourth sensor 3D. The mobile 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. That is, the fourth sensor 3D is a sensor to be calibrated.

[0138] The target sensor may be positioned closer to the tip of the robot arm than other sensors. For example, the fourth sensor 3D is positioned 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 located on the robot arm 12. For example, the fourth sensor 3D is located on one of the robot arms 12. The fourth sensor 3D is optionally located on one of the multiple links L. For example, the fourth sensor 3D is located 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 refers to the distance through the 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, passing sequentially through the connection point of the base 11 of the trolley 10, the connection point of the robot arm 12 to the base 11, and each joint J of the robot arm 12, 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, passing sequentially through the joints J of the robot arm 12 that are located closer to the tip of the robot arm 12 than the fourth sensor 3D, to the tip of the robot arm 12.

[0141] The fourth sensor 3D may detect objects in the three-dimensional space surrounding the mobile body 1. For example, the fourth sensor 3D is 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-position estimation according to the distance to the destination. For example, the control device 6 may perform self-position estimation based on the detection result of the first sensor 3A in the first section where the distance to the destination is outside a predetermined switching range, and perform self-position estimation based on the detection result of the fourth sensor 3D in the 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 using the fourth sensor 3D for self-position estimation, the accuracy of estimating the position of the tip of the robot arm 12 is improved. Furthermore, since the fourth sensor 3D is accurately calibrated by performing calibration based on a point cloud map, the accuracy of estimating the position of the tip of the robot arm 12 is further improved. The second section in which self-position estimation 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 robot arm 12's work is improved.

[0144] The first sensor 3A is relatively far from the tip of the robot arm 12. The positional accuracy of the tip of the robot arm 12 obtained by self-position estimation using the first sensor 3A may be inferior to that obtained by self-position 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 as much as possible by the base 11 and the robot arm 12.

[0146] Furthermore, 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 positioned on the robot arm 12, the detection range of the fourth sensor 3D depends on the movement shape of the robot arm 12. Part of the detection range of the fourth sensor 3D may be obstructed by the base 11, etc. The area of ​​the detection range of the fourth sensor 3D that is obstructed by the base 11, etc. may be larger than the area of ​​the detection range of the first sensor 3A that is obstructed by the base 11, etc.

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

[0148] 《Other Embodiments》 As described above, the embodiments described herein have been presented as examples of the technology disclosed herein. However, the technology in this disclosure is not limited thereto and can be applied to embodiments that have been modified, replaced, added, or omitted as appropriate. It is also possible to combine the components described in the embodiments above to create new embodiments. Furthermore, the components described in the attached 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 illustrate the technology. Therefore, the mere presence of such non-essential components in the attached drawings and detailed description should not be immediately assumed to mean that those non-essential components are essential.

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

[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; there may be one or three or more target sensors.

[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 first transform the position of each point in the sensing data in the sensor coordinate system of the target sensor to the mobile body coordinate system, and then further transform it to the map coordinate system based on the relationship between the mobile body coordinate system and the map coordinate system. The relationship between the mobile body coordinate system and the map coordinate system is determined by the self-position estimation of the mobile body 1. In this case, the transformation of each point on the point cloud map to the reference coordinate system is omitted. In calculating the cost function shown in Figure 10, the point cloud map may be common to all acquisition conditions of the mobile body 1.

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

[0154] The calibrator 610 does not need to calibrate the sensor coordinate system so as to reduce the positional discrepancy 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. In other words, the calibrator 610 may calibrate the sensor coordinate system so as to reduce only the positional discrepancy between corresponding points between the point cloud map and the sensing data in the reference coordinate system.

[0155] The multiple sets of sensing data are not limited to the configuration of the embodiment described above. Multiple sets of sensing data may be acquired by the target sensor when only the position of the mobile body 1 is different, when only the orientation of the mobile body 1 is different, or when both the position and orientation of the mobile body 1 are different. The number of sets of sensing data may be two or four or more. There may be only one set of sensing data. That is, the sensing data may be acquired by the target sensor when the mobile body 1 is in a single position and orientation.

[0156] The calibrator 610 does not need to calculate a cost function by summing up multiple costs related to 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 at which the calibrator 610 acquires sensing data is not limited to when the first sensor 3A collects point cloud data for point cloud map creation, but may also be during the normal autonomous movement of the mobile body 1.

[0158] The convergence condition for the cost function is not limited to the configuration of the above embodiment and can be set arbitrarily.

[0159] The flowchart is merely an example. The steps in the flowchart may be changed, replaced, added, or omitted as appropriate. The order of the steps in the flowchart may also be changed, or sequential processes may be processed in parallel. For example, in the flowchart shown in Figure 6, steps S101 and S102 may be processed sequentially. For example, in the flowchart shown in Figure 10, step S406 may be omitted.

[0160] The functions 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 circuits. The functions 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 circuits. One or more circuits or processing circuits may be programmed using one or more programs stored together or individually in one or more memories, or may be otherwise configured to perform the disclosed functions. A processor is considered a processing circuit or circuit because it includes transistors and other circuits. A processor may be a programmed processor that executes programs stored in memory. In this disclosure, a circuit, unit, or means is hardware that performs the enumerated functions individually or in combination with each other, or hardware programmed to perform the enumerated functions individually or in combination with each other. The hardware may be any hardware disclosed herein that is programmed or configured to perform the listed functions.

[0161] A computer program, including computer instructions, is stored in memory. The computer instructions provide logic and routines that enable hardware to execute the methods disclosed herein. The hardware includes, for example, processing circuits or circuits. The computer program may be implemented in known formats on computer-readable storage media, computer program products, memory devices, recording media such as CD-ROMs or DVDs, and / or in the memory of FPGAs or ASICs.

[0162] [Embodiment] The above embodiment is a specific example of the following embodiment.

[0163] (Aspect 1) A method for calibrating a sensor, comprising: acquiring a point cloud map of the area around the mobile body 1 of an autonomously moving mobile body 100; detecting objects in the surrounding area in the form of point cloud data; determining the position of each point in the point cloud data in the reference coordinate system by transforming the position of each point in the point cloud data (sensing data) detected by the target sensor in the sensor coordinate system of the target sensor to the reference coordinate system; and calibrating the sensor coordinate system so that the position difference between corresponding points in the point cloud map and the point cloud data in the reference coordinate system is reduced.

[0164] This configuration uses a point cloud map as reference point cloud data for evaluating the positional deviation of each point in the sensing data, thereby improving the accuracy of calibration. If point cloud data obtained from a single-shot measurement of one sensor is used as the reference, the number of points included in this reference point cloud data is relatively small, and therefore the number of corresponding points with the sensing data is also relatively small. As a result, the accuracy of calibration may decrease. On the other hand, with this configuration, since a point cloud map is used as the reference, the number of corresponding points with the sensing data can be increased relatively. This improves the accuracy of calibration of the target sensor.

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

[0166] This configuration allows the calibration of the target sensors to be completed before the autonomous movement of the mobile body 100 is performed.

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

[0168] In this configuration, the reference coordinate system is a mobile coordinate system, and the positional shift of each point in the sensing data is corrected within the mobile coordinate system. Since the mobile coordinate system is often used in operational settings, the sensed data after calibration becomes easier to handle.

[0169] (Aspect 4) A sensor calibration method according to any one of aspects 1 to 3, further comprising extracting points that constitute the plane of an object from the point cloud map and the point cloud data, respectively, wherein the calibration involves calibrating the sensor coordinate system such that the positional discrepancy between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is reduced with respect to the extracted points that constitute the plane of an object.

[0170] In this configuration, points other than those constituting a plane are removed from both the point cloud map and the sensing data. These points are measured by detecting moving objects such as people. With respect to the extracted points constituting a plane, the sensor coordinate system is calibrated so that the positional discrepancy between corresponding points in the point cloud map and the sensing data in the reference coordinate system is minimized. By using points constituting a plane for calibration in this way, the use of measurement points of moving objects such as people, which would degrade the calibration accuracy, is suppressed. Furthermore, since the points constituting a plane are arranged in a linear fashion, it is relatively easy to find corresponding points between points constituting a plane. As a result, the calibration accuracy 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 acquires multiple sets of point cloud data by the target sensor in a situation where at least one of the position and orientation of the mobile body 1 is different, and performing the calibration calibrates the sensor coordinate system such that the positional difference between corresponding points between the point cloud map in the reference coordinate system and the multiple sets of point cloud data is reduced.

[0172] This configuration allows for an increase in the number of corresponding points compared to using a single sensing data point. As a result, the accuracy of the target sensor calibration can be further improved.

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

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

[0175] (Aspect 7) A method for calibrating a sensor according to any one of claims 1 to 6, wherein the target sensor includes 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 the sensor coordinate system is calibrated such that, in addition to the positional discrepancy between corresponding points between the point cloud map and the point cloud data in the reference coordinate system, the positional discrepancy 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] With this configuration, the location of the obstacle can be accurately determined, and interference between the mobile unit 1 and the obstacle can be suppressed.

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

[0178] In this configuration, the target sensor is relatively close to the tip of the robot arm 12. Therefore, by performing self-position estimation using the target sensor, the accuracy of estimating the position of the tip of the robot arm 12 is improved. Furthermore, since the target sensor is accurately calibrated by performing calibration based on a point cloud map, the accuracy of estimating 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 comprising a mobile body body 1, a target sensor attached to the mobile body body 1 that detects 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 area around the mobile body body 1, detects objects around the mobile body body 1 using the target sensor, and determines the position of each point in the point cloud data in the reference coordinate system by transforming the position of each point in the point cloud data detected by the target sensor in the sensor coordinate system of the target sensor to a reference coordinate system. The control device 6 then calibrates the sensor coordinate system so that the positional discrepancy between corresponding points in the point cloud map and the point cloud data in the reference coordinate system is reduced.

[0180] This configuration allows for improved calibration accuracy of the target sensor.

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

[0182] This configuration allows for improved calibration accuracy of the target sensor.

[0183] (Aspect 11) The control program is a control program for calibrating a target sensor attached to the mobile body 1 of an autonomously moving mobile body 100, which detects surrounding objects in the form of point cloud data, and enables the computer to perform the following functions: to acquire a point cloud map of the area around the mobile body 1; to allow the target sensor to detect objects around the mobile body 1; to determine the position of each point in the point cloud data in the reference coordinate system by transforming the position of each point in the point cloud data detected by the target sensor in the sensor coordinate system to a reference coordinate system; and to calibrate the sensor coordinate system so that the positional discrepancy between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is reduced.

[0184] This configuration allows for improved calibration accuracy of the target sensor.

[0185] 100 Mobile Unit 1 Mobile Unit Body 12 Robot Arm 3A First Sensor (Another Sensor) 3B Second Sensor (First Target Sensor) 3C Third Sensor (Second Target Sensor) 3D Fourth Sensor (Target Sensor) 6 Control Device

Claims

1. A method for calibrating a sensor, which is attached to the body of an autonomously moving mobile body and detects surrounding objects in the form of point cloud data, comprising: acquiring a point cloud map of the area around the mobile body; detecting objects around the mobile body using the target sensor; determining the position of each point in the point cloud data in the reference coordinate system by transforming the position of each point in the point cloud data detected by the target sensor in the sensor coordinate system of the target sensor to a reference coordinate system; and calibrating the sensor coordinate system so that the positional discrepancy between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is reduced.

2. A sensor calibration method according to claim 1, further comprising creating a point cloud map, wherein a sensor other than the target sensor is attached to the mobile body for detecting surrounding objects in the form of point cloud data, and in creating the point cloud map, the mobile body detects objects around the mobile body with the other sensor while it is moving, and creates the point cloud map from the point cloud data detected by the other sensor, and in detecting objects around the mobile body, the target sensor detects objects in parallel with the detection of objects by the other sensor for creating the point cloud map.

3. A sensor calibration method according to claim 1, further comprising: performing self-position estimation to estimate the position and orientation of the mobile body in the map coordinate system of the point cloud map; and determining the position of each point in the point cloud map in the reference coordinate system by transforming the position of each point in the point cloud map in the map coordinate system to the reference coordinate system based on the position and orientation of the mobile body estimated by the self-position estimation, wherein the reference coordinate system is a mobile body coordinate system defined with respect to the mobile body.

4. A sensor calibration method according to claim 1, further comprising extracting points that constitute the plane of an object from the point cloud map and the point cloud data, wherein the calibration involves calibrating the sensor coordinate system such that the positional discrepancy between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is reduced with respect to the extracted points that constitute the plane of an object.

5. A sensor calibration method according to claim 1, wherein detecting objects around the mobile body acquires multiple sets of point cloud data by the target sensor under conditions where at least one of the position and orientation of the mobile body is different, and calibration involves calibrating the sensor coordinate system such that the positional discrepancy between corresponding points between the point cloud map in the reference coordinate system and the multiple sets of point cloud data is reduced.

6. A sensor calibration method according to claim 5, wherein the calibration includes: determining the cost related to the positional difference between corresponding points in the reference coordinate system between the point cloud map and the point cloud data for each set of the multiple sets of point cloud data; summing up the multiple costs for the multiple sets of point cloud data to calculate a cost function; and determining a coordinate transformation matrix for transforming the position of each point in the point cloud data in the reference coordinate system so as to minimize the cost function.

7. A method for calibrating a sensor according to claim 1, wherein the target sensor includes 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, and the method for calibrating the sensor coordinate system such that, in addition to the positional discrepancy between corresponding points between the point cloud map and the point cloud data in the reference coordinate system, the positional discrepancy 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 method for calibrating a sensor according to claim 1, further comprising creating a point cloud map, wherein the mobile body is a mobile robot including a robot arm, and the mobile body is fitted with a sensor other than the target sensor that detects surrounding objects in the form of point cloud data, the target sensor is positioned closer to the tip of the robot arm than the other sensor, and the method for creating the point cloud map involves the mobile body moving while the other sensor detects objects around the mobile body, and the method for calibrating a sensor that creates the point cloud map from the point cloud data detected by the other sensor.

9. An autonomous mobile body comprising: a mobile body body; a target sensor attached to the mobile body body for detecting surrounding objects in the form of point cloud data; and a control device for calibrating the sensor coordinate system of the target sensor, wherein the control device acquires a point cloud map of the area around the mobile body body; detects objects around the mobile body body using the target sensor; determines the position of each point in the point cloud data in the reference coordinate system by transforming the position of each point in the point cloud data detected by the target sensor in the sensor coordinate system of the target sensor to a reference coordinate system; and calibrates the sensor coordinate system so that the positional discrepancy between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is reduced.

10. A control device for calibrating a target sensor attached to the body of an autonomously moving mobile body, which detects surrounding objects in the form of point cloud data, the control device acquires a point cloud map of the area around the mobile body, detects objects around the mobile body using the target sensor, determines the position of each point in the point cloud data in the reference coordinate system by transforming the position of each point in the point cloud data detected by the target sensor in the sensor coordinate system of the target sensor to a reference coordinate system, and calibrates the sensor coordinate system so that the positional discrepancy between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is reduced.

11. A control program for calibrating a target sensor attached to the body of an autonomously moving mobile body, which detects surrounding objects in the form of point cloud data, the program causing a computer to perform the following functions: a function to acquire a point cloud map of the area around the mobile body; a function to cause the target sensor to detect objects around the mobile body; a function to determine the position of each point in the point cloud data in the reference coordinate system by transforming the position of each point in the point cloud data detected by the target sensor in the sensor coordinate system to a reference coordinate system; and a function to calibrate the sensor coordinate system so that the positional discrepancy between corresponding points between the point cloud map and the point cloud data in the reference coordinate system is reduced.

Images