CN114371702B - 一种应用于视觉伺服的非线性模型预测控制方法 - Google Patents

Abstract

本发明公开了一种应用于视觉伺服的非线性模型预测控制方法，无人车安装四个麦克纳姆轮，得益于麦克纳姆轮特有的结构，无人车能进行全向运动，提高了无人车的灵活性；无人车的底部安装一个竖直向下的摄像头用于检测地面上的特征码，辅助无人车的定位和导航；本发明将非线性模型预测控制与无人车的运动学模型相结合，建立基于视觉伺服的无人车分层控制方法，包括线性模型预测控制和PID控制；外部的非线性模型预测控制计算出无人车的速度值，在满足各类约束的情况下控制无人车运动到特征码的正上方，并确保在控制过程中特征码始终在底部摄像头的视野内；内部的PID控制器负责将速度指令转换为每个电机的转速，控制无人车按照指令运动。

Description

一种应用于视觉伺服的非线性模型预测控制方法

技术领域

本发明属于导航和控制领域，涉及一种应用于视觉伺服的非线性模型预测控制方法。

背景技术

为了实现制造强国的战略目标，在工厂和仓库中使用大规模机器人取代人工执行货物分拣和搬运等繁重工作将成为未来的发展趋势。随着信息物理系统(CPS)和信息与通信技术(ICT)的发展，越来越多的智慧工厂配置无人车来进行货物分拣，将指定的货物从当前区域运输到目标区域进行加工或集中仓储以提高工厂的运作效率。

大多数无人车依靠不同的车载传感器来定位和导航，如激光雷达、摄像头和磁传感器。在现代工厂中，为了完成复杂的运输和分类任务，IMU、摄像头和激光雷达通常被集成在一起来辅助无人车导航。当利用激光雷达进行导航时，不需要固定的导航标识，例如磁条和特征码，虽然提高了无人车的灵活性，但是降低了定位精度不高(Ye H,Chen Y,LiuM.Tightly coupled 3d lidar inertial odometry and mapping[C]//2019International Conference on Robotics and Automation(ICRA).IEEE,2019:3144-3150.)，利用视觉伺服来进行提高无人车的导航精度是一种常用的方法(Xiong J,Liu Y,Ye X,et al.A hybrid lidar-based indoor navigation system enhanced by ceilingvisual codes for mobile robots[C]//2016IEEE International Conference onRobotics and Biomimetics(ROBIO).IEEE,2016:1715-1720.)。视觉伺服方法主要是在对相机进行标定后，通过固定在地面上的QR码或AprilTag码等特征码获取无人车的三维坐标来辅助无人车在仓储系统中移动，但是如何有效地控制无人车按照设计的轨迹移动是一个亟待解决的问题。传统的视觉伺服控制方法不能确保在控制过程中特征码始终在摄像投视野范围内(Nazemzadeh P,Fontanelli D,Macii D,et al.Indoor localization ofmobile robots through QR code detection and dead reckoning data fusion[J].IEEE/ASME Transactions On Mechatronics,2017,22(6):2588-2599.)(王家恩,肖献强.基于QR码视觉定位的移动机器人复合导航方法研究[J].仪器仪表学报,2018,39(8):9.)，如果控制输入过大，无人车无法检测到特征码后，它将无法通过视觉伺服纠正导航偏差。近年来，模型预测控制(MPC)被广泛应用于具有物理约束的复杂系统中，其中控制输入是通过一个包含多个性能指标的约束优化函数来计算的。

发明内容

为解决上述技术问题，本发明提出一种应用于视觉伺服的非线性模型预测控制方法，通过识别地面上的特征码来获取无人车的位姿，当无人车检测到特征码时，将其与特征码的中心的距离差为输入，控制其运动到特征码的正上方，且在整个控制过程中保证特征码始终在摄像头的视野范围内，本发明基于非线性模型预测控制和PID控制的分层控制算法，外部的非线性模型预测控制算法计算出无人车的速度值，内部的PID控制器负责将速度指令转换为每个电机的转速，控制无人车按照指令运动，从而提高室内环境中无人车的导航精度。

本发明至少通过如下技术方案之一实现。

一种应用于视觉伺服的非线性模型预测控制方法，包括以下步骤：

1)针对具有麦克纳姆轮的无人车，建立运动学模型；

