CN110405769A - 一种启发式仿人机器人摔倒保护方法 - Google Patents

Abstract

本发明公开了一种启发式仿人机器人摔倒保护方法，工控机通过传感器获取的信息监测机器人自身状态，并与状态期望值进行对比；若监测到机器人自身状态与期望值的差值大于阈值，则机器人处于不平衡的状态，工控机实施摔倒保护策略，将机器人摔倒冲击减小到最小，机器人在工控机的控制下逐渐稳定，并最终停止运动。本发明获取的机器人摔倒时各关节轨迹，以此减小机器人摔倒时的冲击，保护机器人；本发明可根据机器人的不同摔倒状态进行调整，以得到最适合当前摔倒情况的保护策略，使摔倒保护具备对不同情况的适应性。

Description

一种启发式仿人机器人摔倒保护方法

技术领域

本发明属于仿人机器人技术领域，具体涉及一种启发式仿人机器人摔倒保护方法。

背景技术

仿人机器人在外形与动作行为上都与人类有很高的相似性，因此，仿人机器人与人类一样，拥有极强的环境适应能力，能通过双足与双臂的配合，完成很多复杂的作业。人类在摔倒时，可以很快地做出反应来控制自身摔倒的姿态以及与地面接触的部位，来将摔倒伤害在可能的范围内控制到最小，而仿人机器人重心较高、自身自由度较多而且双足的支撑面积较小，这些特点使得仿人机器人不稳定，在行走或其他作业过程中更容易发生摔倒。因此，对仿人机器人的摔倒保护是现在的一个亟待解决的问题。

现有技术提出一种利用充气背包来保护仿人机器人的方法，当机器人失去平衡时，通过CO₂气体的快速充入空气背包，可以吸收摔倒之后的大部分冲击，但这种被动的方法通用性不强，难以适应不同的摔倒情况。现有技术还通过QP算法在线调节关节刚度，使手臂在与地面发生碰撞时具有一定的柔性，能够吸收摔倒碰撞的一部分冲击。然而，大型仿人机器人的关节一般采用谐波减速器作为传动部件，具有很大的减速比，即使降低了关节的PD控制参数，关节刚度依然会比较大，这种摔倒保护方式可能会对机器人手臂关节造成较大的伤害。

现有仿人机器人的摔倒保护策略大多处于仿真阶段或小型机器人的实验验证阶段，少数进行了大型仿人机器人摔倒保护实验的方法有着通用性不强，保护不完善等问题。

发明内容

为了解决现有技术中存在的问题，本发明设计了一种启发式仿人机器人摔倒保护方法，在碰撞前通过一系列摔倒保护动作降低重心，将摔倒冲击减到最小。

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

一种启发式仿人机器人摔倒保护方法，工控机通过传感器获取的信息监测机器人自身状态，并与状态期望值进行对比；若监测到机器人自身状态与期望值的差值大于阈值，则机器人处于不平衡的状态，工控机实施摔倒保护策略，将机器人摔倒冲击减小到最小，机器人在工控机的控制下逐渐稳定，并最终停止运动。

进一步，所述摔倒保护策略具体为：将机器人的摔倒过程划分为三个阶段：空中运动阶段、碰撞及调整阶段与摔倒结束阶段；建立不同摔倒阶段的状态方程，优化摔到运动，获取机器人最优摔到运动轨迹。

更进一步，所述状态方程的建立，其参数包括动力学方程中的参数，所述动力学方程中的参数利用当前摔倒状态的机器人的总动能K与总势能P、拉格朗日函数与动力学方程联立求解。

进一步，所述优化摔到运动包括设计约束条件与代价函数。

更进一步，所述代价函数以减小机器人在碰撞时所受到的冲击为目的，具体为：

所述其中K₁和K₂为权重， ^为机器人碰撞时的动量，B′是将碰撞力转换广义力的转换坐标，D(θ(t_i))为三个连杆的惯性矩阵，为碰撞过程中角动量，M为连杆质心质量，r为连杆质心到关节端的距离，v为连杆质心速度，I为连杆转动惯量，ω为连杆角速度；

所述 为膝盖速度关于三个杆件的雅克比矩阵的逆，为臀部速度关于三个杆件的雅克比矩阵的逆。

本发明的有益效果为：

(1)本发明将机器人建模为三连杆，以此来计算其状态方程与动能、势能，并结合代价函数来获得机器人摔倒冲击最小时的状态方程参数，从而优化摔倒算法，将摔倒伤害进一步减小。

(2)本发明将机器人的摔倒分为三个阶段的四段不同运动，并根据机器人摔倒运动的方向不同规划不同的代价函数，完成机器人摔倒运动算法的优化。

(3)本发明基于机器人的状态方程，通过设立机器人的关节活动范围、关节最大转矩等约束条件，以及冲击动量为代价函数，用非线性系统的多阶段优化方法得到机器人摔倒时各关节轨迹，以此减小机器人摔倒时的冲击，保护机器人。

(4)本发明可根据机器人的不同摔倒状态进行调整，以得到最适合当前摔倒情况的保护策略，使摔倒保护具备对不同情况的适应性。

附图说明

图1为本发明一种启发式仿人机器人摔倒保护方法流程图；

图2为机器人的摔倒过程示意图，图2(a)为机器人摔倒时在空中运动阶段示意图，图2(b)为机器人摔倒碰撞及调整阶段示意图，图2(c)为机器人摔倒结束阶段示意图。

具体实施方式

下面将结合说明书附图和具体实例进一步阐述本发明的技术方案，但是本发明的保护范围并不限于此。

如图1所示，一种启发式仿人机器人摔倒保护方法，包括步骤：

步骤(1)，仿人机器人通过自身IMU(惯性测量单元，安装在机器人腰部)采集身体姿态、六维力传感器(安装在脚底)采集脚底的受力情况、关节电机码盘采集关节角度，采集的信息发送给工控机，工控机时刻监测自身ZMP(零力矩点)、身体姿态等状态，并与状态期望值进行对比；若监测到机器人自身状态与期望值的差值在阈值范围内，则机器人处于平衡状态，不做出反应；若监测到机器人自身状态与期望值的差值大于阈值，则机器人处于不平衡的状态，执行步骤(2)。

步骤(2)，若机器人处于不平衡状态(即将摔倒)，则通过工控机实施摔倒保护策略，将机器人摔倒冲击减小到最小。

所述摔倒保护策略具体为：

将仿人机器人建立为三阶倒立摆模型，包括三个相互连接的杆件，即小腿、大腿、上身三个连杆；

1)根据摔倒时的状态，构建机器人动力学方程，通过拉格朗日动力学方程，可得出机器人动力学方程为(John J.Craig.机器人学导论[M].贠超等.北京：机械工业出版社，2006：143)：

其中，D(θ)为三个连杆的惯性矩阵，G(θ)为重力项，为科氏力与离心力的合力项，B(θ)为输入的系数矩阵，其中因此可得q₁、q₂为膝关节、髋关节的运动角度，θ₁、θ₂、θ₃为各连杆与竖直方向的夹角，u为膝关节与髋关节的输入力矩。

2)根据工控机监测的机器人状态，计算摔倒过程中的动能与势能变化；由图2(a)可得机器人总动能K为：

K＝K₁+K₂+K₃ (2)

其中K₁、K₂、K₃为小腿、大腿、上半身连杆的动能，且：

机器人总势能为：

P＝M₁gL₁cos(θ₁)+M₂g(L₁cos(θ₁)+L₂cos(θ₂))+M₃g(L₂cos(θ₂)+L₃cos(θ₃)+L₁cos(θ₁)) (6)

其中M₁、M₂、M₃为三个杆件的质量，L₁、L₂、L₃为三个杆件的长度，I₁、I₂、I₃为各杆件的转动惯量。

根据机器人拉格朗日函数其中机器人的广义力Γ＝B(θ)u，为拉格朗日函数、为动能、P(θ)为势能，将机器人总动能K与总势能P带入拉格朗日函数，联立公式(1)，可求解出D(θ)、G(θ)、用于建立状态方程。

3)如图2(b)所示，根据机器人摔倒碰撞及调整阶段模型，计算机器人碰撞时动量的变化，设碰撞发生在t时刻，碰撞时间极短，对机器人动力学方程求积分可得：

其中δF为作用在碰撞点p的碰撞力，B′是将碰撞力δF转换广义力的转换坐标，且由于该碰撞为完全非弹性碰撞，只改变速度而不改变机器人的构型，因此各关节角度在碰撞前后不变，式(7)通过计算，可变为：

其中的“+”与“-”符号分别表示碰撞后与碰撞前的状态，F为碰撞过程的动量变化。由于个各关节角速度与膝盖的速度v_knee之间的关系可用雅克比矩阵表示为：

将此关系代入式(8)，以此得到机器人碰撞时的动量变化：

其中：J_knee为膝盖速度关于三个杆件的雅克比矩阵，表示碰撞前的速度，根据式(11)所列出的关系，通过控制θ和调整F的大小，从而调节仿人机器人在碰撞时的状态。

4)根据机器人动力学方程，将机器人的摔倒过程划分为四个阶段：准备阶段、空中自由运动阶段、碰撞及调整阶段与摔倒结束阶段

如图2所示，除去机器人在监测到摔倒前的准备阶段，把四段运动简化为三个阶段，第一阶段为机器人摔倒时在空中自由运动的阶段(图2(a))，第二阶段为机器人摔倒碰撞及调整阶段(图2(b))，根据机器人摔倒方向(向前、向后)的不同可分为两种不同的碰撞及调整阶段，第三阶段为机器人摔倒结束后的调整阶段(图2(c))。

5)根据不同阶段建立状态方程，得出机器人摔倒过程的初步运动规划

每一个阶段都可以用动力学方程描述，将关节力矩作为机器人的输入力矩u，关节角度和角速度作为机器人的状态量x，根据动力学方程建立运动的状态方程：

将机器人动力学方程式(1)代入式(12)，可得：

摔倒第一阶段(p＝1)的状态方程为：

向前摔倒的第二阶段(p＝2)的状态方程为：

向后摔倒的第二阶段(p＝3)的状态方程为：

第三阶段主要包括了机器人在摔倒稳定后，进行起身或者爬行的运动。

6)在建立了状态方程后，可通过不同阶段的约束条件与代价函数，利用非线性系统的多阶段优化方法来对机器人摔倒运动进行优化，得到最优的摔倒运动过程规划，从而获得机器人的摔倒运动轨迹；其中，约束条件通过仿人机器人自身的力学与运动学限制以及摔倒过程的运动规律来获取，例如关节最大力矩、关节活动范围和电机功率限制等，代价函数则是以减小机器人在碰撞时所受到的冲击为目的而设计的函数：

对于第一阶段，由于向前摔倒时机器人为膝盖先着地，向后摔倒时为臀部先着地，因此根据两种情况分别设计代价函数，根据公式(11)，设计代价函数使机器人碰撞过程中动量最小，以使机器人在与地面碰撞时冲击力最小：

对于第二阶段，为使机器人碰撞过程中角动量与动量尽可能的小，利用权重K₁和K₂，根据公式(11)和机器人的角动量设计代价函数：

其中：为碰撞过程中角动量，r为连杆质心到关节端的距离，v为连杆质心速度，ω为连杆角速度。

7)在向前摔倒时，膝部率先着地减小机器人速度，再由手臂部分着地来稳定机器人；在向后摔倒时，臀部率先着地，再由背部着地来最终稳定机器人，两种摔倒运动情况中，机器人的腿部根据最优摔倒过程运动规划的轨迹来运动，尽量减小碰撞时所受到的冲击。

步骤(3)，机器人在工控机的控制下逐渐稳定，并最终停止运动。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化和修改，并不影响本发明的实质内容。在不冲突的情况下，本申请实施例中的特征可以任意相互组合。

Claims (5)

1.一种启发式仿人机器人摔倒保护方法，其特征在于，工控机通过传感器获取的信息监测机器人自身状态，并与状态期望值进行对比；若监测到机器人自身状态与期望值的差值大于阈值，则机器人处于不平衡的状态，工控机实施摔倒保护策略，将机器人摔倒冲击减小到最小，机器人在工控机的控制下逐渐稳定，并最终停止运动。

2.根据权利要求1所述的启发式仿人机器人摔倒保护方法，其特征在于，所述摔倒保护策略具体为：将机器人的摔倒过程划分为三个阶段：空中运动阶段、碰撞及调整阶段与摔倒结束阶段；建立不同摔倒阶段的状态方程，优化摔倒运动，获取机器人最优摔倒运动轨迹。

3.根据权利要求2所述的启发式仿人机器人摔倒保护方法，其特征在于，所述状态方程的建立，其参数包括动力学方程中的参数，所述动力学方程中的参数利用当前摔倒状态的机器人的总动能K与总势能P、拉格朗日函数与动力学方程联立求解。

4.根据权利要求2所述的启发式仿人机器人摔倒保护方法，其特征在于，所述优化摔到运动包括设计约束条件与代价函数。

5.根据权利要求4所述的启发式仿人机器人摔倒保护方法，其特征在于，所述代价函数以减小机器人在碰撞时所受到的冲击为目的，具体为：

所述其中K₁和K₂为权重，为机器人碰撞时的动量，B′是将碰撞力转换广义力的转换坐标，D(θ(t_i))为三个连杆的惯性矩阵，为碰撞过程中角动量，M为连杆质心质量，r为连杆质心到关节端的距离，v为连杆质心速度，I为连杆转动惯量，ω为连杆角速度；

所述 为膝盖速度关于三个杆件的雅克比矩阵的逆，为臀部速度关于三个杆件的雅克比矩阵的逆。