CN115900716A - 基于梯度投影优化的关节空间规划方法、装置、设备及介质 - Google Patents

Abstract

本发明实施例公开了一种基于梯度投影优化的关节空间规划方法、装置、设备及介质；所述关节空间规划方法包括：根据多体动力学建立多臂航天器对应的初始碰撞检测运动学模型；采用碰撞检测算法对所述初始碰撞检测运动学模型进行检测；当所述初始的碰撞检测运动学模型中的机械臂之间发生了碰撞，采用梯度投影法对所述机械臂的关节运动轨迹进行规划，以得到修正后的碰撞检测运动学模型；当所述修正后的碰撞检测运动学模型中的各条机械臂之间不发生碰撞，且所述多臂航天器能够达到设定的目标地点时，输出所述多臂航天器的质心位置姿态及所述机械臂末端位置姿态的关节运动轨迹。

Description

基于梯度投影优化的关节空间规划方法、装置、设备及介质

技术领域

本发明实施例涉及关节空间规划技术领域，尤其涉及一种基于梯度投影优化的关节空间规划方法、装置、设备及介质。

背景技术

多臂航天器的碰撞检测是通过改变机械臂关节角度在关节空间中的轨迹，从而实现了三维空间下多臂航天器各机构之间的位置变化，进一步避免了各机构本体之间的碰撞。目前主流的机械臂关节空间轨迹的规划按机械臂的类型主要分为六自由度机械臂规划以及冗余机械臂规划。其中六自由度机械臂规划技术已经较为成熟，目前是基于深度学习或神经网络训练机械臂对于物体的操控性能；冗余机械臂规划技术的研究方向则比较广泛，如由于其冗余自由度带来的优化空间、避奇异以及碰撞检测技术等。目前主流的碰撞检测技术大多面向单机械臂或机器人整体与周围环境的避障，对于多分支的机器人碰撞检测技术研究较少。

发明内容

有鉴于此，本发明实施例期望提供一种基于梯度投影优化的关节空间规划方法、装置、设备及介质；能够在多臂航天器的碰撞检测过程中，针对稳定性与构型均匀性要求，采用梯度投影法对关节空间运动轨迹进行了优化，实现了多臂航天器无碰撞的运动轨迹规划。

本发明实施例的技术方案是这样实现的：

第一方面，本发明实施例提供了一种基于梯度投影优化的关节空间规划方法，所述关节空间规划方法包括：

根据多体动力学建立多臂航天器对应的初始碰撞检测运动学模型；

采用碰撞检测算法对所述初始碰撞检测运动学模型进行检测；

当所述初始的碰撞检测运动学模型中的机械臂之间发生了碰撞，采用梯度投影法对所述机械臂的关节运动轨迹进行规划，以得到修正的碰撞检测运动学模型；

当所述修正后的碰撞检测运动学模型中的各条机械臂之间不发生碰撞，且所述多臂航天器能够达到设定的目标地点时，输出所述多臂航天器的质心位置姿态及所述机械臂末端位置姿态的关节运动轨迹。

第二方面，本发明实施例提供了一种基于梯度投影优化的关节空间规划装置，所述关节空间规划装置包括：建立部分、检测部分、规划部分及输出部分；其中，

所述建立部分，经配置为根据多体动力学建立多臂航天器对应的初始碰撞检测运动学模型；

所述检测部分，经配置为采用碰撞检测算法对所述初始碰撞检测运动学模型进行检测；

所述规划部分，经配置为当所述初始的碰撞检测运动学模型中的机械臂之间发生了碰撞，采用梯度投影法对所述机械臂的关节运动轨迹进行规划，以得到修正的碰撞检测运动学模型；

所述输出部分，经配置为当所述修正后的碰撞检测运动学模型中的各条机械臂之间不发生碰撞，且所述多臂航天器能够达到设定的目标地点时，输出所述多臂航天器的质心位置姿态及所述机械臂末端位置姿态的关节运动轨迹。

第三方面，本发明实施例提供了一种基于梯度投影优化的关节空间规划设备，其特征在于，所述关节空间规划设备包括：通信接口，存储器和处理器；各个组件通过总线系统耦合在一起；其中，

所述通信接口，用于在与其他外部网元之间进行收发信息过程中，信号的接收和发送；

所述存储器，用于存储能够在所述处理器上运行的计算机程序；

所述处理器，用于在运行所述计算机程序时，执行第一方面所述基于梯度投影优化的关节空间规划方法步骤。

第四方面，本发明实施例提供了一种介质，所述介质存储有基于梯度投影优化的关节空间规划的程序，所述基于梯度投影优化的关节空间规划的程序被至少一个处理器执行时实现第一方面所述基于梯度投影优化的关节空间规划方法的步骤。

