Course control device, automatic steering device, course control method, and program

Abstract

To provide a course control device which can quickly calculate an efficient route that allows forward and backward movement with a small calculation load, and can instruct a traveling direction and an attitude of a moving object.SOLUTION: A course control device comprises: a first measuring instrument that measures a current position and an attitude of a moving object; a waypoint setting device that sets a position of a waypoint of a planned route and an attitude of the moving object at the waypoint; and a course controller that calculates a traveling direction of the moving object indicating forward or backward movement, and a course azimuth direction indicating the attitude of the moving object when moving in the traveling direction, based on a first arc including a starting point, a second arc including an end point, and a straight line contacting to both the first arc and the second arc, so that the moving object travels along the shortest route from the starting point to the end point, while setting the current position and the attitude of the moving object as the starting point, and setting the position and the attitude of the waypoint as the end point.SELECTED DRAWING: Figure 2

Description

[Technical Field]

[0001] The present disclosure relates to a route control device, an automatic steering device, a route control method, and a program. [Background technology]

[0002] Patent Document 1 describes a mobile object control system including a state quantity detection device, an obstacle detection device, and a mobile object control device. The mobile object control device receives the current state quantity of the mobile object from the state quantity detection device and calculates an obstacle avoidance constraint function as a condition for preventing the mobile object from colliding with the obstacle based on the position of the obstacle detected by the obstacle detection device. The mobile object control device also calculates a margin function based on the uncertainty of the state quantity of the mobile object relative to the state transition equation of the mobile object and on disturbances acting on the mobile object. Next, the mobile object control device performs model predictive control using a function obtained by adding the calculated margin function to the obstacle avoidance constraint function as a constraint to determine an optimal control variable for the mobile object. The mobile object control device then provides the determined optimal control variable to the mobile object as a target value for the control variable of the mobile object.

[0003] Patent Document 2 describes a collision avoidance control device that predicts the future position of a target detected by a millimeter-wave radar or a camera, as well as the future position of the vehicle, and calculates the optimal operation amount to avoid a collision using model predictive control.

[0004] Patent Document 3 describes a path generation device that uses a Dubins path to generate a path for an autonomous moving body to avoid interference with obstacles, etc. A Dubins path is a path consisting of an arc that includes a start point, an arc that includes an end point, and a straight line that is tangent to the two arcs.

[0005] Non-Patent Document 1 describes a technique for generating a Dubins path. Non-Patent Document 2 describes a technique for extending the Dubins path, which is limited to forward-only routes, to generate a Reeds-Shepp path that also includes backward routes. [Prior art documents] [Patent documents]

[0006] [Patent Document 1] Japanese Patent Application Publication No. 2023-149910 [Patent Document 2] Japanese Patent Application Laid-Open No. 2007-276508 [Patent Document 3] Japanese Patent Publication No. 2023-59815 [Non-patent literature]

[0007] [Non-Patent Document 1] LE Dubins, "On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents", American Journal of Mathematics, 1957, Vol. 79, No. 3, 497-516 [Non-patent document 2] James Reeds and Lawrence Shepp, "Optimal paths for a car that goes both forwards and backwards", Pacific journal of mathematics, 1990, Vol.145, No.2, 367-393 Summary of the Invention [Problem to be solved by the invention]

[0008] To avoid an obstacle, a moving object must generate a path based on the obstacle's location. Since an obstacle may suddenly appear in the moving object's direction of travel, a path to avoid the obstacle must be generated in real time. In such cases, the model predictive control described in Patent Documents 1 and 2 is promising. However, the computational load of the optimization control amount performed by model predictive control is high, making it difficult to generate a path in real time using the computation unit of a typical automatic steering device. Therefore, in conventional technologies, a computation unit for model predictive control must be provided separately from the computation unit of a typical automatic steering device. Furthermore, if a path is to be represented by a smooth curve to avoid discontinuous changes in the moving object's attitude, a large number of waypoints must be densely set on the path, further increasing the computational load. In particular, when setting a temporary path for collision avoidance, the computationally intensive process of calculating a large number of waypoints is disadvantageous in terms of responsiveness.

[0009] Furthermore, in conventional technology (for example, Patent Document 3), when generating a route for a moving object while avoiding the influence of obstacles, etc., a route (Dubins path) is generated on the assumption that the moving object will only move forward. Therefore, there is a possibility that a long and inefficient route will be generated for a moving object that can move backward.

[0010] The object of the present disclosure is to provide a path control device, an automatic steering device, a path control method, and a program that can quickly calculate an efficient path that allows forward and backward movement with a small computational load and instruct a moving body on its direction of travel and attitude. [Means for solving the problem]

[0011] According to one aspect of the present disclosure, a course control device includes a first measuring device that measures the current position and attitude of a moving body, a waypoint setting device that sets the position of a waypoint on a planned route and the attitude of the moving body at the waypoint, and a course controller that calculates a direction of travel of the moving body indicating forward or backward movement and a course orientation that indicates the attitude of the moving body when traveling in the direction of travel, so as to move along the shortest path from the starting point to the end point, based on a first arc that includes the starting point, a second arc that includes the end point, and a straight line that is tangent to both the first arc and the second arc, with the current position and attitude of the moving body as a starting point and the position and attitude of the waypoint as an end point, and outputs the calculated course orientation to a movement control device that controls the movement of the moving body.

[0012] According to one aspect of the present disclosure, an automatic steering device includes the above-mentioned course control device and a movement control device that controls the moving body based on the traveling direction and the course orientation output from the course control device, and the movement control device has a speed controller that calculates a speed command value for the moving body based on the traveling direction, and a motion controller that calculates a rudder angle command and a thrust command for the moving body based on the course orientation and the speed command value.

[0013] According to one aspect of the present disclosure, a path control method includes the steps of measuring the current position and attitude of a moving body, setting the positions of waypoints on a planned route and the attitude of the moving body at the waypoints, and calculating a direction of travel of the moving body indicating forward or backward movement and a course orientation indicating the attitude of the moving body when traveling in the direction of travel, based on a first arc including the start point, a second arc including the end point, and a straight line tangent to both the first arc and the second arc, with the current position and attitude of the moving body as a start point and the position and attitude of the waypoint as an end point, so as to move along the shortest path from the start point to the end point, and outputting the results to a mobile control device that controls the movement of the moving body.

[0014] According to one aspect of the present disclosure, the program causes a course control device to perform the following steps: measuring the current position and attitude of a moving body; setting the positions of waypoints on a planned route and the attitude of the moving body at the waypoints; and calculating, based on the current position and attitude of the moving body as a starting point and the position and attitude of the waypoint as an end point, a first arc including the starting point, a second arc including the end point, and a straight line tangent to both the first arc and the second arc, a direction of travel of the moving body indicating forward or backward movement and a course orientation indicating the attitude of the moving body when traveling in the direction of travel, so as to move along the shortest path from the starting point to the end point, and outputting the results to a movement control device that controls the movement of the moving body. [Effects of the Invention]

[0015] According to the above aspect, an efficient path that allows forward and backward movement can be calculated at high speed with a small calculation load, and the moving direction and attitude can be instructed to the moving body. [Brief explanation of the drawings]

[0016] [Figure 1] 1 is a block diagram showing a schematic configuration of a moving body according to a first embodiment. [Figure 2] 1 is a block diagram showing the functional configuration of a route control device according to a first embodiment. [Figure 3] 1 is a block diagram showing a functional configuration of a mobile control device according to a first embodiment. [Figure 4] 4 is a flowchart showing an example of processing of the route control device according to the first embodiment. [Figure 5] FIG. 1 is a first diagram showing an example of a Dubins path. [Figure 6] FIG. 2 is a second diagram showing an example of a Dubins path. [Figure 7] FIG. 3 is a third diagram showing an example of a Dubins path. [Figure 8] FIG. 4 is a fourth diagram showing an example of a Dubins path. [Figure 9] 1 shows an example of a Reeds-Shepp path. [Figure 10] FIG. 10 is a diagram illustrating a comparative example of a planned route. [Figure 11] FIG. 2 is a diagram showing an example of a planned route according to the first embodiment. [Figure 12] FIG. 10 is a block diagram showing the functional configuration of a route control device according to a second embodiment. [Figure 13] 10 is a flowchart showing an example of processing of a route control device according to a second embodiment. [Figure 14] FIG. 10 is a first diagram illustrating an example of a temporary route according to the second embodiment. [Figure 15] FIG. 10 is a second diagram showing an example of a temporary route according to the second embodiment. [Figure 16] FIG. 10 is a third diagram showing an example of a temporary route according to the second embodiment. [Figure 17] FIG. 4 is a fourth diagram showing an example of a temporary route according to the second embodiment. [Figure 18] FIG. 5 is a fifth diagram showing an example of a temporary route according to the second embodiment. [Figure 19] FIG. 6 is a sixth diagram showing an example of a temporary route according to the second embodiment. [Figure 20] FIG. 10 is a block diagram showing the functional configuration of a route control device according to a modified example of the second embodiment. [Figure 21] FIG. 10 is a diagram illustrating an example of a temporary route according to a modified example of the second embodiment. [Figure 22] FIG. 10 is a block diagram showing the functional configuration of a route control device according to a third embodiment. [Figure 23] FIG. 11 is a diagram illustrating an example of a temporary route according to the third embodiment. [Figure 24] FIG. 10 is a block diagram showing the functional configuration of a route control device according to a fourth embodiment. [Figure 25] FIG. 13 is a diagram illustrating an example of a temporary route according to the fourth embodiment. [Figure 26] FIG. 13 is a diagram illustrating an example of a temporary route according to the fifth embodiment. [Figure 27] FIG. 10 is a block diagram showing the functional configuration of a mobile control device according to a fifth embodiment. [Figure 28]10 is a flowchart showing an example of processing by a route control device according to a fifth embodiment. [Figure 29] FIG. 1 is a schematic block diagram illustrating the configuration of a computer according to at least one embodiment. DETAILED DESCRIPTION OF THE INVENTION

[0017] First Embodiment The first embodiment will be described in detail below with reference to FIGS.

