CN106737549B - 一种移动机器人及其防倾覆方法和装置 - Google Patents

Abstract

本发明涉及机器人技术领域，尤其涉及一种移动机器人及其防倾覆方法和装置。该方法包括接收多个姿态传感器的信息，根据多个姿态传感器的信息判断移动机器人的倾覆状态，若移动机器人的倾覆状态为可能倾覆状态，则根据多个姿态传感器的信息计算重心投影偏移矢量d，根据重心投影偏移矢量d，控制移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。当移动机器人遇到凹凸不平、坡道等复杂地形或受到外力作用，移动机器人由于重心偏移而可能发生倾覆，根据重心投影偏移矢量d，控制移动机器人的角速度与线速度，可使移动机器人回复到稳定状态，从而降低了移动机器人由于倾覆带来的损坏，避免经济损失。

Description

一种移动机器人及其防倾覆方法和装置

【技术领域】

本发明涉及机器人技术领域，尤其涉及一种移动机器人及其防倾覆方法和装置。

【背景技术】

轮式移动机器人因其结构简单、承载大、控制机动灵活等优点，而被多数服务移动机器人作为移动机构，如扫地移动机器人，送餐迎宾移动机器人，安防巡逻移动机器人等。当遇到凹凸不平、坡道等复杂地形或受到外力作用，移动机器人由于重心偏移而可能发生倾覆，如果不采取防护措施，会导致移动机器人的损坏，从而造成经济损失。

【发明内容】

本发明针对现有技术移动机器人遇到凹凸不平、坡道等复杂地形或受到外力作用时，由于重心偏移而可能发生倾覆的技术问题，提供一种移动机器人及其防倾覆方法和装置，技术方案如下：

本发明实施例提供一种移动机器人防倾覆方法，包括：

接收多个姿态传感器的信息，其中，所述多个姿态传感器相互之间的连线所在的安装平面与机器人水平支撑面平行，且所述多个姿态传感器的探测方向与机器人本体坐标系的坐标轴重合，所述机器人水平支撑面为所述移动机器人多个运动轮与地面接触点的连线构成的平面；

根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，所述倾覆状态包括可能倾覆状态和稳定状态；

若所述移动机器人的倾覆状态为可能倾覆状态，则根据所述多个姿态传感器的信息计算重心投影偏移矢量d，其中，所述重心投影偏移矢量d由所述移动机器人的质心在机器人水平支撑面内投影变化得到；

根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

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

接收多个防跌红外传感器的信息，其中，所述多个防跌红外传感器分别安装在所述移动机器人的多个运动轮前端，且所述多个防跌红外传感器的探测方向沿垂直方向向下；

根据所述多个防跌红外传感器的信息判断所述移动机器人的悬空状态；

若所述移动机器人的悬空状态为部分悬空状态，则获取悬空位置，控制所述移动机器人往与悬空位置所在的方向相反的方向移动预设距离；

所述根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态包括：

根据所述重心投影偏移矢量d，控制移动预设距离后的所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

可选地，所述运动轮包括驱动轮和从动轮，所述方法还包括：

计算所述移动机器人不同的从动轮旋转角下沿一倾覆轴的倾覆轴稳定角α_i，其中，所述从动轮旋转角的范围为0-2π，所述倾覆轴为机器人水平支撑面的各边所在的直线，所述倾覆轴的数量为多个；

比较多个倾覆轴稳定角α_i，获得最小值min_iα_i＝α^*，其中，α^*为机器人稳定角；

所述根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，所述倾覆状态包括可能倾覆状态和稳定状态包括：

根据所述多个姿态传感器的信息计算当前机器人稳定角α，判断所述移动机器人的倾覆状态，当α＞α^*时，所述移动机器人的倾覆状态为可能倾覆状态，当α≤α^*时，所述移动机器人的倾覆状态为稳定状态。

可选地，所述倾覆轴稳定角α_i为重力矢量和通过移动机器人质心的倾覆轴的法线向量的夹角。

第二方面，本发明实施例还提供一种移动机器人防倾覆装置，包括：

第一接收模块，用于接收多个姿态传感器的信息，其中，所述多个姿态传感器相互之间的连线所在的安装平面与机器人水平支撑面平行，所述机器人水平支撑面为所述移动机器人多个运动轮与地面接触点的连线构成的平面；

倾覆状态判断模块，用于根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，所述倾覆状态包括可能倾覆状态和稳定状态；

第一计算模块，若所述移动机器人的倾覆状态为可能倾覆状态，第一计算模块用于根据所述多个姿态传感器的信息计算重心投影偏移矢量d，其中，所述重心投影偏移矢量d由所述移动机器人的质心在机器人水平支撑面内投影变化得到；

控制模块，用于根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

可选地，所述装置还包括：

第二接收模块，用于接收多个防跌红外传感器的信息，其中，所述多个防跌红外传感器分别安装在所述移动机器人的多个运动轮前端，且所述多个防跌红外传感器的探测方向沿垂直方向向下；

悬空状态判断模块，用于根据所述多个防跌红外传感器的信息判断所述移动机器人的悬空状态；

获取和控制模块，若所述移动机器人的悬空状态为部分悬空状态，所述获取和控制模块用于获取悬空位置，控制所述移动机器人往与悬空位置所在的方向相反的方向移动预设距离；

所述控制模块还用于根据所述重心投影偏移矢量d，控制移动预设距离后的所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

可选地，所述运动轮包括驱动轮和从动轮，所述装置还包括：

