CN112932822B - Pose detection and control method of electric wheelchair - Google Patents

Abstract

The invention discloses a pose detection and control method of an electric wheelchair, which comprises the steps of installing a speed sensor, an acceleration sensor and an MPU6050 pose gyroscope on wheels of the electric wheelchair, defining the motion, the inclination angle and the angular velocity of the wheelchair, and judging the current motion state of the electric wheelchair by calculating the speed, the acceleration, the steering angular velocity, the Euler angle, the acceleration increment and the wheel speed difference parameters of the electric wheelchair in real time during the traveling of the wheelchair, and detecting and correcting the motion process of the electric wheelchair based on the current motion state of the electric wheelchair.

Description

Pose detection and control method of electric wheelchair

Technical Field

The invention belongs to the technical field of electric wheelchair pose control, and particularly relates to an electric wheelchair pose detection and control method.

Background

With the rapid development of social economy and the continuous improvement of living standard, the proportion of the aged population in China is continuously increased, and the aging problem of the population is more serious. Meanwhile, the number of people with lower limb dysfunction is continuously increased due to frequent occurrence of traffic accidents and natural disasters. In order to improve the autonomous life quality of such people and reduce the burden of caregivers, more and more electric wheelchairs are put into development and use.

On the basis of the electric wheelchair, an intelligent wheelchair with voice control and gesture control is further developed, physiological signals such as brain electricity, electromyographic signals and eye movement signals are extracted, then feature recognition is carried out, and the electric wheelchair is controlled through different actions. However, the series of control systems all require users to learn the control instructions of the wheelchair motion in advance, and electrode plates and the like are required to be pasted on the used electric wheelchair to derive physiological signals, which clearly increases the use difficulty of the audience. Meanwhile, the wheelchair does not detect various parameter indexes such as speed, acceleration, steering angle parameter indexes and the like in the running process of the wheelchair in real time. The motion state of the electric wheelchair is controlled by the user, and to some extent, there is a certain uncertainty and low safety.

Disclosure of Invention

Aiming at the technical problems that the pose of the wheelchair cannot be detected in real time and the motion speed and the angle cannot be corrected automatically based on the current motion state of the electric wheelchair, the invention aims to provide a pose detection and control method of the electric wheelchair, which overcomes the uncertainty and low safety in the advancing process of the electric wheelchair.

In order to achieve the above object, the present invention adopts the following technical solutions:

The method is characterized in that a speed sensor, an acceleration sensor and an MPU6050 attitude gyroscope are arranged on wheels of the electric wheelchair, motion, inclination angle and angular velocity of the wheelchair are defined, parameters of speed, acceleration, steering angular velocity, euler angle, acceleration increment and wheel speed difference in the process of traveling of the electric wheelchair are calculated in real time in the process of traveling of the wheelchair, the current motion state of the electric wheelchair is judged, and the following motion process of the electric wheelchair is detected and corrected based on the current motion state of the electric wheelchair, so that the traveling attitude of the electric wheelchair is adjusted and controlled.

The specific calculation steps are as follows:

1) Defining movement of wheelchair

The x component represents the back and forth motion, and the y component represents the left and right motion; left-hand wheel = x+p x y, right-hand wheel = x-p x y, p is the scaling factor, default p = 0.5, i.e.: left wheel = x+y/2, right wheel = x-y/2;

2) Definition of the tilt angle

The projection represents the wheelchair elevation angle on the x negative axis and the wheelchair depression angle on the x positive axis; the projection on the negative y axis represents the left tilting of the wheelchair, and the projection on the positive y axis represents the right tilting of the electric wheelchair; the projection on the negative z axis represents the inversion of the electric wheelchair, and the projection on the positive z axis represents the erection of the electric wheelchair; the definition shows that the x-axis direction is a pitch angle, the y-axis direction is a roll angle, and the pitch angle and the roll angle are two Euler angles;

