CN116413657A

CN116413657A - 一种机器人定位方法和机器人

Info

Publication number: CN116413657A
Application number: CN202111666476.5A
Authority: CN
Inventors: 单鹏辉; 宋洪超; 陈庆勇
Original assignee: Midea Group Co Ltd; Guangdong Midea White Goods Technology Innovation Center Co Ltd
Current assignee: Midea Group Co Ltd; Guangdong Midea White Goods Technology Innovation Center Co Ltd
Priority date: 2021-12-31
Filing date: 2021-12-31
Publication date: 2023-07-11

Abstract

本发明公开了一种机器人定位方法和机器人，该机器人定位方法包括：机器人利用第一传感器获取检测距离，以及利用第二传感器获取移动距离；结合检测距离和移动距离，对检测误差进行最小化求解，得到机器人的定位坐标，机器人的定位坐标至少包括当前位置和前一位置的定位坐标，检测误差为第一误差与第二误差的和，第一误差包括机器人位置的定位坐标与基站之间的距离和检测距离的差值，机器人位置至少包括当前位置；第二误差包括相邻两个位置的定位坐标之间的距离和对应的移动距离的差值，相邻两个位置至少包括当前位置和前一位置。通过上述方式，本发明能够适应复杂环境，提高机器人定位准确性。

Description

一种机器人定位方法和机器人

技术领域

本发明涉及机器人技术领域，特别是涉及一种机器人定位方法和机器人。

背景技术

目前，机器人在室外环境主要利用传感器，如UWB传感器、里程计、GPS-RTK等进行定位，该定位方法容易收到环境干扰，定位信息噪声较大，在一些情况下甚至会造成定位无解不足，不满足机器人实时导航需求。

发明内容

本发明主要解决的技术问题是提供一种机器人定位方法和机器人，能够适应复杂环境，提高机器人定位准确性。

为解决上述技术问题，本发明采用的一个技术方案是：提供一种机器人定位方法，该机器人定位方法包括：机器人利用第一传感器获取检测距离，检测距离至少包括当前位置与基站之间的检测距离，以及利用第二传感器获取移动距离，移动距离至少包括当前位置相对前一位置的移动距离；结合检测距离和移动距离，对检测误差进行最小化求解，得到机器人的定位坐标，机器人的定位坐标至少包括当前位置和前一位置的定位坐标，检测误差为第一误差与第二误差的和，第一误差包括机器人位置的定位坐标与基站之间的距离和检测距离的差值，机器人位置至少包括当前位置；第二误差包括相邻两个位置的定位坐标之间的距离和对应的移动距离的差值，相邻两个位置至少包括当前位置和前一位置。

其中，检测距离包括当前位置与基站之间的检测距离和前一位置与基站之间的检测距离，移动距离包括当前位置相对前一位置的移动距离、前一位置相对前二位置的移动距离，结合检测距离和移动距离，对检测误差进行最小化求解，得到机器人的定位坐标包括：结合检测距离和移动距离，对检测误差进行最小化求解，得到当前位置的定位坐标和前一位置的定位坐标；第一误差包括当前位置的定位坐标与基站之间的距离和检测距离的差值、对应的前一位置的定位坐标与基站之间的检测距离和对应的移动距离的差值；第二误差包括当前位置和对应的前一位置的定位坐标之间的距离和对应的移动距离的差值、前一位置和对应的前二位置的定位坐标之间的距离和对应的移动距离的差值。

其中，利用第二传感器获取移动距离包括：分别利用里程计和惯性测量单元得到机器人的移动速度；获得里程计测得的移动速度和惯性测量单元测得的移动速度的综合速度；结合综合速度以及机器人当前位置相对前一位置的移动时间，得到检测移动距离。

其中，获得里程计测得的移动速度和惯性测量单元测得的移动速度的综合速度包括：求里程计测得的移动速度和惯性测量单元测得的移动速度的平均速度，以作为综合速度。

其中，利用里程计得到机器人的移动速度包括：利用里程计获取机器人的当前位置的坐标和前一位置的坐标；综合当前位置相对前一位置的移动时间、当前位置的坐标和前一位置的坐标，计算里程计测得的移动速度。

其中，利用惯性测量单元得到机器人的移动速度包括：利用惯性测量单元得到机器人在横坐标方向的加速度和纵坐标方向的加速度；综合当前位置相对前一位置的移动时间、横坐标方向的加速度和纵坐标方向的加速度，计算惯性测量单元测得的移动速度。

