CN116476080B - 一种基于几何可行性的空中自动抓取作业规划方法 - Google Patents

Abstract

本发明公开了一种基于几何可行性的空中自动抓取作业规划方法，属于无人机和机械臂技术领域，包括飞行平台运动规划和机械臂运动规划，所述飞行平台运动规划的输入是目标位置和障碍物位置，所述飞行平台运动规划的输出是飞行平台的轨迹；所述机械臂运动规划的输入是目标位置和飞行平台轨迹，所述机械臂运动规划的输出是机械臂的期望关节角，所述飞行平台运动规划和所述机械臂运动规划的规划是在几何可行性条件下完成的。本发明采用一种基于几何可行性约束的分离式的架构实现空中自动抓取规划。相较于传统在构型空间规划的算法，本发明实现了在笛卡尔空间内对自动空中抓取作业进行运动规划，提出的方法更适合空中作业机器人。

Description

一种基于几何可行性的空中自动抓取作业规划方法

技术领域

本发明属于无人机和机械臂技术领域，尤其是涉及一种基于几何可行性的空中自动抓取作业规划方法。

背景技术

无人机具有快速移动能力，机械臂具有高精度操纵能力，二者结合形成的空中作业机器人可以大大拓展现有的无人机和机械臂的应用范围。空中作业机器人可以移动到普通人难以到达的地方，并且完成相关的作业任务。自动抓取是空中作业机器人运动规划的一个典型的场景，相较于传统多旋翼无人机平台，空中作业机器人的优势在于其可以和环境交互。其中抓取就是和环境交互的一个重要任务，如果空中作业机器人具有了抓取的能力，那么它就可以把一些关节的物品或者传感器部署在感兴趣的位置，也可以把环境中感兴趣的样本抓取回来。目前，现有的关于空中抓取任务的运动规划的研究和成果都集中在研究无人机的抓取，而空中作业机器人的抓取研究较少。并且，关于空中作业机器人抓取的现有方法，也主要是在构型空间中去规划，由于空中作业机器人的自由度高，其构型空间的维度高，在高维度的构型空间中去进行运动规划会造成较大的计算量。因此，本申请提出一种在笛卡尔空间中进行空中自动抓取的运动规划方法，该方法采用基于几何可行性约束的分离式的架构，先规划飞行平台的轨迹，再规划末端执行器的轨迹。相较于传统的在构型空间内的算法，我们提出的方法需要处理的维度更低，只需要在笛卡尔空间中进行规划计算，更适合空中作业机器人。

随着社会对生产安全要求的提高，采用智能的机器人和设备替代人工从事高危区域的复杂任务是必然趋势。空中作业机器人具有极强的到达能力和相对高精度的作业能力，具有的极大潜力。开发空中自动抓取作业规划算法不但能让空中作业机器人完成针对关键物品的自动抓取的任务，还能拓展空中作业机器人的潜能，从而提供社会生产安全和效率。

发明内容

本发明的目的是提供一种基于几何可行性的空中自动抓取作业规划方法，解决上述技术存在的空中作业机器人在高维度构型空间中进行运动规划时计算量大的问题。

为实现上述目的，本发明提供了一种基于几何可行性的空中自动抓取作业规划方法，包括飞行平台运动规划和机械臂运动规划，所述飞行平台运动规划的输入是目标位置和障碍物位置，所述飞行平台运动规划的输出是飞行平台的轨迹；所述机械臂运动规划的输入是目标位置和飞行平台轨迹，所述机械臂运动规划的输出是机械臂的期望关节角，所述飞行平台运动规划和所述机械臂运动规划是分离式规划且在几何可行性条件下完成的。

优选的，所述飞行平台运动规划具体包括以下步骤：

S21、面向可抓取点的路径规划，让飞行平台从起始点飞到可抓取点，再从可抓取点飞到终点，可抓取点通过几何可行性约束进行求解，再通过A*算法计算由起点、可抓取点、终点组成的路径；

S22、移动和抓取阶段安全通道生成，生成一个安全的飞行通道，将任务分成两部分：移动阶段和抓取阶段；

S23、通过分段贝塞尔曲线计算出飞行平台的轨迹。

优选的，所述步骤S22中的移动阶段的安全通道通过凸分解方法求得，抓取阶段的安全通道由一个预先设计的凸多面体表示，该凸多面体通过几何可行性约束求得，凸多面体具体表示为

；

其中，为/>是目标抓取方位角，且

。

