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

Abstract

The sensor calibration method of the present invention is to calibrate an object sensor, which is installed on the moving body 1 of a moving body 100 that performs autonomous movement, and detects surrounding objects in the form of point group data. The calibration method includes: obtaining a point group map of the surroundings of the moving body 1; detecting objects around the moving body 1 by means of the object sensor; converting the positions of each point of the point group data detected by the object sensor in the sensor coordinate system of the object sensor into a reference coordinate system, thereby obtaining the positions of each point of the point group data in the reference coordinate system; and calibrating the sensor coordinate system by reducing the positional offset between corresponding points in the point group map and the point group data in the reference coordinate system.

Description

Technical Field

[0001] The technology disclosed in this specification relates to a sensor calibration method, a moving body, a control device, and a control program. [ ] Prior Technology

[0002] Patent Document 1 discloses a mobile body control system for controlling a mobile body. The mobile system includes a range sensor for acquiring point group data of objects located around the mobile body. The mobile system moves autonomously while avoiding obstacles based on the point group data acquired by the range sensor. [Previous Technical Documents] [Patent Literature]

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

[0004] [The problem that the invention aims to solve] However, the mobile device may have multiple sensors for acquiring point data. Each sensor is mounted at a predetermined position on the mobile device. However, the mounting positions of each sensor may contain installation errors. Furthermore, the mounting positions of each sensor may shift due to vibrations during the movement of the mobile device. In these cases, the relative positions between the sensors may shift, resulting in inadequate obstacle detection and potentially negatively impacting the autonomous movement of the mobile device. Therefore, to correct the relative positional relationships between the sensors, it is essential to calibrate the sensors with high accuracy.

[0005] The technology disclosed in this specification was developed in view of the above-mentioned problems, and its purpose is to improve the calibration accuracy of the sensor. [Methods used to solve problems]

[0006] The sensor calibration method disclosed in this specification is for calibrating an object sensor, which is installed on the body of a moving object performing autonomous movement and detects surrounding objects in the form of point group data. The calibration method includes: obtaining a point group map around the moving object; detecting objects around the moving object using the object sensor; converting the positions of each point in the point group data detected by the object sensor into a reference coordinate system, thereby obtaining the positions of each point in the point group data in the reference coordinate system; and calibrating the sensor coordinate system by reducing the positional offset between corresponding points in the point group map and the point group data in the reference coordinate system.

[0007] The mobile body disclosed in this specification is capable of autonomous movement. The movement system comprises: a mobile body body; an object sensor mounted on the mobile body body, which detects surrounding objects in the form of point group data; and a control device that calibrates the sensor coordinate system of the object sensor. The control device performs the following actions: acquiring a point group map of the area surrounding the mobile body body; detecting objects around the mobile body body using the object sensor; converting the positions of each point in the point group data detected by the object sensor in the sensor coordinate system of the object sensor into a reference coordinate system, thereby obtaining the positions of each point in the point group data in the reference coordinate system; and calibrating the sensor coordinate system by reducing the positional offset between corresponding points in the point group map and the point group data in the reference coordinate system.

[0008] The control device disclosed in this specification is a calibration object sensor. The object sensor is installed on the body of the moving body performing autonomous movement and detects surrounding objects in the form of point group data. The control device performs the following actions: acquiring a point group map around the moving body; detecting objects around the moving body using the object sensor; converting the positions of each point in the point group data detected by the object sensor into a reference coordinate system, thereby obtaining the positions of each point in the point group data in the reference coordinate system; and correcting the sensor coordinate system by reducing the positional offset between corresponding points in the point group map and the point group data in the reference coordinate system.

[0009] The control program disclosed in this specification is used to calibrate an object sensor, which is installed on the body of a moving object performing autonomous movement and detects surrounding objects in the form of point group data. The control program enables the computer to: obtain a point group map of the area surrounding the moving object; detect objects around the moving object using the object sensor; convert the positions of each point in the point group data detected by the object sensor into a reference coordinate system, thereby obtaining the positions of each point in the point group data in the reference coordinate system; and calibrate the sensor coordinate system by reducing the positional offset between corresponding points in the point group map and the point group data in the reference coordinate system. [Invention Benefits]

[0010] The aforementioned sensor calibration method can improve the calibration accuracy of the sensor.

[0011] Based on the aforementioned moving body, the calibration accuracy of the sensor can be improved.

[0012] The aforementioned control device can improve the calibration accuracy of the sensor.

[0013] Based on the aforementioned control program, the calibration accuracy of the sensor can be improved. [ ] Simple Explanation of the Diagram

[0014] Figure 1 is a three-dimensional view of the moving body. Figure 2 is a schematic diagram showing the detection range of the sensor. Figure 3 shows the hardware configuration of the control device. Figure 4 is a functional block diagram showing the structure of the processor's control system. Figure 5 is a flowchart of the basic movements of a moving body. Figure 6 shows a flowchart of the sensor calibration method. Figure 7 is a flowchart of the sub-formula of the point extraction method. Figure 8 is an explanatory diagram illustrating the point extraction method. Figure 9 is a flowchart of the secondary constant of the correction method. Figure 10 is a flowchart of the sub-constants of the method for calculating the cost function. Figure 11 is a functional block diagram showing the configuration of the control system of the processor in the modified example. Figure 12 is a side view of the moving body when the robotic arm is in a traveling shape. Figure 13 is a top view of the moving body when the robotic arm is in a traveling shape. Figure 14 is a side view of the moving body in the modified example. [ ] Implementation

[0015] The following is a detailed description of an exemplary embodiment based on the drawings. Figure 1 is a perspective view of the mobile body 100. The mobile body 100 moves autonomously. The mobile body 100 includes: a mobile body body 1, and a control device 6 that enables the mobile body body 1 to perform autonomous movement. For example, the mobile body 100 moves within facilities such as shops, hospitals, or nursing homes. In addition to moving, the mobile body 100 can also perform operations such as receiving and transferring items or opening and closing doors.

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

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

[0018] The trolley 10 comprises a plurality of wheels 13 and is capable of movement. In this example, the trolley 10 comprises four wheels 13. The trolley 10 is also capable of moving straight and turning. In this example, the trolley 10 can move forward, backward, left, right, and diagonally while maintaining its posture, that is, it can move in all directions. In other words, the trolley 10 can also move parallel in directions other than forward and backward. Moreover, the trolley 10 can also rotate in place. For example, the four wheels 13 comprise: a set of wheels 13 arranged in the left-right direction at the front of the bottom of the trolley 10, and a set of wheels 13 arranged in the left-right direction at the rear of the bottom of the trolley 10. They can also be arranged in a quadrilateral configuration at the bottom of the trolley 10. More specifically, the four wheels 13 are located at the four corners of the bottom of the trolley 10.

[0019] In detail, wheel 13 can be an omnidirectional wheel. In this example, wheel 13 is a Mecanum wheel. Wheel 13 has a plurality of cylindrical rollers arranged around its outer circumference. For example, the rotation axis of each roller is tilted at 45 degrees relative to the axle of wheel 13.

[0020] The moving body 1 may also include: a motor 13a for driving the wheels 13, and an encoder 13b for detecting the rotation of the motor 13a (see Figure 3). In this example, the moving body 1 has four sets of motors 13a and encoders 13b corresponding to the four wheels 13. The four wheels 13 can also be driven independently by their respective motors 13a.

[0021] The trolley 10 can also move in any direction on a two-dimensional plane using the four wheels 13 as described above. For example, the trolley 10 can move parallel or turn in any direction, such as forward, backward, left, right, or diagonally. The trolley 10 can also rotate in its position.

[0022] The base 11 can also be mounted on the trolley 10. In this example, the base 11 has a shape that simulates the upper body of a human body. The base 11 can also be fixed to the trolley 10 without being moved.

[0023] The mobile body 1 has two robotic arms 12. A robotic hand 14 can be installed at the front end of each robotic arm 12.

[0024] The two robotic arms 12 are each connected to different parts of the base 11. For example, the two robotic arms 12 are each connected to different parts of the base 11 in the width direction, which is one direction when viewed from above. In other words, the direction in which one robotic arm 12 is connected to the base 11 and the other robotic arm 12 is connected to the base 11 in the width direction when viewed from above. The base 11 may also have a front and a back that face opposite each other when viewed from above. For example, the front and back directions of the base 11 are defined such that the side facing the front is the front and the side facing the back is the back. The width direction may be horizontal and orthogonal to the front and back direction. That is, the width direction is the left and right direction relative to the front and back direction. For example, one robotic arm 12 is connected to the left side of the base 11, and the other robotic arm 12 is connected to the right side of the base 11.

[0025] In the case where the planar shape of the trolley 10 is approximately square with a long side and a short side, the width direction is approximately consistent with the short side direction of the planar shape of the trolley 10.

[0026] For example, as shown in Figure 1, the robotic arm 12 has a plurality of links L and a plurality of joints J connecting the plurality of links L. The robotic arm 12 is configured to operate in a three-dimensional manner. In this example, the robotic arm 12 is a multi-joint type robotic arm. That is, the robotic arm 12 can also freely change its shape by rotating the joints. The robotic arm 12 is supported by a base 11.

[0027] For example, a plurality of links L include: a first link L1, a second link L2, a third link L3, a fourth link L4, a fifth link L5, a sixth link L6, and a seventh link L7 arranged sequentially from the base 11 side. The seventh link L7 is located at the front end of the robotic arm 12. For example, a plurality of joints J include: a first joint J1, a second joint J2, a third joint J3, a fourth joint J4, a fifth joint J5, a sixth joint J6, and a seventh joint J7 arranged sequentially from the base 11 side. The position and orientation of the seventh link L7 have six degrees of freedom: translation and rotation about three orthogonal axes. The robotic arm 12 may also have seven joints J, i.e., a so-called seven-axis robot. That is, the robotic arm 12 has redundancy. Redundancy refers to the characteristic that the rotation angles of the plurality of joints J corresponding to the position and orientation of the front end of the robotic arm 12 are not uniquely determined.

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

[0029] The robotic arm 14 can also be connected to the seventh link L7 at the front end of the robotic arm 12. That is, the robotic arm 14 is connected to the robotic arm 12 in a manner that allows it to rotate about the rotation axis of the seventh joint J7. The robotic arm 14 is mounted on the end effector of the robotic arm 12.

[0030] In detail, the plurality of joints J may also include joints that function as shoulder joints. For example, the plurality of joints J may include joints that function as shoulder joints in terms of horizontal extension and flexion. The rotational axis of the joints functioning as shoulder joints in terms of horizontal extension and flexion extends in a generally vertical direction. The plurality of joints J may also include joints that function as shoulder joints in terms of extension and flexion. The rotational axis of the joints functioning as shoulder joints in terms of extension and flexion extends in a generally horizontal direction.

[0031] For example, the first joint J1 functions as the shoulder joint of the robotic arm 12. The first joint J1 can also have the functions of horizontal extension and horizontal flexion of the shoulder joint. The rotation axis of the first joint J1 extends in a generally vertical direction.

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

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

