CN104546391B - Gyro stabilizer for tactile sticks and complementary filtering method thereof - Google Patents

Gyro stabilizer for tactile sticks and complementary filtering method thereof Download PDF

Info

Publication number
CN104546391B
CN104546391B CN201510049666.0A CN201510049666A CN104546391B CN 104546391 B CN104546391 B CN 104546391B CN 201510049666 A CN201510049666 A CN 201510049666A CN 104546391 B CN104546391 B CN 104546391B
Authority
CN
China
Prior art keywords
angle
gyrosensor
acceleration
stick
angular acceleration
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.)
Active
Application number
CN201510049666.0A
Other languages
Chinese (zh)
Other versions
CN104546391A (en
Inventor
罗洁
贾祥志
蒋庆
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
National Sun Yat Sen University
Original Assignee
National Sun Yat Sen University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by National Sun Yat Sen University filed Critical National Sun Yat Sen University
Priority to CN201510049666.0A priority Critical patent/CN104546391B/en
Publication of CN104546391A publication Critical patent/CN104546391A/en
Application granted granted Critical
Publication of CN104546391B publication Critical patent/CN104546391B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Landscapes

  • Gyroscopes (AREA)

Abstract

Disclosed are a gyro stabilizer for tactile sticks and a complementary filtering method thereof. The gyro stabilizer comprises a microcontroller unit, a posture sensor and a motor and carries a ranging module which is connected with a mobile terminal through a wireless module. The complementary filtering method includes: 1, preprocessing data of the sensors; 2, estimating an iterative process based on the angle of an acceleration sensor; 3, compensating measuring results of the acceleration sensor through angular acceleration of a gyro sensor. The gyro stabilizer and the complementary filtering method thereof have the advantages that shaking caused by movement of a blind person is avoided, a preset direction can be effectively locked for ranging, the danger detecting range of a tactile stick is widened, obstacles in air can be accurately recognized, practicality of the tactile stick is effectively improved, great convenience is brought to the blind person, stability is good, response speed is high, an algorithm is simple, noise and drift are significantly suppressed, data are smoother, and high response and high accuracy are ensured in case of great angular change.

Description

A kind of gyrostabilization unit and its complementary filter method for blind man's stick
Technical field
The present invention relates to gyrostabilization unit and multi-sensor fusion technology field, it is specifically related to a kind of for blind man's stick Gyrostabilization unit and its complementary filter method.
Background technology
Visually impaired people is a colony by social extensive concern.Traditional blind man's stick has perceived distance limited, it is impossible to side Just the distinct disadvantages such as the barrier of the tactile upper part of the body are visited.New blind man's stick increased distance measurement function, to detect aerial barrier And danger.But, the motion of the hand-held blind man's stick of visually impaired people causes range finding unstable, will greatly limit the function of blind man's stick, and with Effective solution is had no toward document and patent.
Gyrostabilization unit is widely used in fields such as military affairs, navigation, detections.By some high-precision sensors, And some data fusion methods, it is possible to quickly and stably tracking control system, at the same with motion servo system and stable control System processed etc. coordinates, it is possible to achieve stable to follow the trail of and correct.Traditional data fusion method for being applied to gyrostabilization unit has Complementary filter and Kalman filtering.Complementary filter method is difficult to solve the influence of noise of acceleration transducer itself.Kalman filters Wave method needs to set up multiple system equations, and for the compact stabilized platform for being applied to blind man's stick, amount of calculation seems excessively huge.It is open Number be given for the patent " carrier speed and corner compensating measurement method based on accelerometer and gyroscope " of CN103913171A The conventional method of angular surveying, but the corner of one degree of freedom can only be measured, and response speed still can not meet will Ask.
The content of the invention
Present invention aims to above-mentioned existing problems and deficiency, there is provided one kind can avoid because of blind-person sports band That what is come rocks, and can effectively lock pre-configured orientation and be found range, and has expanded the scope that blind man's stick surveys danger so that the barrier in space Can be accurately identified, effectively increase the practicality of blind man's stick, bring blind person the gyrocontrol for blind man's stick of great convenience Device.On this basis, further aim of the present invention is to provide one kind with preferable stability and response speed, and calculates Method is relatively simple, suppresses obvious to noise and drift, and smoothness is not only had more in data, while becoming in significantly angle In the case of change, also there is the complementary filter method of the gyrostabilization unit of response speed quickly and very high accuracy.
The technical scheme is that what is be achieved in that:
Gyrostabilization unit for blind man's stick of the present invention, is characterized in including microcontroller and connects with microcontroller The attitude transducer for connecing and motor, wherein the attitude transducer includes acceleration transducer and gyrosensor, the micro-control Device processed is also associated with range finder module and the wireless module for being connected with mobile terminal signal, and the range finder module and motor phase Connection, the motor can be three for steering wheel or brushless electric machine or Servo-controller, the acceleration transducer and gyrosensor Axle sensor.
The complementary filter method of the gyrostabilization unit for blind man's stick of the present invention, is characterized in including following step Suddenly:
The first step:Acceleration transducer and gyrosensor are placed, gyrostabilization unit is built, after pretreatment, The angle of acceleration transducer is obtained respectivelyθ i With the angular acceleration of gyrosensorω i Data;Wherein, angleθ i By three-dimensional space Between geometrical relationship obtain, angular accelerationω i Obtained by sensor acquisition and process;
Second step:First smooth the angle of acceleration transducerθ i , obtain Smoothing angle intermediate quantityx 1, computing formula is:
In formula,θ i For this angle value;θ’ oFor angle output valve after previous correction,K 1It is a certain proportion of smooth to take Weighted value;That is, the angle of smooth acceleration transducerθ i Process be:Using this angle valueθ i After previous correction Angle output valveθ’ oDifference and (1-K 1)2It is multiplied and obtains Smoothing angle intermediate quantityx 1
x 1After computing, being converted into can characterize fast-changing class angular acceleration amountx 2, computing formula is:
In formula, ΔtFor the sampling interval;x’ 2For previous class angular acceleration amount;That is, being converted into class angular accelerationx 2Process be:By Smoothing angle intermediate quantityx 1As integrand, make it be integrated the time, be only herein between sampling Every ΔtBe multiplied, afterwards again with previous class angular acceleration amountx’ 2It is cumulative, that is, obtain this class angular acceleration amountx 2
3rd step:First with the angular acceleration of gyrosensorω i The angle of compensated acceleration sensor, computing formula For:
In formula,x 3For the angle intermediate quantity of calculated acceleration transducer;x 2For class angular acceleration;ω i For gyro The angular acceleration of sensor;x 1For Smoothing angle intermediate quantity;K 2To take a certain proportion of compensation authority weight values;That is, compensation adds The process of the angle of velocity sensor is:Class angular accelerationx 2With the angular acceleration of gyrosensorω i Be added, and with (1-K 2) after multiplication again with angle intermediate quantityx 1The process of addition, by this process angle intermediate quantity is obtainedx 3
Then angle estimation is carried out, this attitude angle is finally givenθ o , computing formula is:
In formula, ΔtFor the sampling interval;θ’ iFor the angle of Iterative;That is, the process of angle estimation is: By angle intermediate quantityx 3As integrand, make it be integrated the time, be only and sampling interval Δ hereintIt is multiplied, afterwards Again with the angle of Iterativeθ’ iIt is cumulative, finally give this angleθ o Process.
In the above-mentioned first step, described pretreatment includes sensor data acquisition and original data processing.
In the above-mentioned first step, described acceleration transducer and gyrosensor needs to be placed in parallel, in order to eliminate The impact of Coordinate Conversion computing, and acceleration transducer and gyrosensor can arbitrarily put in the inside of gyrostabilization unit Put.
In above-mentioned second step, described smoothing weights valueK 1Span be 0~1.
In above-mentioned 3rd step, described compensation authority weight valuesK 2Span be 0~1.
The present invention compared with prior art, has the advantage that:
On the one hand, gyrostabilization unit proposed by the present invention, is applied to blind man's stick, can provide great stability, function Module is connected by gyrostabilization unit with blind man's stick, and under being operated in a steady environment, this application improves blind man's stick Practicality, range finder module improved as functional module, the investigative range for making blind man's stick, and the barrier in space can be by Blind man's stick is accurately identified, and is greatly facilitated so as to bring blind person;
On the other hand, under the premise of service precision of the blind-guiding stick using gyrostabilization unit is met, the present invention proposes one The complementary filter method for improving is planted, traditional complementary filter is compared and only letter is done to acceleration transducer and gyrosensor data Single weighted average, this method is processed respectively for the different qualities of both sensors, i.e., larger to dynamic error Acceierometer sensor introduce it is " smooth " process, and for the gyrosensor with accumulated time effect only takes the acceleration of its angle Angle value is used for algorithm compensation, so as to the accumulated time of the dynamic error and gyrosensor that effectively reduce acceleration transducer is missed Difference, is more efficiently utilized multi-sensor data to realize the fusion of information, therefore can be disappeared to greatest extent by this method Except effect of noise, while simplifying computation complexity, response speed and detection accuracy are drastically increased.
Below in conjunction with the accompanying drawings the present invention is further illustrated.
Description of the drawings
Fig. 1 is the frame structure schematic diagram of the gyrostabilization unit for blind man's stick of the present invention.
Fig. 2 is the system schematic of complementary filter method of the present invention.
Fig. 3 is the ratio of class angular acceleration in the angle that present invention acceleration transducer data are calculated and complementary filter method Relatively scheme.
Fig. 4 be acceleration transducer and conventional complementary filtering method with present invention improves over complementary filter method solve respectively The comparison diagram of the angle of calculation.
Specific embodiment
As shown in figure 1, the gyrostabilization unit for blind man's stick of the present invention, including microcontroller and and microcontroller The attitude transducer and motor of connection, wherein the attitude transducer includes acceleration transducer and gyrosensor, and it is described Acceleration transducer and gyrosensor are three-axis sensor, the microcontroller be also associated with range finder module and for shifting The wireless module of dynamic terminal signaling connection, and the range finder module is connected with motor, the motor be steering wheel, brushless electric machine, Servo-controller or other motors.For the ease of eliminating the impact of Coordinate Conversion computing, the acceleration transducer and gyro are sensed Device is arranged in parallel, but it can arbitrarily be placed in the inside of gyrostabilization unit.
The gyrostabilization unit is the device with any attitude in two-dimensional space, and only in four sides up and down There is movement tendency position, and azimuth motion trend can be captured.The gyrostabilization unit is connected on blind man's stick and uses, by microcontroller Device is controlled, and to range finder module a kind of more stable working environment is provided.Acceleration transducer and gyrosensor are used for blind man's stick appearance The identification of state, identification parameter is mainly angle, is realized by complementary filter method;Complementary filter method of the present invention can be real When resolve the angle of blind man's stick, and rotated using the angle motor;Motor is connected with range finder module, motor rotate angle with Blind man's stick movement angle is offseted, to realize more smoothly finding range.Microcontroller can be handed over by wireless module with mobile terminal Mutually, the data obtained of finding range can be transmitted to mobile terminal, to realize the intelligent barrier avoiding of aerial barrage.Described range finder module is mainly Realize distance measurement function, it is also possible to be to aid in realizing the module of intelligent barrier avoiding.
The present invention has fit closely application in the gesture recognition control of the gyrostabilization unit of blind man's stick.For blind The gyrostabilization unit of cane, in being mainly reflected in two-dimensional environment, i.e., only has movement tendency in four orientation up and down.Blind man's stick connects Then the arm of people, can produce erratic vibrations, thus can produce random noise.Simultaneously as the characteristics of people uses blind man's stick Determine that the gyrostabilization unit does not require very high precision, but to embody very fast response speed.Therefore, this Bright proposed complementary filter method is applicable in the gyrostabilization unit of gesture recognition control.
The use of gyrostabilization unit of the present invention, and its complementary filter method are illustrated below by specific example Feasibility and the effectiveness that embodied during actual test.
As shown in Fig. 2 carrying out substep description to complementary filter method:
The first step:Gyrosensor and acceleration transducer are arranged in parallel in inside gyrostabilization unit, acceleration Sensor and gyrosensor belong to three-axis sensor, so as to gyrosensor each axle can and acceleration sensing The axle of device points to same direction, while sensor is connected with data acquisition module, it is ensured that gyrostabilization unit can be Freely rotate in two-dimensional environment;
Second step:Data are carried out with pretreatment, including sensor data acquisition and original data processing;Setting gyro sensing The sample rate of device and acceleration transducer(In the present embodiment, representative value is 125Hz)With digital low pass filtered frequency(In this reality In applying example, representative value is 5Hz), the acceleration of two axles of acceleration transducer is obtained respectivelya x Witha y , and gyrosensor Angular accelerationω i Data;Using the typical angle of pitch and roll angle computing formula:θ=arcsin(a /g), can obtain corresponding The angle of the corresponding acceleration transducer of axleθ ix Withθ iy , the angle of any one axle is taken here, it is calculated asθ i , as shown in Figure 3;
3rd step:Using the angle value of interative computationθ’ oSmooth this angle valueθ i , concrete calculating process is as follows:
Wherein,x 1For Smoothing angle intermediate quantity,x 2For this class angular acceleration amount,x 2Resolving for interative computation is obtained Previous class angular acceleration amount,θ’ oFor the angle value that the resolving of interative computation is obtained,K 1For smoothing weights value, the smoothing weights value Span be 0~1, value in this exampleK 1=0.005;ΔtFor sampling interval, value Δ in this examplet=0.008 s;By Fig. 3 can see that the curve after smoothing effective filters out mechanical noise and shaking interference compared with primitive curve, can be accurate The class angular acceleration amount variation tendency of ground reflection acceleration transducer;
4th step:Using the angular acceleration of gyrosensorω i The angle of compensated acceleration sensor, and carry out angle and estimate Meter, concrete calculating process is as follows:
Wherein,x 3For angle intermediate quantity,K 2For compensation authority weight values, the span of the compensation authority weight values is 0~1, in this example InK 2=0.2,θ i For the angle that the resolving of interative computation is obtained;ΔtFor sampling interval, value Δ in this examplet=0.008 s, most This angle value is tried to achieve eventuallyθ o , current result of calculation will be preserved, and be subsequently used for improving the precision of angle estimation, by Fig. 4 Can see, the curve after compensation improves the smoothness of angle compared with primitive curve.
Traditional complementary filter method directly allows the data of acceleration transducer and gyrosensor to give certain power respectively Value, cannot thus be prevented effectively from the dynamic error of acceleration transducer and the time integral cumulative error of gyrosensor, and one As be not enough to obtain accurate angle information, also to carry out offset angle measurement error with reference to Kalman filtering.It is proposed by the present invention The complementary filter method for improving, make use of the angle accuracy that acceleration transducer has in static measurement, and gyro to pass The angular acceleration accuracy that sensor has on transient measurement is avoiding problem above.It is not directly to adopt acceleration transducer Intermittent angle angle value and gyrosensor integration angle value directly merging, but by smoothing and compensating the method for combining The fusion of information is realized, the time integral accumulation of the dynamic error and gyrosensor that efficiently reduce acceleration transducer is missed Difference.
From actual application, algorithm provided by the present invention has preferably when blind man's stick gyrostabilization unit is applied to Stability and response speed, and algorithm complex is not high, suppresses obvious to noise and drift.It can be seen from figure 4 that Compared with the angle value that traditional complementary filter method is calculated, complementary filter method proposed by the present invention, not only in data more With smoothness, while in the case of significantly angle change, also there is response speed quickly and accuracy.
The present invention is described by embodiment, but is not limited the invention, with reference to description of the invention, institute Other changes of disclosed embodiment, such as the professional person of this area is readily apparent that, such change should belong to Within the scope of the claims in the present invention are limited.

Claims (4)

1. a kind of complementary filter method of gyrostabilization unit, the gyrostabilization unit for blind man's stick in the method, including it is micro- Controller and the attitude transducer being connected with microcontroller and motor, wherein the attitude transducer include acceleration transducer and Gyrosensor, the microcontroller is also associated with range finder module and the wireless module for being connected with mobile terminal signal, and The range finder module is connected with motor, and the motor is steering wheel, and the acceleration transducer and gyrosensor are used for blind man's stick The identification of attitude, identification parameter is mainly angle, and the angle that the motor is rotated is offseted with blind man's stick movement angle, to realize more For smoothly range finding, it is characterised in that comprise the following steps:
The first step:Acceleration transducer and gyrosensor are placed, gyrostabilization unit is built, after pretreatment, respectively Obtain the angle of acceleration transducerθ i With the angular acceleration of gyrosensorω i Data;Wherein, angleθ i It is several by three dimensions What relation is obtained, angular accelerationω i Obtained by sensor acquisition and process;
Second step:First smooth the angle of acceleration transducerθ i , obtain Smoothing angle intermediate quantityx 1, computing formula is:
In formula,θ i For this angle value;θ’ oFor angle output valve after previous correction,K 1To take a certain proportion of smoothing weights Value;
x 1After computing, being converted into can characterize fast-changing class angular acceleration amountx 2, computing formula is:
In formula, ΔtFor the sampling interval;x’ 2For previous class angular acceleration amount;
3rd step:First with the angular acceleration of gyrosensorω i The angle of compensated acceleration sensor, computing formula is:
In formula,x 3For the angle intermediate quantity of calculated acceleration transducer;x 2For class angular acceleration;ω i For gyro sensing The angular acceleration of device;x 1For Smoothing angle intermediate quantity;K 2To take a certain proportion of compensation authority weight values;
Then angle estimation is carried out, this attitude angle is finally givenθ o , computing formula is:
In formula, ΔtFor the sampling interval;θ’ iFor the angle of Iterative.
2. the complementary filter method of gyrostabilization unit according to claim 1, it is characterised in that in the above-mentioned first step, Described acceleration transducer and gyrosensor needs to be placed in parallel, and in order to eliminate the impact of Coordinate Conversion computing, and adds Velocity sensor and gyrosensor can be placed arbitrarily in the inside of gyrostabilization unit.
3. the complementary filter method of gyrostabilization unit according to claim 1, it is characterised in that in above-mentioned second step, Described smoothing weights valueK 1Span be 0~1.
4. the complementary filter method of gyrostabilization unit according to claim 1, it is characterised in that in above-mentioned 3rd step, Described compensation authority weight valuesK 2Span be 0~1.
CN201510049666.0A 2015-01-31 2015-01-31 Gyro stabilizer for tactile sticks and complementary filtering method thereof Active CN104546391B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510049666.0A CN104546391B (en) 2015-01-31 2015-01-31 Gyro stabilizer for tactile sticks and complementary filtering method thereof

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510049666.0A CN104546391B (en) 2015-01-31 2015-01-31 Gyro stabilizer for tactile sticks and complementary filtering method thereof

Publications (2)

Publication Number Publication Date
CN104546391A CN104546391A (en) 2015-04-29
CN104546391B true CN104546391B (en) 2017-05-17

Family

ID=53064188

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510049666.0A Active CN104546391B (en) 2015-01-31 2015-01-31 Gyro stabilizer for tactile sticks and complementary filtering method thereof

Country Status (1)

Country Link
CN (1) CN104546391B (en)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105651242B (en) * 2016-04-05 2018-08-24 清华大学深圳研究生院 A method of fusion attitude angle is calculated based on complementary Kalman filtering algorithm
CN107485540B (en) * 2017-07-18 2019-12-13 浙江大学城市学院 Energy injection system for intelligent walking stick
CN109583511B (en) * 2018-12-14 2023-06-30 北京百度网讯科技有限公司 Speed fusion method and device
CN112473097B (en) * 2019-09-11 2022-04-01 Tcl科技集团股份有限公司 Mountain climbing assisting method, server, system and storage medium
CN110974641A (en) * 2019-12-24 2020-04-10 中南民族大学 Intelligent walking stick system integrating machine learning and Internet of things technology for blind people

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102519425A (en) * 2011-10-24 2012-06-27 哈尔滨工程大学 Laser range finder stabilized platform used for vessel with single degree of freedom and its control method
CN203519011U (en) * 2013-10-15 2014-04-02 顾捷 Attitude sensor
CN103913171A (en) * 2014-04-28 2014-07-09 北京理工大学 Compensation measurement method for rotary speed and rotary angle of carrier based on accelerometer and gyroscope
CN104068997A (en) * 2014-06-30 2014-10-01 北京信息科技大学 Multifunctional walking stick for blind
CN203898662U (en) * 2014-04-11 2014-10-29 上海工程技术大学 Guide walking stick system for blind person

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6668846B2 (en) * 2001-11-15 2003-12-30 Edward L. Meador Gyroscopically balanced walking cane

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102519425A (en) * 2011-10-24 2012-06-27 哈尔滨工程大学 Laser range finder stabilized platform used for vessel with single degree of freedom and its control method
CN203519011U (en) * 2013-10-15 2014-04-02 顾捷 Attitude sensor
CN203898662U (en) * 2014-04-11 2014-10-29 上海工程技术大学 Guide walking stick system for blind person
CN103913171A (en) * 2014-04-28 2014-07-09 北京理工大学 Compensation measurement method for rotary speed and rotary angle of carrier based on accelerometer and gyroscope
CN104068997A (en) * 2014-06-30 2014-10-01 北京信息科技大学 Multifunctional walking stick for blind

Also Published As

Publication number Publication date
CN104546391A (en) 2015-04-29

Similar Documents

Publication Publication Date Title
CN106705968B (en) Indoor inertial navigation algorithm based on attitude identification and step size model
CN108362282B (en) Inertial pedestrian positioning method based on self-adaptive zero-speed interval adjustment
CN104546391B (en) Gyro stabilizer for tactile sticks and complementary filtering method thereof
CN104121905B (en) Course angle obtaining method based on inertial sensor
CN110398245B (en) Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit
CN107490378B (en) Indoor positioning and navigation method based on MPU6050 and smart phone
CN106908060A (en) A kind of high accuracy indoor orientation method based on MEMS inertial sensor
CN105698822B (en) Initial Alignment Method between autonomous type inertial navigation based on reversed Attitude Tracking is advanced
CN112697138B (en) Bionic polarization synchronous positioning and composition method based on factor graph optimization
CN102445200A (en) Microminiature personal combined navigation system as well as navigating and positioning method thereof
CN104061934A (en) Pedestrian indoor position tracking method based on inertial sensor
CN107941217A (en) A kind of robot localization method, electronic equipment, storage medium, device
EP2951533A1 (en) Method and system for varying step length estimation using nonlinear system identification
CN108132053B (en) Pedestrian track construction method and system and inertia measurement device
CN106370178B (en) Attitude measurement method and device of mobile terminal equipment
CN110057356B (en) Method and device for positioning vehicles in tunnel
CN107255474A (en) A kind of PDR course angles of fusion electronic compass and gyroscope determine method
CN109459028A (en) A kind of adaptive step estimation method based on gradient decline
CN106403952A (en) Method for measuring combined attitudes of Satcom on the move with low cost
CN110672095A (en) Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation
US10466054B2 (en) Method and system for estimating relative angle between headings
CN106643713B (en) Estimation method and device for zero-speed correction pedestrian locus for smooth and adaptive adjustment of threshold
CN112362057A (en) Inertial pedestrian navigation algorithm based on zero-speed correction and attitude self-observation
CN108444468A (en) The bearing compass of vision and inertial navigation information is regarded under a kind of fusion
CN109781096A (en) A kind of integrated navigation and location system and method for intelligent agricultural machinery

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant