Mobile body control device, mobile body control system, mobile body system, mobile body control method, and mobile body control program

Abstract

To achieve both positioning control and tension control.SOLUTION: A mobile control device 1 comprises: a torque calculation unit 2 that receives information regarding the moving target of a moving object 8 and provides a control signal to a winch 5 for placing the moving object 8 at a desired position; and an average tension integral feedback unit 3 that provides the conditions for reducing the evaluated value φ34 of the tension error due to the difference between the target average tension φ31(fc,avg) given in advance and the measured tension φ16(fm) that occurs in each of a plurality of wires 7 when the moving object 8 is at the desired position to the torque calculation unit 2 as a wire length correction value φ36 (ΔlFB). The average tension integral feedback unit 3 calculates the wire length modification value φ36(ΔlFB) for each of the plurality of wires 7 using the distribution ratio vector φ35(ρΔl) derived from the balance condition to maintain the balance of the forces acting on the moving body 8 at the desired position.SELECTED DRAWING: Figure 4

Description

[Technical Field]

[0001] The present invention relates to a mobile object control device, a mobile object control system, a mobile object system, a mobile object control method, and a mobile object control program. [Background technology]

[0002] A parallel wire mechanism connects multiple wires to a moving object and moves the object to a desired position by controlling the length of the wires that are fed out using a winch or the like. Patent Document 1 discloses technology related to the parallel wire mechanism. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Application Laid-Open No. 2009-270321 Summary of the Invention [Problem to be solved by the invention]

[0004] One method for positioning a moving object to a desired position is to control the payout length of the wire. Specifically, first, a rotary encoder or the like attached to the winch is used to measure the payout length of the wire before moving to the target position. Next, the payout length of the wire required to move the moving object to the target position is calculated. Then, the winch is driven to follow a payout pattern that satisfies the target torque obtained based on the payout length of the wire.

[0005] When the moving body completes its movement to the target position through the above control and comes to rest at the target position, the resultant force acting on the moving body is in balance. The moving body is subject to not only forces from the wires but also external forces due to other factors. As a result, depending on the number of wires and the degree of freedom of the system, the force acting on the moving body that has come to rest at the target position may take multiple forms rather than just one. This force form may not be manageable solely by the unreeled length of the wires. For example, if the balance is achieved by weak tension in the wires, the external force acting on the moving body may cause the moving body to move unintentionally. Furthermore, if the balance is achieved by strong tension in the wires, excessive load may be placed on components such as pulleys that make up the parallel wire mechanism.

[0006] Therefore, a method is being considered to control the movement of the moving object based on the wire tension instead of the wire payout length. Specifically, a rotary encoder or the like attached to the winch is used to measure the wire payout length before moving to the target position. Next, the wire payout torque required to move the moving object along the target trajectory is calculated. Then, the wire payout torque is controlled while measuring the tension generated in the wire with a force sensor. In this control, the wire tension is adjusted for each wire.

[0007] In this type of tension-based control, the tension of each individual wire is controlled. As a result, when viewed as the resultant force acting on the moving body, it may not be possible to achieve a balance of forces to hold the moving body stationary at the target position. In this case, the moving body moves to a position where the forces are balanced. As a result, the moving body's position will deviate from the target position.

[0008] In other words, if priority is given to position, the desired balance of tensions cannot be achieved, and if priority is given to the balance of tensions, placement at the desired position cannot be achieved. In short, there has been no technology that can simultaneously place a moving body at a desired position and control the balance of forces acting on the moving body at that position.

[0009] Therefore, an object of the present invention is to provide a mobile body control device, a mobile body control system, a mobile body control method, and a mobile body control program that can achieve both positioning control and tension control. [Means for solving the problem]

[0010] One form of the present invention is a mobile body control device for moving a single mobile body connected to one end of a plurality of wires by adjusting the wire length, which is the length of the wire unwound from a drive unit due to the operation of a plurality of drive units connected to each of the other ends of the plurality of wires, and the device comprises: a first calculation means that receives information regarding a movement target of the mobile body and provides a control signal to the drive unit for positioning the mobile body at the desired position; and a second calculation means that provides the first calculation means with a condition for reducing an evaluation value of the tension error caused by the difference between the target average tension and the measured tension generated in each of the plurality of wires when the mobile body is at the desired position as a wire length correction value, and the second calculation means calculates the wire length correction value for each of the plurality of wires using a balance condition for maintaining the balance of forces acting on the mobile body at the desired position.

[0011] In this movable body control device, the second calculation means calculates a wire length correction value so as to reduce the difference between a predetermined target tension and the measured tension. When calculating the wire length correction value, the second calculation means utilizes a balance condition for maintaining the balance of forces acting on the movable body at a desired position. In other words, when calculating the wire length correction value, the second calculation means incorporates, as a condition, maintaining the balance of forces acting on the movable body at a desired position. As a result, the wire length correction value calculated by the second calculation means can reduce tension deviations while maintaining the position of the movable body. Therefore, the movable body control device can simultaneously control the positioning and tension of the movable body.

[0012] The second calculation means of the above-mentioned mobile body control device may include means for obtaining an evaluation value of the tension error, means for obtaining a distribution ratio derived from the balance condition, and means for obtaining a correction value for the wire length using the evaluation value of the tension error and the distribution ratio. With this configuration, a suitable correction value for the wire length can be obtained.

[0013] The means for obtaining the tension error of the above-mentioned mobile body control device may include means for obtaining a measured average tension which is the average of multiple measured tensions generated in each of the multiple wires, means for obtaining an average tension error which is the difference between the target average tension and the measured average tension, and means for obtaining a value obtained by integrating the average tension error over time as an evaluation value of the tension error. This configuration also makes it possible to obtain an appropriate correction value for the wire length.

[0014] The first calculation means of the above-mentioned mobile body control device may include means for obtaining a target wire length by applying information about the moving target of the mobile body to an inverse kinematics model, means for obtaining a difference between the target wire length and a value related to a measured wire length, which is the wire length obtained by measurement, and means for adding a correction value to the target wire length. The first calculation means may constitute a first feedback system including means for obtaining a difference from the value related to the measured wire length. The first calculation means and the second calculation means may constitute a second feedback system including means for adding a correction value. This configuration also makes it possible to reduce deviations in tension while maintaining the position of the mobile body.

[0015] The above-mentioned mobile object control device may further include a third calculation means for correcting the wire length obtained by measurement, which is provided to the means for obtaining a difference from the wire length included in the first calculation means. The third calculation means may include a means for obtaining a correction wire stiffness corresponding to the measured wire length for each of the plurality of wires, and a means for obtaining the correction wire length for each of the plurality of wires using the correction wire stiffness and the measured wire length. The means for obtaining a difference from the value related to the measured wire length of the first calculation means may receive the correction wire length as the value related to the measured wire length.

[0016] The wire may undergo elastic stretching in response to tension, or tension relaxation, which increases over time. This configuration can prevent a decrease in positioning accuracy due to wire stretching.

[0017] Another aspect of the present invention is a mobile body control system for moving a single mobile body connected to one ends of a plurality of wires by adjusting the wire length, which is the length of the wire unwound from the drive units due to the operation of a plurality of drive units connected to each of the other ends of the plurality of wires, the system comprising: a wire length sensor for obtaining the wire length; a tension sensor for obtaining the tension generated in each of the plurality of wires when the mobile body is at a desired position; and a mobile body control unit for providing control signals to the plurality of drive units, wherein the mobile body control unit has: first calculation means for receiving information regarding a movement target of the mobile body and providing a control signal to the drive units for positioning the mobile body at the desired position; and second calculation means for providing the first calculation means with a condition for reducing tension error resulting from the difference between the target average tension and the measured tension generated in each of the plurality of wires when the mobile body is at the desired position as a wire length correction value, and the second calculation means calculates the wire length correction value for each of the plurality of wires using a balance condition for maintaining balance of forces acting on the mobile body at the desired position.