其中，对检测误差进行最小化求解，得到机器人的定位坐标包括：构建修正函数，修正函数中包含调节因子；通过调整调节因子对机器人的位置进行修正，对检测误差的倒数为零进行求解，得到机器人的定位坐标。

其中，对检测误差进行最小化求解，得到机器人的定位坐标之后还包括：结合定位坐标和变换参数获得第一传感器坐标系下的定位坐标；变换参数通过以下方式获得：分别利用第一传感器和第二传感器得到机器人的移动轨迹；将第一传感器测量的移动轨迹和第二传感器测量的移动轨迹进行对齐得到对齐轨迹；得到第一传感器测量的移动轨迹转换为对齐轨迹的变换参数。

其中，机器人利用第一传感器获取当前位置与基站之间的检测距离包括：第一传感器向基站发送数据信号；接收基站发送的返回信号；结合数据信息和返回信息的时间戳获取当前位置与基站之间的检测距离。

为解决上述技术问题，本发明采用的另一个技术方案是：提供一种机器人，其特征在于，机器人包括：机体；信息采集组件，安装于机体并用于采集检测距离和移动距离；控制器，连接机体和信息采集组件，用于控制机体配合信息采集组件，采用上述的机器人定位方法对机器人进行定位。

本发明的有益效果是：区别于现有技术的情况，本发明通过使用两个不同的传感器分别获取距离信息和移动信息，将机器人自身的运动变化与机器人与固定基站之间的距离作为约束条件，建立误差方程，对误差方程进行最小化求解，得到机器人的定位信息，本申请不仅适用于大多数移动机器人，而且使用两个传感器弥补了一个传感器获取信息的误差，提高定位鲁棒性，使得机器人在复杂的环境下仍然可以提高定位的准确性。

附图说明

图1是本申请机器人定位方法一实施方式的流程示意图；

图2是本申请机器人移动路线的一实施方式的示意图；

图3是本申请机器人定位方法另一实施方式的流程示意图；

图4是本申请机器人获取移动距离方法的一实施方式的流程示意图；

图5是本申请机器人定位方法又一实施方式的流程示意图；

图6是本申请UWB传感器和EKF传感器坐标对齐的一实施方式中的流程示意图；

图7是本申请UWB传感器进行三角定位的示意图；

图8是本申请UWB传感器的定位轨迹和EKF传感器的定位轨迹对齐的一实施方式中的示意图；

图9是本申请机器人一实施方式的结构示意图。

具体实施方式

为使本发明的目的、技术方案及效果更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。

本申请提供一种机器人定位方法，在机器人上安装两个不同的传感器，在机器人周围安装有至少一个基站，分别为第一传感器和第二传感器，通过使用两个不同的传感器分别获取距离信息和移动信息，将机器人自身的运动变化与机器人与固定基站之间的距离作为约束条件，建立误差方程，对误差方程进行最小化求解，得到机器人的定位信息，本申请不仅适用于大多数移动机器人，而且使用两个传感器弥补了一个传感器获取信息的误差，提高定位鲁棒性，使得机器人在复杂的环境下仍然可以提高定位的准确性。

请参阅图1和图2，图1是本申请机器人定位方法一实施方式的流程示意图，图2是本申请机器人移动路线的一实施方式的示意图。需注意的是，若有实质上相同的结果，本实施例并不以图1所示的流程顺序为限。如图1所示，本实施方式包括：

S110：机器人利用第一传感器获取检测距离，利用第二传感器获取移动距离。

请参阅图2，图2中的圆形表示机器人的位置，三角形为基站位置，机器人利用第一传感器向基站发送信号，获取当前位置与基站之间的距离，利用第二传感器获取当前位置相对前一位置的移动距离。

S130：结合检测距离和移动距离，对检测误差进行最小化求解，得到机器人的定位坐标。

机器人的定位坐标至少包括当前位置和前一位置的定位坐标，检测误差为第一误差与第二误差的和，第一误差包括机器人位置的定位坐标与基站之间的检测距离和移动距离的差值，机器人位置至少包括当前位置；第二误差包括相邻两个位置的定位坐标之间的距离和对应的移动距离的差值，相邻两个位置至少包括当前位置和前一位置。

该实施方式中，通过使用两个不同的传感器分别获取距离信息和移动信息，将机器人自身的运动变化与机器人与固定基站之间的距离作为约束条件，建立误差方程，对误差方程进行最小化求解，得到机器人的定位信息，本申请不仅适用于大多数移动机器人，而且使用两个传感器弥补了一个传感器获取信息的误差，提高定位鲁棒性，使得机器人在复杂的环境下仍然可以提高定位的准确性。