第二计算模块，用于计算所述移动机器人不同的从动轮旋转角下沿一倾覆轴的倾覆轴稳定角α_i，其中，所述从动轮旋转角的范围为0-2π，所述倾覆轴为机器人水平支撑面的各边所在的直线，所述倾覆轴的数量为多个；

比较和获取模块，用于比较多个倾覆轴稳定角α_i并获取最小值min_iα_i＝α^*，其中，α^*为机器人稳定角；

所述倾覆状态判断模块还用于根据所述多个姿态传感器的信息计算当前机器人稳定角α，判断所述移动机器人的倾覆状态，当α＞α^*时，所述移动机器人的倾覆状态为可能倾覆状态，当α≤α^*时，所述移动机器人的倾覆状态为稳定状态。

可选地，所述倾覆轴稳定角α_i为重力矢量和通过移动机器人质心的倾覆轴的法线向量的夹角。

第三方面，本发明实施例还提供一种移动机器人，包括：

机身；

多个运动轮，所述多个运动轮安装在机身底部，所述多个运动轮与地面接触点的连线构成的平面为机器人水平支撑面；

多个姿态传感器，所述多个姿态传感器相互之间的连线所在的安装平面与所述机器人水平支撑面平行；

处理器，所述处理器安装在所述机身内，所述处理器用于接收多个姿态传感器的信息；

所述处理器还用于根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，所述倾覆状态包括可能倾覆状态和稳定状态；

若所述移动机器人的倾覆状态为可能倾覆状态，所述处理器还用于根据所述多个姿态传感器的信息计算重心投影偏移矢量d，其中，所述重心投影偏移矢量d由所述移动机器人的质心在机器人水平支撑面内投影变化得到；

所述处理器还用于根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

可选地，所述移动机器人还包括：

多个防跌红外传感器，所述多个防跌红外传感器分别安装在所述多个运动轮前端，且所述多个防跌红外传感器的探测方向沿垂直方向向下；

所述处理器还用于接收多个防跌红外传感器的信息；

所述处理器还用于根据所述多个防跌红外传感器的信息判断所述移动机器人的悬空状态；

若所述移动机器人的悬空状态为部分悬空状态，所述处理器还用于获取悬空位置，控制所述移动机器人往与悬空位置所在的方向相反的方向移动预设距离；

所述处理器还用于根据所述重心投影偏移矢量d，控制移动预设距离后的所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

可选地，所述运动轮包括驱动轮和从动轮，所述处理器还用于：

计算所述移动机器人不同的从动轮旋转角下沿一倾覆轴的倾覆轴稳定角α_i，其中，所述从动轮旋转角的范围为0-2π，所述倾覆轴为机器人水平支撑面的各边所在的直线，所述倾覆轴的数量为多个；

比较多个倾覆轴稳定角α_i，获得最小值min_iα_i＝α^*，其中，α^*为机器人稳定角；

所述根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，所述倾覆状态包括可能倾覆状态和稳定状态包括：

根据所述多个姿态传感器的信息计算当前机器人稳定角α，判断所述移动机器人的倾覆状态，当α＞α^*时，所述移动机器人的倾覆状态为可能倾覆状态，当α≤α^*时，所述移动机器人的倾覆状态为稳定状态。

可选地，所述倾覆轴稳定角α_i为重力矢量和通过移动机器人质心的倾覆轴的法线向量的夹角。

本发明实施例的有益效果在于，本发明实施例提供的移动机器人防倾覆方法，接收多个姿态传感器的信息，根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，若所述移动机器人的倾覆状态为可能倾覆状态，则根据所述多个姿态传感器的信息计算重心投影偏移矢量d，根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。当移动机器人遇到凹凸不平、坡道等复杂地形或受到外力作用，移动机器人由于重心偏移而可能发生倾覆，采用本发明实施例提供的移动机器人防倾覆方法，根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，可使移动机器人回复到稳定状态，从而降低了移动机器人由于倾覆带来的损坏，避免经济损失。

【附图说明】

一个或多个实施例通过与之对应的附图中的图片进行示例性说明，这些示例性说明并不构成对实施例的限定，附图中具有相同参考数字标号的元件表示为类似的元件，除非有特别申明，附图中的图不构成比例限制。

图1为本发明实施例提供的移动机器人的结构示意图。

图2为本发明实施例提供的移动机器人的从动轮运动图。

图3为本发明实施例提供的移动机器人的运动原理示意图。

图4为本发明实施例提供的移动机器人的坐标系的示意图。

图5为本发明实施例提供的移动机器人的质心变化的示意图。

图6为本发明实施例提供的移动机器人的结构框图。

图7为本发明实施例提供的移动机器人防倾覆方法的流程图。

图8为本发明实施例提供的移动机器人防倾覆装置的结构框图。

【具体实施方式】

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1和图6所示，本发明实施例提供一种移动机器人100，包括机身10、多个运动轮20、多个姿态传感器30和处理器40。具体地，运动轮20包括驱动轮21和从动轮23，图1中示意性地画出驱动轮21和从动轮23的数量均为两个。

多个运动轮20安装在机身10底部，所述多个运动轮20与地面接触点的连线构成的平面为机器人水平支撑面；或者说，机器人支撑平面为主动轮21与地面接触点以及从动轮23与地面接触点所构成的凸多边形，图1中示意性地画出两个驱动轮21和两个从动轮23，主动轮21和从动轮23与地面接触点所构成的凸多边形为四边形，其中驱动轮21与地面接触点位置P₁，P₂可以测量，从动轮23与地面接触点P₃，P₄不可测量。一般地，为方便起见，驱动轮旋转中心记为近似地面接触点P₁，P₂，从动轮23的旋转中心记为近似地面接触点P₃，P₄。图2示出一个从动轮与地面接触点的位置从P₃移动到P₃’，另一个从动轮与地面接触点的位置从P₄移动到P₄’。