[0034] The plurality of joints J may also include joints that function as wrist joints. For example, the plurality of joints J may include joints that have the function of internal and external rotation of the wrist joint, or the function of inversion and eversion of the wrist joint. For example, the seventh joint J7 may also have the function of internal and external rotation of the wrist joint. The sixth joint J6 may also have the function of inversion and eversion of the wrist joint.

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

[0036] The robotic arm 12 has motors 12a (see Figure 3) that drive the rotation of each joint J. For example, motor 12a is a servo motor. Each motor 12a has an encoder 12b (see Figure 3).

[0037] The mobile body 100 may also include a sensor 3 that detects objects (hereinafter referred to as "surrounding objects") around the mobile body 1 in the form of point group data. In this invention, "objects" includes both inanimate and living organisms. The sensor 3 is mounted on the mobile body 1. For example, the sensor 3 is mounted on the trolley 10. In this example, the sensor 3 is a ranging 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, for example, has a light-emitting part and a light-receiving part; the light-emitting part irradiates measurement light, specifically laser light, towards the periphery of the mobile body 1; the light-receiving part receives measurement light reflected from the surface of the surrounding objects. The sensor 3 measures the time of flight (ToF) from when the measurement light irradiated by the light-emitting part hits the surface of the surrounding objects and returns to the light-receiving part. The sensor 3 measures the distance from the sensor 3 to the surface of the surrounding objects based on the measured time of flight. Sensor 3 can also generate point group data based on the measured distance. Point group data refers to the three-dimensional position information of the surfaces of surrounding objects. For example, sensor 3 outputs the calculated point group data to control device 6. Sensor 3 can also repeatedly detect surrounding objects at predetermined detection cycles as the moving body 1 moves. Sensor 3 can also output its detection results, i.e., point group data, to control device 6 each time a surrounding object is detected.

[0038] In this example, the mobile body 100 has a plurality of sensors 3. Figure 2 is a schematic diagram showing the detection range of the sensors 3. Figure 2 is a top view of the mobile body 100, and the robotic arm 12, etc., is omitted. The mobile body 100 may also have: 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 moving body 1. The first sensor 3A is mounted on the front of the trolley 10. For example, the first sensor 3A is mounted in the trolley 10 further forward than the base 11 and approximately at the center in the left-right direction. The first sensor 3A detects objects in the three-dimensional space surrounding the moving body 1. The first sensor 3A can also be a 3D LiDAR. The first sensor 3A scans the measurement light in both horizontal and vertical directions. In this example, the first sensor 3A scans the measurement light 360 degrees horizontally, as shown by the two-point chain in Figure 2. In the vertical direction, the first sensor 3A scans the measurement light within a predetermined range including elevation and depression angles. The scanning range of the measurement light is the detection range of the objects by the first sensor 3A.

[0040] The second sensor 3B and the third sensor 3C detect objects at least behind the moving body 1. The second sensor 3B and the third sensor 3C can also be mounted at the rear of the carriage 10. More specifically, the second sensor 3B and the third sensor 3C are mounted in the carriage 10 further rearward than the base 11. The second sensor 3B is mounted at the left rear corner of the carriage 10, and the third sensor 3C is mounted at the right rear corner of the carriage 10. The second sensor 3B and the third sensor 3C can also detect objects in the horizontal two-dimensional space surrounding the moving body 1.

[0041] The third sensor 3C has a detection range that at least partially overlaps with the detection range of the second sensor 3B. That is, the object detection range of the second sensor 3B and the object detection range of the third sensor 3C at least partially overlap. For example, the second sensor 3B and the third sensor 3C are 2D LiDARs. The second sensor 3B and the third sensor 3C scan the measurement light horizontally. The second sensor 3B and the third sensor 3C detect objects within a range that is at least undetectable by the first sensor 3A in the horizontal direction. The second sensor 3B scans the measurement light at least to the left rear of the carriage 10. The third sensor 3C scans the measurement light at least to the right rear of the carriage 10. The scanning range of the measurement light obtained by the second sensor 3B and the scanning range of the measurement light obtained by the third sensor 3C partially overlap at the rear of the carriage 10. In this example, as shown by the dotted chain line in Figure 2, the second sensor 3B scans the measuring light at approximately 270 degrees horizontally, from the front to the right of the region containing the left side of the moving body 1. As shown by the dotted line in Figure 2, the third sensor 3C scans the measuring light at approximately 270 degrees horizontally, from the front to the left of the region containing the right side of the moving body 1. The second sensor 3B and the third sensor 3C detect objects at approximately the same height. That is, the scanning plane of the measuring light obtained by the second sensor 3B and the scanning plane of the measuring light obtained by the third sensor 3C are at approximately the same height. The scanning range of the measuring light of each of the second sensor 3B and the third sensor 3C is the detection range of the objects of each of the second sensor 3B and the third sensor 3C.

[0042] As shown in Figure 2, since the base 11 is disposed behind the first sensor 3A, the first sensor 3A cannot properly scan the range F overlapping with the base 11 with the measurement light. In contrast, since the second sensor 3B and the third sensor 3C are disposed behind the base 11, the second sensor 3B and the third sensor 3C can also scan the measurement light in the range F.

[0043] Hereinafter, when the first sensor 3A, the second sensor 3B, and the third sensor 3C are not distinguished, they will be referred to simply 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 "object sensors". That is, in this example, the second sensor 3B and the third sensor 3C are sensors for calibrating the object. In the following description, the second sensor 3B is also referred to as "the first object sensor 3B". The third sensor 3C is also referred to as "the second object sensor 3C". When the second sensor 3B and the third sensor 3C are not distinguished, they will also be referred to simply as "object sensors".

[0044] Figure 3 shows the hardware configuration of the control device 6. The control device 6 controls the entire mobile body 1. The control device 6 enables the mobile body 1 to move autonomously while estimating its own position. The control device 6 actuates the motor 13a of the wheel 13 to move the mobile body 1. Furthermore, the control device 6 controls the motor 12a of the robotic arm 12 to enable the robotic arm 12 to perform a predetermined task. The control device 6 includes a processor 61, a memory 62, and a memory 63.

[0045] Processor 61 performs various computational operations. For example, processor 61 may be composed of a central processing unit (CPU) or other processors. Processor 61 may also be composed of a micro controller unit (MCU), microprocessor unit (MPU), field programmable gate array (FPGA), programmable logic controller (PLC), system LSI, etc. Through the operation of motor 13a by processor 61, the moving body 1 will operate autonomously.

[0046] Memory 62 stores programs and various data to be executed by processor 61. For example, memory 62 stores control programs. Memory 62 stores map information related to the map of the environment in which the mobile body 1 moves. For example, the map information includes three-dimensional maps and two-dimensional maps. The three-dimensional map is formed using three-dimensional point group data. For example, the three-dimensional map is a three-dimensional point group map. The three-dimensional point group map is an example of a point group map. In the following description, the three-dimensional point group map will be simply referred to as a "point group map". The three-dimensional map uses point group data to represent the three-dimensional shape of obstacles in the environment, such as walls, ceilings, railings, shelves, tables, or chairs. The two-dimensional map is a planar map. For example, a two-dimensional occupancy grid map. The two-dimensional map represents the planar shape of obstacles in the environment, such as walls, ceilings, railings, shelves, tables, or chairs. For example, the two-dimensional map is formed by projecting a three-dimensional map onto a plane. Memory 62 is formed using non-volatile memory, a hard disk drive (HDD), or a solid state drive (SSD). Memory 63 temporarily stores data. For example, memory 63 is formed using non-volatile memory.

[0047] Here, the coordinate systems are explained. In this invention, the coordinate system of the point group map is referred to as the "map coordinate system." The map coordinate system is also called the global coordinate system. The coordinate system defined with respect to the moving body 1 is called the "moving body coordinate system." The coordinate system defined with respect to the sensor 3 is called the "sensor coordinate system."

[0048] Figure 4 is a functional block diagram showing the configuration of the control system of processor 61. Processor 61 reads the control program from memory 62 into memory 63 and expands it to realize various functions. For example, processor 61 performs the following functions: a state estimator 64 for estimating the state of the moving body 1; a map generator 65 for generating a map of the environment in which the moving body 1 moves; a path generator 66 for planning the path of the moving body 1; a track generator 67 for generating a target track according to the path; a motion controller 68 for moving the moving body 1 according to the target track; a corrector 610 for correcting the object sensor; and a corrector 611 for correcting the coordinates of each point in the point group data (hereinafter referred to as "sensing data") detected by the object sensor. Processor 61 can also function as an operational quantity arithmetic unit 69 for the operational quantity of the operational motor 13a.

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

[0050] In this example, the state estimator 64 uses the three-dimensional point group data of the first sensor 3A to perform its own position estimation. The state estimator 64 compares the environmental information surrounding the moving body 1 obtained from the three-dimensional point group data of the first sensor 3A with the three-dimensional map to estimate the position of the moving body 1 in the environment represented by the three-dimensional map, that is, its own position.

[0051] Map generator 65 generates a map based on the detection results of sensor 3. Specifically, map generator 65 generates or corrects a 3D map based on the detection results of sensor 3. In this example, before autonomous movement, simultaneous localization and mapping (SLAM) technology is used to generate a 3D map. Specifically, while the mobile body 1 moves within the environment, state estimator 64 and map generator 65 obtain the detection results from sensor 3 and perform self-position estimation and map generation in parallel. The generated map information, i.e., the 3D map, is stored in memory 62. During map generation before autonomous movement, the movement of the mobile body 1 is manually controlled by the user.

[0052] Furthermore, the map generator 65 updates the two-dimensional map. The updating of the two-dimensional map can also occur during autonomous movement. The map generator 65 detects obstacles in the environment based on the detection results of the sensors 3 obtained during the movement of the moving body 1, and updates the two-dimensional map accordingly.

[0053] The path generator 66 reads the destination and map information from the memory 62. The destination is preset in the memory 62. The map information at this time is, for example, a two-dimensional map. At this time, the path generator 66 can also read the route in addition to the destination. The state quantity (including the estimated position) of the moving body 1 is input from the state estimator 64 to the path generator 66.

[0054] Path generator 66 generates a path from the current position of the moving body 1 to its destination based on map information. Path generator 66 refers to map information to generate paths that avoid interference with obstacles. If a path exists in the environment, path generator 66 generates a path along that path. For example, path generator 66 uses A-star exploration algorithm, RRT algorithm, Dijkstra's algorithm, or geometric methods to generate the path. Path generator 66 outputs the arrangement of positions traversed by the moving body 1 as a path to track generator 67. Each position contains not only location information but also the pose of the moving body 1.

[0055] The trajectory generator 67 generates a target trajectory following the generated path and starting from the current position of the mobile body 1. The trajectory generator 67 generates the target trajectory of the mobile body 1 using a predetermined method (e.g., line-of-sight guidance law). The state variables of the mobile body 1 are input from the state estimator 64 to the trajectory generator 67. The trajectory generator 67 calculates the commanded speed of the mobile body 1.