请参阅图3，图3是本申请机器人定位方法另一实施方式的流程示意图。本申请提出另一种机器人定位方法，通过EKF滤波器，将里程计和IMU进行融合，从而得到机器人相对位移误差较小的移动距离。通过UWB标签和基站之间的通讯，利用超宽带信号的飞行时间差来得到UWB标签和基站之间的检测距离。将上述两个信息作为数据量，输入给图优化算法。最终，通过非线性优化方法(LM)来得到机器人定位信息，该结果能很好处理室外复杂环境下UWB距离误差较大的问题，以及弥补纯编码器对于机器人在草地上行走的情况下里程信息不准确的缺点。

请参阅图2，{p₀,p₁,...,p_n}表示机器人的位姿，{A₀,A₁,...,A_n}表示若干UWB基站的位姿，在该实施方式中，需要至少一个基站。在图2中，多个p点和至少一个A点构成了图的顶点集。在一实施方式中，第一传感器为UWB传感器，第二传感器为EKF传感器。利用UWB传感器获取检测距离，检测距离为机器人与基站之间的距离，即p点与A点之间的距离，利用EKF传感器获取移动距离，移动距离为机器人两个位置之间的距离，即p_i与p_j之间的距离，多个检测距离和多个移动距离构成了图的边集，利用图优化算法结合检测距离和移动距离得到机器人的定位信息。

UWB传感器为利用UWB技术进行测距发传感器，其中，UWB(Ultra Wide Band，超宽带)技术是一种无线载波通信技术，拥有500MHz以上的频带宽度。通过超大带宽和低发射功率，实现低功耗水平上的快速数据传输。由于UWB脉冲的时间宽度极短，因此也可以采用高精度定时来进行距离测算。在机器人上安装有UWB传感器，在机器人运行区域的四周安装有UWB基站，中间尽量避免有遮挡，基站间距离小于50米。利用机器人上安装的UWB传感器向基站发送数据信号，并接收基站发送的返回信号，结合数据信息和返回信息的时间戳信息获取当前位置与基站之间的检测距离。

因利用UWB传感器获取定位信息时，需要进行初始化流程，并且还需要一定质量和数量的UWB距离信息来进行优化，且UWB传感器获取信息频率低，还具有一定延迟，即使该延迟对低俗运动的机器人而言时在可接受范围之内。若单独使用UWB传感器定位，容易收到复杂室外环境的干扰，噪声较大，定位无解。因此，为弥补UWB传感器的问题，本申请另外引入一种EKF传感器，其中，EKF传感器包括有里程计和IMU(Inertial Measurement Unit，惯性测量单元)，在机器人行驶过程中，分别利用里程计获取机器人的移动时间和移动距离，利用IMU获取机器人移动过程中的角速度和加速度，结合里程计和IMU的获取的信息，推算得到机器人在EKF传感器下的坐标值。

EKF分别间隔一定时间获取一次机器人的当前位置信息，将机器人前一次获取机器人信息的所在位置作为前一位置。如图2所示，机器人从p_k-2移动到p_k+1，当前位置为p_k+1，前一位置为p_k。将请参阅图4，图4是本申请机器人获取移动距离方法的一实施方式的流程示意图。需注意的是，若有实质上相同的结果，本实施例并不以图4所示的流程顺序为限。如图4所示，本实施方式包括：

S410：利用里程计得到机器人的移动速度。

利用EKF传感器获取当前位置的坐标为(x_pi，y_pi)，前一位置的坐标为(x_pj，y_pj)。并利用里程计获取从前一位置移动到当前位置的移动时间，利用移动距离和移动时间计算里程计测得的移动速度v_odom为：

其中，当前位置相对前一位置的移动时间为t。

S430：利用惯性测量单元得到机器人的移动速度。

利用惯性测量单元得到机器人在横坐标方向的加速度为a_x，纵坐标方向的加速度为a_y；综合当前位置相对前一位置的移动时间t、横坐标方向的加速度和纵坐标方向的加速度，计算惯性测量单元测得的移动速度v_imu为：

v_imu＝(∫a_xdt+∫a_ydt)/2。

为了适合在嵌入式平台上应用，本申请采用了滑动窗口的方式来对节点进行定义，只对i时刻到j时刻的加速度的相对短时间内进行积分，使得计算量大大减少，实时性强，适合在机器人计算单元上使用。

