1赞

6回答

3收藏

Codesys PLC中实现卡尔曼滤波器

CODESYS 编程环境 CODESYS 编程环境 1612 人阅读 | 6 人回复 | 2022-05-19

一 卡尔曼滤波基本步骤

  卡尔曼滤波包含两个步骤:
  1 预测:
​​

  2 更新:


  其中, 是卡尔曼增益,它是卡尔曼滤波器的精华,定义如下

  以上公式采用的符号与维基百科—卡尔曼滤波保持一致。
  如果你理解上面的公式有困难,那么下面我给出一种简单的解释。
  以机器人定位为例,状态x xx代表机器人的位置和姿态(x,y平面坐标和朝向角度),控制u uu代表机器人的控制输入量(例如左右两车轮的转速)。直观地看,第一步的预测其实就是使用机器人的运动方程进行状态的递推。我们都知道,模型都是有误差的,采集的控制量也不可能完全与实际的一样,这就造成了理论计算值与机器人真实状态的偏差。我们用x xx表示理论计算值,下标t表示true)表示机器人真实的状态。短时间内,这个偏差是可以接受的,但问题是这个偏差会一直累积,导致我们的理论推测值与真实值越差越多,最后完全不可相信了。所以卡尔曼滤波的第一步就是单纯的里程定位或者航迹推测,这没什么新鲜的。
  那么你可能会问,协方差矩阵P PP代表了什么呢?协方差矩阵P PP代表了我们对估计值x xx相信的程度,我们用它对信任程度进行量化。P PP越小说明我们对估计x xx越有信心,反之如果P PP越大我们就越怀疑x xx。这就和人一样,有些人位高权重我们就越倾向于不假思索地相信他们说的话,有些人经常胡说八道我们就会怀疑他观点的正确性。在后面的计算中,我们会根据P PP的大小来分配权重,权重和P PP的大小成反比。
  既然只依靠第一步不靠谱,我们就引入了传感器来弥补。这就是第二步的更新,用传感器的测量结果(z zz)来矫正偏差。可是我们知道,传感器测量值也不是百分之百准确的,它也有不确定度(R RR),我们也不能完全相信它。我们面对两个都没有完全说真话的人,这个时候到底该怎么办呢?是只信任其中一个还是综合考虑两个人的信息呢?肯定不能完全信任一个人,但是怎么综合考虑最好却是门艺术。卡尔曼滤波巧妙的地方就在于,它没有简单粗暴地直接把二者加起来,而是给其中一项前面乘了个系数,这就是卡尔曼增益K KK。卡尔曼证明了这样设计的增益K KK是最优的。没错,卡尔曼增益就是前面提到的权重,它综合了两个信息来源,一个是状态估计,一个是传感器的测量。根据卡尔曼增益K KK的构造方法我们可以猜出来,P PP越大R RR越小,那么K KK就越接近1,这意味着我们就越相信传感器测量,而不信状态的估计值;反之,P PP越小R RR越大,那么K KK就越接近0,这意味着我们就越相信状态的估计值,而不信传感器的测量。
  这样我们就解决了到底该相信谁的问题。

二 数学基础

  如果你要估计的状态是向量,那么卡尔曼滤波就需要矩阵计算。而且它的计算过程基本上把常用的矩阵计算都占了,包括矩阵的加、减、乘、求逆、转置,矩阵与向量的乘,向量的加、减、内积、外积、标量与向量乘等等。这非常有意思,有点像我们练毛笔字中的“永”字,所有常用笔画集于一身。如果把卡尔曼滤波实现了,那么实现其它的算法就触类旁通了。好了,大学里的线性代数课本该拿出来了。
  所有的矩阵运算中,求逆是最复杂的。文献[ 1 ] [1][1]使用了伴随矩阵计算逆矩阵,这种方法适合小规模的矩阵(维数<4),对于大规模矩阵效率较低。目前,矩阵求逆最常用而且比较高效的方法是LU分解法,思路是首先将矩阵分解成两个三角矩阵(的乘积),然后再计算两个三角矩阵的逆。由于计算三角矩阵的逆比较容易,所以这样我们能够高效地求矩阵的逆。但是在实际应用LU分解法时有一点要注意,如果矩阵的对角线上出现了0,就会使分解的三角矩阵不可逆,这可以通过对矩阵的行重新排列来解决,这样就多了一个置换矩阵,细节可以看维基百科。
  维基百科提供了LU分解的C语言代码,我直接将C代码翻译成ST代码。在C语言程序中,直接在输入矩阵上做了计算,即将分解结果保存在了输入矩阵A AA中。如果你想提取出两个三角矩阵(L和U),可以按照下式这样分解,其中I II是单位矩阵。注意这里的矩阵A AA是被更新过的矩阵,不再是原始的输入A AA了哦。当然,从分解的三角矩阵L和U可以还原得到最开始的输入A AA,只需要再乘以一个置换矩阵就行了。
  1. A=(L−I)+U

复制代码
三 PLC具体实现

3.1 PLC编程语言

  我们用PLC做做数字的加减乘除很容易,但是要实现矩阵计算就有点复杂了,有过PLC编程经验的同志都知道,PLC一般不提供矩阵计算所需的函数。原因也很简单,早期的PLC主要被设计用于逻辑控制和过程控制,不是用来实现复杂的运动控制和数据处理算法的。这也是在PLC中实现的卡尔曼滤波主要的难点所在,基础支持太少。常用的PLC编程语言有梯形图和ST。其中,梯形图适合实现逻辑流程控制,不适合数值计算,所以我们采用ST语言这种与高级语言比较类似的PLC语言来实现卡尔曼滤波。由于现在的PLC语言基本都遵守IEC 61131-3国际标准,所以我们只在一种PLC环境中实现,移植到其它厂家的PLC产品将非常容易。我们选择codesys软PLC,它的编程软件免费而且使用方便,还支持本地仿真调试,不像西门子一样还要安装庞大而缓慢的博图软件。
  codesys官方网店提供矩阵计算库(Matrix Library),可实现常用的矩阵操作,例如相加、乘、逆、行列式,但是你要花人民币2500元购买授权才能使用。
[url=http://store.codesys.cn/codesys/store/detail.html?productId=1405783000596307969][/url]
3.2 数据类型

  既然我们用PLC语言实现,那就要熟悉PLC语言的特点。首先面对的问题就是如何定义一个矩阵或者向量。codesys提供了ARRAY数组这种数据类型,可以用来实现矩阵或向量的表示。ARRAY最多支持三维,我们只使用一维和二维就够了。定义二维数组的方法如下,其中,0…2表示数组元素索引的上下界,从A[0,0]开始,最后一位是A[2,2]。可以在定义数组时就初始化,注意对于高维数组,在初始化时采用一维拉直的写法,这就需要约定好是按行拉直还是按列拉直。我们规定一下,本文都采用行主序的存储方式,也就是说矩阵按照行拉直。注意MATLAB采用了Column major order(列主序),而C语言一般是Row major order(行主序),我们与C语言一致。
  1. VAR
  2.         A:ARRAY[0..2,0..2] OF REAL:=[1,2,3,4,5,6,7,8,9];
  3. END_VAR
复制代码
如何操作ARRAY数组呢?如果我们想引用数组中的元素,例如读写,使用A[i,j]即可,其中i和j就是矩阵中元素的行列。
  如果我们想传递数组该怎么办呢,例如将数组作为一个参数传递给函数块?数组名就像C语言中的指针,可以直接传递数组名,例如A:=FUN(B)。也可以直接用数组名给另一个数组赋值,例如A:=B。
  数组的大小可以在定义时设为固定值,在程序执行过程中始终保持不变。这样所有的矩阵运算函数都只接收返回固定大小的数组。但是这样编写的程序没有通用性,遇到一个新的问题或者加入新的条件我们就要修改数组大小,很繁琐。有一个方法是使用可变长度数组[ 2 , 3 ] ^{[2,3]}
[2,3]
(Arrays of variable length)。但是注意,可变长度数组只能用于VAR_IN_OUT声明中,不能用在其它声明中(例如VAR或者VAR_IN),下面我给出两个例子。
  第一个例子是计算两个向量的内积,将其定义为函数(FUN),声明部分如下。Inner 函数的返回值是REAL类型,输入是两个可变长度的一维数组:v1和v2,二者长度必须相等,注意v1和v2的定义放在了VAR_IN_OUT中,而不是VAR中。
  1. FUNCTION Inner : REAL;
  2. VAR
  3.         i:LINT;
  4. END_VAR
  5. VAR_IN_OUT
  6.    v1: ARRAY[*] OF REAL;    //输入向量1
  7.    v2: ARRAY[*] OF REAL;    //输入向量2
  8. END_VAR
复制代码
函数主体部分如下。在遍历数组元素时使用了系统自带的LOWER_BOUND和UPPER_BOUND函数来获取数组的上下界。
  1. Inner:=0;
  2. FOR i:=LOWER_BOUND(v1,1) TO UPPER_BOUND(v1,1) BY 1 DO
  3.         Inner:=Inner+v1[i]*v2[i];
  4. END_FOR;
复制代码
函数使用的方式如下。该函数由于使用了可变长度的数组,所以不限制输入向量的维数,对于任意维数的数组都可以给出正确的结果。
  1. result:=Inner(v1,v2);
复制代码
第二个例子是两个矩阵相加,声明部分代码如下:
  1. FUNCTION MatAdd: BOOL;
  2. VAR_IN_OUT
  3.    A: ARRAY[*,*] OF REAL;   //输入矩阵
  4.    B: ARRAY[*,*] OF REAL;   //输入矩阵
  5.    C: ARRAY[*,*] OF REAL;   //输出矩阵
  6. END_VAR
  7. VAR
  8.         i:LINT;
  9.         j:LINT;
  10. END_VAR
复制代码
函数主体部分如下:
  1. FOR i:=LOWER_BOUND(A,1) TO UPPER_BOUND(A,1) BY 1 DO
  2.         FOR j:=LOWER_BOUND(A,2) TO UPPER_BOUND(A,2) BY 1 DO
  3.                 C[i,j]:=A[i,j]+B[i,j];
  4.         END_FOR;
  5. END_FOR;
复制代码
该函数的使用方式如下,注意MatAdd前面没有赋值符号(:=),而是直接把想要赋值的数组作为参数传递给了这个函数(的第三个输入参数C),C数组里保存着我们想要的结果。
  1. MatAdd(Mat1,Mat2,Mat3);
复制代码

[size=3.0]3.3 矩阵求逆

[size=3.0]  LU分解法的具体ST代码如下,基本与C语言版本一样,只有一个地方不同:C语言可以使用指针,但是ST语言的指针不太好用,所以我们不得不挨个操作矩阵中的每个元素,导致代码略长。

  1. FOR i:=0 TO n-1 BY 1 DO
  2.                 maxA:=0.0;
  3.             imax:=i;
  4.                 FOR k:=i TO n-1 BY 1 DO
  5.                         absA:=ABS(A[k,i]);
  6.                         IF absA>maxA THEN
  7.                                 maxA:=absA;
  8.                                 imax:=k;
  9.                         END_IF
  10.                 END_FOR;
  11.                
  12.                 IF imax<>i THEN
  13.                         //pivoting P
  14.                         j:=P[i];
  15.                         P[i]:=P[imax];
  16.                         P[imax]:=j;
  17.                         //pivoting rows of A
  18.                         FOR k:=0 TO n-1 BY 1 DO
  19.                                 tmp[k]:=A[i,k];
  20.                         END_FOR;
  21.                         FOR k:=0 TO n-1 BY 1 DO
  22.                                 A[i,k]:=A[imax,k];
  23.                         END_FOR;
  24.                         FOR k:=0 TO n-1 BY 1 DO
  25.                                 A[imax,k]:=tmp[k];
  26.                         END_FOR;
  27.                 END_IF
  28.                         
  29.                 FOR j:=i+1 TO n-1 BY 1 DO
  30.                         A[j,i]:=A[j,i]/A[i,i];
  31.                         FOR k:=i+1 TO n-1 BY 1 DO
  32.                                 A[j,k]:=A[j,k]-A[j,i]*A[i,k];
  33.                         END_FOR;
  34.                 END_FOR;
  35.         END_FOR;
复制代码
完成LU分解后,下面再求逆就容易了,代码如下。
  1. FOR j:=0 TO n-1 BY 1 DO
  2.                 FOR i:=0 TO n-1 BY 1 DO
  3.                         IF P[i]=j THEN
  4.                                 IA[i,j]:=1;
  5.                         ELSE
  6.                                 IA[i,j]:=0;        
  7.                         END_IF;        
  8.                         FOR k:=0 TO i-1 BY 1 DO
  9.                                 IA[i,j]:=IA[i,j]-A[i,k]*IA[k,j];
  10.                         END_FOR;
  11.                 END_FOR;
  12.                 FOR i:=n-1 TO 0 BY -1 DO                 
  13.                         FOR k:=i+1 TO n-1 BY 1 DO
  14.                                 IA[i,j]:=IA[i,j]-A[i,k]*IA[k,j];
  15.                         END_FOR;
  16.                         IA[i,j]:=IA[i,j]/A[i,i];
  17.                 END_FOR;
  18.         END_FOR;
复制代码
完整的卡尔曼滤波迭代过程代码如下:
  1. FOR i:=0 TO 100 BY 1 DO
  2.                 //Prediction
  3.                 x:=VecAdd(VecAdd(MatVecMul(F,x),u));
  4.                 P:=MatAdd(MatMul(MatMul(F,P),MatTranspose(F)),Q);
  5.                 //Update
  6.                 K:=VecMul(MatVecMul(P,H),MatInv(KroneckerProduct(H,P),MatTranspose(H))+Rc);
  7.                 x:=VecAdd(x,VecMul(K,z-Inner(H,x)));
  8.                 P:=MatMul(MatSub(IdentityMatrix(n),KroneckerProduct(K,H)),P);
  9.         END_FOR;
复制代码
四 具体案例

  这里我们用一个例子来检验代码的正确性,例子来自于[4],其与文献[1]差不多,是一个小球做自由落体运动,估计它的高度和速度。
  文献[4]给出了MATLAB代码,如下。当然,这里只是仿真结果,噪声用randn函数生成。因为是仿真,为了对比和计算传感器观测值,还定义了真实的系统状态xt。仿真中,真实状态xt是没有噪声的(完美的),估计值是有噪音的。

  1. % Define the system
  2. N = 100;  % number of time steps
  3. dt = 0.01;  % Sampling time (s)
  4. t = dt*(1:N);  % time vector (s)
  5. F = [1, dt; 0, 1];  % system matrix - state
  6. G = [-1/2*dt^2; -dt];  % system matrix - input
  7. H = [1 0];  % observation matrix
  8. Q = [0, 0; 0, 0];  % process noise covariance
  9. u = 9.80665;  % input = acceleration due to gravity (m/s^2)
  10. I = eye(2);  % identity matrix
  11. % Define the initial position and velocity
  12. y0 = 100;  % m
  13. v0 = 0;  % m/s
  14. % Initialize the state vector (true state)
  15. xt = zeros(2, N);  % True state vector
  16. xt(:, 1) = [y0; v0];  % True intial state
  17. % Loop through and calculate the true state
  18. for k = 2:N
  19. xt(:, k) = F*xt(:, k-1) + G*u;  % Propagate the states through the prediction equations
  20. end
  21. % Generate the noisy measurement from the true state
  22. R = 4;  % m^2/s^2
  23. v = sqrt(R)*randn(1, N);  % measurement noise
  24. z = H*xt + v;  % noisy measurement
  25. %% Perform the Kalman filter estimation
  26. % Initialize the state vector (estimated state)
  27. x = zeros(2, N);  % Estimated state vector
  28. x(:, 1) = [105; 0];  % Guess for initial state
  29. % Initialize the covariance matrix
  30. P = [10, 0; 0, 0.01];  % Covariance for initial state error
  31. % Loop through and perform the Kalman filter equations recursively
  32. for k = 2:N
  33. x(:, k) = F*x(:, k-1) + G*u;  % Predict the state vector
  34. P = F*P*F' + Q;  % Predict the covariance
  35. K = P*H'/(H*P*H' + R); % Calculate the Kalman gain matrix
  36. x(:,k) = x(:,k) + K*(z(k) - H*x(:,k)); % Update the state vector
  37. P = (I - K*H)*P; % Update the covariance
  38. end
  39. %% Plot the results
  40. plot(t, x(1,:), 'b--', 'LineWidth', 2);
  41. hold on; plot(t, xt(1,:), 'r:', 'LineWidth', 1.5)
  42. xlabel('t (s)'); ylabel('x_1 = h (m)'); grid on;
  43. legend('Measured','Estimated','True');
