CN111070212B - 随车机械手控制系统及方法 - Google Patents

Abstract

本发明实施方式提供及一种随车机械手控制系统及方法，所述系统包括：控制单元；控制单元被配置为：基于机械手建立三维坐标系，依据三维坐标系建立障碍物虚拟墙坐标点；通过检测组件检测机械手控制点的位置信号，依据机械手控制点的位置信号及三维坐标系计算机械手控制点的坐标；判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向移动。本发明通过建立虚拟墙并实时计算各控制点的坐标，当机械手控制点的坐标及虚拟墙坐标点的坐标之间的相对距离小于阈值时，控制机械手各控制点停止向靠近所述障碍物虚拟墙的方向动作，能有效的避免机械手作业过程中碰撞障碍物。

Description

随车机械手控制系统及方法

技术领域

本发明涉及机械手控制技术领域，具体地涉及一种随车机械手控制系统及一种随车机械手控制方法。

背景技术

机械手在运动过程中可能会出现碰到障碍物的情况，从而导致安全事故或不必要的损失，目前，对于机械手运动避障的方法大多依赖于视觉算法，通过摄像头采集图像，并对图像分析以对障碍物进行识别，并根据识别结果对机械臂的运动路径进行规划。但是，由于摄像头视角的限制，在进行障碍物识别的时候可能出现视觉盲区或者机械手在运动过程中对障碍物有遮挡的情况，这时候就无法识别到障碍物，从而造成误判断。

发明内容

本发明实施方式的目的是提供一种随车机械手控制系统及一种随车机械手控制方法，以解决现有的机械手运动过程中可能碰到障碍物的问题。

为了实现上述目的，在本发明第一方面，提供一种随车机械手控制系统，包括：控制单元；

所述控制单元被配置为：

基于所述机械手建立三维坐标系，依据所述三维坐标系建立障碍物虚拟墙坐标点，所述障碍物虚拟墙坐标点的集合构成障碍物虚拟墙；

通过检测组件检测机械手控制点的位置信号，依据机械手控制点的位置信号及所述三维坐标系计算机械手控制点的坐标，所述位置信号包括机械手控制点的俯仰角度值、回转角度值及伸缩位移量；

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制所述机械手停止向靠近所述障碍物虚拟墙的方向动作。

可选地，所述判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制所述机械手停止向靠近所述障碍物虚拟墙的方向动作，包括：

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第一阈值时，控制所述机械手控制点的俯仰转动角度在第一俯仰角度值以内、控制所述机械手控制点的回转角度在第一回转角度值以内，以及控制所述机械手控制点的伸缩位移量在第一长度值以内；同时，降低所述机械手控制点的俯仰转动速度、回转速度及伸缩速度；

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第二阈值时，控制所述机械手控制点停止向靠近所述障碍物虚拟墙的方向动作；

所述第一阈值大于所述第二阈值。

可选地，所述判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制所述机械手停止向靠近所述障碍物虚拟墙的方向动作，包括：

实时检测机械手的动作速度；

当机械手的动作速度达到第一速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第三阈值时，控制机械手降低动作速度，直至机械手停止向靠近所述障碍物虚拟墙的方向动作；

当机械手的动作速度达到第二速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第四阈值时，控制机械手降低动作速度，直至机械手停止向靠近所述障碍物虚拟墙的方向动作；

当机械手的动作速度达到第三速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第五阈值时，控制机械手降低动作速度，直至机械手停止向靠近所述障碍物虚拟墙的方向动作；

所述第一速度大于所述第二速度，所述第二速度大于所述第三速度，所述第三阈值大于所述第四阈值，所述第四阈值大于所述第五阈值。

可选地，所述检测组件包括：

用于检测所述机械手控制点俯仰角度值的第一角度传感器、用于检测所述机械手控制点回转角度值的第二角度传感器以及用于检测所述机械手控制点伸缩位移量的距离传感器。

可选地，所述控制单元还被配置为：

接收待抓取负载的尺寸参数，依据所述待抓取负载的尺寸参数及机械手末端执行控制点的坐标计算所述待抓取负载的边界坐标，以及当判断所述待抓取负载任一边界坐标与任一障碍物虚拟墙坐标点的坐标的直线距离小于阈值时，控制所述机械手停止向靠近所述障碍物虚拟墙的方向动作；所述待抓取负载的尺寸参数包括所述待抓取负载底部与所述机械手末端执行控制点的垂直距离以及所述待抓取负载轮廓边界与所述机械手末端执行控制点的水平距离。

