CN111949929B - Design method of multi-sensor fusion quadruped robot motion odometer - Google Patents

Design method of multi-sensor fusion quadruped robot motion odometer Download PDF

Info

Publication number
CN111949929B
CN111949929B CN202010805263.5A CN202010805263A CN111949929B CN 111949929 B CN111949929 B CN 111949929B CN 202010805263 A CN202010805263 A CN 202010805263A CN 111949929 B CN111949929 B CN 111949929B
Authority
CN
China
Prior art keywords
odometer
leg
robot
coordinate system
motion
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
CN202010805263.5A
Other languages
Chinese (zh)
Other versions
CN111949929A (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.)
Intelligent Mobile Robot Zhongshan Research Institute
Original Assignee
Intelligent Mobile Robot Zhongshan Research Institute
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 Intelligent Mobile Robot Zhongshan Research Institute filed Critical Intelligent Mobile Robot Zhongshan Research Institute
Priority to CN202010805263.5A priority Critical patent/CN111949929B/en
Publication of CN111949929A publication Critical patent/CN111949929A/en
Application granted granted Critical
Publication of CN111949929B publication Critical patent/CN111949929B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • General Physics & Mathematics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Computational Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Operations Research (AREA)
  • Computing Systems (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)
  • Manipulator (AREA)
  • Navigation (AREA)

Abstract

The invention discloses a design method of a multi-sensor fused quadruped robot motion odometer, which is applied to the field of robot motion control. The four-foot robot motion odometer is a key technology for realizing self accurate position calculation by means of a robot body sensor, the odometer calculation result provides global positioning information for the robot foot setting and also provides feedback data for the position control of the robot, the four-foot robot body is provided with a high-precision IMU, the high-precision IMU can measure the body posture and acceleration data of the robot, and each key angle sensor can measure the joint angle of the robot and calculate the kinematic parameter of each leg, namely the foot end position. According to the method, the estimated disturbance caused by course fluctuation is decoupled by constructing the odometer estimated coordinate system under the body coordinate system, and meanwhile, the Kalman filter is constructed, the terminal speed differential is used for estimating the mileage data, and meanwhile, the support phase position change and the body IMU acceleration measurement result are introduced, so that the accuracy of the odometer is improved, and meanwhile, the real-time performance of the final odometer data is ensured.

Description

