CN108051001B - A robot movement control method, system and inertial sensing control device - Google Patents

Abstract

The invention relates to a robot movement control method, comprising: acquiring angular velocity data of an inertial sensor, filtering and preprocessing the angular velocity data; establishing a quaternion differential equation according to the angular velocity data, and using the Runge-Kutta method to solve the problem The quaternion differential equation is described, and the attitude matrix including the target attitude angle is obtained; the target attitude angle is converted from the carrier coordinate system to the navigation coordinate system; the target attitude angle in the target attitude angle in the navigation coordinate system is not within the threshold range. Exclude; control the robot action according to the target attitude angle within the threshold range. The invention can control the movement of the robot by using the inertial sensor, has high precision and good online recognition effect, has strong universality, and has good application prospect value. The invention also provides a robot movement control system and an inertial sensing control device.

Description

A robot movement control method, system and inertial sensing control device

technical field

The present invention relates to the technical field of robot control, in particular to a robot movement control method, system and inertial sensing control device.

Background technique

In many extended applications, human-computer interaction plays an important role, and human-robot interaction is an important topic in the field of robotics, especially in the field of life-assist robots. For a long time, people have never stopped to find a more natural and humanized way of human-computer interaction, and using gestures to control robots can replace complicated and cumbersome program operations, simply and conveniently manipulate robots, issue commands to robots, and communicate with robots. Interaction has become a research hotspot.

The essence of gesture recognition is to perceive the user's operation intention according to the user's small gesture operations, which belongs to the category of multi-channel interaction. Its research content involves a series of related disciplines such as pattern recognition, robotics, image processing, and computer vision. The research on gesture recognition can not only promote the development of these disciplines to a certain extent, but also has great practical significance because of some inherent advantages of gesture movements.

At present, there are two main methods of gesture recognition. One is the vision-based gesture recognition technology. This technology has developed relatively early and is relatively mature, but it has strict requirements on equipment and environment, and its use is limited. The other is gesture recognition technology based on inertial sensors. This technology is not affected by the environment and light. It mainly measures the changes in acceleration and angular velocity for gesture recognition. However, inertial sensors have drift errors. In gesture recognition, such as judging fingers There is still the problem of low precision and inaccurate judgment in such small movements.

SUMMARY OF THE INVENTION

The technical problem to be solved by the present invention is to provide a robot movement control method, system and inertial sensing control device aiming at the deficiencies of the prior art.

The technical solution of the present invention to solve the above-mentioned technical problems is as follows: a robot movement control method, comprising:

S1, obtain the angular velocity data of the inertial sensor;

S2, performing online adaptive filtering preprocessing on the angular velocity data;

S3, establishing a quaternion differential equation according to the angular velocity data preprocessed by the online adaptive filtering, using the Runge-Kutta method to solve the quaternion differential equation, and obtaining an attitude matrix including the target attitude angle;

S4, convert the target attitude angle from the carrier coordinate system to the navigation coordinate system;

S5, exclude target attitude angles that are not within the threshold range from the target attitude angles in the navigation coordinate system;

S6, control the action of the robot according to the target attitude angle within the threshold range.

Another technical solution of the present invention to solve the above-mentioned technical problems is as follows: a robot movement control system, comprising:

an acquisition unit for acquiring the angular velocity data of the inertial sensor;

a preprocessing unit, configured to perform online adaptive filtering preprocessing on the angular velocity data;

a processing unit, configured to establish a quaternion differential equation according to the angular velocity data preprocessed by the online adaptive filtering, use the Runge-Kutta method to solve the quaternion differential equation, and obtain an attitude matrix including the target attitude angle;

The coordinate system conversion unit is used to convert the target attitude angle from the carrier coordinate system to the navigation coordinate system;

a screening unit, configured to exclude target attitude angles that are not within the threshold range among the target attitude angles in the navigation coordinate system;

The control unit is used to control the robot action according to the target attitude angle within the threshold range.

Another technical solution of the present invention to solve the above technical problem is as follows: an inertial sensing control device, comprising the robot movement control system described in the above technical solution, and the inertial sensing control device wirelessly communicates with the robot.

The beneficial effects of the present invention are: aiming at the problem of real-time processing of sensor data, the present invention utilizes the online adaptive filtering method to realize online denoising, reduces the influence of noise on the later updated attitude matrix, and makes the attitude matrix to solve the attitude angle more accurately; The arity method describes the attitude matrix and solves the differential equation, which requires less calculation and has high precision, and can avoid falling into the "singularity"; the threshold is used to exclude the misoperation of fingers, and the recognition of different tiny gestures is completed; The invention can use the inertial sensor to control the movement of the robot, has high precision and good online recognition effect, has strong universality, and has good application prospect value.

Description of drawings

FIG. 1 is a schematic flowchart of a method for controlling movement of a robot according to an embodiment of the present invention;

