基于线性预测位置和速度控制器的全方向轮式人形机器人的制作方法

本发明涉及机器人编程系统领域。更具体地讲，本发明适用于对通过有关节的肢体来回移动或者使用有关节的肢体的机器人(特别是具有人形或动物形的机器人)的移动的控制。当机器人具有某些人类外观属性(头、躯干、双臂、双手等)时，可以将机器人看作人形机。然而，人形机器人可能多少有些复杂。其肢体可以具有更多或更少数量的关节。其可以静态和动态地控制其自己的平衡，并且可以在基座上转动。其可以从环境中拾取(“听”、“看”、“接触”、“感觉”等)信号，并且根据多少有些复杂的行动做出反应，并且通过话音或手势与其它机器人或人类进行交互。对于当前一代人形机器人，程序员能够创建场景，场景可能多少有些复杂，作为机器人加以反应/执行的事件/动作的序列。可以根据与机器人交互的人的某些行动来调节这些动作。但是在这些第一代的人形机器人中，应用程序编程是在开发工具包中完成的，并且需要通过触发事件来启动每个应用，触发事件的发生已经被包括在应用中。

因此，存在对这样的人形机器人的需求：它们能够像人类那样过“自主生活”，其能够依据其所处环境、按确定的方式行动。本发明的目标是，通过提供能够自主确定其适合于其所处背景的生活的序列而无需程序员的任何干预的机器人，来克服现有技术机器人的局限性。

背景技术：

我们考虑具有连结于身体(也称为上身)的移动基座的机器人。

在标准行动中，机器人与其所有轮子相接触，并且当作为作用于地面与机器人之间的力的重心的压力中心(CoP)严格处于其轮子接触点所限定的凸形外壳内部时，机器人动态稳定。在无干扰的情况下，始终遵守该CoP约束。然而，当干扰出现时，CoP可能处于支撑外壳的边界上，在该情况下，机器人可能开始倾斜，并且如果不予理睬，它可能摔倒。因此，我们的问题是，关于两种动态模型控制机器人：当机器人未倾斜时，以及当其倾斜时。

当机器人未倾斜时，我们可以发现有关利用动态稳定约束来控制移动机器人、或者有关控制人形双腿机器人的一些论文。

一些最近的工作涉及利用由诸如机械臂等肢体所导致的动态约束对机器人进行控制。K.Mingeuk等人致力于使用动态约束的轮式平台的稳定性的研究：“Stabilization of a rapid four-wheeled mobile platform using the zmp stabilization method”。他们使用直接线性二次调节器(LQR)方法来控制平台。该方法的不便之处在于，所给出的动态约束要求具有平台中央的CoP(压力中心)。CoP是机器人与地之间的接触力的重心。该方法需要丧失若干DoF(自由度)：实际上，为了防止机器人摔倒，CoP仅需处于由轮子与地面之间的接触点所限定的凸多边形中。

在另一篇论文中，Y.Li等人介绍了具有动态约束的移动机器人的简单控制器：“The dynamic stability criterion on the wheel-based humanoid robot based on zmp modeling”。与K.Mingeuk等人的公布文本的不同之处在于，其考虑了全CoP约束，全CoP约束是不等式约束之总和。该控制器是对机器人的完整模型进行迭代的PID控制，以找到其中CoP处于支撑多边形中的扭矩命令。

对于人形机器人，P.B.Wieber、H.Diedam和A.Herdt描述了控制具有高度约束的动态的人形双腿机器人的方法：“Walking without thinking about it”。该最近的方案涉及基于3d线性倒立摆模型的线性预测控制。使用了简单的机器人模型，该控制法则预测未来机器人状态的动态，以确保发送到机器人的当前命令不会在几秒钟内导致不可避免的摔倒。关于双足人形机器人NAO，该控制法则的实施可以在D.Gouaillier、C.Collette和K.Kilner所写的论文中找到：“Omni-directional closed-loop walk for NAO”。但机器人NAO很小，并且对于图1中所示的较高的人形机器人，该方法显然不会产生好的结果，例如图1中所示的较高的人形机器人具有下列特征：

-20个自由度(DoF)(头部160上的2个DoF、臂部170上的2×6个DoF、腿部180上的3个DoF以及移动基座140中的3个DoF)，

-1.37m高度110，

-0.65m宽度130，

-0.40m深度120,

-30kg总质量，

-一条腿180，其关联到具有三个轮子141的全方向基座140。移动基座具有0.422m长的三角形形状，并且能够以1:4m/s^-1的最大速度和短时间内的1:7m/s^-2的加速度移动机器人。额定速度和加速度分别为0:5m/s^-1和1:0m/s^-2。

对于该情况，当机器人倾斜时，我们可以将该问题与对有一个或两个轮子的移动机器人的控制联系起来。然而，在我们的情况下，我们不希望像所有的有一个或两个轮子的移动机器人那样围绕不稳定平衡方向(通常为垂直方向)控制机器人。可以找到许多有关控制这些机器人的论文。它们基于不同类型的控制，例如：

-PID力控制，如J.K.Ahn和S.J.Lee的公布文本“Force control application to a mobile manipulator with balancing machanism”(2010)中所描述的，

