一种基于模型预测的人机协同控制方法
一种基于模型预测的人机协同控制方法
【技术领域】
[0001] 本发明属于机器人技术领域，尤其涉及一种基于模型预测的人机协同控制方法。
【背景技术】
[0002] 在对机器人运动自主探索的过程中，避障是移动机器人必需的一项基本功能。传 统的机器人控制技术集中于探索利用机器人自身传感器实施避障的方法，如利用红外传感 器、超声波传感器、激光传感器等。尽管传感器技术与环境建模技术的发展为机器人自主避 障、自主导航带来了巨大的改变，但复杂环境中单纯依靠传感器进行避障仍是不充分的，为 此需要操作员帮助机器人实现避障。
[0003] 在操作员通过遥控协助机器人行走、避障方面，已有的做法集中在现有的机器人 自主避障控制器基础上结合操作员控制指令。即将原有的机器人控制器的控制输出与操作 员的控制输出以某种方式相结合，得到新的控制器。
[0004] 一种新的思路是直接利用操作员的控制指令设计机器人的避障控制器。美国的 MagnusEgerstedt教授及其学生（ChipalkattyR，EgerstedtM.Human-in-the-loop: Terminalconstraintrecedinghorizoncontrolwithhumaninputs[C]//Roboticsand Automation(ICRA)，2010IEEEInternationalConferenceon.IEEE，2010:2712_2717)提 出了一种基于模型预测控制的人机协同控制器设计思路，其思路为：设被控机器人系统的 模型为xk+1=Axk+Buk;根据控制员的当前输入vk、过去输入，预测控制员的未来输入序列 IVk+i};求取控制序列U= {uk，uk+1，…，Uk+Nj^}，使得机器人在该控制输入下，能在N步 后到达目标状态：Mxk+Nk=b，且该控制序列与预测的控制员输入序列最为相似，即其能最 小化下式的代价函数：
[0006] 每一时刻采用求得的控制序列中的第一项进行控制，控制量uk具有形式：
每一时刻的预测窗 口Nk取为固定值或依预测精度进行动态调整，即预测操作员的控制输入越精确时，窗口越 长。
[0007] 但这一控制方法并不能有效地解决地面移动机器人的协同控制问题，其原因有以 下几点：
[0009] 1、地面移动机器人的控制模型最常见的为A=I，B=I的情形，采用零阶保持器 预测方法，末态约束为点约束（即M=I)时，求得的控制量具有以下形式：
[0011] 该形式表明，无论操作员的控制量vk为多少，控制器都将输出相同的控制量给被 控机器人。显然，这与利用操作员控制量进行避障的思想是相违背的。
[0012] 2、当不采用零阶保持器器时，而仍采用A=I，B=I的模型时，控制器的实际输出 将可能出现与操作员控制"反向"的情况。
[0013] 譬如当b= [0,1]T，xk= [0, 0]T，N= 2,vk=v' k+(l= [1，1]T，v' k+1= [2, 2]T 时，代入解得
此式表明，当操作员给出了右向45度 的一个控制量时，实际给机器人的控制量却是向左转直行，这与利用操作员控制量进行避 障的意图也是相违背的。
【发明内容】
[0014] 为解决上述问题，本发明提供一种基于模型预测的人机协同控制方法，该方法，该 方法可以利用操作员的控制量协助机器人进行行驶，同时机器人又能自主驶向预定目标 点、目标区域。
[0015] 本发明的基于模型预测的人机协同控制方法，其包括以下步骤：
[0016] 假设被控机器人的模型为：xk+1=Axk+Buk，目标位置满足：Mxk=b，xk表示第k个 时刻被控机器人的位置坐标，uk表示第k个时刻被控机器人的控制量;A和B是由被控机器 人实际物理模型决定的系统参数矩阵;M，b是目标位置满足的线性方程组的参数矩阵，由 目标的实际物理位置决定；
[0017] 步骤1、按被控机器人的目标位置与初始位置的距离、接收操作员指令的频率、机 器人的移动速度与任务需要设置被控机器人的模型预测窗口长度的最大值1"和最小值Nfflin；
[0018] 步骤2,设0时刻的预测窗口长度为％，则令队=Nmax;
[0019] 步骤3、将k时刻预测窗口长度Nk按下式进行缩短：
[0021] 步骤4,根据滤波与预测算法结合操作员的当前控制指令vk预测操作员的未来Nk 个时刻的控制指令
；i= 〇，1，…，（Nk-1)，其中Vk =
[0022] 步骤5,结合所述未来Nk个时刻的控制指令
利用下式计算当前时刻控制被控机器人实际运动的控制量uk:
[0024] 其中，
A 未来第i个时刻控制量的非对称权重，且满足： 入 0〉人J〉..〉人（Nk_l)。
[0025] 进一步的，当被控机器人模型为A=I，B=I且目标约束中M=I时，I为单位矩 阵；设置中间量Po,令
代入式⑴将Uk简化为：
[0027] 进一步的，采用零阶保持器的预测方法，即令v' k+i=vk时，则式（2)转换为式 (3)：
[0028] uk = (1 -N kp〇) vk+p〇(b-xk) (3)。
[0029] 进一步的，采用零阶保持器的预测方法和指数递减的权重分配方法，零阶保持器 的预测方法即v' k+i=vk，指数递减的权重分配方法即Ai=A"SO< q< 1时，参数矩 阵b=xf，其中q为权重分配方法的底数；则式⑵转换为式（4):
[0031] 进一步的，所述步骤5中\为指数递减形式，即满足：Ai=A乂，〇 <q<1。
[0032] 进一步的，所述步骤5中A,为线性递减形式，即满足：
〇
[0033] 进一步的，所述步骤3中进行缩短时需要先判断xk是否比x^更靠近参考点，判断 方法为：
[0034] 在0时刻计算获得控制量叫后，设置参考点z为z=x d+u。；
[0035] 如果| |xk-z| |彡| |xk_「z| |，则保持参考点和预测窗口长度不变；
[0036] 如果| |xk-z| | > | |xk_「z| |，则更新参考点，令新的参考点为xk+uk，同时收缩预测 窗口。
[0037] 有益效果：
[0038] 本发明解决了原控制器在常用的A=I，B=I模型、点约束（M=I)下，操作人控 制量无法影向被控机器人的问题：其中，利用非对称权重，保证了操作人员的初始干预将在 很大程度上被保留下来，故很难再出现实际控制量与操作人员的控制量"反向"的问题；最 后，逐渐缩短的预测窗口也保证了即使在预测操作人员控制量出现较大偏差时也能让被控 机器人达到原定目标，即实现了"操作人员不需精确地控制机器人"的功能。
【附图说明】
[0039] 图1为本发明的避障轨迹示意图。
[0040]图2为本发明的控制指令与控制量比较示意图。
【具体实施方式】
[0041] 本发明所述的控制器可以直接用于做机器人的控制器，也可与机器人的现有控制 器相结合进行控制，对于移动机器人而言，由于机器人的实际物理模型常常是非线性的，故 多采用与机器人原有控制器相结合的方式。以下叙述本发明所述控制器与移动机器人现有 控制器的一种结合方式及其避障过程。
[0042] 假设被控机器人的模型为：xk+1=Axk+Buk，目标位置满足：Mx=b，xk表示第k个 时刻被控机器人的位置坐标，uk表示第k个时刻被控机器人的控制量;A和B是由被控机器 人实际物理模型决定的系统参数矩形；M，b是目标位置满足的线性方程组的参数矩阵，由 目标的实际物理位置决定；
[0043] 则该控制器对机 器人进行控制时包含以下几个步骤：
[0044] 步骤1、在时刻0设定预测窗口的长度队为最大值：&=Nmax;
[0045] 按目标位置与初始位置的距离、接收操作员指令的频率、机器人的移动速度与任 务需要设置时刻〇的队。的取值越大，给予操作员的控制空间越大，相应的也能处理更大的 障碍，但同时也可能造成机器人不能很快驶向目标点最大值和最小值以及队的选取应根据 实际任务及机器人情况进行多次实验后确定，后面具体说明。
[0046] 步骤2、基于模型预测实现被控机器人的人机协同控制；
[0047] 步骤21，在时刻k，k> 0,预测窗口的Nk长度按预定方式进行缩短：
[0048]Nk=max{Nh-1，Nmin}
[0049] 步骤22,接收控制人员的控制指令vk，根据现有技术中的滤波与预测算法结合控 制指令vk预测控制人员的未来Nk个时刻中的控制指令
i= 0，l，…，Nk-1，其中v'k=vk;
[0050] 步骤23,结合预测得到的控制人员的未来Nk个时刻中的控制指令
?利用下式计算当前时刻的控制量uk:
[0052] 【背景技术】中提到每一时刻采用求得的控制序列中的第一项进行控制，控制量uk具 有形式：
，采用的是对 称权重，本发明在此进行修改，采用非对称权重，即将该现有技术公式修改为式（1)。
[0053] 其中，
\为未来第i个时刻控制量的非对称权重，满足：
入1可设计为指数递减形式，即：A ,= A < q < 1，或设计为 线性递减形式
[0054] 而当被控机器人模型为A=I，B=I且目标约束中M=I时，令
则式（l)uk的计算式将简化为：
[0056] 若采用零阶保持器的预测方法，即令v'k+i=vk，则式（2)转换为（3)
[0057] uk=(1-Nkp〇)vk+p〇 (b-xk) (3)
[0058] 由于采用了非对称权重，
，由PQ计算式可知，式（3)中， l-NkpQ> 0恒成立，故此时仍然将有操作员的控制影响vk影响在内，解决了【背景技术】中第一 个问题。
[0059] 由于采用了收缩预测窗口的方法，随着时间的增长（k的增大），预测窗口长度Nk 将逐渐缩小，由P(l计算式可知，P^将逐渐增大，1-NkP(l将逐渐减小，故操作员的控制指令vk 对实际被控机器人的控制量uk的影响将逐减小。故在操作员干预的初始阶段，控制器的输 出将与操作员的干预越相近（这一定程度上解决了【背景技术】中的第二个问题），而在干预 的末尾阶段，控制器的输出将更多的专注于将机器人导向目标位置（即操作员控制指令影 响的下降）以快速完成任务。
[0060] 上述步骤23中的控制量uk对应于最小化式（4)中代价函数的控制序列的第一 项；
[0062] 其中：
为上述的控制序列，
为当 前时刻预测的控制人员的控制输入。
[0063] 该优化问题的约束条件为：
[0064] 根据上述约束条件优化代价函数获得控制序列的第一项uk。
[0065] 移动机器人不论其移动方式如何，其本身所具有的基本控制器都具有控制机器人 朝指定坐标移动的能力，由此可以将带有基本控制器的移动机器人视为一个A=I，B=I型的线性系统。即具有如下的模型：
[0066]xk+1 =xk+uk
[0067] 其中xk、uk者是二维向量，xk表示机器人当前的位置坐标，即xk= (x，y)。
[0068] 由此，将本发明所述控制器与机器人原有基本控制器相结合，通过对本发明所述 控制器的配置，即可实现有操作员协助的机器人避障及驶向目标点的功能。即在每一时刻 k，本发明所述控制器根据当前收到的操作员控制指令v与当前机器人的位置x计算出当前 的最优控制量U;该控制量发送给机器人原有的基本控制器，该控制器控制机器人向X+U处 行驶，并输送当前机器人的位置给基本控制器。
[0069] 关于步骤1和步骤2中预测窗口长度的最小值、最大值的设定说明：
[0070] 在求解每一时刻的控制量uk时，实质上需要在给定约束xk+1 =Ax k+Buk与
下，求解一能最小化上述式（4)中代价函数J(U，V)的一组控制序列。想要 求解这个问题，其先决条件是问题本身有解：即对于可以用模型xk+1=Axk+Buk描述的这样 一个被控机器人，从当前（k时刻）出发，其能在Nk时刻后到达一个由f
?描述 的预定区域。显然如果当前位置xk与预定区域离得近，Nk便可以取得较小，同时如果机器 人的速度很快、机器人本身的能量很多或者希望机器人能较快的移动到目标位置，Nk取得 较小也可以，反之，如果当前位置xk与预定区域离得远、机器人的速度不够快、能量不够多， 又或者不希望机器人耗费过多的能量达到预定的位置，Nk便应相应取得较大。
[0071] 从另一个角度，Nk的大小也决定了控制器对操作员控制指令的"尊重程度"。求解 式（4)的过程实是在寻找一个与操作员指令序列V(预测得到的）相接近，而同时又能使机 器人的末态达到目标区域的控制序列。这等价于求解一个V序列的修正序列，同时又希望 修正量总体而言尽可能小。因为采用了非对称度量的方法
求解过程中，算法实质上更偏好于对靠近当前时刻的？k+iT以小的修正量，而对与当前 时刻较远的？k+i予以较大的修正量（这种偏好反应在式（4)中即为不同的Xi取值）。 进而，当Nk越大，当前时刻对vk的修正量与总体修正量越可能小（实际上是算法更偏向于 其更小，而实际修正量的比重又与机器人模型有关），故Nk较大时，控制器当前时刻的实际 输出与操作员的控制指令越尽可能的接近，而Nk较小时，实际输出与控制指令间的偏差则 可能很大。
[0072] 以上仅说明的是各K时刻的情况，实际上可以理论上证明，对于可解性的问题，只 需要初始时刻队能保证待求解问题有解，那么按照恰当的预测窗口收缩方式，之后任意时 刻k的预测窗口都应自动能保证待求解问题有解。在队能保证初始时刻问题有解的情况 下（如前所述，这取决于初始位置到目标位置的距离、移动速度、能量、控制期望等），如果 希望给予操作员更大的操作自由度（譬如当协同避障时，障碍物较大），则需要将队设定较 大以减少控制器对操作员控制指令的修正程度。类似的，对于预测窗口的最小值Nmin而言， 其大小决定了在接近目标区域（即控制过程的末尾阶段）时控制器对操作员控制的修正程 度，若令N' min= 1，则所有的误差都被完全修正，此时操作员若想让机器人暂时偏离预定 目标将可能失败，反之，若1^取得较大，虽然给予了操作员在末尾阶段更大的自由度，最终 机器人的位置可能与预定有所偏差（因为操作员的人为误差没有被完全消除）。以此原理 根据实际任务及机器人情况进行多次实验后确定预测窗口长度的最大值和最小值。
[0073] 介绍一下本发明的基于模型预测的人机协同控制方法的一种具体实现方式：
[0074] 1、确定所连接的机器人系统模型，即A，B的取值。如上所述，当机器人现有合适的 基本控制器时，可以采用A=I，B=I型的线性系统。
[0075] 2、确定下一目标点的约束形式，即M，b的取值。对于轨迹跟踪或避障问题，一般要 求机器人在避障后到达某点，设该点的坐标为xf，对应即M=I，b=xf。
[0076] 3、确定操作员指令的预测方法，即如何通过当前及过去时刻操作员输入的控制量 Ivk}确定未来若干时刻操作员的输入。推荐采用的预测方法为零阶保持器方法或一阶预测 器方法，这两种预测方法虽然不一定精确，但其也不会带来过大的预测误差。
[0077]采用零阶保持器预测方法时：vk+i=vk，i= 1，2，… [0078]采用一阶预测器方法时：vk+i=iX(vk-VkJ+Vk，i= 1，2，…
[0079] 4、确定非对称权重{Ak}的取值。一般可采用指数递减的权重分配方式，即
[0080] 入k= q k，q < 1，k = 0, 1，2，...
[0081] q的值越接近于1，则操作员能进行的干预越小。当q= 1时，在点约束、零阶保持 器预测方法下，操作员的输入将完全不能影响机器人的实际行动。
[0082] 5、上述步骤21中的缩短方式有可能因为机器人原有的控制器不一定能在下一采 样时刻使机器人到达期望的位置，所以需要考虑机器人的实际位置信息以确定预测窗口是 否进行收缩。一种推荐的预测窗口的收缩方式为如下所述的利用设置参考点的方法。
[0083] (1)在0时刻，根据式⑵计算得最佳控制量U(l后，设置参考点z为z=xfu。；
[0084] (2)在k时刻，根据式（2)计算得最佳控制量uk后，判断xk是否比Xh更靠近参考 点Z，即IIXk-zII是否小于（小于等于）IIXh-zII。
[0085] 如果| |xk-z| |彡| |xk_「z| |，则保持参考点不变，同时保持预测窗口不变，即Nk+1 = Nk；
[0086] 如果| |xk_z| | > | |xk_「z| |，则更新参考点，令参考点z为xk+uk，同时收缩预测窗 口，即Nk+1=Nk-l〇
[0087] 上述的预测窗口收缩方式可以保证每一时刻机器人的控制量都是由当前时刻操 作员输入计算得到的，同时又保证了预测窗口不会因为机器人基本控制器的响应延迟而过 快收缩。
[0088] 6、确定接近目标点时的行为。如果要求机器人进行轨线跟踪，或需要机器人驶向 下一目标点，则需要重置机器人的目标约束M，b，重置控制器的预测窗口为初始预测窗口 队（如果采用设置参考点的方法来收缩预测窗口的话重置初始参考点z)。
[0089] 由于采用了带有基本控制器的机器人可简化为A=I，B=I型的线性系统模型， 在点约束的情况下，约束矩阵M=I，期望位置b= Xf，此时按本发明所述方法计算控制量 可得如下结果：
[0091]其中，Vk+^对操作员之后控制输入的预测值，
[0093] 当采用零阶保持器（v' k+i=vk)预测、指数递减的权重分配方法（Ai=A&，〇 <q< 1)时，则式（3)控制量的计算方法可以进一步简化为式（5):
[0095]当q=1时权重为对称形式，此即【背景技术】中所得控制量形式相同，可见在此时被 控机器人的控制量uk与控制人员的控制量vk无关。
[0096]当0 <q< 1时采用非对称权重q与收缩的预测窗口Nk后，随着Nk的逐渐减小， Vk所能影响产生对uk的影响也将越来越小，亦即，为了保证机器人能在Nk步后到达目的地，Vk被修正的量也将越来越大。
[0097] 图1中所示即按上述配置下，取Nk= 20进行的一次人机协助避障实验的机器人 行走轨迹图，该实验中，机器人自身并不能获知外部障碍的位置，其依靠操作人员的短时干 预进行避障，并且即使操作人员的控制量不完全精确，其最终也能到达预定目的地。该次实 验中操作人员控制量与计算得到的控制量随时间变化的曲线如图2所示，可见初期实际控 制量与操作人员的输入控制量较为接近，这即是Nk较大时控制器对操作员控制误差修正较 少的表现。而在后期控制器与操作员的控制量差距开始逐渐增大，这表明，随着Nk的减小， 控制器开始加大对操作员控制量误差的修正力度，以使机器人能达到预定目标位置。
[0098] 当然，本发明还可有其他多种实施例，在不背离本发明精神及其实质的情况下，熟 悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变 形都应属于本发明所附的权利要求的保护范围。
【主权项】
1. 一种基于模型预测的人机协同控制方法，其特征在于，包括以下步骤： 假设被控机器人的模型为：xk+1= Ax k+Buk，目标位置满足：Mxk= b，X k表示第k个时刻 被控机器人的位置坐标，uk表示第k个时刻被控机器人的控制量;A和B是由被控机器人实 际物理模型决定的系统参数矩阵;M，b是目标位置满足的线性方程组的参数矩阵，由目标 的实际物理位置决定； 步骤1、按被控机器人的目标位置与初始位置的距离、接收操作员指令的频率、机器人 的移动速度与任务需要设置被控机器人的模型预测窗口长度的最大值Nmax和最小值Nmin; 步骤2,设O时刻的预测窗口长度为Ntl，则令Ntl= N max; 步骤3、将k时刻预测窗口长度Nk按下式进行缩短：步骤4,根据滤波与预测算法结合操作员的当前控制指令Vk预测操作员的未来N k个时 刻的控制指令和'/cV'fc+CiVfc-l)}; i = 〇，！，...，（Nk-I),其中 ν' k = Vk; 步骤5,结合所述未来Nk个时刻的控制指令{vK+1，…A+i?4+(Λ^_υ}利 用下式计算当前时刻控制被控机器人实际运动的控制量uk:其中，A AiS未来第i个时刻控制量的非对称权重，且满足： 又O〉又1〉〉乂1)。2. 如权利要求1所述的基于模型预测的人机协同控制方法，其特征在于，当被控 机器人模型为A = I，B = I且目标约束中M= I时，I为单位矩阵；设置中间量Ptl，令'代入式⑴将Uk简化为：3. 如权利要求2所述的基于模型预测的人机协同控制方法，其特征在于，采用零阶保 持器的预测方法，即令V k+i= Vk时，则式（2)转换为式（3): uk = (I-N kp〇) vk+p〇 (b-xk) (3) 〇4. 如权利要求2所述的基于模型预测的人机协同控制方法，其特征在于，采用零阶保 持器的预测方法和指数递减的权重分配方法，零阶保持器的预测方法即V k+i= vk，指数递 减的权重分配方法即Ai= λ W，〇〈q〈l时，参数矩阵b = Xf，其中q为权重分配方法的底 数；则式⑵转换为式⑷：5. 如权利要求1所述的基于模型预测的人机协同控制方法，其特征在于，所述步骤5中 \为指数递减形式，即满足：入1=人（^，〇〈9〈1。6. 如权利要求1所述的基于模型预测的人机协同控制方法，其特征在于，所述步骤5中 AiS线性递减形式，即满足：7. 如权利要求1所述的基于模型预测的人机协同控制方法，其特征在于，所述步骤3中 进行缩短时需要先判断Xk是否比X η更靠近参考点，判断方法为： 在〇时刻计算获得控制量Utl后，设置参考点Z为Z = X d+U。； 如果I |Xk-z| I彡I |Xk-i-z| |，则保持参考点和预测窗口长度不变； 如果I |Xk-z| |>| Ixk-I-Zl |，则更新参考点，令新的参考点为xk+uk，同时收缩预测窗口。
【专利摘要】本发明提供一种基于模型预测的人机协同控制方法，属于机器人控制领域。其将机器人的预定目的地、预定轨线表示为系统状态的末端约束条件；在每一控制时刻预测人的控制输入；构建带有非对称度量性质的代价函数；通过投影算法求取既能满足末端约束又能最小化代价函数的控制序列；按模型预测控制方法，取所得第一项为当前时刻的控制量；通过令预测窗口逐渐缩小，配合代价函数的非对称度量性质，可以实现机器人的动态自治。本方法以保证机器人自身约束得以满足为基础，最大程度地遵从了人类的控制输入，以此实现避障，此方法也可用于解决传感器失灵或信息不足时的各类人机协同控制问题。
【IPC分类】G05B13/04, G05D1/02
【公开号】CN104898659
【申请号】CN201510106591
【发明人】方浩, 商成思, 陈杰
【申请人】北京理工大学
【公开日】2015年9月9日
【申请日】2015年3月11日
【技术领域】
[0001] 本发明属于机器人技术领域，尤其涉及一种基于模型预测的人机协同控制方法。
【背景技术】
[0002] 在对机器人运动自主探索的过程中，避障是移动机器人必需的一项基本功能。传 统的机器人控制技术集中于探索利用机器人自身传感器实施避障的方法，如利用红外传感 器、超声波传感器、激光传感器等。尽管传感器技术与环境建模技术的发展为机器人自主避 障、自主导航带来了巨大的改变，但复杂环境中单纯依靠传感器进行避障仍是不充分的，为 此需要操作员帮助机器人实现避障。
[0003] 在操作员通过遥控协助机器人行走、避障方面，已有的做法集中在现有的机器人 自主避障控制器基础上结合操作员控制指令。即将原有的机器人控制器的控制输出与操作 员的控制输出以某种方式相结合，得到新的控制器。
[0004] 一种新的思路是直接利用操作员的控制指令设计机器人的避障控制器。美国的 MagnusEgerstedt教授及其学生（ChipalkattyR，EgerstedtM.Human-in-the-loop: Terminalconstraintrecedinghorizoncontrolwithhumaninputs[C]//Roboticsand Automation(ICRA)，2010IEEEInternationalConferenceon.IEEE，2010:2712_2717)提 出了一种基于模型预测控制的人机协同控制器设计思路，其思路为：设被控机器人系统的 模型为xk+1=Axk+Buk;根据控制员的当前输入vk、过去输入，预测控制员的未来输入序列 IVk+i};求取控制序列U= {uk，uk+1，…，Uk+Nj^}，使得机器人在该控制输入下，能在N步 后到达目标状态：Mxk+Nk=b，且该控制序列与预测的控制员输入序列最为相似，即其能最 小化下式的代价函数：
[0006] 每一时刻采用求得的控制序列中的第一项进行控制，控制量uk具有形式：
每一时刻的预测窗 口Nk取为固定值或依预测精度进行动态调整，即预测操作员的控制输入越精确时，窗口越 长。
[0007] 但这一控制方法并不能有效地解决地面移动机器人的协同控制问题，其原因有以 下几点：
[0009] 1、地面移动机器人的控制模型最常见的为A=I，B=I的情形，采用零阶保持器 预测方法，末态约束为点约束（即M=I)时，求得的控制量具有以下形式：
[0011] 该形式表明，无论操作员的控制量vk为多少，控制器都将输出相同的控制量给被 控机器人。显然，这与利用操作员控制量进行避障的思想是相违背的。
[0012] 2、当不采用零阶保持器器时，而仍采用A=I，B=I的模型时，控制器的实际输出 将可能出现与操作员控制"反向"的情况。
[0013] 譬如当b= [0,1]T，xk= [0, 0]T，N= 2,vk=v' k+(l= [1，1]T，v' k+1= [2, 2]T 时，代入解得
此式表明，当操作员给出了右向45度 的一个控制量时，实际给机器人的控制量却是向左转直行，这与利用操作员控制量进行避 障的意图也是相违背的。
【发明内容】
[0014] 为解决上述问题，本发明提供一种基于模型预测的人机协同控制方法，该方法，该 方法可以利用操作员的控制量协助机器人进行行驶，同时机器人又能自主驶向预定目标 点、目标区域。
[0015] 本发明的基于模型预测的人机协同控制方法，其包括以下步骤：
[0016] 假设被控机器人的模型为：xk+1=Axk+Buk，目标位置满足：Mxk=b，xk表示第k个 时刻被控机器人的位置坐标，uk表示第k个时刻被控机器人的控制量;A和B是由被控机器 人实际物理模型决定的系统参数矩阵;M，b是目标位置满足的线性方程组的参数矩阵，由 目标的实际物理位置决定；
[0017] 步骤1、按被控机器人的目标位置与初始位置的距离、接收操作员指令的频率、机 器人的移动速度与任务需要设置被控机器人的模型预测窗口长度的最大值1"和最小值Nfflin；
[0018] 步骤2,设0时刻的预测窗口长度为％，则令队=Nmax;
[0019] 步骤3、将k时刻预测窗口长度Nk按下式进行缩短：
[0021] 步骤4,根据滤波与预测算法结合操作员的当前控制指令vk预测操作员的未来Nk 个时刻的控制指令
；i= 〇，1，…，（Nk-1)，其中Vk =
[0022] 步骤5,结合所述未来Nk个时刻的控制指令
利用下式计算当前时刻控制被控机器人实际运动的控制量uk:
[0024] 其中，
A 未来第i个时刻控制量的非对称权重，且满足： 入 0〉人J〉..〉人（Nk_l)。
[0025] 进一步的，当被控机器人模型为A=I，B=I且目标约束中M=I时，I为单位矩 阵；设置中间量Po,令
代入式⑴将Uk简化为：
[0027] 进一步的，采用零阶保持器的预测方法，即令v' k+i=vk时，则式（2)转换为式 (3)：
[0028] uk = (1 -N kp〇) vk+p〇(b-xk) (3)。
[0029] 进一步的，采用零阶保持器的预测方法和指数递减的权重分配方法，零阶保持器 的预测方法即v' k+i=vk，指数递减的权重分配方法即Ai=A"SO< q< 1时，参数矩 阵b=xf，其中q为权重分配方法的底数；则式⑵转换为式（4):
[0031] 进一步的，所述步骤5中\为指数递减形式，即满足：Ai=A乂，〇 <q<1。
[0032] 进一步的，所述步骤5中A,为线性递减形式，即满足：
〇
[0033] 进一步的，所述步骤3中进行缩短时需要先判断xk是否比x^更靠近参考点，判断 方法为：
[0034] 在0时刻计算获得控制量叫后，设置参考点z为z=x d+u。；
[0035] 如果| |xk-z| |彡| |xk_「z| |，则保持参考点和预测窗口长度不变；
[0036] 如果| |xk-z| | > | |xk_「z| |，则更新参考点，令新的参考点为xk+uk，同时收缩预测 窗口。
[0037] 有益效果：
[0038] 本发明解决了原控制器在常用的A=I，B=I模型、点约束（M=I)下，操作人控 制量无法影向被控机器人的问题：其中，利用非对称权重，保证了操作人员的初始干预将在 很大程度上被保留下来，故很难再出现实际控制量与操作人员的控制量"反向"的问题；最 后，逐渐缩短的预测窗口也保证了即使在预测操作人员控制量出现较大偏差时也能让被控 机器人达到原定目标，即实现了"操作人员不需精确地控制机器人"的功能。
【附图说明】
[0039] 图1为本发明的避障轨迹示意图。
[0040]图2为本发明的控制指令与控制量比较示意图。
【具体实施方式】
[0041] 本发明所述的控制器可以直接用于做机器人的控制器，也可与机器人的现有控制 器相结合进行控制，对于移动机器人而言，由于机器人的实际物理模型常常是非线性的，故 多采用与机器人原有控制器相结合的方式。以下叙述本发明所述控制器与移动机器人现有 控制器的一种结合方式及其避障过程。
[0042] 假设被控机器人的模型为：xk+1=Axk+Buk，目标位置满足：Mx=b，xk表示第k个 时刻被控机器人的位置坐标，uk表示第k个时刻被控机器人的控制量;A和B是由被控机器 人实际物理模型决定的系统参数矩形；M，b是目标位置满足的线性方程组的参数矩阵，由 目标的实际物理位置决定；
[0043] 则该控制器对机 器人进行控制时包含以下几个步骤：
[0044] 步骤1、在时刻0设定预测窗口的长度队为最大值：&=Nmax;
[0045] 按目标位置与初始位置的距离、接收操作员指令的频率、机器人的移动速度与任 务需要设置时刻〇的队。的取值越大，给予操作员的控制空间越大，相应的也能处理更大的 障碍，但同时也可能造成机器人不能很快驶向目标点最大值和最小值以及队的选取应根据 实际任务及机器人情况进行多次实验后确定，后面具体说明。
[0046] 步骤2、基于模型预测实现被控机器人的人机协同控制；
[0047] 步骤21，在时刻k，k> 0,预测窗口的Nk长度按预定方式进行缩短：
[0048]Nk=max{Nh-1，Nmin}
[0049] 步骤22,接收控制人员的控制指令vk，根据现有技术中的滤波与预测算法结合控 制指令vk预测控制人员的未来Nk个时刻中的控制指令
i= 0，l，…，Nk-1，其中v'k=vk;
[0050] 步骤23,结合预测得到的控制人员的未来Nk个时刻中的控制指令
?利用下式计算当前时刻的控制量uk:
[0052] 【背景技术】中提到每一时刻采用求得的控制序列中的第一项进行控制，控制量uk具 有形式：
，采用的是对 称权重，本发明在此进行修改，采用非对称权重，即将该现有技术公式修改为式（1)。
[0053] 其中，
\为未来第i个时刻控制量的非对称权重，满足：
入1可设计为指数递减形式，即：A ,= A < q < 1，或设计为 线性递减形式
[0054] 而当被控机器人模型为A=I，B=I且目标约束中M=I时，令
则式（l)uk的计算式将简化为：
[0056] 若采用零阶保持器的预测方法，即令v'k+i=vk，则式（2)转换为（3)
[0057] uk=(1-Nkp〇)vk+p〇 (b-xk) (3)
[0058] 由于采用了非对称权重，
，由PQ计算式可知，式（3)中， l-NkpQ> 0恒成立，故此时仍然将有操作员的控制影响vk影响在内，解决了【背景技术】中第一 个问题。
[0059] 由于采用了收缩预测窗口的方法，随着时间的增长（k的增大），预测窗口长度Nk 将逐渐缩小，由P(l计算式可知，P^将逐渐增大，1-NkP(l将逐渐减小，故操作员的控制指令vk 对实际被控机器人的控制量uk的影响将逐减小。故在操作员干预的初始阶段，控制器的输 出将与操作员的干预越相近（这一定程度上解决了【背景技术】中的第二个问题），而在干预 的末尾阶段，控制器的输出将更多的专注于将机器人导向目标位置（即操作员控制指令影 响的下降）以快速完成任务。
[0060] 上述步骤23中的控制量uk对应于最小化式（4)中代价函数的控制序列的第一 项；
[0062] 其中：
为上述的控制序列，
为当 前时刻预测的控制人员的控制输入。
[0063] 该优化问题的约束条件为：
[0064] 根据上述约束条件优化代价函数获得控制序列的第一项uk。
[0065] 移动机器人不论其移动方式如何，其本身所具有的基本控制器都具有控制机器人 朝指定坐标移动的能力，由此可以将带有基本控制器的移动机器人视为一个A=I，B=I型的线性系统。即具有如下的模型：
[0066]xk+1 =xk+uk
[0067] 其中xk、uk者是二维向量，xk表示机器人当前的位置坐标，即xk= (x，y)。
[0068] 由此，将本发明所述控制器与机器人原有基本控制器相结合，通过对本发明所述 控制器的配置，即可实现有操作员协助的机器人避障及驶向目标点的功能。即在每一时刻 k，本发明所述控制器根据当前收到的操作员控制指令v与当前机器人的位置x计算出当前 的最优控制量U;该控制量发送给机器人原有的基本控制器，该控制器控制机器人向X+U处 行驶，并输送当前机器人的位置给基本控制器。
[0069] 关于步骤1和步骤2中预测窗口长度的最小值、最大值的设定说明：
[0070] 在求解每一时刻的控制量uk时，实质上需要在给定约束xk+1 =Ax k+Buk与
下，求解一能最小化上述式（4)中代价函数J(U，V)的一组控制序列。想要 求解这个问题，其先决条件是问题本身有解：即对于可以用模型xk+1=Axk+Buk描述的这样 一个被控机器人，从当前（k时刻）出发，其能在Nk时刻后到达一个由f
?描述 的预定区域。显然如果当前位置xk与预定区域离得近，Nk便可以取得较小，同时如果机器 人的速度很快、机器人本身的能量很多或者希望机器人能较快的移动到目标位置，Nk取得 较小也可以，反之，如果当前位置xk与预定区域离得远、机器人的速度不够快、能量不够多， 又或者不希望机器人耗费过多的能量达到预定的位置，Nk便应相应取得较大。
[0071] 从另一个角度，Nk的大小也决定了控制器对操作员控制指令的"尊重程度"。求解 式（4)的过程实是在寻找一个与操作员指令序列V(预测得到的）相接近，而同时又能使机 器人的末态达到目标区域的控制序列。这等价于求解一个V序列的修正序列，同时又希望 修正量总体而言尽可能小。因为采用了非对称度量的方法
求解过程中，算法实质上更偏好于对靠近当前时刻的？k+iT以小的修正量，而对与当前 时刻较远的？k+i予以较大的修正量（这种偏好反应在式（4)中即为不同的Xi取值）。 进而，当Nk越大，当前时刻对vk的修正量与总体修正量越可能小（实际上是算法更偏向于 其更小，而实际修正量的比重又与机器人模型有关），故Nk较大时，控制器当前时刻的实际 输出与操作员的控制指令越尽可能的接近，而Nk较小时，实际输出与控制指令间的偏差则 可能很大。
[0072] 以上仅说明的是各K时刻的情况，实际上可以理论上证明，对于可解性的问题，只 需要初始时刻队能保证待求解问题有解，那么按照恰当的预测窗口收缩方式，之后任意时 刻k的预测窗口都应自动能保证待求解问题有解。在队能保证初始时刻问题有解的情况 下（如前所述，这取决于初始位置到目标位置的距离、移动速度、能量、控制期望等），如果 希望给予操作员更大的操作自由度（譬如当协同避障时，障碍物较大），则需要将队设定较 大以减少控制器对操作员控制指令的修正程度。类似的，对于预测窗口的最小值Nmin而言， 其大小决定了在接近目标区域（即控制过程的末尾阶段）时控制器对操作员控制的修正程 度，若令N' min= 1，则所有的误差都被完全修正，此时操作员若想让机器人暂时偏离预定 目标将可能失败，反之，若1^取得较大，虽然给予了操作员在末尾阶段更大的自由度，最终 机器人的位置可能与预定有所偏差（因为操作员的人为误差没有被完全消除）。以此原理 根据实际任务及机器人情况进行多次实验后确定预测窗口长度的最大值和最小值。
[0073] 介绍一下本发明的基于模型预测的人机协同控制方法的一种具体实现方式：
[0074] 1、确定所连接的机器人系统模型，即A，B的取值。如上所述，当机器人现有合适的 基本控制器时，可以采用A=I，B=I型的线性系统。
[0075] 2、确定下一目标点的约束形式，即M，b的取值。对于轨迹跟踪或避障问题，一般要 求机器人在避障后到达某点，设该点的坐标为xf，对应即M=I，b=xf。
[0076] 3、确定操作员指令的预测方法，即如何通过当前及过去时刻操作员输入的控制量 Ivk}确定未来若干时刻操作员的输入。推荐采用的预测方法为零阶保持器方法或一阶预测 器方法，这两种预测方法虽然不一定精确，但其也不会带来过大的预测误差。
[0077]采用零阶保持器预测方法时：vk+i=vk，i= 1，2，… [0078]采用一阶预测器方法时：vk+i=iX(vk-VkJ+Vk，i= 1，2，…
[0079] 4、确定非对称权重{Ak}的取值。一般可采用指数递减的权重分配方式，即
[0080] 入k= q k，q < 1，k = 0, 1，2，...
[0081] q的值越接近于1，则操作员能进行的干预越小。当q= 1时，在点约束、零阶保持 器预测方法下，操作员的输入将完全不能影响机器人的实际行动。
[0082] 5、上述步骤21中的缩短方式有可能因为机器人原有的控制器不一定能在下一采 样时刻使机器人到达期望的位置，所以需要考虑机器人的实际位置信息以确定预测窗口是 否进行收缩。一种推荐的预测窗口的收缩方式为如下所述的利用设置参考点的方法。
[0083] (1)在0时刻，根据式⑵计算得最佳控制量U(l后，设置参考点z为z=xfu。；
[0084] (2)在k时刻，根据式（2)计算得最佳控制量uk后，判断xk是否比Xh更靠近参考 点Z，即IIXk-zII是否小于（小于等于）IIXh-zII。
[0085] 如果| |xk-z| |彡| |xk_「z| |，则保持参考点不变，同时保持预测窗口不变，即Nk+1 = Nk；
[0086] 如果| |xk_z| | > | |xk_「z| |，则更新参考点，令参考点z为xk+uk，同时收缩预测窗 口，即Nk+1=Nk-l〇
[0087] 上述的预测窗口收缩方式可以保证每一时刻机器人的控制量都是由当前时刻操 作员输入计算得到的，同时又保证了预测窗口不会因为机器人基本控制器的响应延迟而过 快收缩。
[0088] 6、确定接近目标点时的行为。如果要求机器人进行轨线跟踪，或需要机器人驶向 下一目标点，则需要重置机器人的目标约束M，b，重置控制器的预测窗口为初始预测窗口 队（如果采用设置参考点的方法来收缩预测窗口的话重置初始参考点z)。
[0089] 由于采用了带有基本控制器的机器人可简化为A=I，B=I型的线性系统模型， 在点约束的情况下，约束矩阵M=I，期望位置b= Xf，此时按本发明所述方法计算控制量 可得如下结果：
[0091]其中，Vk+^对操作员之后控制输入的预测值，
[0093] 当采用零阶保持器（v' k+i=vk)预测、指数递减的权重分配方法（Ai=A&，〇 <q< 1)时，则式（3)控制量的计算方法可以进一步简化为式（5):
[0095]当q=1时权重为对称形式，此即【背景技术】中所得控制量形式相同，可见在此时被 控机器人的控制量uk与控制人员的控制量vk无关。
[0096]当0 <q< 1时采用非对称权重q与收缩的预测窗口Nk后，随着Nk的逐渐减小， Vk所能影响产生对uk的影响也将越来越小，亦即，为了保证机器人能在Nk步后到达目的地，Vk被修正的量也将越来越大。
[0097] 图1中所示即按上述配置下，取Nk= 20进行的一次人机协助避障实验的机器人 行走轨迹图，该实验中，机器人自身并不能获知外部障碍的位置，其依靠操作人员的短时干 预进行避障，并且即使操作人员的控制量不完全精确，其最终也能到达预定目的地。该次实 验中操作人员控制量与计算得到的控制量随时间变化的曲线如图2所示，可见初期实际控 制量与操作人员的输入控制量较为接近，这即是Nk较大时控制器对操作员控制误差修正较 少的表现。而在后期控制器与操作员的控制量差距开始逐渐增大，这表明，随着Nk的减小， 控制器开始加大对操作员控制量误差的修正力度，以使机器人能达到预定目标位置。
[0098] 当然，本发明还可有其他多种实施例，在不背离本发明精神及其实质的情况下，熟 悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变 形都应属于本发明所附的权利要求的保护范围。
【主权项】
1. 一种基于模型预测的人机协同控制方法，其特征在于，包括以下步骤： 假设被控机器人的模型为：xk+1= Ax k+Buk，目标位置满足：Mxk= b，X k表示第k个时刻 被控机器人的位置坐标，uk表示第k个时刻被控机器人的控制量;A和B是由被控机器人实 际物理模型决定的系统参数矩阵;M，b是目标位置满足的线性方程组的参数矩阵，由目标 的实际物理位置决定； 步骤1、按被控机器人的目标位置与初始位置的距离、接收操作员指令的频率、机器人 的移动速度与任务需要设置被控机器人的模型预测窗口长度的最大值Nmax和最小值Nmin; 步骤2,设O时刻的预测窗口长度为Ntl，则令Ntl= N max; 步骤3、将k时刻预测窗口长度Nk按下式进行缩短：步骤4,根据滤波与预测算法结合操作员的当前控制指令Vk预测操作员的未来N k个时 刻的控制指令和'/cV'fc+CiVfc-l)}; i = 〇，！，...，（Nk-I),其中 ν' k = Vk; 步骤5,结合所述未来Nk个时刻的控制指令{vK+1，…A+i?4+(Λ^_υ}利 用下式计算当前时刻控制被控机器人实际运动的控制量uk:其中，A AiS未来第i个时刻控制量的非对称权重，且满足： 又O〉又1〉〉乂1)。2. 如权利要求1所述的基于模型预测的人机协同控制方法，其特征在于，当被控 机器人模型为A = I，B = I且目标约束中M= I时，I为单位矩阵；设置中间量Ptl，令'代入式⑴将Uk简化为：3. 如权利要求2所述的基于模型预测的人机协同控制方法，其特征在于，采用零阶保 持器的预测方法，即令V k+i= Vk时，则式（2)转换为式（3): uk = (I-N kp〇) vk+p〇 (b-xk) (3) 〇4. 如权利要求2所述的基于模型预测的人机协同控制方法，其特征在于，采用零阶保 持器的预测方法和指数递减的权重分配方法，零阶保持器的预测方法即V k+i= vk，指数递 减的权重分配方法即Ai= λ W，〇〈q〈l时，参数矩阵b = Xf，其中q为权重分配方法的底 数；则式⑵转换为式⑷：5. 如权利要求1所述的基于模型预测的人机协同控制方法，其特征在于，所述步骤5中 \为指数递减形式，即满足：入1=人（^，〇〈9〈1。6. 如权利要求1所述的基于模型预测的人机协同控制方法，其特征在于，所述步骤5中 AiS线性递减形式，即满足：7. 如权利要求1所述的基于模型预测的人机协同控制方法，其特征在于，所述步骤3中 进行缩短时需要先判断Xk是否比X η更靠近参考点，判断方法为： 在〇时刻计算获得控制量Utl后，设置参考点Z为Z = X d+U。； 如果I |Xk-z| I彡I |Xk-i-z| |，则保持参考点和预测窗口长度不变； 如果I |Xk-z| |>| Ixk-I-Zl |，则更新参考点，令新的参考点为xk+uk，同时收缩预测窗口。
【专利摘要】本发明提供一种基于模型预测的人机协同控制方法，属于机器人控制领域。其将机器人的预定目的地、预定轨线表示为系统状态的末端约束条件；在每一控制时刻预测人的控制输入；构建带有非对称度量性质的代价函数；通过投影算法求取既能满足末端约束又能最小化代价函数的控制序列；按模型预测控制方法，取所得第一项为当前时刻的控制量；通过令预测窗口逐渐缩小，配合代价函数的非对称度量性质，可以实现机器人的动态自治。本方法以保证机器人自身约束得以满足为基础，最大程度地遵从了人类的控制输入，以此实现避障，此方法也可用于解决传感器失灵或信息不足时的各类人机协同控制问题。
【IPC分类】G05B13/04, G05D1/02
【公开号】CN104898659
【申请号】CN201510106591
【发明人】方浩, 商成思, 陈杰
【申请人】北京理工大学
【公开日】2015年9月9日
【申请日】2015年3月11日
最新回复(0)