CN115319758A - 一种用于微波远场-近场扫描与成像任务的机械臂控制策略与优化方法 - Google Patents

Abstract

本发明公开了一种用于微波远场‑近场扫描与成像任务的机械臂控制策略与优化方法，首先根据6关节机械臂关节限位角约束和末端执行器天线极化方向平面采样面的限制，得到机械臂的实际不可达工作区，随后依据空间采样面上的末端采样点与不可达区域不存在交集，得到微波源近场成像最大采样面，最后基于笛卡尔坐标系下的路径最短原则以及关节空间下前五轴关节变化幅度最下原则，得到6关节机械臂关节位移的最佳路径规划。可以快速求解约束空间内的最大扫描柱面与最优运动控制策略，解决机械臂灵巧工作空间会出现不可达区域的问题，进而在约束范围内做到对微波成像扫描的最优实现。

Description

一种用于微波远场-近场扫描与成像任务的机械臂控制策略 与优化方法

技术领域

本发明属于电磁兼容技术，通过机器人抓持检测天线完成对目标辐射源的自动检测与定位。该技术方案将多关节机械臂与微波远场-近场变换及成像(Emission SourceMicroscopy，ESM)技术相结合，重点研究其在柱面扫描微波源近场成像任务中针对机械臂的控制策略。通过控制策略，可以在给定探测天线特性和机械臂构型参数的条件下快速生成对应的最大扫描面及机械臂运动路径，实现对首测微波源的高精度快速扫描与进场成像。

背景技术

随着电子工业的发展特别是5G乃至太赫兹技术的兴起，对电子设备的电磁兼容检测(Electro Magnetic Compatibility,EMC)及电磁干扰(Electro MagneticInterference,EMI)检测的要求提出了越来越高的要求。但目前通行的3/5m微波暗室检测标准面临着向上兼容的挑战。在这一背景下，基于合成孔径雷达成像原理的微波远场-近场变换及成像技术逐渐引起人们的重视。另一方面，由于机器人技术的成熟和其在电磁检测中的积极作用，各机构积极将机器人等自动化检测技术应用于电磁检测任务中。虽然机器人拥有更高的定位精度和运行效率，但是其与多样化的电磁兼容检测技术之间仍存在兼容问题，其控制策略需要考虑来自自身与或外在的多重约束条件。以微波远场-近场成像扫描为例，该检测技术要求多关节机械臂抓持检测天线或探头以特定角度指向微波辐射源，这就要求机械臂末端关节在整个扫描过程中保持恒定的指向(即末端关节轴向)。由于机械臂轴关节本身存在转角限制且需要强制闭锁末端关节轴向，使得原本完整的机械臂灵巧工作空间中会出现不可达区域，这与微波成像扫描所要求的连续大尺度扫描面存在矛盾。基于以上问题与需求，需要在对于特定多维约束下的机器人的运动规划与控制策略进行研究，进而在约束范围内做到对微波成像扫描的最优实现。

发明内容

本发明的目的在于将多关节机械臂与微波远场-近场变换及成像(EmissionSource Microscopy，ESM)技术相结合，在给定探测天线特性和机械臂构型参数的条件下快速生成对应的最大扫描面及机械臂运动路径，实现对首测微波源的高精度快速扫描与进场成像。

本发明首先针对微波成像扫描任务特点对机械臂结构、探测天线极化特征与空间采样面等约束条件进行建模，通过采样点的非线性规划获得微波源近场成像采样柱面的最大面积。在给定扫描柱面的前提下，本文基于路径及机械臂前五轴转角最小原则，通过基于关节搜索策略改进的RRT*算法在哈密顿通路前后生成平滑的关节角逆解序列，进而求解机械臂关节位状态的最佳运动规划与控制策略。通过本发明所提出的计算方法，可以在给定机械臂与检测天线类型的约束条件下快速求解该约束空间内的最大扫描柱面与最优运动控制策略，从而为机械臂与微波成像扫描任务的结合提供理论依据和技术保障。

为了实现上述目的，本发明涉及的一种用于微波远场-近场扫描与成像任务的机械臂控制策略与优化方法，具体包括以下步骤：

(1)根据6关节机械臂关节限位角约束和末端执行器天线极化方向平面采样面的限制，得到机械臂的实际不可达工作区；

末端执行器抓持天线中点的不可达区域

对应的笛卡尔封闭边界参数方程如以下公式所示：

其中，

为不可达区域

边界上的任一点(x，y，z)，R_i、R_o表示

的内径与外径，

表示参数方程的参数角；

(2)随后依据空间采样面上的末端采样点与不可达区域不存在交集，得到微波源近场成像最大采样面；

为采样面C上的一取样点，

为末端天线在点

处的正交约束矢量，由于

不同高度有相同的约束效应，将

向x₀oy₀平面做投影得到s_x，y，采样面C上所有的s_x，y构成集合S，基于采样点集合S来寻找满足采样点单映射的动态约束区域Ω₆，Ω₆优先考虑单映射f关系下生成扫描柱面，即

通过设置采样柱面的半径R、高度H以及偏移量D_offset来生成不同的点集S并以采样面的实际采样面积A最大为目标函数进行非线性规划，最大化地生成扫描柱面，其采样面积与约束条件如以下公式所示：

max A＝πRH

其中，

为|q₃|＝q_lim时轴关节4末端坐标，

表示一般情况下即|q₃|≠q_lim时

的实际空间，

表示|q₃|取q_lim时的

的实际空间，R_reach为

的封闭半径，z为坐标(x，y，z)中的z，D_offset为采样面C与Ω₆间最短欧式距离，

为空集；

(3)最后基于笛卡尔坐标系下的路径最短原则以及关节空间下前五轴关节变化幅度最下原则，得到6关节机械臂关节位移的最佳路径规划；

选择Christofides算法与目标损失函数，最终得到最短采样序列Q迹路径及其距离，

目标损失函数：min{∑w(i，j)|v_j∈V-{v_i}}

其中，w(i，j)为端点v_i与后一端点v_j构成边的权重，V为采样点之间的连线，{v_i}为代价函数；

基座坐标系下不考虑正交约束的末端执行器中位点位姿矩阵⁰T₆具体变换为：

⁰T₆＝⁰T1 ¹T₂ ²T₃ ³T₄ ⁴T₅ ⁵T₆R_x(α)R_y(β)

其中，^i-1T_i代表坐标系x_i-1oy_i-1变换至坐标系x_ioy_i的齐次变换矩阵，旋转角α＝-π/2，

其中

代表采样面C底面的圆心坐标，(p_x，p_y，p_z)为抓持器坐标，R_x(α)，R_y(β)为旋转矩阵；

将最短采样序列Q和⁰T₆输入RRT*算法，得到一条序列上相邻关节角变化幅度最小的路径。

与现有技术相比，本发明具有以下有益效果：通过本发明所使用的机械臂控制策略与优化方法对电子系统电磁兼容及电磁干扰的检测技术，可以高效的完成基于柱面扫描的微波远场-近场变换及成像(Emission Source Measurement，ESM)任务。相较于传统3/5m微波暗室检测法本发明所采用的方法实现了对受测设备表面EMI辐射强度分布的反演、成像，这可以直接指导开发企业或研发者对设备EMI性能开展优化与整改。除此之外，通过本发明提供的计算方法，可以快速求解约束空间内的最大扫描柱面与最优运动控制策略，解决机械臂灵巧工作空间会出现不可达区域的问题，进而在约束范围内做到对微波成像扫描的最优实现。

附图说明

图1为6自由度机器臂结构图。

图2为6自由度机器臂结构图(标注连杆转角)。

图3为6关节机械臂空间柱面采样示意图。

图4为未考虑末端执行器约束的连杆a₂末端的不可达区域示意图。

图5为6关节机械臂末端执行器约束下的不可达区域示意图。

图6为x₀oy₀视图下

处

约束对可达区域的影响图。

具体实施例

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例

本实施例以6关节机械臂作为对象，只考虑正交/竖直两个天线极化方向，重点研究其在柱面扫描微波源近场成像任务中的控制策略。为了得到机械臂可达工作区域内采样面的最大采样面积和机械臂末端的路径，本实施例用于微波远场-近场扫描与成像任务的机械臂控制策略与优化方法，包括以下步骤：

(1)根据6关节机械臂关节限位角约束和末端执行器天线极化方向平面采样面的限制，得到机械臂的实际不可达工作区；

(2)随后依据空间采样面上的末端采样点与不可达区域不存在交集，得到微波源近场成像最大采样面；

(3)最后基于笛卡尔坐标系下的路径最短原则以及关节空间下前五轴关节变化幅度最下原则，得到6关节机械臂关节位移的最佳路径规划。

(1)微波成像检测任务及约束条件建模

本实施例选择多自由度的冗余刚性机械臂对空间采样面上的电磁场的复振幅与相位分布信息进行采集。本技术实验对象为6自由度机械臂，其包括轴关节1、轴关节2、轴关节3、轴关节4、轴关节5和轴关节6，机械结构如图1和2所示。其中z_i指示关节轴线方向，基坐标系x_iy_iz_i满足右手螺旋法则，连杆转角q_i表示绕z_i轴旋转角度，连杆距离d_i表示沿z_i轴平移距离，a_i表示沿x_i轴平移距离，连杆扭角α_i表示z_i绕x_i+1轴旋转角度，θ_i表示各个关节可以自由旋转的角度，1≤i≤6。在机械臂实际运行时q₂、q₃∈[-q_lim，q_lim]作为物理约束条件，取q_lim＝3/4π。

本发明假设忽略各关节包络长度。之后取满足在空间采样面C上机械臂存在运动学逆解的点

将该点集与不可达区域

的补集取交集得到D，即满足任意

随后在动态的正交约束条件下，通过不同采样柱面的半径R、高度H生成的不同采样点

以及不同偏移距离D_offset实现非线性规划，最大化地生成扫描柱面(如图3所示)。最后通过RRT^*算法，将采样点从笛卡尔空间转换至关节空间，并在关节空间中探索出一条关节角变化幅度最小的路径。

(2)末端执行器限制下的不可达工作区分析

在实际应用中机械臂轴关节存在转角限制，本发明在设计微波远场-近场变换扫描系统时将机械臂关节限位约束与末端天线极化方向约束结合并求解机械臂的实际不可达工作区。本发明采取分步骤施加末端姿态约束，并对施加约束前后的机械臂可达性进行分析。

首先不考虑末端姿态即轴关节5和6，同时由于轴关节1有各向同性，所以只需任取q₁并对特例下的二维不可达区域进行旋转即可得到对应笛卡尔坐标系下的不可达区域，其对应关系如图4所示。

对于机械臂基座坐标系x₀y₀z₀下的任意坐标点p：(x，y，z)，

γ∈[-q_lim，q_lim]，若机械臂轴关节4末端在p处不存在运动学逆解，则称该点p为轴关节4末端一不可达点

而点集

则称为轴关节4末端的不可达区域

遍历γ在[-q_lim，q_lim]内取到的所有不可达区域称为集合

至此，即可得到轴关节4末端在限位角下的不可达区域

γ∈[-q_lim，q_lim]，即γ取限位角约束条件下所有角度得到的不可达区域集合的交集。对

围绕轴关节1做定轴旋转，即可得到不可达区域

的封闭边界方程(公式1)。其中R_untouch为不可达球面的半径＝d₂sin(π/4)。

即不可达区域

的边界为圆心为(0，0，a₁)，半径为R_untouch的球面。

可达区域就是不可达区域的补集，根据公式(1)可得到不考虑末端姿态下的可达区域Ω₄。

随后施加末端姿态即轴关节5-6对Ω₄的影响。遍历正交约束的所有情况，最终得到末端执行器抓持天线中点的不可达区域

(如图5所示)。对应的笛卡尔封闭边界参数方程如公式(2)所示。

其中，

为不可达区域

边界上的任一点(x，y，z)，R_i、R_o表示

的内径与外径，

表示参数方程的参数角。

(3)六自由度机械臂下微波源近场成像最大扫描面的生成方法

前文已经对6关节机械臂在末端执行器约束下的不可达区域

进行了分析，接下来进一步研究微波源近场成像最大采样柱面参数的选取。假设基座坐标系下有采样面C满足如公式(3)条件。

其中，D_offset为采样面C与Ω₆间最短欧式距离，

为空集。

为采样面C上的一取样点，

为末端天线在点

处的正交约束矢量。由于

在不同的高度具有相同的约束效应，所以本发明只需要研究x₀oz₀视图投影下点

处天线约束矢量

对采样面选取的影响即可。具体如图6所示。

由于

不同高度有相同的约束效应，本发明将

向x₀oy₀平面做投影得到s_x，y，采样面C上所有的s_x，y构成集合S。在点s_x，y处的

偏移约束下Ω₄有且仅对应单一轴关节6末端可达区域

称有单映射

若仅在单一静态约束区域Ω₆内

最大化生成扫描柱面，显然受限于Ω₆每个s_x，y对应的

都没有被充分利用，生成的扫描柱面并非极大值。

为了解决此问题，本发明提出基于采样点集合S来寻找满足采样点单映射的动态约束区域Ω₆。Ω₆优先考虑单映射f关系下生成扫描柱面，即

且优先满足Ω₆＝f(S)，其中

具体可达区域见公式(4)和(5)。

其中，

为|q₃|＝q_lim时轴关节4末端坐标，

表示一般情况下即|q₃|≠q_lim时

的实际空间，

表示|q₃|取q_lim时的

的实际空间，R_reach为

的封闭半径，z为坐标(x,y,z)中的z。

通过设置采样柱面的半径R、高度H以及偏移量D_offset来生成不同的点集S并以采样面的实际采样面积A最大为目标函数进行非线性规划，最大化地生成扫描柱面,其采样面积与约束条件如公式(6)所示。

max A＝πRH

(4)末端姿态约束下的搜索策略与路径规划

在以上得到的微波源近场成像扫描面参数的基础上，本发明在此处对6自由度机械臂路径规划进行设计，在这一过程中，本发明要在保证反演效果的基础上，减小采样序列的路径长度且保证序列相邻关节角变化平稳的路径。

首先本发明将采样柱面栅格化，将采样点与相邻采样点的空间关系构建无向图结构G(V，W)，其中采样点之间的连线构成V、连线的欧式距离构成权重W。为了找到不重复经过所有采样点的最短路径，本发明选择Christofides算法与目标损失函数公式(7)，最终得到最短采样序列Q迹路径及其距离。

min{∑w(i，j)|v_j∈V-{v_i}} (7)

其中，w(i，j)为端点v_i与后一端点v_j构成边的权重，V为采样点之间的连线，{v_i}为代价函数。

假设基座坐标系下不考虑正交约束的末端执行器中位点位姿矩阵为⁰T₆，在求解机械臂运动学逆解时需要再对轴关节4-6进行旋转变换后才能满足正交约束条件。⁰T₆具体变换见公式(8)和(9)。

⁰T₆＝⁰T₁ ¹T₂ ²T₃ ³T₄ ⁴T₅ ⁵T₆R_x(α)R_y(β) (8)

其中，^i-1T_i代表坐标系x_i-1oy_i-1变换至坐标系x_ioy_i的齐次变换矩阵，旋转角α＝-π/2，

其中

代表采样面C底面的圆心坐标，(p_x，p_y，p_z)为抓持器坐标，R_x(α)，R_y(β)为旋转矩阵。

本发明通过一种基于关节搜索策略的RRT*算法，将采样点