本发明的第二方面，提供一种随车机械手控制方法，包括：

基于所述机械手建立三维坐标系，依据所述三维坐标系建立障碍物虚拟墙坐标点，所述障碍物虚拟墙坐标点的集合构成障碍物虚拟墙；

通过检测组件检测机械手控制点的位置信号，依据机械手控制点的位置信号及所述三维坐标系计算机械手控制点的坐标，所述位置信号包括机械手控制点的俯仰角度值、回转角度值及伸缩位移量；

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制所述机械手停止向靠近所述障碍物虚拟墙的方向动作。

可选地，所述判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制所述机械手停止向靠近所述障碍物虚拟墙的方向动作，包括：

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第一阈值时，控制所述机械手控制点的俯仰转动角度在第一俯仰角度值以内、控制所述机械手控制点的回转角度在第一回转角度值以内，以及控制所述机械手控制点的伸缩位移量在第一长度值以内；同时，降低所述机械手控制点的俯仰转动速度、回转速度及伸缩速度；

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第二阈值时，控制所述机械手控制点停止向靠近所述障碍物虚拟墙的方向动作；

所述第一阈值大于所述第二阈值。

可选地，所述判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制所述机械手停止向靠近所述障碍物虚拟墙的方向动作，包括：

实时检测机械手的动作速度；

当机械手的动作速度达到第一速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第三阈值时，控制机械手降低动作速度，直至机械手停止向靠近所述障碍物虚拟墙的方向动作；

当机械手的动作速度达到第二速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第四阈值时，控制机械手降低动作速度，直至机械手停止向靠近所述障碍物虚拟墙的方向动作；

当机械手的动作速度达到第三速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第五阈值时，控制机械手降低动作速度，直至机械手停止向靠近所述障碍物虚拟墙的方向动作；

所述第一速度大于所述第二速度，所述第二速度大于所述第三速度，所述第三阈值大于所述第四阈值，所述第四阈值大于所述第五阈值。可选地，所述检测组件包括：

用于检测所述机械手控制点俯仰角度值的第一角度传感器、用于检测所述机械手控制点回转角度值的第二角度传感器以及用于检测所述机械手控制点伸缩位移量的距离传感器。

可选地，所述方法还包括：

接收待抓取负载的尺寸参数，依据所述待抓取负载的尺寸参数及机械手末端执行控制点的坐标计算所述待抓取负载的边界坐标，以及当判断所述待抓取负载任一边界坐标与任一障碍物虚拟墙坐标点的坐标的最小距离小于阈值时，控制所述机械手停止向靠近所述障碍物虚拟墙的方向动作；所述待抓取负载的尺寸参数包括所述待抓取负载底部与所述机械手末端执行控制点的垂直距离以及所述待抓取负载轮廓边界与所述机械手末端执行控制点的水平距离。

本发明上述技术方案通过控制单元建立虚拟墙并实时计算各控制点的坐标，实现了在控制机械手作业时，当机械手控制点的坐标及虚拟墙坐标点的坐标之间的相对距离小于阈值时，控制机械手各控制点停止动作，从而控制机械手的动作始终在虚拟墙的控制范围内，能有效的避免机械手作业过程中碰撞障碍物，同时避免了因数据采集盲区造成误判的情况，提高了控制精度。

本发明实施方式的其它特征和优点将在随后的具体实施方式部分予以详细说明。

附图说明

附图是用来提供对本发明实施方式的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本发明实施方式，但并不构成对本发明实施方式的限制。在附图中：

图1是本发明一种实施方式提供的一种随车机械手虚拟墙控制系统的系统框图；

图2是本发明一种实施方式提供的检测组件安装示意图；

图3是本发明一种实施方式提供的随车机械手主视图；

图4是本发明一种实施方式提供的随车机械手俯视图；

图5是本发明一种实施方式提供的机械手立体坐标计算辅助示意图；

图6是本发明一种实施方式提供的一种随车机械手虚拟墙控制系统的系统控制示意图；

图7是本发明一种实施方式提供的一种随车机械手虚拟墙控制系统的避障实施流程图；

图8是本发明另一种实施方式提供的一种随车机械手虚拟墙控制系统的避障实施流程图；

图9是本发明一种实施方式提供的一种随车机械手虚拟墙控制方法的方法流程图。

附图标记说明

101-转台回转编码器，102-主臂俯仰角度传感器，103-伸缩臂长度传感器，104-折臂俯仰角度传感器，105-抓手俯仰角度传感器，106-抓手回转角度传感器，107-左爪角度传感器，108-右爪角度传感器，109-行程开关，110左爪接近开关，111-右爪接近开关。

