一种预测控制方法

技术领域

本发明涉及一种预测控制方法，尤其涉及在大扰动、动力学模型不匹配的情况下，使移动平台上的机械臂保持理想的末端执行器状态的预测控制方法。

背景技术

对UVMS的控制难点主要为：UVMS建模具有不确定性；外部干扰；测量噪声。同时要求，控制的输入与状态应限制在位置、速度与加速度的约束范围内，提供实时控制动作。

针对以上要点，P.S.Londhe等人利用鲁棒非线性PID控制来克服外部干扰与动力学模型的不确定性，X.Liu等人利用自适应容错控制器，同时克服了动力学模型的不确定性、外部干扰与测量噪声，C.Yu等人将模糊逻辑与神经网络用来克服动力学模型的不确定性与外部干扰，但是以上研究都存在的问题是，控制律是固定的，未考虑机械限制或状态约束。D.Q.Mayne等人将模型控制预测(MPC)应用于UVMS的多自由度约束跟踪控制，为复杂的约束控制动力系统提供了最优的干扰抑制控制策略，但是MPC成本函数的优化问题复杂，增加了计算时间，无法提供实时最优控制动作。W.Gan等人采用基于量子行为粒子群优化(QPSO-MPC)的MPC方法来解决六自由度的水下自动航行器(AUV)的轨迹跟踪问题，有效提高了计算时效，但是动力学模型的不确定性与外部干扰影响了MPC的效果。Z.Sun等人提出一种改进的管式MPC框架，采用标称常规MPC控制和根据实际状态与标称状态的偏差设计的反馈率来克服动力学模型的不确定性与外部干扰，但仍存在计算效率低的问题。G.P.Incremona等人将每个自由度解耦，对其分别单独使用MPC，提高了计算效率，但导致所有自由度的鲁棒性和最优性下降。

现有技术中的缺陷主要集中在：1)研究停留在运动学层面，对动力学方面研究不足，难以应用于实际系统中；2)对系统采用模型预测控制，而缺少较为准确的系统模型，因此造成控制的失效；3)针对较为准确的动力学模型采用模型预测控制时，计算时效低，难以满足对控制频率的需求；4)对UVMS进行模型预测控制时，对机械限制或状态约束考虑不足，或是在受到多约束时计算效率降低，无法提供实时最优控制动作；5)基于动力学控制策略与实际系统高度相关，且由于扰动、负载或是系统性能衰退等改变系统动力学模型，造成调参困难。

发明内容

本发明的主要目的是提供一种预测控制方法，以解决现有技术的诸多不足。

为了实现上述主要目的，本发明提供了一种预测控制方法，用于配备有机械臂的水下机器人系统中，使移动平台上的机械臂保持理想的末端执行器状态；预测控制方法包括以下步骤：

步骤S10、对水下机器人系统中的移动平台和机械臂进行建模；

步骤S20、对所构建的模型采用模型预测控制(MPC)，考虑运动学可行性，采用二次规划(QP)进行快速求解，对外界扰动进行补偿并得到移动平台和机械臂的未来修正姿态；

步骤S30、对移动平台和末端执行器采用基于强化学习算法(RL)的控制器；控制器依据初始控制策略选择并执行动作，依据预先定义的奖励函数和获取到的实时的环境信号，更新控制策略，并判断是否为最优策略；若为最优控制策略，则结束当前学习并执行相应动作，否则重新进行此学习过程，直到达到最优控制策略为止；奖励函数为：

其中，d∈R

步骤S40、在水下机器人系统受到大扰动、动力学模型不匹配时，控制策略迁移使移动平台上的机械臂保持理想的末端执行器状态。

根据本发明的一种具体实施方式，步骤S10中，

移动平台的动力学方程为：

其中，v为移动平台的速度矩阵，M为包含附加质量的惯性矩阵，C为离心力和科里奥利力矩阵，D为水动力阻尼矩阵，g为重力和浮力的合力矩的矢量，η为移动平台的位置姿态信息矩阵，τ为推进器推力输出矩阵；

在移动平台坐标系中，机械臂的动力学方程为：

其中，ξ

根据本发明的一种具体实施方式，步骤S10中，

步骤S11、定义移动平台的控制器为：

其中，μ

步骤S12、定义末端执行器的控制器为：

其中，v

