Автореферат (Оптимизация алгоритмов управления автоматическим погрузочно-разгрузочным устройством), страница 4

6. Основные компоненты задачиразличным положениям ПРУ в пределаханализа изображениявозможного диапазона координат; 5)разработать алгоритм распознавания полученных эталонов, и по результатам работыалгоритма сделать вывод о наиболее вероятном положении ПРУ относительнобазовых точек ЛА, таким образом, скорректировав координаты ПРУ относительноточки загрузки ЛА.12Результаты анализа изображения элементов фиксации для определениявеличины Rmax и положения ПРУ рассмотрены в седьмой главе.XwOwZwOwXwXзZзZwб)а)Рис. 7.

Сцена получения изображения элементов фиксации в месте, близком точкизагрузки ЛА (а) и трехмерная модель элементов фиксации (б)Шестая глава посвящена реализации алгоритмов ИВК на основе БИНС иисследованию точностных характеристик.Алгоритм инерциальной навигации основан на решении двух классическихзадач теоретической механики:- решения кинематических уравнений Пуассона, определяющих изменение взаимнойориентации связанной (ССК, индекс B) и навигационной (НСК, индекс N) системNNBNNкоординат: C B  CB  ω BI    ω NI  CB1 N B 1 N NNили q B  q B  BI   NI q B ,22NNгде CB - матрица перехода от ССК в НСК, q B - кватернион поворота;- интегрирования динамическихNN BN*a NfBI  CBN a BfBI или a fBI  q B a fBI q B ,уравненийN*НьютонавНСК,причем:Nгде q B - сопряженный кватерниона q B .Схема общего алгоритма навигации показана на рис.

8.Начальные условияДУСω BBIилиДЛУa BfBIω NNIВычисление матрицынаправляющих косинусов иликватернионов пересчетаNBNCqBУгловаяскорость НСКNω EIОценкапараметровориентацииПеревод a BBI в НСКВычислениенавигационныхпараметровa NfBIgГNVBE, X N , YN , Z NОценкапараметровнавигацииНачальные условияОценкагравитационногоускоренияYNРис. 8. Схемаалгоритма работы БИНС( BIB - абсолютная угловаяскорость тележки,записанная в ССК; a BfBI кажущееся ускорениетележки, записанное в ССК;C BN - матрица перехода отССК в НСК; a NfBI - кажущеесяускорение тележки,записанное в НСК; NIN абсолютная угловая скоростьНСК, записанная в НСК; VBEN -скорость тележки относительно Земли, записанная в НСК; X N , YN , Z N - положение тележкиотносительно Земли, записанное в НСК,  - оценка географической широты)Угловая скорость и линейное ускорение измеряются датчиками ГИБ.Функциональные зависимости выходного сигнала датчиков ГИБ от измеряемой13величины имеют вид:1xимп .  1  k xм  x  x0  k xT T   xrand  ,Kxгде x - входная величина; xимп .

- выходная величина (количество импульсов всекунду, aимп. или ωимп. ); K x – номинальная цена импульса (коэффициентмTпреобразования); x0 - смещение нуля; k x - масштабный коэффициент; k x температурный коэффициент изменения смешения нуля;  xrand - случайнаясоставляющая ошибки измерения; T - отклонение текущей температуры датчиковот базовой температуры калибровки.Существуют формально два алгоритма реализации работы БИНС:1.

В «одношаговом» алгоритме для вычисления взаимной ориентации СК иинтегрирования уравнений навигации используется единый шаг по времени,определяемый частотой опроса датчиков ГИБ;2. «Интервальный» алгоритм учитывает наиболее распространенный видвыходных сигналов современных датчиков ГИБ, производящих осреднениемгновенных измерений (отчетов) на интервале времени.