具体实施方式

以下结合附图对本发明的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。

在本发明实施方式中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括要素的过程、方法、商品或者设备中还存在另外的相同要素。

如图1所示，在本实施例的第一方面，提供一种随车机械手控制系统，包括：控制单元；

控制单元被配置为：

基于机械手建立三维坐标系，依据三维坐标系建立障碍物虚拟墙坐标点，所述障碍物虚拟墙坐标点的集合构成障碍物虚拟墙；

通过检测组件检测机械手控制点的位置信号，依据机械手控制点的位置信号及三维坐标系计算机械手控制点的坐标，位置信号包括机械手控制点的俯仰角度值、回转角度值及伸缩位移量；

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向动作。

如此，本实施例的上述技术方案通过控制单元建立虚拟墙并实时计算各控制点的坐标，实现了在控制机械手作业时，当机械手控制点的坐标及虚拟墙坐标点的坐标之间的相对距离小于阈值时，控制机械手各控制点停止动作，从而控制机械手的动作始终在虚拟墙的控制范围内，能有效的避免机械手作业过程中碰撞障碍物，同时避免了因数据采集盲区造成误判的情况，提高了控制精度。

具体的，随车机械手在作业过程中，当车辆的驾驶室及过道舱/方舱可能对机械手的作业路径造成影响时，往往需要控制机械手避开驾驶室、过道舱或方舱，以避免与机械手造成碰撞，造成安全事故及不必要的损失，现有的方法往往通过操作人员肉眼判断或采用图像处理的方法进行机械手的避障，但是，采用肉眼判断容易误判，而采用图像处理的方法则可能因为摄像头存在图像数据获取的盲区或机械手遮挡摄像头造成误判断，同时，采用图像处理的方法还需要在车辆上合适的位置安装摄像头及走线，若没有合适的安装位置，还需要安装其他辅助结构，施工工序多，且摄像头暴露在工作区，易损坏。本实施方式中，控制单元为控制器，机械手包括依次连接的转台、主臂、伸缩臂、折臂及抓手，其中，控制器通过模拟量或PWM输出控制电磁阀的开度来控制液压系统，从而控制伸缩臂的伸缩、俯仰及主臂、折臂的俯仰，控制器还通过驱动电路控制不同液压马达的转动从而控制转台的回转、抓手的回转及抓手的抓取动作，控制器还外接有触摸屏，触摸屏作为人机交互接口用于向控制器发送指令及数据。本实施方式中，以机械手的转台在地面的投影为原点建立三维坐标系，根据车辆的具体外形轮廓，基于建立的三维坐标系构建障碍物虚拟墙坐标点，所有的虚拟墙坐标点围合则形成了与车辆外形轮廓对应的障碍物虚拟墙，将所有的障碍物虚拟墙坐标点组成障碍物虚拟墙坐标库存储在与控制器电连接的存储器中，由于车辆不同的货仓形式往往具有固定的形状和尺寸，故，可以针对货仓的不同形式预先建立多套障碍物虚拟墙坐标库，在使用时，只需要通过控制器调用对应的障碍物虚拟墙坐标库即可。本实施方式中，预先选取机械手边界上的特定点作为控制点，控制器通过与其电连接的检测组件检测机械手各控制点的位置信号，其中，控制点的位置信号包括机械手控制点俯仰角度值、回转角度值及伸缩位移量，以伸缩臂与折臂之间的控制点为例，该控制点的动作包括伸缩位移、俯仰及回转，则该控制点的位置信号包括俯仰角度、回转角度及伸缩位移的长度；以主臂与转台之间的控制点为例，由于主臂不执行伸缩位移动作，则该控制点包括俯仰角度、回转角度及伸缩位移的长度，其中，伸缩位移的长度为0，由于转台的坐标已知，主臂的长度已知，则根据伸缩臂的伸缩位移量、俯仰角度值及回转角度值即可得到基于建立的三维坐标系下的伸缩臂与折臂之间控制点的坐标，其他控制点的坐标同理可得。机械手作业时，控制器实时获取检测组件检测到的控制点位置信号并计算各个控制点的当前坐标值，并将得到的坐标值分别与障碍物虚拟墙坐标库中的坐标点进行比对，计算控制点坐标与障碍物虚拟墙坐标点的最小距离，并判断其是否小于预设的阈值，若任一一个控制点的坐标与任一一个障碍物虚拟墙坐标点的坐标之间的最小距离小于预设的阈值，则控制器判断机械手进入危险区域，控制器控制所有与机械手控制对应的电磁阀及液压马达，从而控制机械手停止向靠近所述障碍物虚拟墙的方向动作，使得机械手只能向安全方向运动，以避免碰撞，同时，控制器可通过外接的报警装置进行报警，对工作人员进行提醒。

