CN112904883B - 四足机器人地形感知方法、运动控制方法及系统 - Google Patents

四足机器人地形感知方法、运动控制方法及系统 Download PDF

Info

Publication number
CN112904883B
CN112904883B CN202110114941.8A CN202110114941A CN112904883B CN 112904883 B CN112904883 B CN 112904883B CN 202110114941 A CN202110114941 A CN 202110114941A CN 112904883 B CN112904883 B CN 112904883B
Authority
CN
China
Prior art keywords
robot
terrain
foot end
planning
formula
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
CN202110114941.8A
Other languages
English (en)
Other versions
CN112904883A (zh
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.)
Delu Power Technology Chengdu Co ltd
Original Assignee
Delu Power Technology Chengdu Co ltd
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 Delu Power Technology Chengdu Co ltd filed Critical Delu Power Technology Chengdu Co ltd
Priority to CN202110114941.8A priority Critical patent/CN112904883B/zh
Publication of CN112904883A publication Critical patent/CN112904883A/zh
Application granted granted Critical
Publication of CN112904883B publication Critical patent/CN112904883B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0891—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for land vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

本发明涉及四足机器人地形感知方法、运动控制方法及系统，其利用四足机器人本体传感器，计算每条腿的足端在世界坐标系下的位置；然后将四条腿的足端位置代入地形三维平面方程，获得地形信息；再根据地形信息，来规划机器人的姿态、和/或足端轨迹、和/或质心轨迹，继而控制机器人的运动。本发明可使四足机器人在不依靠环境感知设备的情况下，准确感知地形信息；再根据地形信息控制机器人运动，可以使四足机器人在穿越较大坡度的地形和台阶地形时的稳定性大大增强；本发明方法鲁棒性强，针对不同环境不需要先验信息；并且该方法的实现仅依靠四足机器人本体的基础传感器，部署成本极低。

Description

