一种基于评估函数的机器人轨迹跟踪策略动态优化方法

1.本发明涉及机器人运动控制技术领域，特别是一种基于评估函数的机器人轨迹跟踪策略动态优化方法。


背景技术：

2.在快速发展的智能机器人相关技术中，机器人的运动控制是移动机器人控制系统中最基本的问题，因为所有任务最终都是通过机器人的运动来实现的。根据控制目的不同，运动控制可以分为点镇定、轨迹跟踪和路径追踪三类，其中轨迹跟踪的研究方法主要有状态反馈控制法、滑模控制法、反演法控制法、自适应控制法和智能控制法五种。
3.状态反馈控制法是指将系统的每一状态变量乘以相应的反馈系数，反馈到输入端与参考输入相加，其和作为被控系统的控制信号；滑模控制法不需要知道系统的精确模型，对满足匹配条件时，系统对外界扰动以及参数变动具有很好的鲁棒性；反演法控制法是一种系统化的控制律设计思想，对线性和非线性系统都适用，能够方便的解决各种情况下的控制律求解问题；自适应控制法和智能控制法通常都具有学习能力和非线性映射的能力，为解决机器人系统的控制问题提供了新的思路。
4.同时，状态反馈控制法在处理参数不确定或者参数摄动的模型时，效果不如其它方法；滑模控制法对参数摄动具有良好的调节能力，但在滑模面切换时会出现抖动的情况；反演法控制法中的微分项膨胀现象，容易造成复杂度爆炸的问题；而自适应控制法需要在线对模型参数做出估计，这种估计会受到测量精度的影响，从而影响整体控制效果；智能控制法无需考虑对象的模型，但也由此产生了控制效果一般且计算量较大的问题。
5.此外，上述常用控制方法在计算控制量时，往往都只考虑了机器人在参考点处的瞬时状态与参考状态的误差，对于机器人的运动趋势没有多加考虑，使得在实际应用中会出现各种意外状况。


技术实现要素：