[0056] Alternatively, the trajectory generator 67 can also calculate the command speed using Model Predictive Control (MPC). MPC obtains the control input, i.e., the speed command, based on a model of the moving body 1 and by successively solving an optimization problem. The trajectory generator 67 predicts future state variables from the current state variables of the moving body 1 and the obstacles, calculates the optimal path for the moving body 1, and calculates the movement speed required to follow this path from the current position to the target position as the command speed.

[0057] The speed command calculated by the track generator 67 is input to the motion controller 68. The motion controller 68 outputs the command value corresponding to the commanded speed to the operation quantity arithmetic unit 69.

[0058] The motion controller 68 performs controls to avoid interference between the moving body 1 and obstacles. The motion controller 68 monitors the approach of the moving body 1 to the obstacle based on the detection results of the sensors 3. In this example, the motion controller 68 uses all the detection results from the first sensor 3A, the second sensor 3B, and the third sensor 3C to monitor the approach of the moving body 1 to the obstacle. For example, the motion controller 68 slows down or stops the moving body 1 based on the monitored distance between the moving body 1 and the obstacle.

[0059] The operand operator 69 computes the command operands of each of the complex number of motors 13a by assigning an instruction value to a complex number of motors 13a . For example, the operating quantity is the rotational speed or torque of the motor.

[0060] The respective motor 13a operates according to the commanded operating amount. In motor 13a there will be a situation where a characteristic controller for actuating the motor 13a will be provided. For example, if the motor 13a is a servo motor, the motor 13a has more servo amplifiers. In this case, the servo amplifier operates the motor 13a depending on the commanded operating amount. As a result, the moving body ontology 1 will move.

[0061] The calibrator 610 calibrates the sensor coordinate system of the object sensor based on: a map of point groups generated by the map generator 65 , and the sensor data detected by the object sensor. In detail, the calibrator 610 obtains the coordinate transformation matrix used to calibrate the sensor coordinate system. The sensor coordinate system is defined using the object sensor as a benchmark. Basically, the position of each point of the sensing data is represented using the sensor coordinate system as a benchmark. The position of each point of the sensing data is coordinately transformed into various coordinate systems according to the purpose of utilization of the sensing data. For example, the position of each point represented by the sensor coordinate system is to perform a coordinate transformation for the moving body coordinate system. The object sensor is mounted on the moving body body 1, so the relationship of the sensor coordinate system with respect to the moving body coordinate system is determined. That is, the position of each point represented by the sensor coordinate system is performed using the relationship of the sensor coordinate system with respect to the moving body coordinate system to perform the coordinate transformation for the moving body coordinate system. Thereby, each point of the sensing data will be represented by the coordinate system benchmarked by the moving body ontology 1 . Moreover, there is a situation where the position of each point represented by the moving body coordinate system is to be performed for the coordinate transformation of the map coordinate system. For example, the relationship of a specific mobile body coordinate system with respect to the map coordinate system is obtained by resorting to the own position estimation of the mobile body ontology 1 . The position of each point represented by the mobile body coordinate system is performed using the relationship of the mobile body coordinate system with respect to the map coordinate system to perform the coordinate transformation for the map coordinate system. Thereby, each point of the sensing data will be represented by a coordinate system benchmarked by the map.

[0062] Here, when the position or orientation of the object sensor relative to the moving body 1 shifts, or when the optical axis of the object sensor shifts, the positions of the points detected by the object sensor will not be properly converted into the moving body coordinate system. That is, the positions of each point will be converted to positions deviating from the actual positions relative to the moving body 1. The corrector 610 obtains a coordinate transformation matrix to convert the positions of each point into appropriate positions relative to the moving body 1. In other words, the coordinate transformation matrix is ​​a coordinate transformation matrix used to correct the sensor coordinate system. When the coordinate transformation matrix is ​​used to transform the positions of each point in the sensed data, the positions of each point are converted into coordinates with the offset corrected.

[0063] The corrector 610 compares the points in the sensed data with the points in the corresponding point group map within a shared coordinate system, i.e., a reference coordinate system, and obtains a coordinate transformation matrix that reduces the positional offset between the two points. At this time, the corrector 610 extracts points representing a specific object from the sensed data and the point group map, and compares the extracted points from the sensed data with the extracted points from the point group map to obtain the coordinate transformation matrix. Specifically, the corrector 610 extracts points constituting a plane from a plurality of sensed points and extracts points constituting a plane from a plurality of map points, and compares the extracted points with each other. That is, the specific object is a plane. The reference coordinate system is a coordinate system used as a reference for determining the positional offset between corresponding points. The reference coordinate system can be, for example, a moving body coordinate system or a map coordinate system.

[0064] In detail, the calibrator 610 acquires a point group map and sensing data. In this example, the calibrator 610 acquires the point group map by reading it from the memory 62. The calibrator 610 acquires sensing data by detecting objects around the moving body 1 using an object sensor. That is, the calibrator 610 acquires sensing data by having the object sensor detect objects around the moving body 1.

[0065] The corrector 610 extracts points that constitute the plane of an object from both the point map and the sensing data. That is, the corrector 610 removes points other than those constituting the plane from both the point map and the sensing data. Points constituting the plane are, for example, points representing the plane of an object, such as a wall or door. Points other than those constituting the plane represent non-planar parts such as curved surfaces or edges.

[0066] The corrector 610 obtains the position of each point in the sensing data of the reference coordinate system by converting the position coordinates of each point in the sensing data of the sensor coordinate system into the reference coordinate system. In this example, the corrector 610 obtains the position of each point in the point group map of the map coordinate system by converting the position coordinates of each point in the point group map of the map coordinate system into the reference coordinate system. Moreover, the corrector 610 obtains a coordinate transformation matrix for transforming the position of each point in the sensing data of the reference coordinate system in a way that reduces the positional offset between corresponding points in the point group map of the reference coordinate system and the sensing data. In this example, the corrector 610 obtains a coordinate transformation matrix for transforming the position of each point in the sensing data of the reference coordinate system in a way that reduces not only the positional offset between corresponding points in the point group map of the reference coordinate system and the sensing data, but also the positional offset between corresponding points in the sensing data of the first object sensor 3B and the sensing data of the second object sensor 3C in the reference coordinate system. The corrector 610 stores the obtained coordinate transformation matrix in memory 62.

[0067] More specifically, the corrector 610 calculates the cost associated with the positional offset between corresponding points in the point group map of the reference coordinate system and the sensing data for each group of complex array sensing data. In this example, the corrector 610 further calculates the cost associated with the positional offset between corresponding points in the sensing data of the first object sensor 3B and the second object sensor 3C in the reference coordinate system. The corrector 610 sums the multiple costs associated with the complex array of sensing data to calculate a cost function. The corrector 610 obtains a coordinate transformation matrix for transforming the positions of each point in the sensing data of the reference coordinate system to minimize the cost function.

[0068] Corrector 611 uses the coordinate transformation matrix obtained by corrector 610 to correct the coordinates of each point in the sensed data. In this example, corrector 611 corrects the coordinates of each point in the sensed data when the sensed data is input from the object sensor during the autonomous movement of the moving body 100. As described above, when the sensed data from the second sensor 3B and the third sensor 3C are used by the movement controller 68, the sensed data is corrected using the coordinate transformation matrix stored in memory 62. In this way, the sensed data is corrected to an appropriate position based on the moving body 1.

[0069] Next, the basic operation of the moving body 100 will be explained. Figure 5 is a flowchart of the autonomous movement of the moving body 100. In this example, the control device 6 repeatedly executes the process of the flowchart in Figure 5 at a predetermined cycle, thereby performing autonomous movement.

