CN104842360A - 用于移动机器人的移动控制方法 - Google Patents

Abstract

本发明涉及用于移动机器人的移动控制方法。构造了用于预测移动机器人状态的预测模型以表示移动机器人在预定持续时间的预测间隔中的状态变化；在预测间隔中使用基于机械平衡的约束条件和预定的稳定性评估准则来获得用于确保移动机器人的稳定性的关于控制输入的时间系列数据；基于时间系列数据和预测模型来获得移动机器人的重心轨迹；将用于放松约束的松弛变量引入到约束条件中；将松弛变量引入到稳定性评估准则中，使得当在约束条件下获得稳定轨迹的解时松弛变量变为零、而当在约束条件下轨迹偏离时生成松弛变量值；并且改变接触点的位置以取消所生成的值。

Description

用于移动机器人的移动控制方法

技术领域

本发明涉及移动机器人的控制。更具体地，本发明涉及用于生成能使移动机器人稳定地移动的轨迹的轨迹生成技术。

背景技术

已开发了具备自主移动的功能的机器人，这种机器人的典型例是双脚步行的脚式机器人。通过使用ZMP的轨迹生成和运动控制，来实现双脚步行机器人的稳定的双脚步行移动(例如日本特开2005-177884号公报)。

发明内容

期待着未来的机器人将在各种环境中工作。例如，期待着服务机器人将与人在相同居住空间工作。人们认为：为了能使机器人在居住空间这样的环境移动，仅有双脚步行的能力是不足的。例如，假设出现图1所示的状况。在图1中，桌子放在墙附近而瓶子放在桌子里侧靠近墙处。机器人无法仅凭双脚步行就抓紧瓶子。机器人必须一边将手放在墙和桌上一边移动、并且倾斜身体以伸出手。这样一来，服务机器人必须具有一边在多个任意的点处接触环境一边执行运动的能力。

为了后面说明方便起见，本文将机器人一边在多个任意的点处接触环境一边运动(移动)称作“多点接触移动”。(然而却不能说由日本特许第5034235号公报公开的方法能实现平滑的多点接触移动，该方法有各种问题。该方法的详情将在本发明的示例实施例中说明。)

当将可实现多点接触移动的服务机器人投入实用时，改善机器人的稳定性是最要紧的。当实际操作实物机器人时，务必考虑到机器人的姿势从假设的姿势偏离的情形。机器人的关节可能会不依照指令工作，误差也可能会积累。如果机器人与人在相同空间，人或物体可能会接触机器人。如果因干扰导致机器人的姿势从假设的位置偏离、而机器人的手和脚却仍在规划好的位置处着地(将手放在规划好的位置处)，则机器人显然难以保证稳定，结果机器人的控制出故障(偏离)。也就是说，如果机器人的重心开始从假设的位置倾斜、而机器人的手和脚却仍按规划着地，则重心更加倾斜。

当机器人的姿势从假设的位置偏离时，可以使着地点从规划好的点处改变以保持重心的稳定。已有这样的已知技术：使双脚步行的机器人通过改变悬空脚的着地位置来改善其稳定性、也即改变预定的着地位置来避免双脚步行的机器人跌倒(例如日本特许第3269852号公报)。

绝大多数现有技术的稳定人型机器人的技术涉及双脚步行的稳定性，迄今尚未开发出应对多点接触移动的技术。因为双脚步行是机器人一边在单脚状态与双脚状态之间来回切换、一边移动的操作，所以可以说双脚步行是多点接触移动的一种。然而，双脚步行的技术与多点接触移动的技术之间存在很大差异。双脚步行技术的很多(以ZMP为代表)是以各接触点(例如双脚机器人的左脚和右脚)被包括在同一平面内为前提的(如例如日本特许第5034235号公报的第0012段处记载的)。具体地，在ZMP控制中，仅能表达本来是6维的力/力矩中的2维信息，因而只能处理极其有限的接触状态的平移。这样，在以ZMP为基础的现有技术的稳定化技术中，不能从原理上解决多点接触移动的情形。也即，目前尚无“一边根据接触点规划来执行多点接触移动，一边按需要改变接触点的位置以改善稳定性”的技术。(作为搜索的结果，未找到公开上述技术的日本专利文献或学术论文。)

本发明的目的是提供基于接触点规划自动地生成稳定轨迹的移动机器人的控制方法，更详细地，本发明的目的是提供一边根据接触点规划来执行多点接触移动、一边按需要改变接触点的位置以改善稳定性的移动机器人的控制方法。

根据本发明的第一示例方面的用于移动机器人的移动控制方法是在使得两个或更多个移动装置与地交替地接触的同时进行移动的用于移动机器人的移动控制方法，所述移动控制方法包括：接触点规划设定步骤：使用表示所述移动装置与地接触于的接触点的位置的时间系列数据来设定接触点规划；以及轨迹生成步骤：生成移动机器人在如在所述接触点规划设定步骤中所设定的那样使得所述移动装置与地接触于接触点的同时所进行的移动的轨迹。所述轨迹生成步骤包括当轨迹偏离时改变接触点的位置。

在本发明的第一示例方面中，所述轨迹生成步骤优选地包括：构造描述了到移动机器人的控制输入与移动机器人的运动之间的关系的预测模型，所述预测模型表示移动机器人在预定持续时间的预测间隔中的状态变化；在所述预测间隔中，通过使用基于机械平衡的约束条件和预定的稳定性评估准则来获得与用于确保移动机器人的稳定性的控制输入有关的时间系列数据；基于所获得的与控制输入有关的时间系列数据和所述预测模型来获得移动机器人的轨迹；将用于放松约束的松弛变量引入到所述约束条件中；以如下方式将松弛变量引入到所述稳定性评估准则中：当在约束条件下获得稳定轨迹的解时所述松弛变量变得基本上为零，而当在约束条件下轨迹偏离时生成基本上等于或大于零的值作为松弛变量；以及改变接触点的位置以取消所生成的松弛变量的值。

在本发明的第一示例方面中，当将松弛变量引入到所述约束条件中时，将松弛变量引入到移动机器人的重心的绕X轴力矩的平衡方程中和移动机器人的重心的绕Y轴力矩的平衡方程中，X轴和Y轴在与对应于竖直方向的Z轴垂直的平面内彼此正交。

在本发明的第一示例方面中，优选地将所述预测间隔分成不包括能够被改变位置的接触点的不可调整的子间隔和包括一个或更多个能够被改变位置的接触点的可调整的子间隔，并且优选地仅在所述可调整的子间隔期间允许生成基本上等于或大于零的值作为松弛变量。

在本发明的第一示例方面中，所述约束条件的表达式包括移动机器人的重心竖直轨迹和移动机器人的绕重心力矩轨迹，所述轨迹生成步骤优选地包括：在预测间隔内临时设定一次移动机器人的重心竖直轨迹和绕重心力矩轨迹；在约束条件下获得一次所述稳定性评估准则的函数的解，并基于所获得的结果改变接触点位置以在预测间隔内稳定移动机器人的运动、并且校正移动机器人的重心竖直轨迹和绕重心力矩轨迹；以及通过使用改变后的接触点位置、经校正的重心竖直轨迹和经校正的绕重心力矩轨迹来再计算至少一次所述稳定性评估准则的函数的解。

在本发明的第一示例方面中，在所述轨迹生成步骤中在预测间隔内获得轨迹；通过将与预测间隔的第一点有关的数据用作当前输入值来驱动移动机器人；以及通过使预测间隔前进少量时间来重复所述轨迹生成步骤。

根据本发明的第二示例方面的移动机器人是使得两个或更多个移动装置与地交替地接触的同时进行移动的移动机器人，所述移动机器人包括：接触点规划设定单元，所述接触点规划设定单元使用表示所述移动装置与地接触于的接触点的位置的时间系列数据来设定接触点规划；以及轨迹生成单元，所述轨迹生成单元生成移动机器人在如由所述接触点规划设定单元所设定的那样使得所述移动装置与地接触于接触点的同时所进行的移动的轨迹。所述轨迹生成单元当轨迹偏离时改变接触点的位置。

在本发明的第二示例方面中，所述轨迹生成单元优选地被配置成：构造描述了到移动机器人的控制输入与移动机器人的运动之间的关系的预测模型，所述预测模型表示移动机器人在预定持续时间的预测间隔中的状态变化；在所述预测间隔中，通过使用基于机械平衡的约束条件和预定的稳定性评估准则来获得与用于确保移动机器人的稳定性的控制输入有关的时间系列数据；基于所获得的与控制输入有关的时间系列数据和所述预测模型来获得移动机器人的轨迹；将用于放松约束的松弛变量引入到所述约束条件中；以如下方式将松弛变量引入到所述稳定性评估准则中：当在约束条件下获得稳定轨迹的解时所述松弛变量变得基本上为零，而当在约束条件下轨迹偏离时生成基本上等于或大于零的值作为松弛变量；以及改变接触点的位置以取消所生成的松弛变量的值。

根据本发明，移动机器人可以基于所设定的接触点的规划自动地生成稳定的重心轨迹、同时自主地移动。此时，可以通过按需要改变接触点来避免控制的故障，从而改善稳定性。

本发明的以上和其它目的、特征和优点将从以下给出的详细说明和附图变得更充分地被理解，该详细说明和附图仅以例示的方式给出，因此不被认为是限制本发明。

附图说明

图1是表示机器人的运动的一例的图；

图2是表示移动机器人的机械构造的一例的图；

图3是移动机器人的功能方框图；

图4是表示6轴力的图；

图5是控制器的功能方框图；

图6是表示接触点规划的概要的一例的图；

图7是表示接触点规划的一例的图；

图8是表示预测间隔的例子的图；

图9是表示在预测间隔中的运动的图；

图10是用于说明预测间隔的平移的图；

图11是表示用于预测的移动机器人的模型的图；

图12是用于说明预测间隔的离散化的图；

图13是表示接触点的坐标系和接触多边形的图；

图14是例示接触点变得不稳定的情形的图；

图15是例示接触点位置无法改变的情形的图；

图16是例示接触点位置能够改变的情形的图；

图17是例示接触点位置的改变量的图；

图18是例示对各接触点定义各位置的改变量的形态的图；

图19是表示用于移动机器人的移动控制方法的整体流程的流程图；

图20是用于说明轨迹生成步骤的程序的详细的流程图；

图21是用于说明轨迹生成步骤的程序的详细的流程图；

图22是用于说明轨迹生成步骤的程序的详细的流程图；

图23A是表示移动机器人的多质点模型的图；

图23B是表示移动机器人的多质点模型的图；

图24是表示对手和脚的轨迹进行间插的形态的图；

图25是表示四脚步行机器人的手和脚可以随机地着地的接触点规划的图；

图26是表示当没有干扰时的执行结果的快照的图；

图27是各自表示重心位置、反作用力中心位置和各接触点的竖直方向负荷的图线；

图28是表示当输入干扰时的执行结果的快照的图；

图29是各自表示重心位置、反作用力中心位置和各接触点的竖直方向负荷的图线；

图30是表示松弛变量值的图线；以及

图31是表示变形例的图。

具体实施方式

下面参照标示附图中各要素的附图标记来说明本发明的示例实施例。

(第一示例实施例)

本示例实施例以移动机器人控制装置为特征，具体地，以生成用于控制移动机器人的移动操作的轨迹的移动机器人控制装置为特征。在说明具体的控制(轨迹生成)之前，说明成为控制对象的移动机器人的硬件构造。

图2是表示移动机器人的机械构造的一例的图。

在移动机器人100中，各股关节由3轴构成、各膝关节由1轴构成、各踝关节由2轴构成、各肩关节由3轴(肩俯仰轴、肩滚转轴和肩偏航轴)构成、各肘关节由1轴构成(肘俯仰轴)、并且各腕关节由3轴(腕俯仰轴、腕滚转轴和腕偏航轴)构成。

(尽管移动机器人的机械构造不限于上述，但是需要6或更多的手(臂)移动的自由度和6或更多的脚(腿)移动的自由度。)

移动机器人100包括设在各关节处的带编码器的马达1、2、…、28。

设在各关节处的马达(关节驱动单元)1a、2a、…、28a(见图3)能够分别调整各关节的关节角度θ1、θ2、…、θ28。

另一方面，设在各关节处的编码器(关节角度检测单元)1b、2b、…、28b能够分别测量各关节的关节角度θ1、θ2、…、θ28。

移动机器人100还包括设在各脚部(脚底部)和各手部(手掌部)的接触力传感器29。

此处说明的接触力是指6轴力。如图4所示，接触力包括x轴、y轴和z轴方向的力f的组(f_x、f_y、f_z)^T和绕x轴、绕y轴和绕z轴的力τ的组(τ_x、τ_y、τ_z)^T。

