本发明属于微重力机器人控制领域，涉及一种中性浮力机器人姿态与轨迹控制方法，具体涉及一种基于分布式模型预测控制的中性浮力机器人姿态与轨迹控制方法。
背景技术
在微重力环境下进行的实验是验证地面空间技术的基本步骤之一。复杂空间操作的测试和演示要求地面测试系统提供长时间、大规模、精确、可控和几乎真实的微重力测试环境，以模拟空间运动与空间的相同程度。因此，应用中性浮力系统来进行微重力环境下的实验被广泛研究。同时，多个中性浮力系统联合执行任务也变得越来越重要。
然而，在中性浮力系统中，机器人工作于水下环境，不仅各个控制力之间相互耦合，机器人严重受到水的黏性阻力影响，同时多个机器人需要知道其他机器人的姿态与轨迹以防止出现碰撞等问题。此外，机器人与控制器之间的通信通过无线网络进行，控制信号的丢失与时延也会影响对机器人姿态和轨迹的控制。因此，在中性浮力机器人姿态与轨迹控制方法设计中，寻求一种可以处理各种各样约束并且可以解决无线网络影响的控制方法显得尤为重要。
当前由于模型预测控制方法具有对模型精度要求不高，系统鲁棒性好，稳定性好并且能够实时弥补由于模型失配、畸变、干扰等因素引起的不确定性，动态性能好，可以处理多变量约束等优点，广泛应用于电力系统、大化工过程、航空领域等。一般来说，模型预测控制方法包含三个过程：预测模型，反馈校正和滚动优化。其中，预测模型具有预测功能，可以根据系统当前时刻的控制输入以及过程的历史信息预测过程输出的未来值；反馈校正是通过将输出的量测值与模型的预估值进行比较，得出模型的预测误差，再利用模型预测误差来校正模型的预测值，从而得到更为准确的将来输出的预测值；滚动优化指的是优化过程不是一次离线完成的，而是反复在线进行的，即在每一采样时刻，优化性能指标只涉及该时刻起到未来有限的事件，而到下一个采样时刻，这一优化时段会同时向前推移。然而，现有的模型预测控制方法不能有效地解决多个耦合系统，且各个系统采样周期不同时的带约束优化问题，从而无法有效地控制各个系统。
技术实现要素：
本发明的目的在于提供一种基于分布式模型预测控制的中性浮力机器人姿态与轨迹控制方法，该方法结合计算机控制的特点，将复杂的在线优化问题转化为二次规划的问题，能够有效解决现有技术在中性浮力机器人姿态与轨迹控制上对耦合与抗扰方面不足的问题，大大减少了计算量，适于工程应用。
本发明是通过以下技术方案来实现：
本发明公开了一种基于分布式模型预测控制的中性浮力机器人姿态与轨迹控制方法，包括以下步骤：
步骤一：将中性浮力系统动力学模型写成状态空间方程式并进行线性化，得到线性模型；
步骤二：利用delta方法将线性模型离散化，得到系统的离散化形式；
步骤三：建立步骤二得到系统的优化问题，并将优化问题中的局部概率约束与耦合概率约束通过变化得到确定性的局部约束和耦合约束；
步骤四：通过求解受局部约束与耦合约束的优化问题，得到最优控制量，从而对中性浮力系统进行控制。
优选地，步骤一中，将中性浮力系统动力学模型写成状态空间方程式并进行线性化的具体操作为：
考虑中性浮力机器人在体坐标系下的动力学模型如式(1)：
其中，m为惯性质量矩阵，c(v)为科里奥利力矩阵，d(v)为机器人在水中受到的黏性阻力，g(η)为负浮力系数，τ为系统输入，
所考虑的中性浮力机器人在地理坐标系oxnynzn和机器人体坐标系oxbybzb的关系如下式(2)：
其中，
联立式(1)和式(2)，得到中性浮力机器人惯性坐标系下的动力学模型：
其中，mη(η)＝j-t(η)mj-1(η)；
dη(η,v)＝j-t(η)d(v)j-1(η)，gη(η)＝j-t(η)g(η)；
令x1＝η，
其中，
将式(4)进行线性化处理，得到系统(5)：
其中，
且(x0,u0)为系统(5)的平衡点，同时考虑系统(5)受到有界随机扰动ω(t)的影响，且ω(t)服从一定的概率分布，则得到：
并且取：
优选地，步骤二中，利用delta方法将线性模型离散化的具体操作为：
考虑np个中性浮力机器人通过无线网络与对应的控制器进行通信，并且这np个中性浮力机器人控制器之间是通过可靠网络进行连接通信。使用无线网络通信需考虑时延与丢包问题，还需要考虑因为各个中性浮力机器人采样周期不同引起的需要更新控制量的控制器对其他中性浮力机器人状态不能准确判断的问题。因此，使用delta算子方法对系统(6)、(7)进行离散化，并且使用预测数据来解决丢包与时延问题，以对各个中性浮力机器人进行准确地控制。经过delta算子方法离散化后的系统模型变为：
δxp(tk)＝apxp(tk)+bpup(tk)+gpωp(tk)
yp(tk)＝cpxp(tk)+dpup(tk)
其中，
t为选定的基础采样周期，即每个中性浮力机器人的控制器都以t为周期来离散化各自的系统；
再将delta算子系统写成离散化的形式得到：
xp(tk+1)＝(tap+ip)xp(tk)+tbpup(tk)+tgpωp(tk)(8)
yp(tk)＝cpxp(tk)+dpup(tk)(9)。
优选地，步骤三具体操作为：
考虑每个中性浮力机器人都受到关于输出yp(tk)的局部概率约束：
pr{yp(tk)≤hp}≥lp(10)
且每个中性浮力机器人耦合输出量scp之和满足如下形式的约束；
scp(tk)＝ecpxp(tk)+fcpup(tk)(11)
其中，行向量cp，dp，ecp，fcp，标量hp，bc和概率lp∈[0,1]，pc∈[0,1]都是事先根据实际情况设定的常量。
优选地，利用双模控制器的思想为每个中性浮力机器人设计如下形式的模型预测控制器：
up(tk+i|tk)＝kpxp(tk+i|tk)+cp(tk+i|tk),i＝0,1,…
其中，kp为离线确定的线性状态反馈增益矩阵，使得对于子系统(ap,bp)，ap+bpkp是严格稳定的；
对于选定的有限预测时域n，cp(tk+i|tk)，i＝0,1,…,n-1是通过在线求解下述优化问题得到的；
当i≥n时，cp(tk+i|tk)＝0，考虑局部成本函数：
其中，lp可以离线求得，通过求解如下优化问题：
minjp(c(tk))
约束为式(10)与(12)，得到最优控制量
与现有技术相比，本发明具有以下有益的技术效果：
本发明公开的基于分布式模型预测控制的中性浮力机器人姿态与轨迹控制方法，针对多个采样周期不同的中性浮力机器人之间的耦合问题以及优化问题进行研究，利用delta算子理论将不同采样周期的中性浮力机器人在控制器端模拟统一的采样周期，当其中某一个或者某几个中性浮力机器人需要更新控制量时，可以通过预测的方式来获取其他中性浮力机器人的状态，从而将其他中性浮力机器人的状态考虑到约束中求解自己的控制量，使得整体系统能够保证耦合概率约束非保守地满足，同时保证系统性能的良好。本发明结合计算机控制的特点，将复杂的在线优化问题转化为二次规划的问题，大大减少了计算量，适于工程应用。
附图说明
图1为本发明方法的流程图。
具体实施方式
下面结合具体的实施例对本发明做进一步的详细说明，所述是对本发明的解释而不是限定。
参见图1，本发明公开的基于分布式模型预测控制的中性浮力机器人姿态与轨迹控制方法，通过以下步骤实现：
(a)：
考虑公式(1)的中性浮力机器人在体坐标系下的动力学模型
其中，m为惯性质量矩阵，c(v)为科里奥利力矩阵，d(v)为机器人在水中受到的黏性阻力，g(η)为负浮力系数，τ为系统输入。所考虑的中性浮力机器人在地理坐标系oxnynzn和机器人体坐标系oxbybzb的关系如下
其中，
其中，j(η)为运动系数矩阵，mη(η)＝j-t(η)mj-1(η)，
其中，
将式(4)进行线性化处理得
其中，
且(x0,u0)为系统(5)的平衡点。
同时考虑系统(5)受到有界随机扰动ω(t)的影响，且ω(t)服从一定的概率分布，则得到
并且取
考虑3个中性浮力机器人通过无线网络与对应的控制器进行通信，并且这3个中性浮力机器人控制器之间是通过可靠网络进行连接通信。使用无线网络通信需考虑时延与丢包问题，还需要考虑因为各个中性浮力机器人采样周期不同引起的需要更新控制量的控制器对其他中性浮力机器人状态不能准确判断的问题。因此，使用delta算子方法对系统(6)、(7)进行离散化，并且使用预测数据来解决丢包与时延问题，以对各个中性浮力机器人进行准确地控制。
经过delta算子方法离散化后的系统模型变为
δxp(tk)＝apxp(tk)+bpup(tk)+gpωp(tk)
yp(tk)＝cpxp(tk)+dpup(tk)
其中p∈{1,2,3}，并且
t＝0.1s为选定的基础采样周期，即每个中性浮力机器人的控制器都以0.1s为周期来离散化各自的系统。3个中性浮力机器人固有的采样周期分别为t1＝0.2s，t2＝0.3s和t3＝0.3s。再将delta算子系统写成离散化的形式得到
xp(tk+1)＝(tap+ip)xp(tk)+tbpup(tk)+tgpωp(tk)(8)
yp(tk)＝cpxp(tk)+dpup(tk)(9)
考虑每个中性浮力机器人都受到关于输出yp(tk)的局部概率约束
pr{yp(tk)≤hp}≥lp(10)
且每个中性浮力机器人耦合输出量scp之和满足如下形式的约束
scp(tk)＝ecpxp(tk)+fcpup(tk)(11)
其中实例中使用的一些参数如下所示
c1＝[11.3]，c2＝[1.40.6]，c3＝[0.90.4]，ecp＝cp，fcp＝0，dp＝0，
lp＝0.8，h1＝15，h2＝8.4，h3＝9,pc＝0.8,bc＝33
(b)
将概率约束(10)和(12)进行处理，得到确定性约束
其中，ψp＝cp+dpkp，δcp＝ecp+fcpkp，φp＝t(ap+bpkp)+ip；
其中γ1＝0.0782，γ2＝0.0680，γ3＝0.0442，ξp＝0.1904，a1＝0.5228，a2＝0.4855，a3＝0.3738，dp＝0.6087。
(c)
利用双模控制器的思想为每个中性浮力机器人设计如下形式的模型预测控制器：
up(tk+i|tk)＝kpxp(tk+i|tk)+cp(tk+i|tk),i＝0,1,…
其中，kp为离线确定的线性状态反馈增益矩阵，使得对于子系统(ap,bp)，ap+bpkp是严格稳定的。对于选定的有限预测时域n，cp(tk+i|tk)，i＝0,1,…,n-1是通过在线求解下述优化问题得到的。当i≥n时，cp(tk+i|tk)＝0。其中，
k1＝[-2.4578-1.3704]
k2＝[-3.1910-1.8029]
k3＝[-1.5109-1.2024]
是lq最优增益。考虑局部成本函数
其中，
则通过求解如下优化问题
minjp(c(tk))
约束为式(13)与(14)，得到最优控制量
模型预测控制器会产生一段时间内的控制量，将这些控制量打包发送给中性浮力机器人，机器人可以挑选当前时刻的控制量进行控制，从而减少网络丢包和时延的影响。通过使用这些控制量，可以完成对中性浮力机器人的姿态和轨迹控制。
综上所述，本发明的优势十分显著：
(1)本发明通过考虑多个采样周期不同的中性浮力机器人，利用分布式模型预测方对其进行姿态与轨迹控制，增强了系统的鲁棒性和抗干扰性。
(2)考虑计算机控制特点，采用delta算子离散化方法对中性浮力进行离散化，同时也方便了不同采样周期系统之间的耦合计算，便于工程实现。
(3)将复杂的优化问题转化成一个变量的二次规划问题，大大减少了预测控制器的在线计算量，便于工程实现。
本发明未详细说明部分属于领域技术人员公知常识。以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。