CN116125447A - 机器人定位恢复方法、系统、电子设备及存储介质 - Google Patents

机器人定位恢复方法、系统、电子设备及存储介质 Download PDF

Info

Publication number
CN116125447A
CN116125447A CN202310151640.1A CN202310151640A CN116125447A CN 116125447 A CN116125447 A CN 116125447A CN 202310151640 A CN202310151640 A CN 202310151640A CN 116125447 A CN116125447 A CN 116125447A
Authority
CN
China
Prior art keywords
data
robot
radar data
time
positioning
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.)
Pending
Application number
CN202310151640.1A
Other languages
English (en)
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.)
Guangzhou Gosuncn Robot Co Ltd
Original Assignee
Guangzhou Gosuncn Robot 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 Guangzhou Gosuncn Robot Co Ltd filed Critical Guangzhou Gosuncn Robot Co Ltd
Priority to CN202310151640.1A priority Critical patent/CN116125447A/zh
Publication of CN116125447A publication Critical patent/CN116125447A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/497—Means for monitoring or calibrating
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/46—Indirect determination of position data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种机器人定位恢复方法、系统、电子设备及存储介质，方法包括：实时获取机器人的轮式里程计数据和基于激光雷达的实时定位数据，并存储为两个定位时间长度的数据队列；获取最新激光雷达数据的时间戳，并在最新激光雷达数据的时间戳与当前时间相差大于时间阈值时，判定激光雷达数据丢失；恢复雷达数据，获取当前雷达数据的时间戳，找到时间最接近的数据，记录轮式里程计坐标系下的三维位置和姿态；计算机器人在雷达数据丢失期间，机器人的相对位移和相对旋转的齐次矩阵；根据齐次矩阵计算当前机器人在从新获得雷达数据后，在地图坐标系下的三维位置和姿态；初始化处理，得到雷达数据恢复后获得的定位。

Description

