JP2011134280A - Positioning device and positioning method - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a positioning device achieving rapid and accurate positioning. <P>SOLUTION: The positioning device includes: a coarse motion mechanism 1; a fine-motion mechanism 2 smaller in unit displacement than the coarse motion mechanism 1; a coarse motion mechanism control unit 10 which controls the coarse motion mechanism 1 by a coarse motion mechanism control signal based on a sensor signal related to at least one of position, angle, speed and angular speed of the coarse motion mechanism 1; a fine-motion mechanism control unit 20 which controls the fine-motion mechanism by a fine-motion mechanism control signal based on a sensor signal related to at least one of position, angle, speed and angular speed of the fine-motion mechanism 2; a storage unit which stores parameters for the coarse motion mechanism 1 and the fine-motion mechanism 2; and a tracking correction signal generation unit 30 which calculates an acceleration or angular acceleration of the fine-motion mechanism, which is generated by the operation of the coarse motion mechanism 1, from a coarse motion control signal, the sensor signal of the coarse motion mechanism, and the parameters for the coarse motion mechanism and fine-motion mechanism stored in the storage unit, generates a fine-motion tracking correction signal according to the acceleration or angular acceleration, and adds it to the fine-motion control signal. <P>COPYRIGHT: (C)2011,JPO&amp;INPIT

Description

  The present invention relates to a positioning device and a positioning method used for a target tracking device and the like.

  With the target tracking device, the target tracking range should be widened, the target moving at a faster speed can be tracked, and even a remote target or a small target can be tracked with high accuracy. Etc. are required. For this reason, the apparatus is often configured by combining a coarse movement mechanism and a fine movement mechanism. In general, a fixed part of the fine movement mechanism is attached on a movable part of the coarse movement mechanism.

  The coarse movement mechanism is large and needs to increase the mass and inertia of the movable part for reasons such as a wider operation range than the fine movement mechanism, and the response frequency must be lowered. Therefore, the tracking performance for the movement of the target of the coarse movement mechanism is lower than the tracking performance for the movement of the target of the fine movement mechanism. Conventionally, even if the fine movement mechanism is mounted on a moving part of a coarse movement mechanism that does not have a good tracking performance with respect to the movement of the target, the movement speed of the target is not so high, and the tracking with respect to the movement of the target is performed. Since even a certain amount of error was allowed, the moving target could be tracked by the tracking performance of the fine movement mechanism.

  On the other hand, in order to realize high-speed and high-accuracy positioning performance and target tracking performance, the following publicly known documents can be cited. Patent Document 1 proposes a control method for improving response characteristics by constructing an adaptive control system with a built-in model for an apparatus having mechanical system resonance or the like. In Patent Document 2, an inertial sensor signal is feedback-controlled to increase the accuracy and bandwidth of a spatial stability function against disturbances such as shaking. In Patent Document 3, the spatial stability accuracy against disturbance such as shaking is improved by feedback control of the signal of the angular acceleration sensor.