Design method of multi-sensor fusion quadruped robot motion odometer
Technical Field
The invention relates to the field of robot motion control, in particular to a state estimation method for a quadruped robot.
Background
The four-foot robot motion odometer is a key technology for realizing self accurate position calculation by means of a robot body sensor, the odometer calculation result provides global positioning information for the falling feet of the robot and also provides feedback data for the position control of the robot, the four-foot robot body is loaded with a high-precision IMU which can measure the body posture and acceleration data of the robot, each key angle sensor can measure the joint angle of the robot and calculate the kinematic parameter of each leg, namely the foot end position, the traditional odometer estimation method only obtains the speed by differentiating the tail end position and obtains the body odometer estimation result by reversely integrating the motion speed of the supporting leg, but the differential data has the noise, the error is further amplified after twice integration, and the finally generated odometer data lags the real robot state due to sampling and differential processing, it is therefore desirable to propose a new odometer estimation method.
Disclosure of Invention
The invention discloses a design method of a multi-sensor fused quadruped robot motion odometer, and aims to solve the problem that odometer data generated by a quadruped robot and a real robot state lag.
The technical scheme of the invention is as follows:
a design method of a multi-sensor fused quadruped robot motion odometer is characterized by comprising the following steps:
step 1: establishing a coordinate system { O } of the terrain odometer along the direction of the robot head;
step 2: initializing odometer XY position data, initializing system state X ═ p of odometer Kalman filterxpy vx vy]And covariance matrix P ═ I4×4；
And step 3: initializing the sensor corresponding to the filter to measure noise parameters, acceleration measurement noise anFoot end velocity measurement noise vnPosition mileage measurement noise pnAnd obtaining a corresponding system noise matrix and a corresponding measurement noise matrix:
Figure BDA0002628886000000021
Figure BDA0002628886000000022
and 4, step 4: constructing a state equation of a uniform accelerated motion system, and obtaining a quaternion [ q ] by adopting attitude solution1 q2 q3 q4]Measuring the acceleration value obtained by the machine body
Figure BDA0002628886000000027
Conversion to { O } system:
Figure BDA0002628886000000023
Figure BDA0002628886000000024
and 5: predicting the state of the odometer at the next moment based on the uniform acceleration model:
Figure BDA0002628886000000025
and 6: the state matrix of the system is obtained in the last step
Figure BDA0002628886000000026
The a priori covariance matrix can be further calculated:
Pk＝APk-1AT+Q
and 7: calculating the position measurement data of the odometer generated based on the accumulation of the position of the foot end, and initializing the origin X of the local coordinate system of the odometerodom_stWith the own motion coordinate system origin X of each legleg_st(i)
And 8: calculating the position deviation of each supporting leg relative to the origin of each sub-motion coordinate system:
dX(i)＝Xleg(i)-Xleg_st(i)
and step 9: calculating the average value of the displacement of the support leg:
Figure BDA0002628886000000031
step 10: obtaining a position measurement value based on the average support translation amount and the current odometer motion coordinate system origin of the robot:
Figure BDA0002628886000000032
step 11: judging the swing switching of the single leg, and adopting the trigger signal to reset an odometer motion coordinate system and a single leg motion coordinate system:
Figure BDA0002628886000000033
Xleg_st(i)＝Xleg(i)
step 12: calculating the reverse movement speed of the tail end speed, and obtaining the speed corresponding to each supporting leg through forward differentiation as follows:
Vleg(i)＝Xleg(i,k)-Xleg(i,k-1)
step 13: calculating the average value of the support leg reverse speed:
Figure BDA0002628886000000034
step 14: taking the reverse velocity as a velocity measurement value and constructing a measurement vector with the position measurement value:
Figure BDA0002628886000000035
step 15: constructing an observation matrix of the system:
Figure BDA0002628886000000041
step 16: computing kalman gain
K＝PHT(HPHT+R)-1
And step 17: odometer estimation result compensation before observation vector correction
Figure BDA0002628886000000042
Step 18: updating a posteriori covariance matrix
P＝(I-KH)P。
The invention has the beneficial effects that: the estimated disturbance caused by course fluctuation is decoupled by constructing the odometer estimated coordinate system under the body coordinate system, and meanwhile, the position change of the support phase and the acceleration measurement result of the body IMU are introduced while the terminal speed differential is used for estimating the odometer data by the Kalman filter, so that the accuracy of the odometer is improved, and the real-time performance of the final odometer data is guaranteed.
Drawings
Fig. 1 is a fluctuation diagram of the pitch angle and roll angle of the robot improved by the method along with the time.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
A design method of a multi-sensor fused quadruped robot motion odometer comprises the following steps:
step 1: establishing a coordinate system { O } of the terrain odometer along the direction of the robot head;
and 2, step: initializing odometer XY position data, initializing system state X ═ p of odometer Kalman filterxpy vx vy]And covariance matrix P ═ I4×4；
And step 3: initializing the sensor corresponding to the filter to measure noise parameters, acceleration measurement noise anFoot end velocity measurement noise vnPosition mileage measurement noise pnAnd obtaining a corresponding system noise matrix and a corresponding measurement noise matrix:
Figure BDA0002628886000000051
Figure BDA0002628886000000052
and 4, step 4: constructing a state equation of a uniform accelerated motion system, and obtaining a quaternion [ q ] by adopting attitude solution1 q2 q3 q4]Measuring the acceleration value obtained by the machine body
Figure BDA0002628886000000056
Conversion to { O } system:
Figure BDA0002628886000000053
Figure BDA0002628886000000054
and 5: predicting the state of the odometer at the next moment based on the uniform acceleration model:
Figure BDA0002628886000000055
step 6: the state matrix of the system is obtained in the last step
Figure BDA0002628886000000061
Then the a priori covariance matrix can be further computed:
Pk＝APk-1AT+Q
and 7: calculating the position measurement data of the odometer generated based on the accumulation of the position of the foot end, and initializing the origin X of the local coordinate system of the odometerodom_stWith the own motion coordinate system origin X of each legleg_st(i)；
And 8: calculating the position deviation of each supporting leg relative to the origin of each sub-motion coordinate system:
dX(i)＝Xleg(i)-Xleg_st(i)
and step 9: calculating the average value of the displacement of the support leg:
Figure BDA0002628886000000062
step 10: obtaining a position measurement value based on the average support translation amount and the current odometer motion coordinate system origin of the robot:
Figure BDA0002628886000000063
step 11: judging the switching of the swing of the single leg, and resetting a odometer motion coordinate system and a single-leg motion coordinate system by adopting the trigger signal:
Figure BDA0002628886000000064
Xleg_st(i)＝Xleg(i)
step 12: calculating the reverse movement speed of the tail end speed, and obtaining the speed corresponding to each supporting leg through forward differentiation as follows:
Vleg(i)＝Xleg(i,k)-Xleg(i,k-1)
step 13: calculating the average value of the support leg reverse speed:
Figure BDA0002628886000000071
step 14: taking the reverse speed as a speed measurement value and constructing a measurement vector with the position measurement value:
Figure BDA0002628886000000072
step 15: constructing an observation matrix of the system:
Figure BDA0002628886000000073
step 16: computing kalman gain
K＝PHT(HPHT+R)-1
And step 17: odometer estimation result compensation before observation vector correction
Figure BDA0002628886000000074
Step 18: updating a posteriori covariance matrix
P＝(I-KH)P。
The invention discloses a design method of a quadruped robot motion odometer with multi-sensor fusion, the quadruped robot motion odometer is a key technology for realizing self accurate position calculation by means of a robot body sensor, the odometer calculation result provides global positioning information for the robot foot falling, and also provides feedback data for the position control of the robot, the quadruped robot body is provided with a high-accuracy IMU, the high-accuracy IMU can measure the body posture and acceleration data of the robot, and each key angle sensor can measure the joint angle of the robot and calculate the kinematic parameter of each leg, namely the foot end position. The invention provides a design method of a moving odometer fusing multi-sensor data, which is characterized in that estimation disturbance caused by decoupling course fluctuation of an odometer estimation coordinate system under a body coordinate system is constructed, and simultaneously, a Kalman filter is constructed, terminal speed differential is used for estimating mileage data, and simultaneously, support phase position change and a body IMU acceleration measurement result are introduced, so that the accuracy of the odometer is improved, and the real-time performance of the final odometer data is ensured, and the specific condition is shown in figure 1.
Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that changes, modifications, substitutions and alterations can be made in these embodiments without departing from the principles and spirit of the invention, the scope of which is defined in the appended claims and their equivalents.