本发明实施例提供了一种基于梯度投影优化的关节空间规划方法、装置、设备及介质；通过根据多体动力学建立多臂航天器对应的初始碰撞检测运动学模型；采用碰撞检测算法对初始碰撞检测运动学模型进行检测；当初始的碰撞检测运动学模型中的机械臂之间发生了碰撞，采用梯度投影法对机械臂的关节运动轨迹进行规划，以得到修正的碰撞检测运动学模型；当修正后的碰撞检测运动学模型中的各条机械臂之间不发生碰撞，且多臂航天器能够达到设定的目标地点时，输出所述多臂航天器的质心位置姿态及所述机械臂末端位置姿态的关节运动轨迹。通过本发明实施例提供的基于梯度投影优化的关节空间规划方法，生成了无碰撞的多臂航天器关节运动轨迹。

附图说明

图1为本发明实施例提供的多臂航天器的结构示意图；

图2为本发明实施例提供的机械臂的三维模型示意图；

图3为本发明实施例提供的一种基于梯度投影优化的关节空间规划方法流程示意图；

图4为本发明实施例提供的机械臂D-H模型坐标系示意图；

图5为本发明实施例提供的碰撞检测运动学模型示意图；

图6为本发明一种实施例提供的机械臂构型分布均匀性指标示意图；

图7为本发明另一种实施例提供的机械臂构型分布均匀性指标示意图；

图8为本发明实施例提供的碰撞检测流程图；

图9为本发明实施例提供的机械臂笛卡尔空间轨迹示意图；

图10为本发明实施例提供的关节角度曲线示意图；

图11为本发明实施例提供的关节角速度曲线示意图；

图12为本发明实施例提供的通过碰撞检测算法计算的各几何体的位置关系示意图；

图13为本发明实施例提供的改进避障后的关节角度曲线示意图；

图14为本发明实施例提供的改进避障后的关节角速度曲线示意图；

图15为本发明实施例提供的改进避障前后的关节角度差值曲线示意图；

图16为本发明实施例提供的改进避障前后的关节角速度差值曲线示意图；

图17为本发明实施例提供的K_H＝0.1的关节角度曲线示意图；

图18为本发明实施例提供的K_H＝0.1的关节角速度曲线示意图；

图19为本发明实施例提供的K_H＝0.1与K_H＝0的角度差曲线示意图；

图20为本发明实施例提供的K_H＝0.1与K_H＝0的角速度差曲线示意图；

图21为本发明实施例提供的K_H＝0.1与K_H＝0的H(q)变化曲线；

图22为本发明实施例提供的一种基于梯度投影优化的关节空间规划装置组成示意图；

图23为本发明实施例提供的一种基于梯度投影优化的关节空间规划设备的具体硬件结构组成示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。

在对本发明实施例进行详细阐述前，需要说明的是，在本发明实施例中仅以包含有4条机械臂的多臂航天器作为示例详细阐述空间移动的步态规划技术方案，但是在具体实施过程中，多臂航天器中的机械臂数量不进行具体限定。

参见图1，其示出了能够实施本发明实施例的多臂航天器1的结构示意图，如图1所示，该多臂航天器1由一个立方体基座11和四条中心对称式分布的机械臂12组成。机械臂12的三维模型具体参见图2。由图1和图2可以看出，每条机械臂12中包含7个旋转关节121、臂杆122及末端123。需要说明的是，在本发明实施例中基于每条机械臂12包含有7个旋转关节121对涉及的技术方案进行了详细阐述，但是本发明实施例中并不限定机械臂12的关节数量。

参见图3，其示出了本发明实施例提供的一种基于梯度投影优化的关节空间规划方法，该方法具体包括：

S301、根据多体动力学建立多臂航天器对应的初始碰撞检测运动学模型；

S302、采用碰撞检测算法对所述初始碰撞检测运动学模型进行检测；

S303、当所述初始的碰撞检测运动学模型中的机械臂之间发生了碰撞，采用梯度投影法对所述机械臂的关节运动轨迹进行规划，以得到修正后的碰撞检测运动学模型；

S304、当所述修正后的碰撞检测运动学模型中的各条机械臂之间不发生碰撞，且所述多臂航天器能够达到设定的目标地点时，输出所述多臂航天器的质心位置姿态及所述机械臂末端位置姿态的关节运动轨迹。

对于图3所示的技术方案，通过根据多体动力学建立多臂航天器对应的初始碰撞检测运动学模型；采用碰撞检测算法对初始碰撞检测运动学模型进行检测；当初始的碰撞检测运动学模型中的机械臂之间发生了碰撞，采用梯度投影法对机械臂的关节运动轨迹进行规划，以得到修正的碰撞检测运动学模型；当修正后的碰撞检测运动学模型中的各条机械臂之间不发生碰撞，且多臂航天器能够达到设定的目标地点时，输出所述多臂航天器的质心位置姿态及所述机械臂末端位置姿态的关节运动轨迹。通过本发明实施例提供的基于梯度投影优化的关节空间规划方法，生成了无碰撞的多臂航天器关节运动轨迹。

