JP2020192906A - Control method for moving body, control method, and program - Google Patents

Abstract

To provide a controller, a control method, and a program for a movable body capable of reducing possibility of collision with a terrain surface while obtaining stability in posture.SOLUTION: A correction target value is calculated by adding a target bias value which is adjusted based on a situation of inclination in a terrain surface to a basic target value of a control parameter when controlling a movable body having at least one of the altitude or stance angle to the terrain surface of the moving body as a control parameter. The moving body is controlled to that the control parameter becomes the correction target value.SELECTED DRAWING: Figure 3

Description

The present disclosure relates to mobile control devices, control methods and programs.

There are known mobiles that can automatically cruise underwater or in the air. In the operation control of such a moving body, it is necessary to prevent the moving body during cruising from colliding with the terrain surface by appropriately securing the altitude with respect to the terrain surface such as the bottom of the water or the ground.

The altitude of the moving object with respect to the terrain surface is detected by a sensor mounted on the moving object. For example, Patent Document 1 describes a motion control device for a navigation body that navigates underwater, in which a distance to the bottom of the water is measured by an altitude sonar mounted on the moving body, and the moving body is based on the measured distance. An example of performing advanced control is disclosed. In this document, in particular, by performing altitude control from the bottom of the navigator based on two distances from two different points on the bottom of the water to the navigator, compared to the case based on the distance to only one point on the bottom of the water. Therefore, it is said that fluctuations in the attitude and altitude deviation of the navigating body can be reduced.

Japanese Patent No. 5010332

The conventional altitude control of a moving body as in Patent Document 1 is carried out so that the measured altitude value detected by the sensor mounted on the moving body becomes the target altitude. The target altitude used for such altitude control is typically set in advance as a constant value. However, since the actual terrain surface includes various uneven shapes, the posture of the moving body cruising along the terrain surface becomes unstable depending on the shape of the terrain surface, and conversely, the stability of the posture of the moving body is ensured. If you try, the distance to the terrain surface may become closer than the target altitude, and the possibility of collision with the terrain surface may increase.

It is also conceivable to set a large margin included in the target altitude in order to reduce the possibility of collision, but the altitude of the moving object with respect to the terrain surface becomes excessive. In particular, when exploring a terrain surface by mounting a measuring device for the purpose of terrain exploration on a moving body, if the altitude of the moving body becomes excessive, the distance between the measuring device and the terrain surface becomes large, and the exploration It causes a decrease in accuracy.

At least one embodiment of the present invention has been made in view of the above circumstances, and provides a control device, a control method, and a program for a moving body capable of reducing the possibility of collision with a terrain surface while stabilizing the posture. The purpose is to do.

(1) The moving body control device according to at least one embodiment of the present invention is used to solve the above problems.
A control device for a moving body whose control parameter is at least one of the altitude or the attitude angle of the moving body with respect to the terrain surface.
An inclination estimation unit that estimates the inclination of the terrain surface and
A target bias value calculation unit that calculates a target bias value adjusted based on the inclination situation,
A correction target value calculation unit that calculates a correction target value by adding the target bias value to the basic target value of the control parameter, and a correction target value calculation unit.
A control unit that controls the moving body so that the control parameter becomes the correction target value,
To be equipped.

According to the configuration of (1) above, a correction target value calculated by adding a target bias value to a basic target value is used as a target value of a control parameter related to motion control of a moving body. The correction target value is variably set according to the shape of the terrain surface by including the target bias value adjusted based on the inclination of the terrain surface. This enables motion control that can reduce the possibility of collision with terrain surfaces while ensuring a stable posture with respect to various terrain surfaces.

(2) In some embodiments, in the configuration of (1) above,
The control parameter is the attitude angle.
The attitude angle is controlled depending on the altitude.

According to the configuration of (2) above, the posture angle is adjusted by the presence of the rudder only in the front or the rear of the moving body whose posture angle changes depending on the altitude (for example, in front of or behind the moving body body). In a moving body whose altitude needs to be changed in order to change it), it is possible to perform motion control capable of reducing the possibility of collision with the terrain surface while ensuring a stable posture with respect to various terrain surfaces.

(3) In some embodiments, in the configuration of (2) above,
Further provided with an altitude estimation unit for estimating the altitude of the moving body,
When the altitude estimated by the altitude estimation unit is lower than the basic target altitude, the target bias value calculation unit sets the sign of the target bias value positively, and the altitude estimated by the altitude estimation unit becomes If it is higher than the basic target altitude, the sign of the target bias value is set to negative.

According to the configuration of (3) above, when the estimated altitude of the moving object is lower than the basic target altitude, the correction target value is made larger than the basic target value by setting the target bias value positively. On the other hand, when the estimated altitude of the moving object is higher than the basic target altitude, the correction target value is made smaller than the basic target value by setting the target bias value to negative. By setting the sign of the target bias value based on the estimated altitude in this way, the altitude of the moving body can be made to follow the corrected target altitude with good responsiveness.

