7-DOF仿人臂飞行物体作业最小加速度轨迹优化的制作方法

本发明涉及一种7-dof仿人臂飞行物体作业最小加速度轨迹优化方法，属于机器人控制技术领域。

背景技术：

七自由度仿人臂轨迹规划问题是当前机器人领域中的研究热点，它为机器人能更好地服务于人类起着重要作用。机器人轨迹规划是指在机器人各运动约束范围内，设计一条满足任务需求合理的动作轨迹。根据规划空间的不同机器人轨迹规划可分为关节空间规划与任务空间规划两种方法，其中关节空间规划具有较好实时性，计算量小；任务空间规划需要将笛卡尔空间末端轨迹通过逆运动学映射到关节空间内，计算量较大。

七自由度仿人臂由于连杆机构与形状设计使得其各关节位置转动范围有限，仿人臂关节驱动能力的限制也致使其运动能力有限，因此在对飞行物体作业任务执行中，需在仿人臂关节物理约束范围内规划一条能满足实际任务需求的光滑、连续的动作轨迹。7-dof仿人臂关节结构冗余，存有无穷多种构型满足末端的作业任务需求，因此需在机构物理约束范围内按照一定的目标准则优选确定一条满足任务需求的最优运动轨迹。目前已有学者对冗余仿人臂轨迹规划问题进行相关研究，如通过关节力矩信息描述“舒适度”指标规划仿人臂的动作轨迹(《simulationmodellingpracticeandtheory》(2005，13(5)：437-449))，综合人臂总势能和腕部不适度作为优化指标准则解决仿人臂的运动冗余问题(《北京工业大学学报》(2014，40(4)：502-508))等等，但这些方法并没有考虑仿人臂面向飞行物体作业特殊环境下轨迹规划存在的问题要求。

七自由度仿人臂在对飞行物体快速运动过程中，由于仿人臂与机器人本体之间的动力学耦合，仿人臂将对本体(如双足仿人机器人本体或不固定/漂浮的基座)产生较大的反作用。仿人臂对本体的反作用一方面使本体基点的位姿产生扰动，影响仿人臂末端的执行精度，同时也将影响到本体的平衡与稳定性，因此在规划仿人臂动作轨迹时需要考虑如何能减少对本体的反作用。对于对本体的反作用力，仿人臂动作轨迹的加速度信息是影响仿人臂对本体(双足机器人本体或不固定/漂浮的基座)反作用大小的其中一关键因素，角加速度小的动作轨迹将有利于减少仿人臂对本体的反作用，利于机器人本体的平衡性(《自动化学报学报》(2014，40(7)：1328-1338))。另外加速度轨迹的连续性是影响仿人臂动作对本体反作用扰动的另一个重要因素，加速度不连续的动作轨迹将可能引起仿人臂动作过程中系统的隐性振动，从而对机器人本体产生较大扰动。因此规划仿人臂动作轨迹一方面需考虑轨迹角加速度连续性，另一方面需考虑将轨迹加速度指标作为7-dof仿人臂对飞行物体作业时轨迹优选的目标准则。

技术实现要素：

鉴于上述原因，本发明提供一种7-dof仿人臂飞行物体作业最小加速度轨迹优化方法，在满足所规划轨迹角加速度连续的同时，又能使轨迹动作过程的加速度范数最小化。本发明方法可减少仿人臂动作过程中对机器人本体的位姿扰动，减轻对本体的平衡性影响，提高仿人臂的执行精度。

7-dof仿人臂飞行物体作业的任务要求是其末端灵巧手(手心)在作业要求时刻tf1从静态初始点运动至目标作业点位置(记p)，并对物体目标能以要求的笛卡尔速度(记v)进行作业，随后从目标作业点返回至静态初始点。由此可看出，仿人臂飞行物体作业的动作轨迹可分两部分：(1)灵巧手心从静态初始点位置运动至目标作业点(记作业过程)，(2)灵巧手心从目标作业点位置返回至静态初始点(记返回过程)。

本发明提供一种7-dof仿人臂飞行物体作业最小加速度轨迹优化方法，7-dof仿人臂包括肩关节两个自由度、肘关节一个自由度、以及腕关节三个自由度，该优化方法包括如下步骤：