-线性二次调节器，如X.Changkai、L.Ming、以及P.Fangyu的公布文本“The system design and lqr control of a two-wheels self-balancing mobile robot”(2011)中所描述的，或者

-滑动模式控制，如K.Sho-Tsung、C.Wan-Jung、以及H.Ming-Tzu的公布文本“Balancing of a spherical inverted pendulum with an moni-direnctional mobile robot”(IEEE Multi-conference on Systems and Control,2013)中所描述的。

所有这些控制器具有依据重力和外部力来围绕不稳定平衡方向控制机器人的目标。对于人形机器人，我们能够找到有关推挤恢复的论文。主要构思是管理控制的四种可能性：

-使用上身移动CoP，

-使用脚踝创建扭矩，

-使用手臂创建一些角动量，以及

-走几步。

它们不适合我们的情况，因为它们很大程度地依赖于双足机构，这意味着脚不能够在地面上转动或者滑动，而且不能够通过走几步来从推挤中恢复过来。在我们的情况中，不能使CoP移位，因为在倾斜方向上无接触表面。我们的问题还有另一个困难，即，要考虑动态模型的离散转换(一些接触力的存在或不存在)。这意味着，可以出现一些碰撞。通常，对于实时控制，对碰撞进行管理是一个难题。

因此，存在着这样的需求：控制人形机器人的移动基座及其身体二者，同时考虑机器人非常可能受到干扰并且可能围绕其轮子倾斜的它们的动态约束。

技术实现要素：

因此，替代提出一种将可用于不同的可能动态状态的独特控制器，我们定义了两个控制器，一个是当机器人未倾斜时的控制器，另一个是当机器人倾斜时的控制器。为了确保两个控制器之间的正确且平滑的过渡，定义了第三控制器来处理着陆阶段，并且定义了监控器来相应地引导它们。

为此，本发明提供了身体连结到全方向移动地面基座的人形机器人，其装备有：

-提供测量结果的身体位置传感器、基座位置传感器以及角速度传感器，

-致动器，包括位于全方向移动基座中的关节电动机和至少3个轮子，

-提取器，用于将感测的测量结果转换成有用数据，

-监控器，用于根据有用数据来计算位置、速度以及加速度命令，

-用于将命令转换成用于致动器的指令的单元。

其主要特征在于，监控器包括：

-未倾斜状态控制器、倾斜状态控制器以及着陆状态控制器，每个控制器包括用于计算的单元，其基于具有倾斜移动的双点质量机器人模型和线性模型预测控制法则根据有用数据、预排序的位置和速度参照、以及设置为0的倾斜角和角速度参照来计算位置、速度和加速度命令，线性模型预测控制法则具有根据预测的样本的取样时间周期T和数目N的离散化时间并且被表示为具有目标的加权总和以及预先定义的线性约束组的二次最优化公式，

-碰撞角速度和着陆碰撞时间估计器，以及

-用于根据估计的碰撞角速度和有用角速度来选择控制器的单元。

该机器人的优点在于对诸多类型的干扰是鲁棒的。所述干扰可以是具有大动态带宽的短、长、快或多重干扰。我们未对这些干扰进行假设。此外，在不能从中恢复的极强干扰的情况下，对机器人实施摔倒管理器策略，以最小化身体与地面的碰撞。

更确切地讲，该机器人的优点首先是能够从使机器人倾斜的强干扰中恢复过来。第二个优点是，以简单且实时的方式控制具有离散的变化和碰撞的非线性动态。第三个优点是，能够通过调谐监控器的参数，例如在倾斜恢复期间选择更多地极限机械碰撞，或者选择极限行进距离，来定义机器人的诸多行动。

根据本发明的实施例，通过常数的积分来计算所估计的碰撞角速度。

在机器人模型中可以无倾斜；例如，未倾斜控制器能够根据有用数据、使用预排序的参照、以如下目标为目标、以如下约束为约束来计算位置、速度以及加速度命令，所述目标为：

-基座位置目标，

-基座速度目标，

-与CoP和基座中心之间的距离相关的目标，CoP是机器人和地面之间的接触力的重心，

所述约束为：

-移动基座的最大速度和加速度，

-身体的运动极限，

-CoP极限。

倾斜控制器能够根据有用数据、使用预排序的参照和设置为0的倾斜角以及角速度参照、并且以倾斜角最小化和角速度最小化为目标、以移动基座的运动极限、身体的运动极限、正倾斜角和身体的移动仅在角速度轴上为约束，来计算位置、速度以及加速度命令。

可以将加权的数值稳定性目标添加到目标的加权总和。

着陆控制器能够根据有用数据、使用预排序的参照和设置为0的倾斜角以及角速度参照、以与CoP和基座中心之间的距离相关的目标(CoP为机器人与地面之间的接触力的重心)和数值稳定性目标为目标、并且以移动基座的最大速度和加速度、身体的运动极限和CoP极限、以及身体的移动仅在角速度轴上为约束，来计算位置、速度以及加速度命令。

可以将基座速度目标添加到这些目标的加权总和。

身体的运动极限可以为空。

根据本发明的实施例，至少一个轮子为全方向的。