[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 sensor 3 and the detection signal from encoder 13b. If sensor 3 is an object sensor, the state estimator 64 acquires the detection signal from sensor 3 after correction by corrector 611.

[0071] In step S2, the state estimator 64 performs its own position estimation.

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

[0073] In step S4, the track generator 67 calculates the command speed based on the estimated position of the moving body 1 according to the generated path.

[0074] In step S5, the motion controller 68 causes the mobile body 1 to perform an action according to the commanded speed. At this time, the motion controller 68 uses all the detection results of the first sensor 3A, the second sensor 3B, and the third sensor 3C to monitor the approach of the mobile body 1 to the obstacle. When the mobile body 1 approaches the obstacle, the motion controller 68 controls the mobile body 1 to avoid the obstacle. At this time, as described above, since the sensing data belonging to the detection results of the second sensor 3B and the third sensor 3C is corrected to the appropriate position based on the mobile body 1, the position of the obstacle is correctly detected, and interference between the mobile body 1 and the obstacle is suppressed.

[0075] The mobile body 100 repeatedly performs the processes up to steps S1 to S5, thereby autonomously moving to its destination while estimating the position of the mobile body 1 itself.

[0076] Next, the calibration method for the object sensor will be explained. Calibration can be performed at any time. For example, calibration can be performed before the start of use of the mobile body 100. Alternatively, calibration can be performed during use of the mobile body 100 when predetermined calibration conditions are met. For example, calibration conditions may include: the elapsed period since the start of use of the mobile body 100 has elapsed for a predetermined period, or a shift in the position or orientation of the object sensor is detected. Here, the calibration method will be explained using the case where calibration is performed when a point group map is generated before autonomous movement begins as an example.

[0077] Figure 6 is a flowchart showing the calibration method of the object sensor.

[0078] First, in step S10, the map generator 65 generates a point group map. As described above, the map generator 65 uses, for example, SLAM technology to generate the point group map. During the movement of the moving body 1, the map generator 65 detects objects around the moving body 1 using the first sensor 3A and collects point group data of these objects. The map generator 65 generates the point group map based on the collected point group data from the first sensor 3A. The map generator 65 stores the generated point group map in memory 62.

[0079] In step S102, the corrector 610 detects objects around the moving body 1 using an object sensor. The corrector 610 acquires sensing data by having the object sensor detect objects around the moving body 1 within an area corresponding to the point group map. In this example, the corrector 610 acquires multiple sets of sensing data using the object sensor under multiple conditions where at least one of the moving body 1's position and orientation differs. The state estimator 64 estimates the moving body 1's own position at the time the sensing data from the object sensor is acquired. The corrector 610 and the state estimator 64 store the sensing data in memory 62 by associating it with the corresponding estimated position. In this way, multiple sets of sensing data under multiple conditions of the moving body 1 can be acquired. The state of the moving body 1 is defined by its position and orientation.

[0080] In this example, the detection of sensing data is performed in parallel with the generation of the point group map. Specifically, while the moving body 1 is moving within the environment, the point group data of surrounding objects is acquired by the first sensor 3A in parallel with the generation of the point group map. The point group data, i.e., the sensing data, is acquired by the object sensor.

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

[0082] In step S104, the corrector 610 converts the positions of each point in the sensing data of the object sensor's sensor coordinate system into a reference coordinate system, thereby obtaining the positions of each point in the sensing data within the reference coordinate system. In this example, the reference coordinate system is a moving body coordinate system. The corrector 610 converts the positions of each point in the sensing data of the sensor coordinate system into the moving body coordinate system based on the relationship between the sensor coordinate system and the moving body coordinate system. The relationship between the sensor coordinate system and the moving body coordinate system is stored in memory 62. In this way, the positions of each point in the sensing data are converted from the sensor coordinate system to the moving body coordinate system. Furthermore, the corrector 610 converts the positions of each point in the point group map of the map coordinate system to the moving body coordinate system in each of the multiple states of the moving body 1. In detail, the corrector 610 converts the positions of each point in the point group map of the map coordinate system to the mobile coordinate system based on the estimated positions of the mobile body 1 associated with the multiple arrays of sensing data. In this way, multiple point group maps corresponding to multiple states of the mobile body 1 are obtained in the mobile coordinate system.

[0083] In step S105, the corrector 610 extracts the points constituting the plane from both the point group map and the sensing data. In other words, the corrector 610 removes points other than those constituting the plane from both the point group map and the sensing data. The details of the point extraction method will be described later.

[0084] Next, in step S106, the corrector 610 corrects the sensor coordinate system by reducing the positional offset between corresponding points in the point group map of the reference coordinate system and the sensing data. Specifically, the corrector 610 identifies points from the extracted points of the point group map that correspond to the extracted points of the sensing data, and obtains the coordinate transformation matrix for correction by reducing the positional offset between the corresponding two extracted points. In this example, the corrector 610 corrects the sensor coordinate system by reducing not only the positional offset between corresponding points in the point group map of the reference coordinate system and the sensing data, but also the positional offset between corresponding points in the sensing data of the first object sensor 3B and the sensing data of the second object sensor 3C in the reference coordinate system. In detail, the corrector 610 obtains the coordinate transformation matrix for correction by not only reducing the positional offset of the two corresponding extraction points related to the point group map and sensing data as described above, but also by specifically identifying points from the extraction points of the sensing data of the second object sensor 3C that correspond to the extraction points of the sensing data of the first object sensor 3B, and reducing the positional offset of these two corresponding extraction points. The method for obtaining the coordinate transformation matrix for correction will be described in detail later. The corrector 610 obtains the coordinate transformation matrix for correction, thereby completing the correction.

[0085] After calibration, the positions of each point in the point group detected by the object sensor will be corrected by the coordinate transformation matrix used for calibration.

[0086] Next, the point extraction method from the point group map and sensing data is explained in detail. Figure 7 is a flowchart of the sub-routine of the point extraction method. Figure 8 is an explanatory diagram illustrating the point extraction method. Point extraction is performed on each of the plurality of point group maps and the plurality of sensing data. The plurality of sensing data includes the sensing data of the second sensor 3B and the sensing data of the third sensor 3C. The sensing data of the second sensor 3B and the third sensor 3C respectively include multiple sensing data under different positions and orientations of the moving body 1. Point extraction from the sensing data is performed according to the type of object sensor and the state of the moving body 1.

[0087] The following describes the extraction of points using sensing data as an example. First, in step S201, the corrector 610 estimates the normals of each point contained in the data. Specifically, the corrector 610 obtains a small plane defined by at least two points within a predetermined range r1 surrounding the point (hereinafter referred to as the "object point") which serves as the object of the normal estimation, and estimates the normal n of the obtained small plane as the normal of the object point. The corrector 610 performs this estimation of normal n for all points contained in the data. In Figure 8, the normals of each point are represented by a single-point chain line.

[0088] Next, in step S202, the corrector 610 determines whether each point contained in the data constitutes a plane. Specifically, the corrector 610 expands the small plane obtained regarding the object point in step S201 to a predetermined exploration range r2. The corrector 610 explores other points located on the plane expanded to the exploration range r2, whose normals are within a predetermined angle threshold of the normal to the object point. This exploration condition is called the "exploration condition." The exploration range r2 is a larger range than range r1. The corrector 610 considers points whose distance from the expanded plane is within a predetermined distance threshold as points located on the expanded plane. When a point meeting the exploration condition is found, it is estimated that the object point and other points are on the same plane. That is, finding a point meeting the exploration condition means that the object point is estimated to be on a plane with a predetermined width, i.e., estimated to be a point constituting the plane.

[0089] When a point that meets the exploration criteria is found, the corrector 610 classifies the object point as a point that constitutes a plane. Conversely, when no point that meets the exploration criteria is found, the corrector 610 classifies the object point as a point that does not constitute a plane.

[0090] In the example shown in Figure 8, point p2 lies on the plane formed by the enlargement of the minute plane of object point p1. Furthermore, the angle between the normals n1 and n2 of object point p1 is within an angle threshold. In this case, object point p1 is determined to be a point constituting the plane.

[0091] As mentioned above, when the object point is one of the points constituting a plane, points near the object point are highly likely to form the same plane as the object point. Therefore, the normal of the micro-plane is highly likely to be a normal formed by the object point. Furthermore, when the plane formed by the object point is a plane of a certain width, points with normals at approximately the same angle as the object point are highly likely to exist on the plane after expanding the micro-plane to the exploration range r2. On the other hand, when the object point does not constitute a plane, the micro-plane formed by the object point and its nearby points is a plane independent of the shape of the area where the object point is located, and the normal of the micro-plane will differ significantly from the actual normal of the object point. Even if the micro-plane is expanded to the exploration range r2 as described above, the probability that other points exist on the expanded plane is low. Even if other points exist on the expanded plane, the object point and other points will not actually be located on the same plane, therefore their normal angles will differ significantly. Therefore, when the aforementioned exploration conditions are met, the object point is highly likely to be one of the points constituting a plane. In contrast, when the exploration conditions are not met, the object point is more likely not to be one of the points constituting the plane.

[0092] When the object point is determined to be a point that constitutes a plane, in step S203, the corrector 610 extracts the object point. The corrector 610 temporarily stores the extracted object point in memory 63.

[0093] If the object point is determined to be a point that does not belong to the plane, in step S204, the corrector 610 does not extract the object point.

[0094] Subsequently, in step S205, the corrector 610 determines whether the determination of whether the points constitute a plane has been performed for all points contained in the data. If the determination is not completed, the corrector 610 returns to step S202, selects other points contained in the data as target points, and repeats the processing from step S202 onwards.

[0095] When the determination of all points contained in the data is completed, the corrector 610 ends the extraction of points.

[0096] The corrector 610 also performs point extraction on all point group maps in the manner described above. In this example, when multiple sensing data are obtained, the corrector 610 performs the same extraction on all sensing data.

[0097] Next, the calibration method for the object sensor will be explained in detail. Figure 9 is a flowchart of the routine of the calibration method for the object sensor.

[0098] First, in step S301, the corrector 610 calculates a cost function related to the positional offset between corresponding points in the point group map of the reference coordinate system (in this example, the moving body coordinate system) and the sensing data. The method for calculating the cost function will be described in detail later.

[0099] In step S302, the corrector 610 derives the coordinate transformation matrix. That is, the corrector 610 obtains the coordinate transformation matrix used to transform the positions of each point in the sensing data of the reference coordinate system to minimize the cost function. In summary, the corrector 610 obtains the coordinate transformation matrix that minimizes the cost function.

[0100] In step S303, the corrector 610 determines whether the optimization of the cost function has converged. Specifically, the corrector 610 determines whether the convergence condition is met. For example, the convergence condition may be that the ratio of the total cost using the coordinate transformation matrix to the total cost before correction (hereinafter referred to as the "cost reduction rate") is below a predetermined reduction 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 change, is below a predetermined difference threshold. The convergence condition may also be that the number of iterations of the coordinate transformation matrix is ​​reached a predetermined number threshold. The convergence condition may also be a combination of at least two of the aforementioned three conditions and satisfy at least one of the plurality of conditions.

[0101] The total cost refers to the value obtained by substituting the derived coordinate transformation matrix into the cost function. In other words, the total cost comprehensively shows the positional offset between corresponding points when the coordinates of the sensed data points are actually transformed using the coordinate transformation matrix. The total cost before correction is not obtained by transforming the sensed data using the coordinate transformation matrix, but rather by using the cost function. That is, the total cost before correction comprehensively shows the positional offset between corresponding points when the coordinates of the sensed data points are not transformed. In summary, the cost reduction rate shows how much the total positional offset between corresponding points has been reduced compared to the case without correction and after correction using the coordinate transformation matrix. A convergence condition related to the cost reduction rate is met when the total positional offset between corresponding points is reduced by a certain amount or more.

[0102] The difference between the previous cost reduction rate and the current cost reduction rate shows how much the total positional offset between corresponding points has decreased after re-deriving the coordinate transformation matrix. If, even after re-deriving the coordinate transformation matrix, the overall positional offset between corresponding points does not decrease significantly, then the convergence condition related to the difference in cost reduction rates is met.

[0103] The convergence condition related to the number of iterations of the coordinate transformation matrix derivation is independent of the cost reduction rate. When the coordinate transformation matrix derivation is performed a certain number of times, the convergence condition related to the number of iterations of the coordinate transformation matrix derivation will be met.

[0104] If the convergence condition is not met, in step S304, the corrector 610 performs coordinate transformation on each point of the complex array of sensed data using the derived coordinate transformation matrix. Then, the corrector 610 returns to step S301 and repeats the process from the derivation of the cost function. That is, using the sensed data transformed by the derived coordinate transformation matrix, the cost function and coordinate transformation matrix are derived again, so the next coordinate transformation matrix is ​​obtained from the previous one. Furthermore, since the next coordinate transformation matrix is ​​derived using sensed data whose positional offset between corresponding points has been reduced through coordinate transformation, the next coordinate transformation matrix has the potential to further reduce the positional offset between corresponding points. The derivation of the coordinate transformation matrix is ​​repeated until the convergence condition is met, thereby improving the accuracy of the coordinate transformation matrix.

[0105] When the convergence condition is met, the corrector 610 stores the coordinate transformation matrix for correction in memory 62 in step S305 and ends the process. For example, the corrector 610 multiplies all the coordinate transformation matrices derived through repeated steps S302 to derive the coordinate transformation matrix for correction. Each point of the sensed data is transformed using the coordinate transformation matrix for correction.

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

[0107] First, in step S401, the corrector 610 generates, based on the acquisition status of each moving body 1, a data set consisting of a point group map, sensing data from the first object sensor 3B (second sensor 3B), and sensing data from the second object sensor 3C (third sensor 3C). The point group map and sensing data are generated by extracting points that constitute the plane.

[0108] Next, in step S402, the corrector 610 extracts one dataset from the plurality of datasets and combines two datasets from the plurality of data contained in the extracted dataset. Specifically, the dataset contains three types of data: a point group map, sensing data from the first object sensor 3B, and sensing data from the second object sensor 3C. Therefore, the corrector 610 creates the following three combinations: a combination of the point group map and the sensing data from the first object sensor 3B, a combination of the point group map and the sensing data from the second object sensor 3C, and a combination of the sensing data from the first object sensor 3B and the sensing data from the second object sensor 3C.

[0109] In step S403, the corrector 610 selects one of the three combinations. In step S404, the corrector 610 explores corresponding points between the two data contained in a combination. For example, when a combination of a point group map and sensing data from the first object sensor 3B is provided, the corrector 610 explores corresponding points between the point group map and the sensing data from the first object sensor 3B. As an example of exploring corresponding points, the corrector 610 explores points in the other data that are within a distance of a point in the closest data. The corrector 610 sets the two closest points as corresponding points. That is, the corrector 610 explores from the other data: a point in one data and a point that detects the same part of the same object. Where a point within a predetermined distance from a point in one data does not exist in the other data, it can also be set as a non-existent corresponding point. The corrector 610 explores the corresponding points of all points contained in the data of the data with fewer points. In addition, the corrector 610 can also explore the corresponding points of data with a smaller total number of points than the data with a smaller number of points.

[0110] In step S405, the corrector 610 calculates the cost related to the positional offset between corresponding points in the reference coordinate system. The cost is set based on the distance between the corresponding point of one data (i.e., the corresponding point available for alignment, hereinafter referred to as the "reference corresponding point") and the point after the coordinates of the other data are transformed by the coordinate transformation matrix (i.e., the corresponding point to be aligned, hereinafter referred to as the "object corresponding point"). In detail, the cost is set based on the distance d between the plane formed by the reference corresponding points and the object corresponding point. The cost is expressed, for example, by the following formula (1).

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

[0112] The coordinate transformation matrix is ​​used to make corresponding points of one set of data closer to corresponding points of another set of data. When the coordinate transformation matrix is ​​appropriate, the distance between two corresponding points will decrease, resulting in a lower cost. The coordinate transformation matrix is ​​a variable at the time point in which the cost is calculated. The constant of the coordinate transformation matrix is ​​obtained in the aforementioned step S302. The corrector 610 calculates the cost for all corresponding points explored in step S403.

[0113] In step S406, the corrector 610 weights the cost of each corresponding point. Weighting refers to the importance of cost. For example, the corrector 610 can also weight the cost based on the incident angle of the measurement light from the object sensor at the corresponding point. A larger incident angle of the measurement light results in greater distortion of the light spot diameter on the object's surface. Greater distortion of the light spot diameter reduces position detection accuracy; therefore, the corrector 610 can also reduce the cost weight of the corresponding point by increasing the incident angle of the measurement light at that point. For example, the corrector 610 can also weight the cost based on the reflection intensity of the measurement light at the corresponding point. Weaker reflection intensity reduces position detection accuracy at the corresponding point. Therefore, the corrector 610 can also reduce the cost weight by decreasing the reflection intensity of the corresponding point.

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

[0115] Cost function = Σ(cost × weighting coefficient)···(2) In step S408, the corrector 610 determines whether the cost addition to the cost function has been completed for all combinations of data contained in the dataset. If the cost addition for all combinations has not been completed, the corrector 610 returns to step S403 and retrieves the other combinations whose cost addition has not been completed. Then, the corrector 610 executes the processing from step S404 onwards again.

[0116] The corrector 610 repeatedly performs the processing from steps S403 to S408 to complete the addition of costs to the cost function for all combinations of data contained in a dataset. In other words, for a dataset: the costs of corresponding points between the point group map and the sensing data of the first object sensor 3B, the costs of corresponding points between the point group map and the sensing data of the second object sensor 3C, and the costs of corresponding points between the sensing data of the first object sensor 3B and the sensing data of the second object sensor 3C are added to the cost function.

[0117] When the cost calculation for all combinations is completed, in step S409, the corrector 610 determines whether the cost addition to the cost function for all data sets has been completed. If the cost addition for all data sets is not yet completed, the corrector 610 returns to step S402 and retrieves other data sets for which the cost addition is not yet completed. The corrector 610 performs the processing from steps S403 to S408 on the retrieved data sets. The corrector 610 repeatedly performs the processing from steps S402 to S408 to complete the cost addition to the cost function for all data sets. In other words, the cost addition to the cost function for all acquisition states of the moving body 1 is completed. Thus, the cost function is calculated.

[0118] As described above, the moving body 100 corrects the sensor coordinate system of the object sensor by reducing the positional offset between corresponding points in the point group map of the reference coordinate system and the sensing data. In this example, a point group map is generated based on the detection results of the first sensor 3A, and its own position estimation is performed. Therefore, by correcting the sensor coordinate system of the object sensor by reducing the positional offset between corresponding points in the point group map of the reference coordinate system and the point group data, the relative positional relationship between the first sensor 3A and the object sensor is corrected to a visually appropriate positional relationship. As a result, when the installation precision of the first sensor 3A on the trolley 10 is high (i.e., the first sensor 3A is installed in the appropriate position on the trolley 10), the position of each point in the sensing data detected by the object sensor will also accurately reflect the actual position of the obstacle. Furthermore, in this example, the sensor coordinate system is corrected not only by reducing the positional offset between corresponding points in the point group map and point group data of the reference coordinate system, but also by reducing the positional offset between corresponding points in the sensing data of the first object sensor 3B and the sensing data of the second object sensor 3C of the reference coordinate system. This ensures accurate determination of the obstacle's location and suppresses interference between the moving body 1 and the obstacle.

[0119] Next, the moving body 100 uses the point group data used as a reference for evaluating the positional offset of each point in the sensing data as a point group map, thus improving the accuracy of the correction. Suppose that the point group data obtained from a single measurement of a single sensor 3 is used as a reference, the number of points contained in the reference point group data is relatively small, and therefore the number of corresponding points in the sensing data is also relatively small. Therefore, the accuracy of the correction decreases. In contrast, the moving body 100 uses a point group map as a reference, thus relatively increasing the number of corresponding points in the sensing data. This improves the accuracy of the correction. In particular, in this example, multiple sets of sensing data are used to determine the positional offset between points in the point group map, thus increasing the number of corresponding points compared to using a single set of sensing data. As a result, the accuracy of the correction is further improved.

[0120] Moreover, in this example, the mobile body 100 computes a cost function by summing the complex number of costs associated with the complex set of sensing data, and obtains the coordinate transformation matrix used to coordinate the position of each point of the sensing data in the datum coordinate system in such a way that the cost function is minimized. That is, the mobile body 100 computes the cost function using all points contained in the complex set of sensing data, and that cost function is optimized in one go. Suppose, in the case where the cost function is calculated for each set of sensing data and this cost function is optimized, although the cost function has been optimized for each set of sensing data, there will be the possibility that the cost function is not optimized in the complex number of sensing data as a whole. That is, even after calibration, there will still be a possibility that the position offset of each point is relatively large in the complex number group of sensing data as a whole. The mobile body 100 optimizes the cost function at one time and thus optimizes the cost function with respect to the complex number of sensing data as a whole. As a result, the precision of calibration can be further improved.

[0121] In addition to the foregoing, the mobile body 100 extracts points constituting the plane with respect to the point group map and each of the sensing data. Also, the moving body 100 removes points other than those constituting the plane from each of the point group map and sensing data. Points other than those constituting the plane are measured, for example, by detecting moving objects such as human bodies and the like. The mobile body 100 calibrates the sensor coordinate system for the points of the extracted constituent plane in such a way that the corresponding points between the point group map of the datum coordinate system and the point group data reduce their positional offsets from each other. In this way, the points constituting the plane are utilized for calibration, thereby inhibiting the utilization of the measurement points of moving objects such as the human body that would reduce the accuracy of the calibration. Moreover, the points that make up the plane are arranged in a straight line, and it will be relatively easy to explore the corresponding points of the points that make up the plane with each other. Such results can further improve the precision of calibration.

[0122] In this example, the object sensor is detected in a manner parallel to the object detection by the first sensor 3A used as a point group map. This enables the calibration of the object sensor to be completed before the execution of autonomous movement.

[0123] Moreover, in this example, the datum coordinate system is the moving body coordinate system, and each point of the sensing data is corrected for the position offset on the moving body coordinate system. Mobility coordinate systems are commonly used in operation, so the calibrated sensing data will be easier to process.

[0124] The control device 6 can also control the robotic arm 12 when performing autonomous movement. Figure 11 is a functional block diagram showing the configuration of the control system of the processor 61 in the modified example. The processor 61 can also function as an arm controller 612 for controlling the robotic arm 12.

[0125] The arm controller 612 actuates the robotic arm 12. For example, the arm controller 612 causes the robotic arm 12 to deform into the shape of a target. The arm controller 612 can also maintain the robotic arm 12 in the shape of the target. The arm controller 612 can also continuously change the shape of the robotic arm 12, thereby actuating the robotic arm 12.

[0126] The arm controller 612 generates command values ​​corresponding to the shape of the target of the robotic arm 12. The arm controller 612 calculates the individual command operations of the plurality of motors 12a based on the command values. For example, the operation operation is the rotational speed or torque of the motor.

[0127] The arm controller 612 maintains the robotic arm 12 in a fixed shape when the moving body 100 is moving, and can also make the robotic arm 12 move when performing operations.

[0128] For example, when the moving body 100 is moving, the arm controller 612 maintains the robotic arm 12 in the moving shape. In other words, the arm controller 612 fixes the shape of the robotic arm 12 to prevent the robotic arm 12 from moving when the moving body 100 is moving.

[0129] Figure 12 is a side view of the moving body 1 when the robotic arm 12 is in a traveling shape. Figure 13 is a top view of the moving body 1 when the robotic arm 12 is in a traveling shape.

[0130] For example, the traveling-shaped robotic arm 12 is positioned at a relatively high position. For example, the traveling-shaped robotic arm 12 bends the intermediate joint between the shoulder and wrist joints, for example, the fourth joint J4, and the portion between the base 11 and the intermediate joint extends obliquely downwards and backwards from the base 11, while the portion between the intermediate joint and the wrist joint extends forwards from the intermediate joint. That is, the traveling-shaped robotic arm 12 pulls the intermediate joint backwards and bends it. In this way, the distal end portion of the robotic arm 12 is positioned at a relatively higher position than the intermediate joint. Moreover, the distal end of the robotic arm 12 is positioned relatively rearwards.

[0131] The robotic arm 12 in a traveling shape can also be positioned higher than the first sensor 3A of the moving body 1. The detection range of the first sensor 3A extends three-dimensionally beyond the first sensor 3A. The space above the first sensor 3A is included in the detection range of the first sensor 3A. Since the robotic arm 12 is positioned above the first sensor 3A, it can obscure a portion of the detection range of the first sensor 3A. The detection results of the first sensor 3A corresponding to the robotic arm 12 are treated as invalid results. The higher the position of the robotic arm 12, the further away the robotic arm 12 is from the first sensor 3A. There is a tendency for the area obscured by the robotic arm 12 within the detection range of the first sensor 3A to become smaller as the robotic arm 12 moves further away from the first sensor 3A. Therefore, in a traveling shape, the detection range of the first sensor 3A will be larger.

[0132] Furthermore, the forward protrusion of the robotic arm 12 from the base 11 is relatively small. The reduced forward protrusion of the robotic arm 12 expands the detection range of the first sensor 3A, extending from the first sensor 3A to the obliquely upward frontal detection range.

[0133] When viewed from above, the width of the overall shape of the robotic arm 12 in its traveling form becomes relatively smaller. For example, in its traveling form, the second link L2 is located on the outermost side in the width direction. Among the plurality of links L, the links other than the second link L2 are positioned further inside in the width direction than the second link L2. This reduces the width of the overall shape of the robotic arm 12 in its traveling form when viewed from above, thereby reducing the possibility of interference between the robotic arm 12 and other objects in the width direction during its travel. Furthermore, the robotic arm 12 in its traveling form can also rotate the first link L1 forward about the rotation axis of the first joint J1, thereby positioning the first link L1 and the second link L2 further forward along the rotation axis of the first joint J1 in the front-back direction. This further reduces the width of the second links L2 of both robotic arms, thus further reducing the width of the overall shape of the robotic arm 12 when viewed from above.

[0134] When viewed from above, the overall shape of the robotic arm 12 as it moves also tends to align with the inward side of the trolley 10 along the front-back direction. This reduces the likelihood of the robotic arm 12 interfering with other objects located in the front-back direction during its movement.

[0135] In the traveling pattern, the shapes of the two robotic arms 12 may not be exactly the same. That is, the shapes of the two robotic arms 12 may also be slightly different. For example, the height of the end of one robotic arm 12 may be different from that of the other robotic arm 12. The rotation angle of the seventh joint J7 of one robotic arm 12 may also be different from that of the other robotic arm 12.

[0136] For example, when the robotic arm 12 is performing a task, the arm controller 612 actuates the robotic arm 12. In other words, the arm controller 612 allows the robotic arm 12 to move freely while performing a task. For example, the arm controller 612 actuates the robotic arm 12 to perform the task after the moving body 1 reaches its destination.

[0137] Figure 14 is a side view of a modified movable body 100. The movable body 100 has a plurality of sensors 3, including not only a first sensor 3A but also a fourth sensor 3D. The movable body 100 may also omit at least one of the second sensor 3B and the third sensor 3C. The fourth sensor 3D is an example of an object sensor. That is, the fourth sensor 3D is a sensor for calibrating the object.

[0138] The object sensor can also be positioned closer to the end of the robotic arm than a sensor different from the target sensor. For example, the fourth sensor 3D is positioned closer to the end of the robotic arm 12 than the first sensor 3A. That is, the distance between the fourth sensor 3D and the end of the robotic arm 12 is shorter than the distance between the first sensor 3A and the end of the robotic arm 12. The end of the robotic arm 12 is, for example, the seventh link L7.

[0139] The fourth sensor 3D can also be configured on the robotic arm 12. For example, the fourth sensor 3D is configured on one of the robotic arms 12. The fourth sensor 3D is arbitrarily configured on one of the plurality of links L. For example, the fourth sensor 3D is configured on the first link L1, the second link L2, the sixth link L6, or the seventh link L7.

[0140] Here, the distance to the end of the robotic arm 12 refers to the distance traversed through the structure. For example, the distance between the first sensor 3A and the end of the robotic arm 12 is the distance from the first sensor 3A through the joint portion of the base 11 in the trolley 10, the joint portion of the robotic arm 12 connected to the base 11, and each joint J of the robotic arm 12 to the end of the robotic arm 12. The distance between the fourth sensor 3D and the end of the robotic arm 12 is the distance from the fourth sensor 3D through the joint J of the robotic arm 12 located closer to the end of the robotic arm 12 than the fourth sensor 3D to the end of the robotic arm 12.

[0141] The fourth sensor 3D can also detect objects in three-dimensional space surrounding the moving body 1. For example, the fourth sensor 3D is a 3D LiDAR. The fourth sensor 3D can also scan the measurement light in both horizontal and vertical directions.

[0142] The control device 6 can also correct the sensor coordinate system of the fourth sensor 3D based on the sensing data and point map detected by the fourth sensor 3D. Furthermore, the control device 6 can switch the sensor 3 used for its own position estimation based on the distance to the destination. For example, the control device 6 can perform its own position estimation based on the detection results of the first sensor 3A in a first interval where the distance to the destination is outside a predetermined switching range; the control device 6 can also perform its own position estimation based on the detection results of the fourth sensor 3D in a second interval where the distance to the destination is within the switching range.

[0143] The fourth 3D sensor is located relatively close to the end effector of the robotic arm 12. Therefore, using the fourth 3D sensor for self-position estimation improves the accuracy of the end effector position estimation. Furthermore, the fourth 3D sensor is calibrated with high precision through a point group map-based calibration, further enhancing the accuracy of the end effector position estimation. The second region used for self-position estimation using the fourth 3D sensor includes the destination. Therefore, when the robotic arm 12 performs operations at the destination, the end effector position is estimated with higher accuracy. As a result, the operational accuracy of the robotic arm 12 is improved.

[0144] The first sensor 3A is relatively far from the end of the robotic arm 12. The accuracy of the end position of the robotic arm 12 estimated using the position of the first sensor 3A may be worse than that estimated using the position of 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 configured in a way that minimizes the obstruction of the three-dimensional detection range by the base 11 and the robotic arm 12.

[0146] Furthermore, the detection range of the fourth sensor 3D may be narrower than that of the first sensor 3A. When the fourth sensor 3D is disposed on the robotic arm 12, its detection range depends on the travel shape of the robotic arm 12. A portion of the detection range of the fourth sensor 3D may be obscured by the base 11 or the like. The area of ​​the fourth sensor 3D's detection range obscured by the base 11 or the like may be larger than the area of ​​the first sensor 3A's detection range obscured by the base 11 or the like.

[0147] The first interval is relatively far from the destination. Therefore, within the first interval, the self-position estimation using the first sensor 3A can be performed in a way that focuses on detecting a wider range of objects, even more so than the positional accuracy of the end effector of the robotic arm 12.

[0148] Other Implementation Modes As described above, the aforementioned embodiments have been illustrated as examples of the technology disclosed in this application. However, the technology of this invention is not limited to these contents and can be applied to embodiments that are appropriately modified, replaced, added, or omitted. Furthermore, the constituent elements described in the aforementioned embodiments can be combined to form new embodiments. Moreover, the constituent elements described in the accompanying drawings and detailed description not only include the constituent elements necessary to solve the problem, but also, for the purpose of illustrating the aforementioned technology, non-essential constituent elements for solving the problem may be included. Therefore, one should not directly assume that non-essential constituent elements are essential elements simply because they are described in the accompanying drawings and detailed description.

[0149] The mobile body 1 may also be a robot without the robotic arm 12. The mobile body 1 is not limited to a robot, but may also be a mobile device such as a ship or a vehicle. The movement path of the mobile body 1 is not limited to a passageway, but may also be a road or air route.

[0150] If the first sensor 3A is capable of acquiring point group data, it is not limited to 3D LiDAR. If the object sensors (in this example, the second sensor 3B and the third sensor 3C) are capable of acquiring point group data, it is not limited to 2D LiDAR. The number of object sensors is also not limited; there may be one or more object sensors.

[0151] The calibrator 610 can also obtain a point group map from outside the moving body 100. For example, the calibrator 610 can also obtain a point group map generated by a sensor installed on a moving body different from the moving body 100.

[0152] The reference coordinate system can be a map coordinate system. In this case, the corrector 610 can also convert the positions of each point of the object sensor's sensing data in the sensor coordinate system into the moving body coordinate system, and then further convert it into the map coordinate system based on the relationship between the moving body coordinate system and the map coordinate system. The relationship between the moving body coordinate system and the map coordinate system is determined by estimating the position of the moving body 1 itself. In this case, the step of converting each point of the point group map into the reference coordinate system is omitted. When the cost function shown in Figure 10 is calculated, the point group map can also be used in all acquisition states of the moving body 1.

[0153] The calibrator 610 may also eliminate the need to extract the points constituting the plane from the point group map and the sensing data. For example, the calibrator 610 may directly use the acquired point group map and sensing data for calibration.

[0154] The calibrator 610 can also calibrate the sensor coordinate system without reducing the positional offset between corresponding points in the reference coordinate system, which is the sensing data of the first object sensor 3B and the sensing data of the second object sensor 3C. That is, the calibrator 610 can also calibrate the sensor coordinate system by reducing the positional offset between corresponding points in the point group map of the reference coordinate system and the sensing data.

[0155] The multiple sets of sensing data are not limited to the configuration of the aforementioned embodiments. Multiple sets of sensing data can be acquired by an object sensor when only the position of the moving body 1 differs, or when only the posture of the moving body 1 differs, or when both the position and posture of the moving body 1 differ. The number of sets of sensing data can be two or more. Sensing data can also be a single set. That is, sensing data can be acquired by an object sensor when the moving body 1 has only a single position and posture.

[0156] The corrector 610 may also calculate the cost function without summing multiple costs associated with multiple sets of sensed data. For example, the corrector 610 may calculate the cost function for each set of sensed data and optimize the cost function for each set of sensed data.

[0157] The timing at which the calibrator 610 acquires sensing data is not limited to the collection of point group data by the first sensor 3A for the purpose of creating a point group map, but can also be during the autonomous movement of the moving body 1 under normal conditions.

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

[0159] The flowchart is just one example. The steps in the flowchart can be changed, replaced, added, or omitted as appropriate, depending on the situation. Furthermore, the order of the steps in the flowchart can be changed, or sequential processes can be processed in parallel. For example, in the flowchart shown in Figure 6, steps S101 and S102 can be processed sequentially. For example, in the flowchart shown in Figure 10, step S406 can be omitted.

[0160] The functions of the elements disclosed in this specification can be implemented using one or more circuits or processing circuits, including general-purpose processors, special-purpose processors, integrated circuits, application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), and / or conventional circuits. The functions of the elements disclosed in this specification can 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 can be programmed using one or more programs stored together or separately in one or more memories, or otherwise configured to perform the disclosed functions. Because a processor contains transistors and other circuitry, it is considered a processing circuit or circuit. A processor can also be a processor that executes programs stored in memory, or a programmed processor. In this disclosure, the terms "circuit," "unit," or "means" refer to hardware that performs the listed functions individually or in combination, or hardware programmed to perform the listed functions individually or in combination. Any hardware that is programmed or configured to perform the listed functions as disclosed in this specification, regardless of its form, is applicable.

[0161] Computer programs containing computer instructions are stored in memory. The computer instructions provide logic and routines that enable hardware to perform the methods disclosed in this specification. The hardware may include, for example, processing circuitry or circuitry. The computer program can be installed in known formats on computer-readable recording media, computer program products, memory devices such as CD-ROMs or DVDs, and / or the memory of FPGAs or ASICs.

[0162] [State / Appearance] The above-mentioned implementation mode is a specific example of the following mode.

[0163] (State 1) The calibration method for sensor 3 is to calibrate the object sensor (in this example, the second sensor 3B and the third sensor 3C). The object sensor is installed on the moving body 1 of the autonomously moving moving body 100 and detects surrounding objects in the form of point group data. The calibration method for sensor 3 includes: obtaining a point group map of the surrounding area of ​​the moving body 1; detecting objects around the moving body 1 by means of the object sensor; converting the position of each point of the point group data (sensing data) detected by the object sensor in the sensor coordinate system of the object sensor into a reference coordinate system, thereby obtaining the position of each point of the point group data in the reference coordinate system; and calibrating the sensor coordinate system by reducing the positional offset between corresponding points in the point group map and the point group data in the reference coordinate system.

[0164] Based on this configuration, the point group data used as a reference for evaluating the positional offset of each point in the sensed data is used as a point group map, thus improving the calibration accuracy. If the point group data obtained from a single measurement of a sensor is used as a reference, the number of points contained in that reference point group data is relatively small, and therefore the number of corresponding points corresponding to the sensed data is also relatively small. Therefore, the calibration accuracy decreases. In contrast, according to the configuration of this sample 1, using a point group map as a reference, the number of corresponding points corresponding to the sensed data can be relatively increased. This improves the calibration accuracy of the object sensor.