s1.建立7-dof仿人臂飞行物体作业最小加速度轨迹优化问题的数学模型：

对于仿人臂机器人系统r，设其静态初始点的关节位置状态为θ0＝[θ10，θ20，θ30，θ40，θ50，θ60，θ70]^t，目标作业点为θf＝[θ1f，θ2f，θ3f，θ4f，θ5f，θ6f，θ7f]^t；仿人臂tf1作业时刻在目标作业点关节角速度矢量为则仿人臂飞行物体作业的轨迹规划问题可描述为在仿人臂机器人系统机构物理约束范围内所有轨迹中优选一条最优的动作轨迹，使得：

并满足如下边界状态条件与关节轨迹在任意时刻角加速度连续特性：

作业过程：

返回过程：

式中f(θ(·))为反映仿人臂动作轨迹关节角加速度范数最小化的目标函数，θ^*(t)表示所优选的最优规划轨迹，tf1为仿人臂动作轨迹的作业过程时间，tf2为返回过程时间，tf＝tf1+tf2为仿人臂运动过程总时间，与分别表示关节i位置的下限与上限，与分别表示关节i角速度的下限与上限，与分别表示关节i角加速度的下限与上限，该目标准则旨在通过规划连续关节角加速度轨迹且角加速度范数最小方式减少仿人臂运动对本体的反作用扰动；

s2.设置粒子群优化算法运行参数；

s3.运行粒子群优化算法，搜索寻优仿人臂飞行物体作业动作轨迹参数化中冗余参数θr、与其中θr为7-dof仿人臂tf1作业时刻所设定的冗余关节位置，为tf1作业时刻末端灵巧手姿态矩阵中绕z轴转动的偏摆角，为关节i在tf1作业时刻的关节角加速度值；通过最小化目标函数得到仿人臂飞行物体作业动作轨迹参数化中一组最优冗余变量θr、与参数值；

s4.将最优冗余变量值代入仿人臂飞行物体作业动作轨迹参数化中，获得一条关节加速度范数最小的仿人臂动作轨迹，减少仿人臂对飞行物体作业时对本体的耦合反作用。

其中，步骤s2中设置粒子群优化算法的运行参数：最大进化代数g＝300、种群规模p＝50、个体编码长度即优化变量个数l＝9、学习因子c1＝c2＝2，粒子速度更新式惯性权系数ω＝0.5及算法终止准则。

其中，步骤s3包括的步骤为：

s31.根据仿人臂飞行物体作业动作轨迹参数化中的9个冗余变量参数θr、与的搜索范围，随机生成初始种群，定义初始位置xi、初始速度vi、历史最优位置pbesti与全局最优位置gbest；定义仿人臂动作轨迹关节角加速度范数最小为目标函数；

s32.将种群中每一粒子作为一组冗余变量参数代入仿人臂飞行物体作业的轨迹参数化过程中，计算该粒子所对应轨迹的目标函数值；若所对应的轨迹不满足仿人臂机构物理约束，则采用约束处理技术f(θ(·))＝c(c＞0)对目标函数值进行惩罚，其中c为较大正常数值；

s33.通过下式更新粒子速度与位置，并产生下一代种群：

vi＝w·vi+c1*rand()*(pbesti-xi)+c2*rand()*(gbest-xi)

xi＝xi+vi

上式产生的新个体作为冗余变量参数代入仿人臂飞行物体作业动作轨迹参数化中计算该个体所对应轨迹的目标函数值；若新个体目标函数值优于其对应的历史最优位置pbesti的目标函数值，则相应历史最优位置pbesti更新为新个体位置，否则不修改历史最优位置；若新个体目标函数值优于其全局最优位置gbest对应的目标函数值，则全局最优位置gbest更新为新个体位置，否则不修改全局最优位置；

s34.算法进行进化迭代，判断当前进化代数是否达到最大进化代数，若满足则迭代终止并进入步骤s35，否则转向步骤s32；

s35.输出最优结果，所得的最优个体即为仿人臂飞行物体作业轨迹优化模型中待优选的变量参数值，将所优选的变量参数值代入动作轨迹参数化中，获得仿人臂对飞行物体作业的最优动作轨迹。