[0018] (General configuration of the moving body) FIG. 1 is a block diagram showing a schematic configuration of a moving body according to a first embodiment. As shown in FIG. 1, the moving body 100 includes an automatic steering device 1 and a controlled object 4. The controlled object 4 includes a steering system and a propulsion system (drive system) of the moving body 100. The automatic steering device 1 includes a course control device 2 and a movement control device 3. The course control device 2 constantly provides course instructions to the movement control device 3 for moving the moving body 100 to a destination (goal) along a predetermined planned route. The movement control device 3 calculates rudder angle commands and thrust commands in accordance with the course instructions and outputs them to the steering system and propulsion system of the moving body 100. The steering system and propulsion system operate in accordance with the commands, thereby changing the position and attitude of the moving body 100. In other words, the moving body 100 moves along the planned route.

[0019] In this embodiment, an example will be described in which the mobile body 100 is an unmanned or manned ship. In other embodiments, the mobile body 100 may be an unmanned autonomous mobile body such as an unmanned underwater vehicle, an unmanned guided vehicle, or an unmanned aerial vehicle. The mobile body 100 may also be a manned mobile body with an automatic driving device, such as a general vehicle or an aircraft.

[0020] (Functional configuration of route control device) 2 is a block diagram showing the functional configuration of the route control device according to the first embodiment. As shown in FIG. 2, the route control device 2 includes a first measuring device 201, a waypoint setting device 202, and a route controller 203.

[0021] The first measuring instrument 201 measures the current position and orientation {p, ^p} of the moving object 100, and the speed u p The first measuring instrument 201 is, for example, an Automatic Identification System (AIS) that detects the position, course, speed, etc. of a ship and transmits this information to other ships. The first measuring instrument 201 may also be a positioning device that uses a Global Navigation Satellite System (GNSS), or a sensor device such as radar or LiDAR, or may be a combination of an AIS and these devices.

[0022] The via point setter 202 calculates the positions and orientations of the via points of the planned route {q r ,^q r The waypoint attitude is the attitude of the moving body 100 at the waypoint.

[0023] The route controller 203 generates a route for the moving body 100. The route controller 203 sets the current position and orientation of the moving body 100 as a starting point {p, ^p} and the positions and orientations of the intermediate points {q r ,^q r} as the end point, and the moving direction indicating forward (FWD) or backward (REV) and the course direction ^p indicating the attitude of the moving body 100 when moving in the moving direction are determined so that the moving body 100 moves along the shortest path from the starting point to the end point. ref and outputs the result to the mobile control device 3. In this embodiment, the route controller 203 generates a route using a Reeds-Shepp path.

[0024] (Functional configuration of the mobile control device) 3 is a block diagram showing the functional configuration of the movement control device according to the first embodiment. As shown in FIG. 3, the movement control device 3 includes a speed controller 31 and a motion controller 32.

[0025] The speed controller 31 calculates a speed command value u based on a command indicating the forward (FWD) or reverse (REV) direction of the moving body 100. ref is output to the motion controller 32.

[0026] The motion controller 32 has a rudder angle controller 321 and a thrust controller 322. The rudder angle controller 321 controls the current attitude ^p of the moving body 100 and a command ^p of the course direction. ref The thrust controller 322 calculates a rudder angle command value based on the current speed u p and the speed command value u ref The thrust command value is calculated based on the current attitude ^p and speed u of the moving body 100 and output to the thrust system. p is the measurement value of the first measuring instrument 201 of the route control device 2.

[0027] (Example of route control device processing) 4 is a flowchart showing an example of the process of the route control device according to the first embodiment. While receiving an automatic navigation command (step S101; YES), the route control device 2 generates a route for the moving body 100 and a course direction command ^p ref The course control device 2 repeatedly executes a process (steps S102 to S105) of outputting a command for the direction of travel (FWR / REV) and a command for the direction of travel to the mobile control device 3. Furthermore, when the automatic navigation command is stopped (step S101: NO), the course control device 2 ends the automatic navigation. For example, when the manned mobile body 100 is switched to manual operation or when a command to stop the movement of the unmanned mobile body 100 is received, the course control device 2 determines that the automatic navigation command has been stopped and ends the automatic navigation.

[0028] Each step during automatic navigation will be described below: First, the first measuring instrument 201 measures the current position and attitude {p, ^p} of the moving body 100 and inputs them to the course controller 203 (step S102).

[0029] The via point setting unit 202 also calculates the position and attitude {q r ,^q r} and inputs it to the route controller 203 (step S103). The route controller 2 is given a planned route for the moving body 100 in advance. The planned route includes the destination and the positions and orientations of one or more waypoints. The waypoint setter 202 extracts from the planned route a waypoint (or destination) that has not yet been reached by the moving body 100 and is closest to the current position, and sets the next waypoint {qr ,^q r} and input to the route controller 203.

[0030] Note that positions p and q r is a two-dimensional or three-dimensional vector. The orientation ^p is the orientation of the moving object at the current location, and the orientation ^q is the orientation of the moving object at the current location. r is the attitude of the moving body 100 when passing through the waypoint. In this application, a hat symbol (^) is used to represent a unit orientation vector. For example, "^q r ” is the distance from the moving object 100 traveling along the route at a unit speed to the position q of the via point. r This is normalized by the velocity vector when passing through.

[0031] The route controller 203 sets the current position and orientation {p, ^p} of the moving body 100 as the starting point, and calculates the position and orientation {q r ,^q r} as the end point, and the course direction command ^p by Reeds-Shepp path ref Then, a forward / reverse command (FWD or REV) is calculated and output (step S104).

[0032] First, the heading command ^p ref In the present application, the heading direction of the moving body 100 is assumed to be the same as the attitude of the moving body 100. For example, if the position and attitude of the moving body 100 are {p, ^p}, the heading direction of the moving body 100 at position p is ^p. Since the attitude of the moving body 100 can be arbitrarily changed by operating the rudder, the moving body 100 can essentially move in any direction. However, in a conventional path generation method using Dubins path, in order to move between two points via the shortest path, the course of the moving body 100 from its current position is limited to three, as shown in Figures 4 to 6. That is, the course of the moving body 100 is one of the following: (1) moving straight (Figure 5), (2) turning left with a radius R set as the limit of the moving body's turning radius (Figure 6), or (3) turning right with a radius R (Figure 7). The radius R can be set as the limit value of the moving body's motor ability, but is generally set to a value greater than the limit value of the motor ability to allow for some leeway.

[0033] 5 to 7 are diagrams showing an example of a Dubins path. In each of the diagrams, the current position and posture of the moving object 100 are set as a start point {p, ̂p}, and the next via point on the planned route is set as an end point {q r ,^q r}, and shows shortest route candidates pp1 to pp3 for the cases where the moving object 100 travels (1) straight (FIG. 5), (2) counterclockwise around a first arc Ap determined for the starting point (FIG. 6), and (3) clockwise around the first arc Ap (FIG. 7). Following the Dubins path, the candidate routes are composed of a first arc Ap determined for the starting point, a second arc Aq determined for the end point, and a straight line L tangent to both the first arc Ap and the second arc Aq.

[0034] The course controller 203 determines a first arc Ap of radius R so as to overlap the starting point, and determines a second arc Aq of radius R so as to overlap the ending point. p , the center of the second arc Aq is O qr It is expressed as center O p , O qr is determined from the end point (way point) by equation (1). The second term on the right side of equation (1) is the unit direction vector ^q r is a vector perpendicular to

[0035]

number

[0036] In the case of (1) going straight in FIG. 5, the route pp1 does not contact the second arc Aq, and therefore is not a candidate for the shortest route.

[0037] In the case of (2) in Figure 6, when the first arc Ap is turned counterclockwise, two routes are compared: one that turns the second arc Aq counterclockwise and one that turns it clockwise, and the shorter route is selected. In the example in Figure 6, the route pp2 that turns the second arc Aq counterclockwise is the candidate.

[0038] In the case of (3) in Figure 7 where the first arc Ap is turned clockwise, the route that turns the second arc Aq counterclockwise and the route that turns clockwise are compared and the shorter route is selected. In the example in Figure 7, the route pp3 that turns the second arc Aq counterclockwise is the candidate.

[0039] In this way, the lengths of the three routes {going straight, going left around the first arc Ap, going right around the first arc Ap} are evaluated, and the route with the shortest route length is selected. In the examples of Figures 5 to 7, (2) route pp2 going left around the first arc Ap is the shortest.

[0040] In the Dubins path, the traveling direction of the moving object 100 is limited to forward. On the other hand, the Reeds-Shepp path is derived from the Dubins path and allows backward movement in addition to forward movement (see Non-Patent Document 2). While an example of the Dubins path has been described in FIGS. 5 to 7, the route controller 203 of this embodiment calculates the shortest route to the waypoint of the moving object 100 using the Reeds-Shepp path in step S104.

[0041] 8 and 9 are diagrams showing examples of Dubins path and Reeds-Shepp path. FIG. 8 shows the first arc Ap, the second arc Aq, and the line L to which both arcs are tangent of the Dubins path. The first arc Ap determined for the start point {p, ^p} has two candidates, clockwise and counterclockwise. The end point {q r ,^q r The second arc Aq determined by} also has two candidates, one going clockwise and one going counterclockwise. A Dubins path is the shortest of the four routes made up of two combinations of candidates. In the example of Figure 8, the route pp4, which goes clockwise around the first arc Ap and also goes clockwise around the second arc Aq, is the shortest and is the Dubins path.

[0042] The Reeds-Shepp path also allows retreat. Allowing retreat makes it possible to select a shorter route, similar to a railroad switchback. In the example of Figure 9, the shortest route is route pp5, which goes clockwise while retreating along the first arc Ap, counterclockwise while retreating along the second arc Aq, and finally goes clockwise while moving forward along the second arc Aq, and this is the Reeds-Shepp path. In the examples of Figures 8 and 9, route pp5, which is the Reeds-Shepp path, is shorter than route pp4, which is the Dubins path.

[0043] 4, the route controller 203 selects the shortest one of the candidates based on the Dubins path and the Reeds-Shepp path as the route of the mobile unit 100. If the route pp4 of the Dubins path as shown in FIG. 8 is the shortest, the route controller 203 sets the route direction command ^p so that the mobile unit 100 moves forward clockwise from the start point {p, ^p} along the first arc Ap. ref 9, the path controller 203 issues a path direction command ^p to move backward counterclockwise along the first arc Ap from the starting point {p, ^p}. ref 5 to 7, it is assumed that the route pp2 in FIG. 6 is the shortest. In this case, the route controller 203 The course command ^p is set to move forward counterclockwise from the starting point {p,^p} along the first arc Ap. ref A forward direction command (FWD) indicating forward movement is also given to the movement control device 3.

[0044] After the route controller 203 outputs a command to the movement control device 3, the route control device 2 waits for the next control cycle (step S105) and returns to the beginning of the process (step S101).

[0045] The route control device 2 performs the above calculations (steps S102 to S105) in each control cycle. For example, at time t1, the route control device 2 calculates the position and attitude {p, ̂p} of the moving body 100. t1Starting from point {q r ,^q r} to the shortest route. ref The moving body 100 moves in the direction and orientation specified by the command (FWD / REV). At time t2, the position and attitude of the moving body 100 are set to {p, ^p} t2 Then, the course control device 2 calculates the position and attitude {p, ^p} of the moving body 100. t2 Starting from point {q r ,^q r} to the shortest route. ref By repeating this process every control cycle, the position and orientation {p, ̂p} of the moving body 100 at each time, for example, time t, are determined. t and via point {q r ,^q r}, the command ^p for the course direction at time t ref,t By calculating only the command for the direction of travel (FWD / REV) and issuing it to the mobile control device 3, the mobile object 100 can be made to arrive at the waypoint via the shortest route by following the Dubins path or the Reeds-Shepp path. In the Dubins path and the Reeds-Shepp path, the route is limited to only arcs and straight lines, so the calculation load for the shortest route can be significantly reduced compared to conventional predictive model control.

[0046] (Example of planned route) Furthermore, the route control device 2 can cause the moving object 100 to travel smoothly along the planned route by calculating the route using the Reeds-Shepp path. An example of the planned route of this embodiment will be described with reference to Figs. 10 and 11.

[0047] 10 and 11 are diagrams showing examples of planned routes. Planned routes are determined by waypoints. 10 and 11 show examples of planned routes pp6 and pp7 in which three waypoints {j-1, j, j+1} are connected by lines.

[0048] The simplest way to connect waypoints with a line is a straight line, as shown in the example in Figure 10. However, in the example in Figure 10, the line breaks when passing through a waypoint, causing the course direction to change discontinuously. This is inconvenient because it causes the moving object to make sudden turns.

[0049] Therefore, in step S104 of FIG. 4, the route control device 2 (route controller 203) of this embodiment connects the waypoints according to a Reeds-Shepp path. By following the Reeds-Shepp path, if the position and orientation of the start point and the position and orientation of the end point are specified, a smooth route can be obtained that connects them in the shortest possible time. To achieve smoothness, the curvature of the route, i.e., the inverse of the radius of rotation, is limited. FIG. 11 shows an example in which a planned route is generated using a Reeds-Shepp path with the minimum allowable radius of rotation set to R. The minimum allowable radius R may be a fixed value according to the motor ability of the mobile object 100, as described above, or may be changed according to the speed of the mobile object 100. For example, a smaller radius of rotation may be allowed when the speed of the mobile object 100 is slow than when the speed is fast.

[0050] In this embodiment, for each via point, the position q r and the posture of the moving object when it passes through the via point ^q r The position qr is a 2D or 3D vector, and the orientation ^q r is position q r The direction control device 2 of this embodiment determines an arc Aq of radius R so that it overlaps with the waypoint. The center of arc Aq is O. qr and the center Oqr is determined by the above equation (1) based on the position and orientation of the via point.

[0051] According to the Reeds-Shepp path, as shown in the example of FIG. 11, when the mobile object 100 is between waypoint j-1 and waypoint j, the position q of the waypoint j to which the mobile object 100 is heading is r,j , posture^q r,j , and the radius of gyration R qr,jand the current position p, attitude ^p, and rotation radius R of the moving body 100, the attitude of the moving body 100, i.e., the command value ^p of the course direction ref can be determined.

[0052]

number

[0053] The moving object 100 receives the command value ^p ref If we follow this formula, for example, the planned route to waypoint j is expressed as follows: T is the distance (Travel) of the planned route starting from the current position p.

[0054]

number

[0055] (Action and effect) As described above, the course control device 2 according to this embodiment uses the current position and orientation {p, ^p} of the moving body 100 as the starting point, and calculates the position and orientation {q r ,^q r} is set as the end point, and based on a first arc Ap including the starting point, a second arc Aq including the end point, and a straight line L tangent to both the first arc Ap and the second arc Aq, a forward or backward direction command (FWD / REV) indicating whether to move forward or backward and a course direction command ^p indicating the attitude of the mobile body are issued so that the mobile body 100 moves along the shortest path (Dubins path or Reeds-Shepp path). ref and output it to the movement control device 3.

[0056] In this way, the course control device 2 calculates the shortest route for the moving body 100 using only arcs and straight lines, which significantly reduces the calculation load for the shortest route compared to conventional predictive model control. Therefore, unlike conventional technology, there is no need to provide a separate calculation device for generating the shortest route, and calculations can be performed by the calculation device (processor) of the automatic steering device 1.

[0057] Furthermore, in the prior art such as Patent Document 3, the shortest route from the current position to the next waypoint (goal) is calculated, a point cloud on the shortest route indicating the positions the mobile object must pass at each time is calculated, and a travel schedule is created, after which the mobile object moves autonomously. In other words, the step of generating a route is separated from the step of calculating control commands for autonomous movement control. Thus, in the prior art, future waypoints must be calculated before the mobile object reaches the waypoint (goal), which increases the computational load. In contrast, the path control device 2 of this embodiment calculates only the course direction and travel direction commands for the mobile object 100 from the shortest route and outputs them to the movement control device 3. In other words, the mobile object 100 can move along the shortest route without calculating the points the mobile object 100 must pass between its current position and the waypoint. As such, the path control device 2 of this embodiment can significantly reduce the computational load compared to the prior art, making it suitable for real-time guidance control.

[0058] Furthermore, by using the Reeds-Shepp path, the route control device 2 can generate a smooth route suitable for the movement of the moving object 100, as in the example of Fig. 11. Furthermore, by generating a route using the Reeds-Shepp path including a retreat, it is possible to obtain a route that is shorter than the conventional technique that uses only the Dubins path, and the moving object 100 can be moved efficiently.

[0059] The automatic steering device 1 also includes the above-mentioned course control device 2, and a movement control device 3 that controls the moving body 100 based on the traveling direction and course orientation output from the course control device 2. The movement control device 3 has a speed controller 31 that calculates a speed command value for the moving body 100 based on the traveling direction, and a motion controller 32 that calculates a rudder angle command and a thrust command for the moving body 100 based on the course orientation and the speed command.

[0060] In this way, the automatic steering device 1 can quickly calculate an efficient path that allows forward and reverse movement with a small calculation load, and move the moving body 100.

[0061] <Second embodiment> The second embodiment will be described below with reference to Figures 12 to 19. Note that among the configurations of the second embodiment, those components that are the same as those of the above-mentioned embodiment will be described using the same reference numerals as those of the above-mentioned embodiment. This embodiment differs from the above-mentioned embodiment in that the course control device 2 generates a temporary route that avoids obstacles in addition to the planned route and commands changes in the orientation and direction of the moving body 100.

[0062] (Functional configuration of route control device) 12 is a block diagram showing the functional configuration of a route control device according to the second embodiment. As shown in FIG. 12, the route control device 2 according to this embodiment further includes a second measuring device 204, a temporary waypoint setting device 205, and an arbitration device 206.

[0063] The second measuring instrument 204 detects obstacles on the path of the moving body 100. The second measuring instrument 204 is a sensor device such as a radar or LiDAR.

[0064] The temporary way point setter 205 sets a temporary way point for avoiding an obstacle. Specifically, the temporary way point setter 205 sets a position and orientation {b M ,^b M} and the position and attitude of the temporary left waypoint {b H ,^b H} and set it.

[0065] The arbitrator 206 is a switch that outputs either a via point of the planned route or a temporary via point for obstacle avoidance as the next via point depending on whether an obstacle exists or not. r ,^q r} and temporary waypoint (right waypoint {b M ,^b M}, left waypoint {b H ,^b H}) is output to the course controller 203 as the position and attitude of the next waypoint.