2 is a flow chart of system signal processing provided by an embodiment of the present invention;

Fig. 3 is the change of pitch angle and heading angle when the finger moves up and down according to an embodiment of the present invention;

Fig. 4 is the change of the pitch angle and the heading angle when the finger moves left and right according to an embodiment of the present invention;

FIG. 5 is a schematic structural block diagram of a robot movement control system provided by an embodiment of the present invention.

Detailed ways

The principles and features of the present invention will be described below with reference to the accompanying drawings. The examples are only used to explain the present invention, but not to limit the scope of the present invention.

FIG. 1 is a schematic flowchart of a method for controlling movement of a robot provided by an embodiment of the present invention. As shown in Figure 1, the method includes:

S1, acquiring the angular velocity data of the inertial sensor, wherein the inertial sensor can be worn on the user's finger;

S2, performing online adaptive filtering preprocessing on the angular velocity data;

S3, establishing a quaternion differential equation according to the angular velocity data preprocessed by the online adaptive filtering, using the Runge-Kutta method to solve the quaternion differential equation, and obtaining an attitude matrix including the target attitude angle;

S4, convert the target attitude angle from the carrier coordinate system to the navigation coordinate system;

S5, exclude target attitude angles that are not within the threshold range from the target attitude angles in the navigation coordinate system;

S6, control the action of the robot according to the target attitude angle within the threshold range.

In this embodiment, for the problem of real-time processing of sensor data, the online adaptive filtering method is used to realize online denoising; the quaternion method is used to describe the attitude matrix, and the differential equation is solved, which requires less calculation, has high precision, and can Avoid falling into the "singularity"; use the threshold to exclude the misoperation of fingers, and complete the recognition of different tiny gestures; the invention can use the inertial sensor to control the movement of the robot, has high precision and good online recognition effect, and has strong universality. The application prospect value is good.

Optionally, as an embodiment of the present invention, the preprocessing of performing online adaptive filtering on the angular velocity data includes:

S2.1, initialize state quantities and system adaptive parameters. specifically,

一般将其设为3维全0列向量，维数为状态过程模型状态向量的维数，角速度、角速度一阶导数和角速度二阶导数为状态向量；2.1.1, set the initial value of the state

Generally, it is set as a 3-dimensional all-zero column vector, the dimension is the dimension of the state vector of the state process model, and the angular velocity, the first derivative of the angular velocity and the second derivative of the angular velocity are the state vector;

取任意正数，例如α₀取值

取值3，2.1.2, the initial value of the system adaptive parameter α=α ₀ and

Take any positive number, such as α ₀ value

take value 3,

2.1.3, the initial values of autocorrelation function r ₀ (0) and r ₀ (1) are taken as

一般

取0。2.1.4, the initial value of the second derivative of the angular velocity

generally

Take 0.

S2.2, establish a state process model with system adaptive parameters. specifically,

2.2.1 Use the following formula to describe the movement characteristics of the target;

其中

为角速度二阶导数均值，a(t)为零均值指数相关有色噪声模型，其相关函数为：Similar to the Singer model, the second derivative of the angular velocity is a time-dependent stochastic process with a non-zero mean

in

is the mean value of the second derivative of the angular velocity, a(t) is a zero-mean exponentially correlated colored noise model, and its correlation function is:

表示加速度方差；α为机动频率，反应了状态的变化随机特性；Among them, t is any sampling time, τ is the correlation measurement parameter, and R _a (τ) represents the correlation function;

represents the acceleration variance; α is the maneuvering frequency, which reflects the random characteristics of the state change;

Whitening the colored noise a(t), we get

where w(t) is zero mean white noise and the variance is

和

得到状态变化的连续状态方程如下：Depend on

and

The continuous state equation for the state change is obtained as follows:

Sampling with period T, the state change of the system after discretization satisfies the following equation:

为3维状态列向量，

分别是角速度、角速度一阶导数和角速度二阶导数，x(k+1)为k+1时刻状态向量，k为采样时刻；A(k+1，k)为状态转移矩阵；x(k)为k时刻目标的状态向量；U(k)为控制矩阵；

为0时刻开始至k时刻目标的角速度二阶导数均值；w(k)为过程噪声，其均值为0,方差为Q(k)；所述A(k+1，k)、U(k)及Q(k)中含有机动频率α和角速度二阶导数方差

随着系统自适应参数的变化而变化；状态转移矩阵A(k+1，k)的表达式如下式in

is a 3-dimensional state column vector,

are the angular velocity, the first derivative of the angular velocity and the second derivative of the angular velocity respectively, x(k+1) is the state vector at time k+1, k is the sampling time; A(k+1, k) is the state transition matrix; x(k) is the state vector of the target at time k; U(k) is the control matrix;

