Lead mobility and posture detection method

Abstract

We provide highly practical lead mobility solutions. [Solution] The present invention provides a surrounding monitoring device 9 equipped with a lidar 15 that acquires measured point cloud data including the following vehicle 20, and a controller 100 configured to periodically perform a position and attitude determination process that determines the relative position and attitude of the following vehicle 20 based on the matching result, by performing matching using reference point cloud data VP, which is the total three-dimensional point cloud data of the following vehicle 20, and the measured point cloud data. Based on the result of the previous position and attitude determination process, the controller 100 estimates the invisible portion of the following vehicle 20 that cannot be measured by the lidar 15 in the current position and attitude determination process, creates modified reference point cloud data by removing the portion corresponding to the invisible portion from the reference point cloud data VP, and performs matching using the modified reference point cloud data and the measured point cloud data in the current position and attitude determination process.

Description

TECHNICAL FIELD

[0001] The present invention relates to a lead mobility device that electronically tows a vehicle, and a posture detection method for the lead mobility device.

BACKGROUND ART

[0002] Conventionally, for example, a controller disclosed in Patent Document 1 is known. The conventional controller is mounted on a self-propelled lead mobility device, and is configured to guide a vehicle as a follower vehicle via remote control so that the vehicle travels along the travel path that the lead mobility device has traveled. In other words, the lead mobility device has a function of maneuvering the vehicle as a following vehicle to follow itself via wireless communication.

PRIOR ART DOCUMENTS

PATENT DOCUMENTS

[0003]

PATENT DOCUMENT 1

SUMMARY OF THE INVENTION

PROBLEMS TO BE SOLVED BY THE INVENTION

[0004] By the way, the controller mounted on the lead mobility device is provided with not only the function of causing the follower vehicle to travel by following the lead mobility device via remote control, that is, the function of electronic towing, but also some additional function, whereby the practicality of the lead mobility device can be improved. Based on the above, an object of the present invention is to provide a lead mobility device with high practicality and a posture detection method for the lead mobility device.

MEANS FOR SOLVING THE PROBLEMS

[0005] The lead mobility of the present invention comprises a mobility body, an ambient monitoring device mounted on the mobility body for detecting the surrounding conditions of the mobility body, and a controller mounted on the mobility body that, based on the detection results of the ambient monitoring device, autonomously moves the mobility body and controls a vehicle to follow the mobility body via wireless communication as a follow vehicle. The ambient monitoring device includes a lidar for acquiring measured point cloud data, including three-dimensional point cloud data of the follow vehicle. The controller is configured to periodically perform a position and attitude determination process that determines the relative position and attitude of the follow vehicle with respect to the mobility body based on the results of the matching, using reference point cloud data, which is the total three-dimensional point cloud data of the follow vehicle, and the measured point cloud data acquired by the lidar. Based on the results of the previous position and attitude determination process, the controller estimates the invisible portion of the follow vehicle that cannot be measured by the lidar in the current position and attitude determination process. The controller creates modified reference point cloud data by removing the portion corresponding to the invisible portion from the reference point cloud data. In the position and orientation determination process described above, the controller performs the matching using the modified reference point cloud data and the measured point cloud data. The orientation detection method of the present invention has the same features as described above.

[0006] According to the present invention, modified reference point cloud data, from which invisible portions have been removed, is used as the matching criterion. By eliminating extraneous comparison points and using a more situation-appropriate criterion, errors in posture determination become less likely. The present invention provides a highly practical lead mobility and posture detection method. [Brief explanation of the drawing]

[0007] [Figure 1] This is a schematic diagram illustrating the electronic towing of a follower vehicle by lead mobility according to this embodiment. [Figure 2] This is a side view illustrating lead mobility. [Figure 3]This is a planar conceptual diagram illustrating the upper part of the lead mobility. [Figure 4] This is a block diagram used to explain the functional configuration of controllers and other components. [Figure 5] This is a planar conceptual diagram illustrating an example of modified reference point cloud data. [Figure 6] This is a flowchart illustrating an example of the position and orientation determination process. [Modes for carrying out the invention]

[0008] Hereinafter, a lead mobility device 10, which is an embodiment of the present disclosure, will be described in detail with reference to the drawings. In addition to the embodiments described below, the present disclosure can be implemented in various forms with various modifications and improvements based on the knowledge of those skilled in the art.

[0009] [A] Lead Mobility Configuration The lead mobility 10 is configured to be autonomously drivable (autonomous), and as shown in Figures 1 and 2, it electronically tows a follower vehicle (corresponding to a "following vehicle") 20 to its destination, maintaining a specific positional relationship through control via wireless communication, i.e., remote control. Here, the lead mobility 10 can be a vehicle that travels on the road surface or an aerial vehicle such as a drone, and in this embodiment, the case where the lead mobility 10 is a vehicle will be described.

[0010] The Lead Mobility 10 of this embodiment is equipped with a pair of left and right drive wheels 11 and driven wheels 12 for autonomous driving. Each of the left and right drive wheels 11 is independently driven by a pair of left and right electric motors (not shown) powered by a battery (not shown) mounted on the body of the Lead Mobility 10. As a result, the Lead Mobility 10 can rotate around a rotation axis along the vertical direction by, for example, applying a difference in rotational speed (difference in driving force) between the left and right drive wheels 11. Thus, the Lead Mobility 10 can turn right or left, change direction, and make turns (including pivot turns in place) during autonomous driving, even without being equipped with a separate steering device to steer the left and right drive wheels 11 around the steering axis. In the following description, right and left turns, changes direction, and turns may be collectively referred to as "right and left turns, etc."

[0011] Each drive wheel 11 is fitted with a friction braking device (drum brake or disc brake) not shown. Therefore, in a stationary state, the friction braking device functions as a parking brake by generating braking force through friction.

[0012] The driven wheel 12 is positioned behind the drive wheel 11 in the longitudinal direction of the lead mobility 10. In this embodiment, the driven wheel 12 is in the form of a swivel caster and is provided so as to have a single axis of rotation extending vertically in the approximately central part of the lead mobility 10 in the width direction (lateral direction).

[0013] As shown in Figure 3, the Lead Mobility 10 is equipped with a self-propelled detector 13, a position detector 14, and a vehicle detector 15, all housed in the upper part 10U of the vehicle body. The self-propelled detector 13 detects the relative positional relationship, specifically the relative distance, between the Lead Mobility 10 and objects such as obstacles (hereinafter sometimes referred to as "detection targets") that are present in the direction of travel when the Lead Mobility 10 is autonomously driving.

[0014] Therefore, in this embodiment, the self-propelled detector 13 is configured to include a LiDAR (Light Detection and Ranging) 13A and a distance measuring device such as a camera 13B. The LiDAR 13A acquires three-dimensional point cloud data indicating the three-dimensional position of point clouds representing the object to be detected with high accuracy. The camera 13B can be, for example, a stereo camera, a monocular camera, or an RGB-D camera (depth camera), and acquires imaging data representing the direction and size of the object to be detected. In addition to or instead of using the LiDAR 13A and camera 13B, it is also possible to use, for example, a ToF (Time of Flight) sensor.

[0015] The self-propelled detector 13 outputs the acquired data, i.e., three-dimensional point cloud data and image data, to the controller 100, which will be described later. The controller 100 then uses the acquired three-dimensional point cloud data and image data in simultaneous localization and mapping (SLAM) for autonomous driving of the lead mobility 10, as will be described later.

[0016] The position detector 14 has, for example, a GNSS (Global Navigation Satellite System) receiver and detects the position of the lead mobility 10 based on the received signal. In this embodiment, the lead mobility 10 has two position detectors 14 positioned at each of the left and right positions in the vehicle width direction (lateral direction) of the upper part 10U of the vehicle body, that is, in a pair on the left and right.

[0017] The lead mobility device 10 is configured to include a vehicle detector 15 in order to detect the follower vehicle 20 that is following it. The vehicle detector 15 is a device for measuring various data used to estimate the relative position of the follower vehicle 20 with respect to the lead mobility device 10 (hereinafter sometimes referred to as "relative position").

