JPH0562764B2 - - Google Patents

Description

[Detailed Description of the Invention] [Industrial Application Field] The present invention provides three-dimensional positioning,
The present invention relates to a method of maintaining posture and controlling the movement of a running robot, and more specifically, to a control method that compensates for the influence of tidal currents on the robot.

[Prior art]

In recent years, we have been involved in various offshore developments, maintenance of underwater structures such as offshore oil production platforms, underwater grounding equipment, etc.
Robots have come to perform inspections, maintenance work, etc. in place of conventional divers, and it is thought that this type of underwater robots to support ocean development will become even more popular in the future.

Some of these robots are those that travel on orbits and those that move on the ocean floor, but in the future, robots that can swim freely under the sea like divers will become increasingly necessary. This has been predicted and various robots are being developed.

The environment in which a robot swimming underwater operates is affected by currents such as tidal currents, the Karman vortex caused by underwater structures, and various other water currents. In general, robots often have a shape such as a disc, where the force acting on them differs significantly depending on the direction in which they receive water resistance.
If the posture of the robot changes during position maintenance and travel control, the force it receives from the water flow will suddenly change as described above, causing it to lose its balance and drift away from its target (position or travel route), and in some cases, the robot may lose its control. There are problems such as becoming disabled.

Therefore, this type of robot driving has traditionally been
Regarding the control method of the robot's attitude, etc., a method is adopted that does not change the attitude of the robot in response to the current, and the activities of the robot under the sea are extremely limited. However, a suitable control method for a robot that automatically maintains its position and posture, runs, and performs work in tidal currents has not yet been obtained. Therefore, robots have not yet been able to completely replace work in areas where divers are at risk of being swept away by currents or caught in the Karman vortex.

[Purpose of the invention]

The present invention has been made to solve the above-mentioned problems, and the present invention allows the robot to maintain its position even if it freely changes its posture during position maintenance and travel control in a work environment with water flow such as tidal currents. The object of the present invention is to provide a control method that allows stable position maintenance and running without the risk of losing balance.

[Structure of the invention]

The configuration of the three-dimensional control method for an underwater robot according to the present invention to achieve the above object is as follows: (a) Control input u(k) at time k and A quadratic performance evaluation function J with the deviation e(k+1) from the target value at time k+1 as a variable is set, a control gain G at which this performance evaluation function J is minimum is determined, and this gain G and state estimation are (b) The robot changes its attitude by calculating the optimal control input u from the value A three-dimensional fluid resistance and moment coefficient table prepared in advance as a function of the elevation angle α and sideslip angle β of the robot, which calculates the thrust force X^b that cancels the force acting on the robot due to the flow velocity and direction of the tidal current against the robot. Calculated from the coefficient C^ij (α, β) obtained from
is characterized in that it is output as the thrust of the robot.

In the present invention, the calculation for determining the optimal feedback control u using G as the gain is performed using a dynamic programming method based on Bellman's principle of optimality.

When implementing the present invention, the above-mentioned secondary performance evaluation function when calculating the above-mentioned thrust force Tc (hereinafter referred to as optimal control thrust force) is, for example, the following formula J= _∞ 〓 ^k=0 {e ² (k+1)+w・u ² (k)} (1) However, in equation (1), w (w≧0) represents a weighting coefficient.

The actual procedure for determining this optimal control thrust is shown below. Note that for convenience of explanation, a case will be described in which the position P and the orientation O are set as target values. In the ground-fixed coordinate system (X _E Y _E Z _E system), the target value P ₀
The state estimation value algorithm (observer) is based on the vertical Tc'' required to correct the deviation between the estimated value P^, etc. achieved by observation, and the estimated value P^ (position) and O^ (orientation). The estimated state of the robot is
(indicates the estimated values of the robot's position, velocity, acceleration, attitude angle, angular velocity, and angular acceleration), and further controls to minimize the quadratic performance evaluation function J for this estimated value X^. A thrust force Tc that compensates for deviations (drift, etc.) caused by the influence of tidal currents, etc. is calculated from the thrust force Tc' calculated from the optimal control u=-G·X^ multiplied by the gain G.

The calculation means for the thrust force Tc is arbitrary, but the CPU
For example, in the control device, the optimal control thrust Tc
is calculated as Tc″−Tc′, and further this Tc is entered into the state estimation algorithm along with the estimated values P^ and X^ of the current position and orientation based on actual measurements, and the calculation is performed again, and this result is output to the robot. be able to.
By repeating the robot control operation using this optimal control thrust Tc, the deviation value from the target value is successively corrected to zero.

When carrying out the present invention, in order to calculate the thrust T^b (hereinafter referred to as dynamic balance thrust), first the fluid resistance and moment coefficient C^ij
It is necessary to find (α, β) from the three-dimensional fluid resistance and moment coefficient table. In this method, the tidal flow directions α, β, and tidal current velocity Vc are determined from the robot's water velocity V^ and the robot's absolute velocity V^' estimated by the robot state estimation algorithm. Based on the current directions α and β, C^ij (α,
β).