优选的，所述机械臂运动规划具体包括以下步骤：

S41、基于微分平坦特性的初始状态计算，其目的是计算末端执行器在抓取阶段的初始位置和初始速度；

S42、基于几何可行性和空中抓取条件的末端执行器轨迹优化方法，获取经过初始点和目标位置的轨迹，采用迭代求解的思路求解末端执行器轨迹，并且在每一次迭代都求解一个优化问题，直到获得不产生碰撞的末端轨迹；

S43、基于小孔映射方法的空中抓取机械臂避障计算，在每一次迭代中，利用小孔映射方法和Gilbert–Johnson–Keerthi（GJK）算法更新优化问题的目标函数，并通过迭代将机械臂的轨迹逐渐远离障碍物，实现机械臂的避障；

S44、协调规划，输入期望的末端执行器位置和期望的飞行平台位置，协调并跟踪飞行平台和末端执行器的轨迹，并通过机械臂的逆运动学完成机械臂驱动关节的求解。

优选的，所述步骤S42中优化问题由目标函数和约束两部分组成，具体求解方法包括以下步骤：

S421、用贝塞尔曲线来表示末端轨迹，在每个一个迭代步长都求解一个QP优化问题，令轨迹为，将其用/>阶贝塞尔曲线表示：

；

其中，为贝塞尔曲线的参数向量；/>为贝塞尔曲线的基底向量，且有/>；

S422、将轨迹求解转化为贝塞尔曲线的参数求解，并将该参数求解问题转化为一个QP问题，定义为轨迹参数向量，QP问题写为

；

其中，是Hessian矩阵并且其是半正定的；/>是一个向量；/>和/>为约束矩阵；/>和为约束向量，约束分为等式约束和不等式约束，QP问题分为目标函数和约束；

S423、对上述QP问题的目标函数进行求解，目标函数定义为，其中/>是代价函数以降低整条轨迹上的加速度，/>是用于避障的惩罚函数，这两项具体为

；

；

其中，为轨迹的加加速度；/>是变化的权重参数；/>是障碍物镜像位置坐标；

S424、获取QP问题的约束；

S425、QP问题求解，在每一次迭代中，都需要求解该优化问题，得到结果后再进行避障计算，根据避障计算结果再更新计算权重参数和障碍物镜像，得到新的QP问题的目标函数，从而进行下一步的迭代计算。直到获得不发生碰撞的轨迹。

优选的，所述步骤S424中QP问题的约束具体如下：

①路径点约束，路径点约束主要是使生成的轨迹通过和/>，路径点约束为

；

其中，表示的/>的第i个控制点；/>,/>；

②动力学可行性约束，为约束速度和加速度，动力学可行性约束为

；

其中，；下标/>表示相应变量的最小值；下标/> 表示相应变量的最大值；

③几何可行性约束，确保飞行平台和末端执行器的轨迹是几何可行的，几何可行性可约束述为

；

其中，为末端执行器轨迹；/>为飞行平台轨迹；/>分别为几何可行性的下界和上界，根据贝塞尔曲线的凸包特性，几何可行性约束描述为

；

其中，，/>为/>的元素；/>为/>的元素；

④抓取条件约束，确保机械爪和目标不出现碰撞，根据贝塞尔曲线的凸包性，贝塞尔曲线被包含在由控制点组成的凸包内，通过最后两个控制点来约束末端执行器轨迹的末端，当，且

；

其中，是圆锥角，对上式进行重写，得到线性模式

。

优选的，所述步骤S43中迭代计算的具体过程如下：

首先，先用一个外形多面体来表示机械臂和末端执行器，外形多面体的角点为，令/>为机械臂基座在惯性坐标系下的坐标，得到如下表达式

；

其中， ；

其中，是常数，角点计算为

；

其中，

；

；其中，也是常数；

其次，引入局部地图，定义局部地图为一个长方体，/>和/>的求解表达式为

；

其中，；/>为向量/>的相应的元素；

最后，采用GJK算法来检测是否发生碰撞，如果没有碰撞则终止迭代计算，如果发生碰撞则计算权重参数和障碍物镜像集合，假设发生碰撞的第i个障碍物的中心坐标为，并对与第i个障碍物的碰撞轨迹段的边界为/>和/>，根据小孔映射原理计算对应的障碍物镜像位置/>

；

权重参数的值通过一个更新策略进行更新，权重更新律为, 其中,/>为一个常数增益，迭代步长/>的求解表达式为

。

优选的，所述几何可行性条件表示为，所述几何可行性条件的计算步骤如下：

首先，计算集合,/>,/>,/>，其中分别为飞行平台俯仰角/>的最小值和最大值，/>分别为飞行平台滚转角的最小值和最大值；

其次，计算上述集合的交集；

最后，确定出上述交集包含的最大长方体，并将该长方体当做，/>和/>通过该长方体的尺寸确定出来。

因此，本发明采用上述结构一种基于几何可行性的空中自动抓取作业规划方法，具有以下有益效果：

1、提出了一种在笛卡尔坐标系内的分离式规划方法。相较于传统的在构型空间内的算法，本申请提出的方法需要处理的维度更低，更适合空中作业机器人；

2、提出了无人机可抓取点和抓取阶段的安全通道计算方法，通过该方法可以计算出抓取目标所需要的无人机位置和轨迹；

3、提出了几何可行性约束，该约束保证了无人机和机械臂的轨迹是几何可行的。这是分离式规划的基础，正是因为有了这个轨迹，才能够分别规划无人机和机械臂的轨迹；

4、提出了一种新型基于小孔映射方法的空中抓取机械臂避障迭代策略，该策略把机械臂和障碍物都近似为凸多面体，并将避障策略引入优化问题的目标函数之中，通过GJK算法来检测每次迭代计算的结果产生的碰撞。如果没有碰撞，那就得到了无碰撞的末端轨迹，如果产生碰撞，那么就可以利用小孔映射方法计算避障的惩罚函数。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1是本发明一种基于几何可行性的空中自动抓取作业规划方法的整体框图；

图2是本发明空中自动抓取目标图。

具体实施方式

实施例

以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参阅图1-2，一种基于几何可行性的空中自动抓取作业规划方法，包括飞行平台运动规划和机械臂运动规划，飞行平台运动规划的输入是目标位置和障碍物位置，飞行平台运动规划的输出是飞行平台的轨迹；机械臂运动规划的输入是目标位置和飞行平台轨迹，机械臂运动规划的输出是机械臂的期望关节角，飞行平台运动规划和机械臂运动规划是分离式规划且在几何可行性条件下完成的。几何可行性条件表示为，几何可行性条件的计算步骤如下：

首先，计算集合,/>,/>,/>，其中分别为飞行平台俯仰角/>的最小值和最大值，/>分别为飞行平台滚转角的最小值和最大值；

其次，计算上述集合的交集；

最后，确定出上述交集包含的最大长方体，并将该长方体当做，/>和/>通过该长方体的尺寸确定出来。

飞行平台运动规划具体包括以下步骤：

S21、面向可抓取点的路径规划。其目标是规划出一条路径，让飞行平台从起始点飞到可以抓取点，再从可抓取点飞到终点。可抓取点可以通过几何可行性约束进行求解，这是路径规划的重点。为了保证机械臂的操纵能力，当空中作业机器人抓起目标时，让机械臂末端处于工作空间的中心。因此，可抓取点的求解表达式为：

；

其中，为可抓取点位置；/>为期望飞行平台在抓取点时的期望旋转矩阵；为目标位置；/>为机械臂坐标系到机体坐标系的旋转矩阵；/>是机械臂工作空间的中心到机械臂坐标系原点的距离；/>为机械臂坐标系的原点在机体坐标系的位置。

获得了可抓取点之后，就可以利用A*算法计算由起点-可抓取点-终点组成的路径；

S22、移动和抓取阶段安全通道生成。其目标是生成一个安全的飞行通道，为了得到这个安全通道，将任务分成两部分：移动阶段和抓取阶段。移动阶段的安全通道可以通过凸分解方法求得。在抓取阶段的安全通道可以由一个预先设计的凸多面体表示，该凸多面体可以通过几何可行性约束求得。所设计的凸多面体可以表示为

；

其中，为/>是目标抓取方位角，且

；

在该设计凸多面体的持续时间受限于机械爪的性能，令持续时间为，该时间为机械爪的闭爪时间，飞行平台进入该凸多面体的时间为/>，进入该凸多面体之后，机械爪开始闭合，因此/>也为机械爪开始闭合的时间；

S23、通过分段贝塞尔曲线计算出飞行平台的轨迹。

机械臂运动规划具体包括以下步骤：

S41、基于微分平坦特性的初始状态计算，其目的是计算末端执行器在抓取阶段的初始位置和初始速度。

抓取阶段的开始时间可以通过下面的表达式求解