(4) In some embodiments, in the configuration of (2) or (3) above,
The target bias value calculation unit calculates the target bias value corresponding to the altitude estimated by the altitude estimation unit based on a characteristic function that defines the relationship between the absolute value of the target bias value and the altitude. ..

According to the configuration of (4) above, the target bias value is set variably according to the altitude of the moving body, so that the control target value (correction target value) is adjusted according to the altitude of the moving body from the terrain surface. It is possible to achieve a good balance between reducing the possibility of a moving object colliding with a terrain surface and stabilizing the posture.

(5) In some embodiments, in the configuration of (4) above,
The characteristic function is defined so that the absolute value of the target bias value decreases as the altitude increases.

According to the configuration of (5) above, the correction amount with respect to the basic correction value is reduced by reducing the absolute value of the target bias value as the altitude of the moving body increases. As a result, when the moving object is close to the terrain surface, the correction amount based on the target bias value is increased to preferentially reduce the possibility of collision with the terrain surface, while when the moving object is far from the terrain surface. Since the possibility of collision is low, it is possible to control with priority on stability by reducing the amount of correction by the target bias value.

(6) In some embodiments, in the configuration of (4) or (5) above,
The characteristic function is defined so that the absolute value of the target bias value becomes the maximum value when the altitude estimated by the altitude estimation unit is equal to or less than the lower limit of altitude.

According to the configuration of (6) above, when the altitude of the moving object approaches the terrain surface to the extent that it is below the lower limit of altitude, the absolute value of the target bias value is set to the maximum value to avoid collision with the terrain surface. Can be given top priority.

(7) In some embodiments, in any one of the above (4) to (6),
The target bias value calculation unit can be used as the characteristic function.
A first characteristic function in which the absolute value of the target bias value and the altitude show a linear characteristic,
A second characteristic function in which the absolute value of the target bias value with respect to the altitude is set larger than that of the first characteristic function.
A third characteristic function in which the absolute value of the target bias value with respect to the altitude is set smaller than that of the first characteristic function.
Any one of them can be selected.

According to the configuration of (7) above, by configuring a plurality of characteristic functions having different characteristics to be selectable, more flexible operation control can be performed according to the situation of the moving body.

(8) In some embodiments, in the configuration of (7) above,
The target bias value calculation unit
When the slope is zero, the first characteristic function is selected as the characteristic function.
If the slope is greater than zero, the second characteristic function is selected as the characteristic function.
If the slope is less than zero, the third characteristic function is selected as the characteristic function.

According to the configuration of (8) above, by selecting the characteristic function according to the inclination of the terrain surface, it is possible to stabilize the posture of the moving body while reducing the possibility of the moving body colliding with the terrain surface. it can.

(9) In some embodiments, in the configuration of (1) above,
The attitude angle and the altitude can be controlled independently.

According to the configuration of (9) above, there are various types of moving bodies in which the posture angle and altitude of the moving body can be controlled independently (for example, a moving body capable of generating thrust in the vertical and vertical directions by providing a thruster or the like). It is possible to control the motion to reduce the possibility of collision with the terrain surface while ensuring a stable posture with respect to the terrain surface.

(10) In some embodiments, in the configuration of (9) above,
The target bias value calculation unit calculates a target altitude bias value corresponding to the altitude based on the inclination, and a target attitude angle bias value corresponding to the attitude angle based on the inclination. Including the target attitude angle bias value calculation unit to be calculated
The correction target value calculation unit includes a correction target altitude value calculation unit that calculates a correction target altitude value by adding the target altitude bias value to the basic altitude target value, and the target attitude angle to the basic attitude angle target value. Includes a correction target attitude angle value calculation unit that calculates a correction target attitude angle value by adding a bias value.
The control unit controls the moving body so that the altitude becomes the correction target altitude value and the posture angle becomes the correction target posture angle value.

According to the configuration of (10) above, by controlling the altitude and posture angle of the moving body to the correction target altitude value and the correction posture angle target value, respectively, while ensuring a stable posture on various terrain surfaces. , Motion control that can reduce the possibility of collision with the terrain surface becomes possible.

(11) The method for controlling a moving body according to at least one embodiment of the present invention is to solve the above problems.
A method for controlling a moving body in which at least one of the altitude or the attitude angle with respect to the terrain surface of the moving body is used as a control parameter.
The step of estimating the slope of the terrain surface and
The step of calculating the target bias value adjusted based on the inclination situation, and
A step of calculating a correction target value by adding the target bias value to the basic target value of the control parameter, and
A step of controlling the moving body so that the control parameter becomes the correction target value, and
To be equipped.