对于图3所示的技术方案，在一些可能的实施方式中，在所述根据多体动力学建立多臂航天器对应的初始碰撞检测运动学模型之前，所述关节空间规划方法还包括：

基于所述多臂航天器的机械臂逆运动学计算方法，获取所述机械臂关节角度与关节角速度。

对于上述的实施方式，在一些示例中，所述基于所述多臂航天器的机械臂逆运动学计算方法，获取所述机械臂关节角度与关节角速度，包括：

根据式(1)所示的雅可比矩阵的第一伪逆求解方法计算得到式(2)所示的所述机械臂逆运动学的第一求解公式：

其中，

表示所述机械臂关节角速度向量；

表示所述机械臂任务空间角速度向量；J表示所述雅可比矩阵；J⁺表示所述雅可比矩阵的伪逆，且J⁺＝J^T(JJ^T)^-1；I表示单位矩阵；

表示所述机械臂关节空间内任一角速度向量，基于机械臂当前运动作为指标进行优化；

将式(3)所示的变阻尼系数β带入式(1)，得到避奇异处理后的如式(4)所示的雅可比矩阵的第二伪逆求解方法，并根据式(4)所示的雅可比矩阵的第二伪逆求解方法计算得到式(5)所示的所述机械臂逆运动学的第二求解公式：

其中，λ₀表示最大阻尼系数；σ_m表示最小奇异值；σ₀表示所述最小奇异值的边界值；J^*表示所述雅可比矩阵的奇异鲁棒性逆，且J⁺＝J^T(JJ^T+λ²I)^-1。

可以理解地，在具体实施过程中，当上述多臂航天器质心的位姿路径以及各条机械臂末端的位姿路径获知后，基于冗余机械臂逆运动学的雅可比矩阵的伪逆求解方法能够得到机械臂关节角速度和关节角度向量。

需要说明的是，基于伪逆法的机械臂逆运动学需要进行避奇异处理，否则在机械臂的奇异位置会发生规划关节角速度的突变，因此进一步引入了变阻尼系数β以调节机械臂末端的跟踪误差和关节角速度变化，以避免规划过程中的关节角速度发生不符合机构安全约束的突变现象。

此外，通过引入变阻尼系数β对伪逆法求解方法进行优化，使得关节角速度在跟踪机械臂末端误差时不会出现异常突变的现象，从而使冗余机械臂的逆运动学求得的解在物理上可行，具有实际意义。

对于图3所示的技术方案，在一些可能的实施方式中，所述根据多体动力学建立多臂航天器对应的初始碰撞检测运动学模型，包括：

假定所述多臂航天器的基座与各条机械臂中的关节、臂杆及末端均为刚体，从所述基座处对各条机械臂建立D-H(Denavit-Hartenberg)模型；

基于所述D-H模型，利用多体动力学对所述基座及各条机械臂中的关节、臂杆及末端分别采用立方体与圆柱体进行包络处理以获得所述碰撞检测运动学模型。

具体来说，基于图2所示的机械臂的三维模型，建立的机械臂12的D-H模型坐标系如图4所示，机械臂各杆件参数如表1和表2所示。

表1机械臂各臂杆的尺寸参数

编号	<![CDATA[a<sub>0</sub>]]>	<![CDATA[a<sub>1</sub>]]>	<![CDATA[a<sub>2</sub>]]>	<![CDATA[a<sub>3</sub>]]>	<![CDATA[a<sub>4</sub>]]>	<![CDATA[a<sub>5</sub>]]>	<![CDATA[a<sub>6</sub>]]>	<![CDATA[a<sub>7</sub>]]>
									长度(mm)	159	98	400	98	400	86	86	20

表2机械臂D-H模型参数

连杆i	<![CDATA[θ<sub>i</sub>(°)]]>	<![CDATA[α<sub>i-1</sub>(°)]]>	<![CDATA[d<sub>i</sub>]]>	<![CDATA[a<sub>i-1</sub>]]>	关节变量
						1	180	0	<![CDATA[a<sub>0</sub>]]>	0	<![CDATA[ψ<sub>1</sub>]]>
2	90	90	<![CDATA[a<sub>1</sub>]]>	0	<![CDATA[ψ<sub>2</sub>]]>
						3	90	-90	<![CDATA[a<sub>2</sub>]]>	0	<![CDATA[ψ<sub>3</sub>]]>
4	180	0	<![CDATA[a<sub>4</sub>]]>	<![CDATA[a<sub>3</sub>]]>	<![CDATA[ψ<sub>4</sub>]]>
						5	180	-90	<![CDATA[a<sub>6</sub>]]>	<![CDATA[a<sub>5</sub>]]>	<![CDATA[ψ<sub>5</sub>]]>
6	180	-90	<![CDATA[a<sub>7</sub>]]>	0	<![CDATA[ψ<sub>6</sub>]]>
						7	0	-90	<![CDATA[a<sub>8</sub>]]>	0	<![CDATA[ψ<sub>7</sub>]]>

需要说明的是，图4中x_EE轴、y_EE轴及z_EE轴分别表示在机械臂末端所建立的坐标系的三个坐标轴。

对于上述的实施方式，在一些示例中，在所述D-H模型中，关节坐标系o_ix_iy_iz_i相对于原点坐标系o_i-1x_i-1y_i-1z_i-1的变换矩阵

为：

其中，i表示关节数，i＝1,2,3,...,r；a_i-1表示从原点坐标系Σ_i-1的原点到x_i-1轴和z_i轴的交点的偏置距离；α_i-1表示绕x_i-1轴按右手规则由z_i-1轴转向z_i轴的角度；d_i表示从关节坐标系Σ_i的原点到z_i轴和x_i-1轴的交点沿z_i轴的距离；θ_i表示绕z_i轴按右手规则由x_i-1轴转向x_i轴的关节角；r表示各机械臂的关节数量；

所述机械臂的末端相对于所述机械臂与基座连接处的矩阵变化为：

其中，

表示所述机械臂的末端相对于所述机械臂与基座连接处的变换矩阵；R_3×3表示所述机械臂的末端相对于所述机械臂与基座连接处的旋转变换；P_3×1表示所述机械臂末端相对于所述机械臂与基座连接处的平移变换。

需要说明的是，在本发明实施例中多臂航天器的基座11用立方体包络，机械臂12的关节、臂杆以及末端分别用多个圆柱体进行包络；可以理解地，碰撞检测运动学模型的尺寸根据基座及机械臂的最大尺寸确定，建立的多臂航天器1的碰撞检测运动学模型如图5所示。

对于图3所示的技术方案，在一些可能的实施方式中，所述采用碰撞检测算法对所述初始碰撞检测运动学模型进行检测，包括：

基于已知的关节角度和关节角速度，利用所述机械臂正动力学获取所述碰撞检测运动学模型中各刚体的位置p和姿态A；

基于所述碰撞检测运动学模型中各刚体的位置p和姿态A，获取所述碰撞检测运动学模型中各刚体之间的最小距离||d_kj||，若所述最小距离||d_kj||小于0，判定两个刚体之间会发生碰撞；其中，k,j表示不同的刚体。

对于图3所示的技术方案，在一些可能的实施方式中，所述若所述机械臂之间发生了碰撞，采用梯度投影法对所述关节角速度进行梯度上的优化规划，包括：

基于所述梯度投影法，将式(8)带入式(5)获得所述关节角度与性能指标之间的关系如式(9)所示：

其中，K_H表示优化系数；▽H(q)表示性能指标H相对于关节角度q的梯度，且

q₁、q₂、…、q_r表示r个自由度的机械臂各关节角度；r表示各机械臂的关节数量；

基于所述机械臂与基座的动力学耦合特性以及所述机械臂构型分布均匀性的参数，分别根据式(10)和式(11)对性能指标进行优化，以得到性能指标的表达式如式(12)所示：

其中，

表示动力学耦合特性性能指标；σ_hb(q)表示所述机械臂末端到所述基座的耦合关系；

表示机械臂构型分布均匀性性能指标；

表示相邻的两条机械臂的第四个关节位置p₄在多臂航天器XOY平面内投影的夹角；γ_s表示所述机械臂的末端与机械臂的第四个关节位置p₄在多臂航天器XOY平面内投影的夹角；K_σ表示动力学耦合特性优化系数；

表示机械臂构型分布均匀性优化系数；

若在规划的过程中刚体V_k与V_j之间发生了碰撞，且碰撞的位置p_k与p_j在发生碰撞前两者之间的向量为d_kj，在发生碰撞后，分别对刚体V_k与V_j所在的机械臂在规划时根据式(13)修改方向梯度：

其中，k_d表示避碰系数，随着在碰撞检测循环中迭代次数的变大而变大。

可以理解的是，机械臂与基座的动力学耦合与多臂航天器的轨迹规划密切相关，因此需要通过动量守恒，推导机械臂与基座间的位置级、速度级耦合关系。由

可得：

其中，

表示机械臂速度；J_m表示机械臂转动雅可比矩阵；J_b表示机械臂基座雅可比矩阵；

表示基座速度。

因此，机械臂末端到基座的耦合关系可表示为：

其中，C_hb表示机械臂末端到基座的耦合矩阵；A_hb表示三阶实对称耦合方阵，且A_hb＝(C_hb)^TC_hb。

