CN112472432B - Walking stick-wheelchair automatic following system and method - Google Patents

Walking stick-wheelchair automatic following system and method Download PDF

Info

Publication number
CN112472432B
CN112472432B CN202011344588.4A CN202011344588A CN112472432B CN 112472432 B CN112472432 B CN 112472432B CN 202011344588 A CN202011344588 A CN 202011344588A CN 112472432 B CN112472432 B CN 112472432B
Authority
CN
China
Prior art keywords
wheelchair
following
target
walking stick
module
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
CN202011344588.4A
Other languages
Chinese (zh)
Other versions
CN112472432A (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.)
China Assistive Devices And Technology Centre For Persons With Disabilities
Tianjin Taisite Instrument Co ltd
Wuhan University of Technology WUT
Original Assignee
China Assistive Devices And Technology Centre For Persons With Disabilities
Tianjin Taisite Instrument Co ltd
Wuhan University of Technology WUT
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 China Assistive Devices And Technology Centre For Persons With Disabilities, Tianjin Taisite Instrument Co ltd, Wuhan University of Technology WUT filed Critical China Assistive Devices And Technology Centre For Persons With Disabilities
Priority to CN202011344588.4A priority Critical patent/CN112472432B/en
Publication of CN112472432A publication Critical patent/CN112472432A/en
Application granted granted Critical
Publication of CN112472432B publication Critical patent/CN112472432B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • AHUMAN NECESSITIES
    • A61MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61GTRANSPORT, PERSONAL CONVEYANCES, OR ACCOMMODATION SPECIALLY ADAPTED FOR PATIENTS OR DISABLED PERSONS; OPERATING TABLES OR CHAIRS; CHAIRS FOR DENTISTRY; FUNERAL DEVICES
    • A61G5/00Chairs or personal conveyances specially adapted for patients or disabled persons, e.g. wheelchairs
    • A61G5/04Chairs or personal conveyances specially adapted for patients or disabled persons, e.g. wheelchairs motor-driven
    • AHUMAN NECESSITIES
    • A61MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61GTRANSPORT, PERSONAL CONVEYANCES, OR ACCOMMODATION SPECIALLY ADAPTED FOR PATIENTS OR DISABLED PERSONS; OPERATING TABLES OR CHAIRS; CHAIRS FOR DENTISTRY; FUNERAL DEVICES
    • A61G5/00Chairs or personal conveyances specially adapted for patients or disabled persons, e.g. wheelchairs
    • A61G5/10Parts, details or accessories
    • AHUMAN NECESSITIES
    • A61MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61GTRANSPORT, PERSONAL CONVEYANCES, OR ACCOMMODATION SPECIALLY ADAPTED FOR PATIENTS OR DISABLED PERSONS; OPERATING TABLES OR CHAIRS; CHAIRS FOR DENTISTRY; FUNERAL DEVICES
    • A61G5/00Chairs or personal conveyances specially adapted for patients or disabled persons, e.g. wheelchairs
    • A61G5/10Parts, details or accessories
    • A61G5/1051Arrangements for steering
    • AHUMAN NECESSITIES
    • A61MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61HPHYSICAL 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/00Appliances for aiding patients or disabled persons to walk about
    • A61H3/02Crutches
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/12Target-seeking control
    • AHUMAN NECESSITIES
    • A61MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61GTRANSPORT, PERSONAL CONVEYANCES, OR ACCOMMODATION SPECIALLY ADAPTED FOR PATIENTS OR DISABLED PERSONS; OPERATING TABLES OR CHAIRS; CHAIRS FOR DENTISTRY; FUNERAL DEVICES
    • A61G2203/00General characteristics of devices
    • A61G2203/10General characteristics of devices characterised by specific control means, e.g. for adjustment or steering
    • A61G2203/22General characteristics of devices characterised by specific control means, e.g. for adjustment or steering for automatically guiding movable devices, e.g. stretchers or wheelchairs in a hospital
    • AHUMAN NECESSITIES
    • A61MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61GTRANSPORT, PERSONAL CONVEYANCES, OR ACCOMMODATION SPECIALLY ADAPTED FOR PATIENTS OR DISABLED PERSONS; OPERATING TABLES OR CHAIRS; CHAIRS FOR DENTISTRY; FUNERAL DEVICES
    • A61G2203/00General characteristics of devices
    • A61G2203/30General characteristics of devices characterised by sensor means
    • AHUMAN NECESSITIES
    • A61MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61GTRANSPORT, PERSONAL CONVEYANCES, OR ACCOMMODATION SPECIALLY ADAPTED FOR PATIENTS OR DISABLED PERSONS; OPERATING TABLES OR CHAIRS; CHAIRS FOR DENTISTRY; FUNERAL DEVICES
    • A61G2203/00General characteristics of devices
    • A61G2203/30General characteristics of devices characterised by sensor means
    • A61G2203/40General characteristics of devices characterised by sensor means for distance
    • AHUMAN NECESSITIES
    • A61MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61GTRANSPORT, PERSONAL CONVEYANCES, OR ACCOMMODATION SPECIALLY ADAPTED FOR PATIENTS OR DISABLED PERSONS; OPERATING TABLES OR CHAIRS; CHAIRS FOR DENTISTRY; FUNERAL DEVICES
    • A61G2205/00General identification or selection means
    • A61G2205/60General identification or selection means using magnetic or electronic identifications, e.g. chips, RFID, electronic tags
    • YGENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02DCLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00Reducing energy consumption in communication networks
    • Y02D30/70Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Health & Medical Sciences (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Animal Behavior & Ethology (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Engineering & Computer Science (AREA)
  • Physical Education & Sports Medicine (AREA)
  • Pain & Pain Management (AREA)
  • Rehabilitation Therapy (AREA)
  • Epidemiology (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention discloses a walking stick-wheelchair automatic following system and a method, wherein the system comprises a wheelchair following module, a walking stick following remote control module and a wheelchair intervention control module; the walking stick following remote control module is responsible for serving as a signal tag for positioning, and can transmit a key control instruction to the wheelchair following module through the tag so as to control the movement of the wheelchair; the wheelchair following module is provided with a signal processing device, and can position the tag under a dynamic positioning algorithm, and judge the motion of the wheelchair according to the information such as the relative distance of the target, so that the wheelchair can follow the target; the wheelchair intervention control module is a module added in a wheelchair controller, allows an external signal to control the wheelchair to move, enables the walking stick to control the wheelchair to move after communicating with the wheelchair following module, and enables the wheelchair to follow a target. The invention can remotely control the wheelchair through the following and control functions, can effectively prevent the transmission of infectious diseases and ensure the physical health of guardianship personnel.

Description

Walking stick-wheelchair automatic following system and method
Technical Field
The invention relates to the intelligent walking stick and wheelchair technology, in particular to an automatic walking stick-wheelchair following system and method.
Background
Along with the progress of science and technology and the development of society, the robot industry in China is vigorously developed, the current robot technology has moved from the manufacturing industry to the fields of medical treatment, service, resource acquisition and the like, and in the field of mobile robots, the following technology is more and more focused by people, and the research of mobile robots with the following function is also gradually growing. The following robot has fine application in many scenes, and people can not avoid meeting the condition of transport article in living, and people can select personally carrying generally, develop a following robot and follow user's help its transport under the condition that the user allows, can effectively liberate user's both hands, provide convenience. In the medical field, the number of disabled people is increased continuously along with the continuous aggravation of the aging phenomenon at the present stage, the nursing staff of hospitals and some medical institutions is increased in monitoring burden, and the wheelchair with the autonomous following function can enable wheelchair users to automatically follow nursing staff, so that monitoring pressure is reduced. In addition, the body can be effectively prevented from directly contacting the wheelchair in the monitoring process, the monitoring distance between the guardian and the wheelchair user is enlarged, and the transmission of infectious diseases can be effectively prevented.
From the view of numerous practical demands, the following robot has high comprehensiveness and strong permeability, and can bring great convenience to the life of people. The walking stick-wheelchair following system with the following function is developed by combining the following technology with a traditional medical auxiliary appliance wheelchair and a walking stick, so that the walking stick and the wheelchair are integrated, and a wheelchair user can conveniently manage the wheelchair when walking needs exist. A new strength is injected into the medical and aged industries in China, and convenience is provided for users.
At present, some automatic following systems are in a theoretical research stage, and rarely applied to actual life, particularly an intelligent wheelchair. The existing following system generally utilizes ultrasonic ranging or visual recognition to realize the following, but in the ultrasonic following system, due to factors such as environmental interference, absorption of ultrasonic waves by human clothing and the like, the accuracy of ultrasonic ranging is lower, so that the rapid and accurate following is difficult to realize; the existing vision algorithm has poor real-time performance in specific application, and the robustness of the object detection algorithm is insufficient.
Disclosure of Invention
The invention aims to solve the technical problem of providing an automatic walking stick-wheelchair following system and an automatic walking stick-wheelchair following method aiming at the defects in the prior art.
The invention solves the technical problems by adopting a technical scheme that: an automatic walking stick and wheelchair following method comprises the following steps:
1) Acquiring distance information between a signal tag serving as a following target and a signal processing device, wherein the signal tag is arranged in a portable walking stick of a wheelchair user; the signal processing device is fixed at the bottom of the wheelchair;
2) According to the distance information between the signal tag and the signal processing device, the signal tag target is positioned, specifically as follows:
2.1 Distance measurement is carried out to respectively obtain the distance from the signal processing device to the tag, and then initial positioning is carried out to obtain the initial position (x, y, z) of the target;
finishing distance measurement between the signal labels and the signal processing device through an SDC-TWR ranging algorithm, setting the initial height of the signal labels from the ground as h after finishing ranging, and carrying out two-dimensional positioning on the signal labels through the TOA algorithm by utilizing the signal processing device to obtain target initial positions (x, y, z);
2.2 Carrying out dynamic positioning based on Taylor series expansion by fusion Kalman filtering to finally obtain (x, y) values of the targets in the constraint x, y directions;
2.2.1 Firstly, the true distance from the signal processing device to the signal tag is equal to the measured distances R1, R2 and R3 plus corresponding errors to obtain an equation:
Figure BDA0002799466430000031
after Taylor expansion is carried out at the estimated point of the target position, the deviation between the estimated point and the target real point is separated by using a weighted least square method, the estimated value of the target position is updated by using the deviation, the updated estimated position is calculated again to obtain the deviation, and the process above the iteration is repeated until the deviation |delta in the x and y directions in the deviation x |≤t thres And delta y |≤t thres (wherein t thres For a preset error threshold value), and finally obtaining an optimal solution (x, y) value of the target point;
2.2.2 It can be seen that in the positioning method based on taylor expansion in the last step, each step needs to be carried into an estimated point first, and coordinates of the target point are obtained after iteration. The estimated points determine the number of iterations and the accuracy of the final result, and the estimated points at each moment in the movement process are obtained by kalman filtering in 2.2.1.
In the whole Kalman filtering process, the measurement equation is:
Z(k)=HX(k)+V(k)
wherein Z (k) is an observed value at a moment of a system k, a target optimal solution T0 (x, y, Z) obtained through a Taylor series expansion method at each moment is taken as the observed value, V (k) is measurement process noise, and H is a measurement matrix:
the state prediction equation is:
X(k|k-1)=AX(k-1)+BU(k)
where X (k|k-1) is the current state result estimated from the state quantity X (k-1) of the target at time k-1, A is the state transition matrix, and BU (k) is the control input.
The estimated value of the target at the next moment obtained by Kalman filtering is brought into a Taylor expansion method to obtain an optimal solution of the target, and the optimal solution is continuously updated as a measured value in the Kalman filtering process, so that the optimal solution of the target position at each moment can be obtained, and the effect of dynamic positioning is realized.
3) And according to the signal tag positioning information, combining the ultrasonic sensor information for obstacle avoidance, obtaining a following control instruction comprising the wheelchair following speed and the wheelchair following angular speed, and controlling the movement of the wheelchair to realize accurate following.
An automatic walking stick and wheelchair following system comprising the steps of:
the wheelchair following distance module is used for acquiring distance information between a signal tag serving as a following target and the signal processing device, and the signal tag is arranged in a portable walking stick of a wheelchair user; the signal processing device is fixed at the bottom of the wheelchair;
the wheelchair following positioning module is used for positioning a signal tag target according to the distance information between the signal tag and the signal processing device, and specifically comprises the following steps:
1) Ranging to obtain the distance between the signal processing device and the tag respectively, and then performing initial positioning to obtain the initial position (x, y, z) of the target;
finishing distance measurement between the signal labels and the signal processing device through an SDC-TWR ranging algorithm, setting the initial height of the signal labels from the ground as h after finishing ranging, and carrying out two-dimensional positioning on the signal labels through the TOA algorithm by utilizing the signal processing device to obtain target initial positions (x, y, z);
2) Carrying out dynamic positioning based on Taylor series expansion by fusion Kalman filtering to finally obtain (x, y) values of the targets in the constraint x, y directions;
after Taylor expansion is carried out at the estimated point of the target position, the deviation between the estimated point and the target real point is separated by using a weighted least square method, the estimated value of the target position is updated by using the deviation, the updated estimated position is calculated again to obtain the deviation, and the process above the iteration is repeated until the deviation |delta in the x and y directions in the deviation x |≤t thres And delta y |≤t thres Wherein t is thres The iteration is terminated for a preset error threshold value, and finally an optimal solution (x, y) value of the target point is obtained;
the wheelchair following control module is used for acquiring following control instructions comprising wheelchair following speed and wheelchair following angular speed according to signal tag positioning information and combining ultrasonic sensor information for obstacle avoidance, and controlling the movement of the wheelchair to realize accurate following.
According to the scheme, the step 2) in the wheelchair following positioning module is specifically as follows:
2.1 A three-dimensional coordinate system is established, and a signal processing device is utilized for three-dimensional positioning, so that (x, y) values which are not influenced along with the height are obtained for following;
the signal processing device is provided with three points A0, A1 and A2 respectively, an X-y-z three-axis coordinate system is established by taking A1A0 as an X axis and the center of A1A0 as an origin, a signal label coordinate is set as T0, and the measuring distances from T0 to the points A0, A1 and A2 are R1, R2, R3 and epsilon respectively i For the difference between the corresponding measured distance and the true distance, the coordinates of the three points are A 0 (x 1 ,y 1 ,z 1 ),A 1 (x 2 ,y 2 ,z 2 ),A 2 (x 3 ,y 3 ,z 3 ) The target point is an unknown point T 0 (x, y, z), the following equation is obtained:
Figure BDA0002799466430000061
the definition function is as follows:
Figure BDA0002799466430000062
f i (x,y,z)=R ii
let T be 0 ′(x v ,y v ,z v ) For the target point T 0 The estimated positions of (x, y, z) are:
Figure BDA0002799466430000063
wherein delta x ,δ y ,δ z To estimate the error of the point corresponding to the target point in each coordinate direction, a function f is applied i (x, y, z) at point T 0 ′(x v ,y v ,z v ) The Taylor expansion is carried out, the components above 1 st order are ignored, and the first two items are taken to obtain:
f i,vi,1 δ xi,2 δ yi,3 δ z ≈R ii
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure BDA0002799466430000071
2.2 Estimating the initial position of the target, continuously optimizing the initial value under the principle of least square, and finding an optimal solution for the distance true value to reach a limiting threshold value;
calculating the updated estimated position again to obtain deviation, repeating the above process until |delta in the deviation x |≤t thres And delta y |≤t thres Wherein t is thres For a preset error threshold, the position T of the tag at the moment 0 (x, y, z) is (x) vx ,y vy ,z vz ) To ensure that the located point is always above the base station plane, for z vz Absolute value of the valueAnd when the position of the target is calculated each time, an initial estimated value is brought in, then the initial positioning based on TOA is combined, the signal processing device is used for positioning the initial two-dimensional coordinates of the tag, and then the estimated position of the target at each moment is obtained by combining Kalman filtering in the moving process.
According to the scheme, the wheelchair follows the estimated value (x, y, z) of the initial position when the position of the target is calculated in the step 2.2) in the positioning module each time, and the estimated value is obtained through Kalman filtering prediction; the method comprises the following steps:
taking the optimal solution coordinate of the target at the moment k-1 obtained by the Taylor series expansion method as an observation value, and entering the iterative process of Kalman filtering;
the measurement equation is:
Z(k-1)=HX(k-1)+V(k-1)
wherein Z (k) is an observed value of a system k-1 moment, namely a coordinate of a target k-1 moment obtained by a Taylor series expansion method, is an initial estimated value of the target k-1 moment, V (k) is measurement process noise, and H is a measurement matrix:
Figure BDA0002799466430000081
the system state transfer equation in kalman filtering is:
X(k)=AX(k-1)+BU(k)+W(k)
wherein k is discrete time, U (k) is control input of the k moment system, B is a corresponding conversion matrix, in this process, B and U (k) are both 0 matrices, W (k) is white noise, X (k) is a k moment target state estimation value, that is, X (k) is a space coordinate of the k moment target and a speed corresponding to each direction:
X(k)=[x(k),y(k),z(k),v x (k),v y (k),v z (k)]
wherein A is a transition matrix of target states from time k-1 to time k:
Figure BDA0002799466430000091
wherein T is the sampling time, and corresponds to the transmission frequency between the signal modules.
According to the scheme, the wheelchair following speed and the wheelchair following angular speed in the wheelchair following control module are obtained as follows: after the variable blurring and the fuzzy rule establishment are carried out, the wheelchair following speed is determined according to the target distance deviation and the distance deviation change rate, and the wheelchair following angular speed is determined according to the target distance deviation and the angle deviation.
The invention has the beneficial effects that:
1. the walking stick-wheelchair automatic following system and the walking stick-wheelchair automatic following method have high accuracy in a short distance, are not easy to be interfered by environmental factors, and have accurate following and quick response speed.
2. The walking stick-wheelchair automatic following system and the walking stick-wheelchair automatic following method can meet the requirement that a plurality of wheelchair users use the wheelchair and the walking stick at the present stage, and effectively solve the problem that the wheelchair cannot be arranged when the wheelchair users leave the wheelchair to walk and move by using the walking stick.
3. The walking stick-wheelchair automatic following system and the walking stick-wheelchair automatic following method can realize automatic following of a walking partner and a carer, save manual control and enable a user to be more convenient and comfortable.
4) The walking stick-wheelchair automatic following system and the walking stick-wheelchair automatic following method are not only suitable for wheelchair following, but also can be widely applied to other following scenes. And each device in the system has small volume, convenient installation and good concealment, and can not influence the normal life of a user.
Drawings
The invention will be further described with reference to the accompanying drawings and examples, in which:
FIG. 1 is a block diagram of an automatic following wheelchair system according to an embodiment of the present invention.
Fig. 2 is a structural flow chart of a wheelchair following system module according to an embodiment of the present invention.
FIG. 3 is a flow chart of a positioning system of an automatic following wheelchair system according to an embodiment of the present invention.
FIG. 4 is a fuzzy following block diagram of an automatic following wheelchair system in accordance with an embodiment of the present invention.
Fig. 5 is a schematic diagram of autonomous wheelchair following according to an embodiment of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the drawings and examples, in order to make the objects, technical solutions and advantages of the present invention more apparent. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention.
As shown in fig. 1, in the embodiment of the invention, the automatic walking stick and wheelchair following system comprises three parts, namely a walking stick following remote control module, a wheelchair following module and a wheelchair intervention control module, wherein the walking stick following remote control module comprises a key module, a walking stick microprocessor and a UWB tag, is responsible for serving as a tag for positioning by using the UWB and can transmit a key control instruction to the wheelchair following module through the tag; the wheelchair following module comprises a UWB base station group, an ultrasonic sensor and a master control micro-controller, wherein the UWB base station group receives data information transmitted by a UWB tag and transmits the data information to the master control micro-controller, the tag can be positioned under a dynamic positioning algorithm, the motion of the wheelchair can be subjected to fuzzy control according to information such as the relative distance of a target, so that the wheelchair can follow the target, the ultrasonic sensor checks the surrounding environment, and check data are transmitted to the master control micro-controller; the wheelchair intervention control module comprises a microcontroller and a DA output module, wherein the microcontroller and the DA output module are added in a wheelchair controller, the original wheelchair controller can only send a control instruction to a bottom layer control chip in the controller through a single signal of a control lever, and after the intervention control module designed in the specification is added, an external signal can control the module to simulate voltage change caused by the control lever, and the analog signal is transmitted to a bottom layer wheelchair motion control chip, so that the function of controlling wheelchair motion by the external signal is achieved.
An automatic walking stick and wheelchair following system comprising the steps of:
the wheelchair following distance module is used for acquiring distance information between a signal tag serving as a following target and the signal processing device, and the signal tag is arranged in a portable walking stick of a wheelchair user; the signal processing device is fixed at the bottom of the wheelchair;
the wheelchair following positioning module is used for positioning a signal tag target according to the distance information between the signal tag and the signal processing device, and specifically comprises the following steps:
1) Ranging to obtain the distance between the signal processing device and the tag respectively, and then performing initial positioning to obtain the initial position (x, y, z) of the target;
finishing distance measurement between the signal labels and the signal processing device through an SDC-TWR ranging algorithm, setting the initial height of the signal labels from the ground as h after finishing ranging, and carrying out two-dimensional positioning on the signal labels through the TOA algorithm by utilizing the signal processing device to obtain target initial positions (x, y, z);
2) Carrying out dynamic positioning based on Taylor series expansion by fusion Kalman filtering to finally obtain (x, y) values of the targets in the constraint x, y directions;
after Taylor expansion is carried out at the estimated point of the target position, the deviation between the estimated point and the target real point is separated by using a weighted least square method, the estimated value of the target position is updated by using the deviation, the updated estimated position is calculated again to obtain the deviation, and the process above the iteration is repeated until the deviation |delta in the x and y directions in the deviation x |≤t thres And delta y |≤t thres Wherein t is thres The iteration is terminated for a preset error threshold value, and finally an optimal solution (x, y) value of the target point is obtained;
the wheelchair following control module is used for acquiring following control instructions comprising wheelchair following speed and wheelchair following angular speed according to signal tag positioning information and combining ultrasonic sensor information for obstacle avoidance, and controlling the movement of the wheelchair to realize accurate following.
The wheelchair following positioning module comprises the following step 2):
2.1 A three-dimensional coordinate system is established, and a signal processing device is utilized for three-dimensional positioning, so that (x, y) values which are not influenced along with the height are obtained for following;
the signal processing device is provided with three points A0, A1 and A2 respectively, an X-y-z three-axis coordinate system is established by taking A1A0 as an X axis and the center of A1A0 as an origin, the signal label coordinate is set as T0, and T0 is from T0 to T0The measured distances of points A0, A1 and A2 are R1, R2, R3 and epsilon respectively i For the difference between the corresponding measured distance and the true distance, the coordinates of the three points are A 0 (x 1 ,y 1 ,z 1 ),A 1 (x 2 ,y 2 ,z 2 ),A 2 (x 3 ,y 3 ,z 3 ) The target point is an unknown point T 0 (x, y, z), the following equation is obtained:
Figure BDA0002799466430000131
the definition function is as follows:
Figure BDA0002799466430000132
f i (x,y,z)=R ii
let T be 0 ′(x v ,y v ,z v ) For the target point T 0 The estimated positions of (x, y, z) are:
Figure BDA0002799466430000133
wherein delta x ,δ y ,δ z To estimate the error of the point corresponding to the target point in each coordinate direction, a function f is applied i (x, y, z) at point T 0 ′(x v ,y v ,z v ) The Taylor expansion is carried out, the components above 1 st order are ignored, and the first two items are taken to obtain:
f i,vi,1 δ xi,2 δ yi,3 δ z ≈R ii
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure BDA0002799466430000134
2.2 Estimating the initial position of the target, continuously optimizing the initial value under the principle of least square, and finding an optimal solution for the distance true value to reach a limiting threshold value;
calculating the updated estimated position again to obtain deviation, repeating the above process until |delta in the deviation x |≤t thres And delta y |≤t thres Wherein t is thres For a preset error threshold, the position T of the tag at the moment 0 (x, y, z) is (x) vx ,y vy ,z vz ) To ensure that the located point is always above the base station plane, for z vz The value is processed by taking absolute value, an initial estimated value is firstly brought in when the position of the target is calculated each time, then the initial two-dimensional coordinates of the label are positioned by a signal processing device in combination with TOA-based initialization positioning, and then the estimated position of the target at each moment is obtained by combining Kalman filtering in the moving process.
The wheelchair follows the estimated value (x, y, z) of the initial position at each calculation of the position of the target in step 2.2) in the positioning module, obtained by kalman filtering prediction; the method comprises the following steps:
taking the optimal solution coordinate of the target at the moment k-1 obtained by the Taylor series expansion method as an observation value, and entering the iterative process of Kalman filtering;
the measurement equation is:
Z(k-1)=HX(k-1)+V(k-1)
wherein Z (k) is an observed value of a system k-1 moment, namely a coordinate of a target k-1 moment obtained by a Taylor series expansion method, is an initial estimated value of the target k-1 moment, V (k) is measurement process noise, and H is a measurement matrix:
Figure BDA0002799466430000151
the system state transfer equation in kalman filtering is:
X(k)=AX(k-1)+BU(k)+W(k)
wherein k is discrete time, U (k) is control input of the k moment system, B is a corresponding conversion matrix, in this process, B and U (k) are both 0 matrices, W (k) is white noise, X (k) is a k moment target state estimation value, that is, X (k) is a space coordinate of the k moment target and a speed corresponding to each direction:
X(k)=[x(k),y(k),z(k),v x (k),v y (k),v z (k)]
wherein A is a transition matrix of target states from time k-1 to time k:
Figure BDA0002799466430000152
wherein T is the sampling time, and corresponds to the transmission frequency between the signal modules.
The wheelchair following speed and the wheelchair following angular velocity in the wheelchair following control module are obtained as follows: after the variable blurring and the fuzzy rule establishment are carried out, the wheelchair following speed is determined according to the target distance deviation and the distance deviation change rate, and the wheelchair following angular speed is determined according to the target distance deviation and the angle deviation.
As shown in fig. 2, the wheelchair system according to the embodiment of the present invention follows the workflow of the mode, firstly, the system is initialized, then the data of the ultrasonic module is read, and it is determined whether the obstacle in front of the wheelchair is too close, if so, emergency braking information is sent to the intervention module, and the wheelchair intervention module sends a signal to the wheelchair bottom control center, so as to change the wheelchair state; if not, the data of the serial port UART1 is read, whether the data head format is mv or mw is judged, if yes, the RANG0 value is read and packaged into the data format, a walking stick control instruction is sent to the intervention module, if not, whether the data head format is mc is judged, if not, abnormal information is sent to the intervention module, if yes, whether the MASK value is 7 is judged again, if not, abnormal information is sent, if yes, the values of the three base stations RANG0, RANG1 and RANG2 are sequentially read, a dynamic positioning algorithm is utilized to obtain target coordinates, then the relative distance and speed of the target relative to a wheelchair are obtained, a fuzzy control algorithm is adopted to conduct data processing, and a wheelchair following instruction is sent to the intervention module to realize following.
As shown in FIG. 3, the automatic following wheelchair system positioning system according to the embodiment of the invention obtains the distance information between the three base stations and the tag through the SDC-TWR ranging algorithm, then performs initial positioning based on TOA to obtain initial coordinates (x, y, z), then fuses the three-dimensional positioning of the Taylor series expansion of the Kalman filtering to finally obtain (x, y) values of the targets constrained in the x and y directions, and when the UWB transmission path is blocked by an obstacle, the system reduces the generated non-line-of-sight error by using the gain self-adaption method of the Kalman filtering.
As shown in fig. 4, the control process of the fuzzy control following structure of the automatic following wheelchair system according to the embodiment of the present invention mainly comprises three parts: variable fuzzification, fuzzy reasoning and output definition. The variable fuzzification is to convert the variable into a language variable so that the language variable can be matched with rules in an established rule base, and the rules in the rule base are summarized according to human experience and mastery of rules. Fuzzy reasoning is to match the actual state with rules in a rule base, as if a person finds out a coping strategy from his own experience. And finally, the process of deblurring and converting the fuzzy quantity obtained by reasoning into an accurate value is output definition. The system determines the angular speed and the linear speed of the wheelchair at any time through fuzzy reasoning according to the distance deviation, the distance deviation change rate and the angle deviation between the wheelchair and the target in the following process, and forms the MIMO system consisting of a plurality of inputs and a plurality of outputs. Specifically, the linear speed of the wheelchair is determined through the following distance deviation and the distance deviation change rate, and the angular speed of the wheelchair is determined through the following distance deviation and the following angle deviation. The following algorithm of the wheelchair in an unobstructed environment requires: and (3) fuzzifying the variables, establishing a fuzzy rule base, completing fuzzy reasoning and outputting the wheelchair motion quantity.
As shown in fig. 5, according to the principle of autonomous wheelchair following in the embodiment of the present invention, when a front target person moves using a cane, a UWB base station is used to locate the relative position of the cane, i.e., the target person, where the origin of the coordinate system is the center of A0 A1.
According to the system, a corresponding method can be obtained:
an automatic walking stick and wheelchair following method comprises the following steps:
1) Acquiring distance information between a signal tag serving as a following target and a signal processing device, wherein the signal tag is arranged in a portable walking stick of a wheelchair user; the signal processing device is fixed at the bottom of the wheelchair;
2) According to the distance information between the signal tag and the signal processing device, the signal tag target is positioned, specifically as follows:
2.1 Distance measurement is carried out to respectively obtain the distance from the signal processing device to the tag, and then initial positioning is carried out to obtain the initial position (x, y, z) of the target;
finishing distance measurement between the signal labels and the signal processing device through an SDC-TWR ranging algorithm, setting the initial height of the signal labels from the ground as h after finishing ranging, and carrying out two-dimensional positioning on the signal labels through the TOA algorithm by utilizing the signal processing device to obtain target initial positions (x, y, z);
2.2 Carrying out dynamic positioning based on Taylor series expansion by fusion Kalman filtering to finally obtain (x, y) values of the targets in the constraint x, y directions;
2.2.1 Firstly, the true distance from the signal processing device to the signal tag is equal to the measured distances R1, R2 and R3 plus corresponding errors to obtain an equation:
Figure BDA0002799466430000181
after Taylor expansion is carried out at the estimated point of the target position, the deviation between the estimated point and the target real point is separated by using a weighted least square method, the estimated value of the target position is updated by using the deviation, the updated estimated position is calculated again to obtain the deviation, and the process above the iteration is repeated until the deviation |delta in the x and y directions in the deviation x |≤t thres And delta y |≤t thres (wherein t thres For a preset error threshold value), and finally obtaining an optimal solution (x, y) value of the target point;
2.2.2 It can be seen that in the positioning method based on taylor expansion in the last step, each step needs to be carried into an estimated point first, and coordinates of the target point are obtained after iteration. The estimated points determine the number of iterations and the accuracy of the final result, and the estimated points at each moment in the movement process are obtained by kalman filtering in 2.2.1.
In the whole Kalman filtering process, the measurement equation is:
Z(k)=HX(k)+V(k)
wherein Z (k) is an observed value at a moment of a system k, a target optimal solution T0 (x, y, Z) obtained through a Taylor series expansion method at each moment is taken as the observed value, V (k) is measurement process noise, and H is a measurement matrix:
the state prediction equation is:
X(k|k-1)=AX(k-1)+BU(k)
where X (k|k-1) is the current state result estimated from the state quantity X (k-1) of the target at time k-1, A is the state transition matrix, and BU (k) is the control input.
The estimated value of the target at the next moment obtained by Kalman filtering is brought into a Taylor expansion method to obtain an optimal solution of the target, and the optimal solution is continuously updated as a measured value in the Kalman filtering process, so that the optimal solution of the target position at each moment can be obtained, and the effect of dynamic positioning is realized.
3) And according to the signal tag positioning information, combining the ultrasonic sensor information for obstacle avoidance, obtaining a following control instruction comprising the wheelchair following speed and the wheelchair following angular speed, and controlling the movement of the wheelchair to realize accurate following.
It will be understood that modifications and variations will be apparent to those skilled in the art from the foregoing description, and it is intended that all such modifications and variations be included within the scope of the following claims.