При этом, частота опросадатчиков ГИБ, частота обработки первичных измерений и частота интегрированияуравнений навигации разные.В общей схеме моделирования работы БИНС содержатся три основанныемодули: модуль измерения, модуль обработки первичных измерений и модульнавигации (рис.

9).МАТЕМАТИЧЕСКАЯ МОДЕЛЬДВИЖЕНИЯ ТЕЛЕЖКИi  циклfизмl  цикл r ,ω BBIg BГ a ra n d  ra n dМОДУЛЬ ИЗМЕРЕНИЯПАСПОРТНЫЕ ДАННЫЕ(от калибровки)Матрица неортогональностей осейдатчиков ax ,  ay ,  az ,  x ,  y ,  zf обрk  циклa BfBIe , a, b, ev lBизмНеточностькалибровкиα lBизмη%Коэффициенты для обработкиизмерения ДЛУK ax , K ay , K az ; ax 0 , a y 0 , az 0 ;kaxм , kayм , kazм ; kaxT , kayT , kazTМОДУЛЬ ОБРАБОТКИ ПЕРВИЧНЫХИЗМЕРЕНИЙКоэффициенты для обработкиизмерения ДУСf навK x , K y , K z ;  x 0 ,  y 0 ,  z 0 ;Обработанныеинформацииkмx , kмy , kмz ; kT x , kT y , kT zНачальноесостояние тележкиМОДУЛЬ НАВИГАЦИИEIN , a, b, e,X n , Z n , VXn , VZn ,  , ,  ,  x1 ,  y1 ,  z1 ,Рис. 9. Общая схема алгоритма моделирования работы БИНС (  r ,  - параметрымикропрофиля опорной поверхности, e - угловая скорость вращения Земли, a , b , e - полуоси ипервый эксцентриситет Земли)14При моделировании «одношагового» алгоритма навигации модульизмерения выдает в качестве выходных своих данных мгновенные значенияизмеряемых линейных ускорений и угловых скоростей vlBизм  aимп.

, αlBизм  ωимп. .Частота обновления параметров ориентации и навигации и частота выдачиинформации от АЦП датчиков равны частоте опроса датчиков fизм  f обр  f нав .При «интервальном» алгоритме навигации выходными сигналами модуляизмерения являются суммирования отсчетов измеряемых параметров (в количествеимпульсов): vBlизмNi  int(aимп. изм ); αj 1BlизмNi  int(ωимп. изм ) , где- функция,int()j 1i  циклотбрасывающая дробную часть числа и возвращающая целое значение; Ni - число i– циклов в одном l – цикле, N i  f изм / f обр .Интервальный алгоритм реализуется в трех циклах с разными частотамиfизм  Ni f обр ; fобр  Nl f нав по схеме, показанной на рис.

10, где Ni , Nl - целые числа.iij 1j 1α iBизм   int(ωимп . изм ), v iBизм   int(aимп . изм )α lBизм , v lBизмα lB , v lBβ( α lBизм , vlBизм )BIl(α lB1 , α lB1 )( α lB )βlBI  βlBI1  βlBIПереданы в следующий циклk  циклSα , SvBBllBBB(αl 1 , vl 1 , αl 1 , vlB1 )( α lB , v lB ) RlsclA ,  RlsclB , Rlscl  Rlscl1   RlsclA   RlsclBBBBB(αlB1 , vlB1 , αlB1 , vlB1 , vlscl1 ,Sα l 1 ,Sv l 1 ) ( α l , v l , Sα l , Sv l ) v , v vlscl1   vlsclSα  Sα SαsclsclllBBB(αl 1 , vl 1 , αl 1 , vlB1 )Blα αBlBl 1Bl 1 αBlBl,( α lB , v lB )Sv  Sv, v vBlВ данном l-ом циклеl  цикл(  βlBI )BlBl 1 vBl 1 SvBl( Sα lB , Sv lB )Bl( α lB , v lB )β kBI , α kB , v kB , v kscl ,Sα kB ,Sv kB , R ksckθkB  α kB  β kB ,θ kN  ω NNI k  tk  tk 1 BN k 1θkBN  θkB  CBNkk11 θkN , qBNkk  qBNkk11 qBN, CBNkkksclv rotk , v k(β kBI )v kN  CBNkk v kB  v krot  v ksclv kN  v kN1  v kN  gTN  tk  tk 1 (α kB , v kB , v kscl )R kBI , R rotksclR kB  R kBI  R rotk  R kR kN  v kN1 нав  CBNkk R kBR kN  R kN1  R kN(α kB , v kB ,Sα kB ,Sv kB , R ksck )Рис.