S450：获得里程计测得的移动速度和惯性测量单元测得的移动速度的综合速度。

将里程计测得的移动速度和惯性测量单元测得的移动速度的平均速度，以作为综合速度，具体地，综合速度v的计算方式为：v＝(v_odom+v_imu)/2。

S470：结合综合速度以及机器人当前位置相对前一位置的移动时间，得到检测移动距离。

在本申请的实施方式中，会将EKF传感器的测距信息与UWB传感器的测距信息相结合，但EKF传感器获取数据的频率较高，UWB传感器获取数据的频率较低，两个传感器的时间戳没有严格对齐，并且两个传感器的噪声模型相差较大，因此，不能直接使用里程计坐标值来推算机器人的移动距离，而是分别利用里程计和惯性测量单元的移动速度，间接获取机器人的移动距离。

因机器人两次获取位置信息的运动时间较短，可以将机器人在这段时间内的运动视为匀速运动模型，利用机器人的综合速度和移动时间，得到检测移动距离s_EKF，具体地，s_EKF＝vt。

在该实施方式中，通过将机器人在短时间内的运行视为匀速运动模型，便于获取机器人的移动距离，同时，考虑到EKF传感器和UWB传感器的时间戳没有严格对齐的问题，分别对EKF传感器中的里程计和IMU计算机器人的运行速度，并融合了里程计和IMU的检测信息，提高了模型准确度，并为之后将EKF传感器和UWB传感器的信息融合提供便利。

请参阅图2和图5，图5是本申请机器人定位方法又一实施方式的流程示意图。需注意的是，若有实质上相同的结果，本实施例并不以图5所示的流程顺序为限。如图5所示，本实施方式包括：

S510：建立第一误差。

在本申请的实施方式中，如图2所示，{p₀,p₁,...,p_n}表示机器人的位姿节点，{A₀,A₁,...,A_n}表示若干UWB基站的位姿节点，将图2中各机器人的位姿点与基站的位姿点之间的距离作为第一约束条件，用于获取机器人的定位坐标。其中，利用上述UWB传感器，可以获取机器人的位姿点与基站之间的检测距离r_uwb，假设机器人的定位坐标与基站之间的距离为r_est，该距离r_est为估计距离，将两个距离的差值作为第一误差

则第一误差的表示为：

其中，/>

表示为节点p_i和节点A_j的约束关系。而当第一误差的值越小时，机器人的定位坐标越准确。

S530：建立第二误差。

在本申请的实施方式中，将图2中相邻两个机器人的位姿点之间的距离作为第二约束条件，用于获取机器人的定位坐标。其中，利用上述UWB传感器，可以获取机器人的相邻两个位姿点之间的移动距离t_ekf，假设机器人的相邻两个定位坐标之间的距离为t_est，该距离t_est为估计距离，将两个距离的差值作为第二误差

则第二误差的表示为：

其中，/>

表示为节点p_i和节点p_j的约束关系。而当第二误差的值越小时，机器人的定位坐标越准确。

S550：建立检测误差，并对检测误差进行最小化求解，得到机器人的定位坐标。

检测误差为第一误差和第二误差的和，当检测误差越小时，机器人的定位坐标越准确。因此，对检测误差进行最小化求解，就可以得到机器人的定位坐标。其中，第一误差至少包括机器人的当前位置的定位坐标与基站之间的距离和检测距离的差值，第二误差至少包括当前位置和前一位置的定位坐标之间的距离和对应的移动距离的差值，机器人的定位坐标至少包括当前位置和前一位置的定位坐标。本申请方法中，基站的数量可以为一个或多个。

在本申请的实施方式中，因每次使用的约束不同，得到的机器人的定位坐标会发生变化，当约束增多时，在获取机器人当前位置的定位坐标的同时，调整之前位置的机器人的定位坐标，以使得检测误差最小。

在一实施方式中，利用当前位置、前一位置和一个基站之间的约束建立检测误差进行求解。如图2所示，当前位置为p_k+1，前一位置为p_k，基站为A₂，对机器人节点集{p_k,p_k+1}和基站节点集{A₂}进行约束。利用UWB传感器获取当前位置和基站之间的距离为d_A2→Pk+1，前一位置与基站之间的距离为d_A2→Pk，估计当前位置和基站之间的距离为r_A2→Pk+1，估计前一位置与基站之间的距离为r_A2→Pk，得到两个第一误差为r_A2→Pk+1-d_A2→Pk+1，r_A2→Pk-d_A2→Pk，利用EKF传感器获取当前位置和前一位置之间的距离为d_Pk→Pk+1，估计当前位置和前一位置定位坐标之间的距离为t_Pk→Pk+1，得到一个第二误差t_Pk→Pk+1-d_Pk→Pk+1，则检测误差为r_A2→Pk+1-d_A2→Pk+1+r_A2→Pk-d_A2→Pk+t_Pk→Pk+1-d_Pk→Pk+1，对检测误差进行最小化求解，得到机器人当前位置和前一位置的定位坐标。

