CN110405762A - 一种基于空间二阶倒立摆模型的双足机器人姿态控制方法 - Google Patents

Abstract

本发明公开了一种基于空间二阶倒立摆模型的双足机器人姿态控制方法，属于双足机器人运动控制技术领域。该方法将双足机器人上身姿态的控制映射到空间二阶倒立摆模型四个关节的控制上，实现了双足机器人姿态的耦合非线性控制，在简化机器人动力学模型的同时，保留了姿态角度的耦合与非线性特性，兼顾计算速度与控制精度，改善了双足机器人的平衡能力与动态特性，并提高了能量利用效率。

Description

一种基于空间二阶倒立摆模型的双足机器人姿态控制方法

技术领域

本发明属于双足机器人运动控制技术领域，具体涉及一种基于空间二阶倒立摆模型的双足机器人姿态控制方法。

背景技术

双足机器人采用双腿轮换支撑的方式进行运动，具有跨越障碍、通过崎岖路面等能力，能够适应室内、楼宇、道路、野外等多种环境。一般通过给定腿部关节轨迹使双足机器人产生期望的运动。当实际执行轨迹与期望轨迹误差较大、环境信息误差较大、外部扰动较大时，机器人容易失去平衡、倾倒从而损坏。上身姿态角度是判断双足机器人是否平衡的重要依据，因此，基于上身姿态角度的反馈控制常被用来保持机器人稳定运动。双足机器人的运动方式主要包括站立、行走、奔跑、跳跃，运动状态包括单脚支撑、双脚支撑、腾空。腾空时机器人无着力点，无法进行上身姿态控制，因此上身姿态控制一般作用在单脚支撑期与双脚支撑期。

目前，双足机器人的上身姿态控制主要包括直接控制与间接控制。现有技术基于上身姿态角度与角速度测量，使用PD反馈控制计算腰部关节调节量，直接叠加在腰部关节轨迹上，实现上身姿态晃动的控制。现有技术还在机器人质心与外部环境之间构建虚拟的弹簧阻尼约束，将机器人位置姿态信息转化为虚拟力与力矩信息，再通过导纳控制生成新的质心位置姿态，实现机器人克服外界扰动的姿态柔顺控制。上述方法均忽略了姿态角度间的耦合关系，控制精度与适用范围有限，且没有考虑机器人本体动力学特性，导致运动不自然、能量利用效率低。

发明内容

为了解决现有技术中存在的问题，本发明提供了一种基于空间二阶倒立摆模型的双足机器人姿态控制方法，通过坐标变换、雅可比矩阵将双足机器人的姿态控制映射到空间二阶倒立摆的关节控制上，实现姿态角度的耦合控制；且空间二阶倒立摆与双足机器人动力学特性相似，能够产生更为自然的运动，从而改善机器人的动态特性、提高能量利用效率。

本发明是采用以下技术方案实现上述技术目的的。

一种基于空间二阶倒立摆模型的双足机器人姿态控制方法，获取空间二阶倒立摆模型各关节角度的参考值和实际值以及各关节角速度的参考值和实际值，输入关节力矩控制器，再根据关节力矩限幅，获取实际力矩，从而得到关节修正角度q^*，更新机器人髋部参考轨迹，实现姿态控制。

进一步，所述空间二阶倒立摆模型包括踝部两个关节、髋部两个关节。

进一步，所述踝部两个关节旋转轴相互垂直、髋部两个关节旋转轴相互垂直，使得连杆HB与连杆AH在三维空间内复合运动。

进一步，所述各关节角速度的参考值为：其中J⁺为雅可比矩阵J的伪逆，为空间二阶倒立摆模型连杆HB绕基坐标各坐标轴的参考旋转速度。

更进一步，所述参考旋转速度与绕空间二阶倒立摆模型基坐标系Σ₀各坐标轴的旋转速度之间的关系为：

且其中为参考旋转矩阵微分，⁰R_HB为上半身相对基坐标系Σ₀的旋转矩阵，为参考姿态角速度，⁰θ_HB,p、⁰θ_HB,r为实际姿态角度。

本发明的有益效果为：

本发明将双足机器人动力学模型简化为空间二阶倒立摆模型，该模型中配置踝部两个关节、髋部两个关节，且与双足机器人不同支撑状态下存在映射关系；另外将姿态角控制转化到二阶倒立摆模型关节控制上，保留姿态耦合的同时兼顾动力学特性与能量利用效率。

附图说明