根据本发明的一种具体实施方式，步骤S20中，

步骤S21、对移动平台运动预测，将移动平台的运动建模为具有仿射扰动的线性时不变系统：x

将扰动观测与状态预测统一为对状态的估计，利用历史状态数据，采用自回归模型(AR)进行估计、预测未来的状态；

步骤S22、将惯性系中对目标点的跟踪分解为惯性系下移动平台的运动与移动平台坐标系下机械臂的运动，以消除移动平台运动对末端执行器的干扰；

步骤S23、对末端执行器运动预测控制，末端执行器的期望位置由任意时刻末端执行器的位置与控制输入得到：

其中，x

根据本发明的一种具体实施方式，步骤S30中，

在每个时间步长t内，系统接收状态观察值s

定义值函数Q

利用Bellman方程的递归关系得到：

使用贪婪策略μ(s，a)＝argmax

反复更新μ(s|θ

根据本发明的一种具体实施方式，步骤S40中策略迁移的方法为：

系统在源域D

状态、动作的源域与目标域有如下映射：m

定义

其中，||.||

本发明具备以下有益效果：

本发明采用模型预测控制算法，对末端执行器的运动进行补偿，以减小其跟踪误差；同时可以提高模型预测控制算法处理多自由度与多约束系统的计算时效，提高控制频率；另外，本发明采用迁移强化学习的方法，给出较为准确的系统模型，尤其在负载发生改变、系统性能退化等改变系统动力学模型后依旧能够给出较为准确的系统模型，以形成有效控制。

为了更清楚地说明本发明的目的、技术方案和优点，下面结合附图和具体实施方式对本发明作进一步的详细说明。

附图说明

图1是本发明的控制框架示意图一；

图2是本发明的控制框架示意图二；

图3是本发明移动平台的两个坐标系和运动表示的示意图；

图4是本发明模型预测控制算法的框架图；

图5是相对于惯性参考系，固定目标变现为运动目标的示意图；

图6是本发明强化学习算法的框架图；

图7是本发明强化学习的学习机制；

图8是本发明原环境与新环境的状态对齐的框架图；

图9是本发明控制策略转移的框架图；

图10是本发明源域与目标域之间状态与动作的对应关系示意图；

图11是本发明源域与目标域之间的策略迁移过程的示意图。

具体实施方式

为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不限于下面公开的具体实施例的限制。

本发明实施例提供一种预测控制方法，用于配备有机械臂的水下机器人系统中，使移动平台上的机械臂保持理想的末端执行器状态；如图1-2所示，预测控制方法包括以下步骤：

步骤S10、对水下机器人系统中的移动平台和机械臂进行建模；

其中，如图1所示，输入末端执行器的期望姿态p(t)，将其分解为移动平台期望姿态x

p(t)＝x

步骤S11、建立移动平台的模型，并定义移动平台的控制器；

对移动平台的期望姿态x

η＝[x，y，z，ψ]

μ＝[u，v，w，r]

固地参考系下：τ＝[F

其中，η为移动平台原点相对于固地参考系的位置和方向信息，μ为在该坐标系中原点速度的描述，τ表示在该坐标系中作用在移动平台上的力和力矩，如图3所示。

在静水中，移动平台的动力学方程为：

其中，v为移动平台的速度矩阵，M为包含附加质量的惯性矩阵，C为离心力和科里奥利力矩阵，D为水动力阻尼矩阵，g为重力和浮力的合力矩的矢量，η为移动平台的位置姿态信息矩阵，τ为推进器推力输出矩阵。

将连续时间模型转化为离散时间模型，应用一阶泰勒展开式将方程改写为：

f(v(t)，η(t))＝C(v(t))v(t)+D(v(t))v(t)+g(η(t))

v(t+1)＝v(t)+M

将移动平台的控制器定义为：

/>

其中，μ

步骤S12、建立机械臂的模型，并定义末端执行器的控制器；

对机械臂(六自由度)的期望姿态x

x

采用笛卡尔坐标系法对机械臂位姿进行描述：

其中，左上角3×3为空间旋转矩阵，通过欧拉角来确定，第一列为偏航角，表示绕x

绕x轴、y轴与z轴旋转θ的旋转矩阵为：

由D-H建模方法得到各个坐标系之间的变换矩阵为：

其中，

末端执行器位置参数相对于水下机器人本体坐标系的总变换矩阵为：

通过齐次变换的逆矩阵，求解每个关节的转动角度：

θ

机械臂每个连杆i的合力F

其中，m

其中，

在已知当前状态、环境没有障碍且给定的末端执行器的姿态是可达到的情况下，移动平台坐标系中，机械臂的动力学方程表示为：

其中，ξ

将末端执行器的控制器定义为：

其中，v

步骤S20、对所构建的模型采用模型预测控制(MPC)，考虑运动学可行性，采用二次规划(QP)进行快速求解，对外界扰动进行补偿并得到移动平台和机械臂的未来修正姿态；如图4所示，具体如下：

步骤S21、对移动平台运动预测，将移动平台的运动建模为具有仿射扰动的线性时不变系统：x

从任意时间节点N对未来状态的预测

其中，

将扰动观测与状态预测统一为对状态的估计，利用历史状态数据，采用自回归模型(AR)进行估计、预测未来的状态

其中，y

步骤S22、将惯性系中对目标点的跟踪分解为惯性系下移动平台的运动与移动平台坐标系下机械臂的运动，以消除移动平台运动对末端执行器的干扰；

展开来说，一个相对于固定坐标系的静止点将作为相对于运动坐标系的运动点出现。移动机械臂在惯性坐标系中保持一个固定的末端执行器位姿的问题转化为一个从机械臂的角度研究的轨迹跟踪问题。由此得出，在惯性坐标系中跟踪一个轨迹涉及到跟踪一个相对于机械臂的变换轨迹。通过预测移动平台的运动，则可以生成未来轨迹供机械臂遵循。通过跟踪变换后的轨迹，将消除对末端执行器的干扰，如图5所示。

将移动平台相对于固地坐标系{B}的姿态转换到以移动平台为原点的惯性坐标系{/}中：

其中，

给定在前方时间步长i的移动平台的预测姿势

步骤S23、对末端执行器运动预测控制，末端执行器的期望位置由任意时刻末端执行器的位置与控制输入得到：

其中，x

在固地坐标系中，当前末端执行器位置

用四元数表示误差为：

其中，

同样，末端执行器的期望角度与当前角度的误差

其中，ω∈R

用凸代价函数c

其中，

添加成本函数c

其中，

受到关节的运动学可行性的约束，控制行为被限制在整个预测控制范围的一个可行区域内，需要求解约束解：

其中，i＝1，...，N-1，

关节位置以递归、非线性方式在整个控制范围内传播：

已知关节的当前状态θ(t)，考虑运动学可行性，提出如下QP问题以提供快速的解决方案：

考虑预测控制范围内的关节状态传播：

若

θ(t+1|t+1)＝θ(t+1|t)

对于任意的i，θ(t+i|t+i)始终是可行的，即机械臂的关节的轨迹始终是可行的。

步骤S30、对移动平台和末端执行器采用基于强化学习算法(RL)的控制器；当动力学模型发生变化时，对源域训练的策略进行相应的调整，使之更适应于目标域，如图6-7所示。

对移动平台与末端执行器分别定义强化学习算法(RL)的奖励函数为：

其中，d∈R

在每个时间步长t内，系统接收状态观察值s

定义值函数Q

利用Bellman方程的递归关系得到：

使用贪婪策略μ(s，a)＝argmax

更新μ(s|θ

反复更新μ(s|θ

控制器依据初始控制策略选择并执行动作，依据预先定义的奖励函数和获取到的实时的环境信号，更新控制策略，并判断是否为最优策略；若为最优控制策略，则结束当前学习并执行相应动作，否则重新进行此学习过程，直到达到最优控制策略为止，从而实现最优的控制效果。

步骤S40、在水下机器人系统受到大扰动、动力学模型不匹配时，控制策略迁移使移动平台上的机械臂保持理想的末端执行器状态。

如图8-11所示，系统在源域D

为使得策略转移具有可行性，状态、动作的源域与目标域有如下映射：m

定义

其中，||.||

虽然本发明以具体实施例揭露如上，但这些具体实施例并非用以限定本发明实施的范围。任何本领域的普通技术人员，在不脱离本发明的发明范围内，当可作些许的变化/修改，即凡是依照本发明所做的同等变化/修改，应为本发明的保护范围所涵盖。