[发明专利]一种基于二阶锥规划的移动作业机器人的轨迹生成方法

权利要求书

1.一种基于二阶锥规划的移动作业机器人的轨迹生成方法，其特征在于：包含以下步骤：

步骤一：给定世界坐标系，给定移动作业机器人的初始位置以及待抓取的目标物体在世界坐标系中的位置，用RRT-Connect算法规划出一条路径使机器人从初始位置出发，绕过中途所有的障碍物，移动到待抓取目标附近，并且抓取目标物体；

步骤二：利用移动作业机器人整体雅可比矩阵，结合拉格朗日方程，推导移动作业机器人整体的动力学方程；并构造一个目标函数使得机器人轨迹满足如下要求：能够以最快速度达到目的地，即时间消耗最小，在此过程中能耗最小且力矩变化尽可能小；针对已经得到的移动作业机器人机械臂N个关节以及底盘的广义路径，首先，推导了动力学方程，

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

b_ij是B矩阵的第i行第j列的元素，其余类推；z_i是机械臂第i个连杆的质心在竖直方向的高度；m_i表示机械臂第i连杆的质量；ξ_k为上文广义坐标ξ的第k个分量；

接下来构造一个目标函数使得机器人轨迹满足如下要求：能够以最快速度达到目的地，即时间消耗最小，在此过程中能耗最小且力矩变化尽可能小；基于上述要求，根据能耗正比于电流的平方，而电流正比于机器人第i个电机的力矩τ_i，即i＝N，或者底盘的转矩即i＝N+1或者底盘在x方向的驱动力即i＝N+2或者底盘在y方向的驱动力即i＝N+3，因此目标函数构造如下：

其中，γ₁，γ₂是常数系数，是人为给定的第i个自由度电机的力矩上限值，是第i个自由度电机的力矩随时间的变化率；t为运动总时间，是可以优化的变量；

步骤三：建立包括底盘速度约束、机械臂各个关节角速度约束、各电机力矩约束、各电机力矩变化率约束在内的约束条件；并且利用伪轨迹对目标函数和各个约束条件重新表示，使多变量优化问题变成单变量优化问题；

步骤四：对规划问题的形式进行离散化，使之转化为一个标准的离散二阶锥规划形式；调用Matlab中的Sedumi凸优化包对这个离散化的形式进行优化求解，得到一条伪轨迹；

步骤五：将上述伪轨迹代回到各关节角速度、角加速度、底盘线速度、加速度以及力矩的表达式之中，求解出各个自由度的速度、角速度、加速度、角加速度、力以及力矩；既已得到路径，又得到了各路径点上的各个自由度的速度，则移动作业机器人的轨迹就规划完成。