CN107309872A - 一种带有机械臂的飞行机器人及其控制方法 - Google Patents
一种带有机械臂的飞行机器人及其控制方法 Download PDFInfo
- Publication number
- CN107309872A CN107309872A CN201710320306.9A CN201710320306A CN107309872A CN 107309872 A CN107309872 A CN 107309872A CN 201710320306 A CN201710320306 A CN 201710320306A CN 107309872 A CN107309872 A CN 107309872A
- Authority
- CN
- China
- Prior art keywords
- mtd
- mrow
- msub
- mtr
- mfrac
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1679—Programme controls characterised by the tasks executed
- B25J9/1689—Teleoperation
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J11/00—Manipulators not otherwise provided for
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1612—Programme controls characterised by the hand, wrist, grip control
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- General Health & Medical Sciences (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种带有机械臂的飞行机器人及其控制方法,飞行机器人包括多旋翼无人机飞行平台、六自由度机械臂和操作端控制***,六自由度机械臂安装在多旋翼无人机飞行平台下方。将无人机与机械臂相结合,拓展其任务执行能力;对于机械臂,将其运动范围从地基拓宽到空中,同时对其控制方式加以改进,新的控制方式可以实现机械臂跟随人体手臂运动的功能,使其能够更加自然灵活地执行操作者的想法。该机器人及其控制方法扩大了无人机和机械臂作业范围,实现对机械臂的远程控制;机械臂随动人体手臂位姿,避免了复杂的模型检测与运算,响应效果更好;并且在尽可能保留舵机快速响应特性的同时减缓了舵机快速响应时对机械臂和飞行平台带来的冲击问题。
Description
技术领域
本发明属于作业型无人机技术领域,具体是一种带有机械臂的飞行机器人及其控制方法。
背景技术
目前多旋翼无人机技术自身已发展得较为成熟,其在日常生活中的应用一般需结合其它部 件来完成,如和云台拍摄设备结合实现高空侦查拍摄功能、和喷洒装置结合实现农田植保的功能。 目前也有相关学者和研究人员提出将旋翼无人机和手爪或机械臂结合来完成更复杂的功能,早期 的这类飞行机器人多是将旋翼无人机和手爪相结合,但是这类机器人***的作业能力十分有限, 并且由于没有机械臂***,作业能力受到很大限制。于是有学者提出飞行器结合多自由度机械臂 的概念,但机械臂***自身不具备主动作业的功能,目前在这一领域较先进的机械臂控制方式是 通过外界视觉捕捉***辅助定位来实现动态抓取目标,这样的控制方法很大程度上依赖于视觉捕 捉功能,目前视觉捕捉技术发展不够成熟,且对于没有视觉捕捉***的外界复杂环境这样的控制 方式会束手无策,因而此类控制方式可靠程度不够高。
目前常见的机械臂多用于工业生产,旨在执行高精度、高复杂性的装配工作,从而替代人 工,提高生产效率。这类机械臂一般造价高昂、体积较大,用途相对固定,运动范围有限,运动 方式只能由预先编制的程序控制,难以动态响应外部的运动指令。与工业机械臂相对,另一类小 型机械臂立足于桌面级应用,被用于教学演示、产品原型设计、科研验证等方面,这类机械臂大 多采用步进电机作为驱动,步进电机带来较好的驱动效果的同时也因体积和重量较大使得机械臂 ***使用范围受到限制,同时这类机械臂***也有响应速度与力度有限、响应方式不灵活的局限 性,在使用时需要与电脑和电源连接以获得运动指令。不管是工业级机械臂或者小型机械臂在人 机交互方面均未能实现较理想的效果,即目前的机械臂技术一般只能按照预先编制的程序指令来 控制,这种控制方式不够自然和灵活。
目前市面上销售的舵机控制方式即由使用者发送角度指令,而后舵机快速响应至目标位置, 一般情况下无需操作者进行干预,但舵机快速响应的特性决定了其在响应时的角加速度很大,因 此会对耦合装置造成较大的冲击,对于扭力较大的舵机这种现象尤为明显。
发明内容
本发明要解决的技术问题是提供一种带有机械臂的飞行机器人及其控制方法,该机器人及 其控制方法能够扩大无人机和机械臂作业范围,实现对机械臂的远程控制;机械臂随动人体手臂 位姿,避免了复杂的模型检测与运算,响应效果更好;并且在尽可能保留舵机快速响应特性的同 时减缓舵机快速响应时的冲击问题。
本发明基本思路是将小型机械臂***装配在多旋翼无人机下方,其上载有无线通讯设备来 接收操作端的指令。在操作端的人体姿态采集装置捕捉人体关节与手势信息,然后经过程序运算 处理,即时计算出机械臂的运动指令并发出,从而实现在无线信号可达范围内的三维灵活作业。
为实现上述发明目的,本发明采用以下技术方案:
一种带有机械臂的飞行机器人,包括多旋翼无人机飞行平台、六自由度机械臂和操作端控制 ***,所述六自由度机械臂安装在多旋翼无人机飞行平台下方。
进一步的,所述多旋翼无人机飞行平台,包括六轴无人机、飞控***、GPS模块、电池组、 图像采集***,所述飞控***、GPS模块、电池组和图像采集***位于六轴无人机上方。
另一种改进,所述六自由度机械臂包括底座、肘部和腕部,所述底座和肘部之间采用大臂连 接,同时在底座和肘部之间设置与大臂平行的连杆,底座、大臂、连杆和肘部组成四连杆机构; 所述肘部和腕部之间采用小臂连接;所述六自由度机械臂通过与飞行器接合结构安装到多旋翼无 人机飞行平台下方。
更进一步,所述底座上安装有三个舵机,分别为第一、二、三舵机;所述第一舵机位于上方, 所述第二、三舵机位于第一舵机下方,并分列于第一舵机两侧;第一舵机用于驱动机械臂和飞行 器平台之间的平面内转动,第二舵机用于驱动大臂相对于底座的转动,第三舵机用于驱动小臂相 对于大臂的转动;所述大臂呈工字型,所述大臂上固接有第一、二、三法兰轴承,大臂侧面开孔 以便于舵机电源及信号走线;所述肘部是由四片板材和舵机结合而形成的立体结构;所述小臂由 前、后连接件和圆管组成;所述连杆包括第二连杆和第三连杆;所述第二连杆头部短轴与第一法 兰轴承内径装配实现大臂和第二连杆的相对转动,第二连杆尾部开孔与第三连杆铰接实现相互转 动;所述腕部包括第一部分和第二部分;所述第一部分的左挡板开孔装配有法兰轴承,后挡板的 四个开孔与小臂的前端连接件上的孔利用螺栓螺母固接,右挡板安装有第五舵机,舵机转轴和左 挡板上的法兰轴承转轴共轴;所述第二部分的左挡板固接有短轴,与第一部分左挡板上法兰轴承 内径配合,前侧挡板安装有第六舵机,右侧挡板的四个孔与第一部分右侧挡板上的舵机舵盘利用 螺栓螺母固接。
再进一步,所述底座与大臂间连接的实现方式是:第二舵机舵盘处的四个圆形孔和大臂上部 左侧的四个螺纹孔用螺栓螺母固接,第三舵机舵盘处四个圆形孔和第二连杆头部的四个螺纹孔用 螺栓螺母固接。通过这样的连接方式,第二舵机可直接驱动大臂相对于底座绕第二舵机的转轴转 动,第三舵机的转动将直接驱动第二连杆绕其转轴转动,而不对大臂产生影响。
另一种改进,所述操作端控制***包括Kinect和Leap Motion设备;所述LeapMotion用于 检测手掌的运动并定位人手的各主要关节;所述Kinect用于实现对其可视范围内物体深度数据 的获取,对人体全身主要关节进行捕捉和定位。
一种带有机械臂的飞行机器人的控制方法,包括基于Kinect的正向控制方式、基于Leap Motion的逆向控制方式和七段S曲线舵机平滑运动控制方法;所述基于Kinect的正向控制方式, 通过Kinect捕获的手臂关节空间坐标求解出各个关节间的角度,通过将求解得到的各个关节的 角度映射到机械臂的关节间夹角实现对其控制。
进一步的,所述基于Leap Motion的逆向控制方式,通过Leap Motion获取手掌的6自由度 信息:3个位移量、3个姿态角;利用这6自由度信息作为机械臂末端关节位置姿态条件,逆向 求解出各个关节之间的夹角;采用DH法确定各关节坐标系;将相邻坐标系i-1和i的齐次变换 记为Ti,如以T1表示坐标系1相对于坐标系0的位置姿态,Ti由参数ai、αi、di、θi决定:
由此得到从机械臂末端关节坐标系6相对于机械臂底座坐标系0的总变换Tall为:
Tall=T1T2T3T4T5T6 (13)
设更新间隔时间为Δt,Δt间隔内程序检测到的手掌位姿信息变化记为AΔt:
AΔt=[Δx Δy Δz Δα Δβ Δγ]T
其中x、y、z为手掌在Leap Motion坐标系下的相对位移,α、β、γ为手掌在LeapMotion坐 标系下的回转角、俯仰角、偏转角;
记时刻t机械臂末端在底座坐标系下的位置姿态为为At′:
At'=[xt' yt' zt' αt' βt' γt']T
其中x′、y′、z′为机械臂末端在底座坐标系下的位置坐标,α′、β′、γ′为机械臂末端在底座坐 标系下的回转角、俯仰角、偏转角,将Leap Motion检测到的人手的位姿变化映射到机械臂末端 的位姿,则t+Δt时刻机械臂末端位置At+Δt′为:
其中k1、k2分别为手掌位置、姿态的控制灵敏度系数,通过变化k1、k2的值可改变手掌运动 映射到机械臂末端的幅度大小。T为一3×3矩阵,是Leap Motion坐标系到机械臂底座坐标系的 方向变换。将At+Δt′转化为与Tall对应的齐次形式,其中姿态表示方法采用X-Y-Z固定角坐标系, 记转化后的At+Δt″为:
其中:
由此得到机械臂随动手掌的空间位置和姿态,即:
Tall=At+Δt″ (16)
结合式(2),即可解出各关节角度θ1~θ6。
进一步的,所述七段S曲线舵机平滑运动控制方法过程如下:
为了使机械臂的运动足够平滑,需要保证舵机的角加速度α(t)连续没有突变。将舵机运动至 目标值的整个运动过程分为7段:加加速、匀加速、减加速,匀速,加减速,匀减速,减减速, 从原始的控制变量着手,推算出舵机在每一个时刻的位置θ(t),在控制时以此为基准来改变输出 的PWM脉宽从而达到平滑控制舵机运动的目的,在这个过程中的θ(t)曲线即为S型曲线。
对于舵机转角为α、转动时间为T的一次运动,取边界条件为ω(0)=0、ω(T)=0、θ(0)=0, θ(T)=α,即运动始末角速度为0,运动角位移为α,设7段运动的时间分别为:t0~t6。S曲线在 给定每一段的时间长度和整条曲线的起始位置后便可唯一确定,其中最核心的两个参数是加速段 的最大加速度a和减速段的最小减速度b,通过积分推导出S曲线的形式,进而由边界条件得到 a和b。对于每段的加速度,表示为:
其中,每一行与7段运动过程的每一段相对应。pi为:
对角加速度积分得到二次形式的角速度:
由角速度的连续性,上式中的ω(pi)可根据上一时间段末的角速度值依次求出,对角速度积 分得到三次形式的角位移曲线:
其中,ω[i][j]为式(8)中ω矩阵中的i行、j列元素,θi为:
由角度的连续性,上式中的θ(pi)可根据上一时间段末的角度值依次求出。最终,根据终点 的位移边界条件,推导得出两个重要参数a、b如下:
其中M(pi,ti)可由时间量pi和ti得到。可以看出,一旦给定每段的运动时间,即可确定两个 加速度量a、b,将a、b进一步代入速度矩阵与位移矩阵,便可得到满足加速度不突变的平滑的 运动轨迹。
本发明的一种带有机械臂的飞行机器人及其控制方法,具有以下有益效果:
(1)使得无人机和机械臂作业范围广,受地理位置限制小,可实现远程操作,使得操作者 无需达到作业环境,避免了危险与不便(如危险地区、地质灾害地区、水面、高空等);
(2)机械臂的控制方式更加自然灵活,根据人体姿态做出响应,实现机械臂随动人体手臂 位姿的效果,充分体现操作者的决策和灵活性,同时避免了复杂的模型检测与运算,可提供更好 的响应效果;
(3)在舵机控制方式上,通过7段S曲线的平滑控制方法,减缓了舵机快速响应时的冲击 问题。
附图说明
图1是本发明的多旋翼无人机飞行平台和六自由度机械臂结构示意图;
图2是图1所示的多旋翼无人机飞行平台和六自由度机械臂另一角度结构示意图;
图3是本发明六自由度机械臂结构示意图;
图4是图3所示的六自由度机械臂另一角度结构示意图;
图5是底座示结构意图;
图6是图5所示的底座的另一角度结构示意图;
图7是肘部示结构意图;
图8是本发明控制方法示意图;
图9是机械臂DH坐标系;
图10是舵机在1s内转动100°其角位移、角速度、角加速度随时间的变化示意图。
具体实施方式
下面结合附图,对本发明提出的一种带有机械臂的飞行机器人及其控制方法进行详细说明。 在本发明的描述中,需要理解的是,术语“左侧”、“右侧”、“上部”、“下部”、“底部”等指示的 方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明和简化描述,而不 是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,“第一”、“第 二”等并不表示零部件的重要程度,因此不能理解为对本发明的限制。本实施例中采用的具体尺 寸只是为了举例说明技术方案,并不限制本发明的保护范围。
如图1和2所示,本发明的一种带有机械臂的飞行机器人,包括多旋翼无人机飞行平台1、 六自由度机械臂2和操作端控制***,六自由度机械臂安装在多旋翼无人机飞行平台下方。
多旋翼无人机飞行平台1,包括六轴无人机、飞控***、GPS模块、电池组、图像采集***, 所述飞控***、GPS模块、电池组和图像采集***位于六轴无人机上方。
如图3至4所示,六自由度机械臂2包括底座21、肘部24和腕部26,所述底座21和肘部 24之间采用大臂22连接,同时在底座21和肘部24之间设置与大臂22平行的连杆23,底座21、 大臂22、连杆23和肘部24组成四连杆机构;所述肘部24和腕部26之间采用小臂25连接;六 自由度机械臂2通过与飞行器接合结构27安装到多旋翼无人机飞行平台下方腹部。
如图3至7所示,下面将详细说明六自由度机械臂2不同部分的结构及相邻部分间的连接形 式。
与飞行器接合结构27:
自上而下分别为与飞行器固接部分、平面轴承、与底座固接部分。
与飞行器固接部分:与飞行器固接依靠三个孔使用螺栓螺母固接。
平面轴承:实现底座和飞行器之间的相互平面转动的功能,平面轴承较小内径的部分位于上 侧。
与飞行器固接部分、与底座固接部分侧面开螺纹孔可通过螺钉进一步锁紧平面轴承。
与底座固接部分与底座依靠两个孔通过螺栓螺母连接。
底座21:底座21结构组成如图5和6所示。底座上固接有三个舵机,舵机和舵盘相连接, 在图6中,由上至下、由左至右分别第一、二、三舵机,本发明中舵机安装的方法均为在安装位 置开孔通过螺栓螺母连接。第一舵机用来驱动机械臂和飞行器平台之间的平面内转动,第二舵机 用来驱动大臂相对于底座的转动,第三舵机用来驱动小臂相对于大臂的转动。其中第一舵机上通 过舵盘固接了轴,轴的直径和平面轴承内径关系是大于其较小的内径而小于其较大的内径,轴与 平面轴承上部的平面垫圈形成过盈配合而固接,当第一舵机转动时,带动轴、平面轴承上部垫圈 转动,实现平面轴承的平面内转动,即机械臂相对于飞行器平台的平面内转动。
大臂22:大臂22上固接有三个法兰轴承,大臂侧面开孔以便于舵机电源及信号走线。
大臂22本身作为大臂22小臂25之间的四连杆机构的连杆之一,四连杆机构的第二连杆头 部短轴可与第一法兰轴承内径装配实现大臂和第二连杆的相对转动,第二连杆尾部开孔与第三连 杆铰接实现相互转动。大臂、第二连杆、第三连杆装配后如图3和4所示。
底座22与大臂23间连接的实现方式是:第二舵机舵盘处的四个圆形孔和大臂上部左侧的四 个螺纹孔用螺栓螺母固接、第三舵机舵盘处四个圆形孔和第二连杆头部的四个螺纹孔用螺栓螺母 固接;通过这样的连接方式,第二舵机可直接驱动大臂相对于底座绕第二舵机的转轴转动,第三 舵机的转动将直接驱动第二连杆绕其转轴转动,而不对大臂产生影响。底座、大臂、第二连杆、 第三连杆连接后如图3和4所示。
如图7所示,肘部24是由四片板材和舵机结合而形成立体结构。其中第一部件241上开孔, 孔径相比小臂外径略大;第一部件241通过螺纹孔使用螺钉与第二、三部件242、243固接配合, 第四舵机安装于第三部件243上,第四舵机转轴与第一部件241上的孔共轴;第二、四部件242、 244上的大孔用来与大臂22末端的第二、三法兰轴承内孔通过法兰短轴形成共轴配合,从而使 得腕部相对大臂22可绕大臂22末端的法兰轴承转轴转动;第四部件后部开孔可实现与第三连杆 铰接,实现相对转动,即肘部24同时也是大臂22和小臂25之间的四连杆机构的第四根连杆。
小臂25:小臂结构较为简单,其由前、后连接件和圆管组成,圆管前后端打孔利用细长螺 栓和螺母与前后连接件固接。后端连接件上的四个圆孔与4号舵机舵盘固接,4号舵机由此能驱 动小臂绕其自身转轴转动。
腕部26:
腕部26主要由两大部分组成。
第一部分左挡板开孔装配有法兰轴承,后挡板的四个开孔可与小臂的前端连接件上的孔利用 螺栓螺母固接,右挡板安装有第五舵机,舵机转轴和左挡板上的法兰轴承转轴共轴。
第二部分左挡板固接有短轴,可与第一部分左挡板上第四法兰轴承内径配合,前侧挡板安装 有第六舵机,右侧挡板的四个孔可与第一部分右侧挡板上的舵机舵盘利用螺栓螺母固接。
作业部说明:
作业部安装于第六舵机的舵盘上,可依据任务需求安装执行装置,常见的如机械爪类的抓取 装置或真空吸附装置等等。
***工作原理
本发明可利用人体手掌或者手臂的位置和姿态信息来控制机械臂运动,人体手掌的位置和姿 态信息表现为三个方向的位移量和三个方向(偏航、俯仰、滚转)的转动量,手臂的姿态表现为 各关节之间的夹角。人体姿态捕捉设备可以是例如购买自Leap Motion的Leap Motion装置,或 者是购买自Microsoft的Kinect装置。
本发明中,使用Leap Motion控制是通过捕捉到的人体手掌位置和姿态6个自由度的信息作 为机械臂末端的位姿信息,通过逆向运动学方法反解出各个关节的角度,对应到舵机的转动角度。 使用Kinect控制是通过将捕捉到的手臂关节间的夹角映射到机械臂关节夹角,对应到舵机的转 动角度。
整套***如图8所示。由如前所述的空中飞行平台及机械臂和地基操作端组成。地基操作端 由计算机、人体姿态捕捉装置、相应的控制软件包组成。操作端通过无线讯号向空中端发送机械 臂运动指令,空中端通过无线讯号向操作端传送即使拍摄到的图像数据,操作端的人依据图像对 飞行平台进行控制,同时操作端的人利用手掌或者手臂位置姿态对机械臂进行控制,控制均通过 无线讯号远程实现。通过这样的控制方法,将人自身作为主要环节引入到控制当中去,使得控制 方式更加灵活自然。
Leap Motion和Kinect手势、人体姿态检测设备:
在对机械臂的控制测试中对比分析了目前较易购买到的Kinect和Leap Motion设备,其中 Leap Motion可检测手掌的运动并定位人手的各主要关节,可以达到0.01mm的识别精度,Kinect 则可实现对其可视范围内物体深度数据的获取,特别地还能对人体全身主要关节进行捕捉和定位, 常被用于人体动作识别和场景建模。
基于Kinect的正向控制方式:
由于Kinect检测的是全身骨骼关节空间坐标,可直接获取手臂的运动,因此基于Kinect的 控制采用的是正向控制方式,即通过Kinect捕获的手臂关节空间坐标求解出各个关节间的角度, 如求解大臂和小臂之间的角度时,可由肩部、肘部、腕部的空间坐标得到沿大臂和小臂的单位向 量方向,进而得到两向量间的夹角。通过将求解得到的各个关节的角度映射到机械臂的关节间夹 角实现对其控制,这种控制方式思路简单,在程序上易于实现。
基于Leap Motion的逆向控制方式:
与Kinect不同,Leap Motion是对手掌运动进行检测,通过Leap Motion可以获取手掌的6 自由度信息:3个位移量、3个姿态角。利用这6自由度信息作为机械臂末端关节位置姿态条件, 可逆向求解出各个关节之间的夹角。考虑到机械臂连杆机构只是作为动力传动机构,在推导机械 臂的运动学逆解时不考虑连杆。采用DH法确定各关节坐标系,如图9所示。机械臂DH参数如 表1所示。
表1机械臂DH参数
将相邻坐标系i-1和i的齐次变换记为Ti,如以T1表示坐标系1相对于坐标系0的位置姿态, Ti由参数ai、αi、di、θi决定:
由此得到从机械臂末端关节坐标系6相对于机械臂底座坐标系0的总变换Tall为:
Tall=T1T2T3T4T5T6 (24)
对于本文构型的机械臂,已知其末端关节的位置和姿态,可逆向推导出各关节间夹角的解析 解,但解的个数不唯一,对于某一给定位姿有八组逆解,在实际应用中结合舵机特性和机械臂安 装方式,选择了一组最合适的解。
Leap Motion自身的数据更新频率在测试用的计算机上约为120Hz,在控制程序中,需以低 于此频率的更新速率计算求解。设更新间隔时间为Δt,Δt间隔内程序检测到的手掌位姿信息变 化记为AΔt:
AΔt=[Δx Δy Δz Δα Δβ Δγ]T
其中x、y、z为手掌在Leap Motion坐标系下的相对位移,α、β、γ为手掌在LeapMotion坐 标系下的回转角、俯仰角、偏转角。
记时刻t机械臂末端在底座坐标系下的位置姿态为为At′:
At'=[xt' yt' zt' αt' βt' γt']T
其中x′、y′、z′为机械臂末端在底座坐标系下的位置坐标,α′、β′、γ′为机械臂末端在底座坐 标系下的回转角、俯仰角、偏转角,将Leap Motion检测到的人手的位姿变化映射到机械臂末端 的位姿,则t+Δt时刻机械臂末端位置At+Δt′为:
其中k1、k2分别为手掌位置、姿态的控制灵敏度系数,通过变化k1、k2的值可改变手掌运动 映射到机械臂末端的幅度大小。T为一3×3矩阵,是Leap Motion坐标系到机械臂底座坐标系的 方向变换。将At+Δt′转化为与Tall对应的齐次形式,其中姿态表示方法采用X-Y-Z固定角坐标系, 记转化后的At+Δt″为:
其中:
由此得到机械臂随动手掌的空间位置和姿态,即:
Tall=At+Δt″ (27)
结合式(2),即可解出各关节角度θ1~θ6。
七段S曲线舵机平滑运动控制方法:
用于舵机的近似调速方法:
为尽量减小机械臂装置重量并提高响应速度,选用舵机作为其动力装置。在实际测试中发现: 机械臂过于快速的响应会造成自身运动的不稳定,且由机械臂运动导致的对飞行器的耦合影响使 得飞控***难以处理。因此为维持***的稳定性,有必要寻求一种能够使机械臂响应平滑、减缓 其对飞行器冲击的控制方式。
舵机自身的动力控制特性决定了难以直接对其工作时的角加速度和角速度进行干预,所以可 以采取一种近似的方式:即先让舵机以固有的角速度速度转动t1个单位时间,然后再停下t2个单 位时间,这样在dt=t1+t2这个时间段内的平均速度即可通过t2来调节,若dt取得足够小(不小 于舵机控制型号周期),则可近似地用平均角速度代替舵机在dt时间内的真实角速度。
七段S型曲线调速的实现:
为了使机械臂的运动足够平滑,需要保证舵机的角加速度α(t)连续没有突变。将舵机运动至 目标值的整个运动过程分为7段:加加速、匀加速、减加速,匀速,加减速,匀减速,减减速, 从原始的控制变量着手,推算出舵机在每一个时刻的位置θ(t),在控制时以此为基准来改变输出 的PWM脉宽从而达到平滑控制舵机运动的目的,在这个过程中的θ(t)曲线即为S型曲线。
对于舵机转角为α、转动时间为T的一次运动,取边界条件为ω(0)=0、ω(T)=0、θ(0)=0, θ(T)=α,即运动始末角速度为0,运动角位移为α,设7段运动的时间分别为:t0~t6。S曲线在 给定每一段的时间长度和整条曲线的起始位置后便可唯一确定,其中最核心的两个参数是加速段 的最大加速度a和减速段的最小减速度b,通过积分推导出S曲线的形式,进而由边界条件得到 a和b。对于每段的加速度,表示为:
其中,每一行与7段运动过程的每一段相对应。pi为:
对角加速度积分得到二次形式的角速度:
由角速度的连续性,上式中的ω(pi)可根据上一时间段末的角速度值依次求出,对角速度积 分得到三次形式的角位移曲线:
其中,ω[i][j]为式(8)中ω矩阵中的i行、j列元素,θi为:
由角度的连续性,上式中的θ(pi)可根据上一时间段末的角度值依次求出。最终,根据终点 的位移边界条件,推导得出两个重要参数a、b如下:
其中M(pi,ti)可由时间量pi和ti得到。可以看出,一旦给定每段的运动时间,即可确定两个 加速度量a、b,将a、b进一步代入速度矩阵与位移矩阵,便可得到满足加速度不突变的平滑的 运动轨迹。
以舵机在1s内转动100°为例,图10给出了基于上述方式得到的舵机角位移、角速度、角加 速度随时间的变化,其中t0=80ms、t1=140ms、t2=80ms,t3=400ms,t4=80ms、t5=140ms、t6=80ms。 由第三幅图中的角加速度随时间的变化可清晰地看出整个运动分为加加速、匀加速、减加速,匀 速,加减速,匀减速,减减速7个阶段,角加速度连续没有突变。
基于对本发明优选实施方式的描述,应该清楚,由所附的权利要求书所限定的本发明并不 仅仅局限于上面说明书中所阐述的特定细节,未脱离本发明宗旨或范围的对本发明的许多显而易 见的改变同样可能达到本发明的目的。
Claims (9)
1.一种带有机械臂的飞行机器人,其特征在于,包括多旋翼无人机飞行平台、六自由度机械臂和操作端控制***,所述六自由度机械臂安装在多旋翼无人机飞行平台下方。
2.根据权利要求1所述的带有机械臂的飞行机器人,其特征在于,所述多旋翼无人机飞行平台,包括六轴无人机、飞控***、GPS模块、电池组、图像采集***,所述飞控***、GPS模块、电池组和图像采集***位于六轴无人机上方。
3.根据权利要求1所述的带有机械臂的飞行机器人,其特征在于,所述六自由度机械臂包括底座、肘部和腕部,所述底座和肘部之间采用大臂连接,同时在底座和肘部之间设置与大臂平行的连杆,底座、大臂、连杆和肘部组成四连杆机构;所述肘部和腕部之间采用小臂连接;所述六自由度机械臂通过与飞行器接合结构安装到多旋翼无人机飞行平台下方。
4.根据权利要求3所述的带有机械臂的飞行机器人,其特征在于,所述底座上安装有三个舵机,分别为第一、二、三舵机;所述第一舵机位于上方,所述第二、三舵机位于第一舵机下方,并分列于第一舵机两侧;第一舵机用于驱动机械臂和飞行器平台之间的平面内转动,第二舵机用于驱动大臂相对于底座的转动,第三舵机用于驱动小臂相对于大臂的转动;
所述大臂呈工字型,所述大臂上固接有第一、二、三法兰轴承,大臂侧面开孔以便于舵机电源及信号走线;
所述肘部是由四片板材和舵机结合而形成的立体结构;
所述小臂由前、后连接件和圆管组成;所述连杆包括第二连杆和第三连杆;所述第二连杆头部短轴与第一法兰轴承内径装配实现大臂和第二连杆的相对转动,第二连杆尾部开孔与第三连杆铰接实现相互转动;
所述腕部包括第一部分和第二部分;所述第一部分的左挡板开孔装配有法兰轴承,后挡板的四个开孔与小臂的前端连接件上的孔利用螺栓螺母固接,右挡板安装有第五舵机,舵机转轴和左挡板上的法兰轴承转轴共轴;所述第二部分的左挡板固接有短轴,与第一部分左挡板上法兰轴承内径配合,前侧挡板安装有第六舵机,右侧挡板的四个孔与第一部分右侧挡板上的舵机舵盘利用螺栓螺母固接。
5.根据权利要求4所述的带有机械臂的飞行机器人,其特征在于,所述底座与大臂间连接的实现方式是:第二舵机舵盘处的四个圆形孔和大臂上部左侧的四个螺纹孔用螺栓螺母固接,第三舵机舵盘处四个圆形孔和第二连杆头部的四个螺纹孔用螺栓螺母固接;通过这样的连接方式,第二舵机可直接驱动大臂相对于底座绕第二舵机的转轴转动,第三舵机的转动将直接驱动第二连杆绕其转轴转动,而不对大臂产生影响。
6.根据权利要求1所述的带有机械臂的飞行机器人,其特征在于,所述操作端控制***包括Kinect和Leap Motion设备;所述Leap Motion用于检测手掌的运动并定位人手的各主要关节;所述Kinect用于实现对其可视范围内物体深度数据的获取,对人体全身主要关节进行捕捉和定位。
7.一种带有机械臂的飞行机器人的控制方法,其特征在于,包括基于Kinect的正向控制方式、基于Leap Motion的逆向控制方式和七段S曲线舵机平滑运动控制方法;所述基于Kinect的正向控制方式,通过Kinect捕获的手臂关节空间坐标求解出各个关节间的角度,通过将求解得到的各个关节的角度映射到机械臂的关节间夹角实现对其控制。
8.根据权利要求7所述的带有机械臂的飞行机器人的控制方法,其特征在于,所述基于Leap Motion的逆向控制方式,通过Leap Motion获取手掌的6自由度信息:3个位移量、3个姿态角;利用这6自由度信息作为机械臂末端关节位置姿态条件,逆向求解出各个关节之间的夹角;采用DH法确定各关节坐标系;将相邻坐标系i-1和i的齐次变换记为Ti,如以T1表示坐标系1相对于坐标系0的位置姿态,Ti由机械臂结构参数ai、αi、di、θi决定:
<mrow>
<msub>
<mi>T</mi>
<mi>i</mi>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>cos&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>sin&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>cos&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>sin&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>sin&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>cos&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>sin&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>cos&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>cos&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>cos&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>sin&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
<msub>
<mi>sin&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>sin&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>cos&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mi>d</mi>
<mi>i</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
由此得到从机械臂末端关节坐标系6相对于机械臂底座坐标系0的总变换Tall为:
Tall=T1T2T3T4T5T6 (2)
设Leap Motion更新间隔时间为Δt,Δt间隔内程序检测到的手掌位姿信息变化记为AΔt:
AΔt=[Δx Δy Δz Δα Δβ Δγ]T
其中x、y、z为手掌在Leap Motion坐标系下的相对位移,α、β、γ为手掌在Leap Motion坐标系下的回转角、俯仰角、偏转角;
记时刻t机械臂末端在底座坐标系下的位置姿态为为At′:
At'=[xt' yt' zt' αt' βt' γt']T
其中x′、y′、z′为机械臂末端在底座坐标系下的位置坐标,α′、β′、γ′为机械臂末端在底座坐标系下的回转角、俯仰角、偏转角,将Leap Motion检测到的人手的位姿变化映射到机械臂末端的位姿,则t+Δt时刻机械臂末端位置At+Δt′为:
<mrow>
<msup>
<msub>
<mi>A</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msup>
<msub>
<mi>x</mi>
<mi>t</mi>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<msub>
<mi>y</mi>
<mi>t</mi>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<msub>
<mi>z</mi>
<mi>t</mi>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<msub>
<mi>&alpha;</mi>
<mi>t</mi>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<msub>
<mi>&beta;</mi>
<mi>t</mi>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<msub>
<mi>&gamma;</mi>
<mi>t</mi>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>k</mi>
<mn>1</mn>
</msub>
<mi>T</mi>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>&Delta;</mi>
<mi>x</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>&Delta;</mi>
<mi>y</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>&Delta;</mi>
<mi>z</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>k</mi>
<mn>2</mn>
</msub>
<mi>T</mi>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>&Delta;</mi>
<mi>&alpha;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>&Delta;</mi>
<mi>&beta;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>&Delta;</mi>
<mi>&gamma;</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msup>
<msub>
<mi>x</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<msub>
<mi>y</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<msub>
<mi>z</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<msub>
<mi>&alpha;</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<msub>
<mi>&beta;</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<msub>
<mi>&gamma;</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
其中k1、k2分别为手掌位置、姿态的控制灵敏度系数,通过变化k1、k2的值可改变手掌运动映射到机械臂末端的幅度大小;T为一3×3矩阵,是Leap Motion坐标系到机械臂底座坐标系的方向变换;将At+Δt′转化为与Tall对应的齐次形式,其中姿态表示方法采用X-Y-Z固定角坐标系,记转化后的At+Δt″为:
<mrow>
<msup>
<msub>
<mi>A</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mrow>
<mo>&prime;</mo>
<mo>&prime;</mo>
</mrow>
</msup>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>n</mi>
<mi>x</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>o</mi>
<mi>x</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>m</mi>
<mi>x</mi>
</msub>
</mtd>
<mtd>
<mrow>
<msup>
<msub>
<mi>x</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>n</mi>
<mi>y</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>o</mi>
<mi>y</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>m</mi>
<mi>y</mi>
</msub>
</mtd>
<mtd>
<mrow>
<msup>
<msub>
<mi>y</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>n</mi>
<mi>z</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>o</mi>
<mi>z</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>m</mi>
<mi>z</mi>
</msub>
</mtd>
<mtd>
<mrow>
<msup>
<msub>
<mi>z</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中:
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>n</mi>
<mi>x</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>o</mi>
<mi>x</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>m</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>n</mi>
<mi>y</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>o</mi>
<mi>y</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>m</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>n</mi>
<mi>z</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>o</mi>
<mi>z</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>m</mi>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msup>
<msub>
<mi>cos&gamma;</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msup>
<msub>
<mi>sin&gamma;</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<msub>
<mi>sin&gamma;</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
<mtd>
<mrow>
<msup>
<msub>
<mi>cos&gamma;</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msup>
<msub>
<mi>cos&beta;</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msup>
<msub>
<mi>sin&beta;</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msup>
<msub>
<mi>sin&beta;</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msup>
<msub>
<mi>cos&beta;</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msup>
<msub>
<mi>cos&alpha;</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msup>
<msub>
<mi>sin&alpha;</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msup>
<msub>
<mi>sin&alpha;</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
<mtd>
<mrow>
<msup>
<msub>
<mi>cos&alpha;</mi>
<mrow>
<mi>t</mi>
<mo>+</mo>
<mi>&Delta;</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>&prime;</mo>
</msup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>)</mo>
</mrow>
</mrow>
由此得到机械臂随动手掌的空间位置和姿态,即:
Tall=At+Δt″ (5)
结合式(2),即可解出各关节角度θ1~θ6。
9.根据权利要求7所述的带有机械臂的飞行机器人的控制方法,其特征在于,所述七段S曲线舵机平滑运动控制方法过程如下:
为了使机械臂的运动足够平滑,需要保证舵机的角加速度α(t)连续没有突变;将舵机运动至目标值的整个运动过程分为7段:加加速、匀加速、减加速,匀速,加减速,匀减速,减减速,从原始的控制变量着手,推算出舵机在每一个时刻的位置θ(t),在控制时以此为基准来改变输出的PWM脉宽从而达到平滑控制舵机运动的目的,在这个过程中的θ(t)曲线即为S型曲线;
对于舵机转角为α、转动时间为T的一次运动,取边界条件为ω(0)=0、ω(T)=0、θ(0)=0,θ(T)=α,即运动始末角速度为0,运动角位移为α,设7段运动的时间分别为:t0~t6;S曲线在给定每一段的时间长度和整条曲线的起始位置后便可唯一确定,其中最核心的两个参数是加速段的最大加速度a和减速段的最小减速度b,通过积分推导出S曲线的形式,进而由边界条件得到a和b;对于每段的加速度,表示为:
<mrow>
<mover>
<mi>&omega;</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mfrac>
<mi>a</mi>
<msub>
<mi>p</mi>
<mn>0</mn>
</msub>
</mfrac>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>a</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mfrac>
<mrow>
<msub>
<mi>ap</mi>
<mn>2</mn>
</msub>
</mrow>
<msub>
<mi>t</mi>
<mn>2</mn>
</msub>
</mfrac>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mfrac>
<mi>a</mi>
<msub>
<mi>t</mi>
<mn>2</mn>
</msub>
</mfrac>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mi>b</mi>
<mfrac>
<msub>
<mi>p</mi>
<mn>3</mn>
</msub>
<msub>
<mi>t</mi>
<mn>4</mn>
</msub>
</mfrac>
</mrow>
</mtd>
<mtd>
<mfrac>
<mi>b</mi>
<msub>
<mi>t</mi>
<mn>4</mn>
</msub>
</mfrac>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>b</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mfrac>
<mrow>
<msub>
<mi>bp</mi>
<mn>6</mn>
</msub>
</mrow>
<msub>
<mi>t</mi>
<mn>6</mn>
</msub>
</mfrac>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mfrac>
<mi>b</mi>
<msub>
<mi>t</mi>
<mn>6</mn>
</msub>
</mfrac>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>t</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>6</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,每一行与7段运动过程的每一段相对应;pi为:
<mrow>
<msub>
<mi>p</mi>
<mi>i</mi>
</msub>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mn>0</mn>
<mi>i</mi>
</munderover>
<msub>
<mi>t</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>7</mn>
<mo>)</mo>
</mrow>
</mrow>
对角加速度积分得到二次形式的角速度:
<mrow>
<mi>&omega;</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mfrac>
<mi>a</mi>
<mrow>
<mn>2</mn>
<msub>
<mi>p</mi>
<mn>0</mn>
</msub>
</mrow>
</mfrac>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mfrac>
<mrow>
<msub>
<mi>ap</mi>
<mn>0</mn>
</msub>
</mrow>
<mn>2</mn>
</mfrac>
</mrow>
</mtd>
<mtd>
<mi>a</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&omega;</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>p</mi>
<mn>1</mn>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mfrac>
<mrow>
<msub>
<mi>ap</mi>
<mn>1</mn>
</msub>
</mrow>
<msub>
<mi>t</mi>
<mn>2</mn>
</msub>
</mfrac>
<mrow>
<mo>(</mo>
<msub>
<mi>p</mi>
<mn>2</mn>
</msub>
<mo>-</mo>
<mfrac>
<msub>
<mi>p</mi>
<mn>1</mn>
</msub>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mfrac>
<mrow>
<msub>
<mi>ap</mi>
<mn>2</mn>
</msub>
</mrow>
<msub>
<mi>t</mi>
<mn>2</mn>
</msub>
</mfrac>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mfrac>
<mi>a</mi>
<mrow>
<mn>2</mn>
<msub>
<mi>t</mi>
<mn>2</mn>
</msub>
</mrow>
</mfrac>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&omega;</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>p</mi>
<mn>2</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&omega;</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>p</mi>
<mn>3</mn>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mfrac>
<mrow>
<msup>
<msub>
<mi>bp</mi>
<mn>3</mn>
</msub>
<mn>2</mn>
</msup>
</mrow>
<mrow>
<mn>2</mn>
<msub>
<mi>t</mi>
<mn>4</mn>
</msub>
</mrow>
</mfrac>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mfrac>
<mi>b</mi>
<msub>
<mi>t</mi>
<mn>4</mn>
</msub>
</mfrac>
</mrow>
</mtd>
<mtd>
<mfrac>
<mi>b</mi>
<mrow>
<mn>2</mn>
<msub>
<mi>t</mi>
<mn>4</mn>
</msub>
</mrow>
</mfrac>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&omega;</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>p</mi>
<mn>4</mn>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>bp</mi>
<mn>4</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mi>b</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&omega;</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>p</mi>
<mn>5</mn>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mfrac>
<mi>b</mi>
<msub>
<mi>t</mi>
<mn>6</mn>
</msub>
</mfrac>
<mrow>
<mo>(</mo>
<msub>
<mi>p</mi>
<mn>5</mn>
</msub>
<msub>
<mi>p</mi>
<mn>6</mn>
</msub>
<mo>-</mo>
<mfrac>
<mrow>
<msup>
<msub>
<mi>p</mi>
<mn>5</mn>
</msub>
<mn>2</mn>
</msup>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mfrac>
<mrow>
<msub>
<mi>bp</mi>
<mn>6</mn>
</msub>
</mrow>
<msub>
<mi>t</mi>
<mn>6</mn>
</msub>
</mfrac>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mfrac>
<mi>b</mi>
<mrow>
<mn>2</mn>
<msub>
<mi>t</mi>
<mn>6</mn>
</msub>
</mrow>
</mfrac>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>t</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mi>t</mi>
<mn>2</mn>
</msup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>8</mn>
<mo>)</mo>
</mrow>
</mrow>
由角速度的连续性,上式中的ω(pi)可根据上一时间段末的角速度值依次求出,对角速度积分得到三次形式的角位移曲线:
<mrow>
<mi>&theta;</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mfrac>
<mi>a</mi>
<mrow>
<mn>6</mn>
<msub>
<mi>t</mi>
<mn>0</mn>
</msub>
</mrow>
</mfrac>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&theta;</mi>
<mn>1</mn>
</msub>
</mtd>
<mtd>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>1</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>0</mn>
<mo>&rsqb;</mo>
</mrow>
</mtd>
<mtd>
<mfrac>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>1</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>1</mn>
<mo>&rsqb;</mo>
</mrow>
<mn>2</mn>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>1</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>2</mn>
<mo>&rsqb;</mo>
</mrow>
<mn>3</mn>
</mfrac>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&theta;</mi>
<mn>2</mn>
</msub>
</mtd>
<mtd>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>2</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>0</mn>
<mo>&rsqb;</mo>
</mrow>
</mtd>
<mtd>
<mfrac>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>2</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>1</mn>
<mo>&rsqb;</mo>
</mrow>
<mn>2</mn>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>2</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>2</mn>
<mo>&rsqb;</mo>
</mrow>
<mn>3</mn>
</mfrac>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&theta;</mi>
<mn>3</mn>
</msub>
</mtd>
<mtd>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>3</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>0</mn>
<mo>&rsqb;</mo>
</mrow>
</mtd>
<mtd>
<mfrac>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>3</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>1</mn>
<mo>&rsqb;</mo>
</mrow>
<mn>2</mn>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>3</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>2</mn>
<mo>&rsqb;</mo>
</mrow>
<mn>3</mn>
</mfrac>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&theta;</mi>
<mn>4</mn>
</msub>
</mtd>
<mtd>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>4</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>0</mn>
<mo>&rsqb;</mo>
</mrow>
</mtd>
<mtd>
<mfrac>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>4</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>1</mn>
<mo>&rsqb;</mo>
</mrow>
<mn>2</mn>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>4</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>2</mn>
<mo>&rsqb;</mo>
</mrow>
<mn>3</mn>
</mfrac>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&theta;</mi>
<mn>5</mn>
</msub>
</mtd>
<mtd>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>5</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>0</mn>
<mo>&rsqb;</mo>
</mrow>
</mtd>
<mtd>
<mfrac>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>5</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>1</mn>
<mo>&rsqb;</mo>
</mrow>
<mn>2</mn>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>5</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>2</mn>
<mo>&rsqb;</mo>
</mrow>
<mn>3</mn>
</mfrac>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&theta;</mi>
<mn>6</mn>
</msub>
</mtd>
<mtd>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>6</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>0</mn>
<mo>&rsqb;</mo>
</mrow>
</mtd>
<mtd>
<mfrac>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>6</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>1</mn>
<mo>&rsqb;</mo>
</mrow>
<mn>2</mn>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mn>6</mn>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>2</mn>
<mo>&rsqb;</mo>
</mrow>
<mn>3</mn>
</mfrac>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>t</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mi>t</mi>
<mn>2</mn>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mi>t</mi>
<mn>3</mn>
</msup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>9</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,ω[i][j]为式(8)中ω矩阵中的i行、j列元素,θi为:
<mrow>
<msub>
<mi>&theta;</mi>
<mi>i</mi>
</msub>
<mo>=</mo>
<mi>&theta;</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>p</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<mn>3</mn>
</mfrac>
<mrow>
<mo>(</mo>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mi>i</mi>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>0</mn>
<mo>&rsqb;</mo>
<msub>
<mi>p</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>+</mo>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mi>i</mi>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>1</mn>
<mo>&rsqb;</mo>
<mfrac>
<msubsup>
<mi>p</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mn>2</mn>
</msubsup>
<mn>2</mn>
</mfrac>
<mo>+</mo>
<mi>&omega;</mi>
<mo>&lsqb;</mo>
<mi>i</mi>
<mo>&rsqb;</mo>
<mo>&lsqb;</mo>
<mn>2</mn>
<mo>&rsqb;</mo>
<mfrac>
<msubsup>
<mi>p</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mn>3</mn>
</msubsup>
<mn>3</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>10</mn>
<mo>)</mo>
</mrow>
</mrow>
由角度的连续性,上式中的θ(pi)可根据上一时间段末的角度值依次求出;最终,根据终点的位移边界条件,推导得出两个重要参数a、b如下:
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<mi>a</mi>
<mo>=</mo>
<mfrac>
<mrow>
<mn>6</mn>
<mi>&alpha;</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mn>4</mn>
</msub>
<mo>+</mo>
<mn>2</mn>
<msub>
<mi>t</mi>
<mn>5</mn>
</msub>
<mo>+</mo>
<msub>
<mi>t</mi>
<mn>6</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mi>M</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>p</mi>
<mi>i</mi>
</msub>
<mo>,</mo>
<msub>
<mi>t</mi>
<mi>i</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>b</mi>
<mo>=</mo>
<mfrac>
<mrow>
<mi>a</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mn>0</mn>
</msub>
<mo>+</mo>
<mn>2</mn>
<msub>
<mi>t</mi>
<mn>1</mn>
</msub>
<mo>+</mo>
<msub>
<mi>t</mi>
<mn>2</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>t</mi>
<mn>4</mn>
</msub>
<mo>+</mo>
<mn>2</mn>
<msub>
<mi>t</mi>
<mn>5</mn>
</msub>
<mo>+</mo>
<msub>
<mi>t</mi>
<mn>6</mn>
</msub>
</mrow>
</mfrac>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>11</mn>
<mo>)</mo>
</mrow>
</mrow>
其中M(pi,ti)可由时间量pi和ti得到;可以看出,一旦给定每段的运动时间,即可确定两个加速度量a、b,将a、b进一步代入速度矩阵与位移矩阵,便可得到满足加速度不突变的平滑的运动轨迹。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710320306.9A CN107309872B (zh) | 2017-05-08 | 2017-05-08 | 一种带有机械臂的飞行机器人及其控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710320306.9A CN107309872B (zh) | 2017-05-08 | 2017-05-08 | 一种带有机械臂的飞行机器人及其控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107309872A true CN107309872A (zh) | 2017-11-03 |
CN107309872B CN107309872B (zh) | 2021-06-15 |
Family
ID=60184824
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710320306.9A Active CN107309872B (zh) | 2017-05-08 | 2017-05-08 | 一种带有机械臂的飞行机器人及其控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107309872B (zh) |
Cited By (21)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108248845A (zh) * | 2018-01-31 | 2018-07-06 | 湖南大学 | 一种基于动态重心补偿的旋翼飞行机械臂***及算法 |
CN108312137A (zh) * | 2018-03-26 | 2018-07-24 | 河南工程学院 | 基于多关节机械臂的无人机着陆对接机构 |
CN108377781A (zh) * | 2018-03-08 | 2018-08-10 | 南京太司德智能科技有限公司 | 一种悬挂刀具结构的树障清理空中机器人 |
CN108388272A (zh) * | 2018-03-30 | 2018-08-10 | 五邑大学 | 一种基于时间最优的无人机前飞速度计算方法 |
CN108482678A (zh) * | 2018-04-07 | 2018-09-04 | 邢明的 | 一种空中飞行抓取无人机 |
CN108513817A (zh) * | 2018-03-08 | 2018-09-11 | 贵州电网有限责任公司 | 作业臂可伸缩且纵向弯曲的树障清理空中机器人和方法 |
CN109164829A (zh) * | 2018-10-23 | 2019-01-08 | 哈尔滨工业大学(深圳) | 一种基于力反馈装置和vr感知的飞行机械臂***及控制方法 |
CN109176559A (zh) * | 2018-10-24 | 2019-01-11 | 上海思依暄机器人科技股份有限公司 | 一种机器人及控制方法、*** |
CN109187102A (zh) * | 2018-09-30 | 2019-01-11 | 广州地理研究所 | 一种无人机载空气孢粉采样装置 |
CN109597407A (zh) * | 2018-11-12 | 2019-04-09 | 初速度(苏州)科技有限公司 | 调节方法和装置 |
CN109969395A (zh) * | 2017-12-28 | 2019-07-05 | 极光飞行科学公司 | 用于固定飞行器的地面操纵***和方法 |
CN109969396A (zh) * | 2017-12-28 | 2019-07-05 | 极光飞行科学公司 | 用于航空器的操纵***和方法 |
WO2019172253A1 (ja) * | 2018-03-09 | 2019-09-12 | Thk株式会社 | 飛行ロボット |
CN110989382A (zh) * | 2019-12-06 | 2020-04-10 | 河南师范大学 | 一种多功能云端服务家居机器人 |
CN111070217A (zh) * | 2019-12-07 | 2020-04-28 | 北京大学 | 一种基于无人机搭载机械臂的采茶装置 |
CN111895997A (zh) * | 2020-02-25 | 2020-11-06 | 哈尔滨工业大学 | 一种无需标准矫正姿势的基于惯性传感器的人体动作采集方法 |
CN111993427A (zh) * | 2020-09-03 | 2020-11-27 | 湖南大学 | 一种空中作业机器人的自增稳控制方法、装置、终端、***及可读存储介质 |
CN112148032A (zh) * | 2020-10-20 | 2020-12-29 | 福州大学 | 一种微型树洞检测飞行器及控制方法 |
CN113272756A (zh) * | 2020-09-08 | 2021-08-17 | 深圳市大疆创新科技有限公司 | 云台控制方法、装置、云台及存储介质 |
CN113886935A (zh) * | 2021-11-18 | 2022-01-04 | 西南交通大学 | 适用于工程结构健康状态监测的多模式多用途机器人*** |
CN114415731A (zh) * | 2022-03-25 | 2022-04-29 | 季华实验室 | 多飞行机器人协同作业方法、装置、电子设备和存储介质 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO1999005580A2 (de) * | 1997-07-23 | 1999-02-04 | Duschek Horst Juergen | Verfahren zum kontrollieren eines unbemannten fortbewegungsmittels und unbemanntes fortbewegungsmittelsystem zum durchführen dieses verfahrens |
CN103386683A (zh) * | 2013-07-31 | 2013-11-13 | 哈尔滨工程大学 | 一种基于Kinect的体感控制机械臂方法 |
CN105459090A (zh) * | 2016-01-04 | 2016-04-06 | 江苏科技大学 | 一种示教型六自由度搬运机械手 |
WO2016065625A1 (en) * | 2014-10-31 | 2016-05-06 | SZ DJI Technology Co., Ltd. | Systems and methods for walking pets |
CN105643590A (zh) * | 2016-03-31 | 2016-06-08 | 河北工业大学 | 一种手势控制的轮式移动机器人及其操作方法 |
CN106020494A (zh) * | 2016-06-20 | 2016-10-12 | 华南理工大学 | 基于移动跟踪的三维手势识别方法 |
CN205891228U (zh) * | 2016-07-29 | 2017-01-18 | 华南理工大学 | 一种飞行机器人 |
CN206679256U (zh) * | 2017-05-02 | 2017-11-28 | 南京乐乐飞电子科技有限公司 | 飞行机械臂及无人机*** |
-
2017
- 2017-05-08 CN CN201710320306.9A patent/CN107309872B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO1999005580A2 (de) * | 1997-07-23 | 1999-02-04 | Duschek Horst Juergen | Verfahren zum kontrollieren eines unbemannten fortbewegungsmittels und unbemanntes fortbewegungsmittelsystem zum durchführen dieses verfahrens |
CN103386683A (zh) * | 2013-07-31 | 2013-11-13 | 哈尔滨工程大学 | 一种基于Kinect的体感控制机械臂方法 |
WO2016065625A1 (en) * | 2014-10-31 | 2016-05-06 | SZ DJI Technology Co., Ltd. | Systems and methods for walking pets |
CN105459090A (zh) * | 2016-01-04 | 2016-04-06 | 江苏科技大学 | 一种示教型六自由度搬运机械手 |
CN105643590A (zh) * | 2016-03-31 | 2016-06-08 | 河北工业大学 | 一种手势控制的轮式移动机器人及其操作方法 |
CN106020494A (zh) * | 2016-06-20 | 2016-10-12 | 华南理工大学 | 基于移动跟踪的三维手势识别方法 |
CN205891228U (zh) * | 2016-07-29 | 2017-01-18 | 华南理工大学 | 一种飞行机器人 |
CN206679256U (zh) * | 2017-05-02 | 2017-11-28 | 南京乐乐飞电子科技有限公司 | 飞行机械臂及无人机*** |
Non-Patent Citations (1)
Title |
---|
刘鹏飞等: "‘S’型加减速曲线在机器人轨迹插补算法中的应用研究", 《制造业自动化》 * |
Cited By (31)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109969396A (zh) * | 2017-12-28 | 2019-07-05 | 极光飞行科学公司 | 用于航空器的操纵***和方法 |
CN109969395A (zh) * | 2017-12-28 | 2019-07-05 | 极光飞行科学公司 | 用于固定飞行器的地面操纵***和方法 |
CN109969395B (zh) * | 2017-12-28 | 2023-11-24 | 极光飞行科学公司 | 用于固定飞行器的地面操纵***和方法 |
CN108248845A (zh) * | 2018-01-31 | 2018-07-06 | 湖南大学 | 一种基于动态重心补偿的旋翼飞行机械臂***及算法 |
CN108513817B (zh) * | 2018-03-08 | 2023-06-16 | 贵州电网有限责任公司 | 作业臂可伸缩且纵向弯曲的树障清理空中机器人和方法 |
CN108513817A (zh) * | 2018-03-08 | 2018-09-11 | 贵州电网有限责任公司 | 作业臂可伸缩且纵向弯曲的树障清理空中机器人和方法 |
CN108377781B (zh) * | 2018-03-08 | 2023-10-20 | 南京太司德智能科技有限公司 | 一种悬挂刀具结构的树障清理空中机器人 |
CN108377781A (zh) * | 2018-03-08 | 2018-08-10 | 南京太司德智能科技有限公司 | 一种悬挂刀具结构的树障清理空中机器人 |
JP7120509B2 (ja) | 2018-03-09 | 2022-08-17 | Thk株式会社 | 飛行ロボット |
WO2019172253A1 (ja) * | 2018-03-09 | 2019-09-12 | Thk株式会社 | 飛行ロボット |
JP2019155505A (ja) * | 2018-03-09 | 2019-09-19 | Thk株式会社 | 飛行ロボット |
CN108312137B (zh) * | 2018-03-26 | 2023-08-15 | 河南工程学院 | 基于多关节机械臂的无人机着陆对接机构 |
CN108312137A (zh) * | 2018-03-26 | 2018-07-24 | 河南工程学院 | 基于多关节机械臂的无人机着陆对接机构 |
CN108388272A (zh) * | 2018-03-30 | 2018-08-10 | 五邑大学 | 一种基于时间最优的无人机前飞速度计算方法 |
CN108482678A (zh) * | 2018-04-07 | 2018-09-04 | 邢明的 | 一种空中飞行抓取无人机 |
CN109187102A (zh) * | 2018-09-30 | 2019-01-11 | 广州地理研究所 | 一种无人机载空气孢粉采样装置 |
CN109164829A (zh) * | 2018-10-23 | 2019-01-08 | 哈尔滨工业大学(深圳) | 一种基于力反馈装置和vr感知的飞行机械臂***及控制方法 |
CN109176559A (zh) * | 2018-10-24 | 2019-01-11 | 上海思依暄机器人科技股份有限公司 | 一种机器人及控制方法、*** |
CN109597407A (zh) * | 2018-11-12 | 2019-04-09 | 初速度(苏州)科技有限公司 | 调节方法和装置 |
CN110989382A (zh) * | 2019-12-06 | 2020-04-10 | 河南师范大学 | 一种多功能云端服务家居机器人 |
CN111070217B (zh) * | 2019-12-07 | 2020-12-01 | 北京大学 | 一种基于无人机搭载机械臂的采茶装置 |
CN111070217A (zh) * | 2019-12-07 | 2020-04-28 | 北京大学 | 一种基于无人机搭载机械臂的采茶装置 |
CN111895997B (zh) * | 2020-02-25 | 2022-10-25 | 哈尔滨工业大学 | 一种无需标准矫正姿势的基于惯性传感器的人体动作采集方法 |
CN111895997A (zh) * | 2020-02-25 | 2020-11-06 | 哈尔滨工业大学 | 一种无需标准矫正姿势的基于惯性传感器的人体动作采集方法 |
CN111993427A (zh) * | 2020-09-03 | 2020-11-27 | 湖南大学 | 一种空中作业机器人的自增稳控制方法、装置、终端、***及可读存储介质 |
CN113272756A (zh) * | 2020-09-08 | 2021-08-17 | 深圳市大疆创新科技有限公司 | 云台控制方法、装置、云台及存储介质 |
WO2022051916A1 (zh) * | 2020-09-08 | 2022-03-17 | 深圳市大疆创新科技有限公司 | 云台控制方法、装置、云台及存储介质 |
CN112148032A (zh) * | 2020-10-20 | 2020-12-29 | 福州大学 | 一种微型树洞检测飞行器及控制方法 |
CN112148032B (zh) * | 2020-10-20 | 2023-04-18 | 福州大学 | 一种微型树洞检测飞行器及控制方法 |
CN113886935A (zh) * | 2021-11-18 | 2022-01-04 | 西南交通大学 | 适用于工程结构健康状态监测的多模式多用途机器人*** |
CN114415731A (zh) * | 2022-03-25 | 2022-04-29 | 季华实验室 | 多飞行机器人协同作业方法、装置、电子设备和存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN107309872B (zh) | 2021-06-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107309872A (zh) | 一种带有机械臂的飞行机器人及其控制方法 | |
CN107368091A (zh) | 一种基于有限时间神经动力学的多旋翼无人飞行器的稳定飞行控制方法 | |
CN205891228U (zh) | 一种飞行机器人 | |
US11721219B2 (en) | Method for controlling steady flight of unmanned aircraft | |
CN105014687A (zh) | 一种带有多旋翼无人机的机械臂 | |
CN104656664B (zh) | 车载多旋翼无人直升机着陆导引控制***及导引控制方法 | |
CN105691613B (zh) | 一种平稳升降的无人机装置及控制方法 | |
CN106347650A (zh) | 一种纵列双涵道式飞行机器人及其动力学协同控制方法 | |
CN108415460B (zh) | 一种组合分离式旋翼与足式移动操作机器人集中-分布式控制方法 | |
CN107933915A (zh) | 一种基于六旋翼无人机的空中机器人 | |
CN205428282U (zh) | 自转旋翼机模拟器 | |
CN107219857A (zh) | 一种基于三维全局人工势函数的无人机编队路径规划算法 | |
Park et al. | Ric (robust internal-loop compensator) based flight control of a quad-rotor type uav | |
CN107121256A (zh) | 一种连续在轨运动的六自由度捕获轨迹试验方法 | |
CN206679256U (zh) | 飞行机械臂及无人机*** | |
CN106155076A (zh) | 一种多旋翼无人飞行器的稳定飞行控制方法 | |
CN104035338A (zh) | 一种多发动机的直升机动力***动态匹配的仿真方法 | |
CN111137082A (zh) | 单涵道陆空跨域机器人及其控制方法 | |
CN106371442A (zh) | 一种基于张量积模型变换的移动机器人控制方法 | |
CN105676859A (zh) | 无人机三自由度空中自主加油装置及其实施方法 | |
CN108556577B (zh) | 一种陆空两用球形机器人 | |
CN114967719A (zh) | 一种结合单足弹跳机器人的四旋翼无人机及控制方法 | |
CN112061390B (zh) | 仿蝙蝠柔性变形扑翼机器人的飞行控制装置及控制方法 | |
CN108313291A (zh) | 一种全向飞行器 | |
CN115343949B (zh) | 一种固定翼无人机跟踪制导律设计方法及验证平台 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |