空中机器人的6d交互控制方法
技术领域
1.本发明属于空中机器人领域，具体涉及一种空中机器人的6d交互控制方法。
背景技术：
2.为了进行物理交互，空中机器人配备了刚性工具或n自由度(dof)机械臂。在第一种情况下，该工具被牢固地固定在机身上。该解决方案得缺点是：典型的空中平台驱动不足，因此不可能独立控制末端执行器的6d(位置加方向)动力学。实际上，这已经表明，在存在与飞机质心(com)不同的机身点的交互作用下，不能保证欠驱动多旋翼的内部动力稳定，并且通常来说，既不稳定又不实用用于实际应用。
3.第二种可能性是将n-dof机械臂连接到空中平台，旨在通过利用手臂提供的致动器数量的增加来克服末端执行器动力学的欠驱动。这样一来，在末端执行器侧进行有效的6d力控制成为可能。但是，该解决方案也具有以下缺点：i)机械臂由于自身的重量而极大地减少了有效载荷和运输时间；ii)从机械的角度来看，该系统比带有刚性工具的单个机身要复杂得多，因此，其建造成本更高，并且在其整个使用寿命期间还需要更多的维护和维修成本；iii)不能由飞机平台本身提供的横向力，而是通过手臂与空中机器人之间的动态/惯性耦合产生的横向力：必须正确掌握动态耦合，才能从中获得所需的收益—6d力控制方面。反过来，这需要了解精确的动力学模型以及对系统输入和状态(位置，方向，线性和角速度)的非常精确的测量。实际上，在现实条件下(尤其是前者)很难达到这些要求。由于这个原因，对于现实世界的验证，仅采用运动学方法是优选的，但以失去引入机械手的主要优点为代价。
技术实现要素：
4.本发明的目的在于克服现有技术的不足，提供一种空中机器人的6d交互控制方法。
5.为实现上述目的，本发明采用的技术方案是：一种空中机器人的6d交互控制方法，通过利用倾斜式螺旋桨驱动，使空中机器人控制完整的6d姿态并施加完整的扭力，并附有牢固的末端执行器；通过导纳控制实现相互作用，其中外环控制所需的导纳行为，即相互作用顺应性刚度、阻尼和质量，而基于逆动力学的内环则确保完整的6d姿态跟踪，其中相互作用力由imu增强的基于动量的观测器估算。
6.进一步地，空中机器人采用具有非共线固定倾斜式螺旋桨ncftp的多转子机器人，从而在不受约束的情况下独立控制平移和角加速度，或者在接触扳手时独立控制施加的扳手，从而实现6d力控制。
7.进一步地，交互扳手的测量采用力/扭矩传感器，其中将空中机械手腕上传感器的扳手测量结果送入导纳过滤器。
8.进一步地，所述ncftp六旋翼通用模型的建模方法为：
9.建立以下坐标系：
10.惯性世界框架其轴(单位矢量)和原点分别用{xw，yw，zw}和ow表示；
11.车架已安装在机器人上，起点为or，末端为执行器的飞行器com；
12.末端执行器框架已附加到机器人末端执行器上，并且在交互点oe处具有起点；
13.在和中oe的位置分别由pe∈r3和表示，其中是恒定时间；用rr∈so(3)和re∈so(3)表示旋转矩阵，分别表示帧的方向，其中so(3)＝{a∈r3×3|aa
t
＝i∧deta＝1}；相对于惯性系为此外，表示恒定旋转矩阵，表示相对于的方向；最终使三角肌和和和相对于的角速度分别用帧内和表示；
[0014][0015][0016]
其中，[
·
]
x
∈so(3)表示与向量
·
∈r3相关的倾斜对称矩阵；使用newton-euler形式主义，空中机器人的运动方程表示为：
[0017][0018]
其中，m和j∈r3×3表示机器人质量及其惯性矩阵，分别对应于or并以表示，g为重力加速度，f∈r3和τr∈r3为力和转矩输入，而和为施加于机器人com上的外力和转矩末端执行器上的环境；
[0019]
在式(3)中，平移动力学表示在框架中，而旋转动力学表示在框架中；
[0020]
倾斜十六进制在不损失任何全驱动高空平台的通用性的情况下，导出式(3)用于ncftp平台；ncftp平台基于六旋翼结构，螺旋桨的等距间距与的x-y平面中的or等距；通过刚性适配器实现完全驱动，使每个单独的电动机-螺旋桨组合倾斜；考虑6个框架，其中o
pi
与第i个螺旋桨组的旋转中心重合；相对于的方向由旋转矩阵表示：
[0021][0022]
第i个电动螺旋桨组相对于的倾斜度由常数参数α和β定义；在式(4)中为每个其他螺旋桨选择一个交替的α符号使飞行器完全动作；
[0023]
第i个电动螺旋桨组相对于or的位置定义为：
[0024][0025]
其中从旋转旋转中心到电动机螺旋桨组o
pi
的中心向量，并从or到倾斜旋转中心的
中心列出向量；
[0026]
旋转的螺旋桨在o
pi
中产生推力和阻力；通过利用纺丝速度的正负平方来模拟两者：
[0027][0028][0029]
其中，cf和c
τ
为常数参数，将螺旋桨旋转速度与产生的推力和阻力力矩联系起来；式(7)中的(-1)
i-1
表示对所有偶数螺旋桨的反向旋转转子的作用；
[0030]
将施加在上or的总力表示为：
[0031][0032]
f1∈r3×6包含空中机器人的几何和物理特性，即尺寸、倾斜角度、推力系数，而u＝[u1...u6]
t
是收集6个平方螺旋桨旋转速度的矢量；
[0033]
在相同的条件下，将总扭矩合并进去，使用式(6)和式(7)表示fb中的推力贡献和阻力力矩：
[0034][0035]
其中f2∈r3×6再次包括几何和陆地物理性质；通过替换式(3)中的(8)和(9)，获得：
[0036][0037]
以此作为在慢速飞行条件下的可靠动力学模型。
[0038]
进一步地，所述空中飞行器的控制框架基于一个外环导纳控制和一个内环全姿态控制器；空中机器人的状态由unscented kalman滤波器ukf估算，该滤波器将惯性测量单元imu的加速度和角速度测量值与姿态传感器的位置和方向融合在一起。
[0039]
进一步地，内环姿态控制法则旨在找到六个所需的螺旋桨旋转速度u∈r6，使pr和rr跟踪任意全姿态参考轨迹(pr，r(t)，rr，r(t))：r
→
r3∈so(3)；
[0040]
通过忽略作用在空中机器人的结构或末端执行器上的外力和扭矩，将动态模型重写为：
[0041][0042]
其中g(ωr)∈r6是误差向量，结合重力效应和科里奥利效应，将jr称为内环系统*的机器人输入雅可比行列；如果输入的jr是可逆的，选择：
[0043][0044]
作为控制输入，v作为虚拟的附加输入，从而获得：
[0045]
[0046]
针对位置误差p
r，r-pr＝e
p
到0的指数收敛，自然的选择是线性控制器：
[0047][0048]
其中，对角正定义增益矩阵k
p1
，k
p2
定义了hurwitz多项式；
[0049]
在so(3)上开发方向控制器；
[0050]
假设rr，和其中[
·
]v表示从so(3)到r3的逆映射；姿态跟踪误差er∈r3由下式给出：
[0051][0052]
角速度e
ω
∈r3的跟踪误差由下式给出：
[0053][0054]
为了使旋转误差er渐近收敛到0，选择以下方向控制器：
[0055][0056]
其中，对角正定义增益矩阵k
r1
，k
r2
再次定义了hurwitz多项式。
[0057]
与现有技术相比，本发明具有以下有益效果：提供了一种空中机器人的6d交互控制方法，该方法提高了空中机器人的可靠性并降低空中机器人的重量和实现成本，在功能性、可靠性、复杂性和成本等方面都优于现有技术采用的航空操纵和物理相互作用解决方案。
附图说明
[0058]
图1为本发明实施例中带有倾斜式螺旋桨的ncftp空中平台(内部固定有末端执行器)。
[0059]
图2为本发明实施例中带有刚性连接的末端执行器的矢量示意图。
[0060]
图3为本发明实施例中内环低位姿势控制器的框图。
[0061]
图4为本发明实施例中控制框架的信号框图。
[0062]
图5为本发明实施例中空中机器人的末端执行器。
[0063]
图6为本发明实施例中以恒定的力在坚硬的倾斜表面上滑动的示意图。
[0064]
图7为本发明实施例中用工具提示将孔任务固定在漏斗中的示意图。
[0065]
图8为本发明实施例中滑动多个触点平台的示意图。
[0066]
图9为本发明实施例中突出显示“滑动”任务的快照示意图。
具体实施方式
[0067]
下面结合附图及实施例对本发明做进一步说明。
[0068]
应该指出，以下详细说明都是示例性的，旨在对本技术提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本技术所属技术领域的普通技术人员通常理解的相同含义。
[0069]
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本技术的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式
也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
[0070]
本实施例提供了一种空中机器人的6d交互控制方法，该方法通过利用倾斜式螺旋桨驱动，使空中机器人控制完整的6d姿态(独立于位置和方向)并施加完整的扭力(独立于力和扭矩)，并附有牢固的末端执行器；通过导纳控制实现相互作用，其中外环控制所需的导纳行为，即相互作用顺应性刚度、阻尼和质量，而基于逆动力学的内环则确保完整的6d姿态跟踪，其中相互作用力由imu增强的基于动量的观测器估算。
[0071]
其中，空中机器人采用具有非共线固定倾斜式螺旋桨ncftp的多转子机器人，从而在不受约束的情况下独立控制平移和角加速度，或者在接触扳手时独立控制施加的扳手，从而实现6d力控制。
[0072]
在本实施例中，交互扳手的测量采用力/扭矩传感器，其中将空中机械手腕上传感器的扳手测量结果送入导纳过滤器。
[0073]
将配备了任意安装的末端执行器工具的全驱动飞行器视为空中机器人。所呈现的物理交互框架对于任何能够通过末端执行器跟踪全姿势轨迹的全自动系统都是通用的。下面将从建模的通用部分开始，对ncftp六旋翼通用模型的通用模型作进一步说明。
[0074]
建模方法
[0075]
建立以下坐标系(参见图2)：
[0076]
惯性世界框架其轴(单位矢量)和原点分别用{xw，yw，zw}和ow表示。
[0077]
车架已安装在机器人上，起点为or，末端为执行器的飞行器com。
[0078]
末端执行器框架已附加到机器人末端执行器上，并且在交互点oe处具有起点。
[0079]
对于通用向量v，表示法v
*
(*＝r，e)表示向量v在帧中表示。如果上标等价线的编辑意味着矢量在惯性系中被按下。
[0080]
如图3所示，在和中oe的位置分别由pe∈r3和表示，其中是恒定时间。用rr∈so(3)和re∈so(3)表示旋转矩阵，分别表示帧的方向，其中so(3)＝{a∈r3×3|aa
t
＝iadeta＝1}。相对于惯性系为此外，表示恒定旋转矩阵，表示相对于的方向。最终使三角肌和和和相对于的角速度分别用帧内和表示。
[0081][0082][0083]
其中，[
·
]
x
∈so(3)表示与向量
·
∈r3相关的倾斜对称矩阵。使用newton-euler形式主义，空中机器人的运动方程表示为：
[0084][0085]
其中，m和j∈r3×3表示机器人质量及其惯性矩阵，分别对应于or并以表示，g为重力加速度，f∈r3和τr∈r3为力和转矩输入，而和为施加于机器人com上的外力和转矩末端执行器上的环境。
[0086]
值得注意的是，与航空机器人领域一样，在式(3)中，平移动力学表示在框架中，而旋转动力学表示在框架中。
[0087]
倾斜十六进制在不损失任何全驱动高空平台的通用性的情况下，导出式(3)用于ncftp平台。ncftp平台基于六旋翼结构，螺旋桨的等距间距与的x-y平面中的or等距。通过刚性适配器实现完全驱动，使每个单独的电动机-螺旋桨组合倾斜(参见图1和2)。考虑6个框架，其中o
pi
与第i个螺旋桨组的旋转中心重合(参见图2)。相对于的方向由旋转矩阵表示：
[0088][0089]
第i个电动螺旋桨组相对于的倾斜度由常数参数α和β定义。α和β的选择决定了最大的侧向力，代价是较高的侧向力会导致较高的内力和能量的预先浪费。在式(4)中为每个其他螺旋桨选择一个交替的α符号可以使飞行器完全动作。
[0090]
第i个电动螺旋桨组相对于or的位置定义为：
[0091][0092]
其中从旋转旋转中心到电动机螺旋桨组o
pi
的中心向量，并从or到倾斜旋转中心的中心列出向量(参见图2)。
[0093]
众所周知，旋转的螺旋桨在o
pi
中产生推力和阻力。通过利用纺丝速度的正负平方来很好地模拟两者：
[0094][0095][0096]
本发明使用的大多数符号概述。如果在所有实验中均保持不变，则还会显示一个值。
[0097]
其中，cf和c
τ
为常数参数，将螺旋桨旋转速度与产生的推力和阻力力矩联系起来。式(7)中的(-1)
i-1
表示对所有偶数螺旋桨的反向旋转转子的作用。
[0098]
将施加在上or的总力表示为：
[0099][0100]
f1∈r3×6包含空中机器人的几何和物理特性，即尺寸、倾斜角度、推力系数，而u＝[u1...u6]
t
是收集6个平方螺旋桨旋转速度的矢量。
[0101]
在相同的条件下，将总扭矩合并进去，使用式(6)和式(7)表示fb中的推力贡献和阻力力矩：
[0102][0103]
其中f2∈r3×6再次包括几何和陆地物理性质。该模型以及f1和f2的详细推导及其完全致动的必要条件可以在rajappa(2015年)和michieletto(2017)等人的文章中找到。通过替换式(3)中的(8)和(9)，获得：
[0104][0105]
以此作为在慢速飞行条件下(＜0.5m/s)的可靠动力学模型。我们忽略了任何空气动力效应，例如众所周知的一阶效应转子风阻，机身风阻和h力，因为这些效应线性地取决于车辆的速度，因此对于本文中考虑的慢速空中相互作用可以忽略不计。
[0106]
控制器
[0107]
所述空中飞行器的控制框架基于一个外环导纳控制和一个内环全姿态控制器(请参见图4)。空中机器人的状态由unscented kalman滤波器(ukf)估算，该滤波器将惯性测量单元(imu)的加速度和角速度测量值与姿态传感器的位置和方向(在我们的情况下为运动捕获系统)融合在一起，使用“透视点(pnp)”算法轻松地替换为车载摄像头。相互作用的扭矩和作用力由扳手观察者估算。
[0108]
姿态控制器
[0109]
内环姿态控制法则旨在找到六个所需的螺旋桨旋转速度u∈r6，使pr和rr跟踪任意全姿态参考轨迹(pr，r(t)，rr，r(t))：r
→
r3∈so(3)。
[0110]
通过忽略作用在空中机器人的结构或末端执行器上的外力和扭矩，将动态模型重写为：
[0111][0112]
其中g(ωr)∈r6是误差向量，结合重力效应和科里奥利效应，将jr称为内环系统*的机器人输入雅可比行列。如果输入的jr是可逆的，选择：
[0113][0114]
作为控制输入，v作为虚拟的附加输入，从而获得：
[0115][0116]
静态反馈线性化系统。图3的框图显示了控制方案的体系结构。
[0117]
针对位置误差p
r，r-pr＝e
p
到0的指数收敛，自然的选择是线性控制器：
[0118][0119]
其中，对角正定义增益矩阵k
p1
，k
p2
定义了hurwitz多项式。
[0120]
现在，我们来考虑方向跟踪。代表方向跟踪的常见选择是使用欧拉角。但是，欧拉
角容易出现奇点问题。因此，本发明直接在so(3)上开发方向控制器，从而避免局部坐标系的常见问题。
[0121]
紧接着，假设rr，和其中[
·
]v表示从so(3)到r3的逆映射。姿态跟踪误差er∈r3由下式给出：
[0122][0123]
角速度e
ω
∈r3的跟踪误差由下式给出：
[0124][0125]
为了使旋转误差er渐近收敛到0，选择以下方向控制器：
[0126][0127]
其中，对角正定义增益矩阵k
r1
，k
r2
再次定义了hurwitz多项式。
[0128]
接触扳手估算
[0129]
为了正确处理空中机器人与外部环境的物理相互作用，了解工具尖端与环境之间的接触相互作用扳手的知识是必不可少的。为此，可以在机器人的工具提示上安装一个力/转矩传感器，通常可以提供可靠的措施，但是这种解决方案既增加了机器人的成本，又增加了机器人的重量。在空中机器人领域，一种更可行的解决方案是采用扳手估算器，该估算器可以在存在以下情况的情况下提供足够准确的估算值，表示为：精确测量位置，速度以及加速度(如果有)。
[0130]
机器人的外部扳手可以看作是环境在工具尖端上施加的扳手对扳手com的影响。
[0131][0132]
实验中使用的ncftp平台的传感器设备可以对平台位置和速度(角度和线性)进行足够准确的测量，而只有imu提供的线性加速度才可以在扳手观察者中合理使用。因此，在本文中，tomic等人已经提出了混合方法，已被遵循。更详细地说，是由尤克塞尔(yuksel)等人提出的基于加速度的观测器。为了估计机器人com上的外部相互作用力fr，而外部扭矩是通过利用基于动量的观测器获得的。
[0133]
接触力的估计下面的扰动观测器需要车辆加速措施，最初是由尤克瑟勒塔尔的空中机器人提出的，被用来估计接触力
[0134][0135]
其中l∈r3×3是要设计的增益矩阵，而是估计fr。尤克瑟尔等人给出了在恒定或缓慢变化的外力存在下的误差动态。
[0136][0137]
其中o3∈r3是(3
×
1)零向量。因此，对于任何正定矩阵l，误差动力学指数均收敛于原点。
[0138]
接触扭矩估算为了估算由外部环境施加在工具提示上的相互作用扭矩，已设计了基于动量的观测器。参考系统动力学(10)，可以将帧fr中的角动量qr∈r3计算为
[0139][0140]
从(3)可以将(21)的时间导数表示为
[0141][0142]
通过利用(22)，估计可以看作是残差矢量
[0143][0144]
其中t和t0分别是当前时刻和初始时刻，k
l
是正定增益矩阵。通过合理地假设这意味着qr(t0)也为零。通过取(23)到(22)的时间导数，可以得到残差矢量的以下动力学特性
[0145][0146]
公式(24)是一阶低通动态系统：可以很容易地认识到，对于任何正定义的基本矩阵k
l
，当t
→
∞时，观察者的滤波特性：较大的增益值可以加快收敛速度，而较小的值可以滤波高频噪声。
[0147]
扳手作用于刀尖和均已知后，估算的作用于刀尖的扳手的计算公式为
[0148][0149]
扳手观察员演示了精度的一个示例。在此测试中，空中机器人已牢固地连接到ati45力-扭矩传感器，后者本身已安装在测试台上，通过让空中机器人跟踪轨迹并且仅利用内环姿态控制器，空中机器人就可以在力/力矩传感器上应用由轨迹定义的力/力矩轮廓。为了正确测试扳手观察者的极限，已将同时围绕多个轴的线性调频信号(频率逐渐增加的余弦信号)用作轨迹。
[0150]
相互作用扳手补偿
[0151]
为了获得导纳滤波器的最佳结果，需要一个高刚性的低水平跟踪。这可以通过增加(14)和(17)中的增益来实现。该解决方案的缺点是噪声也会被放大，这可能会使低级系统更趋于不稳定。此外，仍然不会实现真正的零跟踪误差。取而代之的是，为了提高收敛性，将估计扳手反馈给低层控制器，作为(12)中的附加项。因此，由于将任何稳态误差驱动为零，因此即使是无接触飞行跟踪也得到了改善(参见图4)。更详细地，最终控制输入由下式给出
[0152][0153]
如果相互作用力和扭矩都得到了完美的补偿，则误差动力学与(14)和(17)相同，
因此，保持相同的稳定性。否则，在交互作用扳手有界的微不足道的假设下，扳手估计误差也可以视为有界项。之前已经研究了所提出的控制器在没有相互作用的情况下的稳定性能。详细地，已经证明，如果对于tilthex平台可行，则在初始角速度误差e
ω
的温和条件下，跟踪误差将指数误差收敛为零。因此，通过在没有消失的扰动的情况下重现扰动系统的稳定性理论，可以指出，在有界扳手估计误差的情况下，跟踪误差最终是有界的。此外，如果交互作用扳手是恒定的，则扳手错误会收敛到零，因此跟踪错误也会消失，也将收敛到零。
[0154]
导纳过滤器
[0155]
为了获得与环境交换的有限力，可以在末端执行器的位置和方向与相互作用的广义力之间强制执行顺应性行为。根据位置p
e，d
，方向r
e，d
，速度和加速度分配末端执行器的计划理想轨迹，可通过导纳过滤器生成对应的一组参考运动变量具有以下动态特征
[0156][0157]
其中δve＝v
e，d-v
e，r
是速度误差，而ee是下式给出的姿态误差
[0158][0159]
(27)表示惯性me，阻尼de和刚度ke的6自由度机械阻抗的动力学：这些矩阵都是正定的，并以适当的方式施加了适当的选择-降低了系统的行为。此外，为了保证整个系统的稳定性，增益矩阵必须确保运动控制器(内环)具有相对于导纳滤波器更快的动态特性。一旦计算出末端执行器的参考轨迹，就应以com参考轨迹表示，以便由内环姿态控制器进行跟踪。然后计算机器人的参考位置和方向(见图2)为
[0160][0161]
而com参考速度和加速度是通过采用(29)的时间导数获得的。详细地，参考速度由下式给出：
[0162][0163]
而参考加速度是
[0164][0165]
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。