CN105278533A - 一种全向移动平台导航方法 - Google Patents

Abstract

本发明提供一种全向移动平台导航方法，属于导航技术领域，作为一种新的全向移动平台导航方法，本发明的一种全向移动平台导航方法，获取iGPS全局定位数据与电机编码器测量的电机转动量进行组合定位，在拓扑地图内搜索从当前位置到目标位置的最短路径；路径搜索完成后，进行路径跟踪，向umac控制器发送速度控制指令与位移控制指令，umac控制器控制电机转动实现全向移动平台自主行驶，实现自主导航。该方法通过实时路径搜索完成，定位精度高、适应能力强。

Description

一种全向移动平台导航方法

技术领域

本发明属于导航技术领域，涉及室内移动机器人导航，具体涉及一种基于麦克纳姆轮的全向移动平台的自主导航方法。

背景技术

目前，全向移动平台导航主要通过色带识别、磁条感应、激光雷达等方式进行导航。这些导航方式中，色带识别可靠性不高，定位精度低，只能按照粘贴的路径行驶；磁条感应导航应用成熟，但存在布局、改造与维护困难，而且定位精度低，路径缺少灵活性；激光雷达导航在静态固定环境中具有行走路径灵活、定位精度高的优点，但在动态环境中应用受到一定限制。

发明内容

本发明要解决的技术问题是提供一种全向移动平台导航方法，该方法是基于xml文档建立地图的全向移动平台导航方法，为一种通过实时路径搜索完成定位精度高、适应能力强的全向移动平台导航方法。

为了解决上述技术问题，本发明的技术方案是，一种全向移动平台导航方法：

该方法包括以下步骤：

S1、通过记录全向移动平台行驶的路径点坐标、全向移动平台在路径点的航向角、以及路径点坐标的ID号形成xml文档；读取xml文档，建立路径搜索的拓扑地图；

S2、在初始时刻，通过4个接收机的iGPS数据获取全向移动平台的初始位置坐标、初始航向；

S3、间隔100ms获取电机编码器的电机转动量，通过电机编码器的电机转动量获取当前时刻全向移动平台的位置坐标与航向；并且间隔1s获取一次iGPS数据，若iGPS值无效，此时，仍然以通过电机编码器获取的全向移动平台的坐标及航向作为定位数据，如果获取到的iGPS数据有效，则以通过iGPS数据获取的位置坐标和航向值作为当前时刻的位置坐标和航向值：

S4、给定目标节点位置后，采用A*算法在拓扑地图内搜索从当前位置到目标位置的路径；通过A*算法搜索最短路径，路径搜索完成后，向运动控制器发送控制指令，使全向移动平台按照搜索的路径自主行驶到目标位置。

2、根据权利要求1所述的一种全向移动平台导航方法，其特征在于，在所述的步骤3中，具体限定为：

(1)通过电机编码器的电机转动量，获取全向移动平台位置坐标与航向的步骤如下：

通过电机转动量计算4个麦克纳姆轮轮组转动量，然后根据公式(5)计算全向移动平台的移动量(Δx_i,Δy_i,Δθ_i)：

$\left(\begin{array}{c}{\mathrm{\Δx}}_i\\ {\mathrm{\Δy}}_i\\ {\mathrm{\Δ\θ}}_i\end{array}\right)=\frac{R\·\mathrm{\π}}{2\·n\·\mathrm{\ϵ}}\left[\begin{array}{cccc}-\tan \mathrm{\α}& \tan \mathrm{\α}& -\tan \mathrm{\α}& \tan \mathrm{\α}\\ 1& 1& 1& 1\\ \frac{1}{l_0}& \frac{1}{l_0}& \frac{1}{l_0}& \frac{1}{l_0}\end{array}\right]\left(\begin{array}{c}{\mathrm{\Δ\ω}}_{1i}\\ {\mathrm{\Δ\ω}}_{2i}\\ {\mathrm{\Δ\ω}}_{3i}\\ {\mathrm{\Δ\ω}}_{4i}\end{array}\right)---\left(5\right)$