2)当安装在无人车底的摄像头检测AprilTag码时，获取无人车在世界坐标系的位姿；

3)以AprilTag码的中心做为目标点，构建无人车当前位置与目标点的运动偏差方程；

4)根据分层控制算法控制无人车的速度指令运动，其中分层控制算法包括外部控制器和内部控制器，所述外部控制器为非线性模型预测控制器，用于输出无人车的速度信息；内部控制器为PID控制器，用于将速度信息转换为每个电机的转速，控制无人车按照外部控制器输出的速度指令运动；

5)构建非线性模型预测控制模型，控制无人车从当前位置精准移动到目标点，且在整个控制过程中，保持AprilTag码始终在底部摄像头的视野范围内；

6)无人车到达目标点后，发送前进命令直到底部摄像头再次检测到AprilTag码。

进一步地，无人车安装四个麦克纳姆轮，分别由四个电机控制，麦克纳姆轮因为其特有的构造，能够实现全向运动，麦克纳姆轮的运动学模型如下：

首先将无人车速度v分为无人车坐标系内x轴速度v_x和y轴速度v_y两个分量：

v＝v_x+v_y

麦克纳姆轮上安装的滚子与车轮旋转轴的夹角为λ度，所以其中一个麦克纳姆轮的运动学方程为：

v_1x＝-v_1gsin(λ),v_1y＝v₁+v_1gcos(λ)

v_1x＝v_x-r_y·ω,v_1y＝v_y-r_x·ω

其中ω为无人车的角速度，r是无人车中心到麦克纳姆轮轴心的距离，v₁是其中一个麦克纳姆轮的运动速度，v_1g为车轮上滚子在地面上产生的线速度，v_1x和v_1y是无人车坐标系中v₁的x轴分量和y轴分量，r_x和r_y是r的x轴和y轴分量，因此：

v₁＝v_1x+v_1y

其他三个轮子的速度与第一个轮子的计算方法相似，因此，当无人车的速度为v时，四个麦克纳姆轮各自的速度为：

进一步地，当安装在无人车底的摄像头检测AprilTag码时，获取无人车在世界坐标系的位姿为：

当四麦克纳姆轮无人车运动到AprilTag码附近时，位于无人车底部的摄像头能够拍摄到AprilTag码，利用Opencv图像处理库中的findcounters函数获得AprilTag码的边界，再计算边界的均值获得AprilTag的中点，将中点作为在该AprilTag坐标系中的底部摄像机的坐标(x_m,y_m)；

根据下式计算无人车在世界坐标系的坐标(x_w,y_w,z_w)；

其中，z是底部摄像头的高度，是从AprilTag坐标系转换到世界坐标系的转换矩阵，f_x、f_y、u₀、v₀是相机的内部参数，通过相机标定获得。

进一步地，在解码时，AprilTag码内每个黑色方块代表1，白色方块代表0，每次将AprilTag码旋转90度，当AprilTag码的解码值与其正方向时的记录的解码值相等时，记录此时的旋转次数β；最后按下式确定无人车的角度α：

其中α_i是AprilTag码每条边的倾斜角度。

进一步地，以AprilTag码的中心做为目标点，构建无人车当前位置与目标点的运动偏差方程：

四麦克纳姆轮无人车的运动学模型如下：

其中，θ是无人车的航向角，是无人车位姿的导数，v_x和v_y表示无人车坐标系内x轴速度v_x和y轴速度，ω为无人车的角速度。

进一步地，定义无人车在世界坐标系与目标点的姿态偏差x_e为：

其中，(x,y)是无人车当前位姿包括无人车的坐标和航向角，(x_r,y_r,θ_r)是目标位姿，即AprilTag码的中点和规定路线中无人车在该码位置时的航向角，(x_e,y_e,θ_e)表示当前位姿与目标位姿的偏差，随后对x_e求导：

