间 中所有满足-P iC λ i< ε <〇〇点的集合。
[0075] 第三步，由于集合Q1中的每一点都属于Ω i中的一个扩展球面，我们可以在Ω 构建曲面函数
[0077] 进而，Ω ;可以表不为Ω {p ; G WI - p f is (Pi) < ε }。5^是关于曲面函数 fls的一条等值球面，BP
[0078] 3)模块 P3
[0079] 模块P3是用来设计运动体沿着球面法向量上的控制力部分，使得运动体到目标 球面的距离误差减少到满足设计的要求，同时保证运动体始终在可运动范围内运动，具体 的设计步骤如下：
[0080] 第一步：由曲面函数fls和运动体的当前位置P1，计算运动体到目标球面的距离误 差 dis (t)
[0082] 第二步：由轨道函数fls计算球面法向量N i
[0084] 由N1和运动体的位置和速度，计算距离误差d ls(t)对时间的导数
[0086] 第三步：设计运动体沿球面法向量上的控制力
[0091] 控制参数1^> 0 ,函数Φ ; (dis)是用于保证初始位于Ω ;的第i个运动体始终在Ω ; 中运动且最终运动于目标球面。
[0092] 4)模块 P4
[0093] 模块P4是用来设计运动体沿着球面经线方向上的控制力部分，使得运动体所在 球面上的炜度与期望圆轨道对应的炜度间的炜度误差减少到满足设计的要求，具体的设计 步骤如下：
[0094] 第一步：由运动体的当前位置Pi,计算运动体位于球面上的炜度爲(?
[0095]
[0096] 由期望轨道对应的维度碎，计算维度误差<9(〇
[0097]
[0098] 第二步：由维度误差和运动体动态，计算维度误差对时间的导数⑴
[0099]
[0100] 其中B1为球面经线方向向量，具体表达式如下：
[0102] 第三步：设计运动体沿着球面经线方向上的控制力
[0107] 控制参数1^2，1^>0。
[0108] 5)模块 P5
[0109] 模块P5根据信息交互得到相邻运动体的广义旋转角度ξ 其导数處来设计第i 个运动体沿着球面炜线方向上的控制力用以实现编队，具体的设计步骤如下：
[0110] 第一步：规定Z1轴为旋转轴，运动体的旋转角定义为绕Z i轴逆时针旋转角度。运 动体的旋转角的初始值定义为θ1(]
[0116] 第二步：旋转角ejt)和期望队形，定义广义旋转角I1U)中的参数和 计算广义旋转角C i (t)
[0118] 于是，计算广义弧长对时间的导数?.(/)
[0120] 第四步：根据信息交互所得到的相邻运动体的广义旋转角及其导数，设计运动体 沿着球面炜线方向上的控制力
[0124] 为期望的广义旋转角速度；{〇, 1}，当a。= 1时表示编队任务中对旋转 角速度有要求，否则a。= 0;
[0127] 控制参数k4, k6> 0 ;如果含有全局可达点的信息交换拓扑是双向的，控制参数k 5 选择大于〇的常数即可；如果含有全局可达点的信息交换拓扑是单向的，控制参数k5需要 满足
[0129] 其中 μ i，i = 1，2, . . .，n，是-L 的特征值。
[0130] 6)模块 P6
[0131] 模块P6综合模块P3、P4和P5的结果计算运动体的控制力输入，完成运动体的运 动控制，具体的按照以下步骤实现：
[0132] 第一步：综合模块P3设计出的运动体沿球面法向量上的控制力部分、模块P4设计 出沿着球面经线方向上的控制力部分以及模块P5设计出沿着球面炜线方向上的控制力部 分，联列求解出最终的每个运动体的控制力输入U 1
[0134] 第二步：上位机将运动体的控制力输入以命令的形式发送给下位机，由运动体的 伺服系统来完成对运动体的运动控制，并返回到模块P3。
【主权项】
1. 一种球面环绕编队控制的几何设计方法，其特征在于其中的目标球面、球面上的期 望圆轨道以及运动体动态是在轨道坐标系下描述的，该方法包括如下步骤： a) 将惯性坐标系下的球面、圆轨道及运动体动态在轨道坐标系下重新表示； b) 将目标球面扩展为不同曲面函数值表示的球面簇； c) 由曲面函数值计算运动体到目标球面的距离误差及其导数，设计沿球面法向量上的 控制力实现球面登陆； d) 计算运动体所在球面的炜度与期望轨道对应的炜度间的误差及其导数，设计沿球面 经线方向上的控制力实现环绕运动； e) 计算运动体绕轴广义旋转角及其导数，设计沿球面炜线方向上的控制力完成编队； f) 综合步骤c)-步骤e)得到的运动体沿球面法向量、经线方向以及炜线方向上的控制 力部分，联列求解出运动体的控制力输入。2. 按权利要求1所述的球面环绕编队控制的几何设计方法，其特征在于：其中所述步 骤a)包括如下步骤： al)将惯性坐标系的原点平移到目标球面的球心、将z轴旋转到平行于期望圆轨道所 在平面的法向量，从而得到轨道坐标系及坐标转移矩阵； a2)在轨道坐标系下重新描述目标球面、圆轨道和运动体动态。3. 按权利要求1所述的球面环绕编队控制的几何设计方法，其特征在于：其中所述步 骤b)包括如下步骤： bl)通过同心压缩扩张球面，将目标球面扩展为一组等值球面； b2)根据曲面的正则性，确定对应运动体的运动范围； b3)在运动范围上构建曲面函数，使扩展出的球面簇可以由曲面函数取不同的值来表 不。4. 按权利要求1所述的球面环绕编队控制的几何设计方法，其特征在于：其中所述步 骤c)包括如下步骤： cl)由运动体的位置和曲面函数，计算运动体到目标球面的距离误差； c2)由距离误差对时间的导数，计算距离误差的变化量； c3)由距离误差和距离误差变化量，设计运动体沿球面法向量上的控制力部分。5. 按权利要求1所述的球面环绕编队控制的几何设计方法，其特征在于：其中所述步 骤d)包括如下步骤： dl)由运动体的位置，计算运动体所在球面上的炜度与期望轨道对应的炜度间的炜度 误差； d2)由炜度误差对时间的导数，计算炜度误差的变化量； d3)由炜度误差和炜度误差的变化量，设计运动体沿球面经线方向上的控制力部分。6. 按权利要求1所述的球面环绕编队控制的几何设计方法，其特征在于：其中所述步 骤e)包括如下步骤： el)规定好旋转轴和初始旋转角； e2)由运动体的位置和速度，计算运动体绕轴旋转角； e3)根据编队要求确定广义旋转角与旋转角间的函数关系，计算广义旋转角及其对时 间的导数； e4)由信息交互得到的相邻运动体的信息，设计运动体沿球面炜线方向上的控制力部 分。7.按权利要求1所述的球面环绕编队控制的几何设计方法，其特征在于：其中所述步 骤f)包括如下步骤： fl)综合步骤c)-步骤e)计算运动体的控制力输入； f2)由上位机将运动体的控制力输入发送给下位机中，通过伺服系统来完成运动控制。
【专利摘要】本发明是一种球面环绕编队控制的几何设计方法，包括如下步骤：a)将惯性坐标系下的球面、圆轨道及运动体动态在轨道坐标系下重新表示；b)将目标球面扩展为不同曲面函数值表示的球面簇；c)由曲面函数值计算运动体到目标球面的距离误差及其导数，设计沿球面法向量上的控制力实现球面登陆；d)计算运动体所在球面的纬度与期望轨道对应的纬度间的误差及其导数，设计沿球面经线方向上的控制力实现环绕运动；e)计算运动体绕轴广义旋转角及其导数，设计沿球面纬线方向上的控制力完成编队；f)综合步骤c)-e)计算运动体的控制力输入。本发明对轨道坐标系下描述的球面、圆轨道以及运动体动态尤其适用。该方法简单可靠、精度较高，可用于协同探测等任务。
【IPC分类】G05D1/10
【公开号】CN105045285
【申请号】CN201510582120
【发明人】陈杨杨, 王凯旋
【申请人】东南大学
【公开日】2015年11月11日
【申请日】2015年9月14日