CN101226405A - Two-dimension positioning control system based on ultrasound wave source - Google Patents

Abstract

The invention relates to a two-dimension positioning control system based on ultrasonic source, which comprises two static ultrasonic base stations, an ultrasonic receiver and a positioning controller mounted on a machine in a controlled two-dimension plane, while the ultrasonic base stations receive ultrasonic signals and transfer the ultrasonic signals, the positioning controller comprises a distance calculation module for calculating the distances L1, L2 between current point and the two ultrasonic base stations, and the distances L3, L4 between target point and the two ultrasonic base stations, a data memory module for storing the distances L, L1, L2, L3, L4 between the two static ultrasonic base stations, and a positioning movement control module for moving X and Y axis to move the machine from current point to target current, and outputting the distance control command of the X and Y axis to the X-axis controlled motor and Y-axis controlled motor of the machine. The invention provides a two-dimension positioning control system based on ultrasonic source, with high positioning accuracy and high practicality.

Description

Two-dimension positioning control system based on ultrasound source

Technical field

The present invention relates to a kind of two-dimension positioning control system.

Background technology

More common unmanned agricultural makes machine or robot is telecommunication and the control that realizes data command and video signal by the radio magnetic wave communication technology, the problem that exists has two: the one, and it is remote-operated that on-the-spot explosive release is subjected to radio magnetic wave often, the operation of the wireless electromagnetic of robot will may influence explosive release greatly, in addition cause misquote quick-fried; The 2nd, there is certain difficulty in the electromagnetic wave technology on the location, and the automatic running of making machine as unmanned agricultural often needs the place running of comparison rule road warp, to self positioning requirements than higher.

Summary of the invention

For the low deficiency of bearing accuracy that overcomes existing two-dimensional positioning system, the invention provides a kind of bearing accuracy height, practical two-dimension positioning control system based on ultrasound source.

The technical solution adopted for the present invention to solve the technical problems is:

A kind of two-dimension positioning control system based on ultrasound source, comprise two static ultrasound wave base stations, be installed in the ultrasonic transmitter-receiver mechanically and the register control of controlled two dimensional surface operation, described ultrasound wave base station transmitted ultrasonic wave signal after receiving ultrasonic signal, described register control comprises: distance calculation module, be used for sending the time and the ultrasound wave time that receives the forwarding of static ultrasound wave base station of ultrasonic signal according to ultrasonic transmitter-receiver, according to hyperacoustic velocity of propagation, calculate the distance L 1 and the L2 of current point and two ultrasound wave base stations, the distance L 3 and the L4 of impact point and two ultrasound wave base stations; Data memory module is used to store the distance L 1 of two distance L, current point and two ultrasound wave base stations between the static ultrasound wave base station and the distance L 3 and the L4 of L2 and impact point and two ultrasound wave base stations;

Locate mobile control module, be used for when machinery when current point moves to impact point, move by X-axis and y-axis shift and to realize, wherein:

