CN116237938A - 一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法 - Google Patents

Abstract

本发明提出一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法，生成满足运动约束并最小化系统质心偏移的协调运动轨迹。首先建立机械臂的运动学模型，以及飞行机械臂的一体化运动学模型，计算对应的雅各比矩阵。根据四旋翼无人机的欠驱动性，将雅各比矩阵分解为可控部分与不可控部分。随后建立系统的质心偏移模型，计算对应的雅各比矩阵。利用分层二次规划框架，以飞行机械臂末端跟踪精度为优化目标，建立并求解一个二次规划问题；在此基础上，将质心偏移量纳入优化目标函数，建立并求解新的二次规划问题，得到满足末端目标跟踪性能且最小化质心偏移的参考运动轨迹。本发明适用用于目标捕获、设备操作等空中作业任务。

Description

一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划 方法

技术领域

本发明属于飞行机器人运动规划领域，具体涉及一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法，适用于搭载机械臂进行主动操作任务的飞行机器人系统。

背景技术

近年来，一类由旋翼无人机和多自由度机械臂相结合而成的新型飞行机械臂系统受到了学术界与工业界的广泛关注。相较于广泛应用在航拍、巡检、测绘、植保等传统行业的旋翼无人机，旋翼飞行机械臂通常面向空中操作任务，旨在利用载荷实施自主操作，实现与外界环境的交互，进而实现从“察”到“察-作”一体的性能飞跃。旋翼飞行机械臂系统通常具有冗余自由度特性。旋翼无人机平台是一个六自由度刚体，当加装了n自由度机械臂之后，整个系统的自由度变成了6+n。当系统执行抓取任务时，机械臂末端只需要消耗3个自由度即可，此时会出现有无数种抓取姿态可供选择的情况。同时，机械臂的运动会造成整个系统在作业时的质心偏移，进而影响系统的控制精度以及飞行性能。此外，飞行机械臂执行作业任务时往往会受到运动约束，例如：无人机位置/速度约束、机械臂关节角度/角速度约束、末端位置/速度约束等。因此，为了提升系统的作业性能、提高作业任务的成功率，必须解决无人机平台与机械臂关节的协调运动规划问题，以期实现在保证末端位置情况下尽可能减小质心偏移，并且满足各类运动约束条件。

中国发明专利CN201810477920.0提出了一种携带冗余自由度机械臂的飞机器人系统设计方法。对于冗余度机械臂运动规划，该发明采用二次规划进行规划方案设计，但存在两个缺点：(1)未考虑无人机平台与机械臂的一体化运动规划。(2)二次规划的优化目标只选择了末端位姿，而未考虑调节质心偏移等其他优化目标。中国发明专利CN201810094313.6提出了一种基于动态重心补偿的旋翼飞行机械臂系统及算法，通过建立质心偏移计算模型，在控制系统中将质心偏移量进行提前补偿。但该发明未考虑在运动规划层尽可能减小质心偏移，保守性较强。

发明内容

针对现有技术发明中存在的问题，本发明提出一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法，即考虑了无人机平台与机械臂的轨迹协调，同时也从规划层面满足各类任务的约束条件、最大程度地减小系统的质心偏移。

为达到上述目的，本发明采用以下的技术方案：

一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法，包括以下步骤：

第一步、根据旋量理论与指数积公式，建立机械臂的运动学模型；

第二步、建立飞行机械臂的一体化运动学模型，计算相应的雅各比矩阵，将运动学模型分解为可控部分与不可控部分；

第三步、建立飞行机械臂的水平方向质心偏移模型，推导与质心偏移变化率对应的雅各比矩阵；

第四步、将机械臂末端轨迹跟踪精度与质心偏移量同时作为优化目标，建立并求解分层二次规划问题模型，得到满足末端目标跟踪性能且最小化质心偏移的飞行机械臂运动轨迹。

进一步地，所述第一步包括：

定义当机械臂位于初始位姿时，机械臂第i关节的运动旋量ξ_i：

其中，ω_i表示在机械臂基坐标系下关节i的单位角速度矢量，v_i表示在机械臂基坐标系下关节i的线速度矢量，r_i表示机械臂基坐标系下关节i转轴上任意一点的坐标，所述初始位姿为各个关节角为0的位姿；

将运动旋量ξ_i转化为矩阵形式

其中，

S(·)为反对称矩阵运算符；

根据指数积公式，计算得到机械臂的前向运动学齐次变换矩阵

其中，

表示机械臂末端相对于机械臂基坐标系的旋转矩阵，/>

表示机械臂末端在机械臂基坐标系下的位置，n是机械臂关节数目，q_i是关节i的关节角度，/>

是关节i的指数映射，T₀是初始位姿下的齐次变换矩阵，且：

计算机械臂的雅各比矩阵：

其中，

表示关节i在任意位姿下的运动旋量。/>

可由ξ_i经过伴随变换运算后得到：

进一步地，所述第二步包括：

依据齐次变换原理建立飞行机械臂的一体化前向运动学模型：

其中，P_e表示机械臂执行器在惯性坐标系下的位置，R_e是机械臂末端在惯性坐标下的旋转矩阵，P_b表示无人机机体坐标系在惯性坐标系下的位置，R_b是无人机机体坐标系相对于惯性坐标系下的旋转矩阵。

对上式进行微分，得到飞行机械臂的微分运动学方程：

其中，Ω_b和Ω_e分别代表无人机质心和飞行机械臂末端在惯性坐标系下的角速度；

是飞行机械臂末端在机体标系下的角速度；/>

是机械臂的关节角速度向量；

分别是P_e，P_b，R_b，/>

对时间的导数。

得到飞行机械臂末端在惯性坐标系下的表示：

其中，

代表飞行机械臂末端坐标系的线速度和角速度，

代表无人机机体坐标系的线速度和角速度。

定义机械臂末端和无人机的角速度与欧拉角速度之间的转换矩阵分别为T_e与T_b，得到飞行机械臂末端位姿在惯性坐标系的表达式为：

其中，x_e＝[P_e Θ_e]^T，x_b＝[P_b Θ_b]^T；Θ_e＝[φ_e θ_e ψ_e]^T，Θ_b＝[φ_b θ_b ψ_b]^T分别是机械臂末端与无人机的欧拉角；φ_e，φ_b分别为飞行机械臂末端和无人机的滚转角；θ_e，θ_e分别为飞行机械臂末端和无人机的俯仰角；ψ_e，ψ_b分别为飞行机械臂末端和无人机的偏航角。

由于四旋翼无人机是一个欠驱动系统，其三维位置和偏航角通道可控，而俯仰角和滚转角通道不可控，故将微分运动学方程重新排列，得到：

其中，ζ_c＝[P_b ψ_b q]^T，ζ_uc＝[φ_b θ_b]^T。J_c与J_uc是重排列后得到的可控部分雅各比矩阵与不可控部分雅各比矩阵。

进一步地，所述第三步包括：

计算机械臂质心矢量在机体坐标系下的坐标：

其中，m_i，

分别代表机械臂连杆i的质量与其质心位置在机体系下的坐标；

计算机械臂质心在惯性坐标系下的坐标，并将其投影到水平方向：

将上式对机械臂关节角向量q求微分，得到

/>

计算出

的2-范数与ζ_c之间的微分映射关系与对应的雅各比矩阵：

其中，

是/>

的2-范数对时间t的导数，将其定义为质心偏移变化率。

进一步地，所述第四步包括：

将运动学方程离散化，k时刻的运动学方程为：

求出飞行机械臂末端的期望速度：

其中，x_t，k和

表示目标在离散时刻k的位置与速度，/>

表示离散时刻k的末端期望速度，K_p为增益系数，/>