所述的仿人臂动作轨迹的作业过程采用4次多项式插补静态初始点(零时刻)运动至目标作业点(tf1作业时刻)关节轨迹，其数学方程描述为：

θi(t)＝ai0+ai1t+ai2t²+ai3t³+ai4t⁴t∈[0，tf1]i＝1，2，…，7

式中ai0，ai1，ai2，ai3，ai4为多项式待定的系数，根据作业过程的边界条件：

得作业过程4次多项式待定系数ai0，ai1，ai2，ai3，ai4为：

同样，所述的仿人臂返回过程采用4次多项式插补目标作业点(tf1作业时刻)返回至静态初始点(tf1+tf2时刻)的关节轨迹，其数学方程可描述为：

θi(t)＝bi0+bi1(t-tf1)+bi2(t-tf1)²+bi3(t-tf1)³+bi4(t-tf1)⁴t∈[tf1，tf1+tf2]i＝1，2，…，7

式中bi0，bi1，bi2，bi3，bi4为多项式待定的系数，根据返回过程的边界条件：

得返回过程4次多项式待定系数bi0，bi1，bi2，bi3，bi4为：

本发明根据7-dof仿人臂末端灵巧手要求的目标作业点位置(记p)，对目标物体作业要求的笛卡尔速度(记v)及作业时刻tf1规划仿人臂飞行物体作业的最优轨迹。本发明所优选的仿人臂动作轨迹具有关节角加速度连续特性，且能使动作过程的关节加速度范数最小，可减少仿人臂动作过程中对机器人本体的位姿扰动，减轻对本体平衡性影响，提高仿人臂的执行精度。

附图说明

图1所示为本发明中仿人臂关节结构模型图；

图2所示为本发明中仿人臂飞行物体作业最小加速度轨迹优化流程图；

图3所示为采用本发明中仿人臂各关节位置轨迹曲线；

图4所示为采用本发明中仿人臂各关节速度轨迹曲线；

图5所示为采用本发明中仿人臂各关节角加速度轨迹曲线。

具体实施方式

下面将结合说明书附图对本发明作进一步说明：

如图1所示，本发明提供一种7-dof仿人臂关节结构模型，其肩部3有三个自由度、肘部2一个自由度、腕部1有三个自由度，其中肩部3对应肩关节轴心，肘部2对应肘关节轴心，腕部1对应腕关节轴心，仿人臂的末端连接有机械手，图中∑w表示世界坐标系，ai(i＝1，2，…，7)表示关节轴矢量，即7个关节轴矢量a1-a7为：

另关节模型中肩宽长度为d，肩至肘部长度为l1，肘至腕部长度为l2，腕部至手心距离为l3。

本发明针对仿人臂飞行物体作业动作过程中对本体产生较大的反作用，使机器人本体(双足仿人机器人本体或不固定/漂浮的基座)位姿产生扰动，影响仿人臂自身末端执行精度问题，提出一种7-dof仿人臂飞行物体作业最小加速度的轨迹优化方法；通过规划关节角加速度连续且角加速度范数最小的动作轨迹，减少仿人臂动作过程中对机器人本体反作用与位姿扰动，提高仿人臂的执行精度。

本发明提供一种仿人臂飞行物体作业最小加速度轨迹优化方法方法，包括如下步骤：

s1.建立7-dof仿人臂飞行物体作业最小加速度轨迹规划问题的数学模型：

对于仿人臂机器人系统r，设其静态初始点的关节位置状态为θ0＝[θ10，θ20，θ30，θ40，θ50，θ60，θ70]^t，目标作业点为θf＝[θ1f，θ2f，θ3f，θ4f，θ5f，θ6f，θ7f]^t；仿人臂tf1作业时刻在目标作业点关节角速度矢量为则仿人臂飞行物体作业的轨迹规划问题可描述为在仿人臂机器人系统机构物理约束范围内所有轨迹中优选一条最优的动作轨迹，使得：

并满足如下边界状态条件与关节轨迹在任意时刻角加速度连续特性：

作业过程：

返回过程：