有利地，地面为平坦且水平的。

本发明还提供了用于对身体连结到全方向移动地面基座的人形机器人进行控制的方法，所述方法按预先定义的取样时间来实施，并且包括：

-检索身体的位置测量结果、基座的位置测量结果、机器人的倾斜角以及机器人的角速度测量结果，

-将这些测量结果转换成有用数据，

-使用有用数据，并且根据机器人的状态，使用基于线性模型预测控制法则的控制法则来计算位置、速度以及角速度命令，其中线性模型预测控制法则具有根据预测的样本的取样时间周期和数目的离散化时间，并且被表示为具有带有预先定义的权重的目标的加权总和以及线性约束组的二次最优化公式。

-将这些命令转换成用于机器人致动器的指令。

可以根据下列步骤定义机器人的状态：

如果倾斜角测量结果和/或角速度测量结果之一大于零，则对估计的碰撞角速度和估计的碰撞时间进行估计，

最初，机器人处于未倾斜状态，

未倾斜状态：

·如果不存在估计的碰撞时间，则切换至倾斜状态；

·如果碰撞角速度高于预先定义的极限1，则切换至倾斜状态；

·如果测量的角速度高于预先定义的极限2，则切换至倾斜状态；

·否则，如果倾斜角不为空，则切换至着陆状态，

如果倾斜角为空，则停留在未倾斜状态。

倾斜状态：

·如果测量的角速度高于预先定义的极限2，则停留在倾斜状态；

·如果估计的碰撞速度低于预先定义的极限1，则切换至着陆状态；

·否则，停留在倾斜状态。

着陆状态：

·如果不存在估计的碰撞时间，则切换至倾斜状态；

·如果碰撞角速度高于预先定义的极限1，则切换至倾斜状态；

·如果测量的角速度高于预先定义的极限2，则切换至倾斜状态；

·如果倾斜角降至0，并且如果下一次测量的角速度为空，则切换至未倾斜状态，否则，停留在着陆状态。

本发明还提供了一种计算机程序，其包括计算机代码，计算机代码适合于当在计算机上运行时执行所描述的方法。

附图说明

根据以下对多个示范性实施例及其附图的描述，将更好地理解本发明并且其各种特征和优点将显露出来，在附图中：

-图1显示本发明的若干实施例中的人形机器人的物理架构；

-图2显示本发明的若干实施例中的人形机器人的软件模块的功能架构；

-图3显示根据本发明设计的机器人的质量再分配的模型化；

-图4显示根据本发明的机器人的倾斜动态的模型化；

-图5图示了约束的几何表示；

-图6示意性地表示监控器的功能方案。

在各图之间，以相同的附图标记来称呼相同的元素。

具体实施方式

图1显示本发明的若干实施例中的人形机器人的物理架构。

图中的具体机器人100仅被看作其中能够实施本发明的人形机器人的示例。图中的机器人的下肢180不用于行走，而是能够在其基座140上沿任何方向移动，基座140在其所处表面上转动。例如，该机器人具有可以是大约120cm的高度110、大约65cm的深度120以及大约40cm的宽度130。在具体实施例中，本发明的机器人具有平板电脑150，机器人可以利用平板电脑150向其环境传递消息(音频、视频、网页)，或者通过平板电脑的触觉界面从用户接收输入。除了平板电脑的处理器外，本发明的机器人还使用其自己母板的处理器，例如，母板的处理器可以是来自Intel^TM的ATOM^TM Z530。在本发明的具体实施例中，本发明的机器人有利地还包括专用于处理母板与致动器之间的数据流的处理器，致动器控制肢体中的关节的电动机和机器人用作轮子的球。电动机可以具有不同类型，取决于明确的关节所需的最大扭矩的量级。例如，可以使用来自e-minebea^TM的有刷DC无核电动机(例如SE24P2CTCA)、或者来自Maxon^TM的无刷DC电动机(例如EC45_70W)。MRE是优选类型，其使用霍尔效应，具有12或14比特的精度。

在本发明的实施例中，图1中所示的机器人还包括各种种类的传感器。其中的一些用于控制机器人的位置和移动。例如，这是位于机器人的躯干中的惯性单元包括3轴陀螺测试仪和3轴加速度计的情况。机器人还可以包括位于机器人的前额(顶部和底部)上的芯片上系统(SOC)类型的两部2D彩色RGB照相机，例如来自Shenzen V-Vision Technology Ltd^TM的(OV5640)的照相机，其具有每秒5帧下的5兆像素分辨率、以及大约57°水平和44°垂直的视场(FOV)。也可以将一个3D传感器包括在机器人的眼睛后方，例如，所述3D传感器为具有每秒20帧下的0.3兆像素的分辨率、具有与2D照相机大约相同的FOV的ASUS XTION^TM SOC传感器。本发明的机器人还可以配备有激光线生成器，例如三个位于头部、三个位于基座，从而能够感测其相对其环境中的物体/人的相对位置。本发明的机器人还可以包括麦克风，其能够感测机器人的环境中的声音。在实施例中，可以将四个具有1kHz下的300mV/Pa+/-3dB的灵敏度和300Hz到12kHz的频率范围(-10dB相当于1kHz)的麦克风植入机器人的头部。本发明的机器人还可以包括两个太阳能传感器，可能位于其基座的前面或背后，以测量至其环境中的物体/人的距离。机器人还可以包括位于其头部和其手上的触觉传感器，以允许与人类进行交互。