四足机器人地形感知方法、运动控制方法及系统
技术领域
本发明涉及机器人技术领域，尤其涉及四足机器人地形感知方法、运动控制方法及系统。
背景技术
目前，地形感知技术主要有以下两种：
一、基于SLAM的地形感知技术：利用深度视觉传感器或者激光雷达建立深度场信息，构建地形地图；
二、基于深度学习的地形感知技术：利用庞大的视觉图像数据以及巨大的算力，构建地形检测和识别网络模型，对地形进行检测和识别。
现有技术具有以下缺陷：
(1)感知传感器(深度场视觉传感器、激光雷达等)成本昂贵；
(2)算力花费巨大，通常需要附加的专门用于感知地形的处理器；
(3)算法实践成本大，算法鲁棒性差。
发明内容
本发明为了解决上述技术问题提供四足机器人地形感知方法、运动控制方法及系统。
本发明通过下述技术方案实现：
四足机器人地形感知方法，包括以下步骤：
S1，利用四足机器人本体传感器，计算每条腿的足端在世界坐标系下的位置；
S2，将四条腿的足端位置代入地形三维平面方程，获得地形信息。
进一步的，所述的地形三维平面方程式为：
z(x,y)＝α0+α1x+α2y (1)
式(1)中，x,y,z为足端在世界坐标系下的位置。
进一步的，所述S2具体包括：
S2.1，将四条腿的足端位置代入公式(1)，建立超定方程组Wa＝Pz；
S2.2，采用公式(2)求解超定方程组Wa＝Pz的最小二乘解a：
Figure BDA0002917566490000011
式(2)中，W＝[1，Px，Py]4×3，
Figure BDA0002917566490000021
表示违逆；
Figure BDA0002917566490000022
Py表示四条腿足端在世界坐标系下位置的y轴分量；
Figure BDA0002917566490000023
Px表示四条腿足端在世界坐标系下位置的x轴分量；
a＝(α0,α1,α2)T，a为地形三维平面方程参数。
其中，所述S1中利用四足机器人的惯性传感器和电机编码器，计算每条腿的足端在世界坐标系下的位置。
四足机器人运动控制方法，它根据所述的四足机器人地形感知方法获得的地形信息，来规划机器人的姿态、和/或足端轨迹、和/或质心轨迹。
进一步的，采用公式(3)规划机器人的姿态：
Figure BDA0002917566490000024
Figure BDA0002917566490000025
式(3)中，Θabs为机器人在世界坐标系的期望姿态，Θrel为输入的机器人相对于地形平面的期望姿态，Rpla为地面地形旋转矩阵，φ为地形平面相对于海平面的翻滚角，θ为俯仰角，
Figure BDA0002917566490000026
为偏航角；rpyToRot()表示欧拉角转旋转矩阵，rotToRPY()表示旋转矩阵转欧拉角。
进一步的，φ＝-actan(α2)，θ＝actan(α1)，
Figure BDA0002917566490000027
进一步的，设落足点在水平面的投影坐标为
Figure BDA0002917566490000028
采用公式(4)规划机器人的足端轨迹：
Figure BDA0002917566490000029
进一步的，设期望质心在水平面的投影坐标为
Figure BDA00029175664900000210
采用公式(5)规划机器人的质心轨迹：
Figure BDA00029175664900000211
四足机器人运动控制系统，包括：
足端位置测量模块：负责根据四足机器人本体传感器的检测数据，计算每条腿的足端在世界坐标系下的位置；
地形感知模块：负责根据每条腿的足端位置获得地形信息；
姿态规划模块：负责根据地形信息以及输入的期望姿态规划机器人的姿态；
足端规划模块：负责根据地形信息规划机器人的足端轨迹；
质心规划模块：负责根据地形信息规划机器人的质心轨迹；
运动控制器：负责根据规划的姿态、足端轨迹、质心轨迹，控制机器人的运动。
与现有技术相比，本发明具有以下有益效果：
1，本发明可使四足机器人在不依靠环境感知设备(摄像头、雷达等)的情况下，准确感知地形信息；
2，本发明根据地形信息进行的姿态规划、足端轨迹规划和质心轨迹规划，可以使四足机器人在穿越较大坡度的地形和台阶地形时的稳定性大大增强；
3，本发明方法鲁棒性强，针对不同环境不需要先验信息；并且该方法的实现仅依靠四足机器人本体的基础传感器，部署成本极低。
附图说明
此处所说明的附图用来提供对本发明实施例的进一步理解，构成本申请的一部分，并不构成对本发明实施例的限定。
图1是本发明的流程图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。
本发明公开的四足机器人地形感知方法，包括以下步骤：
S1，利用四足机器人本体传感器，例如IMU和电机编码器，计算每条腿的足端在世界坐标系下的位置：p1(x,y,z)，p2(x,y,z)，p3(x,y,z)，p4(x,y,z)。
S2，将四条腿的足端位置代入地形三维平面方程z(x,y)＝α0+α1x+α2y，获得地形信息。其计算过程如下：
S2.1，将四条腿的足端位置代入公式(1)，建立超定方程组Wa＝Pz；
S2.2，采用公式(2)求解超定方程组Wa＝Pz的最小二乘解a：
Figure BDA0002917566490000041
式(2)中，W＝[1，Px，Py]4×3，
Figure BDA0002917566490000042
表示违逆；
Figure BDA0002917566490000043
Py表示四条腿足端在世界坐标系下位置的y轴分量；
Figure BDA0002917566490000044
Px表示四条腿足端在世界坐标系下位置的x轴分量；
a＝(α0,α1,α2)T，a为地形三维平面方程参数。
如图1所示，本发明公开的四足机器人运动控制方法，该方法根据上述形感知方法获得的地形信息，来规划机器人的姿态、和/或足端轨迹、和/或质心轨迹，继而控制机器人的运动。
姿态规划：采用公式(3)规划机器人的姿态：
Figure BDA0002917566490000045
Figure BDA0002917566490000046
式(3)中，Θabs为机器人在世界坐标系的期望姿态，Θrel为输入的机器人相对于地形平面的期望姿态，Rpla为地面地形旋转矩阵，φ为地形平面相对于海平面的翻滚角，φ＝-actan(α2)；θ为俯仰角，θ＝actan(α1)；忽略偏航角
Figure BDA0002917566490000047
Figure BDA0002917566490000048
rpyToRot()表示欧拉角转旋转矩阵，rotToRPY()表示旋转矩阵转欧拉角。
足端规划：已知落足点在水平面的投影坐标为
Figure BDA0002917566490000049
采用公式(4)规划机器人的足端轨迹：
Figure BDA00029175664900000410
质心规划：已知期望质心在水平面的投影坐标为
Figure BDA00029175664900000411
采用公式(5)规划机器人的质心轨迹：
Figure BDA00029175664900000412
基于上述四足机器人地形感知方法，本发明还公开了四足机器人运动控制系统，包括：
足端位置测量模块：负责根据四足机器人本体传感器的检测数据，计算每条腿的足端在世界坐标系下的位置；
地形感知模块：负责根据每条腿的足端位置获得地形信息；
姿态规划模块：负责根据地形信息以及输入的期望姿态规划机器人的姿态；
足端规划模块：负责根据地形信息规划机器人的足端轨迹；
质心规划模块：负责根据地形信息规划机器人的质心轨迹；
运动控制器：负责根据规划的姿态、足端轨迹、质心轨迹，控制机器人的运动。
本发明不依靠附加的外部环境感知设备，利用四足机器人传感器，对未知的复杂地形进行精确的感知和识别，并指导四足机器人控制算法进行动力学模型修正和质心、足端的轨迹规划，使四足机器人可以稳定穿越各种复杂地形，例如台阶、上下坡等。
以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.四足机器人地形感知方法，其特征在于：包括以下步骤：
S1，利用四足机器人本体传感器，计算每条腿的足端在世界坐标系下的位置；
S2，将四条腿的足端位置代入地形三维平面方程，获得地形信息；
所述的地形三维平面方程式为：
Figure DEST_PATH_IMAGE001
（1）
式（1）中，
Figure 331930DEST_PATH_IMAGE002
为足端在世界坐标系下的位置；
所述S2具体包括：
S2.1，将四条腿的足端位置代入公式（1），建立超定方程组
Figure DEST_PATH_IMAGE003
；
S2.2，采用公式（2）求解超定方程组
Figure 304302DEST_PATH_IMAGE003
的最小二乘解
Figure 915412DEST_PATH_IMAGE004
：
Figure DEST_PATH_IMAGE005
（2）
式（2）中，
Figure 816503DEST_PATH_IMAGE006
，
Figure DEST_PATH_IMAGE007
表示违逆；
Figure 979807DEST_PATH_IMAGE008
，
Figure DEST_PATH_IMAGE009
表示四条腿足端在世界坐标系下位置的y轴分量；
Figure 444287DEST_PATH_IMAGE010
，
Figure DEST_PATH_IMAGE011
表示四条腿足端在世界坐标系下位置的x轴分量；
Figure 785269DEST_PATH_IMAGE012
，
Figure 247475DEST_PATH_IMAGE004
为地形三维平面方程参数。
2.根据权利要求1所述的四足机器人地形感知方法，其特征在于：所述S1中，利用四足机器人的惯性传感器和电机编码器，计算每条腿的足端在世界坐标系下的位置。
3.四足机器人运动控制方法，其特征在于：它根据如权利要求1或2所述的四足机器人地形感知方法获得的地形信息，来规划机器人的姿态、和/或足端轨迹、和/或质心轨迹；
采用公式（3）规划机器人的姿态：
Figure DEST_PATH_IMAGE013
（3）
Figure 906864DEST_PATH_IMAGE014
；
式（3）中，
Figure DEST_PATH_IMAGE015
为机器人在世界坐标系的期望姿态，
Figure 925767DEST_PATH_IMAGE016
为输入的机器人相对于地形平面的期望姿态，
Figure DEST_PATH_IMAGE017
为地面地形旋转矩阵，
Figure 91562DEST_PATH_IMAGE018
为地形平面相对于海平面的翻滚角，
Figure DEST_PATH_IMAGE019
为俯仰角，
Figure 52565DEST_PATH_IMAGE020
为偏航角；
Figure DEST_PATH_IMAGE021
表示欧拉角转旋转矩阵，
Figure 372819DEST_PATH_IMAGE022
表示旋转矩阵转欧拉角；
设落足点在水平面的投影坐标为
Figure DEST_PATH_IMAGE023
，采用公式（4）规划机器人的足端轨迹：
Figure 287423DEST_PATH_IMAGE024
（4）；
设期望质心在水平面的投影坐标为
Figure DEST_PATH_IMAGE025
，采用公式（5）规划机器人的质心轨迹：
Figure 258790DEST_PATH_IMAGE026
（5）。
4.根据权利要求3所述的四足机器人运动控制方法，其特征在于：
Figure DEST_PATH_IMAGE027
，
Figure 610268DEST_PATH_IMAGE028
，
Figure DEST_PATH_IMAGE029
。
5.四足机器人运动控制系统，其特征在于：可实现如权利要求3或4中的四足机器人运动控制方法，四足机器人运动控制系统包括：
足端位置测量模块：负责根据四足机器人本体传感器的检测数据，计算每条腿的足端在世界坐标系下的位置；
地形感知模块：负责根据每条腿的足端位置获得地形信息；
姿态规划模块：负责根据地形信息以及输入的期望姿态规划机器人的姿态；
足端规划模块：负责根据地形信息规划机器人的足端轨迹；
质心规划模块：负责根据地形信息规划机器人的质心轨迹；
运动控制器：负责根据规划的姿态、足端轨迹、质心轨迹，控制机器人的运动。
CN202110114941.8A 2021-01-26 2021-01-26 四足机器人地形感知方法、运动控制方法及系统 Active CN112904883B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110114941.8A CN112904883B (zh) 2021-01-26 2021-01-26 四足机器人地形感知方法、运动控制方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110114941.8A CN112904883B (zh) 2021-01-26 2021-01-26 四足机器人地形感知方法、运动控制方法及系统

