CN113848908B - 一种全方位移动机器人视觉伺服系统的建模与控制方法 - Google Patents

Abstract

一种全方位移动机器人视觉伺服系统的建模与控制方法，考虑非完整性移动机器人存在不可积分的速度约束问题，本发明研究由三个主动偏心轮组成的全方位移动机器人视觉伺服系统；针对视觉伺服系统具有视觉可见性约束和执行器约束，本发明提出了一种基于模型预测控制策略的视觉伺服控制器；首先，基于主动偏心轮全方位移动机器人的运动学方程和视觉伺服系统误差模型，对全方位移动机器人偏心方向轮滚动角速度和偏转角速度的视觉伺服误差系统进行建模；在此基础上，结合模型预测控制策略，建立全方位移动机器人的视觉伺服预测模型，进而设计了视觉伺服预测控制策略；所设计的控制器不仅可以驱动移动机器人到达目标点，而且可以较好地处理约束条件。

Description

一种全方位移动机器人视觉伺服系统的建模与控制方法

技术领域

本发明涉及机器人视觉伺服控制技术领域，具体涉及一种全方位移动机器人视觉伺服系统的建模与控制方法。

背景技术

传统移动机器人在实际应用过程中有着各种移动限制，如移动方向固定，不能原地旋转等等，全方位移动机器人的出现突破了这种限制，克服了不能横向移动、原地旋转移动等问题，已经成为新型移动机器人的研究方向与热点，有着广阔的发展前景。

视觉伺服控制法是目前常用的一种全方位移动机器人驱动与控制方法，而目前的视觉伺服控制研究中往往不能满足视觉可见性约束和执行器约束，不能很好的处理约束条件。

发明内容

针对现有技术中存在的问题，且考虑非完整性移动机器人存在不可积分的速度约束问题，本发明研究由三个主动偏心轮组成的全方位移动机器人视觉伺服系统。针对视觉伺服系统具有视觉可见性约束和执行器约束，本发明提出了一种基于模型预测控制策略的视觉伺服控制器。

本发明的技术方案如下：

一种全方位移动机器人视觉伺服系统的建模与控制方法，包括如下步骤：

1)全方位移动机器人描述：

在平面内设置一个全方位移动机器人，该机器人由三个偏心轮独立驱动，且在该机器人上方搭载一个单目相机；全方移动机器人在平面内运动时，全方位移动机器人的运动学方程为：

其中(x,y)表示全方位移动机器人质心在世界坐标系X_w-Y_w平面的坐标，θ表示世界坐标系到机器人坐标系的夹角，逆时针为正，v_x和v_y表示全方位移动机器人的纵向线速度和横向线速度，ω表示全方位移动机器人的角速度，令q＝[xyθ]^T表示全方位移动机器人的位姿，ν_r＝[v_xv_yω]^T表示全方位移动机器人的速度；

2)建立视觉伺服误差模型：

假设特征点P在图像坐标系投影点p的图像坐标为(x_p,y_p)，在像素坐标系中的坐标为(u_p,v_p)，图像平面中心为(u0,v0)，相机的焦距为f，根据图像投影模型，可得到归一化图像坐标：

其中dx，dy表示感光芯片每单位像素的实际大小；

为了完成视觉伺服控制任务，控制器需要通过移动机器人的位置误差调整移动机器人的速度，从而使移动机器人到达期望位姿，因此引入机器人当前位姿变量ρ₁，ρ₂，

定义全方位移动机器人的误差模型：

其中θ^*为期望角度，ρ₁ ^*，ρ₂ ^*为期望位姿变量；

对式(4)求导可得全方位移动机器人视觉伺服误差模型为：

3)建立全方位移动机器人运动学模型：

全方位移动机器人在运动时由三个偏心方向轮驱动，其中点M表示全方位移动机器人的质心，点A、B、C分别表示三个偏心方向轮在运动过程中，相对于机器人本体的不动点，位于相应各个偏心方向轮竖直转向轴的中心轴上；MA、MB、MC之间的夹角均为2π/3，r表示偏心方向轮的半径，l为偏心方向轮竖直轴中心到M点的距离，即MA＝MB＝MC＝l；d表示偏心方向轮的偏心距，α表示机器人坐标系X_r轴到直线MA的角度，逆时针为正，恒为常数；β表示偏心方向轮的转向角，从直线MA到偏心方向轮轴线方向的角度，逆时针为正，表示偏心方向轮行走时的滚动角；