[0018] The vehicle detector 15 is provided with a LIDAR as a main component, which measures three-dimensional point cloud data of a follower vehicle 20 that is electronically towed by a lead mobility 10. Here, in a controller 100 to be described later, for example, only the relative position of the follower vehicle 20 detected by the vehicle detector 15 (LIDAR) may be acquired, and the relative posture and traveling direction of the follower vehicle 20 may be estimated by grasping the temporal change and the like of the follower vehicle 20 based on the intermittently acquired relative positions.

[0019] The self-propulsion detector 13 and the vehicle detector 15 constitute a surrounding monitoring device 9 that detects the surrounding conditions of the lead mobility 10. That is, the surrounding monitoring device 9 is mounted on a mobility main body 7 which is the main body portion (housing, wheels, etc.) of the lead mobility 10, and is composed of a plurality of detectors 13, 15 that detect the surrounding conditions of the mobility main body 7. The vehicle detector 15 is a LIDAR, and is also referred to as "LIDAR 15" in the following description.

[0020] The controller 100 executes driving control by remote operation so that the follower vehicle 20 maintains a specific positional relationship with respect to the lead mobility 10. The controller 100 executes control related to autonomous movement of the lead mobility 10 and remote operation of the follower vehicle 20. The lead mobility 10 includes the controller 100, the surrounding monitoring device 9 including the LIDAR 15, and the mobility main body 7 that mounts these components and is configured to be movable. The mobility main body 7 can be said to be a vehicle body main portion including a vehicle body upper portion 10U, wheels 11, 12, and respective actuators.

[0021] [B]Configuration and Basic Functions of Controller As shown in FIG. 4, the controller 100 includes a CPU 110, a storage device 120, an interface circuit 130, and a communication device 140. The CPU 110, the storage device 120, and the interface circuit 130 are connected so as to be capable of bidirectional communication via an internal bus. The communication device 140 performs wireless communication with the follower vehicle 20 via a network or the like.

[0022] The CPU 110 executes a computer program stored in the storage device 120 to implement at least some of the functions provided in this embodiment. By executing this computer program, the CPU 110 functions as a remote control unit 111, a point cloud data acquisition unit 112, a position determination unit 113, a relative position estimation unit 114, a SLAM unit 115, an automatic driving control unit 116, and a special control unit 117. However, some or all of these functions can also be configured by hardware circuits.

[0023] The remote control unit 111 generates control commands for remote control and transmits them wirelessly to the follower vehicle 20, for example, as if the lead mobility 10 were towing the follower vehicle 20 with a rope, so that the follower vehicle 20 maintains a specific positional relationship with the lead mobility 10 and follows the lead mobility 10. Here, the state in which the follower vehicle 20 is towed by the control commands transmitted by the remote control unit 111 of the controller 100 mounted on the lead mobility 10 via wireless communication using the communication device 140 is called "electronic towing".

[0024] The remote control unit 111 can generate a control command as a command including, for example, a driving force or braking force and a steering angle. Alternatively, the remote control unit 111 can generate a control command as a command including at least one of the position and orientation of the follower vehicle 20 and the future travel route. As a result, the follower vehicle 20 can follow the lead mobility 10 by receiving a control command for remote control, as will be described later.

[0025] The point cloud data acquisition unit 112 acquires, for example, three-dimensional point cloud data (hereinafter sometimes referred to as "measured point cloud data") measured by the vehicle detector 15. The position determination unit 113 determines, for example, the starting position for starting the matching of the measured point cloud data with the three-dimensional point cloud data around the lead mobility 10 acquired by the lidar 13A of the self-propelled detector 13.

[0026] The storage device 120 stores three-dimensional point cloud data representing the entire follower vehicle 20 as reference point cloud data VP, which serves as the basis for matching. The three-dimensional point cloud data may be represented in software such as 3D CAD. Multiple types of reference point cloud data VP may be stored in preparation for cases where the controller 100 controls multiple types of follower vehicles 20. The reference point cloud data VP includes information for identifying the orientation (attitude) of the follower vehicle 20. As a result, the position determination unit 113 and the relative position estimation unit 114 can estimate the position and orientation (attitude) of the follower vehicle 20 in the surrounding three-dimensional point cloud data by matching the measured point cloud data and the reference point cloud data VP.

[0027] As an example, the position determination unit 113 may determine the starting position of the matching using information regarding the position of the follower vehicle 20 in the three-dimensional point cloud data (hereinafter sometimes referred to as "position-related information"). The position-related information is data used to estimate the position of the follower vehicle 20 in the three-dimensional point cloud data, and / or the position next to the follower vehicle 20. In order to speed up the matching process, the position-related information is preferably small-capacity data or data obtained by simple processing, such as a GNSS signal.

[0028] The relative position estimation unit 114 estimates the relative position of the follower vehicle 20 in the measured point cloud data, including its relative orientation (attitude) relative to the lead mobility 10. Examples of relative positions include the relative distance to the follower vehicle 20 in the direction of travel, the deviation of the follower vehicle 20 in the vehicle width direction (lateral direction) relative to the movement trajectory of the lead mobility 10, and the relative turning attitude (right or left turning attitude) of the follower vehicle 20 relative to the lead mobility 10, based on the position and attitude of the lead mobility 10.

[0029] The relative position estimation unit 114 estimates the relative position, including the relative attitude, of the follower vehicle 20 in the acquired three-dimensional point cloud data by performing matching with the measured point cloud data using the reference point cloud data VP. For the matching of the measured point cloud data with the reference point cloud data VP performed by the position determination unit 113 and the relative position estimation unit 114, for example, well-known algorithms such as the ICP (Interactive Closest Point) algorithm or the NDT (Normal Distribution Transform) algorithm can be used. Details of the process for determining (calculating) the position and attitude of the follower vehicle 20 will be described later.

[0030] The SLAM unit 115 performs SLAM using data (image data and three-dimensional point cloud data) detected by the self-propelled detector 13 to generate a map that the lead mobility 10 will use for autonomous driving. The automatic driving control unit 116 controls the operation of actuators 150, such as the electric motors that drive the drive wheels 11 mounted on the lead mobility 10 and the electric motors that constitute the friction braking system, thereby enabling the lead mobility 10 to drive autonomously. Specifically, by controlling the operation of the actuators 150, the automatic driving control unit 116 uses the map generated by the SLAM unit 115 to enable the lead mobility 10 to drive autonomously along the lead vehicle route GR to a set destination TP, for example. When the lead mobility 10 is driving autonomously, the automatic driving control unit 116 detects the position of the lead mobility 10 based on the GNSS signal received by the position detector 14.

[0031] The special control unit 117 is a functional unit that enables special functions in the lead mobility 10. The details of this special control unit 117 will be explained in detail later.

[0032] The storage device 120 can be exemplified by RAM, ROM, HDD, and SSD. The read / write area of ​​the storage device 120 stores, for example, reference point cloud data VP, lead vehicle route GR, destination TP, actuator drive history AC, and previous matching position BM.

[0033] Here, the lead vehicle route GR is a target route that can be set for the lead mobility 10 to travel. The destination TP is an arbitrarily set destination for the lead mobility 10. However, when the automatic driving control unit 116 autonomously drives the lead mobility 10 using a map generated by the SLAM unit 115 based on data output from the self-driving detector 13, the lead vehicle route GR can be omitted. However, in this case, the automatic driving control unit 116 generates a driving route to the set destination TP, for example, and drives the lead mobility 10 along the generated driving route.

[0034] The actuator drive history AC is the history of input and output values ​​for each actuator 220 of the follower vehicle 20, which will be described later. The actuator drive history AC can also be described as the history of control command values ​​transmitted from the controller 100 to the follower vehicle 20. The actuator drive history AC may also be measured values ​​detected by the detectors of the follower vehicle 20, such as the vehicle speed, steering angle, braking force, and rotation angle of the follower vehicle 20. The previous matching position BM is, for example, the coordinate value in the previous matching result.