如图2所示，检测组件包括：

用于检测机械手控制点俯仰角度值的第一角度传感器、用于检测机械手控制点回转角度值的第二角度传感器以及用于检测机械手控制点伸缩位移量的距离传感器。本实施例中，第二角度传感器可以为角度传感器或编码器，具体的，控制器根据转台回转编码器101检测输出的主臂回转角度信息、主臂俯仰角度传感器102检测输出的主臂俯仰角度信息、伸缩臂长度传感器103检测输出的伸缩臂长度信息、折臂俯仰角度传感器104检测输出的折臂俯仰角度信息、抓手俯仰角度传感器105检测输出的抓手俯仰角度信息、抓手回转角度传感器106检测输出的抓手回转角度信息计算各控制点的坐标。控制器还通过左爪角度传感器107检测输出的左爪张开角度信息，通过右爪角度传感器108检测输出的右爪角度信息，以及根据设置在抓手正下方的行程开关109的接触通断信号、设置在左爪上的左爪接近开关110及设置在右爪上的右爪接近开关111的接近开关信号判定抓手是否抓取负载物体。

如图3及图4所示，作业车辆的驾驶舱高为h2，过道舱高为h3，在建立障碍物虚拟墙时，机械手在回转a角度时，机械手长度在≥15/sina与≤12/cos(180-a)范围内禁止随车机械手及其抓取物进入离地小于h2高度范围；在回转b角度时，长度在≥11/sinb与≤12/sinb范围内禁止随车机械手及其抓取物进入离地小于h3高度范围。以此方式在机械手回转0°到360°范围内及机械手可伸缩半径范围内，在XYZ立体三维空间中通过各三角函数公式即可构建出障碍物空间。

如图5及图6所示，依据获取的上述角度信息，可进一步建立机械手实时立体坐标，例如，机械手抓手最低点离地实时高度为：

Hx＝H1+(L2+L3)sina1+L4sina2-L5cosa3；

或Hx＝H1+(L2+L3)sina1+L4sina2-(L6+L7cosa4)cosa3；

或Hx＝H1+(L2+L3)sinal+L4sina2-(L6+L7cosa5)cosa3；

以上述三个公式比较取最小值为抓手最低点。

机械手抓手内侧边际线离转台原点距离最近点为：

LX＝-L1+(L2+L3)cosa1+L4cosa2-|(L9/cosa3-L5sina3)cosa6|；

或LX＝-L1+(L2+L3)cosa1+L4cosa2-|(L8/cosa3-L6sina3+L7sina5)cosa6|，抓手回转角度传感器在正180°区域；

或LX＝-L1+(L2+L3)cosa1+L4cosa2-|(L8/cosa3-L6sina3+L7sina4)cosa6|，抓手回转角度传感器在负180°区域；

将上述三个公式进行比较取最小值作为机械手抓手内侧边际线离转台原点距离最近点。

其中：

H1为主臂变幅铰座(主臂控制点)离地高度，为固定值；

L1为主臂变幅铰座离原点距离，为固定值；

L2为主臂长度，为固定值；

L3为伸缩臂长度，为伸缩臂长度传感器检测值；

a1为主臂俯仰角度，为主臂俯仰角度传感器检测值；

L4为折臂长度，为固定值；

a2为折臂(折臂控制点)俯仰角度，为折臂俯仰角度传感器检测值；

L5为抓手抓取物外形长度，为固定值，可以是输入值；

L6为抓手俯仰铰座(抓手控制点)中点到抓手张开铰座中点高度距离，为固定值；

L7为抓手长度，为固定值；

L8为抓手俯仰铰座中点到抓手张开铰座中点宽度距离，为固定值；

L9为抓手抓取物外形宽度，为固定值，可以是输入值；

a3为抓手俯仰角度，为抓手俯仰角度传感器检测值；

a4为左爪张开角度，为左爪张开角度传感器检测值；

a5为右爪张开角度，为右爪张开角度传感器检测值；

a6为抓手回转角度，为抓手回转角度传感器检测值。

如果存在主臂碰撞，也需计算主臂碰撞点：