(x轴和y轴在与相当于竖直方向的Z轴垂直的平面内彼此正交。)

该移动机器人在移动时，使右脚、左脚、右手和左手中的至少一个接触地板、墙壁或桌子等同时移动。

因而，在以下说明中，将右脚、左脚、右手和左手也称作接触点候选。

机器人的手和脚表示移动单元。

图3是移动机器人100的功能方框图。

移动机器人100包括设在各关节处的马达1a～28a和编码器1b～28b、力传感器29和控制器210。

控制器210从各关节的编码器1b～28b和力传感器29接收传感器检测值。控制器210向各关节的马达1a～28a输出驱动信号。

控制器210作为主要硬件构造由包括CPU(中央处理单元)210a、ROM(只读存储器)210b和RAM(随机读写存储器)210c的微计算机构成。CPU210a执行控制处理、算术运算处理等。ROM210b存储由CPU210a执行的控制程序、算术运算程序等。RAM210c临时地存储处理数据等。CPU210a、ROM210b和RAM210c经由数据总线210d相互连接。可以将必要的程序记录在非易失性记录媒介中，并且按照需要将程序从非易失性记录媒介安装到控制器210中。

图5表示控制器210的功能方框图。

控制器210包括接触点规划设定单元221、轨迹生成单元222和运动控制单元223。接触点规划设定单元221存储由用户指令的接触点规划，轨迹生成单元222生成能被稳定地执行的轨迹，运动控制单元223根据所生成的轨迹使移动机器人100执行全身运动。

在此情形中，轨迹生成单元222生成允许移动机器人执行根据接触点规划的运动的轨迹，但是轨迹生成包括根据需要的接触点改变。

这些功能单元的详细的处理操作将在后面说明。

(用于多点接触移动的轨迹生成方法)

本发明具有以下特征：(1)生成能实现多点接触移动的轨迹；以及(2)按需要改变接触点。在本申请的提交的时间点，本申请意图要求的特征是特征(2)，即改变(校正)接触点以改善多点接触移动的稳定性。

然而，由于不能说用于多点接触移动的轨迹生成方法在目前是公知技术，因而首先说明用于(1)生成能实现多点接触移动的轨迹的方法。

注意，本申请人虽然提交了日本特愿2013-254989(2013年12月10日提交)但是专利申请尚未公开。

如上所述，日本特许5034235号记载了应对多点接触移动而开发的控制系统。

根据本发明，不仅由日本特许5034235号公开的方法生成多点接触移动的轨迹，而且还可以由后面说明的方法校正接触点。(进一步地，由于本发明主要着眼于“(2)按需要改变接触点”这一特征，因而轨迹生成的详细方法不限于由日本特许5034235号公开的方法，只要是能够实现多点接触移动就行。)

也就是说，本发明作为轨迹生成方法、不排除由日本特许5034235号公开的方法，但是由日本特许5034235号公开的方法有以下指出的问题。

日本特许5034235号公开的技术有这样的问题：如果不给出接触点的规划以及目标重心位置和目标运动量，则不能生成稳定的轨迹。

原本，因为难以预测将来的目标重心位置，所以用户无法设定作为未知的控制目标值的将来的重心位置。

最好是：用户只给机器人提供接触点的规划，并且机器人基于所设定的接触点的规划自动地生成稳定的重心轨迹、同时自主地移动。

日本特许5034235号公开的技术还有如下问题：与接触点的切换(接触状态的平移)伴随的力的变化很剧烈，这导致难以得到平滑的力的平移。

为了使机器人稳定地执行多点接触移动，需要利用一种根据每时每刻都在变化的接触点平滑地且适当地分配接触力、并且生成稳定的重心轨迹的技术。

为此，本发明人试图使用模型预测控制。

首先说明模型预测控制的概要。

(模型预测控制的概要说明)

例如假设要使机器人执行图1所示的移动操作的情形。

此处假设使具有2条臂和2条腿的人型机器人抓取放在桌子里侧的瓶子的一系列运动。

在此情形中，用户创建执行该一系列运动(任务)的接触点规划。

具体地，如图6所示，创建确定以怎样的顺序放置手和脚、以及在何处和如何放置手和脚的规划。

在图6中，脚和手接触地板、墙壁和桌子的地点被标上标记。

该接触点规划具体地如图7所示。

接触点规划是表示以怎样的顺序放置左手(LH)、右手(RH)、左脚(LF)和右脚(RF)、以及在何处和如何放置左手(LH)、右手(RH)、左脚(LF)和右脚(RF)的时间系列数据。

简单地说明图1、图6和图7的对应关系。

最初(t0)机器人仅以左脚单脚站立，移动作为悬空脚的右脚向前迈步，然后使右脚着地(t1)。

为了指令移动机器人执行该运动规划，必须指定左脚最初着地的地板上的接触点的坐标P_LF1、此时左脚的姿势rLF1、右脚随后着地的地板上的接触点的坐标P_RF1和此时右脚的姿势r_RF1。

各接触点的坐标由空间坐标P＝(P_x、P_y、P_z)的组表示。

姿势是指各脚在接触点着地时脚底的方向，姿势由例如一组欧拉角r＝(r_x、r_y、r_z)表示。(具体地，r_x、r_y和r_z分别表示俯仰角、滚转角和偏航角。)

后面的说明同样适用于脚着地的接触点的坐标和对此时的姿势作指令的形式，因此酌情省略其说明。

在双脚站立后，移动机器人移动左脚向前迈步(t2)然后使左脚着地(t4)。

在此期间，将左手放在墙壁上(t3)。

此时，指定放置左手的墙壁上的接触点的坐标P_LH1和此时左手的姿势r_LH1。

各接触点的坐标由空间坐标P＝(P_x、P_y、P_z)的组表示。姿势是指各手放置在接触点时手掌的方向，姿势由例如一组欧拉角r＝(r_x、r_y、r_z)表示。

由于后续的接触点规划从图1、图6和图7显而易见，故省略其说明。

以此方式，用户创建接触点规划作为时间系列数据。

当将这样创建的接触点规划输入到移动机器人时，移动机器人生成轨迹、同时自主地移动，以实现该接触点规划。

此时，移动机器人在轨迹生成期间执行模型预测控制。

具体地，生成使移动机器人在具有某一持续时间的预测间隔内稳定地移动的轨迹，并且逐次将预测间隔平移少量时间(Δt)、同时依次更新能进行稳定运动的轨迹。

图8表示预测间隔的例子。

将当前与到预定时间(例如1.6秒)终止之间的间隔设定为预测间隔。

接着，生成稳定的轨迹以在该预测间隔期间不偏离。

图9表示在预测间隔中的运动的图像。

如上所述，在具有某一持续时间的预测间隔生成稳定的轨迹，然后仅使用轨迹的最初一点作为当前输入值。

在下一轨迹更新周期(Δt秒后)将预测间隔平移，在新的预测间隔中以与前一轨迹更新周期同样的方式生成稳定的轨迹(参见图10)。

不是仅关注当前或从当前起到下一控制周期(Δt秒)为止，而是如上述将当前与到某一段时间终止之间的间隔设定为预测间隔，以生成在该预测间隔内不偏离的轨迹。

重复上述处理，移动机器人就能够稳定地移动。

此处的问题是：怎样才能根据在具有某一持续时间的预测间隔中每时每刻都在变化的接触点平滑地且适当地分配接触力、并且怎样才能生成稳定的重心轨迹？

本发明人构思出将在某一预测间隔中稳定轨迹的生成问题归结为凸二次规划问题(二次规划：QP)的想法。

具体地，通过求解使包含各接触点中接触力的平方和以及上述6轴力的微分值的平方和的评估函数J最小化的问题，来获得多点接触移动的稳定轨迹。

接下来说明评估函数J的导出及它的解法(归结为凸二次规划问题)。

该解法表明：获得了用于在某一预测间隔内实现稳定的多点接触移动的重心位置、重心速度、接触力和接触力的微分值的时间系列数据。(在以下的说明中，先假设将手和脚放置在如接触点规划所指示的位置(接触点)上。也就是说，不必考虑改变接触点的情形。虽然，为了也能按需要改变接触点，必须采取条件式或评估式的放松等措施，但是这些将在后面详细地说明。)

图11示出用于预测的移动机器人的模型。

移动机器人整体的惯性由一个重心G表示。对各接触点定义6轴力。

此时，设重心G的直移运动量记为P、绕重心的旋转运动量(角运动量)记为L、且接触点的数目记为n，则可得以下运动方程式。

$\stackrel{.}{P}=m\stackrel{..}{G}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{f}_i-\mathrm{mg}$

$\stackrel{.}{L}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(p_i-G)\×f_i+{\mathrm{\τ}}_i\·\·\·\left(1\right)$

下标i表示接触点的索引。

例如，当接触点的候选有4个即左手、右手、左脚和右脚时，可以设n＝4(左手：LH＝1、右手：RH＝2、左脚：LF＝3、右脚：RF＝4)。然而此时以设与不接触地板和墙壁的接触点候选关联的接触力为0的方式设定约束条件。例如在图11的例子中建立下式。

f_RH＝0，τ_RH＝0

f_LF＝0，τ_LF＝0···(2)

对式(1)的第1式和第2式求微分，则得到下式。(式(1)由向量表示，将向量分解成x、y和z，并从上到下依次称为第1式、第2式、…、第6式。)

${\stackrel{...}{G}}_x=\frac{1}{m}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\stackrel{.}{f}}_{\mathrm{ix}}$

${\stackrel{...}{G}}_y=\frac{1}{m}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\stackrel{.}{f}}_{\mathrm{iy}}\·\·\·\left(3\right)$

在本实施例中，使用这两式作为系统。将式(1)的第3式到第5式作为约束条件公式化。

${m\stackrel{..}{G}}_z=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{f}_{\mathrm{iz}}-\mathrm{mg}$

${\stackrel{\·}{L}}_x=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}((p_{\mathrm{iy}}-G_y)f_{\mathrm{iz}}-(p_{\mathrm{iz}}-G_z)f_{\mathrm{iy}}+{\mathrm{\τ}}_{\mathrm{ix}})$

${\stackrel{.}{L}}_y=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}((p_{\mathrm{iz}}-G_z)f_{\mathrm{ix}}-(p_{\mathrm{ix}}-G_x)f_{\mathrm{iz}}+{\mathrm{\τ}}_{\mathrm{iy}})\·\·\·\left(4\right)$

另外，如图12所示将预测间隔分割成N个间隔，并且将式(3)和式(4)离散化。

$\begin{array}{c}{G}_x[j+1]=G_x\left[j\right]+{\stackrel{.}{G}}_x\left[j\right]\mathrm{\Δt}+\mathrm{\Σ}{f}_{\mathrm{ix}}\left[j\right]{\mathrm{\Δt}}^2/2m+\mathrm{\Σ}{\stackrel{.}{f}}_{\mathrm{ix}}\left[j\right]{\mathrm{\Δt}}^3/6m\\ G_y[j+1]=G_y\left[j\right]+{\stackrel{.}{G}}_y\left[j\right]\mathrm{\Δt}+\mathrm{\Σ}{f}_{\mathrm{iy}}\left[j\right]{\mathrm{\Δt}}^2/2m+\mathrm{\Σ}{\stackrel{.}{f}}_{\mathrm{iy}}\left[j\right]{\mathrm{\Δt}}^3/6m\\ {\stackrel{.}{G}}_x[j+1]={\stackrel{.}{G}}_x\left[j\right]+\mathrm{\Σ}{f}_{\mathrm{ix}}\left[j\right]\mathrm{\Δt}/m+\mathrm{\Σ}{\stackrel{.}{f}}_{\mathrm{ix}}\left[j\right]{\mathrm{\Δt}}^2/2m\\ {\stackrel{.}{G}}_y[j+1]={\stackrel{.}{G}}_y\left[j\right]+\mathrm{\Σ}{f}_{\mathrm{iy}}\left[j\right]\mathrm{\Δt}/m+\mathrm{\Σ}{\stackrel{.}{f}}_{\mathrm{iy}}\left[j\right]{\mathrm{\Δt}}^2/2m\\ f_i[j+1]=f_i\left[j\right]+f_i\left[j\right]\mathrm{\Δt}(i=1...m)\\ {\mathrm{\τ}}_i[j+1]={\mathrm{\τ}}_i\left[j\right]+{\stackrel{.}{\mathrm{\τ}}}_i\left[j\right]\mathrm{\Δt}(i=1...n)\end{array}\·\·\·\left(5\right)$

假设在各取样点处式(4)的约束条件总是成立，则式(4)被离散化如下。

