等值超楠球面，即P,. β礙，则fis(Pi) 二入i ;当 P,' E，fis(Pi)二 0〇
[0080] 第Ξ部分:模块P3
[0081 ]模块P3是用来设计运动体沿着超楠球面法向量上的控制力部分，使得运动体到目 标超楠球面的曲面距离误差减少到满足设计的要求，同时保证运动体始终在可运动范围内 运动，具体的设计步骤如下：
[0082] 第一步：由曲面函数fis和运动体的当前位置Pi，计算运动体到目标超楠球面的曲 面距离误差dis(t)
[0083]
[0084] 第二步：由曲面函数fis计算超楠球面的法向量Ni
[0085]
[0086] 由Ni和运动体的位置和速度，计算曲面距离误差dis(t)对时间的导数
[0087]
[0088] 第Ξ步:设计运动体沿超楠球曲面法向量上的控制力
[0093] ▽乂是曲面函数fis的海森矩阵控制，参数ki〉0,函数ih(dis)是用于保证初始位于 Ω i的第i个运动体始终在Ω i中运动且最终运动于目标超楠球面。
[0094] 第四部分:模块P4
[00M]模块P4是用来设计运动体沿着平面法向量上的控制力部分，使得运动体到期望轨 道所在平面PiO间的平面距离误差减少到满足设计的要求，具体的设计步骤如下：
[0096] 第一步：由运动体的当前位置pi，计算运动体到期望轨道所在平面Pio间的平面距 离误差dip(t)
[0097]
[0098] 第二步：由平面距离误差dip(t)和运动体动态，计算平面距离误差dip(t)对时间的 导数4货
[0099]
[0100] 第Ξ步:设计运动体沿着平面法向量上的控制力
[0103] 第五部分:模块P5
[0104] 模块P5根据信息交互得到相邻运动体的广义旋转角度ξι及其导数^来设计第i个 运动体沿着旋转方向上的控制力用W实现编队，具体的设计步骤如下：
[0105]第一步：由曲面法向量Ni和平面法向量Bi，计算法向量为Bi的平面与超楠球面相切 的切点於1=山1、，9*巧，9柄^，利用牛顿-拉夫森迭代法或割线法数值求解下列方程 [0106] Ni(p*i)=Bi
[0107]数值求解切点p*i。连接切点p*i和坐标原点ois即为旋转轴。旋转轴的方向定义为化 = P*i/Mp*i II。接着，计算第i个运动体所在的位置Pi指向其投影到旋转轴的位置坐标Pip向 量 lip(t)
[010 引
[0109]于是，运动体绕旋转轴旋转的初始角度θι(0)为lip(O)与平面riOisXis间的夹角，计 算公式如下：
[0115]第Ξ步:旋转角0i(t)和期望队形，定义广义旋转角Ci(t)中的参数bi辛0和ΘΛ计算 广义旋转角ξι(υ
[0116] ξι = ?3? 白广白 i*。
[0117] 于是，计算广义旋转角对时间的导数4(0 [011 引
[0119]第四步:根据信息交互所得到的相邻运动体的广义旋转角及其导数，设计运动体 沿着旋转方向上的控制力
[0123] f 0)为期望的广义旋转角速度;a日e {0,1}，当日日=1时表示编队任务中对旋转角 速度有要求，否则曰日=〇;
[0126] 控制参数k4，k6〉〇;如果含有全局可达点的信息交换拓扑是双向的，控制参数ks选 择大于0的常数即可;如果含有全局可达点的信息交换拓扑是单向的，控制参数ks需要满足
其中，叫，1 = 1,2,··· ,η，是-L的特征值。
[0127] 第六部分:模块Ρ6
[012引模块Ρ6综合模块Ρ3，Ρ4和Ρ5的结果计算运动体的控制力输入，完成运动体的运动 控制，具体的按照W下步骤实现：
[0129] 第一步:综合模块Ρ3设计出的运动体沿曲面法向量上的控制力部分、模块Ρ4设计 出沿着平面法向量的控制力部分W及模块Ρ5设计出沿着旋转方向上的控制力部分，联列求 解出最终的每个运动体的控制力输入m
[0130]
[0131] 第二步:上位机将运动体的控制力输入W命令的形式发送给下位机，由运动体的 伺服系统来完成对运动体的运动控制，并返回到模块P3。
[0132] W上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人 员来说，在不脱离本发明原理的前提下，还可W做出若干改进和润饰，运些改进和润饰也应 视为本发明的保护范围。
【主权项】
1. 一种多层环绕编队包围的几何设计方法，其特征在于：多层环绕编队包围中的目标 超椭球面、目标超椭球面上的期望简单凸闭轨道以及运动体动态是在超二次曲面中心坐标 系下描述的，目标超椭球面通过同心压缩的方式扩展为关于曲面函数的等值超椭球面簇， 由曲面的正则性确定第i个运动体的可运动范围。2. 根据权利要求1所述的多层环绕编队包围的几何设计方法，其特征在于:该方法包括 如下步骤： (1) 将全局坐标系下第i个运动体的目标超椭球面、期望简单凸闭轨道以及运动体动态 在超二次曲面中心坐标系下重新表示； (2) 将目标超椭球面扩展为关于曲面函数的等值超椭球面簇； (3) 由曲面函数计算第i个运动体到目标超椭球面的距离，设计第i个运动体沿目标超 椭球面法向量上的控制力，实现第i个运动体的曲面登陆； (4) 计算第i个运动体到期望简单凸闭轨道平面的距离，设计第i个运动体沿期望简单 凸闭轨道平面法向量上的控制力，实现第i个的轨道跟踪； (5) 设计协同环绕运动的旋转轴，计算第i个运动体绕旋转轴的旋转角，设计第i个运动 体沿旋转方向上的控制力，实现第i个运动体的编队运动。3. 根据权利要求2所述的多层环绕编队包围的几何设计方法，其特征在于：所述步骤 (1) 具体包括如下步骤： (11) 建立全局坐标系和超二次曲面中心坐标系间的齐次坐标变换矩阵； (12) 在超二次曲面中心坐标系下重新描述各个第i个运动体对应的目标超椭球面、期 望简单凸闭轨道以及运动体动态。4. 根据权利要求2所述的多层环绕编队包围的几何设计方法，其特征在于：所述步骤 (2) 具体包括如下步骤： (21) 通过同心压缩的方式扩展目标超椭球面，将目标超椭球面扩展为一组等值超椭球 面簇； (22) 根据曲面的正则性，确定第i个运动体的运动范围； (23) 在运动范围内构建第i个运动体的曲面函数，使扩展出的一组等值超椭球面簇可 以由曲面函数取不同的值来表示。5. 根据权利要求2所述的多层环绕编队包围的几何设计方法，其特征在于：所述步骤 (3) 具体包括如下步骤： (31) 由第i个运动体的位置和对应的曲面函数，计算第i个运动体到目标超椭球面的曲 面距离误差； (32) 由曲面距离误差对时间的导数，计算曲面距离误差的变化量； (33) 根据曲面距离误差及其对时间的导数，设计第i个运动体沿目标超椭球面法向量 上的控制力。6. 根据权利要求2所述的多层环绕编队包围的几何设计方法，其特征在于：所述步骤 (4) 具体包括如下步骤： (41) 由第i个运动体的位置，计算第i个运动体到期望简单凸闭轨道平面的平面距离误 差； (42) 由平面距离误差对时间的导数，计算平面距离误差的变化量； (43)根据平面距离误差及其对时间的导数，设计第i个运动体沿期望简单凸闭轨道平 面法向量上的控制力。7.根据权利要求2所述的多层环绕编队包围的几何设计方法，其特征在于：所述步骤 (5)具体包括如下步骤： (51) 由目标超椭球面和期望简单凸闭轨道平面，设计协同环绕运动的旋转轴，并规定 初始旋转角； (52) 由第i个运动体的位置和速度，计算第i个运动体绕旋转轴的旋转角； (53) 根据编队要求确定广义旋转角与旋转角间的函数关系，计算广义旋转角及其对时 间的导数； (54) 由信息交互得到的相邻运动体的信息，设计第i个运动体沿旋转方向上的控制力。
【专利摘要】本发明公开了一种多层环绕编队包围的几何设计方法，多层环绕编队包围中的目标超椭球面、目标超椭球面上的期望简单凸闭轨道以及运动体动态是在超二次曲面中心坐标系下描述的，目标超椭球面通过同心压缩的方式扩展为关于曲面函数的等值超椭球面簇，由曲面的正则性确定第i个运动体的可运动范围。本发明对超二次曲面中心坐标系描述超椭球、轨道和运动体动态尤其适用，该方法简单可靠、精度较高，可用于协同采集等任务。
【IPC分类】G05D1/00
【公开号】CN105629966
【申请号】CN201610069641
【发明人】陈杨杨, 王凯旋, 张亚
【申请人】东南大学
【公开日】2016年6月1日
【申请日】2016年2月1日