；

其中，和/>分别为末端执行器的最大速度和最大加速度。

初始位置可以通过几何特征求得，考虑空中作业机器人的几何特征，则有

；

其中，分别为在/>时刻的/>。从上式可以得到，想要计算末端执行器的初始位置/>必须先求解/>。/>可以直接通过飞行平台的轨迹得到。而/>可以通过飞行平台的微分平坦特性求得。

初始速度可以通过差分方法得到，令差分的补偿为。则令初始速度为

；

S42、基于几何可行性和空中抓取条件的末端执行器轨迹优化方法，其目的是获取经过初始点和目标位置的轨迹，并且该轨迹必须满足一系列的约束要求。空中作业机器人采用几何可行性约束，以保证生成的末端执行器的轨迹和飞行平台的轨迹满足空中作业机器人的几何约束。令为机械爪开始闭合的时间。则在/>到/>时间段的末端执行器轨迹求解问题，可以转化为一个优化问题，优化问题由目标函数和约束两部分组成。具体求解方法可以分为五个步骤完成：

第一，用贝塞尔曲线来表示末端轨迹。在每个一个迭代步长都求解一个QP优化问题。令轨迹为，将其用/>阶贝塞尔曲线表示：

；

其中，为贝塞尔曲线的参数向量；/>为贝塞尔曲线的基底向量，且有/>。

第二，将轨迹求解转化为贝塞尔曲线的参数求解，并将该参数求解问题转化为一个QP问题。定义为轨迹参数向量，则QP问题可以写为

；

其中，是Hessian矩阵并且其是半正定的；是一个向量；/>和/>为约束矩阵；和/>为约束向量。上式中的约束可以分为等式约束（路径点约束）和不等式约束（动力学可行性约束、几何可行性约束和抓取条件约束）。从上式，可以看出QP问题分为两部分，分别为目标函数和约束。

第三，对上述QP优化问题的目标函数进行求解。目标函数可以定义为，其中/>是代价函数以降低整条轨迹上的加加速度，而/>是用于避障的惩罚函数。这两项具体为

；

；

其中，为轨迹的加加速度；/>是变化的权重参数；/>是障碍物镜像位置坐标。令障碍物镜像集合为/>，其中/>是在整个迭代过程中遭遇的障碍物数量。对上面的表达式化简即可得到/>。

第四，获取QP问题的约束，约束具体为：

第一个约束就是路径点约束，路径点约束主要是使生成的轨迹通过和/>，因此路径点约束可以写为

；

其中，表示的/>的第i个控制点；/>,/>。

第二个约束是动力学可行性约束，其主要是约束速度和加速度，以保证生成的轨迹是可以被执行的。可以写为

；

其中，；下标/>表示相应变量的最小值；下标/> 表示相应变量的最大值。

第三个约束是几何可行性约束用于确保飞行平台和末端执行器的轨迹是几何可行的即满足空中作业机器人的几何特征。几何可行性可以描述为

；

其中，为末端执行器轨迹；/>为飞行平台轨迹；/>分别为几何可行性的下界和上界。

末端执行器的轨迹被表示为阶贝塞尔曲线，而飞行平台的轨迹被表示为/>阶贝塞尔曲线。用/>贝塞尔曲线去拟合飞行平台在/>到/>时间段内的轨迹。通过求得的飞行平台轨迹可以得到/>个点。定义贝塞尔曲线，其中/>为/>；为/>的基底向量。参数/>向量可以通过拟合得到。因此，几何可行性约束可写为

；

其中，为/>的元素；/>为的元素。根据贝塞尔曲线的凸包特性，几何可行性约束可以描述为

；

其中，。

第四个约束是抓取条件约束，用于确保机械爪和目标不会出现碰撞。根据贝塞尔曲线的凸包性，贝塞尔曲线被包含在由控制点组成的凸包内，因此用最后两个控制点来约束末端执行器轨迹的末端。当，且

;

其中，是圆锥角。对上式进行重写，得到线性模式

第五，QP问题求解。在每一次迭代中，都需要求解上述优化问题，得到结果后再进行避障计算，根据避障计算结果再更新计算权重参数和障碍物镜像，得到新的QP问题的目标函数，从而进行下一步的迭代计算。直到获得不发生碰撞的轨迹。具体地，根据QP问题的求解结果再更新计算和/>，从而得到新的QP问题的目标函数。在迭代计算之前，设置并且设置/>为空集即/>并且设置/>。/>和/>将在下文详细介绍；