is the mean value of the second derivative of the angular velocity of the target from time 0 to time k; w(k) is the process noise, its mean is 0, and the variance is Q(k); the A(k+1, k), U(k) and Q(k) contains the maneuvering frequency α and the variance of the second derivative of the angular velocity

It changes with the change of the system adaptive parameters; the expression of the state transition matrix A(k+1,k) is as follows

The expression of the control matrix U(k) is as follows

The expression of the variance Q(k) of the process noise w(k) is as follows

in,

2.2.2, the measurement equation is as follows

y(k)=H(k)x(k)+v(k) (8)

Where k is the sampling time, y(k) is the measurement value at y(k) time, H(k) is the measurement matrix, x(k) is the state vector of the target at time k; v(k) is the Gaussian measurement white noise, Its variance is R and is independent of the process noise w(k).

S2.3: Predict the target moving state according to the established state process model with system adaptive parameters, and obtain a state prediction value and a state covariance prediction value.

2.3.1 According to the established state process model with system adaptive parameters and the initial value to complete the one-step prediction of the state, the prediction equation is as follows:

表示k-1时刻预测目标在k时刻的状态，k为采样时刻，A(k，k-1)为状态转移矩阵；

表示目标k-1时刻状态估计值；U(k-1)为控制矩阵；

为从0时刻开始至k-1的角速度二阶导数均值；in

Indicates the state of the prediction target at time k at time k-1, k is the sampling time, and A(k, k-1) is the state transition matrix;

Represents the estimated value of the state at the time of target k-1; U(k-1) is the control matrix;

is the mean value of the second derivative of the angular velocity from time 0 to k-1;

2.3.2 Complete the one-step prediction of the state covariance according to the following formula:

P(k|k-1)=A(k,k-1)P(k-1|k-1)A ^T (k,k-1)+Q(k-1) (10)

where P(k|k-1) represents the state covariance of the prediction target at time k-1, k is the sampling time, | represents the conditional operator; P(k-1|k-1) represents time k-1 The estimated value of the state covariance of the target; A(k, k-1) is the state transition matrix; Q(k-1) is the process noise covariance.

S2.4: Update the target moving state according to the state predicted value, the measured data value, and the state covariance predicted value to obtain a state estimated value.

2.4.1 Calculate the filter gain according to the following formula according to the predicted value of state covariance, measurement matrix and measurement noise variance;

K(k)=P(k|k-1)H ^T (k)[H(k)P(k|k-1)H ^T (k)+R] ^T (11)

Among them, K(k) is the filter gain, k is the sampling time, P(k|k-1) is the state covariance of the prediction target at time k-1 at time k, H(k) is the measurement matrix at time k, R is the variance of Gaussian measurement white noise, and H ^T (k) is the transpose of the measurement matrix at time k;

2.4.2 Use the state predicted value and the observed data value to calculate the current state estimate value of the target, as follows

表示k时刻状态估计值，

表示k-1时刻时预测在k时刻的状态，k为采样时刻，K(k)为k时刻滤波器增益，y(k)为在k时刻的观测值，H(k)为k时刻的测量矩阵；in,

represents the estimated state value at time k,

Represents the state predicted at time k at time k-1, k is the sampling time, K(k) is the filter gain at time k, y(k) is the observed value at time k, and H(k) is the measurement at time k matrix;

2.4.3 Calculate the estimated value of the state covariance according to the following formula;

P(k|k)=(I-K(k)H(k))P(k|k-1) (13)

where I is a 3-dimensional identity matrix, P(k|k) is the estimated value of the state covariance at time k, k is the sampling time, K(k) is the filter gain at time k, and H(k) is the measurement at time k Matrix, P(k|k-1) represents the state covariance predicted at time k at time k-1.

S2.5: Calculate the mean value of the second-order derivative of the angular velocity and the estimated value of the second-order derivative of the angular velocity according to the state estimated value.

Use the following formula to calculate the mean value of the second derivative of the angular velocity;

为0至k时刻角速度二阶导数均值，

为k时刻的状态估计值

的第三行值，k为采样时刻；并按照下式获取系统k-1时刻和k时刻的角速度二阶导数估计值

in

is the mean value of the second derivative of the angular velocity from time 0 to k,

is the estimated value of the state at time k

The third row value of , k is the sampling time; and obtain the estimated value of the second derivative of the angular velocity at time k-1 and time k of the system according to the following formula

为k-1时刻状态估计

的第三行值，

为k时刻状态估计

的第三行值。in

Estimate the state at time k-1

The third row value of ,

Estimate the state at time k

the third row of values.

S2.6, revise the system adaptive parameter according to the estimated value of the second derivative of the angular velocity.

的方法，若k小于等于4进入步骤2.6.1，若k大于4进入步骤2.6.2，According to the value of k at the sampling time, the adaptive parameters α and

method, if k is less than or equal to 4, go to step 2.6.1, if k is greater than 4, go to step 2.6.2,