多个姿态传感器30相互之间的连线所在的安装平面与所述机器人水平支撑面平行；具体地，多个姿态传感器30可以安装在机身10内、机身10上或机身10下，只要多个姿态传感器30相互之间的连线所在的安装平面与机器人水平支撑面平行即可，多个姿态传感器30采集的信息即可被配合利用。

处理器40安装在所述机身10内，所述处理器40用于接收多个姿态传感器30的信息；所述处理器40还用于根据所述多个姿态传感器30的信息判断所述移动机器人100的倾覆状态，所述倾覆状态包括可能倾覆状态和稳定状态；若所述移动机器人100的倾覆状态为可能倾覆状态，所述处理器40还用于根据所述多个姿态传感器30的信息计算重心投影偏移矢量d，其中，所述重心投影偏移矢量d由所述移动机器人100的质心在机器人水平支撑面内投影变化得到；所述处理器40还用于根据所述重心投影偏移矢量d，控制所述移动机器人100的角速度与线速度，以使移动机器人100回复到稳定状态。

本发明实施例的有益效果在于，本发明实施例提供的移动机器人100包括机身10、多个运动轮20、多个姿态传感器30和处理器40，多个运动轮20安装在机身10底部，多个姿态传感器30的安装平面与机器人水平支撑面平行。处理器40用于接收多个姿态传感器的信息，根据所述多个姿态传感器30的信息判断所述移动机器人100的倾覆状态，若所述移动机器人100的倾覆状态为可能倾覆状态，则根据所述多个姿态传感器30的信息计算重心投影偏移矢量d，根据所述重心投影偏移矢量d，控制所述移动机器人100的角速度与线速度，以使移动机器人100回复到稳定状态。当移动机器人100遇到凹凸不平、坡道等复杂地形或受到外力作用，移动机器人100由于重心偏移而可能发生倾覆，本发明实施例提供的移动机器人100，根据所述重心投影偏移矢量d，控制所述移动机器人100的角速度与线速度，可使移动机器人100回复到稳定状态，本发明实施例的移动机器人100可防倾覆，从而降低了由于倾覆带来的损坏，避免经济损失。

进一步地，如图1和6所示，所述移动机器人100还包括多个防跌红外传感器50，多个防跌红外传感器50分别安装在所述多个运动轮20前端，且所述多个防跌红外传感器50的探测方向沿垂直方向向下。具体地，运动轮20包括驱动轮21和从动轮23，每个驱动轮21的前端均安装有防跌红外传感器50，每个从动轮23的前端也均安装有防跌红外传感器50。

所述处理器40还用于接收多个防跌红外传感器50的信息；

所述处理器40还用于根据所述多个防跌红外传感器50的信息判断所述移动机器人100的悬空状态；

若所述移动机器人100的悬空状态为部分悬空状态，所述处理器40还用于获取悬空位置，控制所述移动机器人100往与悬空位置所在的方向相反的方向移动预设距离；具体地，预设距离可根据移动机器人100的机身10悬空的部分是多少而设定。例如，机身10的一角悬空最多处悬空10cm，预设距离可设置为10-15cm，优选地，预设距离设置为12-15cm，移动回来后距离造成悬空的地面留有一定的富余。

所述处理器40还用于根据所述重心投影偏移矢量d，控制移动预设距离后的所述移动机器人100的角速度与线速度，以使移动机器人100回复到稳定状态。

本发明实施例的有益效果在于，当移动机器人100遇到凹凸不平、坡道等复杂地形或受到外力作用，移动机器人100由于重心偏移而可能发生跌倒和倾覆时，本发明实施例提供的移动机器人100的处理器40还用于根据所述多个防跌红外传感器50的信息判断所述移动机器人100的悬空状态，处理器40还用于控制所述移动机器人100往与悬空位置所在的方向相反的方向移动预设距离，然后再根据所述重心投影偏移矢量d，控制所述移动机器人100的角速度与线速度，可使移动机器人100的回复到稳定状态。本发明实施例的移动机器人100先处理防跌倒，再处理防倾覆，从而降低了移动机器人100由于跌倒和倾覆带来的损坏，进一步避免经济损失。

更进一步地，处理器40还用于计算所述移动机器人100不同的从动轮23的旋转角下沿一倾覆轴的倾覆轴稳定角α_i，其中，所述从动轮旋转角的范围为0-2π，所述倾覆轴为机器人水平支撑面的各边所在的直线，所述倾覆轴的数量为多个；图1中示意性地画出机器人支撑平面为主动轮21和从动轮23与地面接触点所构成的四边形，四边形的机器人水平支撑面有四条边，故倾覆轴的数量为四个。移动机器人100沿每个倾覆轴的倾覆轴稳定角α_i，是从动轮23的旋转角在0-2π的范围内变化带来的移动机器人100的重心变化下仍能保持稳定的角度。显然，重力对该轴施加的抗翻转矩越大，则越难沿该轴翻转。