式中f(θ(·))为反映仿人臂动作轨迹关节角加速度范数最小化的目标函数，θ^*(t)表示所优选的最优规划轨迹，tf1为仿人臂动作轨迹的作业过程时间，tf2为返回过程时间，tf＝tf1+tf2为仿人臂运动过程总时间，与分别表示关节i位置的下限与上限，与分别表示关节i角速度的下限与上限，与分别表示关节i角加速度的下限与上限；该目标准则旨在通过规划连续关节角加速度轨迹且角加速度范数最小方式减少仿人臂运动对本体的反作用扰动；

s2.设置粒子群优化算法运行参数；

s3.运行粒子群优化算法，搜索寻优仿人臂飞行物体作业动作轨迹参数化中冗余参数θr、与其中θr为7-dof仿人臂tf1作业时刻所设定的冗余关节位置，为末端灵巧手姿态矩阵中绕z轴转动的偏摆角，为关节i在tf1作业时刻的关节角加速度值；通过最小化目标函数得到仿人臂飞行物体作业动作轨迹参数化中的一组最优冗余变量θr、与参数值；

s4.将最优冗余变量值代入仿人臂飞行物体作业动作轨迹参数化中，获得一条关节加速度范数最小的仿人臂动作轨迹，减少仿人臂对飞行物体作业时对本体的耦合反作用。

其中，步骤s2中设置粒子群优化算法的运行参数：最大进化代数g＝300、种群规模p＝50、个体编码长度即优化变量个数l＝9、学习因子c1＝c2＝2，粒子速度更新式惯性权系数ω＝0.5及算法终止准则。

具体地，步骤s3中所述的运行粒子群优化算法，搜索寻优仿人臂飞行物体作业动作轨迹参数化中冗余参数θr、与通过最小化目标函数得到仿人臂飞行物体作业动作轨迹参数化中一组未知参数θr、与的最优变量参数值的步骤为：

s31.根据仿人臂飞行物体作业动作轨迹参数化中的9个冗余变量参数θr、与的搜索范围，随机生成初始种群，定义初始位置xi、初始速度vi、历史最优位置pbesti与全局最优位置gbest；定义仿人臂动作轨迹关节角加速度范数最小为目标函数；

s32.将种群中每一粒子作为一组冗余变量参数代入仿人臂飞行物体作业的轨迹参数化过程中，计算该粒子所对应轨迹的目标函数值；若所对应的轨迹不满足仿人臂机构物理约束，则采用约束处理技术f(θ(·))＝c(c＞0)对目标函数值进行惩罚，其中c为较大正常数值；

s33.通过下式更新粒子速度与位置，并产生下一代种群：

vi＝w·vi+c1*rand()*(pbesti-xi)+c2*rand()*(gbest-xi)

xi＝xi+vi

上式产生的新个体作为冗余变量参数代入仿人臂飞行物体作业动作轨迹参数化中计算该个体所对应轨迹的目标函数值；若新个体目标函数值优于其对应的历史最优位置pbesti的目标函数值，则相应历史最优位置pbesti更新为新个体位置，否则不修改历史最优位置；若新个体目标函数值优于其全局最优位置gbest对应的目标函数值，则全局最优位置gbest更新为新个体位置，否则不修改全局最优位置；

s34.算法进行进化迭代，判断当前进化代数是否达到最大进化代数，若满足则迭代终止并进步步骤s35，否则转向步骤s32；

s35.输出最优结果，所得的最优个体即为仿人臂飞行物体作业轨迹优化模型中待优选的变量参数值，将所优选的变量参数代入动作轨迹参数化中，获得仿人臂对飞行物体作业的最优动作轨迹。

为满足仿人臂对飞行物体作业动作过程中关节角加速度轨迹具有连续特性，所述的仿人臂动作轨迹的作业过程(从静态初始点位置运动至目标作业点)将采用4次多项式插补关节轨迹，其数学方程可描述为：

θi(t)＝ai0+ai1t+ai2t²+ai3t³+ai4t⁴t∈[0，tf1]i＝1，2，…，7

式中ai0，ai1，ai2，ai3，ai4为多项式待定的系数，根据作业过程的边界条件：

得作业过程4次多项式待定系数ai0，ai1，ai2，ai3，ai4为：