在另一实施方式中，利用当前位置、前一位置、前二位置和一个基站之间的约束建立检测误差进行求解。即在机器人节点集中新增一个节点，得到机器人节点集为{p_k-1,p_k,p_k+1}。利用UWB传感器获取当二位置和基站之间的距离为d_A2→Pk-1，估计前二位置和基站之间的距离为r_A2→Pk-1，新增一个第一误差为r_A2→Pk-1-d_A2→Pk-1，利用EKF传感器获取前一位置和前二位置之间的距离为d_Pk-1→Pk，估计前一位置和前二位置定位坐标之间的距离为r_Pk-1→Pk，新增一个第二误差为r_Pk-1→Pk-d_Pk-1→Pk，则检测误差为r_A2→Pk+1-d_A2→Pk+1+r_A2→Pk-d_A2→Pk+t_Pk→Pk+1-d_Pk→Pk+1+r_Pk-1→Pk-d_Pk-1→Pk，对检测误差进行最小化求解，得到机器人当前位置、前一位置和前二位置的定位坐标。

一般地，选择多个机器人节点和基站节点，分别获取每个机器人节点和每个基站节点之间的距离，估计每个定位坐标与每个基站之间的距离，构建对应的第一误差。分别获取相邻机器人节点之间的距离，估计相邻定位坐标之间的距离，构建对应的第二误差。对检测误差进行最小化求解，得到每个机器人节点的定位坐标。具体地，请继续参阅图2，获取四个机器人节点的节点集为{p_k-2,p_k-1,p_k,p_k+1}，四个基站节点的节点集为{A₀，A₁，A₂，A₃}，利用UWB传感器获取各个机器人节点和各个基站之间的距离，如基站节点A₀与机器人节点P_k-2之间的距离为d_A0→Pk-2，基站节点A₀与机器人节点P_k-1之间的距离为d_A0→Pk-1，基站节点A₀与机器人节点P_k之间的距离为d_A0→Pk，基站节点A₀与机器人节点P_k+1之间的距离为d_A0→Pk+1，基站节点A₁与机器人节点P_k-2之间的距离为d_A1→Pk-2，基站节点A₁与机器人节点P_k-1之间的距离为d_A1→Pk-1，基站节点A₁与机器人节点P_k之间的距离为d_A1→Pk，基站节点A₁与机器人节点P_k+1之间的距离为d_A1→Pk+1。利用16个机器人节点和基站之间的距离构建16个第一误差。机器人节点P_k-2与机器人节点P_k-1之间的距离为d_Pk-2→Pk-1，机器人节点P_k-1与机器人节点P_k之间的距离为d_Pk-1→Pk，机器人节点P_k与机器人节点P_k+1之间的距离为d_Pk→Pk+1。利用4个机器人节点直接的距离构建和三个第二误差。

将机器人节点和基站节点作为一个节点集ν＝{ν₁,ν₂,...,ν_n}，机器人的平移矩阵为

综合误差函数为：/>

对该综合误差函数进行最小化求解。

设误差函数为f(t)，则求解误差最小的问题转化成非线性最小二乘问题：

设误差函数f(t)为非线性函数，对其进行一阶泰勒近似有：

f(t+Δt)≈l(Δt)≡f(t)+J(t)·Δt，

其中，J为残差函数f(t)的雅克比矩阵，将上式带入到

中：

对上式求导，令其导数为零：

J(t)^TJ(t)Δt＝-J(t)^Tf(t)，

令H＝J(t)^TJ(t)，B＝-J(t)^Tf(t)则上式为：

HΔt＝B，

根据Levenberg-Marquarelt算法原理，对上式进行改造，得到修正函数：

(H+λI)Δt＝B，

对上式添加一个调节因子λI，其中λ是步长，I是单位矩阵。