为了翻译其情感并与其环境中的人类进行交流，本发明的机器人还可以包括：

-LED，例如位于眼睛、耳朵中，以及位于其肩膀上；

-扬声器，例如两个，位于其耳朵中。

本发明的机器人可以通过以太网RJ45或WiFi 802.11连接与基站或其它机器人通信。

可以通过具有大约400Wh的能量的锂离子磷酸电池为本发明的机器人供电。机器人可以接入适合于其包括的电池的类型的充电站。

就传感器的测量结果而言，机器人的电动机可以使用激活每个肢体定义的链和在每个肢体的末端定义的受动器的算法来控制机器人的位置/移动。

图2显示本发明的若干实施例中的人形机器人的软件模块的功能架构。

本发明的目的是提供一种方法，所述方法允许人形机器人按自主方式执行活动，而无需程序员的任何干预以预期机器人将面临的条件。在现有技术中，机器人能够执行已经被编程并且已经上载到其母板的场景。这些场景可以包括对在其环境中改变的条件的反应，但机器人将不能对还未预测到且未包括在被上载到其母板或远距离访问的代码中的条件做出反应。相比之下，本发明的目的是允许机器人自主行动，甚至是就程序员还未预测到的事件/条件而言。通过图2中所示的功能架构实现了该目的。

实际上，该功能架构包含四个主要软件模块。

服务模块210包括至少三种类型的服务：

-提取器服务211，其从结合图1所描述的类型的机器人传感器接收读数作为输入；对这些传感器读数进行预处理，以便提取出与机器人的位置、在其环境中的物体/人类的辨识、距所述物体/人类的距离、人类说出的单词或者他们的情感有关的相关数据(也称为有用数据)；提取器服务的示例是：感知机器人附近人类的存在的人感知，检测这些人类的移动的移动检测；为声音定位的声音定位；解释对机器人触觉传感器的触摸的触摸检测；通过其话语或手势来辨识机器人附近的人类所表达的情感的话音识别、情感识别；

-致动器服务212，其控制机器人的物理动作，例如：激活关节或基座的电动机的移动，跟随机器人环境中的人类的移动的跟踪器，点亮机器人的LED以交流情感，动画话音(话音与手势的组合)，行动；行动是可以表达机器人情感并且允许其执行复杂动作的移动、话语、点亮的组合；

-系统服务213，其主要包括数据服务；数据服务提供，瞬时的或长期的存储数据；数据服务的示例包括：

○用户会话服务，其存储用户数据，以及他们对机器人所做的事的历史；

○包管理器服务，其提供对由机器人执行的规程、以及该规程的高级别定义、启动条件和标签的可放缩存储。

活动模块220包括机器人的已经被预先编程的行动221。可以使用图形集成开发环境(Choregaphe^TM)实现对行动的编程，这是以n° EP2435216公布、被转让给本专利申请的申请人的欧洲专利申请的目标。用Choregaphe^TM编程的行动将基于时间的逻辑与基于事件的逻辑组合。用清单222标记每个行动，清单222是主要包括行动的启动条件的文本文件。这些启动条件基于提取器211能够感知的东西。

意愿模块230负责选择一个或几个要启动的活动。为了实现这一点，意愿模块预订提取器并且评估所有安装的活动的启动条件。这些条件的变量基于事件。因此，有效的做法是，仅需要重新评估包含改变的变量的条件语句。基于分类算法的结果、其优先级、以及生命周期(见下)，可以启动活动，并且可能停止一些活动。

执行的活动将依赖于对服务的API(法文表示“Application Pour Interface”的首字母缩略语)调用，以执行其已经被编程为要执行的任务。每当活动正在进行或者被停止，意愿模块收集有关该活动何时发生的数据、条件的当前状态、以及看起来有助于学习的用户反馈的内容。

如果对于这些条件现有的提取器事件是不够的，则开发商可以创建附加的提取器，以生成新的事件，并且将其分配到具有他们的应用的包中。

为此，意愿模块230通过控制活动的选择、并且基于提取器的读数和在意愿模块调用的选择器中执行的算法来启动致动器，来将服务与活动模块绑定在一起。选择器的示例包括：

-自主生活231，其执行活动；基于情形的背景，意愿模块可以告诉自主生活要关注的活动(参见以下示例)；任何活动可以完全访问模块API的所有调用规程；活动可以包括将自主生活导向关注明确的活动的约束；

-基本意识232，其预订提取器服务，例如用于告知移动服务进行移动的人感知、移动检测、以及声音定位；意愿模块基于情形配置基本意识的行动；在其它时间，基本意识自己采取动作，或由运行的活动来配置；

-对话233，其预订话音识别提取器，并且使用动画话音致动器服务来讲话；基于情形的背景，意愿模块可以告诉对话要关注什么话题；清单中的元数据将该信息绑定到意愿模块；对话还具有其算法以用于管理会话，并且通常自己采取动作。