比较多个倾覆轴稳定角α_i，获得最小值min_iα_i＝α^*，其中，α^*为机器人稳定角。以图1为例，设定四条倾覆轴的倾覆轴稳定角α_i为α₁、α₂、α₃和α₄，分别比较α₁、α₂、α₃和α₄，取其中最小值min_iα_i＝α^*，为机器人稳定角。例如α₁、α₂、α₃和α₄分别等于20度、25度、28度和30度，α₁为最小值，α₁＜α₂，移动机器人100最可能沿着α₁所在的倾覆轴倾斜。由于α₂所在的倾覆轴的稳定角度为25度，大于α₁的20度，重力对α₂所在的倾覆轴施加的抗翻转矩大于α₂所在的倾覆轴的翻转矩，而最可能沿着α₁所在的倾覆轴倾斜。

图3为从动轮23对机器人稳定角α^*的影响，其中，横坐标是从动轮旋转角的变化范围0-2π区间4000个的采样值的索引。从图2可以看出，从动轮旋转角在π附近时，相当于移动机器人100直行或少偏移，机器人支撑平面的四边形面积最大时，机器人稳定角α^*最大，移动机器人100最不容易发生倾覆。

所述根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，所述倾覆状态包括可能倾覆状态和稳定状态包括：

根据所述多个姿态传感器的信息计算当前机器人稳定角α，判断所述移动机器人的倾覆状态，当α＞α^*时，所述移动机器人的倾覆状态为可能倾覆状态，当α≤α^*时，所述移动机器人的倾覆状态为稳定状态。

下面结合移动机器人100的机器人本体坐标系和姿态传感器坐标系具体解释移动机器人100如何防跌倒和防倾覆。

移动机器人100在未知的不确定地形中运动时，倾覆将直接造成移动机器人100损坏，因此在设计移动机器人100过程中，以及移动机器人100的运动控制中必须避免此情况的发生。静态稳定判别方法主要有重心投影法(CG Projection Method)，抗倾覆的主要性能指标为机器人稳定角α^*。

移动机器人100的坐标系定义如下，“base_link”为机器人本体坐标系，机器人本体坐标系为XYZ平面坐标系，“IMU”是姿态传感器坐标系。图4示出移动机器人的坐标系。如图4所示，定义θ，φ，ψ分别为绕“base_link”坐标系Y轴、X轴、Z轴的旋转角度，分别对应于横滚角、俯仰角、航向角。“IMU”坐标系相对于“base_link”坐标系的变换矩阵遵循YXZ次序欧拉变换：

图5中画出移动机器人移动过程中质心O变化的示意图。如图5所示，在“IMU”坐标系中，移动机器人100的质心O坐标为[x₀，y₀，z₀]，驱动轮或从动轮与地面接触点为P_i(x_i，y_i，z_i)，i＝1，2，3，4，即驱动轮或从动轮与地面接触点分别为P₁、P₂、P₃和P₄。

记矢量p_i＝[x_i-x₀，y_i-y₀，z_i-z₀]，其中，p_i为移动机器人100的质心O到驱动轮或从动轮与地面接触点为P_i的矢量。

下面定义倾覆轴：

a₄＝p₁-p₄,i＝4

倾覆轴a_i为驱动轮或从动轮与地面接触点为P_i到其相邻的驱动轮或从动轮与地面接触点为P_i+1的矢量。

显然，重力对该倾覆轴施加的抗翻转矩越大，移动机器人100则越难沿该倾覆轴翻转。

定义通过移动机器人100质心O的倾覆轴的法线为

l_i＝p_i+1-(p_i+1·A_i)A_i，i＝1，2，3；l₄＝p₁-(p₁·A₄)A₄，i＝4

其中，Ai为a_i的单位向量，具体地：

对矢量a_i、f_g、l_i分别进行单位化：

其中，f_g为“base_link”坐标系内的重力矢量，L_i在“IMU”坐标系中表示为[x_i，y_i，z_i]，相对于“base_link”坐标系的矢量可以表示为重力矢量F_g相对于“base_link”坐标系的坐标为[0，0，z_g]，相对于“IMU”坐标系的可以表示为定义倾覆轴稳定角α_i为重力矢量F_g和法线向量L_i的夹角为

其中

α为当前机器人稳定角，机器人稳定角α^*为各倾覆轴稳定角α_i中的最小值，当移动机器人100处于临界稳定状态时，

实际上，移动机器人100抗倾覆稳定性主要取决于重力施加在倾覆轴的力矩，且力矩越大，移动机器人100越难倾翻。

在又一实施例中，为移动机器人100防倾覆设计防倾覆控制算法。具体地，移动机器人100为底盘机器人，具体地，为轮式移动机器人。

为保证移动机器人100足够的稳定性，通过设计防倾覆控制算法调整控制的重心或远离凸起坡道等区域，避免移动机器人100发生倾覆。

当θ＝φ＝0时，质心O点在“base_link”坐标系XY平面内投影记为C；XY平面也为机器人支撑平面。当θ≠0或φ≠0时，质心O′点在“base_link”坐标系XY平面内投影记为C′；记重心投影偏移矢量d为C′到C的矢量，其公式为：

式中，I_3×3为3*3的单位矩阵，d_x和d_y分别为重心投影偏移矢量d沿x、y方向的分量，即为、d_x和d_y分别为重心投影偏移矢量d沿“base_link”坐标系的X轴方向和Y轴方向的分量。

底盘机器人分为全向移动机器人和差分移动机器人两种。全向移动机器人的移动方向为360度。差分移动机器人的轴向运动受到约束。

对于全向移动机器人，在“base_link”坐标系内设计防倾覆控制率为：

[v_x，v_y]^T＝[k_xd_x，k_yd_y]^T