According to the method (11) above, a correction target value calculated by adding a target bias value to a basic target value is used as a target value of a control parameter related to motion control of a moving body. The correction target value is variably set according to the shape of the terrain surface by including the target bias value adjusted based on the inclination of the terrain surface. This enables motion control that can reduce the possibility of collision with terrain surfaces while ensuring a stable posture with respect to various terrain surfaces.

(12) In order to solve the above problems, the program according to at least one embodiment of the present invention
A program for controlling a moving body whose control parameter is at least one of the altitude or the attitude angle of the moving body with respect to the terrain surface.
On the computer
The step of estimating the slope of the terrain surface and
The step of calculating the target bias value adjusted based on the inclination situation, and
A step of calculating a correction target value by adding the target bias value to the basic target value of the control parameter, and
A step of controlling the moving body so that the control parameter becomes the correction target value, and
To execute.

According to the program (12) above, a correction target value calculated by adding a target bias value to a basic target value is used as a target value of a control parameter related to motion control of a moving body. The correction target value is variably set according to the shape of the terrain surface by including the target bias value adjusted based on the inclination of the terrain surface. This enables motion control that can reduce the possibility of collision with terrain surfaces while ensuring a stable posture with respect to various terrain surfaces.

According to at least one embodiment of the present invention, it is possible to provide a control device, a control method and a program for a moving body capable of reducing the possibility of collision with a terrain surface while stabilizing the posture.

It is a schematic diagram which shows the mode that the moving body which concerns on 1st Embodiment moves. It is a schematic block diagram which shows the hardware configuration of the control device of FIG. It is a block diagram which functionally shows the internal structure of the control device of FIG. It is a flowchart which shows an example of the sign setting method of a target attitude angle bias value. This is an example of a characteristic function that defines the relationship between the absolute value of the target attitude angle bias value and the altitude. It is a flowchart which shows an example of the absolute value setting method of a target attitude angle bias value. It is a schematic diagram which shows the locus with respect to the topographical surface of the moving body which concerns on 1st Embodiment. It is a block diagram which shows the internal structure of the control device of the moving body which concerns on 2nd Embodiment.

Hereinafter, some embodiments of the present invention will be described with reference to the accompanying drawings. However, the dimensions, materials, shapes, relative arrangements, etc. of the components described as embodiments or shown in the drawings are not intended to limit the scope of the present invention to this, but are merely explanatory examples. Absent.

(First Embodiment)
FIG. 1 is a schematic view showing how the moving body 1 according to the first embodiment moves. The moving body 1 is an underwater moving body that can automatically move in water. The movement control of the moving body 1 is automatically performed by a control device described later, and a predetermined altitude and posture angle are maintained with respect to the topographical surface 2 which is the bottom of the water. In the present embodiment, an underwater mobile body that travels in water is described as an example of the moving body 1, but there is no particular description about an aerial moving body (flying body) that can automatically move in the air on the ground. The same is true as far as it goes. Further, the moving body 1 may be manned or unmanned.

The use of the moving body 1 is not limited, but the moving body 1 of the present embodiment is equipped with a measuring device 4 for measuring the topographical surface 2 for the purpose of topographical exploration. The measuring device 4 is, for example, a sonar device, which can measure information about the terrain surface 2 by irradiating a sound wave from a moving body 1 moving in water toward the terrain surface 2 and receiving a reflection component from the terrain surface 2. It is composed.

The moving body 1 includes a main body 1a and a thrust generator 1b provided on the main body 1a. The thrust generator 1b is, for example, a screw, and generates a thrust for the moving body 1 to move. The thrust generator 1b is controlled by the control device 10 housed in the main body 1a, so that the movement control of the moving body 1 is realized. In the moving body 1 of FIG. 1, since the thrust generator 1b is provided behind the main body 1a, the attitude angle α and the altitude H from the terrain surface depend on each other (that is, when the attitude angle α is to be changed). The altitude H also changes, and the attitude angle α also changes when trying to change the altitude H).

Although the pitch angle is illustrated in FIG. 1, the posture angle α may be a roll angle or a yaw angle.

FIG. 2 is a schematic configuration diagram showing a hardware configuration of the control device 10 of FIG. As shown in FIG. 2, the control device 10 may be configured by a computer. Specifically, as shown in FIG. 2, the control device 10 includes a CPU 11 (Central Processing Unit), a RAM 12 (Random Access Memory), a ROM 13 (Read Only Memory), an HDD 14 (Hard Disk Drive), an input I / F15, and an input I / F15. And outputs I / F16, which are configured to be connected to each other via a bus 17.
The hardware configuration of the control device 10 is not limited to the above, and may be configured by a combination of a control circuit and a storage device.

Further, a program (movement control program) for realizing the function of the control device 10 may be stored in the cloud or a storage medium. The control device 10 is provided with an external communication device such as a 4G or 5G line communication device or a wireless LAN communication device such as Wi-Fi (registered trademark), and the CPU 11 reads a program from the cloud via the external communication device. It may be loaded into RAM 12 and executed. Further, the control device 10 may include a driver for reading the data of the storage medium, and the CPU 11 may read the program from the storage medium, load the program into the RAM 12, and execute the program. Regardless of the type of storage medium, various storage media such as an SD card, a USB memory, and an external HDD can be used according to the capacity of the program.