通过启发式地调整λ来改善算法在局部极值附近的特性，对检测误差的倒数为零进行求解，即对F(x)＝0方程进行求解，即可得到优化后的机器人定位坐标P_opt，该定位坐标为机器人的当前位置的定位坐标，前n个定位坐标不进行输出。其中，核函数为柯西鲁棒核函数。

在上述实施方式中，在优化的时候综合利用UWB测距信息和机器人轨迹信息，减少了UWB在NLOS情况下测距误差较大的影响，增加了现实条件下物理学运动的强约束。同时弥补了EKF传感器现在机器人行走过程中里程信息不准确的问题。

但利用上述方法获取的机器人的定位坐标为将UWB传感器和EKF传感器融合之后的坐标系下的定位坐标。而EKF传感器在进行机器人的位姿推算时，结合了里程计和IMU的信息，具体地，设机器人的X状态包含3个变量，分别是横坐标x，纵坐标y，航向角theta，输入u量是线加速度linear_x，角速度angle_z，则X状态的预测为：

X_pre＝A*x+B*u，

其中，A是状态的矩阵系数，B是u输入矩阵系数。观测量Z为：

Z＝H*X，

系统偏差Y为：

Y＝Z-H*X_pre，

卡尔曼增益K为：

K＝Cov_pre*H^T*S^-1，

其中，Cov_pre是运动过程协方差矩阵，等于：

其中，J_F是状态的雅克比矩阵，等于：

Q是运动模型的噪声矩阵，S是关于Cov_pre的更新，S为：

S＝H*Cov_pre*H^T+R，

R是测量模型的噪声矩阵。最终，得到的X状态的估计为：

X_est＝X_pre+I*(Z-I*X_pre)，

协方差矩阵更新迭代式为：

Cov_est＝(I-K*H)Cov_pre。

如上述所示，EKF传感器结合了里程计和IMU进行机器人位姿推算，二里程计和IMU在机器人每次重新上电时，输出的都为相对量，即每次输出基准为零。而UWB坐标系是固定的，和机器人重新上电动作无关，输出的都为绝对量。因此，UWB传感器和EKF传感器的坐标系不同。

为方便机器人定位，需要将上述方法得到的定位坐标转换为其中一个传感器的坐标系下的坐标。因此，需要获取将UWB传感器和EKF传感器的坐标对齐所需的变换参数。

在进行本申请的机器人定位方法之前，先在驱动机器人行驶一段距离，分别利用UWB传感器和EKF传感器获取机器人在这段时间内的移动轨迹。本申请将两段移动轨迹进行坐标对齐，同时寻找一个变换参数，使得两端轨迹的均方根误差最小。

请参阅图6，图6是本申请UWB传感器和EKF传感器坐标对齐的一实施方式中的流程示意图。需注意的是，若有实质上相同的结果，本实施例并不以图6所示的流程顺序为限。如图6所示，本实施方式包括：

S610：检测各个传感器。

分别检测UWB传感器、里程计和IMU传感器是否工作正常，当三个传感器工作均正常，则进行步骤S630。

S630：静止机器人，利用三角定位法对机器人的当前位置进行定位。

将机器人静止，机器人利用安装的UWB传感器向周围的至少三个UWB基站发送数据信号，并接收三个UWB基站发送的返回信号，结合数据信息和返回信息的时间戳信息获取机器人当前位置与各个UWB基站之间的距离。请参阅图7，图7是本申请UWB传感器进行三角定位的示意图。如图7所示，机器人当前所在位置为P(x_p,y_p)，三个UWB基站的位置分别为R₁(x₁,y₁)，R₂(x₂,y₂)，和R₁(x₃,y₃)，利用上述UWB传感器，获取机器人当前位置与三个UWB基站之间的距离为d1，d2和d3。通过最小二乘法算法解析出机器人的当前位置的坐标。具体地，通过图7可以建立以下关系式：

由上式可知，在UWB传感器的坐标下，机器人的当前位置坐标为：

x＝(A^TA)^-1A^Tb，

其中：

计算，得到位置坐标x为

采集一段时间内的定位数据作为当前位置的起点，并通过其数据的标准差判断是否符合条件，若符合条件，则进行步骤S650。

S650：调用行走指令，控制机器人行走一段预设轨迹，并在行走过程中实时检测传感器数据。

控制机器人行走一段预设轨迹，该预设轨迹可以为直线，也可以为任意曲线。在行走过程中实时获取UWB传感器和EKF传感器的定位坐标，并且实时判断UWB传感器和EKF传感器的状态是否持续在检测计算中，判断UWB传感器和EKF传感器的数据状态是否合理，是否满足预设轨迹。机器人行走过程中的各项数据都满足条件，则在完成预设轨迹之后停止机器人运行或者停止机器人上的UWB传感器和EKF传感器的运行。

