CN116661469B - 机器人轨迹误差修正方法及系统 - Google Patents
机器人轨迹误差修正方法及系统 Download PDFInfo
- Publication number
- CN116661469B CN116661469B CN202310955385.6A CN202310955385A CN116661469B CN 116661469 B CN116661469 B CN 116661469B CN 202310955385 A CN202310955385 A CN 202310955385A CN 116661469 B CN116661469 B CN 116661469B
- Authority
- CN
- China
- Prior art keywords
- target
- information
- track
- robot
- target robot
- 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 37
- 238000012937 correction Methods 0.000 title claims abstract description 16
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 58
- 230000033001 locomotion Effects 0.000 claims abstract description 51
- 239000002245 particle Substances 0.000 claims description 84
- 230000006870 function Effects 0.000 claims description 49
- 230000004913 activation Effects 0.000 claims description 40
- 238000004364 calculation method Methods 0.000 claims description 29
- 230000008569 process Effects 0.000 claims description 14
- 239000011159 matrix material Substances 0.000 claims description 13
- 238000005259 measurement Methods 0.000 claims description 13
- 238000005070 sampling Methods 0.000 claims description 11
- 230000007613 environmental effect Effects 0.000 claims description 7
- 238000009825 accumulation Methods 0.000 claims description 5
- 238000004590 computer program Methods 0.000 claims description 5
- 238000011524 similarity measure Methods 0.000 claims description 4
- 238000003860 storage Methods 0.000 claims description 4
- 238000013507 mapping Methods 0.000 description 4
- 238000005457 optimization Methods 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 230000001186 cumulative effect Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000002243 precursor Substances 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000012163 sequencing technique Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
本发明提供一种机器人轨迹误差修正方法及系统，包括基于目标机器人中预设的传感器对所述目标机器人所在环境进行感知和定位，获取所述目标机器人的实时位置信息和姿态信息；结合预先下发至所述目标机器人的目标位置信息结合所述目标机器人的实时位置信息，通过轨迹规划算法确定所述目标机器人的目标轨迹信息；根据所述目标机器人的实时位置信息和姿态信息生成所述目标机器人的实际轨迹信息，根据改进的动态时间规整算法确定所述实际轨迹信息与所述目标轨迹信息的匹配度，若所述匹配度不满足预设匹配条件，则根据所述目标机器人的实时位置信息和姿态信息进行运动误差补偿，直至所述匹配度满足预设匹配条件。
Description
技术领域
本发明涉及轨迹修正技术，尤其涉及一种机器人轨迹误差修正方法及系统。
背景技术
机器人轨迹误差修正是在机器人运动过程中，通过不断检测和纠正轨迹偏差，使机器人能够更精准地按照预定路径进行移动的一种技术。在机器人应用中，精确的轨迹跟踪对于实现任务目标和安全导航非常重要。然而，由于各种因素（例如传感器噪声、运动误差、环境变化等），机器人在执行任务时可能会产生轨迹偏差，影响其准确性和稳定性。因此，开发有效的机器人轨迹误差修正方法对于提高机器人的自主性和应用范围至关重要。
发明内容
本发明实施例提供一种机器人轨迹误差修正方法及系统，能够至少解决现有技术中部分问题，也即解决现有技术对轨迹误差判断不准以及对轨迹误差进行补偿不够准确且高效的问题。
本发明实施例的第一方面，
提供一种机器人轨迹误差修正方法，包括：
基于目标机器人中预设的传感器对所述目标机器人所在环境进行感知和定位，获取所述目标机器人的实时位置信息和姿态信息；
结合预先下发至所述目标机器人的目标位置信息结合所述目标机器人的实时位置信息，通过轨迹规划算法确定所述目标机器人的目标轨迹信息；
根据所述目标机器人的实时位置信息和姿态信息生成所述目标机器人的实际轨迹信息，根据改进的动态时间规整算法确定所述实际轨迹信息与所述目标轨迹信息的匹配度，若所述匹配度不满足预设匹配条件，则根据所述目标机器人的实时位置信息和姿态信息进行运动误差补偿，直至所述匹配度满足预设匹配条件。
在一种可选的实施方式中，
所述结合预先下发至所述目标机器人的目标位置信息结合所述目标机器人的实时位置信息，通过轨迹规划算法确定所述目标机器人的目标轨迹信息包括：
基于目标机器人中预设的传感器所获取的环境数据，将所述目标机器人所在环境进行网格化划分，将所述目标机器人当前位置设置为起点，将所述目标机器人当前位置所在网格的距离值设置为零，其中，每个网格代表一个位置；
从所述目标机器人当前位置开始，在每一步中，选择与所述当前位置距离值最小的网格，并计算从该网格出发到达相邻网格的距离，逐步更新相邻网格的距离值，直到到达目标位置或者更新所有可行动的网格的距离值；
从目标位置开始，通过比较相邻网格的距离值，反向回溯到起点，将距离值累加最小的路径确定为所述目标机器人的目标轨迹信息。
在一种可选的实施方式中，
所述根据改进的动态时间规整算法确定所述实际轨迹信息与所述目标轨迹信息的匹配度包括：
通过预设滑动采样窗口结合预设降采样因子将所述实际轨迹信息与所述目标轨迹信息划分为多个实际轨迹子序列和多个目标轨迹子序列，将每个实际轨迹子序列作为实际节点，将每个目标轨迹子序列作为目标节点，确定多个实际节点和目标节点之间的节点距离，并将多个节点距离存储在距离计算矩阵中；
将所述距离计算矩阵中各个节点按照节点距离大小进行顺序排列，并遍历计算进行顺序排列后相邻节点的相邻距离，结合所述改进的动态时间规整算法的相似性度量函数确定所述实际轨迹信息与所述目标轨迹信息的匹配度。
在一种可选的实施方式中，
结合所述改进的动态时间规整算法的相似性度量函数确定所述实际轨迹信息与所述目标轨迹信息的匹配度包括：
按照如下公式确定匹配度：
；
其中，P(M,N)表示所述实际轨迹信息与所述目标轨迹信息的匹配度，D()表示相似性度量函数，用于指示两条序列之间的累计距离，d()表示距离函数，M、N分别表示实际轨迹子序列和目标轨迹子序列，c表示插值参数。
在一种可选的实施方式中，
所述根据所述目标机器人的实时位置信息和姿态信息进行运动误差补偿包括：
根据目标机器人运动过程中的实际位置信息和实际姿态信息，结合目标机器人运动过程中的目标位置信息和目标姿态信息构建运动误差目标函数，将所述运动误差目标函数的参数进行初始化构建为初始种群，设置所述初始种群中粒子的飞行方向以及确定粒子的初始速度信息和初始位置信息；
获取所述初始种群的种群多样性，并且结合速度激活函数确定各个粒子对应的速度激活信息，根据所述速度激活信息对所述初始速度信息和初始位置信息进行更新；
确定速度和位置更新后粒子的适应度值，并判断所述适应度值是否优于预设适应度阈值，若是，则将当前粒子的适应度值作为新的适应度阈值，并且将当前粒子的飞行方向以及位置信息作为全局飞行方向和全局最优位置。
在一种可选的实施方式中，
所述获取所述初始种群的种群多样性，并且结合速度激活函数确定各个粒子对应的速度激活信息，根据所述速度激活信息对所述初始速度信息和初始位置信息进行更新包括：
按照如下公式对所述初始速度信息和初始位置信息进行更新：
；
；
；
其中，、/>分别表示第i个粒子在第k+1次和第k次迭代时的速度信息，k表示迭代次数，w表示速度惯性系数，c 1 、c 2 分别表示个体常数和群体常数，r 1 、r 2 分别表示个体均匀变量和群体均匀变量，/>、/>分别表示第i个粒子在第k次迭代时的个体最优解和全局最优解，/>、/>分别表示第i个粒子在第k次迭代和第k+1次迭代时的位置信息，/>表示第i个粒子在第k次迭代的速度激活信息；
I表示速度激活常数，f AC 表示速度激活函数，p i 表示第i个粒子对应的种群多样性。
本发明实施例的第二方面，
提供一种机器人轨迹误差修正系统，包括：
第一单元，用于基于目标机器人中预设的传感器对所述目标机器人所在环境进行感知和定位，获取所述目标机器人的实时位置信息和姿态信息；
第二单元，用于结合预先下发至所述目标机器人的目标位置信息结合所述目标机器人的实时位置信息，通过轨迹规划算法确定所述目标机器人的目标轨迹信息；
第三单元，用于根据所述目标机器人的实时位置信息和姿态信息生成所述目标机器人的实际轨迹信息，根据改进的动态时间规整算法确定所述实际轨迹信息与所述目标轨迹信息的匹配度，若所述匹配度不满足预设匹配条件，则根据所述目标机器人的实时位置信息和姿态信息进行运动误差补偿，直至所述匹配度满足预设匹配条件。
本发明实施例的第三方面，
提供一种电子设备，包括：
处理器；
用于存储处理器可执行指令的存储器；
其中，所述处理器被配置为调用所述存储器存储的指令，以执行前述所述的方法。
本发明实施例的第四方面，
提供一种计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令被处理器执行时实现前述所述的方法。
本发明实施例的有益效果可以参考具体实施方式中技术特征对应的技术效果，在此不再赘述。
附图说明
图1为本发明实施例机器人轨迹误差修正方法的流程示意图；
图2为本发明实施例机器人轨迹误差修正系统的结构示意图。
实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
下面以具体地实施例对本发明的技术方案进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例不再赘述。
图1为本发明实施例机器人轨迹误差修正方法的流程示意图，如图1所示，所述方法包括：
S101. 基于目标机器人中预设的传感器对所述目标机器人所在环境进行感知和定位，获取所述目标机器人的实时位置信息和姿态信息；
示例性地，可以根据目标机器人的应用场景和定位要求，选择适合的传感器，如激光雷达、摄像头、惯性测量单元（IMU）等，对所选传感器进行集成，确保传感器能够实时获取环境信息并提供高精度的位置和姿态数据，本申请实施例对目标机器人中预设的传感器进行限定。可选地，可以使用同时定位与地图构建（SLAM）算法，将传感器获取的数据用于构建实时的环境地图，并实现目标机器人的自主定位。
基于传感器数据和地图信息，运用定位算法（如扩展卡尔曼滤波、粒子滤波等）来估计目标机器人的实时位置和姿态，定期更新位置和姿态估计值，确保高精度的定位信息。
S102.结合预先下发至所述目标机器人的目标位置信息结合所述目标机器人的实时位置信息，通过轨迹规划算法确定所述目标机器人的目标轨迹信息；
示例性地，目标位置信息预先通过通信系统下发至目标机器人，包含目标位置的坐标信息。在接收到目标位置信息后，进行目标位置的优化和校验。根据实际场景需求，可能需要考虑避障、避免不可行区域等条件，确保目标位置是可达且合理的。
利用机器人内置的传感器系统，获取目标机器人的实时位置信息和姿态信息，根据目标机器人的动力学特性、运动约束和环境要求，选择合适的轨迹规划算法。使用所选的轨迹规划算法，在机器人的当前位置和优化后的目标位置之间生成预设的理想轨迹，连接路径上的航点。通过预先下发目标位置信息和实时位置信息，结合轨迹规划算法，实现目标机器人的准确轨迹跟踪和运动控制。
在一种可选的实施方式中，
所述结合预先下发至所述目标机器人的目标位置信息结合所述目标机器人的实时位置信息，通过轨迹规划算法确定所述目标机器人的目标轨迹信息包括：
基于目标机器人中预设的传感器所获取的环境数据，将所述目标机器人所在环境进行网格化划分，将所述目标机器人当前位置设置为起点，将所述目标机器人当前位置所在网格的距离值设置为零，其中，每个网格代表一个位置；
从所述目标机器人当前位置开始，在每一步中，选择与所述当前位置距离值最小的网格，并计算从该网格出发到达相邻网格的距离，逐步更新相邻网格的距离值，直到到达目标位置或者更新所有可行动的网格的距离值；
从目标位置开始，通过比较相邻网格的距离值，反向回溯到起点，将距离值累加最小的路径确定为所述目标机器人的目标轨迹信息。
示例性地，根据目标机器人所在环境的地图或感知数据，将环境进行网格化划分，每个网格代表一个位置，并记录该网格的位置坐标，将目标机器人当前位置设置为起点网格，并将其距离值设置为零，其他网格的距离值初始化为一个较大的值，表示暂时不可达。
从起点网格开始，使用Dijkstra算法等寻路算法，在每一步中选择与当前位置距离值最小的相邻网格，并计算从当前网格出发到达相邻网格的距离，将该距离与相邻网格的当前距离值进行比较，若小于当前距离值，则更新相邻网格的距离值，并将当前网格标记为该相邻网格的前驱。
继续进行路径规划，直到到达目标位置或者更新所有可行动的网格的距离值，此时，所有网格的距离值已更新完毕。从目标位置开始，通过比较相邻网格的距离值，反向回溯到起点，将距离值累加最小的路径确定为目标机器人的目标轨迹信息。
该技术方案通过利用预设的传感器和实时位置信息，实时地进行环境感知和轨迹规划，从而可以在机器人运动过程中快速响应环境变化，保证机器人能够准确、及时地修正轨迹；考虑环境网格化划分，可以对复杂和动态的环境进行有效处理，通过在每一步中选择距离值最小的网格，保证了路径的可行性，使得目标机器人能够在实际环境中顺利行动。网格化划分和距离值计算是一种高效的方法，可以在较短的时间内找到目标机器人的目标轨迹。此外，反向回溯的过程也不需要重新计算距离，进一步提高了计算效率。
在一种可选的实施方式中，
工业机器人是智能制造的关键装备之一，随着工业机器人在精密制造、航空航天、精密测量等高精度领域应用的不断增加，对机器人的运动性能要求也越来越高。机器人轨迹准确度指标是衡量机器人性能最关键的指标之一，国内外对其检测与评价制定了相应的规范和标准。检测过程中受机器人运动偏差、测量系统的采样频率和测量误差的影响，理论轨迹与实际轨迹可能出现映射错误的问题。
国际标准ISO 9283中规定，轨迹准确度表示机器人在同一方向上沿指令轨迹次移动其机械接口的能力。轨迹准确度由两个因素决定：指令轨迹的位置与实到轨迹位置集群的中心线之间的偏差(即位置轨迹准确度)，和指令姿态与实到姿态平均值之间的偏差(即姿态轨迹准确度)。工业机器人轨迹准确度由指令轨迹的位置与实到轨迹位置集群中心线之间的偏差决定。
实际应用中，由于运动偏差或测量误差的影响，采样点处的法平面可能相交，导致映射的错误，造成微小的轨迹测量误差。此外，为保证运动的连续平稳，机器人在拐角处的运动轨迹为近似轨迹，实测轨迹的形状与指令轨迹在拐角处存在一定的偏差，导致明显的映射错误，造成较大的轨迹测量误差。拐角处实测轨迹为接近过渡方式，在拐角处实测轨迹采样点的法平面与指令轨迹相交于明显错误的位置甚至不相交，造成极大的轨迹测量误差。
动态时间规整（Dynamic Time Warping，简称DTW）算法用于计算两个序列之间的相似度，它在处理时间序列数据的匹配和对齐问题中非常有用。然而，DTW算法在处理大规模数据时可能会变得相对较慢，因为它需要计算所有可能的路径。本申请针对现有DTW算法的问题，在保留其技术优点的前提下，对DTW算法进行改进，用于确定所述实际轨迹信息与所述目标轨迹信息的匹配度。
在一种可选的实施方式中，
所述根据改进的动态时间规整算法确定所述实际轨迹信息与所述目标轨迹信息的匹配度包括：
通过预设滑动采样窗口结合预设降采样因子将所述实际轨迹信息与所述目标轨迹信息划分为多个实际轨迹子序列和多个目标轨迹子序列，将每个实际轨迹子序列作为实际节点，将每个目标轨迹子序列作为目标节点，确定多个实际节点和目标节点之间的节点距离，并将多个节点距离存储在距离计算矩阵中；
将所述距离计算矩阵中各个节点按照节点距离大小进行顺序排列，并遍历计算进行顺序排列后相邻节点的相邻距离，结合所述改进的动态时间规整算法的相似性度量函数确定所述实际轨迹信息与所述目标轨迹信息的匹配度。
其中，本申请在动态时间规整算法的基础上，结合了降采样和索引优化提高动态时间规整算法的计算准确度和计算效率。
示例性地，通过预设滑动采样窗口结合预设降采样因子将所述实际轨迹信息与所述目标轨迹信息划分为多个实际轨迹子序列和多个目标轨迹子序列，其中，可以确定一个滑动采样窗口的大小，窗口的大小决定了每个子序列的长度，较小的窗口将导致更多的子序列，而较大的窗口将导致较少的子序列。将参与DTW计算的两个序列按照窗口大小进行划分，从序列的起始位置开始，将窗口覆盖在序列上，将窗口内的数据作为一个子序列。在进行窗口划分后，将窗口从序列的起始位置逐步滑动到末尾，每次滑动的步长可以是一个固定值，也可以根据实际需求进行调整。每次滑动窗口后，将窗口内的数据作为一个新的子序列。
通过窗口划分和窗口移动，将序列分成了多个较小的子序列，每个子序列都是DTW算法的输入之一，可以将长序列的计算问题转化为多个独立的小序列的计算问题，从而减少计算复杂性。
进一步地，设定降采样因子K，表示每隔K个数据点取一个样本点。较大的K值会减少序列的数据点数，从而加快DTW计算，但可能会损失一些细节信息。根据降采样因子可以逐步减少序列长度，并通过多层次的逼近来加速DTW计算，通过合理选择降采样因子K和分层的层数，可以在保证一定匹配准确性的前提下，显著加快DTW算法的计算速度。
确定多个实际节点和目标节点之间的节点距离，并将多个节点距离存储在距离计算矩阵中，可选地，将参与DTW计算的两个序列分别划分成较小的子序列，将每个子序列视为一个节点，并构建一个网格状结构，形成网格索引，计算节点之间的距离，并将这些距离值存储在一个距离计算矩阵中，通过参考距离计算矩阵，只计算相邻节点之间的距离和，避免了计算无效节点之间的距离。
在一种可选的实施方式中，
结合所述改进的动态时间规整算法的相似性度量函数确定所述实际轨迹信息与所述目标轨迹信息的匹配度包括：
按照如下公式确定匹配度：
；
其中，P(M,N)表示所述实际轨迹信息与所述目标轨迹信息的匹配度，D()表示相似性度量函数，用于指示两条序列之间的累计距离，d()表示距离函数，M、N分别表示实际轨迹子序列和目标轨迹子序列，c表示插值参数。
其中，在机器人轨迹规划和路径插值中，插值参数是用来控制路径在离散轨迹点之间进行插值的一组参数；插值是一种通过已知数据点之间的计算，来估计在两个已知数据点之间的数值的方法。在机器人轨迹规划中，轨迹通常是由离散的航点或路标点组成，而插值参数可以用来控制机器人在这些航点之间的运动轨迹。
通过滑动采样窗口和降采样因子的设置，可以将长序列划分为多个子序列，从而减少了实际轨迹和目标轨迹的数据点数，降低了计算复杂性；通过节点距离的计算和距离计算矩阵的使用，可以获得实际轨迹和目标轨迹之间的节点距离信息，从而更准确地计算匹配度；通过对距离计算矩阵中节点距离进行排序和相邻距离的计算，综合考虑了实际轨迹和目标轨迹之间多个子序列的匹配情况，提供了全局匹配度的信息；改进的动态时间规整算法采用相似性度量函数来进一步优化匹配过程，可以更灵活地调整匹配权重，提高匹配的准确性。
S103.根据所述目标机器人的实时位置信息和姿态信息生成所述目标机器人的实际轨迹信息，根据改进的动态时间规整算法确定所述实际轨迹信息与所述目标轨迹信息的匹配度，若所述匹配度不满足预设匹配条件，则根据所述目标机器人的实时位置信息和姿态信息进行运动误差补偿，直至所述匹配度满足预设匹配条件。
示例性地，预设匹配条件可以包括可以是设置一个阈值，用于判断实际轨迹信息与目标轨迹信息之间的匹配度是否满足要求，这个阈值可以是一个数值，代表两个轨迹之间的最大容许误差距离。比较改进的动态时间规整算法输出的匹配度评分与预设的阈值。
如果匹配度评分小于等于阈值，认为实际轨迹与目标轨迹之间的匹配度满足要求；
如果匹配度评分大于阈值，认为实际轨迹与目标轨迹之间的匹配度不满足要求，需要进行运动误差补偿。运动误差补偿算法可以根据匹配度和轨迹规划算法的输出，调整机器人的运动指令，修正实际轨迹，使其更接近目标轨迹。
在一种可选的实施方式中，
所述根据所述目标机器人的实时位置信息和姿态信息进行运动误差补偿包括：
根据目标机器人运动过程中的实际位置信息和实际姿态信息，结合目标机器人运动过程中的目标位置信息和目标姿态信息构建运动误差目标函数，将所述运动误差目标函数的参数进行初始化构建为初始种群，设置所述初始种群中粒子的飞行方向以及确定粒子的初始速度信息和初始位置信息；
获取所述初始种群的种群多样性，并且结合速度激活函数确定各个粒子对应的速度激活信息，根据所述速度激活信息对所述初始速度信息和初始位置信息进行更新；
确定速度和位置更新后粒子的适应度值，并判断所述适应度值是否优于预设适应度阈值，若是，则将当前粒子的适应度值作为新的适应度阈值，并且将当前粒子的飞行方向以及位置信息作为全局飞行方向和全局最优位置。
示例性地，本申请通过改进的粒子群算法进行运动误差补偿。具体地，本申请在粒子群算法的基础上，结合种群多样性，进行速度激活，能够有效避免传统粒子群算法容易陷入局部最优的困境。
示例性地，将目标机器人运动过程中的实际位置信息和实际姿态信息与目标位置信息和目标姿态信息进行对比，构建运动误差目标函数。该目标函数可以包括位置误差和姿态误差，以及其他需要考虑的误差因素。其中，运动误差目标函数可以现有的运动误差函数，本申请的运动误差目标函数可以综合位置误差和姿态误差，通过改进的粒子群算法确定运动误差目标函数中各项参数，用以弥补误差。
将运动误差目标函数的参数作为粒子群的初始解，构建初始种群，设定初始种群中粒子的飞行方向以及确定粒子的初始速度信息和初始位置信息；
评估初始种群的多样性，确保种群中粒子在解空间中具有足够的探索性；根据速度激活函数，确定各个粒子对应的速度激活信息，该信息可以控制粒子的运动速度。根据速度激活信息对初始速度信息和位置信息进行更新，以引导粒子向全局最优解和局部最优解靠近。
确定速度和位置更新后粒子的适应度值，即运动误差目标函数的值。该值表示当前粒子的解在目标函数中的优劣程度；判断当前粒子的适应度值是否优于预设的适应度阈值。如果优于阈值，则将当前粒子的适应度值作为新的适应度阈值，并将当前粒子的飞行方向以及位置信息作为全局飞行方向和全局最优位置。根据全局最优位置和速度激活函数，继续迭代更新粒子的位置和速度，直至满足收敛条件或达到预设的最大迭代次数。最终收敛的粒子将代表优化后的运动误差补偿策略，用于实际的轨迹修正和运动控制。
种群多样性是指粒子群中的各个粒子在解空间中的分布程度，较高的种群多样性意味着粒子在解空间中探索得更广，更有可能找到全局最优解；高种群多样性使得算法具有全局搜索能力，能够更快地收敛到全局最优解，从而提高了算法的效率和准确性；速度激活函数是一种控制粒子速度更新的机制，它可以控制粒子的运动步长，从而避免过大的速度变化。合理的速度激活函数可以保证算法的稳定性和收敛性；适当的速度激活函数可以防止粒子在搜索过程中过于剧烈地振荡，从而有助于更快地找到收敛解。
在一种可选的实施方式中，
所述获取所述初始种群的种群多样性，并且结合速度激活函数确定各个粒子对应的速度激活信息，根据所述速度激活信息对所述初始速度信息和初始位置信息进行更新包括：
按照如下公式对所述初始速度信息和初始位置信息进行更新：
；
；
；
其中，、/>分别表示第i个粒子在第k+1次和第k次迭代时的速度信息，k表示迭代次数，w表示速度惯性系数，c 1 、c 2 分别表示个体常数和群体常数，r 1 、r 2 分别表示个体均匀变量和群体均匀变量，/>、/>分别表示第i个粒子在第k次迭代时的个体最优解和全局最优解，/>、/>分别表示第i个粒子在第k次迭代和第k+1次迭代时的位置信息，/>表示第i个粒子在第k次迭代的速度激活信息；
I表示速度激活常数，f AC 表示速度激活函数，p i 表示第i个粒子对应的种群多样性。
示例性地，个体常数用于控制粒子根据自身历史的最优解进行更新。每个粒子在搜索过程中都会记住自己历史上的最好位置，个体常数决定了在更新速度时，粒子会考虑自己历史上的最优解对当前位置的影响；群体常数用于控制粒子根据整个群体的最优解进行更新。每个粒子在搜索过程中都会考虑整个群体中最好的位置，群体常数决定了在更新速度时，粒子会考虑整个群体最优解对当前位置的影响。
示例性地，个体均匀变量是一个取值范围在[0, 1]之间的随机数，每个粒子都会独立地生成自己的个体均匀变量；个体均匀变量用于控制粒子根据自身历史最优位置进行速度更新，它与个体常数结合，决定了粒子根据自身历史最优位置的影响程度。
群体均匀变量也是一个取值范围在[0, 1]之间的随机数，但它是由整个群体共享的，所有粒子都使用同一个群体均匀变量；群体均匀变量用于控制粒子根据整个群体最优位置进行速度更新，它与群体常数结合，决定了粒子根据整个群体最优位置的影响程度。
速度激活常数是用于调整粒子速度更新幅度的参数，速度激活常数决定了粒子速度更新时的惯性程度。较大的速度激活常数会增加粒子的惯性，使其在搜索空间中移动较快。较小的速度激活常数会减少粒子的惯性，使其更容易在局部区域进行搜索。速度激活函数是一个用于调整粒子速度更新的函数，它根据速度激活常数和个体均匀变量、群体均匀变量来计算粒子速度更新的幅度。
个体均匀变量和群体均匀变量用于确定粒子在更新速度时对个体历史最优位置和群体最优位置的关注程度。速度激活常数和速度激活函数则决定了粒子速度更新的幅度和方向，从而控制了粒子在搜索空间中的移动。这些参数和机制相互协作，共同影响着粒子群优化算法的搜索性能和收敛性。
本发明实施例的第二方面，
提供一种机器人轨迹误差修正系统，图2为本发明实施例机器人轨迹误差修正系统的结构示意图，包括：
第一单元，用于基于目标机器人中预设的传感器对所述目标机器人所在环境进行感知和定位，获取所述目标机器人的实时位置信息和姿态信息；
第二单元，用于结合预先下发至所述目标机器人的目标位置信息结合所述目标机器人的实时位置信息，通过轨迹规划算法确定所述目标机器人的目标轨迹信息；
第三单元，用于根据所述目标机器人的实时位置信息和姿态信息生成所述目标机器人的实际轨迹信息，根据改进的动态时间规整算法确定所述实际轨迹信息与所述目标轨迹信息的匹配度，若所述匹配度不满足预设匹配条件，则根据所述目标机器人的实时位置信息和姿态信息进行运动误差补偿，直至所述匹配度满足预设匹配条件。
本发明实施例的第三方面，
提供一种电子设备，包括：
处理器；
用于存储处理器可执行指令的存储器；
其中，所述处理器被配置为调用所述存储器存储的指令，以执行前述所述的方法。
本发明实施例的第四方面，
提供一种计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令被处理器执行时实现前述所述的方法。
本发明可以是方法、装置、系统和/或计算机程序产品。计算机程序产品可以包括计算机可读存储介质，其上载有用于执行本发明的各个方面的计算机可读程序指令。
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
Claims (6)
1.一种机器人轨迹误差修正方法，其特征在于，包括：
基于目标机器人中预设的传感器对所述目标机器人所在环境进行感知和定位，获取所述目标机器人的实时位置信息和姿态信息；
结合预先下发至所述目标机器人的目标位置信息结合所述目标机器人的实时位置信息，通过轨迹规划算法确定所述目标机器人的目标轨迹信息；
根据所述目标机器人的实时位置信息和姿态信息生成所述目标机器人的实际轨迹信息，根据改进的动态时间规整算法确定所述实际轨迹信息与所述目标轨迹信息的匹配度，若所述匹配度不满足预设匹配条件，则根据所述目标机器人的实时位置信息和姿态信息进行运动误差补偿，直至所述匹配度满足预设匹配条件；
所述结合预先下发至所述目标机器人的目标位置信息结合所述目标机器人的实时位置信息，通过轨迹规划算法确定所述目标机器人的目标轨迹信息包括：
基于目标机器人中预设的传感器所获取的环境数据，将所述目标机器人所在环境进行网格化划分，将所述目标机器人当前位置设置为起点，将所述目标机器人当前位置所在网格的距离值设置为零，其中，每个网格代表一个位置；
从所述目标机器人当前位置开始，在每一步中，选择与所述当前位置距离值最小的网格，并计算从该网格出发到达相邻网格的距离，逐步更新相邻网格的距离值，直到到达目标位置或者更新所有可行动的网格的距离值；
从目标位置开始，通过比较相邻网格的距离值，反向回溯到起点，将距离值累加最小的路径确定为所述目标机器人的目标轨迹信息；
所述根据改进的动态时间规整算法确定所述实际轨迹信息与所述目标轨迹信息的匹配度包括：
通过预设滑动采样窗口结合预设降采样因子将所述实际轨迹信息与所述目标轨迹信息划分为多个实际轨迹子序列和多个目标轨迹子序列，将每个实际轨迹子序列作为实际节点，将每个目标轨迹子序列作为目标节点，确定多个实际节点和目标节点之间的节点距离，并将多个节点距离存储在距离计算矩阵中；
将所述距离计算矩阵中各个节点按照节点距离大小进行顺序排列，并遍历计算进行顺序排列后相邻节点的相邻距离，结合所述改进的动态时间规整算法的相似性度量函数确定所述实际轨迹信息与所述目标轨迹信息的匹配度；
结合所述改进的动态时间规整算法的相似性度量函数确定所述实际轨迹信息与所述目标轨迹信息的匹配度包括：
按照如下公式确定匹配度：
P(M,N)＝min{D(M,N-1)+d(M+c,N),D(M-1,N)+d(M,N+c)}；
其中，P(M,N)表示所述实际轨迹信息与所述目标轨迹信息的匹配度，D()表示相似性度量函数，用于指示两条序列之间的累计距离，d()表示距离函数，M、N分别表示实际轨迹子序列和目标轨迹子序列，c表示插值参数。
2.根据权利要求1所述的方法，其特征在于，所述根据所述目标机器人的实时位置信息和姿态信息进行运动误差补偿包括：
根据目标机器人运动过程中的实际位置信息和实际姿态信息，结合目标机器人运动过程中的目标位置信息和目标姿态信息构建运动误差目标函数，将所述运动误差目标函数的参数进行初始化构建为初始种群，设置所述初始种群中粒子的飞行方向以及确定粒子的初始速度信息和初始位置信息；
获取所述初始种群的种群多样性，并且结合速度激活函数确定各个粒子对应的速度激活信息，根据所述速度激活信息对所述初始速度信息和初始位置信息进行更新；
确定速度和位置更新后粒子的适应度值，并判断所述适应度值是否优于预设适应度阈值，若是，则将当前粒子的适应度值作为新的适应度阈值，并且将当前粒子的飞行方向以及位置信息作为全局飞行方向和全局最优位置。
3.根据权利要求2所述的方法，其特征在于，所述获取所述初始种群的种群多样性，并且结合速度激活函数确定各个粒子对应的速度激活信息，根据所述速度激活信息对所述初始速度信息和初始位置信息进行更新包括：
按照如下公式对所述初始速度信息和初始位置信息进行更新：
其中，分别表示第i个粒子在第k+1次和第k次迭代时的速度信息，k表示迭代次数，w表示速度惯性系数，c1、c2分别表示个体常数和群体常数，r1、r2分别表示个体均匀变量和群体均匀变量，/>分别表示第i个粒子在第k次迭代时的个体最优解和全局最优解，/>分别表示第i个粒子在第k次迭代和第k+1次迭代时的位置信息，/>表示第i个粒子在第k次迭代的速度激活信息；
I表示速度激活常数，fAC表示速度激活函数，pi表示第i个粒子对应的种群多样性。
4.一种机器人轨迹误差修正系统，其特征在于，包括：
第一单元，用于基于目标机器人中预设的传感器对所述目标机器人所在环境进行感知和定位，获取所述目标机器人的实时位置信息和姿态信息；
第二单元，用于结合预先下发至所述目标机器人的目标位置信息结合所述目标机器人的实时位置信息，通过轨迹规划算法确定所述目标机器人的目标轨迹信息；
第三单元，用于根据所述目标机器人的实时位置信息和姿态信息生成所述目标机器人的实际轨迹信息，根据改进的动态时间规整算法确定所述实际轨迹信息与所述目标轨迹信息的匹配度，若所述匹配度不满足预设匹配条件，则根据所述目标机器人的实时位置信息和姿态信息进行运动误差补偿，直至所述匹配度满足预设匹配条件；
所述结合预先下发至所述目标机器人的目标位置信息结合所述目标机器人的实时位置信息，通过轨迹规划算法确定所述目标机器人的目标轨迹信息包括：
基于目标机器人中预设的传感器所获取的环境数据，将所述目标机器人所在环境进行网格化划分，将所述目标机器人当前位置设置为起点，将所述目标机器人当前位置所在网格的距离值设置为零，其中，每个网格代表一个位置；
从所述目标机器人当前位置开始，在每一步中，选择与所述当前位置距离值最小的网格，并计算从该网格出发到达相邻网格的距离，逐步更新相邻网格的距离值，直到到达目标位置或者更新所有可行动的网格的距离值；
从目标位置开始，通过比较相邻网格的距离值，反向回溯到起点，将距离值累加最小的路径确定为所述目标机器人的目标轨迹信息；
所述根据改进的动态时间规整算法确定所述实际轨迹信息与所述目标轨迹信息的匹配度包括：
通过预设滑动采样窗口结合预设降采样因子将所述实际轨迹信息与所述目标轨迹信息划分为多个实际轨迹子序列和多个目标轨迹子序列，将每个实际轨迹子序列作为实际节点，将每个目标轨迹子序列作为目标节点，确定多个实际节点和目标节点之间的节点距离，并将多个节点距离存储在距离计算矩阵中；
将所述距离计算矩阵中各个节点按照节点距离大小进行顺序排列，并遍历计算进行顺序排列后相邻节点的相邻距离，结合所述改进的动态时间规整算法的相似性度量函数确定所述实际轨迹信息与所述目标轨迹信息的匹配度；
结合所述改进的动态时间规整算法的相似性度量函数确定所述实际轨迹信息与所述目标轨迹信息的匹配度包括：
按照如下公式确定匹配度：
P(M,N)＝min{D(M,N-1)+d(M+c,N),D(M-1,N)+d(M,N+c)}；
其中，P(M,N)表示所述实际轨迹信息与所述目标轨迹信息的匹配度，D()表示相似性度量函数，用于指示两条序列之间的累计距离，d()表示距离函数，M、N分别表示实际轨迹子序列和目标轨迹子序列，c表示插值参数。
5.一种电子设备，其特征在于，包括：
处理器；
用于存储处理器可执行指令的存储器；
其中，所述处理器被配置为调用所述存储器存储的指令，以执行权利要求1至3中任意一项所述的方法。
6.一种计算机可读存储介质，其上存储有计算机程序指令，其特征在于，所述计算机程序指令被处理器执行时实现权利要求1至3中任意一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310955385.6A CN116661469B (zh) | 2023-08-01 | 2023-08-01 | 机器人轨迹误差修正方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310955385.6A CN116661469B (zh) | 2023-08-01 | 2023-08-01 | 机器人轨迹误差修正方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116661469A CN116661469A (zh) | 2023-08-29 |
| CN116661469B true CN116661469B (zh) | 2023-11-03 |
Family
ID=87710115
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310955385.6A Active CN116661469B (zh) | 2023-08-01 | 2023-08-01 | 机器人轨迹误差修正方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116661469B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116901090B (zh) * | 2023-09-14 | 2023-11-28 | 浩科机器人（苏州）有限公司 | 多轴自由度机器人的控制方法 |
| CN117798937B (zh) * | 2024-03-01 | 2024-05-31 | 深圳市大寰机器人科技有限公司 | 机器人标定方法、系统及机器人 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112549019A (zh) * | 2020-11-06 | 2021-03-26 | 北京工业大学 | 一种基于连续动态时间规整的工业机器人轨迹准确度分析方法 |
| CN113741453A (zh) * | 2021-08-31 | 2021-12-03 | 广州文远知行科技有限公司 | 一种非结构化环境的路径规划方法、装置、设备和介质 |
| CN113753082A (zh) * | 2021-09-17 | 2021-12-07 | 京东鲲鹏(江苏)科技有限公司 | 无人车轨迹更新方法及装置、控制方法及电子设备 |
| CN115213897A (zh) * | 2022-06-06 | 2022-10-21 | 安徽省配天机器人集团有限公司 | 轨迹补偿方法、装置以及计算机可读存储介质 |
| CN116483097A (zh) * | 2023-06-25 | 2023-07-25 | 小舟科技有限公司 | 人机交互智能轮椅的控制方法及装置、轮椅、存储介质 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20230157506A1 (en) * | 2021-11-22 | 2023-05-25 | Irobot Corporation | Trajectory-based localization and mapping |
-
2023
- 2023-08-01 CN CN202310955385.6A patent/CN116661469B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112549019A (zh) * | 2020-11-06 | 2021-03-26 | 北京工业大学 | 一种基于连续动态时间规整的工业机器人轨迹准确度分析方法 |
| CN113741453A (zh) * | 2021-08-31 | 2021-12-03 | 广州文远知行科技有限公司 | 一种非结构化环境的路径规划方法、装置、设备和介质 |
| CN113753082A (zh) * | 2021-09-17 | 2021-12-07 | 京东鲲鹏(江苏)科技有限公司 | 无人车轨迹更新方法及装置、控制方法及电子设备 |
| CN115213897A (zh) * | 2022-06-06 | 2022-10-21 | 安徽省配天机器人集团有限公司 | 轨迹补偿方法、装置以及计算机可读存储介质 |
| CN116483097A (zh) * | 2023-06-25 | 2023-07-25 | 小舟科技有限公司 | 人机交互智能轮椅的控制方法及装置、轮椅、存储介质 |
Non-Patent Citations (1)
| Title |
|---|
| 基于改进粒子群算法的机器人机构误差补偿;金宁宁;科技通报;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116661469A (zh) | 2023-08-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116661469B (zh) | 机器人轨迹误差修正方法及系统 | |
| CN111536964B (zh) | 机器人定位方法及装置、存储介质 | |
| CN107239076B (zh) | 基于虚拟扫描与测距匹配的agv激光slam方法 | |
| US9995589B2 (en) | Path plan generating apparatus for mobile body | |
| CN109597864B (zh) | 椭球边界卡尔曼滤波的即时定位与地图构建方法及系统 | |
| CN110398969A (zh) | 自动驾驶车辆自适应预测时域转向控制方法及装置 | |
| CN112444246B (zh) | 高精度的数字孪生场景中的激光融合定位方法 | |
| CN112904358A (zh) | 基于几何信息的激光定位方法 | |
| JP2019191498A (ja) | 地図作成装置 | |
| Zhu et al. | Online state-time trajectory planning using timed-esdf in highly dynamic environments | |
| CN113910001A (zh) | 一种数控机床空间误差辨识方法 | |
| CN109848988A (zh) | 一种基于历史多帧点云信息融合的扫描匹配方法及系统 | |
| CN112612034A (zh) | 基于激光帧和概率地图扫描的位姿匹配方法 | |
| CN116576868A (zh) | 一种多传感器融合精确定位及自主导航方法 | |
| CN112067007A (zh) | 地图生成方法、计算机存储介质及电子设备 | |
| CN115454061B (zh) | 一种基于3d技术的机器人路径避障方法及系统 | |
| CN115328168A (zh) | 基于自适应强跟踪的移动机器人同步定位与建图方法及系统 | |
| CN111912411B (zh) | 一种机器人导航定位方法、系统及存储介质 | |
| CN113296515A (zh) | 一种双独立电驱动车辆显式模型预测路径跟踪方法 | |
| Zhao et al. | Location technology of indoor robot based on laser sensor | |
| Oussalah et al. | New fusion methodology approach and application to mobile robotics: investigation in the framework of possibility theory | |
| Kuncara et al. | Enhancing accuracy in field mobile robot state estimation with GNSS and encoders | |
| CN116000939B (zh) | 一种基于定位波动估计的机器人自适应鲁棒控制方法 | |
| Suwoyo et al. | A MAPAEKF-SLAM Algorithm with Recursive Mean and Covariance of Process and Measurement Noise Statistic | |
| Rathnayake et al. | A Path Planning Technique to Reduce the Travel Time of a Micromouse |
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 |