式中，v_x和v_y分别是“base_link”坐标系内沿x和y方向的速度，k_x和k_y是误差反馈系数。通过v_x、v_y、d_x和d_y可得出k_x和k_y，即可用PID控制器(PID(比例(proportion)、积分(integral)、导数(derivative))控制器)来实现调节，PID控制器中仅取比例控制。

对于差分移动机器人，由于轴向运动受到约束，v_y＝0，在“base_link”坐标系内设计防倾覆控制率：

式中，v_x和v_y分别是“base_link”坐标系内沿x和y方向的速度，是机器人沿Z轴的旋转角速度，φ为“base_link”坐标系内重心投影偏移矢量d与X轴夹角。k_x是误差反馈系数。通过v_x、d_x和φ可得出k_x和k_θ，即可用PID控制器来实现调节，PID控制器中仅取比例控制。

上式中，φ由下面公式求得：

φ＝atan2(d_x，d_y)

式中，atan2()反三角函数为C语言体系中特有函数，需要说明的是，上式只是求得φ的一种具体实现方式，基于这种思路的求法，还可以有其他表现形式，都在本发明实施例的保护范围内。例如，还可以用arctan()反正切函数的形式表示，本发明对此不做限制。

在又一实施例中，如图6所示，移动机器人100还可以包括存储器60。

存储器60作为一种非易失性计算机可读存储介质，可用于存储非易失性软件程序、非易失性计算机可执行程序以及模块，如本申请实施例中的移动机器人防倾覆方法对应的程序指令/模块(例如，附图8中第一接收模块210、倾覆状态判断模块220、第一计算模块230和控制模块240)。处理器40通过运行存储在存储器60中的非易失性软件程序、指令以及模块，从而执行移动机器人100的各种数据处理以及动作执行，即实现下述方法实施例移动机器人防倾覆方法方法。

存储器60可以包括存储程序区和存储数据区，其中，存储程序区可存储操作***、至少一个功能所需要的应用程序；存储数据区可存储根据移动机器人防倾覆装置的使用所创建的数据等。此外，存储器60可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中，存储器60可选包括相对于处理器40远程设置的存储器，这些远程存储器可以通过网络连接至展示汽车的行车状态信息的装置。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

本发明实施例还提供一种移动机器人防倾覆方法，如图7所示，包括：

步骤310：接收多个姿态传感器的信息，其中，所述多个姿态传感器相互之间的连线所在的安装平面与机器人水平支撑面平行，且所述多个姿态传感器的探测方向与机器人本体坐标系的坐标轴重合，所述机器人水平支撑面为所述移动机器人多个运动轮与地面接触点的连线构成的平面；

步骤320：根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，所述倾覆状态包括可能倾覆状态和稳定状态；

步骤330：若所述移动机器人的倾覆状态为可能倾覆状态，则根据所述多个姿态传感器的信息计算重心投影偏移矢量d，其中，所述重心投影偏移矢量d由所述移动机器人的质心在机器人水平支撑面内投影变化得到；

步骤340：根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

需要说明的是，本发明实施例提出的移动机器人防倾覆方法与实体装置实施例提出的移动机器人基于相同的发明构思，实体装置实施例与方法实施例中的相应技术内容可相互适用，此处不再详述。

本发明实施例的有益效果在于，本发明实施例提供的移动机器人防倾覆方法，接收多个姿态传感器的信息，根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，若所述移动机器人的倾覆状态为可能倾覆状态，则根据所述多个姿态传感器的信息计算重心投影偏移矢量d，根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。当移动机器人遇到凹凸不平、坡道等复杂地形或受到外力作用，移动机器人由于重心偏移而可能发生倾覆，采用本发明实施例提供的移动机器人防倾覆方法，根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，可使移动机器人回复到稳定状态，从而降低了移动机器人由于倾覆带来的损坏，避免经济损失。

在另一实施例中，所述移动机器人防倾覆方法还包括：

步骤350：接收多个防跌红外传感器的信息，其中，所述多个防跌红外传感器分别安装在所述移动机器人的多个运动轮前端，且所述多个防跌红外传感器的探测方向沿垂直方向向下；

步骤360：根据所述多个防跌红外传感器的信息判断所述移动机器人的悬空状态；

步骤370：若所述移动机器人的悬空状态为部分悬空状态，则获取悬空位置，控制所述移动机器人往与悬空位置所在的方向相反的方向移动预设距离；

所述根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态包括：

步骤341：根据所述重心投影偏移矢量d，控制移动预设距离后的所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

需要说明的是，本发明实施例提出的移动机器人防倾覆方法与实体装置实施例提出的移动机器人基于相同的发明构思，实体装置实施例与方法实施例中的相应技术内容可相互适用，此处不再详述。

本发明实施例的有益效果在于，当移动机器人遇到凹凸不平、坡道等复杂地形或受到外力作用，移动机器人由于重心偏移而可能发生跌倒和倾覆时，本发明实施例提供的移动机器人防倾覆方法根据所述多个防跌红外传感器的信息判断所述移动机器人的悬空状态，控制所述移动机器人往与悬空位置所在的方向相反的方向移动预设距离，然后再根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，可使移动机器人的回复到稳定状态。本发明实施例的移动机器人防倾覆方法先处理防跌倒，再处理防倾覆，从而降低了移动机器人由于跌倒和倾覆带来的损坏，进一步避免经济损失。

在又一实施例中，所述运动轮包括驱动轮和从动轮，所述移动机器人防倾覆方法还包括：