执行引擎240启动API调用，以唤起服务。

更具体地讲，本发明的目的是控制可能受到强烈干扰并且围绕其轮子倾斜的全方向轮式人形机器人。所述方法基于两个线性模型预测控制器，依据机器人的动态模型和第三控制器，以确保前两个控制器之间的正确且平滑的过渡。指定估计器检测机器人何时进入倾斜动态，或者其何时返回至其额定稳定动态。

使用：

-预排序的位置和速度参照、以及设置为0的倾斜角和角速度参照，

-从感测的测量结果提取的身体位置、基座位置和角速度数据、以及调用的有用数据，

每个控制器基于以下项来计算位置、速度以及加速度命令：

-双点质量机器人模型，其具有如图3和4中所示的倾斜移动。第一点质量b代表移动基座的质量中心(CoM)，并且第二点质量c代表上身的CoM；将基座140和身体(或上身)190之间的关节视为不具有质量。当机器人未倾斜时，上身和移动基座的CoM为cC和bC。倾斜角为ψ；h为cC的高度。存储该模型作为系统数据服务213。

-线性模型预测控制法则，其具有根据预测的样本的取样时间周期T和数目N的离散化时间，被表示为具有目标的加权总和以及预先定义的线性约束组的二次最优化公式。为了定义受控的身体和基座(c和b)的动态行动，我们必须首先选择预测的持续时间(水平线)和每个取样时刻之间的周期。选择尽可能短的水平线和尽可能大的周期，将减少计算时间，但也将降低控制的稳定性和鲁棒性，这取决于动态分类。为了保存系统的线性，我们为身体和基座轨迹选择3阶多项式类，以具有连续的CoP轨迹，以避免机器人中的力的峰值。在该控制器中，对时间进行取样，取样周期为T。预测的样本的数目为N。该种类的控制的另一个优点在于，可以简单地用若干不等式约束，例如机器人的运动极限、移动基座的最大速度和加速度以及CoP极限进行管理。存储该控制法则作为调用移动API的系统数据服务213。

计算位置、速度以及加速度命令需要控制机器人的倾斜角和角加速度。

第一控制器专用于机器人未倾斜时的情形：于是，倾斜角为空。

为了进行实时控制，机器人的模型需要尽可能简单。我们需要计算由地面上的基座作用的力的好的近似。单点质量模型不能完成机器人中的质量的再分配，因为质量的大约一半集中在基座中，其余部分集中在上身。

现在，我们可以针对该系统写出牛顿和欧拉方程，其中，轴z为纵轴，x和y为两个水平轴：

其中，mc和mb为分别关联到c和b的质量，和是每个点质量的角动量。对应于移动基座对上身作用的力，对应于地面对移动基座的三个轮子作用的力。另外，p为力的CoP，其为它们的重心。由于其定义，仅在由三个轮子的接触点代表的凸多边形中定义p。

在该模型中，我们直接考虑了c和b之间的动量(3)、(4)。这意味着我们忽略了臂产生的动量。我们之所以可以这样做是因为在我们的操作情况中，不存在快速移动。组合方程(1)、(2)、(3)、(4)，我们可以将系统的动态方程写为：

在方程(5)中我们可以注意到，两个质量模型的动态方程简单地是两个单一质量模型的总和。

现在，为了使方程(5)线性化，我们提出一些假设。首先，我们可以忽略总的角动量因为我们已经选择了仅考虑c和b之间的动量。其次，由于我们具有冗余的机器人，所以我们可以围绕轴x和y移动CoM，而不修改其在轴z上的位置。因此，我们考虑被约束在常数值的c^z；该假设对获得线性模型是重要的，但在没有该假设的情况下机器人仍可以在其基座上转动。此外，为了简化描述，我们优选考虑平坦且水平的地面，因此P^z＝0。不存在用于控制b^z的位置的DoF，我们可以将其设置为常数l。最后，我们可以注意到g^x＝g^y＝0和g^z＝g，重力范数。

使用这些假设和注意，我们可以重写方程(5)，如下：

现在，我们可以使用方程(6)来提供基座与身体的CoP和位置和加速度之间的线性关系：

我们可以注意到，该模型未考虑任何倾斜的可能性。因此，为了确保模型的有效性，必须将对CoP的约束添加到控制器，以确保CoP严格处于由机器人的三个轮子定义的凸多边形内：定义了鲁棒性标准。我们考虑其为每个轮子与地面的接触点所代表的凸多边形；图5中描述了该D多边形的示例。通过定义，我们始终有为了具有旋转不变的CoP约束，在中心处于b上的机器人的框架中，我们设计了保守约束：其中，是半径为r的、中心处于b上的圆，具有性质

量化该系统的鲁棒性是未解决的问题。在不存在任何干扰力的模型化的情况下，我们不能对方向、幅度以及它们的动态进行任何假设。可以将机器人的抵消干扰的能力关联到CoP和c的CoM位置。由于机器人能够移动，机器人可以对强干扰做出反应。我们可以注意到，在不存在对干扰的任何假设的情况下，CoP和c的CoM位置如果接近b，则它们具有沿任何方向移动的最大范围。因此，我们提出了鲁棒性标准ζ，在最大鲁棒性的情况下，其等于0：