Claims (5)

1. An automatic walking stick-wheelchair following method is characterized by comprising the following steps:
s1, placing a signal tag serving as a following target in a walking stick, establishing communication with a signal processing device arranged on a wheelchair through a wireless signal, and acquiring distance information between the signal tag and the signal processing device; the signal tag is a UWB tag;
s2, transmitting distance information to a main control microprocessor through a UART to perform positioning processing;
the method for positioning processing by the main control microprocessor in the step S2 comprises the following steps:
firstly, obtaining the distance between a wheelchair and a walking stick, then, carrying out initial positioning to obtain coordinates (x, y, h) of a target signal tag, and then, carrying out dynamic positioning by a Taylor series expansion method fused with Kalman filtering to finally obtain (x, y) values of the target coordinates in the x and y axis directions, so as to realize a dynamic positioning method, wherein the dynamic positioning algorithm can effectively eliminate the inaccurate positioning influence caused by the height change of the walking stick during movement;
the signal processing device is provided with three points A0, A1 and A2 respectively, an X-y-z three-axis coordinate system is established by taking A1A0 as an X axis and the center of A1A0 as an origin, a signal label coordinate is set as T0, and the measuring distances from T0 to the points A0, A1 and A2 are respectively R1, R2 and R3,
Figure QLYQS_1
for the difference between the corresponding measured distance and the true distance, the coordinates of the three points are +.>
Figure QLYQS_2
,/>
Figure QLYQS_3
,/>
Figure QLYQS_4
The target point is unknown point->
Figure QLYQS_5
The Taylor series expansion method specifically comprises the following steps: the true distance from the signal processing device to the tag is equal to the measured distance value plus an error to obtain the equation:
Figure QLYQS_6
at the target position
Figure QLYQS_7
After taylor expansion is carried out on the estimated points of the target position, the deviation between the estimated points and the target real points is separated by using a weighted least square method, the estimated value of the target position is updated by using the deviation, the updated estimated position is calculated again to obtain the deviation, the estimated value of the target position is updated by using the deviation, the process of calculating the updated estimated position again to obtain the deviation is iterated until the deviation in the x and y directions in the deviation is obtained>
Figure QLYQS_8
And->
Figure QLYQS_9
The method comprises the steps of carrying out a first treatment on the surface of the Wherein->
Figure QLYQS_10
An error threshold value is set in advance; ending the iteration, and finally obtaining an optimal solution (x, y) value of the target point for positioning;
the method comprises the steps that an estimated value of a target position is needed in each iteration, so that in the initial process, the distance from a base station group to a tag is obtained based on a ranging algorithm, a walking stick is set to be vertical in the initial process, the height of the tag from the ground is h, the tag is subjected to two-dimensional positioning by the base station group through a TOA algorithm to obtain the estimated value of the target initial position (x, y, h); in the repeated iteration process, the estimated value of the initial position of the target is obtained through Kalman filtering prediction when the position of the target is calculated each time;
in the moving process, using a value predicted by Kalman filtering as an estimated value of a target position of a Taylor series expansion method; the coordinates of each moment of the target obtained by the Taylor series expansion method are used as the measured value of the target position, and enter the iterative process of Kalman filtering to meet the system requirements;
s3, the main control microprocessor combines the obstacle avoidance sensor information and transmits a following control instruction to the wheelchair intervention control module through the IIC;
s4, transmitting the instruction signal to a wheelchair bottom layer control center through a microcontroller and a D/A output module of the wheelchair intervention control module, so as to control the wheelchair to move and realize the rapid and accurate following of the walking stick by the wheelchair.
2. An automatic walking stick-wheelchair following system adopting the automatic walking stick-wheelchair following method as claimed in claim 1, comprising a walking stick following remote control module, a wheelchair following module and a wheelchair intervention control module, wherein the walking stick following remote control module is responsible for serving as a signal tag for positioning and can transmit a key control instruction to the wheelchair following module through the tag so as to control the movement of a wheelchair; the wheelchair following module is provided with a signal processing device, and can position the tag under a dynamic positioning algorithm, and judge the movement of the wheelchair according to the relative distance information of the target so that the wheelchair can follow the target; the wheelchair intervention control module is a module added in a wheelchair controller, allows an external signal to control the wheelchair to move, enables the walking stick to control the wheelchair to move after communicating with the wheelchair following module, and enables the wheelchair to follow a target.
3. The automatic cane-wheelchair following system of claim 2 wherein the cane following remote control module includes a key module, a cane microprocessor, a signal tag, the key module and the signal tag each establishing a connection with the microprocessor.
4. The automatic walking stick-wheelchair following system according to claim 2, wherein the wheelchair following module comprises a signal processing device, an obstacle avoidance sensor and a main microprocessor, wherein the signal receiving device is arranged to acquire position information of the walking stick after communication with a signal tag of the walking stick is established, and simultaneously can directly transmit remote control information of the walking stick, and the obstacle avoidance sensor is used for acquiring obstacle information in surrounding environment; the main control microprocessor processes the data and transmits the processed data to the wheelchair intervention control module to realize the following and remote control functions of the wheelchair.
5. The automatic cane-wheelchair following system of claim 2 wherein the wheelchair intervention control module comprises a microcontroller, a D/a output module that converts the processed digital signal to an analog voltage value that simulates the voltage change when the joystick is controlled and transfers it to the underlying motion interface to control the speed and direction of motion of the wheelchair.
CN202011344588.4A 2020-11-25 2020-11-25 Walking stick-wheelchair automatic following system and method Active CN112472432B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011344588.4A CN112472432B (en) 2020-11-25 2020-11-25 Walking stick-wheelchair automatic following system and method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011344588.4A CN112472432B (en) 2020-11-25 2020-11-25 Walking stick-wheelchair automatic following system and method