步骤380：计算所述移动机器人不同的从动轮旋转角下沿一倾覆轴的倾覆轴稳定角α_i，其中，所述从动轮旋转角的范围为0-2π，所述倾覆轴为机器人水平支撑面的各边所在的直线，所述倾覆轴的数量为多个。

进一步地，倾覆轴稳定角α_i为重力矢量和通过移动机器人质心的倾覆轴的法线向量的夹角。

步骤390：比较多个倾覆轴稳定角α_i，获得最小值min_iα_i＝α^*，其中，α^*为机器人稳定角；

所述根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，所述倾覆状态包括可能倾覆状态和稳定状态包括：

步骤321：根据所述多个姿态传感器的信息计算当前机器人稳定角α，判断所述移动机器人的倾覆状态，当α＞α^*时，所述移动机器人的倾覆状态为可能倾覆状态，当α≤α^*时，所述移动机器人的倾覆状态为稳定状态。

本发明实施例通过计算所述移动机器人不同的从动轮旋转角下沿一倾覆轴的倾覆轴稳定角α_i，比较多个倾覆轴稳定角α_i，获得最小值α^*为机器人稳定角，根据所述多个姿态传感器的信息计算当前机器人稳定角α并将其与机器人稳定角α^*比对，以判断所述移动机器人的倾覆状态是可能倾覆状态还是稳定状态。可以预先通过无限次试验，得到的不同的从动轮旋转角下沿一倾覆轴的倾覆轴稳定角α_i，再经过比对、计算等步骤得到机器人稳定角α^*，以此作为判断所述移动机器人的倾覆状态是可能倾覆状态的依据，机器人稳定角α^*取多个倾覆轴稳定角α_i中的最小值，可实现移动机器人在此角度下不发生倾覆。

现将轮式移动机器人100的防倾覆算法表述如下：

Step0：机器人支撑平面为主动轮/从动轮与地面接触点所构成的凸多边形，其中驱动轮与地面接触点位置P₁，P₂可以测量，从动轮与地面接触点P₃，P₄不可测量。从动轮旋转角β₃，β₄∈[0，2π]，任一组合(β₃，β₄)_i下倾覆轴稳定角为α_i，遍历所有角度组合寻找最小值α^*＝min_iα_i，将其定义为稳定裕量。初始化“IMU”坐标系下倾覆轴单位方向矢量A_i，单位法线L_i,重力矢量F_g。

Step1：由姿态传感器30的θ，φ，ψ数据更新“IMU”坐标系相对于“base_link”坐标系的旋转矩阵更新各倾覆轴稳定角γ_i及当前机器人稳定角α，当α＜α^*时防倾覆控制算法启动，转Step2；

Step2：防跌红外传感器40检测到移动机器人100部分悬空时，防跌落控制算法启动，转Step3；

Step3：计算机器人“base_link”坐标系下重心投影偏移矢量d，由防倾覆控制算法控制移动机器人100运动，直到重新回复到稳定状态α＜α^*。

Step4：首先判断悬空部位位于移动机器人100前侧还是后侧，然后向悬空部位的相反方向移动确定距离，远离地形凹陷区域。

本发明实施例还提供一种移动机器人防倾覆装置200，如图8所示，包括第一接收模块210、倾覆状态判断模块220、第一计算模块230和控制模块240。

第一接收模块210，用于接收多个姿态传感器的信息，其中，所述多个姿态传感器相互之间的连线所在的安装平面与机器人水平支撑面平行，所述机器人水平支撑面为所述移动机器人多个运动轮与地面接触点的连线构成的平面；

倾覆状态判断模块220，用于根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，所述倾覆状态包括可能倾覆状态和稳定状态；

第一计算模块230，若所述移动机器人的倾覆状态为可能倾覆状态，第一计算模块用于根据所述多个姿态传感器的信息计算重心投影偏移矢量d，其中，所述重心投影偏移矢量d由所述移动机器人的质心在机器人水平支撑面内投影变化得到；

控制模块240，用于根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

需要说明的是，本发明实施例提出的移动机器人防倾覆装置200与实体装置实施例提出的移动机器人基于相同的发明构思、移动机器人防倾覆装置200与本发明方法实施例提出的移动机器人防倾覆方法基于相同的发明构思，实体装置实施例、方法实施例与装置实施例中的相应技术内容可相互适用，此处不再详述。

本发明实施例的有益效果在于，本发明实施例提供的移动机器人防倾覆装置200，第一接收模块210用于接收多个姿态传感器的信息，倾覆状态判断模块220用于根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，若所述移动机器人的倾覆状态为可能倾覆状态，第一计算模块230用于根据所述多个姿态传感器的信息计算重心投影偏移矢量d，控制模块240用于根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。当移动机器人遇到凹凸不平、坡道等复杂地形或受到外力作用，移动机器人由于重心偏移而可能发生倾覆，采用本发明实施例提供的移动机器人防倾覆装置200，根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，可使移动机器人回复到稳定状态，从而降低了移动机器人由于倾覆带来的损坏，避免经济损失。

在另一实施例中，移动机器人防倾覆装置200还包括第二接收模块250、悬空状态判断模块260和获取和控制模块270。

第二接收模块250，用于接收多个防跌红外传感器的信息，其中，所述多个防跌红外传感器分别安装在所述移动机器人的多个运动轮前端，且所述多个防跌红外传感器的探测方向沿垂直方向向下；

悬空状态判断模块260，用于根据所述多个防跌红外传感器的信息判断所述移动机器人的悬空状态；

获取和控制模块270，若所述移动机器人的悬空状态为部分悬空状态，所述获取和控制模块用于获取悬空位置，控制所述移动机器人往与悬空位置所在的方向相反的方向移动预设距离；