10. Численный алгоритм реализации «интервального» алгоритма навигации (следуяPaul G. Savage, Strapdown Associates, Inc., США)15Здесь параметры, подчеркнутые и указанные в скобках являются входнымидля вычисления остальных параметров.Алгоритмы реализованы и используются в ПО для конкретных датчиков ипринятых параметров алгоритмов в главе VII.В седьмой главе произведен анализ результатов комплексного моделирования.Для исследования поведения ПРУ с ИВК на неровной опорной поверхностипостроен комплекс математического моделирования, структура которогопоказана на рис. 11.ИнициализациямоделированияМОДЕЛИРОВАНИЕ КОНТУРА УПРАВЛЕНИЯ(СРЕДА,ТЕЛЕЖКА, ИВК, САУ)X 0 , Z0X Oз , ZOз- Моделирование движениятележки- Шаг интегрирования: DTПЛАНИРОВАНИЕ ДВИЖЕНИЯЭтапнавигацииX нгЭтапнаведенияAK1 , AK 21 , 2X задXАлгоритмстабилизацииUМодельтележкиМикропрофильопорнойповерхностиXX з , Z з , зТекущее изображениеАлгоритмэлементов ЛА,распознаванияучаствующих визображенияфиксации грузовКоррекция алгоритманавигации по результатамработы ОЭСX з , Z з , зМодель ГИБ(коэффициенты из калибровки сучетом невыявленныхпогрешностей)- Шаг моделирования: DTIZАлгоритм обработка первичныхизмерений- Шаг моделирования: DTOBR(Алгоритм выполнен однократно припереходе на третий этап движения)ВЫВОДX n , Z n ,VXn ,VZn ,  , , x1 ,  y1 , z1 , , VXn , VZn ,АЛГОРИТМ НАВИГАЦИИ- Шаг моделирования: DTNAVРис.

11. Комплекс моделированияПри моделировании движения ПРУ приняты следующие данные:- параметры двигателей: Tу . м  0.05 c, k у . м  10 , Lя  23 мГн, Rя  1.91 Ом,2КПД  79 %,Pном .  11.3 кВт,U ном .  420 В, ном .  1280 обр./мин.,J д  0.09 кг.м ,J р  0.01 кг.м2, mд  115 кг, M с .д  0.05886 Н.м, kвт.д  0.03 , i  30 ,   92 %;- параметры тележки ПРУ: M  1845 кг, L1  0.5 м, L2  0.8 м, I x  1000 кг.м2 ,I y  1500 кг.м2, I z  1200 кг.м2, k  25000 Н/м, b  3500 Н.с/м;- параметры колес: m  25 кг, r  0.25 м, I w  3 кг.м2 , M с . н  0.05886 Н.м,kвт .

н  0.05 ;- параметр препятствия: rпр  3м ;- параметры микропрофиля неровной поверхности :  r  1 см,   0.14 ;- географические координаты точки начала движения:   0.872,  0.51;- координаты точки начала движения и точки загрузки ЛА в ЗСК:X О  0м, ZO  0м , X Оз  100м, ZOз  30м ;- СК OЗ X ЗYЗ Z З отличается от СК OX gYg Z g углом  з  30 град;16- шаг интегрирования уравнения движения тележки: DT  1мс ;- шаг измерения ДЛУ и ДУС: DTIZ  3мс , т.е. частота опроса датчиковfизм  1 / DTIZ  333Гц ;- шаг обработки первичных измерений DTOBR  3DTIZ  9мс , т.е.f обр  fизм / N i  f изм / 3  111Гц .На рис. 12 показан результат адаптивного управления вращением ПРУ наместе в течение 4с.

После завершения данного процесса ПРУ вернулось в исходноесостояние с нулевой угловой скоростью и нулевым углом курса.Рис. 12. Результатадаптивного управлениявращением на месте:  зад заданная угловая скорость ПРУ(град/с); н . л . - действительнаяугловая скорость ПРУ (град/с);ид. - угловая скорость,полученная поидентифицируемой линейноймодели (град/с); н . л . - угол курса(град) н. л.н .

л .ид . задРезультат идентификации момента инерции тележки ПРУ показан ниже втаблице 1. Видно, что ошибка идентификации очень мала (<0.05%).Табл. 1. Результат идентификации моментаинерции тележки ПРУИстинное значениеРезультат2идентификации(кг.м)IY(кг.м2)15001500.0915501550.10116001600.10616501650.10917001700.11418001800.12320002000.142При нахождении оптимальнойтраектории обхода препятствия былиприняты следующие параметры:1  18.7 град , 2  16.16град , AK1 =9.64м ,AK2 =9.44м .