其中控制输入u＝[u₁,u₂,u₃]^T被设定为[-v_x,-v_y,-ω]^T。

进一步地，构建非线性模型预测控制模型，控制无人车从当前位置移动到目标点，且在整个控制过程中，保持AprilTag码始终在底部摄像头的视野范围内：

无人车的离散运动学模型如下：

x_e(k+1)＝f(x_e(k))+h(k)u(k)

其中：

h(k)＝TI,I∈R^3×3

式中，ω为无人车的角速度，I是单位矩阵，T是控制算法的采样周期，x_e(k)是系统在k时刻的状态，即离目标位姿的差(x_e(k)，y_e(k)，θ_e(k))，x_e(k+1)是系统在k+1时刻离目标位姿的差，f(x_e(k))和h(k)为非线性函数，u(k)为k时刻的系统控制输入，定义在k时刻的控制输入增量为Δu(k∣k)＝u(k+1∣k)-u(k∣k)，其中，u(k∣k)为在k时刻的控制输入，u(k+1∣k)为在k时刻预测的k+1时刻无人车控制输入。则非线性模型预测控制的优化方程J(x_e,Δu)如下所示：

其中，R和Q是权重矩阵，x_e(k+i∣k)是系统无人车在k时刻预测的k+i时刻的当前姿态与目标姿态的差，N是预测域长度，控制输入u和控制输入增量Δu受限于无人车的性能，因此且/>即小于无人车速度的最大值u_max和加速度的最大值Δu_max且大于无人车速度的最小值u_min和加速度的最小值Δu_min，为了确保无人车能够在控制过程中识别AprilTag码，设置x_e∈(x_emin,x_emax)，其中x_emax是最大位姿差，x_emin是最小位姿差，超过此范围后，底部摄像头将无法检测AprilTag码。

进一步地，非线性模型预测控制器的预测状态x_e(k+i∣k)定义为：

x_e(k+i∣k)＝U_k+i+f(x_e(k+i-1∣k)+h(k+i-1∣k)u(k)

其中，U_k+i是在预测域内控制增量的和，即 Δu(k+j-1∣k)是系统在k时刻预测k+j-1时刻的系统输入增量。h(k+j-1∣k)是k时刻预测k+j-1时刻的非线性方程；将非线性模型预测控制的优化方程转化为二次规划问题V：

其中，C_k+i＝u(k)+x_e(k+i-1∣k)，这样通过求解方程V(k)就可以得到k时刻的最优控制器输入u(k)，内部控制器接收到控制输入u(k)后将其转换为每个轮子在该时刻所需的转速。

进一步地，所述AprilTag码替换为QR码。

进一步地，内部控制器中的速度命令通过PID算法结合电机编码器返回的实际转速作为反馈控制电机按指定的转速运动。

与现有的技术相比，本发明的有益效果为：

本发明提出了一种应用于视觉伺服的非线性模型预测控制方法，与现有技术相比，具有以下优点：本发明在建立无人车运动学模型和视觉伺服模型的基础上，通过将位姿误差模型转化为二次规划问题，设计了一种非线性模型预测控制器，便于求解最优解。此外，当底部摄像头检测到AprilTag码时，能够修正导航偏差，并检索到当前世界坐标系中的位置。随后采取非线性模型预测方程，通过设置约束条件，确保无人车在视场范围内快速、准确地移动到目标位姿。本发明的控制方案相对于传统的PID算法，能够满足各类无人车的约束条件，提高了控制算法的收敛速度，在控制效率上有显著优势。

附图说明

图1为本实施例一种应用于视觉伺服的非线性模型预测控制方法的结构图；

图2为本实施例四麦克纳姆轮运动无人车模型示意图；

图3为本实施例AprilTag码示意图；

图4为本实施例四麦克纳姆轮无人车运动学模型示意图；

图5为本实施例差分轮无人车运动学模型示意图；

图6为本实施例放置有各类AprilTag码的仓储系统示意图。

具体实施方式

下面结合实施例及附图，对本发明作进一步地详细说明，但本发明的实施方式不限于此。

实施例1：

如图1所示的一种应用于视觉伺服的非线性模型预测控制方法，包括如下步骤：

1)当四麦克纳姆轮无人车经过AprilTag码时，根据底部摄像头拍摄的图像确定无人车的位姿，具体过程为：