Publications (2)

Publication Number Publication Date
CN112472432A CN112472432A (en) 2021-03-12
CN112472432B true CN112472432B (en) 2023-06-30

Family

ID=74934912

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011344588.4A Active CN112472432B (en) 2020-11-25 2020-11-25 Walking stick-wheelchair automatic following system and method

Country Status (1)

Country Link
CN (1) CN112472432B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113791535B (en) * 2021-09-29 2024-04-19 北京布科思科技有限公司 Track following real-time control method, device and system and storage medium

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102010029637A1 (en) * 2010-06-02 2011-12-08 Robert Bosch Gmbh Method for tracking e.g. potential obstruction, for driver assistance system of radar system for motor car, involves performing Kalman filtering based on distance and velocity of object by using movement model and auxiliary variable
CN103925925A (en) * 2014-03-14 2014-07-16 四川九洲空管科技有限责任公司 Real-time high-precision position solution method for multilateration system
WO2018014449A1 (en) * 2016-07-19 2018-01-25 纳恩博(北京)科技有限公司 Method and device for improving performance of relative-position sensor, and computer storage medium
CN108566178A (en) * 2018-04-16 2018-09-21 武汉理工大学 A kind of random opportunistic network characteristic value filtering method of unstable state
WO2020087846A1 (en) * 2018-10-31 2020-05-07 东南大学 Navigation method based on iteratively extended kalman filter fusion inertia and monocular vision
CN111536967A (en) * 2020-04-09 2020-08-14 江苏大学 EKF-based multi-sensor fusion greenhouse inspection robot tracking method

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102579207A (en) * 2012-03-13 2012-07-18 冀中能源峰峰集团有限公司总医院 Wheelchair provided with transfusion frame and cane for walking and added with lying function
CN205598117U (en) * 2016-03-01 2016-09-28 南阳医学高等专科学校 Wheelchair is used in nursing of traceable formula
US11602479B2 (en) * 2018-03-14 2023-03-14 Toyota Motor North America, Inc. Systems and methods for providing synchronized movements of a powered wheelchair and an exoskeleton
CN109270963A (en) * 2018-09-29 2019-01-25 武汉理工大学 A kind of wheelchair automatic following system and method based on reflective fabric and laser radar
CN109283490B (en) * 2018-11-14 2022-11-01 东南大学 Hybrid least square method-based Taylor series expansion UWB positioning method
CN110728701B (en) * 2019-08-23 2022-07-12 珠海格力电器股份有限公司 Control method and device for walking stick with millimeter wave radar and intelligent walking stick

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102010029637A1 (en) * 2010-06-02 2011-12-08 Robert Bosch Gmbh Method for tracking e.g. potential obstruction, for driver assistance system of radar system for motor car, involves performing Kalman filtering based on distance and velocity of object by using movement model and auxiliary variable
CN103925925A (en) * 2014-03-14 2014-07-16 四川九洲空管科技有限责任公司 Real-time high-precision position solution method for multilateration system
WO2018014449A1 (en) * 2016-07-19 2018-01-25 纳恩博(北京)科技有限公司 Method and device for improving performance of relative-position sensor, and computer storage medium
CN108566178A (en) * 2018-04-16 2018-09-21 武汉理工大学 A kind of random opportunistic network characteristic value filtering method of unstable state
WO2020087846A1 (en) * 2018-10-31 2020-05-07 东南大学 Navigation method based on iteratively extended kalman filter fusion inertia and monocular vision
CN111536967A (en) * 2020-04-09 2020-08-14 江苏大学 EKF-based multi-sensor fusion greenhouse inspection robot tracking method

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
New nonlinear iterated filter with applications to target tracking;R.Bellaire等人;PROCEEDINGS OF SPIE;第2561卷;第240-251页 *
基于均值漂移与卡尔曼预测相结合的视频运动目标跟踪算法;姜忠民;赵建民;朱信忠;徐慧英;;计算机时代(02);第22-24页 *
基于高维状态空间的移动机器人定位;庄燕滨;徐则中;;电路与系统学报(01);第109-113页和54页 *
陈慧岩等著.《智能车辆理论与应用》.北京理工大学出版社,2018,第8-9页. *