[0018] A still further embodiment of the present invention provides a mobile body system comprising a plurality of wires, a plurality of drive units connected to one end of each of the plurality of wires, a single mobile body connected to the other end of the plurality of wires, and a mobile body control unit that provides control signals to the plurality of drive units, wherein the mobile body control unit has a first calculation means that receives information regarding a moving target of the mobile body and provides a control signal to the drive unit for positioning the mobile body at a desired position, and a second calculation means that provides the first calculation means with a condition for reducing tension error resulting from the difference between a target average tension and the measured tension generated in each of the plurality of wires when the mobile body is at a desired position as a wire length correction value, and the second calculation means calculates the wire length correction value for each of the plurality of wires using a balance condition for maintaining balance of forces acting on the mobile body at a desired position.

[0019] Yet another form of the present invention is a mobile body control method for moving a single mobile body connected to one end of a plurality of wires by adjusting the wire length, which is the length of the wire unwound from a drive unit due to the operation of a plurality of drive units connected to each of the other ends of the plurality of wires, the method comprising: a first step of receiving information regarding a movement target of the mobile body and providing a control signal to the drive unit for positioning the mobile body at a desired position; and a second step of returning to the processing of the first step a condition for reducing tension error resulting from the difference between the target average tension and the measured tension generated in each of the plurality of wires when the mobile body is at the desired position as a wire length correction value, wherein the second step calculates the wire length correction value for each of the plurality of wires using a balance condition for maintaining the balance of forces acting on the mobile body at the desired position.

[0020] Yet another form of the present invention is a mobile body control program that causes a computer to execute control for moving a single mobile body connected to one end of a plurality of wires by adjusting the wire length, which is the length of the wire unwound from the drive units due to the operation of a plurality of drive units connected to each of the other ends of the plurality of wires. The computer is operated as a first calculation means that receives information regarding a movement target of the mobile body and provides a control signal to the drive units for positioning the mobile body at a desired position, and a second calculation means that provides the first calculation means with a condition for reducing tension error caused by the difference between the target average tension and the measured tension generated in each of the plurality of wires when the mobile body is at the desired position as a wire length correction value, and the second calculation means calculates the wire length correction value for each of the plurality of wires using a balance condition for maintaining the balance of forces acting on the mobile body at the desired position.

[0021] The mobile body control system, mobile body system, mobile body control method, and mobile body control program, which are other aspects of the present invention, can also achieve both positioning control and tension control, similar to the mobile body control device. [Effects of the Invention]

[0022] According to the present invention, it is possible to achieve both positioning control and tension control. [Brief explanation of the drawings]

[0023] [Figure 1] FIG. 1 is a diagram schematically illustrating components of a mobile system according to an embodiment. [Figure 2] FIG. 2 is a block diagram of a mobile system according to an embodiment. [Figure 3] FIG. 3 is a block diagram showing the physical components of the mobile object control device of the embodiment. [Figure 4] FIG. 4 is a functional block diagram of the mobile object control device according to the embodiment. [Figure 5] FIG. 5 is a functional block diagram of the torque calculation unit shown in FIG. [Figure 6] FIG. 6 is a functional block diagram of the average tension integral feedback unit shown in FIG. [Figure 7] FIG. 7 is a functional block diagram of the static wire stretch compensation unit shown in FIG. [Figure 8] FIG. 8 is an example of a specific control block diagram of a mobile object control device according to the embodiment. [Figure 9] FIG. 9 is a flowchart showing a mobile object control method executed by the mobile object control device. [Figure 10] FIG. 10 is a functional block diagram of a mobile object control device according to a modified example. [Figure 11] FIG. 11 is an example of a specific control block diagram of a mobile object control device according to a modified example. DETAILED DESCRIPTION OF THE INVENTION

[0024] Hereinafter, embodiments of the present disclosure will be described in detail with reference to the accompanying drawings. In the description of the drawings, the same elements are denoted by the same reference numerals, and duplicated explanations will be omitted.

[0025] 1 and 2, the mobile body system 101 includes a mobile body control device 1 (mobile body control unit), multiple winches 5 (drive units), multiple pulley units 6, multiple wires 7, and a mobile body 8. The mobile body system 101 moves the mobile body 8 to a desired position by controlling the payout lengths of the multiple wires 7. Such a mechanism is called a parallel wire drive mechanism.

[0026] The mobile body control device 1 can simultaneously control the positioning and tension of a mobile body 8, which is a type of inspection device hung from a wall surface. A mobile body system 101 including the mobile body control device 1 can be applied to a system in which a tile peeling inspection device is used as the mobile body 8, as an example. The tile peeling inspection device applies water hammer from a position approximately 0.5 m to 1 m away from the wall surface. The presence or absence of peeled tiles is inspected based on the sound generated by the water hammer. The collected sound is provided to an acoustic analysis system, which uses AI or machine learning to determine the presence or absence of defects. The tile peeling inspection device may include a water gun for water hammering, a gun microphone for collecting sound, and a camera for image confirmation.

[0027] The mobile body control device 1 can increase or decrease the tension acting on the wire 7 while maintaining the position of the mobile body 8. Such tension can be input to the mobile body control device 1 as a target average tension φ31. For example, when an external force due to wind or the like acts on the mobile body 8, it is necessary to increase the tension of the wire 7 in order to keep the mobile body 8 stationary. In such a case, the mobile body control device 1 can achieve rigidity sufficient to withstand the external force by controlling the tension of the wire 7. Furthermore, when no external force is acting, the rigidity that can be achieved by controlling the tension of the wire 7 does not necessarily need to be high. In such a case, the mobile body control device 1 can also loosen the tension of the wire 7 to the extent that excessive load is not applied to the pulley unit 6, etc.

[0028] The winch 5 has a motor driver 51, a motor 52, and a rotary encoder 53 (wire length sensor) (see FIG. 2). The motor driver 51 receives a command torque φ14 (τ c ), a motor control signal is generated to operate the motor 52. The motor 52 adjusts the wire length.

[0029] The rotary encoder 53 has a measurement wire length of φ15 (l m ) The rotary encoder 53 measures the rotation angle of the output shaft of the motor 52. The rotary encoder 53 may also be provided on a winch drum that is rotated by the motor 52. By using the rotation angle of the output shaft of the motor 52, the length of the wire that has been reeled out by the motor 52 can be obtained.

[0030] The means for obtaining the wire length is not limited to the rotary encoder 53. The wire length may be obtained by a means other than the rotary encoder 53.

[0031] The pulley unit 6 has a pulley 61 and a tension sensor 62. FIG. 1 illustrates a configuration in which one pulley unit 6 is arranged between the motor 52 and the movable body 8. Two or more pulley units 6 may be arranged between the motor 52 and the movable body 8. The winch 5 and the movable body 8 may also be connected without the pulley unit 6. In other words, the pulley unit 6 may be omitted. When the pulley unit 6 is omitted, a tension sensor may be provided at a desired position.

[0032] The pulley 61 changes the direction in which the wire 7 stretched from the motor 52 to the moving body 8 extends. The tension sensor 62 measures the tension φ16 (f m ) is output. The tension sensor 62 can be provided, for example, as a shear load cell on the intermediate sheave shaft. The tension sensor 62 may also be provided at a location separate from the pulley unit 6.

[0033] The rotary encoder 53, which is a wire length sensor, the tension sensor 62, and the mobile object control device 1 constitute a mobile object control system 100.

[0034] <Mobile device control device> The hardware configuration of the mobile object control device 1 will be described with reference to Fig. 3. The mobile object control device 1 is realized, for example, by a computer 500 executing a mobile object control program PG. The mobile object control program PG causes the computer 500 to execute a mobile object control method.