如图2所示，AprilTag码是一个视觉基准库，由于它能够存储信息且识别简便而被广泛应用于机器人系统。当四麦克纳姆轮无人车运动到AprilTag码附近时，位于无人车底部的摄像头能够拍摄到AprilTag码，然后利用Opencv图像处理库中的findcounters函数获得AprilTag码的边界，再计算边界的均值获得AprilTag的中点，将它作为在该AprilTag坐标系中的底部摄像机的坐标(x_m,y_m)。

由于AprilTag码固定在地面，所以每个码在世界坐标系中的坐标已知，因此可以根据下式计算无人车在世界坐标系的坐标(x_w,y_w,z_w)：

其中，z是底部摄像头的高度，是从AprilTag坐标系转换到世界坐标系的转换矩阵，f_x、f_y、u₀、v₀是相机的内部参数，通过相机标定获得；

在解码时，AprilTag码内每个黑色方块代表1，白色方块代表0，从码左上方开始解码到右下方结束，因此不同角度的AprilTag码具有不同的解码值。以图3为例，它的解码值为d5d628584。因此不同角度的AprilTag码具有不同的解码值。每次将AprilTag码旋转90度，当AprilTag码的解码值与其正向的解码值相等时，记录其旋转次数β，因此利用AprilTag码的旋转角度确定无人车的角度α：

其中α_i是AprilTag码每条边的倾斜角度，由该条边内的点通过最小二乘法计算获得。

2)如图5所示，结合四麦克纳姆轮的无人车运动学模型基于非线性模型预测控制算法控制无人车，具体过程为：

不同于差分式无人车的运动学模型，四麦克纳姆轮式无人车可以沿不同的角度移动。麦克纳姆轮的分布和无人车的运动学模型如图4所示，由于麦克纳姆轮上安装的滚子与车轮旋转轴的夹角为45度，所以

v_1x＝v_x-r_y·ω,v_1y＝v_y-r_x·ω.

其中ω为无人车的角速度，r是无人车中心到麦克纳姆轮轴心的距离，v₁是轮子1的运动速度，v_1g为车轮上滚子在地面上产生的线速度，v_1x和v_1y是无人车坐标系中v₁的x轴分量和y轴分量，r_x和r_y是r的x轴和y轴分量，因此：

v₁＝v_1x+v_1y

在此之后，其他三个轮子的速度与第一个轮子的计算方法相似。因此,

四麦克纳姆轮无人车的运动学模型如下所示，

其中，θ是无人车的航向角，是无人车位姿的导数。定义无人车在世界坐标系与目标点的距离差为：

其中，(x,y)是无人车当前位姿包括无人车的坐标和航向角，(x_r,y_r,θ_r)是目标位姿，即AprilTag码的中点和规定路线中无人车在该码位置时的航向角。(x_e,y_e,θ_e)表明当前位姿与目标位姿的偏差，随后对x_e求导可得

其中u＝[u₁,u₂,u₃]^T，它被设定为[-v_x,-v_y,-ω]^T。无人车的离散运动学模型如下所示：

x_e(k+1)＝f(x_e(k))+h(k)u(k)

其中：

h(k)＝TI,I∈R^3×3

式中，ω为无人车的角速度，I是单位矩阵，T是控制算法的采样周期，x_e(k)是系统在k时刻的状态，即离目标位姿的差(x_e(k)，y_e(k)，θ_e(k))，x_e(k+1)是系统在k+1时刻离目标位姿的差，f(x_e(k))和h(k)为非线性函数，u(k)为k时刻的系统控制输入。定义k时刻的控制输入增量为Δu(k∣k)＝u(k+1∣k)-u(k∣k)，其中，u(k∣k)为在k时刻的控制输入，u(k+1∣k)为在k时刻预测的k+1时刻无人车控制输入。则非线性模型预测控制的优化方程J(x_e,Δu)如下所示：