The table can be determined, for example, by actually measuring the forces received when the robot is placed in a water tank and held in various postures against water flowing from a certain direction. The fluid resistance and moment coefficient determined in this way are previously stored in the control device. _The dynamic balance thrust is defined _by three- _dimensional x, y,
The component force on the z-axis and the moment around each axis (φ, θ, and ψ represent the x, y, and z axes, respectively) are T^bx, T^by, T^bz, and T^bφ, respectively. ,
If T^bθ, T^bψ, T^b, x, y, z = C^ij (α, β) ・1/2・ρ・Vc ²・Vb ^2/3 (2) T^b, φ , θ, ψ=C^ij (α, β) ・1/2・ρ・Vc ²・Vb (3) However, in the formula, Vc is the flow velocity, Vb is the dimensionless reference volume, and Vb ^2/3 is the dimensionless Each represents the standard area.

The coefficients C^ij (α, β) are the elevation angle α, which is the relative angle of the robot to the tidal flow, and the sideslip angle β.
By expressing it as , it is possible to obtain T^b without considering the absolute attitude.

〔Example〕

DESCRIPTION OF THE PREFERRED EMBODIMENTS The robot control method of the present invention will be explained below by way of an embodiment with reference to the drawings.

Fig. 1 is a block diagram showing the contents of the control calculation processing of the control device installed in the robot that is the subject of this embodiment. FIG. 2 is a block diagram showing the overall configuration of the control software.

The robot used in this embodiment is configured to be operated according to three control modes. That is, as shown in FIG. 1, each operation of the control means 1 in the position and attitude control mode, the control means 2 in the constant velocity route tracking control mode, and the control means 3 in the wall follow control mode can be selected. Then, the operator appropriately selects each of these control modes using the switching operation means 4 according to the work procedure. This switching operation means 4 can be switched at any time during work by remote control. Therefore, by this selection operation, a program for the work procedure is executed from a program input in advance to a control device provided in the robot.

The position and attitude control mode is a control mode for moving the robot to a new underwater position and attitude set by the operator based on the robot's current position and attitude in the ground-fixed coordinate system (X _E Y _E Z _E system). The mode is the position P (three-dimensional position: x, y, z) that the robot should reach and the robot's orientation, that is, the posture O (angles with respect to each axis of the three-dimensional coordinate fixed in the sea: φ, θ, ψ). The specification for controlling in this mode in this embodiment specifies the values to be ultimately controlled for the position P and orientation O, but does not specify the moving speed U.

In the constant speed route tracking control mode, firstly, the robot is guided to a target structure position along a route set in underwater space by an operator by remote control, and secondly, the robot is guided remotely to a target structure location along a route set in underwater space. This is _a control mode in which the object is _traveled at a constant _speed along the target route in order to observe the object from the target route. The moving speed U of 1 is sequentially specified by a program.

The wall follow control mode is a control mode for observing a target while facing a curved structure, keeping a constant distance from the structure, and moving around the structure at a constant speed. , the distance L to be maintained from the wall in addition to the moving speed U in the route direction
is specified.

Note that although FIG. 2 illustrates the case where position and attitude control mode 1 is selected among the three control modes, the other configurations are exactly the same when other modes are selected. That is clear. The calculation means according to the present invention are the target deviation value calculation means 5 ₁ and the thrust calculation means 5 ₂ surrounded by the dotted line in FIG. 2, and the others are conventional means used for controlling this type of robot. It is configured.