当全方位移动机器人由三个偏心方向轮驱动时，可得轮子的转动速度及偏转速度/>与移动机器人速度v_r之间的转换关系为：

其中分别表示三个偏心方向轮的转动速度，/>分别表示三个偏心方向轮的偏转速度，雅克比矩阵J(β_i,θ)表示如下：

结合式(5)可以得到关于轮子转动速度和偏转速度的移动机器人视觉伺服误差系统，即

其中

式(8)中为矩阵，J₁(β_i)的伪逆矩阵，/>

为保证视觉伺服任务的有效执行，在全方位移动机器人执行视觉伺服任务中需要满足误差约束和速度约束条件；

e_1min≤e₁≤e_1max,e_2min≤e₂≤e_2max (9)

其中，下标min表示对应变量的取值下界，下标max表示对应变量的取值上界；

4)视觉伺服预测控制器设计：

考虑全方位移动机器人视觉伺服误差系统(7)，取采样时间为T_s，采用欧拉离散化，可得到对应的离散时间视觉伺服误差系统：

e(k+1)＝e(k)+B(k)u(k) (11)

其中：

由于z_c是相机坐标系原点到特征点P的高度信息，因此z_c是一个常数，i＝1,2,3是控制输入；

式(11)可以写成更紧凑的形式：

e(k+1)＝f(e(k),u(k)) (12)

其中e(k+1)是误差状态，u(k)是控制输入，f(·)代表对应的映射关系；

考虑全方位移动机器人视觉伺服误差系统(11)，采用序列二次规划来最小化二次目标函数的控制输入，定义全方位移动机器人视觉伺服误差模型的目标函数为：

其中Q和R分别为状态和控制的正定加权矩阵，目标函数中的第1项表示了系统镇定到期望点的能力，第2项说明了它约束控制输入的能力；其中N_p为预测时域，N_c为控制时域，N_p和N_c会影响迭代步长，N_p值和N_c值越小，控制响应越快；e(k+i|k)表示状态量在k时刻对k+i时刻的预测值，u(k+i-1|k)表示控制输入在k时刻对k+i-1时刻的预测值；定义k时刻N_c步预测控制序列u(k)＝{u(k|k),u(k+1|k),…,u(k+N_c-1|k)}，N_p步预测状态序列为e(k)＝{e(k+1|k),e(k+2|k),…,e(k+N_p|k)}；

通过全方位移动机器人的二次目标函数(13)可以得到全方位移动机器人视觉伺服的预测模型为：

e(k+i|k)＝f(e(k+i-1|k),u(k+i-1|k)) (14)

考虑视觉可见性约束(9)和执行器约束(10)，全方位移动机器人预测镇定控制器为：

其中e₀表示在初始时刻获取的误差值，u^*(k)表示优化问题的最优解；

5)全方位移动机器人向目标点行进移动，全方位移动机器人最终到达目标点。

本发明的有益效果如下：本发明基于主动偏心轮全方位移动机器人的运动学方程和视觉伺服系统误差模型，对全方位移动机器人偏心方向轮滚动角速度和偏转角速度的视觉伺服误差系统进行建模，在此基础上，结合模型预测控制策略，建立全方位移动机器人的视觉伺服预测模型，进而设计了视觉伺服预测控制策略。所设计的控制器不仅可以驱动移动机器人到达目标点，而且可以较好地处理约束条件。

附图说明

图1本发明全方位移动机器人视觉伺服模型；

图2本发明全方位移动机器人俯视图；

图3本发明全方位移动机器人运动轨迹图。

具体实施方式

以下结合说明书附图，对本发明作进一步描述。

全方位移动机器人视觉伺服系统的建模与控制方法，具体步骤如下：

1)全方位移动机器人描述：

如图1所示，在平面内有一个全方位移动机器人，该机器人由三个偏心轮独立驱动，且在移动机器人上方搭载一个单目相机。其中O_w-X_wY_wZ_w为世界坐标系，O_r-X_rY_rZ_r为机器人坐标系，原点O_r位于移动机器人的质心处，O_c-X_cY_cZ_c为相机坐标系，原点O_c位于机器人坐标系的Z_r轴上，X_c，Y_c轴与X_r，Y_r轴方向相同，其中Y_c轴与相机的光轴方向一致。

移动机器人在平面内运动时，那么全方位移动机器人的运动学方程为：