[0165] (State 2) The sensor calibration method described in Sample 1 further includes the step of creating the aforementioned point group map. The aforementioned moving body 1 is equipped with another sensor (first sensor 3A) that is different from the aforementioned object sensor. This other sensor detects surrounding objects in the form of point group data. When creating the aforementioned point group map, the objects around the aforementioned moving body 1 are detected simultaneously with the aforementioned moving body 1 by the aforementioned other sensor. The aforementioned point group map is created based on the point group data detected by the aforementioned other sensor. When detecting objects around the aforementioned moving body 1, the objects are detected by the aforementioned object sensor in parallel with the object detection performed by the aforementioned other sensor for creating the aforementioned point group map.

[0166] Based on this configuration, the calibration of the object sensor can be completed before the autonomous movement of the moving body 100 is executed.

[0167] (State 3) The sensor calibration method described in Sample 1 or Sample 2 further includes: a step of estimating the position and posture of the aforementioned moving body in the map coordinate system of the aforementioned point group map; and a step of converting the position of each point in the aforementioned point group map of the aforementioned map coordinate system into a reference coordinate system based on the position and posture of the aforementioned moving body estimated by the aforementioned self-position estimation, thereby obtaining the position of each point in the aforementioned point group map of the reference coordinate system; the aforementioned reference coordinate system is a moving body coordinate system defined with the aforementioned moving body 1 as a reference.