参照图3，主臂俯仰角度a1必须满足(l1/sinb+L1)sina1+H1＞h3；从而在机械手运行中通过在各回转角度上控制主臂俯仰角度a1的最小值，防止主臂与障碍物干涉。

如图7所示，为了提高机械手在虚拟墙限定范围内的作业的控制精度，在本实施例的一种可选实施方式中，判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向动作，包括：

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第一阈值时，控制机械手控制点的俯仰转动角度在第一俯仰角度值以内、控制机械手控制点的回转角度在第一回转角度值以内，以及控制机械手控制点的伸缩位移量在第一长度值以内；同时，降低所述机械手控制点的俯仰转动速度、回转速度及伸缩速度；

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第二阈值时，控制机械手各控制点停止向靠近所述障碍物虚拟墙的方向动作；

第一阈值大于第二阈值。

具体的，预先设定表示机械手控制点与障碍物虚拟墙坐标点之间的预警距离的第一阈值及第二阈值，其中，第一阈值大于第二阈值，机械手作业过程中，控制器实时计算机械手各控制点的坐标并分别计算该控制点与障碍物虚拟墙坐标点的坐标之间的直线距离，当得到的直线距离小于设定的第一阈值时，控制器通过控制对应的电磁阀及液压马达，从而控制机械手各控制点的每次回转角度在第一回转角度值以内，各控制点的每次俯仰转动角度在第一俯仰角度值以内，各控制点的每次伸缩位移量在第一长度值以内，同时，控制器通过报警装置发送报警，提示工作人员机械手已经接近虚拟墙；如此，当机械手在触发第一阈值报警后，通过控制机械手每次动作在各个自由度的位移量在一个较小的值，能够控制机械手更精确的抓取靠近虚拟墙的待抓取负载，在避免了机械手与障碍物碰撞的同时使得机械手在虚拟墙限定范围内能更精确的抓取靠近虚拟墙的负载。同时，降低机械手各控制点处的俯仰转动速度、回转速度及伸缩速度，从而降低机械手的动作速度，进一步降低了机械手动作的机械惯性与控制延时的风险。当控制器判断机械手任一控制点坐标与任一障碍物虚拟墙坐标点的坐标的直线距离小于第二阈值时，控制器控制机械手各控制点停止动作，从而避免机械手与障碍物发生碰撞。

如图8所示，为了进一步提高机械手在虚拟墙限定范围内的作业的控制精度，在本实施例的另一种可选实施方式中，判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的坐标之间的最小距离小于阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向动作，包括：

通过检测组件实时检测机械手的动作速度；

当机械手的动作速度达到第一速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第三阈值时，控制机械手降低动作速度，直至机械手停止向靠近障碍物虚拟墙的方向动作；

当机械手的动作速度达到第二速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第四阈值时，控制机械手降低动作速度，直至机械手停止向靠近障碍物虚拟墙的方向动作；

当机械手的动作速度达到第三速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第五阈值时，控制机械手降低动作速度，直至机械手停止向靠近障碍物虚拟墙的方向动作；

第一速度大于第二速度，第二速度大于第三速度，第三阈值大于第四阈值，第四阈值大于第五阈值。

机械手在作业时，不同的作业情况下，机械手的各动作运行速度不同，控制器可以通过速度传感器实时获取机械手的动作速度或者通过控制器输出的动作控制量得到机械手的动作速度，从而判断机械手的当前速度是否达到第一速度，当达到第一速度时，控制器根据机械手接近障碍物虚拟墙时的动作速度的正比例关系自动计算第三阈值的值，同时，降低机械手的动作速度，使得机械手在第三阈值范围内停止向靠近所述障碍物虚拟墙的方向动作；同理，当机械手的当前速度达到第二速度时，控制器根据机械手接近障碍物虚拟墙时的动作速度的正比例关系自动计算第四阈值的值，同时，降低机械手的动作速度，使得机械手在第四阈值范围内停止向靠近所述障碍物虚拟墙的方向动作；当机械手的当前速度达到第三速度时，控制器根据机械手接近障碍物虚拟墙时的动作速度的正比例关系自动计算第五阈值的值，同时，降低机械手的动作速度，使得机械手在第五阈值范围内停止向靠近所述障碍物虚拟墙的方向动作。即，实现了对于高速动作接近障碍物虚拟墙时，控制器为其分配的阈值距离与降速曲线最长，对于中速动作接近障碍物虚拟墙时，控制器为其分配的阈值距离与降速曲线稍短于高速时，对于低速动作接近障碍物虚拟墙时，控制器为其分配的阈值距离与降速曲线最短，通过上述技术方案，能有效的消除机械惯性并实现精确定位防撞。