In order to ensure the running safety of the wheelchair, the forward acceleration value of the electric wheelchair is limited to be not more than 0.2g, the reverse acceleration value is not more than-0.5 g, and the g is the earth gravity acceleration value. The acceleration limit value is not set, and only the speed and angular speed limit value is set;

3) Definition of angular velocity

The heading angle is rotated by taking the z axis as the central axis, the roll angle is rotated by taking the x axis as the central axis, and the pitch angle is rotated by taking the y axis as the central axis. The maximum angular velocity of the z-axis is 1 radian/s, which corresponds to one revolution of 6.28 seconds;

4) Calculation basis

(1) Speed of: the pulse interval time of the wheel of the wheelchair is 100 mu s/3 as a time scale, and is calculated once when the PID of the motor of the electric wheelchair is regulated each time, and the PID regulation interval is 10ms; taking the average value of the two wheels as a forward speed v, wherein v is positive number and forward, and v is negative number and backward;

(2) Acceleration: speed difference of two acquisition time intervals;

(3) Steering angular velocity:

a. from the wheel speed difference calculation, the following formula is calculated 1 time each time the wheelchair motor PID adjusts, and the PID adjusting interval is 10ms:

wherein v _Left is the travel speed of the left wheel of the electric wheelchair, v _Left is the travel speed of the right wheel of the electric wheelchair, and r is the track;

b. Reading the steering angular velocity from the MPU6050 attitude gyroscope;

the difference value of a and b in the step (3) can be used for judging whether the wheel of the electric wheelchair slips, and the difference value is not less than 0, which indicates that the wheel of the electric wheelchair does not slip.

(4) When the pitch angle is calculated, an acceleration value is read from the MPU6050, and atan2 (accX, accZ) is taken; accX the increment of acceleration introduced by the steering angular velocity needs to be subtracted;

when the roll angle is calculated, an acceleration value is read from the MPU6050, and atan2 (accY, accZ) is taken; accY subtracting an acceleration increment introduced by the steering angular velocity, when the electric wheelchair is acted by a vertical force, if the electric wheelchair suddenly accelerates and decelerates in an elevator, the angle cannot be calculated from the acceleration value;

(5) And (3) calculating acceleration increment:

(6) Wheel speed difference calculation:

the electric wheelchair is used as an origin for recording, the wheelbase is b, the forward motion dy and the right deviation dx are as follows:

Hypotenuse is d=sqrt (dy 2+dx 2);

deflection angle θ=atan2 (dx, dy);

turning center radius r=d/(2 sin θ);

center arc length l=2θ (2×pi×r)/(2×pi) = 2*R ×θ=d×θ/sin θ;

the left wheel advances: l1=2 (r+b/2) ×θ= (2 r+b) ×θ;

The right wheel advances: l2=2+ (R-b/2) θ= (2R-b) θ;

Based on the left wheel, the right wheel should be multiplied by a coefficient P, p=l1/l2= (2 r+b)/(2R-b) = (d+ bsin θ)/(d-bsin θ).

5) And (3) adjusting and controlling the wheelchair travelling posture according to the calculated result in the step 4).

Compared with an electric wheelchair controlled by voice and gestures and an intelligent wheelchair based on physiological signal feature recognition, the electric wheelchair pose detection and control method has the following beneficial technical effects: the learning time of a user is reduced to a certain extent, the using steps of sticking electrode plates and the like are also reduced, and the range of the user population is enlarged. Meanwhile, a theoretical basis is provided for the electric wheelchair to realize automatic driving.

Drawings

FIG. 1 is a schematic workflow diagram of a method for detecting and controlling the pose of an electric wheelchair according to the present invention;

FIG. 2 is a top view of the electric wheelchair, wherein x is the forward direction;

FIG. 3 is a schematic view of the movement of the wheelchair in a turn with the x-axis pointing forward and the y-axis pointing to the right and the z-axis pointing downward. The acceleration direction is the same as this. The center of the rear axle of the vehicle is projected to the ground as the origin (i.e., center of rotation).