JP 2005-267296 A JP 2004-145422 A Japanese Patent No. 3203855

  However, when tracking is performed on a target moving at a higher speed or when tracking is performed with higher accuracy, the tracking performance of the coarse motion mechanism and the fine motion mechanism is not sufficient. It has been difficult to reduce it sufficiently.

  The present invention has been made paying attention to the above circumstances, and an object thereof is to provide a positioning device and a positioning method capable of tracking a target moving at high speed with high accuracy. .

  In order to achieve the above object, a positioning apparatus according to the present invention includes a coarse movement mechanism, a fine movement mechanism having a smaller unit displacement than the coarse movement mechanism, and at least one of a position, an angle, a speed, and an angular velocity of the coarse movement mechanism. A coarse movement mechanism control unit for controlling the coarse movement mechanism by a coarse movement mechanism control signal based on a sensor signal relating to the two, and a fine movement mechanism control based on a sensor signal relating to at least one of position, angle, speed and angular velocity of the fine movement mechanism A fine movement mechanism control section controlled by a signal; a storage section storing parameters relating to the coarse movement mechanism and the fine movement mechanism; a coarse movement control signal; a sensor signal of the coarse movement mechanism; and a coarse movement stored in the storage section. The acceleration or angular acceleration of the fine movement mechanism generated by the operation of the coarse movement mechanism based on the equation of motion of the mechanism from the parameters relating to the movement mechanism and the fine movement mechanism A tracking correction signal generating unit that calculates and generates a fine movement mechanism tracking correction signal according to the acceleration or angular acceleration to reduce a tracking error with respect to the target of the fine movement mechanism unit and adds it to the fine movement mechanism control signal; It has.

  In addition, the positioning method according to the present invention provides a coarse motion mechanism, a fine motion mechanism having a smaller unit displacement than the coarse motion mechanism, and sensor signals relating to at least one of the position, angle, speed, and angular velocity of the coarse motion mechanism. A coarse movement mechanism control unit for controlling the coarse movement mechanism by a coarse movement mechanism control signal based thereon, and a fine movement controlled by a fine movement mechanism control signal based on a sensor signal related to at least one of position, angle, velocity and angular velocity of the fine movement mechanism. A method used in a positioning device including a mechanism control unit, storing parameters relating to the coarse movement mechanism and the fine movement mechanism, and storing the coarse movement control signal and a sensor signal of the coarse movement mechanism in the storage unit. Parameters of the coarse mechanism and the fine mechanism are added to the fine mechanism generated by the operation of the coarse mechanism based on the motion equation of the mechanism. A degree or angular acceleration is calculated, a fine movement mechanism tracking correction signal for reducing a tracking error with respect to the target of the fine movement mechanism unit is generated according to the acceleration or angular acceleration, and added to the fine movement control signal. .

  Therefore, according to the present invention, it is possible to provide a positioning device and a positioning method capable of tracking with high accuracy a target moving at high speed.

The block diagram which shows one Embodiment of the positioning device which concerns on this invention. The functional block diagram of the positioning device shown in FIG. The flowchart which shows operation | movement of the tracking correction signal generation part shown in FIG. The figure which shows the structural example of a coarse movement mechanism and a fine movement mechanism.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.

  In FIG. 1, the block diagram which shows one Embodiment of the positioning device based on this invention is shown. FIG. 2 shows a functional block diagram of this positioning device.

  The coarse / fine movement mechanism unit 5 includes a coarse movement mechanism 1 and a fine movement mechanism 2. The coarse movement mechanism 1 is a mechanism for directing the position and direction of an object at high speed within a wide movement range. The fine movement mechanism 2 has a smaller unit displacement than the coarse movement mechanism 1 and is precise within a narrow movement range. It is a mechanism to match the correct position and direction.

  The control drive unit 50 includes a coarse movement mechanism control drive unit 10, a fine movement mechanism control drive unit 20, and a tracking correction signal generation unit 30. The coarse movement mechanism control drive unit 10 controls and drives the coarse movement mechanism 1 using a coarse movement mechanism control signal based on sensor signals relating to the position, angle, speed, angular velocity, and the like of the coarse movement mechanism 1. The fine movement mechanism control drive unit 20 controls and drives the fine movement mechanism 2 by a fine movement mechanism control signal based on sensor signals relating to the position, angle, speed, angular velocity, and the like of the fine movement mechanism 2.

  The tracking correction signal generation unit 30 includes a fine movement mechanism tracking correction signal calculation unit 31 and a mechanism parameter storage unit 32. In the mechanism parameter storage unit 32, for example, the mass of the movable part, the moment of inertia, the position of the center of gravity, the type (number of translation, rotation) and the number of operation axes, and the installation of the operation axis for each of the coarse movement mechanism 1 and the fine movement mechanism The position and orientation, and the attachment position and orientation of the fine movement mechanism with respect to the coarse movement mechanism movable part are stored. The fine movement mechanism tracking correction signal calculation unit 31 stores the coarse movement mechanism drive signal and the coarse movement mechanism sensor signal input to and output from the coarse movement mechanism control system 11 in the coarse movement mechanism control drive unit 10 and the mechanism parameter storage unit 32. From the coarse motion mechanism parameters and the fine motion mechanism parameters, the acceleration or angular acceleration of the fine motion mechanism 2 generated by operating the coarse motion mechanism 1 is calculated based on the motion equation of the mechanism, and the coarse motion fine motion mechanism section The fine movement mechanism tracking correction signal for reducing the tracking error with respect to the target 5 is generated according to the acceleration or the angular acceleration, and is output from the fine movement mechanism control system 21 in the fine movement mechanism control drive unit 20. Add to signal.

