本发明属于移动作业机器人自主物体检测、抓取以及人机协作技术领域，涉及到移动底盘与机械臂的联合运动规划方法，尤其是移动作业机器人在固定时长内可达工作空间求解方法。
背景技术：
移动作业机器人是在移动底盘上加装一个六自由度工业机械臂以及智能传感器所构成的具有主动作业能力的新型智能移动机器人系统，它具有环境感知、自主决策以及运动控制等一系列功能，可以在室内外大范围场景下自主移动与作业。相对于传统的固定基座机械臂，移动作业机器人有很多优势。加装在移动底盘上的机械臂具有更大的工作空间，可以在更加广阔的范围内进行作业；机械臂还可以清除障碍以及提供辅助支撑，增强了移动底盘的地形通过能力。移动作业机器人的上述优点使得其在物流仓储、工业生产、抗震救灾等领域具有广阔的应用场景。
在移动作业机器人的路径规划、避障和运动控制中，工作空间的求解始终是一个需要考虑的重要问题，它是衡量移动作业机器人工作能力的一个重要指标。移动作业机器人的工作空间是指机械臂末端执行器所能达到的位置点的集合。每个移动作业机器人的工作空间与它自己的结构形式、约束条件等息息相关。国内外学者对此已经进行了很多的研究工作，主要致力于机械臂工作空间分析，并取得了一定的成果。
文献(胡朝晖,郑玉航,羊帆.六自由度机械臂建模与工作空间分析[j].传感器与微系统,2020,39(01):35-37.)提出一种应用于危险环境下的六自由度机械臂工作空间求解方法。该方法根据denavit-hartenberg多体运动学建模方法，分析建立机械臂各关节坐标系并确定连杆参数，通过齐次变换推导末端执行器的姿态和位置。但是，该方法仅仅考虑了机械臂的完整工作空间，没有基于固定运动时间对可达工作空间进行进一步研究。此外，该方法仅仅适用于单独的机械臂工作空间求解，并不适用于加装了机械臂的移动机器人。
文献(战茜,屠大维.移动机器人自主抓取作业[j].计算机应用,2016,36(s1):95-98 102.)提出了一种移动作业机器人自主作业研究方法。该方法分别为机械臂和移动底盘建立了运动学模型，可以有效地使机器人在不同环境下，自主完成对于目标物体的抓取作业。但是，该方法没有为移动作业机器人建立一个完整的运动学模型。此外，该方法没有考虑移动作业机器人的运动时间，无法实现最短时间作业。
技术实现要素：
为了解决现有机器人工作空间求解方法的缺陷与不足，本发明提出了一种移动作业机器人定时长可达工作空间求解方法。该方法首先确定了移动作业机器人的运动时长，为后续有关移动作业机器人最短时间作业的研究做准备。然后分别为六自由度机械臂和移动底盘建立运动学模型。最后基于移动作业机器人的运动时长、机械臂末端执行器的可达工作空间与各个关节之间的转换关系以及移动底盘的运动学模型，采用蒙特卡洛法对末端执行器的工作区域进行分析，得到机器人定时长可达工作空间的点云图。
本发明的技术方案：
一种移动作业机器人定时长可达工作空间的求解方法，步骤如下：
(1)六自由度机械臂运动学模型建立
机械臂可以看做是由一系列刚体(称为连杆)通过转动关节串联形成开式运动链。连杆的一端固定在基座上，另一端安装工具完成各种作业。本发明采用d-h法建立机械臂关节坐标系，其主要思想是用一个4×4的齐次变换矩阵描述相邻两连杆的空间关系。如图1所示，选取连杆n的末端关节轴线为坐标系{n}的zn轴，坐标系{n-1}的zn-1轴与坐标系{n}的zn轴的公法线方向定义为xn的方向，yn的方向根据右手法确定。
建立以上机械臂关节坐标系，还需要用于描述各连杆以及两相邻连杆之间的4个参数：连杆长度an(沿着xn从zn-1到zn之间的距离)；连杆扭角αn(绕着xn从zn-1到zn之间的角度)；关节偏置距离dn(沿着zn-1从xn-1到xn之间的距离)；关节转角θn(绕着zn-1从xn-1到xn之间的角度)。
沿zn-1轴的位姿变换可以用公式(1)来描述：
沿xn轴的位姿变换可以用公式(2)来描述：
将公式(1)和公式(2)两个位姿变换矩阵进行叠加即可以得到完整的位姿坐标转换矩阵，如公式(3)所示：
基于机械臂的运动学正解来对机械臂末端执行器的位姿进行求解。根据d-h法以及机械臂相邻关节之间的位姿变换矩阵公式(3)，可以得到机械臂的末端执行器的坐标系相对于基座坐标系的位姿变换矩阵，如公式(4)所示：
其中，
(2)机械臂定时长工作空间求解
机械臂末端上参考点在固定时长内可以到达的所有点的集合构成了机械臂定时长可达工作空间，本发明选取蒙特卡洛(montecarlo)法对机械臂的工作空间进行描述，该方法是一种通过随机变量的统计实验、随机模拟来求解数学和工程技术问题近似解的数值方法，也称为模拟统计法。蒙特卡洛法简单易行，对于机械臂的关节类型没有限制，误差与自由度无关，计算量与自由度成比例。使用蒙特卡洛法求解工作空间的思路是：各关节在允许取值的范围内遍历取值，得到末端点的集合也就是工作空间，具体的步骤如下：
1)根据公式(4)的正向运动学模型获取机械臂的末端执行器相对于基座坐标系的位置向量；
2)根据移动作业机器人的固定运动时长t，得到机械臂各个关节运动角度的取值范围，θnmin为第n个关节角度取值范围的下限，θnmax为第n个关节角度取值范围的上限；
3)基于在(0,1)上的均匀分布概率模型来产生0～1之间的随机步长，接着使用随机步长来产生每个关节在运动角度范围内的随机值
4)将上述步骤中产生的关节运动角度随机值带入获得的位置向量中，得到末端执行器在基座坐标系下的位置坐标；
5)循环n次步骤1)至步骤5)，得到n个机械臂末端执行器的空间三维坐标点；
6)通过描点的方式将所得到的n个三维坐标点显示出来，便可得到六自由度机械臂末端执行器相对于机械臂基座坐标系的可达空间点云图，如图2所示。
(3)移动底盘定时长可达工作空间求解
本发明的移动作业机器人的移动底盘运动学模型为两轮差速驱动模型，理想情况下移动底盘的运动轨迹是一段一段的圆弧或者直线，一对(v,ω)就代表了一个圆弧轨迹，其中v和ω分别为移动底盘的线速度和角速度。用x(t)和y(t)表示在t时刻移动底盘在全局坐标系下的坐标，用θ(t)表示移动底盘方位角，如图3所示。三元组<x(t),y(t),θ(t)>表示移动底盘在t时刻的位姿。
移动底盘的线速度只有在移动底盘沿着前后方向运动时才会产生，这是两轮差速驱动模型的约束。用x(t0)和x(tn)分别表示在t0和tn时刻移动底盘的x坐标(y(t0)和y(tn)同理)，v(t)和ω(t)分别表示移动底盘在t时刻的线速度和角速度，则x(tn)和y(tn)则可以分别用公式(6)和公式(7)来表示。
本发明假设移动底盘的初始线速度为v或者-v，初始角速度ω的取值范围为ωmin～ωmax，角速度与线速度在运动时间t内保持恒定。移动底盘在t0时刻的位置设为位于全局坐标系的原点，tn＝t0 t，为了减少计算资源，采用栅格法对移动底盘在恒定时间t内的运动轨迹进行求解，具体步骤如下：
1)将运动时间t分为m份，在第i个时间间隔(t0 t/m·(i-1),t0 t/m·i)内移动底盘的运动可以简化为先旋转再平移；
2)在第i个时间间隔内移动底盘旋转的角度deg可以由公式(8)来表示：
deg＝t/m·ω(8)
根据公式(9)可以得到移动底盘旋转对应的位姿变换矩阵tr：
3)在第i个间隔内移动底盘平移的距离dis可以由公式(10)来表示：
dis＝t/m·v(10)
根据公式(11)可以得到移动底盘平移对应的位姿变换矩阵tt：
4)假设移动底盘在(t0 t/m·(i-1))时刻的位姿矩阵为told，则移动底盘在(t0 t/m·i)时刻的位姿矩阵tnew可以根据公式(12)得到：
tnew＝tt·tr·told(12)
5)假设移动底盘的初始角速度ω的取值范围为ωmin～ωmax，每次增加的步长为(ωmax-ωmin)/k，将移动底盘的初始线速度v和-v、初始角速度ω、t0时刻的位姿矩阵带入公式(12)中，经过m次迭代即可得到移动底盘在固定运动时长t内的2·m·k个位姿矩阵；
6)通过描点的方式将所得到的2·m·k个位姿矩阵对应的三维坐标点显示出来，便可得到移动底盘相对于全局坐标系的可达空间点云图，如图4所示。
(4)移动底盘与机械臂联合运动的定时长可达工作空间求解
由于移动作业机器人的机械臂固定在移动底盘上，所以机器人在运动与作业的时候，机械臂的基座相对于移动底盘的位姿矩阵保持不变。为了简化模型，假设机械臂基座坐标系与移动底盘坐标系为同一坐标系，将公式(4)中的机械臂末端执行器相对于基座坐标系的位姿矩阵带入公式(12)，即可得到机械臂末端执行器相对于全局坐标系的位姿矩阵。通过描点的方式将所得到的位姿矩阵对应的三维坐标点显示出来，便可得到移动作业机器人定时长可达工作空间点云图。
本发明的有益效果：该方法实现了移动作业机器人定时长可达工作空间的求解，针对移动底盘加装六自由度机械臂的新型智能机器人，建立了运动学模型，采用蒙特卡洛法对机器人定时长可达工作空间进行分析，求解过程相对简单，准确性较高，有助于提升移动作业机器人在大范围场景下对特定物体的抓取效率。本发明可以用在移动作业机器人的路径规划、避障和运动控制以及人机协作等技术领域。
附图说明
图1两相邻连杆之间的坐标关系图。
图2机械臂定时长可达工作空间图。
图3两轮差速驱动模型图。
图4移动底盘定时长可达工作空间图。
图5移动作业机器人定时长可达工作空间图。
具体实施方式
以下结合技术方案和附图详细叙述本发明的具体实施方式。
本发明选择matlabr2018a中的机器人学工具箱roboticstoolbox10.3作为仿真实验平台，机器人学工具箱是matlab中专门用于机器人仿真的工具箱，在机器人建模、轨迹规划、控制、可视化方面使用非常方便。
第一步，定利用d-h法构建六自由度机械臂模型所需要的d-h参数，a1取值0mm，a2取值0mm，a3取值420mm，a4取值375mm，a5取值0mm，a6取值0mm，θ1取值0rad，θ2取值0rad，θ3取值0rad，θ4取值0rad，θ5取值0rad，θ6取值0rad，d1取值0mm，d2取值138mm，d3取值-127mm，d4取值114mm，d5取值114mm，d6取值90mm，α1取值0rad，α2取值-π/2rad，α3取值0rad，α4取值0rad，α5取值-π/2rad，α6取值π/2rad。根据上述参数在matlabr2018a中使用机器人学工具箱roboticstoolbox10.3可以得到机械臂的仿真模型。
第二步，移动作业机器人的固定运动时长t取值1s，据此可以得到机械臂各个关节运动的角度范围，θ1min为-πrad，θ1max为πrad，θ2min为-πrad，θ2max为0rad，θ3min为-π/2rad，θ3max为π/2rad，θ4min为-πrad，θ4max为πrad，θ5min为-πrad，θ5max为πrad，θ6min为-πrad，θ6max为πrad。循环次数n取值1000，基于蒙特卡洛法以及公式(5)在各关节允许取值的范围内遍历取值，将产生的关节运动角度随机值带入公式(4)中，得到末端执行器在基座坐标系下的位置坐标的集合，也就是机械臂的可达工作空间点云图。
第三步，移动底盘的初始线速度v取值0.5m/s，初始角速度ω的范围ωmin取值-π/2rad，ωmax取值π/2rad，初始角速度变化的步长(ωmax-ωmin)/k中的k取值100。采用栅格法将运动时间t分为m份，m取值10。将上述参数代入公式(12)可以得到移动底盘相对于全局坐标系的可达工作空间点云图。
第四步，根据机械臂的定时长工作空间、移动底盘的定时长工作空间以及公式(12)可以得到移动作业机器人定时长可达工作空间点云图。
1.一种移动作业机器人定时长可达工作空间的求解方法，其特征在于，步骤如下：
(1)六自由度机械臂运动学模型建立
采用d-h法建立机械臂关节坐标系，用一个4×4的齐次变换矩阵描述相邻两连杆的空间关系；选取连杆n的末端关节轴线为坐标系{n}的zn轴，坐标系{n-1}的zn-1轴与坐标系{n}的zn轴的公法线方向定义为xn的方向，yn的方向根据右手法确定；
建立以上机械臂关节坐标系，还需要用于描述各连杆以及两相邻连杆之间的4个参数：连杆长度an，即沿着xn从zn-1到zn之间的距离；连杆扭角αn，即绕着xn从zn-1到zn之间的角度；关节偏置距离dn，即沿着zn-1从xn-1到xn之间的距离；关节转角θn，即绕着zn-1从xn-1到xn之间的角度；
沿zn-1轴的位姿变换用公式(1)来描述：
沿xn轴的位姿变换用公式(2)来描述：
将公式(1)和公式(2)两个位姿变换矩阵进行叠加即可以得到完整的位姿坐标转换矩阵，如公式(3)所示：
基于机械臂的运动学正解来对机械臂末端执行器的位姿进行求解；根据d-h法以及机械臂相邻关节之间的位姿变换矩阵公式(3)，得到机械臂的末端执行器的坐标系相对于基座坐标系的位姿变换矩阵，如公式(4)所示：
其中，
(2)机械臂定时长工作空间求解
使用蒙特卡洛法求解工作空间：各关节在允许取值的范围内遍历取值，得到末端点的集合也就是工作空间，具体的步骤如下：
1)根据公式(4)的正向运动学模型获取机械臂的末端执行器相对于基座坐标系的位置向量；
2)根据移动作业机器人的固定运动时长t，得到机械臂各个关节运动角度的取值范围，θnmin为第n个关节角度取值范围的下限，θnmax为第n个关节角度取值范围的上限；
3)基于在(0,1)上的均匀分布概率模型来产生0～1之间的随机步长，接着使用随机步长来产生每个关节在运动角度范围内的随机值
4)将上述步骤中产生的关节运动角度随机值带入获得的位置向量中，得到末端执行器在基座坐标系下的位置坐标；
5)循环n次步骤1)至步骤5)，得到n个机械臂末端执行器的空间三维坐标点；
6)通过描点的方式将所得到的n个三维坐标点显示出来，便可得到六自由度机械臂末端执行器相对于机械臂基座坐标系的可达空间点云图；
(3)移动底盘定时长可达工作空间求解
移动作业机器人的移动底盘运动学模型为两轮差速驱动模型，理想情况下，移动底盘的运动轨迹是一段一段的圆弧或者直线，一对(v,ω)就代表了一个圆弧轨迹，其中v和ω分别为移动底盘的线速度和角速度；用x(t)和y(t)表示在t时刻移动底盘在全局坐标系下的坐标，用θ(t)表示移动底盘方位角；则三元组<x(t),y(t),θ(t)>表示移动底盘在t时刻的位姿；
移动底盘的线速度只有在移动底盘沿着前后方向运动时才会产生，这是两轮差速驱动模型的约束；用x(t0)和x(tn)分别表示在t0和tn时刻移动底盘的x坐标，y(t0)和y(tn)同理，v(t)和ω(t)分别表示移动底盘在t时刻的线速度和角速度，则x(tn)和y(tn)则可以分别用公式(6)和公式(7)来表示；
设移动底盘的初始线速度为v或者-v，初始角速度ω的取值范围为ωmin～ωmax，角速度与线速度在运动时间t内保持恒定；移动底盘在t0时刻的位置设为位于全局坐标系的原点，tn＝t0 t，为了减少计算资源，采用栅格法对移动底盘在恒定时间t内的运动轨迹进行求解，具体步骤如下：
1)将运动时间t分为m份，在第i个时间间隔(t0 t/m·(i-1),t0 t/m·i)内移动底盘的运动可以简化为先旋转再平移；
2)在第i个时间间隔内移动底盘旋转的角度deg可以由公式(8)来表示：
deg＝t/m·ω(8)
根据公式(9)可以得到移动底盘旋转对应的位姿变换矩阵tr：
3)在第i个间隔内移动底盘平移的距离dis可以由公式(10)来表示：
dis＝t/m·v(10)
根据公式(11)可以得到移动底盘平移对应的位姿变换矩阵tt：
4)设移动底盘在(t0 t/m·(i-1))时刻的位姿矩阵为told，则移动底盘在(t0 t/m·i)时刻的位姿矩阵tnew可以根据公式(12)得到：
tnew＝tt·tr·told(12)
5)设移动底盘的初始角速度ω的取值范围为ωmin～ωmax，每次增加的步长为(ωmax-ωmin)/k，将移动底盘的初始线速度v和-v、初始角速度ω、t0时刻的位姿矩阵带入公式(12)中，经过m次迭代即可得到移动底盘在固定运动时长t内的2·m·k个位姿矩阵；
6)通过描点的方式将所得到的2·m·k个位姿矩阵对应的三维坐标点显示出来，便可得到移动底盘相对于全局坐标系的可达空间点云图；
(4)移动底盘与机械臂联合运动的定时长可达工作空间求解
设机械臂基座坐标系与移动底盘坐标系为同一坐标系，将公式(4)中的机械臂末端执行器相对于基座坐标系的位姿矩阵带入公式(12)，即可得到机械臂末端执行器相对于全局坐标系的位姿矩阵；通过描点的方式将所得到的位姿矩阵对应的三维坐标点显示出来，便可得到移动作业机器人定时长可达工作空间点云图。
技术总结