本发明涉及服务机器人的控制，尤其涉及一种适应交互力的服务机器人轨迹和速度同时安全的控制方法。
背景技术：
1、由于高龄人口和下肢残疾者无法完成日常独立生活，给家庭照顾人员和社会带来沉重负担，坐式服务机器人可以代替老年人和下肢残疾者的步行功能，只需坐在机器人上利用自己健康的上肢就可以完成简单的日常生活活动，给老年人和下肢残障者带来了一种全新的生活范式，因此服务机器人的研究得到了学者们的广泛关注。由于使用者操作各种生活动作过程中，会和机器人之间产生交互力，这种人机交互力严重干扰机器人的跟踪运动，甚至产生不安全的运动轨迹和运动速度，威胁使用者的安全。因此，解决服务机器人运动轨迹和运动速度同时安全的跟踪控制问题具有重要意义。
2、关于服务机器人跟踪控制已经取得了一些研究成果，然而这些成果都忽略了人机合作操作生活动作过程中产生的不确定交互力，更重要的是现有成果没有解决人机系统运动轨迹和运动速度同时安全问题，不仅导致跟踪精度不理想，而且过大的轨迹跟踪误差和速度跟踪误差都会威胁使用者的安全。到目前为止，还没有关于适应人机交互力的轨迹和速度同时安全的控制方法。因此，研究如何抑制人机不确定交互力对跟踪精度的影响，并解决轨迹和速度同时安全的控制问题对提高服务机器人性能具有重要意义。
技术实现思路
1、本发明就是针对现有技术存在的缺陷，提供一种适应交互力的服务机器人轨迹和速度同时安全的控制方法。
2、为实现上述目的，本发明采用如下技术方案，适应交互力的服务机器人轨迹和速度同时安全的控制方法，包括：
3、1)基于服务机器人动力学模型，通过分解广义输入力，建立不确定交互力的人机系统动力学模型，并设计不确定交互力观测器，同时构造自适应控制器抑制交互力对机器人运动的影响，保证人机系统轨迹跟踪和速度跟踪的稳定性；
4、2)建立人机系统安全轨迹和安全速度的约束条件，根据约束条件分别设计安全轨迹与安全速度的控制障碍函数，并将轨迹和速度同时安全的控制器设计转化为求解二次规划问题，进而得到服务机器人稳定跟踪的最优安全控制器，保障人机系统具有同时安全的运动轨迹和运动速度。
5、步骤如下：
6、步骤1)通过分解广义输入力，建立不确定交互力的人机系统动力学模型，并设计不确定交互力观测器，同时构造自适应控制器抑制交互力对机器人运动的影响，保证人机系统轨迹跟踪和速度跟踪的稳定性，其特征在于：服务机器人动力学模型描述如下：
7、
8、其中
9、
10、u(t)＝[f1 f2 f3]t
11、m表示服务机器人的质量，m表示使用者的质量；x(t)表示服务机器人实际运动轨迹；i0表示服务机器人转动惯量，r0表示系统中心与重心之间的距离。u(t)表示广义输入力，f1，f2和f3分别表示各个全向轮的广义输入力。m0，b(θ)表示系数矩阵，θ表示水平轴和机器人中心与第一个轮子中心连线间的夹角，l表示机器人中心到各个轮子的距离。
12、实际应用中，使用者操作生活动作时将产生不确定交互力，并从控制通道进入系统影响服务机器人的跟踪性能，为此将广义输入力u(t)分解如下：
13、
14、其中u0(t)表示服务机器人的控制输入力，u1(t)表示人机系统的耗散力。
15、令表示人机系统不确定交互力，则式(2)转化为如下形式：
16、
17、其中d(t)由物理含义可知有界，即||d(t)||≤ρ，ρ为正常数。
18、设计人机系统不确定交互力的观测器为：
19、
20、其中d(t)＝[d1(t)d2(t)d3(t)]t分别为x，y轴和旋转角方向的不确定交互力，为d(t)的估计值，为估计误差。z为中间辅助变量，p为观测器增益矩阵。
21、利用式(4)，对求导可得：
22、
23、将式(3)-式(4)代入式(5)可得：
24、
25、对式(6)求导整理可得：
26、
27、令x1(t)＝x(t)，将式(3)转化为如下形式：
28、
29、其中x1(t)＝[x11(t) x12(t) x13(t)]t分别为x，y轴和旋转角方向的实际运动轨迹；x2(t)＝[x21(t) x22(t) x23(t)]t分别为x，y轴和旋转角方向的实际运动速度。
30、设xd(t)表示指定的运动轨迹，则轨迹跟踪误差e1(t)为：
31、e1(t)＝x1(t)-xd(t) (9)
32、其中e1(t)＝[e11(t) e12(t) e13(t)]t分别为x，y轴和旋转角方向的轨迹跟踪误差。xd(t)＝[xd1(t) xd2(t) xd3(t)]t分别为x，y轴和旋转角方向的指定运动轨迹。
33、定义虚拟变量α1(t)为：
34、
35、其中α1(t)＝[α11(t)α12(t)α13(t)]t分别为x，y轴和旋转角方向的虚拟变量，λ＞0表示调节参数。
36、设速度跟踪误差e2(t)为：
37、
38、其中e2(t)＝[e21(t) e22(t) e23(t)]t分别为x，y轴和旋转角方向的速度跟踪误差。
39、设计自适应控制器u0(t)如下：
40、
41、其中c＞0表示控制器调节参数。
42、为了抑制人机不确定交互力对服务机器人运动的影响，交互力观测的自适应律设计如下：
43、
44、其中r＞0为自适应律调节参数。
45、对式(11)求导可得：
46、
47、进一步结合式(9)和式(11)可得：
48、
49、设计lyapunov函数v(t)如下：
50、
51、对式(16)求导并将式(14)-式(15)代入得：
52、
53、将式(7)与式(12)代入式(17)可得：
54、
55、对式(13)求导代入式(18)可得：
56、
57、由式(19)可知，设计的自适应控制器(12)抑制了交互力对机器人运动的影响，使人机系统轨迹跟踪和速度跟踪实现了渐近稳定，同时使不确定交互力观测误差达到渐近稳定，设计的观测器(4)获得了人机系统不确定交互力
58、步骤2)建立人机系统安全轨迹和安全速度的约束条件，根据约束条件分别设计安全轨迹与安全速度的控制障碍函数，并将轨迹和速度同时安全的控制器设计转化为求解二次规划问题，进而得到服务机器人稳定跟踪的最优安全控制器。其特征在于：将服务机器人动力学模型(1)转化为非线性仿射系统如下：
59、
60、其中
61、服务机器人与障碍物保持一定距离的安全轨迹约束条件设计如下：
62、x1i(t)-xki(t)≥di (21)
63、其中xk(t)＝[xk1(t) xk2(t) xk3(t)]t分别表示障碍物x，y轴和旋转角方向的位置，且d＝[d1 d2 d3]t，di≥0，i＝1,2,3。
64、为了保证服务机器人具有安全运动轨迹，设计轨迹控制障碍函数如下：
65、
66、由式(22)可知hx(x)≥0，并对hx(x)沿x(t)求偏导得：
67、
68、对hx(x)沿g(x)求lie导数，有成立，于是对轨迹控制障碍函数hx(x)再进行微分计算，得到从而定义安全轨迹的控制输入集合a为：
69、a＝{u(t)∈u|l2fhx(x)+lglfhx(x)u(t)+κ[hx(x)]…0} (24)
70、其中为允许的控制输入集合，并且κ[hx(x)]＝k0hx(x)+k1lfhx(x)，k0＞0和k1＞0表示调节参数，lfhx(x)表示hx(x)沿f(x)的lie导数，l2fhx(x)表示lfhx(x)沿f(x)的lie导数。
71、服务机器人各轴安全速度的约束条件设计如下：
72、|x2i(t)|≤vi (25)
73、其中vi＞0表示x，y轴和旋转角方向的安全速度值。
74、为了保证服务机器人具有安全运动速度，设计速度控制障碍函数如下：
75、
76、由式(25)可得hv(x)≥0，并对hv(x)沿x(t)求偏导得：
77、
78、对hv(x)沿g(x)求lie导数，得到于是定义安全速度的控制输入集合b为：
79、b＝{u(t)∈u|lfhv(x)+lghv(x)u(t)+k3hv(x)…0} (28)
80、其中k3＞0表示调节参数，lfhv(x)表示hv(x)沿f(x)的lie导数。
81、根据服务机器人轨迹控制障碍函数与速度控制障碍函数，将轨迹和速度同时安全的控制器设计转化为求解如下的二次规划问题：
82、
83、其中h表示给定的正定矩阵，μ＞0为惩罚因子，δ为松弛变量。
84、通过求解上述优化问题式(29)，可得服务机器人运动轨迹避开障碍物且具有速度约束的最优控制器u*(t)，获得了人机系统同时安全的运动轨迹和运动速度。
85、步骤3)基于msp430系列单片机将输出pwm信号提供给电机驱动模块，使服务机器人稳定跟踪指定的运动轨迹，并保证人机系统具有同时安全的运动轨迹和运动速度。其特征在于：以msp430系列单片机为主控制器，主控制器的输入接电机测速模块、输出接电机驱动模块；电机驱动电路与直流电机相连；电源系统给各个电气设备供电。主控制器控制方法为读取电机编码器的反馈信号与主控制器给定的控制命令信号x1(t)和并计算得出误差信号。根据误差信号，主控制器按照预定的控制算法计算出电机的控制量，送给电机驱动模块，电机转动带动轮子维持自身平衡及按指定方式运动。
86、与现有技术相比本发明有益效果。
87、本发明适应人机不确定交互力的服务机器人轨迹和速度同时安全的跟踪控制方法，具有如下优点：建立了具有不确定交互力的人机系统动力学模型，并设计观测器对交互力进行估计；设计了自适应控制器抑制交互力对服务机器人运动的影响并保证了跟踪误差系统的稳定性；构建了安全轨迹和安全速度约束条件并分别设计控制障碍函数，将轨迹和速度同时安全的控制器设计转化为求解二次规划问题，获得了人机系统最优安全控制器，保障了人机系统的安全性。
1.适应交互力的服务机器人轨迹和速度同时安全的控制方法，其特征在于：包括步骤：
2.根据权利要求1所述的适应交互力的服务机器人轨迹和速度同时安全的控制方法，其特征在于：步骤s1具体分为：
3.根据权利要求1所述的适应交互力的服务机器人轨迹和速度同时安全的控制方法，其特征在于：步骤s2具体分为：
4.根据权利要求1所述的适应交互力的服务机器人轨迹和速度同时安全的控制方法，其特征在于：还包括：基于msp430系列单片机将输出pwm信号提供给电机驱动模块，使服务机器人稳定跟踪指定的运动轨迹，并保证人机系统具有同时安全的运动轨迹和运动速度；具体包括：以msp430系列单片机为主控制器，主控制器的输入接电机测速模块、输出接电机驱动模块；电机驱动电路与直流电机相连；电源系统给各个电气设备供电；主控制器控制方法为读取电机编码器的反馈信号与主控制器给定的控制命令信号x1(t)和并计算得出误差信号；根据误差信号，主控制器按照预定的控制算法计算出电机的控制量，送给电机驱动模块，电机转动带动轮子维持自身平衡及按指定方式运动。
