基于多运动模态多视点几何的多机器人系统队形变换方法
【专利摘要】本发明公开了一种基于多运动模态多视点几何的多机器人系统队形变换方法，包括：计算目标队形多机器人系统的相互投影的几何张量；计算每一时刻的几何张量，与上述几何张量进行比较，若相同则表示队形变换完成，不相同则选取一台跟随机记为C1，两台先导机记为C2和C3，计算C1与C2、C3之间的两焦点张量和C1目标位置在图像上的两条投影直线，两条投影直线的交点即为C1目标位置在图像上的投影点x；令C1沿初始位置与x的连线运动，直到C1在C2、C3摄像机图像上的投影点位置与队形变换后的投影位置重合，对剩余跟随机逐一进行上述处理，从而完成多机器人系统的队形变换。本发明提供的方法使系统在复杂的野外环境中具备高精度的队形变换功能。
【专利说明】
基于多运动模态多视点几何的多机器人系统队形变换方法
技术领域
[0001] 本发明涉及机器人技术领域，具体涉及基于多运动模态多视点几何的多机器人系 统队形变换方法。
【背景技术】
[0002] 机器人学(Robotics)是一门涉及传感、机械传动、计算机科学、认知社会科学等领 域的复合型系统学科。在军事、国防、安全、工业、运输等领域中，移动机器人代替人类执行 各种任务，在效率、安全上的优势也得以体现。其中，以多个结构和传感手段相对简单的机 器人协作完成某项复杂的任务，在降低机器人机构成本的同时，增加了安全性。例如，在危 险环境中，机器人协作执行运输任务，能避免危险环境对人员造成的各种伤害。于是人们考 虑由多个机器人组成的群体系统通过协调、协作来完成单机器人无法或难以完成的工作， 多机器人技术应运而生。但是面对复杂的野外环境，现有的多机器人控制技术需要统一的 队列表示框架，在运动过程中队形变换能力差，适用性能较弱。
【发明内容】
[0003] 发明目的：针对现有技术的不足，本发明提供一种基于多运动模态多视点几何的 多机器人系统队形变换方法，使系统在复杂的野外环境中具备高精度的队形变换功能。
[0004] 技术方案：本发明所述基于多运动模态多视点几何的多机器人系统队形变换方 法，用于具有至少两台先导机和若干台跟随机的多机器人系统；其特征在于，包括如下步 骤：
[0005] (1)利用基于融合多运动模态多视点几何计算相互投影的多机器人系统目标队形 的几何张量；
[0006] (2)利用三维空间中的点及机器人携带摄像机的投影像计算每一时刻的几何张 量，并且将每一时刻的几何张量与步骤（1)中的几何张量进行比较，若与其相同则表示队形 变换完成，不相同表示未完成，则执行步骤(3);
[0007] (3)选取一台跟随机记为&，两台先导机分别记为CdPC3,队形变换前&Χ 2、(：3的位 置分别记为(^、(^、(^，(：2和(：3先于(： 1做队形变换运动，运动后的位置为(^+1、(^+1;计算(^ 和C 2t+1之间的两焦点张量F12，利用公式lFF^e，2计算&目标位置(:严在心图像上的投影 直线h，其中对极点e 21t+2是CP在C2t+1图像上的投影;计算W与C 3t+1之间的两焦点张量F13， 利用公式l2 = F13e31t+2计算&目标位置(^+2在&图像上的投影直线1 2,其中对极点e31t+2是C。 +2在C 3t+1图像上的投影，投影直线13与投影直线12的交点X即为&目标位置C， 2在&图像上的 投影点；令&沿(^与X的连线运动，直到C^C2、C 3摄像机图像上的投影点位置e21与e31与队 形变换后的目标投影位置e 21t+2与e31t+2重合，即实现队形变换的控制；
[0008] (4)对剩余跟随机逐一进行步骤(3)的处理，从而完成多机器人系统的队形变换。
[0009] 进一步完善上述技术方案，所述步骤(1)的计算方法为:首先对具有射影摄像机的 机器人进行运动轨迹建模，选择B-Spl ine曲线模型表示机器人在静止、直线运动、曲线运动 的状态和运动轨迹;其次建立空间点与图像点的映射关系，将非线性映射转换为线性映射； 再次，构建融合多运动模态多视点几何的表达式并分析其代数几何特性，联合多台摄像机 的线性映射关系式，用张量表达式来综合表示所有有效的关系式并分析其代数几何特性， 构建融合多运动模态的多视点几何;令机器人相互投影到其他机器人的摄像机平面，其投 影点称为对极点，研究每一组对极点与多焦点张量的多线性关系，接着计算每组对极点对 应的多线性方程的线性独立的关系式个数，结合多焦点张量的自由度可以推算出计算张量 需要的最少对应点数，实现利用相互投影法优化融合多运动模态多视点几何。
[0010] 进一步地，所述步骤(2)中的计算方法与步骤(1)相同。
[0011 ] 进一步地，所述步骤(3)中C#PC3先于&做队形变换运动，队形变换完成后的&、&、 C3的位置分别为&t+2、C2t+1、C3 t+1，&在&和C3上的对极点分别为e21t+2和e 31t+2，计算&、C2、C3之 间的三焦点张量T，通过确定Ci目标位置Ci t+2在&图像上的 投影点X，从而确定&目标位置(：严;令心沿(^与X的连线运动，直到C^C 2、C3摄像机图像上 的投影点位置e21与e31与队形变换后的投影位置e 21t+2与e31t+2重合，即实现&Χ 2、(：3队形变换 的控制；
[0012] (4)对剩余跟随机逐一进行步骤(3)的处理，从而完成多机器人系统的队形变换。
[0013] 进一步地，当所述先导机的台数大于等于3时，利用Ν焦点张量(Ν>3)计算每个跟随 机的目标位置，实现多机器人系统的队形变换。
[0014] 进一步地，所述步骤(3)中直到(：2和(：3在cr图像上的e21与e 31的位置与队形变换后 的投影位置e21t+2与e31 t+2误差小于等于5pixel时，即实现&Χ2、(：3队形变换的控制。
[0015]有益效果:与现有技术相比，本发明的优点:本发明针对复杂的野外环境提供的多 机器人系统队形变换方法，无需统一的队形表示框架，利用基于融合多运动模态的多视点 几何、相互投影法、精确定位等思想，为地面运动的多机器人系统设计可行的队形控制方 案，使系统在复杂的野外环境中具备高精度的队形变换功能。
【附图说明】
[0016] 图1为一台先导机时利用两焦点张量实现机器人协调运动的实验原理图；
[0017] 图2为二台先导机时利用两焦点张量实现机器人协调运动的实验原理图；
[0018] 图3为二台先导机时利用三焦点张量实现机器人协调运动的实验原理图。
【具体实施方式】
[0019] 下面通过附图对本发明技术方案进行详细说明。
[0020] 实施例1:基于多运动模态多视点几何的多机器人系统队形变换方法，用于具有至 少两台先导机和若干台跟随机的多机器人系统，每个机器人均装有一台摄像机，摄像机所 拍图像为该机器人的图像;变换方法包括如下步骤：
[0021 ] (1)利用基于融合多运动模态多视点几何计算相互投影的多机器人系统目标队形 的几何张量:首先对具有射影摄像机的机器人进行运动轨迹建模，选择B-Spline曲线模型 表示机器人在静止、直线运动、曲线运动的状态和运动轨迹;建立空间点与图像点的映射关 系，将非线性映射转换为线性映射;再次，构建融合多运动模态多视点几何的表达式并分析 其代数几何特性，联合多台摄像机的线性映射关系式，用张量表达式来综合表示所有有效 的关系式并分析其代数几何特性，构建融合多运动模态的多视点几何;令机器人相互投影 到其他机器人的摄像机平面，其投影点称为对极点，研究每一组对极点与多焦点张量的多 线性关系，接着计算每组对极点对应的多线性方程的线性独立的关系式个数，结合多焦点 张量的自由度可以推算出计算张量需要的最少对应点数，实现利用相互投影法优化计算每 一时刻的几何张量，
[0022] (2)利用空间中的点或摄像机的投影像计算每一时刻的几何张量，计算方法与步 骤（1)相同并与步骤（1)中的几何张量进行比较，若与其相同则表示队形变换完成，不相同 表示未完成，则需要调整，执行如下步骤；
[0023] (3)基于两焦点张量的队形变换算法：
[0024]如果队列里有一个机器人是先导机，从某一时刻首先做了队形变换，那么需要调 整其它所有机器人的位置，以变换到目标队形，算法如下:如图1所示，两个机器人队形变换 前的位置分别记为，当第二个机器人先运动时，把C 2设为先导机，则&为跟随机，当先 导机C2运动至C2 t+1时，跟随机&还在初始位置，队形变换后两者的位置记为(：广2、C2 t+1，对 极点e21t+2用来标记移动后的队形。首先计算和C 2t+1之间的两焦点张量F12，根据多视点几 何得到图像上的投影直线h，计算公式是在这条直线1:上移动， 直到e 21与e21t+2重合(误差不超过5pixel)，但这种方法不能快速精确地控制机器人运动的 位置，所以考虑在有二个机器人作为先导机的情况下利用两焦点张量对机器人进行队形变 换的方法。
[0025] 第二台先导机设为C3,两个先导机CdPC3先运动，跟随机为Q，假设两个先导机先 进行队列变换，则跟两个摄像机的情况类似，能够根据c3t+1和之间的两焦点张量F13和对 极点 e31t+2之间关系式l2 = F13e31t+2算出Cit+2在&图像的投影直线1 2,结合之前得到的投影直 线h，就能找出两条直线的交点X，也就是目的位置(^+2在&图像的投影，如图2所示，令跟随 机心沿着W与X的连线运动，直到&在(： 2和(：3图像上的投影e21和e31的位置和e21 t+2与e31t+2的 位置重合(误差不超过5pixel)，即完成对多机器人系统队形变换的控制。同理，其他的机器 人也可以用这种方法"跟进"。
[0026] (4)利用三焦点张量的队形变换算法：
[0027]和利用两焦点张量实现机器人队形变换的方法类似，我们也可以用对极点来表示 机器人的位置信息。把队形变换之后三台机器人之间的对极点设为et+2,则第二和第三台机 器人在第一台机器人上的对极点分别为e 12t+2和e13t+2,而第一台机器人在第二和第三台机 器人上的对极点分别为e 21t+2和e31t+2。假设第二，第三台机器人作为先导机先运动，第一台 机器人跟随，那么在先导机运动后，三台机器人的位置分别是和C 2t+1和C3t+1，计算此时三 台机器人之间的三焦点张量T，再通过确定&目标位置(^+ 2 在C·像上的投影点X，从而确定Cig标位置定义为:对r，s，t = l，2,3,张量 Srst^J ：
[0029] TF为三焦点张量;而跟随机器人的目的位置(：广2在第一台机器人上的投影点X就 能唯一的确定，那么跟随机器人的目的位置(：广 2也可以确定，其原理见图3。令跟随机&沿着 cr与X的连线运动，直到&在(：2和(：3上的投影e21与e 31的位置和e21t+2与e31t+2的位置重合(误 差不超过5pixel)，即完成对多机器人系统队形变换的控制。同理，其他的机器人也可以用 这种方法"跟进"。
[0030] 5)利用N焦点张量的队形变换算法。当先导机的个数大于等于3时，可以利用N焦点 张量( N>3)计算每个跟随机的目标位置，例如先导机有3台时，可利用四焦点张量来计算跟 随机位置，计算方法如步骤(4)所示，从而实现多机器人系统的队形变换。
[0031] 参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身 的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细 节上作出各种变化。
【主权项】
1. 基于多运动模态多视点几何的多机器人系统队形变换方法，用于具有至少两台先导 机和若干台跟随机的多机器人系统;其特征在于，包括如下步骤： (1) 利用基于融合多运动模态多视点几何计算相互投影的多机器人系统目标队形的几 何张量； (2) 利用三维空间中的点及机器人携带摄像机的投影像计算每一时刻的几何张量，并 且将每一时刻的几何张量与步骤（1)中的几何张量进行比较，若与其相同则表示队形变换 完成，不相同表示未完成，则执行步骤(3); (3) 选取一台跟随机记为&，两台先导机分别记为C4PC3,队形变换前&Χ2、(： 3的位置分 别记为&\(^、(^，(：2和(：3先于(： 1做队形变换运动，运动后的位置为(^+1、(^+1;计算(^和(^+1 之间的两焦点张量F12，利用公式lFF^e，2计算&目标位置&"2在&图像上的投影直线h， 其中对极点e 21t+2是Cit+2在C2t+1图像上的投影;计算W与C 3t+1之间的两焦点张量F13,利用公 式l2 = F13e31t+2计算Q目标位置(^+?^图像上的投影直线1 2,其中对极点e31t+2是CP在Cf +1图像上的投影，投影直线13与投影直线12的交点X即为(^目标位置(^^在心图像上的投影 点；令&沿(^与X的连线运动，直到C^C 2、C3摄像机图像上的投影点位置e21与e31与队形变 换后的目标投影位置e 21t+2与e31t+2重合，即实现&Χ 2、(：3队形变换的控制； (4) 对剩余跟随机逐一进行步骤(3)的处理，从而完成多机器人系统的队形变换。2. 根据权利要求1所述的基于多运动模态多视点几何的多机器人系统队形变换方法； 其特征在于:所述步骤(1)的计算方法为:首先对具有射影摄像机的机器人进行运动轨迹建 模，选择B-Spline曲线模型表示机器人在静止、直线运动、曲线运动的状态和运动轨迹;其 次建立空间点与图像点的映射关系，将非线性映射转换为线性映射;再次，构建融合多运动 模态多视点几何的表达式并分析其代数几何特性，联合多台摄像机的线性映射关系式，用 张量表达式来综合表示所有有效的关系式并分析其代数几何特性，构建融合多运动模态的 多视点几何;令机器人相互投影到其他机器人的摄像机平面，其投影点称为对极点，研究每 一组对极点与多焦点张量的多线性关系，接着计算每组对极点对应的多线性方程的线性独 立的关系式个数，结合多焦点张量的自由度可以推算出计算张量需要的最少对应点数，实 现利用相互投影法优化融合多运动模态多视点几何。3. 根据权利要求2所述的基于多运动模态多视点几何的多机器人系统队形变换方法； 其特征在于:所述步骤(2)中的计算方法与步骤(1)相同。4. 根据权利要求1所述的基于多运动模态多视点几何的多机器人系统队形变换方法： 所述步骤⑶中C#PC3先于&做队形变换运动，队形变换完成后的&Χ 2、(：3的位置分别为C。 +2、(^+1、(^ +1，&在(：2和(：3上的对极点分别为62广 2和63广2，计算(：1、(：2、(： 3之间的三焦点张量1'， 再通过确定C!目标位置Cit+2在&图像上的投影点X，从而确 定&目标位置Cf 2,令&沿(^与X的连线运动，直到C^C2、C3摄像机图像上的投影点位置e 21 与e31与队形变换后的目标投影位置e21t+2与e31 t+2重合，即实现队形变换的控制； (4)对剩余跟随机逐一进行步骤(3)的处理，从而完成多机器人系统的队形变换。5. 根据权利要求1所述的基于多运动模态多视点几何的多机器人系统队形变换方法： 当所述先导机的台数大于等于3时，利用Ν焦点张量(Ν>3)计算每个跟随机的目标位置，从而 实现多机器人系统的队形变换。6. 根据权利要求1至5任一所述的基于多运动模态多视点几何的多机器人系统队形变 换方法:所述步骤(3)中直到C#PC3在C。图像上的e21与e31的位置与队形变换后的投影位置 e21t+2与e31t+2误差小于等于5pixel时，即实现&Χ 2Χ3队形变换的控制。
【文档编号】G05D1/12GK105867428SQ201610331615
【公开日】2016年8月17日
【申请日】2016年5月18日
【发明人】万程
【申请人】南京航空航天大学