[0035] The mobile object control device 1 includes one or more computers 500. The computer 500 has a CPU (Central Processing Unit) 501, which is a processor, a main memory unit 502, an auxiliary memory unit 503, a communication control unit 504, an input device 505, and an output device 506. The mobile object control device 1 is configured by one or more computers 500 configured by these pieces of hardware and software such as programs.

[0036] When the mobile object control device 1 is configured by multiple computers 500, these computers 500 may be connected locally or via a communication network such as the Internet or an intranet. This connection logically constructs a single mobile object control device 1.

[0037] The CPU 501 executes an operating system, application programs, etc. The main memory 502 is composed of a ROM (Read Only Memory) and a RAM (Random Access Memory). The auxiliary memory 503 is a storage medium composed of a hard disk, flash memory, etc. The auxiliary memory 503 generally stores a larger amount of data than the main memory 502. The communication control unit 504 is composed of a network card or a wireless communication module. The input device 505 is composed of a keyboard, mouse, touch panel, and microphone for voice input, etc. The output device 506 is composed of a display, printer, etc.

[0038] The auxiliary storage unit 503 stores in advance the mobile object control program PG and data necessary for processing. The mobile object control program PG causes the computer 500 to execute each functional element of the mobile object control device 1. For example, the mobile object control program PG is read by the CPU 501 or the main storage unit 502, and causes at least one of the CPU 501, the main storage unit 502, the auxiliary storage unit 503, the communication control unit 504, the input device 505, and the output device 506 to operate. For example, the mobile object control program PG reads and writes data from and to the main storage unit 502 and the auxiliary storage unit 503.

[0039] The mobile object control program PG may be provided in the form of a tangible recording medium such as a CD-ROM, a DVD-ROM, or a semiconductor memory. The mobile object control program PG may be provided as a data signal via a communication network.

[0040] Here, we will explain the control executed by the moving body control device 1. First, the force balance equations in the moving body 8 of this parallel wire drive mechanism are as shown in equations (1) to (3).

number

number

number

[0041] When the target position and posture of the moving body 8 and the external forces acting at these times are given in equations (1) to (3), it is possible to find a combination of wire tensions that balances these. However, there are an infinite number of combinations of tension vectors that satisfy these.

[0042] When the moving body 8 is in a balanced state, the resultant force of the wire tension can be considered to be the sum of the "force that balances the external force" and the "internal force." Now, assume that the moving body 8 is in a balanced state, and that the "force that balances the external force" and the "internal force" each have a certain error from their target values. If the tension of each wire is controlled in this state to match the target value, the equation for balance may no longer hold. As a result, there is a possibility that a drift will occur, in which the moving body 8 moves to a new balanced position.

[0043] Therefore, the mobile body control device 1 controls the internal force while satisfying equation (3), thereby indirectly controlling only the internal force while maintaining the resultant wire tension. The average tension of the wire 7 is focused on as a physical quantity that indirectly expresses the internal force. Then, the average tension of the wire 7 is matched to the target average tension by integral feedback. At this time, in order to maintain the resultant wire tension, the payout amount of each wire 7 that satisfies the equation of balance is calculated.

[0044] Next, calculation of wire stiffness and static wire elongation compensation will be described. First, a simple compensation is performed using the stiffness of the wire 7 for the static elongation of the wire 7 when tension is applied. At this time, the stiffness of the i-th wire 7 (k i [N / m]) is the stiffness (k W [N / m]), it is shown by equation (4).

number

[0045] In addition, "l sp,i " is the wire length from the wire payout point of the winch 5 to the final pulley for the i-th wire 7. i " is the wire length from the final pulley to the moving body 8, or the wire length from the final pulley to the connection point between the moving body 8 and the wire 7. m,i " is the wire length (l) calculated from the measurement value of the rotary encoder 53. i ) is the measured value (measurement wire length).

[0046] Diagonal matrix (K W ) and the length of the measurement wire (l) when a certain tension (f) is applied. m ) is the corrected wire length (l m,cmp ) is obtained as

number

[0047] Next, we will explain the integral feedback of the average tension. As will be described later, in this control system, the command tension (f c ) is calculated. c ) and the measured tension (f m ) is multiplied by the gain to obtain the corrected value of the tension. W ) and the structural matrix (A T ) to correct the resultant tension so that it does not change. First, the change in tension (Δf) that does not change the resultant tension is shown by equation (6) or equation (7).

number

number

[0048] Next, the components of the distribution ratio regarding the change in tension of the i-th wire 7 are shown in equation (8).

number

[0049] Then, both sides of equation (7) are divided by the change in tension (Δf4) of the fourth wire 7. If the result is used in equation (8) that defines the distribution ratio, equation (9) is obtained.

number

[0050] Furthermore, using the definition shown in equation (8), the ratio of the tension change (Δf4) of the fourth wire 7 to the tension change (Δf1, Δf2, Δf3) of the first, second, and third wires 7 is expressed as a distribution ratio vector (ρ Δf ) can be summarized as equation (10).

number

number

[0051] Next, the measured tension (f m ) and the target average tension (f c,avg ) for the difference (average tension error: Δf avg ) is obtained.

number

[0052] The integral of the mean tension error and the distribution ratio vector (ρ Δl ) and the wire length correction value (Δl FB ) is obtained by equation (13).

number

[0053] Next, calculation of the command tension by the calculated torque method will be explained. First, the target wire length (l) is calculated by inverse kinematic calculation using the target position, target posture, target speed, and target acceleration of the moving body 8 given in advance. d ), the first derivative of the target wire length (target wire length velocity), and the second derivative of the target wire length (target wire length acceleration) are obtained.

[0054] Next, based on these, the command wire length (l c ) to obtain the second derivative of the command wire length (l c ) is calculated by the inverse kinematics calculation and the second derivative of the target wire length (l m,cmp ) and the wire length correction value (Δl FB ) and terms modified by

number