2.6.1 When the sampling time k is less than or equal to 4, because the sampling data is less, the parameter value method of the current statistical model is used to calculate the system adaptive parameters α and

α=α ₀ where α ₀ is the initial value of the system adaptive parameter α,

则取

if

then take

则取

if

then take

则

取(0,10]之间的任意数，if

but

Take any number between (0,10],

为k时刻角速度二阶导数估计值，π为圆周率，取为3.14，a_M为正的常数，取为3，a_-M为与a_M绝对值相等的负常数，取为-3；in,

is the estimated value of the second derivative of the angular velocity at time k, π is the pi, which is taken as 3.14, a _M is a positive constant, which is taken as 3, and a _-M is a negative constant equal to the absolute value of a _M , which is taken as -3;

2.6.2 When the sampling time k is greater than 4, the system adaptive parameters α and

和

分别为k-1时刻和k时刻角速度二阶导数估计值；r_k(0)为k-1时刻角速度二阶导数自相关函数，r_k-1(0)为k-1时刻角速度二阶导数自相关函数；where b is a constant greater than 1, r _k (1) is the one step forward correlation function of the second derivative of the angular velocity at time k, and r _k-1 (1) is the one step forward correlation function of the second derivative of the angular velocity at time k-1,

and

are the estimated values of the second derivative of the angular velocity at time k-1 and time k, respectively; r _k (0) is the autocorrelation function of the second derivative of the angular velocity at time k-1, and r _k-1 (0) is the second derivative of the angular velocity at time k-1 autocorrelation function;

For example, taking b in equations (17) and (18) as 10, it is as follows:

得到角速度二阶导数满足如下一阶马尔科夫随机序列：According to the system equation

The second-order derivative of the angular velocity is obtained to satisfy the following first-order Markov random sequence:

为k+1时刻的角速度二阶导数，

为k时刻的加速度，β为离散后加速度随机序列的机动频率，w^a(k)为零均值白噪声离散序列，方差为

其中

为零均值白噪声w(t)的方差，β与α的关系为β＝e^-αT；in

is the second derivative of the angular velocity at time k+1,

is the acceleration at time k, β is the maneuver frequency of the random sequence of acceleration after discrete, w ^a (k) is a discrete sequence of zero mean white noise, and the variance is

in

The variance of zero mean white noise w(t), the relationship between β and α is β=e ^−αT ;

The first-order Markov time acceleration sequence satisfies the following parameter relationships:

可按照下式计算得到：Among them, r _k (1) is the acceleration one step forward autocorrelation function at time k, r _k (0) is the autocorrelation function of the acceleration at time k, α and β are the maneuvering frequency of acceleration and its discretized acceleration sequence, respectively. maneuvering frequency, adaptive parameter α and

It can be calculated according to the following formula:

为系统自适应参数，T为采样间隔。Among them, r _k (1) is the acceleration one step forward correlation function at time _k , rk (0) is the acceleration autocorrelation function at time k, ln is the logarithm calculation with the base e; α and

is the system adaptive parameter, and T is the sampling interval.

S2.7, update the state process model according to the mean value of the second derivative of the angular velocity and the modified system adaptive parameter, and obtain the angular velocity data after online adaptive filtering.

Optionally, as an embodiment of the present invention, a quaternion differential equation is established according to the angular velocity data, the Runge-Kutta method is used to solve the quaternion differential equation, and obtaining an attitude matrix including the target attitude angle includes:

S3.1, use the filtered angular velocity data and quaternion to establish a quaternion differential equation;

Specifically, using the angular velocity information of the gyroscope in the filtered inertial sensor, the following differential equation is established by using quaternions:

为三轴角速度的矩阵表达式；Among them, the left side of the equation is the quaternion derivation operation, q(t) represents the quaternion, and the symbol ο represents the quaternion multiplication,

is the matrix expression of the triaxial angular velocity;

S3.2, use the fourth-order Runge-Kutta method to solve the quaternion differential equation, obtain the attitude matrix described by the quaternion, update the attitude matrix by updating the element value of the quaternion, and update the target attitude angle;

Wherein, the initial value of the slope required to solve the quaternion differential equation using the fourth-order Runge-Kutta method is determined by the initial value of the quaternion, and the initial value of the quaternion is determined by the initial value of the target attitude angle.

进而即可求解当前时刻的姿态角信息。Using the fourth-order Runge-Kutta method to solve the above differential equation, the initial value of the slope required for the first step of the iterative update algorithm can be determined by the initial value of the quaternion, and the initial value of the quaternion is determined by the initial value of the attitude angle. After each element of the quaternion in the next step is obtained, it can be substituted into the attitude matrix described by the quaternion

Then, the attitude angle information at the current moment can be obtained.

其中

为三轴角速度的矩阵表达式。where K is the slope, t is the current moment, h is the update step size and it is usually the same as the sensor sampling period, where

in

is the matrix expression of the triaxial angular velocity.

