Technical background
Anthropomorphic robot is a kind of robot that imitates the skeleton structure, and it and other machines people's maximum difference is the foot sport mode of its apery.Bionical the analysis showed that, anthropomorphic robot have advantages such as adaptive capacity to environment is strong, flexible movements, therefore, have more wide application and development prospect.
Different with wheeled robot and industrial robot, the supporting domain of anthropomorphic robot is littler, easier falling down.Therefore need know the attitude angle of robot, comprise pitching and roll angle, so that detection machine people's state, the prevention robot is fallen down.
A kind of attitude algorithm for estimating relatively more commonly used is the high-frequency signal that earlier integration of angular velocity is obtained angle through a Hi-pass filter, again to the angle that obtains through acceleration calculation through a low-pass filter, obtain the low frequency signal of attitude angle, the stack of these two angle signals can obtain complete attitude angle (referring to " K. Loffler; M.Gienger; F.Pfeiffer:Sensor and Control Design of a Dynamically Stable Biped Robot; International Conference on Robotics and Automation; September; 2003 "), but this method need be set the parameter of suitable low-pass filter and Hi-pass filter, estimate to reach accurately, and for different walking patterns, need to select different filter parameters, so the scope of application of this method is less.
Because it is non-linear that attitude is estimated, another common method is, attitude motion is learned model carry out linearization process at equilibrium point, obtain the linear model of this kinematical equation, utilize the method for kalman filtering to estimate the state of this linear model, thereby obtain the attitude angle (referring to " An Attitude Control System for small unmanned helicopters based on gyrometer and angular rate gyrosc
Henrik Rehbinder and Xiaoming Hu have proposed the another kind of method (referring to " Drift-free attitude estimation for accelerated rigid bodies, Automatica, 2004 ") that attitude is estimated.In the method, utilize the attitude angle of acceleration of gravity correction robot.But this method needs the transverse acceleration of robot less.For the acceleration change in the anthropomorphic robot walking, because the model error that transverse acceleration is introduced can not ignore, so this method and be not suitable for the walking of anthropomorphic robot.
Chinese invention patent application 200810224579.4 has proposed a humanoid robot foot posture real-time detection and method, and this attitude detecting method utilizes the 3-axis acceleration information calculations to obtain static tilt angles information; The angular velocity information of gathering is calculated the changing value of angle; According to described static face obliquity information and described angle changing value, obtain real-time obliquity information.But since in robot motion's process with the collision on ground, the static tilt angles information of utilizing accelerometer to calculate gained is inaccurate, and this scheme also can not overcome the integral error of angular velocity.
Therefore, be necessary to design a kind of can be real-time, fast and can effectively overcome the anthropomorphic robot attitude angle estimation scheme of gyroscope integral error.
Summary of the invention
The objective of the invention is at the deficiencies in the prior art, a kind of anthropomorphic robot body posture angle real-time estimation method is provided.
The objective of the invention is to be achieved through the following technical solutions: a kind of anthropomorphic robot body posture angle real-time estimation method, the anthropomorphic robot posture estimation system comprises three-axis gyroscope, three axis accelerometer, kalman wave filter and two sextuple power/torque sensors, wherein, three-axis gyroscope, three axis accelerometer and two sextuple power/torque sensors link to each other with the kalman wave filter respectively; This method comprises following step:
(1) sextuple power/torque sensor and three-axis gyroscope and three axis accelerometer are installed; Wherein, two sextuple power/torque sensors are installed in respectively on two ankle-joints of robot, and three axis accelerometer and three-axis gyroscope are installed in the hip central point of robot.
(2) set up the kinematics model of robot pose;
(3) according to the kinematics model of robot pose, set up the linear discrete stochastic system model of band noise, utilize the correlation method of Kalman filtering, based on the acceleration information of three axis accelerometer, the attitude state of the angular velocity data of three-axis gyroscope and sextuple power/torque sensor three-dimensional force data estimation robot;
(4), obtain the attitude angle of robot reality according to the robot pose information of estimating;
(5) according to the attitude angle of the robot reality that obtains, correct the current attitude of robot
The invention has the beneficial effects as follows, anthropomorphic robot body posture angle real-time estimation method of the present invention is realized by the technology of soft measurement, kinematical equation according to robot pose, utilize the method for kalman filtering, obtain the optimal estimation of robot pose state, thereby calculate corresponding pitching and roll angle.This method has overcome the drift and the conventional linear method measurement range smaller defect of angular velocity integration, has realized the estimation accurately in real time of anthropomorphic robot attitude.
Embodiment
The principle of Kalman filtering is as follows: utilize given system's output observation sequence
, make state
Optimal estimation
, make k+1 estimation index (both estimation error covariance matrixes) J constantly reach minimum:
As shown in Figure 1, the anthropomorphic robot posture estimation system comprises three-axis gyroscope, three axis accelerometer, kalman wave filter and two sextuple power/torque sensors, wherein, three-axis gyroscope, three axis accelerometer and two sextuple power/torque sensors link to each other with the kalman wave filter respectively.Three axis linear acceleration informations when three axis accelerometer provides the robot motion, three-axis gyroscope provides three angular velocity data, and two sextuple power/torque sensors provide the three-dimensional force data.Set up system state equation based on the kinematics model that robot pose changes, according to the force data that sensor obtains, acceleration information and angular velocity data obtain the optimal estimation of robot pose by the method for kalman filtering.Obtain robot pose according to optimal estimation, calculating can get corresponding attitude angle.
Anthropomorphic robot body posture angle real-time estimation method of the present invention comprises following step: at first, sextuple power/torque sensor and three-axis gyroscope and acceierometer sensor are installed; Secondly, set up the kinematics model of robot pose; Once more, kinematics model according to robot pose, set up the linear discrete stochastic system model of band noise, utilize the correlation method of Kalman filtering, based on the acceleration information of three axis accelerometer, the attitude state of the angular velocity data of three-axis gyroscope and sextuple power/torque sensor three-dimensional force data estimation robot; According to the robot pose information after estimating, obtain the attitude angle of robot reality.At last, according to the robot pose pitching and the roll angle that obtain, correct the current attitude of robot.
It is specific as follows,
The first step is installed sextuple power/torque sensor and three-axis gyroscope and acceierometer sensor
The scheme of installation of sensor is as shown in Figure 2: two sextuple power/torque sensors are installed in respectively on two ankle-joints (connecting portion of sole and shank) of robot, and the x axle positive dirction of sensor local coordinate system is along the robot working direction, y axle positive dirction from right to left, z axle positive dirction is straight up.Three axis accelerometer and three-axis gyroscope are installed in the hip central point of robot, and the coordinate system direction of its local coordinate system direction and sextuple power/torque sensor is consistent.
Second goes on foot, and sets up the kinematics model of robot pose.
Attitude can be described with 3 * 3 rotation matrix, corresponding rotation angle around the x axle be called roll angle (
), corresponding rotation matrix:
The corresponding anglec of rotation around the y axle be called the angle of pitch (
) corresponding rotation matrix is:
The corresponding anglec of rotation around the z axle be called deflection angle (
), corresponding rotation matrix is:
For the attitude of robot, we with Euler angle by deflection (
), pitching (
), roll (
) order describe, then Dui Ying attitude matrix is:
To this rotation matrix differentiate:
Be written as two combinations of matrices,
, wherein, in the time interval
In, rotate to be around the trace of axle K
, then following formula can be rewritten as:
Following formula is carried out the angle differential transform gets,
So
Be installed in the three axis accelerometer and the three-axis gyroscope of robot hip central point, robot measurement hip central point is along the acceleration and the corresponding angular velocity of self coordinate system in real time.When robot was static, the acceleration of accelerometer measures was an acceleration of gravity.In the time of the robot motion, the acceleration that the power that the friction force on ground provides robot to advance, robot are subjected to provides (see figure 2) by bonding force.
The sole of supposing robot contacts fully with ground, and robot is a rigidity.We are the initial point of world coordinate system with ground, set up Descartes's right-handed coordinate system, and the positive dirction of x axle is the working direction of robot, and the square first vertical ground of z axle upwards.And the accelerometer of robot hip is identical with the attitude of world coordinate system with the initial attitude of attitude sensor.The bonding force f that robot foot section is subjected to, the acceleration that causes on the accelerometer of robot hip are a, and this moment, the attitude of robot was R, then:
a?=?R*(f/M)
Consider the state transition equation of attitude R, then the kinematics model of robot pose is:
Wherein, M is the quality of robot,
, be making a concerted effort of measuring of two sextuple power/torque sensor, the bonding force that the expression robot foot section is received is at each component of coordinate system,
Be 3 * 1 vector, the three axis angular rate data that the expression three-axis gyroscope is measured are used respectively
,
,
Expression.R is 3 * 3 matrix, the attitude matrix of expression robot;
, the accekeration that the expression three axis accelerometer is measured.
The 3rd step, according to the kinematics model of robot pose, set up the linear discrete stochastic system model of band noise,
Utilize the correlation method of Kalman filtering again, based on the acceleration information of three axis accelerometer, the attitude state of the angular velocity data of three-axis gyroscope and sextuple power/torque sensor three-dimensional force data estimation robot.
Consider that R is 3 * 3 matrix, the acceleration a of measurement and bonding force f are 3 * 1 vectors, and be stretching by row to attitude matrix, supposes the noise white noise of sensor measurement simultaneously, and then the system model restructural is:
State variable in the formula
,
Be plant noise, variance is
,
Be observation noise, variance is
,
Be 3-axis acceleration,
The Kalman filtering equations of discretize is adopted in the estimation of this model state, then be with Ts discrete time at interval, the discretization equation of this model is:
Be 9 * 1 vector, the plant noise of expression system,
Be 3 * 1 vector, the observation noise of expression system.
Be 9 * 1 vector, the state variable of expression system.
Put forward the back predictand with state
Be the basic variable of recursion, can write out Kalman filtering recursion formula and be:
It is the accekeration of k three axis accelerometer constantly
Kalman forecasts gain matrix
For:
Covariance matrix for prediction error:
Be 9 * 9 matrix, the variance of representation model noise,
Be 3 * 3 matrix, the variance of expression observation noise.
In the 4th step,, obtain the attitude angle of robot reality according to the robot pose information after estimating.
Utilize the kalman filtering algorithm, online estimation
, according to the attitude matrix of estimation, calculate the attitude angle of pitching and rolling:
In the formula
Represent state vector respectively
The element of the 7th, 8,9 row
In the 5th step,, correct the current attitude of robot according to the robot pose pitching and the roll angle that obtain.
In order to keep the stable operation of robot, the rolling of the setting of robot operational process and luffing angle are 0, and the joint angles of pitching and rolling by the control robot coxa joint utilizes PID to adjust mode, realizes the stable control of robot body posture.
Embodiment 1
Fig. 3 and Fig. 4 show on anthropomorphic robot use method of the present invention and existing integration method difference, the walking speed of setting robot is 0.8km/h, as seen from Figure 3, because the deficiency of robot hip rigidity, robot in the process of walking, there is cyclical variation in roll angle, and these data have reflected that the roll angle in robot motion's process changes.Because the initial position that robot is put or the deviation of zero-bit cause roll angle and non-vanishing.From with the contrast of integration method as can be seen, this method has overcome the drifting problem of angular velocity integration effectively.As seen from Figure 4, after the robot motion finished, the data of luffing angle remained stable.In sum, this method can effectively overcome the drifting problem of angular velocity integration, and the variation of pitching and roll attitude angle realizes effective measurement of robot pitching and roll attitude angle in the reflection robot motion process.