CN108287524A - A kind of high precision plane by stages positioning device based on MPU6050 - Google Patents
A kind of high precision plane by stages positioning device based on MPU6050 Download PDFInfo
- Publication number
- CN108287524A CN108287524A CN201810024530.8A CN201810024530A CN108287524A CN 108287524 A CN108287524 A CN 108287524A CN 201810024530 A CN201810024530 A CN 201810024530A CN 108287524 A CN108287524 A CN 108287524A
- Authority
- CN
- China
- Prior art keywords
- positioning
- section
- anchor point
- point
- plane
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/402—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by control arrangements for positioning, e.g. centring a tool relative to a hole in the workpiece, additional detection means to correct position
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/37—Measurements
- G05B2219/37404—Orientation of workpiece or tool, surface sensor
Landscapes
- Engineering & Computer Science (AREA)
- Human Computer Interaction (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Position Input By Displaying (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Abstract
The high precision plane by stages positioning device based on MPU6050 that the invention discloses a kind of, which includes the following contents：Specific positioning device is built, which includes the components such as suspension transmission device, carbon pipe, sensor MPU6050, and build its mathematical model.After obtaining mathematical model, positioning plane is divided into 4 sections to solve positioning Dead Core Problems.Angular speed and acceleration information are obtained by sensor MPU6050 and calculate roll angle and yaw angle with known Quaternion Algorithm.After obtaining angle value, section where judgement anchor point, then normalized to the inclination value of anchor point in section one by normalization algorithm.The polar value that current anchor point present position is calculated finally by the solution formula that front mathematical modeling obtains, completes one-time positioning.The device can be used as the feedback element of plane positioning in process control, also can be used as a kind of new handwriting input tool.
Description
Technical field
The invention belongs to process control plane positioning field more particularly to a kind of high precision planes based on MPU6050 point
Deciding field device.
Background technology
With the development of modern automation science and technology, traditional industry just gradually makes the transition to automatic industrial, and production level is just
It is continuously improved, plane positioning technology has become industrial products production and a very important crucial skill in development process
Art is widely used in industrial process control, including lathe positioning, hard disc of computer positioning system, weaving
Industry, automatic tin soldering etc..Plane positioning is also used as the control terminal of many digital equipments, including remote controler, mouse etc..
Currently, the country in the research of plane positioning technology there is also many deficiencies, including the volume of positioning system it is excessive,
The problems such as structure is excessively complicated, price is prohibitively expensive, this makes the popularization of two-dimensional positioning system be greatly limited.And make
It uses MPU6050 to be capable of providing accurate location information as the two-dimensional positioning system that state sensor is built, there is structure letter
The advantages such as single, cheap, registration and exploitation popularization, can be as a closed loop of most of plane control system
Feedback element, such as mechanical hand.On the other hand, the control signal as number setting, the two-dimensional positioning system can also be set
It is calculated as a kind of new electronic handwritten input tool.
Invention content
The present invention uses MPU6050 3-axis accelerations gyroscope as attitude transducer, is handled and is adopted using Kalman filtering
The data of collection, on specific device, by establishing model space geometric and carrying out geometric operation and interval division to model,
Design a kind of high-precision two-dimensional positioning system.
The technical solution used in the present invention is as follows：
(1) mechanical structure is built.As shown in Figure 1, the two-dimensional positioning system using universal joint by a hard carbon pipe with
Suspension fixed point links together.A MPU6050 sensor, sensor plane and positioning are installed in any position on carbon pipe
Plane keeping parallelism.The red dot laser pen of low-power, mark of the incident point as anchor point, incident point are installed in carbon pipe end
It is projeced into positioning plane, by swinging carbon pipe movable locating point to the specific position for positioning plane, MPU6050 will be acquired
Position data be transferred in STM32 systems by IIC agreements and carry out localization process, and by location information be transferred to host computer into
Row display.
(2) mathematical model of system, Preliminary design plane positioning algorithm are built.The system model is as shown in Fig. 2, wherein ∠
OAB and ∠ OAC are attitude angle of the MPU6050 input quantities after the resolving of quaternary number.Straight line AO is fixed value (actual suspension point to ground
The height in face).Then in △ AOB, there is OB=AO × tan (∠ OAB) there are OC=AO × tan (∠ in triangle AOC
OAC), due to OB=CD, therefore haveIt can then obtain ∠ DOC=arctan (DC/OC), thus just
Go out the polar coordinates (OD, ∠ DOC) of point D.
(3) Dead Core Problems are handled, interval variable is introduced.Due to function f=arctan (x) codomain do not include 90 ° and
270 °, when anchor point is close to 90 ° and 270 °, (DC/OC) will tend to be infinitely great, and computer system can not handle so big number
According to this can so that there are dead angles in localization region.In order to solve this problem, entire plane domain is divided into four portions by we
Point, and introduce new variable i in polar value and indicate section where anchor point, at this time in plane certain point positioning
Coordinate is (i, ρ, θ), wherein -45 °≤θ<45°.As shown in Fig. 3 (a), we are artificial to set two 0 ° in the positioning plane
Line, 0 ° of line for system, one is the positioning polar 0 ° of line in section.Can be precisely calculated by being derived by (1) by -45 °
The locator value of all the points within the scope of to 45 °, by this interval division be section 1, the point coordinates on this section be expressed as (1, ρ,
θ).When it is 45 ° to 135 ° to position section, 0 ° of line of system is moved at 90 °, as shown in Fig. 3 (b), department of computer science at this time
System is considered as 90 ° of lines of positioning plane on 0 ° of line of system.Positioning so to 45 ° to 135 ° will be instead of to 315 ° to 45 °
This section, is set as section 2, and location information is changed to (2, ρ, θ) by positioning.The rest may be inferred, and positioning section 3 is ranging from
135 ° to 225 °, location information is (3, ρ, θ)；Ranging from 225 ° to 315 ° of section 4 are positioned, location information is (4, ρ, θ).This
Sample realizes plane being accurately positioned without dead angle once solving Dead Core Problems.
Description of the drawings
Fig. 1 is Construction of A Model figure
Fig. 2 is mathematical model of the positioning system in a section
Fig. 3 is interval division figure
Fig. 4 positioning system flow charts
Specific implementation mode
It uses the real-time handwriting input device of the high precision plane positioning device based on MPU6050 as example below, and ties
It closes attached drawing the present invention is described in detail, specific implementation mode following steps：
Step 1：By STM32 initialize MPU6050, by IIC agreements read MPU6050 sensors acceleration with
Angular velocity information reduces systematic error, we introduce Kalman filtering to handle to improve the stability of system
The initial information of MPU6050, to reduce because the influence brought to system is interfered in temperature drift, vibrations etc..
Step 2：The initial information that MPU6050 is read is respectively the angular speed and acceleration of three axis, and we are final
What is obtained is the angle value of x-axis and y-axis, resolves attitude angle with existing Quaternion Algorithm at present herein, quaternary number is by one
Three element i, j, k compositions, quaternary number are generally represented by a+bi+cj+dk, and a2+b2+c2+d2=1 on a real add.It is false
If rigid body coordinate system is b systems, geographic coordinate system is R systems, then the coordinate of geographic coordinate system R to rigid body coordinate system b are transformed to：
The transform that quaternary number is converted into Eulerian angles is：
θ=arcsin (2 (q0q2-q1q3))(6)
The pitch angle (pitch) and roll angle of MPU6050 can be obtained using three axis angular rates and acceleration as input
(roll) and yaw angle (yaw).And the plane positioning system pertains only to the roll of MPU6050 with pitching movement without regard to it
The rotation of itself, therefore do not need yaw angle.
Step 3：Initialization system zero, after obtaining pitch angle (pitch) and roll angle (roll), in order to make positioning
More accurate, the anchor point where system initial position is set as system zero by us.
Step 4：After determining system zero, system enters hand-written station-keeping mode, and under this pattern, system will record laser
Track is simultaneously shown in liquid crystal display by the track of pen operation.
Step 5：Section where determining anchor point.Into after station-keeping mode, system is believed according to the angle of MPU6050 first
It ceases to judge which section anchor point belongs to, it is x, roll angle y to enable pitch angle, and anchor point is distinguished below according to four kinds of situations
Place section：
(1) if | x |>| y |, and x>0, then the point be in first interval, i=1；
(2) if | x |<| y |, and y<0, then the point be in first interval, i=2；
(3) if | x |>| y |, and x<0, then the point be in first interval, i=3；
(4) if | x |<| y |, and y>0, then the point be in first interval, i=4；
Step 6：Normalized, after having determined the section residing for anchor point, by the coordinate of current anchor point according to institute
The section at place normalizes in section one.Specifically implementation is：
Flow one：Section i where determining current anchor point
Flow two：If i=1, x and y are remained unchanged；
If i=2, x=-y, y=x are enabled；
If i=3, then x=-x, y=-y are enabled；
If i=4, then x=y, y=-x are enabled；
Step 7：After Unitary coordinate, the polar value of anchor point is calculated according to the model formation established, and is calculated
Formula is：
θ=arctan (tanx/tany) (8)
Wherein, (ρ, θ) be anchor point polar value, L be hitch point to position plane vertical range, x is pitch angle, y
For roll angle.
Step 6：After calculating current positioning coordinate by step 5, shown in real time as the display data of LED liquid crystal displays
It shows to come.Thus real-time hand-write input function is just realized.
Claims (5)
1. a kind of high precision plane by stages positioning device based on MPU6050, it is characterised in that the device includes step：
(1) mechanical structure, founding mathematical models are built；
(2) interval division is carried out, the Dead Core Problems in location model are solved；
(3) interval algorithm where judgement anchor point；
(4) anchor point normalizes and calculates anchor point coordinate value.
2. positioning device according to claim 1, which is characterized in that the step (1) builds mechanical structure, establishes mathematics
Model, specifically：
The system transposition structure and mathematical model such as Fig. 1, shown in 2, wherein ∠ OAB and ∠ OAC are MPU6050 input quantities through quaternary
Attitude angle after number resolving.Straight line AO is fixed value (height of actual suspension point to ground).Then in △ AOB, there is OB=AO
× tan (∠ OAB) has OC=AO × tan (∠ OAC), due to OB=CD, therefore has in triangle AOCIt then obtains ∠ DOC=arctan (DC/OC), thus just obtains polar coordinates (OD, the ∠ of point D
DOC).Above procedure is summarized as 1 formula 2 of formula：
θ=arctan (tanx/tany) (1)
Wherein, (ρ, θ) is the polar value of anchor point, and L is hitch point to positioning plane vertical range, and x is pitch angle, and y is cross
Roll angle.Further according to the section i where anchor point, the locator value finally obtained is (i, ρ, θ).
3. positioning device according to claim 1, which is characterized in that the step (2) carries out interval division, solves positioning
Dead Core Problems in model, specifically：
Entire plane domain is divided into four parts, and a new variable i is introduced to indicate anchor point in polar value
Place section, the elements of a fix of certain point are (i, ρ, θ) in plane at this time, wherein -45 °<θ<45°.As shown in Fig. 3 (a), at this
It positions and sets two 0 ° of lines in plane, 0 ° of line for system, one is the positioning polar 0 ° of line in section.It is pushed away by step 1
The locator value of all the points within the scope of -45 ° to 45 ° can be precisely calculated by leading, and be section 1 by this interval division, in this area
Between on point coordinates be expressed as (1, ρ, θ).When it is 45 ° to 135 ° to position section, 0 ° of line of system is moved to polar
At 90 °, as shown in Fig. 3 (b), computer system is considered as 90 ° of lines of positioning plane on 0 ° of line of system at this time.So extremely to 45 °
135 ° of positioning is set as section 2 by instead of the positioning to 315 ° to 45 °, this section, and location information is changed to (2, ρ,
θ).The rest may be inferred, ranging from 135 ° to 225 ° of positioning section 3, and location information is (3, ρ, θ)；Position section 4 ranging from
225 ° to 315 °, location information is (4, ρ, θ).So Dead Core Problems are just solved, realize plane without the accurate of dead angle
Positioning.
4. positioning device according to claim 1, which is characterized in that calculate in section where step (3) the judgement anchor point
Method, specifically：
It is x, roll angle y to enable pitch angle, and section where anchor point is distinguished below according to four kinds of situations：
(1) if | x |>| y |, and x>0, then the point be in first interval, i=1；
(2) if | x |<| y |, and y<0, then the point be in first interval, i=2；
(3) if | x |>| y |, and x<0, then the point be in first interval, i=3；
(4) if | x |<| y |, and y>0, then the point be in first interval, i=4.
5. positioning device according to claim 1, which is characterized in that step (4) anchor point, which is normalized and calculated, to be determined
Site coordinate value, specifically：
Flow one：Section i where determining current anchor point
Flow two：If i=1, x and y are remained unchanged；
If i=2, x=-y, y=x are enabled；
If i=3, then x=-x, y=-y are enabled；
If i=4, then x=y, y=-x are enabled；
Flow three：X after normalization, y are substituted into formula 1, formula 2 calculates current anchor point coordinate value.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810024530.8A CN108287524B (en) | 2018-01-11 | 2018-01-11 | Positioning method of planar partition positioning device based on MPU6050 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810024530.8A CN108287524B (en) | 2018-01-11 | 2018-01-11 | Positioning method of planar partition positioning device based on MPU6050 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108287524A true CN108287524A (en) | 2018-07-17 |
| CN108287524B CN108287524B (en) | 2021-02-19 |
Family
ID=62835002
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810024530.8A Expired - Fee Related CN108287524B (en) | 2018-01-11 | 2018-01-11 | Positioning method of planar partition positioning device based on MPU6050 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108287524B (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20030127436A1 (en) * | 2001-07-31 | 2003-07-10 | 3D Systems, Inc. | Selective laser sintering with optimized raster scan direction |
| CN102980574A (en) * | 2012-11-20 | 2013-03-20 | 清华大学 | LED-based indoor visible light accurate positioning reception model and positioning method therefor |
| CN103365249A (en) * | 2013-07-10 | 2013-10-23 | 西安电子科技大学 | Rapid solving method for failure workspace of six-degree-of-freedom parallel robot |
| CN103499348A (en) * | 2013-09-24 | 2014-01-08 | 成都市星达通科技有限公司 | High-precision attitude data calculation method for AHRS (Attitude and Heading Reference System) |
| US9205919B1 (en) * | 2010-09-09 | 2015-12-08 | Groen Brothers Aviation, Inc. | Apparatus and method for roll moment equalization at high advance ratios for rotary wing aircraft |
| CN107389063A (en) * | 2017-07-26 | 2017-11-24 | 重庆邮电大学 | The indoor fusion and positioning method of high accuracy based on GSM/MEMS fusions |
-
2018
- 2018-01-11 CN CN201810024530.8A patent/CN108287524B/en not_active Expired - Fee Related
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20030127436A1 (en) * | 2001-07-31 | 2003-07-10 | 3D Systems, Inc. | Selective laser sintering with optimized raster scan direction |
| US9205919B1 (en) * | 2010-09-09 | 2015-12-08 | Groen Brothers Aviation, Inc. | Apparatus and method for roll moment equalization at high advance ratios for rotary wing aircraft |
| CN102980574A (en) * | 2012-11-20 | 2013-03-20 | 清华大学 | LED-based indoor visible light accurate positioning reception model and positioning method therefor |
| CN103365249A (en) * | 2013-07-10 | 2013-10-23 | 西安电子科技大学 | Rapid solving method for failure workspace of six-degree-of-freedom parallel robot |
| CN103499348A (en) * | 2013-09-24 | 2014-01-08 | 成都市星达通科技有限公司 | High-precision attitude data calculation method for AHRS (Attitude and Heading Reference System) |
| CN107389063A (en) * | 2017-07-26 | 2017-11-24 | 重庆邮电大学 | The indoor fusion and positioning method of high accuracy based on GSM/MEMS fusions |
Non-Patent Citations (1)
| Title |
|---|
| 李士垚 等: "基于MPU6050 六轴传感器的悬空鼠标设计与实现", 《电子制作》 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108287524B (en) | 2021-02-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105091744B (en) | The apparatus for detecting position and posture and method of a kind of view-based access control model sensor and laser range finder | |
| CN106979780A (en) | A kind of unmanned vehicle real-time attitude measuring method | |
| CN106290968B (en) | A kind of large space pulsatile flow field three-dimension measuring system and measurement method | |
| CN112729221A (en) | Method for measuring deflection angle of airplane control surface | |
| CN108317999B (en) | Communication iron tower inclination angle measuring method based on inertial sensor | |
| CN106483330A (en) | One kind is based on reflective silk thread attitude angle visual identity two-D wind speed wind direction method of testing | |
| CN110455275A (en) | A kind of macrotype spherical storage tank climbing robot Position Fixing Navigation System and method | |
| CN106353529A (en) | Three-dimensional measurement device for large-space steady flow field | |
| CN107677207A (en) | Laser range sensor site error scaling method based on EKF | |
| CN109465829A (en) | A kind of industrial robot geometric parameter discrimination method based on transition matrix error model | |
| CN111562563A (en) | Laser radar rotary table calibration method and device and computer readable storage medium | |
| CN105735969A (en) | Oil well bore track plotting device and method | |
| CN106375716A (en) | Optical device stable tracking system and method for ship | |
| CN102809367A (en) | Space rotating angle measuring method based on double-shaft obliquity sensor | |
| CN101586964A (en) | Method for calibrating theodolite in aquatic measurement | |
| CN106643601A (en) | Industrial robot dynamic six-dimensional parameter measuring method | |
| CN108287524A (en) | A kind of high precision plane by stages positioning device based on MPU6050 | |
| CN105136142A (en) | Indoor positioning method based on micro inertial sensor | |
| CN206020457U (en) | A kind of five-hole probe based on inertial sensor | |
| CN106248000B (en) | The measurement method of part axially bored line | |
| CN107907110B (en) | Multi-angle identification method for structural plane occurrence and properties based on unmanned aerial vehicle | |
| CN107678568B (en) | Method and device for simulating laser pen through mobile terminal | |
| CN110095118A (en) | A kind of method for real-time measurement and system at body gesture angle | |
| US11964401B2 (en) | Robot globally optimal visual positioning method and device based on point-line features | |
| CN115857520A (en) | Unmanned aerial vehicle carrier landing state monitoring method based on combination of vision and ship state |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20210219 Termination date: 20220111 |
|
| CF01 | Termination of patent right due to non-payment of annual fee |