所述控制模块240还用于根据所述重心投影偏移矢量d，控制移动预设距离后的所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

需要说明的是，本发明实施例提出的移动机器人防倾覆装置200与实体装置实施例提出的移动机器人基于相同的发明构思、移动机器人防倾覆装置200与本发明方法实施例提出的移动机器人防倾覆方法基于相同的发明构思，实体装置实施例、方法实施例与装置实施例中的相应技术内容可相互适用，此处不再详述。

本发明实施例的有益效果在于，当移动机器人遇到凹凸不平、坡道等复杂地形或受到外力作用，移动机器人由于重心偏移而可能发生跌倒和倾覆时，本发明实施例提供的移动机器人防倾覆装置200的悬空状态判断模块260根据第二接收模块250接收的所述多个防跌红外传感器的信息判断所述移动机器人的悬空状态，获取和控制模块270控制所述移动机器人往与悬空位置所在的方向相反的方向移动预设距离，然后控制模块240再根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，可使移动机器人的回复到稳定状态。本发明实施例的移动机器人防倾覆装置200先处理防跌倒，再处理防倾覆，从而降低了移动机器人由于跌倒和倾覆带来的损坏，进一步避免经济损失。

在又一实施例中，移动机器人防倾覆装置200还包括第二计算模块280和比较和获取模块290。

第二计算模块280，用于计算所述移动机器人不同的从动轮旋转角下沿一倾覆轴的倾覆轴稳定角α_i，其中，所述从动轮旋转角的范围为0-2π，所述倾覆轴为机器人水平支撑面的各边所在的直线，所述倾覆轴的数量为多个；

进一步地，倾覆轴稳定角α_i为重力矢量和通过移动机器人质心的倾覆轴的法线向量的夹角。

比较和获取模块290，用于比较多个倾覆轴稳定角α_i并获取最小值min_iα_i＝α^*，其中，α^*为机器人稳定角；

所述倾覆状态判断模块240还用于根据所述多个姿态传感器的信息计算当前机器人稳定角α，判断所述移动机器人的倾覆状态，当α＞α^*时，所述移动机器人的倾覆状态为可能倾覆状态，当α≤α^*时，所述移动机器人的倾覆状态为稳定状态。

本发明实施例通过计算所述移动机器人不同的从动轮旋转角下沿一倾覆轴的倾覆轴稳定角α_i，比较多个倾覆轴稳定角α_i，获得最小值α^*为机器人稳定角，根据所述多个姿态传感器的信息计算当前机器人稳定角α并将其与机器人稳定角α^*比对，以判断所述移动机器人的倾覆状态是可能倾覆状态还是稳定状态。可以预先通过无限次试验，得到的不同的从动轮旋转角下沿一倾覆轴的倾覆轴稳定角α_i，再经过比对、计算等步骤得到机器人稳定角α^*，以此作为判断所述移动机器人的倾覆状态是可能倾覆状态的依据，机器人稳定角α^*取多个倾覆轴稳定角α_i中的最小值，可实现移动机器人在此角度下不发生倾覆。

最后应说明的是：以上实施例仅用以说明本申请的技术方案，而非对其限制；在本申请的思路下，以上实施例或者不同实施例中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本申请的不同方面的许多其它变化，为了简明，它们没有在细节中提供；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的范围。

Claims (12)

1.一种移动机器人防倾覆方法，其特征在于，包括：

接收多个姿态传感器的信息，其中，所述多个姿态传感器相互之间的连线所在的安装平面与机器人水平支撑面平行，且所述多个姿态传感器的探测方向与机器人本体坐标系的坐标轴重合，所述机器人水平支撑面为所述移动机器人多个运动轮与地面接触点的连线构成的平面；

根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，所述倾覆状态包括可能倾覆状态和稳定状态；

若所述移动机器人的倾覆状态为可能倾覆状态，则根据所述多个姿态传感器的信息计算重心投影偏移矢量d，其中，所述重心投影偏移矢量d由所述移动机器人的质心在机器人水平支撑面内投影变化得到；

根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

2.如权利要求1所述的方法，其特征在于，所述方法还包括：

接收多个防跌红外传感器的信息，其中，所述多个防跌红外传感器分别安装在所述移动机器人的多个运动轮前端，且所述多个防跌红外传感器的探测方向沿垂直方向向下；

根据所述多个防跌红外传感器的信息判断所述移动机器人的悬空状态；

若所述移动机器人的悬空状态为部分悬空状态，则获取悬空位置，控制所述移动机器人往与悬空位置所在的方向相反的方向移动预设距离；

所述根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态包括：

根据所述重心投影偏移矢量d，控制移动预设距离后的所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

3.如权利要求1或2所述的方法，其特征在于，所述运动轮包括驱动轮和从动轮，所述方法还包括：

计算所述移动机器人不同的从动轮旋转角下沿一倾覆轴的倾覆轴稳定角α_i，其中，所述从动轮旋转角的范围为0-2π，所述倾覆轴为机器人水平支撑面的各边所在的直线，所述倾覆轴的数量为多个；

比较多个倾覆轴稳定角α_i，获得最小值min_i α_i＝α^*，其中，α^*为机器人稳定角；

所述根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，所述倾覆状态包括可能倾覆状态和稳定状态包括：

根据所述多个姿态传感器的信息计算当前机器人稳定角α，判断所述移动机器人的倾覆状态，当α>α^*时，所述移动机器人的倾覆状态为可能倾覆状态，当α≤α^*时，所述移动机器人的倾覆状态为稳定状态。