Publications (2)

Publication Number Publication Date
CN112904883A CN112904883A (zh) 2021-06-04
CN112904883B true CN112904883B (zh) 2022-08-05

Family

ID=76119296

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110114941.8A Active CN112904883B (zh) 2021-01-26 2021-01-26 四足机器人地形感知方法、运动控制方法及系统

Country Status (1)

Country Link
CN (1) CN112904883B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116069027A (zh) * 2022-12-29 2023-05-05 苏州光格科技股份有限公司 机器人的爬梯规划方法、装置、计算机设备和存储介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102122172A (zh) * 2010-12-31 2011-07-13 中国科学院计算技术研究所 机器运动控制的摄像系统及其控制方法
CN106525049A (zh) * 2016-11-08 2017-03-22 山东大学 一种基于计算机视觉的四足机器人本体位姿跟踪方法
CN109465830A (zh) * 2018-12-11 2019-03-15 上海应用技术大学 机器人单目立体视觉标定系统及方法
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及系统
CN111123943A (zh) * 2019-12-30 2020-05-08 汕头大学 一种基于伪逆约束的超冗余机器人轨迹规划方法及系统
CN111483532A (zh) * 2020-06-02 2020-08-04 山东大学 一种静步态上楼梯的四足机器人运动控制方法
CN111506100A (zh) * 2020-06-15 2020-08-07 深圳市优必选科技股份有限公司 多足机器人关节控制方法、装置和多足机器人

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1728600B1 (en) * 2005-05-31 2008-03-12 Honda Research Institute Europe GmbH Controlling the trajectory of an effector
CN107807655B (zh) * 2017-10-27 2019-08-30 山东大学 基于三维空间中步态调整的四足机器人平衡稳定控制方法
CN110497414B (zh) * 2019-08-27 2022-04-26 山东科技大学 四足机器人连续躯干运动轨迹规划方法
CN110842921B (zh) * 2019-11-19 2022-06-24 山东大学 四足机器人大坡度地形或高障碍物攀爬跨越的步态规划方法
CN112000098A (zh) * 2020-08-18 2020-11-27 广州市优普科技有限公司 一种基于对角小跑步态的四足机器人运动控制方法及系统

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102122172A (zh) * 2010-12-31 2011-07-13 中国科学院计算技术研究所 机器运动控制的摄像系统及其控制方法
CN106525049A (zh) * 2016-11-08 2017-03-22 山东大学 一种基于计算机视觉的四足机器人本体位姿跟踪方法
CN109465830A (zh) * 2018-12-11 2019-03-15 上海应用技术大学 机器人单目立体视觉标定系统及方法
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及系统
CN111123943A (zh) * 2019-12-30 2020-05-08 汕头大学 一种基于伪逆约束的超冗余机器人轨迹规划方法及系统
CN111483532A (zh) * 2020-06-02 2020-08-04 山东大学 一种静步态上楼梯的四足机器人运动控制方法
CN111506100A (zh) * 2020-06-15 2020-08-07 深圳市优必选科技股份有限公司 多足机器人关节控制方法、装置和多足机器人

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
四足机器人参数辨识及步态规划研究;陈建宇;《中国优秀博硕士学位论文全文数据库（硕士）信息科技辑》;20210115(第1期);全文 *
基于柔性及反驱特性的六足机器人建模与仿真研究;金马;《中国优秀博硕士学位论文全文数据库（博士）信息科技辑》;20190115(第1期);全文 *
面向未知坡面的四足机器人trot步态研究;张树龙;《中国优秀博硕士学位论文全文数据库（硕士）信息科技辑》;20200115(第2期);第I140-600页 *