和/>

分别表示ζ_c和ζ_uc随时间的导数。

为了使飞行机械臂末端跟踪上目标位置，设计优化目标函数①：

展开化简后得到：

上式的最后一项

在每一个离散时刻k与可控输入/>

均无关，省略该项，得到时变凸二次规划问题模型①：

其中，

对系统各可控自由度的位置、速度，以及飞行机械臂末端的速度进行约束，作为问题模型①的约束条件：

其中，

与/>

为可控输入的速度上下界，ζ_c，max与ζ_c，min为可控输入的位置上下界，△T为离散采样时间间隔，/>

与/>

为飞行机械臂末端速度的上下界。

根据时变凸二次规划问题模型①与其对应的约束条件，求出每个时刻对应的最优解α_0，k；

在机械臂末端跟踪期望轨迹的基础上，进一步最小化机械臂的质心偏移，为此，设计优化目标函数②：

其中，

是离散时刻k的期望质心偏移变化率；

由下式计算：

K_G为增益系数，

为惯性坐标系下机械臂质心坐标在水平方向的投影。

将

展开化简后可得：

上式最后一项

在每一个离散时刻k与可控输入/>

均无关，省略该项，再次得到时变凸二次规划问题模型②：

其中，

二次规划问题模型②采用与二次规划问题①相同的约束条件；

在此基础上，将二次规划问题①的解添加到二次规划问题模型②中作为一项等式约束：

二次规划问题模型②的约束为：

根据时变凸二次规划问题模型②与其对应的约束条件，在每个时刻求出对应的最优解α_1，k；α_1，k是分层二次规划问题模型的最终解，即旋翼飞行机械臂系统各个自由度的期望速度。

本发明与现有技术相比的优点在于：

本发明中涉及的一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法，主要面向由旋翼无人机和多自由度机械臂所组成的飞行机械臂系统，能够保证无人机平台与机械臂之间的协调运动与相互配合，并完成各类空中操作任务。针对各类任务约束条件下的协调运动规划问题，本方法首先利用旋量法与指数积公式建立了机械臂的运动学模型，和传统的D-H参数法相比，需要的坐标系与参数更少，降低了计算量。然后构建飞行机械臂的一体化前向运动学方程与微分运动学方程，获得对应的雅各比矩阵，并根据四旋翼无人机的欠驱动特性，将雅各比矩阵分解为可控部分与不可控部分。紧接着建立飞行机械臂在水平方向质心偏移模型，并获得对应的雅各比矩阵。为了求解飞行机械臂系统各个自由度的期望运动轨迹，本方法首次应用分层二次规划的框架，先构建一个时变凸二次规划问题模型，求解使得飞行机械臂末端跟踪目标的运动轨迹；在此基础上再构建一个时变凸二次规划问题模型，最终求解出在满足飞行机械臂末端跟踪目标的同时最小化质心偏移的运动轨迹。

附图说明

图1为本发明提出的一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法的设计流程图；

图2为本发明的飞行机械臂系统结构图。

具体实施方式

为了使本发明的目的、技术方案及优点更为清晰，以下结合附图及实施例，对本发明进行详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，本发明提出一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法，具体实施步骤如下：

第一步，根据旋量理论与指数积公式，建立机械臂的运动学模型。

定义机械臂位于初始位姿(各个关节角为0)时关节i的运动旋量ξ_i：

其中，ω_i表示在机械臂基坐标系下关节i的单位角速度矢量，v_i表示在机械臂基坐标系下关节i的线速度矢量，r_i表示机械臂基坐标系下关节i转轴上任意一点的坐标。

将运动旋量ξ_i转化为矩阵形式：

其中，

S(·)为反对称矩阵运算符。根据指数积公式，可以计算机械臂的前向运动学齐次变换矩阵：

其中，

表示机械臂末端相对于机械臂基坐标系的旋转矩阵，/>