Claims (1)

1. A design method of a multi-sensor fused quadruped robot motion odometer is characterized by comprising the following steps:
step 1: establishing a coordinate system { O } of the terrain odometer along the direction of the robot head;
step 2: initializing odometer XY position data, initializing system state X ═ p of odometer Kalman filterx pyvx vy]And covariance matrix P ═ I4×4；
And 3, step 3: initializing the sensor corresponding to the filter to measure noise parameters, acceleration measurement noise anFoot end velocity measurement noise vnPosition mileage measurement noise pnAnd obtaining a corresponding system noise matrix Q and a corresponding measurement noise matrix R:
Figure FDA0003566942480000011
Figure FDA0003566942480000012
and 4, step 4: constructing a state equation of a uniform accelerated motion system, and obtaining a quaternion [ q ] by adopting attitude solution0 q1 q2 q3]Measuring the acceleration value obtained by the machine body
Figure FDA0003566942480000015
Conversion to { O } system:
Figure FDA0003566942480000013
Figure FDA0003566942480000014
and 5: predicting the state of the odometer at the next moment based on the uniform acceleration model:
Figure FDA0003566942480000021
step 6: obtaining the state matrix of the system from the last step
Figure FDA0003566942480000022
The a priori covariance matrix can be further calculated:
Pk＝APk-1AT+Q；
and 7: calculating the position measurement data of the odometer generated based on the accumulation of the position of the foot end, and initializing the origin X of the local coordinate system of the odometerodom_stWith the own motion coordinate system origin X of each legleg_st(i)；
And 8: calculating the position deviation of each supporting leg relative to the origin of each sub-motion coordinate system:
dX(i)＝Xleg(i)-Xleg_st(i)
and step 9: calculating the average value of the displacement of the support leg:
Figure FDA0003566942480000023
step 10: obtaining a position measurement value based on the average support translation amount and the origin of the current odometer motion coordinate system of the robot:
Figure FDA0003566942480000024
step 11: judging the switching of the swing of the single leg, and resetting a odometer motion coordinate system and a single-leg motion coordinate system by adopting a trigger signal:
Figure FDA0003566942480000025
Xleg_st(i)＝Xleg(i)
step 12: calculating the reverse movement speed of the tail end speed, and obtaining the speed corresponding to each supporting leg through forward differentiation as follows:
Vleg(i)＝Xleg(i,k)-Xleg(i,k-1)
step 13: calculating the average value of the support leg reverse speed:
Figure FDA0003566942480000031
step 14: taking the reverse velocity as a velocity measurement value and constructing a measurement vector with the position measurement value:
Figure FDA0003566942480000032
step 15: constructing an observation matrix of the system:
Figure FDA0003566942480000033
step 16: computing kalman gain
K＝PHT(HPHT+R)-1
And step 17: odometer estimation before correction with observation vectors
Figure FDA0003566942480000034
Step 18: updating a posteriori covariance matrix
P＝(I-KH)P。
CN202010805263.5A 2020-08-12 2020-08-12 Design method of multi-sensor fusion quadruped robot motion odometer Active CN111949929B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010805263.5A CN111949929B (en) 2020-08-12 2020-08-12 Design method of multi-sensor fusion quadruped robot motion odometer

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010805263.5A CN111949929B (en) 2020-08-12 2020-08-12 Design method of multi-sensor fusion quadruped robot motion odometer