$X=|X1-\mathrm{<figure-callout\; id="2"\; label="X"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">X</figure-callout>}2|=|\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2}{2L}-\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{L3}^2-{L4}^2}{2L}|$

$=\left|\frac{{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2-{L3}^3+{L4}^2}{2L}\right|---\left(5\right)$

$Y=|Y1-\mathrm{<figure-callout\; id="2"\; label="Y"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">Y</figure-callout>}2|=|\sqrt{{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\left(\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2}{2L}\right)}^2}-\sqrt{{L3}^2-{\left(\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{L3}^2-{L4}^2}{2L}\right)}^2}|---\left(6\right)$

With described X-axis and moving X-axis controlled motor and the Y-axis controlled motor that outputs to machinery apart from steering order of y-axis shift.

Technical conceive of the present invention is: the introducing of ultrasonic technology has just in time solved problems of the prior art, ultrasound wave belongs to a kind of mechanical wave, when ultrasound wave during at spatial transmission, if running into barrier just can reflect, speed and the sound wave propagated are similar, if the mistiming that the ultrasound wave of recording strip modulation sends and receives, just can be fairly simple measure distance with barrier.

As Fig. 1, made up two static ultrasound wave base station JZ1, JZ2, each base station can be received and dispatched ultrasonic signal simultaneously, and the ultrasonic signal that sends is to be with code modulatedly, and can be moved ultrasound source has the reception of distinguishing.Removable ultrasound source be framework on removable mechanical M, this mechanically moving M can be that stepper motor driven robot or agricultural are made machine, removable ultrasound source also can be received and dispatched ultrasonic signal simultaneously, the ultrasonic signal that sends also be with code modulated.In this two dimensional surface, enough strong of the band coded modulation ultrasonic signal that base station and removable ultrasound source send promptly adopted high-power ultrasound generator, and the ultrasonic signal that removable ultrasound source sends can be received by two base stations simultaneously.As Fig. 2, when removable ultrasound source during at the A point, can through type 1 and formula 2 obtain:

The coordinate that A is ordered (X1, Y1), wherein:

$\cos \mathrm{\&alpha;}=\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2}{2L.\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}$

$\mathrm{<figure-callout\; id="1"\; label="X"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">X</figure-callout>}1=\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2}{2L}---\left(1\right)$

$\mathrm{<figure-callout\; id="1"\; label="Y"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">Y</figure-callout>}1=\sqrt{{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\left(\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2}{2L}\right)}^2}---\left(2\right)$

L is two distances between stationary base station; L1 is the distance of the mobile ultrasound source and the first stationary base station JZ1; L2 is the distance of the mobile ultrasound source and the second stationary base station JZ2; α is the angle of the mobile ultrasound source and the first stationary base station JZ1;

Removable ultrasound source when the B point, can through type 3 and formula 4 obtain: the coordinate that B is ordered (X2, Y2), wherein:

$\cos \mathrm{\&beta;}=\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{L3}^2-{L4}^2}{2L.L3}$

$\mathrm{<figure-callout\; id="2"\; label="X"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">X</figure-callout>}2=\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{L3}^2-{L4}^2}{2L}---\left(3\right)$

$\mathrm{<figure-callout\; id="2"\; label="Y"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">Y</figure-callout>}2=\sqrt{{L3}^2-{\left(\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{L3}^2-{L4}^2}{2L}\right)}^2}---\left(4\right)$

L is two distances between stationary base station; L3 is the distance of the mobile ultrasound source and the first stationary base station JZ1; L4 is the distance of the mobile ultrasound source and the second stationary base station JZ2; β is the angle of the mobile ultrasound source and the first stationary base station JZ1;

When mobile ultrasound source when the A point moves to the B point, can move respectively by X-axis and Y-axis and realize, from an A (X1, Y1) move to a B (X2 Y2) needs to move on X-axis:

$X=|X1-\mathrm{<figure-callout\; id="2"\; label="X"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">X</figure-callout>}2|=|\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2}{2L}-\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{L3}^2-{L4}^2}{2L}|$

$=\left|\frac{{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2-{L3}^3+{L4}^2}{2L}\right|---\left(5\right)$

From an A (X1, Y1) move to a B (X2 Y2) needs to move on Y-axis:

$Y=|Y1-\mathrm{<figure-callout\; id="2"\; label="Y"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">Y</figure-callout>}2|=|\sqrt{{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\left(\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2}{2L}\right)}^2}-\sqrt{{L3}^2-{\left(\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{L3}^2-{L4}^2}{2L}\right)}^2}|---\left(6\right)$

Can realize the moving freely and locating in two dimensional surface of removable ultrasound source by above method.

Beneficial effect of the present invention mainly shows: bearing accuracy height, practical.

Description of drawings

Fig. 1 is the X-Y scheme of mobile ultrasound source and the base station two-way communication of static ultrasound wave.

Fig. 2 is the location map of removable ultrasound source in two dimensional surface.

Embodiment

Below in conjunction with accompanying drawing the present invention is further described.

See figures.1.and.2, a kind of two-dimension positioning control system based on ultrasound source, comprise two static ultrasound wave base station JZ1, JZ2, be installed in ultrasonic transmitter-receiver and register control on the mechanical M of controlled two dimensional surface operation, described ultrasound wave base station transmitted ultrasonic wave signal after receiving ultrasonic signal, described register control comprises: distance calculation module, be used for sending the time and the ultrasound wave time that receives the forwarding of static ultrasound wave base station of ultrasonic signal according to ultrasonic transmitter-receiver, according to hyperacoustic velocity of propagation, calculate the distance L 1 and the L2 of current some A and two ultrasound wave base stations, the distance L 3 and the L4 of impact point B and two ultrasound wave base stations; Data memory module is used to store the distance L 1 of two distance L, current point and two ultrasound wave base stations between the static ultrasound wave base station and the distance L 3 and the L4 of L2 and impact point and two ultrasound wave base stations;

Locate mobile control module, be used for when mechanical M when current some A moves to impact point B, move by X-axis and y-axis shift and to realize, wherein:

$X=|X1-\mathrm{<figure-callout\; id="2"\; label="X"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">X</figure-callout>}2|=|\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2}{2L}-\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{L3}^2-{L4}^2}{2L}|$

$=\left|\frac{{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2-{L3}^3+{L4}^2}{2L}\right|---\left(5\right)$

$Y=|Y1-\mathrm{<figure-callout\; id="2"\; label="Y"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">Y</figure-callout>}2|=|\sqrt{{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\left(\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2-{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2}{2L}\right)}^2}-\sqrt{{L3}^2-{\left(\frac{{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="S2008100593139E00011.png"\; state="\{\{state\}\}">L</figure-callout>}}^2+{L3}^2-{L4}^2}{2L}\right)}^2}|---\left(6\right)$

With described X-axis and moving X-axis controlled motor and the Y-axis controlled motor that outputs to machinery apart from steering order of y-axis shift.

With movable small agricultural rice cutting machine is example, determines two stationary base station point JZ1, JZ2 earlier on the limit, rice field, installs behind the high power static ultrasonic generator and records the distance of two base sites, makes the mobile ultrasonic generator of installation on the machine in mobile small-sized agricultural.Suppose to pass through program setting, need agricultural to make machine in starting point (3,0) locates vertical X-axis and advance to point (3 along Y direction, 10) locate to stop, removable ultrasound source needs only the distance of real-time measurement and two stationary base station, calculate by the mobile control module in the location of inside, needing on X-axis and the Y-axis to obtain the distance that moves, commander's motor rotates and just can arrive the destination smoothly.

Claims (1)

1. two-dimension positioning control system based on ultrasound source, it is characterized in that: comprise two static ultrasound wave base stations, be installed in the ultrasonic transmitter-receiver mechanically and the register control of controlled two dimensional surface operation, described ultrasound wave base station transmitted ultrasonic wave signal after receiving ultrasonic signal, described register control comprises:

Distance calculation module, be used for sending the time and the ultrasound wave time that receives the forwarding of static ultrasound wave base station of ultrasonic signal according to ultrasonic transmitter-receiver, according to hyperacoustic velocity of propagation, calculate the distance L 1 and the L2 of current point and two ultrasound wave base stations, the distance L 3 and the L4 of impact point and two ultrasound wave base stations;

Data memory module is used to store the distance L 1 of two distance L, current point and two ultrasound wave base stations between the static ultrasound wave base station and the distance L 3 and the L4 of L2 and impact point and two ultrasound wave base stations;

Locate mobile control module, be used for when machinery when current point moves to impact point, move by X-axis and y-axis shift and to realize, wherein:

$X=|X1-X2|=|\frac{L^2+{L1}^2-{L2}^2}{2L}-\frac{L^2+{L3}^2-{L4}^2}{2L}|$

$=\left|\frac{{L1}^2-{L2}^2-{L3}^3+{L4}^2}{2L}\right|---\left(5\right)$

$Y=|Y1-Y2|=|\sqrt{{L1}^2-{\left(\frac{L^2+{L1}^2-{L2}^2}{2L}\right)}^2}-\sqrt{{L3}^2-{\left(\frac{L^2+{L3}^2-{L4}^2}{2L}\right)}^2}|---\left(6\right)$

With described X-axis and moving X-axis controlled motor and the Y-axis controlled motor that outputs to machinery apart from steering order of y-axis shift.

Images