CN111949929B - 一种多传感器融合的四足机器人运动里程计设计方法 - Google Patents
一种多传感器融合的四足机器人运动里程计设计方法 Download PDFInfo
- 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
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
本发明公开了一种多传感器融合的四足机器人运动里程计设计方法,应用于机器人运动控制领域。四足机器人运动里程计是借助机器人本体传感器实现对自身精确位置推算的关键技术,里程计推算结果除了为机器人落足提供全局定位信息外,同时也为机器人的位置控制提供反馈数据,四足机器人本体搭载了高精度IMU,其能测量机器人的机体姿态与加速度数据、各关键的角度传感器能测量机器人关节角度并计算出各腿运动学参数即足端位置。本发明通过构建在机体坐标系下的里程计估计坐标系解耦航向波动引起的估计扰动,同时构建了卡尔曼滤波器在使用末端速度微分估计里程数据的同时引入支撑相位置变化与本体IMU加速度测量结果,从而在提高里程计精度的同时保证最终里程计数据的实时性。
Description
技术领域
本发明涉及机器人运动控制领域,具体涉及一种四足机器人状态估计方法。
背景技术
四足机器人运动里程计是借助机器人本体传感器实现对自身精确位置推算的关键技术,里程计推算结果除了为机器人落足提供全局定位信息外,同时也为机器人的位置控制提供反馈数据,四足机器人本体搭载了高精度IMU,其能测量机器人的机体姿态与加速度数据、各关键的角度传感器能测量机器人关节角度并计算出各腿运动学参数即足端位置,传统里程计估计方法仅通过对末端位置微分得到速度,通过对支撑腿运动速度的反向积分得到本体里程计估计结果,但微分数据存在噪声经过二次积分后误差将进一步放大,同时由于采样和微分处理使得最终产生的里程计数据与真实机器人状态存在滞后,因此需要提出一种新的里程计估计方法。
发明内容
本发明公开了一种多传感器融合的四足机器人运动里程计设计方法,其目的在于解决背景技术中提到的四足机器人产生的里程计数据与真实机器人状态存在滞后的问题。
本发明的技术方案如下:
一种多传感器融合的四足机器人运动里程计设计方法,其特征在于,包括如下步骤:
步骤1:沿机器人机头方向建立地形里程计坐标系{O};
步骤2:初始化里程计XY位置数据,初始化里程计卡尔曼滤波器的***状态X=[pxpy vx vy]和协方差矩阵P=I4×4;
步骤3:初始化滤波器对应的传感器测量噪声参数,加速度测量噪声an,足端速度测量噪声vn,位置里程测量噪声pn,得到对应的***噪声矩阵与测量噪声矩阵:
步骤5:基于匀加速模型预测下一时刻里程计的状态:
Pk=APk-1AT+Q
步骤7:计算基于足端位置累加产生的里程计位置测量数据,初始化里程计局部坐标系原点Xodom_st与各腿自己的运动坐标系原点Xleg_st(i)
步骤8:计算每个支撑腿相对各子运动坐标系原点的位置偏差:
dX(i)=Xleg(i)-Xleg_st(i)
步骤9:计算支撑腿位移量的平均值:
步骤10:基于平均支撑平移量与机器人当前里程计运动坐标系原点得到位置测量值:
步骤11:判断单腿摆动切换,采用该触发信号复位里程计运动坐标系与单腿运动坐标系:
Xleg_st(i)=Xleg(i)
步骤12:计算末端速度的反向运动速度,通过前向微分得到各支撑腿对应的速度为:
Vleg(i)=Xleg(i,k)-Xleg(i,k-1)
步骤13:计算支撑腿反向速度的平均值:
步骤14:将反向速度作为速度测量值并与位置测量值构建测量向量:
步骤15:构建***的观测矩阵:
步骤16:计算卡尔曼增益
K=PHT(HPHT+R)-1
步骤17:采用观测向量修正之前的里程计估计结果偿
步骤18:更新后验协方差矩阵
P=(I-KH)P。
本发明的有益效果:通过构建在机体坐标系下的里程计估计坐标系解耦航向波动引起的估计扰动,同时构建了卡尔曼滤波器在使用末端速度微分估计里程数据的同时引入支撑相位置变化与本体IMU加速度测量结果,从而在提高里程计精度的同时保证最终里程计数据的实时性。
附图说明
图1为采用本方法改进后的机器人的俯仰角和横滚角随时间变化的波动图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例,都属于本发明保护的范围。
一种多传感器融合的四足机器人运动里程计设计方法,包括如下步骤:
步骤1:沿机器人机头方向建立地形里程计坐标系{O};
步骤2:初始化里程计XY位置数据,初始化里程计卡尔曼滤波器的***状态X=[pxpy vx vy]和协方差矩阵P=I4×4;
步骤3:初始化滤波器对应的传感器测量噪声参数,加速度测量噪声an,足端速度测量噪声vn,位置里程测量噪声pn,得到对应的***噪声矩阵与测量噪声矩阵:
步骤5:基于匀加速模型预测下一时刻里程计的状态:
Pk=APk-1AT+Q
步骤7:计算基于足端位置累加产生的里程计位置测量数据,初始化里程计局部坐标系原点Xodom_st与各腿自己的运动坐标系原点Xleg_st(i);
步骤8:计算每个支撑腿相对各子运动坐标系原点的位置偏差:
dX(i)=Xleg(i)-Xleg_st(i)
步骤9:计算支撑腿位移量的平均值:
步骤10:基于平均支撑平移量与机器人当前里程计运动坐标系原点得到位置测量值:
步骤11:判断单腿摆动切换,采用该触发信号复位里程计运动坐标系与单腿运动坐标系:
Xleg_st(i)=Xleg(i)
步骤12:计算末端速度的反向运动速度,通过前向微分得到各支撑腿对应的速度为:
Vleg(i)=Xleg(i,k)-Xleg(i,k-1)
步骤13:计算支撑腿反向速度的平均值:
步骤14:将反向速度作为速度测量值并与位置测量值构建测量向量:
步骤15:构建***的观测矩阵:
步骤16:计算卡尔曼增益
K=PHT(HPHT+R)-1
步骤17:采用观测向量修正之前的里程计估计结果偿
步骤18:更新后验协方差矩阵
P=(I-KH)P。
本发明公开的一种多传感器融合的四足机器人运动里程计设计方法,四足机器人运动里程计是借助机器人本体传感器实现对自身精确位置推算的关键技术,里程计推算结果除了为机器人落足提供全局定位信息外,同时也为机器人的位置控制提供反馈数据,四足机器人本体搭载了高精度IMU,其能测量机器人的机体姿态与加速度数据、各关键的角度传感器能测量机器人关节角度并计算出各腿运动学参数即足端位置。本发明提出了一种融合多传感器数据的运动里程计设计方法,通过构建在机体坐标系下的里程计估计坐标系解耦航向波动引起的估计扰动,同时构建了卡尔曼滤波器在使用末端速度微分估计里程数据的同时引入支撑相位置变化与本体IMU加速度测量结果,从而在提高里程计精度的同时保证最终里程计数据的实时性,具体情况见图1。
尽管已经示出和描述了本发明的实施例,对于本领域的普通技术人员而言,可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型,本发明的范围由所附权利要求及其等同物限定。
Claims (1)
1.一种多传感器融合的四足机器人运动里程计设计方法,其特征在于,包括如下步骤:
步骤1:沿机器人机头方向建立地形里程计坐标系{O};
步骤2:初始化里程计XY位置数据,初始化里程计卡尔曼滤波器的***状态X=[px pyvx vy]和协方差矩阵P=I4×4;
步骤3:初始化滤波器对应的传感器测量噪声参数,加速度测量噪声an,足端速度测量噪声vn,位置里程测量噪声pn,得到对应的***噪声矩阵Q与测量噪声矩阵R:
步骤5:基于匀加速模型预测下一时刻里程计的状态:
Pk=APk-1AT+Q;
步骤7:计算基于足端位置累加产生的里程计位置测量数据,初始化里程计局部坐标系原点Xodom_st与各腿自己的运动坐标系原点Xleg_st(i);
步骤8:计算每个支撑腿相对各子运动坐标系原点的位置偏差:
dX(i)=Xleg(i)-Xleg_st(i)
步骤9:计算支撑腿位移量的平均值:
步骤10:基于平均支撑平移量与机器人当前里程计运动坐标系原点得到位置测量值:
步骤11:判断单腿摆动切换,采用触发信号复位里程计运动坐标系与单腿运动坐标系:
Xleg_st(i)=Xleg(i)
步骤12:计算末端速度的反向运动速度,通过前向微分得到各支撑腿对应的速度为:
Vleg(i)=Xleg(i,k)-Xleg(i,k-1)
步骤13:计算支撑腿反向速度的平均值:
步骤14:将反向速度作为速度测量值并与位置测量值构建测量向量:
步骤15:构建***的观测矩阵:
步骤16:计算卡尔曼增益
K=PHT(HPHT+R)-1
步骤17:采用观测向量修正之前的里程计估计结果
步骤18:更新后验协方差矩阵
P=(I-KH)P。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010805263.5A CN111949929B (zh) | 2020-08-12 | 2020-08-12 | 一种多传感器融合的四足机器人运动里程计设计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010805263.5A CN111949929B (zh) | 2020-08-12 | 2020-08-12 | 一种多传感器融合的四足机器人运动里程计设计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111949929A CN111949929A (zh) | 2020-11-17 |
CN111949929B true CN111949929B (zh) | 2022-06-21 |
Family
ID=73332744
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010805263.5A Active CN111949929B (zh) | 2020-08-12 | 2020-08-12 | 一种多传感器融合的四足机器人运动里程计设计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111949929B (zh) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112506190A (zh) * | 2020-11-19 | 2021-03-16 | 深圳市优必选科技股份有限公司 | 一种机器人定位方法、机器人定位装置及机器人 |
WO2023082050A1 (zh) * | 2021-11-09 | 2023-05-19 | 浙江大学 | 一种基于双层滤波框架的高精度里程估计方法 |
CN114046800B (zh) * | 2021-11-09 | 2023-09-29 | 浙江大学 | 一种基于双层滤波框架的高精度里程估计方法 |
CN114021376B (zh) * | 2021-11-17 | 2024-04-09 | 中国北方车辆研究所 | 一种四足机器人地形坡度估计方法 |
CN116429094B (zh) * | 2023-06-15 | 2023-09-26 | 小米汽车科技有限公司 | 定位方法、装置、电子设备及存储介质 |
Citations (7)
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 (zh) * | 2017-01-04 | 2017-08-22 | 北京航空航天大学 | 一种基于Xtion设备的多功能六足机器人的远程控制*** |
CN108621161A (zh) * | 2018-05-08 | 2018-10-09 | 中国人民解放军国防科技大学 | 基于多传感器信息融合的足式机器人本体状态估计方法 |
CN109341705A (zh) * | 2018-10-16 | 2019-02-15 | 北京工业大学 | 智能探测机器人同时定位与地图构建*** |
CN110146077A (zh) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
CN110986939A (zh) * | 2020-01-02 | 2020-04-10 | 东南大学 | 一种基于imu预积分的视觉惯性里程计方法 |
CN111136660A (zh) * | 2020-02-19 | 2020-05-12 | 清华大学深圳国际研究生院 | 机器人位姿定位方法及*** |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR102403504B1 (ko) * | 2015-11-26 | 2022-05-31 | 삼성전자주식회사 | 이동 로봇 및 그 제어 방법 |
CN105666498B (zh) * | 2016-04-21 | 2017-07-07 | 奇弩(北京)科技有限公司 | 提高六足机器人行走稳定性的步态规划方法 |
-
2020
- 2020-08-12 CN CN202010805263.5A patent/CN111949929B/zh active Active
Patent Citations (7)
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 (zh) * | 2017-01-04 | 2017-08-22 | 北京航空航天大学 | 一种基于Xtion设备的多功能六足机器人的远程控制*** |
CN108621161A (zh) * | 2018-05-08 | 2018-10-09 | 中国人民解放军国防科技大学 | 基于多传感器信息融合的足式机器人本体状态估计方法 |
CN109341705A (zh) * | 2018-10-16 | 2019-02-15 | 北京工业大学 | 智能探测机器人同时定位与地图构建*** |
CN110146077A (zh) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
CN110986939A (zh) * | 2020-01-02 | 2020-04-10 | 东南大学 | 一种基于imu预积分的视觉惯性里程计方法 |
CN111136660A (zh) * | 2020-02-19 | 2020-05-12 | 清华大学深圳国际研究生院 | 机器人位姿定位方法及*** |
Non-Patent Citations (2)
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 (zh) | 2020-11-17 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111949929B (zh) | 一种多传感器融合的四足机器人运动里程计设计方法 | |
CN111207774B (zh) | 一种用于激光-imu外参标定的方法及*** | |
CN110926460B (zh) | 一种基于IMU的uwb定位异常值处理方法 | |
CN113311411B (zh) | 一种用于移动机器人的激光雷达点云运动畸变校正方法 | |
CN110554396A (zh) | 一种室内场景下激光雷达建图方法、装置、设备及介质 | |
CN111721288B (zh) | 一种mems器件零偏修正方法、装置及存储介质 | |
CN111896007B (zh) | 一种补偿足地冲击的四足机器人姿态解算方法 | |
JP6083279B2 (ja) | 移動状況情報算出方法及び移動状況情報算出装置 | |
CN110561424A (zh) | 基于多传感器混合滤波器的在线机器人运动学校准方法 | |
CN111795686A (zh) | 一种移动机器人定位与建图的方法 | |
CN110702091A (zh) | 一种沿地铁轨道移动机器人的高精度定位方法 | |
JP2012173190A (ja) | 測位システム、測位方法 | |
CN109959374B (zh) | 一种行人惯性导航全时全程逆向平滑滤波方法 | |
CN108748220B (zh) | 一种基于惯性测量单元的脚-机跟随控制方法及控制*** | |
CN110887481A (zh) | 基于mems惯性传感器的载体动态姿态估计方法 | |
CN113252033A (zh) | 基于多传感器融合的定位方法、定位***及机器人 | |
CN115143954B (zh) | 一种基于多源信息融合的无人车导航方法 | |
CN108871323B (zh) | 一种低成本惯性传感器在机动环境下的高精度导航方法 | |
CN116007620A (zh) | 一种组合导航滤波方法、***、电子设备及存储介质 | |
CN112362057A (zh) | 基于零速修正与姿态自观测的惯性行人导航算法 | |
CN115388899A (zh) | 基于变分贝叶斯的移动机器人视觉惯性融合slam方法 | |
WO2022160811A1 (zh) | 足式机器人轨迹跟踪方法、设备及可读存储介质 | |
Bai et al. | Graph-optimisation-based self-calibration method for IMU/odometer using preintegration theory | |
CN114046800B (zh) | 一种基于双层滤波框架的高精度里程估计方法 | |
CN113447018B (zh) | 一种水下惯性导航***的姿态实时估计方法 |
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 |