$\begin{array}{c}{F}_{\mathrm{Gz}}\left[j\right]:=m\left({\stackrel{..}{G}}_z\right[j]+g)=\mathrm{\Σ}{f}_{\mathrm{iz}}\left[j\right]\\ {\stackrel{.}{L}}_x\left[j\right]=-F_z\left[j\right]G_y\left[j\right]+\mathrm{\Σ}\left(p_{\mathrm{iy}}\right[j\left]f_{\mathrm{iz}}\right[j]-\left(p_{\mathrm{iz}}\right[j]-G_z[j\left]f_{\mathrm{iy}}\right[j]+{\mathrm{\τ}}_{\mathrm{ix}}[j\left]\right))\\ {\stackrel{.}{L}}_y\left[j\right]=F_z\left[j\right]G_x\left[j\right]+\mathrm{\Σ}\left(\left(p_{\mathrm{iz}}\right[j]-G_z[j\left]\right)f_{\mathrm{ix}}\left[j\right]-p_{\mathrm{ix}}\left[j\right]f_{\mathrm{iz}}\left[j\right]+{\mathrm{\τ}}_{\mathrm{iy}}\left[j\right]\right)\end{array}\·\·\·\left(6\right)$

在此情形中，按下面设定参数。

$\begin{array}{c}{\mathrm{\θ}}_i\left[j\right]:={\left[\begin{array}{cccccc}{f}_{\mathrm{ix}}\left[j\right]& f_{\mathrm{iy}}\left[j\right]& f_{\mathrm{iz}}\left[j\right]& {\mathrm{\τ}}_{\mathrm{ix}}\left[j\right]& {\mathrm{\τ}}_{\mathrm{iy}}\left[j\right]& {\mathrm{\τ}}_{\mathrm{iz}}\left[j\right]\end{array}\right]}^T\\ :={\left[\begin{array}{cc}{f}_i{\left[j\right]}^T& {\mathrm{\τ}}_i{\left[j\right]}^T\end{array}\right]}^T\\ x\left[j\right]:={\left[\begin{array}{ccccccc}{G}_x\left[j\right]& {\stackrel{.}{G}}_x\left[j\right]& G_y\left[j\right]& {\stackrel{.}{G}}_y\left[j\right]& {\mathrm{\θ}}_1{\left[j\right]}^T& ...& {\mathrm{\θ}}_n{\left[j\right]}^T\end{array}\right]}^T\\ u\left[j\right]:=\begin{array}{c}\end{array}\left[\begin{array}{ccc}{\stackrel{.}{\mathrm{\θ}}}_1{\left[j\right]}^T& ...& {\stackrel{.}{\mathrm{\θ}}}_n{\left[j\right]}^T\end{array}\right]\end{array}\·\·\·\left(7\right)$

θ_i表示排列有作为6轴力的接触力的向量。x表示排列有重心G的x坐标、重心G的x轴方向速度、重心G的y坐标、重心G的y轴方向速度和各接触点中的接触力(6轴力)的向量。向量“x”称作状态变量，另外u表示排列有接触力(6轴力)的微分值的向量。

当如以上那样设定参数时，可以如下表述式(5)。

x[j+1]＝Ax[j]+Bu[j]····(8)

$A_0:=\left[\begin{array}{cccc}1& \mathrm{\Δt}& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& \mathrm{\Δt}\\ 0& 0& 0& 1\end{array}\right]$

$A_f:=\left[\begin{array}{cccccc}{\mathrm{\Δt}}^2/2m& 0& 0& 0& 0& 0\\ \mathrm{\Δt}/m& 0& 0& 0& 0& 0\\ 0& {\mathrm{\Δt}}^2/2m& 0& 0& 0& 0\\ 0& \mathrm{\Δt}/m& 0& 0& 0& 0\end{array}\right]$

$B_f:=\left[\begin{array}{cccccc}{\mathrm{\Δt}}^3/6m& 0& 0& 0& 0& 0\\ \mathrm{\Δ}{t}^2/2m& 0& 0& 0& 0& 0\\ 0& {\mathrm{\Δt}}^3/6m& 0& 0& 0& 0\\ 0& \mathrm{\Δ}{t}^2/2m& 0& 0& 0& 0\end{array}\right]$

式(8)表示(i+1)时的状态变量x可以由它的前一状态来描述。使用式(8)顺序地计算预测间隔内的状态变量x时，得到下式。

$\begin{array}{c}x[k+1]=\mathrm{Ax}\left[k\right]+\mathrm{Bu}\left[k\right]\\ x[k+2]=\mathrm{Ax}[k+1]+\mathrm{Bu}[k+1]\\ =A\left(\mathrm{Ax}\right[k]+\mathrm{Bu}[k\left]\right)+\mathrm{Bu}[k+1]\\ =A^{2}x\left[k\right]+\mathrm{ABu}\left[k\right]+\mathrm{Bu}[k+1]\\ x[k+3]=\mathrm{Ax}[k+2]+\mathrm{Bu}[k+2]\\ =A\left(A^{2}x\right[k]+\mathrm{ABu}[k]+\mathrm{Bu}[k+1\left]\right)+\mathrm{Bu}[k+2]\\ =A^{3}x\left[k\right]+A^2\mathrm{Bu}\left[k\right]+\mathrm{ABu}[k+1]+\mathrm{Bu}[k+2]\\ .\\ .\\ .\\ x[k+N]=\mathrm{Ax}[k+N-1]+\mathrm{Bu}[k+N-1]\\ =A^{N}x\left[k\right]+A^{N-1}\mathrm{Bu}\left[k\right]...+\mathrm{ABu}[k+N-2]+\mathrm{Bu}[k+N-1]\end{array}\·\·\·\left(9\right)$

相应地，当由大写字母“X”表示以时间序列获得的状态变量x的排列时，可以如下表示状态变量的时间系列数据X。

式(10)表示这样的预测模型：该预测模型使用接触力的微分值(U[k])作为输入值，以表示某一预测间隔内的移动机器人的状态平移。

这里，本发明人为了在预测间隔内生成稳定的轨迹而引入如下评估函数J。

$\begin{array}{c}\min J={\∫}_0^{\mathrm{N\Δt}}\frac{1}{2}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}({{\mathrm{\θ}}_i}^T{Q}_i{\mathrm{\θ}}_i+{{\stackrel{.}{\mathrm{\θ}}}_i}^T{R}_i{\stackrel{.}{\mathrm{\θ}}}_i)\mathrm{dt}\\ Q_i=\mathrm{diag}(Q_1^i,...,Q_6^i)\\ R_i=\mathrm{diag}(R_1^i,...,R_6^i)\end{array}\·\·\·\left(11\right)$

注意Q_i和R_i各自表示按需要设定的权重。

后面将详细说明权重的意义和如何为接触点候选分配权重。此处假设例如为接触点候选均等分配力；Q_i值全部置1、R_i值全部设为1×10^-6。

此处，θ_i表示排列有作为6轴力的接触力的成分的向量。θ_i(·)表示排列有接触力成分的微分值的向量(括号中的点表示微分)。相应地，式(11)意味着“在预测间隔内，使接触力(6轴力)和接触力(6轴力)微分值的平方和最小化”。式(11)的第1项意味着使接触力(6轴力)的平方和最小化。该第1项包括以下作用：

(1)对各接触点均等分配接触力。这就达到了将重心移动到尽可能稳定的位置的效果。

(2)消除不必要的内力。

(3)提高接触点的接地稳定性。这提供了将接触面内的反作用力中心点设定在接触面的中心的效果。

式(11)的第2项意味着使接触力(6轴力)微分值(6轴力的时间变化率)的平方和最小化。该第1项包括以下作用：

(1)抑制重心的偏离。

(2)平滑地平移接触力。

由权重Q和R将这些作用适当地相加、从而使评估函数J最小化——这意味着“一边满足高的接触稳定性、平滑的接触力平移、最低限的内力、一边输出稳定的重心轨迹和各接触点的接触力”。

将式(11)离散化时得到下式。

$\min J=\frac{1}{2}\underset{j=1}{\overset{N-1}{\mathrm{\Σ}}}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left({\mathrm{\θ}}_i{[k+j+1]}^T{Q}_i{\mathrm{\θ}}_i\right[k+j+1]+{\stackrel{.}{\mathrm{\θ}}}_i{[k+j]}^T{R}_i{\stackrel{.}{\mathrm{\θ}}}_i[k+j\left]\right)\·\·\·\left(12\right)$

由于接触力(6轴力)被包括在状态变量x中，式(7)可变形如下：

相应地，式(12)的第一个Σ可计算如下。

$Q_a:=T_f^T\mathrm{diag}(Q_1,...,Q_n)T_f$

R_a：＝diag(R₁，…，R_n)

相应地，式(12)可以表达如下。

$J=\frac{1}{2}\underset{j=1}{\overset{N-1}{\mathrm{\Σ}}}\left(x{[k+j+1]}^T{Q}_{a}x\right[k+j+1]+u{[k+j]}^T{R}_{a}u[k+j\left]\right)$

Q_w：＝diag(Q_a，…，Q_a)

R_w：＝diag(R_a，…，R_a)

另外，可以使用式(10)将评估函数J公式化如下。

$\begin{array}{c}J=\frac{1}{2}X{[k+1]}^T{Q}_{w}X[k+1]+\frac{1}{2}U{\left[k\right]}^T{R}_{w}U\left[k\right]\\ =\frac{1}{2}{\left(G_{1}x\right[k]+G_{2}U[k\left]\right)}^T{Q}_w\left(G_{1}x\right[k]+G_{2}U[k\left]\right)+\frac{1}{2}U{\left[k\right]}^T{R}_w\left[k\right]\\ =\frac{1}{2}U{\left[k\right]}^T(G_2^T{Q}_w{G}_2+R_w)U\left[k\right]+{\left(G_2^T{Q}_w{G}_{1}x\right[k\left]\right)}^{T}U\left[k\right]\\ :=\frac{1}{2}U{\left[k\right]}^T\mathrm{HU}\left[k\right]+F^{T}U\left[k\right]+\mathrm{const}\end{array}\·\·\·\left(16\right)$

接下来考虑约束条件。

在预测间隔的全部取样点中必须满足以下约束条件，

约束条件如下：

如式(2)所示，对非接触的接触点候选将6轴力设为0的约束；

使式(6)所示的竖直方向的力平衡的约束；以及

使式(6)所示的绕xy轴的力矩平衡的约束。

在此，例如设在某一取样点j处，第i个接触点和第i+2个接触点是非接触点。

此时，式(2)和式(6)可以如下表述。

C[j]x[j]＝d[j]···(17)

$C\left[j\right]:=\left[\begin{array}{cccccccc}{C}_0\left[j\right]& C_1\left[j\right]& ...& O& C_{i+1}\left[j\right]& O& ...& C_n\left[j\right]\\ O& O& ...& I& O& O& ...& O\\ O& O& ...& O& O& I& ...& O\end{array}\right]$

$d\left[j\right]:=\left[\begin{array}{c}{d}_0\left[j\right]\\ O\\ O\end{array}\right]$

$C_0\left[j\right]:=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& -m\left({\stackrel{..}{G}}_z\right[j]+g)& 0\\ m\left({\stackrel{..}{G}}_z\right[j]+g)& 0& 0& 0\end{array}\right]$

$C_i\left[j\right]:=\left[\begin{array}{cccccc}0& 0& 1& 0& 0& 0\\ 0& -\left(p_{\mathrm{iz}}\right[j]-G_z[j\left]\right)& p_{\mathrm{iy}}\left[j\right]& 1& 0& 0\\ p_{\mathrm{iz}}\left[j\right]-G_z\left[j\right]& 0& -p_{\mathrm{ix}}\left[j\right]& 0& 1& 0\end{array}\right]$

$d_0\left[j\right]:=\left[\begin{array}{c}m\left({\stackrel{..}{G}}_z\right[j]+g)\\ {\stackrel{.}{L}}_x\left[j\right]\\ {\stackrel{.}{L}}_y\left[j\right]\end{array}\right]$

此处，系数矩阵C[j]和d[j]的成分随取样点而异，然而已给出了接触点候选的接触/非接触的信息和接触点位置。

重心竖直轨迹(Gz[j])和绕重心力矩(Lx(·)[j]、Ly(·)[j])将在后面详细说明，这里简要说明如下。

如果重心竖直轨迹和绕重心力矩未确定，则设定并使用临时的值。例如，可以使用在前一控制周期中使用的重心竖直轨迹和绕重心力矩作为临时的值。

或者，可以将重心竖直轨迹和绕重心力矩临时设定为它们在预测间隔内不从当前时刻的状态移动。

接着使用临时的值执行一次计算，然后使用得到的结果更新重心竖直轨迹和绕重心力矩，并且重复获得解。

这样，重心竖直轨迹和绕重心力矩也被作为已知的值以继续进行计算，并且在更新计算中纠正偏差。