其中(x,y)表示全方位移动机器人质心在世界坐标系X_w-Y_w平面的坐标，θ表示世界坐标系到机器人坐标系的夹角，逆时针为正，v_x和v_y表示全方位移动机器人的纵向线速度和横向线速度，ω表示全方位移动机器人的角速度。令q＝[xyθ]^T表示全方位移动机器人的位姿，ν_r＝[v_xv_yω]^T表示全方位移动机器人的速度。

2)误差模型的建立：

如图1所示，假设特征点P在图像坐标系投影点p的图像坐标为(x_p,y_p)，在像素坐标系中的坐标为(u_p,v_p)，图像平面中心为(u₀,v₀)，相机的焦距为f，根据图像投影模型，可得到归一化图像坐标：

其中dx，dy表示感光芯片每单位像素的实际大小，这些参数可以通过对摄像机离线标定获得。

为了完成视觉伺服控制任务，控制器需要通过移动机器人的位置误差调整移动机器人的速度，从而使移动机器人到达期望位姿，因此引入机器人当前位姿变量ρ₁，ρ₂，

定义移动机器人的误差模型：

其中θ^*为期望角度，ρ₁ ^*，ρ₂ ^*为期望位姿变量。

对式(4)求导可得移动机器人视觉伺服误差模型为

3)全方位移动机器人运动学模型的建立：

全方位移动机器人在运动时由三个偏心方向轮驱动，俯视图如图2所示。其中点M表示全方位移动机器人的质心，点A、B、C分别表示三个偏心方向轮在运动过程中，相对于机器人本体的不动点，位于相应各个偏心方向轮竖直转向轴的中心轴上。MA、MB、MC之间的夹角均为2π/3，r表示偏心方向轮的半径，l为偏心方向轮竖直轴中心到M点的距离，即MA＝MB＝MC＝l。d表示偏心方向轮的偏心距，α表示机器人坐标系X_r轴到直线MA的角度，逆时针为正，恒为常数。β表示偏心方向轮的转向角，从直线MA到偏心方向轮轴线方向的角度，逆时针为正，表示偏心方向轮行走时的滚动角。

当全方位移动机器人由三个偏心方向轮驱动时，可得轮子的转动速度及偏转速度/>和移动机器人速度v_r之间的转换关系为:

其中分别表示三个偏心方向轮的转动速度，/>分别表示三个偏心方向轮的偏转速度，雅克比矩阵J(β_i,θ)表示如下:

结合式(5)可以得到关于轮子转动速度和偏转速度的移动机器人视觉伺服误差系统，即

其中

式(8)中J-₁ ¹(β_i)为矩阵J₁(β_i)的伪逆矩阵，

为保证视觉伺服任务的有效执行，在全方位移动机器人执行视觉伺服任务中需要满足误差约束和速度约束条件。

e_1min≤e₁≤e_1max,e_2min≤e₂≤e_2max (9)

其中，下标min表示对应变量的取值下界，下标max表示对应变量的取值上界。

本设计的目标是在满足约束条件(9)和(10)的情况下，全方位移动机器人能到达目标位置，并且在设计过程中误差e和速度v_r最终都趋向于零。

4)控制器设计：

全方位移动机器人在执行视觉伺服任务过程中，应确保它在运动过程中满足视觉可见性约束和执行器物理约束条件，模型预测控制作为一种最优控制策略，具有对约束的处理能力，因此设计视觉伺服预测控制器。

模型预测控制是以被控对象的当前状态作为初始状态，在每个采样瞬间求解有限时域开环最优控制问题，从而获得当前控制动作的一种控制形式；在每个时刻，该优化产生一个最优控制序列，并将序列中的第一个控制应用于模型。由于模型预测控制在处理系统约束和平衡多个控制目标方面的优势，因此在越来越多的实用移动机器人中得到了应用。

考虑全方位移动机器人视觉伺服误差系统(7)，取采样时间为T_s，采用欧拉离散化，可得到对应的离散时间视觉伺服误差系统：

e(k+1)＝e(k)+B(k)u(k) (11)

其中：

由于z_c是相机坐标系原点到特征点P的高度信息，因此z_c是一个常数，i＝1，2，3是控制输入。

式(11)可以写成更紧凑的形式：

e(k+1)＝f(e(k),u(k)) (12)

其中e(k+1)是误差状态，u(k)是控制输入，f(·)代表对应的映射关系。

考虑全方位移动机器人视觉伺服误差系统(11)，采用序列二次规划来最小化二次目标函数的控制输入，定义全方位移动机器人视觉伺服误差模型的目标函数为：