ζ＝ζf||p-b||+(1-ζf)||c-b|| (8)

其中，ζf是在范围[0；1]中的因数，其确定了我们认为什么类型的鲁棒性对使CoP居于中心或对使c的CoM居于中心是最重要的。

该机器人模型被定义为解决机器人未倾斜时的情况，我们可以解决对应的控制法则。

我们可以使用欧拉显式方法将每个状态之间的关系写为：

在取样周期T的选择中存在重要的结果。允许轨迹超出两个样本之间的约束，因为我们仅在每个取样时间对轨迹进行约束。由于实时的原因，我们不能将T的值选择得太小。因此，需要考虑每个约束中的该溢出作为安全裕量。

考虑并且采用与针对c的导数和针对b和p的相同的方式。另外，考虑初始状态并且按与针对相同的方式。

使用方程(9)、(10)、(11)，我们可以写出身体轨迹的每个导数(C，和)与命令之间的关系：

其中：

使用相同的方式定义b的动态。我们可以注意到，Uc、以及是不可逆的，因为它们是对角线中没有零的正方形下三角形矩阵。

关于P，使用方程(6)，我们可以写出该关系：

其中：

在可以用于求解确定这种满足这些位置、鲁棒性以及动态行动条件的控制法则的问题的不同方法之中，我们选择将其作为优化问题来解决。而且，为了求解该优化问题，我们选择用公式将其表示为线性约束下的最小平方最小化或者表示为具有目标和线性约束的二次最优化。主要原因在于，这种问题的求解装置计算速度快。添加非线性约束或非二次最优化目标明显增加了计算时间。

最优化变量对应于受控的身体和基座因此，必须将每个目标和约束表示为X的函数。

1)控制目标：

将目标Oi表示为最小平方最小化和QP(二次问题)公式：

当转置时，X^t为X。

第一目标是跟踪控制。在该控制中，我们选择进行位置/速度跟踪。令和为水平线之上的位置和速度目标。使用(13)，我们可以将速度控制目标O1写为：

使用(12)，我们可以将位置控制目标O2写为：

下一个目标是鲁棒性最大化。通过使ζ最小化，我们最大化了鲁棒性。令Upbb＝Upb-Ub。使用(8)、(12)、(16)，我们可以将鲁棒性控制目标O3写为：

优选地，我们定义了另一个目标以避免机器人上的抖动峰值；因此我们向所述控制添加了抖动最小化目标O4：

关于该第四个目标，我们可以注意到，当机器人摔倒时，c和b的轨迹呈指数增长。于是，通过使这些轨迹的抖动最小化，我们直接更好地避免了c和b的指数发散，这有助于稳定机器人。

2)控制约束：

在下文中，约束Ri将被表示为QP线性不等式约束公式：

第一约束R1用于保证稳定性，以确保机器人不会自己倾斜：ζ必须小于或等于1。该约束是非线性的，因此，我们引入了保守的线性约束组，以近似该约束。我们选择使用图4中所示的内接在圆中的八边形形状约束：其定义了CoP极限约束。令将有关CoP边界的该约束R1写为如下：

第二约束R2有关移动基座的运动极限。令和为移动基座的最大速度和加速度。我们可以将约束R2写为如下：

第三也是最后的约束R3有关身体的运动极限。由于其关节的原因，机器人可以在围绕移动基座的CoM的矩形区中移动其上身的CoM。令k^xy为矩形形状的极限，将约束R3写为如下：

R3：k^xy≤c^xy-b^xy≤k^xy (32)

身体的运动极限可以为空。于是，我们有：c^xy＝b^xy。

为了用这些目标和约束求解该最优化问题，我们使用线性二次问题求解器。可以在J.Nocedal和S.J.Wright(2000)的书“Numerical optimization,second editon”中找到有关求解QP的一些文献资料。该种类的求解器找到了问题的最优解决方案，如下：

其中，Q为对称且正定的。使用方程(19)、(21)、(25)、(27)、(29)、(31)、(33)，我们可以填充Q、p、V、v^-以及v⁺的值：

Q＝α1Q1+α2Q2+α3Q3+α4Q4+α5Q5 (35)

p＝α1p1+α2p2+α3p3+α4p4+α5p5 (36)

其中，αi是与每个目标相关联的权重。可以实验性地选择它们。

αi值的选择最初是为了定义控制法则的行动。每个αi之间的相对间隙确定了哪一个目标将被优先考虑，以及将忽略哪一个目标。如果α1和α2大于其它权重，则轨迹跟踪将非常有效，但鲁棒性将是低效的，并且身体和基座的抖动可能很高。如果α3大于其它权重，则机器人对强干扰将是鲁棒的。我们可以注意到，在该行动模式中，如果我们将正速度定义为跟踪目标，则机器人将在向前移动之前开始向后移动，以使CoP居于中心。权重α4对c和b的轨迹具有平滑效果，如果其大于其它权重，则最优解决方案应该是不移动。因此，该权重必须小。