Next, the operation of the positioning device configured as described above will be described.
FIG. 3 shows a flowchart of the operation in the tracking correction signal generator 30 described above. FIG. 4 shows a configuration example in the case where the coarse motion mechanism 1 has two rotational axes and the fine motion mechanism 2 has two rotational axes. In the present embodiment, the positioning device having the configuration shown in FIG. 4 will be described as an example. However, the present invention is not limited to this configuration, and similarly applies to a configuration in which the operation axis is one rotation axis or three rotation axes. Applicable.

  In FIG. 3, the tracking correction signal generation unit 30 reads the parameter values of the coarse movement mechanism 1 and the fine movement mechanism 2 from the mechanism parameter storage unit 32 in advance at the time of activation (step S1).

  The following process is repeated for each control time step of the fine movement mechanism 2.

  First, the fine movement mechanism tracking correction signal calculation unit 31 takes in the coarse movement mechanism control signal and the coarse movement mechanism sensor signal from the coarse movement mechanism control system 11 (step S2).

Next, fine movement mechanism tracking correction signal calculation unit 31 calculates the acceleration or angular acceleration of fine movement mechanism 2 caused by moving coarse movement mechanism 1 based on the equation of motion of the mechanism, and reduces the tracking error of fine movement mechanism 2. A fine movement mechanism tracking correction signal is generated (step S3). In the following, the equation of motion of the coarse movement mechanism when the movement axis of the coarse movement mechanism 1 is two rotation axes is shown in the following equation.

  Next, the acceleration or angular acceleration of the movement axis of the fine movement mechanism is calculated from the direction cosine of the angle formed by the direction of the movement axis of the coarse movement mechanism and the movement axis of the fine movement mechanism. Hereinafter, as shown in FIG. 4, the first and second operation axes of the fine movement mechanism 2 when the movement axis of the coarse movement mechanism 1 is two rotation axes and the movement axis of the fine movement mechanism 2 is two rotation axes. An example of the calculation formula of angular acceleration is shown.

Angular acceleration of the first movement axis of the fine movement mechanism =
Angular acceleration of the first movement axis of the coarse movement mechanism × cos (θ11)
+ Angular acceleration of the second movement axis of the coarse movement mechanism × cos (θ21)
Angular acceleration of the second movement axis of the fine movement mechanism =
Angular acceleration of the first movement axis of the coarse movement mechanism × cos (θ12)
+ Angular acceleration of the second movement axis of the coarse movement mechanism × cos (θ22)
here,
θ11: angle formed between the first operation axis of the coarse movement mechanism and the first movement axis of the fine movement mechanism
θ12: angle formed between the first operation axis of the coarse movement mechanism and the second movement axis of the fine movement mechanism
θ21: angle formed between the second operation axis of the coarse movement mechanism and the first movement axis of the fine movement mechanism
θ22: Angle formed between the second operation axis of the coarse movement mechanism and the second movement axis of the fine movement mechanism Next, the tracking error of the fine movement mechanism 2 is reduced from the acceleration or angular acceleration of the movement axis of the coarse movement mechanism 1. A fine movement mechanism tracking correction signal is generated. An example of a calculation formula of a fine movement mechanism tracking correction signal when the movement axis of the fine movement mechanism 2 is two rotation axes will be shown.
Tracking correction signal of first axis of fine movement mechanism = angular acceleration of first movement axis of fine movement mechanism × first gain
The second axis tracking correction signal of the fine movement mechanism = the angular acceleration of the second movement axis of the fine movement mechanism × the second gain The fine movement mechanism tracking correction signal calculation unit 31 uses the fine movement mechanism tracking correction signal calculated in step S3. This is added to the fine movement mechanism control signal (step S4). As described above, the fine movement mechanism 2 is driven by the fine movement mechanism control signal to which the fine movement mechanism tracking correction signal is added, thereby improving the tracking performance of the coarse movement fine movement mechanism unit 5 with respect to the target moving at high speed. The fine movement mechanism tracking correction signal calculation unit 31 repeats the processes of steps S1 to S4 for each control time step until the operation of the coarse movement / fine movement mechanism unit 5 ends (step S5).

  As described above, in the above embodiment, the coarse motion control signal and the sensor signal of the coarse motion mechanism 1 and the parameters related to the coarse motion mechanism 1 and the fine motion mechanism are used to determine the fine motion mechanism 2 generated by the operation of the coarse motion mechanism 1. An acceleration or angular acceleration is calculated, and a fine movement mechanism tracking correction signal for reducing a tracking error with respect to the target of the coarse / fine movement mechanism unit 5 is generated according to the acceleration or angular acceleration and added to the fine movement control signal. ing.

  Therefore, according to the above embodiment, by using the acceleration and angular acceleration information of the fine movement mechanism 2 generated by the operation of the coarse movement mechanism 1, the moving direction of the target can be estimated, and the target moving at high speed can be estimated. Thus, a positioning device that can be tracked with high accuracy can be realized.

  Note that the present invention is not limited to the above-described embodiment as it is, and can be embodied by modifying the constituent elements without departing from the scope of the invention in the implementation stage. In addition, various inventions can be formed by appropriately combining a plurality of constituent elements disclosed in the embodiment. For example, some components may be deleted from all the components shown in the embodiment. Furthermore, you may combine suitably the component covering different embodiment.

  DESCRIPTION OF SYMBOLS 5 ... Coarse movement fine movement mechanism part, 1 ... Coarse movement mechanism, 2 ... Fine movement mechanism, 50 ... Control drive part, 10 ... Coarse movement mechanism control drive part, 20 ... Fine movement mechanism control drive part, 30 ... Tracking correction signal generation part, DESCRIPTION OF SYMBOLS 11 ... Coarse movement mechanism control system, 12 ... Coarse movement mechanism drive system, 21 ... Fine movement mechanism control system, 22 ... Fine movement mechanism drive system, 31 ... Fine movement mechanism tracking correction signal calculation part, 32 ... Mechanism parameter storage part