[0055] Next, the target average tension (f c,avg The tension solution is derived when the command tension (f c )

number

number

[0056] Target average tension (f c,avg ) we obtain equation (17).

number

[0057] Substituting equation (17) into equation (15) and rearranging it gives equation (18). The terms included in equation (18) are shown by equations (19) and (20).

number

number

number

[0058] Here, the command acceleration (X c The second derivative of the Jacobian matrix (J) and the command wire length (l c ) are expressed by equations (21) and (22).

number

number

[0059] To summarize the above, the command tension (f c ) is shown by equation (23).

number

[0060] Next, the command torque (τ c According to the equation of motion of the winch 5, the feedforward term of the tension, and the compensation terms of inertia and viscosity, the command torque (τ c ) is shown by equation (24).

number

[0061] <Mobile device control device> The moving body control device 1 that realizes the above-mentioned control will be described below. The moving body control device 1 applies a command torque φ14 (τ c ) to the winch 5. c ) can be output, the installation position and connection manner are not important. The mobile object control device 1 may be connected to the winch 5 by wire. The mobile object control device 1 may also be connected wirelessly via a network. As shown in FIG. 4, the mobile object control device 1 has a first device input N11, a second device input N12, a third device input N13, a fourth device input N14, and a first device output T11.

[0062] The first device input N11 is the target position φ11 (X d), target velocity φ12, and target acceleration φ13. The second device input N12 receives the measurement wire length φ15 (l m ) The third device input N13 receives the measured tension φ16 (f m ) The fourth device input N14 receives the target average tension φ31 (f c,avg ) The first device output T11 receives the command torque φ14 (τ c ) (control signal).

[0063] The mobile object control device 1 has a torque calculation unit 2 (first calculation means), an average tension integral feedback unit 3 (second calculation means), and a static wire elongation compensation unit 4 (third calculation means).

[0064] <Torque calculation section> As shown in FIG. 5, the torque calculation unit 2 calculates the target position φ11(X d ), target speed φ12, target acceleration φ13, and correction wire length φ42 (l m,cmp ) and wire length correction value φ36(Δl FB ) and the torque calculation unit 2 receives the command torque φ14(τ c ) is output to the outside of the moving body control device 1. Furthermore, the torque calculation unit 2 outputs the command tension φ28(f c ) and the structural matrix φ29(A T ) and outputs the command tension φ28 (f c ) and the structural matrix φ29(A T ) is used for internal processing of the mobile object control device 1.

[0065] The torque calculation unit 2 calculates a command torque φ14 (τ c The calculated torque method is a dynamic control system that uses both feedback of wire length and wire speed and feedforward of wire acceleration.

[0066] The torque calculation unit 2 has a first calculation input N21, a second calculation input N22, a third calculation input N23, a first calculation output T21, a second calculation output T22, and a third calculation output T23.

[0067] The first calculation input N21 is the target position φ11(X d ), the target speed φ12, and the target acceleration φ13. The second calculation input N22 receives the wire length correction value φ36 (Δl FB ) The third calculation input N23 receives the correction wire length φ42 (l m,cmp ) The first calculation output T21 is the command torque φ14 (τ c ) is output. The second calculation output T22 is the command tension φ28 (f c ) is output. The third calculation output T23 is the structural matrix φ29(A T ) is output.

[0068] The torque calculation unit 2 has an inverse kinematics calculation unit 21 (means for obtaining a target wire length), a wire length correction value addition unit 22 (means for adding together correction values), a command tension calculation unit 23, and a command torque calculation unit 24.

[0069] The inverse kinematics calculation unit 21 receives the target position φ11(X d ), the target velocity φ12, and the target acceleration φ13. The inverse kinematics calculation unit 21 calculates the target wire acceleration φ21, the target wire velocity φ22, and the target wire length φ23 (l d ) are obtained by calculation. The inverse kinematics calculation unit 21 outputs the target wire acceleration φ21 and the target wire speed φ22 to the command tension calculation unit 23. The inverse kinematics calculation unit 21 outputs the target wire length φ23(l d ) is output.

[0070] The wire length correction value adding unit 22 receives the target wire length φ23(l d ) from the average tension integral feedback unit 3 via the second calculation input N22. FB ) The wire length correction value adding unit 22 receives the corrected target wire length φ24(l d,FB Specifically, the wire length correction value adding unit 22 calculates the target wire length φ23(l d ) to wire length correction value φ36(Δl FB) to obtain the corrected target wire length φ24(l d,FB The wire length correction value adding unit 22 outputs the corrected target wire length φ24(l d,FB ) is output.

[0071] The command tension calculation unit 23 receives the target wire acceleration φ21 and the target wire speed φ22 from the inverse kinematics calculation unit 21. The command tension calculation unit 23 receives the corrected target wire length φ24(l d,FB The command tension calculation unit 23 receives the corrected wire length φ42 (l) from the static wire elongation compensation unit 4 via the third calculation input N23. m,cmp The command tension calculation unit 23 receives the command wire acceleration φ26, the command wire speed φ27, and the command tension φ28 (f c ) and is calculated. c ) is obtained by equation (25).

number

[0072] The command tension calculation unit 23 outputs to the command torque calculation unit 24 a command wire acceleration φ26, a command wire speed φ27, and a command tension φ28 (f c ) and outputs the command tension φ28 (f c ) to the average tension integral feedback unit 3 via the third calculation output T23. T ) is output.

[0073] The command torque calculation unit 24 receives the command wire acceleration φ26, the command wire speed φ27, and the command tension φ28 (f c ) and the command torque calculation unit 24 receives the command torque φ14(τ c ) is calculated using equation (26). The command torque calculation unit 24 outputs the command torque φ14(τ c ) is output.

number

[0074] <Average tension integral feedback section> The average tension integral feedback unit 3 is for simultaneously adjusting the tension of each wire 7 by the winch 5 so that the average tension of the wires 7 (the average value of the tension applied to each wire 7) becomes the target value and so as not to disrupt the tension balance of the wires. Specifically, the target average tension φ31 (f c,avg ) and the actual measured tension φ16 (f m ) average value (measurement average tension φ32 (f m,avg )), the time integral of the difference between them (evaluation value φ34) is calculated. Meanwhile, by modifying equation (3) for the balance of forces acting on the moving body 8, it is possible to obtain the equation (distribution ratio vector equation (11)) that must be satisfied to maintain the tension balance of the wire 7. Using equation (11) for the distribution ratio vector, the integral value of the error in the average value of the tension is distributed to each wire 7 to obtain the wire length correction value φ36 (Δl FB ) is found.

[0075] As shown in FIG. 6, the average tension integral feedback section 3 has a first feedback input N31, a second feedback input N32, a target value input N33, a fourth feedback input N34, and a first feedback output T31.

[0076] The first feedback input N31 is the correction wire stiffness φ41 (K W ) The second feedback input N32 receives the structural matrix φ29(A T ) is received. The target value input N33 receives the target average tension φ31 (f c,avg ) The fourth feedback input N34 receives the measured tension φ16 (f m ) The first feedback output T31 receives the wire length correction value φ36 (Δl FB ) is output.

[0077] In addition, the target average tension φ31(f c,avg) is the target value of the tension of the wire 7. The target value input N33 is one target average tension φ31(f c,avg ) value is directly received. In other words, a target value is not set for each of the first to fourth wires 7, but a single target average tension φ31 (f c,avg ) is input to the target value input N33. In this embodiment, the target average tension φ31(f c,avg ) itself as a target.

[0078] The average tension integral feedback unit 3 has a tension error evaluation value acquisition unit 31 (means for obtaining an evaluation value of the tension error), a distribution ratio vector calculation unit 32 (means for obtaining the distribution ratio), and a wire length correction value calculation unit 33 (means for obtaining a correction value of the wire length).

[0079] The tension error evaluation value acquisition unit 31 receives the target average tension φ31 (f c,avg The tension error evaluation value acquisition unit 31 receives the measured tension φ16 (f m The tension error evaluation value acquisition unit 31 obtains the tension error evaluation value φ34 by calculation. The tension error evaluation value acquisition unit 31 outputs the tension error evaluation value φ34 to the wire length correction value calculation unit 33.

[0080] The tension error evaluation value acquisition unit 31 has a measured average tension calculation unit 311 (means for obtaining the measured average tension), an average tension error calculation unit 312 (means for obtaining the average tension error), and an average tension error integration unit 313 (means for obtaining the tension error as an evaluation value).

[0081] The measured average tension calculation unit 311 receives the measured tension φ16 (f m ) is received. The measured average tension calculation unit 311 calculates the measured average tension φ32(f m,avg ) is calculated. Specifically, the measured average tension φ32(f m,avg ) is the measured tension φ16 (f m) divided by the number of wires 7 (f4). m,avg ) is output.

[0082] The average tension error calculation unit 312 calculates the target average tension φ31(f c,avg The average tension error calculation unit 312 receives the measured average tension φ32(f m,avg ) The average tension error calculation unit 312 receives the average tension error φ33(Δf avg Specifically, the average tension error calculation unit 312 calculates the measured average tension φ32(f m,avg ) to the target average tension φ31(f c,avg The average tension error calculation unit 312 outputs the average tension error φ33(Δf avg ) is output.

[0083] The processing of the above-described measured average tension calculation unit 311 and average tension error calculation unit 312 can be collectively expressed as shown in equation (29).

number

[0084] The average tension error integrator 313 calculates the average tension error φ33(Δf avg ) The average tension error integrator 313 obtains an evaluation value φ34 of the tension error by calculation. The average tension error integrator 313 outputs the evaluation value φ34 of the tension error to the wire length correction value calculator 33 via a first feedback output T31.

[0085] The distribution ratio vector calculation unit 32 receives the corrected wire stiffness φ41 (K W ) from the torque calculation unit 2 via the second feedback input N32. T ) is received by the distribution ratio vector calculation unit 32. Δl) is obtained by calculation using equations (27) and (28). The distribution ratio vector calculation unit 32 outputs the distribution ratio vector φ35(ρ Δl ) is output.

number

number

[0086] The wire length correction value calculation unit 33 receives the tension error evaluation value φ34 from the tension error evaluation value acquisition unit 31. The wire length correction value calculation unit 33 receives the distribution ratio vector φ35(ρ Δl ) The wire length correction value calculation unit 33 receives the wire length correction value φ36(Δl FB ) is obtained by calculation. The wire length correction value calculation unit 33 outputs the wire length correction value φ36(Δl FB ) is output.

[0087] The processing of the average tension error integrator 313 and the wire length correction value calculator 33 can be collectively expressed as shown in equation (30).

number

[0088] <Static wire elongation compensation section> The static wire stretch compensation unit 4 is configured to obtain torque balance of the motor 52 of the winch 5 by utilizing wire tension that takes into account the elastic stretch due to the spring constant of the wire 7 in order to improve the positional accuracy of the moving body 8. Taking into account the elastic stretch due to the spring constant of the wire 7 means, for example, performing control such that the wire 7 is wound up by an amount equivalent to the calculated stretch of the wire 7. The amount by which the wire length beyond the winch 5 increases due to elastic stretch (stretch amount) is determined by the spring constant. The spring constant differs depending on the wire length of the wire 7. For example, if the wire length is doubled, the spring constant is halved. This relationship can be expressed as "wire length = original wire length × (1 + spring constant 1 / K × tension T)."

[0089] As shown in FIG. 7, the static wire stretch compensation unit 4 has a first compensation input N41, a second compensation input N42, a first compensation output T41, and a second compensation output T42.

[0090] The first compensation input N41 is a measurement wire length of φ15 (l m ) The second compensation input N42 receives the command tension φ28 (f c ) The first compensation output T41 receives the correction wire stiffness φ41 (K W ) is output. The second compensation output T42 is the compensation wire length φ42 (l m,cmp ) is output.

[0091] The static wire elongation compensation unit 4 has a corrected wire stiffness calculation unit 41 and a corrected wire length calculation unit 42 (means for obtaining the corrected wire length).

[0092] The correction wire stiffness calculation unit 41 receives the measurement wire length φ15 (l m ) is received. The correction wire stiffness calculation unit 41 receives the correction wire stiffness φ41(K W ) is obtained by calculation using equation (31).

number

[0093] The correction wire stiffness calculation unit 41 outputs the correction wire stiffness φ41 (K W ) is output from the correction wire stiffness calculation unit 41. The correction wire stiffness calculation unit 41 also outputs the correction wire stiffness φ41 (K W ) is output.

[0094] The correction wire length calculation unit 42 calculates the correction wire stiffness φ41 (K W ) from the winch 5. The correction wire length calculation unit 42 receives the measurement wire length φ15 (l m ) from the torque calculation unit 2. c ) The correction wire length calculation unit 42 receives the correction wire length φ42(l m,cmp ) is calculated using equation (32). The correction wire length calculation unit 42 outputs the correction wire length φ42(l) to the torque calculation unit 2 via the second compensation output T42. m,cmp ) is output.

number

[0095] The static wire elongation compensation unit 4 for correcting elastic elongation described above can improve the accuracy of the position and stiffness of the moving body 8. The moving body control device 1 equipped with the static wire elongation compensation unit 4 can perform average tension integral feedback that takes elastic elongation into account.

[0096] The mobile body control device 1 can also omit the static wire elongation compensator 4. A mobile body control device 1A that does not have the static wire elongation compensator 4 can perform average tension integral feedback that does not take elastic elongation into account. The mobile body control device 1A that does not have the static wire elongation compensator 4 will be described later.

[0097] FIG. 8 is an example of a specific block diagram of the mobile body control device 1. The details of the operation shown in this block diagram are as described above, but to summarize, the target wire length, wire winding speed, and wire winding acceleration are calculated by inverse kinematics from the target trajectory of the mobile body 8 (position and posture, their first-order derivatives, namely velocity and angular velocity, and their second-order derivatives, namely acceleration and angular acceleration). Of these, the measured wire length φ15 (l m ) multiplied by the gains (Kp, Kd) and added to the wire winding acceleration to obtain the command wire acceleration φ26. An inverse dynamics calculation is performed using this command wire acceleration φ26 and the equation of motion of the parallel wire drive mechanism, and the command tension φ28(f c ) is calculated. c ) and the equation of motion of winch 5, an inverse dynamics calculation is performed, and the command torque φ14(τ c ) is found.

[0098] For example, the command tension calculation unit 23 has a structural matrix calculation unit 231, a differentiation unit 232, a first calculation unit 233, a first gain unit 234, a second calculation unit 235, a second gain unit 236, a third calculation unit 237, and a dynamics calculation unit 238.

[0099] The structural matrix calculation unit 231 calculates the target position φ11(X d ) is received. The structural matrix calculation unit 231 receives the target position φ11(X d ) to obtain the structural matrix φ29(A T The structural matrix calculation unit 231 generates the structural matrix φ29(A T ) is output to the structural matrix calculation unit 231. Furthermore, the structural matrix calculation unit 231 also outputs the structural matrix φ29(A T ) is output to the structural matrix calculation unit 231. The structural matrix calculation unit 231 also outputs the structural matrix φ29(A T ) is output.

[0100] The differentiation unit 232 calculates the correction wire length φ42 (l m,cmp ) The differentiation unit 232 receives the correction wire length φ42 (lm,cmp ) is the result of differentiating the correction wire length φ42(l m,cmp ) is output to the first calculation unit 233, the dynamics calculation unit 238, and the command torque calculation unit 24.

[0101] The first calculation unit 233 calculates the target wire length φ23(l d ) first derivative (target wire speed) and the correction wire length φ42 (l m,cmp ) and the first differential of the target wire length φ23(l d ) first derivative (target wire speed) to correct wire length φ42 (l m,cmp The first calculation unit 233 outputs the result of the subtraction (wire speed difference) to the first gain unit 234.

[0102] The first gain unit 234 applies a first gain (K d The first gain unit 234 multiplies the first gain (K d ) and outputs the result to the third calculation unit 237.

[0103] The second calculation unit 235 calculates the corrected target wire length φ24(l d,FB ) and correction wire length φ42 (l m,cmp ) and the second calculation unit 235 receives the corrected target wire length φ24(l d,FB ) to correction wire length φ42 (l m,cmp The second calculation unit 235 outputs the result of the subtraction (wire length difference) to the second gain unit 236.

[0104] The second gain unit 236 applies a second gain (K p ) is multiplied by the second gain unit 236. p ) and outputs the result to the third calculation unit 237.

[0105] The third calculation unit 237 calculates the target wire length φ23(l d), the output result of the first gain unit 234 (the first differential of the wire speed difference), and the output result of the second gain unit 236 (the second differential of the wire length difference). The third calculation unit 237 calculates the target wire length φ23(l d ), the output result of the first gain unit 234 (the first derivative of the wire speed difference), and the output result of the second gain unit 236 (the second derivative of the wire length difference) are added together. The result of adding these together is the target wire acceleration.

[0106] The dynamics calculation unit 238 calculates the structural matrix φ29(A T ), target wire length acceleration, and measurement wire length φ15 (l m ) (measured wire speed) and the first derivative of the command tension φ28 (f c The dynamics calculation unit 238 outputs the command tension φ28(f c ) is output.

[0107] <Mobile object control method> Next, a mobile object control method performed by the mobile object control device 1 will be described with reference to the flowchart shown in FIG.

[0108] The moving body control device 1 is configured to control the command torque φ14(τ c ) to the winch 5 (step S1), and the correction wire length φ42 (l m,cmp ) (Step S2), and the wire length correction value φ36 (Δl FB ) (Step S3), and repeat the above steps. Each of Steps S1, S2, and S3 will be explained below.

[0109] <Command torque φ14(τ c ) to the winch 5 (Step S1) Command torque φ14(τ c) to the winch 5 (step S1) is executed by the torque calculation unit 2. Step S1 includes a first command torque calculation operation (step S11), a first wire length correction operation (step S12), a second wire length correction operation (step S13), a second command torque calculation operation (step S14), and a third command torque calculation operation (step S15).

[0110] First, a first command torque calculation operation (step S11) is executed. In the first command torque calculation operation (step S11), the target position φ11 (X d ), target speed φ12, and target acceleration φ13 are used to calculate target wire acceleration φ21, target wire speed φ22, and target wire length φ23 (l d ) and the following is obtained. Step S11 is executed by the inverse kinematics calculation unit 21.

[0111] Next, a first wire length correction operation (step S12) is performed. In the first wire length correction operation (step S12), the wire length correction value φ36 (Δl FB ) to determine the target wire length φ23(l d Step S12 is executed by the wire length correction value adding unit 22.

[0112] Next, a second wire length correction operation (step S13) is performed. In the second wire length correction operation (step S13), the correction wire length φ42 (l m,cmp ) to determine the target wire length φ23(l d ) is further corrected. Step S13 is executed by the command tension calculation unit 23.

[0113] In the operation of repeating steps S1, S2, and S3, when step S1 is executed for the first time, steps S2 and S3 are not executed. FB ) and correction wire length φ42 (l m,cmp Therefore, when step S1 is executed for the first time, the wire length correction value φ36(Δl FB ) and correction wire length φ42 (l m,cmp) may be set to zero.

[0114] Next, a second command torque calculation operation (step S14) is executed. In the second command torque calculation operation (step S14), the target wire acceleration φ21, the target wire speed φ22, and the corrected target wire length φ24 (l d,FB ), and the command wire acceleration φ26, the command wire speed φ27, and the command tension φ28 (f c ) and the following is obtained. Step S14 is executed by the command tension calculation unit 23.

[0115] Then, a third command torque calculation operation (step S15) is executed. In the third command torque calculation operation (step S15), a command wire acceleration φ26, a command wire speed φ27, and a command tension φ28 (f c ) and the command torque φ14(τ c Step S15 is executed by the command torque calculation unit 24.

[0116] <Correction wire length φ42 (l m,cmp ) (Step S2) Correction wire length φ42 (l m,cmp The operation of obtaining the wire elongation compensation (step S2) is executed by the static wire elongation compensation unit 4. Step S2 includes a first wire elongation compensation calculation operation (step S21) and a second wire elongation compensation calculation operation (step S22).

[0117] First, a first wire elongation compensation calculation operation (step S21) is performed. In the first wire elongation compensation calculation operation (step S21), the corrected wire stiffness φ41 (K W ) is obtained. Step S21 is executed by the corrected wire stiffness calculation unit 41.

[0118] Next, a second wire elongation compensation calculation operation (step S22) is performed. In the second wire elongation compensation calculation operation (step S22), the corrected wire length φ42 (l m,cmp ) is obtained. Step S22 is executed by the corrected wire length calculation unit 42.

[0119] <Wire length correction value φ36(Δl FB ) (Step S3) Wire length correction value φ36(Δl FB The operation of obtaining the average tension integral feedback unit 3 (step S3) is executed by the average tension integral feedback unit 3. Step S3 includes a first wire length correction value calculation operation (step S31), a second wire length correction value calculation operation (step S32), and a third wire length correction value calculation operation (step S33).

[0120] First, a first wire length correction value calculation operation (step S31) is performed. In the first wire length correction value calculation operation (step S31), a tension error evaluation value φ34 is obtained. Step S31 is performed by the tension error evaluation value acquisition unit 31.

[0121] More specifically, first, the target average tension φ31(f c,avg ) is input. Target average tension φ31(f c,avg ) may be input by an operator operating the moving body control device 1 using the input device 505. c,avg ) may be input without the intervention of an operator as a calculated value obtained using measurement values ​​(for example, acceleration) obtained by various sensors equipped in the mobile system 101. Next, the measured average tension φ32(f m,avg ) is obtained. This operation is performed by the measured average tension calculation unit 311. Next, the average tension error φ33(Δf avg ) is obtained. This operation is performed by the average tension error calculation unit 312. Then, the evaluation value φ34 of the tension error is obtained. This operation is performed by the average tension error integration unit 313.

[0122] Next, a second wire length correction value calculation operation (step S32) is executed. In the second wire length correction value calculation operation (step S32), the distribution ratio vector φ35(ρ Δl Step S32 is executed by the distribution ratio vector calculation unit 32.

[0123] Then, a third wire length correction value calculation operation (step S33) is executed. In the third wire length correction value calculation operation (step S33), the wire length correction value φ36 (Δl FB Step S33 is executed by the wire length correction value calculation unit 33.

[0124] The configurations and effects of the mobile object control device 1, mobile object control system 100, mobile object system 101, mobile object control method, and mobile object control program PG explained in detail up to this point are as follows.

[0125] The mobile body control device 1 moves one mobile body 8 connected to the other end of the multiple wires 7 by adjusting the wire length, which is the length of the wire 7 unwound from the winch 5 due to the operation of multiple winches 5 connected to each of one ends of the multiple wires 7.

[0126] The moving body control device 1 includes a torque calculation unit 2 that receives information about the moving target of the moving body 8 and gives a control signal to the winch 5 to place the moving body 8 at a desired position, and a target average tension φ31(f c,avg ) and the measured tension φ16 (f m ) is the average of the measured average tension φ32(f m,avg ) and the condition for reducing the evaluation value φ34 of the tension error caused by the difference between FB ) to the torque calculation unit 2. The average tension integral feedback unit 3 provides the wire length correction value φ36(Δl FB ) is derived from the balance condition to maintain the balance of the forces acting on the moving body 8 at the desired position, and the distribution ratio vector φ35(ρ Δl ) is used to calculate for each of the multiple wires 7.

[0127] In this mobile body control device 1, the target average tension φ31(f c,avg ) and the measured average tension φ32 (f m,avg) so that the difference between the wire length correction value φ36 (Δl FB ) is calculated. The average tension integral feedback unit 3 calculates the wire length correction value φ36(Δl FB ) is calculated, the distribution ratio vector φ35(ρ Δl ) is used. In other words, the average tension integral feedback unit 3 uses the wire length correction value φ36 (Δl FB ) is calculated, the condition is adopted that the balance of the forces acting on the moving body 8 at the desired position is maintained. As a result, the wire length correction value φ36 (Δl FB ), it is possible to reduce deviations in tension while maintaining the position of the movable body 8. Therefore, the movable body control device 1 can simultaneously control the positioning of the movable body 8 and the tension.

[0128] Furthermore, the moving body control device 1 can increase the stiffness against in-plane displacement and the stiffness against out-of-plane displacement by increasing or decreasing the target average tension of the wires 7 while maintaining the tension balance of the multiple wires 7. As a result, the movement of the moving body 8 can be stably controlled.

[0129] Furthermore, according to the moving body control device 1, torque control is performed taking into consideration the elastic elongation due to the spring multiplier of each wire 7, thereby making it possible to improve the positional accuracy of the moving body 8.

[0130] The average tension integral feedback unit 3 of the mobile body control device 1 includes a tension error evaluation value acquisition unit 31 that obtains an evaluation value φ34 of the tension error, and a distribution ratio vector φ35(ρ Δl ) and a distribution ratio vector calculation unit 32 that calculates the tension error evaluation value φ34 and the distribution ratio vector φ35(ρ Δl ) to obtain the wire length correction value φ36(Δl FB ) and a wire length correction value calculation unit 33 for obtaining a suitable wire length correction value φ36(Δl FB ) can be obtained.

[0131] The tension error evaluation value acquisition unit 31 of the above-described moving body control device 1 calculates the tension error φ16 (f m ) is the average of the measured average tension φ32(f m,avg ) and a target average tension φ31(f c,avg ) and the measured average tension φ32 (f m,avg ) the average tension error φ33(Δf avg ) and an average tension error calculation unit 312 that calculates the average tension error φ33(Δf avg ) over time as the evaluation value φ34 of the tension error. FB ) can be obtained.

[0132] The torque calculation unit 2 of the above-mentioned moving body control device 1 calculates the target wire length φ23(l) by applying information about the moving target of the moving body 8 to the inverse kinematics model. d ) and the inverse kinematics calculation unit 21 obtains the target wire length φ23(l d ) and the measured wire length φ15 (l m ) and a second calculation unit 235 for calculating the difference between the target wire length φ23(l d ) with wire length correction value φ36(Δl FB The torque calculation unit 2 includes a wire length correction value addition unit 22 that adds up the measurement wire length φ15 (l m The torque calculation unit 2 and the average tension integral feedback unit 3 form a first feedback system including a second calculation unit 235 that obtains the difference between the wire length correction value φ36 (Δl FB ) is added to the wire length correction value adding unit 22. This configuration also makes it possible to reduce deviations in tension while maintaining the position of the movable body 8.

[0133] The moving body control device 1 further includes a static wire elongation compensation unit 4 that corrects the wire length obtained by measurement and is provided to the second calculation unit 235, which obtains the difference from the wire length included in the torque calculation unit 2 of the moving body control device 1. The static wire elongation compensation unit 4 corrects the wire length obtained by measurement and is provided to the second calculation unit 235, which obtains the difference from the wire length included in the torque calculation unit 2 of the moving body control device 1. m ) corresponding to the correction wire stiffness φ41 (K W ) for each of the plurality of wires 7, and a correction wire stiffness calculation unit 41 for calculating the correction wire stiffness φ41(K W ) and measurement wire length φ15 (l m ) and for each of the plurality of wires 7, the correction wire length φ42 (l m,cmp The torque calculation unit 2 includes a correction wire length calculation unit 42 that calculates the measurement wire length φ15 (l m The second calculation unit 235 obtains the difference between the value of the measurement wire length φ15(l m ) as a value for the correction wire length φ42 (l m,cmp ) is received.

[0134] The wire 7 may undergo elastic stretching in response to tension, or tension relaxation, which increases over time. This configuration can prevent a decrease in positioning accuracy due to stretching of the wire 7.

[0135] The mobile body control system 100 includes a rotary encoder 53 which is a wire length sensor that obtains the wire length, a tension sensor 62 which obtains the tension generated in each of the multiple wires 7 when the mobile body 8 is at a desired position, and a mobile body control device 1 which provides control signals to the multiple winches 5.

[0136] The mobile body system 101 comprises a plurality of wires 7, a plurality of winches 5 connected to one end of each of the plurality of wires 7, one mobile body 8 connected to the other end of each of the plurality of wires 7, and a mobile body control device 1 that provides control signals to the plurality of winches 5.

[0137] The moving body control method moves one moving body 8 connected to one end of the plurality of wires 7 by adjusting the wire length, which is the length of the wire 7 unwound from the winch 5 due to the operation of the plurality of winches 5 connected to each of the other ends of the plurality of wires 7. The moving body control method includes a first step (step S1) of receiving information about a movement target of the moving body 8 and providing a control signal to the winch 5 for placing the moving body 8 at a desired position, and a second step (step S2) of calculating a command tension φ28(f) required for each of the plurality of wires 7 obtained based on the information about the movement target of the moving body 8. c ) and the measured tension φ16 (f m ) and the condition for reducing the tension error caused by the difference between FB The second step (step S3) provides the wire length correction value φ36(Δl) to the torque calculation unit 2. FB ) is calculated for each of the wires 7 using a balance condition for maintaining the balance of the forces acting on the moving body 8 at a desired position.

[0138] The mobile object control program PG causes the computer 500 to execute control for moving one mobile object 8 connected to one end of the plurality of wires 7 by adjusting the wire length, which is the length of the wire 7 unwound from the winch 5 due to the operation of the plurality of winches 5 connected to each of the other ends of the plurality of wires 7. The mobile object control program PG then causes the computer 500 to control a torque calculation unit 2 that receives information regarding the movement target of the mobile object 8 and gives a control signal to the winch 5 for placing the mobile object 8 at a desired position, and a command tension φ28(f c ) and the measured tension φ16 (f m ) and the condition for reducing the tension error caused by the difference between FB) to the torque calculation unit 2. The average tension integral feedback unit 3 operates as the wire length correction value φ36 (Δl FB ) is calculated for each of the wires 7 using a balance condition for maintaining the balance of the forces acting on the moving body 8 at a desired position.

[0139] Similarly to the mobile body control device 1, the mobile body control system 100, the mobile body system 101, the mobile body control method, and the mobile body control program PG can also achieve both positioning control and tension control.

[0140] The mobile object control device, the mobile object control system, the mobile object system, the mobile object control method, and the mobile object control program of the present invention are not limited to the above-described embodiments.

[0141] For example, the static wire elongation compensation unit 4 may be omitted, as in a modified mobile body control device 1A shown in Figures 10 and 11. That is, the modified mobile body control device 1A includes a torque calculation unit 2 and an average tension integral feedback unit 3A.

[0142] Torque calculation part 2 is a correction wire length φ42 (l m,cmp ) instead of the measurement wire length φ15 (l m ) The other configurations are the same as those of the torque calculation unit 2 of the embodiment.

[0143] The average tension integral feedback unit 3A is a correction wire stiffness φ41 (K W ) is not affected. In other words, the distribution ratio vector calculation unit 32 of the average tension integral feedback unit 3A does not receive the structural matrix φ29(A T ) using only the distribution ratio vector φ35(ρ Δl ) is obtained. The other configurations are the same as those of the average tension integral feedback unit 3 of the embodiment.

[0144] The movable body control device 1A of the modified example can also perform both positioning control and tension control, similar to the movable body control device 1 of the embodiment. [Explanation of symbols]

[0145] 1, 1A... Mobile body control device (mobile body control unit), 2... Torque calculation unit (first calculation means), 21... Inverse kinematics calculation unit (means for obtaining target wire length), 22... Wire length correction value addition unit (means for adding together correction values), 23... Command tension calculation unit, 24... Command torque calculation unit, 3... Average tension integral feedback unit (second calculation means), 31... Tension error evaluation value acquisition unit (means for obtaining an evaluation value of a tension error), 32... Distribution ratio vector calculation unit (means for obtaining a distribution ratio), 33... Wire length correction value calculation unit (means for obtaining a correction value of the wire length), 4... Static wire elongation compensation unit (third calculation means), 41... Corrected wire stiffness calculation unit, 42... Corrected wire length calculation unit (means for obtaining a corrected wire length), 5... Winch (drive unit), 51...motor driver, 52...motor, 53...rotary encoder (wire length sensor), 6...pulley unit, 61...pulley, 62...tension sensor, 7...wire, 8...moving body, 100...moving body control system, 101...moving body system, 501...CPU, 311...measured average tension calculation unit (means for obtaining measured average tension), 312...average tension error calculation unit (means for obtaining average tension error), 313...average tension error integration unit (means for obtaining as an evaluation value of tension error), 500...computer, 501...CPU, 502...main memory unit, 503...auxiliary memory unit, 504...communication control unit, 505...input device, 506...output device, PG...moving body control program, φ11...target position (X d ), φ12...Target speed, φ13...Target acceleration, φ14...Command torque (τ c ), φ15...Measurement wire length (l m ), φ16...Measurement tension (f m ), φ21...target wire acceleration, φ22...target wire speed, φ23...target wire length (l d ), φ24...corrected target wire length (l d,FB ), φ26...command wire acceleration, φ27...command wire speed, φ28...command tension (f c ), φ29...Structure matrix (A T ), φ31…Target average tension (f c,avg ), φ32…Measurement average tension (f m,avg ), φ33…average tension error (Δf avg ), φ34...Evaluation value, φ35...Distribution ratio vector (ρΔl ), φ36...wire length correction value (Δl FB ), φ41... Correction wire stiffness (K W ), φ42...Correction wire length (l m,cmp ).

Claims

1. A mobile body control device for moving a single mobile body connected to one end of a plurality of wires by adjusting a wire length, which is a length of the wire unwound from a plurality of drive units due to operation of the drive units connected to the other end of the plurality of wires, comprising: a first calculation means for receiving information about a movement target of the moving body and giving a control signal to the driving unit for disposing the moving body at a desired position; a second calculation means for providing a condition for reducing an evaluation value of a tension error caused by a difference between a target average tension and a measured tension generated in each of the plurality of wires when the movable body is at the desired position, as a correction value for the wire length, to the first calculation means; The second calculation means calculates the correction value of the wire length for each of the plurality of wires, using a balance condition for maintaining balance of forces acting on the movable body at the desired position, to control the drive unit so that the target average tension is generated in the wire while maintaining the position of the movable body.

2. A mobile body control device for moving a single mobile body connected to one end of a plurality of wires by adjusting a wire length, which is the length of the wire unwound from a drive unit due to the operation of a plurality of drive units connected to each of the other ends of the plurality of wires, a first calculation means for receiving information about a movement target of the moving body and giving a control signal to the driving unit for disposing the moving body at a desired position; a second calculation means for providing a condition for reducing an evaluation value of a tension error caused by a difference between a target average tension and a measured tension generated in each of the plurality of wires when the movable body is at the desired position, as a correction value for the wire length, to the first calculation means; the second calculation means calculates the correction value of the wire length for each of the plurality of wires using a balance condition for maintaining a balance of forces acting on the movable body at the desired position; The second calculation means means for obtaining an evaluation value of the tension error; means for obtaining a distribution ratio derived from said balance condition; means for obtaining the correction value for the wire length using the estimated value of the tension error and the distribution ratio.

3. The means for obtaining the evaluation value of the tension error includes: a means for obtaining a measured average tension, which is an average of the measured tensions generated in each of the plurality of wires; a means for obtaining an average tension error, which is the difference between a target average tension and the measured average tension; 3. The mobile body control device according to claim 2, further comprising means for integrating the average tension error over time to obtain an evaluation value of the tension error.

4. A mobile body control device for moving a single mobile body connected to one end of a plurality of wires by adjusting a wire length, which is the length of the wire unwound from a drive unit due to the operation of a plurality of drive units connected to each of the other ends of the plurality of wires, comprising: a first calculation means for receiving information about a movement target of the moving body and giving a control signal to the driving unit for disposing the moving body at a desired position; a second calculation means for providing a condition for reducing an evaluation value of a tension error caused by a difference between a target average tension and a measured tension generated in each of the plurality of wires when the movable body is at the desired position, as a correction value for the wire length, to the first calculation means; a third calculation means for correcting the wire length obtained by measurement and provided to a means for obtaining a difference with the wire length included in the first calculation means; the second calculation means calculates, for each of the plurality of wires, the correction value of the wire length for controlling the drive unit so that the target average tension is generated in the wire while the position of the movable body is maintained, using a balance condition for maintaining a balance of forces acting on the movable body at the desired position; The first calculation means a means for obtaining a target wire length by applying information about a moving target of the moving body to an inverse kinematics model; means for obtaining a difference between the target wire length and a value relating to a measured wire length, which is the wire length obtained by measurement; means for adding the correction value to the target wire length; the first calculation means constitutes a first feedback system including means for obtaining a difference between the measured value and the value related to the measurement wire length; the first calculation means and the second calculation means constitute a second feedback system including a means for adding up the correction values; The third calculation means a means for obtaining a corrected wire stiffness corresponding to the measurement wire length for each of the plurality of wires; means for obtaining a corrected wire length for each of the plurality of wires using the corrected wire stiffness and the measured wire length; The means for obtaining a difference from the value related to the measurement wire length of the first calculation means receives the correction wire length as the value related to the measurement wire length.

5. A mobile object control system for moving a single mobile object connected to one end of a plurality of wires by adjusting a wire length, which is a length of the wire unwound from a plurality of drive units due to operation of the plurality of drive units connected to the other end of the plurality of wires, comprising: a wire length sensor for obtaining the wire length; a tension sensor for detecting tension generated in each of the plurality of wires when the moving body is at a desired position; a moving body control unit that supplies control signals to the plurality of driving units, The moving body control unit a first calculation means for receiving information about a movement target of the moving body and giving a control signal to the driving unit for disposing the moving body at a desired position; a second calculation means for providing a condition for reducing an evaluation value of a tension error caused by a difference between a target average tension and a measured tension generated in each of the plurality of wires when the movable body is at the desired position, to the first calculation means as a correction value for the wire length; a second calculation means for calculating, for each of the plurality of wires, the correction value of the wire length for controlling the drive unit so that the target average tension is generated in the wire while maintaining the position of the movable body, using a balance condition for maintaining balance of forces acting on the movable body at the desired position.

6. A plurality of wires; a plurality of driving units connected to one end of each of the plurality of wires; a moving body connected to the other end of the plurality of wires; a moving body control unit that provides a control signal to the plurality of driving units to move one of the moving bodies connected to the other ends of the plurality of wires by adjusting a wire length, which is the length of the wires unwound from the driving units, due to the operation of the driving units; The moving body control unit a first calculation means for receiving information about a movement target of the moving body and giving a control signal to the driving unit for disposing the moving body at a desired position; a second calculation means for providing a condition for reducing an evaluation value of a tension error caused by a difference between a target average tension and a measured tension generated in each of the plurality of wires when the movable body is at the desired position, to the first calculation means as a correction value for the wire length; a second calculation means for calculating, for each of the plurality of wires, the correction value of the wire length for controlling the drive unit so that the target average tension is generated in the wire while maintaining the position of the movable body, using a balance condition for maintaining balance of forces acting on the movable body at the desired position.

7. A moving body control method for moving a single moving body connected to one end of a plurality of wires by adjusting a wire length, which is a length of the wire unwound from a plurality of drive units due to operation of the drive units, each drive unit being connected to one end of the plurality of wires, the method comprising: a first step of receiving information about a movement target of the moving body and providing a control signal to the driving unit to place the moving body at a desired position; a second step of returning a condition for reducing an evaluation value of a tension error caused by a difference between a target average tension and a measured tension generated in each of the plurality of wires when the movable body is at the desired position to the processing of the first step as a correction value for the wire length, The second step of the mobile body control method calculates the correction value of the wire length for each of the plurality of wires using a balance condition for maintaining balance of forces acting on the mobile body at the desired position, in order to control the drive unit so that the target average tension is generated in the wire while maintaining the position of the mobile body.

8. A mobile object control program that causes a computer to execute control for moving one mobile object connected to one end of a plurality of wires by adjusting a wire length, which is a length of the wire unwound from a plurality of drive units due to operation of the plurality of drive units connected to the other end of the plurality of wires, The computer a first calculation means for receiving information about a movement target of the moving body and giving a control signal to the driving unit for disposing the moving body at a desired position; a second calculation means that provides a condition for reducing an evaluation value of a tension error caused by a difference between a target average tension and the measured tension generated in each of the plurality of wires when the movable body is at the desired position as a correction value for the wire length to the first calculation means; The second calculation means calculates the correction value of the wire length for each of the plurality of wires by using a balance condition for maintaining the balance of forces acting on the movable body at the desired position, in order to control the drive unit so that the target average tension is generated in the wire while maintaining the position of the movable body.

Images