[0066] The route controller 203 determines the route direction command ^p of the moving object based on the position and attitude of the waypoint output from the arbitrator 206. ref It also calculates and outputs the forward / reverse direction command (FWD / REV).

[0067] (Example of route control device processing) 13 is a flowchart showing an example of the process of the route control device according to the second embodiment. As in the first embodiment, while receiving an automatic navigation command (step S201; YES), the route control device 2 generates a route for the moving body 100 and outputs a course direction command ^p ref The route control device 2 repeatedly executes a process (steps S202 to S208) of outputting a command for the direction of travel (FWR / REV) to the mobile control device 3. When the automatic navigation command is stopped (step S201: NO), the route control device 2 ends the automatic navigation. Furthermore, while the mobile body 100 is in operation, the second measuring device 204 constantly performs a process of detecting obstacles in the direction of travel of the mobile body 100.

[0068] Each step during automatic navigation will be described below: First, the first measuring instrument 201 measures the current position and attitude {p, ^p} of the moving body 100 and inputs them to the course controller 203 (step S202).

[0069] The via point setting unit 202 also calculates the position and attitude {q r ,^q r} and inputs it to the arbitrator 206 (step S203).

[0070] The temporary way point setter 205 determines whether or not the obstacle obstructs the path of the moving body 100 based on the position of the obstacle detected by the second measuring instrument 204 (step S204). If there is no obstacle obstructing the path (step S204; NO), the temporary way point setter 205 does not output the temporary way point to the arbitrator 206. Therefore, the arbitrator 206 determines whether or not the way point {qr ,^q r} is input to the path controller 203 as the next via point {q, ^q}. Then, the path controller 203 sets the current position and orientation {p, ^p} of the moving object as the starting point, and the position and orientation {q r ,^q r} as the end point, and the Reeds-Shepp path heading ^p ref Then, a forward / reverse command (FWD or REV) is calculated and output (step S207). This process is the same as that in the first embodiment (step S104 in FIG. 4).