为了式(17)对全部未来的取样点成立，必须满足下式。

将式(10)代入该式。

$W:=\left[\begin{array}{c}d[k+1]\\ d[k+2]\\ .\\ .\\ .\\ d[k+N]\end{array}\right]-\left[\begin{array}{c}C[k+1]A\\ C[k+2]A^2\\ .\\ .\\ .\\ C[k+N]A^N\end{array}\right]x\left[k\right]$

最后，引入用于稳定各接触点和保持接触的条件。

图13表示接触点的坐标系(标有上标字母l)和接触多边形(接触点的支持多边形)。

假设将各接触点设为原点，并且根据接触面的姿势r_i定义各接触点的坐标系。

此处，如下表示由接触点的坐标系定义的接触力(6轴力)θ_i ^l。

θ_i ^l＝[f_ix ^l，f_iy ^l，f_iz ^l，τ_ix ^l，τ_iy ^l，τ_iy ^l]^T

然后，接触力(6轴力)θ_i ^l可以使用接触面的姿势矩阵Φ_i＝rot(r_i)来表示。

注意“rot”是将欧拉角变换成姿势矩阵的函数。

${\mathrm{\θ}}_i^l=\left[\begin{array}{cc}{\mathrm{\Φ}}_i& O\\ O& {\mathrm{\Φ}}_i\end{array}\right]{\mathrm{\θ}}_i:=T_i{\mathrm{\θ}}_i\·\·\·\left(20\right)$

为了稳定接触点和保持接触，必须满足以下3项条件：

(1)防止接触点相互远离；

(2)防止接触点滑动；

(3)防止接触点被剥夺。

为了更好理解上述3项条件，图14例示接触点变得不稳定的情形。

(1)为了防止接触点相互远离，只需要接触面的竖直力为正，即必须满足下式。

$f_{\mathrm{iz}}^l\≥0\·\·\·\left(21\right)$

(2)为了防止接触点滑动，只需要与接触面平行的2轴力小于或等于摩擦力，即必须满足作为条件的下式。此时接触面的摩擦系数由μ_i表示。

$\begin{array}{c}-{\mathrm{\μ}}_i{f}_{\mathrm{iz}}^l\≤f_{\mathrm{ix}}^l\≤{\mathrm{\μ}}_i{f}_{\mathrm{iz}}^l\\ -{\mathrm{\μ}}_i{f}_{\mathrm{iz}}^l\≤f_{\mathrm{iv}}^l\≤{\mathrm{\μ}}_i{f}_{\mathrm{iz}}^l\end{array}\·\·\·\left(22\right)$

(3)防止接触点被剥夺的条件使用接触多边形的h个顶点坐标表示如下。

(x_i1 ^l，y_i1 ^l)，……，(x_ih ^l，y_ih ^l)

(此处假设接触多边形的顶点按逆时针顺序给出)。

$\left\{\begin{array}{c}(x_{i1}^l-x_{i2}^l){\mathrm{\τ}}_{\mathrm{ix}}^l+(y_{i1}^l-y_{i2}^l){\mathrm{\τ}}_{\mathrm{iy}}^l\≤(x_{i1}^l{y}_{i2}^l-x_{i2}^l{y}_{i1}^l)f_{\mathrm{iz}}^l\\ .\\ .\\ .\\ (x_{\mathrm{ih}}^l-x_{i1}^l){\mathrm{\τ}}_{\mathrm{ix}}^l+(y_{\mathrm{ih}}^l-y_{i1}^l){\mathrm{\τ}}_{\mathrm{iy}}^l\≤(x_{\mathrm{ih}}^l{y}_{i1}^l-x_{i1}^l{y}_{\mathrm{ih}}^l)f_{\mathrm{iz}}^l\end{array}\right.\·\·\·\left(23\right)$

以上式(21)、(22)和(23)可以被整理如下。

$\left[\begin{array}{cccccc}0& 0& -1& 0& 0& 0\\ 1& 0& -{\mathrm{\μ}}_i& 0& 0& 0\\ -1& 0& -{\mathrm{\μ}}_i& 0& 0& 0\\ 0& 1& -{\mathrm{\μ}}_i& 0& 0& 0\\ 0& -1& -{\mathrm{\μ}}_i& 0& 0& 0\\ 0& 0& x_{i2}^l-y_{i1}^2-x_{i1}^l{y}_{i2}^l& x_{i1}^l-x_{i2}^l& y_{i1}^l-y_{i2}^l& 0\\ & & & .& & \\ & & & .& & \\ & & & .& & \\ 0& 0& x_{i1}^l{y}_{\mathrm{ih}}^l-x_{\mathrm{ih}}^l{y}_{i1}^l& x_{\mathrm{ih}}^l-x_{i1}^l& y_{\mathrm{ih}}^l-y_{i1}^l& 0\end{array}\right]{\mathrm{\θ}}_i^l\≤\⇔{\mathrm{\Γ}}_i{\mathrm{\θ}}_i^l\≤O$

$\·\·\·\left(24\right)$

将式(20)代入式(24)并且作为第j个取样点被索引。

$\begin{array}{c}{\mathrm{\Γ}}_i\left[j\right]T_i\left[j\right]{\mathrm{\θ}}_i\left[j\right]\≤O\⇔E_i\left[j\right]{\mathrm{\θ}}_i\left[j\right]\≤O\\ E_i\left[j\right]:={\mathrm{\Γ}}_i\left[j{]T}_i\right[j]\end{array}\·\·\·\left(25\right)$

式(25)表示为了保持第j个取样点中的第i个接触点的稳定的接触而应满足的条件，为了实现稳定的多点接触操作，必须在全部取样点的全部接触点中都满足式(25)。

首先在第j个取样点中全部接触点都满足式(25)的条件书写如下。

满足全部取样点的条件可以书写如下。

将式(10)代入该式。

$\begin{array}{c}{E}_w\left(G_{1}x\right[k]+G_{2}U[k\left]\right)\≤O\\ \⇔E_w{G}_{2}U\left[k\right]\≤-E_w{G}_{1}x\left[k\right]\\ \⇔\mathrm{PU}\left[k\right]\≤S\end{array}\·\·\·\left(28\right)$

P：＝E_wG₂

S：＝-E_wG₁x[k]

最终，由式(16)、(19)和(28)，多点接触时的稳定轨迹生成问题被归结为如下的凸二次规划问题。

$\begin{array}{c}\underset{U}{\min }\frac{1}{2}{U}^T\mathrm{HU}+F^{T}U\\ s.t.\mathrm{PU}\≤S\\ \mathrm{VU}=W\end{array}\·\·\·\left(29\right)$

如果问题能被归结为凸二次规划问题(二次规划：QP)，则作为广为公知的最优问题之一的凸二次规划问题(QP)的解可被高速地获得。(获得解后可获得U[k])。

在获得QP的解后，如下计算重心轨迹和各接触点处的接触力。

x_out[k+1]：＝G₁x[k]+G₂U[k]···(30)

以此方式，获得在预测间隔内总是满足约束条件(1)～(3)并且使评估函数J最小化的X[k+1]。

从式(7)和式(10)可见，X[k+1]表示重心的x坐标、重心的x轴方向速度、重心的y坐标、重心的y轴方向速度和各接触点的接触力(6轴力)的时间系列数据。

如在式子的展开过程中所说明的，获得在更新计算中被校正的重心的竖直轨迹(z坐标)和绕重心力矩(后面将详细说明)。即，能够得到在预测间隔内实现稳定的移动的轨迹。

为了使此处使用的术语免于混乱，下面更详细地解释术语。

如上述算术运算过程和由上述算术运算过程获得的结果可知，此处所述术语“轨迹”，如果除了重心的三维轨迹外、还考虑到在各接触点生成的接触力(6轴力)的时间系列数据，则更容易理解。

使移动机器人能移动的“轨迹”一般包括手和脚的轨迹，但在以上说明中使用的“轨迹”既可以包括也可以不包括手和脚的轨迹。

以手和脚根据接触点规划而放置为前提，手和脚的轨迹由从接触点规划、重心轨迹和接触力的时间系列数据的间插大体上自动地确定。

(然而，如后面所述，当连改变接触点的情形也考虑到时，手和脚的轨迹也重要。)

(约束的放松)

以上的说明以手和脚根据接触点规划而放置为前提。然而，如果有各种干扰，则会出现仅根据接触点规划放置手和脚不能确定机器人的稳定轨迹的情况。例如，假设当机器人被从后侧使劲推时输入大的干扰。这样，按从后侧施加的推动量，机器人的重心具有大的速度。假设从如上述在机器人的重心生成大的速度的状态由模型预测控制获得轨迹，也就是假设解出式(29)以如式(30)所示地求解。则在各接触点可生成的接触力不足以使机器人停止，导致轨迹偏离的情况。

例如，考虑双脚步行机器人的情形。当机器人被从后侧使劲推时，机器人的上身姿势前倾，必须比悬空脚的假设着地点更大程度地向前迈步以防止机器人的上身向前倒。如果手和脚如规划那样放置则难以防止上身向前倒，结果机器人更为前倾而倒下。(一旦强行求解以图不改变规划好的着地点而防止机器人倒下，则必须在各接触点(脚底)生成过大的接触力并且以极大的力驱动各关节，但这不可能。)这表明这种状态：当因干扰在重心生成大的速度时，在未来规划的接触点的接触位置不能使轨迹物理地稳定。

顺带地说，式(1)的动态表示与重心相关的力的平衡式和力矩的平衡式。然而如果忽视力矩的平衡式则不出现“偏离”。例如假设机器人以一点与地面接触的状况(也即机器人单脚站立的状况)，则机器人具有倒立振子的动态，因而当使机器人无法站立在脚底上的大的外力作用在机器人上时，重心偏离。然而如果忽视力矩的平衡式则机器人的动态仅具有力的平衡式(力与加速度的关系式)，从而不出现“偏离”。即，可以认为，当将力矩的平衡式加至力的平衡式时，生成倒立振子的动态。

因而，通过引入松弛变量允许打破力矩的平衡式。具体地，在式(17)的约束条件中，将松弛项ε_x和ε_y引入力矩的平衡式，从而允许打破力矩的平衡式。

C[j]x[j]＝d[j]

$C\left[j\right]:=\left[\begin{array}{cccccccc}{C}_0\left[j\right]& C_1\left[j\right]& ...& O& C_{i+1}\left[j\right]& O& ...& C_n\left[j\right]\\ O& O& ...& I& O& O& ...& O\\ O& O& ...& O& O& I& ...& O\end{array}\right]$

$d\left[j\right]:=\left[\begin{array}{c}{d}_a\left[j\right]\\ O\\ O\end{array}\right]$

$C_0\left[j\right]:=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& -m\left({\stackrel{..}{z}}_g\right[j]+g)& 0\\ m\left({\stackrel{..}{z}}_g\right[j]+g)& 0& 0& 0\end{array}\right]$

$C_i\left[j\right]:=\left[\begin{array}{cccccc}0& 0& 1& 0& 0& 0\\ 0& -\left(p_{\mathrm{iz}}\right[j]-z_g[j\left]\right)& p_{\mathrm{iy}}\left[j\right]& 1& 0& 0\\ p_{\mathrm{iz}}\left[j\right]-z_g\left[j\right]& 0& -p_{\mathrm{ix}}\left[j\right]& 0& 1& 0\end{array}\right]$

$d_a\left[j\right]:=\left[\begin{array}{c}m\left({\stackrel{..}{z}}_g\right[j]+g)\\ {\stackrel{.}{L}}_x\left[j\right]+{\mathrm{\ϵ}}_x\\ {\stackrel{.}{L}}_y\left[j\right]+{\mathrm{\ϵ}}_y\end{array}\right]\·\·\·\left(31\right)$

将该松弛项引入评估函数(式(12))。

$\begin{array}{c}\min J_1\\ =\frac{1}{2}\underset{j=1}{\overset{N-1}{\mathrm{\Σ}}}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left({\mathrm{\θ}}_i{[k+j+1]}^T{Q}_i{\mathrm{\θ}}_i\right[k+j+1]+{\stackrel{.}{\mathrm{\θ}}}_i{[k+j]}^T{R}_i{\stackrel{.}{\mathrm{\θ}}}_i[k+j\left]\right)+\mathrm{\ρ}\left(\right|{\mathrm{\ϵ}}_x|+|{\mathrm{\ϵ}}_y\left|\right)\end{array}\·\·\·\·\·\left(32\right)$

式(32)中的松弛变量项(ρ(|ε_x|+|ε_y|))通过给松弛变量加绝对值符号而被加至评估函数。这意味着将松弛变量设得尽量接近0。ρ表示乘以松弛变量的权重。

