CN114341761B - 防撞方法、移动机器及存储介质 - Google Patents
防撞方法、移动机器及存储介质 Download PDFInfo
- Publication number
- CN114341761B CN114341761B CN202180004867.2A CN202180004867A CN114341761B CN 114341761 B CN114341761 B CN 114341761B CN 202180004867 A CN202180004867 A CN 202180004867A CN 114341761 B CN114341761 B CN 114341761B
- Authority
- CN
- China
- Prior art keywords
- mobile machine
- point
- rotation
- distance
- calculated
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 238000003860 storage Methods 0.000 title claims abstract description 27
- 230000033001 locomotion Effects 0.000 claims description 34
- 230000015654 memory Effects 0.000 claims description 19
- 238000004590 computer program Methods 0.000 claims description 8
- 238000012545 processing Methods 0.000 description 55
- 238000004891 communication Methods 0.000 description 20
- 238000001514 detection method Methods 0.000 description 13
- 238000010586 diagram Methods 0.000 description 12
- 238000004364 calculation method Methods 0.000 description 8
- 230000007246 mechanism Effects 0.000 description 5
- 230000008569 process Effects 0.000 description 4
- 230000005540 biological transmission Effects 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000007667 floating Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 239000007787 solid Substances 0.000 description 2
- 241001465754 Metazoa Species 0.000 description 1
- 108010001267 Protein Subunits Proteins 0.000 description 1
- -1 acryl Chemical group 0.000 description 1
- 238000010411 cooking Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 239000011521 glass Substances 0.000 description 1
- 238000011065 in-situ storage Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000010408 sweeping Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Measurement Of Optical Distance (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
- Image Analysis (AREA)
Abstract
一种防撞方法、移动机器及存储介质,应用于防撞技术领域。该方法用于具有多个传感器的移动机器,包括:融合从该多个传感器接收的传感器数据以获得对应于该物体的多个数据点,计算该移动机器与各个数据点之间的距离的封闭解,基于该移动机器与多个数据点之间的最短距离以及该移动机器的当前速度计算该移动机器的最大允许速度,以及控制该移动机器按照该最大允许速度移动,从而避免该移动机器与碰撞物体发生碰撞。
Description
技术领域
本申请属于防撞技术,尤其涉及一种防撞方法、移动机器及存储介质。
背景技术
机器人可以以自主或半自主的方式工作,以执行例如自主导航和自动驾驶。因为在日常生活中,有各种各样的机器人,比如:家务机器人、做饭机器人、早教机器人等。在无人看管的情况下,安全是这些机器人设计的一个重要课题。
防撞是保障其使用安全的最重要手段之一,尤其是对于人形机器人、扫地机器人、自动驾驶汽车等经常在地面上自动移动的移动机器人,因为不可避免存在一些地面上的障碍(例如:垃圾、石头、车辆、人等)引起碰撞、进而影响移动机器人在地面上的移动。
在现有的防撞技术中,有一种机器人控制方法通过距离传感器检测与障碍物的距离、并根据该距离识别危险区域,从而实现防撞。再来,确定机器人在危险区域移动而不会与障碍物碰撞的安全速度。然而,上述控制方法通常对于小物体、透明物体和不透明物体的检测有不敏感的缺点,而且计算距离的速度缓慢,这对机器人的避免碰撞和刹车影响很大。
发明内容
本申请提供一种防撞方法、移动机器及存储介质,用以通过结合多种传感器数据,解决现有技术中对小物体、透明物体和不透明物体的检测不敏感等问题。
本申请的实施例提供了一种防撞方法,用于避免移动机器与物体的碰撞,其中该移动机器具有多个传感器,所述方法包括:
融合从多个传感器接收的传感器数据以获得对应于该物体的多个数据点;
计算该移动机器与各个数据点之间的距离的封闭解;
基于该移动机器与多个数据点之间的最短距离以及该移动机器的当前速度计算该移动机器的最大允许速度;以及
控制该移动机器按照该最大允许速度移动。
进一步的,所述计算该移动机器与各个数据点之间的距离的封闭解包括:
通过将该移动机器的线速度除以该移动机器的角速度来计算该移动机器的旋转半径;
根据该旋转半径获取该移动机器对应于各个数据点的碰撞点;以及
根据该旋转半径计算该碰撞点和相应的数据点相对于旋转中心的角度,并通过将该角度和该旋转中心与该相应的数据点之间的距离相乘来计算该碰撞点和该相应的数据点之间的距离。
进一步的,所述根据该旋转半径计算该碰撞点和该相应的数据点相对于该旋转中心的角度包括:
通过以下式子计算该旋转中心和该移动机器的圆形足迹之间的最短距离a:
a=|r-sgn(r)n|;
其中,r是该移动机器的该旋转半径,n是该圆形足迹的半径;
通过以下式子计算该旋转中心和该移动机器的该圆形足迹之间的最长距离c:
c=|r+sgn(r)n|;
通过以下式子计算该旋转中心与该相应的数据点之间的距离h:
其中,px是该相应的数据点的x坐标,py是该相应的数据点的y坐标;
当h≥a且h≤c时,通过以下式子计算该碰撞点的坐标(qx,qy):
其中,v是该移动机器的该线速度;
通过以下式子计算该碰撞点和该相应的数据点之间的中点的坐标(kx,ky):
(kx,ky)=((px+qx)/2,(py+qy)/2);
其中,qx是该碰撞点的x坐标,qy是该碰撞点的y坐标;以及
通过以下式子计算该碰撞点和该相应的数据点相对于该旋转中心的角度θ:
其中,kx是该中点的x坐标,ky是该中点的y坐标;以及
所述通过将该角度和该旋转中心与该相应的数据点之间的距离相乘来计算该碰撞点和该相应的数据点之间的距离包括:
通过以下式子计算该碰撞点与该相应的数据点之间的距离l:
l=θh。
进一步的,所述根据该旋转半径计算该碰撞点和该相应的数据点相对于该旋转中心的角度包括:
通过以下式子计算该碰撞点和该相应的数据点之间的中点的坐标(kx,ky):
(kx,ky)=((px+qx)/2,(py+qy)/2);
其中,px是该相应的数据点的x坐标,qx是该碰撞点的x坐标,py是该相应的数据点的y坐标,qy是该碰撞点的y坐标;以及
通过以下式子计算该碰撞点和该相应的数据点相对于该旋转中心的角度θ:
其中,kx是该中点的x坐标,ky是该中点的y坐标,r是该移动机器的该旋转半径;以及
所述通过将该角度和该旋转中心与该相应的数据点之间的距离相乘来计算该碰撞点和该相应的数据点之间的距离包括:
通过以下式子计算该旋转中心与该相应的数据点之间的距离h:
以及
通过以下式子计算该碰撞点与该相应的数据点之间的距离l:
l=θh。
进一步的,所述根据该旋转半径获取该移动机器对应于各个数据点的该碰撞点包括:
当该线速度不是0时:
通过以下式子计算该旋转中心与该移动机器的矩形足迹的最近侧边之间的距离a:
a=|r-sgn(r)n|;
其中,n是该矩形足迹的一半宽度;
通过以下式子计算该旋转中心和该移动机器的该矩形足迹的多个角之间的最短距离b:
其中,m是该矩形足迹的一半长度;
通过以下式子计算该旋转中心和该移动机器的该矩形足迹的该多个角之间的最长距离c:
当h≥a且h<b时,通过以下式子计算该碰撞点的坐标(qx,qy):
以及
当h≥b且h≤c时,通过以下式子计算该碰撞点的坐标(qx,qy):
其中,v是该移动机器的该线速度;以及
当该线速度是0时:
当px≥m且-n≤py≤n时,通过以下式子计算该碰撞点的坐标(qx,qy):
其中,ω是该移动机器的该角速度;
当px≤-m且-n≤py≤n时,通过以下式子计算该碰撞点的坐标(qx,qy):
当-m≤px≤m且py≥n时,通过以下式子计算该碰撞点的坐标(qx,qy):
以及
当-m≤px≤m且py≤-n时,通过以下式子计算该碰撞点的坐标(qx,qy):
进一步的,所述该移动机器与各个数据点之间的距离的封闭解包括:
基于该移动机器的线速度和角速度确定该移动机器的运动状态;以及
根据该运动状态计算该移动机器与各个数据点之间的距离的封闭解。
进一步的,该多个传感器中的一个是深度相机,所述融合从该多个传感器接收的该传感器数据以获得对应于该物体的该多个数据点包括:
对接收自该深度相机的该传感器数据进行分割,以得到对应于该物体的经分割的传感器数据;
基于该经分割的传感器数据估计该物体的速度,并基于该速度预测该物体的轨迹;
将该经分割的传感器数据与接收自该多个传感器中的另一个的该传感器数据进行融合,以得到对应于该物体的该多个数据点;以及
将所预测的该轨迹中的最近点添加到该多个数据点中。
进一步的,所述基于该移动机器与该多个数据点之间的该最短距离以及该移动机器的该当前速度计算该移动机器的该最大允许速度包括:
获取该移动机器与该多个数据点之间的该最短距离l;
通过以下式子计算最大允许线速度
其中,vc是该移动机器的当前线速度,d=dmax-dmin,dmax是最大有效范围,dmin是最小有效范围;以及
通过以下式子计算最大允许角速度
其中,ωc是该移动机器的当前角速度, 是最大有效范围,/>是最小有效范围。
本申请的实施例还提供了一种移动机器,包括:多个传感器;一个或多个处理器;以及一或多个存储器,存储有一个或多个计算机程序。其中,该一个或多处理器用于执行该一个或多个计算机程序以实施前述防撞方法。。
本申请的实施例还提供了一种存储介质,存储有一或多个计算机程序,该一或多个计算机程序被执行时,实施前述防撞方法。
从上述本申请的实施例可知,本申请提供的防撞方法、移动机器及存储介质,通过结合多种传感器数据来提高移动机器对各种障碍物的检测感知力和准确度,从而解决现有技术中对小物体、透明物体和不透明物体的检测不敏感等问题,同时通过在计算碰撞距离的过程中使用封闭解来减少移动机器对碰撞避免的响应时间。
附图说明
为了更清楚地说明本申请的实施例中的技术方案,下面对实施例中或现有技术的描述中所使用的附图进行简要的介绍。在以下的附图中,相似的标号指代的是在这些图中的对应构件。应当理解的是,以下描述中的附图仅为本申请的示例。对于本领域的技术人员来说,在没有创造性劳动的情况下,可以基于这些附图来获得其他的附图。
图1A是本申请的一些实施例中的移动机器的透视图。
图1B是说明图1A的移动机器的示意框图。
图2是在图1A的移动机器中实现防撞的示意框图。
图3是本申请的一些实施例中的移动机器上布置传感器的示意图。
图4是在图2的防撞机制中计算碰撞距离的示意框图。
图5是图1A的移动机器的运动状态的示意框图。
图6A是图1A的具有圆形足迹的移动机器的移动的示意图。
图6B是在图1A的具有圆形足迹的移动机器的移动中计算碰撞点的示意图。
图6C是在图1A的具有圆形足迹的移动机器的移动中计算碰撞点和相应的数据点之间的中点的示意图。
图7A是图3的具有矩形足迹的移动机器的移动的示意图。
图7B是在图3的具有矩形足迹的移动机器的移动中计算碰撞点的示意图。
图8是图的2防撞机制中最大允许线速度的计算函数的效果的示意图。
具体实施方式
改:为使本申请的目的、特征和优点更加明显易懂,下面将结合附图对本申请的实施例中的技术方案进行清楚、完整的描述。显然,所描述的实施例仅仅是本申请的一部分实施例,而非全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例,都在本申请所保护的范围内。
应当理解的是,当在本申请的说明书和所附的权利要求中使用时,术语″包括″、″包含″、″具有″及其变体表示所述特征、整体、步骤、操作、元素和/或组件的存在,但不排除可以存在或添加一个或多个其他特征、整体、步骤、操作、元素、组件和/或其集合。
还应当理解的是,在本申请的说明书中所使用的术语仅仅是出于描述特定实施例的目的,而非限制本申请的范围。如同在本申请的说明书和所附的权利要求中所使用的那样,除非上下文清楚地指明了其他情况,否则单数形式的″一″、″一个″以及″该″意在包括复数形式。
还应当进一步理解,在本发明说明书和所附的权利要求中所使用的术语″和/或″是指相关列出的项中的一个或多个的任何组合以及所有可能组合,并且包括这些组合。
本申请中的术语″第一″、″第二″、″第三″仅仅是出于描述的目的,而不能理解为指示或暗示了相对重要性、或是暗示了所指的技术特征的数量。由此,由″第一″、″第二″、″第三″所限定的特征可以显式或隐式地包括其中的至少一个技术特征。在本申请的描述中,″多个″的含义是至少两个,例如两个、三个等,除非另有明确的定义。
在本申请的说明书中叙述的″一个实施例″或″一些实施例″等意味着在本申请的一个或多个实施例中可以包括与该实施例的描述内容相关的特定特征、结构或特点。由此,在说明书的不同地方出现的语句″在一个实施例中″、″在一些实施例中″、″在其他一些实施例中″、″在另外一些实施例中″等不是意味着所描述的实施例应该被所有其他实施例所引用,而是被″一个或多个但不是所有其他实施例所引用″,除非另有特别强调。
本申请涉及了用于移动机器的防撞机制。这里所使用的术语″防撞″是指防止或降低碰撞的严重性,″移动机器″是指具有在其环境中四处移动的能力的诸如车辆或移动机器人之类的机器,″导航″是指监视和控制移动机器从一个地方到另一个地方的运动的过程,″传感器″是指目的在于检测其环境中的事件或变化并将相关信息发送到其他电子设备(例如处理器)的设备、模块、机器或子系统(例如环境光传感器和图像传感器),而传感器的术语″融合″是指整合多个传感器以产生比任何单个传感器所能提供的信息更加一致、准确和有用的信息的过程。
图1A是本申请的一些实施例中的移动机器100的透视图;图1B是说明图1A的移动机器100的示意框图。移动机器100可以是移动机器人,例如轮式机器人,其可以包括通过一条或多条通信总线或信号线L进行通信的处理单元110、存储单元120和控制单元130。应当注意的是,移动机器100仅仅是移动机器的一个示例,并且可以具有比以上或以下所示更多或更少的组件(例如,单元、子单元和模块),可以组合两个或更多个组件,或者可以具有不同的组件配置或排列。
处理单元110执行存储在存储单元120中的各种(各组)指令,这些指令可以是以软件程序的形式来执行移动机器100的各种功能并处理相关数据。处理单元110可以包括一个或多个处理器(例如CPU)。存储单元120可以包括一个或多个存储器(例如高速随机存取存储器(RAM)和非暂态存储器)、一个或多个存储控制器以及一个或多个非暂态计算机可读存储介质(例如固态驱动器(SSD))。控制单元130可以包括各种控制器(例如相机控制器、显示控制器和物理按钮控制器)和用于将移动机器100的输入输出外围设备的外围设备接口,例如外部端口(比如USB)、无线通信电路(比如RF通信电路)、音频电路(比如扬声器电路)、传感器(比如RGB-D相机、激光雷达和加速度计)等,耦合至处理单元110和存储单元120。
存储单元120可以包括导航模块121,导航模块121可以存储在一个或多个存储器(以及一个或多个非易失性计算机可读存储介质)中。导航模块121可以是具有用于实现移动机器100的导航的指令In和防撞子模块1211的软件模块。防撞子模块1211可以是具有用于实现移动机器100的防撞的指令Ic的软件模块,指令Ic可以是用于实现移动机器100的导航和防撞的指令In的一部分,可以是与指令Ic分离的子模块,也可以是导航模块121的其他子模块。防撞子模块1211还可以具有数据(例如与移动机器100的碰撞避免相关的输入/输出数据和临时数据),该数据可以存储在一个或多个存储器中、并且由处理单元110访问。在其他实施例中,导航模块121可以是通过一条或多条通信总线或信号线L与处理单元110、存储单元120和控制单元130通信的导航单元,还可以包括用于存储指令In和避免碰撞子模块1211的一个或多个存储器(例如,高速随机存取存储器(RAM)和非暂态存储器),以及用于执行所存储的指令In和Ic以实现移动机器100的导航和防撞的一个或多个处理器(例如MPU或MCU)。
移动机器100还可包括传感器S(图未示),用于检测其所处的环境以实现其导航。传感器S通过一条或多条通信总线或信号线与控制单元130进行通信,所述一条或多条通信总线或信号线可以与上述一条或多条通信总线或信号线L相同或至少部分不同。在其他实施例中,在导航模块121是上述的导航单元的情况下,传感器S可以通过一条或多条通信总线或信号线与导航单元进行通信,所述一条或多条通信总线或信号线可以与上述一条或多条通信总线或信号线L相同或至少部分不同。
移动机器100还可以包括相机组131、通信子单元132和致动子单元133。相机组131、通信子单元132和致动子单元133通过一条或多条通信总线或信号线与控制单元130进行通信,所述一条或多条通信总线或信号线可以与上述一条或多条通信总线或信号线L相同或至少部分不同。
相机组131用于采集移动机器100所处环境的静止图像或视频,其可以包括一个或多个摄像头(例如普通摄像头和红外摄像头)。通讯子单元132耦接移动机器100的通讯接口,诸如用于移动机器100与另一设备(例如遥控器或智能电话)通过网络(例如无线网络)进行通信的网络接口1321和I/O接口1322(如物理按钮)。致动子单元133耦接用于通过例如致动轮子W或关节J的马达(motor)来实现移动机器100的运动的部件/装置。
通信子单元132可以包括用于移动机器100的上述通讯接口的控制器,致动子单元133可以包括用于实现移动机器100的运动的上述部件/装置的控制器。在其他实施例中,通信子单元132和/或致动子单元13仅仅是抽象部件,用于表示移动机器100的部件之间的逻辑关系。
图1A和图1B中所示的各种部件可以通过硬件、软件或硬件和软件的组合来实现。在一些实施例中,处理单元110、存储单元120、控制单元130、导航模块121和其他单元/子单元/模块中的两个或更多个可以实现在单个芯片或电路上。在其他实施例中,它们的至少一部分可以在单独的芯片或电路上实现。
图2是在图1A的移动机器100中实现防撞的示意框图。在一些实施例中,在移动机器100中通过将对应于防撞方法的指令(组)Ic在导航模块121的一个或多个存储器中存储为防撞子模块1211、并通过导航模块121的一个或多个处理器执行所存储的指令Ic来实现移动机器100的防撞方法,从而避免移动机器100与例如人、车辆、垃圾或其他障碍物的碰撞物体C(见图6B和图7B)发生碰撞。
根据该防撞方法,处理单元110融合从传感器S接收的传感器数据以获得对应于对象C的数据点P(框210)。在一些实施例中,传感器S可以包括RGB-D相机S1、激光雷达(LiDAR)S2、声纳S3和红外测距仪(infrared range finder,简称IR)S4。诸如卡尔曼滤波之类的融合算法可用于融合接收到的传感器数据,以便例如减少由噪声数据所引起的不确定性、融合具有不同速率的数据以及组合相同对象的数据。例如,在移动机器100检测到人(即物体C)站在前面的情况下,激光雷达S2以40赫兹(Hz)的数据传输率检测人的腿,RGB-D相机S1以15赫兹的数据传输率检测人的身体,而后卡尔曼滤波器用于以40赫兹的数据传输率融合激光雷达S2和RGB-D相机S1的传感器数据以输出数据点P(例如一组二维(2D)点)。
在一些实施例中,为了便于检测移动物体C,例如移动的人,从RGB-D相机(或其他深度相机)S1接收的传感器数据可以通过例如生成一个贴合物体C的形状的(矩形)紧密边框(bounding box)、并将该边框投影到与地平面平行的2D平面,从而在被融合之前获得与物体C对应的经分割的传感器数据(该数据被离散化为2D点)(框211),而后可以将经分割的传感器数据和从其他传感器接收的传感器数据进行融合以获得对应于对象C的数据点P(框210)。
处理单元110进一步计算移动机器100与各个数据点P之间的距离的封闭解(即″解析解″),以获得移动机器100与所有数据点P之间的距离(即″碰撞距离”)(框220)。在一些实施例中,该距离包括碰撞点Q与对应的数据点P之间的距离l,以及碰撞点Q与对应的数据点P之间相对于旋转中心O(见图6B和图7B)的角度θ。其中,距离l和角度θ以方程表示,并且共同形成移动机器100和每个数据点P之间的距离的封闭解。
处理单元110进一步基于所获得的移动机器100和所有数据点之间的距离中的最短距离以及移动机器100的当前速度来计算移动机器100的最大允许速度(框230)。在一些实施例中,最大允许速度包括最大允许线速度和最大允许角速度/>当前速度包括当前线速度vc和当前角速度ωc。
处理单元110进一步控制移动机器10按照该最大允许速度(例如最大允许线速度和最大允许角速度/>)移动(框240)。在一些实施例中,处理单元110可以从轮子W的里程表订阅每个轮子W的马达的当前速度,并将根据每个轮子W的马达的当前速度和最大允许速度生成、而可能以电子信号的形式呈现的速度指令(例如在当前速度大于最大允许速度的情况下的减速指令)发送到控制器(例如PID控制器),从而根据速度指令控制轮子W转动,使得移动机器100按照最大允许线速度/>和最大允许角速度/>移动以避免与物体C发生碰撞。在其他实施例中,对于具有腿的移动机器100,例如具有两条腿的双足机器人,处理单元110可以订阅每条腿的关节J的马达的当前速度,并将根据每个关节J的马达的当前速度和最大允许速度生成的速度指令发送到马达的控制器,从而控制关节J按照速度指令转动,使得移动机器100按照最大允许线速度/>阳最大允许角速度/>移动以避免与物体C发生碰撞。
图3是本申请的一些实施例中的移动机器M上布置传感器的示意图。在一些实施例中,在移动机器M(以及图1的移动机器100)中有多个传感器,即安装在移动机器M的正面的一个RGB-D相机S1、一个激光雷达S2、五个声纳S3和六个红外测距仪S4,以便在例如移动机器M向前移动时检测物体C。RGB-D相机S1可能有利于检测具有丰富的特征或轮廓明显的小物体(例如手机和地毯上的水渍),以及上部比下部大的悬浮物体,例如对于有四条腿的桌子,RGB-D摄像头S1可以识别桌子的表面,而激光雷达S2只能检测到桌子的四条腿、而不清楚其上的桌子表面。声纳S3可能有利于检测透明物体(例如玻璃或亚克力)、深色物体(例如黑色物体)和可反射障碍物(例如镜子)。红外测距仪S4可能有利于检测超过5厘米高度的小障碍物。在其他实施例中,传感器S中的上述传感器的数量可以根据实际需要而改变,传感器S可以只包括上述种类传感器中的一部分(例如RGB-D相机S1、激光雷达S2和声纳S3)、并且可以包括其他类型的传感器(例如,雷达)。
在一些实施例中,RGB-D相机S1安装在正面的中间部分、并面向移动机器M的前方(见图1A),以拍摄移动机器M前方的范围R1内的深度图像,该深度图像可以是三维(3D)图像,从而有利于检测尺寸较小的物体C。激光雷达S2安装在正面的底下部分、并面向移动机器M的前方(见图1A),以采集激光雷达数据,该激光雷达数据可以是移动机器M的前方、斜前方、侧方、斜后方的范围R2内的二维数组,从而有利于在更广的范围内检测不透明的浅色物体C。五个声纳S3安装在正面的中间和底下部分,它们之间有一定的间隔(见图1A),每个声纳S3所采集的声纳数据可以是R3范围内的一个二维点,以便五个声纳S3共同采集移动机器M前方、斜前方、侧方的R3范围内的声纳数据,从而有利于检测透明物体C。六个红外测距仪S4以一定间隔排列安装在正面的底边(见图1A),其中每个红外测距仪S4收集距离数据,该距离数据可能是移动机器M前方的R4范围内的一个二维点,从而有利于检测不透明、浅色且高度较小的物体C。
除了安装在移动机器M的正面的五个声纳S3和六个红外测距仪S4之外,还有五个安装在移动机器M的背面的声纳S3和六个红外测距仪S4以便在例如移动机器M向后移动时检测物体C。在其他实施例中,移动机器M上用于检测移动机器M所处环境以实现其导航的传感器S的种类、布置和数量可以根据实际需要而改变,例如,可以将安装在正面的上面部分的相机组131作为传感器S之一来拍摄图像/视频、以检测较远的物体C,也可以在正面安装六个或更多的声纳S3、以缩短它们之间的间隔,从而缩小相邻的两个声纳S3的R3范围内之间的间隙、以改善检测效果。
图4是在图2的防撞机制中计算碰撞距离的示意框图。为了计算碰撞距离(图2的框220),在一些实施例中,处理单元110可通过将移动机器100的线速度v除以移动机器100的角速度ω来计算移动机器100的旋转半径r(即r=v/ω)(框221),根据旋转半径r获取移动机器100对应于各个数据点P的碰撞点Q(框222),即获取所有的数据点P所对应的碰撞点Q,并根据旋转半径r计算碰撞点Q和相应的数据点P相对于旋转中心O(见图6B)(O=(0,r))的角度θ,并通过将角度θ和旋转中心O与相应的数据点P之间的距离h相乘来计算碰撞点Q和相应的数据点P之间的距离l(即l=θh)(框223),即获取所有数据点P所对应的角度θ和距离l。在一些实施例中,线速度v可以通过例如融合从移动机器100的加速度计接收的数据、从轮子W的里程计订阅的上述当前速度和(基于激光雷达的)同步定位与建图(simultaneouslocalization and mapping,简称SLAM)的结果。角速度ω可以通过例如将从移动机器100的惯性测量单元(IMU)接收的数据和从轮子W的里程计订阅的上述当前速度进行融合来获取。
图5是图1A的移动机器100的运动状态的示意框图。移动机器100具有五种状态,对应移动机器100的不同线速度v和角速度ω)。在一些实施例中,当计算碰撞距离(图2的框220)时,处理单元110可以首先据移动机器100的线速度v和角速度ω来确定对应于移动机器100的当前运动(例如,停止、旋转和向前/向后移动)的移动机器100的运动状态,然后根据所确定的运动状态来确定用来获取碰撞点Q(图4的框222)和计算相对角度θ和距离l(图4的方框223)的方法(取决于移动机器100的运动状态,见图6A和图7A),以便于碰撞距离的计算(图4的框220)。若线速度v与角速度ω均不为0,则判断移动机器100处于运动状态I,即圆周运动。若线速度v为0且角速度ω不为0,则判断移动机器100处于状态II,即原地旋转。若线速度v大于0且角速度ω为0,则判断移动机器100处于状态III,即向前移动。若线速度v小于0且角速度ω为0,则判断移动机器100处于状态IV,即向后移动。若线速度v和角速度ω均为0,则判断移动机器100处于状态V,即静止不动。
碰撞距离计算:用于圆形足迹的移动机器
碰撞距离的计算将因移动机器的″足迹″而异,该足迹可以是移动机器的移动底座(或底盘)的轮廓。例如,对于具有圆形移动基座(或底盘,见图1A)的移动机器100,它具有圆形足迹101(见图6A),而对于具有矩形移动基座(或底盘,见图3)的移动机器M,它具有矩形足迹M1(见图3)。移动机器的足迹可以通过以下方式确定:以移动机器的设计图所示的移动机器的移动底座(或底盘)的轮廓为足迹,或分析移动机器的移动底座(或底盘)的俯视图照片来确定移动底座(或底盘)的轮廓、并将确定的轮廓作为足迹。在一些实施例中,为了安全起见,可以根据例如尺寸、重量和/或最大速度将足迹定义为(略)大于移动机器的移动底座(或底盘)的实际轮廓,例如移动机器的尺寸、重量和/或最大速度越大,所定义的足迹就越大。
图6A是图1A的具有圆形足迹101的移动机器100(以及具有椭圆形足迹的另一个移动机器)的移动的示意图。在状态I中,移动机器100如同图6A的(a)部分所示地进行圆周运动、而只会与区域①中的数据点P碰撞,因此区域①之外的数据点P将被忽略。图6B是在图1A的具有圆形足迹101的移动机器100(以及前述的具有椭圆形足迹的移动机器)的移动中计算碰撞点Q的示意图。为了在状态I中获取具有圆形足迹101的移动机器100(或前述的具有椭圆足形迹的移动机器)的碰撞点Q(图4的框222),在一些实施例中,处理单元110可通过以下式子计算旋转中心O与移动机器100的圆形足迹101之间的最短距离a:
a=|r-sgn(r)n|;
其中,r是移动机器100的旋转半径,n是圆形足迹100的半径。
处理单元110进一步通过以下式子计算旋转中心O与移动机器100的圆形足迹101之间的最长距离c:
c=|r+sgn(r)n|
处理单元110进一步通过以下式子计算旋转中心O与相应的数据点P之间的距离h:
其中,px是相应的数据点P的x坐标,py是相应的数据点P的y坐标.
处理单元110进一步在h≥a且h≤c时(即对于图6A的(a)部分的区域①中的数据点P),通过以下式子计算碰撞点Q的坐标(qx,qy):
其中,v是移动机器的100线速度。
图6C是在图1A的具有圆形足迹101的移动机器100(以及前述的具有椭圆形足迹的移动机器)的移动中计算碰撞点Q和相应的数据点P之间的中点的K示意图。为了计算具有圆形足迹101的移动机器100(或前述的具有椭圆形足迹的移动机器)的相对角度θ和距离l(图4的框223),在一些实施例中,处理单元110可以通过以下式子计算碰撞点Q和相应的数据点P之间的中点K的坐标(kx,ky):
(kx,ky)=((px+qx)/2,(py+qy)/2);
其中,qx是碰撞点Q的x坐标,qy是碰撞点Q的y坐标。
处理单元110可以进一步通过以下式子计算碰撞点Q和相应的数据点P相对于旋转中心O的角度θ:
其中,kx是中点K的x坐标,ky是中点K的y坐标。
处理单元110可以进一步通过以下式子计算碰撞点Q与相应的数据点P之间的距离l:
l=θh
在状态II中,移动机器100在原地旋转、而且不会发生碰撞,因此不需要避免碰撞(并且可以忽略所有的数据点P)。
在状态III中,移动机器100如同图6A的(b)部分所示地向前移动,而只会与区域①中的数据点P碰撞,因此区域①之外的数据点P将被忽略。处理单元110可以在-n≤py≤n且时(即对于图6A的(b)部分的区域①中的数据点P),通过以下式子计算碰撞点Q的坐标(qx,qy):
/>
处理单元110可以进一步将碰撞点Q与相应的数据点P相对于旋转中心O的角度θ设为∞。
处理单元l10可以进一步通过以下式子计算碰撞点Q与相应的数据点P之间的距离l:
l=px-qx
在状态IV中,移动机器100如同图6A的(c)部分所示地向后移动,而只会与区域①中的数据点P碰撞,因此区域①之外的数据点P将被忽略。处理单元110在-n≤py≤n且时(即对于图6A的(c)部分的区域①中的数据点P),通过以下式子计算碰撞点Q的坐标(qx,qy):
处理单元110可以进一步将碰撞点Q与相应的数据点P相对于旋转中心O的角度θ设为∞。
处理单元110可以进一步通过以下式子计算碰撞点Q与相应的数据点P之间的距离l:
l=qx-px
在状态V中,移动机器100停留在一个地方、而且不会发生碰撞,因此不需要避免碰撞(并且可以忽略所有的数据点P)。
碰撞距离计算:用于具有矩形足迹的移动机器
图7A是图3的具有矩形足迹M1的移动机器M(以及具有例如正方形、梯形和五边形等其他多边形足迹的另一个移动机器)的移动的示意图。在状态I中,移动机器M如同图7A的(a)部分所示地进行圆周运动、而只会与区域①和②中的数据点P发生碰撞,因此区域①和②之外的数据点P将被忽略。图7B是在图3的具有矩形足迹M1的移动机器M(以及前述的具有其他多边形足迹的移动机器)的移动中计算碰撞点Q的示意图。为了在状态I中获取具有矩形足迹M1的移动机器M(或前述的具有其他多边形足迹的移动机器)的碰撞点Q(图4的框222),在一些实施例中,处理单元110可以通过以下式子计算旋转中心O与移动机器M的矩形足迹M1的最近侧边之间的距离a:
a=|r-sgn(r)n|;
其中,n是矩形足迹M1的一半宽度。
处理单元110可以进一步通过以下式子计算旋转中心O和移动机器M的矩形足迹M1的多个角之间的最短距离b:
其中,m是矩形足迹M1的一半长度;
处理单元110可以进一步通过以下式子计算旋转中心O和移动机器M的矩形足迹M1的该多个角之间的最长距离c:
处理单元110可以进一步在h≥a且h<b时(即对于图7A的(a)部分的区域①中的数据点P),通过以下式子计算碰撞点Q的坐标(qx,qy):
处理单元110可以进一步在h≥b且h≤c时(即对于7A的(a)部分的区域②中的数据点P),通过以下式子计算碰撞点Q的坐标(qx,qy):
其中,v是移动机器M的线速度。
在状态II中,移动机器M如同图7A的(b)部分所示地在原地旋转,而只会与区域①、②、③和④中的数据点P碰撞,因此区域①、②、③和④之外的数据点P将被忽略。为了在状态II中获取具有矩形足迹M1的移动机器M(或或前述的具有其他多边形足迹的移动机器)的碰撞点Q(图4的框222),在一些实施例中,处理单元110可以在px≥m且-n≤py≤n时(即对于图7A的部分(b)的区域①中的数据点P),通过以下式子计算碰撞点Q的坐标(qx,qy):
其中,ω是移动机器M的角速度。
处理单元110可以进一步在px≤-m且-n≤py≤n时(即对于图7A的(b)部分的区域②中的数据点P),通过以下式子计算碰撞点Q的坐标(qx,qy):
处理单元110可以进一步在-m≤px≤m且py≥n时(即对于图7A的(b部分的区域③中的数据点P),通过以下式子计算碰撞点Q的坐标(qx,qy):
处理单元110可以进一步在-m≤px≤m且py≤-n时(即对于图7A的(b)部分的区域④中的数据点P),通过以下式子计算碰撞点Q的坐标(qx,qy):
在状态I和II中,为了计算具有矩形足迹M1的移动机器M(或前述的具有其他多边形足迹的移动机器)的相对角度θ和距离l(图4的框223),在一些实施例中,处理单元110可以通过以下式子计算碰撞点Q和相应的数据点P之间的中点K(见图6C)的坐标(kx,ky):
(kx,ky)=((px+qx)/2,(py+qy)/2);
其中,px是相应的数据点P的x坐标,qx是碰撞点Q的x坐标,py是相应的数据点P的y坐标,qy是碰撞点Q的y坐标。
处理单元110可以进一步通过以下式子计算碰撞点Q和相应的数据点P相对于旋转中心O的角度θ:
其中,kx是中点K的x坐标,ky是中点K的y坐标,r是移动机器M的旋转半径。
处理单元110可以进一步通过以下式子计算碰撞点Q与相应的数据点P之间的距离l:
l=θh
在状态III中,移动机器M如同图7A的(c)部分所示地向前移动,而只会与区域①中的数据点P碰撞,因此区域①之外的数据点P将被忽略。处理单元110可以进一步在px≥m且-n≤py≤n时(即对于图7A的(c)部分的区域①中的数据点P),通过以下式子计算碰撞点Q的坐标(qx,qy):
(qx,qy)=(m,py)
处理单元110可以进一步将碰撞点Q与相应的数据点P相对于旋转中心O的角度θ设为∞。
处理单元110可以进一步通过以下式子计算碰撞点Q与相应的数据点P之间的距离l:
l=px-qx
在状态IV中,移动机器M如同图7A的(d)部分所示地向后移动,而只只会与区域①中的数据点P碰撞,因此区域①之外的数据点P将被忽略。处理单元110可以在px≥m且-n≤py≤n时(即对于图7A的部分(d)的区域①中的数据点P),通过以下式子计算碰撞点Q的坐标(qx,qy):
(qx,qy)=(-m,py)
处理单元110可以进一步将碰撞点Q与相应的数据点P相对于旋转中心O的角度θ设为∞。
处理单元110可以进一步通过以下式子计算碰撞点Q与相应的数据点P之间的距离l:
l=qx-px
需要注意的是,所有计算出的距离l和角度θ都用方程、而非数值表示,因此,每个距离l及其对应的角度θ共同形成了移动机器100和每个数据点P之间的距离的封闭解。在一些实施例中,为了防止与如移动中的人的移动物体C发生碰撞,在对传感器数据进行分段以获取对应于对象C的经分割的传感器数据(图2的方框211)之后,处理单元110可以基于经分割的传感器数据来估计对象C的速度、并基于该速度来预测对象C的轨迹;而且在计算碰撞距离(图2的框220)之前,处理单元110可以将所预测的轨迹中的最近点添加到多个数据点中,使得在计算碰撞距离(图2的框220)时也可以考虑物体C的预测轨迹中的最近点。
为了计算最大允许速度(图2的框230),在一些实施例中,处理单元110可以获取移动机器100和数据点P之间的最短距离l,并通过以下式子计算最大允许线速度
其中,vc是移动机器100的当前线速度,d=dmax-dmin,dmax是最大有效范围(例如dmax=12m),dmin是最小有效范围(例如dmin=0.2m)。只有在有效范围d内的数据点P会被考虑。图8是图的2防撞机制中的前述式子的效果的示意图。当刹车距离为1米时,最大允许线速度/>应该是0.715541753米/秒。最大允许线速度/>取决于移动机器100所需的刹车距离。例如,对于0.2米或以下的刹车距离,最大允许线速度/>应该是0米/秒。在一些实施例中,可以根据例如移动机器100的尺寸和/或最大速度来定义移动机器100的刹车距离。例如,尺寸越大和/或移动机器100的最大速度越大,所定义的刹车距离越大。/>
处理单元110可以进一步通过以下式子计算最大允许角速度
其中,ωc是移动机器100的当前角速度, 是最大有效范围(例如),/>是最小有效范围(例如/>)。只有在有效范围/>内的数据点P会被考虑。
根据实验中的测试,如果移动机器100被控制为按照计算出的最大允许线速度(见图8)和最大允许角速度/>移动,则其可以在所需刹车距离内停止而不与超过5厘米高度的小物体C(例如夹克、毛巾、小包)和上述的具有0.6米到0.8米高度的表面的悬浮物体C(例如桌子和椅子)碰撞。
图2中的防撞方法的优点如下。第一,它可以与多种传感器协同工作,通过结合多种传感器数据来提高移动机器100对包括小型、透明、悬浮、移动和静态障碍物的各种障碍物的检测感知力和准确度。第二,它减少了移动机器100对碰撞避免的响应时间,同时节省了计算资源,因为在碰撞距离的计算过程中使用了封闭解(即解析解)、而非更复杂的数值解。第三,通过将移动机器100的当前速度作为确定最大允许速度的一个重要因素,在确保移动机器100平稳移动的同时实现移动机器100的碰撞避免。第四,它通过融合传感器数据来减少检测的不确定性,从而实现高系统鲁棒性。第五,它可以在简单的结构上实现,并且易于实现和维护。
图2中的防撞方法可以在例如室内或室外、或者不同场景(比如医院、工厂、仓库、商场、或街道)的很多领域的移动机器上实现。在一些实施例中,移动机器可以是任何类型的地面车辆,例如独轮车/类汽车机器人或全向移动机器人。它赋予移动机器有效避免与人、汽车、动物、地面突起、墙壁和家具等障碍物碰撞的能力。
本领域技术人员可以理解,上述实施例中的全部或部分方法(例如图2的碰撞避免)可以通过一个或多个计算机程序来实现指示相关硬件。此外,一个或多个程序可以存储在非易失性计算机可读存储介质中。当执行一个或多个程序时,执行上述实施例中的相应方法的全部或部分。对存储(storage)、存储器、数据库或其他介质的任何引用均可包括非易失性和/或易失性存储器。非易失性存储器可以包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)、闪存、固态驱动器(SSD)等。易失性存储器可以包括随机存取存储器(RAM)、外部高速缓冲存储器等。
处理单元110(和上述处理器)可以包括中央处理单元(central processingunit,简称CPU),或者是其他通用处理器、数字信号处理器(digital signal processor,简称DSP)、专用集成电路(application specific integrated circuit,简称ASIC)、现场可编程门阵列(field-programmable gate array,简称FPGA)或者其他可编程逻辑器件、分立门、晶体管逻辑器件和分立硬件组件。通用处理器可以是微处理器,也可以是任何常规的处理器。存储单元120(和上述存储器)可以包括诸如硬盘和内部存储器的内部存储单元。存储单元120还可以包括例如插入式硬盘、智能媒体卡(smart media card,简称SMC)、安全数字(secure digital,简称SD)卡和闪存卡的外部存储设备。
实施例中所描述的示例性单元/模块和方法/步骤可以通过软件、硬件或者软件和硬件的结合来实现。这些功能究竟是通过软件还是硬件来实现,取决于技术方案的具体应用和设计约束。上述防撞方法和移动机器可以通过其他方式实现。例如,单元/模块的划分仅仅是一种逻辑上的功能划分,在实际实现中还可以采用其他划分方式,即多个单元/模块可以组合或集成到另一个系统中,或者某些特征可以被忽略或不执行。另外,上述的相互耦合/连接可以是直接耦合/连接或通信连接,也可以是通过一些接口/设备的间接耦合/连接或通信连接,还可以是电性、机械或其他的形式。
上述实施例仅用于说明本申请的技术方案,而非用于限制本申请的技术方案。虽然本申请已经结合上述实施例进行了详细说明,但是上述各个实施例中的技术方案仍然可以进行修改,或者对其中部分技术特征可以进行等同替换,使得这些修改或替换并不使得相应技术方案的本质脱离本申请的各个实施例的技术方案的精神和范围,而均应包含在本申请的保护范围内。
Claims (20)
1.一种防撞方法,用于避免移动机器与物体的碰撞,其中该移动机器具有多个传感器,所述方法包括:
融合从多个传感器接收的传感器数据以获得对应于该物体的多个数据点;
根据该移动机器的足迹,计算该移动机器与各个数据点之间的距离的封闭解,其中该距离包括:该移动机器的碰撞点与相应的数据点之间的距离,以及,该碰撞点与该相应的数据点之间相对于该移动机器的旋转中心的角度,该封闭解基于该碰撞点与该相应的数据点之间的距离和该角度形成,该碰撞点与该相应的数据点对应并基于该移动机器的旋转半径和运动状态获得;
基于该移动机器与多个数据点之间的最短距离以及该移动机器的当前速度计算该移动机器的最大允许速度;以及
控制该移动机器按照该最大允许速度移动。
2.根据权利要求1所述的方法,其中所述计算该移动机器与各个数据点之间的距离的封闭解包括:
通过将该移动机器的线速度除以该移动机器的角速度来计算该移动机器的旋转半径;
根据该旋转半径获取该移动机器对应于各个数据点的碰撞点;以及
根据该旋转半径计算该碰撞点和该相应的数据点相对于该旋转中心的角度,并通过将该角度和该旋转中心与该相应的数据点之间的距离相乘来计算该碰撞点和该相应的数据点之间的距离。
3.根据权利要求2所述的方法,其中所述根据该旋转半径计算该碰撞点和该相应的数据点相对于该旋转中心的角度包括:
通过以下式子计算该旋转中心和该移动机器的圆形足迹之间的最短距离a:
;
其中,r是该移动机器的该旋转半径,n是该圆形足迹的半径;
通过以下式子计算该旋转中心和该移动机器的该圆形足迹之间的最长距离c:
;
通过以下式子计算该旋转中心与该相应的数据点之间的距离h:
;
其中,px是该相应的数据点的x坐标,py是该相应的数据点的y坐标;
当h≥a且h≤c时,通过以下式子计算该碰撞点的坐标(qx,qy):
;
其中,v是该移动机器的该线速度;
通过以下式子计算该碰撞点和该相应的数据点之间的中点的坐标(kx,ky):
;
其中,qx是该碰撞点的x坐标,qy是该碰撞点的y坐标;以及
通过以下式子计算该碰撞点和该相应的数据点相对于该旋转中心的角度θ:
;
其中,kx是该中点的x坐标,ky是该中点的y坐标;以及
所述通过将该角度和该旋转中心与该相应的数据点之间的距离相乘来计算该碰撞点和该相应的数据点之间的距离包括:
通过以下式子计算该碰撞点与该相应的数据点之间的距离l:
l=θh。
4.根据权利要求2所述的方法,其中所述根据该旋转半径计算该碰撞点和该相应的数据点相对于该旋转中心的角度包括:
通过以下式子计算该碰撞点和该相应的数据点之间的中点的坐标(kx,ky):
;
其中,px是该相应的数据点的x坐标,qx是该碰撞点的x坐标,py是该相应的数据点的y坐标,qy是该碰撞点的y坐标;以及
通过以下式子计算该碰撞点和该相应的数据点相对于该旋转中心的角度θ:
;
其中,kx是该中点的x坐标,ky是该中点的y坐标,r是该移动机器的该旋转半径;以及
所述通过将该角度和该旋转中心与该相应的数据点之间的距离相乘来计算该碰撞点和该相应的数据点之间的距离包括:
通过以下式子计算该旋转中心与该相应的数据点之间的距离h:
;以及
通过以下式子计算该碰撞点与该相应的数据点之间的距离l:
l=θh。
5.根据权利要求4所述的方法,其中所述根据该旋转半径获取该移动机器对应于各个数据点的该碰撞点包括:
当该线速度不是0时:
通过以下式子计算该旋转中心与该移动机器的矩形足迹的最近侧边之间的距离a:
;
其中,n是该矩形足迹的一半宽度;
通过以下式子计算该旋转中心和该移动机器的该矩形足迹的多个角之间的最短距离b:
;
其中,m是该矩形足迹的一半长度;
通过以下式子计算该旋转中心和该移动机器的该矩形足迹的该多个角之间的最长距离c:
;
当h≥a且h<b时,通过以下式子计算该碰撞点的坐标(qx,qy):
;以及
当h≥b且h≤c时,通过以下式子计算该碰撞点的坐标(qx,qy):
;
其中,v是该移动机器的该线速度;以及
当该线速度是0时:
当px≥m且-n≤py≤n时,通过以下式子计算该碰撞点的坐标(qx,qy):
;
其中,ω是该移动机器的该角速度;
当px≤-m且-n≤py≤n时,通过以下式子计算该碰撞点的坐标(qx,qy):
;
当-m≤px≤m且py≥n时,通过以下式子计算该碰撞点的坐标(qx,qy):
;以及
当-m≤px≤m且py≤-n时,通过以下式子计算该碰撞点的坐标(qx,qy):
。
6. 根据权利要求1所述的方法,其中所述计算该移动机器与各个数据点之间的距离的封闭解包括:
基于该移动机器的线速度和角速度确定该移动机器的运动状态;以及
根据该运动状态计算该移动机器与各个数据点之间的距离的封闭解。
7.根据权利要求l所述的方法,其中该多个传感器中的一个是深度相机,所述融合从该多个传感器接收的该传感器数据以获得对应于该物体的该多个数据点包括:
对接收自该深度相机的该传感器数据进行分割,以得到对应于该物体的经分割的传感器数据;
基于该经分割的传感器数据估计该物体的速度,并基于该速度预测该物体的轨迹;
将该经分割的传感器数据与接收自该多个传感器中的另一个的该传感器数据进行融合,以得到对应于该物体的该多个数据点;以及
将所预测的该轨迹中距离该移动机器的最近点添加到该多个数据点中。
8.根据权利要求1所述的方法,其中所述基于该移动机器与该多个数据点之间的该最短距离以及该移动机器的该当前速度计算该移动机器的该最大允许速度包括:
获取该移动机器与该多个数据点之间的该最短距离l;
通过以下式子计算最大允许线速度:
;
其中,vc是该移动机器的当前线速度,d=dmax -dmin,dmax是最大有效范围,dmin是最小有效范围;以及
通过以下式子计算最大允许角速度:
;
其中,ωc是该移动机器的当前角速度,,φ max是最大有效范围,φ min是最小有效范围,θ是该碰撞点和该相应的数据点相对于该旋转中心的角度。
9.一种移动机器,包括:
多个传感器;
一或多个处理器;以及
一或多个存储器,存储有一或多个计算机程序;
其中,该一或多个计算机程序包括多个指令用于:
融合从多个传感器接收的传感器数据以获得对应于物体的多个数据点;
根据该移动机器的足迹,计算该移动机器与各个数据点之间的距离的封闭解,其中该距离包括:该移动机器的碰撞点与相应的数据点之间的距离,以及,该碰撞点与该相应的数据点之间相对于该移动机器的旋转中心的角度,该封闭解基于该碰撞点与该相应的数据点之间的距离和该角度形成;
基于该移动机器与多个数据点之间的最短距离以及该移动机器的当前速度计算该移动机器的最大允许速度;以及
控制该移动机器按照该最大允许速度移动,该碰撞点与该相应的数据点对应并基于该移动机器的旋转半径和运动状态获得。
10.根据权利要求9所述的移动机器,其中所述计算该移动机器与各个数据点之间的距离的封闭解包括:
通过将该移动机器的线速度除以该移动机器的角速度来计算该移动机器的旋转半径;
根据该旋转半径获取该移动机器对应于各个数据点的碰撞点;以及
根据该旋转半径计算该碰撞点和该相应的数据点相对于该旋转中心的角度,并通过将该角度和该旋转中心与该相应的数据点之间的距离相乘来计算该碰撞点和该相应的数据点之间的距离。
11.根据权利要求10所述的移动机器,其中所述根据该旋转半径计算该碰撞点和该相应的数据点相对于该旋转中心的角度包括:
通过以下式子计算该旋转中心和该移动机器的圆形足迹之间的最短距离a:
;
其中,r是该移动机器的该旋转半径,n是该圆形足迹的半径;
通过以下式子计算该旋转中心和该移动机器的该圆形足迹之间的最长距离c:
;
通过以下式子计算该旋转中心与该相应的数据点之间的距离h:
;
其中,px是该相应的数据点的x坐标,py是该相应的数据点的y坐标;
当h≥a且h≤c时,通过以下式子计算该碰撞点的坐标(qx,qy):
;
其中,v是该移动机器的该线速度;
通过以下式子计算该碰撞点和该相应的数据点之间的中点的坐标(kx,ky):
;
其中,qx是该碰撞点的x坐标,qy是该碰撞点的y坐标;以及
通过以下式子计算该碰撞点和该相应的数据点相对于该旋转中心的角度θ:
;
其中,kx是该中点的x坐标,ky是该中点的y坐标;以及
所述通过将该角度和该旋转中心与该相应的数据点之间的距离相乘来计算该碰撞点和该相应的数据点之间的距离包括:
通过以下式子计算该碰撞点与该相应的数据点之间的距离l:
l=θh。
12.根据权利要求10所述的移动机器,其中所述根据该旋转半径计算该碰撞点和该相应的数据点相对于该旋转中心的角度包括:
通过以下式子计算该碰撞点和该相应的数据点之间的中点的坐标(kx,ky):
;
其中,px是该相应的数据点的x坐标,qx是该碰撞点的x坐标,py是该相应的数据点的y坐标,qy是该碰撞点的y坐标;以及
通过以下式子计算该碰撞点和该相应的数据点相对于该旋转中心的角度θ:
;
其中,kx是该中点的x坐标,ky是该中点的y坐标,r是该移动机器的该旋转半径;以及
所述通过将该角度和该旋转中心与该相应的数据点之间的距离相乘来计算该碰撞点和该相应的数据点之间的距离包括:
通过以下式子计算该旋转中心与该相应的数据点之间的距离h:
;以及
通过以下式子计算该碰撞点与该相应的数据点之间的距离l:
l=θh。
13.根据权利要求12所述的移动机器,其中所述根据该旋转半径获取该移动机器对应于各个数据点的该碰撞点包括:
当该线速度不是0时:
通过以下式子计算该旋转中心与该移动机器的矩形足迹的最近侧边之间的距离a:
;
其中,n是该矩形足迹的一半宽度;
通过以下式子计算该旋转中心和该移动机器的该矩形足迹的多个角之间的最短距离b:
;
其中,m是该矩形足迹的一半长度;
通过以下式子计算该旋转中心和该移动机器的该矩形足迹的该多个角之间的最长距离c:
;
当h≥a且h<b时,通过以下式子计算该碰撞点的坐标(qx,qy):
;以及
当h≥b且h≤c时,通过以下式子计算该碰撞点的坐标(qx,qy):
;
其中,v是该移动机器的该线速度;以及
当该线速度是0时:
当px≥m且-n≤py≤n时,通过以下式子计算该碰撞点的坐标(qx,qy):
;
其中,ω是该移动机器的该角速度;
当px≤-m且-n≤py≤n时,通过以下式子计算该碰撞点的坐标(qx,qy):
;
当-m≤px≤m且py≥n时,通过以下式子计算该碰撞点的坐标(qx,qy):
;以及
当-m≤px≤m且py≤-n时,通过以下式子计算该碰撞点的坐标(qx,qy):
。
14. 根据权利要求9所述的移动机器,其中所述计算该移动机器与各个数据点之间的距离的封闭解包括:
基于该移动机器的线速度和角速度确定该移动机器的运动状态;以及
根据该运动状态计算该移动机器与各个数据点之间的距离的封闭解。
15.根据权利要求9所述的移动机器,其中该多个传感器中的一个是深度相机,所述融合从该多个传感器接收的该传感器数据以获得对应于该物体的该多个数据点包括:
对接收自该深度相机的该传感器数据进行分割,以得到对应于该物体的经分割的传感器数据;
基于该经分割的传感器数据估计该物体的速度,并基于该速度预测该物体的轨迹;
将该经分割的传感器数据与接收自该多个传感器中的另一个的该传感器数据进行融合,以得到对应于该物体的该多个数据点;以及
将所预测的该轨迹中距离该移动机器的最近点添加到该多个数据点中。
16.根据权利要求9所述的移动机器,其中所述基于该移动机器与该多个数据点之间的该最短距离以及该移动机器的该当前速度计算该移动机器的该最大允许速度包括:
获取该移动机器与该多个数据点之间的该最短距离l;
通过以下式子计算最大允许线速度:
;
其中,vc是该移动机器的当前线速度,d=dmax -dmin,dmax是最大有效范围,dmin是最小有效范围;以及
通过以下式子计算最大允许角速度:
;
其中,ωc是该移动机器的当前角速度,,φ max是最大有效范围,φ min是最小有效范围,θ是该碰撞点和该相应的数据点相对于该旋转中心的角度。
17.一种存储介质,存储有一或多个计算机程序,其中该一或多个计算机程序包括多个指令,当该多个指令由具有多个传感器的移动机器执行时,使该移动机器:
融合从多个传感器接收的传感器数据以获得对应于物体的多个数据点;
根据该移动机器的足迹,计算该移动机器与各个数据点之间的距离的封闭解,其中该距离包括:该移动机器的碰撞点与相应的数据点之间的距离,以及,该碰撞点与该相应的数据点之间相对于该移动机器的旋转中心的角度,该封闭解基于该碰撞点与该相应的数据点之间的距离和该角度形成,该碰撞点与该相应的数据点对应并基于该移动机器的旋转半径和运动状态获得;
基于该移动机器与多个数据点之间的最短距离以及该移动机器的当前速度计算该移动机器的最大允许速度;以及
控制该移动机器按照该最大允许速度移动。
18.根据权利要求17所述的存储介质,其中所述计算该移动机器与各个数据点之间的距离的封闭解包括:
通过将该移动机器的线速度除以该移动机器的角速度来计算该移动机器的旋转半径;
根据该旋转半径获取该移动机器对应于各个数据点的碰撞点;以及
根据该旋转半径计算该碰撞点和该相应的数据点相对于该旋转中心的角度,并通过将该角度和该旋转中心与该相应的数据点之间的距离相乘来计算该碰撞点和该相应的数据点之间的距离。
19.根据权利要求18所述的存储介质,其中所述根据该旋转半径计算该碰撞点和该相应的数据点相对于该旋转中心的角度包括:
通过以下式子计算该旋转中心和该移动机器的圆形足迹之间的最短距离a:
;
其中,r是该移动机器的该旋转半径,n是该圆形足迹的半径;
通过以下式子计算该旋转中心和该移动机器的该圆形足迹之间的最长距离c:
;
通过以下式子计算该旋转中心与该相应的数据点之间的距离h:
;
其中,px是该相应的数据点的x坐标,py是该相应的数据点的y坐标;
当h≥a且h≤c时,通过以下式子计算该碰撞点的坐标(qx,qy):
;
其中,v是该移动机器的该线速度;
通过以下式子计算该碰撞点和该相应的数据点之间的中点的坐标(kx,ky):
;
其中,qx是该碰撞点的x坐标,qy是该碰撞点的y坐标;以及
通过以下式子计算该碰撞点和该相应的数据点相对于该旋转中心的角度θ:
;
其中,kx是该中点的x坐标,ky是该中点的y坐标;以及
所述通过将该角度和该旋转中心与该相应的数据点之间的距离相乘来计算该碰撞点和该相应的数据点之间的距离包括:
通过以下式子计算该碰撞点与该相应的数据点之间的距离l:
l=θh。
20.根据权利要求18所述的存储介质,其中所述根据该旋转半径计算该碰撞点和该相应的数据点相对于该旋转中心的角度包括:
通过以下式子计算该碰撞点和该相应的数据点之间的中点的坐标(kx,ky):
;
其中,px是该相应的数据点的x坐标,qx是该碰撞点的x坐标,py是该相应的数据点的y坐标,qy是该碰撞点的y坐标;以及
通过以下式子计算该碰撞点和该相应的数据点相对于该旋转中心的角度θ:
;
其中,kx是该中点的x坐标,ky是该中点的y坐标,r是该移动机器的该旋转半径;以及
所述通过将该角度和该旋转中心与该相应的数据点之间的距离相乘来计算该碰撞点和该相应的数据点之间的距离包括:
通过以下式子计算该旋转中心与该相应的数据点之间的距离h:
;以及
通过以下式子计算该碰撞点与该相应的数据点之间的距离l:
l=θh。
Applications Claiming Priority (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
US17/134,219 | 2020-12-25 | ||
US17/134,219 US11797013B2 (en) | 2020-12-25 | 2020-12-25 | Collision avoidance method and mobile machine using the same |
PCT/CN2021/127836 WO2022134863A1 (zh) | 2020-12-25 | 2021-11-01 | 防撞方法、移动机器及存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114341761A CN114341761A (zh) | 2022-04-12 |
CN114341761B true CN114341761B (zh) | 2024-04-02 |
Family
ID=81022839
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202180004867.2A Active CN114341761B (zh) | 2020-12-25 | 2021-11-01 | 防撞方法、移动机器及存储介质 |
Country Status (2)
Country | Link |
---|---|
JP (1) | JP2024504020A (zh) |
CN (1) | CN114341761B (zh) |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105955303A (zh) * | 2016-07-05 | 2016-09-21 | 北京奇虎科技有限公司 | 无人机自主避障方法、装置 |
CN107817796A (zh) * | 2017-10-25 | 2018-03-20 | 上海思岚科技有限公司 | 移动机器人贴障碍物运动的方法和设备 |
DE102018105014A1 (de) * | 2017-03-06 | 2018-09-06 | GM Global Technology Operations LLC | Vorhersagealgorithmus für einen fahrzeugcrash unter verwendung eines radarsensors und eines upa-sensors |
CN110488319A (zh) * | 2019-08-22 | 2019-11-22 | 重庆长安汽车股份有限公司 | 一种基于超声波和摄像头融合的碰撞距离计算方法及系统 |
CN110928283A (zh) * | 2018-09-18 | 2020-03-27 | 深圳市优必选科技有限公司 | 一种机器人及其智能移动方法和装置 |
CN111267107A (zh) * | 2020-03-23 | 2020-06-12 | 上海高仙自动化科技发展有限公司 | 控制方法、机器人、电子设备和可读存储介质 |
CN111309013A (zh) * | 2020-02-24 | 2020-06-19 | 广州小鹏汽车科技有限公司 | 一种碰撞距离确定方法及系统、车辆、存储介质 |
CN111757822A (zh) * | 2018-02-26 | 2020-10-09 | 联邦快递服务公司 | 用于使用多传感器检测融合来在物流地面支持设备上进行增强的碰撞避免的系统和方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE102019206012A1 (de) * | 2019-04-26 | 2020-10-29 | Kuka Deutschland Gmbh | Verfahren und System zum Betreiben eines Roboters |
-
2021
- 2021-11-01 CN CN202180004867.2A patent/CN114341761B/zh active Active
- 2021-11-01 JP JP2023539382A patent/JP2024504020A/ja active Pending
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105955303A (zh) * | 2016-07-05 | 2016-09-21 | 北京奇虎科技有限公司 | 无人机自主避障方法、装置 |
DE102018105014A1 (de) * | 2017-03-06 | 2018-09-06 | GM Global Technology Operations LLC | Vorhersagealgorithmus für einen fahrzeugcrash unter verwendung eines radarsensors und eines upa-sensors |
CN107817796A (zh) * | 2017-10-25 | 2018-03-20 | 上海思岚科技有限公司 | 移动机器人贴障碍物运动的方法和设备 |
CN111757822A (zh) * | 2018-02-26 | 2020-10-09 | 联邦快递服务公司 | 用于使用多传感器检测融合来在物流地面支持设备上进行增强的碰撞避免的系统和方法 |
CN110928283A (zh) * | 2018-09-18 | 2020-03-27 | 深圳市优必选科技有限公司 | 一种机器人及其智能移动方法和装置 |
CN110488319A (zh) * | 2019-08-22 | 2019-11-22 | 重庆长安汽车股份有限公司 | 一种基于超声波和摄像头融合的碰撞距离计算方法及系统 |
CN111309013A (zh) * | 2020-02-24 | 2020-06-19 | 广州小鹏汽车科技有限公司 | 一种碰撞距离确定方法及系统、车辆、存储介质 |
CN111267107A (zh) * | 2020-03-23 | 2020-06-12 | 上海高仙自动化科技发展有限公司 | 控制方法、机器人、电子设备和可读存储介质 |
Also Published As
Publication number | Publication date |
---|---|
JP2024504020A (ja) | 2024-01-30 |
CN114341761A (zh) | 2022-04-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US10611023B2 (en) | Systems and methods for performing occlusion detection | |
El-laithy et al. | Study on the use of Microsoft Kinect for robotics applications | |
JP5946147B2 (ja) | 可動式ヒューマンインターフェースロボット | |
EP3224649B1 (en) | Systems and methods for performing simultaneous localization and mapping using machine vision systems | |
TWI558525B (zh) | 機器人及其控制方法 | |
US9519289B2 (en) | Systems and methods for performing simultaneous localization and mapping using machine vision systems | |
US11407116B2 (en) | Robot and operation method therefor | |
JP2020126691A (ja) | 移動ロボットの移動の制限 | |
US20180210442A1 (en) | Systems and methods for controlling a vehicle using a mobile device | |
Hinkel et al. | Environment perception with a laser radar in a fast moving robot | |
Kuramachi et al. | G-ICP SLAM: An odometry-free 3D mapping system with robust 6DoF pose estimation | |
WO2019001237A1 (zh) | 一种移动电子设备以及该移动电子设备中的方法 | |
WO2022228019A1 (zh) | 移动目标跟随方法、机器人和计算机可读存储介质 | |
WO2022134863A1 (zh) | 防撞方法、移动机器及存储介质 | |
CN112987748A (zh) | 机器人狭窄空间的控制方法、装置、终端及存储介质 | |
CN114341761B (zh) | 防撞方法、移动机器及存储介质 | |
CN103744110A (zh) | 超声与单目视觉传感器结合的障碍物识别装置 | |
Poomarin et al. | Automatic docking with obstacle avoidance of a differential wheel mobile robot | |
JPS6029995B2 (ja) | 3次元物体の検出方法 | |
Sun et al. | Detection and state estimation of moving objects on a moving base for indoor navigation | |
Yang et al. | A new algorithm for obstacle segmentation in dynamic environments using a RGB-D sensor | |
CN112489131A (zh) | 基于路面检测构建代价地图的方法、装置、介质和机器人 | |
CN112308033A (zh) | 一种基于深度数据的障碍物碰撞警告方法及视觉芯片 | |
AU2015202200A1 (en) | Mobile human interface robot | |
WO2023127337A1 (ja) | 情報処理装置、情報処理方法、及びプログラム |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
TA01 | Transfer of patent application right |
Effective date of registration: 20230224 Address after: Room 202-1, Building 3, No. 8, Shengshui Road, Laoshan District, Qingdao, Shandong 266000 Applicant after: Ubicon (Qingdao) Technology Co.,Ltd. Address before: 518000 16th and 22nd Floors, C1 Building, Nanshan Zhiyuan, 1001 Xueyuan Avenue, Nanshan District, Shenzhen City, Guangdong Province Applicant before: Shenzhen Youbixuan Technology Co.,Ltd. |
|
TA01 | Transfer of patent application right | ||
GR01 | Patent grant | ||
GR01 | Patent grant |