S43、基于小孔映射方法的空中抓取机械臂避障计算，其目的在于保证生成的末端执行器的轨迹不会让机械臂和环境中的障碍物发生碰撞。通过一种迭代避障策略并且采用了小孔映射方法求解，迭代避障策略是指在每一次计算完上述QP问题之后，都进行碰撞检测，如果发生碰撞，则跟新优化问题的目标函数。如果不发生碰撞则得到了期望的轨迹并终止计算。具体的处理方法如下：

首先，先用一个外形多面体来表示机械臂和末端执行器。外形多面体的角点为。令/>为机械臂基座在惯性坐标系下的坐标。则可以得到

;

其中，

;

其中，是常数，用于确保安全性。此外，下面的角点可以计算为

;

其中，

;

其中，也是常数，用于确保安全性，给碰撞检测一些安全裕度。

其次，引入局部地图以保证安全。定义局部地图为一个长方体。此外，/>和/>的求解表达式为

;

其中，；/>为向量/>的相应的元素。

最后，采用Gilbert–Johnson–Keerthi算法来检测是否发生碰撞。如果没有碰撞则终止迭代计算。如果发生碰撞则计算权重参数和，假设发生碰撞的第i个障碍物的中心坐标为/>，并对与第i个障碍物的碰撞轨迹段的边界为/>和/>。则可以根据小孔映射原理计算对应的障碍物镜像位置/>。可得

;

权重参数的值可以通过一个更新策略进行更新，权重更新律为,其中/>,/>为一个常数增益。迭代步长/>非常重要，其求解表达式为

；

S44、协调规划，其输入为期望的末端执行器位置和期望的飞行平台位置。末端执行器的位置的求解表达式为

;

其中，为飞行平台位置；/>为机体坐标系相对地面坐标系的旋转矩阵；/>为机械臂末端的位置；/>为反对称变换矩阵（skew-symmetric matrix）。则可以得到

;

在机械臂坐标系内的期望的末端执行器速度被设计为

;

其中，是期望的末端速度；/>为期望的末端速度；/>是正对角矩阵。

引入空中作业机器人的物理限制。令和/>分别为/>和/>的后三个元素组成的向量。则有

;

这样，在机械臂坐标系内的期望的末端执行器位置可以通过对/>积分得到。求得期望末端位置/>后，可通过机械臂的逆运动学完成机械臂驱动关节/>的求解。

因此，本发明采用上述一种基于几何可行性的空中自动抓取作业规划方法，可以实现对空中自动抓取作业的运动规划，以解决现有技术存在的空中作业机器人在高维度构型空间中进行运动规划时计算量大的问题。本发明设计结构合理，采用基于几何可行性约束的分离式的架构，先规划飞行平台的轨迹，再规划末端执行器的轨迹。空中作业机器人由无人机和机械臂组成，其自由度高，构型空间的维度也高。相较于传统的在高维度的构型空间内的算法，本申请只需要在笛卡尔空间中进行运动规划计算，需要处理的维度更低，更适合空中作业机器人。

最后应说明的是：以上实施例仅用以说明本发明的技术方案而非对其进行限制，尽管参照较佳实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对本发明的技术方案进行修改或者等同替换，而这些修改或者等同替换亦不能使修改后的技术方案脱离本发明技术方案的精神和范围。

Claims (1)

1.一种基于几何可行性的空中自动抓取作业规划方法，其特征在于：包括飞行平台运动规划和机械臂运动规划，所述飞行平台运动规划的输入是目标位置和障碍物位置，所述飞行平台运动规划的输出是飞行平台的轨迹；所述机械臂运动规划的输入是目标位置和飞行平台轨迹，所述机械臂运动规划的输出是机械臂的期望关节角，所述飞行平台运动规划和所述机械臂运动规划是分离式规划且在几何可行性条件下完成的；

所述飞行平台运动规划具体包括以下步骤：

S21、面向可抓取点的路径规划，让飞行平台从起始点飞到可抓取点，再从可抓取点飞到终点，可抓取点通过几何可行性约束进行求解，再通过算法计算由起点、可抓取点、终点组成的路径；

S22、移动和抓取阶段安全通道的生成，生成一个安全的飞行通道，将任务分成两部分：移动阶段和抓取阶段；

S23、通过分段贝塞尔曲线计算出飞行平台的轨迹；