式中，(Δω_1i,Δω_2i,Δω_3i,Δω_4i)^T是100ms内电机转动量，R是麦克纳姆轮半径，α是辊子轴线与轮毂轴线的夹角，n是减速器减速比，ε电机转动系数；l₀＝l+Lcotα，l是轮子着地点轮距的一半，L是轮子着地点轴距的一半；

按照公式(6)～(8)是根据电机编码器数据累加计算全向移动平台当前的位置坐标与航向；

$x_k=x_0+\underset{i=0}{\overset{k}{\Σ}}{\mathrm{\Δx}}_i.{\mathrm{cos\θ}}_i-\underset{i=0}{\overset{k}{\Σ}}{\mathrm{\Δy}}_i.{\mathrm{sin\θ}}_i---\left(6\right)$

$y_k=y_0+\underset{i=0}{\overset{k}{\Σ}}{\mathrm{\Δx}}_i.{\mathrm{sin\θ}}_i+\underset{i=0}{\overset{k}{\Σ}}{\mathrm{\Δy}}_i.{\mathrm{cos\θ}}_i---\left(7\right)$

${\mathrm{\θ}}_k={\mathrm{\θ}}_0+\underset{i=0}{\overset{k}{\Σ}}{\mathrm{\Δ\θ}}_i---\left(8\right)$

式中，x_k,y_k是在间隔1s整数倍时间内的两次iGPS数据为有效时，之间的第k个100ms(k＝0、1、2…n)采样时刻下全向移动平台的当前位置坐标，θ_k是第k个100ms采样时刻下全向移动平台当前航向；

k＝0时，且在初始时刻，(x₀,y₀,θ₀)值为步骤S2中获得的全向移动平台的初始位置坐标和航向；

k＝0时，且不在初始时刻，(x₀,y₀,θ₀)的取值按照下述(2)的方法；

(2)间隔1s获取一次iGPS数据，如果获取的iGPS值无效，k的取值，以及当前坐标位置和航向依照(1)中方式确定；如果获取的iGPS数据有效，则通过iGPS数据获得的当前坐标位置和航向作为(x₀,y₀,θ₀)值。

根据预先设定的平台目标位置信息，利用A*算法搜索从当前位置到目标位置的最短路径，得到最短路径的坐标信息，最后进行路径跟踪，自主导航行驶到目标位置。解算全向移动平台几何中心在现场坐标系下的坐标与航向，确定平台的当前位置信息。

本发明技术效果是：

一、建立xml格式的道路文档，读取xml文档建立拓扑地图，然后在地图内采用A*算法搜索最短路径。A*算法能保证搜索的路径最短。

二、定位采用了iGPS与电机编码器数据组合定位的方法，融合了iGPS全局定位与电机编码器数据局部定位的优点，同时采用了速度与位移两种控制模式，使全向移动平台的导航具有定位精度高、抗干扰能力强、行走路径灵活的优点。

附图说明

图1为本发明的方法所使用的系统的结构示意图；

图2为本发明方法的流程图；

图3为本发明的方法的组合定位功能流程图；

图4为本发明中的路径跟踪原理示意图。

图1中，1-iGPS接收机、2-工控机、3-麦克纳姆轮轮组、4-uMac多轴控制器、5-电机驱动器、6-全向移动平台车架。

具体实施方式

为使本发明的目的、内容和优点更加清楚，下面结合附图和实施例，对本发明作进一步详细描述。

如图1所示，本发明的一种全向移动平台导航方法，采用一种全向移动平台导航系统，如图1所示，该系统包括有4个麦克纳姆轮轮组3(包含伺服驱动电机)、4个iGPS接收机1、工控机2、运动控制器4(uMac多轴控制器)、电机驱动器5、全向移动平台车架6。

4个麦克纳姆轮驱动轮组，用于实现平台的前进、后退、横移、旋转以及其它复合运动；所述的工控机2的包括导航处理模块。

所述的导航处理模块读取xml文档，建立路径搜索的拓扑地图。

所述的xml文档是通过记录全向移动平台行驶的路径点坐标，全向移动平台在路径点的航向角，路径点坐标的ID号形成的。

所述的导航处理模块包括组合定位模块。

所述的导航处理模块建立拓扑地图后，组合定位模块实现全向移动平台导航方法中步骤S2至S3的功能。

