[发明专利]一种移动作业机器人定时长可达工作空间的求解方法

权利要求书

1.一种移动作业机器人定时长可达工作空间的求解方法，其特征在于，步骤如下：

(1)六自由度机械臂运动学模型建立

采用D-H法建立机械臂关节坐标系，用一个4×4的齐次变换矩阵描述相邻两连杆的空间关系；选取连杆n的末端关节轴线为坐标系{n}的z_n轴，坐标系{n-1}的z_n-1轴与坐标系{n}的z_n轴的公法线方向定义为x_n的方向，y_n的方向根据右手法确定；

建立以上机械臂关节坐标系，还需要用于描述各连杆以及两相邻连杆之间的4个参数：连杆长度a_n，即沿着x_n从z_n-1到z_n之间的距离；连杆扭角α_n，即绕着x_n从z_n-1到z_n之间的角度；关节偏置距离d_n，即沿着z_n-1从x_n-1到x_n之间的距离；关节转角θ_n，即绕着z_n-1从x_n-1到x_n之间的角度；

沿z_n-1轴的位姿变换用公式(1)来描述：

沿x_n轴的位姿变换用公式(2)来描述：

将公式(1)和公式(2)两个位姿变换矩阵进行叠加即可以得到完整的位姿坐标转换矩阵，如公式(3)所示：

基于机械臂的运动学正解来对机械臂末端执行器的位姿进行求解；根据D-H 法以及机械臂相邻关节之间的位姿变换矩阵公式(3)，得到机械臂的末端执行器的坐标系相对于基座坐标系的位姿变换矩阵，如公式(4)所示：

其中，表示基座与连杆6之间的位姿变换矩阵，T₁⁰表示基座与连杆1之间的位姿变换矩阵，表示连杆1与连杆2之间的位姿变换矩阵，表示连杆2与连杆3之间的位姿变换矩阵，表示连杆3与连杆4之间的位姿变换矩阵，表示连杆4与连杆5之间的位姿变换矩阵，表示连杆5与连杆6之间的位姿变换矩阵，的前三列[r₁₁ r₂₁ r₃₁]^T，[r₁₂ r₂₂ r₃₂]^T，[r₁₃ r₂₃ r₃₃]^T为末端执行器上参考点相对于基座坐标系的姿态向量，第四列为在三维空间中机械臂末端执行器上参考点相对于基座坐标系的位置向量，c_x、c_y、c_z分别代表末端执行器在空间中的x、y、z坐标；

(2)机械臂定时长工作空间求解

使用蒙特卡洛法求解工作空间：各关节在允许取值的范围内遍历取值，得到末端点的集合也就是工作空间，具体的步骤如下：

1)根据公式(4)的正向运动学模型获取机械臂的末端执行器相对于基座坐标系的位置向量；

2)根据移动作业机器人的固定运动时长T，得到机械臂各个关节运动角度的取值范围，θ_nmin为第n个关节角度取值范围的下限，θ_nmax为第n个关节角度取值范围的上限；

3)基于在(0,1)上的均匀分布概率模型来产生0～1之间的随机步长，接着使用随机步长来产生每个关节在运动角度范围内的随机值如公式(5)所示；

4)将上述步骤中产生的关节运动角度随机值带入获得的位置向量中，得到末端执行器在基座坐标系下的位置坐标；

5)循环N次步骤1)至步骤5)，得到N个机械臂末端执行器的空间三维坐标点；

6)通过描点的方式将所得到的N个三维坐标点显示出来，便可得到六自由度机械臂末端执行器相对于机械臂基座坐标系的可达空间点云图；

(3)移动底盘定时长可达工作空间求解

移动作业机器人的移动底盘运动学模型为两轮差速驱动模型，理想情况下，移动底盘的运动轨迹是一段一段的圆弧或者直线，一对(v,ω)就代表了一个圆弧轨迹，其中v和ω分别为移动底盘的线速度和角速度；用x(t)和y(t)表示在t时刻移动底盘在全局坐标系下的坐标，用θ(t)表示移动底盘方位角；则三元组x(t),y(t),θ(t)表示移动底盘在t时刻的位姿；

移动底盘的线速度只有在移动底盘沿着前后方向运动时才会产生，这是两轮差速驱动模型的约束；用x(t₀)和x(t_n)分别表示在t₀和t_n时刻移动底盘的x坐标，y(t₀)和y(t_n)同理，v(t)和ω(t)分别表示移动底盘在t时刻的线速度和角速度，则x(t_n)和y(t_n)则可以分别用公式(6)和公式(7)来表示；

设移动底盘的初始线速度为v或者-v，初始角速度ω的取值范围为ω_min～ω_max，角速度与线速度在运动时间T内保持恒定；移动底盘在t₀时刻的位置设为位于全局坐标系的原点，t_n＝t₀+T，为了减少计算资源，采用栅格法对移动底盘在恒定时间T内的运动轨迹进行求解，具体步骤如下：

1)将运动时间T分为m份，在第i个时间间隔(t₀+T/m·(i-1),t₀+T/m·i)内移动底盘的运动可以简化为先旋转再平移；

2)在第i个时间间隔内移动底盘旋转的角度deg可以由公式(8)来表示：

deg＝T/m·ω (8)

根据公式(9)可以得到移动底盘旋转对应的位姿变换矩阵T_r：

3)在第i个间隔内移动底盘平移的距离dis可以由公式(10)来表示：

dis＝T/m·v (10)

根据公式(11)可以得到移动底盘平移对应的位姿变换矩阵T_t：

4)设移动底盘在(t₀+T/m·(i-1))时刻的位姿矩阵为T_old，则移动底盘在(t₀+T/m·i)时刻的位姿矩阵T_new可以根据公式(12)得到：

T_new＝T_t·T_r·T_old (12)

5)设移动底盘的初始角速度ω的取值范围为ω_min～ω_max，每次增加的步长为(ω_max-ω_min)/k，将移动底盘的初始线速度v和-v、初始角速度ω、t₀时刻的位姿矩阵带入公式(12)中，经过m次迭代即可得到移动底盘在固定运动时长T内的2·m·k个位姿矩阵；

6)通过描点的方式将所得到的2·m·k个位姿矩阵对应的三维坐标点显示出来，便可得到移动底盘相对于全局坐标系的可达空间点云图；

(4)移动底盘与机械臂联合运动的定时长可达工作空间求解

设机械臂基座坐标系与移动底盘坐标系为同一坐标系，将公式(4)中的机械臂末端执行器相对于基座坐标系的位姿矩阵带入公式(12)，即可得到机械臂末端执行器相对于全局坐标系的位姿矩阵；通过描点的方式将所得到的位姿矩阵对应的三维坐标点显示出来，便可得到移动作业机器人定时长可达工作空间点云图。