[0168] Based on this configuration, the reference coordinate system is a moving body coordinate system, and the positional offset of each point in the sensed data is corrected using the moving body coordinate system. Moving body coordinate systems are typically used in operations, thus the corrected sensed data is easier to process.

[0169] (State 4) The sensor calibration method described in any of the states 1 to 3 further includes: the step of extracting points constituting a plane from each of the aforementioned point group map and the aforementioned point group data; during the aforementioned calibration, the aforementioned sensor coordinate system is calibrated by reducing the positional offset between corresponding points in the aforementioned point group map and the aforementioned point group data in the aforementioned reference coordinate system for the extracted points constituting the plane of the object.

[0170] In accordance with this configuration, points other than those constituting the plane were removed from each of the point group maps and sensing data. Points other than those constituting the plane are measured, for example, by detecting moving objects such as human bodies and the like. For the points of the extracted constituent plane, the sensor coordinate system is calibrated in such a way that the position offset of the corresponding points between the point group map and the point group data of the datum coordinate system is reduced from each other. In this way, the points constituting the plane are utilized for calibration, thereby inhibiting the utilization of the measurement points of moving objects such as the human body that would reduce the accuracy of the calibration. Moreover, the points that make up the plane are arranged in a straight line, and it will be relatively easy to explore the corresponding points of the points that make up the plane with each other. Such results can further improve the precision of calibration.

[0171] (Aspect 5) In the calibration method of the sensors described in any of the states from swatches 1 to 4, the foregoing object sense is resorted to in detecting objects surrounding the preceding mobile body ontology in a condition where at least one of the positions and poses of the preceding mobile body ontology is different meter to obtain complex group point group data, in the preceding calibration, in such a way that the position offset of corresponding points between the aforementioned point group map and the aforementioned complex group point group data is reduced from each other in the aforementioned datum coordinate system.

[0172] According to this configuration, the number of corresponding points can be made larger than the case where a single sensing data is used. As a result, the precision of the calibration of the object sensor can be further improved.

[0173] (Aspect 6) In the calibration method for the sensors recorded in any of the states from swatches 1 to 5, the preceding calibration is comprised of: the cost associated with obtaining the point group data for each group of the preceding complex set of point group data and the location offset of the corresponding points between the point group map and the point group data in the datum coordinate system, and the steps of calculating the cost function by summing the complex number of the foregoing costs associated with the foregoing complex set of point group data; and for obtaining the coordinate transformation matrix used to coordinate the positions of each point of the foregoing point group data in the foregoing datum coordinate system by minimizing the foregoing cost function.

[0174] Based on this configuration, the cost function is calculated using all points contained in the complex array of sensed data, and this cost function is optimized all at once. It is assumed that even if the cost function is calculated and optimized for each group of sensed data, there is still a possibility that the cost function in the entire complex array of sensed data may not be optimized. That is, even after correction, there is still a possibility that the positional offset of each point in the entire complex array of sensed data may be relatively large. By optimizing the cost function all at once according to this configuration, the cost function is optimized for the entire complex array of sensed data. As a result, the accuracy of the correction can be further improved.

[0175] (Style 7) In the sensor calibration method described in any of the states 1 to 6, the object sensor has a first object sensor 3B and a second object sensor 3C, the second object sensor 3C having a detection range that at least partially overlaps with the detection range of the first object sensor 3B; when calibrating the aforementioned sensor coordinate system, the aforementioned sensor coordinate system is calibrated in a manner that not only reduces the positional offset between corresponding points in the aforementioned point group map and the aforementioned point group data in the aforementioned reference coordinate system, but also reduces the positional offset between corresponding points in the aforementioned point group data of the aforementioned first object sensor 3B and the aforementioned point group data of the aforementioned second object sensor 3C in the aforementioned reference coordinate system.

[0176] Based on this configuration, the location of the obstacle can be accurately determined, and interference between the moving body 1 and the obstacle can be suppressed.

[0177] (Style 8) The sensor calibration method described in any of the states 1 to 7 further includes: the step of creating the aforementioned point group map; the aforementioned mobile body 1 is a robot that includes a robotic arm 12 and is movable, and the aforementioned mobile body 1 is equipped with other sensors (first sensor 3A) that are different from the aforementioned object sensor (fourth sensor 3D), which detect surrounding objects in the form of point group data; the aforementioned object sensor is positioned closer to the end of the aforementioned robotic arm 12 than the aforementioned other sensors, and when creating the aforementioned point group map, the aforementioned mobile body 1 is moved while the aforementioned other sensors detect surrounding objects of the aforementioned mobile body 1, and the aforementioned point group map is created based on the point group data detected by the aforementioned other sensors.

[0178] Based on this configuration, the object sensor is positioned relatively close to the end effector of the robotic arm 12. Therefore, by using the object sensor to estimate its own position, the accuracy of the position estimation of the end effector of the robotic arm 12 is improved. Furthermore, the object sensor is calibrated with high precision through a calibration process based on a point group map, which further enhances the accuracy of the position estimation of the end effector of the robotic arm 12.

[0179] (Style 9) The mobile body 100 is an autonomous mobile entity, comprising: a mobile body 1; an object sensor installed on the mobile body 1, which detects surrounding objects in the form of point group data; and a control device 6, which calibrates the sensor coordinate system of the object sensor. The control device 6 performs the following actions: acquiring a point group map around the mobile body 1; detecting objects around the mobile body 1 using the object sensor; converting the positions of each point in the point group data detected by the object sensor in the sensor coordinate system of the object sensor into a reference coordinate system, thereby obtaining the positions of each point in the point group data in the reference coordinate system; and calibrating the sensor coordinate system by reducing the positional offset between corresponding points in the point group map and the point group data in the reference coordinate system.

[0180] Based on this configuration, the accuracy of object sensor calibration can be improved.

[0181] (Style 10) The control device 6 is a calibration object sensor, which is installed on the moving body 1 of the autonomously moving moving body 100 and detects surrounding objects in the form of point group data. The control device performs the following actions: obtaining a point group map around the moving body 1; detecting objects around the moving body 1 by means of the object sensor; converting the position of each point of the point group data detected by the object sensor in the sensor coordinate system of the object sensor into a reference coordinate system, thereby obtaining the position of each point of the point group data in the reference coordinate system; and calibrating the sensor coordinate system by reducing the positional offset between corresponding points in the point group map and the point group data in the reference coordinate system.

[0182] Based on this configuration, the accuracy of object sensor calibration can be improved.

[0183] (Style 11) A control program is used to calibrate an object sensor installed on the body 1 of an autonomously moving body 100, which detects surrounding objects in the form of point group data. The control program enables a computer to: obtain a point group map of the area surrounding the body 1; detect objects around the body 1 using the object sensor; convert the positions of each point in the point group data detected by the object sensor into a reference coordinate system, thereby obtaining the positions of each point in the point group data in the reference coordinate system; and calibrate the sensor coordinate system by reducing the positional offset between corresponding points in the point group map and the point group data in the reference coordinate system.

[0184] Based on this configuration, the accuracy of object sensor calibration can be improved.

[0185] 1: The moving body itself 3: Sensor 3A: First sensor (other sensors) 3B: Second sensor (first object sensor) 3C: Third sensor (second object sensor) 3D: The Fourth Sensor (Object Sensor) 6: Control device 10: trolley 11: Base 12: Robotic Arm 12a: Motor 12b: Encoder 13: Wheels 13a: Motor 13b: Encoder 14: Robotic Arm 61: Processor 62: Memory 63: Memory 64: State Estimator 65: Map Generator 66: Path Generator 67: Track Generator 68: Motion Controller 69: Operation Calculator 100: Moving body 610: Calibrator 611: Corrector 612: Arm Controller d: distance J: Joint J1: First joint J2: Second joint J3: Third joint J4: Fourth joint J5: Fifth Joint J6: Sixth Joint J7: Seventh Joint L: Linkage L1: First Link L2: Second Link L3: Third Link L4: Fourth Link L5: Fifth Link L6: Sixth Link L7: Seventh Link n1, n2: Normal lines p1: Coordinates of the reference point p2: Coordinates of the point corresponding to the object r1: Predefined range r2: Exploration range S1, S2, S3, S4, S5: Steps S101, S102, S103, S104, S105, S106, S201, S202, S203, S204, S205, S301, S302, S303, S304, S401, S402, S403, S404, S405, S406, S407, S408, S409: Steps