FIG. 3 is a block diagram functionally showing the internal configuration of the control device 10 of FIG. The control device 10 includes an altitude estimation unit 20, a basic target attitude angle calculation unit 22, an inclination estimation unit 24, a target attitude angle bias value calculation unit 26 (target bias value calculation unit), and a correction target attitude angle calculation unit 28. (Correction target value calculation unit), posture angle control unit 30 (control unit), and posture angle detection unit 32 are provided.

The altitude estimation unit 20 estimates the altitude (altitude estimation value Hs) of the moving body 1 with respect to the terrain surface 2 based on various sensors (not shown) provided on the moving body 1. In the present embodiment, since the moving body 1 is equipped with the measuring device 4 which is a sonar device for the purpose of terrain exploration, the altitude estimation unit 20 acquires the measurement result of the measuring device 4 to obtain the altitude estimated value Hs. May be sought.

The basic target attitude angle calculation unit 22 calculates the basic target attitude angle αbt, which is the basic target value of the attitude angle α, which is the control parameter of the control device 10. As described above, in the moving body 1 of FIG. 1, the altitude H and the posture angle α are in a mutually dependent relationship. In view of such characteristics of the moving body 1, the basic target attitude angle calculation unit 22 has a target altitude Ht (for example, input by an operator) input from the outside and an altitude estimated value Hs estimated by the altitude estimation unit 20. The basic target attitude angle αbt is calculated based on the difference ΔH with. The relationship between the difference ΔH and the basic target posture angle αbt is defined in advance as, for example, a map, and the basic target posture angle calculation unit 22 basically corresponds to the input difference ΔH by referring to the map or the like. The target attitude angle αbt is output.
The relationship between the difference ΔH and the basic target posture angle αbt is obtained in advance by an experimental or simulation method, and may be configured to be appropriately readable from a storage device (not shown).

The inclination estimation unit 24 estimates the inclination (inclination estimation value Ls) of the terrain surface 2 through which the moving body 1 passes. The position of the terrain surface 2 whose slope is estimated by the slope estimation unit 24 may be, for example, a vertically downward position of the moving body 1 or a recent position from the moving body 1. In the present embodiment, since the moving body 1 is equipped with the measuring device 4 which is a sonar device for the purpose of topographical exploration, the tilt estimation unit 24 acquires the measurement result of the measuring device 4 to obtain the tilt estimation value Ls. May be sought.

The target attitude angle bias value calculation unit 26 (target bias value calculation unit) calculates the target attitude angle bias value αb (target bias value) to be added to the basic target attitude angle αbt. The target attitude angle bias value αb is adjusted based on the situation of the altitude estimation value Hs input from the altitude estimation unit 20 and the inclination estimation value Ls input from the inclination estimation unit 24, so that the moving body 1 The control target value of the attitude angle α can be variably set according to the altitude H and the inclination of the terrain surface 2.

The correction target posture angle calculation unit 28 (correction target calculation unit) corrects the basic target posture angle αbt calculated by the basic target posture angle calculation unit 22 to correct the correction target which is the final target value of the posture angle α. The posture angle αt (correction target value) is calculated. Specifically, with respect to the basic target attitude angle αbt, the inclination estimation value Ls calculated by the inclination estimation unit 24 and the target attitude angle bias value αb (target bias) calculated by the target attitude angle bias value calculation unit 26. The final correction target attitude angle αt (correction target value) is calculated by adding the values).

The posture angle control unit 30 (control unit) has a posture angle control amount Sα based on the difference Δα between the correction target posture angle αt (correction target value) and the posture angle actual measurement value αd detected by the posture angle detection unit 32. Is output to control the moving body 1 (thrust generator 1b). For example, the relationship between the difference Δα and the attitude angle control amount Sα is defined in advance as a map or the like, and the attitude angle control unit 30 (control unit) obtains the input difference Δα by referring to the map or the like. The corresponding attitude angle control amount Sα is output.
The relationship between the difference Δα and the attitude angle control amount Sα is obtained in advance by an experimental or simulation method, and may be configured to be appropriately readable from a storage device (not shown).

In such a control device 10, the target posture angle bias value αb (target bias value) is set with respect to the basic target posture angle αbt (basic target value) as the target value of the posture angle α which is a control parameter related to the motion control of the moving body 1. ) Is added and the correction target attitude angle αt (correction target value) is used. The correction target attitude angle αt (correction target value) corresponds to the shape of the terrain surface 2 by including the target attitude angle bias value αb (target bias value) calculated based on the inclination estimated value Ls of the terrain surface 2. Is set to be variable. As a result, it is possible to perform motion control capable of reducing the possibility of collision with the terrain surface 2 while ensuring a stable posture with respect to various terrain surfaces 2.