S670：通过最小化二乘法估计UWB传感器的定位轨迹与EKF传感器的定位轨迹之间的旋转和平移，得到机器人与UWB传感器的坐标系的位置和朝向角度差。

请参阅图8，图8是本申请UWB传感器的定位轨迹和EKF传感器的定位轨迹对齐的一实施方式中的示意图。如图8左侧所示，该两条轨迹线分别为UWB传感器的定位轨迹和EKF传感器的定位轨迹。在该实施方式中，预设轨迹为预设曲线。设定变换参数(R,t,c)，将图8左侧两条定位轨迹变换为图8右侧的一条定位轨迹，即使两段轨迹的均方根误差最小，具体计算方式为：

其中，R是旋转矩阵，t是平移矩阵，c是尺度因子。

采用umeyama算法来求解变换参数，求解公式为：

R＝USV^T，

t＝μ_y-cRμ_x，

其中，U和V是对旋转矩阵R进行QR分解之后得到的左奇异矩阵和右奇异矩阵，UU^T＝VV^T＝I，S是单位阵或对角阵，具体为：

其中，μ_x为轨迹一的均值

μ_y为轨迹二的均值/>

为方差

D为对角阵D＝diag(d_i),d₁≥d₂≥...d_m≥0。

利用上述方法，求解得到变换参数(R,t,c)，利用坐标对齐得到的变换参数(R,t,c)将机器人在UWB传感器和EKF传感器融合之后的坐标系下的定位坐标P_opt转换为UWB传感器的坐标系下的定位坐标P_robot，具体转换方式如下：

P_robot＝cRP_opt+t。

在本申请的实施方式中，既在利用EKF获取移动距离时，用匀速运动模型和EKF的方式融合了里程计、IMU数据，提高了模型的准确度，又在优化的时候综合利用UWB测距信息和机器人轨迹信息，减少了UWB在NLOS情况下测距误差较大的影响，增加了现实条件下物理学运动的强约束。同时还考虑到EKF传感器和UWB传感器的坐标系不同的问题，利用坐标对齐步骤进行定位校准，使得最终定位误差大大减少，鲁棒性较强。

请参阅图9，图9是本申请机器人一实施方式的结构示意图。该实施方式中，割草机器人包括机体91、信息采集组件92和控制器93。

其中，信息采集组件92安装于机体并用于采集检测距离和移动距离；控制器93安装于机体并用于控制机体配合信息采集组件，采用上述的机器人定位方法对所述机器人进行定位。该机器人通过使用两个不同的传感器分别获取距离信息和移动信息，将机器人自身的运动变化与机器人与固定基站之间的距离作为约束条件，建立误差方程，对误差方程进行最小化求解，得到机器人的定位信息，本申请不仅适用于大多数移动机器人，而且使用两个传感器弥补了一个传感器获取信息的误差，提高定位鲁棒性，使得机器人在复杂的环境下仍然可以提高定位的准确性。

本申请的机器人定位方法可以应用于多种场景下的机器人。在一具体实施方式中，可以应用于割草机器人，现有的智能割草机器人定位技术一般为UWB传感器技术和GPS-RTK技术，其中，UWB技术比较成熟，通过无线定位，通过测量距离，结合三角定位算法得到定位信息。但由于UWB传感器容易受复杂环境的干扰，噪声UWB测距误差较大，对于割草机器人而言，需要在复杂的草场环境进行工作，使用UWB传感器时，当UWB信号在遮挡时间较长情况下会发生割草机器人定位漂移的问题。因此，将本申请的机器人定位方法应用于割草机器人，在UWB传感器的基础上增加EKF传感器，使得割草机器人可以很好地处理复杂室外草场环境在UWB传感器误差较大的问题，也可以弥补割草机器人在草地环境行走的情况下，EKF传感器中里程计信息不准确的问题。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

以上所述仅为本发明的实施方式，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims

1.一种机器人定位方法，其特征在于，所述方法包括：

所述机器人利用第一传感器获取检测距离，所述检测距离至少包括当前位置与基站之间的检测距离，以及利用第二传感器获取移动距离，所述移动距离至少包括所述当前位置相对前一位置的移动距离；