假设λ₁,…,λ_n(0≤λ₁≤…≤λ_n)为A_hb方阵的特征值，由二阶范数的性质有如下关系：

耦合矩阵C_hb与基座、机械臂各臂杆的质量特性、机械臂的关节角、姿态基座等相关，可以通过改变矩阵A_hb奇异值的分布，从而减小动力学的耦合特性，因此将该系数引入性能指标当中，使机械臂运动过程中更倾向于对基座影响更小的关节空间路径，具体如上式(10)所示。

另一方面，对于多机械臂的规划来说，各机械臂之间相互协同也是很重要的。每条机械臂在移动的过程中相对于整个多臂航天器的布局的均匀性是衡量多臂航天器稳定性的一个重要指标，如果关节空间规划的结果使得多条机械臂都倾向多臂航天器的某一侧，那么会使整个多臂航天器的质心偏离多臂航天器结构的中心，并且其各关节运动产生的相对于多臂航天器基座的力矩也会变大。这种不均匀的结果显然是不利于多臂航天器的操控，因此在关节空间的规划过程中引入机械臂构型分布均匀性的参数，作为多机械臂协同的评价指标之一，具体如式(17)所示：

其中，

表示相邻的两条机械臂的第四个关节位置p₄在多臂航天器XOY平面内投影的夹角，具体如图6所示。

同时，为了避免机械臂在保证了均匀性的同时，其中间关节相对于其末端绕着基座中心均向顺时针或者逆时针旋转，将式(17)改进为上式(11)。需要说明的是，式(11)中的夹角γ_s具体如图7所示。

因此，在本发明实施例中的性能指标H(q)定义为式(12)。

参见图8，其示出了本发明实施例提供的碰撞检测流程图。具体来说：

S801、根据逆运动学计算公式得到机械臂中各关节角度，并得到碰撞检测运动学模型的位置以采用碰撞检测算法对碰撞检测模型进行碰撞检测；

S802、如果检测结果判断发生了碰撞，则对规划的关节空间路径采用梯度投影法进行方向梯度上的修改，并再次通过机械臂正运动学计算出修改后的碰撞检测运动学模型的位置；

S803、采用碰撞检测算法检测修改后的碰撞检测运动学模型是否会发生碰撞检测；若修改后的碰撞检测模型依然发生碰撞时，继续执行步骤S802；若修改后的碰撞检测模型没有发生碰撞时，执行步骤S804；

S804、检测多臂航天器是否到达目标点；若多臂航天器没有达到目标点，则继续执行步骤S801；若多臂航天器达到目标点，则输出关节运动轨迹。

下面将以具体的仿真示例对本发明实施例提供的技术方案进行详细阐述。

多臂航天器的自身碰撞检测是路径规划任务中的安全性约束需求，是保证地面实验以及空间移动任务安全顺利完成的关键技术。首先在不施加约束的情况下直接基于梯度投影法进行机械臂关节空间规划，仿真参数设置如下所示：

p_e＝[0.2,0.1,0.6；-0.1,0.2,0.6；-0.2,-0.1,0.6；0.1,-0.2,0.6]

仿真结果如图9至图11所示。其中图9中的单位为：米(m)。由图9至图11可以看出，机械臂1至机械臂4的初始状态与目标位置设置相同，因此四条机械臂的关节角度与关节角速度曲线相同。经过仿真验证，在t＝15s后，关节1与臂杆4发生了碰撞，通过碰撞检测算法计算其对应的几何体的位置关系，如图12所示，验证了上述臂杆1与臂杆4对应的几何体之间发生了碰撞。因此该碰撞检测方法证明合理有效，仿真验证正确。

对基于上述碰撞检测技术的改进避障梯度投影法进行仿真，结果如图13至图20所示。

通过改进避障后的关节角速度曲线分析可以看出，采用了碰撞检测技术之后，在0、4、8、9秒左右时，关节角速度相较与未采用碰撞检测技术时发生了突变，这说明在这些时刻机械臂的某些部位发生了碰撞，因此在规划过程中采取了机动策略，以避免碰撞的发生。

分别将图13和图14中的关节角度、关节角速度结果做差，得到改进避障前后的关节角度、关节角速度差值曲线，由图15和图16可以看出，在避障的过程中，所有的关节角度相比于改进避障前都有所变化，其中部分关节的差值在10秒左右达到峰值，达到了0.3281°。在10秒之后基本没有较大的机动现象，说明无碰撞现象发生，并且角度差也一直维持在0°，说明在采用碰撞检测技术后如果没有发生碰撞情况时，该方法均能按照之前的梯度收敛并到达目标位置。经过验证，在采用了基于碰撞检测技术的改进避障梯度投影法关节空间规划后，多臂航天器的各包络体之间确无碰撞现象发生，证明该方法有效。

