CN109992014A - 一种斜坡路面下爬行机器人的视觉导航云台装置及控制 - Google Patents
一种斜坡路面下爬行机器人的视觉导航云台装置及控制 Download PDFInfo
- Publication number
- CN109992014A CN109992014A CN201910331172.XA CN201910331172A CN109992014A CN 109992014 A CN109992014 A CN 109992014A CN 201910331172 A CN201910331172 A CN 201910331172A CN 109992014 A CN109992014 A CN 109992014A
- Authority
- CN
- China
- Prior art keywords
- holder
- stepper motor
- forearm
- arm
- revolving speed
- 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
- 230000009194 climbing Effects 0.000 title claims abstract description 21
- 210000000245 forearm Anatomy 0.000 claims abstract description 80
- 230000000007 visual effect Effects 0.000 claims abstract description 17
- NJPPVKZQTLUDBO-UHFFFAOYSA-N novaluron Chemical compound C1=C(Cl)C(OC(F)(F)C(OC(F)(F)F)F)=CC=C1NC(=O)NC(=O)C1=C(F)C=CC=C1F NJPPVKZQTLUDBO-UHFFFAOYSA-N 0.000 claims abstract description 15
- 238000005457 optimization Methods 0.000 claims abstract description 10
- 239000002184 metal Substances 0.000 claims description 27
- 229910052751 metal Inorganic materials 0.000 claims description 27
- 230000008859 change Effects 0.000 claims description 7
- 238000004891 communication Methods 0.000 claims description 6
- 230000009514 concussion Effects 0.000 claims description 6
- 230000006872 improvement Effects 0.000 claims description 6
- 238000000034 method Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 4
- PCHJSUWPFVWCPO-UHFFFAOYSA-N gold Chemical compound [Au] PCHJSUWPFVWCPO-UHFFFAOYSA-N 0.000 claims description 3
- 239000010931 gold Substances 0.000 claims description 3
- 229910052737 gold Inorganic materials 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 230000009467 reduction Effects 0.000 claims description 3
- 229920006395 saturated elastomer Polymers 0.000 claims description 3
- 239000007787 solid Substances 0.000 claims description 2
- 238000013459 approach Methods 0.000 abstract description 2
- 230000035939 shock Effects 0.000 abstract 1
- 230000001429 stepping effect Effects 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 238000013461 design Methods 0.000 description 3
- 230000005611 electricity Effects 0.000 description 3
- 230000007246 mechanism Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 210000001367 artery Anatomy 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 210000003462 vein Anatomy 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
- G01C11/02—Picture taking arrangements specially adapted for photogrammetry or photographic surveying, e.g. controlling overlapping of pictures
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D3/00—Control of position or direction
- G05D3/12—Control of position or direction using feedback
- G05D3/20—Control of position or direction using feedback using a digital comparing device
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Multimedia (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Feedback Control In General (AREA)
- Control Of Position Or Direction (AREA)
Abstract
一种斜坡路面下爬行机器人的视觉导航云台装置及控制属于爬行机器人领域；解决了现有技术中视觉导航云台装置获取导航图像不全和控制精度差的问题；所述视觉导航云台装置包括：视觉传感器、云台小臂、云台中臂、云台大臂、云台底座和控制系统；所述云台装置增加了横滚角方向的转动，提高了云台装置获取导航图像时的转向灵活性；所述控制系统针对爬行机器人在斜坡路面导航时受地面冲击的影响，云台装置会有一定程度的抖动，采用一种基于转速偏差变化率优化的模糊算法，通过对步进电机转速偏差变化率的取值进行优化，降低了云台装置的抖动情况，能够准确稳定地完成航向角、横滚角和俯仰角方向的转动，使获取的导航图像更完整，控制精度更高。
Description
技术领域
本发明属于爬行机器人领域，尤其涉及一种斜坡路面下爬行机器人的视觉导航云台装置及控制。
背景技术
我国具有复杂的地理环境，多山地和丘陵等非结构化环境，爬行机器人在斜坡路面导航时，视觉导航云台装置对机器人导航和避障起着至关重要的作用，而现有技术中，视觉导航云台存在获取导航图像不全和控制精度差的问题。
爬行机器人在斜坡路面导航时，视觉传感器需要调整一定角度对姿态进行补偿，现有技术中，视觉导航云台装置一般采用两个自由度，缺乏有效转动方向的自由度设计，使其在航向角、横滚角和俯仰角的方向上视野受限，降低了云台装置的灵活性进而导致获取图像不全。
视觉导航云台控制系统一般采用常规的PID算法，但在斜坡路面导航时受地面冲击的影响，云台装置会有一定程度的抖动，PID控制精度以及稳定性较差，同时，模糊控制具有参数自整定能力可提高控制精度，但用于斜坡路面时，存在因积分项的饱和、跳变而引起的不稳定震荡和波动现象。
发明内容
本发明克服了上述现有技术上的不足，提供一种斜坡路面下爬行机器人的视觉导航云台装置及控制，机构设计上增加了横滚角方向的转动，提高了云台装置转向的灵活性，使视觉传感器获取充足的导航图像信息；在控制系统上建立视觉导航云台运动学模型，对模糊PID算法的转速偏差变化率取值进行了优化，可有效避免因积分项的饱和、跳变而引起的不稳定震荡和波动现象，使视觉导航云台装置的控制精度更高。
为了解决上述问题，本发明的第一目的在于提供一种斜坡路面下爬行机器人的视觉导航云台装置，第二目的在于提供一种斜坡路面下爬行机器人视觉导航云台的控制方法。
本发明所采取的第一技术方案是：
一种斜坡路面下爬行机器人的视觉导航云台装置，包括：视觉传感器、云台小臂、云台中臂、云台大臂、云台底座和控制系统；所述视觉传感器安装在云台小臂上；所述云台小臂安装在云台中臂上；所述云台中臂安装在云台大臂上；所述云台大臂安装在云台底座上；所述控制系统安装在云台底座上；
所述云台小臂包括云台小臂步进电机、云台小臂步进电机架、云台小臂金属连接盘、云台小臂L型支架；所述云台小臂步进电机与云台小臂步进电机架固定连接，所述云台小臂步进电机与云台小臂金属连接盘固定连接，所述云台小臂金属连接盘与云台小臂L型支架固定连接，所述云台小臂L型支架与视觉传感器固定连接。
所述云台中臂包括云台中臂步进电机、云台中臂步进电机架、云台中臂金属连接盘；所述云台中臂步进电机与云台中臂步进电机架固定连接，所述云台中臂步进电机与云台中臂金属连接盘固定连接，所述云台中臂金属连接盘与云台小臂步进电机架固定连接。
所述云台大臂包括云台大臂步进电机、云台大臂步进电机架、云台大臂金属连接盘；所述云台大臂步进电机与云台大臂步进电机架固定连接，所述云台大臂步进电机与云台大臂金属连接盘固定连接，所述云台大臂金属连接盘与云台中臂步进电机架固定连接，所述云台大臂步进电机架与云台底座固定连接。
所述控制系统包括主控芯片、云台小臂步进电机驱动芯片、云台中臂步进电机驱动芯片、云台大臂步进电机驱动芯片和六轴传感器芯片；所述主控芯片分别与云台小臂步进电机驱动芯片、云台中臂步进电机驱动芯片、云台大臂步进电机驱动芯片和六轴传感器芯片控制连接。
进一步地，所述主控芯片与步进电机驱动芯片采用共阳极接法，主控芯片的PA0、PA1引脚分别与云台小臂步进电机驱动芯片的DIR-、PUL-引脚连接，用于给云台小臂步进电机驱动芯片脉冲信号和方向信号的输入；所述主控芯片的PA2、PA3引脚分别与云台中臂步进电机驱动芯片的DIR-、PUL-引脚连接，用于云台中臂步进电机驱动芯片脉冲信号和方向信号的输入；所述主控芯片的PA4、PA5引脚分别与云台大臂步进电机驱动芯片的DIR-、PUL-引脚连接，用于云台大臂步进电机驱动芯片脉冲信号和方向信号的输入；所述主控芯片的PB8、PB9分别与六轴传感器芯片的SCL、SDA引脚连接，用于主控芯片和六轴传感器芯片IIC时钟通信和数据通信，通过DMP获取六轴传感器芯片的航向角、横滚角和俯仰角姿态数据。
进一步地，所述云台小臂步进电机驱动芯片的A+、A-、B+、B-引脚分别与云台小臂步进电机的MOTOR1A+、MOTOR1A-、MOTOR1B+、MOTOR1B-引脚连接，用于控制云台小臂步进电机脉冲信号和正反转；所述云台中臂步进电机驱动芯片的A+、A-、B+、B-引脚分别与云台中臂步进电机的MOTOR2A+、MOTOR2A-、MOTOR2B+、MOTOR2B-引脚连接，用于控制云台中臂步进电机脉冲信号和正反转；所述云台大臂步进电机驱动芯片的A+、A-、B+、B-引脚分别与云台大臂步进电机的MOTOR3A+、MOTOR3A-、MOTOR3B+、MOTOR3B-引脚连接，用于控制云台大臂步进电机脉冲信号和正反转。
本发明所采取的第二技术方案是：
一种斜坡路面下爬行机器人视觉导航云台的控制方法，包括以下步骤：
步骤S1、根据斜面坡度θ建立视觉导航云台装置的运动学控制模型；
步骤S2、设置水平初始位姿，获取云台当前状态；
步骤S3、六轴传感器芯片捕捉到位姿变化，并将位姿信息传递给主控芯片；
步骤S4、主控芯片通过输入的六轴传感器芯片位姿信息和步进电机反馈的信号，采用模糊PID算法输出控制信号；
步骤S5、根据步进电机接收的控制信号，控制视觉导航云台分别在横滚角、俯仰角和航向角方向精确转动一定角度，使云台保持水平稳定状态，并获取充足的导航图像。
进一步地，所述步骤S1具体包括：
对视觉导航云台装置建立坐标系和DH参数表，并计算出该装置的i-1Ti坐标变换矩阵；
| 关节i | θ<sub>i</sub> | α<sub>i-1</sub> | d<sub>i-1</sub> | a<sub>i-1</sub> |
| 1 | θ<sub>1</sub> | 0 | 0 | 0 |
| 2 | θ<sub>2</sub> | -90° | d<sub>2</sub> | 0 |
| 3 | θ<sub>3</sub> | -90° | d<sub>3</sub> | 0 |
| 4 | θ<sub>4</sub> | 0 | 0 | a<sub>4</sub> |
视觉导航云台装置的运动学方程为：
云台末端相对于地面的位姿为：
其中，si、ci分别代表sinθi、cosθi，代表相对于俯仰关节，云台末端的位姿矩阵，代表相对于横滚关节，俯仰关节的位姿矩阵，代表相对于航向关节，横滚关节的位姿矩阵，代表相对于基座，航向关节的为子矩阵，代表相对于基座，云台末端的位姿矩阵，θ代表斜面坡度；
进一步地，由转速偏差e和转速偏差率ec得出模糊控制表，并对控制参数进行参数整定；当转速偏差e较大时，需要较快的跟踪性能，因而比例增量ΔKp给定数值较大；另外，在转速偏差较大且转速偏差减小率也逐渐减小的过程中，ΔKp给出的值也需要逐步减小；同样的，当e一定时，转速偏差变化增大的过程中，ΔKp也随之增大；而在转速偏差较小，同时转速偏差率也较小的情况下，可以减小ΔKp以防止过度震荡；
对于微分增量ΔKD：转速偏差的微分就是转速偏差的变化速度，速度越大则微分绝对值越大；一般地，控制器输出的微分部分和转速偏差的微分成正比，用于反应被控量的变化趋势；在转速偏差e较大时，可选择较小的ΔKD，防止变化过大造成微分项饱和；对于转速偏差处于中间位置时，ΔKD应该和转速偏差率ec呈反向选择；当e较小，即准备达到目标位置时，一般ΔKD取小，同时与ec的大小稍微反向变化一点；
积分增量ΔKI趋势上大体与ΔKD相同，但为了保证视觉导航云台装置控制系统的平稳性和稳定性，需要对模糊PID控制的转速偏差变化率ec的取值进行优化；
| NB | NM | NS | ZO | PS | PM | PB | |
| NB | PB/NB/PS | PB/NB/NS | PM/NM/NB | PM/NM/NB | PS/NS/NM | ZO/ZO/NM | ZO/ZO/PS |
| NM | PB/NB/PS | PB/NB/NS | PM/NM/NB | PS/NS/NS | PS/NS/NS | ZO/ZO/NS | NS/ZO/ZO |
| NS | PM/NB/ZO | PM/NM/NS | PM/NS/NM | ZO/ZO/NS | ZO/ZO/NS | NS/PS/NS | NS/PS/ZO |
| ZO | PM/NM/ZO | PM/NM/NS | PS/NS/NS | NS/PS/NS | NS/PS/PS | NM/PM/NS | NM/PM/ZO |
| PS | PS/NM/ZO | PS/NS/ZO | ZO/ZO/ZO | NS/PS/ZO | NS/PS/ZO | NM/PM/ZO | NM/PM/ZO |
| PM | PS/ZO/PB | ZO/ZO/NS | NS/PS/PS | NM/PM/PS | NM/PM/PS | NM/PB/PS | NB/PB/PB |
| PB | ZO/ZO/PB | ZO/ZO/PM | NM/PS/PM | NM/PM/PS | NM/PM/PS | NB/PB/PS | NB/PB/PB |
进一步地，所述步骤S4具体包括：
S401、优化转速偏差变化率ec的取值；当视觉导航云台控制系统存在一个转速偏差，视觉导航云台转速达到最大值，不能再增加转速，改进是：对积分项ec(i)做出处理，规定(ec)max＝40，计算ec(i)：若ec(i-1)>(ec)max，ec(i)<0项继续加和，否则舍弃；ec(i-1)<(ec)min时，ec(i)>0项继续加和，否则舍弃，可有效避免积分项饱和而发生不稳定震荡等现象；视觉导航云台装置的位置环建立在速度环上，当给定目标参数时，速度环的控制无法在极短的时间内达到目标值，会造成ec(i)累积，导致目标参数急速上升，位置控制时会发生速度波动较大的情况，改进是：给定a＝0.6m，b＝1.6系数和ec(i)上下限max和min，若ec(i)>max则ec(i)＝a×ec(i)，若ec(i)<min，则ec(i)＝b×ec(i)，则ec(i)被限定在区间内，不会因积分项跳变而发生波动现象；S402、模糊控制器输入转速偏差e和优化后的转速偏差变化率ec，通过模糊控制器输出比例增益K′P、积分增益K′I和差分增益K′D的增量ΔKP、ΔKI和ΔKD到PID控制器；
S403、PID控制器通过原始的K′P、K′I和K′D参数及其调整后的增量进行融合，控制步进电机转速趋于平滑；
进一步地，所述PID控制器控制方程为：
控制器的输出
控制偏差e(t)＝rin(t)-yout(t)；
其中，rin(t)为给定值，yout(t)为实际输出值；
进一步地，由输入变量e和ec来修正PID控制器的参数KP、KI和KD，改进后的计算公式为：
其中K′P、K′I和K′D为原始PID参数，ΔKP、ΔKI和ΔKD为模糊控制增量。
本发明相对于现有技术具有以下有益效果：
本发明提供了一种斜坡路面下爬行机器人的视觉导航云台装置及控制，机构设计上增加了横滚角方向的转动，提高了云台装置转向的灵活性，使视觉传感器获取充足的导航图像信息；在控制系统上建立视觉导航云台运动学模型，对模糊PID算法的转速偏差变化率进行了优化，可有效避免因积分项的饱和、跳变而引起的不稳定震荡和波动现象，使视觉导航云台装置的控制精度更高。
附图说明
图1是本发明装置主视图；
图2是本发明装置俯视图；
图3是本发明装置整体结构图；
图4是本发明控制系统主控芯片电路图；
图5是本发明云台小臂步进电机驱动芯片电路图；
图6是本发明云台中臂步进电机驱动芯片电路图；
图7是本发明云台大臂步进电机驱动芯片电路图；
图8是本发明六轴传感器芯片电路图；
图9是本发明运动学模型图；
图10是本发明工作流程图；
图11是本发明一种基于转速偏差变化率优化的模糊算法示意图；
图中：1视觉传感器、2云台小臂、3云台中臂、4云台大臂、5云台底座和6控制系统、21云台小臂步进电机、22云台小臂步进电机架、23云台小臂金属连接盘、24云台小臂L型支架、31云台中臂步进电机、32云台中臂步进电机架、33云台中臂金属连接盘、41云台大臂步进电机、42云台大臂步进电机架、43云台大臂金属连接盘。
具体实施方式
以下将结合附图对本发明进行详细说明。
具体实施方式一
一种斜坡路面下爬行机器人的视觉导航云台装置，如图1-3所示，包括1视觉传感器、2云台小臂、3云台中臂、4云台大臂、5云台底座和6控制系统；所述1视觉传感器安装在2云台小臂上；所述2云台小臂安装在3云台中臂上；所述3云台中臂安装在4云台大臂上；所述4云台大臂安装在5云台底座上；所述6控制系统安装在5云台底座上。
具体实施方式二
如图1-3所示，在具体实施方式一的基础上，一种斜坡路面下爬行机器人的视觉导航云台装置包括1视觉传感器、2云台小臂、3云台中臂、4云台大臂、5云台底座和6控制系统，所述2云台小臂包括21云台小臂步进电机、22云台小臂步进电机架、23云台小臂金属连接盘、24云台小臂L型支架；所述21云台小臂步进电机与22云台小臂步进电机架固定连接，所述21云台小臂步进电机与23云台小臂金属连接盘固定连接，所述23云台小臂金属连接盘与24云台小臂L型支架固定连接，所述24云台小臂L型支架与1视觉传感器固定连接。
本实施方式中1视觉传感器为现有技术，生产厂家为微软公司，型号为Kinect v1。
本实施方式中21云台小臂步进电机为现有技术，生产厂家为TELESKY，型号为42BYGH24。
所述3云台中臂包括31云台中臂步进电机、32云台中臂步进电机架、33云台中臂金属连接盘；所述31云台中臂步进电机与32云台中臂步进电机架固定连接，所述31云台中臂步进电机与33云台中臂金属连接盘固定连接，所述33云台中臂金属连接盘与22云台小臂步进电机架固定连接。
本实施方式中31云台中臂步进电机为现有技术，生产厂家为TELESKY，型号为42BYGH24。
所述4云台大臂包括41云台大臂步进电机、42云台大臂步进电机架、43云台大臂金属连接盘；所述41云台大臂步进电机与42云台大臂步进电机架固定连接，所述41云台大臂步进电机与43云台大臂金属连接盘固定连接，所述43云台大臂金属连接盘与32云台中臂步进电机架固定连接，所述42云台大臂步进电机架与5云台底座固定连接。
本实施方式中41云台大臂步进电机为现有技术，生产厂家为TELESKY，型号为42BYGH24。
如图4-8所示，所述6控制系统包括主控芯片、云台小臂步进电机驱动芯片、云台中臂步进电机驱动芯片、云台大臂步进电机驱动芯片和六轴传感器芯片；所述主控芯片分别与云台小臂步进电机驱动芯片、云台中臂步进电机驱动芯片、云台大臂步进电机驱动芯片和六轴传感器芯片控制连接；所述主控芯片与步进电机驱动芯片采用共阳极接法，主控芯片的PA0、PA1引脚分别与云台小臂步进电机驱动芯片的DIR-、PUL-引脚连接，用于云台小臂步进电机驱动芯片脉冲信号和方向信号的输入；所述主控芯片的PA2、PA3引脚分别与云台中臂步进电机驱动芯片的DIR-、PUL-引脚连接，用于云台中臂步进电机驱动芯片脉冲信号和方向信号的输入；所述主控芯片的PA4、PA5引脚分别与云台大臂步进电机驱动芯片的DIR-、PUL-引脚连接，用于给云台大臂步进电机驱动芯片脉冲信号和方向信号的输入；所述主控芯片的PB8、PB9分别与六轴传感器芯片的SCL、SDA引脚连接，用于主控芯片和六轴传感器芯片IIC时钟通信和数据通信，通过DMP获取六轴传感器芯片的航向角、横滚角和俯仰角姿态数据；所述云台小臂步进电机驱动芯片的A+、A-、B+、B-引脚分别与云台小臂步进电机的MOTOR1A+、MOTOR1A-、MOTOR1B+、MOTOR1B-引脚连接，用于控制云台小臂步进电机脉冲信号和正反转；所述云台中臂步进电机驱动芯片的A+、A-、B+、B-引脚分别与云台中臂步进电机的MOTOR2A+、MOTOR2A-、MOTOR2B+、MOTOR2B-引脚连接，用于控制云台中臂步进电机脉冲信号和正反转；所述云台大臂步进电机驱动芯片的A+、A-、B+、B-引脚分别与云台大臂步进电机的MOTOR3A+、MOTOR3A-、MOTOR3B+、MOTOR3B-引脚连接，用于控制云台大臂步进电机脉冲信号和正反转。
本实施方式中主控芯片为现有技术，生产厂家为意法半导体，型号为STM32F407。
本实施方式中云台小臂步进电机驱动芯片为现有技术，生产厂家为TELESKY，型号为TB6600。
本实施方式中云台中臂步进电机驱动芯片为现有技术，生产厂家为TELESKY，型号为TB6600。
本实施方式中云台大臂步进电机驱动芯片为现有技术，生产厂家为TELESKY，型号为TB6600。
本实施方式中六轴传感器芯片为现有技术，生产厂家为Risym，型号为ATK-MPU6050。
工作过程：
所述视觉导航云台装置设置水平初始位姿并获取当前位姿状态，六轴传感器芯片检测到1视觉传感器位姿的变化，将六轴传感器芯片获取的位姿信息和云台各步进电机的角度信息传递给主控芯片，主控芯片对输入的数据采用一种基于转速偏差变化率优化的模糊算法进行数据处理，获取当前采样值i(k)和o(k)，计算误差e和误差变化率ec，优化转速偏差变化率ec的取值，对e(i)和ec(i)模糊化处理，根据模糊规则查表，计算ΔKp、ΔKI、ΔKD的值，PID控制器通过原始的K′P、K′I和K′D参数及其调整后的增量ΔKp、ΔKI、ΔKD的值进行融合，通过云台小臂步进电机驱动芯片、云台中臂步进电机驱动芯片和云台大臂步进电机驱动芯片输出步进电机的控制信号，控制21云台小臂步进电机、31云台中臂步进电机和41云台大臂步进电机分别在横滚角、俯仰角和航向角方向精确转动一定角度，使1视觉传感器保持水平稳定状态，并获取充足的导航图像。
具体实施方式三
如图9-11所示，一种斜坡路面下爬行机器人视觉导航云台的控制方法，包括以下步骤：
步骤S1、根据斜面坡度θ建立视觉导航云台装置的运动学控制模型；
步骤S2、设置水平初始位姿，获取云台当前状态；
步骤S3、六轴传感器芯片捕捉到位姿变化，并将位姿信息传递给主控芯片；
步骤S4、主控芯片通过输入的六轴传感器芯片位姿信息和步进电机反馈的信号，采用模糊PID算法输出控制信号；
步骤S5、根据步进电机接收的控制信号，控制视觉导航云台分别在横滚角、俯仰角和航向角方向精确转动一定角度，使云台保持水平稳定状态，并获取充足的导航图像；
进一步地，所述步骤S1具体包括：
对视觉导航云台装置建立坐标系和DH参数表，并计算出该装置的i-1Ti坐标变换矩阵；
| 关节i | θ<sub>i</sub> | α<sub>i-1</sub> | d<sub>i-1</sub> | a<sub>i-1</sub> |
| 1 | θ<sub>1</sub> | 0 | 0 | 0 |
| 2 | θ<sub>2</sub> | -90° | d<sub>2</sub> | 0 |
| 3 | θ<sub>3</sub> | -90° | d<sub>3</sub> | 0 |
| 4 | θ<sub>4</sub> | 0 | 0 | a<sub>4</sub> |
视觉导航云台装置的运动学方程为：
云台末端相对于地面的位姿为：
其中，si、ci分别代表sinθi、cosθi，代表相对于俯仰关节，云台末端的位姿矩阵，代表相对于横滚关节，俯仰关节的位姿矩阵，代表相对于航向关节，横滚关节的位姿矩阵，代表相对于基座，航向关节的为子矩阵，代表相对于基座，云台末端的位姿矩阵，θ代表斜面坡度；
进一步地，由转速偏差e和转速偏差率ec得出模糊控制表，并对控制参数进行参数整定；当转速偏差e较大时，需要较快的跟踪性能，因而比例增量ΔKp给定数值较大；另外，在转速偏差较大且转速偏差减小率也逐渐减小的过程中，ΔKp给出的值也需要逐步减小；同样的，当e一定时，转速偏差变化增大的过程中，ΔKp也随之增大；而在转速偏差较小，同时转速偏差率也较小的情况下，可以减小ΔKp以防止过度震荡；
对于微分增量ΔKD：转速偏差的微分就是转速偏差的变化速度，速度越大则微分绝对值越大；一般地，控制器输出的微分部分和转速偏差的微分成正比，用于反应被控量的变化趋势；在转速偏差e较大时，可选择较小的ΔKD，防止变化过大造成微分项饱和；对于转速偏差处于中间位置时，ΔKD应该和转速偏差率ec呈反向选择；当e较小，即准备达到目标位置时，一般ΔKD取小，同时与ec的大小稍微反向变化一点；
积分增量ΔKI趋势上大体与ΔKD相同，但为了保证视觉导航云台装置控制系统的平稳性和稳定性，需要对模糊PID控制的转速偏差变化率ec的取值进行优化；
| NB | NM | NS | ZO | PS | PM | PB | |
| NB | PB/NB/PS | PB/NB/NS | PM/NM/NB | PM/NM/NB | PS/NS/NM | ZO/ZO/NM | ZO/ZO/PS |
| NM | PB/NB/PS | PB/NB/NS | PM/NM/NB | PS/NS/NS | PS/NS/NS | ZO/ZO/NS | NS/ZO/ZO |
| NS | PM/NB/ZO | PM/NM/NS | PM/NS/NM | ZO/ZO/NS | ZO/ZO/NS | NS/PS/NS | NS/PS/ZO |
| ZO | PM/NM/ZO | PM/NM/NS | PS/NS/NS | NS/PS/NS | NS/PS/PS | NM/PM/NS | NM/PM/ZO |
| PS | PS/NM/ZO | PS/NS/ZO | ZO/ZO/ZO | NS/PS/ZO | NS/PS/ZO | NM/PM/ZO | NM/PM/ZO |
| PM | PS/ZO/PB | ZO/ZO/NS | NS/PS/PS | NM/PM/PS | NM/PM/PS | NM/PB/PS | NB/PB/PB |
| PB | ZO/ZO/PB | ZO/ZO/PM | NM/PS/PM | NM/PM/PS | NM/PM/PS | NB/PB/PS | NB/PB/PB |
进一步地，所述步骤S4具体包括：
S401、优化转速偏差变化率ec的取值；当视觉导航云台控制系统存在一个转速偏差，视觉导航云台转速达到最大值，不能再增加转速，改进是：对积分项ec(i)做出处理，规定(ec)max＝40，计算ec(i)：若ec(i-1)>(ec)max，ec(i)<0项继续加和，否则舍弃；ec(i-1)<(ec)min时，ec(i)>0项继续加和，否则舍弃，可有效避免积分项饱和而发生不稳定震荡等现象；视觉导航云台装置的位置环建立在速度环上，当给定目标参数时，速度环的控制无法在极短的时间内达到目标值，会造成ec(i)累积，导致目标参数急速上升，位置控制时会发生速度波动较大的情况，改进是：给定a＝0.6m，b＝1.6系数和ec(i)上下限max和min，若ec(i)>max则ec(i)＝a×ec(i)，若ec(i)<min，则ec(i)＝b×ec(i)，则ec(i)被限定在区间内，不会因积分项跳变而发生波动现象；S402、模糊控制器输入转速偏差e和优化后的转速偏差变化率ec，通过模糊控制器输出比例增益K′P、积分增益K′I和差分增益K′D的增量ΔKP、ΔKI和ΔKD到PID控制器；
S403、PID控制器通过原始的K′P、K′I和K′D参数及其调整后的增量进行融合，控制步进电机转速趋于平滑；
进一步地，所述PID控制器控制方程为：
控制器的输出
控制偏差e(t)＝rin(t)-yout(t)；
其中，rin(t)为给定值，yout(t)为实际输出值；
进一步地，由输入变量e和ec来修正PID控制器的参数KP、KI和KD，改进后的计算公式为：
其中K′P、K′I和K′D为原始PID参数，ΔKP、ΔKI和ΔKD为模糊控制增量。
Claims (3)
1.一种斜坡路面下爬行机器人的视觉导航云台装置，包括(1)视觉传感器、(2)云台小臂、(3)云台中臂、(4)云台大臂、(5)云台底座和(6)控制系统；所述(1)视觉传感器安装在(2)云台小臂上；所述(2)云台小臂安装在(3)云台中臂上；所述(3)云台中臂安装在(4)云台大臂上；所述(4)云台大臂安装在(5)云台底座上；所述(6)控制系统安装在(5)云台底座上。
2.根据权利要求1所述一种斜坡路面下爬行机器人的视觉导航云台装置，其特征在于，所述(2)云台小臂包括(21)云台小臂步进电机、(22)云台小臂步进电机架、(23)云台小臂金属连接盘、(24)云台小臂L型支架；所述(21)云台小臂步进电机与(22)云台小臂步进电机架固定连接，所述(21)云台小臂步进电机与(23)云台小臂金属连接盘固定连接，所述(23)云台小臂金属连接盘与(24)云台小臂L型支架固定连接，所述(24)云台小臂L型支架与(1)视觉传感器固定连接；
所述(3)云台中臂包括(31)云台中臂步进电机、(32)云台中臂步进电机架、(33)云台中臂金属连接盘；所述(31)云台中臂步进电机与(32)云台中臂步进电机架固定连接，所述(31)云台中臂步进电机与(33)云台中臂金属连接盘固定连接，所述(33)云台中臂金属连接盘与(22)云台小臂步进电机架固定连接；
所述(4)云台大臂包括(41)云台大臂步进电机、(42)云台大臂步进电机架、(43)云台大臂金属连接盘；所述(41)云台大臂步进电机与(42)云台大臂步进电机架固定连接，所述(41)云台大臂步进电机与(43)云台大臂金属连接盘固定连接，所述(43)云台大臂金属连接盘与(32)云台中臂步进电机架固定连接，所述(42)云台大臂步进电机架与(5)云台底座固定连接；
所述(6)控制系统包括主控芯片、云台小臂步进电机驱动芯片、云台中臂步进电机驱动芯片、云台大臂步进电机驱动芯片和六轴传感器芯片；所述主控芯片分别与云台小臂步进电机驱动芯片、云台中臂步进电机驱动芯片、云台大臂步进电机驱动芯片和六轴传感器芯片控制连接；所述主控芯片与步进电机驱动芯片采用共阳极接法，主控芯片的PA0、PA1引脚分别与云台小臂步进电机驱动芯片的DIR-、PUL-引脚连接，用于云台小臂步进电机驱动芯片脉冲信号和方向信号的输入；所述主控芯片的PA2、PA3引脚分别与云台中臂步进电机驱动芯片的DIR-、PUL-引脚连接，用于云台中臂步进电机驱动芯片脉冲信号和方向信号的输入；所述主控芯片的PA4、PA5引脚分别与云台大臂步进电机驱动芯片的DIR-、PUL-引脚连接，用于给云台大臂步进电机驱动芯片脉冲信号和方向信号的输入；所述主控芯片的PB8、PB9分别与六轴传感器芯片的SCL、SDA引脚连接，用于主控芯片和六轴传感器芯片IIC时钟通信和数据通信，通过DMP获取六轴传感器芯片的航向角、横滚角和俯仰角姿态数据；所述云台小臂步进电机驱动芯片的A+、A-、B+、B-引脚分别与云台小臂步进电机的MOTOR1A+、MOTOR1A-、MOTOR1B+、MOTOR1B-引脚连接，用于控制云台小臂步进电机脉冲信号和正反转；所述云台中臂步进电机驱动芯片的A+、A-、B+、B-引脚分别与云台中臂步进电机的MOTOR2A+、MOTOR2A-、MOTOR2B+、MOTOR2B-引脚连接，用于控制云台中臂步进电机脉冲信号和正反转；所述云台大臂步进电机驱动芯片的A+、A-、B+、B-引脚分别与云台大臂步进电机的MOTOR3A+、MOTOR3A-、MOTOR3B+、MOTOR3B-引脚连接，用于控制云台大臂步进电机脉冲信号和正反转。
3.一种基于权利要求1至2任一所述一种斜坡路面下爬行机器人的视觉导航云台的控制方法，其特征在于，包括以下步骤：
步骤S1、根据斜面坡度θ建立视觉导航云台装置的运动学控制模型；
步骤S2、设置水平初始位姿，获取云台当前状态；
步骤S3、六轴传感器芯片捕捉到位姿变化，并将位姿信息传递给主控芯片；
步骤S4、主控芯片通过输入的六轴传感器芯片位姿信息和步进电机反馈的信号，采用模糊PID算法输出控制信号；
步骤S5、根据步进电机接收的控制信号，控制视觉导航云台分别在横滚角、俯仰角和航向角方向精确转动一定角度，使云台保持水平稳定状态，并获取充足的导航图像；
进一步地，所述步骤S1具体包括：
对视觉导航云台装置建立坐标系和DH参数表，并计算出该装置的i-1Ti坐标变换矩阵；
视觉导航云台装置的运动学方程为：
云台末端相对于地面的位姿为：
其中，si、ci分别代表sinθi、cosθi，代表相对于俯仰关节，云台末端的位姿矩阵，代表相对于横滚关节，俯仰关节的位姿矩阵，代表相对于航向关节，横滚关节的位姿矩阵，代表相对于基座，航向关节的为子矩阵，代表相对于基座，云台末端的位姿矩阵，θ代表斜面坡度；
进一步地，由转速偏差e和转速偏差率ec得出模糊控制表，并对控制参数进行参数整定；当转速偏差e较大时，需要较快的跟踪性能，因而比例增量ΔKp给定数值较大；另外，在转速偏差较大且转速偏差减小率也逐渐减小的过程中，ΔKp给出的值也需要逐步减小；同样的，当e一定时，转速偏差变化增大的过程中，ΔKp也随之增大；而在转速偏差较小，同时转速偏差率也较小的情况下，可以减小ΔKp以防止过度震荡；
对于微分增量ΔKD：转速偏差的微分就是转速偏差的变化速度，速度越大则微分绝对值越大；一般地，控制器输出的微分部分和转速偏差的微分成正比，用于反应被控量的变化趋势；在转速偏差e较大时，可选择较小的ΔKD，防止变化过大造成微分项饱和；对于转速偏差处于中间位置时，ΔKD应该和转速偏差率ec呈反向选择；当e较小，即准备达到目标位置时，一般ΔKD取小，同时与ec的大小稍微反向变化一点；
积分增量ΔKI趋势上大体与ΔKD相同，但为了保证视觉导航云台装置控制系统的平稳性和稳定性，需要对模糊PID控制的转速偏差变化率ec的取值进行优化；
进一步地，所述步骤S4具体包括：
S401、优化转速偏差变化率ec的取值；当视觉导航云台控制系统存在一个转速偏差，视觉导航云台转速达到最大值，不能再增加转速，改进是：对积分项ec(i)做出处理，规定(ec)max＝40，计算ec(i)：若ec(i-1)>(ec)max，ec(i)<0项继续加和，否则舍弃；ec(i-1)<(ec)min时，ec(i)>0项继续加和，否则舍弃，可有效避免积分项饱和而发生不稳定震荡等现象；视觉导航云台装置的位置环建立在速度环上，当给定目标参数时，速度环的控制无法在极短的时间内达到目标值，会造成ec(i)累积，导致目标参数急速上升，位置控制时会发生速度波动较大的情况，改进是：给定a＝0.6m，b＝1.6系数和ec(i)上下限max和min，若ec(i)>max则ec(i)＝a×ec(i)，若ec(i)<min，则ec(i)＝b×ec(i)，则ec(i)被限定在区间内，不会因积分项跳变而发生波动现象；
S402、模糊控制器输入转速偏差e和优化后的转速偏差变化率ec，通过模糊控制器输出比例增益K′P、积分增益K′I和差分增益K′D的增量ΔKP、ΔKI和ΔKD到PID控制器；
S403、PID控制器通过原始的K′P、K′I和K′D参数及其调整后的增量进行融合，控制步进电机转速趋于平滑；
进一步地，所述PID控制器控制方程为：
控制器的输出
控制偏差e(t)＝rin(t)-yout(t)；
其中，rin(t)为给定值，yout(t)为实际输出值；
进一步地，由输入变量e和ec来修正PID控制器的参数KP、KI和KD，改进后的计算公式为：
其中K′P、K′I和K′D为原始PID参数，ΔKP、ΔKI和ΔKD为模糊控制增量。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910331172.XA CN109992014B (zh) | 2019-04-24 | 2019-04-24 | 一种斜坡路面下爬行机器人的视觉导航云台装置及控制 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910331172.XA CN109992014B (zh) | 2019-04-24 | 2019-04-24 | 一种斜坡路面下爬行机器人的视觉导航云台装置及控制 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109992014A true CN109992014A (zh) | 2019-07-09 |
| CN109992014B CN109992014B (zh) | 2022-03-22 |
Family
ID=67132982
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910331172.XA Expired - Fee Related CN109992014B (zh) | 2019-04-24 | 2019-04-24 | 一种斜坡路面下爬行机器人的视觉导航云台装置及控制 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109992014B (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111798514A (zh) * | 2020-06-29 | 2020-10-20 | 山东大学日照智能制造研究院 | 一种海洋牧场智能运动目标跟踪监测方法及系统 |
| CN113721450A (zh) * | 2021-08-05 | 2021-11-30 | 杭州海康威视数字技术股份有限公司 | 一种终端设备及其控制方法和装置 |
| CN113911206A (zh) * | 2021-11-29 | 2022-01-11 | 中国人民解放军陆军装甲兵学院士官学校 | 一种农业机械的自动控制方法 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104679003A (zh) * | 2015-02-02 | 2015-06-03 | 东北大学 | 一种全方位移动云台机器人 |
| DE102014008385A1 (de) * | 2014-06-04 | 2015-12-17 | Mbda Deutschland Gmbh | Verfahren zum Ausrichten eines Kamerasvstems auf ein Zielobjekt und Kamerasystem |
| US20160029619A1 (en) * | 2014-07-30 | 2016-02-04 | The Governing Council Of The University Of Toronto | System and methods for automated vitrification of biological materials |
| CN106200693A (zh) * | 2016-08-12 | 2016-12-07 | 东南大学 | 土地调查小型无人机的云台实时控制系统及控制方法 |
| CN206141458U (zh) * | 2016-11-11 | 2017-05-03 | 张恒熙 | 一种警用车载稳定摄像装置 |
| CN107150329A (zh) * | 2017-04-27 | 2017-09-12 | 中国矿业大学 | 一种基于三自由度视觉平台的移动机器人及其控制方法 |
| CN107463187A (zh) * | 2017-09-21 | 2017-12-12 | 桂林师范高等专科学校 | 云台增稳系统 |
-
2019
- 2019-04-24 CN CN201910331172.XA patent/CN109992014B/zh not_active Expired - Fee Related
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102014008385A1 (de) * | 2014-06-04 | 2015-12-17 | Mbda Deutschland Gmbh | Verfahren zum Ausrichten eines Kamerasvstems auf ein Zielobjekt und Kamerasystem |
| US20160029619A1 (en) * | 2014-07-30 | 2016-02-04 | The Governing Council Of The University Of Toronto | System and methods for automated vitrification of biological materials |
| CN104679003A (zh) * | 2015-02-02 | 2015-06-03 | 东北大学 | 一种全方位移动云台机器人 |
| CN106200693A (zh) * | 2016-08-12 | 2016-12-07 | 东南大学 | 土地调查小型无人机的云台实时控制系统及控制方法 |
| CN206141458U (zh) * | 2016-11-11 | 2017-05-03 | 张恒熙 | 一种警用车载稳定摄像装置 |
| CN107150329A (zh) * | 2017-04-27 | 2017-09-12 | 中国矿业大学 | 一种基于三自由度视觉平台的移动机器人及其控制方法 |
| CN107463187A (zh) * | 2017-09-21 | 2017-12-12 | 桂林师范高等专科学校 | 云台增稳系统 |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111798514A (zh) * | 2020-06-29 | 2020-10-20 | 山东大学日照智能制造研究院 | 一种海洋牧场智能运动目标跟踪监测方法及系统 |
| CN113721450A (zh) * | 2021-08-05 | 2021-11-30 | 杭州海康威视数字技术股份有限公司 | 一种终端设备及其控制方法和装置 |
| CN113911206A (zh) * | 2021-11-29 | 2022-01-11 | 中国人民解放军陆军装甲兵学院士官学校 | 一种农业机械的自动控制方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109992014B (zh) | 2022-03-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109992014A (zh) | 一种斜坡路面下爬行机器人的视觉导航云台装置及控制 | |
| Wenzel et al. | Automatic take off, tracking and landing of a miniature UAV on a moving carrier vehicle | |
| JP6021470B2 (ja) | フリップ型運動を自動的に実行するための無人機の姿勢の動力学的制御方法 | |
| CN109333506B (zh) | 一种人形智能机器人系统 | |
| CN104811588B (zh) | 一种基于陀螺仪的船载稳像控制方法 | |
| CN101872198B (zh) | 车载摄像稳定平台 | |
| CN101872196B (zh) | 车载摄像稳定平台控制系统 | |
| JP2013144539A (ja) | 遠隔制御装置によって無人機を直観的に操縦するための方法 | |
| CN112987763A (zh) | 一种基于ros的自主导航机器人控制系统的智能小车 | |
| Achtelik et al. | Inversion based direct position control and trajectory following for micro aerial vehicles | |
| CN110785601A (zh) | 手持云台的控制方法和手持云台 | |
| CN108731681A (zh) | 旋翼无人机领航方法、相关计算机程序、电子设备和无人机 | |
| CN105620737A (zh) | 用于直升型飞行器的语音遥控系统及直升型遥控飞行器 | |
| CN106292716A (zh) | 一种攀岩用跟踪系统及跟踪方法 | |
| CN208888629U (zh) | 一种基于磁场检测的板球控制系统 | |
| CN110312078A (zh) | 一种自动环绕目标拍摄方法及系统 | |
| CN205396532U (zh) | 用于直升型飞行器的语音遥控系统及直升型遥控飞行器 | |
| CN109500817B (zh) | 多足机器人的360度视觉追踪控制系统及控制方法 | |
| CN116395167A (zh) | 一种仿生腿式起落装置运动控制系统 | |
| Baron et al. | Gesture and voice driven mobile tribot robot using Kinect sensor | |
| CN113110542B (zh) | 一种基于单轴云台的四旋翼无人机目标追踪系统 | |
| CN115060266A (zh) | 一种用于农田建图的定位系统 | |
| US11225306B2 (en) | Hydrodynamic intelligent robot and control method thereof | |
| CN109483548B (zh) | 一种智能跟踪对抗射击机器人及其控制方法 | |
| CN113867132A (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 | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20220322 |
|
| CF01 | Termination of patent right due to non-payment of annual fee |