[0071] On the other hand, if there is an obstacle blocking the path (step S204; YES), the temporary way point setter 205 inputs a temporary way point of the moving body 100 to the arbitrator 206 (step S205). The temporary way point is a point on the circumference of an avoidance circle of a predetermined radius Rb centered on the position of the obstacle. The temporary way point setter 205 sets a right way point {b M ,^b M} and the left waypoint {b H ,^b H When the temporary waypoints are input, the arbitrator 206 calculates the route of the planned route by r ,^q r}, and the right way point {b M ,^b M} and left waypoint {b H ,^b H} is input to the route controller 203 as the next waypoint {q, ^q}.

[0072] When the temporary waypoint is input, the route controller 203 sets the current position and orientation {p, ^p} of the moving object as the starting point, and the position and orientation {b M ,^b M} or {b H ,^b H} as the end point, and the Reeds-Shepp path heading ^p refThen, a forward / reverse command (FWD or REV) is calculated and output (step S206). Note that this process is the same as step S207.

[0073] After the route controller 203 outputs a command to the movement control device 3, the route control device 2 waits for the next control cycle (step S105) and returns to the beginning of the process (step S101).

[0074] (Example of temporary route) 14 to 19 are diagrams showing examples of temporary routes according to the second embodiment. With reference to these figures, an example of generating a temporary route by the above-mentioned processing (steps S202 to S207) will be described in detail.

[0075] An important feature of this embodiment is the function of the temporary way point setter 205. When the second measuring instrument 204 detects an obstacle, the temporary way point setter 205 generates a temporary way point to avoid the obstacle. As described above, a point on the circumference of an avoidance circle with a radius Rb and centered on the obstacle is set as a temporary way point. An advantage of this embodiment is that by simply setting a temporary way point, the same route controller 203 as in the first embodiment can also generate a temporary route to avoid the obstacle. In this embodiment, the route controller 203 generates a route from the input way points in the same way whether there is an obstacle or not. In other words, the difference between when there is an obstacle and when there is not an obstacle is the difference between when the value of the way point is the value {q r ,^q r} or the value of the temporary waypoint {b M ,^b M},{b H ,^b H} is the only thing that matters.

[0076] If an obstacle appears in the path of the moving object 100, there is a risk that the moving object 100 will collide with the obstacle. In this case, the path control device 2 guides the moving object 100 to a temporary route that deviates from the planned route to avoid the collision. A feature of this embodiment is that the path control device 2 generates the temporary route according to the Reeds-Shepp path, just like the planned route.

[0077] 13, the temporary waypoint setting unit 205 first sets an avoidance circle Ab to surround the obstacle detected by the second measuring unit 204. As shown in FIG. b The center of the avoidance circle Ab is O b The value of the radius of the avoidance circle Ab may be set to a different value that is larger than the minimum turning radius R allowed for the moving body 100. In the following, the radius of the avoidance circle Ab is set to a predetermined value R b The explanation will continue assuming that the temporary waypoint is set on the circumference of the avoidance circle Ab. Figure 14 shows the position and attitude of the temporary right waypoint {b M ,^b M As shown in equation (4), the temporary right waypoint posture ^b M may be the heading of the planned route.

[0078]

number

[0079] Posture^b M By specifying the position b on the avoidance circle Ab, the avoidance circle Ab is placed to the right M The temporary left way point {b H ,^b H} can be determined in the same way. The temporary left waypoint attitude ^b H can be the orientation of the planned route in equation (4). H If you specify, the position b on the avoidance circle Ab will be moved to the left of the avoidance circle Ab. H is determined.

[0080] Furthermore, the arbitrator 206 selects whether to avoid the avoidance circle Ab to the right or left. Of the two points, the right way point and the left way point, the one whose line of sight direction from the moving body 100 is closest to the attitude ^p of the moving body 100, or the one of the two points whose distance to the moving body 100 is closer, may be selected. Figure 14 shows an example in which the right way point is selected from the left and right.

[0081] The arbitrator 206 determines the route points {q r ,^q r}, temporary right waypoint {b M ,^b M}, temporary left waypoint {b H ,^b H} is instructed to the path controller 203 as the via point position and orientation {q, ^q}. If no obstacles are blocking the path of the moving body 100, the arbitrator 206 instructs the path controller 203 to use the position and orientation {q, ^q} of the via point on the planned route. r ,^q r When an obstacle blocks the path of the moving body 100, the position and orientation {q, ^q} of the waypoint that the arbitrator 206 commands the path controller 203 are the temporary right waypoint {b M ,^b M} or temporary left waypoint {b H ,^b H}.

[0082] Obstacles b Whether or not an obstacle obstructs the path of the moving body 100 is determined by the line segment (line segment O) connecting the obstacle and the temporary waypoint. b b M and line segment O (not shown) b b H ) and the route pp connecting the position of the moving object 100 and the via point. b b M and line segment O b b H If either of these intersects with the path pp, then the obstacle O b In this case, the temporary waypoint setter 205 detects the obstacle O b The arbitrator 206 sets temporary right waypoints and left waypoints that avoid the above-mentioned obstacles and inputs them to the arbitrator 206. The arbitrator 206 adopts either the temporary right waypoint or the temporary left waypoint. The arbitrator 206 selects, for example, the left or right point whose line of sight direction from the moving body 100 is closest to the attitude ^p of the moving body 100. Alternatively, the arbitrator 206 may select, for example, the left or right point whose path from the current position of the moving body 100 is the shortest.

[0083] Once the via point {q,^q} is determined, the end point is {q,^q}, the starting point is the position and orientation of the moving object {p,^p}, and the direction of the temporary route is determined based on the Reeds-Shepp path. b The direction of travel (forward or backward) can be determined. The route pp in Fig. 14 is the planned route. The route pb shown by the dashed line in Fig. 14 represents a temporary route obtained as the wake of the moving object 100 due to obstacle avoidance.

[0084]

number

[0085] In equation (4), the attitude of the temporary way point is set to the direction of the planned route. To supplement this advantage, Figure 15 shows an example in which a temporary way point (marked with an X in Figure 15) is set so that the avoidance circle Ab is moved to the right and its attitude matches the direction of the planned route, as in equation (4). Since temporary way point X is the furthest away from the planned route pp, matching the attitude of the temporary way point with the direction of the planned route makes it possible to prevent detours like the route pb' shown by the dashed line.

[0086] Another advantage of the path controller 203 using the Reeds-Shepp path is that it allows for backward movement. In this embodiment, a start point and an end point are first determined, and then a route is generated by connecting the two with a Reeds-Shepp path. As described in the first embodiment, the first advantage of the Reeds-Shepp path is smoothness, and the second advantage is that it allows for backward movement. This second advantage, that it allows for backward movement, is very useful for the automatic obstacle avoidance of this embodiment, as will be explained below.

[0087] Figure 16 shows the obstacle O b This shows a situation where a temporary waypoint {b,^b} suddenly appears and a temporary waypoint {b,^b} is set. For simplicity, the temporary waypoint is written as {b,^b} without distinguishing between left and right. In the figure, only the example of the right waypoint is shown. In this situation, if you try to obtain a temporary route pb by moving forward, you will violate the avoidance circle Ab at the position pt1 as shown by the dashed line pb. For this reason, if you move forward, you will not be able to avoid the obstacle O. bNo temporary route pb that can avoid this is available.

[0088] So, what about retreat? Figure 17 shows a temporary path pb resulting from retreat. If you retreat once as shown by the dotted line pb_a and then advance as shown by the dashed line pb_b, a temporary path pb is generated without violating the avoidance circle Ab. This temporary path pb consisting of the dotted line pb_a and dashed line pb_b is generated according to the Reeds-Shepp path, with the position and orientation of the mobile unit 100 as the starting point and the temporary via point as the end point. The Reeds-Shepp path allows for the creation of the shortest path from the starting point to the end point, with retreat also being an option, so it is possible to generate a path with the shortest path from the starting point to the end point without violating the obstacle O in this way. b This is effective for avoiding collisions when an obstacle appears in front of you.

[0089] If the mobile body 100 is a ship, it may not be easy to move the mobile body 100 backward precisely along the route. However, moving the mobile body 100 backward is undoubtedly effective in avoiding collisions. Even if the mobile body 100 deviates from the route when moving backward, the route controller 203 outputs a command for the course direction from time to time based on the position and attitude of the mobile body 100 so that the mobile body 100 travels along the Reeds-Shepp path to the temporary waypoint, and the temporary route pb can be changed according to the position of the mobile body 100. This allows the mobile body 100 to avoid obstacles O. b can be avoided autonomously.

[0090] The Reeds-Shepp path will be explained with reference to Figure 18. The position and orientation {p1, ^p1} of the moving object 100 are b appears in front of the moving object 100. In this case, when the moving object 100 moves forward, the radius R restricts the moving object 100's turning radius. b The Reeds-Shepp path is known in advance as a path that provides the shortest route from the start point to the end point, and in the prior art, it is not used for a route that retreats in real time so as not to violate the avoidance circle Ab.

[0091] It is known that the shortest path between two points is generally given by two arcs and a tangent line connecting the arcs, such as the Dubins path described in Non-Patent Document 1. As described above, the Reeds-Shepp path also uses the results of the Dubins path, and if the position and orientation of the moving object 100 are {p2, ^p2}, the Reeds-Shepp path also uses the results of the Dubins path, p2 The first arc Ap2 and the obstacle O are centered at b The second arc Ab is centered at and the straight line L2 is tangent to both.

[0092] However, if the position and orientation of the moving object 100 is {p1, ^p1}, then O p1 If the first arc Ap1 is set as the center, the obstacle O is moved from the first arc Ap1 to the temporary waypoint so that the moving body is in the attitude (forward) of the command ^b of the course direction. b It is not possible to draw a tangent to the second arc Ab centered at . For the moving body to face forward at the temporary waypoint, it must either move forward counterclockwise along the first arc Ap1, as shown by the dashed-dotted line FH, or move backward counterclockwise as shown by the dotted line RH. The Reeds-Shepp path compares the two and selects the shorter path. In other words, the path pb1 that moves backward counterclockwise along the dotted line RH is the Reeds-Shepp path.

[0093] The Reeds-Shepp path is a technique for searching for the shortest route for the moving object 100. However, if the radius Rb of the avoidance circle Ab is set as the radius of the second arc as in this embodiment, the avoidance circle Ab is not violated and a route pb with the shortest route is generated, and the obstacle O is avoided. b It can be used as a means to avoid