其中，R和Q是权重矩阵，x_e(k+i∣k)是系统无人车在k时刻预测的k+i时刻的当前姿态与目标姿态的差，N是预测域长度，控制输入u和控制输入增量Δu受限于无人车的性能，因此且/>即小于无人车速度的最大值u_max和加速度的最大值Δu_max且大于无人车速度的最小值u_min和加速度的最小值Δu_min。为了确保无人车能够在控制过程中识别AprilTag码，设置x_e∈(x_emin,x_emax)，其中x_emax是最大位姿差，x_emin是最小位姿差，超过此范围后，底部摄像头将无法检测AprilTag码；随后，非线性模型预测控制器的预测状态x_e(k+i∣k)定义为：

x_e(k+i∣k)＝U_k+i+f(x_e(k+i-1∣k)+h(k+i-1∣k)u(k)

其中，U_k+i是在预测域内控制增量的和，即 Δu(k+j-1∣k)是系统在k时刻预测k+j-1时刻的系统输入增量。h(k+j-1∣k)是k时刻预测k+j-1时刻的非线性方程，这里仍为TI。将非线性模型预测控制的优化方程转化为二次规划问题V：

其中，C_k+i＝u(k)+x_e(k+i-1∣k)，这样通过求解方程V(k)就可以得到k时刻的最优控制器输入u(k)。随后内部控制器接收到控制输入u(k)后将其转换为每个轮子在该时刻所需的转速，并通过PID算法结合电机编码器返回的实际转速作为反馈控制电机按指定的转速运动。

实施例2

本发明还能适用于差分轮无人车的视觉伺服，不同于四麦克纳姆轮无人车可以朝任意方向运动，差分轮无人车的两个动力轮一般位于无人车地盘的左右两侧，分别给两个轮子施加不同的转速以实现地盘的转向。如图4所示，设置顺时针旋转为正方形，其运动学模型如下所示：

其中,θ是无人车的航向角，是无人车位姿的导数，v是差分轮无人车直行速度，ε是差分轮无人车的角速度。与四麦克纳姆轮无人车定义无人车在世界坐标系与目标点的距离差为：

其中，(x,y)是无人车当前的坐标，(x_r,y_r,θ_r)是目标位姿，即AprilTag码的中点和规定路线中无人车在该码位置时的航向角，。(x_e,y_e,θ_e)表明当前位姿与目标位姿的偏差，随后对x_e求导可得：

其中u＝[u₁,u₂]^T，它被设定为[-v,-ω]^T。随后与实施例1的控制方法类似，即可实现基于非线性模型预测控制的差分轮无人车导航。

实施例3

在一些工业场景中，无人车需要从一个目标点运动到另一个目标点，为此我们设计当前地图由多个节点构成，每个节点上放置有对应的AprilTag码，用于记录其位置信息，当无人车从起始节点出发时，途径每个节点，车底摄像头能够获取自身的位置信息，判断在既定路线中下一个节点的方向，借助AprilTag码来实现无人车的转向和节点间的移动。

图6显示了在一个4×4米的地图中AprilTag码的分布情况，其中码与码之间的距离为2米，无人车需要从起始节点3运动到目标节点7。这个过程中，当无人车检测到AprilTag码时，除了利用视觉伺服和非线性模型预测控制算法来控制无人车纠偏外，无人车还能够获取当前的位姿，以判断是否在该节点处转弯。以节点2为例，无人车识别到AprilTag码后，除了控制无人车移动到特征码中心外，还需要控制无人车逆时针旋转90度以到达下一个节点。当无人车从一个节点驶向另一个节点时，底部摄像头无法检测到AprilTag码，此时只有内部控制器发挥作用，持续接收直行命令，直到检测到下一个AprilTag码。即使没有外部传感器会导致内部控制器会产生累计误差，无人车每次检测到AprilTag码时都能够消除累计误差且保证其能准确地旋转对应的角度，所以无人车能够稳定地从起始节点运动到目标节点。

实施例4

如果一些仓储系统要求地面特征码存储更多的信息，也可以采用QR码来进行视觉伺服。相对于有黑色外边框的AprilTag码，QR码的定位精度稍差，但是它能够存储更多的信息，能够满足大规模智慧仓库和工厂的需求。QR码带有三个定位标识，可以用于确定无人车的位置和航向角。在仓库内沿既定线路，每隔一段距离放置一个独特QR码用于记录当前节点的位置信息。与AprilTag码类似，当无人车识别到QR码时，根据其三个定位图案计算QR的中点作为非线性模型预测控制的目标点，控制无人车运动到QR码的中点，然后持续发送直行命令，直到无人车检测到下一个目标节点。这样无人车能够从起始节点运动到目标节点以顺利完成各类运输任务。

上述实施例仅用于详细阐述以帮助理解本发明的技术方案，对本领域技术人员而言，在不脱离本发明原理的前提下做出的任何改进与替换，均属于本发明的保护范围。

Claims (8)

1.一种应用于视觉伺服的非线性模型预测控制方法，其特征在于，包括以下步骤：

1)针对具有麦克纳姆轮的无人车，建立运动学模型；