所述步骤S22中的移动阶段的安全通道通过凸分解方法求得，抓取阶段的安全通道由一个预先设计的凸多面体表示，所述凸多面体通过几何可行性约束求得，所述凸多面体具体表示为

；

其中，为/>是目标抓取方位角，且

；

所述机械臂运动规划具体包括以下步骤：

S41、基于微分平坦特性的初始状态计算，计算末端执行器在抓取阶段的初始位置和初始速度；

S42、基于几何可行性和空中抓取条件的末端轨迹优化方法，获取经过初始点和目标位置的轨迹，将末端执行器轨迹求解问题转化为一个优化问题；

S43、基于小孔映射方法的空中抓取机械臂避障迭代计算；

S44、协调规划，输入期望的末端执行器位置和期望的飞行平台位置，通过机械臂的逆运动学完成机械臂驱动关节的求解；

所述步骤S42中优化问题由目标函数和约束两部分组成，具体求解方法包括以下步骤：

S421、用贝塞尔曲线来表示末端轨迹，在每个一个迭代步长都求解一个QP优化问题，令轨迹为，将其用/>阶贝塞尔曲线表示：

；

其中，为贝塞尔曲线的参数向量；/>为贝塞尔曲线的基底向量，且有；

S422、将轨迹求解转化为贝塞尔曲线的参数求解，并将该参数求解问题转化为一个QP问题，定义为轨迹参数向量，QP问题写为

；

其中，是Hessian矩阵并且其是半正定的；/>是一个向量；/>和/>为约束矩阵；/>和/>为约束向量，约束分为等式约束和不等式约束，QP问题分为目标函数和约束；

S423、对上述QP问题的目标函数进行求解，目标函数定义为，其中/>是代价函数以降低整条轨迹上的加速度，/>是用于避障的惩罚函数，这两项具体为

；

；

其中，为轨迹的加加速度；/>是变化的权重参数；/>是障碍物镜像位置坐标；

S424、获取QP问题的约束；

S425、QP问题求解，通过一种迭代求解末端执行器轨迹的方法，在每一次迭代中计算QP问题，根据QP问题的求解结果再更新计算权重参数和障碍物镜像集合，得到新的QP问题的目标函数；

所述步骤S424中QP问题的约束具体如下：

①路径点约束，路径点约束主要是使生成的轨迹通过和/>，路径点约束为

；

其中，表示的/>的第i个控制点；/>, />；

②动力学可行性约束，为约束速度和加速度，动力学可行性约束为

；

其中，；下标/>表示相应变量的最小值；下标 /> 表示相应变量的最大值；

③几何可行性约束为

；

其中，为末端执行器轨迹；/>为飞行平台轨迹；/>分别为几何可行性的下界和上界，根据贝塞尔曲线的凸包特性，几何可行性约束描述为

；

其中，，/>为/>的元素；/>为/>的元素；

④抓取条件约束，根据贝塞尔曲线的凸包性，贝塞尔曲线被包含在由控制点组成的凸包内，通过最后两个控制点来约束末端执行器轨迹的末端，当，且

；

其中，是圆锥角，对上式进行重写，得到线性模式

；

所述步骤S43中迭代计算的具体过程如下：

首先，先用一个外形多面体来表示机械臂和末端执行器，外形多面体的角点为，令/>为机械臂基座在惯性坐标系下的坐标，得到如下表达式

；

其中，

；

其中，是常数，角点计算为

；

其中，

；

其中，也是常数；

其次，引入局部地图，定义局部地图为一个长方体，和/>的求解表达式为

；

其中，；/>为向量/>的相应的元素；

最后，采用Gilbert–Johnson–Keerthi算法来检测是否发生碰撞，如果没有碰撞则终止迭代计算，如果发生碰撞则计算权重参数和障碍物镜像集合，假设发生碰撞的第i个障碍物的中心坐标为，并对与第i个障碍物的碰撞轨迹段的边界为/>和/>，根据小孔映射原理计算对应的障碍物镜像位置/>；

；

权重参数的值通过一个更新策略进行更新，权重更新律为, 其中, />为一个常数增益，迭代步长/>的求解表达式为

；

所述几何可行性条件表示为，所述几何可行性条件的计算步骤如下：

首先，计算集合, />, />, />，其中/>分别为飞行平台俯仰角/>的最小值和最大值，/>分别为飞行平台滚转角/>的最小值和最大值；

其次，计算上述集合的交集；

最后，确定出上述交集包含的最大长方体，并将该长方体当做，/>和/>通过该长方体的尺寸确定出来。