The data given to the calculation means 5 ₁ and 5 ₂ according to the present invention include the target value of the position Po (three-dimensional) that the robot should take based on the program, the direction the robot should face, that is, the posture Oo, and the moving speed V of the robot. ₀
The required value out of the three values, the estimated values P^, O^ of the current position and orientation based on actual measurements, and the estimated absolute velocity of the robot V^ among the X^ obtained as a result of state estimation. , each component that supplies data to the calculation means 4 and 5 will be explained in order of explanation.

First, the control device of this embodiment is equipped with an obstacle avoidance route calculation means 6 to prevent the control in each of the modes from becoming impossible due to obstacles on the work route. The control calculation result by the control means can be modified as appropriate. The data given to the obstacle avoidance route calculating means 6 as obstacles to be avoided include a model 6 for obstacles that can be inputted in advance.
₂ and the detection data and current detected by the obstacle detection means ₆₃ using various sensors equipped while the robot is moving.

That is, based on the above data, the obstacle avoidance route calculation means 6 performs a calculation for correction as shown in FIG. 1 (this part is omitted in FIG. 2), and calculates the selected The target correcting means 7, the route correcting means 8, and the follow route correcting means 9 are provided for making corrections based on the calculation results of the respective control means 1, 2, and 3 and the calculation results of the calculation means 6 depending on the control mode. In the calculations by the route correction means 8 and the follow route correction means 9, the speed U along the route is set to the absolute speed V ₀ ' of the robot, and the relative distance L between the object and the robot is set to the absolute position Po of the robot. It also includes an operation to convert to ′.

Then, the correction values of each of the above indicated values, P ₀ ′, O ₀ ′,
V ₀ ′ (all of these are vector values) is given to the target deviation value calculation means 5 ₁ as described above.

Next, the procedure for calculating the estimated values P^, O^, and V^ will be explained. The robot body is equipped with an inertial navigation device (inertia p) or an acoustic measuring device (acoustic n) as means for detecting the current position, and a gyro compass (gyro compass) for detecting the direction the robot is facing, that is, the robot's attitude. o) and ship speed v (relative speed to water)
The robot is equipped with a three-axis water velocity meter as a detection means, and constantly detects the robot's position, attitude, and speed. As is well known, these detected values include detector drift and measurement noise, and if they are input as data to the control device, malfunctions will occur. Therefore, measurement noise can be removed using a Kalman filter, as is conventionally done. Regarding noise filter processing and drift, position data P^, azimuth data O^, and relative velocity obtained by correction using a measurement noise filter and drift compensation means 11 that periodically compensate for drift by pattern matching using visual positioning. V^ is determined, and this correction value P^,
O^ and V^ are given to the calculation means 5 ₁ and 5 ₂ .

Further, as for the speed of the robot, an estimated value V' of the speed of the robot in X' obtained by the state estimating means 13 is given to the calculation means ₅₁ and ₅₂ . Note that the visual positioning shown in Figure 2 is performed for drift compensation, and is performed by a processor installed on the mother ship at sea. Further, the filter and drift compensation means 11 are processed by a processor installed in the mother ship, and the estimated values P^, O^, and V^ of these processes are sent to the robot and input to the control device.

Next, calculations for determining the dynamic balance thrust T^b and the optimum control thrust Tc in this embodiment will be explained based on FIG. 1.

