CN114021376A - 一种四足机器人地形坡度估计方法 - Google Patents
一种四足机器人地形坡度估计方法 Download PDFInfo
- Publication number
- CN114021376A CN114021376A CN202111364706.2A CN202111364706A CN114021376A CN 114021376 A CN114021376 A CN 114021376A CN 202111364706 A CN202111364706 A CN 202111364706A CN 114021376 A CN114021376 A CN 114021376A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- robot
- model
- angle
- slope
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 26
- 241001123248 Arma Species 0.000 claims abstract description 8
- 238000001914 filtration Methods 0.000 claims abstract description 5
- 210000002414 leg Anatomy 0.000 claims description 37
- 210000004394 hip joint Anatomy 0.000 claims description 14
- 239000011159 matrix material Substances 0.000 claims description 12
- 238000012545 processing Methods 0.000 claims description 12
- 238000012937 correction Methods 0.000 claims description 6
- 210000000689 upper leg Anatomy 0.000 claims description 6
- 230000002159 abnormal effect Effects 0.000 claims description 3
- 238000003491 array Methods 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000002474 experimental method Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000005021 gait Effects 0.000 abstract description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 210000001503 joint Anatomy 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 239000003550 marker Substances 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
- G06F2119/14—Force analysis or force optimisation, e.g. static or dynamic forces
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A10/00—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE at coastal zones; at river basins
- Y02A10/23—Dune restoration or creation; Cliff stabilisation
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Hardware Design (AREA)
- Evolutionary Computation (AREA)
- Geometry (AREA)
- General Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Manipulator (AREA)
Abstract
本发明提出一种四足机器人地形坡度估计方法,主要步骤包括单腿运动学解算,姿态解算坐标系转换,TD状态缓存。最小二乘平面拟合和ARMA模型回归预测。本发明基于ARMA模型,引入地形类型先验知识实现对坡面角度拟合数据的滤波、预测与估计,面向四足机器人常用Trot步态仅有两腿支撑的特点,以历史足地接触信号为触发采集足端运动学数据与机载惯性传感器测量姿态,从而获取四个离散点数据并进一步采用最小二乘法进行拟合坡面角度,能够提高拟合结果的平滑性能。
Description
技术领域
本发明属于自动控制技术领域,具体涉及一种结合最小二乘法与ARMA时间序列的四足机器人地形坡度估计方法。
背景技术
地形坡度估计是四足机器人在通过起伏地形时的重要状态估计数据,四足机器人在坡度地形移动中姿态需要跟随地形调节,从而为腿部提供充足工作空间,避免摆动腿与地形干涉。因此,地形坡度估计技术十分重要。为实现在线地形坡度估计并降低由于摆动控制误差、误触地以及姿态失稳引起的估计误差,需要提出一种能拟合融合地形先验知识对坡度平滑、滤波与预测的估计方法。
发明内容
(一)要解决的技术问题
本发明提出一种四足机器人地形坡度估计方法,以解决如何提高地形坡度估计精度的技术问题。
(二)技术方案
为了解决上述技术问题,本发明提出一种四足机器人地形坡度估计方法,该方法包括如下步骤:
S1.基于机器人结构参数与单腿构型,使用运动学正解计算各支撑腿足端在机体坐标系{B}下的三维位置
S1-1.使用关节角度反馈值,通过运动学正解计算各支撑腿足端在胯关节坐标系{H}下的三维位置:
Pi H=f(c1,c2,c3),i=1…4
其中,Pi H为第i条支撑腿足端在胯关节坐标系{H}下的三维位置;f(c1,c2,c3)为对应单腿构型的运动学正解计算函数:
其中,c1为机器人大腿角度,c2为小腿角度,c3为胯关节角度;L1为大腿长度,L2为小腿长度,L3为侧摆电机相对跨关节的偏差;
S1-2.将各支撑腿足端在胯关节坐标系{H}下的三维位置转换为机体坐标系{B}下的三维位置:
其中,Pi B为第i条支撑腿足端在机体坐标系{B}下的三维位置,W为机器人左右两侧支撑腿胯关节间距,H为机器人前后两侧支撑腿胯关节间距;
S2.基于机器人机载IMU测量得到的姿态四元数,获取当前全局坐标系{N}与机体坐标系{B}间的转换矩阵,并将各支撑腿足端在机体坐标系{B}下的三维位置转换为全局坐标系{N}下的三维位置:
S3.定义第i条支撑腿着地触发标志位TDi,当该标志位由摆动状态切换为支撑状态时刻,将当前全局坐标系{N}系下支撑腿足端位置Pi N存入数组当数组中数据数量大于4时,对当前支撑平面的方程系数进行拟合:
S3-1.设支撑平面函数为z=ax+by+c,其待拟合系数为X=[a b c];设最小二乘法代价函数为:
对代价函数求偏导:
其对应矩阵形式如下:
通过对上述矩阵求逆运算,计算出拟合系数X=[a b c];
S3-2.采用如下公式计算t时刻支撑平面的拟合坡度角:
其中,θt为t时刻拟合坡度角,α为坡度俯仰角,β为坡度横滚角;
S4.采用ARMA模型对拟合坡度角进行滤波与预测
采用滑动平均模型,按如下公式剔除时间序列t内数据中存在的异常值:
其中,为滑动平均模型修正结果,n为滑动平均模型对应的系统阶次,μj为滑动平均模型对应的系数,为相对t的前一时刻滑动平均模型的历史处理结果,θt-j为相对t的j时刻的坡度角历史拟合值,为相对t的j-1时刻滑动平均模型的历史处理结果;
其中,λ为模型分配权重。
进一步地,步骤S4-2中,使用外部基于图像数据识别的结果对当前所处地形进行分类,采用离线数据实验和专家数据库获取分配权重。
(三)有益效果
本发明提出一种四足机器人地形坡度估计方法,主要步骤包括单腿运动学解算,姿态解算坐标系转换,TD状态缓存。最小二乘平面拟合和ARMA模型回归预测。本发明基于ARMA模型,引入地形类型先验知识实现对坡面角度拟合数据的滤波、预测与估计,面向四足机器人常用Trot步态仅有两腿支撑的特点,以历史足地接触信号为触发采集足端运动学数据与机载惯性传感器测量姿态,从而获取四个离散点数据并进一步采用最小二乘法进行拟合坡面角度,能够提高拟合结果的平滑性能。
附图说明中
图1为本发明实施例的机器人坐标系示意图:(a)侧视图,(b)俯视图({H}为胯关节坐标系,{B}为机体坐标系);
图2为本发明实施例的地形坡度估计方法流程;
图3为本发明实施例中坡度角拟合结果。
具体实施方式
为使本发明的目的、内容和优点更加清楚,下面结合附图和实施例,对本发明的具体实施方式作进一步详细描述。
本实施例提出一种四足机器人地形坡度估计方法,所涉及的机器人坐标系如图1所示,其中{H}为胯关节坐标系,{B}为机体坐标系,该方法的主要流程如图2所示,具体包括如下步骤:
S1.基于机器人结构参数与单腿构型,使用运动学正解计算各支撑腿足端在机体坐标系{B}下的三维位置
S1-1.使用关节角度反馈值,通过运动学正解计算各支撑腿足端在胯关节坐标系{H}下的三维位置:
Pi H=f(c1,c2,c3),i=1…4
其中,Pi H为第i条支撑腿足端在胯关节坐标系{H}下的三维位置;f(c1,c2,c3)为对应单腿构型的运动学正解计算函数:
其中,c1为机器人大腿角度,c2为小腿角度,c3为胯关节角度;L1为大腿长度,L2为小腿长度,L3为侧摆电机相对跨关节的偏差。
S1-2.将各支撑腿足端在胯关节坐标系{H}下的三维位置转换为机体坐标系{B}下的三维位置:
其中,Pi B为第i条支撑腿足端在机体坐标系{B}下的三维位置,W为机器人左右两侧支撑腿胯关节间距,H为机器人前后两侧支撑腿胯关节间距。
S2.基于机器人机载IMU测量得到的姿态四元数,获取当前全局坐标系{N}与机体坐标系{B}间的转换矩阵,并将各支撑腿足端在机体坐标系{B}下的三维位置转换为全局坐标系{N}下的三维位置:
S3.定义第i条支撑腿着地触发标志位TDi,当该标志位由摆动状态切换为支撑状态时刻,将当前全局坐标系{N}系下支撑腿足端位置Pi N存入数组当数组中数据数量大于4时,对当前支撑平面的方程系数进行拟合,计算支撑平面的拟合坡度角
S3-1.设支撑平面函数为z=ax+by+c,其待拟合系数为X=[a b c];设最小二乘法代价函数为:
对代价函数求偏导:
其对应矩阵形式如下:
通过对上述矩阵求逆运算,计算出拟合系数X=[a b c]。
S3-2.采用如下公式计算t时刻支撑平面的拟合坡度角
其中,θt为t时刻支撑平面的拟合坡度角,α为坡度俯仰角,β为坡度横滚角。坡度角的拟合结果如图3所示。
S4.采用ARMA模型对拟合坡度角进行滤波与预测
采用滑动平均(MA)模型,按如下公式剔除时间序列t内数据中存在的异常值:
其中,为MA模型修正结果,n为滑动平均模型对应的系统阶次,μj为滑动平均模型对应的系数,为相对t的前一时刻滑动平均模型的历史处理结果,θt-j为相对t的j时刻的坡度角历史拟合值,为相对t的j-1时刻滑动平均模型的历史处理结果。
其中,λ为模型分配权重,可使用外部基于图像数据识别的结果对当前所处地形进行分类,如平地、起伏坡地、砾石地、台阶等,采用离线数据实验和专家数据库获取分配权重。
以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明技术原理的前提下,还可以做出若干改进和变形,这些改进和变形也应视为本发明的保护范围。
Claims (2)
1.一种四足机器人地形坡度估计方法,其特征在于,所述方法包括如下步骤:
S1.基于机器人结构参数与单腿构型,使用运动学正解计算各支撑腿足端在机体坐标系{B}下的三维位置
S1-1.使用关节角度反馈值,通过运动学正解计算各支撑腿足端在胯关节坐标系{H}下的三维位置:
Pi H=f(c1,c2,c3),i=1…4
其中,Pi H为第i条支撑腿足端在胯关节坐标系{H}下的三维位置;f(c1,c2,c3)为对应单腿构型的运动学正解计算函数:
其中,c1为机器人大腿角度,c2为小腿角度,c3为胯关节角度;L1为大腿长度,L2为小腿长度,L3为侧摆电机相对跨关节的偏差;
S1-2.将各支撑腿足端在胯关节坐标系{H}下的三维位置转换为机体坐标系{B}下的三维位置:
其中,Pi B为第i条支撑腿足端在机体坐标系{B}下的三维位置,W为机器人左右两侧支撑腿胯关节间距,H为机器人前后两侧支撑腿胯关节间距;
S2.基于机器人机载IMU测量得到的姿态四元数,获取当前全局坐标系{N}与机体坐标系{B}间的转换矩阵,并将各支撑腿足端在机体坐标系{B}下的三维位置转换为全局坐标系{N}下的三维位置:
S3.定义第i条支撑腿着地触发标志位TDi,当该标志位由摆动状态切换为支撑状态时刻,将当前全局坐标系{N}系下支撑腿足端位置Pi N存入数组当数组中数据数量大于4时,对当前支撑平面的方程系数进行拟合:
S3-1.设支撑平面函数为z=ax+by+c,其待拟合系数为X=[a b c];设最小二乘法代价函数为:
对代价函数求偏导:
其对应矩阵形式如下:
通过对上述矩阵求逆运算,计算出拟合系数X=[a b c];
S3-2.采用如下公式计算t时刻支撑平面的拟合坡度角:
其中,θt为t时刻拟合坡度角,α为坡度俯仰角,β为坡度横滚角;
S4.采用ARMA模型对拟合坡度角进行滤波与预测
采用滑动平均模型,按如下公式剔除时间序列t内数据中存在的异常值:
其中,为滑动平均模型修正结果,n为滑动平均模型对应的系统阶次,μj为滑动平均模型对应的系数,为相对t的前一时刻滑动平均模型的历史处理结果,θt-j为相对t的j时刻的坡度角历史拟合值,为相对t的j-1时刻滑动平均模型的历史处理结果;
其中,λ为模型分配权重。
2.如权利要求1所述的地形坡度估计方法,其特征在于,步骤S4-2中,使用外部基于图像数据识别的结果对当前所处地形进行分类,采用离线数据实验和专家数据库获取分配权重。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111364706.2A CN114021376B (zh) | 2021-11-17 | 2021-11-17 | 一种四足机器人地形坡度估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111364706.2A CN114021376B (zh) | 2021-11-17 | 2021-11-17 | 一种四足机器人地形坡度估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114021376A true CN114021376A (zh) | 2022-02-08 |
CN114021376B CN114021376B (zh) | 2024-04-09 |
Family
ID=80065169
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111364706.2A Active CN114021376B (zh) | 2021-11-17 | 2021-11-17 | 一种四足机器人地形坡度估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114021376B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114625129A (zh) * | 2022-02-22 | 2022-06-14 | 中国科学院自动化研究所 | 位控腿足机器人的运动控制方法及系统 |
CN115167389A (zh) * | 2022-06-07 | 2022-10-11 | 中国北方车辆研究所 | 行为驱动的四足机器人中枢层细粒度演进优化模型 |
CN115407790A (zh) * | 2022-08-16 | 2022-11-29 | 中国北方车辆研究所 | 一种基于深度学习的四足机器人侧向速度估算方法 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20080119961A1 (en) * | 2006-11-16 | 2008-05-22 | Samsung Electronics Co., Ltd. | Methods, apparatus, and medium for estimating pose of mobile robot using particle filter |
CN107065867A (zh) * | 2017-03-28 | 2017-08-18 | 浙江大学 | 一种面向未知崎岖地形的四足机器人运动规划方法 |
CN109282810A (zh) * | 2018-09-05 | 2019-01-29 | 广州市蚺灵科技有限公司 | 一种惯导和角度传感器融合的蛇形机器人姿态估计方法 |
JP2019025104A (ja) * | 2017-08-01 | 2019-02-21 | 株式会社国際電気通信基礎技術研究所 | 運動学習支援装置および運動学習支援方法 |
CN110929402A (zh) * | 2019-11-22 | 2020-03-27 | 哈尔滨工业大学 | 一种基于不确定分析的概率地形估计方法 |
CN111896007A (zh) * | 2020-08-12 | 2020-11-06 | 智能移动机器人(中山)研究院 | 一种补偿足地冲击的四足机器人姿态解算方法 |
CN111891252A (zh) * | 2020-08-06 | 2020-11-06 | 齐鲁工业大学 | 一种四足仿生机器人的身体姿态斜坡自适应控制方法 |
CN111949929A (zh) * | 2020-08-12 | 2020-11-17 | 智能移动机器人(中山)研究院 | 一种多传感器融合的四足机器人运动里程计设计方法 |
WO2020253854A1 (zh) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
-
2021
- 2021-11-17 CN CN202111364706.2A patent/CN114021376B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20080119961A1 (en) * | 2006-11-16 | 2008-05-22 | Samsung Electronics Co., Ltd. | Methods, apparatus, and medium for estimating pose of mobile robot using particle filter |
CN107065867A (zh) * | 2017-03-28 | 2017-08-18 | 浙江大学 | 一种面向未知崎岖地形的四足机器人运动规划方法 |
JP2019025104A (ja) * | 2017-08-01 | 2019-02-21 | 株式会社国際電気通信基礎技術研究所 | 運動学習支援装置および運動学習支援方法 |
CN109282810A (zh) * | 2018-09-05 | 2019-01-29 | 广州市蚺灵科技有限公司 | 一种惯导和角度传感器融合的蛇形机器人姿态估计方法 |
WO2020253854A1 (zh) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
CN110929402A (zh) * | 2019-11-22 | 2020-03-27 | 哈尔滨工业大学 | 一种基于不确定分析的概率地形估计方法 |
CN111891252A (zh) * | 2020-08-06 | 2020-11-06 | 齐鲁工业大学 | 一种四足仿生机器人的身体姿态斜坡自适应控制方法 |
CN111896007A (zh) * | 2020-08-12 | 2020-11-06 | 智能移动机器人(中山)研究院 | 一种补偿足地冲击的四足机器人姿态解算方法 |
CN111949929A (zh) * | 2020-08-12 | 2020-11-17 | 智能移动机器人(中山)研究院 | 一种多传感器融合的四足机器人运动里程计设计方法 |
Non-Patent Citations (3)
Title |
---|
张亚强;: "基于最小二乘法的四足机器人地形估计", 南方农机, no. 17, 15 September 2020 (2020-09-15) * |
陈新伟;孟祥刚;阙雅静;汪星一;陈瑶;郑美香;: "基于IMU的移动机器人精确角度获取方法", 闽江学院学报, no. 02, 25 March 2017 (2017-03-25) * |
魏扬帆等: "基于CPG的四足机器人坡面稳定行走控制研究", 《控制工程》, 20 June 2021 (2021-06-20) * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114625129A (zh) * | 2022-02-22 | 2022-06-14 | 中国科学院自动化研究所 | 位控腿足机器人的运动控制方法及系统 |
CN114625129B (zh) * | 2022-02-22 | 2023-09-12 | 中国科学院自动化研究所 | 位控腿足机器人的运动控制方法及系统 |
CN115167389A (zh) * | 2022-06-07 | 2022-10-11 | 中国北方车辆研究所 | 行为驱动的四足机器人中枢层细粒度演进优化模型 |
CN115167389B (zh) * | 2022-06-07 | 2024-07-19 | 中国北方车辆研究所 | 行为驱动的四足机器人中枢层细粒度演进优化模型 |
CN115407790A (zh) * | 2022-08-16 | 2022-11-29 | 中国北方车辆研究所 | 一种基于深度学习的四足机器人侧向速度估算方法 |
CN115407790B (zh) * | 2022-08-16 | 2024-04-26 | 中国北方车辆研究所 | 一种基于深度学习的四足机器人侧向速度估算方法 |
Also Published As
Publication number | Publication date |
---|---|
CN114021376B (zh) | 2024-04-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114021376A (zh) | 一种四足机器人地形坡度估计方法 | |
US7606634B2 (en) | Control device for mobile body | |
CN111674486B (zh) | 双足机器人稳定步行控制方法、装置、设备及可读介质 | |
CN108621161B (zh) | 基于多传感器信息融合的足式机器人本体状态估计方法 | |
US7873436B2 (en) | Gait generator for mobile robot | |
CN107891920B (zh) | 一种用于双足机器人的腿部关节补偿角度自动获取方法 | |
US9964956B2 (en) | Operating environment information generating device for mobile robot | |
Kuehn et al. | Additional DOFs and sensors for bio-inspired locomotion: Towards active spine, ankle joints, and feet for a quadruped robot | |
US6177776B1 (en) | Apparatus for recognizing the landed state of foot of legged moving robot | |
CN112644599B (zh) | 双足机器人的姿态调整方法、装置、设备及存储介质 | |
CN113985864A (zh) | 一种动态行走的四足探测机器人及控制方法 | |
US9802315B2 (en) | Controller for mobile robot | |
CN113002659B (zh) | 双足机器人的姿态测量方法、装置、设备及存储介质 | |
CN114161411A (zh) | 一种基于视觉的多足机器人运动学参数标定方法 | |
Liu et al. | State estimation of a heavy-duty hexapod robot with passive compliant ankles based on the leg kinematics and IMU data fusion | |
CN113253748B (zh) | 并联构型仿人机器人的状态估计方法、装置、设备及介质 | |
CN113465822A (zh) | 基于多刚体机器人的质心位置计算方法、装置及介质 | |
Fedorov | Humanoid bipedal balancing and locomotion | |
Hashimoto et al. | Terrain-adaptive control to reduce landing impact force for human-carrying biped robot | |
JP3674787B2 (ja) | ロボット装置の動作制御装置及び動作制御方法、並びにコンピュータ・プログラム | |
CN112783043B (zh) | 一种仿人机器人控制方法、装置、计算机设备及存储介质 | |
CN118456407A (zh) | 运动轨迹获取方法和装置、足式机器人、存储介质 | |
CN118392173A (zh) | 一种融合运动学信息的机器人实时定位与建图方法和装置 | |
Hashimoto et al. | Terrain-adaptive control with small landing impact force for biped vehicle | |
JP2004306253A (ja) | ロボット装置及びロボット装置の制御方法 |
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 |