Next, a method of setting the target posture angle bias value αb will be described in detail. First, a method of setting the sign of the target posture angle bias value αb will be described with reference to FIG. FIG. 4 is a flowchart showing an example of a code setting method for the target attitude angle bias value αb.

The altitude estimation unit 20 calculates the altitude estimated value Hs of the moving body 1 (step S10). Then, the target attitude angle bias value calculation unit 26 compares the altitude estimated value Hs calculated in step S10 with the basic target altitude Hb (step S11). Here, the basic target altitude Hb is a threshold value preset as an appropriate standard value of the altitude of the moving body 1 with respect to the terrain surface 2, and is determined based on, for example, input from the operator.

When the altitude estimated value Hs is lower than the basic target altitude Hb (Hs <Hb), the target attitude angle bias value calculation unit 26 sets the sign of the target attitude angle bias value αb positively (step S12). As a result, when the altitude of the moving body 1 is less than the basic target altitude Hb, that is, when the moving body 1 is closer than the standard value to the terrain surface 2, the target attitude angle bias value having a positive sign is obtained. By adding αb to the basic target posture angle αbt, a corrected target posture angle αt larger than the basic target posture angle αbt can be obtained. By performing the movement control according to the basic target posture angle αbt obtained in this way, it is urged to correct the altitude of the moving body 1 which is closer than the standard value to the terrain surface 2 so as to be closer to the standard value. Can be done.

On the other hand, when the altitude estimation value Hs is larger than the basic target altitude Hb (Hs> Hb), the target attitude angle bias value calculation unit 26 sets the sign of the target attitude angle bias value αb to be negative (step S13). As a result, when the altitude of the moving body 1 exceeds the basic target altitude Hb, that is, the state is far from the standard value with respect to the terrain surface 2, the target attitude angle bias value αb having a negative sign is set. By adding to the basic target posture angle αbt, a corrected target posture angle αt smaller than the basic target posture angle αbt can be obtained. By performing the movement control according to the basic target posture angle αbt obtained in this way, it is possible to urge the correction so that the altitude of the moving body 1 exceeding the standard value with respect to the terrain surface 2 approaches the standard value. it can.

When the estimated altitude value Hs is equal to the basic target altitude Hb (Hs = Hb), the target attitude angle bias value calculation unit 26 may set the target attitude angle bias value αb to zero (step S14). As a result, when the altitude of the moving body 1 is in a state of matching the basic target altitude Hb, the altitude of the moving body 1 can be properly maintained as it is by not performing the correction by the target posture angle bias value αb. ..

As described above, in the present embodiment, the sign of the target posture angle bias value αb to be added to the basic target posture angle αbt is set based on the magnitude of the altitude estimated value Hs. As a result, the posture of the moving body 1 can be stabilized while reducing the possibility of collision by appropriately correcting the altitude of the moving body 1 with respect to the terrain surface 2 having various irregularities.

Next, a method of setting the absolute value of the target posture angle bias value αb will be described. The target attitude angle bias value calculation unit 26 corresponds to the altitude estimation value Hs estimated by the altitude estimation unit 20 based on the characteristic function that defines the relationship between the absolute value | αb | of the target attitude angle bias value and the altitude H. The absolute value | αb | of the target bias value to be performed may be calculated. As a result, the target posture angle bias value αb can be variably set according to the altitude H of the moving body 1, and the control target value (corrected posture angle target value αt) is set according to the altitude of the moving body 1 from the terrain surface 2. ) Can be adjusted to satisfactorily balance the reduction of the possibility of the moving body 1 colliding with the terrain surface 2 and the stabilization of the posture.

Here, FIG. 5 is an example of a characteristic function that defines the relationship between the absolute value | αb | of the target attitude angle bias value and the altitude H. In FIG. 5, the first characteristic function f1, the second characteristic function f2, and the third characteristic function f3 are shown. The target posture angle bias value calculation unit 26 is configured so that the characteristic function used for calculating the target posture angle bias value αb can be appropriately selected from these characteristic functions. Specifically, the second characteristic function f2 is a characteristic function that calculates a larger bias value than the first characteristic function f1. Similarly, the third characteristic function f3 is a characteristic function that calculates the bias value smaller than that of the first characteristic function f1. The first characteristic function f1, the second characteristic function f2, and the third characteristic function f3 are not limited to one type of function as shown in FIG. 5, and a plurality of each may be prepared. For example, within the range of the above-mentioned features, a plurality of characteristic functions may be prepared and configured to be selectable based on the speed and control mode of the moving body 1.