本发明的一种全向移动平台导航方法，获取iGPS全局定位数据与电机编码器测量的电机转动量进行组合定位，在拓扑地图内搜索从当前位置到目标位置的最短路径；路径搜索完成后，进行路径跟踪，向umac控制器发送速度控制指令与位移控制指令，umac控制器控制电机转动实现全向移动平台自主行驶，实现自主导航。

本发明的一种全向移动平台导航方法，包括如下步骤：

S1通过记录全向移动平台行驶的路径点坐标、全向移动平台在路径点的航向角、以及路径点坐标的ID号形成xml文档；读取xml文档，建立路径搜索的拓扑地图；

S2在初始时刻，通过4个iGPS数据获取全向移动平台的初始位置坐标、初始航向；

通过iGPS数据，获取计算全向移动平台的当前位置坐标、当前航向的步骤如下：

接收到4个iGPS数据，按照公式(1)～(2)计算全向移动平台的当前位置坐标(x₀,y₀)(全向移动平台的当前位置坐标是平台的几何中心在导航现场坐标系下的坐标)。按照公式(3)～(4)计算全向移动平台当前航向θ₀。式中，(X_i,Y_i)是第i个接收机的位置坐标(导航坐标系)，i＝1、2、3、4，(x_i,y_i)是接收机在全向移动平台坐标系下(该坐标系下全向移动平台几何中心为原点，平台正前方为Y轴，右侧为X轴)的坐标。

$x_0=X_i-\frac{(X_i-X_j)(x_i-x_j)+(y_i-y_j)(Y_i-Y_j)}{{(x_i-x_j)}^2+{(y_i-y_j)}^2}\·x_i+\frac{(Y_i-Y_j)(x_i-x_j)-(y_i-y_j)(X_i-X_j)}{{(x_i-x_j)}^2+{(y_i-y_j)}^2}\·y_i---\left(1\right)$

$y_0=Y_i-\frac{(Y_i-Y_j)(x_i-x_j)-(y_i-y_j)(X_i-X_j)}{{(x_i-x_j)}^2+{(y_i-y_j)}^2}\·x_i-\frac{(X_i-X_j)(x_i-x_j)+(y_i-y_j)(Y_i-Y_j)}{{(x_i-x_j)}^2+{(y_i-y_j)}^2}\·y_i---\left(2\right)$

${\mathrm{sin\θ}}_0=\frac{(Y_i-Y_j)(x_i-x_j)-(y_i-y_j)(X_i-X_j)}{{(x_i-x_j)}^2+{(y_i-y_j)}^2}---\left(3\right)$

${\mathrm{cos\θ}}_0=\frac{(X_i-X_j)(x_i-x_j)+(y_i-y_j)(Y_i-Y_j)}{{(x_i-x_j)}^2+{(y_i-y_j)}^2}---\left(4\right)$

S3、(1)间隔100ms获取电机编码器的电机转动量，获取当前时刻全向移动平台的位置坐标与航向；

通过获取电机编码器的电机转动量，获取全向移动平台位置坐标与航向的步骤如下：

通过电机转动量计算4个麦克纳姆轮轮组转动量，然后根据轮组转动量和麦克纳姆轮运动学方程公式(5)计算全向移动平台的移动量(Δx_i,Δy_i,Δθ_i)：

$\left(\begin{array}{c}{\mathrm{\Δx}}_i\\ {\mathrm{\Δy}}_i\\ {\mathrm{\Δ\θ}}_i\end{array}\right)=\frac{R\·\mathrm{\π}}{2\·n\·\mathrm{\ϵ}}\left[\begin{array}{cccc}-\tan \mathrm{\α}& \tan \mathrm{\α}& -\tan \mathrm{\α}& \tan \mathrm{\α}\\ 1& 1& 1& 1\\ \frac{1}{l_0}& \frac{1}{l_0}& \frac{1}{l_0}& \frac{1}{l_0}\end{array}\right]\left(\begin{array}{c}{\mathrm{\Δ\ω}}_{1i}\\ {\mathrm{\Δ\ω}}_{2i}\\ {\mathrm{\Δ\ω}}_{3i}\\ {\mathrm{\Δ\ω}}_{4i}\end{array}\right)---\left(5\right)$