[0094] An example of returning the moving object 100 from the temporary route pb to the planned route pp will be described with reference to Fig. 19. As shown in Fig. 19, when the moving object 100 returns to the temporary route point {b, ^b M}, the next planned route point j{q r,j ,^q r,jTherefore, the route control device 2 sets the route direction command ^p as the end point of the waypoint j of the planned route and the current position and attitude {p, ^p} = {b, ^b} of the mobile object as the start point. ref The moving object 100 then moves from the temporary waypoint to waypoint j on the planned route.

[0095] (Action and effect) As described above, the path control device 2 according to this embodiment is configured by the second measuring device 204 that detects the position of an obstacle on the path of the moving body 100, and the position and attitude {b M ,^b M} and the position and attitude of the temporary left waypoint {b H ,^b H}, and an arbitrator 206 that outputs the position and attitude of any one of the via points, right via points, and left via points of the planned route to the course controller 203 as the position and attitude of the next via point {q, ^q}.

[0096] In this way, the route control device 2 can generate a temporary route that can avoid an obstacle that appears on the route, and move the mobile body 100 along the temporary route, by the same processing as when moving the mobile body 100 along the planned route. This allows the mobile body 100 to avoid obstacles autonomously.

[0097] Furthermore, by generating a temporary route using the Reeds-Shepp path, the route control device 2 can more reliably avoid obstacles even in cases where avoidance is difficult using the Dubins path, such as when an obstacle is suddenly detected near the mobile body 100. Furthermore, as described above, the computational load of the Reeds-Shepp path is light, so the mobile body 100 can take evasive action with high responsiveness even in situations where an obstacle needs to be avoided urgently.

[0098] <Modification of the second embodiment> In the second embodiment, a technique for avoiding a single obstacle was described. In this modified example, a technique for avoiding multiple obstacles will be described. Note that in this modified example, an example will be described in which multiple independent obstacles are avoided as a single obstacle group (the avoidance circles of each obstacle are connected), but the present invention is not limited to this. When obstacles include ships, public buses, articulated vehicles, etc., the slender shape of these obstacles may be represented by a group of multiple obstacles. In other words, even if there is a single obstacle, if it has a slender shape (longer in the front-to-back direction than in the width direction), multiple avoidance circles corresponding to the lateral width of this obstacle may be arranged in accordance with the length of the front-to-back direction of the obstacle, and the multiple connected avoidance circles may be avoided.

[0099] (Functional configuration of route control device) Fig. 20 is a block diagram showing the functional configuration of a route control device according to a modification of Embodiment 2. As shown in Fig. 20, the route control device 2 according to this modification further includes an avoidance circle setter 207 and a connected obstacle extractor 208.

[0100] The avoidance circle setting device 207 sets an avoidance circle surrounding each of the plurality of obstacles based on the positions of the obstacles detected by the second measuring device 204.

[0101] The connected obstacle extractor 208 extracts elements with multiple overlapping avoidance circles from a set of multiple obstacles, and determines a set of connected obstacles.

[0102] Further, the temporary way point setter 205 sets the positions and orientations of the right way point and the left way point for avoiding a plurality of obstacles based on the set of connected obstacles.

[0103] (Example of temporary route) Fig. 21 is a diagram showing an example of a temporary route according to a modification of the second embodiment. Fig. 21 shows a case where there are five obstacles as an example of a case where there are multiple obstacles. In step S205 of Fig. 13, the avoidance circle setter 207, the connected obstacle extractor 208, and the temporary way point setter 205 perform the following processes.

[0104] Numbers are assigned to the obstacles, and the set of obstacle numbers is denoted as B. In the example of FIG. 21, B={1, 2, 3, 4, 5}. The obstacles numbered {1, 2, 3, 4} have avoidance circles Ab_p1 to Ab_p4 that overlap with each other. Overlapping avoidance circles are grouped together to form one connected obstacle. A connected obstacle set is determined for one connected obstacle. One connected obstacle set is a collection of the numbers of the obstacles that make up one connected obstacle. The connected obstacle set is denoted as C. In the example of FIG. 21, C={1, 2, 3, 4}. A connected obstacle set is a set of obstacles that are grouped together based on the position O of the obstacle. b and the radius R of the avoidance circle Ab b It is determined by the following equation (6).

[0105]

number

[0106] When a certain avoidance circle is seen from the moving body, the avoidance circle has a right end and a left end. A certain connected obstacle also has a right end and a left end, and the element of the obstacle at the right end in the obstacle set is the right-most obstacle. When the coordinates are taken in a right-handed system, the direction to the right is negative relatively. Therefore, the element i that is the right-most obstacle M is expressed by the following equation (7).

[0107]

number

[0108] For the rightmost obstacle, the position and orientation of the temporary right way point {b} is set on the circumference of the avoidance circle of the rightmost obstacle so that the moving body 100 avoids the avoidance circle of the rightmost obstacle to the right. M ,^b M}. Posture ^b Mis determined by, for example, the above equation (4).

[0109] Since the relative left direction is positive, the element i of the leftmost obstacle H is expressed by the following equation (8).

[0110]

number

[0111] For the leftmost obstacle, a temporary left way point {b H ,^b H In the example of FIG. 21, the temporary waypoint setter 205 determines the obstacle O b4 On the circumference of the avoidance circle Ab_p4, the right way point {b M ,^b M} on the circumference of the avoidance circle Ab_p1 of the obstacle Ob1, H ,^b H}.

[0112] The processing after determining the right way point and the left way point is the same as in the second embodiment. That is, the right way point and the left way point are input to the arbitrator 206, and the arbitrator 206 determines, for example, the right way point {b M ,^b M} is selected as the next waypoint {q, ^q} and instructed to the route controller 203. The route controller 203 selects the next waypoint {q, ^q}={b M ,^b M}, a temporary route pb for the moving body 100 is generated, and a command ^ for the course direction is issued. pref It also calculates and outputs the direction of travel command (FWD / REV).

[0113] (Action and effect) As described above, the course control device 2 according to this embodiment further comprises an avoidance circle setter 207 that sets an avoidance circle surrounding each of the multiple obstacles based on the position of each of the multiple obstacles detected by the second measuring device 204, and a linked obstacle extractor 208 that determines a linked obstacle set by extracting elements where multiple avoidance circles overlap from a set whose elements are the multiple obstacles. In addition, the temporary way point setter 205 sets the positions and orientations of the right way point and left way point that avoid the multiple obstacles based on the linked obstacle set.

[0114] In this way, even if there are multiple obstacles on the path of the moving body, the path control device 2 can generate a temporary path that can safely avoid these obstacles and move the moving body 100.

[0115] Further, the temporary way point setter 205 determines the element whose line of sight direction to the right of each of the plurality of avoidance circles as seen from the moving body 100 based on the elements of the connected obstacle set as the right obstacle, and determines the element whose line of sight direction to the left of each of the plurality of avoidance circles as the left obstacle, and calculates the position and attitude {b M ,^b M}, and the position and orientation of the left way point {b H ,^b H}.

[0116] In this way, the temporary way point setter 205 can limit the way points to only two points when avoiding multiple obstacles, even when there are multiple obstacles. This makes it possible to easily generate a route that can avoid multiple obstacles without increasing the calculation load.

[0117] <Third embodiment> The third embodiment will be described below with reference to Figures 22 and 23. Note that, among the configurations of the third embodiment, the same configurations as those of the above-mentioned embodiments will be described using the same reference numerals as those of the above-mentioned embodiments. In this embodiment, a technique for avoiding an obstacle (opponent moving object) when the obstacle is a moving object will be described.

[0118] (Functional configuration of route control device) 22 is a block diagram showing the functional configuration of a route control device according to Embodiment 3. As shown in FIG.

[0119] When the obstacle is a moving body, the relative position predictor 209 predicts the relative position of the other moving body when the own moving body and the other moving body come closest to each other.

[0120] The second measuring instrument 204 measures the position and attitude {g, ^g} of the other moving object and its speed u g Measure.

[0121] The temporary waypoint setting unit 205 sets a temporary waypoint for avoiding other moving objects based on the relative positions of the own moving object and the other moving object.

[0122] (Example of temporary route) If the obstacle is stationary as in the second embodiment, the own moving body 100 will not collide with the obstacle if it avoids the current position of the obstacle. However, if the obstacle is a moving body, even if the own moving body 100 avoids the current position of the other moving body, it cannot generally be said that there will be no collision. This is because it takes time for the own moving body to reach the current position of the obstacle (other moving body), and in the meantime the other moving body moves to another position. In order to avoid a collision with the other moving body, it is effective to predict the future positions of the own moving body 100 and the other moving body in some way and avoid the predicted position.

[0123] FIG. 23 is a diagram showing an example of a temporary route according to the third embodiment. As shown in FIG. 23, if it is assumed that both the other moving body and the own moving body move at a constant speed on their current courses, the closest point of approach (CPA) can be calculated. In FIG. 23, the positions of the other moving body V' and the own moving body 100' at the closest point of contact are shown by dashed lines. At this time, the relative position δ when the other moving body V' is viewed from the own moving body 100 is CPAis expressed by the following equation (9), where × is the cross product, ○ is the inner product, and e3 is the vertically upward basis vector.

[0124]

number

[0125] Here, u g is the speed of the other moving body V, g is the position of the other moving body V, and ^g is the unit direction vector representing the attitude of the other moving body V. Information on the speed, position, and attitude of the other moving body V may be obtained using any of a variety of known technologies, but for example, the second measuring instrument 204 may be an Automatic Identification System (AIS).

[0126] From the speed and attitude, the velocity vector g (g with a dot symbol; the same applies hereafter) of the other moving body V is expressed by the following equation (10).

[0127]

number

[0128] Similarly, the velocity vector p of the own moving body 100 is also expressed by the following equation (11).

[0129]

number

[0130] Then, the position of the other moving body V at the closest point is calculated as the obstacle position O b The temporary waypoint {b,^b} is determined. Obstacle position O b is calculated using the following equation (12).

[0131]

number

[0132] 13, the relative position predictor 209 and the temporary way point setter 205 predict the relative position at the closest point to the other moving body V as described above and set a temporary way point. The processing after setting the temporary way point is the same as in the second embodiment.