当重心开始偏离时，重心的加速度和猝动急剧增加，以致式(32)的第1项和第2项急剧增大。当通过增加松弛变量以打破力矩的平衡时，偏离被抑制，从而抑制式(32)的第1项和第2项的增加。当然，松弛变量项的值却增加了。

为了更好地理解式(32)和(31)，以下简要地说明该式的意义。

在式(32)可见，松弛项放在二重求和符号外。

形象地说，这表示：当在预测间隔内破坏了力矩的平衡并且重心轨迹偏离时，允许机器人在预测间隔整体稍微倾斜的状态的解。(不过此时在预测间隔内有时允许生成松弛变量、有时又不允许生成松弛变量。这将在后面说明。)

以何程度允许生成松弛变量由Q和R与松弛变量权重ρ之间的比值(均衡度)决定。通过适当地设定权重(Q，R，ρ)的均衡度可以实现如下功能：即，当轨迹不偏离时松弛变量(ε_x，ε_y)保持0，只当轨迹偏离时生成必要且充分的松弛变量。如后面所说明的，“松弛变量值约等于接触点位置的改变量”，因而，用户可以使用松弛变量权重ρ来调整在改变接触点位置时对干扰的敏感程度。

下面使用式(31)和(32)来推导凸二次规划问题。

如下替换松弛项ε_x和ε_y，以去掉绝对值符号。