Publications (2)

Publication Number Publication Date
CN111949929A CN111949929A (en) 2020-11-17
CN111949929B true CN111949929B (en) 2022-06-21

Family

ID=73332744

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010805263.5A Active CN111949929B (en) 2020-08-12 2020-08-12 Design method of multi-sensor fusion quadruped robot motion odometer

Country Status (1)

Country Link
CN (1) CN111949929B (en)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112506190A (en) * 2020-11-19 2021-03-16 深圳市优必选科技股份有限公司 Robot positioning method, robot positioning device and robot
WO2023082050A1 (en) * 2021-11-09 2023-05-19 浙江大学 High-precision mileage estimation method based on double-layer filter framework
CN114046800B (en) * 2021-11-09 2023-09-29 浙江大学 High-precision mileage estimation method based on double-layer filtering frame
CN114021376B (en) * 2021-11-17 2024-04-09 中国北方车辆研究所 Terrain gradient estimation method for quadruped robot
CN116429094B (en) * 2023-06-15 2023-09-26 小米汽车科技有限公司 Positioning method, positioning device, electronic equipment and storage medium

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3117967A1 (en) * 2015-07-15 2017-01-18 ETH Zurich Transparency control method for robotic devices and a control device therefor
CN107085422A (en) * 2017-01-04 2017-08-22 北京航空航天大学 A kind of tele-control system of the multi-functional Hexapod Robot based on Xtion equipment
CN108621161A (en) * 2018-05-08 2018-10-09 中国人民解放军国防科技大学 Method for estimating body state of foot type robot based on multi-sensor information fusion
CN109341705A (en) * 2018-10-16 2019-02-15 北京工业大学 Intelligent detecting robot simultaneous localization and mapping system
CN110146077A (en) * 2019-06-21 2019-08-20 台州知通科技有限公司 Pose of mobile robot angle calculation method
CN110986939A (en) * 2020-01-02 2020-04-10 东南大学 Visual inertial odometer method based on IMU pre-integration
CN111136660A (en) * 2020-02-19 2020-05-12 清华大学深圳国际研究生院 Robot pose positioning method and system

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR102403504B1 (en) * 2015-11-26 2022-05-31 삼성전자주식회사 Mobile Robot And Method Thereof
CN105666498B (en) * 2016-04-21 2017-07-07 奇弩（北京）科技有限公司 Improve the gait planning method of Hexapod Robot walking stability

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3117967A1 (en) * 2015-07-15 2017-01-18 ETH Zurich Transparency control method for robotic devices and a control device therefor
CN107085422A (en) * 2017-01-04 2017-08-22 北京航空航天大学 A kind of tele-control system of the multi-functional Hexapod Robot based on Xtion equipment
CN108621161A (en) * 2018-05-08 2018-10-09 中国人民解放军国防科技大学 Method for estimating body state of foot type robot based on multi-sensor information fusion
CN109341705A (en) * 2018-10-16 2019-02-15 北京工业大学 Intelligent detecting robot simultaneous localization and mapping system
CN110146077A (en) * 2019-06-21 2019-08-20 台州知通科技有限公司 Pose of mobile robot angle calculation method
CN110986939A (en) * 2020-01-02 2020-04-10 东南大学 Visual inertial odometer method based on IMU pre-integration
CN111136660A (en) * 2020-02-19 2020-05-12 清华大学深圳国际研究生院 Robot pose positioning method and system

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
A federated derivative cubature Kalman filter for IMU-UWB indoor positioning;Chengyang He et.al;《sensors》;20200621;第1-20页 *
ArUco-SLAM：基于ArUco二维码阵列的单目实时建图定位系统;邢伯阳;《北京理工大学学报》;20200430;第427-433页 *