[0133] (Action and effect) As described above, the course control device 2 according to this embodiment further includes a relative position predictor 209 that predicts the relative position of the host moving body 100 and the other moving body V, which is the obstacle, when they come closest to each other when the obstacle is a moving body. The temporary way point setter 205 sets a temporary way point {b, ^b} to avoid the other moving body V based on the relative position with respect to the other moving body V.

[0134] In this way, even when the obstacle is not a stationary object but a moving object, the route control device 2 can quickly generate a route that can avoid the other moving object and move the own moving object 100.

[0135] <Fourth embodiment> The fourth embodiment will be described below with reference to Figures 24 and 25. Note that among the configurations of the fourth embodiment, those configurations that are the same as those of the above-mentioned embodiments will be described using the same reference numerals as those of the above-mentioned embodiments. In this embodiment, a technology for changing the radius of an avoidance circle depending on the position of an obstacle will be described.

[0136] (Functional configuration of route control device) 24 is a block diagram showing the functional configuration of a route control device according to the third embodiment. As shown in FIG.

[0137] The radius setter 210 changes the radius Rb of the avoidance circle Ab based on whether the obstacle is to the right or left of the moving body 100.

[0138] 24 shows an example in which the configuration of the radius setter 210 is added to the second embodiment, but the present invention is not limited to this. The radius setter 210 may be added to each of the above-described embodiments or modified examples.

[0139] (Example of temporary route) Fig. 25 is a diagram showing an example of a temporary route according to the fourth embodiment. In Fig. 25, when an obstacle V is seen on the right side of the path, the radius R of the avoidance circle Ab is b The value of R b,M When obstacle V is seen to the left of the path, the radius of the avoidance circle Ab is set to R b The value of R b,M Larger R b,H When the moving body 100 encounters an obstacle V, the obstacle V blocks the view. In some cases, there is a risk that another moving body may appear from behind the obstacle V. Water traffic is generally on the right side of the road, so if there is room on the right side of the moving body 100 for other ships to pass, the risk of collision can be reduced.

[0140] The radius setter 210 of the avoidance circle Ab is configured to set the radius of the obstacle O in step S205 of FIG. b and the position and attitude {p, ^p} of the moving body 100 are input, and when the obstacle V is to the right of the course direction (attitude) of the moving body 100, the radius R b as the value of R b,M When the obstacle V is to the left of the course direction (attitude) of the moving body 100, the radius R b as the value of R b,H Output.

[0141] (Action and effect) As described above, the course control device 2 according to this embodiment adjusts the radius R of the avoidance circle Ab according to the position of the obstacle. b The radius setter 210 changes the radius R when the obstacle is visible to the left as seen from the moving body. b,H The value of the radius R b,M Set it to a value greater than the value of

[0142] In this way, the route control device 2 can reduce the risk of colliding with other moving bodies near obstacles, particularly in water traffic involving ships.

[0143] <Fifth embodiment> Hereinafter, the fifth embodiment will be described with reference to Figs. 26 to 28. Note that among the configurations of the fifth embodiment, the same configurations as those of the above-mentioned embodiments will be described using the same reference numerals as those of the above-mentioned embodiments. Some moving objects, such as fixed-wing aircraft and flying vehicles, cannot move backward. This embodiment discloses means for applying the technology described in the above-mentioned embodiments to such objects.

[0144] Fixed-wing aircraft and flying objects can generally slow down, even if they cannot move backward. In the third embodiment, the case where the opposing obstacle is a moving object has been described, but in the case where the opposing obstacle is a moving object, it is effective to slow down and wait for the obstacle to pass (slow down and avoid). Avoidance by slowing down can be applied not only to aircraft and flying objects, but also to moving objects that can move backward, such as ships and automobiles.

[0145] Setting a temporary waypoint {b, ^b} according to the method of the second embodiment to avoid an obstacle is also effective when a fixed-wing aircraft or flying object encounters an obstacle. In this case, if the path of the moving object violates the obstacle's avoidance circle, a reverse command (REV) is output from the path controller 203 according to the method of the second embodiment.

[0146] Since a moving object cannot change its speed instantaneously, it takes time to move forward in response to a reverse command, decelerate, stop, and then move backward. Therefore, immediately after a reverse command (REV) is issued, the moving object continues moving forward despite the command. This transitional forward path will be called a second temporary path. This embodiment is characterized by setting a second temporary waypoint to generate a second temporary path.

[0147] (Example of temporary route) FIG. 26 is a diagram showing an example of a temporary route according to the fifth embodiment. The radius of the second temporary waypoint is intentionally set small so that the moving body 100 can reach it by moving forward without violating the obstacle's avoidance circle. This avoidance circle with a small radius is called the second avoidance circle Ab2. The avoidance circle with the original radius Rb is called the first avoidance circle Ab1. The temporary waypoint setter 205 adjusts the radius of the second avoidance circle Ab2 to a value equal to or smaller than the radius of the first avoidance circle Ab1 based on the position of the moving body 100, the position of the obstacle Ob, and the rotation radius R of the moving body 100. Specifically, the rotation center of the moving body 100 is O p , the obstacle position is O b , the radius of rotation of the moving body 100 is R, so the radius R of the second avoidance circle Ab2 b2 is expressed as the following equation (13).

[0148]

number

[0149] Just as the temporary way point {b, ^b} is set on the circumference of the avoidance circle Ab in the second embodiment, the second temporary way point {b2, ^b2} is set on the circumference of the second avoidance circle Ab2.

[0150] Then, as shown in the following equation (14), the course direction of the second temporary route ^p b2 is determined by a Dubins path that generates a path only by moving forward, with the position and orientation {p, ^p} of the moving body 100 as the starting point and the second temporary waypoint {b2, ^b2} as the end point.

[0151]

number

[0152] The radius R of the second avoidance circle Ab2 is changed every control period until the moving body 100 changes from forward movement to backward movement. b2 , and recalculate the second temporary route. During this time, the moving object 100 moves forward along the latest second temporary route. bIf the moving body 100 changes from moving forward to moving backward, the moving body 100 is made to move backward along a temporary route based on the Reeds-Shepp path, as in the second embodiment.

[0153] If the own moving body 100 reduces its speed, the obstacle (opponent moving body) may pass through the original CPA (before deceleration) before the own moving body 100 starts to retreat. If the opponent moving body passes through the original CPA first as expected, O b O p As a result, the radius R of the second avoidance circle Ab2 is determined according to equation (13). b2 is the original radius R b When this happens, the second temporary route pb2 will no longer be useful, and the moving object 100 will move toward the first temporary waypoint {b1, ^b1} on the circumference of the first avoidance circle Ab1 along the original temporary route pb, as described in the second embodiment.

[0154] (Functional configuration of the mobile control device) Fig. 27 is a block diagram showing the functional configuration of a mobile control device according to the fifth embodiment. The speed of the mobile body 100 is switched from forward to backward in the following manner in the mobile control device 3 shown in Fig. 27.

[0155] When the command for the traveling direction is switched from forward (FWD) to reverse (REV), the speed controller 31 changes the speed command value u ref is reduced at a predetermined deceleration rate. The speed of the moving body 100 also follows this. The speed command value may be reduced in steps, but the change rate limiter 311 limits the deceleration rate to a value that the moving body 100 can follow, thereby preventing the moving body 100 from fluctuating unnecessarily. The set value of the speed command when moving forward is u ref,FWD The speed command setting for reversing is u ref,REV is.

[0156] Setback value u ref,REV is generally a negative value. For moving objects that do not allow retreat, such as fixed-wing aircraft and flying vehicles, u ref,REVSet a positive value for u. If you want to make a car stop and wait when crossing other cars, like a car on a public road, ref,REV Set u to zero. Cars are also allowed to back up in parking lots. In that case, u ref,REV may be changed over time. It may be set to zero immediately after the moving body 100 switches from moving forward to moving backward, and may be set to the original negative value after a predetermined time (for example, one minute) by a timer.

[0157] In this embodiment, whether the moving body 100 is allowed to move backward or not is determined by the set value u of the speed command for moving backward. ref,REV If retreat is allowed, u ref,REV Set a negative value for u. ref,REV Set to zero or a positive value.

[0158] The route control device 2 and the movement control device 3 of this embodiment may be applied to each of the above-described embodiments.

[0159] (Example of route control device processing) 28 is a flowchart showing an example of the process of the route control device according to the fifth embodiment. As in the above-described embodiments, while receiving an automatic navigation command (step S301; YES), the route control device 2 generates a route for the moving body 100 and outputs a course direction command ^p ref The route control device 2 repeatedly executes a process (steps S302 to S312) of outputting a command for the direction of travel (FWR / REV) to the mobile control device 3. When the automatic navigation command is stopped (step S301: NO), the route control device 2 ends the automatic navigation. Furthermore, while the mobile body 100 is in operation, the second measuring device 204 constantly performs a process of detecting obstacles in the direction of travel of the mobile body 100.

[0160] Each step during automatic navigation will be described below. The processing of steps S302 to S304 and S313 is the same as that of the second embodiment (steps S202 to S204 and S207 in FIG. 13), so a description thereof will be omitted.

[0161] In this embodiment, when an obstacle is present on the path (step S304; YES), the temporary way point setter 205 sets a first temporary way point {b1, ^b1} on the circumference of the first avoidance circle Ab1. The first temporary way point is the right way point {b 1,M ,^b 1,M} and left waypoint {b 1,H ,^b 1,H}. The method of determining the temporary waypoint is the same as in the second embodiment. In addition, the temporary waypoint setting unit 205 inputs the first temporary waypoint {b1, ^b1} to the arbitration unit 206. When the first temporary waypoint is input, the arbitration unit 206 determines whether the planned route is a route point {q r ,^q r}, the first temporary way point {b1, ^b1} (either the right way point or the left way point) is input to the route controller 203 as the next way point {q, ^q} (step S305).

[0162] When the first temporary waypoint is input, the course controller 203 sets the current position and attitude {p, ^p} of the mobile unit as the starting point and the position and attitude {b1, ^b1} of the first temporary waypoint as the end point, and calculates the course direction command ^p according to the Reeds-Shepp path. ref Then, the forward / reverse command (FWD or REV) is calculated (step S306).