所述的仿人臂返回过程也采用4次多项式插补目标作业点(tf1作业时刻)返回至静态初始点(tf1+tf2时刻)的关节轨迹，其数学方程可描述为：

θi(t)＝bi0+bi1(t-tf1)+bi2(t-tf1)²+bi3(t-tf1)³+bi4(t-tf1)⁴t∈[tf1，tf1+tf2]i＝1，2，…，7

式中bi0，bi1，bi2，bi3，bi4为多项式待定的系数，根据返回过程的边界条件：

得返回过程4次多项式待定系数bi0，bi1，bi2，bi3，bi4为：

所述的仿人臂飞行物体作业的轨迹参数化优选过程可参考《自动化学报》(2015，41(6)：1131-1144)，根据七自由度仿人臂飞行物体作业目标要求，将其轨迹规划问题转化为动作轨迹的参数化优选问题，其中7-dof仿人臂轨迹参数化优选中设定tf1作业时刻第五关节位置θ5为冗余关节变量，即θr＝θ5，按照《自动化学报》(2015，41(6)：1131-1144)中仿人臂对飞行物体作业轨迹规划优选策略优选仿人臂动作轨迹，其中仿人臂动作轨迹的作业过程与返回过程采用4次多项式插补关节轨迹，保证仿人臂关节角加速度轨迹曲线的连续性。

实施例：本发明的效果可以通过以下实验仿真进一步说明：

根据如图1所示所设计的七自由度仿人臂关节结构模型，下表1至表3为该仿人臂各关节角度、速度与加速度约束范围。

表17自由度冗余灵巧臂各关节角度范围(度)

表27自由度冗余灵巧臂各关节最大速度值(弧度/秒)

表37自由度冗余灵巧臂各关节最大加速度值(弧度/秒²)

设仿人臂肩宽d＝0.14m，上臂肩至肘部长度l1＝0.26m，下臂肘至腕部距离l2＝0.25m，腕部至手心距离l3＝0.14m；设定肩部为世界坐标系原点，仿人臂颈部位姿分别为p0＝(0.000.140.00)^tm，r0＝e(单位阵)，仿人臂静态初始点关节位置设置为θ0＝[0.0200，-0.5909，-0.8551，1.5930，1.5708，0.8441，-0.5763]，若仿人臂对飞行物体tf1作业时刻的手心位置与笛卡尔速度分别为：

根据要求的手心位置、笛卡尔速度矢量信息与作业时刻，设定仿人臂作业时刻机械手速度方向与其手背面垂直，按照《自动化学报》(2015，41(6)：1131-1144)中仿人臂对飞行物体作业轨迹规划优选策略，将其轨迹规划问题转化为动作轨迹的参数化优选问题，其中轨迹参数化优选中待优化的变量参数为θr、与共九个，其中θr为7-dof仿人臂tf1作业时刻所设定的冗余关节即第五关节位置θr＝θ5，为末端球拍姿态矩阵中绕z轴转动的偏摆角，为关节i在tf1作业时刻的关节角加速度值。

下面将通过代入数据的具体实施例对本发明进行进一步解释：

s1.如下式定义的最小化目标函数：

并满足边界状态条件与关节角加速度轨迹曲线具有连续特性：

作业过程：

返回过程：

式中f(θ(·))为优选仿人臂动作轨迹的目标函数，θ^*(t)表示所优选的最优规划轨迹，作业过程时间tf1＝0.3秒，返回过程时间tf2＝0.5秒，动作过程总时间为0.8秒。此优化指标作为所述粒子群算法对仿人臂飞行物体作业最小加速度轨迹优化中搜索冗余变量参数θr(θ5)、与目标函数；

s2.置粒子群优化算法的参数：最大进化代数g＝300、种群规模p＝50、个体编码长度即优化变量个数l＝9、学习因子c1＝c2＝2，粒子速度更新式惯性权系数ω＝0.5及算法运行达到最大进化代数g时迭代终止准则；

s3.运行粒子群优化算法，搜索寻优仿人臂飞行物体作业动作轨迹参数化中冗余参数θr(θ5)、与通过最小化目标函数得到仿人臂飞行物体作业动作轨迹参数化中一组最优冗余变量参数值；