2)当安装在无人车底的摄像头检测AprilTag码时，获取无人车在世界坐标系的位姿；

3)以AprilTag码的中心做为目标点，构建无人车当前位置与目标点的运动偏差方程；

在解码时，AprilTag码内每个黑色方块代表1，白色方块代表0，每次将AprilTag码旋转90度，当AprilTag码的解码值与其正方向时的记录的解码值相等时，记录此时的旋转次数β；最后按下式确定AprilTag码的角度α：

其中α_i是AprilTag码每条边的倾斜角度；

4)根据分层控制算法控制无人车的速度指令运动，其中分层控制算法包括外部控制器和内部控制器，所述外部控制器为非线性模型预测控制器，用于输出无人车的速度信息；内部控制器为PID控制器，用于将速度信息转换为每个电机的转速，控制无人车按照外部控制器输出的速度指令运动；

5)构建非线性模型预测控制模型，控制无人车从当前位置精准移动到目标点，且在整个控制过程中，保持AprilTag码始终在底部摄像头的视野范围内：无人车的离散运动学模型如下：

x_e(k+1)＝f(x_e(k))+h(k)u(k)

其中：

h(k)＝TI，I∈R^3×3

式中，ω为无人车的角速度，I是单位矩阵，T是控制算法的采样周期，x_e(k)是系统在k时刻的状态，即离目标位姿的差(x_e(k)，y_e(k)，θ_e(k))，x_e(k+1)是系统在k+1时刻离目标位姿的差，f(x_e(k))和h(k)为非线性函数，u(k)为k时刻的系统控制输入，定义在k时刻的控制输入增量为Δu(k|k)＝u(k+1|k)-u(k|k)，其中，u(k|k)为在k时刻的控制输入，u(k+1|k)为在k时刻预测的k+1时刻无人车控制输入，则非线性模型预测控制的优化方程J(x_e，Δu)如下所示：

x_e∈Ω

其中，R和Q是权重矩阵，x_e(k+i|k)是系统无人车在k时刻预测的k+i时刻的当前姿态与目标姿态的差，N是预测域长度，控制输入u和控制输入增量Δu受限于无人车的性能，因此且/>即小于无人车速度的最大值u_max和加速度的最大值Δu_max且大于无人车速度的最小值u_min和加速度的最小值Δu_min，为了确保无人车能够在控制过程中识别AprilTag码，设置x_e∈(x_emin，x_emax)，其中x_emax是最大位姿差，x_emin是最小位姿差，超过此范围后，底部摄像头将无法检测AprilTag码；

6)无人车到达目标点后，发送前进命令直到底部摄像头再次检测到AprilTag码。

2.根据权利要求1所述的一种应用于视觉伺服的非线性模型预测控制方法，其特征在于，无人车安装四个麦克纳姆轮，分别由四个电机控制，麦克纳姆轮因为其特有的构造，能够实现全向运动，麦克纳姆轮的运动学模型如下：

首先将无人车速度v分为无人车坐标系内x轴速度v_x和y轴速度v_y两个分量：

v＝v_x+v_y

麦克纳姆轮上安装的滚子与车轮旋转轴的夹角为λ度，所以其中一个麦克纳姆轮的运动学方程为：

v_1x＝-v_1gsin(λ)，v_1y＝v₁+v_1gcos(λ)

v_1x＝v_x-r_y·ω，v_1y＝v_y-r_x·ω

其中ω为无人车的角速度，r是无人车中心到麦克纳姆轮轴心的距离，v₁是其中一个麦克纳姆轮的运动速度，v_1g为车轮上滚子在地面上产生的线速度，v_1x和v_1y是无人车坐标系中v₁的x轴分量和y轴分量，r_x和r_y是r的x轴和y轴分量，因此：

v₁＝v_1x+v_1y

其他三个轮子的速度与第一个轮子的计算方法相似，因此，当无人车的速度为v时，四个麦克纳姆轮各自的速度为：

3.根据权利要求1所述的一种应用于视觉伺服的非线性模型预测控制方法，其特征在于，当安装在无人车底的摄像头检测AprilTag码时，获取无人车在世界坐标系的位姿为：

当四麦克纳姆轮无人车运动到AprilTag码附近时，位于无人车底部的摄像头能够拍摄到AprilTag码，利用Opencv图像处理库中的findcounters函数获得AprikTag码的边界，再计算边界的均值获得AprilTag的中点，将中点作为在该AprilTag坐标系中的底部摄像机的坐标(x_m，y_m)；

根据下式计算无人车在世界坐标系的坐标(x_w，y_w，z_w)；

其中，z是底部摄像头的高度，是从AprilTag坐标系转换到世界坐标系的转换矩阵，f_x、f_y、u₀、v₀是相机的内部参数，通过相机标定获得。

4.根据权利要求1所述的一种应用于视觉伺服的非线性模型预测控制方法，其特征在于，以AprilTag码的中心做为目标点，构建无人车当前位置与目标点的运动偏差方程：

四麦克纳姆轮无人车的运动学模型如下：

其中，θ是无人车的航向角；是无人车位姿的导数，v_x和v_y表示无人车坐标系内x轴速度v_x和y轴速度，ω为无人车的角速度。

5.根据权利要求1所述的一种应用于视觉伺服的非线性模型预测控制方法，其特征在于，定义无人车在世界坐标系与目标点的姿态偏差x_e为：

其中，(x，y)是无人车当前位姿包括无人车的坐标和航向角，(x_r，y_r，θ_r)是目标位姿，即AprilTag码的中点和规定路线中无人车在该码位置时的航向角，(x_e，y_e，θ_e)表示当前位姿与目标位姿的偏差，随后对x_e求导：

其中控制输入u＝[u₁，u₂，u₃]^T被设定为[-v_x，-v_y，-ω]^T。

6.根据权利要求1所述的一种应用于视觉伺服的非线性模型预测控制方法，其特征在于，非线性模型预测控制器的预测状态x_e(k+i|k)定义为：

x_e(k+i|k)＝U_k+i+f(x_e(k+i-1|k))+h(k+i-1|k)u(k)

其中，U_k+i是在预测域内控制增量的和，即 Δu(k+j-1|k)是系统在k时刻预测k+j-1时刻的系统输入增量；h(k+j-1|k)是k时刻预测k+j-1时刻的非线性方程，将非线性模型预测控制的优化方程转化为二次规划问题V：

x_e∈Ω

其中，C_k+i＝u(k)+x_e(k+i-1|k)，这样通过求解方程V(k)就可以得到k时刻的最优控制器输入u(k)，内部控制器接收到控制输入u(k)后将其转换为每个轮子在该时刻所需的转速。

7.根据权利要求1所述的一种应用于视觉伺服的非线性模型预测控制方法，其特征在于，所述AprilTag码替换为QR码。

8.根据权利要求1～7任一项所述的一种应用于视觉伺服的非线性模型预测控制方法，其特征在于，内部控制器中的速度命令通过PID算法结合电机编码器返回的实际转速作为反馈控制电机按指定的转速运动。