Claims (2)

Coarse motion mechanism;
A fine movement mechanism having a smaller unit displacement than the coarse movement mechanism;
A coarse movement mechanism control unit for controlling the coarse movement mechanism by a coarse movement mechanism control signal based on a sensor signal related to at least one of the position, angle, speed, and angular velocity of the coarse movement mechanism;
A fine movement mechanism control unit for controlling by a fine movement mechanism control signal based on a sensor signal related to at least one of the position, angle, speed and angular velocity of the fine movement mechanism;
A storage unit for storing parameters relating to the coarse movement mechanism and the fine movement mechanism;
The coarse motion control signal, the sensor signal of the coarse motion mechanism, and the parameters related to the coarse motion mechanism and the fine motion mechanism stored in the storage unit are generated by the operation of the coarse motion mechanism based on the motion equation of the mechanism. Calculates the acceleration or angular acceleration of the fine movement mechanism, generates a fine movement tracking correction signal for reducing the tracking error of the fine movement mechanism with respect to the target according to the acceleration or angular acceleration, and adds it to the fine movement mechanism control signal And a tracking correction signal generating unit. The coarse motion is controlled by a coarse motion mechanism, a fine motion mechanism having a smaller unit displacement than the coarse motion mechanism, and a coarse motion mechanism control signal based on a sensor signal related to at least one of position, angle, speed, and angular velocity of the coarse motion mechanism. Used in a positioning device including a coarse movement mechanism control section that controls a mechanism and a fine movement mechanism control section that is controlled by a fine movement mechanism control signal based on a sensor signal related to at least one of the position, angle, speed, and angular velocity of the fine movement mechanism. A method,
Storing parameters relating to the coarse movement mechanism and the fine movement mechanism;
The coarse motion control signal, the sensor signal of the coarse motion mechanism, and the parameters related to the coarse motion mechanism and the fine motion mechanism stored in the storage unit are generated by the operation of the coarse motion mechanism based on the motion equation of the mechanism. The acceleration or angular acceleration of the fine movement mechanism is calculated, and a fine movement mechanism tracking correction signal for reducing a tracking error with respect to the target of the fine movement mechanism unit is generated according to the acceleration or angular acceleration and added to the fine movement control signal. A positioning method characterized by the above.

Images