Optionally, as an embodiment of the present invention, converting the target attitude angle from the carrier coordinate system to the navigation coordinate system includes: using the quaternion method to solve the attitude matrix

The attitude angle is solved according to the attitude matrix as follows

θ _main = arcsin C ₃₂

θ、γ的真值，下面对姿态角的定义域做了定义，其中俯仰角的定义域为(-90°,90°)，橫滚角的定义域为(-180°,180°)，航向角的定义域为(0°,360°)。公式中的θ_主、γ_主和

分别是由姿态矩阵计算得到的俯仰角、横滚角和航向角，θ、γ和

表示转换到定义域内的角度值。In order to accurately determine the attitude angle

The true values of θ and γ are defined below. The definition domain of the attitude angle is defined below. The definition domain of the pitch angle is (-90°, 90°), and the definition domain of the roll angle is (-180°, 180°) , the domain of the heading angle is (0°, 360°). Theta _main , gamma _{main sum} in the formula

are the pitch angle, roll angle and heading angle calculated from the attitude matrix, respectively, θ, γ and

Represents an angle value converted into the domain.

theta = theta _main

Since the inertial components of the strapdown inertial navigation system are fixed on the carrier, the output values of the sensors are all output values in the carrier coordinate system, so these measured values must be converted to a coordinate that is convenient for calculating the required navigation parameters Under the system, that is, the navigation coordinate system, and the attitude matrix is the conversion relationship between the various data measured on the carrier coordinate system from the carrier coordinate system to the navigation coordinate system. When the attitude matrix of the carrier is determined, the attitude of the carrier can be expressed. horn.

Figure 2 shows the signal processing flow of the system. It can be seen from the attached figure that the data collected from the inertial sensing control device is filtered, the quaternion is solved to obtain the attitude angle, and finally the control command of the robot is obtained.

Figure 3 shows the changes of the pitch angle and the heading angle when the finger moves up and down. In the figure, Pitch represents the pitch angle, and Yaw represents the heading angle. It can be seen from the attached figure that the pitch angle presents a cycle with the up and down movement of the finger. Sex changes, but the heading angle does not change much.

Figure 4 shows the changes of the pitch angle and the heading angle when the finger moves left and right. In the figure, Pitch represents the pitch angle, and Yaw represents the heading angle. It can be seen from the attached figure that the heading angle presents a cycle with the left and right movement of the finger. Sex changes, but the pitch angle does not change much.

Optionally, as an embodiment of the present invention, excluding target attitude angles that are not within the threshold range among the target attitude angles in the navigation coordinate system includes:

Gestures are defined as four actions: finger up, down, right and left. Through experiments, we found that when the finger is raised, the pitch angle is positive, which defines the gesture to control the robot to move forward; when the finger is down, the pitch angle is positive. When the angle is negative, the gesture controls the robot to stop; when the finger is to the right, the heading angle is positive, and the gesture controls the robot to turn to the right; when the finger is left, the heading angle is negative and the robot is controlled to the left.

During this process, when the finger wears the inertial sensor, the inaccuracy of finger recognition will be affected by the slight or large movement of the finger. After experimental demonstration, when the finger is raised (Finger_Up), the pitch angle is 20 degrees (threshold 1, we Indicated by T ₁ ) to 50 degrees (threshold 2, we denote by T ₂ ), it is the normal range of finger movement, as the correct gesture to control the robot forward; when the finger is down (Finger_Down), the pitch angle is between -20 degrees to When -50 degrees, it is the normal range of finger movement, which is the correct gesture to control the robot to stop. When the finger turns to the right (Finger_Right), the heading angle is between 20 degrees (threshold 3, we use T ₃ ) to 40 degrees (threshold 4, we use T ₄ ), it is the normal range of finger turning, as the control robot right The correct gesture of turning; when the finger turns to the left (Finger_Left), when the heading angle is -20 degrees to -40 degrees, it is the normal range of finger turning, as the correct gesture to control the robot to turn left.

Using the above thresholds, the following control gesture instructions can be obtained:

Among them, Pitch represents the pitch angle, and Yaw represents the heading angle.

Optionally, as an embodiment of the present invention, controlling the robot action according to the target attitude angle within the threshold range includes:

When the finger wears the sensor, it communicates with the robot through WIFI connection, and uses the inertial sensor to collect the movement signal of the finger. The computer obtains the movement signal, judges the posture of the finger, and then transmits the movement command to the robot through the wireless module, as listed in Table 1. 4 commands described by "G", "S", "R" and "L". When the gesture is raised, the sensor captures the movement of the finger and transmits it to the computer for posture judgment. When the set threshold range is met, the computer sends the "G" command to the robot, and the robot moves forward; when the gesture is downward, the The sensor captures the movement of the finger and transmits it to the computer for posture judgment. When the set threshold range is met, the computer sends the "S" command to the robot, and the robot stops; when the gesture is to the right, the sensor captures the movement of the finger, Send it to the computer for posture judgment. When the set threshold range is met, the computer sends the "R" command to the robot, and the robot makes a right turn; when the gesture is to the left, the sensor captures the movement of the finger and transmits it to the computer for posture When the set threshold range is met, the computer sends the "L" command to the robot, and the robot makes a left turn.