[0035] [C] Follower Vehicle Configuration The follower vehicle 20 is a vehicle (e.g., a passenger car, truck, bus, construction vehicle, motorcycle, or tricycle) equipped with a driving control device 200 and a communication device 230. The driving control device 200 and the communication device 230 are basic devices currently installed in vehicles and can also be retrofitted. Each communication device 140 and 230 is in a state where it is authorized to communicate with each other. The lead mobility 10 electronically tows the authorized follower vehicle 20.

[0036] As shown in Figure 4, the driving control device 200 is equipped with an ECU (Electronic Control Unit) 210. The ECU 210 is a microcomputer whose main components are a CPU 211, a storage device 212, and an interface circuit 213. The CPU 211, storage device 212, and interface circuit 213 are connected via an internal bus to enable bidirectional communication. The interface circuit 213 is connected to an actuator 220 and a communication device 230. The communication device 230 communicates wirelessly with the communication device 140 of the controller 100 mounted on the lead mobility 10, either via a network or directly.

[0037] The CPU 211 implements the function of driving control of the follower vehicle 20 by executing a computer program stored in the read / write area of ​​the storage device 212. Here, driving control refers to various controls for driving the actuators 220 that perform the functions of "driving," "turning," and "stopping" of the follower vehicle 20, such as adjusting the acceleration, deceleration, speed, and steering angle of the follower vehicle 20. The actuators 220 are, for example, drive actuators, brake actuators, and steering actuators.

[0038] The CPU 211 controls the operation of the actuator 220 in response to control commands sent from the controller 100, regardless of whether the follower vehicle 20 has a driver, thereby enabling the follower vehicle 20 to follow the lead mobility 10 while maintaining a specific positional relationship with it.

[0039] [D] Remote control via lead mobility The controller 100 acquires information about the electronically towed follower vehicle 20 in advance, such as specifications, minimum turning radius, wheelbase length, acceleration performance, braking performance, and other information related to the follower vehicle 20's ability to "drive," "turn," and "stop." The controller 100 then electronically tows (follows) the follower vehicle 20, for example, while autonomously driving according to the lead vehicle route GR stored in the memory device 120, from which the specifications information has been acquired.

[0040] The controller 100 causes the follower vehicle 20 to follow the lead mobility 10's trajectory while maintaining a specific distance between the lead mobility 10 and the follower vehicle 20, specifically by keeping the distance between the two vehicles a specific distance.

[0041] [E] Position and orientation determination process As described above, the controller 100 comprises a mobility unit 7, a surrounding monitoring device 9 mounted on the mobility unit 7 for detecting the surrounding conditions, and the controller 100 mounted on the mobility unit 7. Based on the detection results of the surrounding monitoring device 9, the controller 100 is configured to autonomously move the mobility unit 7 and to control a vehicle as a follower vehicle 20 to follow the mobility unit 7 via wireless communication. The surrounding monitoring device 9 is equipped with a LiDAR 15 for acquiring measured point cloud data, including the three-dimensional point cloud data of the follower vehicle 20. The controller 100 is configured to periodically perform position and attitude determination processing. The position and attitude determination processing involves matching (point cloud matching) between the reference point cloud data VP, which is the overall three-dimensional point cloud data of the follower vehicle 20, and the measured point cloud data acquired by the LiDAR 15, and determining the relative position and attitude of the follower vehicle 20 with respect to the mobility body 7 based on the matching results. Note that the matching of point cloud data itself can be performed using well-known methods, such as setting the initial position for the current operation from the previous timestamp data (performing a rough alignment), so a detailed explanation is omitted.

[0042] The controller 100 (special control unit 117) estimates the invisible portion of the follower vehicle 20 that cannot be measured by the lidar 15 in the current position and attitude determination process, based on the previous position and attitude determination process. In this explanation, the lidar 15 positioned to measure the area behind the lead mobility 10 will also be referred to as the rear lidar 15. The position and attitude determination process will now be described assuming that the follower vehicle 20 is located behind the lead mobility 10.