通过图17与图18可以看出，机械臂的关节空间路径发生了明显的变化，机械臂的关节角速度分别在0、5、6、12、14秒左右时刻发生了突变，说明在这些时刻机械臂判断发生了碰撞，采取了避障措施，在K_H＝0.1与K_H＝0的情况下发生碰撞的时间与位置也有所不同。

分别将图17与图13、图18与图14的结果做差，得到图19、图20的关节角度、关节角速度差值曲线，可以看出，在K_H＝0.1情况下机械臂的关节空间路径与K_H＝0情况下有着较大不同，多数关节的最终规划结果有着5～10°不等的偏差，最大的关节角度偏差达到了14.65°。在3秒、13秒、17秒左右关节角速度差有着较大的突变，说明性能指标H(q)对于q的变化是不连续的。

分别将K_H＝0.1与K_H＝0的关节角度仿真结果代回入式(12)，计算得到性能指标H(q)随时间变化曲线，如图21所示，由图21可以看出规划后的关节空间路径全程的性能指标都更高，提升了14.31％～30.50％，说明关节空间轨迹具有沿着动力学耦合系数更小、构型分布更均匀的梯度规划的趋势，同时也验证了算法的有效性。

基于前述技术方案相同的发明构思，参见图22，其示出了本发明实施例提供的一种基于梯度投影优化的关节空间规划装置220组成，所述关节空间规划装置220包括：建立部分2201、检测部分2202、规划部分2203及输出部分2204；其中，

所述建立部分2201，经配置为根据多体动力学建立多臂航天器对应的初始碰撞检测运动学模型；

所述检测部分2202，经配置为采用碰撞检测算法对所述初始碰撞检测运动学模型进行检测；

所述规划部分2203，经配置为当所述初始的碰撞检测运动学模型中的机械臂之间发生了碰撞，采用梯度投影法对所述机械臂的关节运动轨迹进行规划，以得到修正的碰撞检测运动学模型；

所述输出部分2204，经配置为当所述修正后的碰撞检测运动学模型中的各条机械臂之间不发生碰撞，且所述多臂航天器能够达到设定的目标地点时，输出所述多臂航天器的质心位置姿态及所述机械臂末端位置姿态的关节运动轨迹。

需要说明的是，对于上述各组件所配置功能的具体实现方式或实施示例内容，可参见前述技术方案相应的步骤及实现方式和示例，本发明实施例在此不作赘述。可以理解地，在本实施例中，“部分”可以是部分电路、部分处理器、部分程序或软件等等，当然也可以是单元，还可以是模块也可以是非模块化的。

另外，在本实施例中的各组成部分可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。

所述集成的单元如果以软件功能模块的形式实现并非作为独立的产品进行销售或使用时，可以存储在一个计算机可读取存储介质中，基于这样的理解，本实施例的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或processor(处理器)执行本实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

因此，本实施例提供了一种计算机存储介质，所述计算机存储介质存储有基于梯度投影优化的关节空间规划的程序，所述基于梯度投影优化的关节空间规划的程序被至少一个处理器执行时实现上述技术方案中所述基于梯度投影优化的关节空间规划方法步骤。

根据上述基于梯度投影优化的关节空间规划装置220以及计算机存储介质，参见图23，其示出了本发明实施例提供的一种能够实施上述基于梯度投影优化的关节空间规划装置220的计算设备230的具体硬件结构，该计算设备230可以为无线装置、移动或蜂窝电话(包含所谓的智能电话)、个人数字助理(PDA)、视频游戏控制台(包含视频显示器、移动视频游戏装置、移动视频会议单元)、膝上型计算机、桌上型计算机、电视机顶盒、平板计算装置、电子书阅读器、固定或移动媒体播放器，等。计算设备230包括：通信接口2301，存储器2302和处理器2303；各个组件通过总线系统2304耦合在一起。可理解，总线系统2304用于实现这些组件之间的连接通信。总线系统2304除包括数据总线之外，还包括电源总线、控制总线和状态信号总线。但是为了清楚说明起见，在图23中将各种总线都标为总线系统2304。其中，

所述通信接口2301，用于在与其他外部网元之间进行收发信息过程中，信号的接收和发送；

所述存储器2302，用于存储能够在所述处理器2303上运行的计算机程序；

所述处理器2303，用于在运行所述计算机程序时，执行前述技术方案中所述基于梯度投影优化的关节空间规划方法步骤。