The present invention aims at accurately recognizing finger movements by inertial sensors, and takes the finger-controlled robot as an example to carry out an experiment flow chart as shown in FIG. 1 . We choose a typical robot platform, the NAO robot. The robot system has a complete self-balancing module. When there is a command input, NAO can walk smoothly by itself. Therefore, we only consider whether NAO can obtain accurate gesture-based commands.

The gesture recognition method provided by the present invention mainly relies on inertial sensors to complete data collection, and completes the system realization by controlling the movement of the robot. First, the gyroscope and accelerometer are used to measure the angular movement and line movement information of the carrier respectively, and online adaptive filtering is performed to remove noise; then, the fourth-order Runge-Kutta method is used to solve the differential equation described by the quaternion method , the pitch and heading angles are calculated by inverting the trigonometric functions through the elements of the quaternion; finally, the recognition thresholds of the pitch and heading angles are adaptively determined according to the movement range of the finger, so as to exclude the wrong operation of the finger.

The invention provides a method of collecting small-range motion data of a finger through an inertial sensor, performing online adaptive filtering and denoising processing on the data, constructing a quaternion attitude matrix, using the Runge-Kutta method to solve the quaternion differential equation, and updating the quaternion In order to update the posture matrix to obtain the real-time posture angle, and because of the recognition of small-scale movements of the finger, the threshold is set to eliminate the misoperation of the finger.

The above describes in detail a method for providing a robot movement control method according to an embodiment of the present invention with reference to FIGS. 1 to 4 . The following describes in detail the robot movement control system provided by the embodiment of the present invention with reference to FIG. 5 .

FIG. 5 is a schematic structural block diagram of a robot movement control system provided by an embodiment of the present invention. As shown in FIG. 5 , the system includes: a collection unit 510 , a preprocessing unit 520 , a processing unit 530 , a coordinate system conversion unit 540 , and a screening unit 550 .

The acquisition unit 510 acquires the angular velocity data of the inertial sensor, wherein the inertial sensor can be worn on the user's finger; the preprocessing unit 520 performs online adaptive filtering on the angular velocity data; The preprocessed angular velocity data establishes a quaternion differential equation, uses the Runge-Kutta method to solve the quaternion differential equation, and obtains an attitude matrix including the target attitude angle; the coordinate system conversion unit 540 converts the target attitude angle from the carrier. The coordinate system is converted into a navigation coordinate system; the screening unit 550 excludes target attitude angles that are not within the threshold range among the target attitude angles in the navigation coordinate system; the control unit 560 controls the robot action according to the target attitude angles within the threshold range.

In this embodiment, for the problem of real-time processing of sensor data, the online adaptive filtering method is used to realize online denoising; the quaternion method is used to describe the attitude matrix, and the differential equation is solved, which requires less calculation, has high precision, and can Avoid falling into the "singularity"; use the threshold to exclude the misoperation of fingers, and complete the recognition of different tiny gestures; the invention can use the inertial sensor to control the movement of the robot, has high precision and good online recognition effect, and has strong universality. The application prospect value is good.

The system is mainly implemented by combining attitude angle calculation and adaptive threshold analysis. First, the measurement data is denoised through adaptive filtering, and then the attitude calculation algorithm of the strapdown inertial navigation system based on quaternion is completed. Attitude matrix, navigation parameter extraction and calculation, initial condition given and initial data calculation, and the heading, attitude, speed and position information are continuously calculated through the update of the attitude matrix; then the threshold analysis method is used to realize noise suppression and misoperation judgment. Real-time detection and recognition of the slight movement of gestures, and then complete the corresponding movement control of the robot.

An embodiment of the present invention further provides an inertial sensing control device, including the robot movement control system described in the above technical solution, and the inertial sensing control device communicates wirelessly with the robot.

Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process of the above-described devices and units may refer to the corresponding processes in the foregoing method embodiments, which will not be repeated here.

In the several embodiments provided in this application, it should be understood that the disclosed apparatus and method may be implemented in other manners. For example, the apparatus embodiments described above are only illustrative. For example, the division of units is only a logical function division. In actual implementation, there may be other division methods, for example, multiple units or components may be combined or integrated. to another system, or some features can be ignored, or not implemented.

Units described as separate components may or may not be physically separated, and components shown as units may or may not be physical units, that is, may be located in one place, or may be distributed to multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solutions in the embodiments of the present invention.

In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically alone, or two or more units may be integrated into one unit. The above-mentioned integrated units may be implemented in the form of hardware, or may be implemented in the form of software functional units.