[0043] The rear rider 15 measures the follower vehicle 20 behind it and acquires measurement point cloud data. In this case, the point cloud data of the follower vehicle 20 included in the measurement point cloud data is the point cloud data of the part of the follower vehicle 20 that is visible to the rear rider 15 (visible portion). The part of the follower vehicle 20 that is not visible to the rear rider 15 (invisible portion) cannot be measured due to its structure.

[0044] For example, when both mobility devices 10 and 20 are moving straight ahead, the rear portion of the follower vehicle 20 is not visible to the rear rider 15 and therefore cannot be measured. In other words, in this case, as shown in Figure 5, the front portion of the follower vehicle 20 becomes the visible portion (measurable portion), and the rear portion of the follower vehicle 20 becomes the invisible portion (unmeasurable portion). For example, when at least one of the mobility devices 10 and 20 is turning, the portion of the follower vehicle 20 visible from the left and right diagonally in front becomes the visible portion, and the opposite side becomes the invisible portion.

[0045] The controller 100 can recognize the relative position and orientation of the follower vehicle 20 at the time of the previous position and orientation determination process based on the results of the previous position and orientation determination process. The controller 100 can also acquire detection results from various sensors (such as acceleration sensors) mounted on the mobility body 7 and the follower vehicle 20, respectively, as state information for the mobility body 7 and the follower vehicle 20. The controller 100 can also grasp the control content for each mobility 10 and 20. Based on the results of the previous position and orientation determination process and the state information of both mobility 10 and 20, the controller 100 estimates the relative position and orientation of the follower vehicle 20 in the current position and orientation determination process (for example, the current position and orientation). Strict accuracy is not required for this estimation. Based on the estimated relative position and orientation of the follower vehicle 20, the controller 100 estimates the invisible portion of the follower vehicle 20.

[0046] When the controller 100 estimates an invisible portion, it creates modified reference point cloud data by removing the portion corresponding to that invisible portion from the reference point cloud data VP. The controller 100 deletes the portion corresponding to the invisible portion from the reference point cloud data VP and saves it separately. As a result, the portion of the reference point cloud data corresponding to that invisible portion is excluded from the matching target. As shown in Figure 5, if the rear portion of the follower vehicle 20 is an invisible portion, the controller 100 deletes the rear portion from the point cloud data of the reference point cloud data VP and stores it as modified reference point cloud data (VP1). The modified reference point cloud data is updated with each position and attitude determination process. Each time, the latest modified reference point cloud data is used as the matching criterion. For the sake of explanation, Figure 5 is shown as a top view, not a rear view (front view).

[0047] When the controller 100 creates (sets) corrected reference point cloud data, it performs matching using the corrected reference point cloud data and the measured point cloud data in the current position and attitude determination process. The corrected reference point cloud data corresponding to the current process (latest corrected reference point cloud data) is matched with the measured point cloud data corresponding to the current process (latest measured point cloud data). Since the corrected reference point cloud data is limited to the visible portion, it becomes a more suitable criterion for the measured point cloud data, which measures the visible portion of the follower vehicle 20. Because extraneous comparison targets are removed from the reference point cloud data, the likelihood of attitude determination errors is reduced.

[0048] In a conventional configuration where point cloud data representing the entire follower vehicle 20 is used as the reference, if an object exists near the follower vehicle 20, as shown in Figure 5, that object may be measured as part of the follower vehicle 20. In this case, the controller may determine that the measurement result matches the shape of the follower vehicle 20 viewed from the diagonal front in the reference point cloud data, and may mistakenly determine that the posture of the follower vehicle 20 is diagonal. Because the controller searches for a pattern (posture / appearance) that matches the measurement result from the reference point cloud data of the entire vehicle, misjudgments regarding posture are more likely to occur.