机器人定位恢复方法、系统、电子设备及存储介质
技术领域
本发明涉及机器人制造技术领域，更具体地，涉及一种机器人定位恢复方法、系统、电子设备及存储介质。
背景技术
在机器人需要长时间运行的场景中，机器人的各个器件的稳定性都会受到挑战。激光雷达是机器人用于实时定位的部件之一，在实际环境中，激光雷达数据有可能因为激光雷达自身的稳定性不足、网络传输阻塞、电源模块稳定性差等导致激光雷达重启，使得激光雷达数据丢失一段时间，而后雷达数据又恢复正常。
在机器人在判定雷达数据真正丢失前通常需要几秒的时间，而这个时间机器人已经行走了一段距离或旋转了一定的角度。当雷达数据恢复后，机器人定位在雷达数据丢失前的位置，此时定位已经产生偏差，造成定位丢失或者定位不准确，从而导致机器人乱跑。
发明内容
本发明的一个目的是提供一种机器人定位恢复方法、系统、电子设备及存储介质的新技术方案，至少能够解决现有技术中机器人在恢复丢失数据后，定位丢失或定位不准确等问题。
本发明的第一方面，提供了一种机器人定位恢复方法，包括：
实时获取机器人的轮式里程计数据和基于激光雷达的实时定位数据，并存储为两个定位时间长度的数据队列；
获取最新激光雷达数据的时间戳，并在所述最新激光雷达数据的时间戳与当前时间相差大于时间阈值时，判定所述激光雷达数据丢失；
恢复雷达数据，获取当前雷达数据的时间戳，并根据当前雷达数据的时间戳在所述轮式里程计数据的队列中找到时间最接近的数据，以及根据丢失的当前雷达数据的时间戳在所述轮式里程计数据的队列中找到时间最接近的数据，并分别记录轮式里程计坐标系下的三维位置和姿态；
计算所述机器人在雷达数据丢失期间，所述机器人的相对位移和相对旋转的齐次矩阵；
根据所述齐次矩阵计算当前所述机器人在从新获得雷达数据后，在地图坐标系下的三维位置和姿态；
初始化处理，得到雷达数据恢复后获得的定位。
可选地，所述机器人包括底盘和激光雷达，所述激光雷达设在所述底盘上，所述激光雷达探测的数据带有时间戳。
可选地，在所述数据队列的第一个数据与当前最新数据相差大于预设值时，剔除第一个数据，添加最新数据，并不断循环。
可选地，在判定所述激光雷达数据丢失的步骤包括：保存当前所述激光雷达数据的时间戳，并上报雷达数据异常，等待所述激光雷达数据恢复。
可选地，计算所述机器人在雷达数据丢失期间，所述机器人的相对位移和相对旋转的齐次矩阵的计算公式为：
T_get_delta＝T_lose_odom.inverse()*T_get_odom
其中，T_get_delta表示所述机器人的相对位移和相对旋转的齐次矩阵，T_get_odom表示雷达数据恢复时，获取当前所述激光雷达数据的时间戳，并根据获取的当前所述激光雷达数据的时间戳在所述轮式里程计数据的队列中找到的时间最接近的数据，T_lose_odom.inverse()表示T_lose_odom矩阵的逆。
可选地，根据所述齐次矩阵计算当前所述机器人在从新获得雷达数据后，在地图坐标系下的三维位置和姿态的计算公式为：
T_guess_now＝T_lose_local*T_get_delta
其中，T_guess_now表示得到当前机器人在重新获得雷达数据后，当前机器人在地图坐标系下的三维位置和姿态的一个估计位置，T_lose_local表示根据判定雷达数据丢失时，保存当前激光雷达数据的时间戳在定位数据队列中找到的时间最接近的数据，T_get_delta表示所述机器人的相对位移和相对旋转的齐次矩阵。
可选地，所述的机器人定位恢复方法，其特征在于，还包括：初始化成功后，所述机器人消除雷达数据异常的告警，并继续执行所述机器人的任务。
本发明的第二方面，提供一种机器人定位恢复系统，应用于上述实施例中所述的机器人定位恢复方法，所述机器人定位恢复系统包括：
第一获取模块，所述第一获取模块用于实时获取机器人的轮式里程计数据和基于激光雷达的实时定位数据，并存储为两个定位时间长度的数据队列；
第二获取模块，所述第二获取模块用于获取最新激光雷达数据的时间戳，并在所述最新激光雷达数据的时间戳与当前时间相差大于时间阈值时，判定所述激光雷达数据丢失；
第三获取模块，所述第三获取模块用于恢复雷达数据，获取当前雷达数据的时间戳，并根据当前雷达数据的时间戳在所述轮式里程计数据的队列中找到时间最接近的数据，以及根据丢失的当前雷达数据的时间戳在所述轮式里程计数据的队列中找到时间最接近的数据，并分别记录轮式里程计坐标系下的三维位置和姿态；
第一计算模块，所述第一计算模块用于计算所述机器人在雷达数据丢失期间，所述机器人的相对位移和相对旋转的齐次矩阵；
第二计算模块，所述第二计算模块用于根据所述齐次矩阵计算当前所述机器人在从新获得雷达数据后，在地图坐标系下的三维位置和姿态；
初始化模块，所述初始化模块用于初始化处理，得到雷达数据恢复后获得的定位。
本发明的第三方面，提供一种电子设备，包括：处理器和存储器，在所述存储器中存储有计算机程序指令，其中，在所述计算机程序指令被所述处理器运行时，使得所述处理器执行上述实施例中所述的机器人定位恢复方法的步骤。
本发明的第四方面，提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器运行时，使得所述处理器执行上述实施例中所述的机器人定位恢复方法的步骤。
本发明的机器人定位恢复方法，根据机器人的轮式里程计信息，来获取雷达数据丢失期间的相对位姿，在雷达数据恢复的同时能快速定位初始化成功，达到机器人定位恢复的目的。该机器人定位恢复方法可以在雷达数据因不可控因素导致的数据丢失时，在雷达数据丢失后定位能够快速初始化成功，避免人为介入恢复，提高机器人的稳定性和可靠性。同时利用轮式里程计来获取雷达数据丢失期间的相对位姿，避免繁重的全局初始化，达到机器人快速定位恢复的目的。
通过以下参照附图对本发明的示例性实施例的详细描述，本发明的其它特征及其优点将会变得清楚。
附图说明
被结合在说明书中并构成说明书的一部分的附图示出了本发明的实施例，并且连同其说明一起用于解释本发明的原理。
图1是根据本发明实施例的机器人定位恢复方法的流程图；
图2是根据本发明实施例的电子设备的原理图。
附图标记：
处理器201；
存储器202；操作系统2021；应用程序2022；
网络接口203；
输入设备204；
硬盘205；
显示设备206。
具体实施方式
现在将参照附图来详细描述本发明的各种示例性实施例。应注意到：除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。
以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。
对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为说明书的一部分。
在这里示出和讨论的所有例子中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它例子可以具有不同的值。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。
下面结合附图具体描述根据本发明实施例的机器人定位恢复方法。
如图1所示，根据本发明实施例的机器人定位恢复方法，包括：
S1、实时获取机器人的轮式里程计数据和基于激光雷达的实时定位数据，并存储为两个定位时间长度的数据队列；
S2、获取最新激光雷达数据的时间戳，并在最新激光雷达数据的时间戳与当前时间相差大于时间阈值时，判定激光雷达数据丢失；
S3、恢复雷达数据，获取当前雷达数据的时间戳，并根据当前雷达数据的时间戳在轮式里程计数据的队列中找到时间最接近的数据，以及根据丢失的当前雷达数据的时间戳在轮式里程计数据的队列中找到时间最接近的数据，并分别记录轮式里程计坐标系下的三维位置和姿态；
S4、计算机器人在雷达数据丢失期间，机器人的相对位移和相对旋转的齐次矩阵；
S5、根据齐次矩阵计算当前机器人在从新获得雷达数据后，在地图坐标系下的三维位置和姿态；
S6、初始化处理，得到雷达数据恢复后获得的定位。
换言之，在本发明实施例的机器人定位恢复方法中，参见图1，首先，可以实时获取机器人的轮式里程计数据和基于激光雷达的实时定位数据，并存储为两个定位时间长度的数据队列，记时间长度为time_len，time_len一般可根据激光雷达实际工作情况设定，确保雷达数据恢复时能够time_len时间内恢复。然后，可以获取最新激光雷达数据的时间戳，并在最新激光雷达数据的时间戳与当前时间相差大于时间阈值时，判定激光雷达数据丢失。该时间阈值可以记为time_max。如time_max为3s，则判定雷达数据丢失，保存当前激光雷达数据的时间戳t_laser_lose，上报雷达数据异常，同时等待激光雷达数据恢复。
接着，恢复雷达数据，获取当前雷达数据的时间戳，该时间戳记为t_laser_get。并根据当前雷达数据的时间戳在轮式里程计数据的队列中找到时间最接近的数据，以及根据丢失的当前雷达数据的时间戳在轮式里程计数据的队列中找到时间最接近的数据，并分别记录轮式里程计坐标系下的三维位置和姿态。
雷达数据恢复时，获取当前雷达数据的时间戳t_laser_get，根据t_laser_lose在轮式里程计数据的队列中找到时间最接近的数据，记为T_lose_odom，T_lose_odom为4x4的齐次矩阵，可代表在t_laser_lose时刻轮式里程计坐标系下的三维位置和姿态。根据t_laser_lose在实时定位数据的队列中找到时间最接近的数据，记为T_lose_local，T_lose_local为4x4的齐次矩阵，可代表在t_laser_lose时刻机器人在地图上三维位置和姿态。再根据t_laser_get在轮式里程计数据的队列中找到时间最接近的数据，记为T_get_odom，T_get_odom为4x4的齐次矩阵，可代表在t_laser_get时刻轮式里程计坐标系下的三维位置和姿态。
然后，可以计算机器人在雷达数据丢失期间，机器人的相对位移和相对旋转的齐次矩阵。并且可以根据齐次矩阵计算当前机器人在从新获得雷达数据后，在地图坐标系下的三维位置和姿态。最后，初始化处理，初始化成功后，得到雷达数据恢复后获得的准确定位。
由此，根据本发明实施例的机器人定位恢复方法，根据机器人的轮式里程计信息，来获取雷达数据丢失期间的相对位姿，在雷达数据恢复的同时能快速定位初始化成功，达到机器人定位恢复的目的。该机器人定位恢复方法可以在雷达数据因不可控因素导致的数据丢失时，在雷达数据丢失后定位能够快速初始化成功，避免人为介入恢复，提高机器人的稳定性和可靠性。同时利用轮式里程计来获取雷达数据丢失期间的相对位姿，避免繁重的全局初始化，达到机器人快速定位恢复的目的。
根据本发明的一个实施例，机器人包括底盘和激光雷达，激光雷达设在底盘上，激光雷达探测的数据带有时间戳。
也就是说，机器人包括底盘和激光雷达，激光雷达设置在底盘上，底盘可获取得到实时机器人的轮式里程计数据，轮式里程计数据带有时间戳。激光雷达探测的数据带有时间戳。基于激光雷达数据能输出实时定位数据，定位数据带有时间戳。
根据本发明的一个实施例，在数据队列的第一个数据与当前最新数据相差大于预设值时，剔除第一个数据，添加最新数据，并不断循环。
换句话说，通过实时获取机器人的轮式里程计数据和基于激光雷达的实时定位数据，储存为两个定时间长度的数据队列，记时间长度为time_len，如time_len为60s的数据队列，当数据队列的第一个数据与当前最新数据相差大于time_len时，剔除第一个数据，添加最新数据，不断循环。time_len一般可根据激光雷达实际工作情况设定，确保雷达数据恢复时能够time_len时间内恢复。
在本发明的一些具体实施方式中，在判定激光雷达数据丢失的步骤包括：保存当前激光雷达数据的时间戳，该时间戳记为t_laser_lose，上报雷达数据异常，等待激光雷达数据恢复。
在本发明的一些具体实施方式中，计算机器人在雷达数据丢失期间，机器人的相对位移和相对旋转的齐次矩阵的计算公式为：
T_get_delta＝T_lose_odom.inverse()*T_get_odom
其中，T_get_delta表示机器人的相对位移和相对旋转的齐次矩阵，T_get_odom表示雷达数据恢复时，获取当前激光雷达数据的时间戳，并根据获取的当前激光雷达数据的时间戳在轮式里程计数据的队列中找到的时间最接近的数据，T_lose_odom.inverse()表示T_lose_odom矩阵的逆。
根据齐次矩阵计算当前机器人在从新获得雷达数据后，在地图坐标系下的三维位置和姿态的计算公式为：
T_guess_now＝T_lose_local*T_get_delta
其中，T_guess_now表示得到当前机器人在重新获得雷达数据后，当前机器人在地图坐标系下的三维位置和姿态的一个估计位置，T_lose_local表示根据判定雷达数据丢失时，保存当前激光雷达数据的时间戳在定位数据队列中找到的时间最接近的数据，T_get_delta表示机器人的相对位移和相对旋转的齐次矩阵。用T_guess_now作为当前位置的估计来进行定位的初始化，从而在雷达数据恢复后获得准确的定位。
根据本发明实施例的机器人定位恢复方法，还包括：初始化成功后，机器人消除雷达数据异常的告警，并继续执行机器人的任务。
总而言之，根据本发明实施例的机器人定位恢复方法，根据机器人的轮式里程计信息，来获取雷达数据丢失期间的相对位姿，在雷达数据恢复的同时能快速定位初始化成功，达到机器人定位恢复的目的。该机器人定位恢复方法可以在雷达数据因不可控因素导致的数据丢失时，在雷达数据丢失后定位能够快速初始化成功，避免人为介入恢复，提高机器人的稳定性和可靠性。同时利用轮式里程计来获取雷达数据丢失期间的相对位姿，避免繁重的全局初始化，达到机器人快速定位恢复的目的。
根据本发明实施例的第二方面，提供一种机器人定位恢复系统，应用于上述实施例中的机器人定位恢复方法，机器人定位恢复系统包括第一获取模块、第二获取模块、第三获取模块、第一计算模块、第二计算模块和初始化模块，其中，第一获取模块用于实时获取机器人的轮式里程计数据和基于激光雷达的实时定位数据，并存储为两个定位时间长度的数据队列。第二获取模块用于获取最新激光雷达数据的时间戳，并在最新激光雷达数据的时间戳与当前时间相差大于时间阈值时，判定激光雷达数据丢失。
第三获取模块用于恢复雷达数据，获取当前雷达数据的时间戳，并根据当前雷达数据的时间戳在轮式里程计数据的队列中找到时间最接近的数据，以及根据丢失的当前雷达数据的时间戳在轮式里程计数据的队列中找到时间最接近的数据，并分别记录轮式里程计坐标系下的三维位置和姿态。第一计算模块用于计算机器人在雷达数据丢失期间，机器人的相对位移和相对旋转的齐次矩阵第二计算模块用于根据齐次矩阵计算当前机器人在从新获得雷达数据后，在地图坐标系下的三维位置和姿态。初始化模块用于初始化处理，得到雷达数据恢复后获得的定位。
根据本发明实施例的机器人定位恢复系统，根据机器人的轮式里程计信息，来获取雷达数据丢失期间的相对位姿，在雷达数据恢复的同时能快速定位初始化成功，达到机器人定位恢复的目的。该机器人定位恢复方法可以在雷达数据因不可控因素导致的数据丢失时，在雷达数据丢失后定位能够快速初始化成功，避免人为介入恢复，提高机器人的稳定性和可靠性。同时利用轮式里程计来获取雷达数据丢失期间的相对位姿，避免繁重的全局初始化，达到机器人快速定位恢复的目的。
根据本发明的第三方面，还提供一种电子设备，包括：处理器201和存储器202，在存储器202中存储有计算机程序指令，其中，在计算机程序指令被处理器201运行时，使得处理器201执行上述实施例中的机器人定位恢复方法的步骤。
进一步地，如图2所示，电子设备还包括网络接口203、输入设备204、硬盘205、和显示设备206。
上述各个接口和设备之间可以通过总线架构互连。总线架构可以包括任意数量的互联的总线和桥。具体由处理器201代表的一个或者多个中央处理器201(CPU)，以及由存储器202代表的一个或者多个存储器202的各种电路连接在一起。总线架构还可以将诸如外围设备、稳压器和功率管理电路等之类的各种其它电路连接在一起。可以理解，总线架构用于实现这些组件之间的连接通信。总线架构除包括数据总线之外，还包括电源总线、控制总线和状态信号总线，这些都是本领域所公知的，因此本文不再对其进行详细描述。
网络接口203，可以连接至网络(如因特网、局域网等)，从网络中获取相关数据，并可以保存在硬盘205中。
输入设备204，可以接收操作人员输入的各种指令，并发送给处理器201以供执行。输入设备204可以包括键盘或者点击设备(例如，鼠标，轨迹球(trackball)、触感板或者触摸屏等。
显示设备206，可以将处理器201执行指令获得的结果进行显示。
存储器202，用于存储操作系统2021运行所必须的程序和数据，以及处理器201计算过程中的中间结果等数据。
可以理解，本发明实施例中的存储器202可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(ROM)、可编程只读存储器(PROM)、可擦除可编程只读存储器(EPROM)、电可擦除可编程只读存储器(EEPROM)或闪存。易失性存储器可以是随机存取存储器(RAM)，其用作外部高速缓存。本文描述的装置和方法的存储器202旨在包括但不限于这些和任意其它适合类型的存储器202。
在一些实施方式中，存储器202存储了如下的元素，可执行模块或者数据结构，或者他们的子集，或者他们的扩展集：操作系统2021和应用程序2022。
其中，操作系统2021，包含各种系统程序，例如框架层、核心库层、驱动层等，用于实现各种基础业务以及处理基于硬件的任务。应用程序2022，包含各种应用程序2022，例如浏览器(Browser)等，用于实现各种应用业务。实现本发明实施例方法的程序可以包含在应用程序2022中。
上述处理器201，当调用并执行存储器202中所存储的应用程序2022和数据，具体的，可以是应用程序2022中存储的程序或指令时，执行根据上述实施例的机器人定位恢复方法的步骤。
本发明上述实施例揭示的方法可以应用于处理器201中，或者由处理器201实现。处理器201可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器201中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器201可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件，可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器201也可以是任何常规的处理器201等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器202，处理器201读取存储器202中的信息，结合其硬件完成上述方法的步骤。
可以理解的是，本文描述的这些实施例可以用硬件、软件、固件、中间件、微码或其组合来实现。对于硬件实现，处理单元可以实现在一个或多个专用集成电路(ASIC)、数字信号处理器DSP)、数字信号处理设备(DSPD)、可编程逻辑设备(PLD)、现场可编程门阵列(FPGA)、通用处理器、控制器、微控制器、微处理器、用于执行本申请功能的其它电子单元或其组合中。
对于软件实现，可通过执行本文功能的模块(例如过程、函数等)来实现本文的技术。软件代码可存储在存储器202中并通过处理器201执行。存储器202可以在处理器201中或在处理器201外部实现。
具体地，处理器201还用于读取计算机程序，执行如下步骤:对桩充电方法预测并输出用户提问的问题答案。
本发明第四方面实施例，还提供一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器201运行时，使得处理器201执行上述实施例的机器人定位恢复方法的步骤。
在本申请所提供的几个实施例中，应该理解到，所揭露方法和装置，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理包括，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。
上述以软件功能单元的形式实现的集成的单元，可以存储在一个计算机可读取存储介质中。上述软件功能单元存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例收发方法的部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，简称ROM)、随机存取存储器(Random Access Memory，简称RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
虽然已经通过例子对本发明的一些特定实施例进行了详细说明，但是本领域的技术人员应该理解，以上例子仅是为了进行说明，而不是为了限制本发明的范围。本领域的技术人员应该理解，可在不脱离本发明的范围和精神的情况下，对以上实施例进行修改。本发明的范围由所附权利要求来限定。

Claims (10)

1.一种机器人定位恢复方法，其特征在于，包括：
实时获取机器人的轮式里程计数据和基于激光雷达的实时定位数据，并存储为两个定位时间长度的数据队列；
获取最新激光雷达数据的时间戳，并在所述最新激光雷达数据的时间戳与当前时间相差大于时间阈值时，判定所述激光雷达数据丢失；
恢复雷达数据，获取当前雷达数据的时间戳，并根据当前雷达数据的时间戳在所述轮式里程计数据的队列中找到时间最接近的数据，以及根据丢失的当前雷达数据的时间戳在所述轮式里程计数据的队列中找到时间最接近的数据，并分别记录轮式里程计坐标系下的三维位置和姿态；
计算所述机器人在雷达数据丢失期间，所述机器人的相对位移和相对旋转的齐次矩阵；
根据所述齐次矩阵计算当前所述机器人在从新获得雷达数据后，在地图坐标系下的三维位置和姿态；
初始化处理，得到雷达数据恢复后获得的定位。
2.根据权利要求1所述的机器人定位恢复方法，其特征在于，所述机器人包括底盘和激光雷达，所述激光雷达设在所述底盘上，所述激光雷达探测的数据带有时间戳。
3.根据权利要求1所述的机器人定位恢复方法，其特征在于，在所述数据队列的第一个数据与当前最新数据相差大于预设值时，剔除第一个数据，添加最新数据，并不断循环。
4.根据权利要求1所述的机器人定位恢复方法，其特征在于，在判定所述激光雷达数据丢失的步骤包括：保存当前所述激光雷达数据的时间戳，并上报雷达数据异常，等待所述激光雷达数据恢复。
5.根据权利要求1所述的机器人定位恢复方法，其特征在于，计算所述机器人在雷达数据丢失期间，所述机器人的相对位移和相对旋转的齐次矩阵的计算公式为：
T_get_delta＝T_lose_odom.inverse()*T_get_odom
其中，T_get_delta表示所述机器人的相对位移和相对旋转的齐次矩阵，T_get_odom表示雷达数据恢复时，获取当前所述激光雷达数据的时间戳，并根据获取的当前所述激光雷达数据的时间戳在所述轮式里程计数据的队列中找到的时间最接近的数据，T_lose_odom.inverse()表示T_lose_odom矩阵的逆。
6.根据权利要求5所述的机器人定位恢复方法，其特征在于，根据所述齐次矩阵计算当前所述机器人在从新获得雷达数据后，在地图坐标系下的三维位置和姿态的计算公式为：
T_guess_now＝T_lose_local*T_get_delta
其中，T_guess_now表示得到当前机器人在重新获得雷达数据后，当前机器人在地图坐标系下的三维位置和姿态的一个估计位置，T_lose_local表示根据判定雷达数据丢失时，保存当前激光雷达数据的时间戳在定位数据队列中找到的时间最接近的数据，T_get_delta表示所述机器人的相对位移和相对旋转的齐次矩阵。
7.根据权利要求1所述的机器人定位恢复方法，其特征在于，还包括：初始化成功后，所述机器人消除雷达数据异常的告警，并继续执行所述机器人的任务。
8.一种机器人定位恢复系统，应用于权利要求1-7中任一项所述的机器人定位恢复方法，其特征在于，所述机器人定位恢复系统包括：
第一获取模块，所述第一获取模块用于实时获取机器人的轮式里程计数据和基于激光雷达的实时定位数据，并存储为两个定位时间长度的数据队列；
第二获取模块，所述第二获取模块用于获取最新激光雷达数据的时间戳，并在所述最新激光雷达数据的时间戳与当前时间相差大于时间阈值时，判定所述激光雷达数据丢失；
第三获取模块，所述第三获取模块用于恢复雷达数据，获取当前雷达数据的时间戳，并根据当前雷达数据的时间戳在所述轮式里程计数据的队列中找到时间最接近的数据，以及根据丢失的当前雷达数据的时间戳在所述轮式里程计数据的队列中找到时间最接近的数据，并分别记录轮式里程计坐标系下的三维位置和姿态；
第一计算模块，所述第一计算模块用于计算所述机器人在雷达数据丢失期间，所述机器人的相对位移和相对旋转的齐次矩阵；
第二计算模块，所述第二计算模块用于根据所述齐次矩阵计算当前所述机器人在从新获得雷达数据后，在地图坐标系下的三维位置和姿态；
初始化模块，所述初始化模块用于初始化处理，得到雷达数据恢复后获得的定位。
9.一种电子设备，其特征在于，包括：处理器和存储器，在所述存储器中存储有计算机程序指令，其中，在所述计算机程序指令被所述处理器运行时，使得所述处理器执行权利要求1-7中任一项所述的机器人定位恢复方法的步骤。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器运行时，使得所述处理器执行权利要求1-7中任一项所述的机器人定位恢复方法的步骤。
CN202310151640.1A 2023-02-22 2023-02-22 机器人定位恢复方法、系统、电子设备及存储介质 Pending CN116125447A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310151640.1A CN116125447A (zh) 2023-02-22 2023-02-22 机器人定位恢复方法、系统、电子设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310151640.1A CN116125447A (zh) 2023-02-22 2023-02-22 机器人定位恢复方法、系统、电子设备及存储介质

Publications (1)

Publication Number Publication Date
CN116125447A true CN116125447A (zh) 2023-05-16

Family

ID=86311664

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310151640.1A Pending CN116125447A (zh) 2023-02-22 2023-02-22 机器人定位恢复方法、系统、电子设备及存储介质

Country Status (1)

Country Link
CN (1) CN116125447A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116840820A (zh) * 2023-08-29 2023-10-03 上海仙工智能科技有限公司 一种检测2d激光定位丢失的方法及系统、存储介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116840820A (zh) * 2023-08-29 2023-10-03 上海仙工智能科技有限公司 一种检测2d激光定位丢失的方法及系统、存储介质
CN116840820B (zh) * 2023-08-29 2023-11-24 上海仙工智能科技有限公司 一种检测2d激光定位丢失的方法及系统、存储介质

Similar Documents

Publication Publication Date Title
KR102525502B1 (ko) 자동운전 테스트 데이터 저장 방법, 장치, 전자 기기, 저장 매체 및 프로그램
CN111006682A (zh) 导航路线规划方法、装置、电子设备及存储介质
CN110413432B (zh) 一种信息处理方法、电子设备及存储介质
CN116125447A (zh) 机器人定位恢复方法、系统、电子设备及存储介质
CN116662038B (zh) 基于共享内存的工业信息检测方法、装置、设备及介质
CN115620799A (zh) 基于固件仿真的故障现场回溯方法、设备及可读存储介质
CN109739232B (zh) 障碍物追踪方法、装置、车载终端及存储介质
CN117761722A (zh) 激光雷达slam退化检测方法、系统、电子设备及存储介质
CN112905668B (zh) 基于分布式数据流处理引擎的数据库导数方法、设备和介质
CN114020527A (zh) 快照恢复方法、装置、计算机设备及存储介质
CN109521419B (zh) 基于车辆雷达的目标跟踪方法和装置
CN113050020A (zh) 双芯智能电表的超差事件记录方法、装置和计算机设备
CN104657239A (zh) 基于分离式日志的多核处理器瞬时故障恢复系统及其瞬时故障恢复方法
CN111625948B (zh) 超长场景的回放型仿真方法、装置、设备和介质
CN110996091B (zh) 在可移动设备的显示图像上叠加信息的方法及装置
CN109885475B (zh) 页面转化率计算方法、装置、计算机设备及存储介质
CN113656286A (zh) 软件测试方法、装置、电子设备及可读存储介质
CN114003172A (zh) 存储容量校正方法、装置、计算机设备以及存储介质
CN112286909A (zh) 执行状态确定方法、装置、服务器及存储介质
JP2006079485A (ja) 電子計算機における障害解析用情報収集方式
CN110554822A (zh) 标签添加和删除方法以及计算机存储介质
EP3846039B1 (en) Data writing method and apparatus, and electronic device
CN111475342B (zh) 用于电子终端的应用切换方法和应用切换装置
CN116662140B (zh) 一种仿真数据的自动化采集与回放方法、装置和设备
CN109992510B (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