本实施例还提供另一种可选实施方式：

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第六阈值时，控制机械手关节的俯仰转动角度在第二俯仰角度值以内、控制机械手关节的回转角度在第二回转角度值以内，以及控制机械手关节的伸缩位移量在第二长度值以内；

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第七阈值时，控制机械手关节的俯仰转动角度在第三俯仰角度值以内、控制机械手关节的回转角度在第三回转角度值以内，以及控制机械手关节的伸缩位移量在第三长度值以内；

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第八阈值时，控制机械手各控制点停止向靠近所述障碍物虚拟墙的方向动作；

第六阈值大于第七阈值，第七阈值大于第八阈值，第二俯仰角度值大于第三俯仰角度值，第二回转角度值大于第三回转角度值，第二长度值大于第三长度值。

具体的，预先设定表示机械手控制点与障碍物虚拟墙坐标点之间的预警距离的第六阈值、第七阈值及第八阈值，其中，第六阈值大于第七阈值，第七阈值大于第八阈值，在机械手作业过程中，当控制器计算得到的机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的坐标之间的最小距离小于第六阈值时，控制器控制机械手各控制点的每次回转角度在第二回转角度值以内，各控制点的每次俯仰转动角度在第二俯仰角度值以内，各控制点的每次伸缩位移量在第二长度值以内；机械手继续动作，当机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的坐标之间的最小距离小于第七阈值时，控制器控制机械手各控制点的每次回转角度在第三回转角度值以内，各控制点的每次俯仰转动角度在第三俯仰角度值以内，各控制点的每次伸缩位移量在第三长度值以内，其中，第二俯仰角度值大于第三俯仰角度值，第二回转角度值大于第三回转角度值，第二长度值大于第三长度值，如此，每一次在机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的坐标之间的最小距离达到对应的设定阈值时，控制器都将机械手各关节的回转角度值、俯仰角度值及伸缩位移量限定在比上一次机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的坐标之间的直线距离达到对应的设定阈值时控制器对各关节的回转角度值、俯仰角度值及伸缩位移量的限定值更小的范围内，使得机械手各关节的转角度值、俯仰角度值及伸缩位移量在机械手每次达到对应阈值时，其最大动作范围递减，从而能在机械手接近虚拟墙时避免因机械手动能过大导致未能及时停止动作而越过虚拟墙与障碍物发生碰撞，同时能在机械手靠近虚拟墙时更精确的作业；直至控制器判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的坐标的最小距离小于第八阈值时，控制机械手各控制点停止动作。

当控制器判断抓手抓取到负载物体时，控制器还需根据负载物体的尺寸参数判断是否增加负载物体的边界作为控制点，因此，控制单元还被配置为：

接收待抓取负载的尺寸参数，依据待抓取负载的尺寸参数及机械手末端执行控制点的坐标计算待抓取负载的边界坐标，以及当判断待抓取负载任一边界坐标与任一障碍物虚拟墙坐标点的坐标的直线距离小于阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向动作；待抓取负载的尺寸参数包括待抓取负载底部与机械手末端执行控制点的垂直距离以及待抓取负载轮廓边界与机械手末端执行控制点的水平距离。

机械手的末端执行关节，即抓手，在抓取负载前，工作人员通过触摸屏输入待抓取负载的类型与尺寸参数，待抓取负载的类型主要分为轮胎、石头、滚木、钢管以及特殊异形物等，尺寸参数包括负载的长度、高度、直径等参数，从而完善与细化待抓取负载的外形轮廓参数，控制器根据输入的负载外形轮廓参数，确定是否增加边界尺寸。以待抓取负载为备胎为例，工作人员通过触摸屏输入的备胎底部到抓手关节中心点的垂直距离，此处为备胎的直径，该距离根据实际情况确定，以及输入备胎水平距离上最远的两个边界点分别与抓手关节中心点的水平距离，此处默认抓手抓取位置位于备胎被抓取面的中心，即抓手关节中心点与备胎宽度中心点在同一轴线上，则，当控制器判定抓手抓取到备胎时，控制器根据输入的直径及备胎宽度方向两边界点分别距离抓手关节中心点的水平距离即可计算得到备胎的边界点坐标，在抓手抓取到备胎后，控制器通过判断备胎边界点坐标及各控制点坐标与障碍物虚拟墙坐标点的坐标之间的直线距离是否小于阈值即可判断在抓取到负载状态下的机械手是否存在碰撞危险，从而有效避免机械手或负载物体与障碍物碰撞。当被抓取负载为异形物体时，控制器按照异形物体的外切立方体增加机械手末端关节的边界尺寸。