[0049] However, according to this embodiment, since the modified reference point cloud data, which is the point cloud data of the follower vehicle 20 with the invisible portion removed, is used as the basis for matching, the accuracy of estimating the attitude of the follower vehicle 20 is improved. Even in the case described above, according to this embodiment, since the point cloud data of the front portion of the follower vehicle 20 is used as the basis, it is easier to determine that the measurement result includes the front portion of the follower vehicle 20 and other objects. In other words, according to this embodiment, the attitude of the follower vehicle 20 can be detected with high accuracy. Even if there is a discrepancy between the estimated invisible portion and the actual invisible portion, misjudgment is less likely to occur because the comparison area is narrowed compared to when the entire point cloud data is used as the basis.

[0050] As shown in Figure 6, the controller 100 reads the reference point cloud data VP (S1). The controller 100 acquires the measured point cloud data M1, which is the measurement result of the LiDAR 15 (S2). The controller 100 removes ground information (e.g., point cloud data corresponding to the road surface) from the measured point cloud data M1 (S3). The controller 100 sets the current initial position for the measured point cloud data M1a, which has been processed for removal, based on the previous matching result R0 (matching result corresponding to the most recent timestamp data) (S4).

[0051] The controller 100 estimates the invisible portion of the follower vehicle 20 based on the previous matching result R0 (S5). The controller 100 removes the portion corresponding to the invisible portion from the reference point cloud data VP and stores it as modified reference point cloud data VP1 (S6). The controller 100 performs matching between the modified reference point cloud data VP1 and the measured point cloud data M1a (S7). Based on the matching result R1, the controller 100 estimates the relative position and attitude of the follower vehicle 20 and controls both mobilitys 10 and 20 (S8). Based on the matching result R1, the controller 100 updates the initial position for the next matching (S9). The initial position set in the next step S4 will be the initial position updated in step S9.

[0052] The attitude detection method of this embodiment is an attitude detection method for a follower vehicle 20 that is performed by a lead mobility 10 configured to periodically perform a position and attitude determination process that performs matching using reference point cloud data VP and measured point cloud data, and determines the relative position and attitude of the follower vehicle 20 relative to itself based on the matching results. This attitude detection method includes an estimation step, a creation step, and a matching step. The estimation step is a step of estimating the invisible portion of the follower vehicle 20 that cannot be measured by the LiDAR 15 in the nth position and attitude determination process, based on the result of the n-1th (n is an integer of 2 or more) position and attitude determination process. The creation step is a step of creating reference point cloud data for the nth time by modifying the reference point cloud data VP so that the portion of the reference point cloud data VP corresponding to the invisible portion is excluded from the matching target. The matching step is a step of performing matching using the reference point cloud data for the nth time and the measured point cloud data in the nth position and attitude determination process. According to this embodiment, the modified reference point cloud data, from which the invisible portion has been removed from the reference point cloud data, is used as the basis for matching. By eliminating unnecessary comparison points and using criteria more appropriate to the situation, errors in judging posture become less likely. According to this embodiment, a highly practical lead mobility system can be provided.

[0053] The present invention is not limited to the above embodiments. For example, in the calculation of the invisible portion, the controller 100 may estimate the invisible portion based on the relative position and attitude of the follower vehicle 20 corresponding to the result of the previous position and attitude determination process. In other words, the controller 100 may be configured to estimate the invisible portion relative to the previous relative position and attitude without estimating the current relative position and attitude through calculation. This configuration is advantageous in terms of calculation processing because it does not estimate the current positional relationship. Simplification of such calculation processing is also possible by adopting the premise that the previous positional relationship can be roughly applied to the current one, such as when the processing interval of the position and attitude determination process is short. This configuration is particularly effective in situations where there is little change in the positional relationship of both mobilitys 10 and 20 between processing of the position and attitude determination process. The controller 100 may determine a driving section with little change in positional relationship based on the lead vehicle route GR, and calculate the invisible portion in the previous position and attitude as the invisible portion in the current one in that driving section.