其中Q和R分别为状态和控制的正定加权矩阵。目标函数中的第1项表示了系统镇定到期望点的能力，第2项说明了它约束控制输入的能力。其中N_p为预测时域，N_c为控制时域，N_p和N_c会影响迭代步长，N_p值和N_c值越小，控制响应越快。e(k+i|k)表示状态量在k时刻对k+i时刻的预测值，u(k+i-1|k)表示控制输入在k时刻对k+i-1时刻的预测值。定义k时刻N_c步预测控制序列u(k)＝{u(k|k),u(k+1|k),…,u(k+N_c-1|k)}，N_p步预测状态序列为e(k)＝{e(k+1|k),e(k+2|k),…,e(k+N_p|k)}。

通过全方位移动机器人的二次目标函数(13)可以得到全方位移动机器人视觉伺服的预测模型为

e(k+i|k)＝f(e(k+i-1|k),u(k+i-1|k)) (14)

考虑视觉可见性约束(9)和执行器约束(10)，全方位移动机器人预测镇定控制器为：

其中e₀表示在初始时刻获取的误差值，u^*(k)表示优化问题(15)的最优解。

实施例：

1)实物选取，选取的全方位移动机器人的偏心方向轮半径r＝0.1m，偏心方向轮竖直轴中心到M点的距离l＝0.8m，偏心方向轮的偏心距d＝0.2m。机器人坐标系X_r轴到直线MA、MB、MC的角度分别为α₁＝π/3，α₂＝π，α₃＝-π/3。选取相机的焦距f＝6mm，图像分辨率为640×480pixels，相机坐标系原点到特征点P的高度z_c＝0.4m。

2)通讯建立，PC端采用MATLAB控制，控制器选择STM32单片机，型号为STM32F103RCT6，PC与单片机实现串口通信，单片机与驱动器实现CAN通信。建立通讯使用MATLAB发送速度等指令，控制全方位移动机器人的初始零点调整以及移动行走。

3)采样时间T_s＝0.2s，加权矩阵Q＝{0.3,0.5,5}，R＝{0.01,0.01,0.01,4,4,4}，预测时域N_p＝4，控制时域N_c＝4。

4)测得全方位移动机器人的三个偏心方向轮的初始转向角分别为β₁＝-3π/2，β₂＝3π/2，β₃＝2π/3，在世界坐标系下全方位移动机器人的初始位姿为(1.6,-5,0)，期望位姿为(0,0,0)。偏差初始值为e₀＝(-4,-12.5,0)。

全方位移动机器人视觉可见性约束和系统约束为：

[-4-12.50]≤e(k)≤[220.4]

[-50-50-50-30-30-30]^T

≤u(k)≤[505050303030]^T

5)将实际数据带回模型，进行预测控制，具体算法为：

a.令k＝0，根据预测时域N_p和控制时域N_c以及初始状态e₀，求解优化问题(15)，得到最优化控制序列u^*(k)。

b.将最优控制序列u^*(k)的第一个分量用于全方位移动机器人视觉伺服误差系统(7)，测量k时刻状态e_k。

c.令k＝k+1，返回执行b，不断循坏，使全方位移动机器人向目标点行进移动。

6)全方位移动机器人最终到达目标点，而且较好地处理了约束条件。具体运动轨迹如图3。

Claims (1)

1.一种全方位移动机器人视觉伺服系统的建模与控制方法，其特征在于，包括如下步骤：

1)全方位移动机器人描述：

在平面内设置一个全方位移动机器人，该机器人由三个偏心轮独立驱动，且在该机器人上方搭载一个单目相机；全方移动机器人在平面内运动时，全方位移动机器人的运动学方程为：

其中(x,y)表示全方位移动机器人质心在世界坐标系X_w-Y_w平面的坐标，θ表示世界坐标系到机器人坐标系的夹角，逆时针为正，v_x和v_y表示全方位移动机器人的纵向线速度和横向线速度，ω表示全方位移动机器人的角速度，令q＝[xyθ]^T表示全方位移动机器人的位姿，ν_r＝[v_xv_yω]^T表示全方位移动机器人的速度；

2)建立视觉伺服误差模型：

假设特征点P在图像坐标系投影点p的图像坐标为(x_p,y_p)，在像素坐标系中的坐标为(u_p,v_p)，图像平面中心为(u0,v0)，相机的焦距为f，根据图像投影模型，可得到归一化图像坐标：