6.本发明的目的在于提供一种能够减小机器人的实际轨迹与设定轨迹之间的偏差、提高机器人在精密场所中使用的安全性、计算量小、具有处理参数不确定性的能力、对于模型精度的要求低、适应性强的机器人轨迹跟踪策略动态优化方法。
7.实现本发明目的的技术解决方案为：一种基于评估函数的机器人轨迹跟踪策略动态优化方法，包括以下步骤：
8.步骤1、分析机器人的机械结构，并根据实际的机械结构，建立机器人的运动学或动力学的相关模型；
9.步骤2、利用机器人相关状态变量，设计机器人的轨迹追踪效果评估函数；
10.步骤3、根据经验设置控制算法计算出的控制量相关参数的初始值；
11.步骤4、在机器人运行时，采集所需要的数据传递给机器人的轨迹跟踪效果评估函数；
12.步骤5、利用机器人的轨迹跟踪效果评估函数的结果及变化趋势，在线地对控制量进行修正；
13.步骤6、在机器人运动过程中重复步骤4和步骤5，实现对控制量的动态优化。
14.进一步地，步骤1所述的分析机器人的机械结构，并根据实际的机械结构，建立机器人的运动学或动力学的相关模型，具体如下：
15.以机器人中心位置为基点a，建立机器人随动右手坐标系axyz以及机器人自身右手坐标系axyz，规定逆时针旋转方向为正方向，v
a
表示基点a的速度大小，θ表示a点速度与世界坐标系的ax轴的夹角，ω表示机器人绕a点旋转的角速度，表示机器人朝向与ax轴的夹角，则机器人的运动学模型表示为：
[0016][0017]
其中，x
a
表示机器人中心沿x轴运动的距离，和v
ax
表示机器人中心沿x轴的线速度；y
a
表示机器人中心沿x轴运动的距离，和v
ay
表示机器人中心沿y轴的线速度；表示机器人朝向与ax轴的夹角，和ω都表示机器人绕a点旋转的角速度；
[0018]
令机器人的参考坐标为参考运动模型表示为：
[0019][0020]
其中，机器人参考轨迹在t时刻的全局坐标为和v
rx
都表示机器人参考点处的沿x轴的速度，v
r
表示参考点处机器人的平动速度，θ
r
表示参考点处机器人的平动方向；和v
ry
表示机器人参考点处的沿y轴的速度；和ω
r
表示参考点处机器人的绕轴旋转速度；
[0021]
得到位姿的误差方程如下：
[0022][0023]
其中，机器人参考轨迹在t时刻的全局坐标为此时机器人实际的坐标为位姿误差为
[0024]
利用状态反馈控制法，得出机器人运动控制律如下：
[0025][0026]
其中，v
ax
表示机器人中心沿x轴的线速度，v
ay
表示机器人中心沿y轴的线速度；k
v1
、k
v2
分别为x、y方向平动系数，k
ω
为转动系数，且k
v1
＞0，k
v2
＞0，k
ω
＞0。
[0027]
进一步地，步骤2所述的利用机器人相关状态变量，设计机器人的轨迹追踪效果评估函数，具体如下：
[0028]
设计机器人的轨迹追踪效果评估函数cost为：
[0029]
cost＝w1d1+w2d2[0030]
其中d1表示机器人坐标与参考点处给定的运动方向所在直线之间的距离，d2表示机器人实际坐标与参考点坐标之间的距离，w1，w2为权重系数；
[0031]
d1，d2的表达式如下：
[0032][0033][0034]
其中，θ
r
表示参考点处机器人的平动方向，表示机器人的参考坐标，θ表示实际的机器人的平动方向，表示机器人的实际坐标。
[0035]
进一步地，步骤3所述的根据经验设置控制算法计算出的控制量相关参数的初始值，具体如下：
[0036]
结合机器人实际使用中的速度限制以及轨迹规划算法规划出的路径参考点之间的距离参数，对机器人的轨迹跟踪状况进行计算机仿真，确定控制量相关参数k
v1
、k
v2
以及k
ω
的初始值。
[0037]
进一步地，步骤4所述的在机器人运行时，采集所需要的数据传递给机器人的轨迹跟踪效果评估函数，具体如下：
[0038]
在机器人实际运行时，利用各类传感器采集机器人的状态信息，并将数据实时传递给处理单元；在进行多传感器数据融合时，利用时间戳结合卡尔曼滤波的方式，进行各传感器间数据的同步。
[0039]
进一步地，步骤5所述的利用机器人的轨迹跟踪效果评估函数的结果及变化趋势，在线地对控制量进行修正，具体如下：
[0040]
对步骤3中给出的机器人的轨迹跟踪效果评估函数，分别对x和y求偏导，并将结果用于对控制量的修正，如下式所示：
[0041][0042]
本发明与现有技术相比，其显著优点在于：(1)减小了机器人的实际轨迹与设定轨迹之间的偏差，提高了机器人在精密场所中使用的安全性；(2)具有计算量小的优势，同时又具有处理参数不确定性的能力，降低了对于模型精度的要求，更加适合实际的应用。
附图说明
[0043]
图1是本发明基于评估函数的机器人轨迹跟踪策略动态优化方法的流程示意图。
[0044]
图2是本发明实施例中建立的机器人坐标系示意图。
具体实施方式
[0045]
下面结合附图和具体实例对本发明作进一步的详细说明。
[0046]
结合图1～2，本发明一种基于评估函数的机器人轨迹跟踪策略的动态优化方法，包括以下步骤：
[0047]
步骤1、分析机器人的机械结构，并根据实际的机械结构，建立机器人的运动学或动力学的相关模型，具体如下：
[0048]
以机器人中心位置为基点a，建立机器人随动右手坐标系axyz以及机器人自身右手坐标系axyz，规定逆时针旋转方向为正方向，v
a
表示基点a的速度大小，θ表示a点速度与世界坐标系的ax轴的夹角，ω表示机器人绕a点旋转的角速度，表示机器人朝向与ax轴的夹角，则机器人的运动学模型表示为：
[0049][0050]
其中，x
a
表示机器人中心沿x轴运动的距离，和v
ax
表示机器人中心沿x轴的线速度；y
a
表示机器人中心沿x轴运动的距离，和v
ay
表示机器人中心沿y轴的线速度；表示机器人朝向与ax轴的夹角，和ω都表示机器人绕a点旋转的角速度；
[0051]
令机器人的参考坐标为参考运动模型表示为：
[0052][0053]
其中，机器人参考轨迹在t时刻的全局坐标为和v
rx
都表示机器人参考点处的沿x轴的速度，v
r
表示参考点处机器人的平动速度，θ
r
表示参考点处机器人的平动方向；和v
ry
表示机器人参考点处的沿y轴的速度；和ω
r
表示参考点处机器人的绕轴旋转速度；
[0054]
得到位姿的误差方程如下：
[0055][0056]
其中，机器人参考轨迹在t时刻的全局坐标为此时机器人实际的坐标为位姿误差为
[0057]
利用状态反馈控制法，得出机器人运动控制律如下：
[0058][0059]
其中，v
ax
表示机器人中心沿x轴的线速度，v
ay
表示机器人中心沿y轴的线速度；k
v1
、
k
v2
分别为x、y方向平动系数，k
ω
为转动系数，且k
v1
＞0，k
v2
＞0，k
ω
＞0。
[0060]
步骤2、利用机器人相关状态变量，设计机器人的轨迹追踪效果评估函数，具体如下：
[0061]
设计机器人的轨迹追踪效果评估函数cost为：
[0062]
cost＝w1d1+w2d2[0063]
其中d1表示机器人坐标与参考点处给定的运动方向所在直线之间的距离，d2表示机器人实际坐标与参考点坐标之间的距离，w1，w2为权重系数；
[0064]
d1，d2的表达式如下：
[0065][0066][0067]
其中，θ
r
表示参考点处机器人的平动方向，表示机器人的参考坐标，θ表示实际的机器人的平动方向，表示机器人的实际坐标。
[0068]
步骤3、根据经验设置控制算法计算出的控制量相关参数的初始值，具体如下：
[0069]
结合机器人实际使用中的速度限制以及轨迹规划算法规划出的路径参考点之间的距离参数，对机器人的轨迹跟踪状况进行计算机仿真，确定控制量相关参数k
v1
、k
v2
以及k
ω
的初始值。
[0070]
步骤4、在机器人运行时，采集所需要的数据传递给机器人的轨迹跟踪效果评估函数，具体如下：
[0071]
在机器人实际运行时，利用各类传感器采集机器人的状态信息，并将数据实时传递给处理单元；在进行多传感器数据融合时，利用时间戳结合卡尔曼滤波的方式，进行各传感器间数据的同步。
[0072]
步骤5、利用机器人的轨迹跟踪效果评估函数的结果及其变化趋势，在线地对控制量进行修正，具体如下：
[0073]
对步骤3中给出的机器人的轨迹跟踪效果评估函数，分别对x和y求偏导，并将结果用于对控制量的修正，如下式所示：
[0074][0075]
步骤6、在机器人运动过程中重复步骤4和步骤5，实现对控制量的动态优化。
[0076]
实施例1
[0077]
如图1所示，下面结合轮式移动机器人在室外变电站场景中的实施实例，对个步骤的实现做出说明。
[0078]
步骤1、分析机器人的机械结构，并根据实际的机械结构，建立机器人的运动学或动力学的相关模型，具体如下：
[0079]
如图2所示，设定a为机器人中心基点，v
a
表示基点a的速度大小，θ表示a点速度与
ax轴的夹角，ω表示机器人绕a点旋转的角速度，表示机器人朝向与ax轴的夹角，则机器人的运动学模型可以表示为：
[0080][0081]
令机器人的参考坐标为其参考运动模型可以表示为：
[0082][0083]
其中θ
r
表示参考点处机器人的平动方向，v
r
表示参考点处机器人的平动速度，ω
r
表示参考点处机器人的绕轴旋转速度，则可以得到位姿的误差方程如下：
[0084][0085]
利用状态反馈控制法，的出机器人运动控制律如下：
[0086][0087]
其中k
v1
＞0，k
v2
＞0，k
ω
＞0；
[0088]
接着可以通过构造李雅普诺夫函数验证控制律的稳定性，完成状态反馈轨迹跟踪控制律的设计；
[0089]
进一步地，本步骤中可使用的控制律构造方式不局限于上述状态反馈控制律，所有能够对机器人的运动控制律进行控制的设计方法都应包括在内。
[0090]
步骤2、利用机器人相关状态变量，设计机器人的轨迹追踪效果评估函数，具体如下：
[0091]
设计机器人的轨迹追踪效果评估函数为：
[0092]
cost＝w1d1+w2d2[0093]
其中d1表示机器人坐标与参考点处给定的运动方向所在直线之间的距离，d2表示机器人实际坐标与参考点坐标之间的距离，w1，w2为系数；
[0094]
d1，d2的表达式如下：
[0095][0096][0097]
其中θ
r
表示参考点处机器人的平动方向，表示机器人的参考坐标，θ表示实际的机器人的平动方向，表示机器人的实际坐标；
[0098]
本评估函数不仅包含了某个时刻机器人对参考点的跟踪情况d2，还包含了在经过
参考点之后的运动过程中，机器人整体轨迹与参考轨迹的偏离程度d1，可以很好的对整体的跟踪效果进行评价和预测。
[0099]
步骤3、根据经验设置控制算法计算出的控制量相关参数的初始值，具体如下：
[0100]
结合机器人实际使用中的速度限制以及轨迹规划算法规划出的路径参考点之间的距离的等参数，对机器人的轨迹跟踪状况进行计算机仿真，选用一组合适的k
v1
，k
v2
以及k
ω
。
[0101]
步骤4、在机器人运行时，采集所需要的数据传递给机器人的轨迹根据效果评估函数，具体如下：
[0102]
在机器人实际运行时，利用编码器、激光雷达以及视觉传感器在内的各类传感器，实现机器人状态信息的采集，并将数据实时的传递给处理单元；
[0103]
进一步地，在采集机器人数据，尤其是机器人的运动数据时，考虑目前多数移动机器人都采用四轮独立驱动独立转向的结构，尤其需要注意多个电机传感器的信息同步问题，本发明利用标志量与数据缓存的组合，保证了采集多电机运动信息时数据更新的原子性，从而保证了运动数据更新的同步性；
[0104]
进一步地，在进行多传感器数据融合时，利用时间戳的思想，结合卡尔曼滤波的方式，极大的提升了各传感器间数据的同步性。
[0105]
步骤5：利用评估函数的结果及其变化趋势在线地对控制量进行修正，具体如下：
[0106]
对步骤3中给出的轨迹跟踪效果评估函数，分别对x和y求偏导，并将结果用于对控制量的修正，如下式所示：
[0107][0108]
步骤6：在机器人运动过程中重复步骤4
‑
5，实现对控制量的动态优化。
[0109]
本发明设计的评估函数考虑了机器人实际的运动趋势，使得机器人的运动能够更好的在实际设定的限制条件内，尤其在面对例如变电站等对机器人轨迹有严格限制的场合中，可以通过对机器人的运动趋势判断，减小机器人的实际轨迹与参考轨迹间的偏差，大大提高了在某些精密场所中使用机器人的安全性；
[0110]
本发明给出的轨迹跟踪策略在线优化方法，相较于自适应算法具有计算量小的优势，同时相较于传统状态反馈算法又具有了一定的处理参数不确定性的能力，降低了对于模型精度的要求，更加适合实际的应用。