[0054] The controller 100 may determine, based on the state information (sensor detected values ​​and / or control content) of both mobilitys 10 and 20, whether or not there has been a change in the relative position and attitude between the previous and current position and attitude determination processes. If the controller 100 determines, based on the target route of the mobility body 7 and / or the state information of the mobility body 7 and the follower vehicle 20, that there has been no change in the relative position and attitude of the follower vehicle 20 between the position and attitude determination processes, it may estimate the invisible portion based on the relative position and attitude of the follower vehicle 20 corresponding to the result of the previous position and attitude determination process. Otherwise, as described above, the controller 100 may estimate the invisible portion based on the estimated position and attitude.

[0055] If there is no change in the relative position and attitude of both mobility devices 10 and 20, it can be estimated that the invisible portion remains unchanged, and the corrected reference point cloud data can also be estimated to remain unchanged. If the controller 100 determines that there is no change in relative position and attitude between position and attitude determination processes, it may set the corrected reference point cloud data used in the previous matching as the corrected reference point cloud data for the current matching. For example, if both mobility devices 10 and 20 are continuously moving in a straight line at the same speed, the controller 100 may determine that there is no change in relative position and attitude during that straight-line movement period.

[0056] 1...Electronic towing system, 10...Lead mobility, 20...Follower vehicle, 7...Mobility unit, 9...Surrounding monitoring device.

Claims

1. The mobility device itself, A surrounding monitoring device mounted on the mobility unit detects the surrounding conditions of the mobility unit, A controller mounted on the mobility unit, which autonomously moves the mobility unit based on the detection results of the surrounding monitoring device, and controls a vehicle to follow the mobility unit via wireless communication as a follow vehicle, It is a lead mobility device equipped with, The surrounding monitoring device includes a lidar for acquiring measured point cloud data, including three-dimensional point cloud data of the following vehicle. The controller is configured to periodically perform a position and attitude determination process that determines the relative position and attitude of the following vehicle to the mobility unit, using reference point cloud data which is the overall three-dimensional point cloud data of the following vehicle and the measured point cloud data acquired by the LiDAR, based on the results of the matching. The aforementioned controller, Based on the results of the previous position and attitude determination process, the invisible portion of the following vehicle that cannot be measured by the RID is estimated in the current position and attitude determination process. A modified reference point cloud data is created by removing the portion corresponding to the invisible portion from the aforementioned reference point cloud data. In the position and orientation determination process described above, the matching is performed using the modified reference point cloud data and the measured point cloud data. Lead mobility.

2. The aforementioned controller, Based on the results of the previous position and attitude determination process and the status information of the mobility unit and the following vehicle, the relative position and attitude of the following vehicle in the current position and attitude determination process are estimated. Based on the estimated relative position and attitude of the following vehicle, the invisible portion is estimated. The lead mobility according to claim 1.

3. The controller estimates the invisible portion based on the relative position and attitude of the following vehicle corresponding to the result of the previous position and attitude determination process. The lead mobility according to claim 1.

4. If the controller determines, based on the target route of the mobility unit and / or the status information of the mobility unit and the following vehicle, that there has been no change in the relative position and attitude of the following vehicle during the processing of the position and attitude determination process, it estimates the invisible portion based on the relative position and attitude of the following vehicle corresponding to the result of the previous position and attitude determination process. Lead mobility according to any one of claims 1 to 3.

5. A method for detecting the attitude of a following vehicle, performed by a lead mobility device that is autonomously moving and capable of controlling a vehicle to follow it via wireless communication, wherein the lead mobility device is configured to perform matching using reference point cloud data, which is the total three-dimensional point cloud data of the following vehicle, and measured point cloud data, which is three-dimensional point cloud data including the following vehicle measured by a LiDAR, and periodically performs a position and attitude determination process to determine the relative position and attitude of the following vehicle with respect to itself based on the result of the matching, Based on the result of the (n-1)th position and attitude determination process (where n is an integer of 2 or more), the step of estimating the invisible portion of the following vehicle that cannot be measured by the RID in the nth position and attitude determination process, The steps include creating nth-time reference point cloud data by removing the portion corresponding to the invisible portion from the aforementioned reference point cloud data, In the nth position and orientation determination process, the steps include performing the matching using the reference point cloud data for the nth time and the measured point cloud data, A posture detection method including the following.

Images