复制代码


本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?立即注册

x
德国 CODESYS(欧德神思)软件集团是一家长期致力于研发高端装备控制系统软件开发平台及智能工厂解决方案的跨国公司。集团以 CODESYS 软件工具套件为核心,向用户提供一系列工业智能技术、自动化软件产品和柔性制造软件解决方案。多年来在包括能源、冶金、轨道交通、包装印刷、数控机床、智能机器人、工程机械等诸多领域,备受青睐。
分享到:
回复

使用道具 举报

回答|共 6 个

Django

发表于 2022-5-19 22:59:44 | 显示全部楼层

二维数组使用方法不错,学习了。有完整版的工程吗?
回复

使用道具 举报

tnt131421

发表于 2022-5-19 23:04:16 | 显示全部楼层

优秀,看着就很牛逼
回复

使用道具 举报

geshangzhineng

发表于 2022-6-7 09:21:42 | 显示全部楼层

看着很牛
回复

使用道具 举报

王贝塔

发表于 2022-6-10 14:03:29 来自手机 | 显示全部楼层

学习学习学习一下
回复

使用道具 举报

zhu

发表于 2022-6-10 17:28:19 | 显示全部楼层

优秀..............
回复

使用道具 举报

nihaota

发表于 2022-9-3 19:26:17 | 显示全部楼层

回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

2252 积分
4 主题