The integrated unit, if implemented as a software functional unit and sold or used as a stand-alone product, may be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention is essentially or a part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium , including several instructions to cause a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods of the various embodiments of the present invention. The aforementioned storage medium includes: U disk, removable hard disk, Read-Only Memory (ROM, Read-Only Memory), Random Access Memory (RAM, Random Access Memory), magnetic disk or optical disk and other media that can store program codes .

The above are only preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included in the protection of the present invention. within the range.

Claims (8)

1. a robot movement control method, is characterized in that, comprises: S1, obtain the angular velocity data of the inertial sensor; S2, performing online adaptive filtering preprocessing on the angular velocity data; The S2 includes: S2.1, initialize state quantities and system adaptive parameters, the state quantities include angular velocity, first-order derivative of angular velocity and second-order derivative of angular velocity; S2.2, establish a state process model with system adaptive parameters; S2.3, predict the state quantity according to the established state process model with system adaptive parameters, and obtain the state predicted value and the state covariance predicted value; S2.4, update the state quantity according to the state predicted value, the measured data value and the state covariance predicted value, and obtain the state estimated value; S2.5, calculate the mean value of the second-order derivative of the angular velocity and the estimated value of the second-order derivative of the angular velocity according to the estimated state value; Use the following formula to calculate the mean value of the second derivative of the angular velocity;

in

is the mean value of the second derivative of the angular velocity from time 0 to k,

is the estimated value of the state at time k

The third row value of , k is the sampling time; and obtain the estimated value of the second derivative of the angular velocity at time k-1 and time k of the system according to the following formula

in

Estimate the state at time k-1

The third row value of ,

Estimate the state at time k

The third row value of ; S2.6, modifying the system adaptive parameter according to the estimated value of the second-order derivative of the angular velocity; According to the value of k at the sampling time, the adaptive parameters α and

method, if k is less than or equal to 4, go to step 2.6.1, if k is greater than 4, go to step 2.6.2, 2.6.1 When the sampling time k is less than or equal to 4, because the sampling data is less, the parameter value method of the current statistical model is used to calculate the system adaptive parameters α and

α=α ₀ where α ₀ is the initial value of the system adaptive parameter α, if

then take

if

then take

if

but

Take any number between (0,10], in,

is the estimated value of the second derivative of the angular velocity at time k, π is the pi, which is taken as 3.14, a _M is a positive constant, which is taken as 3, and a _-M is a negative constant equal to the absolute value of a _M , which is taken as -3; 2.6.2 When the sampling time k is greater than 4, the system adaptive parameters α and

where b is a constant greater than 1, r _k (1) is the one step forward correlation function of the second derivative of the angular velocity at time k, and r _k-1 (1) is the one step forward correlation function of the second derivative of the angular velocity at time k-1,

and

are the estimated values of the second derivative of the angular velocity at time k-1 and time k, respectively; r _k (0) is the autocorrelation function of the second derivative of the angular velocity at time k-1, and r _k-1 (0) is the second derivative of the angular velocity at time k-1 autocorrelation function; The adaptive parameter α and

It can be calculated according to the following formula:

Among them, r _k (1) is the acceleration one step forward correlation function at time _k , rk (0) is the acceleration autocorrelation function at time k, ln is the logarithm calculation with the base e; α and

is the system adaptive parameter, T is the sampling interval; S2.7, update the state process model according to the mean value of the second derivative of the angular velocity and the modified system adaptive parameter, and obtain the angular velocity data after online adaptive filtering; S3, establishing a quaternion differential equation according to the angular velocity data preprocessed by the online adaptive filtering, using the Runge-Kutta method to solve the quaternion differential equation, and obtaining an attitude matrix including the target attitude angle; S4, convert the target attitude angle from the carrier coordinate system to the navigation coordinate system; S5, exclude target attitude angles that are not within the threshold range from the target attitude angles in the navigation coordinate system; S6, control the action of the robot according to the target attitude angle within the threshold range. 2. The method according to claim 1, wherein the S3 comprises: S3.1, use the filtered angular velocity data and quaternion to establish a quaternion differential equation; S3.2, use the fourth-order Runge-Kutta method to solve the quaternion differential equation, obtain the attitude matrix described by the quaternion, update the attitude matrix by updating the element value of the quaternion, and update the target attitude angle; Wherein, the initial value of the slope required to solve the quaternion differential equation using the fourth-order Runge-Kutta method is determined by the initial value of the quaternion, and the initial value of the quaternion is determined by the initial value of the target attitude angle . 3. method according to claim 2, is characterized in that, described utilizes Runge-Kutta method to solve described quaternion differential equation, obtains the attitude matrix described by quaternion as follows:

Among them, K is the slope, t is the current moment, h is the update step size, where