The first characteristic function f1, the second characteristic function f2, and the third characteristic function f3 are defined so that the absolute value | αb | of the target attitude angle bias value decreases as the altitude H increases. That is, these characteristic functions reduce the amount of correction for the basic target posture angle αbt by reducing the absolute value | αb | of the target posture angle bias value as the altitude H of the moving body 1 increases. As a result, when the moving body 1 is close to the terrain surface 2, the correction amount by the target attitude angle bias value αb is increased to preferentially reduce the possibility of collision with the terrain surface 2, while the moving body 1 When is far from the terrain surface 2, the possibility of collision is low. Therefore, by reducing the amount of correction by the target attitude angle bias value αb, control with priority on stability becomes possible.

Further, in the first characteristic function f1, the second characteristic function f2, and the third characteristic function f3, when the altitude H is Hmin below the altitude lower limit value, the absolute value | αb | of the target attitude angle bias value becomes the maximum value. Is regulated. As a result, when the altitude H of the moving body 1 approaches the terrain surface 2 to the extent that it is below the lower limit value Hmin, the absolute value | αb | of the target attitude angle bias value is set to the maximum value to reach the terrain surface 2. Collision avoidance can be given top priority.

Further, in the first characteristic function f1, the second characteristic function f2, and the third characteristic function f3, when the altitude H is equal to or higher than the altitude upper limit value, the absolute value | αb | of the target attitude angle bias value becomes the minimum value (zero). Is stipulated to be. As a result, when the altitude H of the moving body 1 is separated from the terrain surface 2 to the extent that it exceeds the upper limit value Hmax, the absolute value | αb | of the target attitude angle bias value is set to the minimum value to deviate from the planned route. Can be suppressed.

The first characteristic function f1 is defined so that the absolute value | αb | of the target attitude angle bias value and the altitude H exhibit linear characteristics. In the second characteristic function f2, the absolute value | αb | of the target attitude angle bias value with respect to the altitude H is set larger than that in the first characteristic function f1. The third characteristic function f3 is set so that the absolute value | αb | of the target attitude angle bias value with respect to the altitude H is smaller than that of the first characteristic function f1. By configuring a plurality of characteristic functions having different characteristics to be selectable in this way, more flexible operation control can be performed according to the situation of the moving body 1.

The first characteristic function f1, the second characteristic function f2, and the third characteristic function f3 described above are selected based on, for example, the inclination estimation value Ls calculated by the inclination estimation unit 24. FIG. 6 is a flowchart showing an example of an absolute value setting method of the target posture angle bias value αb.

First, the inclination estimation unit 24 calculates the inclination estimation value Ls of the moving body 1 (step S20). Then, the target attitude angle bias value calculation unit 26 determines the sign of the tilt estimation value Ls calculated in step S20 (step S21). For example, when the tilt estimation value Ls is zero (Ls = 0), the first characteristic function f1 is selected as the characteristic function (step S22), and the absolute value of the target attitude angle bias value αb is used using the first characteristic function f1. Is calculated (step S23). When there is no inclination of the terrain surface 2 in this way, stable altitude control can be achieved by selecting the characteristic function f1 which has a standard linear characteristic as compared with the second characteristic function f2 and the third characteristic function f3. It will be possible.

On the other hand, when the tilt estimation value Ls is larger than zero (Ls> 0), the second characteristic function f2 is selected as the characteristic function (step S24), and the target attitude angle bias value αb is absolute using the second characteristic function f2. The value is calculated (step S23). The second characteristic function f2 is set to have a larger absolute value | αb | of the target attitude angle bias value with respect to the altitude H than the standard first characteristic function f1. Therefore, when the terrain surface 2 has an uphill slope, the attitude angle α can be easily directed in the positive direction by selecting the second characteristic function f2.

When the tilt estimation value Ls is smaller than zero (Ls <0), the third characteristic function f3 is selected as the characteristic function (step S25), and the absolute value of the target attitude angle bias value αb is used using the third characteristic function f3. Is calculated (step S23). The third characteristic function f3 is set so that the absolute value | αb | of the target attitude angle bias value with respect to the altitude H is smaller than that of the standard first characteristic function f1. Therefore, when the terrain surface 2 has a downward slope, if the attitude angle α is excessively directed downward, the possibility of collision with the terrain surface 2 increases. Therefore, by selecting the second characteristic function f2, this is done. The possibility of collision can be effectively reduced.

By setting the sign and absolute value of the target attitude angle bias value αb based on the altitude estimated value Hs and the inclination estimated value Ls in this way, the terrain can be secured while ensuring a stable attitude with respect to various terrain surfaces 2. Motion control that can reduce the possibility of collision with the surface 2 becomes possible. For example, as shown in FIG. 7, the locus of the moving body 1 with respect to the terrain surface 2 including unevenness is clear as compared with FIG. 1, and the posture angle changes while securing the altitude within the range where the minimum altitude can be secured. Stable operation is realized with less.