本实施例中，控制器还被配置为判断接收到手动强制信号时，解除对机械手的锁定，某些特殊情况下，工作人员需要控制机械手进入危险区域，此时可以通过人工按下强制键解除控制器对机械手的锁定，为了防止人员疏忽出现误动作，需要一直按下强制建，控制器判断机械手进入虚拟墙限制区域且未接收到强制信号，则立即锁定机械手，使其停止动作。

如图9所示，本发明的第二方面，提供一种随车机械手控制方法，包括：

基于机械手建立三维坐标系，依据三维坐标系建立障碍物虚拟墙坐标点，所述障碍物虚拟墙坐标点的集合构成障碍物虚拟墙；

通过检测组件检测机械手控制点的位置信号，依据机械手控制点的位置信号及三维坐标系计算机械手控制点的坐标，位置信号包括机械手控制点的俯仰角度值、回转角度值及伸缩位移量；

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向动作。

可选地，判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向动作，包括：

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第一阈值时，控制机械手关节的俯仰转动角度在第一俯仰角度值以内、控制机械手关节的回转角度在第一回转角度值以内，以及控制机械手关节的伸缩位移量在第一长度值以内；同时，降低所述机械手控制点的俯仰转动速度、回转速度及伸缩速度；

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第二阈值时，控制机械手各控制点停止向靠近所述障碍物虚拟墙的方向动作；

第一阈值大于第二阈值。

可选地，判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向动作，包括：

实时检测机械手的动作速度；

当机械手的动作速度达到第一速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第三阈值时，控制机械手降低动作速度，直至机械手停止向靠近障碍物虚拟墙的方向动作；

当机械手的动作速度达到第二速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第四阈值时，控制机械手降低动作速度，直至机械手停止向靠近障碍物虚拟墙的方向动作；

当机械手的动作速度达到第三速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第五阈值时，控制机械手降低动作速度，直至机械手停止向靠近障碍物虚拟墙的方向动作；

第一速度大于第二速度，第二速度大于第三速度，第三阈值大于第四阈值，第四阈值大于第五阈值。

可选地，检测组件包括：

用于检测机械手控制点俯仰角度值的第一角度传感器、用于检测机械手控制点回转角度值的第二角度传感器以及用于检测机械手控制点伸缩位移量的距离传感器。

可选地，方法还包括：

接收待抓取负载的尺寸参数，依据待抓取负载的尺寸参数及机械手末端执行控制点的坐标计算待抓取负载的边界坐标，以及当判断待抓取负载任一边界坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向动作；待抓取负载的尺寸参数包括待抓取负载底部与机械手末端执行控制点的垂直距离以及待抓取负载轮廓边界与机械手末端执行控制点的水平距离。