可以理解，本发明实施例中的存储器2302可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(Read-Only Memory，ROM)、可编程只读存储器(Programmable ROM，PROM)、可擦除可编程只读存储器(Erasable PROM，EPROM)、电可擦除可编程只读存储器(Electrically EPROM，EEPROM)或闪存。易失性存储器可以是随机存取存储器(Random Access Memory，RAM)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器(Static RAM，SRAM)、动态随机存取存储器(Dynamic RAM，DRAM)、同步动态随机存取存储器(Synchronous DRAM，SDRAM)、双倍数据速率同步动态随机存取存储器(Double DataRate SDRAM，DDRSDRAM)、增强型同步动态随机存取存储器(Enhanced SDRAM，ESDRAM)、同步连接动态随机存取存储器(Synchlink DRAM，SLDRAM)和直接内存总线随机存取存储器(Direct Rambus RAM，DRRAM)。本文描述的系统和方法的存储器2302旨在包括但不限于这些和任意其它适合类型的存储器。

而处理器2303可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器2303中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器2303可以是通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器2302，处理器2303读取存储器2302中的信息，结合其硬件完成上述方法的步骤。可以理解的是，本文描述的这些实施例可以用硬件、软件、固件、中间件、微码或其组合来实现。对于硬件实现，处理单元可以实现在一个或多个专用集成电路(Application Specific Integrated Circuits，ASIC)、数字信号处理器(Digital SignalProcessing，DSP)、数字信号处理设备(DSP Device，DSPD)、可编程逻辑设备(ProgrammableLogic Device，PLD)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)、通用处理器、控制器、微控制器、微处理器、用于执行本申请所述功能的其它电子单元或其组合中。

对于软件实现，可通过执行本文所述功能的模块(例如过程、函数等)来实现本文所述的技术。软件代码可存储在存储器中并通过处理器执行。存储器可以在处理器中或在处理器外部实现。具体来说，处理器2303还配置为运行所述计算机程序时，执行前述技术方案中所述基于梯度投影优化的关节空间规划方法步骤，这里不再进行赘述。

需要说明的是：本发明实施例所记载的技术方案之间，在不冲突的情况下，可以任意组合。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (10)

1.一种基于梯度投影优化的关节空间规划方法，其特征在于，所述关节空间规划方法包括：

根据多体动力学建立多臂航天器对应的初始碰撞检测运动学模型；

采用碰撞检测算法对所述初始碰撞检测运动学模型进行检测；

当所述初始的碰撞检测运动学模型中的机械臂之间发生了碰撞，采用梯度投影法对所述机械臂的关节运动轨迹进行规划，以得到修正后的碰撞检测运动学模型；

当所述修正后的碰撞检测运动学模型中的各条机械臂之间不发生碰撞，且所述多臂航天器能够达到设定的目标地点时，输出所述多臂航天器的质心位置姿态及所述机械臂末端位置姿态的关节运动轨迹。

2.根据权利要求1所述的关节空间规划方法，其特征在于，在所述根据多体动力学建立多臂航天器对应的初始碰撞检测运动学模型之前，所述关节空间规划方法还包括：

基于所述多臂航天器的机械臂逆运动学计算方法，获取所述机械臂关节角度与关节角速度。

3.根据权利要求2所述的关节空间规划方法，其特征在于，所述基于所述多臂航天器的机械臂逆运动学计算方法，获取所述机械臂关节角度与关节角速度，包括：

根据式(1)所示的雅可比矩阵的第一伪逆求解方法计算得到式(2)所示的所述机械臂逆运动学的第一求解公式：

其中，

表示所述机械臂关节角速度向量；

表示所述机械臂任务空间角速度向量；J表示所述雅可比矩阵；J⁺表示所述雅可比矩阵的伪逆，且J⁺＝J^T(JJ^T)^-1；I表示单位矩阵；

表示所述机械臂关节空间内任一角速度向量，基于机械臂当前运动作为指标进行优化；

将式(3)所示的变阻尼系数β带入式(1)，得到避奇异处理后的如式(4)所示的雅可比矩阵的第二伪逆求解方法，并根据式(4)所示的雅可比矩阵的第二伪逆求解方法计算得到式(5)所示的所述机械臂逆运动学的第二求解公式：

其中，λ₀表示最大阻尼系数；σ_m表示最小奇异值；σ₀表示所述最小奇异值的边界值；J^*表示所述雅可比矩阵的奇异鲁棒性逆，且J⁺＝J^T(JJ^T+λ²I)^-1。

4.根据权利要求1所述的关节空间规划方法，其特征在于，所述根据多体动力学建立多臂航天器对应的初始碰撞检测运动学模型，包括：

假定所述多臂航天器的基座与各条机械臂中的关节、臂杆及末端均为刚体，从所述基座处对各条机械臂建立D-H模型；

基于所述D-H模型，利用多体动力学对所述基座及各条机械臂中的关节、臂杆及末端分别采用立方体与圆柱体进行包络处理以获得所述碰撞检测运动学模型。

5.根据权利要求4所述的关节空间规划方法，其特征在于，在所述D-H模型中，关节坐标系o_ix_iy_iz_i相对于原点坐标系o_i-1x_i-1y_i-1z_i-1的变换矩阵

