一种基于四参数变换及最小二乘的人工路标定位方法与流程

1.本发明涉及移动机器人室内人工路标定位技术领域，尤其涉及一种基于四参数变换及最小二乘的人工路标定位方法。


背景技术：

2.基于人工路标的定位在移动机器人室内定位技术上应用广泛，借助人工特征，机器人可以较快地计算出当前位姿(x,y,θ)，且该方法不存在累积误差。但目前基于提取出的人工路标定位方法主要有三角法和三边法，三角定位和三边定位都是计算机器人位置(x,y)，无法直接计算出机器人当前的姿态角θ，且三角法及衍生的算法对人工路标的布置及两两之间的角度要求非常高，且要求两圆圆心距离足够远，测量信号有极高的角度分辨率要求等。如果通过单纯的坐标转换去求解位姿，由于误差，姿态角经常无法加算出。


技术实现要素：

3.本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本技术的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。
4.鉴于上述现有存在的问题，提出了本发明。
5.因此，本发明提供了一种基于四参数变换及最小二乘的人工路标定位方法，能够解决传统人工路标定位方法无法直接计算出机器人当前位姿和传统方法存在累计误差的问题。
6.为解决上述技术问题，本发明提供如下技术方案，一种基于四参数变换及最小二乘的人工路标定位方法，包括：
7.采集位置点二维平面位置数据；
8.根据所述数据采用四参数变换方法构造目标方程；
9.对所述目标方程进行求解，并计算比例因子k；
10.所述目标方程能够通过最小二乘求解；
11.设定所述比例因子的取值范围，并将所述取值范围作为求解结果对人工路标定位判断条件，进行判断得到定位结果；
12.作为本发明所述的基于四参数变换及最小二乘的人工路标定位方法的一种优选方案，其中：所述数据包括，向道头程序输入机器人定位数据，获取位置信息，分选出坐标位置数据。
13.作为本发明所述的基于四参数变换及最小二乘的人工路标定位方法的一种优选方案，其中：所述构造目标方程包括，记一个点在变换之后的坐标系中的坐标为(x,y),则其在原有坐标系中的坐标表示为：
[0014][0015]
其中，x',y'表示该店变换前在坐标系中的位置(x',y')，k表示比例因子，x,y表示该点的变换后位置(x,y)，δx该点在二维坐标系x轴上的偏移向量，δy表示该点在二维坐标系y轴上的偏移向量，δθ表示该点在二维平面上的旋转角度。
[0016]
作为本发明所述的基于四参数变换及最小二乘的人工路标定位方法的一种优选方案，其中：所述构建目标方程还包括，将公式(1)等式的右侧看作是变换参数(k,δθ,δx,δy)的非线性函数，表示如下：
[0017][0018]
其中，p、q均为引入的辅助变量，辅助目标方程的运算，其他各个变量的物理含义和前述公式中的定义相同，将上述非线性方差转换为线性形式，表示如下：
[0019][0020]
其中，各个变量的物理含义和前述公式中的定义相同。
[0021]
作为本发明所述的基于四参数变换及最小二乘的人工路标定位方法的一种优选方案，其中：所述非线性方差的线性形式包括，
[0022]
在确定p和q的值之后,计算出相应的比例因子k和旋转角度δθ，表示为：
[0023][0024]
其中，各个变量的物理含义和前述公式中的定义相同。
[0025]
作为本发明所述的基于四参数变换及最小二乘的人工路标定位方法的一种优选方案，其中：所述最小二乘求解包括，
[0026]
记人工路标在传感器坐标系中的坐标为(x,y),在全局地图中的坐标为(x',y')则变换参数中,(δx,δy,δθ)就是激光扫描仪在全局坐标系中的位姿，表示为：
[0027][0028]
其中，xi,yi表示人工路标在传感器坐标系中的坐标，即{(x1,y1),(x2,y2)
…
(xn,yn)}，其他各个变量的物理含义和前述公式中的定义相同。
[0029]
作为本发明所述的基于四参数变换及最小二乘的人工路标定位方法的一种优选方案，其中：所述最小二乘求解包括，
[0030]
假设识别出了n个全局地图中的人工路标,则根据上述公式可以得到n组等式，表示如下：
[0031][0032]
其中，各个变量的物理含义和前述公式中的定义相同。将上述方程等式用公式表示为：
[0033][0034]
其中，a、b均表示矩阵。
[0035]
作为本发明所述的基于四参数变换及最小二乘的人工路标定位方法的一种优选方案，其中：所述最小二乘求解还包括，
[0036]
四个参数,至少需要两组人工路标才能有唯一解，如果有多于2组人工路标,就可以通过最小二乘方法提高精度，最小二乘解的形式为：
[0037]
x＝(a
t
a)-1atb[0038]
其中，各个变量的物理含义和前述公式中的定义相同。
[0039]
作为本发明所述的基于四参数变换及最小二乘的人工路标定位方法的一种优选方案，其中：所述计算比例因子验证k包括，
[0040]
计算出的比例因子k的理论值为1，经过实际计算得出k的值应满足范围，则认为此次计算结果正确，如果k不满足上述范围，则通过对人工路标进行筛选从而重新计算。
[0041]
本发明的有益效果：本发明方法基于四参数变换方法，构建了目标函数的数学模型，该模型表明，可以通过构造含位姿(x,y,θ)三个参数的目标方程，可直接求解出位姿(x,y,θ)，再根据四参数中的比例因子进行验证，如果比例因子超出误差范围，可重新选取人工路标进行计算，且能够保证定位数据不跳动，为该领域提供一种鲁棒性高、位姿求解准确、方便的人工路标定位方法，且带有结果验证方法，避免了海现有人工路标方法计算位姿位置(x,y)和姿态θ是分开求解，且θ对(x,y)的精度要求较高，否则求解不出的问题。
附图说明
[0042]
为了更清楚地说明本发明实施例的技术方案，下面将对
·
实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。其中：
[0043]
图1为本发明一个实施例提供的一种基于四参数变换及最小二乘的人工路标定位方法流程示意图；
[0044]
图2为本发明一个实施例提供的一种基于四参数变换及最小二乘的人工路标定位方法的四参数变换坐标系原理图。
[0045]
图3为本发明一个实施例提供的一种基于四参数变换及最小二乘的人工路标定位方法的反光板定位示意图；
具体实施方式
[0046]
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合说明书附图对本发明的具体实施方式做详细的说明，显然所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的保护的范围。
[0047]
在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。
[0048]
其次，此处所称的“一个实施例”或“实施例”是指可包含于本发明至少一个实现方式中的特定特征、结构或特性。在本说明书中不同地方出现的“在一个实施例中”并非均指同一个实施例，也不是单独的或选择性的与其他实施例互相排斥的实施例。
[0049]
本发明结合示意图进行详细描述，在详述本发明实施例时，为便于说明，表示器件结构的剖面图会不依一般比例作局部放大，而且所述示意图只是示例，其在此不应限制本发明保护的范围。此外，在实际制作中应包含长度、宽度及深度的三维空间尺寸。
[0050]
同时在本发明的描述中，需要说明的是，术语中的“上、下、内和外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一、第二或第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。
[0051]
本发明中除非另有明确的规定和限定，术语“安装、相连、连接”应做广义理解，例如：可以是固定连接、可拆卸连接或一体式连接；同样可以是机械连接、电连接或直接连接，也可以通过中间媒介间接相连，也可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
[0052]
实施例1
[0053]
参照图1-3，为本发明的第一个实施例，该实施例提供了一种基于四参数变换及最小二乘的人工路标定位方法，包括：
[0054]
s1：采集位置点二维平面位置数据，根据所述数据采用四参数变换方法构造目标方程；
[0055]
更进一步的，所述预处理包括向道头关键字程序输入位置点信息数据，获取定位信息，并对定位信息进行筛选，筛选出能够描述位置信息的变换后的定位坐标，通过四参数变换方法构建目标方程。
[0056]
应说明的是，所述四参数参数为：，比例因子k、旋转角度δθ以及平移向量(δx,δy)；
[0057]
应说明的是，所述四参数变换是在二维平面的一种helmert变换,用于描述两个坐标系之间的相对关系一种运算方法，将一个点在变换之后的坐标系中的坐标为(x,y),则其在原有坐标系中的坐标表示为：
[0058]
[0059]
其中，x',y'表示该店变换后在坐标系中的位置(x',y')，k表示比例因子，x,y表示该点的初始位置(x,y)，δx该点在二维坐标系x轴上的偏移向量，δy表示该点在二维坐标系y轴上的偏移向量，δθ表示该点在二维平面上的旋转角度；
[0060]
应说明的是，所述公式的右侧可以看做是变换参数(k,δθ,δx,δy)的非线性函数，并进行辅助运算，表示如下：
[0061][0062]
其中，p、q均为引入的辅助变量，辅助目标方程的运算，其他各个变量的物理含义和前述公式中的定义相同，将上述非线性方差转换为线性形式，表示如下：
[0063][0064]
其中，各个变量的物理含义和前述公式中的定义相同。
[0065]
应说明的是，所述非线性方差的线性形式在确定p和q的值之后,计算出相应的比例因子k和旋转角度δθ，表示为：
[0066][0067]
其中，各个变量的物理含义和前述公式中的定义相同。
[0068]
还应说明的是，图2中可以看坐标系在二维平面的三种变换方式。
[0069]
s2：所述目标方程能够通过最小二乘求解；
[0070]
更进一步的，根据已经计算出的比例因子k和旋转角度δθ，当人工路标多于两组可通过最小二乘方法提升数据精度。
[0071]
应说明的是，最小二乘求解需要记人工路标在传感器坐标系中的坐标为(x,y),在全局地图中的坐标为(x',y')则变换参数中,(δx,δy,δθ)就是激光扫描仪在全局坐标系中的位姿，表示如下：
[0072][0073]
其中，xi,yi表示人工路标在传感器坐标系中的坐标，即{(x1,y1),(x2,y2)
…
(xn,yn)}，其他各个变量的物理含义和前述公式中的定义相同。
[0074]
应说明的是，假设识别出了n个全局地图中的人工路标,则根据上述公式可以得到n组等式，表示如下：
[0075][0076]
其中，各个变量的物理含义和前述公式中的定义相同。将上述方程等式用公式表
示为：
[0077][0078]
其中，a、b均表示矩阵。
[0079]
更进一步的，最小二乘解的形式为：
[0080]
x＝(a
t
a)-1atb[0081]
其中，各个变量的物理含义和前述公式中的定义相同。
[0082]
应说明的是，最小二乘法是当人工路标多于两组时采用的提升数据精度的方法。
[0083]
s3：设定所述比例因子的取值范围，并将所述取值范围作为求解结果对人工路标定位判断条件，进行判断得到定位结果；
[0084]
更进一步的，经计算得出的比例因子k，在理论上k值为1，经过实际计算得出k的值应满足范围，则认为此次计算结果正确，如果k不满足上述范围，则通过对人工路标进行筛选从而重新计算。
[0085]
应说明的是，若计算出的k值符合限定k的取值范围为c＜k＜d，即k∈(0.997，1.003)，则认为此次计算结果正确，全部计算成功完成；若若计算出的k值不符合限定k的取值范围为c＜k＜d，即k∈(0.997，1.003)不成立，则认为此次计算结果不正确，需输入局部人工路标及匹配的全局人工路标进行重新计算。
[0086]
应说明的是，所述局部人工路标包括，在机器人运行环境中设置的数量足够用，位置精确、特征明显、易识别提取的路标，局部人工路标在位置上表示为：
[0087]
{(x1,y1),(x2,y2)
……
(xn,yn)}
[0088]
应说明的是，所述匹配的全局人工路标是环境中提前放置好，位置已知的路标，匹配的全局人工路标在位置上表示为：
[0089]
{(x1',y1'),(x'2,y'2)
……
(x'n,y'n)}
[0090]
还应说明的是，图1中可以看出具体过程的流程说明。
[0091]
实施例2
[0092]
参照图3，为本发明的一个实施例，提供了一种基于四参数变换及最小二乘的人工路标定位方法，为了验证本发明的有益效果，通过实验进行科学论证。
[0093]
以图3所示的反光板定位作为输入(人工路标选取为反光板，移动机器人通过扫描提取局部反光板)，借助匹配出的全局反光板进行定位计算。
[0094]
通过数据得出反光板定位的实际计算结果，该数据为根据图3所示的模型模拟得到，计算过程如下：
[0095][0096][0097]
可以分析得到，此次计算出的机器人位姿为,此时比例因子k＝0.99977，满足k的有效取值范围，故认为该次计算结果正确，说明该发明可直接求解出位姿，且对于求解出的位姿，可根据四参数中的比例因子进行验证，是一种鲁棒性高、位姿求解准确、方便的人工路标定位方法，且带有结果验证方法。
[0098]
本发明是一种基于四参数变换及最小二乘的人工路标定位方法技术，主要用于移动机器人室内人工路标的定位。本方法首先采集位置点二维平面位置数据，根据所述数据采用四参数变换方法构造目标方程，再对所述目标方程进行求解，并计算比例因子k，当人工路标多于2组时，所述目标方程能够通过最小二乘求解，设定所述比例因子的取值范围，并将所述取值范围作为求解结果对人工路标定位判断条件，再设定参数值c,d，限定k的取值范围为c＜k＜d，即k∈(0.997，1.003)，并以此作为判断条件，若结果符合判断条件，则直接输出结果；若结果不符合判断条件，则输入局部人工路标及匹配的全局人工路标重新计
算。
[0099]
应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。
[0100]
应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。
[0101]
本领域内的技术人员应明白，本技术的实施例可提供为方法、系统、或计算机程序产品。因此，本技术可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本技术可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。本技术实施例中的方案可以采用各种计算机语言实现，例如，面向对象的程序设计语言java和直译式脚本语言javascript等。
[0102]
本技术是参照根据本技术实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0103]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0104]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0105]
尽管已描述了本技术的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本技术范围的所有变更和修改。
[0106]
显然，本领域的技术人员可以对本技术进行各种改动和变型而不脱离本技术的精神和范围。这样，倘若本技术的这些修改和变型属于本技术权利要求及其等同技术的范围之内，则本技术也意图包含这些改动和变型在内。