is the matrix expression of the triaxial angular velocity, and q(t) represents the quaternion. 4. The method according to any one of claims 1 to 3, wherein the target attitude angle comprises a pitch angle and a heading angle; the absolute value of the threshold range of the pitch angle is twenty to fifty degrees, The threshold range of the heading angle is twenty degrees to forty degrees. 5. a robot movement control system, is characterized in that, comprises: an acquisition unit for acquiring angular velocity data of the inertial sensor; a preprocessing unit for performing online adaptive filtering preprocessing on the angular velocity data; The preprocessing unit is specifically used for: Initializing state quantities and system adaptive parameters, the state quantities include angular velocity, first-order derivative of angular velocity and second-order derivative of angular velocity; Build a state process model with system adaptive parameters; Predict the state quantity according to the established state process model with system adaptive parameters, and obtain the state predicted value and the state covariance predicted value; Update the state quantity according to the state predicted value, the measured data value and the state covariance predicted value to obtain the state estimated value; Calculate the mean value of the second-order derivative of the angular velocity and the estimated value of the second-order derivative of the angular velocity according to the state estimated value; Use the following formula to calculate the mean value of the second derivative of the angular velocity;

in

is the mean value of the second derivative of the angular velocity from time 0 to k,

is the estimated value of the state at time k

The third row value of , k is the sampling time; and obtain the estimated value of the second derivative of the angular velocity at time k-1 and time k of the system according to the following formula

in

Estimate the state at time k-1

The third row value of ,

Estimate the state at time k

The third row value of ; modifying the system adaptive parameter according to the estimated value of the second derivative of the angular velocity; According to the value of k at the sampling time, the adaptive parameters α and

method, if k is less than or equal to 4, go to step 2.6.1, if k is greater than 4, go to step 2.6.2, 2.6.1 When the sampling time k is less than or equal to 4, because the sampling data is less, the parameter value method of the current statistical model is used to calculate the system adaptive parameters α and

α=α ₀ where α ₀ is the initial value of the system adaptive parameter α, if

then take

if

then take

if

but

Take any number between (0,10], in,

is the estimated value of the second derivative of the angular velocity at time k, π is the pi, which is taken as 3.14, a _M is a positive constant, which is taken as 3, and a _-M is a negative constant equal to the absolute value of a _M , which is taken as -3; 2.6.2 When the sampling time k is greater than 4, the system adaptive parameters α and

where b is a constant greater than 1, r _k (1) is the one step forward correlation function of the second derivative of the angular velocity at time k, and r _k-1 (1) is the one step forward correlation function of the second derivative of the angular velocity at time k-1,

and

are the estimated values of the second derivative of the angular velocity at time k-1 and time k, respectively; r _k (0) is the autocorrelation function of the second derivative of the angular velocity at time k-1, and r _k-1 (0) is the second derivative of the angular velocity at time k-1 autocorrelation function; The adaptive parameter α and

It can be calculated according to the following formula:

Among them, r _k (1) is the acceleration one step forward correlation function at time _k , rk (0) is the acceleration autocorrelation function at time k, ln is the logarithm calculation with the base e; α and

is the system adaptive parameter, T is the sampling interval; Update the state process model according to the mean value of the second derivative of the angular velocity and the modified system adaptive parameter, and obtain the angular velocity data after online adaptive filtering; a processing unit, configured to establish a quaternion differential equation according to the angular velocity data preprocessed by online adaptive filtering, solve the quaternion differential equation by using the Runge-Kutta method, and obtain an attitude matrix including the target attitude angle; The coordinate system conversion unit is used to convert the target attitude angle from the carrier coordinate system to the navigation coordinate system; a screening unit, configured to exclude target attitude angles that are not within the threshold range among the target attitude angles in the navigation coordinate system; The control unit is used to control the robot action according to the target attitude angle within the threshold range. 6. The system according to claim 5, wherein the processing unit is specifically configured to: Use the filtered angular velocity data and quaternion to establish a quaternion differential equation; Use the fourth-order Runge-Kutta method to solve the quaternion differential equation, obtain the attitude matrix described by the quaternion, update the attitude matrix by updating the element value of the quaternion, and update the target attitude angle; Wherein, the initial value of the slope required to solve the quaternion differential equation using the fourth-order Runge-Kutta method is determined by the initial value of the quaternion, and the initial value of the quaternion is determined by the initial value of the target attitude angle . 7. The system according to any one of claims 5 to 6, wherein the described quaternion differential equation is solved by the Runge-Kutta method, and the attitude matrix described by the quaternion is obtained as follows:

Among them, K is the slope, t is the current moment, h is the update step size, where

is the matrix expression of the triaxial angular velocity, and q(t) represents the quaternion. 8 . An inertial sensing control device, characterized in that it comprises the robot movement control system according to any one of claims 5 to 7 , and the inertial sensing control device communicates wirelessly with the robot. 9 .

Images