In calculating the thrust force Tc (vector) required to reach the instruction value (a part or all of Po', Oo', and Vo') given based on each selected mode, the robot uses the present embodiment. Here, it is given by executing each calculation means of the target deviation calculation means 5 ₁ , thrust conversion calculation means 12 , state means 13 , multivariable control gain and thrust conversion calculation means 14 , and addition calculation means 15 . That is, the target deviation value calculation means ₅₁ calculates the deviation value ΔP between the instruction value Po' (hereinafter, the instruction value will be explained as being based on "position") and the actual estimated arrival value P^, and then the thrust conversion calculation means In step 12, the thrust force Tc'' required to make this ΔP zero is calculated.The thrust force calculation means ₅₂ shown in FIG.
Each of the means 12 to 19 shown in the figure is summarized and omitted.

In parallel with the above calculation, the estimated values P^, O^ and the value of Tc obtained by the addition/subtraction calculation means 15 are provided to the state estimating means 13, and X^ indicating the motion state of the robot is estimated. The composition _of be. Based on this value, the multivariable control gain and thrust conversion calculation means 14 calculate the deviations ΔP and ΔO.
Addition calculation means 15 calculates the corrected thrust force Tc' that corrects (including ΔV depending on the control mode) to the optimum value.
The calculation Tc''+Tc' is calculated, and the resulting Tc is again given to the state estimating means 13 together with the estimated values P^ and O^ of the current position and orientation based on actual measurements, and the above calculation is repeated to optimally correct the deviation.

Next, the means for calculating the dynamic balance thrust T^b will be explained. This calculation is achieved by carrying out calculations by the tidal flow inflow angle and velocity calculation means 17, the dynamic balance thrust calculation means 18, and the three-dimensional fluid resistance and moment coefficient table reference means 19.

That is, the estimated values P^, O^, V^ and the estimated velocity value V^' in the X^ are calculated by the tidal flow inflow angle and flow velocity calculation means 1.
7, where the angle representing the relative posture of the robot with respect to the tidal current, i.e., the angle formed by the front upright direction of the robot, i.e., the elevation angle α, the angle formed by the front lateral direction, i.e., the sideslip angle β, and the absolute of the tidal current. Flow velocity Vc
is calculated, and the fluid resistance coefficient C^ij (α, β) corresponding to the values of α and β is determined by the three-dimensional fluid resistance and moment coefficient table reference means 19,
The above Vc and C^ij (α, β) are given to the dynamic balance thrust calculation means 18, and from the above equations (2) and (3),
T^b is calculated and the operation is performed in the addition calculation means 16, that is, the sum of the T^ and Tc finally found in the addition calculation means 15 (T=T^b+
Tc) is the thrust force T actually given to the robot.

The value T obtained in this way is calculated based on the change in the external force received from the tidal current and the deviation of the robot from its target point when the thrust output when the robot performs the posture, movement, etc. instructed based on the robot's program. Since it is compensated, it is possible to perform control without losing balance due to tidal currents or moving away from the target point.

This thrust force T is subjected to arithmetic processing by the thrust distribution means 20 to be distributed into thrust forces for driving each of a plurality of propulsors attached to the robot, and is output to the robot. Note that the symbol T'F/B in FIG. 2 indicates a feedback output for checking whether the thrust force Tc is accurately driving the thruster.

〔Effect of the invention〕

As explained above, the structure of the three-dimensional control method for an underwater robot according to the present invention is such that the thrust is determined by the control gain that minimizes the quadratic performance evaluation function with the deviation from the target value and the control input as variables, and the robot The thrust force that cancels the external force received due to the change in posture is determined by referring to a table prepared in advance, and
Because the robot is propelled by the vector sum of two thrust forces, when the robot is held in position or operated in the presence of fluid flows such as tidal currents, it compensates for the imbalance of external forces received from the fluid due to posture changes. Since the robot can be operated using the generated thrust, it is possible to stably control the robot's movements, posture maintenance, etc., and this has the effect of facilitating the robot's work in fluids. be able to.

[Brief explanation of the drawing]

FIG. 1 is a block diagram showing the contents of control calculation processing of a control device according to one embodiment, and FIG. 2 is a block diagram showing the overall software configuration of the control block shown in FIG. 1. 5 ₁ ... Target deviation value calculation means, 12... Thrust conversion calculation means, 13... State estimation calculation means, 14...
multivariable control gain and thrust change calculation means, 15;
16... Addition calculation means, 17... Tidal current inflow angle and flow velocity calculation means, 18... Dynamic balance thrust calculation means, 19... Three-dimensional fluid resistance and moment coefficient table reference means, R... Robot body.

Claims (1)

[Claims] 1. In the control of a robot for swimming in a flowing fluid such as a tidal current in the ocean, (a) the deviation e(k+1) between the control input u(k) at time k and the target value at time k+1; A quadratic performance evaluation function J is set with , and a control gain G at which this performance evaluation function J becomes minimum is determined. From the estimated state value X^ obtained from the estimated values of position and attitude, the optimal control input u is determined from the formula u=-G・X^, and the thrust force Tc that achieves the target is calculated by feeding back the deviation e from the target. (b) When the robot changes its attitude, the thrust force X^b that cancels out the tidal force acting on the robot due to the velocity and direction of the tidal current relative to the robot is calculated in advance by calculating the angle of elevation α of the tidal current relative to the robot and sideslip. Coefficient C^ij (α, β) obtained from the three-dimensional fluid resistance and moment coefficient table prepared as a function of angle β
and (c) a vector sum of the two thrust forces T=Tb+Tc is output as the thrust force of the robot.