$\begin{array}{c}\left\{\begin{array}{c}{\mathrm{\ϵ}}_x:={\mathrm{\ϵ}}_{x1}-{\mathrm{\ϵ}}_{x2}\\ {\mathrm{\ϵ}}_y:={\mathrm{\ϵ}}_{y1}-{\mathrm{\ϵ}}_{y2}\end{array}\right.\\ {\mathrm{\ϵ}}_{x1}\≥0,{\mathrm{\ϵ}}_{x2}\≥0\\ {\mathrm{\ϵ}}_{y1}\≥0,{\mathrm{\ϵ}}_{y2}\≥0\end{array}\·\·\·\left(33\right)$

另外，如下扩展输入向量U。也即，将松弛变量引入输入向量U。

$U\left[k\right]:=\left[\begin{array}{c}u\left[k\right]\\ u[k+1]\\ .\\ .\\ .\\ u[k+N-2]\\ u[k+N-1]\end{array}\right]\→U_{\mathrm{\ϵ}}\left[k\right]:=\left[\begin{array}{c}u\left[k\right]\\ u[k+1]\\ .\\ .\\ .\\ u[k+N-2]\\ u[k+N-1]\\ \mathrm{\ϵ}\end{array}\right]\·\·\·\left(34\right)$

ε：[ε_x1 ε_x2 ε_y1 ε_y2]^T

这里，松弛变量项可以由式(33)变换成如下。

$\begin{array}{c}\min \mathrm{\ρ}\left(\right|{\mathrm{\ϵ}}_x|+|{\mathrm{\ϵ}}_y\left|\right)=\min \mathrm{\ρ}({\mathrm{\ϵ}}_{x1}+{\mathrm{\ϵ}}_{x2}+{\mathrm{\ϵ}}_{y1}+{\mathrm{\ϵ}}_{y2})\\ {\mathrm{\ϵ}}_{x1}\≥0,{\mathrm{\ϵ}}_{x2}\≥0,{\mathrm{\ϵ}}_{y1}\≥0,{\mathrm{\ϵ}}_{y2}\≥0\end{array}\·\·\·\left(35\right)$

相应地，式(32)的评估函数J₁表示如下。

$\begin{array}{c}{J}_1=\frac{1}{2}\underset{j=1}{\overset{N-1}{\mathrm{\Σ}}}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left({\mathrm{\θ}}_i{[k+j+1]}^T{Q}_i{\mathrm{\θ}}_i\right[k+j+1]+{\stackrel{.}{\mathrm{\θ}}}_i{[k+j]}^T{R}_i{\stackrel{.}{\mathrm{\θ}}}_i)\\ +\mathrm{\ρ}({\mathrm{\ϵ}}_{x1}{+\mathrm{\ϵ}}_{x2}+{\mathrm{\ϵ}}_{y1}+{\mathrm{\ϵ}}_{y2})\\ =\frac{1}{2}\underset{j=1}{\overset{N-1}{\mathrm{\Σ}}}\left(x{[k+j+1]}^T{Q}_{a}x\right[k+j+1]+u{[k+j]}^T{R}_{a}u[k+j\left]\right)\\ +\left[\begin{array}{cccc}\mathrm{\ρ}& \mathrm{\ρ}& \mathrm{\ρ}& \mathrm{\ρ}\end{array}\right]\mathrm{\ϵ}\\ \begin{array}{cc}=\frac{1}{2}X{[k+1]}^T{Q}_{w}X[k+1]+\frac{1}{2}U{\left[k\right]}^T{R}_{w}U\left[k\right]+{\mathrm{\ρ}}^T\mathrm{\ϵ}& \mathrm{\ρ}:={\left[\begin{array}{cccc}\mathrm{\ρ}& \mathrm{\ρ}& \mathrm{\ρ}& \mathrm{\ρ}\end{array}\right]}^T\end{array}\\ =\frac{1}{2}U{\left[k\right]}^T\mathrm{HU}\left[k\right]+F^{T}U\left[k\right]+{\mathrm{\ρ}}^T\mathrm{\ϵ}+\mathrm{const}\\ =\frac{1}{2}{U}_{\mathrm{\ϵ}}{\left[k\right]}^T\left[\begin{array}{cc}H& O\\ O& O\end{array}\right]U_{\mathrm{\ϵ}}\left[k\right]+{\left[\begin{array}{c}F\\ \mathrm{\ρ}\end{array}\right]}^T{U}_{\mathrm{\ϵ}}\left[k\right]+\mathrm{const}\end{array}\·\·\·\left(36\right)$

另外，进一步整理上式，评估函数J₁表示如下。

$J_1:=\frac{1}{2}{U}_{\mathrm{\ϵ}}{\left[k\right]}^T{H}_{\mathrm{\ϵ}}{U}_{\mathrm{\ϵ}}\left[k\right]+{F_{\mathrm{\ϵ}}}^T{U}_{\mathrm{\ϵ}}\left[k\right]+\mathrm{const}$

$\begin{array}{cc}{H}_{\mathrm{\ϵ}}:=\left[\begin{array}{cc}H& O\\ O& O\end{array}\right]& F_{\mathrm{\ϵ}}:=\left[\begin{array}{c}F\\ \mathrm{\ρ}\end{array}\right]\end{array}\·\·\·\left(37\right)$

接下来考虑约束条件。

如下改写约束条件(式(31))。(明示地表示松弛项，而在前面的式(31)中，松弛项被包括在d[j]中。)

C[j]x[j]＝d[j]+d_ε·····(38)

$d\left[j\right]:=\left[\begin{array}{c}{d}_0\left[j\right]\\ O\\ O\end{array}\right],d_0\left[j\right]:=\left[\begin{array}{c}m\left({\stackrel{..}{z}}_g\right[j]+g)\\ {\stackrel{.}{L}}_x\left[j\right]\\ {\stackrel{.}{L}}_y\left[j\right]\end{array}\right]$

此处应注意，不一定总是生成松弛项。

即，必须区分情况。区分情况是指在预测间隔内，彼此区别包括有能改变位置的接触点的子间隔和不包括能改变位置的接触点的子间隔。

引入式(31)的松弛变量的生成意味着从机器人应物理地满足的机械平衡式(力矩平衡式)偏离。

如后所述，当生成松弛变量时，必须通过改变接触位置来取消松弛项，最终生成不会生成松弛变量的轨迹。

也即，在没有能改变位置的接触点的状况下即使生成松弛项，也不能通过改变接触位置来补偿松弛项，因此不应使约束放松。这可以通过考虑例如预测间隔内的接触点规划表示如图15所示机器人保持双脚站立的状况，而变得容易理解。在这种状况下，不可能通过改变接触位置使机器人稳定化，所以不应由松弛变量来放松约束。

另一方面，在如图16所示的状况中，在该预测间隔的途中左脚和右脚开始着地，因而存在通过改变左脚和右脚的接触位置能使机器人稳定化的子间隔。也就是说，在该子间隔中，可以改变接触点的位置来取消松弛项。因此，在图16的预测间隔中，预测间隔的前半表示“不包括能改变位置的接触点的子间隔”，而在作为悬空脚的左脚着地的时刻以后的间隔表示“包括有能改变位置的接触点的子间隔”。

如果至少有一点能改变位置的接触点，则在该接触点以后的间隔全部是“包括有能改变位置的接触点的子间隔”。

相应地，在“不包括能改变位置的接触点的子间隔”中，松弛项全部为0。

在以下的说明中，将“包括有能改变位置的接触点的子间隔”称作“可调整子间隔”而将“不包括能改变位置的接触点的子间隔”称作“不可调整子间隔”。

$d_{\mathrm{\ϵ}}:=\left[\begin{array}{c}O\\ O\\ O\end{array}\right]\·\·\·\left(39\right)$

另一方面，在“包括有能改变位置的接触点的子间隔”(可调整子间隔)中，引入松弛项以允许松弛项。

$d_{\mathrm{\ϵ}}:=\left[\begin{array}{c}{D}_{\mathrm{\ϵ}}\mathrm{\ϵ}\\ O\\ O\end{array}\right]\·\·\·\left(40\right)$

$D_{\mathrm{\ϵ}}\mathrm{\ϵ}=\left[\begin{array}{c}0\\ {\mathrm{\ϵ}}_{x1}-{\mathrm{\ϵ}}_{x2}\\ {\mathrm{\ϵ}}_{y1}-{\mathrm{\ϵ}}_{y2}\end{array}\right]=\left[\begin{array}{c}0\\ {\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y\end{array}\right]$

$D_{\mathrm{\ϵ}}:=\left[\begin{array}{cccc}0& 0& 0& 0\\ 1& -1& 0& 0\\ 0& 0& 1& -1\end{array}\right]$

ε：＝[ε_x1 ε_x2 ε_y1 ε_y2]^T

以此方式，仅在“包括有能改变位置的接触点的子间隔”(可调整子间隔)使用松弛变量来允许约束放松。

鉴于以上内容，将式(38)变形。

式(38)中的元素可明示地书写如下。

当将式(10)代入式(41)并且移项和整理时，得到下式。

在此情形中，V_ε表达如下。

将松弛项引入V_ε的最后一列。

在此情形中，将前半的松弛项设为0，并且生成允许后半的松弛项。

这清楚地表明：预测间隔的前半是“不包括能改变位置的接触点的子间隔”(不可调整子间隔)，而预测间隔的后半是“包括有能改变位置的接触点的子间隔”(可调整子间隔)。

不一定总是这样。例如，预测间隔整体可以都是“不包括能改变位置的接触点的子间隔”(不可调整子间隔)。

然而，因为预测间隔整体都是“不包括能改变位置的接触点的子间隔”(不可调整子间隔)的情形不需要继续说明，所以前面借助例子说明了在预测间隔内包括可调整子间隔的情形。

另外，根据U_ε扩展式(28)。在留意到如式(33)所示将松弛变量设为非负的同时，导出用于稳定接触点以保持接触的不等条件式。

$\begin{array}{c}\left[\begin{array}{cc}P& O\\ O& -I\end{array}\right]\left[\begin{array}{c}U\left[k\right]\\ \mathrm{\ϵ}\end{array}\right]\≤\left[\begin{array}{c}S\\ O\end{array}\right]\\ \⇔P_{\mathrm{\ϵ}}{U}_{\mathrm{\ϵ}}\left[k\right]\≤S_{\mathrm{\ϵ}}\end{array}\·\·\·\·\left(44\right)$

归纳上式(37)、(42)和(44)，则下式表示为第一凸二次规划问题。

$\begin{array}{c}\underset{U_{\mathrm{\ϵ}}}{\min }\frac{1}{2}{U}_{\mathrm{\ϵ}}^T{H}_{\mathrm{\ϵ}}{U}_{\mathrm{\ϵ}}+F_{\mathrm{\ϵ}}^T{U}_{\mathrm{\ϵ}}\\ s.t.\\ P_{\mathrm{\ϵ}}{U}_{\mathrm{\ϵ}}\≤S_{\mathrm{\ϵ}}\\ V_{\mathrm{\ϵ}}{U}_{\mathrm{\ϵ}}=W\end{array}\·\·\·\left(45\right)$

通过解该第一凸二次规划问题可以获得轨迹。(因为是按需要生成松弛变量，所以，获得按需要包括松弛变量的U_ε[k]，当忽略松弛变量而将U[k]代入式(30)时，获得X[k+1]。也就是说，获得xy重心轨迹和在各接触点生成的力(6轴力)。)

此时，当可允许生成松弛变量值、即在“包括有能改变位置的接触点的子间隔”(可调整子间隔)中无法满足平衡式时，获得将值包括在松弛项中的解(U_ε[k])。

结果，虽然可以避免轨迹的偏离，但是必须通过改变接触位置来补偿松弛项的值。

不用说，当不生成松弛变量也满足平衡式时，以及在“不包括能改变位置的接触点的子间隔”(不可调整子间隔)，松弛变量是0。

严格地说，即使是当不生成松弛变量也满足平衡式时，松弛变量也不为零。作为计算的结果，生成极其微小的值作为松弛变量。例如，获得以10^-10量级生成松弛变量的解。然而，按照常识可以说10^-10等的量级的值基本上为零。

(接触位置改变量的计算)

下面说明从所生成的松弛变量值获得维持稳定性所需的接触位置改变量的计算处理。

当求解式(45)时，获得U_ε[k]并且获得ε，因而使用式(33)获得ε_x和ε_y。

此处，第i接触点的接触位置改变量Δpi表示如下，并且加至式(6)的力矩平衡式。结果得到下式(46)。

Δp_i：＝[Δp_ix Δp_iy Δp_iz]^T

$\begin{array}{c}{\stackrel{.}{L}}_x\left[j\right]+{\mathrm{\ϵ}}_x\\ =-F_z\left[j\right]y_g\left[j\right]+\mathrm{\Σ}\left(\left(p_{\mathrm{iy}}\right[j]+\mathrm{\Δ}{p}_{\mathrm{iy}})f_{\mathrm{iz}}\right[j]-\left(p_{\mathrm{iz}}\right[j]+\mathrm{\Δ}{p}_{\mathrm{iz}}-z_g[j\left]\right)f_{\mathrm{iy}}[j]+{\mathrm{\τ}}_{\mathrm{ix}}[j\left]\right)\end{array}$

$\begin{array}{c}{\stackrel{.}{L}}_y\left[j\right]+{\mathrm{\ϵ}}_y\\ =F_z\left[j\right]x_g\left[j\right]+\mathrm{\Σ}\left(p_{\mathrm{iz}}\right[j]+\mathrm{\Δ}{p}_{\mathrm{iz}}-z_g[j\left]f_{\mathrm{ix}}\right[j]-\left(p_{\mathrm{ix}}\right[j]+\mathrm{\Δ}{p}_{\mathrm{ix}})f_{\mathrm{iz}}[j]+{\mathrm{\τ}}_{\mathrm{iy}}[j\left]\right)\end{array}\·\·\·\left(46\right)$

当从式(46)减去首先应被满足的式(6)时，得到下式。

$\left\{\begin{array}{c}\mathrm{\Σ}\left(\mathrm{\Δ}{p}_{\mathrm{iy}}{f}_{\mathrm{iz}}\right[j]-\mathrm{\Δ}{p}_{\mathrm{iz}}{f}_{\mathrm{iy}}[j\left]\right)={\mathrm{\ϵ}}_x\\ \mathrm{\Σ}\left(\mathrm{\Δ}{p}_{\mathrm{iz}}{f}_{\mathrm{ix}}\right[j]-\mathrm{\Δ}{p}_{\mathrm{ix}}{f}_{\mathrm{iz}}[j\left]\right)={\mathrm{\ϵ}}_y\end{array}\right.\·\·\·\·\left(47\right)$

以下考虑将接触位置改变限定在接触面内的情形。

具体地，以在接触面内平行移动的形式进行接触位置改变的操作。在步行的情形中，该操作相当于使规划好的地面上的着地位置在地面上平行移动。

(可以更自由地移动脚着地(或手放置)位置。如果告知机器人在空间内有何种表面，则可以更自由地选择脚着地(或手放置)位置。例如，手放置位置可以从架子下层变到架子上层。然而，当然增加计算负荷。)

此处，引入接触点在坐标系中的接触位置移动量Δp_i ^l(上标表示小写字母“l”)。

$\mathrm{\Δ}{p}_i^l={\left[\begin{array}{cc}\mathrm{\Δ}{p}_{\mathrm{ix}}^l& \mathrm{\Δ}{p}_{\mathrm{iy}}^l\end{array}\right]}^T\·\·\·\·\left(48\right)$

如图17所示，Δp_ix ^l和Δp_ix ^l由接触点在坐标系中的xy方向变化量定义。将接触点在坐标系中的接触位置移动变换成全局坐标系。

$\left[\begin{array}{c}\mathrm{\Δ}{p}_{\mathrm{ix}}\\ \mathrm{\Δ}{p}_{\mathrm{iy}}\\ \mathrm{\Δ}{p}_{\mathrm{iz}}\end{array}\right]{=\mathrm{\Φ}}_i\left[\begin{array}{c}\mathrm{\Δ}{p}_{\mathrm{ix}}^l\\ \mathrm{\Δ}{p}_{\mathrm{iy}}^l\\ 0\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}^i& r_{12}^i& r_{13}^i\\ r_{21}^i& r_{22}^i& r_{23}^i\\ r_{31}^i& r_{32}^i& r_{33}^i\end{array}\right]\left[\begin{array}{c}\mathrm{\Δ}{p}_{\mathrm{ix}}^l\\ \mathrm{\Δ}{p}_{\mathrm{iy}}^l\\ 0\end{array}\right]=\left[\begin{array}{c}{r}_{11}^i\mathrm{\Δ}{p}_{\mathrm{ix}}^l+r_{12}^i\mathrm{\Δ}{p}_{\mathrm{iy}}^l\\ r_{21}^i\mathrm{\Δ}{p}_{\mathrm{ix}}^l+r_{22}^i\mathrm{\Δ}{p}_{\mathrm{iy}}^l\\ r_{31}^i\mathrm{\Δ}{p}_{\mathrm{ix}}^l+r_{32}^i\mathrm{\Δ}{p}_{\mathrm{iy}}^l\end{array}\right]\·\·\·\·\left(49\right)$

当将上式代入式(47)时可得下式。

$\begin{array}{c}\left\{\begin{array}{c}\mathrm{\Σ}\left((r_{21}^i\mathrm{\Δ}{p}_{\mathrm{ix}}^l+r_{22}^i\mathrm{\Δ}{p}_{\mathrm{iy}}^l)f_{\mathrm{iz}}\right[j]-(r_{31}^i\mathrm{\Δ}{p}_{\mathrm{ix}}^l+r_{32}^i\mathrm{\Δ}{p}_{\mathrm{iy}}^l)f_{\mathrm{iy}}[j\left]\right)={\mathrm{\ϵ}}_x\\ \mathrm{\Σ}((r_{31}^i\mathrm{\Δ}{p}_{\mathrm{ix}}^l+r_{32}^i\mathrm{\Δ}{p}_{\mathrm{iy}}^l)f_{\mathrm{ix}}j]-(r_{11}^i\mathrm{\Δ}{p}_{\mathrm{ix}}^l+r_{}^{12}\mathrm{\Δ}{p}_{\mathrm{iy}}^l{)f}_{\mathrm{iz}}\left[j\right])={\mathrm{\ϵ}}_y\end{array}\right.\\ \⇒\left\{\begin{array}{c}\mathrm{\Σ}(\left(r_{21}^i{f}_{\mathrm{iz}}\right[j]-r_{31}^i{f}_{\mathrm{iy}}[j\left]\right)\mathrm{\Δ}{p}_{\mathrm{ix}}^l+\left(r_{22}^i{f}_{\mathrm{iz}}\right[j]-r_{32}^i{f}_{\mathrm{iy}}[j\left]\right)\mathrm{\Δ}{p}_{\mathrm{iy}}^l)={\mathrm{\ϵ}}_x\\ \mathrm{\Σ}(\left(r_{31}^i{f}_{\mathrm{ix}}\right[j]-r_{11}^i{f}_{\mathrm{iz}}[j\left]\right)\mathrm{\Δ}{p}_{\mathrm{ix}}^l+\left(r_{32}^i{f}_{\mathrm{ix}}\right[j]-r_{12}^i{f}_{\mathrm{iz}}[j\left]\right)\mathrm{\Δ}{p}_{\mathrm{iy}}^l)={\mathrm{\ϵ}}_y\end{array}\right.\end{array}$

$\⇒\left\{\begin{array}{c}\mathrm{\Σ}\left({\mathrm{\σ}}_{11}^i\right[j]\mathrm{\Δ}{p}_{\mathrm{ix}}^l+{\mathrm{\σ}}_{12}^i[j\left]\mathrm{\Δ}{p}_{\mathrm{iy}}^l\right)={\mathrm{\ϵ}}_x\\ \mathrm{\Σ}\left({\mathrm{\σ}}_{21}^i\right[j]\mathrm{\Δ}{p}_{\mathrm{ix}}^l+{\mathrm{\σ}}_{22}^i[j\left]\mathrm{\Δ}{p}_{\mathrm{iy}}^l\right)={\mathrm{\ϵ}}_y\end{array}\right.\·\·\·\left(50\right)$

其中，

${\mathrm{\σ}}_{11}^i\left[j\right]:=r_{21}^i{f}_{\mathrm{iz}}\left[j\right]-r_{31}^i{f}_{\mathrm{iy}}\left[j\right]$

${\mathrm{\σ}}_{12}^i\left[j\right]:=r_{22}^i{f}_{\mathrm{iz}}\left[j\right]-r_{32}^i{f}_{\mathrm{iy}}\left[j\right]$

${\mathrm{\σ}}_{21}^i\left[j\right]:=r_{31}^i{f}_{\mathrm{ix}}\left[j\right]-r_{11}^i{f}_{\mathrm{iz}}\left[j\right]$

${\mathrm{\σ}}_{22}^i\left[j\right]:=r_{32}^i{f}_{\mathrm{ix}}\left[j\right]-r_{12}^i{f}_{\mathrm{iz}}\left[j\right]$

在此情形中，用于补偿所生成的松弛变量所需的接触位置改变量的条件可以表示如下。

if ε_x≥0

$\mathrm{\Σ}\left({\mathrm{\σ}}_{11}^i\right[j]\mathrm{\Δ}{p}_{\mathrm{ix}}^l+{\mathrm{\σ}}_{12}^i[j\left]\mathrm{\Δ}{p}_{\mathrm{iy}}^l\right)\≥{\mathrm{\ϵ}}_x$

else

$\mathrm{\Σ}\left({\mathrm{\σ}}_{11}^i\right[j]\mathrm{\Δ}{p}_{\mathrm{ix}}^l+{\mathrm{\σ}}_{12}^i[j\left]\mathrm{\Δ}{p}_{\mathrm{iy}}^l\right)\≤{\mathrm{\ϵ}}_x$

end

if ε_y≥0

$\mathrm{\Σ}\left({\mathrm{\σ}}_{21}^i\right[j]\mathrm{\Δ}{p}_{\mathrm{ix}}^l+{\mathrm{\σ}}_{22}^i[j\left]\mathrm{\Δ}{p}_{\mathrm{iy}}^l\right)\≥{\mathrm{\ϵ}}_y$

else

$\mathrm{\Σ}\left({\mathrm{\σ}}_{21}^i\right[j]\mathrm{\Δ}{p}_{\mathrm{ix}}^l+{\mathrm{\σ}}_{22}^i[j\left]\mathrm{\Δ}{p}_{\mathrm{iy}}^l\right)\≤{\mathrm{\ϵ}}_y$

end

····(51)

式(51)表示通过改变接触位置得到的效果(左边)最好是等于或大于松弛变量(右边)。

以符号的方式可以将上式整理成如下。

$\left\{\begin{array}{c}-\mathrm{sgn}\left({\mathrm{\ϵ}}_x\right)\mathrm{\Σ}\left({\mathrm{\σ}}_{11}^i\right[j]\mathrm{\Δ}{p}_{\mathrm{ix}}^l+{\mathrm{\σ}}_{12}^i[j\left]\mathrm{\Δ}{p}_{\mathrm{iy}}^l\right)\≤-\mathrm{sgn}\left({\mathrm{\ϵ}}_x\right){\mathrm{\ϵ}}_x\\ -\mathrm{sgn}\left({\mathrm{\ϵ}}_y\right)\mathrm{\Σ}\left({\mathrm{\σ}}_{21}^i\right[j]\mathrm{\Δ}{p}_{\mathrm{ix}}^l+{\mathrm{\σ}}_{22}^i[j\left]\mathrm{\Δ}{p}_{\mathrm{iy}}^l\right)\≤-\mathrm{sgn}\left({\mathrm{\ϵ}}_y\right){\mathrm{\ϵ}}_y\end{array}\right.\·\·\·\left(52\right)$

可以将式(52)整理如下。

$\mathrm{\Σ}{\mathrm{\Λ}}_i\left[j\right]\mathrm{\Δ}{p}_i^l\≤{\mathrm{\σ}}_{\mathrm{\ϵ}}\·\·\·\left(53\right)$

$\begin{array}{cc}{\mathrm{\Λ}}_i\left[j\right]:=-\left[\begin{array}{cc}\mathrm{sgn}\left({\mathrm{\ϵ}}_x\right){\mathrm{\σ}}_{11}^i\left[j\right]& \mathrm{sgn}\left({\mathrm{\ϵ}}_x\right){\mathrm{\σ}}_{12}^i\left[j\right]\\ \mathrm{sgn}\left({\mathrm{\ϵ}}_y\right){\mathrm{\σ}}_{21}^i\left[j\right]& \mathrm{sgn}\left({\mathrm{\ϵ}}_y\right){\mathrm{\σ}}_{22}^i\left[j\right]\end{array}\right]& {\mathrm{\σ}}_{\mathrm{\ϵ}}:=\end{array},\left[\begin{array}{c}-\mathrm{sgn}\left({\mathrm{\ϵ}}_x\right){\mathrm{\ϵ}}_x\\ -\mathrm{sgn}\left({\mathrm{\ϵ}}_y\right){\mathrm{\ϵ}}_y\end{array}\right]$

顺带地说，接触位置的改变量当然是仅当存在对象接触点时才被允许。

例如，在图18所示的例子中，在“不包括能改变位置的接触点的子间隔”(不可调整子间隔)当然没有接触位置改变量。

而且在“包括有能改变位置的接触点的子间隔”(可调整子间隔)中，仅在包括有能实际改变脚着地(或手放置)位置的对象的地点才定义接触位置的改变量。

具体地，Δp_LF2 ^l(左脚的着地位置的改变)只在k+s～k+N的范围中被允许，Δp_RF2 ^l(右脚的着地位置的改变)只在k+N时被允许。(为了容易理解，将i表示为“LF”和“RF”。)

相应地，根据例如该图18所示的例子改写式(41)，得到下式。

$\begin{array}{cc}{\mathrm{\Λ}}_{\mathrm{LF}2}\left[j\right]\mathrm{\Δ}{p}_{\mathrm{LF}2}^l\≤{\mathrm{\σ}}_{\mathrm{\ϵ}}& (j=k+s,...,k+N-1)\\ {\mathrm{\Λ}}_{\mathrm{LF}2}\left[j\right]\mathrm{\Δ}{p}_{\mathrm{LF}2}^l+{\mathrm{\Λ}}_{\mathrm{RF}2}\left[j\right]\mathrm{\Δ}{p}_{\mathrm{RF}2}^l\≤{\mathrm{\σ}}_{\mathrm{\ϵ}}& (j=k+N)\end{array},...\left(54\right)$

另外，定义了将接触位置改变量纵向排列的向量UΔ。

$U_{\mathrm{\Δ}}:=\left[\begin{array}{c}\mathrm{\Δ}{p}_1^l\\ \mathrm{\Δ}{p}_2^l\\ .\\ .\\ .\end{array}\right]\·\·\·\left(55\right)$

然而，在本例中，

$U_{\mathrm{\Δ}}:=\left[\begin{array}{c}\mathrm{\Δ}{p}_{\mathrm{LF}2}^l\\ \mathrm{\Δ}{p}_{\mathrm{RF}2}^l\end{array}\right]$

式(54)可以被整理如下。

$\begin{array}{c}\left[\begin{array}{cc}{\mathrm{\Λ}}_{\mathrm{LF}2}[k+s]& O\\ .& .\\ .& .\\ .& .\\ {\mathrm{\Λ}}_{\mathrm{LF}2}[k+N-1]& O\\ {\mathrm{\Λ}}_{\mathrm{LF}2}[k+N]& {\mathrm{\Λ}}_{\mathrm{RF}2}[k+N]\end{array}\right]\left[\begin{array}{c}\mathrm{\Δ}{p}_{\mathrm{LF}2}^l\\ \mathrm{\Δ}{p}_{\mathrm{RF}2}^l\end{array}\right]\≤\left[\begin{array}{c}{\mathrm{\σ}}_{\mathrm{\ϵ}}\\ .\\ .\\ .\\ {\mathrm{\σ}}_{\mathrm{\ϵ}}\\ {\mathrm{\σ}}_{\mathrm{\ϵ}}\end{array}\right]\\ \⇔P_{\mathrm{\Δ}}{U}_{\mathrm{\Δ}}\≤S_{\mathrm{\Δ}}\end{array}\·\·\·\·\left(56\right)$

式中：

$P_{\mathrm{\Δ}}:=\left[\begin{array}{cc}{\mathrm{\Λ}}_{\mathrm{LF}2}[k+s]& O\\ .& .\\ .& .\\ .& .\\ {\mathrm{\Λ}}_{\mathrm{LF}2}[k+N-1]& O\\ {\mathrm{\Λ}}_{\mathrm{LF}2}[k+N]& {\mathrm{\Λ}}_{\mathrm{RF}2}[k+N]\end{array}\right]$

$S_{\mathrm{\Δ}}:=\left[\begin{array}{c}{\mathrm{\σ}}_{\mathrm{\ϵ}}\\ .\\ .\\ .\\ {\mathrm{\σ}}_{\mathrm{\ϵ}}\\ {\mathrm{\σ}}_{\mathrm{\ϵ}}\end{array}\right]$

假设希望接触位置的改变量最小化(也即提供最小必要的接触位置的改变量)，则进一步引入下式的评估函数。

$\min J_2=\frac{1}{2}\mathrm{\Σ}{\left|\mathrm{\Δ}{p}_i^l\right|}^2\·\·\·\left(57\right)$

式(57)可以被整理如下。

$J_2=\frac{1}{2}\mathrm{\Σ}{\left|\mathrm{\Δ}{p}_i^l\right|}^2=\frac{1}{2}{U}_{\mathrm{\Δ}}^T{U}_{\mathrm{\Δ}}\·\·\·\left(58\right)$

当化简式(56)和式(58)时，最终得到下式。

$\underset{U_{\mathrm{\Δ}}}{\min }\frac{1}{2}{U}_{\mathrm{\Δ}}^T{U}_{\mathrm{\Δ}}$

s.t. P_ΔU_Δ≤S_Δ···(59)

这样一来就能构建第二凸二次规划问题。

通过求解该问题，可以获得用于维持稳定性所需的最小的接触位置改变量。

通过对式(59)的不等式约束条件追加条件，可以容易实现例如将接触力改变量限制在某一凸区域内(考虑到可移动范围)等功能。

(移动机器人的控制)

下面说明用于移动机器人的移动控制方法。

图19表示用于移动机器人的移动控制方法的整体流程。

控制移动机器人的移动的主要步骤包括：

接触点规划的设定(ST100)；轨迹生成(ST200)；运动指令(ST300)；运动控制的执行(ST400)。

如上面所概略说明的，在接触点规划的设定(ST100)中，用户将表示以怎样的顺序放置手和脚、以及在何处和如何放置手和脚的多点接触移动的规划(图7)输入到移动机器人。

所输入的接触点规划被存储在控制器210的接触点规划设定单元221。

下面说明轨迹生成步骤(ST200)。

图20、21和22是用于说明轨迹生成步骤(ST200)的程序的详细的流程图。

轨迹生成步骤(ST200)由控制器210的轨迹生成单元222执行。

首先，在ST201中，轨迹生成单元222设定当前状态x[k]。在此，作为当前状态x[k]应设定当前得到的重心的x坐标、重心的x轴方向速度、重心的y坐标、重心的y轴方向速度和各接触点的接触力(6轴力)。各接触点中的接触力可以由来自力传感器29的检测值得到。重心的x坐标、重心的x轴方向速度、重心的y坐标和重心的y轴方向速度是从移动机器人的当前状态估计和获得的值。例如，通过获取来自各编码器的传感器值并且掌握移动机器人的全身姿势，可获得重心的x坐标、重心的x轴方向速度、重心的y坐标和重心的y轴方向速度。或者，通过掌握实际的移动机器人的姿势从假设的姿势倾斜(偏离)了何种程度，可以校正重心位置。

接下来，在ST202A中，对各接触点候选设定权重(Q_i、R_i)。(注意在前面的式子的展开中，权重(Q_i、R_i)在式(11)中被合并至评估函数J。在放松约束并且合并松弛项的式(32)的评估函数J₁中，也沿用该权重(Q_i、R_i)。)

权重(Q_i、R_i)是用户预先设定和输入的，轨迹生成单元222按需要读出该权重，并且更新和设定该权重。(如果不必要更新，则没有必要重设该权重。)

当将力均等地分配给接触点候选时，例如可以将Q_i全部设为1并且将R_i全部设为1×10^-6。还可以做出进一步更详细的设定。

该权重的调整使得可以做出使接触力的改变平滑到何种程度的调整、以及将很小的力施加至手的调整等调整。

例如，设施加全部相同的权重。作为算术运算结果，得到将力均等地施加至放置好的手和脚(各接触点)的解。一般地认为此情形是移动机器人的最稳定的形式(姿势)。

另一方面，在某些情形中需要改变权重的分配。

例如，在移动机器人的手比脚更弱(更脆弱)并且最好不给手施加过重的负荷的情形中，手上的权重“增大”。随着权重增大，乘以权重的手的值θi等减小，因而接触力对手的分配减小。

另外，假设一只手放在桌上、另一只手向前方大幅伸出的情形。在此情形中，将力均等地分配给手和脚是不充分的，需要使身体更加向前倾斜。为了使身体倾斜并且将重心更加向前方平移，需要略微增大施加在一只手上的力的分配。在此情形中，需要做出减小一只手的权重以使身体倾斜、并且使另一只手大幅向前方伸出的调整。

虽然在一般的操作中没有必要改变权重，但是可以取决于状况每当更新轨迹时改变权重。

在ST202B中，设定松弛项权重ρ。

注意，松弛项权重ρ是在式(32)中引入的。如前所述，以何程度允许生成松弛变量由Q和R与松弛变量权重ρ之间的比值(均衡度)决定。

接下来，在ST203中，设定预测间隔。

具体地，如以上参照图8说明的，将从现在时间起预定的持续时间(例如1.6秒)设定为预测间隔。

接下来，在ST204中，设定该预测间隔内的接触点位置和姿势(pi，ri)。该操作相当于从接触点规划(图7)中切出预测间隔内的接触点规划。

接下来，在ST205中，临时设定重心竖直轨迹和绕重心力矩。

在下一算术运算步骤(ST206)中解式(45)(凸二次规划问题)而得到重心的xy方向轨迹、各接触点的接触力和接触力微分值。然而，该求解是以给出各接触点的位置和姿势、重心竖直轨迹和绕重心力矩为前提的。

由接触点规划给出预测间隔内的各接触点的位置和姿势。

另一方面，该预测间隔内的重心竖直轨迹和绕重心力矩未知。因而，临时设定一次重心竖直轨迹和绕重心力矩，以在算术运算步骤(ST206)中解式(45)(凸二次规划问题)。

在第一个控制周期中，例如，以重心竖直轨迹和绕重心力矩在预测间隔内从当前状态不动的方式临时设定重心竖直轨迹和绕重心力矩。(例如，可以假设重心高度不变。)

或者，在前一控制周期使用的重心竖直轨迹和绕重心力矩可以被用作临时值。

预测间隔是像1.6秒这样微小的时间量。在最终获得的稳定轨迹中，该预测间隔的重心高度的改变也很微小。

相应地，有可能假设重心高度不变、或假设前一控制周期使用的重心竖直轨迹和绕重心力矩被用作临时值而推进第一次临时计算。

在后续步骤(ST209)中，校正重心竖直轨迹和绕重心力矩，以在由临时算术运算获得的重心的xy方向轨迹处也能稳定移动机器人的移动。接着使用经校正的重心竖直轨迹和经校正的绕重心力矩重复循环以再度执行算术运算(ST206)，因此能理解：最终能正确地获得包括重心竖直轨迹的稳定的轨迹。

此处，定义绕重心力矩。

当将移动机器人看作绞链在各关节处结合在一起的绞链构造体时，绕重心力矩是由分布到各绞链的质点的运动绕重心生成的力矩的总和值。

设移动机器人由图23A所示的多质点模型来表示，并且M个质点各自具有如下参数，

质量：m_i，

位置：s_i＝[x_i，y_i，z_i]^T

设移动机器人整体的重心由G＝(G_x，G_y，G_z)来表示，则绕重心力矩表示如下(见图23B)。

$\stackrel{.}{L}=\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{m}_i(s_i-G)\×({\stackrel{..}{s}}_i-\stackrel{..}{G})\·\·\·\left(60\right)$

接下来，在ST206A中执行第一算术运算步骤。具体地，解式(45)所示的第一凸二次规划问题。在预测间隔内将力适当地分散至各接触点、并且实现该接触力，同时获得重心(此处x和y对应于重心)可被稳定地平移的轨迹。

具体地，获得H_ε、F_ε、P_ε、S_ε、V_ε和W_ε，还获得U_ε[k]。(该U_ε[k]表示接触力微分值的向量。如上所述，扩展该向量以在式(34)引入松弛变量。另外，如上所述，在V_ε中，彼此区别“不包括能改变位置的接触点的子间隔”(不可调整子间隔)和“包括有能改变位置的接触点的子间隔”(可调整子间隔)。参见式(43)。)

接着由式(30)获得X_out[k+1](ST206B)。(该X_out[k+1]表示重心的x坐标、重心的x轴方向速度、重心的y坐标、重心的y轴方向速度和各接触点的接触力(6轴力)的时间系列数据。)

另外，在ST207A中执行第二算术运算步骤。具体地，解式(59)所示的第二凸二次规划问题。通过求解该问题，可以获得用于维持稳定性所需的最小的接触位置改变量。相应地，在ST207B中校正接触点位置。

此后，在ST208中，通过间插获得手和脚的轨迹。

在接触点规划中，仅设定接触点和此时的姿势。另外，在前面的步骤ST207B中改变接触位置以维持稳定性。基于这些因素，获得手和脚的间插轨迹以连接各接触点。结果，如图24所示，插入了在接触点规划中指示的接触点。

接下来，在ST209中，进行用于校正重心竖直轨迹和绕重心力矩的操作。

首先校正重心竖直轨迹。

在前面的步骤中，得到了重心的xy方向轨迹和手和脚的轨迹。(另外还临时设定了重心竖直轨迹。)基于这些因素，可以估计预测间隔内的移动机器人的姿势的改变。然而，此时如果有例如膝或臂充分伸开等不稳定的因素，则调整重心竖直轨迹以维持稳定。例如，以这样的方式进行调整：如果膝快要充分伸开则降低重心竖直轨迹。在确定了预测间隔内的移动机器人的姿势后，基于该姿势计算绕重心力矩。

接下来，在ST210中，判断是否重复至少一次步骤ST206A～ST209。如果不重复那些步骤，则处理返回ST206A以获得第一凸二次规划问题的解。

使用通过重复该计算(ST206)校正的重心竖直轨迹和绕重心力矩，再度获得X_out[k+1]和UΔ[k]。

以此方式，获得在某一预测间隔内实现移动机器人的稳定的移动所必需的重心位置、重心速度、必要的接触点位置的改变和接触力的时间系列数据(ST211)。

这样，在具有某持续时间的预测间隔生成稳定的轨迹，并且仅使用最初一点作为当前输入值。即，只将最初一点作为操作指令输出至运动控制单元223(ST300)。

更新周期(ST213)即令当前时刻前进微小的时间量，直到满足预定的结束条件为止(ST213)，并且处理返回第一步骤(ST201)以重复循环。以此方式，使预测间隔只前进微小的时间量、同时重复轨迹生成。这样一来，进行稳定地实现多点接触移动的轨迹的计算。

接收操作指令的运动控制单元223控制移动机器人100运动(驱动它的关节)的方法尽管不是本示例实施例的特征，但也将简单地说明。

在本示例实施例中，不仅是重心以及手和脚的轨迹、还有各接触点中的接触力都被作为操作指令而给出。相应地，需要进行手和脚的位置(轨迹)的细微调整、以生成在各接触点指示的接触力的步骤(ST410)。例如，在当移动机器人的手放在桌上时指示较大的接触力的情形中，移动机器人进行手的轨迹的细微调整以使手按住桌子。

驱动设在各关节处的马达以使移动机器人的全身操作(ST420)。这使得移动机器人能操作。

在操作的执行(ST400)期间，因进行接触力控制(ST410)、未能如指示那样正确地驱动关节、或受到干扰的影响，实际机器人与模型之间出现偏差。相应地，在轨迹生成(ST200)的循环中，每次都需要设定当前状态x(ST201)。

如上所述，根据该示例实施例，移动机器人可以基于所设定的接触点的规划自动地生成稳定的重心轨迹、同时自主地移动。

此时，如果由根据接触点规划的脚着地(或手放置)不能保持机器人的稳定，则通过按需要改变接触位置来避免轨迹的偏离，从而使机器人能继续执行稳定运动。

从用于轨迹生成的评估函数J₁的意义可见，与接触点的切换(接触状态的平移)伴随的力的变化变得平滑，从而实现了移动机器人的稳定且平滑的移动。

(实验例)

下面说明实验例。

设将四脚步行机器人的手和脚可以随机地放置的接触点规划给予机器人。

图25表示该接触点规划的时间表。

图26和27表示当没有干扰时得到的仿真结果。

图26表示执行结果的快照。

图27是表示执行期间的重心位置、反作用力中心位置和各接触点的竖直方向负荷的图线。

在此情形中，很明显，手和脚放在根据接触点规划的着地位置(或手放置位置)、同时与接触点的切换伴随的接触点反作用力平滑地平移，并且实现了平滑的重心移动。

另一方面，图28、29和30表示当出现干扰时得到的仿真结果。

图28表示执行结果的快照。此处，设约2.2秒左右输入大的干扰。

图29是表示执行期间的重心位置、反作用力中心位置和各接触点的竖直方向负荷的图线。

图30是表示松弛变量值的图线。

当将图26与图28作比较时，明显看出：通过在输入干扰后(2.2秒左右)立即改变左脚的着地位置，从而防止机器人跌倒。从图30可见，在输入干扰后立即生成松弛变量，而此后，在没有干扰的状态松弛变量变成0。

注意本发明不限于以上示例实施例，并且可以不脱离本发明的范围而适当地变更。

移动机器人的硬件构造无特别的限制。例如，移动机器人可以是双脚步行机器人或四脚步行机器人，另外移动机器人的手和脚的数目无特别的限制。例如，移动机器人可以有6脚或8脚。

然而，需要至少一个接触点接地。(接地可以包括机器人的手放在墙壁的情形。)

在极端的情形中，例如，如图31所示，移动机器人100可以具有1只脚和1只手，并且使它的手和脚交替地放在地板和墙壁上、同时移动。

按需要可以进行获得必要的接触点的改变量(ST207A)并且根据获得的改变量改变接触点(ST207B)的处理。

也即，在ST206A与ST207A之间可以并入将松弛变量的大小与预定阈值作比较的判断步骤，从而判断是否生成了基本上等于或大于零的松弛变量。

当生成了基本上等于或大于零的松弛变量时，必须改变接触点位置。当松弛变量基本上为零时，不必执行步骤ST207A和ST207B的处理。

或者，可无论松弛变量的大小如何都执行ST207A，并在ST207A与ST208B之间并入将接触点位置的改变量与预定阈值作比较的判断步骤。

当接触点位置的改变量等于或大于预定值时，必须改变接触点(ST207B)。当接触点位置的改变量基本上为零时，不必执行ST207B。

虽然阈值由要求的稳定性的精度决定，但是也可以说当接触点位置的改变量是1mm或2mm时必须改变接触点。

取决于要求的稳定性的精度，也可以设0.01mm或0.02mm为改变接触点的阈值。

通过当接触点位置的改变量是1mm或2mm时的松弛变量的逆计算，也可以得到判定松弛变量的大小的阈值。

从如上说明的本发明可以明显看出，本发明的实施例可以以多种途径变形。这种变形不被认为脱离了本发明的精神和范围，并且所有这些对本领域熟练人员显而易见的变更意在被包括在后附权利要求书的范围内。

Claims (8)

1.一种用于移动机器人的移动控制方法，所述移动机器人当使得两个或更多个移动装置与地交替地接触时移动，所述移动控制方法包括：

接触点规划设定步骤：使用表示所述移动装置与地接触的接触点的位置的时间系列数据来设定接触点规划；以及

轨迹生成步骤：生成所述移动机器人当如所述接触点规划设定步骤中所设定的那样使得所述移动装置与地接触于所述接触点时的移动的轨迹，

其中，所述轨迹生成步骤包括当所述轨迹偏离时改变所述接触点的位置。

2.根据权利要求1所述的用于移动机器人的移动控制方法，其中，所述轨迹生成步骤包括：

构造描述对所述移动机器人的控制输入与所述移动机器人的运动之间的关系的预测模型，所述预测模型表示所述移动机器人在预定持续时间的预测间隔中的状态变化；

在所述预测间隔中，通过使用基于机械平衡的约束条件和预定的稳定性评估准则来获得用于确保所述移动机器人的稳定性的关于所述控制输入的时间系列数据；

基于所获得的关于所述控制输入的时间系列数据和所述预测模型来获得所述移动机器人的轨迹；

将用于放松约束的松弛变量引入到所述约束条件中；

以如下方式将松弛变量引入到所述稳定性评估准则中：当在所述约束条件下获得稳定轨迹的解时所述松弛变量变为基本为零的值，而当在所述约束条件下轨迹偏离时生成等于或大于基本为零的值的值，以作为所述松弛变量；以及

改变所述接触点的位置以取消所述松弛变量的所生成的值。

3.根据权利要求2所述的用于移动机器人的移动控制方法，其中，当将所述松弛变量引入到所述约束条件中时，将所述松弛变量引入到所述移动机器人的重心的绕X轴力矩的平衡方程中和所述移动机器人的重心的绕Y轴力矩的平衡方程中，X轴和Y轴在与对应于竖直方向的Z轴垂直的平面内彼此正交。

4.根据权利要求2或3所述的用于移动机器人的移动控制方法，其中，

所述预测间隔被分成不包括能够被改变位置的接触点的不可调整的子间隔，和包括一个或更多个能够被改变位置的接触点的可调整的子间隔，并且

仅在所述可调整的子间隔期间允许生成等于或大于基本为零的值的值作为所述松弛变量。

5.根据权利要求2至4中的任一项所述的用于移动机器人的移动控制方法，其中，

所述约束条件的表达式包括所述移动机器人的重心竖直轨迹和绕所述移动机器人的重心的力矩的轨迹，并且

所述轨迹生成步骤包括：

在所述预测间隔内临时设定一次所述移动机器人的重心竖直轨迹和绕重心的力矩的轨迹；

在所述约束条件下获得一次所述稳定性评估准则的函数的解，基于所获得的结果改变所述接触点的位置以在所述预测间隔内稳定所述移动机器人的运动，以及校正所述移动机器人的重心竖直轨迹和绕重心的力矩的轨迹；以及

通过使用所述接触点的经改变的位置、经校正的所述重心竖直轨迹和经校正的所述绕重心的力矩的轨迹来再计算至少一次所述稳定性评估准则的函数的解。

6.根据权利要求2至5中的任一项所述的用于移动机器人的移动控制方法，其中，

在所述轨迹生成步骤中在所述预测间隔内获得所述轨迹；

通过将关于所述预测间隔的第一点的数据用作当前输入值来驱动所述移动机器人；以及

通过使所述预测间隔前进少量时间来重复所述轨迹生成步骤。

7.一种移动机器人，所述移动机器人当使得两个或更多个移动装置与地交替地接触时移动，所述移动机器人包括：

接触点规划设定单元，所述接触点规划设定单元使用表示所述移动装置与地接触的接触点的位置的时间系列数据来设定接触点规划；以及

轨迹生成单元，所述轨迹生成单元生成所述移动机器人当如所述接触点规划设定单元所设定的那样使得所述移动装置与地接触于所述接触点时的移动的轨迹，

其中，所述轨迹生成单元当所述轨迹偏离时改变所述接触点的位置。

8.根据权利要求7所述的移动机器人，其中，

所述轨迹生成单元被配置成：

构造描述对所述移动机器人的控制输入与所述移动机器人的运动之间的关系的预测模型，所述预测模型表示所述移动机器人在预定持续时间的预测间隔中的状态变化；

在所述预测间隔中，通过使用基于机械平衡的约束条件和预定的稳定性评估准则来获得用于确保所述移动机器人的稳定性的关于所述控制输入的时间系列数据；

基于所获得的关于所述控制输入的时间系列数据和所述预测模型来获得所述移动机器人的轨迹；

将用于放松约束的松弛变量引入到所述约束条件中；

以如下方式将松弛变量引入到所述稳定性评估准则中：当在所述约束条件下获得稳定轨迹的解时所述松弛变量变为基本为零的值，而当在所述约束条件下轨迹偏离时生成等于或大于基本为零的值的值，以作为所述松弛变量；以及

改变所述接触点的位置以取消所述松弛变量的所生成的值。