表示机械臂末端在机械臂基坐标系下的位置，n是机械臂关节数目，q_i是关节i的关节角度，/>

是关节i的指数映射，T₀是初始位姿下的齐次变换矩阵，且：

机械臂的雅各比矩阵可以表示为：

其中，

表示关节i在任意位姿下的运动旋量。/>

可由ξ_i经过伴随变换运算后得到：

第二步，针对图2中的飞行机械臂的设计结构，假设机械臂基坐标系与无人机机体坐标系重合，依据齐次变换原理建立飞行机械臂的一体化前向运动学模型：

式中，P_e表示机械臂执行器在惯性坐标系下的位置，R_e是机械臂末端在惯性坐标下的旋转矩阵；P_b表示无人机机体坐标系在惯性坐标系下的位置，R_b是无人机机体坐标系相对于惯性坐标系下的旋转矩阵；

对上式进行微分，可以得到飞行机械臂的微分运动学方程：

其中，Ω_b和Ω_e分别代表无人机质心和飞行机械臂末端在惯性坐标系下的角速度；

是飞行机械臂末端在机体标系下的角速度；/>

是机械臂的关节角速度向量；

分别是P_e，P_b，R_b，/>

对时间的导数。

进一步地，可以得到飞行机械臂末端在惯性坐标系下的表示：

其中，

代表飞行机械臂末端坐标系的线速度和角速度，

代表无人机机体坐标系的线速度和角速度。

定义机械臂末端和无人机的角速度与欧拉角速度之间的转换矩阵分别为T_e与T_b，可得飞行机械臂末端位姿在惯性坐标系的表达形式：

其中，x_e＝[P_e Θ_e]^T，x_b＝[P_b Θ_b]^T；Θ_e＝[φ_e θ_e ψ_e]^T，Θ_b＝[φ_b θ_b ψ_b]^T分别是机械臂末端与无人机的欧拉角；φ_e，φ_b分别为飞行机械臂末端和无人机的滚转角；θ_e，θ_e分别为飞行机械臂末端和无人机的俯仰角；ψ_e，ψ_b分别为飞行机械臂末端和无人机的偏航角。

将微分运动学方程重新排列，可得：

其中，ζ_c＝[P_b ψ_b q]^T，ζ_uc＝[φ_b θ_b]^T。J_c与J_uc是重排列后得到的可控部分雅各比矩阵与不可控部分雅各比矩阵。

第三步，建立飞行机械臂水平方向质心偏移模型。

机械臂质心矢量在机体坐标系下可表示为：

其中，m_i，

分别代表机械臂连杆i的质量与其质心位置在机体系下的坐标。计算机械臂质心在惯性坐标系下的坐标，并将其投影到水平方向：

将上式对机械臂关节角向量q求微分，可以得到

进一步计算出/>

的2-范数与ζ_c之间的微分映射关系与对应的雅各比矩阵：

其中，

是/>

的2-范数对时间t的导数，可将其定义为质心偏移变化率。

第四步，构建分层二次规划问题模型，求解系统各个自由度的期望运动轨迹。

首先将离散运动学方程离散化：

求解飞行机械臂末端的期望速度：

其中，x_t，k和

表示目标在离散时刻k的位置与速度，/>

表示离散时刻k的末端期望速度，K_p为增益系数，/>

和/>

分别表示ζ_c和ζ_uc随时间的导数。

为了使飞行机械臂末端跟踪上目标位置，设计优化目标函数①：

展开化简后可得：

上式的最后一项

在每一个离散时刻k与可控输入/>

均无关。将该项省略后可以得到时变凸二次规划问题模型①：

其中，

问题模型①的约束条件设为：/>

其中，

与/>

为可控输入的速度上下界，ζ_c，max与ζ_c，min为可控输入的位置上下界，△T为离散采样时间间隔，/>

与/>

为飞行机械臂末端速度的上下界。

