一种轮式移动平台运动轨迹预瞄跟踪控制系统、方法与流程

1.本发明属于自动控制技术领域，具体涉及一种轮式移动平台运动轨迹预瞄跟踪控制系统、方法。


背景技术：

2.基于无轨导航方式是轮式移动平台，如agv智能车、巡检机器人等自主导航设备最重要的一种导航方式。相比于有轨磁导航，配备无轨导航系统的自主移动平台运动更加灵活，更适合于平凡调整工艺流程的柔性生产线。
3.在激光或者视觉导航式轮式移动平台设备中，常使用参考曲线引导车辆运动到达目标位置。
4.参照说明书附图5，现有技术一般采用最近点轨迹跟踪控制技术对轮式移动平台进行位姿调控。该轨迹跟踪控制技术首先根据激光或者视觉等定位装置确定轮式移动平台在绝对坐标系中的位置，然后通过计算出轮式移动平台中心点在距离参考曲线最近的那一点作为计算位姿误差调控的参考点，然后由运动控制器对此误差进行控制，从而使得轮式移动平台能够跟踪参考曲线。
5.这种误差计算方法是纯理想状态，即定位装置无延迟，驱动机构无滞后。但是对于现有的大部分轮式移动平台而言，显然很难满足这种要求。为使得现有轮式移动平台能够精确跟踪轨迹，最奏效的轮式移动平台位姿计算应该具备一定的预测功能。


技术实现要素：