s4.将最优冗余变量值代入仿人臂飞行物体作业动作轨迹参数化中，获得一条动作过程中关节角加速度范数最小的仿人臂轨迹。

步骤s3中设置粒子群优化算法运行参数：最大进化代数g＝300、种群规模p＝50、个体编码长度即优化变量个数l＝9、学习因子c1＝c2＝2，粒子速度更新式惯性权系数ω＝0.5。

步骤s3中粒子群优化对仿人臂飞行物体作业动作轨迹参数化中待优化变量参数θr(θ5)、与进行搜索寻优的步骤为：

s31.根据表1冗余关节变量搜索区间θr(θ5)∈[-π，π]，球拍姿态矩阵中绕z轴转动的偏摆角搜索区间根据表3中tf1作业时刻各关节加速度搜索区间和并对个体采用实数编码方式随机产生初始种群；

s32.将种群中每一个体作为相应冗余变量参数代入仿人臂飞行物体作业的轨迹参数化优选过程中，动作轨迹参数化优选过程具体参考《自动化学报》(2015，41(6)：1131-1144)，仿人臂动作轨迹的作业过程与返回过程关节轨迹按上述采用4次多项式在关节空间内插补规划，计算相应的目标函数值；若所对应的轨迹不满足仿人臂机构物理约束，则采用约束处理技术f(θ(·))＝10⁶对目标函数值进行惩罚；

s33.更新粒子速度与位置，并产生下一代种群：

vi＝w·vi+c1*rand()*(pbesti-xi)+c2*rand()*(gbest-xi)

xi＝xi+vi

产生的新个体作为冗余变量参数代入仿人臂飞行物体作业动作轨迹参数化中计算该个体所对应轨迹的目标函数值；若新个体目标函数值优于其对应的历史最优位置pbesti的目标函数值，则相应历史最优位置pbesti更新为新个体位置，否则不修改历史最优位置；若新个体目标函数值优于其全局最优位置gbest对应的目标函数值，则全局最优位置gbest更新为新个体位置，否则不修改全局最优位置；

s34.算法进行进化迭代，判断当前进化代数是否达到最大进化代数，若满足则迭代终止并进入步骤s35，否则转向步骤s32；

s35.输出最优结果，所得的最优个体即为待优选的θr(θ5)、与最优变量参数值，将其代入仿人臂飞行物体作业动作轨迹参数化中，获得仿人臂对飞行物体作业的最优动作轨迹。

通过pso粒子群优化方法迭代搜索，得仿人臂轨迹参数化模型中各冗余变量的最优参数，即对应的最优个体为

对应的目标函数值f(θ(·))＝29.8209。根据该目标函数值，可看出所优选个体对应的仿人臂关节轨迹满足所有物理约束条件。图3～图5分别为所优选的仿人臂最优轨迹各关节位置、速度与角加速度的轨迹曲线图，从图中可看出，仿人臂的关节位置轨迹满足连续、可导特性，且各关节的位置、速度与角加速度轨迹均在各自允许的约束范围之内；另外各关节的角加速度轨迹曲线处处连续，不存在非连续点，避免了仿人臂动作过程中因不连续的角加速度轨迹引起的系统隐性振动；再则本发明以仿人臂动作过程的角加速度范数为优化目标，优选生成具有最小角加速度范数动作轨迹，这在一定程度上可减少仿人臂动作对机器人本体的反作用及位姿扰动影响，利于机器人本体的平衡，提高仿人臂末端的执行精度。

综上所述，本发明与《自动化学报》(2015，41(6)：1131-1144)中给出的仿人臂飞行物体作业轨迹优选方法比较，虽然本发明提出方法待优选的变量参数多，但该方法可保证在仿人臂整个动作过程中各关节角加速度曲线在任意时刻都保持连续特性，这可避免仿人臂动作过程中因角加速度不连续而引起的系统振动；而《自动化学报》(2015，41(6)：1131-1144)中方法所规划的轨迹不能满足关节加速度处处连续特性(关节加速度轨迹在tf1＝0.3秒作业时刻不连续)，将会引起系统的振动，尤其在仿人臂快速作业环境下，系统振动将会给机器人带来较大危害。

以上所述的仅是本发明的优选实施方式，应当指出，对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。