图1为空间二阶倒立摆模型及其与双足机器人映射关系图,图1(a)为空间二阶倒立摆模型示意图，图1(b)为空间二阶倒立摆模型与双足机器人映射关系图；

图2为本发明姿态控制流程图。

具体实施方式

下面将结合附图，以北京理工大学BHR-6P双足机器人为例，对本发明的技术方案作进一步的说明，但是本发明的保护范围并不限于此。

本发明采用的空间二阶倒立摆模型及其与双足机器人的映射关系如图1所示，图1(a) 为空间二阶倒立摆模型示意图，其中X轴正方向为机器人前进方向，绕X轴正方向旋转为Roll (滚动)，Y轴正方向为机器人左侧，绕Y轴正方向旋转为Pitch(俯仰)，Z轴正方向为机器人竖直向上方向，空间二阶倒立摆模型包括四个关节，踝部(Ankle，图中用A点表示)两个关节q₁、q₂，髋部(Hip，图中用H点表示)两个关节q₃、q₄，踝部两个关节即图1(a) 中1、2关节(图中A点)，髋部两个关节为图1(a)中3、4关节，3、4关节为双足机器人左、右髋连线的中点(图中H点)；1、2关节在不同支撑状态下对应不同的位置，如图1(b) 所示，空间二阶倒立摆模型与双足机器人映射关系，具体为：双脚支撑时，在两踝连线的中点上，右脚支撑时，与右踝重合，左脚支撑时，与左踝重合。

空间二阶倒立摆模型基坐标系与各关节坐标系定义如图1(a)所示。基坐标系定义为Σ₀，原点在A点，坐标轴为X₀、Y₀、Z₀；空间二阶倒立摆模型1关节坐标系定义为Σ₁，原点在A点，坐标轴为X₁、Y₁、Z₁，其中关节旋转轴为X₁；2关节坐标系定义为Σ₂，原点在A点，坐标轴为X₂、Y₂、Z₂，其中关节旋转轴为Y₂，且X₁垂直于Y₂；3关节坐标系定义为Σ₃，原点在H点，坐标轴为X₃、Y₃、Z₃，其中关节旋转轴为Y₃；4关节坐标系定义为Σ₄，原点在H点，坐标轴为X₄、Y₄、Z₄，其中关节旋转轴为X₄，且X₄垂直于Y₃；由于X₁垂直于Y₂、X₄垂直于Y₃，所以连杆HB与连杆AH不是在一个二维平面内的单一运动，而是在三维空间内的复合运动。上方连杆HB坐标系定义为Σ_HB，与Σ₄等价。此外，双足机器人世界坐标系定义为Σ_W，其坐标轴方向与Σ₀相同(因为坐标轴方向相同，所以两者作为姿态角度与角速度的参考坐标系时可互换)，原点位置任意；双足机器人上身坐标系定义为Σ_Body，与Σ₄等价；IMU(惯性测量单元)自身坐标系定义为Σ_IMU，原点在IMU 自身中心位置，坐标轴方向与Σ₄相同。

为了便于描述与区分各物理量的参考坐标系，在本实施例中使用左上标表示参考坐标系，右下标表示该物理量的从属对象，如世界坐标系下的机器人上身姿态角度表示为^Wθ_UpperBody，其中W表示参考坐标系为世界坐标系Σ_W，UpperBody表示机器人上半身的姿态角度。

参考文献(Qingqing Li,Zhangguo Yu,Xuechao Chen,et al,Contact Force/Torque Control Based on Viscoelastic Model for Stable Bipedal Walking onIndefinite Uneven Terrain[J],IEEE Trans.Autom.Sci.Eng.,early access,2019.)仿真与实验部分(VI.SIMULATION AND EXPERIMENT)对BHR-6P机器人的介绍，IMU安装在机器人上身，可以测量机器人上身 Pitch(机器人俯仰方向)、Roll(机器人滚动方向)角度；IMU还可以测量其绕自身坐标系Σ_IMU各坐标轴的旋转速度。空间二阶倒立摆模型上方连杆HB与机器人上半身固连，因此连杆HB的姿态、机器人上身姿态、IMU姿态，三者互相等价，在此使用⁰θ_HB＝[⁰θ_HB,p ⁰θ_HB,r]^T表示三者的姿态(⁰θ_HB,p代表Picth方向姿态角度，⁰θ_HB,r代表Roll 方向姿态角度，参考的坐标系是Σ₀或Σ_W)。

基于空间二阶倒立摆模型的双足机器人姿态控制流程如图2所示，具体步骤如下：

步骤(1)，获取机器人规划轨迹

由任意一种现有的双足机器人运动轨迹方法(如预观控制、模型预测控制、基于运动发散分量的行走轨迹规划等)获取规划轨迹，规划轨迹包括：

①世界坐标系下机器人两踝位置向量

②世界坐标系下机器人两髋连线中点的位置向量

③世界坐标系下机器人两髋连线中点的姿态角度

④机器人规划双足支撑状态S^ref，取值包括LS(左脚支撑)、RS(右脚支撑)、DS (双脚支撑)；

⑤世界坐标系下机器人上身期望姿态角度等价于空间二阶倒立摆模型基坐标系下连杆HB的姿态角度

此外，在BHR-6P机器人中，机器人上身姿态角度与两髋姿态角度、两髋连线中点的姿态角度完全等价。

步骤(2)，获取空间二阶倒立摆模型各关节角度的参考值

基于机器人规划轨迹①～④，根据规划双足支撑状态S^ref与图1(b)中的映射关系，计算空间二阶倒立摆模型四个关节角度的参考值q^ref＝[q₁ ^ref q₂ ^ref q₃ ^ref q₄ ^ref]^T，如下所示：

其中，InverseKinematics(.)为一种基于雅可比矩阵的逆运动学数值迭代求解方法(梶田秀司.仿人机器人[M].管贻生.北京：清华大学出版社，2007：50-54)。

步骤(3)，获取IMU姿态角度测量值

由IMU获取IMU姿态角度测量值，即空间二阶倒立摆模型连杆HB与机器人上半身的实际姿态角度⁰θ_HB＝[⁰θ_HB,p ⁰θ_HB,r]^T。

步骤(4)，姿态角度反馈控制

构建上身姿态反馈控制器其中为机器人规划轨迹⑤，K 为反馈控制器反馈矩阵(可根据仿真实验结果手动设置，也可由现代控制理论方法计算得出)，从而得到参考姿态角速度

步骤(5)，参考姿态角速度转化为各轴角速度

依据实际姿态角度⁰θ_HB求取上半身相对空间二阶倒立摆基坐标系Σ₀的旋转矩阵⁰R_HB＝RotY(⁰θ_HB,p)RotX(⁰θ_HB,r)；依据参考姿态角速度求取参考旋转矩阵微分依据将参考姿态角速度转化为绕空间二阶倒立摆基坐标系Σ₀各坐标轴的旋转速度：

其中，是由得到的3阶反对称矩阵，其与(连杆HB绕基坐标各坐标轴的参考旋转速度)的关系式如下所示：

由此可得到空间二阶倒立摆模型连杆HB绕基坐标Σ₀系各坐标轴的参考旋转速度

步骤(6)，获取机器人实际运动轨迹

通过机器人关节编码器获取机器人实际关节角度Q_LeftLeg、Q_RightLeg，结合机器人上半身实际姿态角度⁰θ_HB，通过正运动学(梶田秀司.仿人机器人[M].管贻生.北京：清华大学出版社，2007：44-47)计算机器人实际运动轨迹，包括

①机器人上半身坐标系Σ_Body下两踝实际位置向量^Wp_LeftAnkle、^Wp_RightAnkle；

②机器人上半身坐标系Σ_Body下两髋连线中点的实际位置向量^Wp_Hip；

③机器人上半身坐标系Σ_Body下两髋连线中点的实际姿态角度^Wθ_Hip；

④机器人双足实际支撑状态S，取值包括LS、RS、DS；

步骤(7)，获取空间二阶倒立摆各关节角度的实际值

依据机器人实际运动轨迹①～④，通过逆运动学(同步骤(2)中的方法)转化为二阶倒立摆模型四个关节角度的实际值q＝[q₁ q₂ q₃ q₄]^T。

步骤(8)，由IMU获取角速度测量值^IMUω_IMU。

步骤(9)，各轴角速度坐标变换

由于IMU测量的角速度是相对于自身坐标系Σ_IMU的矢量，因此需将其转化到空间二阶倒立摆基坐标系Σ₀下：⁰ω_IMU＝⁰R_HB·^IMUω_IMU，⁰ω_IMU为基坐标系Σ₀下IMU角速度测量值，等价于空间二阶倒立摆模型连杆HB的实际旋转速度⁰ω_HB。

步骤(10)，转化为空间二阶倒立摆模型各关节角速度的实际值