Claims

1. A method for calibrating a sensor, comprising calibrating an object sensor, wherein the object sensor is mounted on the body of an autonomously moving body and detects surrounding objects in the form of point group data, the method comprising: obtaining a point group map of the surroundings of the moving body; performing a self-position estimation of the moving body's position and orientation based on point group data detected by another sensor mounted on the moving body and detecting surrounding objects in the form of point group data and the aforementioned point group map; and detecting the moving body by the object sensor. The steps include: determining the positions of points in the aforementioned point group data detected by the aforementioned object sensor within the sensor coordinate system of the aforementioned object sensor; converting the positions of each point in the aforementioned point group data within the aforementioned reference coordinate system to obtain the positions of each point in the aforementioned point group data within the aforementioned reference coordinate system; determining the positions of corresponding points in the aforementioned point group map corresponding to the aforementioned point group data within the aforementioned reference coordinate system; and correcting the aforementioned sensor coordinate system by reducing the positional offset between corresponding points in the aforementioned point group map and the aforementioned point group data within the aforementioned reference coordinate system; wherein... In at least one of determining the position of each point in the aforementioned point group data in the aforementioned reference coordinate system and determining the position of the corresponding point in the aforementioned point group map in the aforementioned reference coordinate system, the position and orientation of the aforementioned moving body are estimated by inferring its own position.

2. The sensor calibration method as described in claim 1 further includes the step of creating the aforementioned dot group map. Other sensors, different from the aforementioned object sensor, are installed in the aforementioned moving body system. These other sensors detect surrounding objects in the form of dot group data. When creating the aforementioned dot group map, the objects around the aforementioned moving body are detected by the aforementioned other sensors while the aforementioned moving body is moving. The aforementioned dot group map is created based on the dot group data detected by the aforementioned other sensors. When detecting objects around the aforementioned moving body, the objects are detected by the aforementioned object sensor in parallel with the object detection performed by the aforementioned other sensors for creating the aforementioned dot group map.

3. The sensor calibration method as described in claim 1 further includes the step of extracting points constituting the plane of the object from each of the aforementioned point group map and the aforementioned point group data; during the aforementioned calibration, the aforementioned sensor coordinate system is calibrated for the extracted points constituting the plane of the object in a manner that reduces the positional offset between corresponding points in the aforementioned point group map and the aforementioned point group data in the aforementioned reference coordinate system.

4. The calibration method for the sensor as described in claim 1, wherein, When detecting objects around the aforementioned moving body, the object sensor acquires complex point group data under conditions where at least one of the position and orientation of the aforementioned moving body is different. During the aforementioned correction, the sensor coordinate system is corrected in a way that reduces the positional offset between corresponding points in the aforementioned point group map and the aforementioned complex point group data in the aforementioned reference coordinate system.

5. The calibration method for the sensor as described in claim 1, wherein, The aforementioned object sensor has a first object sensor and a second object sensor, the second object sensor having a detection range that at least partially overlaps with the detection range of the aforementioned first object sensor; when calibrating the aforementioned sensor coordinate system, the aforementioned sensor coordinate system is calibrated in a manner that not only reduces the positional offset between corresponding points in the aforementioned point group map and the aforementioned point group data in the aforementioned reference coordinate system, but also reduces the positional offset between corresponding points in the aforementioned point group data of the aforementioned first object sensor and the aforementioned point group data of the aforementioned second object sensor in the aforementioned reference coordinate system.

6. A method for calibrating a sensor, comprising calibrating an object sensor, the object sensor being mounted on the body of an autonomously moving body and detecting surrounding objects in the form of point group data, the calibration method comprising: obtaining a point group map of the surroundings of the moving body; detecting objects around the moving body using the object sensor; converting the positions of each point of the point group data detected by the object sensor in the sensor coordinate system of the object sensor to a reference coordinate system, thereby determining the positions of each point of the point group data in the reference coordinate system. The steps include: estimating the position and orientation of the moving body within the map coordinate system of the aforementioned point group map; converting the positions of each point on the aforementioned point group map of the aforementioned map coordinate system into coordinates of the aforementioned reference coordinate system based on the estimated position and orientation of the moving body; and correcting the sensor coordinate system by reducing the positional offset between corresponding points in the aforementioned point group map and the aforementioned point group data within the aforementioned reference coordinate system. The aforementioned reference coordinate system is a moving body coordinate system defined with the aforementioned moving body itself as the reference.

7. A method for calibrating a sensor, comprising calibrating an object sensor, the object sensor being mounted on the body of an autonomously moving body and detecting surrounding objects in the form of point group data, the method comprising: obtaining a point group map of the surroundings of the moving body; detecting objects around the moving body using the object sensor; converting the positions of each point of the point group data detected by the object sensor in the sensor coordinate system of the object sensor to a reference coordinate system, thereby determining the positions of each point of the point group data in the reference coordinate system; and calibrating the sensor coordinate system by reducing the positional offset between corresponding points in the point group map and the point group data in the reference coordinate system; wherein, In detecting objects around the aforementioned moving body, when at least one of the position and posture of the aforementioned moving body is different, a complex set of point group data is obtained by the aforementioned object sensor. The aforementioned correction is performed to reduce the positional offset between corresponding points in the aforementioned reference coordinate system and the aforementioned complex set of point group data. The aforementioned correction includes: calculating the cost related to the positional offset between corresponding points in the aforementioned reference coordinate system and the aforementioned point group data for each set of point group data of the aforementioned complex set of point group data, and summing the plurality of the aforementioned costs related to the aforementioned complex set of point group data to calculate a cost function; and obtaining a coordinate transformation matrix for coordinate transformation of the position of each point in the aforementioned reference coordinate system of the aforementioned point group data in order to minimize the aforementioned cost function.

8. A method for calibrating a sensor, comprising calibrating an object sensor, the object sensor being mounted on the body of a self-moving mobile body and detecting surrounding objects in the form of point group data, the method comprising: obtaining a point group map of the surroundings of the mobile body; detecting objects around the mobile body using the object sensor; converting the positions of each point of the point group data detected by the object sensor in the sensor coordinate system of the object sensor to a reference coordinate system, thereby determining the positions of each point of the point group data in the reference coordinate system; and using the correlation between the point group map and the point group data in the reference coordinate system. The step of correcting the aforementioned sensor coordinate system by reducing the positional offset between points; the aforementioned moving body is a robot that includes a robotic arm and is movable, and other sensors different from the aforementioned object sensor are installed in the aforementioned moving body system. These other sensors detect surrounding objects in the form of point group data; the aforementioned object sensor is positioned closer to the end of the aforementioned robotic arm than the aforementioned other sensors. When obtaining the aforementioned point group map, the aforementioned moving body is moved while the aforementioned other sensors detect objects around the aforementioned moving body, and the aforementioned point group map is created based on the point group data detected by the aforementioned other sensors, thereby obtaining the aforementioned point group map.

9. A mobile body capable of autonomous movement, the movement system comprising: a mobile body body; an object sensor mounted on the mobile body body and detecting surrounding objects in the form of point cluster data; other sensors mounted on the mobile body body and detecting surrounding objects in the form of point cluster data; and a control device comprising a sensor coordinate system for calibrating the object sensor; wherein, The aforementioned control device performs the following actions: acquiring a point group map surrounding the aforementioned moving body; estimating the position and posture of the aforementioned moving body by comparing the point group data detected by the aforementioned other sensors with the aforementioned point group map; detecting objects around the aforementioned moving body by the aforementioned object sensor; converting the positions of each point in the aforementioned point group data detected by the aforementioned object sensor into a reference coordinate system, thereby obtaining the positions of each point in the aforementioned point group data in the aforementioned reference coordinate system; obtaining the positions of corresponding points in the aforementioned point group map corresponding to the aforementioned point group data in the aforementioned reference coordinate system; correcting the aforementioned sensor coordinate system by reducing the positional offset between corresponding points in the aforementioned point group map and the aforementioned point group data in the aforementioned reference coordinate system; and using the estimated position and posture of the aforementioned moving body obtained by estimating its own position, based on at least one of obtaining the positions of each point in the aforementioned point group data in the aforementioned reference coordinate system and obtaining the positions of corresponding points in the aforementioned point group map in the aforementioned reference coordinate system.

10. A control device comprising calibrating an object sensor, the object sensor being mounted on the body of a moving body performing autonomous movement and detecting surrounding objects in the form of point group data, the control device performing the following actions: acquiring a point group map of the area surrounding the moving body; performing a self-position estimation of the moving body's position and posture based on point group data detected by another sensor mounted on the moving body and detecting surrounding objects in the form of point group data, and the aforementioned point group map; detecting objects around the moving body using the object sensor; converting the positions of each point in the point group data detected by the object sensor within the sensor coordinate system of the object sensor into a reference coordinate system, thereby obtaining the positions of each point in the point group data within the reference coordinate system; obtaining the positions of corresponding points in the point group map corresponding to the point group data within the reference coordinate system; and calibrating the sensor coordinate system by reducing the positional offset between corresponding points in the aforementioned point group map and the point group data within the reference coordinate system; wherein... In at least one of determining the position of each point in the aforementioned point group data in the aforementioned reference coordinate system and determining the position of the corresponding point in the aforementioned point group map in the aforementioned reference coordinate system, the position and orientation of the aforementioned moving body are estimated by inferring its own position.

11. A control program for calibrating an object sensor, the object sensor being installed on the body of a moving body performing autonomous movement and detecting surrounding objects in the form of point group data, the control program enabling a computer to: acquire a point group map of the area surrounding the moving body; perform self-position estimation by estimating the position and posture of the moving body based on point group data detected by other sensors installed on the moving body that detect surrounding objects in the form of point group data and the aforementioned point group map; and detect the moving body using the aforementioned object sensor. The function of the sensor system includes: converting the positions of each point in the aforementioned point group data detected by the aforementioned object sensor into a reference coordinate system, thereby obtaining the positions of each point in the aforementioned point group data in the aforementioned reference coordinate system; determining the positions of corresponding points in the aforementioned point group map corresponding to the aforementioned point group data in the aforementioned reference coordinate system; and correcting the aforementioned sensor coordinate system by reducing the positional offset between corresponding points in the aforementioned point group map and the aforementioned point group data in the aforementioned reference coordinate system; wherein... The position and orientation of the moving body are estimated by using the self-position to infer the position and orientation of the moving body itself, based on at least one of the following: determining the position of each point in the aforementioned point group data in the aforementioned reference coordinate system and determining the position of the corresponding point in the aforementioned point group map in the aforementioned reference coordinate system.

Images