4.如权利要求3所述的方法，其特征在于，所述倾覆轴稳定角α_i为重力矢量和通过移动机器人质心的倾覆轴的法线向量的夹角。

5.一种移动机器人防倾覆装置，其特征在于，包括：

第一接收模块，用于接收多个姿态传感器的信息，其中，所述多个姿态传感器相互之间的连线所在的安装平面与机器人水平支撑面平行，所述机器人水平支撑面为所述移动机器人多个运动轮与地面接触点的连线构成的平面；

倾覆状态判断模块，用于根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，所述倾覆状态包括可能倾覆状态和稳定状态；

第一计算模块，若所述移动机器人的倾覆状态为可能倾覆状态，第一计算模块用于根据所述多个姿态传感器的信息计算重心投影偏移矢量d，其中，所述重心投影偏移矢量d由所述移动机器人的质心在机器人水平支撑面内投影变化得到；

控制模块，用于根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

6.如权利要求5所述的装置，其特征在于，所述装置还包括：

第二接收模块，用于接收多个防跌红外传感器的信息，其中，所述多个防跌红外传感器分别安装在所述移动机器人的多个运动轮前端，且所述多个防跌红外传感器的探测方向沿垂直方向向下；

悬空状态判断模块，用于根据所述多个防跌红外传感器的信息判断所述移动机器人的悬空状态；

获取和控制模块，若所述移动机器人的悬空状态为部分悬空状态，所述获取和控制模块用于获取悬空位置，控制所述移动机器人往与悬空位置所在的方向相反的方向移动预设距离；

所述控制模块还用于根据所述重心投影偏移矢量d，控制移动预设距离后的所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

7.如权利要求5或6所述的装置，其特征在于，所述运动轮包括驱动轮和从动轮，所述装置还包括：

第二计算模块，用于计算所述移动机器人不同的从动轮旋转角下沿一倾覆轴的倾覆轴稳定角α_i，其中，所述从动轮旋转角的范围为0-2π，所述倾覆轴为机器人水平支撑面的各边所在的直线，所述倾覆轴的数量为多个；比较和获取模块，用于比较多个倾覆轴稳定角α_i并获取最小值min_i α_i＝α^*，其中，α^*为机器人稳定角；

所述倾覆状态判断模块还用于根据所述多个姿态传感器的信息计算当前机器人稳定角α，判断所述移动机器人的倾覆状态，当α>α^*时，所述移动机器人的倾覆状态为可能倾覆状态，当α≤α^*时，所述移动机器人的倾覆状态为稳定状态。

8.如权利要求7所述的装置，其特征在于，所述倾覆轴稳定角α_i为重力矢量和通过移动机器人质心的倾覆轴的法线向量的夹角。

9.一种移动机器人，其特征在于，包括：

机身；

多个运动轮，所述多个运动轮安装在机身底部，所述多个运动轮与地面接触点的连线构成的平面为机器人水平支撑面；

多个姿态传感器，所述多个姿态传感器相互之间的连线所在的安装平面与所述机器人水平支撑面平行；

处理器，所述处理器安装在所述机身内，所述处理器用于接收多个姿态传感器的信息；

所述处理器还用于根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，所述倾覆状态包括可能倾覆状态和稳定状态；

若所述移动机器人的倾覆状态为可能倾覆状态，所述处理器还用于根据所述多个姿态传感器的信息计算重心投影偏移矢量d，其中，所述重心投影偏移矢量d由所述移动机器人的质心在机器人水平支撑面内投影变化得到；

所述处理器还用于根据所述重心投影偏移矢量d，控制所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

10.如权利要求9所述的移动机器人，其特征在于，所述移动机器人还包括：

多个防跌红外传感器，所述多个防跌红外传感器分别安装在所述多个运动轮前端，且所述多个防跌红外传感器的探测方向沿垂直方向向下；

所述处理器还用于接收多个防跌红外传感器的信息；

所述处理器还用于根据所述多个防跌红外传感器的信息判断所述移动机器人的悬空状态；

若所述移动机器人的悬空状态为部分悬空状态，所述处理器还用于获取悬空位置，控制所述移动机器人往与悬空位置所在的方向相反的方向移动预设距离；

所述处理器还用于根据所述重心投影偏移矢量d，控制移动预设距离后的所述移动机器人的角速度与线速度，以使移动机器人回复到稳定状态。

11.如权利要求9或10所述的移动机器人，其特征在于，所述运动轮包括驱动轮和从动轮，所述处理器还用于：

计算所述移动机器人不同的从动轮旋转角下沿一倾覆轴的倾覆轴稳定角α_i，其中，所述从动轮旋转角的范围为0-2π，所述倾覆轴为机器人水平支撑面的各边所在的直线，所述倾覆轴的数量为多个；

比较多个倾覆轴稳定角α_i，获得最小值min_i α_i＝α^*，其中，α^*为机器人稳定角；

所述根据所述多个姿态传感器的信息判断所述移动机器人的倾覆状态，所述倾覆状态包括可能倾覆状态和稳定状态包括：

根据所述多个姿态传感器的信息计算当前机器人稳定角α，判断所述移动机器人的倾覆状态，当α>α^*时，所述移动机器人的倾覆状态为可能倾覆状态，当α≤α^*时，所述移动机器人的倾覆状态为稳定状态。

12.如权利要求11所述的移动机器人，其特征在于，所述倾覆轴稳定角α_i为重力矢量和通过移动机器人质心的倾覆轴的法线向量的夹角。