根据时变凸二次规划问题模型①与其对应的约束条件，可以在每个时刻求出对应的最优解α_0，k。

在机械臂末端跟踪期望轨迹的基础上，进一步最小化机械臂的质心偏移。为此，设计优化目标函数②：

其中，

是离散时刻k的期望质心偏移变化率。在旋翼飞行机械臂执行任务的过程中，总是期望质心偏移尽可能小，因此/>

可以由下式计算：

K_G为增益系数，

为惯性坐标系下机械臂质心坐标在水平方向的投影。

将

展开化简后可得：

和优化函数

类似，上式最后一项/>

在每一个离散时刻k与可控输入

均无关。将该项省略后可以再次得到时变凸二次规划问题模型②：

其中，

二次规划问题模型②采用与二次规划问题①相同的约束条件，并在此基础上，将二次规划问题①的解添加到二次规划问题模型②中作为一项等式约束：

因此，二次规划问题模型②的约束可以表示为：

根据时变凸二次规划问题模型②与其对应的约束条件，可以在每个时刻求出对应的最优解α_1，k。α_1，k是分层二次规划问题模型的最终解，即旋翼飞行机械臂系统各个自由度的期望速度。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法，其特征在于，包括以下步骤：

第一步、根据旋量理论与指数积公式，建立机械臂的运动学模型；

第二步、建立飞行机械臂的一体化运动学模型，计算相应的雅各比矩阵，将运动学模型分解为可控部分与不可控部分；

第三步、建立飞行机械臂的水平方向质心偏移模型，推导与质心偏移变化率对应的雅各比矩阵；

第四步、将机械臂末端轨迹跟踪精度与质心偏移量同时作为优化目标，建立并求解分层二次规划问题模型，得到满足末端目标跟踪性能且最小化质心偏移的飞行机械臂运动轨迹。

2.根据权利要求1所述的一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法，其特征在于，所述第一步包括：

定义当机械臂位于初始位姿时，机械臂第i关节的运动旋量ξ_i：

其中，ω_i表示在机械臂基坐标系下关节i的单位角速度矢量,v_i表示在机械臂基坐标系下关节i的线速度矢量，r_i表示机械臂基坐标系下关节i转轴上任意一点的坐标。所述初始位姿为机械臂各个关节角为0时的位姿；

将运动旋量ξ_i转化为矩阵形式

其中，

S(·)为反对称矩阵运算符；

根据指数积公式，计算得到机械臂的前向运动学齐次变换矩阵

其中，

表示机械臂末端相对于机械臂基坐标系的旋转矩阵，/>

表示机械臂末端在机械臂基坐标系下的位置，n是机械臂关节数目，q_i是关节i的关节角度，/>

是关节i的指数映射，T₀是机械臂在初始位姿下的齐次变换矩阵，且：

计算机械臂的雅各比矩阵

其中，

表示关节i在任意位姿下的运动旋量。/>

可由ξ_i经过伴随变换运算后得到：

/>

3.根据权利要求2所述的一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法，其特征在于，所述第二步包括：

依据齐次变换原理建立飞行机械臂的一体化前向运动学模型：

其中，P_e表示机械臂执行器在惯性坐标系下的位置，R_e是机械臂末端在惯性坐标下的旋转矩阵，P_b表示无人机机体坐标系在惯性坐标系下的位置，R_b是无人机机体坐标系相对于惯性坐标系下的旋转矩阵；

将上式对时间求导，得到飞行机械臂的微分运动学方程：

其中，Ω_b和Ω_e分别代表无人机质心和飞行机械臂末端在惯性坐标系下的角速度；

是飞行机械臂末端在机体标系下的角速度；/>

是机械臂的关节角速度向量；/>

分别是P_e，P_b，R_b，/>

对时间的导数；

进一步得到飞行机械臂末端在惯性坐标系下的表示：

其中，

代表飞行机械臂末端坐标系的线速度和角速度，/>