式中，(Δω_1i,Δω_2i,Δω_3i,Δω_4i)^T是100ms内电机转动量，R是麦克纳姆轮半径，α是辊子轴线与轮毂轴线的夹角，n是减速器减速比，ε电机转动系数。l₀＝l+Lcotα，l是轮子着地点轮距的一半，L是轮子着地点轴距的一半。

按照公式(6)～(8)是根据电机编码器数据累加计算的全向移动平台位置坐标与航向。

$x_k=x_0+\underset{i=0}{\overset{k}{\Σ}}{\mathrm{\Δx}}_i.{\mathrm{cos\θ}}_i-\underset{i=0}{\overset{k}{\Σ}}{\mathrm{\Δy}}_i.{\mathrm{sin\θ}}_i---\left(6\right)$

$y_k=y_0+\underset{i=0}{\overset{k}{\Σ}}{\mathrm{\Δx}}_i.{\mathrm{sin\θ}}_i+\underset{i=0}{\overset{k}{\Σ}}{\mathrm{\Δy}}_i.{\mathrm{cos\θ}}_i---\left(7\right)$

${\mathrm{\θ}}_k={\mathrm{\θ}}_0+\underset{i=0}{\overset{k}{\Σ}}{\mathrm{\Δ\θ}}_i---\left(8\right)$

式中，x_k,y_k是在两次iGPS数据有效之间的第k个100ms(k＝0、1、2…n)采样时刻下全向移动平台的坐标，θ_k是第k个100ms采样时刻下全向移动平台航向。通过有效iGPS数据获取的坐标位置和航向作为(x₀,y₀,θ₀)值。

(2)：间隔1s获取一次iGPS数据，若iGPS值无效，此时，仍然以电机编码器获取的全向移动平台的坐标及航向作为定位数据。如果获取到的iGPS数据有效，则以有效的iGPS数据获取的位置和航向值作为新的(x₀,y₀,θ₀)，用作上述S3步骤(1)中全向移动平台通过电机编码器定位的(x₀,y₀,θ₀)值。

如果由于遮挡、电磁干扰或者iGPS信号薄弱导致获取的iGPS数据与实际值比较偏差过大，则为无效iGPS值,否则获取的iGPS数据为有效。

S4：获取定位数据确定平台的当前位置，给定目标节点位置后，采用A*算法在拓扑地图内搜索从当前位置到目标位置的路径；通过A*算法搜索最短路径，路径搜索完成后，向运动控制器发送控制指令，使全向移动平台按照搜索的路径自主行驶到目标位置。导航处理模块完成上述功能。

通过A*算法搜索最短路径，路径搜索完成后，向运动控制器发送控制指令，使全向移动平台按照搜索的路径自主行驶到目标位置，此过程称为路径跟踪。跟踪过程如图4所示，搜索的路径由7个节点组成，全向移动平台已经从起始位置节点1行驶到节点3与节点4之间。

在节点3与节点4之间，导航处理模块以节点4作为预瞄点，节点4与节点5之间的连线的斜率作为全向移动平台在节点4的航向。此时，导航处理模块根据坐标(x,y)、航向θ与预瞄点的坐标和航向进行比较，确定全向移动平台的平移速度与旋转速度，并向运动控制器发送速度控制指令。

同时，导航处理模块将平台自身位置投影到节点3与节点4的连线上，投影点作为平台在路径上的位置，当投影点与节点4的距离小于设定阈值，会发生路径切换，导航处理模块将确定全向移动平台行驶到节点4与节点5之间，并以节点5为预瞄点。

最终接近目标位置节点7时，为了保证全向移动平台的定位精度，导航处理模块采用位移控制模式：当平台自身位置与目标位置节点7比较，满足位移控制设置的阈值，导航处理模块向运动控制器发送位移控制指令，控制平台的位移量，直至到达目标位置，满足设置的精度。

Claims (2)

1.一种全向移动平台导航方法，其特征在于，该方法包括如下步骤：

S1、通过记录全向移动平台行驶的路径点坐标、全向移动平台在路径点的航向角、以及路径点坐标的ID号形成xml文档；读取xml文档，建立路径搜索的拓扑地图；

S2、在初始时刻，通过4个接收机的iGPS数据获取全向移动平台的初始位置坐标、初始航向；

