CN114911225A - 一种双轮差速机器人打滑判定方法及装置 - Google Patents

一种双轮差速机器人打滑判定方法及装置 Download PDF

Info

Publication number
CN114911225A
CN114911225A CN202110207292.6A CN202110207292A CN114911225A CN 114911225 A CN114911225 A CN 114911225A CN 202110207292 A CN202110207292 A CN 202110207292A CN 114911225 A CN114911225 A CN 114911225A
Authority
CN
China
Prior art keywords
robot
data
odometer
relative displacement
inertial sensor
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.)
Granted
Application number
CN202110207292.6A
Other languages
English (en)
Other versions
CN114911225B (zh
Inventor
谭高洋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Yingdi Mande Technology Co ltd
Original Assignee
Beijing Yingdi Mande Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Yingdi Mande Technology Co ltd filed Critical Beijing Yingdi Mande Technology Co ltd
Priority to CN202110207292.6A priority Critical patent/CN114911225B/zh
Publication of CN114911225A publication Critical patent/CN114911225A/zh
Application granted granted Critical
Publication of CN114911225B publication Critical patent/CN114911225B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/028—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using a RF signal
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

本发明提供一种双轮差速机器人打滑判定方法及装置，其中，方法如下步骤：获取惯性传感器数据以及里程计数据；根据所述惯性传感器的数据，得到机器人的旋转角增量以及第一相对位移量；根据每一个轮子的里程计数据以及所述旋转角增量，得到第二相对位移量；当所述第一相对位移量与所述第二相对位移量差值大于第一阈值，则判定所述机器人打滑。通过实施本发明，根据惯性传感器数据以及里程计数据求解第一相对位移量和第二相对位移量，通过第一相对位移量和第二相对位移量差值判定是否发生打滑，而不需要根据视觉传感器数据进行打滑判定，实现了在视觉状况不良好的情况下，依旧能够准确进行打滑判定，提高了打滑判定的准确性。

Description

一种双轮差速机器人打滑判定方法及装置
技术领域
本发明涉及机器人技术领域，具体涉及一种双轮差速机器人打滑判定方法及装置。
背景技术
现在市场上许多扫地机器人都是双轮差速的轮式机器人，这种机器人具备控制方便，结构简单等特点。但是机器人在清扫的过程中会遇到打滑的情况，从而导致编码里程计的值不准确，最后会造成定位有较大误差。相关技术中，一般采用视觉传感器对里程计的打滑判定进行补充，但在视觉状况不良好的情况下，导致视觉传感器获取的数据不准确，使得打滑判定不准确。
发明内容
有鉴于此，本发明实施例提供了一种双轮差速机器人打滑判定方法及装置，以解决现有技术中打滑判定不准确的缺陷。
根据第一方面，本发明实施例提供一种双轮差速机器人打滑判定方法，所述机器人包括惯性传感器和里程计，包括如下步骤：获取惯性传感器数据以及里程计数据；根据所述惯性传感器的数据，得到机器人的旋转角增量以及第一相对位移量；根据每一个轮子的里程计数据以及所述旋转角增量，得到第二相对位移量；当所述第一相对位移量与所述第二相对位移量差值大于第一阈值，则判定所述机器人打滑。
可选地，所述机器人还包括图像传感器；所述方法还包括：获取相邻两帧图像信息；提取相邻两帧图像信息的视觉特征点；当相邻两帧图像信息中的视觉特征点匹配数量大于点数阈值，则根据所述惯性传感器数据、里程计数据以及所述图像传感器数据，判定所述机器人是否出现打滑。
可选地，根据所述惯性传感器数据、里程计数据以及所述图像传感器数据，判定所述机器人是否出现打滑，包括：根据所述惯性传感器的数据，得到机器人的旋转角增量；根据所述里程计数据以及所述旋转角增量，得到两帧图像之间的相对位移量；根据所述两帧图像信息以及两帧图像之间的相对位移量，得到里程计和视觉联合的误差方程；根据所述误差方程，得到打滑量；当所述打滑量大于第二阈值，则判定所述机器人打滑。
可选地，还包括：当判定出所述机器人打滑，则更新所述机器人的里程计数据。
可选地，所述根据所述里程计数据以及所述旋转角增量，得到第二相对位移量，包括：
Figure BDA0002949054750000021
其中，PN-PM为第二相对位移量，
Figure BDA0002949054750000022
Δdk为第k帧的机器人的里程增量，Δθk为第k帧的机器人旋转角增量，Rk为第k帧的旋转矩阵，
Figure BDA0002949054750000023
为左轮里程计的零偏，
Figure BDA0002949054750000024
为右轮里程计的零偏，
Figure BDA0002949054750000025
Figure BDA0002949054750000026
其中，Δθm为第m帧的机器人旋转角增量，ez[0；0；1]。
可选地，所述里程计和视觉联合的误差方程为：
Figure BDA0002949054750000027
式中，C为误差，rp表示先验残差，rcil为视觉重投影误差，roij里程计预积分的残差，∑p为先验的协方差矩阵，∑C为视觉的协方差矩阵，∑Oij为里程计预积分的协方差矩阵。
根据第二方面，本发明实施例提供一种双轮差速机器人打滑判定装置，所述机器人包括惯性传感器和里程计，包括：数据获取模块，用于获取惯性传感器数据以及里程计数据；第一位移量确认模块，用于根据所述惯性传感器的数据，得到机器人的旋转角增量以及第一相对位移量；第二位移量确认模块，用于根据每一个轮子的里程计数据以及所述旋转角增量，得到第二相对位移量；第一打滑判定模块，用于当所述第一相对位移量与所述第二相对位移量差值大于第一阈值，则判定所述机器人打滑。
可选地，所述机器人还包括图像传感器；还包括：图像获取装置，用于获取相邻两帧图像信息；特征点提取模块，用于提取相邻两帧图像信息的视觉特征点；第二打滑判定模块，用于当相邻两帧图像信息中的视觉特征点匹配数量大于点数阈值，则根据所述惯性传感器数据、里程计数据以及所述图像传感器数据，判定所述机器人是否出现打滑。
根据第三方面，本发明实施例提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现第一方面或第一方面任一实施方式所述的双轮差速机器人打滑判定方法的步骤。
根据第四方面，本发明实施例提供一种存储介质，其上存储有计算机指令，该指令被处理器执行时实现第一方面或第一方面任一实施方式所述的双轮差速机器人打滑判定方法的步骤。
本发明技术方案，具有如下优点：
本实施例提供的双轮差速机器人打滑判定方法/装置，通过惯性传感器数据以及里程计数据求解第一相对位移量和第二相对位移量，根据相对位移量差值判定是否发生打滑，而不需要根据视觉传感器数据进行打滑判定，实现了在视觉状况不良好的情况下，依旧能够准确进行打滑判定，提高了打滑判定的准确性。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例中双轮差速机器人打滑判定方法的一个具体示例的流程图；
图2为本发明实施例中双轮差速机器人打滑判定方法的一个具体示例的流程图；
图3为本发明实施例中双轮差速机器人打滑判定方法的一个具体示例的流程图；
图4为本发明实施例中双轮差速机器人打滑判定方法的一个具体示例的流程图；
图5为本发明实施例中双轮差速机器人打滑判定方法的一个具体示例的流程图；
图6为本发明实施例中双轮差速机器人打滑判定装置的一个具体示例原理框图；
图7为本发明实施例中电子设备的一个具体示例的原理框图。
具体实施方式
下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。
在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，还可以是两个元件内部的连通，可以是无线连接，也可以是有线连接。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
此外，下面所描述的本发明不同实施方式中所涉及的技术特征只要彼此之间未构成冲突就可以相互结合。
本实施例提供一种双轮差速机器人打滑判定方法，机器人包括惯性传感器和里程计，如图1所示，包括如下步骤：
S101，获取惯性传感器数据以及里程计数据；
示例性地，惯性传感器可以包括陀螺仪、加速度计等等。惯性传感器数据可以包括陀螺仪的角速度和陀螺仪零偏数据。里程计数据可以包括左右双轮的里程计数据。获取惯性传感器数据以及里程计数据的方式可以是在机器人上安装惯性传感器以及里程计，从惯性传感器以及里程计中获取所需的数据。本实施例对惯性传感器数据以及里程计数据的种类不做限定，本领域技术人员可以根据需要确定。
S102，根据惯性传感器的数据，得到机器人的旋转角增量以及第一相对位移量；
示例性地，旋转角增量表征的是机器人行驶过程的角度信息，如图2所示，旋转角增量为Δθk，
Figure BDA0002949054750000061
其中，
Figure BDA0002949054750000062
为右轮里程计的增量，
Figure BDA0002949054750000063
为左轮里程计的增量，L为两个轮子的间距。将在里程计坐标系下通过里程计递推出的旋转角增量，转化到世界坐标系时，其递推过程为：
Figure BDA0002949054750000064
其中，Ri为旋转矩阵，表示第i时刻机器人的旋转数据，因为里程计的递推只能推算Z轴的角度，可以认为x、y轴为0，那么ez为[0；0；1]，Δθk为标量，ez为矢量，乘完后就是(0，0，Δθk)表示z轴的角度变化。
而通过惯性传感器数据，同样可以建立旋转矩阵的递推公式如下：
Figure BDA0002949054750000065
其中，wk为陀螺仪角速度，bg为陀螺仪零偏，Δt为相邻连个时刻的时间差。
通过里程计的数据以及公式
Figure BDA0002949054750000066
求解Δθk时，由于里程计数据在机器人打滑时，会导致求解的Δθk出现较大的误差，因此，本实施例通过惯性传感器数据得到的旋转矩阵递推公式(2)与公式(1)联立，即可求解Δθk。
根据惯性传感器的数据，得到机器人的第一相对位移量的方式如下所示：
Figure BDA0002949054750000067
Figure BDA0002949054750000071
其中，Pi G为惯性传感器推算的第i时刻时机器人的位置，Pj G为惯性传感器推算的第j时刻时机器人的位置，Pj G-Pi G为惯性传感器推算的第i时刻到第j时刻的第一相对位移，vi第i时刻时机器人的速度，vj第j时刻时机器人的速度，ak为k时刻加速度计的值，Δt为相邻连个时刻的时间差，g为重力加速度，ba为加速度计的零偏，Rk为第k时刻的旋转矩阵，可以通过公式(2)求得。
S103，根据每一个轮子的里程计数据以及旋转角增量，得到第二相对位移量；
示例性地，如图2所示，对于双轮差速的机器人，其里程计的运动模型可以是：
Figure BDA0002949054750000072
其中，PΔ(Δdk,Δθk)x、PΔ(Δdk,Δθk)y、PΔ(Δdk,Δθk)z为里程计递推出来的在里程计坐标系上的相对位姿偏移量，Δdk为机器人的里程增量，
Figure BDA0002949054750000073
将其转移至世界坐标系中，其递推过程如下：
Pk+1 L＝Pk L+RkPΔ(Δdk,Δθk)；
其中，Pk L为里程计推算的第k时刻时机器人的位置，Pk+1 L为里程计推算的第k+1时刻时机器人的位置，Rk为第k时刻的旋转矩阵，可以通过公式(1)求得。
那么对于第i时刻到第j时刻的递推公式则可以是：
Figure BDA0002949054750000081
其中，Pj L-Pi L为里程计推算的第i时刻到第j时刻的第二相对位移。
S104，当第一相对位移量与第二相对位移量差值大于第一阈值，则判定机器人打滑。
示例性地，第一阈值可以是5cm；本实施例对第一阈值的大小不做限定，本领域技术人员可以根据需要确定。当第一相对位移量与第二相对位移量差值大于第一阈值，则判定机器人打滑。
本实施例提供的双轮差速机器人打滑判定方法，通过惯性传感器数据以及里程计数据求解第一相对位移量和第二相对位移量，根据相对位移量差值判定是否发生打滑，而不需要根据视觉传感器数据进行打滑判定，实现了在视觉状况不良好的情况下，依旧能够准确进行打滑判定，提高了打滑判定的准确性。
作为本实施例一种可选的实施方式，所述机器人还包括图像传感器；所述方法还包括：
首先，获取相邻两帧图像信息；提取相邻两帧图像信息的视觉特征点；
示例性地，获取相邻两帧图像信息的方式可以是获取图像传感器得到的图像数据，图像传感器可以是摄像头。提取相邻两帧图像信息视觉特征点的方式可以是利用SURF、ORB、LBP、HAAR、sift等算法。本实施例对提取相邻两帧图像信息视觉特征点的方式不做限定，本领域技术人员可以根据需要确定。
再次，当相邻两帧图像信息中的视觉特征点匹配数量大于点数阈值，则根据惯性传感器数据、里程计数据以及图像传感器数据，判定机器人是否出现打滑。
示例性地，点数阈值可以是60，也可以是70，本实施例对点数阈值大小不做限定，本领域技术人员可以根据需要设定。当相邻两帧图像信息中的视觉特征点匹配数量大于点数阈值，则表示当前机器人所处的环境视觉条件良好，则可以根据惯性传感器数据、里程计数据以及图像传感器数据，判定机器人是否出现打滑；当相邻两帧图像信息中的视觉特征点匹配数量小于或者等于点数阈值，则表示当前机器人所处的环境视觉条件比较差，则执行上述S101－S104的步骤，用惯性传感器数据以及里程计数据，判定机器人是否出现打滑。
本实施例提供的双轮差速机器人打滑判定方法，通过相邻两帧图像信息中的视觉特征点匹配数量是否满足预设条件来判定当前的视觉条件是否足够好，当视觉状况足够好的情况下，执行根据惯性传感器数据、里程计数据以及图像传感器数据，判定机器人是否出现打滑，当视觉状况不良好的情况下，则执行上述S101－S104的步骤，用惯性传感器数据以及里程计数据，判定机器人是否出现打滑。
作为本实施例一种可选的实施方式，根据惯性传感器数据、里程计数据以及图像传感器数据，判定机器人是否出现打滑，包括：
首先，根据惯性传感器的数据，得到机器人的旋转角增量；具体内容参见上述实施例S102部分，在此不再赘述。
其次，根据里程计数据以及旋转角增量，得到两帧图像之间的相对位移量；
示例性地，一般来说里程计的频率为50hz，即20ms一帧数据，图像传感器获取图像的频率一般5hz，即200ms一帧图像，这意味着两帧图像之间有10个里程计，因此需要这10个左右的里程计数据递推出两帧图像的相对位移。
机器人的打滑体现在里程计的数值偏大或偏小。可以把里程计打滑当作里程计的零偏。其公式如下：
Figure BDA0002949054750000101
Figure BDA0002949054750000102
Figure BDA0002949054750000103
其中，
Figure BDA0002949054750000104
为左轮里程计的零偏，即左轮的打滑量，
Figure BDA0002949054750000105
为右轮里程计的零偏，即右轮的打滑量。
从公式(3)可知，打滑影响里程计递推的有Δdk以及Δθk，其中，Δθk已经由公式(1)和公式(2)联立求解出，不受里程计打滑的影响，那么唯一受到影响的则是Δdk。而Δdk表示的是左里程计增量和右里程计增量和的平均，因此在打滑时并不需要具体知道左轮和右轮里程计的零偏，只需要知道两者之和即可，因此，可以假设左右轮里程计的零偏相等：
Figure BDA0002949054750000106
假设，相邻两帧图像分别为图像M和图像N，那么通过以下公式，可以得到图像M和图像N之间，由里程计递推出的位置数据：
Figure BDA0002949054750000107
Figure BDA0002949054750000108
其中，RM为在获取到图像M时的旋转矩阵，RN为在获取到图像N时的旋转矩阵，PM为在获取到图像M时的位置信息，PN为在获取到图像N时的位置信息，
Figure BDA0002949054750000109
表征Δdk与零偏相关的数学关系，也即
Figure BDA00029490547500001010
那么，图像M到图像N之间的相对位移量为：
Figure BDA00029490547500001011
为了避免重复迭代，可以进行里程计预积分：
Figure BDA0002949054750000111
Figure BDA0002949054750000112
其中，ΔRMN为图像M到图像N的旋转矩阵预积分量，ΔPMN为图像M到图像N位置的预积分量。ΔRM,k为当前里程计获取第k帧里程计数据时与第M帧图像的相对旋转矩阵。
然后，根据两帧图像信息以及两帧图像之间的相对位移量，得到里程计和视觉联合的误差方程；
示例性地，误差方程可以是：
Figure BDA0002949054750000113
式中，C为误差，rp表示先验残差，rcil为视觉重投影误差，roij里程计预积分的残差，∑p为先验的协方差矩阵，∑C为视觉的协方差矩阵，∑oij为里程计预积分的协方差矩阵。
其中，里程计预积分的残差为：
Figure BDA0002949054750000114
Figure BDA0002949054750000115
为里程计预积分值，Pi为第i帧的图像位置，Pj为第j帧的图像位置。
视觉重投影误差方程为：
rCil＝Zil-π(Pi,Ri,αl)；
上式中，Zil为第i帧图像中第l个特征点的像素坐标，Pi为第i帧的图像位置，Ri为第i帧的旋转矩阵，αl为第l个特征点的世界系下的坐标，π(Pi,Ri,αl)＝K(Riαl+Pi)/sl；K为相机的内参，由相机标定给出，sl为每个特征点对应的深度值。
该误差方程的状态量为Pi(当前帧的位置)，Pj(上一帧的位置)，Ri(当前帧的旋转矩阵)，还有左右里程计的零偏
Figure BDA0002949054750000121
优化这些状态量使得误差和最小。
再次，根据误差方程，得到打滑量；
示例性地，根据误差方程，得到打滑量的方式可以是求解误差方程最小化的条件下，得到里程计的左右零偏之和，也即求解以下公式：
Figure BDA0002949054750000122
然后，当打滑量大于第二阈值，则判定机器人打滑。
示例性地，第二阈值可以是0.005m；当打滑量(里程计的零偏)大于第二阈值，则判定机器人打滑。
作为本实施例一种可选的实施方式，双轮差速机器人打滑判定方法还包括：当判定出机器人打滑，则更新机器人的里程计数据。
示例性地，更新机器人的里程计数据的方式可以是：如果通过里程计和视觉联合的误差方程判定出打滑，则把其估计出来的里程计零偏反馈给里程计递推出当前帧初值；如果是通过惯性传感器数据以及里程计数据得到第一相对位移量和第二相对位移量作差判定出里程计打滑，则在用惯性传感器递推解算出当前帧的位姿作为当前帧的初值。
为了进一步说明本实施例中双轮差速机器人打滑判定方法的有效性，本实施例提供实验内容如下：
在机器人运动过程中，压住机器人，此时机器人几乎移动，但是它的轮子还一直在空转，这就是一个典型的打滑过程。如果不加打滑处理的话打滑过程中位移还在移动，但是加了打滑处理，在打滑的过程中，位移几乎不变。
在实验过程中，本实施例压住机器人两次，每次压住机器人导致它打滑的时间为8s。一次是机器人沿着y方向直线运行时，一次是机器人沿着x方向直线运动。
图3表示机器人在沿着y轴方向直线运动时在88s—96s之间按住机器人，使其打滑，图中点表示的为加打滑处理的y方向运动轨迹，线表示的为不加打滑的运动轨迹。从图3中可以看出加了打滑处理，在打滑的时间段运动轨迹并没有漂移，而没加打滑处理的轨迹漂移了1.2m左右。
图4表示机器人在沿着x轴方向直线运动时在103s—111s之间按住机器人，使其打滑，图中点表示的为加打滑处理的x方向运动轨迹，线表示的为不加打滑处理的运动轨迹。从图4中可以看出加了打滑处理，在打滑的时间段运动轨迹并没有漂移，而没加打滑处理的轨迹漂移了1.9m左右。
图5表示的视觉和里程计联合优化估计出来的打滑量。因为在打滑的过程中，视觉良好，所以用视觉和里程计的联合判定。在88s—96s之间估计出来的里程计零偏(打滑量)约为0.19m/s，在103s—111s之间估计出来的里程计零偏(打滑量)约为0.26m/s。
本实施例提供一种双轮差速机器人打滑判定装置，所述机器人包括惯性传感器和里程计，如图6所示，包括：
数据获取模块201，用于获取惯性传感器数据以及里程计数据；具体内容参见上述实施例对应部分，在此不再赘述。
第一位移量确认模块202，用于根据所述惯性传感器的数据，得到机器人的旋转角增量以及第一相对位移量；具体内容参见上述实施例对应部分，在此不再赘述。
第二位移量确认模块203，用于根据每一个轮子的里程计数据以及所述旋转角增量，得到第二相对位移量；具体内容参见上述实施例对应部分，在此不再赘述。
第一打滑判定模块204，用于当所述第一相对位移量与所述第二相对位移量差值大于第一阈值，则判定所述机器人打滑。具体内容参见上述实施例对应部分，在此不再赘述。
作为本实施例一种可选的实施方式，所述机器人还包括图像传感器；还包括：
图像获取装置，用于获取相邻两帧图像信息；具体内容参见上述实施例对应部分，在此不再赘述。
特征点提取模块，用于提取相邻两帧图像信息的视觉特征点；具体内容参见上述实施例对应部分，在此不再赘述。
第二打滑判定模块，用于当相邻两帧图像信息中的视觉特征点匹配数量大于点数阈值，则根据所述惯性传感器数据、里程计数据以及所述图像传感器数据，判定所述机器人是否出现打滑。具体内容参见上述实施例对应部分，在此不再赘述。
作为本实施例一种可选的实施方式，第二打滑判定模块，包括：
旋转角增量确定模块，用于根据所述惯性传感器的数据，得到机器人的旋转角增量；具体内容参见上述实施例对应部分，在此不再赘述。
相对位移量确认模块，用于根据所述里程计数据以及所述旋转角增量，得到两帧图像之间的相对位移量；具体内容参见上述实施例对应部分，在此不再赘述。
误差方程确认模块，用于根据所述两帧图像信息以及两帧图像之间的相对位移量，得到里程计和视觉联合的误差方程；具体内容参见上述实施例对应部分，在此不再赘述。
打滑量确定模块，用于根据所述误差方程，得到打滑量；具体内容参见上述实施例对应部分，在此不再赘述。
第二打滑判定子模块，用于当所述打滑量大于第二阈值，则判定所述机器人打滑。具体内容参见上述实施例对应部分，在此不再赘述。
作为本实施例一种可选的实施方式，还包括：
更新模块，用于当判定出所述机器人打滑，则更新所述机器人的里程计数据。具体内容参见上述实施例对应部分，在此不再赘述。
作为本实施例一种可选的实施方式，所述第二位移量确认模块203，包括：计算模块，用于执行
Figure BDA0002949054750000151
其中，PN-PM为第二相对位移量，
Figure BDA0002949054750000152
Δdk为第k帧的机器人的里程增量，Δθk为第k帧的机器人旋转角增量，Rk为第k帧的旋转矩阵，
Figure BDA0002949054750000153
为左轮里程计的零偏，
Figure BDA0002949054750000154
为右轮里程计的零偏，
Figure BDA0002949054750000155
Figure BDA0002949054750000156
其中，Δθm为第m帧的机器人旋转角增量，ez[0；0；1]。具体内容参见上述实施例对应部分，在此不再赘述。
作为本实施例一种可选的实施方式，误差方程确认模块，包括：误差方程计算模块，用于执行
Figure BDA0002949054750000161
式中，C为误差，rp表示先验残差，rcil为视觉重投影误差，roij里程计预积分的残差，∑p为先验的协方差矩阵，∑C为视觉的协方差矩阵，∑Oij为里程计预积分的协方差矩阵。具体内容参见上述实施例对应部分，在此不再赘述。
本申请实施例还提供一种电子设备，如图7所示，处理器310和存储器320，其中处理器310和存储器320可以通过总线或者其他方式连接。
处理器310可以为中央处理器(Central Processing Unit，CPU)。处理器310还可以为其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field－Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等芯片，或者上述各类芯片的组合。
存储器320作为一种非暂态计算机可读存储介质，可用于存储非暂态软件程序、非暂态计算机可执行程序以及模块，如本发明实施例中的双轮差速机器人打滑判定方法对应的程序指令/模块。处理器通过运行存储在存储器中的非暂态软件程序、指令以及模块，从而执行处理器的各种功能应用以及数据处理。
存储器320可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储处理器所创建的数据等。此外，存储器可以包括高速随机存取存储器，还可以包括非暂态存储器，例如至少一个磁盘存储器件、闪存器件、或其他非暂态固态存储器件。在一些实施例中，存储器320可选包括相对于处理器远程设置的存储器，这些远程存储器可以通过网络连接至处理器。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
所述一个或者多个模块存储在所述存储器320中，当被所述处理器310执行时，执行如图1所示实施例中的双轮差速机器人打滑判定方法。
上述电子设备的具体细节可以对应参阅图1所示的实施例中对应的相关描述和效果进行理解，此处不再赘述。
本实施例还提供了一种计算机存储介质，所述计算机存储介质存储有计算机可执行指令，该计算机可执行指令可执行上述任意方法实施例1中双轮差速机器人打滑判定方法。其中，所述存储介质可为磁碟、光盘、只读存储记忆体(Read－Only Memory，ROM)、随机存储记忆体(Random Access Memory，RAM)、快闪存储器(Flash Memory)、硬盘(Hard DiskDrive，缩写：HDD)或固态硬盘(Solid－State Drive，SSD)等；所述存储介质还可以包括上述种类的存储器的组合。
显然，上述实施例仅仅是为清楚地说明所作的举例，而并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。

Claims (10)

1.一种双轮差速机器人打滑判定方法，所述机器人包括惯性传感器和里程计，其特征在于，包括如下步骤：
获取惯性传感器数据以及里程计数据；
根据所述惯性传感器的数据，得到机器人的旋转角增量以及第一相对位移量；
根据每一个轮子的里程计数据以及所述旋转角增量，得到第二相对位移量；
当所述第一相对位移量与所述第二相对位移量差值大于第一阈值，则判定所述机器人打滑。
2.根据权利要求1所述的方法，其特征在于，所述机器人还包括图像传感器；所述方法还包括：
获取相邻两帧图像信息；
提取相邻两帧图像信息的视觉特征点；
当相邻两帧图像信息中的视觉特征点匹配数量大于点数阈值，则根据所述惯性传感器数据、里程计数据以及所述图像传感器数据，判定所述机器人是否出现打滑。
3.根据权利要求2所述的方法，其特征在于，根据所述惯性传感器数据、里程计数据以及所述图像传感器数据，判定所述机器人是否出现打滑，包括：
根据所述惯性传感器的数据，得到机器人的旋转角增量；
根据所述里程计数据以及所述旋转角增量，得到两帧图像之间的相对位移量；
根据所述两帧图像信息以及两帧图像之间的相对位移量，得到里程计和视觉联合的误差方程；
根据所述误差方程，得到打滑量；
当所述打滑量大于第二阈值，则判定所述机器人打滑。
4.根据权利要求1-3中任一项所述的方法，其特征在于，还包括：
当判定出所述机器人打滑，则更新所述机器人的里程计数据。
5.根据权利要求1所述的方法，其特征在于，所述根据所述里程计数据以及所述旋转角增量，得到第二相对位移量，包括：
Figure FDA0002949054740000021
其中，PN-PM为第二相对位移量，
Figure FDA0002949054740000022
Δdk为第k帧的机器人的里程增量，Δθk为第k帧的机器人旋转角增量，Rk为第k帧的旋转矩阵，
Figure FDA0002949054740000023
为左轮里程计的零偏，
Figure FDA0002949054740000024
为右轮里程计的零偏，
Figure FDA0002949054740000025
Figure FDA0002949054740000026
其中，Δθm为第m帧的机器人旋转角增量，ez[0；0；1]。
6.根据权利要求3所述的方法，其特征在于，所述里程计和视觉联合的误差方程为：
Figure FDA0002949054740000027
式中，C为误差，rp表示先验残差，rcil为视觉重投影误差，roij里程计预积分的残差，∑p为先验的协方差矩阵，∑C为视觉的协方差矩阵，∑Oij为里程计预积分的协方差矩阵。
7.一种双轮差速机器人打滑判定装置，所述机器人包括惯性传感器和里程计，其特征在于，包括：
数据获取模块，用于获取惯性传感器数据以及里程计数据；
第一位移量确认模块，用于根据所述惯性传感器的数据，得到机器人的旋转角增量以及第一相对位移量；
第二位移量确认模块，用于根据每一个轮子的里程计数据以及所述旋转角增量，得到第二相对位移量；
第一打滑判定模块，用于当所述第一相对位移量与所述第二相对位移量差值大于第一阈值，则判定所述机器人打滑。
8.根据权利要求7所述的装置，其特征在于，所述机器人还包括图像传感器；还包括：
图像获取装置，用于获取相邻两帧图像信息；
特征点提取模块，用于提取相邻两帧图像信息的视觉特征点；
第二打滑判定模块，用于当相邻两帧图像信息中的视觉特征点匹配数量大于点数阈值，则根据所述惯性传感器数据、里程计数据以及所述图像传感器数据，判定所述机器人是否出现打滑。
9.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现权利要求1－6任一所述的双轮差速机器人打滑判定方法的步骤。
10.一种存储介质，其上存储有计算机指令，其特征在于，该指令被处理器执行时实现权利要求1－6任一所述的双轮差速机器人打滑判定方法的步骤。
CN202110207292.6A 2021-02-23 2021-02-23 一种双轮差速机器人打滑判定方法及装置 Active CN114911225B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110207292.6A CN114911225B (zh) 2021-02-23 2021-02-23 一种双轮差速机器人打滑判定方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110207292.6A CN114911225B (zh) 2021-02-23 2021-02-23 一种双轮差速机器人打滑判定方法及装置

Publications (2)

Publication Number Publication Date
CN114911225A true CN114911225A (zh) 2022-08-16
CN114911225B CN114911225B (zh) 2023-08-15

Family

ID=82761223

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110207292.6A Active CN114911225B (zh) 2021-02-23 2021-02-23 一种双轮差速机器人打滑判定方法及装置

Country Status (1)

Country Link
CN (1) CN114911225B (zh)

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103389087A (zh) * 2013-08-07 2013-11-13 上海海事大学 一种轮式机器人位姿解算方法
CN108638053A (zh) * 2018-04-03 2018-10-12 珠海市微半导体有限公司 一种机器人打滑的检测方法及其矫正方法
CN108731673A (zh) * 2018-06-05 2018-11-02 中国科学院电子学研究所 机器人自主导航定位方法及系统
CN108748135A (zh) * 2018-04-04 2018-11-06 上海物景智能科技有限公司 一种机器人驱动轮打滑位姿矫正方法以及一种机器人
CN110000813A (zh) * 2019-03-22 2019-07-12 深圳拓邦股份有限公司 机器人打滑检测方法、系统及装置
CN110946511A (zh) * 2018-09-27 2020-04-03 科沃斯机器人股份有限公司 打滑判断的方法、设备及存储介质
CN111103877A (zh) * 2019-12-05 2020-05-05 小狗电器互联网科技（北京）股份有限公司 移动机器人打滑预警方法、存储介质以及移动机器人
CN111220155A (zh) * 2020-03-04 2020-06-02 广东博智林机器人有限公司 基于双目视觉惯性里程计估计位姿的方法、装置与处理器
US20200198149A1 (en) * 2018-12-24 2020-06-25 Ubtech Robotics Corp Ltd Robot vision image feature extraction method and apparatus and robot using the same
CN111481113A (zh) * 2019-01-29 2020-08-04 北京奇虎科技有限公司 一种判断扫地机器人打滑的方法和装置
CN112220413A (zh) * 2020-09-30 2021-01-15 小狗电器互联网科技（北京）股份有限公司 一种扫地机器人打滑检测方法、装置及可读存储介质

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103389087A (zh) * 2013-08-07 2013-11-13 上海海事大学 一种轮式机器人位姿解算方法
CN108638053A (zh) * 2018-04-03 2018-10-12 珠海市微半导体有限公司 一种机器人打滑的检测方法及其矫正方法
CN108748135A (zh) * 2018-04-04 2018-11-06 上海物景智能科技有限公司 一种机器人驱动轮打滑位姿矫正方法以及一种机器人
CN108731673A (zh) * 2018-06-05 2018-11-02 中国科学院电子学研究所 机器人自主导航定位方法及系统
CN110946511A (zh) * 2018-09-27 2020-04-03 科沃斯机器人股份有限公司 打滑判断的方法、设备及存储介质
US20200198149A1 (en) * 2018-12-24 2020-06-25 Ubtech Robotics Corp Ltd Robot vision image feature extraction method and apparatus and robot using the same
CN111481113A (zh) * 2019-01-29 2020-08-04 北京奇虎科技有限公司 一种判断扫地机器人打滑的方法和装置
CN110000813A (zh) * 2019-03-22 2019-07-12 深圳拓邦股份有限公司 机器人打滑检测方法、系统及装置
CN111103877A (zh) * 2019-12-05 2020-05-05 小狗电器互联网科技（北京）股份有限公司 移动机器人打滑预警方法、存储介质以及移动机器人
CN111220155A (zh) * 2020-03-04 2020-06-02 广东博智林机器人有限公司 基于双目视觉惯性里程计估计位姿的方法、装置与处理器
CN112220413A (zh) * 2020-09-30 2021-01-15 小狗电器互联网科技（北京）股份有限公司 一种扫地机器人打滑检测方法、装置及可读存储介质

Also Published As

Publication number Publication date
CN114911225B (zh) 2023-08-15

Similar Documents

Publication Publication Date Title
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN108932737B (zh) 车载相机俯仰角标定方法和装置、电子设备以及车辆
CN110411457B (zh) 基于行程感知与视觉融合的定位方法、系统、终端和存储介质
JP2018124787A (ja) 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム
CN112083725A (zh) 一种面向自动驾驶车辆的结构共用多传感器融合定位系统
CN112254741A (zh) 里程传感器的异常检测方法、自移动机器人及存储介质
CN110458885B (zh) 基于行程感知与视觉融合的定位系统和移动终端
CN112017236B (zh) 一种基于单目相机计算目标物位置的方法及装置
JP7173471B2 (ja) ３次元位置推定装置及びプログラム
CN113188557A (zh) 一种融合语义特征的视觉惯性组合导航方法
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam系统
CN109506674B (zh) 一种加速度的校正方法及装置
CN114911225A (zh) 一种双轮差速机器人打滑判定方法及装置
CN115560744A (zh) 机器人以及基于多传感器的三维建图方法、存储介质
CN115585805A (zh) 车辆定位方法、定位装置、定位系统以及车辆
CN113034538A (zh) 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备
CN110864685B (zh) 一种基于松耦合的车辆单目视觉轮式里程计定位方法
CN112633043B (zh) 一种车道线确定方法、装置、电子设备及存储介质
CN111284496A (zh) 用于自动驾驶车辆的车道追踪方法及系统
TWI689433B (zh) 用於自動駕駛車輛的車道追蹤方法及系統
CN112325877B (zh) 车辆属性确定方法、装置、存储介质和电子设备
CN113155121B (zh) 一种车辆定位方法、装置及电子设备
JPH11339012A (ja) 車両用画像処理装置
CN113608523B (zh) 一种基于单目视觉和惯性融合的车辆场景动态性分析方法
WO2022179047A1 (zh) 一种状态信息估计方法及装置

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
GR01 Patent grant
GR01 Patent grant