小天体协同附着鲁棒避障控制方法转让专利
申请号 : CN202010639213.4
文献号 : CN111766883B
文献日 : 2021-06-15
发明人 : 崔平远 , 葛丹桐 , 朱圣英 , 梁子璇 , 徐瑞
申请人 : 北京理工大学
摘要 :
权利要求 :
1.小天体协同附着鲁棒避障控制方法，其特征在于：包括如下步骤，步骤一、考虑多探测器状态不确定性，结合协同探测一致性算法，采用全对称多胞形对主探测器与跟随探测器的可达集边界进行快速估计，构建协同探测碰撞预测模型，量化评估系统受到的碰撞威胁，保证在线估计效率的同时提高估计结果的鲁棒性；
步骤二、根据步骤一中得到的协同探测碰撞预测模型，设置障碍规避有效区间，设计安全协同目标状态，形成障碍规避性能指标，实现下降过程中障碍规避指标权重的自主调整；
在协同探测碰撞预测模型基础上，为了避免多探测器系统与小天体表面障碍发生碰撞，设计安全协同目标状态，当探测器距离小天体表面较近时，驱动多探测器系统自主向安全协同目标状态转移，提高附着安全性；
步骤三、结合步骤二形成的障碍规避性能指标，在线求解主探测器的混合动态指标模型预测控制问题，得到协同附着鲁棒障碍规避控制律，实现状态不确定下协同附着探测障碍规避，提高任务在状态不确定条件下的安全性与可靠性。
2.如权利要求1所述的小天体协同附着鲁棒避障控制方法，其特征在于：步骤一实现方法为，
探测器动力学方程为
xk+1＝f(xk,uk,wk)＝g(xk,uk)+G(xk,uk)wk (1)其中探测器k时刻状态 由位置rk与速度vk构成， 为控制量，为有界环境扰动；多探测器系统在下降过程中采用主‑从结构，主探测器A的控制量uA在线求解，跟随探测器B1,B2,...BL在下降过程中与主探测器保持固定构型，速度一致；
d
记跟随探测器Bi相对主探测器A的理想构型为ri ，则理想情况下跟随探测器Bi的位置为速度为 其中rA,vA分别为主探测器A的位置与速度；根据一致性算法，跟随探测器Bi的控制律由下式给出其中γi1,γi2为正常数，rBi,vBi分别为跟随探测器Bi的真实位置与速度；根据导航系统状态估计结果，k时刻主探测器A的状态为 跟随探测器Bi的状态为为了构建协同探测碰撞预测模型，首先对主探测器A的可达集边界进行估计；采用全对称多胞形对主探测器状态不确定性XA进行描述m
其中 为主探测器状态估计均值，HA为状态估计误差区间矩阵，B为m个单位m
区间构成的方格，z为B中任意向量，两集合间的闵可夫斯基和定义为主探测器A受到的有界环境扰动WA表达为其中cwA为环境扰动均值，CwA为环境扰动区间矩阵， 为mw个单位区间构成的方格；对m
于任意全对称多胞形集合 其中p为n维实数向量，M为实数区间矩阵，B为m个单位区间构成的方格，定义其外包络为其中mid(M)为区间矩阵中心，对角矩阵G的对角元素为diam(Mij)为区间长度，则有 由此结合探测器动力学方程(1)，得到主探测器在当前状态不确定XA与有界环境扰动WA下的可达集边界函数式中实数向量qA与区间矩阵SA由下式确定qA＝g(pA,uA)+G(pA,uA)cwA (9)SA＝G(pA,uA)CwA (10)区间矩阵 由区间算法计算得到；
根据式(2)与探测器间构型约束对跟随探测器Bi的状态进行估计；同样采用全对称多胞形对跟随探测器状态不确定性XBi与受到的有界环境扰动WBi进行描述；对于状态估计误差集合中任意元素 与主探测器可达集边界函数中任意元素跟随探测器控制律由下式决定
跟随探测器Bi可达集边界函数为式中实数向量qBi、区间矩阵SBi与MBi确定方式与主探测器相同；
结合主探测器可达集边界函数ψA(XA,uA,WA)与跟随探测器可达集边界函数ψB1(XB1,uB1,WB1),ψB2(XB2,uB2,WB2),...,ψBL(XBL,uBL,WBL)，构建协同探测碰撞预测模型；考虑到小天体表面连续密集分布着障碍，采用多面体模型对小天体复杂地形进行近似，每个近似平面记为其中 为拟合参数；多探测器系统与近似平面间的最小距离即一步预测下的剩余安全距离为
T
其中Ψr为集合Ψ＝ψA∪ψB1∪ψB2∪...ψBL在位置空间的投影，r＝[x,y,z] 为集合Ψr中任意元素， 为距离多探测器系统最近的近似平面，对应的规避方向即为该近似平面的法线方向
根据式(14)‑(15)得到的协同探测碰撞预测模型，从剩余安全距离dr与规避方向η两方面对多探测器系统受到的环境碰撞威胁实现量化评估，且能够保证在线估计效率的同时提高估计结果的鲁棒性。
3.如权利要求2所述的小天体协同附着鲁棒避障控制方法，其特征在于：步骤二实现方法为，
考虑到多探测器距离小天体表面较远时没有避障需求，距离小天体表面过近时难以满足避障最小转移距离要求，设置障碍规避有效区间为[εl,εu]，即当且仅当式(14)得到的剩余安全距离dr∈[εl,u]时，协同探测系统进行避障转移；由于主探测器A的运动决定了整个协同探测系统的运动，针对主探测器A设定安全协同目标位置rsafe与速度vsafe分别为其中 为主探测器A当前位置rA在近似平面 上的投影，vmax为协同探测系统最大运动速度；由此得到安全协同目标状态进一步基于安全协同目标状态设计障碍规避性能指标其中N为预测步数，xA(k)为主探测器在第t步的状态，Q'为动态权重矩阵；为了实现有效区间范围内控制目标由精确附着向障碍规避的自主调整，将权重矩阵Q'设计为剩余安全距离的函数
其中
根据公式(19)(20)实现下降过程中障碍规避指标权重的自主调整。
4.如权利要求3所述的小天体协同附着鲁棒避障控制方法，其特征在于：步骤三实现方法为，
主探测器控制向量通过在线求解模型预测控制问题得到，根据式(19)中的障碍规避性能指标，针对以下混合动态指标
在滚动时域上形成最优控制问题
s.t.
xA(t+1|j)＝g(xA(t|j),uA(t|j))+G(xA(t|j),uA(t|j))wA(t|j)其中N为预测步数，t为当前滚动时域的第t步，xA(t|j)为主探测器从j时刻起第t步的状态向量，uA(t|j)为从j时刻起第t步的控制向量，P,Q,R为常值权重矩阵，为主探测器j时刻位置与速度， 为主探测器末端时刻tf位置与速度， 为主探测器允许状态集合， 为探测器系统控制不变集，为主探测器可行控制集合；
对以上优化问题进行求解，得到的最优控制序列第一项施加在主探测器上；为了得到协同附着鲁棒障碍规避控制律，将更新后的主探测器状态代入式(2)，形成构型约束下跟随探测器的避障控制；随后将新的主探测器状态作为初始状态代入问题(23)，结合协同探测碰撞预测模型更新混合动态指标，重新求解最优控制问题，直到协同探测系统附着在小天体表面，实现状态不确定下协同附着探测障碍规避，提高任务在状态不确定条件下的安全性与可靠性。
说明书 :
小天体协同附着鲁棒避障控制方法
技术领域
背景技术
靠性与自主性，通过探测器间相互协作配合提高观测与控制精度，从而完成更复杂的深空
机动操作与科学任务。在开展协同附着探测时，小天体表面的多样异构形貌对探测器安全
构成威胁。为了避免探测器在下降过程中与小天体表面障碍发生碰撞，文献中分别提出了
势函数法、避障轨迹优化、轨迹曲率设计等方法提高附着任务的安全性。在此基础上，还需
结合协同探测模式特点，考虑探测器间构型约束，综合考量各探测器与附近障碍距离，准确
评估协同探测系统所受到的安全威胁。此外，针对下降过程中探测器状态不确定性对障碍
规避的影响，有文献提出构造安全膨胀椭球对潜在碰撞概率进行估计，通过优化轨迹降低
附着风险。当多探测器位置与速度同时存在不确定性时，评估环境带来的碰撞威胁将变得
更为复杂棘手，且需要采用鲁棒性更强的控制方法，实现多探测器状态不确定性下的安全
精确附着任务目标。
发明内容
上碰撞预测机制，实现对潜在碰撞风险的及时发现与规避，在线调整多探测器控制策略，在
满足构型约束条件下，实现精确附着与障碍规避目标间的自主切换，生成协同避障鲁棒控
制律，实现协同模式下的鲁棒障碍规避控制，提高任务在状态不确定条件下的安全性与可
靠性。
同探测碰撞预测模型，对协同探测系统受到的碰撞威胁进行量化评估；进而基于安全目标
状态设计障碍规避性能指标，实现有效区间范围内控制目标由精确附着向障碍规避的自主
调整；最后在线求解动态指标模型预测控制问题，得到协同附着鲁棒障碍规避控制律，提高
任务在状态不确定条件下的安全性与可靠性。
化评估系统受到的碰撞威胁，保证在线估计效率的同时提高估计结果的鲁棒性。
制量uA在线求解，跟随探测器B1,B2,...BL在下降过程中与主探测器保持固定构型，速度一
d
致。记跟随探测器Bi相对主探测器A的理想构型为ri ，则理想情况下跟随探测器Bi的位置为
速度为 其中rA,vA分别为主探测器A的位置与速度。根据一致性算法，
跟随探测器Bi的控制律由下式给出
态为
单位区间构成的方格，z为B中任意向量，两集合间的闵可夫斯基和定义为
m
B为m个单位区间构成的方格，定义其外包络为
计误差集合中任意元素 与主探测器可达集边界函数中任意元素
跟随探测器控制律由下式决定
表面连续密集分布着障碍，采用多面体模型对小天体复杂地形进行近似，每个近似平面记
为
的规避方向即为该近似平面的法线方向
时提高估计结果的鲁棒性。
调整。
安全协同目标状态转移，提高附着安全性。
的剩余安全距离dr[εl,εu]时，协同探测系统进行避障转移。由于主探测器A的运动决定了整
个协同探测系统的运动，针对主探测器A设定安全协同目标位置rsafe与速度vsafe分别为
安全距离的函数
测障碍规避，提高任务在状态不确定条件下的安全性与可靠性。
为主探测器j时刻位置与速度， 为主探测
器末端时刻tf位置与速度， 为主探测器允许状态集合， 为探测器系统控制不变集，
为主探测器可行控制集合。
跟随探测器的避障控制。随后将新的主探测器状态作为初始状态代入问题(23)，结合协同
探测碰撞预测模型更新混合动态指标，重新求解最优控制问题，直到协同探测系统附着在
小天体表面，实现状态不确定下协同附着探测障碍规避，提高任务在状态不确定条件下的
安全性与可靠性。
行快速估计，通过构建协同探测碰撞预测模型，设置障碍规避有效区间，设计安全协同目标
状态，形成带障碍规避的混合动态指标，进一步通过在线求解滚动时域最优控制问题，得到
协同附着鲁棒障碍规避控制律，实现下降过程控制目标自主调整，提高状态不确定条件下
的小天体协同附着系统安全性。
响，实现多探测器协同附着鲁棒避障控制。
同时提高估计结果的鲁棒性。
附图说明
具体实施方式
星自转角速度ω＝3.31×10 rad/s，密度ρ＝2.67×10kg/m ，引力常量G＝6.67×10 N·
2 2
m/kg。采用主从探测模式，即一个主探测器A与两个跟随探测器B1,B2，主探测器A初始位置
T T
rA0＝[5000,7000,1000]m，目标着陆点位置rAf＝[4470,4980,600.1]m，跟随探测器B1初始
T T
位置rB10＝[5030,7000,1040]m，目标着陆点位置rB1f＝[4480,5000,610]m，跟随探测器B2
T T
初始位置rB20＝[4980,7000,1000] m，目标着陆点位置rB2f＝[4460,5000,610]m，所有探测
T
器初始速度相同且理想末端速度一致，即vA0＝vB10＝vB20＝[1,‑2,‑1]m/s，vAf＝vB1f＝vB2f＝
T
[0,0,0]m/s。
化评估系统受到的碰撞威胁，保证在线估计效率的同时提高估计结果的鲁棒性。
体引力等环境扰动，满足w～N(0,1e )。
B1相对主探测器A的理想构型为r1＝[10,0,10]，跟随探测器B2相对主探测器A的理想构型
为 跟随探测器Bi的控制律中系数γ11＝γ21＝0.01,γ12＝γ22＝0.1。当
不存在导航误差时，多探测器系统的附着轨迹变化如图2所示，三个探测器保持严格构型，
轨迹基本一致。
估计均值
环境扰动区间矩阵CwA＝3e I3。由此结合探测器动力学方程(23)，考虑[‑0.1,0.1]m/s间所
有可行控制量，根据式(8)得到主探测器在当前状态不确定XA与有界环境扰动WA下10s的可
达集边界函数ψA(XA,uA,WA)。
时，采用式(11)所示控制律的系统10s可达集边界。
T
4980,600.1]m附近范围内100m×100m的复杂地形进行近似，根据式(14)‑(15)得到的协同
探测碰撞预测模型，从剩余安全距离dr与规避方向η两方面对多探测器系统受到的环境碰
撞威胁实现量化评估，保证在线估计效率的同时提高估计结果的鲁棒性。
调整。
的剩余安全距离dr∈[2,20]m时，协同探测系统进行避障转移。针对主探测器A根据式(16)‑
(17)设定安全协同目标位置rsafe与速度vsafe，其中障碍规避有效区间上下界分别为εl＝2m,
εu＝20m，协同探测系统最大运动速度vmax＝5m/s，由此得到安全协同目标状态
根据公式(19)(20)实现下降过程中障碍规避指标权重的自主调
整。
测障碍规避，提高任务在状态不确定条件下的安全性与可靠性。
控制变量对应的权重矩阵分别为Q＝I6,R＝I3，末端状态权重矩阵P通过求解里卡蒂方程得
到。采用YALMIP工具包对非线性模型预测控制问题进行求解，得到的最优控制序列第一项
施加在主探测器上。为了得到协同附着鲁棒障碍规避控制律，将更新后的主探测器状态代
入式(2)，形成构型约束下跟随探测器的避障控制。随后将新的主探测器状态作为初始状态
代入问题(22)，结合协同探测碰撞预测模型更新混合动态指标，重新求解最优控制问题，直
到协同探测系统附着在小天体表面，实现状态不确定下协同附着探测障碍规避，提高任务
在状态不确定条件下的安全性与可靠性。
过程中三个探测器在X，Y，Z轴的位置、速度与控制量的变化，其中实线为各变量在X轴的变
化曲线，点画线为各变量在Y轴的变化曲线，虚线为各变量在Z轴的变化曲线。图7‑8分别给
出了探测器A在运动过程中位置与速度的估计误差在X，Y，Z轴的变化曲线，其中位置与速度
估计误差随着探测器A与小行星表面距离的减小而减小，意味着下降过程中导航精度不断
升高，跟随探测器的控制精度也不断提高。
围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明
的保护范围之内。