S3、间隔100ms获取电机编码器的电机转动量，通过电机编码器的电机转动量获取当前时刻全向移动平台的位置坐标与航向；并且间隔1s获取一次iGPS数据，若iGPS值无效，此时，仍然以通过电机编码器获取的全向移动平台的坐标及航向作为定位数据，如果获取到的iGPS数据有效，则以通过iGPS数据获取的位置坐标和航向值作为当前时刻的位置坐标和航向值：

S4、给定目标节点位置后，采用A*算法在拓扑地图内搜索从当前位置到目标位置的路径；通过A*算法搜索最短路径，路径搜索完成后，向运动控制器发送控制指令，使全向移动平台按照搜索的路径自主行驶到目标位置。

2.根据权利要求1所述的一种全向移动平台导航方法，其特征在于，在所述的步骤3中，具体限定为：

(1)通过电机编码器的电机转动量，获取全向移动平台位置坐标与航向的步骤如下：

通过电机转动量计算4个麦克纳姆轮轮组转动量，然后根据公式(5)计算全向移动平台的移动量(Δx_i,Δy_i,Δθ_i)：

$\left(\begin{array}{c}{\mathrm{\Δx}}_i\\ {\mathrm{\Δy}}_i\\ {\mathrm{\Δ\θ}}_i\end{array}\right)=\frac{R\·\mathrm{\π}}{2\·n\·\mathrm{\ϵ}}\left(\begin{array}{cccc}-\tan \mathrm{\α}& \tan \mathrm{\α}& -\tan \mathrm{\α}& \tan \mathrm{\α}\\ 1& 1& 1& 1\\ \frac{1}{l_0}& \frac{1}{l_0}& \frac{1}{l_0}& \frac{1}{l_0}\end{array}\right)\left(\begin{array}{c}{\mathrm{\Δ\ω}}_{1i}\\ {\mathrm{\Δ\ω}}_{2i}\\ {\mathrm{\Δ\ω}}_{3i}\\ {\mathrm{\Δ\ω}}_{4i}\end{array}\right)---\left(5\right)$

式中，(Δω_1i,Δω_2i,Δω_3i,Δω_4i)^T是100ms内电机转动量，R是麦克纳姆轮半径，α是辊子轴线与轮毂轴线的夹角，n是减速器减速比，ε电机转动系数；l₀＝l+Lcotα，l是轮子着地点轮距的一半，L是轮子着地点轴距的一半；

按照公式(6)～(8)是根据电机编码器数据累加计算全向移动平台当前的位置坐标与航向；

$x_k=x_0+\underset{i=0}{\overset{k}{\Σ}}{\mathrm{\Δx}}_i.{\mathrm{cos\θ}}_i-\underset{i=0}{\overset{k}{\Σ}}{\mathrm{\Δy}}_i.{\mathrm{sin\θ}}_i---\left(6\right)$

$y_k=y_0+\underset{i=0}{\overset{k}{\Σ}}{\mathrm{\Δx}}_i.{\mathrm{sin\θ}}_i+\underset{i=0}{\overset{k}{\Σ}}{\mathrm{\Δy}}_i.{\mathrm{cos\θ}}_i---\left(7\right)$

${\mathrm{\θ}}_k={\mathrm{\θ}}_0+\underset{i=1}{\overset{k}{\Σ}}{\mathrm{\Δ\θ}}_i---\left(8\right)$

式中，x_k,y_k是在间隔1s整数倍时间内的两次iGPS数据为有效时，之间的第k个100ms(k＝0、1、2…n)采样时刻下全向移动平台的当前位置坐标，θ_k是第k个100ms采样时刻下全向移动平台当前航向；

k＝0时，且在初始时刻，(x₀,y₀,θ₀)值为步骤S2中获得的全向移动平台的初始位置坐标和航向；

k＝0时，且不在初始时刻，(x₀,y₀,θ₀)的取值按照下述(2)的方法；

(2)间隔1s获取一次iGPS数据，如果获取的iGPS值无效，k的取值，以及当前坐标位置和航向依照(1)中方式确定；如果获取的iGPS数据有效，则通过iGPS数据获得的当前坐标位置和航向作为(x₀,y₀,θ₀)值。