空间二阶倒立摆模型连杆HB的实际旋转速度⁰ω_HB与关节实际角速度的关系为J为雅可比矩阵(梶田秀司.仿人机器人[M].管贻生.北京：清华大学出版社，2007：54-56)，由此得到空间二阶倒立摆模型各关节角速度的实际值其中， J⁺为雅可比矩阵J的伪逆(因为J为3×4的矩阵，逆不存在，此处使用伪逆，可由MatLab 软件中pinv命令直接求出J⁺)。

步骤(11)，转化为空间二阶倒立摆模型各关节角速度的参考值

同步骤(10)，将转化为空间二阶倒立摆模型各关节角速度的参考值

步骤(12)，反馈线性化控制

基于反馈线性化方法构建二阶倒立摆模型关节力矩控制器，输入为关节参考角度q^ref、实际角度q、参考角速度实际角速度输出为模型各关节参考力矩所述关节力矩控制器包括关节位置-速度反馈控制器与关节力矩动力学计算两部分。

首先构建关节角度、角速度线性控制器(PD，位置-速度反馈控制器)：其中K_P为PD控制器位置项反馈系数，K_D为速度项反馈系数，输入为关节角度误差q-q^ref与角速度误差输出为关节参考角加速度

然后，依据动力学方程计算关节力矩：其中M为空间二阶倒立摆模型惯性矩阵，H为重力-科氏力矩阵(John J.Craig.机器人学导论[M].贠超等.北京：机械工业出版社，2006：143-144)。

步骤(13)，力矩限幅

由于实际双足机器人关节输出力矩有限，映射到空间二阶倒立摆模型各关节上，同样存在最大输出力矩(绝对值)有所限制的问题。所以，本实施例中对参考力矩获τ^ref进行限幅以获取实际可行的关节力矩；力矩限幅器输入为参考力矩τ^ref，输出为实际执行力矩τ^*，如下所示：

步骤(14)，动力学模型计算

依据步骤(13)得到的实际执行力矩τ^*，结合空间二阶倒立摆模型的动力学方程计算空间二阶倒立摆模型关节角加速度积分得到模型各关节修正角度

步骤(15)，更新机器人参考轨迹

根据根据空间二阶倒立摆模型各关节修正角度q^*、模型与机器人的映射关系以及正运动学(梶田秀司.仿人机器人[M].管贻生.北京：清华大学出版社，2007：44-47)，更新机器人参考轨迹(机器人两踝运动轨迹保持不变)，实现姿态控制，包括：

①世界坐标系下机器人两髋连线中点的位置向量修正值

其中，PositionKinematics(q^*)代表空间二阶倒立摆模型正运动学位置计算部分。

②世界坐标系下机器人两髋连线中点的姿态角度修正值

其中，PostureKinematics(q^*)代表空间二阶倒立摆模型正运动学姿态计算部分。

以上所述对本发明进行了简单说明，只要采取本发明思路和工作方法进行简单修改，或在不改变本发明主要构思原理下做出改进和润饰的等行为，均在本发明的保护范围之内。

Claims (5)

1.一种基于空间二阶倒立摆模型的双足机器人姿态控制方法，其特征在于，获取空间二阶倒立摆模型各关节角度的参考值和实际值以及各关节角速度的参考值和实际值，输入关节力矩控制器，再根据关节力矩限幅，获取实际力矩，从而得到关节修正角度q^*，更新机器人髋部参考轨迹，实现姿态控制。

2.根据权利要求1所述的基于空间二阶倒立摆模型的双足机器人姿态控制方法，其特征在于，所述空间二阶倒立摆模型包括踝部两个关节、髋部两个关节。

3.根据权利要求2所述的基于空间二阶倒立摆模型的双足机器人姿态控制方法，其特征在于，所述踝部两个关节旋转轴相互垂直、髋部两个关节旋转轴相互垂直，使得连杆HB与连杆AH在三维空间内复合运动。

4.根据权利要求1所述的基于空间二阶倒立摆模型的双足机器人姿态控制方法，其特征在于，所述各关节角速度的参考值为：其中J⁺为雅可比矩阵J的伪逆，为空间二阶倒立摆模型连杆HB绕基坐标各坐标轴的参考旋转速度。

5.根据权利要求4所述的基于空间二阶倒立摆模型的双足机器人姿态控制方法，其特征在于，所述参考旋转速度与绕空间二阶倒立摆模型基坐标系Σ₀各坐标轴的旋转速度之间的关系为：

且其中为参考旋转矩阵微分，⁰R_HB为上半身相对基坐标系Σ₀的旋转矩阵，为参考姿态角速度，⁰θ_HB,p、⁰θ_HB,r为实际姿态角度。