代表无人机机体坐标系的线速度和角速度；

定义机械臂末端和无人机的角速度与欧拉角速度之间的转换矩阵分别为T_e与T_b，得到飞行机械臂末端位姿在惯性坐标系的表达式为：

其中，x_e＝[P_e Θ_e]^T,x_b＝[P_b Θ_b]^T；Θ_e＝[φ_e θ_e ψ_e]^T，Θ_b＝[φ_b θ_b ψ_b]^T分别是机械臂末端与无人机的欧拉角；θ_e，ψ_b分别为飞行机械臂末端和无人机的滚转角；θ_e，θ_e分别为飞行机械臂末端和无人机的俯仰角；ψ_e，ψ_b分别为飞行机械臂末端和无人机的偏航角；

由于四旋翼无人机是一个欠驱动系统，其三维位置和偏航角通道可控，而俯仰角和滚转角通道不可控，故将微分运动学方程重新排列，得到：

其中，ζ_c＝[P_b ψ_b q]^T，ζ_uc＝[φ_b θ_b]^T，J_c与J_uc是重排列后得到的可控部分雅各比矩阵与不可控部分雅各比矩阵。

4.根据权利要求3所述的基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法，其特征在于，所述第三步包括：

计算机械臂质心矢量在机体坐标系下的坐标

/>

其中，m_i，

分别代表机械臂连杆i的质量与其质心位置在机体系下的坐标；

计算机械臂质心在惯性坐标系下的坐标，并将其投影到水平方向：

将上式对机械臂关节角向量q求微分，得到

计算出

的2-范数与ζ_c之间的微分映射关系与对应的雅各比矩阵：

其中，

是/>

的2-范数对时间t的导数，将其定义为质心偏移变化率。

5.根据权利要求4所述的一种基于冗余自由度利用的旋翼飞行机械臂协调运动规划方法，其特征在于，所述第四步包括：

将运动学方程离散化，k时刻的运动学方程为：

求出飞行机械臂末端的期望速度：

其中，x_k,k和

表示目标在离散时刻k的位置与速度，/>

表示离散时刻k的末端期望速度，K_p为增益系数，/>

和/>

分别表示ζ_c和ζ_uc在离散时刻k随时间的导数；

为了使飞行机械臂末端跟踪上目标位置，设计优化目标函数①：

展开化简后得到：

上式的最后一项

在每一个离散时刻k与可控输入/>

均无关，省略该项，得到时变凸二次规划问题模型①：

其中，

对系统各可控自由度的位置、速度，以及飞行机械臂末端的速度进行约束，作为问题模型①的约束条件：

其中，

与/>

为可控输入的速度上下界，ζ_c,max与ζ_c,min为可控输入的位置上下界，ΔT为离散采样时间间隔，/>

与/>

为飞行机械臂末端速度的上下界；

根据时变凸二次规划问题模型①与其对应的约束条件，求出每个时刻对应的最优解α_0,k；

在机械臂末端跟踪期望轨迹的基础上，进一步最小化机械臂的质心偏移，为此，设计优化目标函数②：

其中，

是离散时刻k的期望质心偏移变化率；

由下式计算：

其中，K_G为增益系数，

为惯性坐标系下机械臂质心坐标在水平方向的投影；

将

展开化简后可得：

上式的最后一项

在每一个离散时刻k与可控输入/>

均无关，省略该项，再次得到时变凸二次规划问题模型②：

其中，

二次规划问题模型②采用与二次规划问题①相同的约束条件；

在此基础上，将二次规划问题①的解添加到二次规划问题模型②中作为一项等式约束：

二次规划问题模型②的约束为：

根据时变凸二次规划问题模型②与其对应的约束条件，在每个时刻求出对应的最优解α_1,k；α_1,k是分层二次规划问题模型的最终解，即旋翼飞行机械臂系统各个自由度的期望速度。

Images