[0163] Next, the temporary waypoint setter 205 calculates the radius Rb2 of the second avoidance circle Ab2 when the moving body 100 continues to move forward using the above equation (13). If the radius Rb2 of the second avoidance circle Ab2 is the same as the radius Rb of the first avoidance circle Ab1 (step S307; NO), the moving body 100 should not violate the first avoidance circle Ab1. Therefore, in this case, the temporary waypoint setter 205 does not calculate the second temporary waypoint. The course controller 203 calculates the course direction command ^p according to the Reeds-Shepp path. ref and a forward direction command (FWD) indicating forward movement are output to the movement control device 3 (step S311). These commands are calculated in step S306.

[0164] On the other hand, if the radius Rb2 of the second avoidance circle Ab2 is smaller than the radius Rb of the first avoidance circle Ab1 (step S307; YES), the temporary way point setter 205 determines whether the moving body 100 is moving forward (step S308).

[0165] If the moving object 100 is moving backward (step S308; NO), the moving object 100 should be able to move along the route defined by the Reeds-Shepp path. Therefore, in this case, the temporary waypoint setter 205 does not calculate the second temporary waypoint. The route controller 203 sets the command ^p for the route direction defined by the Reeds-Shepp path. ref A forward direction command (REV) indicating backward movement is output to the movement control device 3 (step S312). These commands are calculated in step S306.

[0166] Also, if the moving body 100 is moving forward (not switched to backward) (step S308; YES), the temporary way point setter 205 sets the second temporary way point {b2, ^b2} on the circumference of the second avoidance circle Ab2. The second temporary way point is the right way point {b 2,M ,^b 2,M} and left waypoint {b 2,H ,^b 2,H}. The method of determining the temporary waypoint is the same as in the second embodiment. In addition, the temporary waypoint setting unit 205 inputs the second temporary waypoint {b2, ^b2} to the arbitration unit 206. When the second temporary waypoint is input, the arbitration unit 206 determines whether the waypoint {q r ,^q r}, the second temporary way point {b2, ^b2} (either the right way point or the left way point) is input to the route controller 203 as the next way point {q, ^q} (step S309).

[0167] When the second temporary waypoint is input, the course controller 203 calculates the course direction command ^p by the Dubins path, with the current position and attitude {p, ^p} of the mobile unit as the start point and the position and attitude {b2, ^b2} of the second temporary waypoint as the end point. ref Then, a command to move backward (REV) is output to the movement control device 3 (step S310).

[0168] The route control device 2 repeatedly executes the processing of steps S301 to S313 for each control period.