6.本发明的目的在于克服现有技术的不足，提供一种轮式移动平台运动轨迹预瞄跟踪控制系统和方法。对于给定的参考路径，可以根据参考曲线的参数方程、轮式移动平台当前的位姿和预瞄距离，计算出轮式移动平台相对于参考曲线之间的预瞄位姿偏差，进而传递给轮式移动平台的轨迹跟踪控制系统，提高轮式移动平台行走的控制精度。
7.本发明是通过以下技术方案实现的：
8.一种轮式移动平台运动轨迹预瞄跟踪控制系统，包括设置于轮式移动平台上的车载预瞄控制设备和执行机构单元；
9.车载预瞄控制设备包括车载定位装置、运动参考曲线生成器、预瞄准量调节器、处理器、数据存储器、程序存储器，车载定位装置用于测量出轮式移动平台当前的位姿，运动参考曲线生成器用于生成一条轮式移动平台可执行的路径，预瞄准量调节器用于根据轮式移动平台的线速度动态调整瞄准距离，程序存储器存储有预瞄准点位姿误差解算和控制程序，数据存储器作为预瞄准点位姿误差解算和控制程序运行过程中产生的中间数据载体，程序存储器和数据存储器均与处理器通信连接，以计算出轮式移动平台当前的预瞄准点位姿误差、控制线速度和控制角速度；处理器与执行机构单元连接，执行机构单元根据处理器输出的控制线速度和控制角速度驱动轮式移动平台移动。
10.在上述技术方案中，处理器包括预瞄准点位姿误差计算单元、运动控制单元、速度
规划单元和速度反馈单元，其中，预瞄准点位姿误差计算单元用于根据车载定位装置提供的轮式移动平台定位位姿、运动参考曲线生成器提供的路径曲线方程、以及预瞄准量调节器提供的调整量数据计算轮式移动平台预瞄点位姿误差；速度反馈单元用于将轮式移动平台当前速度数据反馈给速度规划单元；速度规划单元根据轮式移动平台与目标点的剩余路程在速度反馈单元反馈的当前速度数据基础上规划当前点的线速度和角速度；运动控制单元，根据预瞄准点位姿误差和规划的线速度与角速度计算出对轮式移动平台的轨迹控制数据，将轨迹控制数据输送给执行机构单元。
11.在上述技术方案中，所述车载定位装置输出轮式移动平台中心点p的当前位姿：p＝(a，b，θ)，其中，a是轮式移动平台中心点p在绝对坐标系xoy中的横坐标，b是轮式移动平台中心点p在绝对坐标系xoy中的纵坐标，θ是随动坐标系的x0轴(即轮式移动平台前行向)与绝对坐标系的x轴的夹角。
12.在上述技术方案中，运动参考曲线生成器输出绝对坐标系中轮式移动平台可行路径的曲线方程f(t)，α是指t的上界，β是指t的下界。
13.在上述技术方案中，预瞄准量调节器输出轮式移动平台预瞄准调整量δl，其中δl是点q到点r的弧长，点q是轮式移动平台中心点p距离曲线方程上的最近点，点r是曲线方程上的预瞄准的点。
14.在上述技术方案中，预瞄准点位姿误差的求解方法如下：
15.假设1，点q在曲线方程f(t)上对应的方程参数为t
q
；
16.假设2，点r在曲线方程f(t)上对应的方程参数为t
r
；
17.假设3，对于曲线方程f(t)，任意两点之间的弧长表示为l(t1，t2)，l(t1，t2)应用高斯
‑
勒让德积分法计算如下：
[0018][0019]
式中，ω
i
是积分系数，τ
i
是高斯点，t1,t2∈[α,β]。
[0020]
表示参考曲线的一阶导数矢量，
[0021][0022]
表示一阶导数矢量的2范数，可以理解为
[0023][0024]
通过不断地迭代参数t
q
和t
r
，使得且l(t
q
,t
r
)≈δl，是指点p与点q连线的矢量，f(t
q
)
′
是指点q处的切线矢量：f(t
q
)
′
＝[x
′
(t
q
),y
′
(t
q
)]；
[0025]
那么，可以得到参数t
q
和t
r
的最佳值，表示为和由此，预瞄准点位姿误差计算式为：其中：
[0026]
e
pr
是预瞄准点位姿误差表示，e
pr
＝(x
e
,y
e
,θ
e
)，x
e
是绝对坐标系中当前轮式移动平台中心点p到运动参考曲线上的预瞄准点r的横坐标偏差量，y
e
是绝对坐标系中当前轮式移动平台中心点p到运动参考曲线上的预瞄准点r的纵坐标偏差量，θ
e
是绝对坐标系中当前轮式移动平台的角位姿与运动参考曲线上的预瞄准点r切矢量方向角之间的角度偏差量；
[0027]
是运动参考曲线上的预瞄准点r的位姿表示，x
r
是绝对坐标系中预瞄准点r的横坐标，y
r
是绝对坐标系中预瞄准点r的纵坐标，θ
r
是绝对坐标系中预瞄准点r的切线方向与绝对坐标系的x轴的夹角(即，θ
r
是绝对坐标系中预瞄准点r的切线矢量方向角)；
[0028]
p是轮式移动平台中心点p的当前位姿表示，p＝(a，b，θ)，其中，a是轮式移动平台中心点p在绝对坐标系xoy中的横坐标，b是轮式移动平台中心点p在绝对坐标系xoy中的纵坐标，θ是随动坐标系的x0轴(即轮式移动平台前行向)与绝对坐标系的x轴的夹角。
[0029]
在上述技术方案中，根据预瞄准点位姿误差对轮式移动平台进行位姿调控，得到的轮式移动平台的控制律为：
[0030][0031]
式中，v
r
是机器人线速度规划值，由点q到终点t的距离以及速度约束得到，ω
r
是机器人角速度规划值，ω
r
由v
r
和q点曲率相乘得到，[v，ω]
t
是线速度和角速度控制量，控制参数k1＞0，k2＞0，k3＞0。
[0032]
一种运动轨迹预瞄跟踪控制方法，包括以下步骤：
[0033]
s100：加载车载定位装置的定位位姿、运动参考曲线生成器的路径方程、预瞄准量调节器的调整量数据，并将数据输入处理器；
[0034]
s200：利用定位位姿、路径方程、调整量数据计算轮式移动平台预瞄点位姿误差；
[0035]
s300：利用轮式移动平台与目标点的剩余路程规划当前点的线速度和角速度；
[0036]
s400：根据预瞄点位姿误差、规划的线速度与角速度计算轮式移动平台运动的控制线速度和控制角速度；
[0037]
s500：将控制线速度和控制角速度作为轮式移动平台的当前控制值，输入给执行机构单元，调整轮式移动平台的位姿，达到目标姿态。
[0038]
本发明的优点和有益效果为：
[0039]
本发明通过处理器调用程序存储器中的预瞄准点位姿误差解算和控制程序，从而计算出轮式移动平台于当前位置的预瞄准点位姿误差、控制线速度和控制角速度。处理器将控制线速度和控制角速度作为轮式移动平台的当前控制值，输入给执行机构单元，调整轮式移动平台的位姿，达到目标姿态，提高轮式移动平台行走的控制精度。
附图说明
[0040]
图1为轮式移动平台运动轨迹预瞄跟踪控制系统示意图；
[0041]
图2为本发明中车载预瞄控制设备及执行机构单元的连接框图；
[0042]
图3为本发明中采用预瞄准轨迹跟踪控制示意图；
[0043]
图4为本发明中运动轨迹预瞄跟踪控制方法的流程图；
[0044]
图5为背景技术中采用最近点轨迹跟踪控制示意图。
[0045]
附图标记：1、车载定位装置；2、运动参考曲线生成器；3、预瞄准量调节器；4、处理器；5、数据存储器；6、程序存储器；7、执行机构单元。
[0046]
对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，可以根据以上附图获得其他的相关附图。
具体实施方式
[0047]
为了使本技术领域的人员更好地理解本发明方案，下面结合具体实施例进一步说明本发明的技术方案。
[0048]
参照图1和图2，为本发明公开的一种轮式移动平台运动轨迹预瞄跟踪控制系统，包括设置于轮式移动平台上的车载预瞄控制设备和执行机构单元7。
[0049]
车载预瞄控制设备包括车载定位装置1、运动参考曲线生成器2、预瞄准量调节器3、处理器4、数据存储器5、程序存储器6，车载定位装置1用于测量出轮式移动平台当前的位姿，运动参考曲线生成器2用于生成一条轮式移动平台可执行的路径，预瞄准量调节器3用于根据轮式移动平台的线速度动态调整瞄准距离，程序存储器6存储有预瞄准点位姿误差解算和控制程序，数据存储器5作为预瞄准点位姿误差解算和控制程序运行过程中产生的中间数据载体，程序存储器6和数据存储器5均与处理器4通信连接，以计算出轮式移动平台当前的预瞄准点位姿误差、控制线速度和控制角速度；处理器4与执行机构单元7连接，执行机构单元7可以根据处理器5输出的控制线速度和控制角速度驱动轮式移动平台移动。
[0050]
为了说明轮式移动平台的运动，需要引入绝对坐标系和随动坐标系的概念，参照图3，绝对坐标系为xoy平面，随动坐标系x0py0是以轮式移动平台中心点p为原点，以轮式移动平台前行向建立x0轴，以此为基础，按照右手定则建立y0轴所构成的坐标系。
[0051]
所述车载定位装置1输出轮式移动平台中心点p的位姿：p＝(a,b,θ)，其中，a是轮式移动平台中心点p在绝对坐标系中的横坐标，b是轮式移动平台中心点p在绝对坐标系中的纵坐标，θ是x0轴(即轮式移动平台前行向)与绝对坐标系的x轴的夹角。
[0052]
运动参考曲线生成器2输出绝对坐标系中轮式移动平台可行路径的曲线方程f(t)，α是指t的上界，β是指t的下界。
[0053]
预瞄准量调节器3输出轮式移动平台预瞄准调整量δl，其中δl是点q到点r的弧长，点q是轮式移动平台中心点p距离曲线方程上的最近点，点r是曲线方程上的预瞄准的点。
[0054]
那么，预瞄准点位姿误差求解方法如下：
[0055]
假设1，点q在曲线方程f(t)上对应的方程参数为t
q
；
[0056]
假设2，点r在曲线方程f(t)上对应的方程参数为t
r
；
[0057]
假设3，对于曲线方程f(t)，任意两点之间的弧长表示为l(t1,t2)，l(t1,t2)应用高斯
‑
勒让德积分法计算如下：
[0058]
[0059]
式中，ω
i
是积分系数，τ
i
是高斯点，参数t1,t2表示曲线上弧的两个端点对应的参数且t1,t2∈[α,β]。
[0060]
表示参考曲线的一阶导数矢量，
[0061][0062]
表示一阶导数矢量的2范数，可以理解为
[0063][0064]
通过不断地迭代参数t
q
和t
r
，使得且l(t
q
,t
r
)≈δl，是指点p与点q连线的矢量，f(t
q
)
′
是指点q处的切线矢量：f(t
q
)
′
＝[x
′
(t
q
),y
′
(t
q
)]；
[0065]
那么，可以得到参数t
q
和t
r
的最佳值，表示为和由此，预瞄准点位姿误差计算式为：其中：e
pr
是预瞄准点位姿误差表示，e
pr
＝(x
e
,y
e
,θ
e
)，x
e
是绝对坐标系中当前轮式移动平台中心点p到运动参考曲线上的预瞄准点r的横坐标偏差量，y
e
是绝对坐标系中当前轮式移动平台中心点p到运动参考曲线上的预瞄准点r的纵坐标偏差量，θ
e
是绝对坐标系中当前轮式移动平台中心点p到运动参考曲线上的预瞄准点r的角度偏差量；是运动参考曲线上的预瞄准点r的位姿表示，是运动参考曲线上的预瞄准点r的位姿表示，x
r
是绝对坐标系中预瞄准点r的横坐标，y
r
是绝对坐标系中预瞄准点r的纵坐标，θ
r
是绝对坐标系中预瞄准点r的切线方向与绝对坐标系的x轴的夹角；p是轮式移动平台中心点p的当前位姿表示，p＝(a,b,θ)，其中，a是轮式移动平台中心点p在绝对坐标系xoy中的横坐标，b是轮式移动平台中心点p在绝对坐标系xoy中的纵坐标，θ是随动坐标系的x0轴(即轮式移动平台前行向)与绝对坐标系的x轴的夹角。
[0066]
进一步的说，处理器4包括预瞄准点位姿误差计算单元、运动控制单元、速度规划单元和速度反馈单元(参见附图1)。其中，预瞄准点位姿误差计算单元用于根据车载定位装置1提供的轮式移动平台定位位姿、运动参考曲线生成器2提供的路径曲线方程、以及预瞄准量调节器3提供的调整量数据计算轮式移动平台预瞄点位姿误差e
pr
；速度反馈单元用于将轮式移动平台当前速度数据反馈给速度规划单元；速度规划单元根据轮式移动平台与目标点的剩余路程在速度反馈单元反馈的当前速度信息基础上规划当前点的线速度和角速度；运动控制单元，根据预瞄准点位姿误差对轮式移动平台进行位姿调控，是实现轮式移动平台精确跟踪参考曲线的核心单元，根据运动学规律和李雅普诺夫稳定性定理，得到轮式移动平台的控制律为：
[0067][0068]
式中，v
r
是机器人线速度规划值，由点q到终点t的距离以及速度约束得到，ω
r
是机器人角速度规划值，ω
r
由v
r
和q点曲率相乘得到，[v,ω]
t
是线速度和角速度控制量，控制参数k1＞0，k2＞0，k3＞0。将e
pr
、[v
r
,ω
r
]
t
带入上式，即可得到对轮式移动平台的轨迹控制数
据，将轨迹控制数据输送给执行机构单元。
[0069]
参照图4，为本发明的一种运动轨迹预瞄跟踪控制方法，包括以下步骤：
[0070]
s100：加载车载定位装置1的定位位姿、运动参考曲线生成器2的路径方程、预瞄准量调节器3的调整量数据，并将数据输入处理器4。
[0071]
s200：利用定位位姿、路径方程、调整量数据计算轮式移动平台预瞄点位姿误差。
[0072]
s300：利用轮式移动平台与目标点的剩余路程规划当前点的线速度和角速度。
[0073]
s400：根据预瞄点位姿误差、规划的线速度与角速度计算轮式移动平台运动的控制线速度和控制角速度。
[0074]
s500：将控制线速度和控制角速度作为轮式移动平台的当前控制值，输入给执行机构单元7，调整轮式移动平台的位姿，达到目标姿态。
[0075]
本发明还公开了一种计算机可读存储介质，其上存储有计算机程序，计算机程序为预瞄准点位姿误差解算和控制程序，其被处理器4执行时可以实现本发明公开的一种轮式移动平台运动轨迹预瞄跟踪控制方法。
[0076]
为了易于说明，实施例中使用了诸如“上”、“下”、“左”、“右”等空间相对术语，用于说明图中示出的一个元件或特征相对于另一个元件或特征的关系。应该理解的是，除了图中示出的方位之外，空间术语意在于包括装置在使用或操作中的不同方位。例如，如果图中的装置被倒置，被叙述为位于其他元件或特征“下”的元件将定位在其他元件或特征“上”。因此，示例性术语“下”可以包含上和下方位两者。装置可以以其他方式定位(旋转90度或位于其他方位)，这里所用的空间相对说明可相应地解释。
[0077]
而且，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个与另一个具有相同名称的部件区分开来，而不一定要求或者暗示这些部件之间存在任何这种实际的关系或者顺序。
[0078]
以上对本发明做了示例性的描述，应该说明的是，在不脱离本发明的核心的情况下，任何简单的变形、修改或者其他本领域技术人员能够不花费创造性劳动的等同替换均落入本发明的保护范围。