通过运动学逆解从笛卡尔空间转换至关节角空间，并在其中搜索出一条序列上相邻关节角变化幅度最小的路径。考虑到六自由度机械臂第六关节对其整体运动影响不大，因此本发明优先规划并找到一条1-5关节角总体变化幅度最小的路径，算法流程图见下表所示。

算法的具体执行过程为：

在选定初始关节角与目标关节角后，

首先，在关节空间中生成随机关节角q_rand，并判断q_rand是否满足边界约束和关节角限位约束。

其次，再搜索随机树上关节角1-5加权变化绝对值之和最小的一个关节角集合q_nearest，在

的矢量方向生成一个关节角集合q_new，再重新依次按照关节角搜索的策略，进行树的更新。

之后，由于在树上的子采样点是通过关节角加权变化绝对值来进行路径选择的，所以对于重新选择父节点与重布线的步骤，对于重新选择父节点的半径r_range，我们需要令其在一个适合的范围内，才能够既保证树正常按照目标的关节角集合生长，同时又能够确保其关节角的总变化量尽可能小。运行全部的迭代的次数，直到其到达目标关节角集合qpath_(x，y，z)。

Claims (1)

1.一种用于微波远场-近场扫描与成像任务的机械臂控制策略与优化方法，其特征在于，包括以下步骤：

(1)根据6关节机械臂关节限位角约束和末端执行器天线极化方向平面采样面的限制，得到机械臂的实际不可达工作区；

末端执行器抓持天线中点的不可达区域

对应的笛卡尔封闭边界参数方程如以下公式所示：

其中，

为不可达区域

边界上的任一点(x，y，z)，R_i、R_o表示

的内径与外径，

θ∈[-π，π]表示参数方程的参数角；

(2)随后依据空间采样面上的末端采样点与不可达区域不存在交集，得到微波源近场成像最大采样面；

为采样面C上的一取样点，

为末端天线在点

处的正交约束矢量，由于

不同高度有相同的约束效应，将

向x₀oy₀平面做投影得到s_x，y，采样面C上所有的s_x，y构成集合S，基于采样点集合S来寻找满足采样点单映射的动态约束区域Ω₆，Ω₆优先考虑单映射f关系下生成扫描柱面，即

通过设置采样柱面的半径R、高度H以及偏移量D_offset来生成不同的点集S并以采样面的实际采样面积A最大为目标函数进行非线性规划，最大化地生成扫描柱面，其采样面积与约束条件如以下公式所示：

max A＝πRH

其中，

为|q₃|＝q_lim时轴关节4末端坐标，

表示一般情况下即|q₃|≠q_lim时

的实际空间，

表示|q₃|取q_lim时的

的实际空间，R_reach为

的封闭半径，z为坐标(x，y，z)中的z，D_offset为采样面C与Ω₆间最短欧式距离，

为空集；

(3)最后基于笛卡尔坐标系下的路径最短原则以及关节空间下前五轴关节变化幅度最下原则，得到6关节机械臂关节位移的最佳路径规划；

选择Christofides算法与目标损失函数，最终得到最短采样序列Q迹路径及其距离，

目标损失函数：min{∑w(i，j)|v_j∈V-{v_i}}

其中，w(i，j)为端点v_i与后一端点v_j构成边的权重，V为采样点之间的连线，{v_i}为代价函数；基座坐标系下不考虑正交约束的末端执行器中位点位姿矩阵⁰T₆具体变换为：

⁰T₆＝⁰T₁ ¹T₂ ²T₃ ³T₄ ⁴T₅ ⁵T₆R_x(α)R_y(β)

其中，^i-1T_i代表坐标系x_i-1oy_i-1变换至坐标系x_ioy_i的齐次变换矩阵，旋转角

其中

代表采样面C底面的圆心坐标，(p_x，p_y，p_z)为抓持器坐标，R_x(α)，R_y(β)为旋转矩阵；

将最短采样序列Q和⁰T₆输入RRT*算法，得到一条序列上相邻关节角变化幅度最小的路径。

Images