为：

其中，i表示关节数，i＝1,2,3,…,r；a_i-1表示从原点坐标系Σ_i-1的原点到x_i-1轴和z_i轴的交点的偏置距离；α_i-1表示绕x_i-1轴按右手规则由z_i-1轴转向z_i轴的角度；d_i表示从关节坐标系Σ_i的原点到z_i轴和x_i-1轴的交点沿z_i轴的距离；θ_i表示绕z_i轴按右手规则由x_i-1轴转向x_i轴的关节角；

所述机械臂的末端相对于所述机械臂与基座连接处的矩阵变化为：

其中，

表示所述机械臂的末端相对于所述机械臂与基座连接处的变换矩阵；R_3×3表示所述机械臂的末端相对于所述机械臂与基座连接处的旋转变换；P_3×1表示所述机械臂末端相对于所述机械臂与基座连接处的平移变换。

6.根据权利要求1所述的关节空间规划方法，其特征在于，所述采用碰撞检测算法对所述初始碰撞检测运动学模型进行检测，包括：

基于已知的关节角度和关节角速度，利用所述机械臂正动力学获取所述碰撞检测运动学模型中各刚体的位置p和姿态A；

基于所述碰撞检测运动学模型中各刚体的位置p和姿态A，获取所述碰撞检测运动学模型中各刚体之间的最小距离||d_kj||，若所述最小距离||d_kj||小于0，判定两个刚体之间会发生碰撞；其中，k,j表示不同的刚体。

7.根据权利要求1所述的关节空间规划方法，其特征在于，所述当所述初始的碰撞检测运动学模型中的机械臂之间发生了碰撞，采用梯度投影法对所述机械臂的关节运动轨迹进行规划，以得到修正的碰撞检测运动学模型，包括：

基于所述梯度投影法，将式(8)带入式(5)获得所述关节角度与性能指标之间的关系如式(9)所示：

其中，K_H表示优化系数；

表示性能指标H相对于关节角度q的梯度，且

基于所述机械臂与基座的动力学耦合特性以及所述机械臂构型分布均匀性的参数，分别根据式(10)和式(11)对性能指标进行优化，以得到性能指标的表达式如式(12)所示：

其中，H_σ(q)表示动力学耦合特性性能指标；σ_hb(q)表示所述机械臂末端到所述基座的耦合关系；

表示机械臂构型分布均匀性性能指标；

表示相邻的两条机械臂的第四个关节位置p₄在多臂航天器XOY平面内投影的夹角；γ_s表示所述机械臂的末端与机械臂的第四个关节位置p₄在多臂航天器XOY平面内投影的夹角；K_σ表示动力学耦合特性优化系数；

表示机械臂构型分布均匀性优化系数；

若在规划的过程中刚体V_k与V_j之间发生了碰撞，且碰撞的位置p_k与p_j在发生碰撞前两者之间的向量为d_kj，在发生碰撞后，分别对刚体V_k与V_j所在的机械臂在规划时根据式(13)修改方向梯度：

其中，k_d表示避碰系数，随着在碰撞检测循环中迭代次数的变大而变大。

8.一种基于梯度投影优化的关节空间规划装置，其特征在于，所述关节空间规划装置包括：建立部分、检测部分、规划部分及输出部分；其中，

所述建立部分，经配置为根据多体动力学建立多臂航天器对应的初始碰撞检测运动学模型；

所述检测部分，经配置为采用碰撞检测算法对所述初始碰撞检测运动学模型进行检测；

所述规划部分，经配置为当所述初始的碰撞检测运动学模型中的机械臂之间发生了碰撞，采用梯度投影法对所述机械臂的关节运动轨迹进行规划，以得到修正的碰撞检测运动学模型；

所述输出部分，经配置为当所述修正后的碰撞检测运动学模型中的各条机械臂之间不发生碰撞，且所述多臂航天器能够达到设定的目标地点时，输出所述多臂航天器的质心位置姿态及所述机械臂末端位置姿态的关节运动轨迹。

9.一种基于梯度投影优化的关节空间规划设备，其特征在于，所述关节空间规划设备包括：通信接口，存储器和处理器；各个组件通过总线系统耦合在一起；其中，

所述通信接口，用于在与其他外部网元之间进行收发信息过程中，信号的接收和发送；

所述存储器，用于存储能够在所述处理器上运行的计算机程序；

所述处理器，用于在运行所述计算机程序时，执行权利要求1至7中任一项所述基于梯度投影优化的关节空间规划方法步骤。

10.一种介质，其特征在于，所述介质存储有基于梯度投影优化的关节空间规划的程序，所述基于梯度投影优化的关节空间规划的程序被至少一个处理器执行时实现权利要求1至7中任一项所述基于梯度投影优化的关节空间规划方法的步骤。

Images