Fig. 4 is a schematic view of pitch and roll angles.

FIG. 5 is a schematic representation of the change in right wheel displacement during movement of the electric wheelchair, wherein (a) is 5 degrees of right wheel displacement and (b) is 10 degrees of right wheel displacement;

Fig. 6 is a schematic diagram of the change of pose parameters in the motion process of the electric wheelchair, wherein (a) is a map of the ready-to-travel parameters after the wheelchair is started, (b) is a real-time change of displacement parameters in the case of (a) in fig. 5, and (c) is a schematic diagram of the real-time change of pose parameters in the case of (b) in fig. 5.

The present invention will be described in further detail with reference to the accompanying drawings and examples.

Detailed Description

Referring to fig. 1, the present embodiment provides a method for detecting and controlling the pose of an electric wheelchair, in which a speed sensor, an acceleration sensor, and an MPU 6050-pose gyroscope are mounted on the wheels of the electric wheelchair, the motion, the inclination angle, and the angular velocity of the wheelchair are defined, the current motion state of the electric wheelchair is determined by calculating parameters such as the velocity, the acceleration, the steering angular velocity, the euler angle, the acceleration increment, the wheel speed difference, etc. of the electric wheelchair in real time during the traveling of the wheelchair, and the adjustment and control of the traveling pose of the electric wheelchair are performed during the detection and correction of the next electric wheelchair motion process based on the current motion state of the electric wheelchair.

For convenience of calculation, a top view of the electric wheelchair is shown in fig. 2, wherein x is the advancing direction; fig. 3 shows a schematic diagram of the movement of the electric wheelchair during turning, with the x-axis pointing forward, the y-axis pointing to the right and the z-axis pointing downward. The acceleration direction is the same as this. The center of the rear axle of the vehicle is projected to the ground as the origin (i.e., center of rotation). Fig. 4 shows a schematic view of pitch and roll angles.

The specific calculation steps are as follows:

1. defining movement of wheelchair

The X component represents the back and forth motion and the y component represents the left and right motion. Left-hand wheel = x+p x y, right-hand wheel = x-p x y, p is the scaling factor (adjustable), default p = 0.5, i.e.: left wheel = x+y/2, right wheel = x-y/2.

2. Definition of tilt angle (reverse gravity acceleration projection)

The projection represents the wheelchair elevation angle on the x negative axis and the wheelchair depression angle on the x positive axis; the projection on the negative y axis represents the left tilting of the wheelchair, and the projection on the positive y axis represents the right tilting of the electric wheelchair; the projection on the negative z axis represents the inversion of the electric wheelchair, and the projection on the positive z axis represents the erection of the electric wheelchair. From this definition, the x-axis direction is the pitch angle and the y-axis direction is the roll angle.

In various practical application scenes, the maximum acceleration of an elevator is 0.1g, the maximum acceleration of a common car is 0.6g, and the maximum acceleration is close to 1g during sudden braking. The forward acceleration of the electric wheelchair is not more than 0.2g, and the reverse acceleration is not more than-0.5 g. The acceleration limit is not set, but only the speed and angular speed limit.

