一种基于二阶锥规划的移动作业机器人的轨迹生成方法技术

本发明专利技术公开了一种基于二阶锥规划的移动作业机器人的轨迹生成方法，该方法将移动作业机器人的移动抓取物体规划过程分解成路径规划过程以及在此基础上规划合适轨迹的过程。首先，用现有的RRT‑Connect方法规划出移动作业机器人的作业路径；然后，利用移动作业机器人整体雅可比矩阵，结合拉格朗日方程，推导移动作业机器人整体的动力学方程；接着，把移动作业机器人的多维轨迹规划转化成一维伪轨迹规划的问题，再将该问题转化为二阶锥规划的标准形式；最后，利用matlab的Sedumi二阶锥规划包完成规划。本发明专利技术所提出的方法能够为给定路径实时提出有动力学约束下的时间最短轨迹或者兼顾时间最短以及能量消耗最小的轨迹。

【技术实现步骤摘要】
一种基于二阶锥规划的移动作业机器人的轨迹生成方法
本专利技术涉及一种基于二阶锥规划的移动作业机器人的轨迹生成方法，属于机器人

技术介绍
移动作业机器人将移动底盘与操作臂相结合，可有效扩大机器人的作业范围，提升机器人的灵活度。目前已被广泛应用于服务机器人、新型工业机器人以及军事航天机器人之中。移动作业机器人根据移动底盘的不同可以分为差分底盘移动作业机器人、全向轮底盘移动作业机器人以及悬浮底盘移动作业机器人。前两种机器人已被普遍用于地面服务机器人以及工业机器人之中，而第三种机器人则包括了水下移动作业机器人、太空作业机器人以及带作业臂的无人机等。当移动作业机器人用于移动抓取物体或者移动长距离操作机械臂执行某项任务等的任务时，由于移动的距离长，周遭环境障碍物复杂，因而以某种指标函数(比如时间、能量等)最优化为目的的轨迹规划也成为一个重要的难点问题。一般而言，机器人轨迹规划的模式基本可分为点到点的轨迹规划和预定义路径的轨迹规划。基于运动学的轨迹规划一般是面向点到点的轨迹规划问题的。譬如不考虑避障的情况下，机械臂从起始位形本文档来自技高网...

【技术保护点】
1.一种基于二阶锥规划的移动作业机器人的轨迹生成方法，其特征在于：包含以下步骤：/n步骤一：给定世界坐标系，给定移动作业机器人的初始位置以及待抓取的目标物体在世界坐标系中的位置，用RRT-Connect算法规划出一条路径使机器人从初始位置出发，绕过中途所有的障碍物，移动到待抓取目标附近，并且抓取目标物体。/n步骤二：利用移动作业机器人整体雅可比矩阵，结合拉格朗日方程，推导移动作业机器人整体的动力学方程。并构造一个目标函数使得机器人轨迹满足如下要求：能够以最快速度达到目的地，即时间消耗最小，在此过程中能耗最小且力矩变化尽可能小。针对已经得到的移动作业机器人机械臂N个关节以及底盘的广义路径，首先，...

【技术特征摘要】
1.一种基于二阶锥规划的移动作业机器人的轨迹生成方法，其特征在于：包含以下步骤：
步骤一：给定世界坐标系，给定移动作业机器人的初始位置以及待抓取的目标物体在世界坐标系中的位置，用RRT-Connect算法规划出一条路径使机器人从初始位置出发，绕过中途所有的障碍物，移动到待抓取目标附近，并且抓取目标物体。
步骤二：利用移动作业机器人整体雅可比矩阵，结合拉格朗日方程，推导移动作业机器人整体的动力学方程。并构造一个目标函数使得机器人轨迹满足如下要求：能够以最快速度达到目的地，即时间消耗最小，在此过程中能耗最小且力矩变化尽可能小。针对已经得到的移动作业机器人机械臂N个关节以及底盘的广义路径，首先，推导了动力学方程，



其中，ξ为机器人机械臂的N关节以及底盘的x，y，θ三个自由度构成的广义坐标。C为科里奥利力与惯性力矩阵，把机械臂的各关节角速度，底盘角速度以及底盘在x和y方向的速度合成为一个广义速度θT是机械臂的N个关节矢量，α是底盘在世界坐标系的角度，xT是底盘在世界坐标系下的位置。把机械臂N个关节的力矩τ1，τ2，...，τN，底盘的力矩τα，以及底盘在世界坐标系的x方向的力和y方向的力Fx，Fy合成为一个广义力矢量τ＝[τ1，τ2，...，τN，τα，Fx，Fy]T，是ξ对时间的二阶导数，假设B矩阵的元素为bij，其他的项：






bij是B矩阵的第i行第j列的元素，其余类推。zi是机械臂第i个连杆的质心在竖直方向的高度。ξk为上文广义坐标ξ的第k个分量。
接下来构造一个目标函数使得机器人轨迹满足如下要求：能够以最快速度达到目的地，即时间消耗最小，在此过程中能耗最小且力矩变化尽可能小。基于上述要求，根据能耗正比于电流的平方，而电流正比于机器人第i个电机的力矩τi，即i＜＝N，或者底盘的转矩即i＝N+1或者底盘在x方向的驱动力即i＝N+2或者底盘在y方向的驱动力即i＝N+3，因此目标函数构造如下：



其中，γ1，γ2是常数系数，是人为给定的第i个自由度电机的力矩上限值，是第i个自由度电机的力矩随时间的变化率。t为运动总时间，是可以优化的变量。
步骤三：建立包括底盘速度约束、机械臂各个关节角速度约束、各电机力矩约束、各电机力矩变化率约束在内的约束条件。并且利用伪轨迹对目标函数和各个约束条件重新表示，使多变量优化问题变成单变量优化问题。
步骤四：对规划问题的形式进行离散化，使之转化为一个标准的离散二阶锥规划形式。调用Matlab中的Sedumi凸优化包对这个离散化的形式进行优化求解，得到一条伪轨迹。
步骤五：将上述伪轨迹代回到各关节角速度、角加速度、底盘线速度、加速度以及力矩的表达式之中，求解出各个自由度的速度、角速度、加速度、角加速度、力以及力矩。既已得到路径，又得到了各路径点上的各个自由度的速度，则移动作业机器人的轨迹就规划完成。


2.如权利要求1所述的基于二阶锥规划的移动作业机器人的轨迹生成方法，其特征在于：步骤二的实现方法中，矩阵B的求解方法如下：
首先，全向轮移动作业机器人机械臂上任意一点的整体雅可比矩阵的表达式为：



它可以重写为：



其中，R为底盘相对于世界坐标系的3×3的旋转矩阵，t＝[T1，T2，0]T，t′＝[0，0，1]T。其中，T1＝-(x+a)sinα-(y+b)cosα，T2＝(x+a)cosα-(y+b)sinα。Jari＝[p1p2…pi0…0]，其中x是机械臂上的点在机械臂基坐标系下的表达，a、b是机械臂基坐标相对世界坐标的偏移，pi是第i个关节轴单位向量在机械臂基坐标系下的表达式。
然后，根据柯尼...

【专利技术属性】
技术研发人员：李玮，沈方岩，顾建军，朱世强，
申请(专利权)人：之江实验室，
类型：发明
国别省市：浙江;33