CN105147505B - It is a kind of based on take aim in advance tracking blind person walk closed loop Induction Control method - Google Patents
It is a kind of based on take aim in advance tracking blind person walk closed loop Induction Control method Download PDFInfo
- Publication number
- CN105147505B CN105147505B CN201510571640.2A CN201510571640A CN105147505B CN 105147505 B CN105147505 B CN 105147505B CN 201510571640 A CN201510571640 A CN 201510571640A CN 105147505 B CN105147505 B CN 105147505B
- Authority
- CN
- China
- Prior art keywords
- blind person
- msub
- mrow
- aim
- mtd
- 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
Links
Classifications
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
- A61H3/00—Appliances for aiding patients or disabled persons to walk about
- A61H3/06—Walking aids for blind persons
- A61H3/061—Walking aids for blind persons with electronic detecting or guiding means
Landscapes
- Health & Medical Sciences (AREA)
- Epidemiology (AREA)
- Pain & Pain Management (AREA)
- Physical Education & Sports Medicine (AREA)
- Rehabilitation Therapy (AREA)
- Life Sciences & Earth Sciences (AREA)
- Animal Behavior & Ethology (AREA)
- General Health & Medical Sciences (AREA)
- Public Health (AREA)
- Veterinary Medicine (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
It is a kind of based on take aim in advance tracking blind person walk closed loop Induction Control method, blind person current location and direction of travel angle are constantly obtained in blind person's walking process, search for pre- take aim at as tracking a target in given path by pre- radius of taking aim at, and using current location to the line taken aim in advance a little as taking aim at direction in advance, calculate deviation of blind person's direction of travel angle on taking aim at deflection in advance, direction of travel angle controlled quentity controlled variable is calculated and exported according to control law, control blind person to adjust direction of travel, realize that blind person's walking tends to the Induction Control of given path.Present invention introduces take aim at follow-up mechanism in advance, the direction of travel angle of blind person continuously detect and control, realizes the closed loop continuous induction to blind person's walking, improving blind person's walking tends to the accuracy of given path, be advantageous to improve the safety and reliability of guide algorithm, be more suitable for popularization and application.
Description
Technical field
Walked closed loop Induction Control method based on the blind person for taking aim at tracking in advance the present invention relates to a kind of, for guide equipment to draw
Guide people walks along given path, and be particularly suitable for use in wearable guide equipment.
Background technology
Difficult trip is always the matter of utmost importance for perplexing blind person and visually impaired people, there is provided a kind of effective blind-guiding method is
System, convenient and reliable induction mode is provided for the trip of visually impaired people, is always the hot issue of society and scientific research field concern.
To solve blind person's trip problem, occur some blind-guide devices successively in recent years, can substantially be categorized as out
Ring controls blind guiding system and closed-loop control blind guiding system.Existing opened loop control blind guiding system have in real time positioning blind person position,
The functions such as surrounding environment, detection obstacle distance are detected, blind person can be helped to recognize the environmental information of surrounding, representative device bag
Include GPS Voice Navigations, ultrasonic wave blind man's stick etc..Existing closed-loop control blind guiding system enters on the basis of opened loop control blind guiding system
Improvement is gone, while environment is perceived and has carried out analysis in real time and control, apply discrete logic feedback control to blind person
Signal, the prompting of left and right turn is provided for blind person in the process of walking.
Above method has the advantages of respective, while also has some defects.Existing opened loop control blind guiding system has
Device is simple, highly reliable, it is applied widely the advantages that, but the system can only provide the environmental information around blind person, but not
Sensitive directly control signal can be provided to guide blind person to walk, the real-time and convenience for making guide effect decline.It is existing
Closed-loop control blind guiding system realizes complete closed-loop control and man-machine interaction, but the discrete logic control that the system provides is believed
Number the direction of travel of blind person can only be controlled, the size of steering angle can not be but adjusted, cause its control accuracy
It is poor.The defects of due to these methods itself and limitation, cause accurate guide to realize.
The content of the invention
The technology of the present invention solves problem：Overcome above-mentioned the deficiencies in the prior art, there is provided a kind of based on taking aim at the blind of tracking in advance
People's walking closed loop Induction Control method.Introducing takes aim at follow-up mechanism in advance, is controlled by simple and reliable walking angle, with a kind of effective
Man-machine interaction control model and the direct mode of intelligence transmission, realize to blind person walking closed loop continuous induction, reach essence
The purpose of true guide.
The present invention technical solution be：It is a kind of based on take aim in advance tracking blind person walk closed loop Induction Control method,
Blind person current location and direction of travel angle are constantly obtained in blind person's walking process, by it is pre- take aim at radius and searched in given path take aim in advance
Point is as tracking target, and using current location to the line taken aim in advance a little as taking aim at direction in advance, calculating blind person's direction of travel angle on
The pre- deviation for taking aim at deflection, direction of travel angle controlled quentity controlled variable is calculated and exported according to control law, control blind person adjusts direction of travel, real
Existing blind person's walking tends to the Induction Control of given path.
Described given path is known, and the path derives from the path detecting, plan or pre-set in real time, can be with
It is described, can also be described using a series of orderly points in the form of equation of locus.
Described pre- point search of taking aim at obtains blind person current location and direction of travel angle first, then using current location as the center of circle,
To take aim at radius L in advance as radius, search on the given path in the range of current direction of travel ± 90 ° and take aim at a little in advance, wherein taking aim in advance
Radius L is the function on speed, and its relation is as follows：
In the formula, LmaxAnd LminIt is the pre- upper and lower bound distance for taking aim at radius respectively；vmaxAnd vminIt is that ordinary people exists respectively
The maximal rate and minimum speed walked under normal circumstances；λ is the speed of travel and the proportionality coefficient for taking aim at radius in advance.
Described control law is PID control, and controlled volume is the deviation for taking aim at deflection and blind person's direction of travel angle in advance.The pre- side of taking aim at
It is that pre- deflection of the vector under earth axes taken aim at a little is pointed in blind person current location to angle, blind person's direction of travel angle refers to
Deflection of the current speed of travel vector of blind person under earth axes.It pre- will take aim at deflection and subtract blind person's direction of travel angle and obtain
Angular deviation, the deviation input to PID controller calculating output and obtain controlled quentity controlled variable；PID controller is using anti-saturation integration and not
Complete differential structure, transmission function are：
Wherein KpFor proportional gain, KiFor storage gain, KdFor the differential gain, TfFor derivative time.
Described adjustment blind person's direction of travel is to induce blind person to turn to using the polarity and size of controlled quentity controlled variable.Controlled quentity controlled variable is
Just and to induce blind person to turn to the left and to the right respectively when bearing, the angle that the more big then blind person of controlled quentity controlled variable turns to is also bigger, blind person's
Motion model can be described with following formula：
J represents the rotary inertia of blind person's yaw axis in the formula, and M represents to put on the kinetic moment of itself, M when blind person turns tod
The damping torque in blind person's yaw freedom is represented, β represents blind person's direction of travel angle, K1Represent to put on when blind person turns to certainly
Proportionality coefficient between the kinetic moment and controlled quentity controlled variable of body, K2Represent damped coefficient, ucRepresent the controlled quentity controlled variable of PID controller output.
The present invention compared with prior art the advantages of be：
(1) compared with existing opened loop control blind guiding system, the present invention can according to blind person current location and walk,
Real-time decision-making and control are made, guiding blind person walks according to given path, realizes effective man-machine interaction and closed-loop control；
(2) compared with existing closed-loop control blind guiding system, the present invention can carry out continuous to the direction of travel angle of blind person
Detection and control, realize the closed loop continuous induction to blind person's walking, and improving blind person's walking tends to the accuracy of given path, favorably
In the safety and reliability for improving guide algorithm, it is more suitable for popularization and application.
Brief description of the drawings
Fig. 1 is the general flow chart of the present invention；
Fig. 2 is that the pre- of the present invention takes aim at point search schematic diagram；
Embodiment
The various pieces in the present invention are elaborated with reference to Figure of description：
The present invention it is a kind of based on take aim in advance tracking blind person walk closed loop Induction Control method overall procedure as shown in figure 1,
Specifically include：Blind person current location and direction of travel angle are constantly obtained in blind person's walking process, by taking aim at radius on given road in advance
Searched in footpath it is pre- take aim at as tracking target, and using current location to the line taken aim in advance a little as taking aim at direction, calculating blind person's row in advance
Deviation of the deflection on taking aim at deflection in advance is walked, direction of travel angle controlled quentity controlled variable is calculated and exported according to control law, control blind person adjusts
Whole direction of travel, realize that blind person's walking tends to the Induction Control of given path.
The given path of the present invention is known, and the path derives from the path detected, plan or pre-set in real time, can
It is described, can also be described using a series of orderly points in the form of using equation of locus.
The pre- current location for taking aim at point search schematic diagram as shown in Fig. 2 wherein M points are blind person of the present invention, N points are to take aim in advance
Point, given path are an arbitrary curve, and the step of taking aim at point search in advance is as follows：
Step 1：By the environment sensing and state detection module of correlation, blind person's current location M points are obtained in geographical coordinates
It is the coordinate (x under XOYc, yc), and current motion state, i.e. velocity v size and the angle under earth axes XOY
Spend β；
Step 2：It is the function on speed to calculate and take aim at radius L, L in advance, and functional relation is as follows：
In the formula, LmaxAnd LminIt is the pre- upper and lower bound distance for taking aim at radius respectively；vmaxAnd vminIt is that ordinary people exists respectively
The maximal rate and minimum speed walked under normal circumstances；λ is to take aim at radius and the proportionality coefficient of speed in advance.
Step 3：Using current location M points as the center of circle, it is radius to take aim at radius L in advance, in the range of current direction of travel ± 90 °
Make circular arc, the intersection point N of camber line and desired trajectory is as taken aim at a little in advance, and its coordinate is designated as (x, y).
The control law of the present invention uses PID control, and controlled volume is the deviation for taking aim at deflection and blind person's direction of travel angle in advance.In detail
Thin step is as follows：
Step 1：Ask pre- and take aim at deflection, i.e., pre- angle of the vector under earth axes XOY taken aim at a little is pointed to by current location
Angle value α：
Step 2：Take aim at deflection by pre- and subtract blind person's direction of travel angle, obtain angular deviation θ:
θ=alpha-beta
Step 3：The deviation is inputted to PID controller, controlled quentity controlled variable is calculated.PID controller is using anti-saturation
Integration and incomplete differential structure, its transmission function are：
Wherein KpFor proportional gain, KiFor storage gain, KdFor the differential gain, TfFor derivative time.
The present invention induces blind person to turn to using the polarity and size of controlled quentity controlled variable.Controlled quentity controlled variable is just and induced respectively when being negative
Blind person turns to the left and to the right, and the angle that the more big then blind person of controlled quentity controlled variable turns to is also bigger, and the motion model of blind person can use following formula
To describe：
J represents the rotary inertia of blind person's yaw axis in the formula, and M represents to put on the kinetic moment of itself, M when blind person turns tod
The damping torque in blind person's yaw freedom is represented, β represents blind person's direction of travel angle, K1Represent to put on itself when blind person turns to
Kinetic moment and controlled quentity controlled variable between proportionality coefficient, K2Represent damped coefficient, ucRepresent the controlled quentity controlled variable of PID controller output.
Claims (2)
1. it is a kind of based on the blind person's walking closed loop Induction Control method for taking aim at tracking in advance, blind person is constantly obtained in blind person's walking process
Current location and direction of travel angle, pre- take aim at as tracking a target, and with present bit search in given path by pre- radius of taking aim at
Put the line taken aim in advance a little and take aim at direction as pre-, deviation of blind person's direction of travel angle on taking aim at deflection in advance is calculated, according to control
Rule calculates and output direction of travel angle controlled quentity controlled variable, control blind person adjust direction of travel, realizes that blind person's walking tends to given path
Induction Control；
Described pre- point search of taking aim at obtains blind person current location and direction of travel angle first, then using current location as the center of circle, with pre-
It is radius to take aim at radius L, searches on the given path in the range of current direction of travel ± 90 ° and takes aim at a little in advance, wherein taking aim at radius L in advance
It is the function on speed, its relation is as follows：
<mrow>
<mi>L</mi>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>L</mi>
<mi>min</mi>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>v</mi>
<mo><</mo>
<msub>
<mi>v</mi>
<mi>min</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&lambda;</mi>
<mi>v</mi>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>v</mi>
<mi>min</mi>
</msub>
<mo><</mo>
<mi>v</mi>
<mo><</mo>
<msub>
<mi>v</mi>
<mi>max</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>L</mi>
<mi>max</mi>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>v</mi>
<mo>></mo>
<msub>
<mi>v</mi>
<mi>max</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
In the formula, LmaxAnd LminIt is the pre- upper and lower bound distance for taking aim at radius respectively；vmaxAnd vminIt is ordinary people respectively normal
In the case of the maximal rate and minimum speed walked；λ is the speed of travel and the proportionality coefficient for taking aim at radius in advance；
Described control law is PID control, and controlled volume is the deviation for taking aim at deflection and blind person's direction of travel angle in advance, takes aim at deflection in advance
It is that pre- deflection of the vector under earth axes taken aim at a little is pointed in blind person current location, blind person's direction of travel angle refers to blind person
Current deflection of the speed of travel vector under earth axes, pre- will take aim at deflection and subtract blind person's direction of travel angle and obtain angle
Deviation, the deviation input to PID controller calculating output and obtain controlled quentity controlled variable；PID controller is using anti-saturation integration and not exclusively
Differential structrue, transmission function are：
<mrow>
<msub>
<mi>G</mi>
<mi>c</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>s</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>K</mi>
<mi>p</mi>
</msub>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>i</mi>
</msub>
<mo>&CenterDot;</mo>
<mfrac>
<mn>1</mn>
<mrow>
<mi>s</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</mfrac>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>d</mi>
</msub>
<mo>&CenterDot;</mo>
<mfrac>
<mrow>
<msub>
<mi>T</mi>
<mi>f</mi>
</msub>
<mo>&CenterDot;</mo>
<mi>s</mi>
</mrow>
<mrow>
<msub>
<mi>T</mi>
<mi>f</mi>
</msub>
<mo>&CenterDot;</mo>
<mi>s</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</mfrac>
</mrow>
Wherein KpFor proportional gain, Ki is storage gain, KdFor the differential gain, TfFor derivative time；
Described adjustment blind person's direction of travel is to induce blind person to turn to using the polarity and size of controlled quentity controlled variable, controlled quentity controlled variable for just and
To induce blind person to turn to the left and to the right respectively when bearing, the angle that the more big then blind person of controlled quentity controlled variable turns to is also bigger, the motion of blind person
Model can be described with following formula：
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mi>J</mi>
<mover>
<mi>&beta;</mi>
<mo>&CenterDot;&CenterDot;</mo>
</mover>
<mo>=</mo>
<mi>M</mi>
<mo>+</mo>
<msub>
<mi>M</mi>
<mi>d</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>M</mi>
<mo>=</mo>
<msub>
<mi>K</mi>
<mn>1</mn>
</msub>
<mo>&CenterDot;</mo>
<msub>
<mi>u</mi>
<mi>c</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>M</mi>
<mi>d</mi>
</msub>
<mo>=</mo>
<mo>-</mo>
<msub>
<mi>K</mi>
<mn>2</mn>
</msub>
<mo>&CenterDot;</mo>
<mover>
<mi>&beta;</mi>
<mo>&CenterDot;&CenterDot;</mo>
</mover>
</mtd>
</mtr>
</mtable>
</mfenced>
J represents the rotary inertia of blind person's yaw axis in the formula, and M represents to put on the kinetic moment of itself, M when blind person turns todRepresent blind
Damping torque in people's yaw freedom, β represent blind person's direction of travel angle, K1Represent to put on the power of itself when blind person turns to
Proportionality coefficient between square and controlled quentity controlled variable, K2Represent damped coefficient, ucRepresent the controlled quentity controlled variable of PID controller output.
2. it is according to claim 1 it is a kind of walked closed loop Induction Control method based on the blind person for taking aim at tracking in advance, its feature exists
In：Described given path is known, and the path derives from the path detected, plan or pre-set in real time, can use
The form of equation of locus is described, and can also be described using a series of orderly points.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510571640.2A CN105147505B (en) | 2015-09-09 | 2015-09-09 | It is a kind of based on take aim in advance tracking blind person walk closed loop Induction Control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510571640.2A CN105147505B (en) | 2015-09-09 | 2015-09-09 | It is a kind of based on take aim in advance tracking blind person walk closed loop Induction Control method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105147505A CN105147505A (en) | 2015-12-16 |
| CN105147505B true CN105147505B (en) | 2018-03-27 |
Family
ID=54788805
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510571640.2A Active CN105147505B (en) | 2015-09-09 | 2015-09-09 | It is a kind of based on take aim in advance tracking blind person walk closed loop Induction Control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105147505B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2018119586A1 (en) * | 2016-12-26 | 2018-07-05 | 深圳前海达闼云端智能科技有限公司 | Blind guiding method and apparatus, and blind guiding device |
| CN107402018B (en) * | 2017-09-21 | 2019-09-17 | 北京航空航天大学 | A kind of apparatus for guiding blind combinatorial path planing method based on successive frame |
| CN112891157B (en) * | 2021-01-21 | 2023-03-28 | 上海傅利叶智能科技有限公司 | Method and device for collecting data based on intelligent walking stick and intelligent walking stick |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101093396A (en) * | 2007-07-04 | 2007-12-26 | 华南农业大学 | Navigation control method for agricultural machinery |
| CN102358287A (en) * | 2011-09-05 | 2012-02-22 | 北京航空航天大学 | Trajectory tracking control method used for automatic driving robot of vehicle |
| US8577552B1 (en) * | 2012-08-13 | 2013-11-05 | Ford Global Technologies, Llc | Forward collision warning system with road-side target filtering |
-
2015
- 2015-09-09 CN CN201510571640.2A patent/CN105147505B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101093396A (en) * | 2007-07-04 | 2007-12-26 | 华南农业大学 | Navigation control method for agricultural machinery |
| CN102358287A (en) * | 2011-09-05 | 2012-02-22 | 北京航空航天大学 | Trajectory tracking control method used for automatic driving robot of vehicle |
| US8577552B1 (en) * | 2012-08-13 | 2013-11-05 | Ford Global Technologies, Llc | Forward collision warning system with road-side target filtering |
Non-Patent Citations (1)
| Title |
|---|
| 视觉导航智能车辆横向运动的自适应预瞄控制;陈无畏，王家恩，汪明磊，王金波;《中国机械功能》;20140331;第25卷(第5期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105147505A (en) | 2015-12-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN103822625B (en) | Line-tracking navigation method and device for intelligent robot | |
| Morales et al. | Pure-pursuit reactive path tracking for nonholonomic mobile robots with a 2D laser scanner | |
| CN107608346A (en) | Ship intelligent barrier avoiding method and system based on Artificial Potential Field | |
| CN105147505B (en) | It is a kind of based on take aim in advance tracking blind person walk closed loop Induction Control method | |
| CN103984342B (en) | The multiple-pulse nerve network controller navigation control method of mobile robot | |
| CN109508007A (en) | A kind of agricultural machinery track following, obstacle avoidance system and method based on Multi-source Information Fusion | |
| CN105425791A (en) | Swarm robot control system and method based on visual positioning | |
| CN103955223A (en) | Posture and path coupling control method for deep space exploration soft landing process | |
| Zhong et al. | Velocity-Change-Space-based dynamic motion planning for mobile robots navigation | |
| CN103760902A (en) | USV straight path tracking method based on fuzzy control | |
| CN106403957A (en) | Amphibious air cushion shipway point guidance control method | |
| CN104898665A (en) | Method and device for programming track of tour inspection robot | |
| CN113093742B (en) | Unmanned ship path tracking system capable of automatically avoiding multiple obstacles | |
| CN108762253B (en) | Man-machine formation control method applied to human navigation system | |
| CN105929849A (en) | Wheeled mobile robot target tracking control method based on point stabilization | |
| CN109900273A (en) | A kind of outdoor mobile robot guidance method and guidance system | |
| Song et al. | A new hybrid method in global dynamic path planning of mobile robot | |
| Pang et al. | A collision-free person-following approach based on path planning | |
| Chen et al. | Intelligent obstacle avoidance control strategy for wheeled mobile robot | |
| CN103170976B (en) | Two-freedom-degree robot motion control method | |
| Belkhouche et al. | Wheeled mobile robot navigation using proportional navigation | |
| Gong et al. | Path tracking of unmanned vehicle based on parameters self-tuning fuzzy control | |
| Ye et al. | A sub goal seeking approach for reactive navigation in complex unknown environments | |
| CN203444334U (en) | Autonomous navigation system of tour guide robot | |
| Zhang et al. | Design of Blind Guiding Robot Based on Speed Adaptation and Visual Recognition |
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 |