结合所述检测距离和所述移动距离，对检测误差进行最小化求解，得到机器人的定位坐标，所述机器人的定位坐标至少包括当前位置和前一位置的定位坐标，所述检测误差为第一误差与第二误差的和，所述第一误差包括机器人位置的所述定位坐标与所述基站之间的距离和所述检测距离的差值，所述机器人位置至少包括所述当前位置；所述第二误差包括相邻两个位置的所述定位坐标之间的距离和对应的所述移动距离的差值，所述相邻两个位置至少包括所述当前位置和所述前一位置。

2.根据权利要求1所述的机器人定位方法，其特征在于，

所述检测距离包括所述当前位置与基站之间的检测距离和前一位置与基站之间的检测距离，所述移动距离包括所述当前位置相对所述前一位置的移动距离、所述前一位置相对前二位置的移动距离，所述结合所述检测距离和所述移动距离，对检测误差进行最小化求解，得到机器人的定位坐标包括：

结合所述检测距离和所述移动距离，对所述检测误差进行最小化求解，得到当前位置的定位坐标和前一位置的定位坐标；

所述第一误差包括所述当前位置的定位坐标与所述基站之间的距离和所述检测距离的差值、对应的所述前一位置的定位坐标与所述基站之间的检测距离和对应的所述移动距离的差值；所述第二误差包括所述当前位置和对应的所述前一位置的所述定位坐标之间的距离和对应的移动距离的差值、所述前一位置和对应的前二位置的所述定位坐标之间的距离和对应的移动距离的差值。

3.根据权利要求1所述的机器人定位方法，其特征在于，

所述利用第二传感器获取移动距离包括：

分别利用里程计和惯性测量单元得到所述机器人的移动速度；

获得所述里程计测得的移动速度和所述惯性测量单元测得的移动速度的综合速度；

结合所述综合速度以及所述机器人当前位置相对前一位置的移动时间，得到所述检测移动距离。

4.根据权利要求3所述的机器人定位方法，其特征在于，

所述获得所述里程计测得的移动速度和所述惯性测量单元测得的移动速度的综合速度包括：

求所述里程计测得的移动速度和所述惯性测量单元测得的移动速度的平均速度，以作为所述综合速度。

5.根据权利要求3所述的机器人定位方法，其特征在于，

所述利用里程计得到所述机器人的移动速度包括：

利用所述里程计获取机器人的当前位置的坐标和前一位置的坐标；

综合所述当前位置相对前一位置的移动时间、所述当前位置的坐标和所述前一位置的坐标，计算所述里程计测得的移动速度。

6.根据权利要求3所述的机器人定位方法，其特征在于，

所述利用惯性测量单元得到所述机器人的移动速度包括：

利用所述惯性测量单元得到机器人在横坐标方向的加速度和纵坐标方向的加速度；

综合所述当前位置相对前一位置的移动时间、所述横坐标方向的加速度和所述纵坐标方向的加速度，计算所述惯性测量单元测得的移动速度。

7.根据权利要求1所述的机器人定位方法，其特征在于，

所述对检测误差进行最小化求解，得到机器人的定位坐标包括：

构建修正函数，所述修正函数中包含调节因子；

通过调整所述调节因子对所述机器人的位置进行修正，对所述检测误差的倒数为零进行求解，得到所述机器人的定位坐标。

8.根据权利要求7所述的机器人定位方法，其特征在于，

所述对检测误差进行最小化求解，得到机器人的定位坐标之后还包括：

结合所述定位坐标和变换参数获得所述第一传感器坐标系下的定位坐标；

所述变换参数通过以下方式获得：

分别利用所述第一传感器和所述第二传感器得到所述机器人的移动轨迹；

将所述第一传感器测量的移动轨迹和所述第二传感器测量的移动轨迹进行对齐得到对齐轨迹；

得到所述第一传感器测量的移动轨迹转换为所述对齐轨迹的所述变换参数。

9.根据权利要求1所述的机器人定位方法，其特征在于，

所述机器人利用第一传感器获取当前位置与基站之间的检测距离包括：

所述第一传感器向基站发送数据信号；

接收所述基站发送的返回信号；

结合所述数据信息和所述返回信息的时间戳获取当前位置与基站之间的所述检测距离。

10.一种机器人，其特征在于，所述机器人包括：

机体；

信息采集组件，安装于所述机体并用于采集检测距离和移动距离；

控制器，连接所述机体和所述信息采集组件，用于控制所述机体配合所述信息采集组件，采用权利要求1至9任一项所述的机器人定位方法对所述机器人进行定位。