(Second Embodiment)
Subsequently, the moving body 1 according to the second embodiment will be described. The moving body 1 according to the second embodiment includes a main body 1a and a thrust generating device 1b as in the moving body 1 according to the first embodiment described above, but the thrust generating device 1b for example applies thrust in the vertical vertical direction. It is a thruster that can be generated, and differs in that the attitude angle and altitude can be controlled independently.

FIG. 8 is a block diagram showing an internal configuration of the control device 10 of the mobile body 1 according to the second embodiment. In the control device 10, the control device 10 includes an altitude estimation unit 20, an inclination estimation unit 24, a target altitude bias value calculation unit 40 (target bias value calculation unit), and a correction target altitude calculation unit 42 (correction target value calculation unit). ), The altitude control unit 44 (control unit), the target attitude angle bias value calculation unit 46 (target bias value calculation unit), the correction target attitude angle calculation unit 48 (correction target value calculation unit), and the attitude angle estimation unit. 50 and a posture angle control unit 52 (control unit) are provided.

The target altitude bias value calculation unit 40 (target bias value calculation unit) calculates, for example, the target altitude bias value Hb (target bias value) to be added to the basic target altitude Hbt input by the operator. The target altitude bias value Hb (target bias value) is calculated based on the inclination estimation value Ls input from the inclination estimation unit 24. As a result, the control target value of the altitude H can be variably set according to the inclination of the terrain surface 2.

The correction target altitude calculation unit 42 (correction target value calculation unit) has a target altitude bias value Hb (target bias value) calculated by the target altitude bias value calculation unit 40 (target bias value calculation unit) with respect to the basic target altitude Hbt. ) Is added to calculate the correction target altitude Ht (correction target value), which is the final target value of the altitude H.

The altitude control unit 44 (control unit) outputs the altitude control amount SH based on the difference ΔH between the correction target altitude Ht (correction target value) and the altitude estimated value Hs estimated by the altitude estimation unit 20. , Control the moving body 1 (thrust generator 1b). For example, the relationship between the difference ΔH and the altitude control amount SH is defined in advance as a map or the like, and the altitude control unit 44 (control unit) corresponds to the input difference ΔH by referring to the map or the like. The altitude control amount SH is output.
The relationship between the difference ΔH and the altitude control amount SH may be obtained in advance by an experimental or simulation method, and may be configured to be appropriately readable from a storage device (not shown).

The target attitude angle bias value calculation unit 46 (target bias value calculation unit) calculates, for example, the target attitude angle bias value αb (target bias value) to be added to the basic target attitude angle αbt input by the operator. The target attitude angle bias value αb (target bias value) is calculated based on the tilt estimation value Ls input from the tilt estimation unit 24. As a result, the control target value of the posture angle α can be variably set according to the inclination of the terrain surface 2.

The correction target attitude angle calculation unit 48 (correction target value calculation unit) has the inclination estimation value Ls calculated by the inclination estimation unit 24 and the target attitude angle bias value calculation unit 46 (target) with respect to the basic target attitude angle αbt. The target attitude angle bias value αb (target bias value) calculated by the bias value calculation unit) is corrected to correct the correction target attitude angle αt (correction target value), which is the final target value of the attitude angle α. calculate.

The posture angle estimation unit 50 estimates the posture angle (posture angle estimated value αs) of the moving body 1 with respect to the terrain surface 2 based on various sensors (not shown) provided on the moving body 1. In the present embodiment, since the moving body 1 is equipped with a measuring device 4 which is a sonar device for the purpose of terrain exploration, the posture angle estimation unit 50 estimates the posture angle by acquiring the measurement result of the measuring device 4. The value αs may be obtained.

The posture angle control unit 52 (control unit) has a posture angle control amount Sα based on the difference Δα between the correction target posture angle αt (correction target value) and the posture angle estimated value αs estimated by the posture angle estimation unit 50. Is output to control the moving body 1 (thrust generator 1b). For example, the relationship between the difference Δα and the attitude angle control amount Sα is defined in advance as a map or the like, and the attitude angle control unit 52 (control unit) obtains the input difference Δα by referring to the map or the like. The corresponding attitude angle control amount Sα is output.
The relationship between the difference Δα and the attitude angle control amount Sα is obtained in advance by an experimental or simulation method, and may be configured to be appropriately readable from a storage device (not shown).

As described above, in the second embodiment, the control is performed according to each correction target value obtained by correcting the basic target values of the altitude H and the posture angle α with the bias values. As a result, even in the moving body 1 in which the posture angle α and the altitude H of the moving body 1 can be controlled independently, a stable posture is secured for various terrain surfaces as in the first embodiment described above. At the same time, motion control that can reduce the possibility of collision with the terrain surface becomes possible.

Further, the target altitude bias value αH and the target attitude angle bias value αb in the second embodiment are variably set in sign and absolute value based on the tilt estimation value Ls, similarly to the target attitude angle bias value αb in the first embodiment. By doing so, it is possible to perform motion control capable of reducing the possibility of collision with the terrain surface 2 while ensuring a stable posture with respect to various terrain surfaces 2.