Also Published As

Publication number Publication date
CN112472432A (en) 2021-03-12

Similar Documents

Publication Publication Date Title
Brigante et al. Towards miniaturization of a MEMS-based wearable motion capture system
KR100883520B1 (en) Method and apparatus for providing indoor eco-map
Zhang et al. An RGB-D camera based visual positioning system for assistive navigation by a robotic navigation aid
CN108536145A (en) A kind of robot system intelligently followed using machine vision and operation method
Chung et al. An improved localization of mobile robotic system based on AMCL algorithm
JP2011209203A (en) Self-position estimating device and self-position estimating method
CN109470238A (en) A kind of localization method, device and mobile terminal
CN110530365A (en) A kind of estimation method of human posture based on adaptive Kalman filter
JP2013531781A (en) Method and system for detecting zero speed state of object
CN112472432B (en) Walking stick-wheelchair automatic following system and method
CN111506199B (en) Kinect-based high-precision unmarked whole-body motion tracking system
CN112131928B (en) Human body posture real-time estimation method based on RGB-D image feature fusion
KR20120072253A (en) Localization device and localization method
Bonnet et al. Towards an affordable mobile analysis platform for pathological walking assessment
Yuan et al. Development of a human-friendly robot for socially aware human-robot interaction
CN112762934B (en) Lower limb movement direction prediction device and method
Qiu et al. Heterogeneous data fusion for three-dimensional gait analysis using wearable MARG sensors
Yan et al. An approach on advanced unscented kalman filter from mobile robot-slam
Hunt et al. Predictive trajectory estimation during rehabilitative tasks in augmented reality using inertial sensors
CN111241936A (en) Human body posture estimation method based on depth and color image feature fusion
CN115240367A (en) UWB (ultra wide band) intelligent positioning based user management early warning method and system
Chen et al. Research on mobile robot SLAM in dynamic environment
Lv et al. FVO: floor vision aided odometry
Wang et al. Indoor localization system based on wearable posture sensors with incomplete observations
Chongyu et al. Deep learning-driven front-following within close proximity: a hands-free control model on a smart walker

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