通过选择适当的权重，可以获得一些其它行动。如果两个目标之间的相对间隙较大(几个数量级)，则将几乎是在较大目标的空的空间中计算较小目标。当我们想要具有伪约束(在其不能得以满足的情况下能够被放宽)时，使用该性质是有用的。例如，为了具有好的视觉行动，我们可以添加上身的CoM的高权重的目标，以使其处于移动基座位置的中心。每当可能时，这将具有将CoM固定在移动基座位置的效果，但如果这不可能，则将放宽该伪约束。

如果我们希望具有更好的轨迹跟踪或鲁棒性，可以固定权重设置，这意味着，我们已经预先进行了选择。

参照图2，存储机器人模型和控制法则作为行动221；它们由执行引擎240和服务模块210根据包括以下步骤的闭环方案来实施：

-从传感器检索身体的位置测量结果和基座的位置测量结果，

-使用提取器211，将这些位置测量结果转换为观测的位置测量结果，

-在系统服务213中，使用先前描述的控制法则计算身体速度和基座速度命令，

-集成这些身体和基座速度，以将它们提供到提取器211，

-将这些命令(身体和基座的位置和速度)转换为用于机器人致动器212(基座的轮子和机器人的关节)的指令。

第二控制器专用于机器人倾斜时的情形。例如，如果机器人因为干扰而将要倾斜，则前一节中定义的模型不充分。我们需要考虑模型中的倾斜移动，如图4中所示：倾斜角ψ为非空。首先，我们考虑机器人可能仅在其两个轮子上倾斜。在该情况下，我们未考虑仅围绕一个轮子倾斜的情况，因为为了对机器人进行适当的控制，机器人损失了过多的DoF。

当机器人倾斜时，CoP仅可以在两个接触轮子之间的线上变化。为了最小化在一个轮子上倾斜的可能性，将CoP约束为处于两个接触轮子中间。此外，已经使机器人的机械结构降低了在一个轮子上倾斜的风险。在倾斜移动期间，机器人损失了腾空轮所允许的一个DoF。结果是，机器人变为不完整的。为了应对这一点必须在控制器中添加一些约束。

被视为xy-平面上的单位向量，并且正交于倾斜轴。p仅能够在vψ所定义的轴上移动。而且，令ψ为倾斜角。由于地面的原因，我们具有ψ≥0。我们可以将c(以及相应的b)划分成两个分量：由机器人的电动机控制的受控部分cc(以及相应的bc)，以及取决于以下运动关系的倾斜角的另一个部分：

c^z＝h+dcsin(ψ) (41)

其中，。

为了简化这些关系，我们做了两个假设：

-可以将dc视为常数，因为在过渡倾斜移动期间忽略了相对于ψ的移动；

-通常，我们相对于离心效应忽略了科里奥利效应：因此，可以重写方程如下：

c^z＝h+dcsin(ψ) (47)

最后，考虑到倾斜角将接近0，我们在ψ＝0附近线性化这些方程。而且，当时，我们可以考虑h＞＞dcsin(ψ)。方程变为：

c^z＝h (53)

并且对于b，采用相同方式，用db取代dc并且用l取代h。现在使用(7)、(50)、(52)、(53)、(55)，我们可以重写动态的方程如下：

我们注意到，当角速度为空时(这意味着机器人处于未倾斜状态)，我们获得了结合未倾斜控制器所描述的机器人模型化的方程(7)。

我们选择了将p的位置约束在两个接触轮子的中心。于是，p完全由以下关系定义：

p＝bc+vψdc (57)

使用(56)、(57)，我们现在可以用其它变量表示ψ与其导数之间的关联：

对于定义了倾斜的该机器人模型，我们可以解决对应的控制法则。

我们使用与前一节中相同的形式。于是，我们可以定义ψ的轨迹：

其中：

并且其中：

使用这些关系和方程(58)，我们得到：

我们可以注意到，使用方程(59)、(61)，我们得到了和C、B以及之间的直接线性关系：

并且合成形式为：

仅在矩阵可逆时该关系才成立。Uψ和Uψ是两个下三角矩阵，具有值的常数对角线并且T为取样周期。因此，仅当：

时，可逆性才成立。

求解该系统，我们对T有条件，以允许该矩阵的可逆性(我们假设T>0)：

对于具有结合图1在前序部分中描述的特征的根据本发明的机器人，我们有T≠220ms。

为了求解该问题，我们使用与前一节中相同的形式(机器人未倾斜)。最优化变量保持相同，因为尽管添加了新的变量ψ，我们已经将CoP p约束在一个点。于是，求解问题的变量的数目不变：

1)控制目标：

第一目标用于使ψ的范数最小化，以从倾斜恢复。我们可以表示ψ与X之间的关系：

其中：

现在，我们可以写出该目标O1：

第二个目标是倾斜速度最小化。为了以最低的角速度着陆，以尽可能地避免回弹并且使机械碰撞最小化，重要的是在倾斜期间使角速度最小化。我们可以表示与X之间的关系：

现在，我们可以写出该目标O2：

第三个目标针对数值稳定性，按与未倾斜的情况相同的方式：

2)控制约束：

在下文中，该约束将被表示为QP线性等式约束公式：