Also Published As

Publication number Publication date
CN111949929A (en) 2020-11-17

Similar Documents

Publication Publication Date Title
CN111949929B (en) Design method of multi-sensor fusion quadruped robot motion odometer
CN111207774B (en) Method and system for laser-IMU external reference calibration
CN110926460B (en) Uwb positioning abnormal value processing method based on IMU
CN113311411B (en) Laser radar point cloud motion distortion correction method for mobile robot
CN110554396A (en) laser radar mapping method, device, equipment and medium in indoor scene
CN111721288B (en) Zero offset correction method and device for MEMS device and storage medium
CN111896007B (en) Attitude calculation method for quadruped robot for compensating foot-ground impact
JP6083279B2 (en) Movement status information calculation method and movement status information calculation device
CN110561424A (en) online robot kinematic calibration method based on multi-sensor hybrid filter
JP2012173190A (en) Positioning system and positioning method
CN110702091A (en) High-precision positioning method for moving robot along subway rail
CN109959374B (en) Full-time and full-range reverse smooth filtering method for pedestrian inertial navigation
CN108748220B (en) Foot-machine following control method and control system based on inertial measurement unit
CN115143954B (en) Unmanned vehicle navigation method based on multi-source information fusion
CN110887481A (en) Carrier dynamic attitude estimation method based on MEMS inertial sensor
CN113252033A (en) Positioning method, positioning system and robot based on multi-sensor fusion
CN108871323B (en) High-precision navigation method of low-cost inertial sensor in locomotive environment
CN112362057A (en) Inertial pedestrian navigation algorithm based on zero-speed correction and attitude self-observation
CN115388899A (en) Mobile robot vision inertia fusion SLAM method based on variational Bayes
WO2022160811A1 (en) Footed robot motion trajectory tracking method and device, and readable storage medium
CN113503872B (en) Low-speed unmanned aerial vehicle positioning method based on fusion of camera and consumption-level IMU
Bai et al. Graph-optimisation-based self-calibration method for IMU/odometer using preintegration theory
CN114046800B (en) High-precision mileage estimation method based on double-layer filtering frame
CN113701755B (en) Optical remote sensing satellite attitude determination method without high-precision gyroscope
CN113447018B (en) Real-time attitude estimation method of underwater inertial navigation system

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