Also Published As

Publication number Publication date
CN112904883A (zh) 2021-06-04

Similar Documents

Publication Publication Date Title
CN108453738B (zh) 一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法
Lobo et al. Vision and inertial sensor cooperation using gravity as a vertical reference
Wang et al. A simple and parallel algorithm for real-time robot localization by fusing monocular vision and odometry/AHRS sensors
US9043146B2 (en) Systems and methods for tracking location of movable target object
US7865267B2 (en) Environment recognizing device, environment recognizing method, route planning device, route planning method and robot
CN108717712A (zh) 一种基于地平面假设的视觉惯导slam方法
CN110095116A (zh) 一种基于lift的视觉定位和惯性导航组合的定位方法
CN106441275A (zh) 一种机器人规划路径的更新方法及装置
Belter et al. Precise self‐localization of a walking robot on rough terrain using parallel tracking and mapping
Sun et al. Autonomous state estimation and mapping in unknown environments with onboard stereo camera for micro aerial vehicles
CN112904883B (zh) 四足机器人地形感知方法、运动控制方法及系统
KR101132713B1 (ko) 이동로봇의 이동 제어장치, 이를 구비하는 이동로봇 시스템 및 이동로봇의 이동 제어방법
Xu et al. A new monocular vision measurement method to estimate 3D positions of objects on floor
CN108927807A (zh) 一种基于点特征的机器人视觉控制方法
Mohareri et al. Autonomous humanoid robot navigation using augmented reality technique
Kang et al. Adaptive trajectory tracking of wheeled mobile robots based on a fish-eye camera
KR20210010309A (ko) 항공사진을 이용하여 3차원 지도를 생성하는 장치 및 방법
Galben New three-dimensional velocity motion model and composite odometry–inertial motion model for local autonomous navigation
Tiwari Position control of an unmanned aerial vehicle from a mobile ground vehicle
CN111649746B (zh) 一种融合惯导测量和ArUco标记的定位与导航方法
Pucci et al. Nonlinear control of unicycle-like robots for person following
MacArthur et al. Unmanned ground vehicle state estimation using an unmanned air vehicle
CN117128953A (zh) 管道爬壁机器人航位推算方法、设备及存储介质
CN114252073A (zh) 一种机器人姿态数据融合方法
Diop et al. A computer vision-aided motion sensing algorithm for mobile robot's indoor navigation

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