3. Angular velocity is defined (taking negative values in a brushless direct current motor (BLDC), giving the following results

As shown in fig. 3, the heading angle is a heading angle rotated about the z-axis, the roll angle is a roll angle rotated about the x-axis, and the pitch angle is a pitch angle rotated about the y-axis. The maximum z-axis angular velocity is 1 radian/s, corresponding to 6.28 seconds.

4. Calculation basis

(1) Speed of: the pulse interval time of the wheel of the wheelchair is 100 mu s/3 as a time scale, the average value of two wheels is taken as the forward speed (the positive number is forward, and the negative number is backward) by calculating once every PID adjustment (10 ms is taken currently).

(2) Acceleration: the speed difference of the two acquisition time intervals.

(3) Steering angular velocity:

a. From the wheel speed difference, 1 calculation is performed every PID adjustment (10 ms is currently taken) Wherein r is the track width.

B. reading the steering angular velocity from the MPU 6050;

The difference between a and b can be used to determine whether the wheelchair wheel is slipping.

(4) When calculating the pitch angle, the acceleration value is read from the MPU6050, and atan2 (accX, accZ) is taken. accX is required to subtract the increment of acceleration introduced by the steering angle speed.

When the roll angle is calculated, the acceleration value is read from the MPU6050, and atan2 (accY, accZ) is taken. accY is required to subtract the acceleration increase introduced by the steering angle velocity. (when the electric wheelchair is subjected to a force in the vertical direction, such as sudden acceleration and deceleration and jolt of the electric wheelchair in an elevator, the angle cannot be calculated from the acceleration value).

(5) The acceleration increment calculating method comprises the following steps:

(5) Wheel speed difference calculation:

With the electric wheelchair as an origin, the wheelbase is b, the forward dy and the right deviation dx are d=sqrt (dy 2+dx 2), the deflection angle θ=atan 2 (dx, dy), the turning center radius r=d/(2sin θ), the center arc length l=2θ (2×pi×r)/(2×pi) = 2*R ×θ=d×θ/sin θ, the left wheel forward l1=2 (r+b/2) ×θ= (2 r+b) θ, the right wheel forward l2=2×θ= (R-b/2) ×θ= (2R-b) ×θ), and the right wheel forward coefficient p=l1/l2= (2 r+b) = (d+ bsin θ)/(d-bsin θ) based on the left wheel.

5. And (4) adjusting and controlling the wheelchair travelling posture according to the calculated result in the step (4).

Experimental example:

Referring to fig. 5 and 6, wherein fig. 5 shows a schematic diagram of the change in the displacement of the right wheel during the movement of the electric wheelchair, wherein (a) is 5 degrees of displacement of the right wheel and (b) is 10 degrees of displacement of the right wheel; fig. 6 shows posture change parameters of the wheelchair during actual traveling in the experimental environment of fig. 5, and as can be seen from fig. 6 (a), the Sensors column in fig. 6 (a) is a parameter of the wheelchair ready for traveling after starting, and it can be seen that at this time, the traveling acceleration accX of the wheelchair is 0.026, accy is-0.077, accz is 9.778, and euler angle eular is X:0.15, Y: -0.44; acceleration X of MPU 6050: 2.084, y: -0.534;

When the right wheel of the electric wheelchair is inclined at an angle of 5 degrees in the environment of fig. 5 (a), the electric wheelchair is detected to need to provide power at the moment, so that the motor is accelerated to rotate, and the pose parameters of the wheelchair are converted into fig. 6 (b); it can be seen clearly that: the parameters of the wheelchair motor after power is provided by the Sensors column can be seen that the running acceleration accX of the wheelchair is 0.297, accY is-0.142, accZ is 9.759, and Euler angle eular is X: -1.78, y: -0.83; acceleration X of MPU 6050: 2.102, y: -0.485; accX from 0.026 to 0.865.

This indicates that the electric wheelchair detects this travel pose and is powered by the motor to overcome the uncertainty and low safety of wheelchair travel. When the right wheel inclination angle of the electric wheelchair in the environment of fig. 5 (b) is 10 degrees, the electric wheelchair is detected to need to provide power at the moment, so that the motor rotates in an accelerating way, and the pose parameters of the wheelchair are converted into fig. 6 (b), so that the following can be seen obviously: the parameters of the wheelchair motor after power is provided by the Sensors column can be seen that the advancing acceleration accX of the wheelchair is 0.865, accY is-0.138, accZ is 9.727, and Euler angle eular is X: -5.09, y: -0.83; acceleration X of MPU 6050: 2.109, Y: -0.549; as compared to fig. 6 (b), accX changes from 0.297 to 0.865, which further shows that the electric wheelchair detects this travel pose and overcomes the uncertainty and low safety of wheelchair travel by motor powering, and motor output power control can be performed with the detected pose parameters.

Claims (1)

1. The method is characterized in that a speed sensor, an acceleration sensor and an MPU6050 attitude gyroscope are arranged on wheels of the electric wheelchair, motion, inclination angle and angular velocity of the wheelchair are defined, the current motion state of the electric wheelchair is judged by calculating parameters of speed, acceleration, steering angular velocity, euler angle, acceleration increment and wheel speed difference of the electric wheelchair in real time during the traveling of the wheelchair, and the following electric wheelchair motion process is detected and corrected based on the current motion state of the electric wheelchair, so that the traveling attitude of the electric wheelchair is adjusted and controlled;

The specific calculation steps are as follows:

1) Defining movement of wheelchair

The X component represents the back and forth motion, and the y component represents the left and right motion; left-hand wheel = x+p x y, right-hand wheel = x-p x y, p is the scaling factor, default p = 0.5, i.e.: left wheel = x+y/2, right wheel = x-y/2;

2) Definition of the tilt angle

The projection represents the wheelchair elevation angle on the x negative axis and the wheelchair depression angle on the x positive axis; the projection on the negative y axis represents the left tilting of the wheelchair, and the projection on the positive y axis represents the right tilting of the electric wheelchair; the projection on the negative z axis represents the inversion of the electric wheelchair, and the projection on the positive z axis represents the erection of the electric wheelchair; the definition shows that the x-axis direction is a pitch angle and the y-axis direction is a roll angle;

The forward acceleration of the electric wheelchair is not more than 0.2g, and the reverse acceleration is not more than-0.5 g. The acceleration limit value is not set, and only the speed and angular speed limit value is set;

3) Definition of angular velocity

The heading angle is rotated by taking the z axis as the central axis, the roll angle is rotated by taking the x axis as the central axis, and the pitch angle is rotated by taking the y axis as the central axis. The maximum angular velocity of the z-axis is 1 radian/s, which corresponds to one revolution of 6.28 seconds;

4) Calculation basis

(1) Speed of: the pulse interval time of the wheelchair wheel is 100 mu s/3 as a time scale, and is calculated once every PID adjustment, and the PID adjustment interval is 10ms; taking the average value of two wheels as the forward speed, wherein the positive number is forward, and the negative number is backward;

(2) Acceleration: speed difference of two acquisition time intervals;

(3) Steering angular velocity:

a. from the wheel speed difference, 1 calculation at each PID adjustment Wherein r is the wheel track, and the PID adjustment interval is 10ms;

b. reading the steering angular velocity from the MPU 6050;

the difference value of a and b can be used for judging whether the wheel of the wheelchair slips;

(4) When the pitch angle is calculated, an acceleration value is read from the MPU6050, and atan2 (accX, accZ) is taken; accX the increment of acceleration introduced by the steering angular velocity needs to be subtracted;

When the roll angle is calculated, an acceleration value is read from the MPU6050, and atan2 (accY, accZ) is taken; accY subtracting an acceleration increment introduced by the steering angular velocity, wherein when the electric wheelchair is acted by a vertical force, the angle cannot be calculated from the acceleration value;

(5) And (3) calculating acceleration increment:

(6) Wheel speed difference calculation:

the electric wheelchair is used as an origin for recording, the wheelbase is b, the forward motion dy and the right deviation dx are as follows:

Hypotenuse is d=sqrt (dy 2+dx 2);

deflection angle θ=atan2 (dx, dy);

turning center radius r=d/(2 sin θ);

center arc length l=2θ (2×pi×r)/(2×pi) = 2*R ×θ=d×θ/sin θ;

the left wheel advances: l1=2 (r+b/2) ×θ= (2 r+b) ×θ;

The right wheel advances: l2=2+ (R-b/2) θ= (2R-b) θ;

Based on the left wheel, the right wheel should be multiplied by a coefficient P, p=l1/l2= (2 r+b)/(2R-b) = (d+ bsin θ)/(d-bsin θ).