本申请是参照根据本申请实施例的方法、装置(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上结合附图详细描述了本发明的可选实施方式，但是，本发明实施方式并不限于上述实施方式中的具体细节，在本发明实施方式的技术构思范围内，可以对本发明实施方式的技术方案进行多种简单变型，这些简单变型均属于本发明实施方式的保护范围。

另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本发明实施方式对各种可能的组合方式不再另行说明。

本领域技术人员可以理解实现上述实施方式的方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得单片机、芯片或处理器(processor)执行本发明各个实施方式所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

Claims (6)

1.一种随车机械手控制系统，其特征在于，包括：控制单元；

所述控制单元被配置为：

基于机械手建立三维坐标系，依据所述三维坐标系建立障碍物虚拟墙坐标点，所述障碍物虚拟墙坐标点的集合构成障碍物虚拟墙；

通过检测组件检测机械手控制点的位置信号，依据机械手控制点的位置信号及所述三维坐标系计算机械手控制点的坐标，所述位置信号包括机械手控制点的俯仰角度值、回转角度值及伸缩位移量；

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向动作；

所述判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向动作，实施为包括：

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第一阈值时，控制所述机械手控制点的俯仰角度值在第一俯仰角度值以内、控制所述机械手控制点的回转角度值在第一回转角度值以内，以及控制所述机械手控制点的伸缩位移量在第一长度值以内；同时，降低所述机械手控制点的俯仰速度、回转速度及伸缩速度；

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第二阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向动作；

所述第一阈值大于所述第二阈值；

或者，所述判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向动作，实施为包括：

实时检测机械手的动作速度；

当机械手的动作速度达到第一速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第三阈值时，控制机械手降低动作速度，直至机械手停止向靠近所述障碍物虚拟墙的方向动作；

当机械手的动作速度达到第二速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第四阈值时，控制机械手降低动作速度，直至机械手停止向靠近所述障碍物虚拟墙的方向动作；

当机械手的动作速度达到第三速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第五阈值时，控制机械手降低动作速度，直至机械手停止向靠近所述障碍物虚拟墙的方向动作；

所述第一速度大于所述第二速度，所述第二速度大于所述第三速度，所述第三阈值大于所述第四阈值，所述第四阈值大于所述第五阈值。

2.根据权利要求1所述的一种随车机械手控制系统，其特征在于，所述检测组件包括：

用于检测所述机械手控制点俯仰角度值的第一角度传感器、用于检测所述机械手控制点回转角度值的第二角度传感器以及用于检测所述机械手控制点伸缩位移量的距离传感器。

3.根据权利要求1所述的一种随车机械手控制系统，其特征在于，所述控制单元还被配置为：

接收待抓取负载的尺寸参数，依据所述待抓取负载的尺寸参数及机械手末端执行控制点的坐标计算所述待抓取负载的边界坐标，以及当判断所述待抓取负载任一边界坐标与任一障碍物虚拟墙坐标点的坐标的直线距离小于阈值时，控制所述机械手停止向靠近所述障碍物虚拟墙的方向动作；所述待抓取负载的尺寸参数包括所述待抓取负载底部与所述机械手末端执行控制点的垂直距离以及所述待抓取负载轮廓边界与所述机械手末端执行控制点的水平距离。

4.一种随车机械手控制方法，其特征在于，包括：

基于所述机械手建立三维坐标系，依据所述三维坐标系建立障碍物虚拟墙坐标点，所述障碍物虚拟墙坐标点的集合构成障碍物虚拟墙；

通过检测组件检测机械手控制点的位置信号，依据机械手控制点的位置信号及所述三维坐标系计算机械手控制点的坐标，所述位置信号包括机械手控制点的俯仰角度值、回转角度值及伸缩位移量；

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向动作；

所述判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向动作，实施为包括：

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第一阈值时，控制所述机械手控制点的俯仰角度值在第一俯仰角度值以内、控制所述机械手控制点的回转角度值在第一回转角度值以内，以及控制所述机械手控制点的伸缩位移量在第一长度值以内；同时，降低所述机械手控制点的俯仰速度、回转速度及伸缩速度；

判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的坐标的直线距离小于第二阈值时，控制所述机械手控制点停止向靠近所述障碍物虚拟墙的方向动作；

所述第一阈值大于所述第二阈值；

或者，所述判断机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于阈值时，控制机械手停止向靠近所述障碍物虚拟墙的方向动作，实施为包括：

实时检测机械手的动作速度；

当机械手的动作速度达到第一速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第三阈值时，控制机械手降低动作速度，直至机械手停止向靠近所述障碍物虚拟墙的方向动作；

当机械手的动作速度达到第二速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第四阈值时，控制机械手降低动作速度，直至机械手停止向靠近所述障碍物虚拟墙的方向动作；

当机械手的动作速度达到第三速度时，且机械手任一控制点的坐标与任一障碍物虚拟墙坐标点的最小距离小于第五阈值时，控制机械手降低动作速度，直至机械手停止向靠近所述障碍物虚拟墙的方向动作；

所述第一速度大于所述第二速度，所述第二速度大于所述第三速度，所述第三阈值大于所述第四阈值，所述第四阈值大于所述第五阈值。

5.根据权利要求4所述的一种随车机械手控制方法，其特征在于，所述检测组件包括：

用于检测所述机械手控制点俯仰角度值的第一角度传感器、用于检测所述机械手控制点回转角度值的第二角度传感器以及用于检测所述机械手控制点伸缩位移量的距离传感器。

6.根据权利要求4所述的一种随车机械手控制方法，其特征在于，所述方法还包括：

接收待抓取负载的尺寸参数，依据所述待抓取负载的尺寸参数及机械手末端执行控制点的坐标计算所述待抓取负载的边界坐标，以及当判断所述待抓取负载任一边界坐标与任一障碍物虚拟墙坐标点的坐标的最小距离小于阈值时，控制所述机械手停止向靠近所述障碍物虚拟墙的方向动作；所述待抓取负载的尺寸参数包括所述待抓取负载底部与所述机械手末端执行控制点的垂直距离以及所述待抓取负载轮廓边界与所述机械手末端执行控制点的水平距离。

Images