其中dx，dy表示感光芯片每单位像素的实际大小；

为了完成视觉伺服控制任务，控制器需要通过移动机器人的位置误差调整移动机器人的速度，从而使移动机器人到达期望位姿，因此引入机器人当前位姿变量ρ₁，ρ₂，

定义全方位移动机器人的误差模型：

其中θ^*为期望角度，ρ₁ ^*，ρ₂ ^*为期望位姿变量；

对式(4)求导可得全方位移动机器人视觉伺服误差模型为：

3)建立全方位移动机器人运动学模型：

全方位移动机器人在运动时由三个偏心方向轮驱动，其中点M表示全方位移动机器人的质心，点A、B、C分别表示三个偏心方向轮在运动过程中，相对于机器人本体的不动点，位于相应各个偏心方向轮竖直转向轴的中心轴上；MA、MB、MC之间的夹角均为2π/3，r表示偏心方向轮的半径，l为偏心方向轮竖直轴中心到M点的距离，即MA＝MB＝MC＝l；d表示偏心方向轮的偏心距，α表示机器人坐标系X_r轴到直线MA的角度，逆时针为正，恒为常数；β表示偏心方向轮的转向角，从直线MA到偏心方向轮轴线方向的角度，逆时针为正，表示偏心方向轮行走时的滚动角；

当全方位移动机器人由三个偏心方向轮驱动时，可得轮子的转动速度及偏转速度/>与移动机器人速度v_r之间的转换关系为：

其中分别表示三个偏心方向轮的转动速度，/>分别表示三个偏心方向轮的偏转速度，雅克比矩阵J(β_i,θ)表示如下：

结合式(5)可以得到关于轮子转动速度和偏转速度的移动机器人视觉伺服误差系统，即

其中：

式(8)中为矩阵，J₁(β_i)的伪逆矩阵，/>

为保证视觉伺服任务的有效执行，在全方位移动机器人执行视觉伺服任务中需要满足误差约束和速度约束条件；

e_1min≤e₁≤e_1max, e_2min≤e₂≤e_2max (9)

其中，下标min表示对应变量的取值下界，下标max表示对应变量的取值上界；

4)视觉伺服预测控制器设计：

考虑全方位移动机器人视觉伺服误差系统(7)，取采样时间为T_s，采用欧拉离散化，可得到对应的离散时间视觉伺服误差系统：

e(k+1)＝e(k)+B(k)u(k) (11)

其中：

由于z_c是相机坐标系原点到特征点P的高度信息，因此z_c是一个常数，i＝1,2,3是控制输入；

式(11)可以写成更紧凑的形式：

e(k+1)＝f(e(k),u(k)) (12)

其中e(k+1)是误差状态，u(k)是控制输入，f(·)代表对应的映射关系；

考虑全方位移动机器人视觉伺服误差系统(11)，采用序列二次规划来最小化二次目标函数的控制输入，定义全方位移动机器人视觉伺服误差模型的目标函数为：

其中Q和R分别为状态和控制的正定加权矩阵，目标函数中的第1项表示了系统镇定到期望点的能力，第2项说明了它约束控制输入的能力；其中N_p为预测时域，N_c为控制时域，N_p和N_c会影响迭代步长，N_p值和N_c值越小，控制响应越快；e(k+i|k)表示状态量在k时刻对k+i时刻的预测值，u(k+i-1|k)表示控制输入在k时刻对k+i-1时刻的预测值；定义k时刻N_c步预测控制序列u(k)＝{u(k|k),u(k+1|k),…,u(k+N_c-1|k)}，N_p步预测状态序列为e(k)＝{e(k+1|k),e(k+2|k),…,e(k+N_p|k)}；

通过全方位移动机器人的二次目标函数(13)可以得到全方位移动机器人视觉伺服的预测模型为：

e(k+i|k)＝f(e(k+i-1|k),u(k+i-1|k)) (14)

考虑视觉可见性约束(9)和执行器约束(10)，全方位移动机器人预测镇定控制器为：

s.t.e(k|k)＝e₀

e(k+i|k)＝f(e(k+i-1|k),

u(k+i-1|k)),i∈[1,N_p](15b)

e_min≤e(k+i|k)≤e_max,i∈[0,N_p]

u_min≤u(k+i|k)≤u_max,i∈[0,N_p]

其中e₀表示在初始时刻获取的误差值，u^*(k)表示优化问题的最优解；

5)全方位移动机器人向目标点行进移动，全方位移动机器人最终到达目标点。