As described above, according to each of the above-described embodiments, it is possible to provide a control device, a control method, and a program for a moving body that can reduce the possibility of collision with a terrain surface while stabilizing the posture.

At least one embodiment of the present invention is available for mobile control devices, control methods and programs.

1 Moving body 1a Main body 1b Thrust generator 2 Topographic surface 4 Measuring device 10 Control device 20 Altitude estimation unit 22 Basic target attitude angle calculation unit 24 Tilt estimation unit 26 Target attitude angle bias value calculation unit 28 Correction target attitude angle calculation unit 30 Posture Angle control unit 32 Posture angle detection unit 40 Target altitude bias value calculation unit 42 Correction target altitude calculation unit 44 Height control unit 50 Posture angle estimation unit

Claims (12)

A control device for a moving body whose control parameter is at least one of the altitude or the attitude angle of the moving body with respect to the terrain surface.
An inclination estimation unit that estimates the inclination of the terrain surface and
A target bias value calculation unit that calculates a target bias value adjusted based on the inclination situation,
A correction target value calculation unit that calculates a correction target value by adding the target bias value to the basic target value of the control parameter, and a correction target value calculation unit.
A control unit that controls the moving body so that the control parameter becomes the correction target value,
A moving body control device. The control parameter is the attitude angle.
The moving body control device according to claim 1, wherein the posture angle is controlled depending on the altitude. Further provided with an altitude estimation unit for estimating the altitude of the moving body,
When the altitude estimated by the altitude estimation unit is lower than the basic target altitude, the target bias value calculation unit sets the sign of the target bias value positively, and the altitude estimated by the altitude estimation unit is set to positive. The moving body control device according to claim 2, wherein the sign of the target bias value is set negative when the altitude is higher than the basic target altitude. The target bias value calculation unit calculates the target bias value corresponding to the altitude estimated by the altitude estimation unit based on a characteristic function that defines the relationship between the absolute value of the target bias value and the altitude. , The moving body control device according to claim 2 or 3. The mobile control device according to claim 4, wherein the characteristic function is defined so that the absolute value of the target bias value decreases as the altitude increases. The characteristic function according to claim 4 or 5, wherein the characteristic function is defined so that the absolute value of the target bias value becomes the maximum value when the altitude estimated by the altitude estimation unit is equal to or less than the lower limit of altitude. Control device for moving objects. The target bias value calculation unit can be used as the characteristic function.
A first characteristic function in which the absolute value of the target bias value and the altitude show a linear characteristic,
A second characteristic function in which the absolute value of the target bias value with respect to the altitude is set larger than that of the first characteristic function.
A third characteristic function in which the absolute value of the target bias value with respect to the altitude is set smaller than that of the first characteristic function.
The moving body control device according to any one of claims 4 to 6, wherein any one of the following can be selected. The target bias value calculation unit
When the slope is zero, the first characteristic function is selected as the characteristic function.
If the slope is greater than zero, the second characteristic function is selected as the characteristic function.
The control device for a moving body according to claim 7, wherein when the inclination is smaller than zero, the third characteristic function is selected as the characteristic function. The moving body control device according to claim 1, wherein the posture angle and the altitude can be controlled independently. The target bias value calculation unit calculates a target altitude bias value corresponding to the altitude based on the inclination, and a target attitude angle bias value corresponding to the attitude angle based on the inclination. Including the target attitude angle bias value calculation unit to be calculated
The correction target value calculation unit includes a correction target altitude value calculation unit that calculates a correction target altitude value by adding the target altitude bias value to the basic altitude target value, and the target attitude angle to the basic attitude angle target value. Includes a correction target attitude angle value calculation unit that calculates a correction target attitude angle value by adding a bias value.
The control device for a moving body according to claim 9, wherein the control unit controls the moving body so that the altitude becomes the corrected target altitude value and the posture angle becomes the corrected target posture angle value. A method for controlling a moving body in which at least one of the altitude or the attitude angle with respect to the terrain surface of the moving body is used as a control parameter.
The step of estimating the slope of the terrain surface and
The step of calculating the target bias value adjusted based on the inclination situation, and
A step of calculating a correction target value by adding the target bias value to the basic target value of the control parameter, and
A step of controlling the moving body so that the control parameter becomes the correction target value, and
A method of controlling a moving body. A program for controlling a moving body whose control parameter is at least one of the altitude or the attitude angle of the moving body with respect to the terrain surface.
On the computer
The step of estimating the slope of the terrain surface and
The step of calculating the target bias value adjusted based on the inclination situation, and
A step of calculating a correction target value by adding the target bias value to the basic target value of the control parameter, and
A step of controlling the moving body so that the control parameter becomes the correction target value, and
A program that runs.

Images