第一个约束有关移动基座的运动极限。令和为移动基座的最大速度和加速度，我们可以写出约束R2如下：

第二个约束有关上身的运动极限。由于其关节的原因，机器人可以将其上身的CoM移动到围绕移动基座的CoM的矩形区中。令k^xy为矩形形状的极限，写出约束R2如下：

R2：-k^xy≤c^xy-b^xy≤k^xy (80)

第三约束对应于地面的存在，这意味着倾斜角总是正的：

R3：ψ≥0 (82)

最后一个约束意味着考虑移动基座的DoF损失。仅可能在vψ轴上移动b。写出约束R4如下：

第三控制器专用于着陆阶段。其与第一控制器(未倾斜状态)相同，但不具有两个跟踪控制目标，并且具有倾斜控制器的约束R4。更精确地讲，所述目标是：

-与CoP和基座中心之间的距离相关的目标，CoP是机器人和地面之间的接触力的重心，以及

-数值稳定性目标。

可以将基座速度目标添加到这些目标。

所述约束是：

-移动基座的最大速度和加速度

-身体的运动极限

-CoP极限

-身体的移动仅在角速度轴上。

在机器人的寿命中，我们必须选择使用哪个控制器(图6中所示的)：

·第一控制器501，用于在机器人未倾斜时控制机器人。

·第二控制器502，在机器人倾斜时使用该控制器，以从干扰中恢复。

·第三控制器503，在机器人处于着陆阶段时使用。

根据这些控制器，我们为机器人定义了三种状态：

·未倾斜状态，当激活第一控制器501时。

·倾斜状态，当激活第二控制器502时。

·着陆状态，当激活第三控制器503时。

我们开发了估计器504，以在机器人倾斜时通过估计着陆碰撞时间和碰撞角速度来预测机器人的未来行动，如以下进一步描述的。

如图4中所示，在每个取样时间，监控器500根据以下步骤选择这三种状态的其中之一。

最初，机器人处于未倾斜状态。

在每个取样时间，我们测量倾斜角和倾斜速度：如果它们之一大于零，则我们使用估计器来估计碰撞角速度和碰撞时间。

A)当机器人处于未倾斜状态时。

·如果不存在所估计的碰撞时间(不稳定的移动)，则切换至倾斜状态，因为机器人不能在不做任何事情的情况下得以恢复。

·如果对于机器人的安全而言碰撞角速度过高(＝碰撞角速度高于预先定义的极限1)，则切换至倾斜状态，以降低碰撞角速度。

·如果所测量的角速度过高(＝所测量的角速度高于预先定义的极限2)，则无论所估计的碰撞速度如何，都切换至倾斜状态。该条件旨在以较小的延迟补偿强的推挤。我们假设如果测量到高的角速度，这是因为某人推挤了机器人以将其推倒。

·否则，不需要恢复该角，但如果倾斜角不为空，则切换至着陆状态，因为由于腾空轮而导致机器人损失了一个DoF。

·否则，如果倾斜角为空，则停留在该状态。

B)当机器人处于倾斜状态时。

在该状态下，激活第二控制器，以使机器人的倾斜角和倾斜角速度最小化。在该模型中，我们未对在碰撞时将添加补偿倾斜移动的力的地面进行模型化。将角控制在0使得机器人加速度处于过高速度，以补偿以该角作用于机器人上的重力。因此，我们不需要等待直到倾斜完成才停止使用该控制器。

·如果所测量的角速度过高(＝所测量的角速度高于预先定义的极限2)，则无论所估计的碰撞速度如何，都停留在该状态；

·如果所估计的碰撞速度低于该极限，则切换至着陆状态；

·否则，停留在该状态。

C)当机器人处于着陆状态时。

该状态管理倾斜状态与未倾斜状态之间的平滑过渡，以便：首先，避免由于倾斜控制器中的非模型化的地面所导致的高基座加速度；其次，在重新激活未倾斜状态之前，使腾空轮落下并且机器人恢复损失的DoF。

·如果不存在所估计的碰撞时间，或者如果碰撞角速度过高，或者如果所测量的角速度过高，则切换至倾斜状态；

·如果倾斜角降至0，则等待一小段时间，以防出现回弹，之后如果没有出现回弹，则切换至未倾斜状态；

·否则，停留在该状态。

现在我们描述估计器。我们开发了估计器，以便了解机器人是否将摔倒，并且如果不是将摔倒，则了解碰撞角速度是多少。使用当前测量的倾斜角ψ0和角速度我们假设我们有恒定加速度当倾斜角ψ为0时，移动基座的CoM与倾斜接触点之间的角为该假设意味着移动基座将停止加速。当使用该估计器了解我们是否可以使用着陆模式(其将移动基座的加速度控制为0)时，认为常数角加速度是合理的。我们可以求解二次相关系统，以找到接触时间ti，并且如果其存在，则计算此时的角速度

如果则没有碰撞。我们可以注意到，这仅在时才会发生。否则，碰撞时间ti可以计算为：

显然，我们选择其中ti>0的方案。并且因此，所估计的碰撞速度为：

也可以存储估计器和监控器作为系统数据服务213。