[0169] (Action and effect) As described above, in the course control device 2 according to this embodiment, the temporary way point setter 205 sets the second avoidance circle Ab2, whose radius is variable, in addition to the avoidance circle (first avoidance circle Ab1) whose radius is fixed, and adjusts the radius Rb2 of the second avoidance circle Ab2 to be equal to or smaller than the radius Rb of the first avoidance circle Ab1 based on the position of the moving body 100, the position of the obstacle, and the rotation radius R of the moving body 100, and sets the second right way point {b 2,M ,^b 2,M} and the second left waypoint {b 2,H ,^b 2,H When the radius Rb2 of the second avoidance circle Ab2 is smaller than the radius Rb of the first avoidance circle Ab1 and the moving object 100 is moving forward, the path controller 203 calculates a path direction ^p that is calculated by restricting the moving direction of the moving object 100 to the forward direction based on the first arc Ap, the second arc (second avoidance circle Ab2), and a straight line L that is tangent to both the first arc Ap and the second arc Ab2. ref to the movement control device 3.

[0170] By doing this, even when it is difficult for the moving body 100 to move backward, the course control device 2 can cause the moving body 100 to avoid the obstacle while minimizing the intrusion of the obstacle into the avoidance circle Ab1.

[0171] In addition, the speed controller 31 of the movement control device 3 calculates the speed command value when the moving body 100 is allowed to move backward so that it becomes a negative value when the moving direction is backward, and calculates the speed command value when the moving direction is backward so that it becomes zero or a positive value when the moving body 100 is not allowed to move backward.

[0172] In this way, the command for the travel direction and course orientation including reverse according to the Reeds-Shepp path can be applied to both the mobile body that allows reverse and the mobile body that does not allow reverse.

[0173] <Other embodiments> Although one embodiment has been described in detail above with reference to the drawings, the specific configuration is not limited to the above, and various design modifications are possible. That is, in other embodiments, the order of the above-described processes may be changed as appropriate. Furthermore, some processes may be executed in parallel.

[0174] <Computer configuration> 29 is a schematic block diagram showing the configuration of a computer according to at least one embodiment. The computer 900 includes a processor 901, a main storage device 902, an auxiliary storage device 903, and an interface 904. The automatic steering device 1, the course control device 2, and the movement control device 3 described above are implemented in the computer 900. The operations of the above-described processing units are stored in the auxiliary storage device 903 in the form of a program. The processor 901 reads the program from the auxiliary storage device 903, loads it into the main storage device 902, and executes the above-described processing in accordance with the program. The processor 901 also allocates a storage area in the main storage device 902 to be used in the above-described processing in accordance with the program.

[0175] The program may be for realizing some of the functions to be performed by the computer 900. For example, the program may be combined with other programs already stored in the auxiliary storage device 903 or other programs implemented in other devices to perform the functions. In other embodiments, the computer may include a custom LSI (Large Scale Integrated Circuit) such as a PLD (Programmable Logic Device) in addition to or instead of the above configuration. Examples of PLDs include PAL (Programmable Array Logic), GAL (Generic Array Logic), CPLD (Complex Programmable Logic Device), and FPGA (Field Programmable Gate Array). In this case, some or all of the functions realized by the processor may be realized by the integrated circuit.

[0176] Examples of the auxiliary storage device 903 include a hard disk drive (HDD), a solid state drive (SSD), a magnetic disk, a magneto-optical disk, a compact disc read-only memory (CD-ROM), a digital versatile disc read-only memory (DVD-ROM), and a semiconductor memory. The auxiliary storage device 903 may be an internal medium directly connected to the bus of the computer 900, or an external medium (external storage device 910) connected to the computer 900 via the interface 904 or a communication line. Furthermore, when this program is distributed to the computer 900 via a communication line, the computer 900 that receives the program may load the program into the main storage device 902 and execute the above-described processing. In at least one embodiment, the auxiliary storage device 903 is a non-transitory tangible storage medium.

[0177] <Additional Notes> The above-described embodiment can be understood, for example, as follows.

[0178] (1) According to the first aspect, the route control device 2 includes a first measuring device 201 that measures the current position and attitude of the moving body 100, a waypoint setting device 202 that sets the positions of waypoints on the planned route and the attitude of the moving body at the waypoints, and a first arc Ap that includes the current position and attitude of the moving body 100 as a start point, a second arc Aq that includes the end point, and a straight line L that is tangent to both the first arc Ap and the second arc Aq, and calculates a traveling direction (FWD / REV) of the moving body indicating forward or backward movement and a course direction ^p that indicates the attitude of the moving body 100 when moving in the traveling direction so as to move along the shortest route from the start point to the end point. ref and a route controller 203 that calculates the distance between the moving object 100 and the target point and outputs the distance to the moving control device 3 that controls the movement of the moving object 100.

[0179] In this way, the course control device 2 calculates the shortest route for the moving object 100 using only arcs and straight lines, which significantly reduces the calculation load for the shortest route compared to conventional predictive model control. Therefore, unlike conventional technology, there is no need to provide a separate calculation device for generating the shortest route, and calculations can be performed by the calculation device (processor) of the automatic steering device 1. Furthermore, by generating a route that includes reverse movement, the course control device 2 can obtain a route that is shorter than conventional technology that only allows forward movement, allowing the moving object 100 to move efficiently.

[0180] (2) According to the second aspect, the course control device 2 according to the first aspect further includes a second measuring device 204 that detects the position of an obstacle on the course of the moving body 100, a temporary way point setter 205 that sets the position and attitude of a temporary right way point to avoid the obstacle to the right along the avoidance circle Ab, and the position and attitude of a temporary left way point to avoid the obstacle to the left, on the circumference of the avoidance circle Ab that is set to surround the obstacle based on the position of the obstacle, and an arbitrator 206 that outputs the position and attitude of any one of the way points, right way point, and left way point of the planned route to the course controller 203 as the position and attitude of the next way point.

[0181] In this way, the route control device 2 can generate a temporary route that can avoid an obstacle that appears on the route, and move the mobile body 100 along the temporary route, by the same processing as when moving the mobile body 100 along the planned route. This allows the mobile body 100 to avoid obstacles autonomously.

[0182] (3) According to a third aspect, the course control device 2 according to the second aspect further includes an avoidance circle setter 207 that sets an avoidance circle surrounding each of the plurality of obstacles based on the positions of the respective obstacles detected by the second measuring device 204, and a linked obstacle extractor 208 that determines a linked obstacle set by extracting elements where multiple avoidance circles overlap from a set whose elements are the plurality of obstacles. The temporary way point setter 205 sets the positions and orientations of a right way point and a left way point that avoid the plurality of obstacles based on the linked obstacle set.

[0183] In this way, even if there are multiple obstacles on the path of the moving body, the path control device 2 can generate a temporary path that can safely avoid these obstacles and move the moving body 100.

[0184] (4) According to the fourth aspect, in the course control device 2 relating to the third aspect, the temporary waypoint setter 205 determines the element whose line of sight direction when looking to the right of each of the multiple avoidance circles as seen from the moving body 100 as the right-hand obstacle based on the elements of the set of connected obstacles, and determines the element whose line of sight direction when looking to the left of each of the multiple avoidance circles as the left-hand obstacle, and sets the position and attitude of the right waypoint on the circumference of the avoidance circle of the right-hand obstacle, and sets the position and attitude of the left waypoint on the circumference of the avoidance circle of the left-hand obstacle.

[0185] In this way, the temporary way point setter 205 can limit the way points to only two points when avoiding multiple obstacles, even when there are multiple obstacles. This makes it possible to easily generate a route that can avoid multiple obstacles without increasing the calculation load.

[0186] (5) According to the fifth aspect, the course control device 2 relating to any one of the second to fourth aspects further includes a relative position predictor 209 that predicts the relative position of the moving body 100 and the other moving body that is the obstacle when they come closest to each other when the obstacle is a moving body, and the temporary waypoint setter 205 sets a temporary waypoint to avoid the other moving body based on the relative position.

[0187] In this way, even when the obstacle is not a stationary object but a moving object, the route control device 2 can quickly generate a route that can avoid the other moving object and move the own moving object 100.

[0188] (6) According to the sixth aspect, the course control device 2 according to any one of the second to fifth aspects further includes a radius setter 210 that changes the radius of the avoidance circle Ab based on whether the obstacle is to the right or left of the moving body 100.

[0189] In this way, the route control device 2 can reduce the risk of colliding with other moving bodies near obstacles, particularly in water traffic involving ships.

[0190] (7) According to the seventh aspect, in the course control device 2 relating to any one of the second to sixth aspects, the temporary way point setter 205 sets a second avoidance circle Ab2 having a variable radius in addition to the avoidance circle (first avoidance circle Ab1) having a fixed radius, and adjusts the radius Rb2 of the second avoidance circle Ab2 to be equal to or less than the radius Rb of the avoidance circle Ab1 based on the position of the moving body 100, the position of the obstacle, and the rotation radius R of the moving body 100, and further sets the positions and attitudes of a second right way point and a second left way point on the circumference of the second avoidance circle Ab2 that avoid the obstacle along the second avoidance circle Ab2. When the radius Rb2 of the second avoidance circle Ab2 is smaller than the radius Rb of the avoidance circle Ab1 and the moving object 100 is moving forward, the course controller 203 calculates a course direction ^p that is calculated by restricting the moving direction of the moving object 100 to the forward direction based on the first arc Ap, the second arc that is the second avoidance circle Ab2, and the straight line L that is tangent to both the first arc Ap and the second arc Ab2. ref is output to the movement control device 3.

[0191] By doing this, even when it is difficult for the moving body 100 to move backward, the course control device 2 can cause the moving body 100 to avoid the obstacle while minimizing the intrusion of the obstacle into the avoidance circle Ab1.

[0192] (8) According to an eighth aspect, an automatic steering device 1 includes a course control device 2 according to any one of the first to seventh aspects, and a movement control device 3 that controls a moving body 100 based on the traveling direction and course orientation output from the course control device 2. The movement control device 3 has a speed controller 31 that calculates a speed command value for the moving body 100 based on the traveling direction, and a motion controller 32 that calculates a rudder angle command and a thrust command for the moving body 100 based on the course orientation and the speed command value.

[0193] In this way, the automatic steering device 1 can quickly calculate an efficient path that allows forward and reverse movement with a small calculation load, and move the moving body 100.

[0194] (9) According to the ninth aspect, in the automatic steering device 1 relating to the eighth aspect, when the moving body 100 is allowed to reverse, the speed controller 31 calculates so that the speed command value when the direction of travel is reverse is a negative value, and when the moving body 100 is not allowed to reverse, the speed command value when the direction of travel is reverse is zero or a positive value.

[0195] In this way, commands for the direction of travel and course orientation, including reverse, can be applied to both mobile bodies that are permitted to reverse and mobile bodies that are not permitted to reverse.

[0196] (10) According to the tenth aspect, the path control method includes the steps of measuring the current position and attitude of the moving body 100, setting the positions of waypoints on the planned route and the attitude of the moving body 100 at the waypoints, and calculating a direction of travel of the moving body 100 indicating forward or backward movement and a course orientation indicating the attitude of the moving body 100 when traveling in the direction of travel, based on a first arc Ap including the starting point, a second arc Aq including the ending point, and a straight line L tangent to both the first arc Ap and the second arc Aq, so that the moving body 100 travels along the shortest path from the starting point to the ending point, and outputting the results to a movement control device 3 that controls the movement of the moving body 100.

[0197] (11) According to the eleventh aspect, the program causes the course control device 2 to execute the following steps: measuring the current position and attitude of the moving body 100; setting the positions of the waypoints of the planned route and the attitude of the moving body 100 at the waypoints; and calculating, based on the current position and attitude of the moving body 100 as a starting point and the position and attitude of the waypoints as an end point, a first arc Ap including the starting point, a second arc Aq including the end point, and a straight line L tangent to both the first arc Ap and the second arc Aq, a direction of travel of the moving body 100 indicating forward or backward movement and a course orientation indicating the attitude of the moving body 100 when traveling in the direction of travel, so as to move along the shortest path from the starting point to the end point, and outputting the results to the movement control device 3 that controls the movement of the moving body 100. [Explanation of symbols]

[0198] 100 Mobile 1. Automatic steering system 2 Route control device 201 First Measuring Instrument 202 Waypoint setting device 203 Route Control Device 204 Second Measuring Instrument 205 Temporary Waypoint Setter 206 Mediator 207 Avoidance circle setting device 208 Connected Obstacle Extractor 209 Relative Position Predictor 210 Radius setting device 3. Movement control device 31 Speed ​​controller 311 Change Rate Limiter 32 Motion Controller 321 Steering angle controller 322 Thrust Controller

Claims

1. a first measuring instrument for measuring the current position and orientation of a moving object; a waypoint setting device that sets the positions of waypoints on the planned route and the attitude of the moving object at the waypoints; a path controller that calculates a traveling direction of the moving body indicating forward or backward movement and a course orientation indicating the attitude of the moving body when traveling in the traveling direction, based on a first arc including the starting point, a second arc including the ending point, and a straight line tangent to both the first arc and the second arc, with the current position and orientation of the moving body as a starting point and the position and orientation of the waypoint as an ending point, so that the moving body will travel along the shortest path from the starting point to the ending point, and outputs the calculated course orientation to a mobile control device that controls the movement of the moving body; A route control device equipped with the above.

2. a second measuring device for detecting the position of an obstacle on the path of the moving object; a temporary way point setter that sets, on the circumference of an avoidance circle that is set based on the position of the obstacle so as to surround the obstacle, a position and orientation of a temporary right way point that avoids the obstacle to the right along the avoidance circle, and a position and orientation of a temporary left way point that avoids the obstacle to the left along the avoidance circle; an arbitrator that outputs a position and attitude at any one of the waypoint, the right waypoint, and the left waypoint of the planned route to the course controller as a position and attitude at a next waypoint; The route control device of claim 1 further comprising:

3. an avoidance circle setter that sets an avoidance circle surrounding each of the plurality of obstacles based on the positions of the plurality of obstacles detected by the second measuring device; a connected obstacle extractor that extracts elements where a plurality of the avoidance circles overlap from a set of the plurality of obstacles as elements to determine a connected obstacle set; Furthermore, the temporary way point setter sets positions and orientations of the right way point and the left way point for avoiding the plurality of obstacles based on the set of connected obstacles. The route control device according to claim 2.

4. The temporary waypoint setting device Based on the elements of the connected obstacle set, an element whose line of sight direction to the right of each of the plurality of avoidance circles as seen from the moving body is determined to be a right-side obstacle, and an element whose line of sight direction to the left of each of the plurality of avoidance circles is determined to be a left-side obstacle, setting the position and orientation of the right way point on the circumference of an avoidance circle for the right obstacle, and setting the position and orientation of the left way point on the circumference of an avoidance circle for the left obstacle; The route control device according to claim 3.

5. If the obstacle is a moving body, a relative position predictor is further provided for predicting a relative position of the other moving body when the moving body and the other moving body that is the obstacle come closest to each other, the temporary waypoint setting device sets a temporary waypoint for avoiding the other moving object based on the relative position; The route control device according to claim 2.

6. a radius setter that changes the radius of the avoidance circle based on whether the obstacle is to the right or left of the moving body; The route control device according to claim 2.

7. the temporary way point setter sets a second avoidance circle having a variable radius in addition to the avoidance circle having a fixed radius, adjusts the radius of the second avoidance circle to be equal to or less than the radius of the avoidance circle based on the position of the moving body, the position of the obstacle, and the radius of rotation of the moving body, and further sets positions and attitudes of a second right way point and a second left way point on the circumference of the second avoidance circle that avoid the obstacle along the second avoidance circle; When the radius of the second avoidance circle is smaller than the radius of the avoidance circle and the moving body is moving forward, the course controller outputs to the movement control device the course orientation calculated by restricting the moving body's direction of travel forward based on the first arc, the second arc which is the second avoidance circle, and the straight line tangent to both the first arc and the second arc. The route control device according to claim 2.

8. A route control device according to any one of claims 1 to 7; a mobile control device that controls the mobile object based on the traveling direction and the course orientation output from the course control device; Equipped with The movement control device includes: a speed controller that calculates a speed command value for the moving body based on the traveling direction; a motion controller that calculates a rudder angle command and a thrust command for the moving body based on the course direction and the speed command value; having Automatic steering device.

9. The speed controller is When the moving body is allowed to move backward, a speed command value when the moving direction is backward is calculated to be a negative value; When the moving body is not allowed to move backward, the speed command value when the moving direction is backward is calculated to be zero or a positive value. The automatic steering device according to claim 8.

10. measuring the current position and orientation of a moving object; setting positions of via points of a planned route and an attitude of the moving object at the via points; a step of calculating a direction of travel of the moving body indicating forward or backward movement and a course orientation indicating the attitude of the moving body when traveling in the direction of travel, based on a first arc including the starting point, a second arc including the ending point, and a straight line tangent to both the first arc and the second arc, with the current position and orientation of the moving body as a starting point and the position and orientation of the waypoint as an ending point, so that the moving body travels along the shortest path from the starting point to the ending point, and outputting the calculated direction of travel to a mobile control device that controls the movement of the moving body; A route control method comprising:

11. measuring the current position and orientation of a moving object; setting positions of via points of a planned route and an attitude of the moving object at the via points; a step of calculating a direction of travel of the moving body indicating forward or backward movement and a course orientation indicating the attitude of the moving body when traveling in the direction of travel, based on a first arc including the starting point, a second arc including the ending point, and a straight line tangent to both the first arc and the second arc, with the current position and orientation of the moving body as a starting point and the position and orientation of the waypoint as an ending point, so that the moving body travels along the shortest path from the starting point to the ending point, and outputting the calculated direction of travel to a mobile control device that controls the movement of the moving body; A program that causes the route control device to execute the above.

Images